Chapter 2 Math Fundamentals

Transcription

1 Chapter 2 Math Fundamentals Part Quaternions 1

2 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 2

3 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 3

4 Quaternions Smart Irishman: Hamilton Probably the most powerful number system in common use. Hamiltonian mechanics Generalization of Lagrange Mechanics Which was a generalization of Newton-Euler Mechanics. Which was a generalization of Newtonian Mechanics. 4 4

5 Was It All the Guinness? 5 5

6 Core Problem and Properties How can we divide a vector by a vector? Answer. Need to have principle imaginaries : Quaternions: i 2 = j 2 = k 2 = ijk = 1 are generalizations of complex numbers which do not commute (complex #s do). can represent every transformation that an HT can represent. 6 6

7 Why Use Em? Only way to solve some problems like the problem of generating regularly spaced 3D angles. Best way to solve some problems. No gimbal lock at Euler angle singularity. Still not a unique representation though. Simplest way to solve some problems. Some problems in registration can be solved in closed form. Fastest way to solve some problems. Quaternion loop in an inertial navigation system updates vehicle attitude 1000 times a second. 7 7

8 Intuition from Complex Numbers Use a second imaginary dimension. Permits manipulation of rotations like a vector. Remember phasors in EE. i θ 8 8

9 Notations 4-tuples Hypercomplex numbers Sum of real and imaginary parts Ordered doublet Exponential ( q 0, q 1, q 2, q 3 ) q = q 0 + q 1 i+ q 2 j+ q 3 k q = q+ q ( qq, ) q = e θw I will use these two Manipulate like Polynomials 9 9

10 My Preference Mostly use the scalar-vector sum form: q = q+ q ~ means quaternion means 3D normal vector means scalar Occasionally write it out to get hypercomplex form: q = q 0 + q 1 i+ q 2 j+ q 3 k 10 10

11 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 11

12 Multiplication Quaternions are elements of a vector space endowed with multiplication. Just Like Complex Numbers The expression: p q = ( p 0 + p 1 i + p 2 j + p 3 k) ( q 0 + q 1 i + q 2 j + q 3 k) Gives the sum of all these elements: So, we need to define what i*i etc mean 12 12

13 Two goals: Multiplication Rule 1) Manipulate like polynomials 2) Product of two quaternions is a quaternion. To get things to work as Hamilton intended we need to have: Diagonals work like complex numbers. Off diagonals work like vector cross product. Or, more compactly: i 2 = j 2 = k 2 = ijk =

14 Product In hypercomplex (polynomial) form: p q = ( p 0 + p 1 i + p 2 j + p 3 k) ( q 0 + q 1 i+ p 2 j+ p 3 k) p q = ( p 0 q 0 p 1 q 1 p 2 q 2 p 3 q 3 ) + ( )i + In vector form: p q = ( p + p) ( q + q) p q = pq+ pq + qp + pq The last term can be written in terms of familiar vector products. pq = p q p q Convenient to summarize like so: 14 p q = pq p q + pq + qp + p q Not the same thing? 2 common vector products 14

15 Non-Commutativity The vector cross product does not commute. Therefore: p q q p What is the source of this property? 15 15

16 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 16

17 Addition Works just like vectors, polynomials, and complex numbers. p + q = ( p 0 + q 0 ) + ( p 1 + q 1 )i + ( p 2 + q 2 )j+ ( p 3 + q 3 )k 17 17

18 Distributivity Works just like vectors, polynomials, and complex numbers. ( p + q )r = p r + q r p ( q + r ) = p q + p r 18 18

19 Dot Product and Norm Works just like vectors, polynomials, and complex numbers. p q = pq + p q Not the same thing Can now define a length (norm): q = q q Unit quaternions have a norm of unity

20 Conjugate Works just like complex numbers. q * = q q Product with conjugate equals dot product: q q * = ( qq + q q) = q q Another way to get the norm is then: q = q q * 20 20

21 Quaternion Inverse The Big Kahuna. Since we have: q q * q 2 = 1 By definition of inverse: q * q 1 = q 2 So. p -- = p q 1 = p q * q q 2 That s how you divide a vector by a vector! 21 21

22 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 22

23 Quaternionize : Vectors as Quaternions x = 0+ x 23 23

24 The unit quaternion: Rotations as Quaternions Represents the operator which rotates by the angle θ around the axis whose unit vector is ww. q = θ cos -- + ŵsin θ The inverse is clearly: ŵ = q q θ = 2atan 2( q, q) Real vectors are just quaternions 0+xi+yj+zk 24 24

25 Rotating a Vector (Point) Use the quaternion sandwich: w x' = q x q * x x θ 25 25

26 Composite Rotations Use the composite quaternion sandwich. Recall: Thus: x' x '' = q x q * = p x' p * = ( p q )x ( q *p * ) Conjugate of a product works like matrices! Composition of operations equals multiplication

27 Quaternion to Rot() Matrix For the quaternion: q = q 0 + q 1 i+ q 2 j+ q 3 k The equivalent Rot() matrix is: R = 2 [ + q 1 ] 1 2q [ 1 q 2 q 0 q 3 ] 2q [ 1 q 3 + q 0 q 2 ] 2q q [ 1 q 2 + q 0 q 3 ] 2q [ 0 + q 2 ] 1 2q [ 2 q 3 q 0 q 1 ] 2 2 2q [ 1 q 3 q 0 q 2 ] 2q [ 0 q 1 + q 2 q 3 ] 2q [ 0 + q 3 ]

28 For the Rot() matrix: Rot() Matrix to Quaternion The equivalent quaternion is determined from: R 2 r 11 + r 22 + r 33 = 4q r 11 r 22 r 33 = 4q 1 1 r 11 r 22 r 33 = + = 2 4q = 2 4q 3 1 r 11 r 22 r 33 r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 Are quaternions unique for a given rotation? 28 28

29 Calculus (wrt scalars) Derivatives work like you would expect: dq dt = dq dt + dq dt Integrals work like you would expect: t 0 q dt = t 0 qdt + t 0 q dt 29 29

30 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 30

31 Angular Velocity Define the angular velocity: ω n () t = ( ω 0 + ω 1 i + ω 2 j + ω 3 k) For a unit quaternion representation of orientation: q () t Its time derivative is: Recall the skew matrix derivative of a rotation matrix. = cos θ () t + ŵsin θ() t 2 2 dq () t 1 = -- ω dt 2 n ()q t () t dubyaq 31 31

32 Angular Velocity That was for angular velocity represented in navigation coordinates: ω n () t = ( ω 0 + ω 1 i + ω 2 j + ω 3 k) If you have it in body coordinates, just use the instantaneous value of q () t itself to convert: Substituting: ω n () t = q ( t)ω b ()q t () t * dq () t 1 = -- q ( t)ω dt 2 b () t Qdubya 32 32

33 Quaternion Loop Runs at 10 khz inside an INS: q () t = t 1 -- q ( t)ω 2 b t 0 () dt 16 * 2 = 32 flops 33 33

34 Quaternion Loop For highest accuracy, we can use Jordan s trick: Define the skew matrix of a quaternion: 34 34

35 Quaternion Loop But such matrices have closed form exponentials: Where: After more manipulation 35 35

36 Outline Representations and Notation Quaternion Multiplication Other Quaternion Operations Representing 3D Rotations Attitude and Angular Velocity Summary 38

37 Summary Quaternions are hypercomplex numbers with an i,j,and k that act like the i in complex numbers. Notation is half the battle. Provide elegant and efficient ways to model 3D transformations of points (and hence 3D coordinate system conversions)