Mobile Robot with Earth and Robot Coordinate Frames

Transcription

1 80 ROBOT ATTITUDE Earth Frame /Fi2 ft LD 't I c-o<!) ~ N\.t>6\ LE ~O~U rs Robot Frame Y v X E Figure 3.1 Mobile Robot with Earth and Robot Coordinate Frames As the robot moves about, it experiences translation or change in position. In addition to this, it also may experience rotation or change in attitude. The various rotations of the robot are now defined. Yaw is rotation about the Z axis in the counter-clockwise direction as viewed looking into the Z axis. Pitch is rotation about the new (after the yaw motion) X axis, in the counter-clockwise direction as viewed looking into the X axis, i.e., front end up is positive pitch. Roll is rotation about the new (after both yaw and pitch) Y axis in the counter-clockwise direction as viewed looking into the Y axis, i.e., left side of vehicle up is positive. In the system used by those in the aerospace field pitch is counter-clockwise rotation about the Y axis while roll is counterclockwise rotation about the X axis, i.e., the roles of the X and Y axes are reversed with respect to these two rotations. 3.2 ROTATION MATRIX FOR YAW The rotation matrices for basic rotations are now derived. For yaw we have the diagram shown in Figure 3.2. Axes 1 represent the robot coordinate frame before rotation and axes 2 represent the robot coordinate frame after positive yaw rotation by the amount w. The z axes come out of the paper. It bears repeating that counter-clockwise rotation about the z axis is taken as positive yaw. We wish to express in the original coordinate frame 1 the location of a point whose coordinates are given in the new frame 2. For x and y we have Xl = X2 cos If/ - Y2 sin lfi Yl = X2 SIll If/ + Y2 COS lfi

2 ROTATION MATRIX FOR YAW 81 Frames Figure 3.2 Frame 2 Yawed with Respect to Frame 1 or change in on or change efined. Yaw is ion as viewed after the yaw ewed looking otation about 1ter -clockwise of vehicle up ~field pitch is III is counter- X and Y axes and for z or Thus the rotation matrix for yaw is -sin tf! cos tf! o.d, For yaw we he robot coorbot coordinate e z axes come kwise rotation EXAMPLE 1 -sm tf! cas tf! o O~] (3.1) A vector expressed in the rotated coordinate system with tf! of tt 12 is given by 1 the location le 2. For x and Express this vector in the original coordinate system.

3 82 ROBOT ATTITUDE SOLUTION 1 The expression of this vector in the original coordinate system becomes ~Q,. \ Xl [COS"'n12 -sin"ni2 1[11 [ 1 [ ~ 1 ~ sin~h COS;/2 ~ ~ ~ ~ Note that the Euclidean norm of each column of the rotation matrix is one and that each column is orthogonal to each of the others. This is the definition of an orthonormal matrix. A convenient property of such matrices is that the inverse is simply the transpose, i.e., (3.2) or or This property can be proved by pre-multiplying an orthonormal matrix by its transpose and then using the properties which it possesses, l.e., (coli,colj) = 1, 1 = ] =0 i s ] 3.3 ROTATION MATRIX FOR PITCH For pitch we have the situation depj ed in Figure 3.3. The x axes come out of the paper. Note again th ront end up corresponds to positive pitch ~ Again we wish to ex ss in the original coordinate frame the location of a point whos coordinates have been given in the new frame. For x and z we h e Y1 = Y2 COS 0 - Z2 sin 0 - Thus EXAM A vectc pitched Exp and for x 21 = Y2 sino + Z2 coso Xl =X2 SOLL The ej

4 Quaternion Rotation Parts of quaternion "\ W = cos(b/2) V == sin(b/2)u Where ii is a unit/normalized vector u (i, j, k) Quaternion can be represented as: q==cos(b/2)+sin(b/2) (xi+yj+zk) or q==cos(b/2)+sin(b/2) u Matrix rotation versus quaternion rotation

5 Rotation of a Quaternion Calculation is still done in matrix form Given a quaternion: (~ATCt-t OIt..O~~~) q = W + xi+ yj + zk The matrix form of quaternion q İS: W +x - y -z 2wz+2xy 2xz-2wy 2xy-2wz W -x + y -z 2wx+2yz 2wy+2xz 2yz-2wx W -x - y +z Matrix entries are taken all from quaternion

6 RbrAT" '"" A-~ev r ~ fib'( '1O'fl C-J.rj 11~ e 1) R L~.z...) -::. 0 -I 0 S IV1 ~L. Co/ t.!) I C4~)= 7-,: Si~~) -l. - D -0 -'J 'l- 1'2 D '" C {} O"'D -- +-\ o - '\ o o o \

7 Operations on Unit Quaternions Perform Rotation, * x == qxq == == (q~ - q. q)x + 2qoq x x + 2q(q x) Composition of rotations, * x == pxp ",* ( * \ ~* ( ) ( )* x == qx q == q pxp H == qp x qp Interpolation pet) == (1-f)/ +tp", P = ((f)1 - Linear pet) - Spherical linear (more later)

8 Example of rotation quaternions using unit y. x',... -/ +Y... I l... x, I I I p(2,l,l) R==1:0:0 o : -1 : 0 I I I 0: 0 : 1 I Rot(z,900) I o I p' == Rp == o For comparison we first use matrices

9 Example (cont) p = ( ) For comparison we use ) / quaternions ( q= p' = (q~ - q. q)p + 2qoq x p + 2q(q p) Next we convert 2 I i j k 0 / to matrices = (~ - ~)I ~ 0 0 ~ (~ ) = I We get the same result G

10 1\ e o s, fd S\!!. n:~r \?~~ tr -r ~ ~\.~,W5 P1>~l r, 0 (\J O~\e-~T,q..Ti,to { 0)0)01 1~ = (0 I o I oj 1) "I-. I 'j I -2. J W ~'lv\ c.e- ANq I..e 1S ~Q A-t.,o N4 )( ~~". b~ Jf, '\ ",.H~'l ~ \lcloc.. t,t e:j. "'N er14fl.!~n ~ 0 LA- t2- f:>n'w( L'''!A.:~)<. A.~ Av\'fu\~~ ~ \~ Cf fo ~ Y'v\ M 4 tv.p ~... 0,,1.~/fo ~'f;' : L." ~Ae~ f q~-\&'d p~thd'n c.e 0 C X -::'0, :;; VIA.) ~ 6.: 0 'f'a.a4e..c.., ~M?A-~ t;.a~"l1::r4 Ae..M r2-"2...~'" ~~y Lt:> e~ To \~rs ['7) '2.'2.4 P'22..)-2.t~ 'f> 0.> fi" 0 F N t>.:rb (~,... CqJt2...t ppe-rz..,) )(.,~,.:c. \ Yl M.-l:Tt='R,.>