6飞控学习资料四元数quaternions

1、QuaternionsSebastian O.H. Madgwick September 24, 2011A quaternion is a four-dimensional complex number that can be used to represent the orientation of a rigid body or coordinate frame in three-dimensional space. An arbitrary orientation of frame B relative to frame A can be achieved through a rotat

2、ion of angle around an axis Ar defined in frame A. This is represented graphically in figure 1 where the mutually orthogonal unit vectors xA, yA and zA, and xB, yB and zB define the principle axis of coordinate frames A and B respectively. The quaternion describing this orientation, ABq, is defined

3、by equation (1) where rx, ry and rz define the components of the unit vector Ar in the x, y and z axes of frame A respectively. A notation system of leading super-scripts and sub-scripts adopted from Craig 1 is used to denote the relative frames of orientations andvectors. A leading sub-script denot

4、es the frame being described and a leading super-scriptdenotes the frame this is with reference to. For example, A q describes the orientation ofBframe B relative to frame A and Ar is a vector described in frame A. Quaternion arithmetic often requires that a quaternion describing an orientation is f

5、irst normalised. It is therefore conventional for all quaternions describing an orientation to be of unit length.zAzBAryByAxAxBFigure 1: The orientation of frame B is achieved by a rotation, from alignment with frameA, of angle around the axis Ar.A q = q0qq3 = cosr2sinrsinr(1)q1sinxyzB2222Thequatern

6、ionconjugate, denotedby, canbe usedtoswaptherelativeframesdescribedby an orientation. For example, B q is the conjugate of A q and describes the orientation ofABframe A relative to frame B. The conjugate of A q is defined by equation (2).B1A q= B q= q0qqq3(2)BA12The quaternion product, denoted by ,c

7、an be used to define compound orientations.For example, for two orientations described by A q and B q, the compounded orientation A qBCCcan be defined by equation (3).A CBA q(3)q=qCBFor two quaternions, a and b, the quaternion product can be determined using the Hamilton rule and defined as equation

8、 (4). A quaternion product is not commutative; that is, a b 6= b a.a b =b3a0a1a2a3b0b1 b20 b0 a b1 1 a b22 a b 3 3 T a0b 1+ a b1 +0 a b 2 3a b 3 2 a0b 2 a b1 +3 a b 2+0a b 3 1 a0b3 + a1b2 a2b1 + a3b0a(4)=A three dimensional vector can be rotated by a quaternion using the relationship de- scribed in

9、equation (5) 2.Avand Bv are the same vector described in frame A and frameB respectively where each vector conta element row vectors.a 0erted as the first element to make them 4B v =AAAq v (5)definedqBBA RThe orientation described by A q can be represented as the rotation matrixBBby equation (6) 2.2

10、(q1q2 + q0q3)2(q1q3 q0q2)22 2q01 + 2q 1A R =222(q1q2 q 0q3 )2 1 + 2(6)q2(q2q3+ q q0 )1q0B22232(q q q q )2 1 + 2q2(q1q3 + q0q2)2 30 1q0A quaternion may be obtain from a rotation matrix using the inverse of the relationshipsdefined in (6); however,ome practical applications an available rotation matri

11、x may notbe orthogonal and so a more robust method is prefered. Bar-Itzhack provides a method 3 to extract the optimal, best fit quaternion from an imprecise and non-orthogonal rotation matrix. The method requires the construction of the symmetric 4 by 4 matrix K (equation(7) where rmn corresponds t

12、o the element of the mth row and nth column of AR. The optimalBquaternion, A q, is found as the normalised Eigen vector corresponding to the maximumBEigen value of K. This is defined by equation (8) where v0 to v3 define the elements of the normalised Eigen vector.r23 r32r21 + r12 r22 r11 r33r32 + r

13、23 r31 r132r11 r22 r33 r21 + r12r31 + r13r31 r1313r32 + r23K =(7)r33 r11 r22r12 r21+ r13r31r23 r32r12 r21r11 + r22 + r33A q = v3(8)achieved by thev0v1v2BThe ZYX Euler angles , and describe an orientation of frame Bsequential rotations, from alignment with frame A, of around zB, around yB, and around

14、 xB. This Euler angle representation of A q can be calculated 4 using equations (9)Bto (11).223! = atan2 2(q qq 1 + 2q(9), 2qq23010p 2(q1 q3 + q0 q2 ) = arctan(10)1 (2q1q3 + 2q0q2)2221 = atan2 2(q q1 2, 2q 1 + 2q(11)q q030References1John J. Craig. Introduction to Robotics Mechanics and Control. Inte

15、rnational, 2005.Pearson Education2J. B. Kuipers. Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality. Princeton University Press, 1999.Itzhack Y Bar-Itzhack. New method for extracting the quaternion from a rotationma- trix. AIAA Journal of Guidance, Control and Dynamics, 23(6):10851087, Nov.Dec 2000. (Engineer