Robotics 3 chuyen dong cua vat ran va phep bien doi toa do thuan 16x9 compatibility mode

slie bài giảng hướng dẫn cho môn học robotics đại học bách khoa hà nội năm học 20222023 . Chuyển động của vật rắn và phép biến đổi tọa độ thuần nhất chương 3 . cách thành lâph bẳng DH3.1 Biểu diễn vị trí3.2 Biểu diễn chuyển động quay3.3. Tổng hợp các phép quay3.3.1. Quay hệ trục tọa độ hiện hành3.3.2. Quay liên tiếp quanh các trục cố định3.4. Tham số hóa ma trận quay3.4.1. Các góc Euler ZXZ và ma trận quay tương ứng3.4.2. Các góc Cardan và ma trận quay Cardan (XYZAngles) 3.4.3. Các góc RollPitchYaw3.4.4. Trục quay và góc quay3.4.5. Các tham số Euler (Euler parameters)

-2- Nội dung Chương CHUYỂN ĐỘNG CỦA VẬT RẮN VÀ PHÉP BIẾN ĐỔI TỌA ĐỘ THUẦN NHẤT Rigid body motion and homogeneous transformations Nguyễn Quang Hoàng Email: hoang.nguyenquang@hust.edu.vn 3.1 3.2 3.3 3.3.1 3.3.2 Biểu diễn vị trí Biểu diễn chuyển động quay Tổng hợp phép quay Quay hệ trục tọa độ hành Quay liên tiếp quanh trục cố định 3.4 3.4.1 3.4.2 3.4.3 3.4.4 3.4.5 Tham số hóa ma trận quay Các góc Euler Z-X-Z ma trận quay tương ứng Các góc Cardan ma trận quay Cardan (XYZ-Angles) Các góc Roll-Pitch-Yaw Trục quay góc quay Các tham số Euler (Euler parameters) 3.5 3.5.1 3.5.2 Chuyển động vật rắn Biểu diễn chuyển động vật rắn Tổng hợp chuyển động vật rắn 3.6 3.6.1 3.6.2 Tọa độ phép biến đổi tọa độ Tọa độ Ma trận biến đổi tọa độ Nguyen Quang Hoang-Department of Applied Mechanics-SME Camera z2 2 1 4 a2 Camera a1 d3 z0 x0 O0 y0 y4 x4 z4 Nguyen Quang Hoang-Department of Applied Mechanics-SME -3- -4- Cấu trúc song song = Chuỗi động học đóng Cấu trúc chuỗi Cấu trúc lai = Chuỗi động học mở = Chuỗi động học đóng mở Nguyen Quang Hoang-Department of Applied Mechanics-SME Chuỗi: Lai: Kuka-R15 Tricept, ABB – IRB 940 Song song: Delta-Roboter, ABB – Flexpicker Nguyen Quang Hoang-Department of Applied Mechanics-SME -5- -6- Biểu diễn vị trí Có hệ quy chiếu (hệ trục tọa độ, khung tọa độ) {a} {b}:     {a } : e{a }  [i , j , k ]T , BIỂU DIỄN VỊ TRÍ VÀ HƯỚNG  Biểu diễn vị trí  Biểu diễn phép quay 2D  Biểu diễn phép quay 3D {b} :     e{b }  [ib , jb , kb ]T z Có điểm P Vị trí điểm P hqc {a} {a } i B rP(a ) Nguyen Quang Hoang-Department of Applied Mechanics-SME Xét chuyển động quay quanh z0 y1       F0 =[i0 , j , k ], F1 =[i1, j1, k1 ] M y   rM  R 10   i(0) 1   j  i   cos  1 0    j1  j   sin     sin    cos    O x1 x  i1   i0 x0 Phép quay đơn Ma trận cosin hướng hay ma trận quay (quanh trục z) Hướng hệ F0 so với hệ F1, xác định R 10   i(1) 0    i0  i1 (1)  j0      i j    j  i1   cos      j  j1    sin    sin    cos    rP(b/)B R 10  (R 10 )T (R 10 )T  (R 10 )1 det(R 10 )  Nguyen Quang Hoang-Department of Applied Mechanics-SME x P(b )     yP(b )  z (b )   P  -8- Biểu diễn chuyển động quay/2D Tổng hợp hai phép quay quanh trục   Hướng hệ F1 so với hệ F0, xác định    i1  i0 (0)   j1     i j   j0 x P(a )     yP(a )  , z (a )   P  Nguyen Quang Hoang-Department of Applied Mechanics-SME -7-  j1 y x Biểu diễn vị trí Nếu có nhiều hệ quy chiếu (nhiều hệ trục tọa độ), cần ghi rõ vị trí điểm hệ trục tọa độ y0  ib y      rP /B  x (b )ib  y (b ) jb  z (b )kb  eT{b }rP(b/)B Phép quay mặt phẳng  jb {b } z  j O x Vị trí điểm P hqc {b}  kb  r  k      r  xi  yj  zk  eT{a }rP(a ) Biểu diễn chuyển động quay/2D  rP /B P    e =[i0 , j ]T ,    e1 =[i1, j1 ]T ,     e  R10 e1, e1  R12 e2     e  R10 R12 e2  R 20 e2  cos  R10     sin  sin   , cos     cos  R10 ( )R12 ( )    sin   R   i(0) 2 x2 y1 R  R ( )R ( ) sin    cos   cos     sin   x1  y2  cos  R12     sin     i2  i0    j   i  j  (0) y0    e =[i2 , j2 ]T x  j0  i1     O  j1  i0 x0 sin    cos    sin   cos(   )  sin(   )   cos    sin(   ) cos(   )       j  i   cos(   )  sin(   ) 2 0      R10 ( )R12 ( ) j2  j   sin(   ) cos(   )     Nguyen Quang Hoang-Department of Applied Mechanics-SME -9- Biểu diễn chuyển động quay/2D Liên hệ hai hệ véctơ đơn vị    e =[i0 , j ]T , y0    e1 =[i1, j1 ]T    i1  cos  i0  sin  j     j1   sin  i0  cos  j     i1   cos        sin  j       e1  R10 e  (R10 )T e y1  sin    i0     cos    j    O    r  eT1 rM(1)  eT0 rM(0) r (0) M  x  x   j ]    eT0 rM(0) y  Rr cos   cos    sin       cos       ax a ay a az a      e10  a cosin hướng vec tơ hệ quy chiếu F0: a  e20  a a  e 30  a Ma trận cosin hướng    hệ quy chiếu F0: { i0 , j , k }    hệ quy chiếu F1: { i1, j1, k1 } Ma trận cosin hướng hệ trục tọa độ F1 hệ F0    i1  i0   R10  [ r1, r2, r3 ]  i1  j  i  k  (0)   i1      j1  i0 k1  i0      j1  j k1  j      j1  k k1  k      (0)  j(0) k  [i1, j1, k1 ] in F 1  Ma trận cosin hướng hệ trục tọa độ F0 hệ F1    R10  [i0, j0 , k ] in F  i(1) 0 j(1)    i0  i1     i0  j1 k(1)   i  k  {F1}  i0  k0 x0       j  i1 k  i1   i1  i0        j  j1 k  j1   i1  j        j  k1 k  k1 i k  1 O0 y j1 O z0  j y0 x  i1 {F0}    e1o x0  a {F0}  e2o y0  cos   e  a     | a |   cos   e20  a  cos   e  a      T j1  i0 k1  i0      j1  j k1  j   (R10 )T     j1  k k1  k   -12- Biểu diễn chuyển động quay/3D Tính chất ma trận cosin hướng R  R10  [ r1, r2, r3 ]  i1(0)  j1(0)    k1(0)   [i1, j1, k1 ] in F  R ma trận trực giao R 1  RT , RRT  RT R  E R  [ r1 r2 r3 ] u  3 cụ thể 1, i  j  riT  rj   0, i  j {F1}  k1 z  i0  k0 x0 O0 y j1 O z0 R có trị riêng Nguyen Quang Hoang-Department of Applied Mechanics-SME  a Nguyen Quang Hoang-Department of Applied Mechanics-SME -11-  k1 z z0  e 3o  a Nguyen Quang Hoang-Department of Applied Mechanics-SME Biểu diễn chuyển động quay/3D  a hệ quy chiếu F0:  a  |a |     a  a xe10  a ye20  a ze 30     (a cos  )e10  (a cos  )e20  (a cos  )e 30 cos       j1 ]    eT1 rM(1)    x M(0)   cos   (0)    y sin   M   (1) M  i0 x0     r   i1   j1  [i1    eT0 R 10 rM(1)  eT0 rM(0)  i1      r  xi0  yj  [i0 Liên hệ tọa độ điểm ttrong hai hệ trục tọa độ Cosin hướng: cosin hướng vec tơ  x1   rM    e  R10 e1 & M y  j0  j1 -10- Biểu diễn chuyển động quay/3D  j y0 x  i1 {F0} Ru  u R có định thức det(R)  Nguyen Quang Hoang-Department of Applied Mechanics-SME -13- Biểu diễn chuyển động quay/3D         e1  [i1, j1, k1 ]T e  [i0 , j , k ]T Liên hệ hai hệ véc tơ đơn vị R  R10  [ r1, r2, r3 ]  i1(0)   k1 z    k1(0)   [i1, j1, k1 ] in F  j1(0) {F1} Ta có phương trình sau:           i1  (i1  i0 )i0  (i1  j ) j  (i1  k )k      i0r11  j 0r21  k 0r31  eT0 r1,     j1  eT0 r2 , k  eT0 r3  i0 j1 O  k0  j y0 O0 x0 Liên hệ hình chiếu véc tơ hai hệ trục             e  [i0 , j , k ]T e1  [i1, j1, k1 ]T e  RT e , e  R 10 e y z0 x  i1 Trong hệ trục F1: {F0} u     u  u x(0)i0  u y(0) j  u z(0)k   eT0 u (0),  [u x(1), u y(1), u z(1) ]T u (0)  [u x(0), u y(0), u z(0) ]T     u  eT0 u (0)  eT1 u (1)  eT0 Ru (1)    eT1  eT0 [r1, r2 , r3 ]  eT0 R 10   e  RT e , (1) u   e  R 10 e1 (to ) R to from u ( from ) Nguyen Quang Hoang-Department of Applied Mechanics-SME Các ma trận quay R  R 10  [ r1, r2 , r3 ]   i 1(0)  z0 z  x0 R xo ( )  rot (x o ,  ) 1   0 0  cos  sin     sin   cos    Tính chất sau nghiệm y  y0  y0, y x0 x R yo ( )  rot (yo ,  )  cos      sin    sin     cos    Rk ( )  RkT ( ), y0 x R zo ( )  rot (z o , )  cos     sin     sin  cos  {F1} y j1 O z0  i0  k0 {F0}  j y0 O0 x0 x  i1 u (0)  Ru (1)  R 10 u (1) Như vậy, ma trận cosin hướng R cho phép biến đổi hình chiếu véctơ (hay tọa độ điểm) hai hệ trục tọa độ gốc Từ ý nghĩa đó, ma trận cosin hướng R gọi ma trận quay (Rotation matrix) -16- Biểu diễn chuyển động quay/3D Tóm lại, ma trận quay thể ba ý nghĩa hình học tương đương sau: • Mơ tả hướng hai hệ trục tọa độ; véc tơ cột cô sin hướng trục hệ sau quay hệ ban đầu • Cho phép biến đối tọa độ điểm hai hệ quy chiếu chung gốc • Cho phép tính tọa độ véc tơ quay quanh gốc tọa độ F0 z0,z  y x0, x    k 1(0)   [i1, j1, k1 ] in  z0 z  j1(0)  u Nguyen Quang Hoang-Department of Applied Mechanics-SME -15- Biểu diễn chuyển động quay/3D  k1 z Trong hệ trục F0:     u  u x(1)i1  u y(1) j1  u z(1)k1   eT1 u (1), Gộp lại  -14- Biểu diễn chuyển động quay/3D 0  0 1  k  x , y, z Nguyen Quang Hoang-Department of Applied Mechanics-SME    i1  i0   R10  [ r1, r2, r3 ]  i1  j  i  k  (0)   i1      j1  i0 k1  i0      j1  j k1  j      j1  k k1  k      j1(0) k1(0)   [i1, j1, k1 ] in F  u(0)  R10 u(1)  k0  k1 z0 z  u  j1  j0 y  i0 O x0 x  y0 i1 Nguyen Quang Hoang-Department of Applied Mechanics-SME -17- -18- Tổng hợp phép quay Quay liên tiếp quanh trục hệ tọa độ hành TỔNG HỢP CÁC PHÉP QUAY F0  {Ox 0y 0z } : Rotx ( )  Roty 1( )  Rotz ( )  Roty ( ) : Fn  {Ox nyn z n }  Quay liên tiếp quanh trục hệ tọa độ hành  Quay liên tiếp quanh trục hệ tọa độ cố định F0  {Ox 0y 0z } R10 F1  {Ox 1y1z1 } R12     el  [il , jl , kl ]T , l  0,1,2   e  R10 e1,   e  R 20 e2 F2  {Ox 2y2z } R 20  R10R12 F0  {Ox 0y 0z } : Rotx ( )  Roty 1( )  Rotz ( )  Roty ( ) : Fn  {Ox nyn z n } Rn0  ?? F0  {Ox 0y 0z } : Rotx ( )  Roty 1( )  Rotz ( )  Roty ( ) : Fn  {Ox nyn z n } R  ?? F0  F1   Fn R 00  E, Rk0  Rk0 1Rkk 1, Rk0  R10R12  Rkk 1 n y y1 z y x0 Vị trí đầu x rot(x0,90) x Vị trí cuối z Ví dụ Tổng hợp quay liên tiếp quanh trục hành y Rotx (90)  Roty 1(90) : z z x0 Vị trí cuối Roty (90)  Rotx 1(90) : R 20  Roty (90)Rotx 1(90) y Vị trí đầu rot(x1,90) x z0 z1 x Nguyen Quang Hoang-Department of Applied Mechanics-SME  x1 z0 z1,2  R 20  Rotx (90)Roty 1(90) rot(y0,90) y R10 R12 F0  {Ox 0y 0z } F1  {Ox 1y1z } F2  {Ox 2y2z } F0  {Ox 0y 0z } : Roty ( )  Rotz 1( ) : F2  {Ox 2y2z } x0 z -20- Tổng hợp phép quay R 20  R10R12 rot(y1,90) z k  1,2, , n Nguyen Quang Hoang-Department of Applied Mechanics-SME -19- Quay liên tiếp quanh trục hệ tọa độ hành Việc tổng hợp phép quay liên tiếp khơng có tính chất giao hoán   e1  R12e2, Định lý Euler Tổng hợp chuyển động quay quanh hai hay nhiều trục giao chuyển động quay quanh trục qua điểm giao đó, với ma trận quay tổng hợp tích theo thứ tự quay ma trận quay thành phần Nguyen Quang Hoang-Department of Applied Mechanics-SME Tổng hợp phép quay Rn0  ?? z0 z1,2 y2 y2 y0, y1  x1   y1 x2 x1  y1 x2 R 20  R10R12  Ry , Rz ,  cos      sin   sin   cos     sin  cos      sin  cos  0   cos  cos    0   sin     sin  cos     cos  sin  cos  sin  sin  sin     cos    Nguyen Quang Hoang-Department of Applied Mechanics-SME -21- Tổng hợp phép quay Quay liên tiếp quanh trục hệ tọa độ cố định a) b) Vị trí đầu Z0 Y0 Vị trí đầu Y0 rot(x,90) X0 Vị trí cuối X0 Như thế, hai phép quay liên tiếp quanh trục x0 sau y0 tương đương bốn phép quay theo thứ tự sau: Z0 Z0 rot(y,90) Rx ( )  Rx ( )  Ry ( )  R x ( ) Ma trận quay từ hệ F0 sang hệ F2 tính sau Y0 X0 Giả sử cần tổng hợp hai phép quay: quay góc  quanh trục x0, sau quay góc  quanh trục y0, ta thực sau: rot(y,90) Y0 X0 Quay liên tiếp quanh trục hệ tọa độ cố định Z0 Z0 Z0 -22- Tổng hợp phép quay Y0 Y0 R20  R x ( )Rx ( )Ry ( )Rx ( )  Ry ( )R x ( ) rot(x,90) Vị trí cuối X0 Kết luận: Ma trận quay phép quay liên tiếp quanh trục cố định tính tương tự quay quanh trục hành theo thứ tự ngược lại Nguyen Quang Hoang-Department of Applied Mechanics-SME -23- Tổng hợp phép quay Kết luận: Ma trận quay phép quay liên tiếp quanh trục cố định tính tương tự quay quanh trục hành theo thứ tự ngược lại a) rot(y,90) Y0 X0 Ví dụ Xét hai phép quay liên tiếp: quay hệ trục F0 góc  quanh trục y0 hệ trục F1, sau quay tiếp F1 góc  quanh trục z0 để nhận hệ trục F2 Ma trận quay từ F0 sang F2 xác định sau: Z0 Z0 Z0 a) Quay liên tiếp quanh trục cố định: Vị trí đầu Y0 Y0 rot(x,90) X0 Vị trí cuối X0 b) z1 z2 z0 z1 z2 z0   y2 cố định   y0, y x1 x1  y2  y1  x1 x2  y1 x2 Z0 rot(y,90) z2 Y0 X0 z0 z1 Tổng hợp quay liên tiếp quanh trục x0 b) Quay liên tiếp quanh trục hành: rot(y,90) + rot(x,90) -24- Tổng hợp phép quay Quay liên tiếp quanh trục hệ tọa độ cố định Quay liên tiếp quanh trục hệ tọa độ cố định rot(x0,90) + rot(y0,90); Nguyen Quang Hoang-Department of Applied Mechanics-SME Vị trí đầu  R  R ( )[R ( )R ( )R ( )]  R R y y z y z , y , y rot(x,90) y2 z x Vị trí cuối x2 Nguyen Quang Hoang-Department of Applied Mechanics-SME cos     sin     sin  cos  0   cos  sin    cos  cos     0     sin  cos     sin  cos     sin      sin  cos  cos  sin    sin  sin   cos    Nguyen Quang Hoang-Department of Applied Mechanics-SME -25- • Định nghĩa góc Euler, thứ tự quay z-x-z (z0-x1-z2) THAM SỐ HÓA MA TRẬN QUAY  Các góc Euler Z-X-Z ma trận quay tương ứng  Các góc Cardan ma trận quay Cardan (XYZ – Angles) Các góc Roll-Pitch-Yaw  Trục quay góc quay  Các tham số Euler (Euler parameters) Xét hệ cố định F0 = {Ox0y0z0} hệ sau qua F1 = {Ox1y1z1} Để quay từ F0 sang F1, ta thực ba phép quay liên tiếp quanh trục hành sau: Rot(z0, ) 1.Quay quanh trục z0 góc  hệ {a}; Rot(xa, ) 2.Quay quanh trục xa góc  hệ {b}; Rot(zb, ) 3.Quay quanh trục zb góc  để hệ F1  k0 z1 z0  x0 z0,a zb  ya  k Rot(z0, ) Rot(xa, ) Rot(zb, ) z1 cos  0  0 1  yb  y  a  i0 y0   x0 xa,b  i x1 ya xa xa Các góc Euler Z-X-Z ma trận quay tương ứng c c  s c s  R E  s c  c c s  s s   O  sin   j y1 • Ma trận quay ứng với ba góc Euler, thứ tự quay z-x-z: cos  sin  1 0  cos       sin cos  0 cos   sin    sin   0  0 sin  cos       c c  s c s c s  s c c s s     s c  c c s s s  c c c c s   s s s c c    K • Xác định góc Euler từ ma trận cô sin hướng z0,a  R10  R z ( )R x ( )R z ( )   j0 Nguyen Quang Hoang-Department of Applied Mechanics-SME -27-  k0 y0  x Nguyen Quang Hoang-Department of Applied Mechanics-SME Các góc Euler Z-X-Z ma trận quay tương ứng yb  y  a y1  y b yb ya y0 xa     i0  j y1 O z0,a zb,1 z0,a  x0  Quay quanh trục z0 góc  hệ {a}; Quay quanh trục xa góc  hệ {b}; Quay quanh trục zb góc  để hệ F1  k Ba góc (, , ) ba góc Euler theo thứ tự quay Z-X-Z  -26- Các góc Euler Z-X-Z ma trận quay tương ứng K xa,b  j0  i x1 Nếu r33  1 c s  s c c s s  c c c s c r11 r12 r13  s s     c s   R  r21 r22 r23  r31 r32 r33  c     c  r33  s    r332   (r31 )2  (r32 )2    atan2(s , c )  atan2( (r31 )2  (r32 )2 , r33 ) Xác định góc  s s   r31 , s c  r32    atan2(r31 / s , r32 / s ) Xác định góc  s s  r13 , c s  r23    atan2(r13 / s , r23 / s ) Hai nghiệm, tùy theo dấu khai (+, -) Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -28- -29- Các góc Euler Z-X-Z ma trận quay tương ứng • Xác định góc Euler từ ma trận cô sin hướng c c  s c s  R E  s c  c c s  s s  • Xác định góc Euler từ ma trận cô sin hướng c s  s c c s s  c c c s c r11 r12 r13  s s     c s   R  r21 r22 r23  r31 r32 r33  c     Nếu r33   c c  s c s  R E  s c  c c s  s s  Nếu r33   c  r33   s     Ma trận cosin hướng trở thành Ma trận cosin hướng trở thành 0  0 1  c(   ) s(   )  r11 r12    R10  s(   ) c(   )   r21 r22  0 1  0      atan2(r31, r32 ) s c r11 r12 r13  s s     c s   R  r21 r22 r23  r31 r32 r33  c     Nếu r33  1 Nếu r33  1  0   chon       atan2(r21, r11 ) chon       atan2(r21, r11 )   atan2(r13 , r23 )  atan2(r12, r22 )  atan2(r12, r22 ) s s  c c c   atan2( (r31 )2  (r32 )2 , r33 ) 0  0 1      atan2(r21, r11 )      atan2(r , r ) 21 11 c s  s c c Nếu r33  1 c  r33  1  s      c(   ) s(   )  r11 r12    R10  s(   ) c(   )   r21 r22  0 1  0    -30- Các góc Euler Z-X-Z ma trận quay tương ứng Lời giải không xác định rõ  = hay  =  Đây điểm kỳ dị phương pháp Chỉ phương trình cho hai ẩn, nên có vơ số nghiệm Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -31- Các góc Euler Z-X-Z ma trận quay tương ứng • Bài tập: Xác định góc Euler từ ma trận cosin hướng sau: • Hàm atan2(y,x), atan2(si,co) y tan(x) R /2 -/2 y  tan(x )  x  arctan(y) -32- Các góc Euler Z-X-Z ma trận quay tương ứng   x  c c  s c s  R E  s c  c c s  s s  c s  s c c s s  c c c s c s s   c s  c   x  (x, y )    atan 2(y, x ),      Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -33- Các góc Cardan X-Y-Z ma trận quay tương ứng Xét hệ cố định F0 = {Ox0y0z0} hệ sau qua F1 = {Ox1y1z1} Để quay từ F0 sang F1, ta thực ba phép quay liên tiếp quanh trục hành sau: Rot(x0, ) Quay quanh trục x0 góc  hệ {a}; Rot(ya, ) Quay quanh trục ya góc  hệ {b}; Rot(zb, ) Quay quanh trục zb góc  hệ F1 za  z0 za ya  zb z0  y0 xa x0 yb  zb,1 x0 xb   c s c c  RC (q)  c s  s s c c c  s s s s s  c s c s c  c s s  y3     O K  yb x1 y1,2  x2   cos    sin   cos    sin   sin    cos     sin  cos      sin    c s c c   cos    c s  s s c c c  s s s  s s  c s c s c  c s s   s   s c  c c   Các góc Cardan X-Y-Z ma trận quay tương ứng Xác định góc Cardan từ ma trận cosin hướng Nếu r13  s  r13  c    r132   (r23 )2  (r33 )2    atan2(s , c )  atan2(r13 ,  (r31 )2  (r32 )2 ) c c  r11, c s   r12 s c  r23 , c c  r33     atan2(r12 / c , r11 / c )   atan2(r23 / c , r33 / c ) Hai nghiệm, tùy theo dấu khai (+, -) Nguyen Quang Hoang-Department of Applied Mechanics-SME  c s c c  RC (q)  c s  s s c c c  s s s s s  c s c s c  c s s  r11 r12 r13  s     s c   R  r21 r22 r23  r31 r32 r33  c c     Xác định góc   x3 R10  Rx ( )Ry ( )Rz ( ) 1    cos  sin  Nếu r13   y0 x0,1 y0 ya xb y1  z2,3 Nguyen Quang Hoang-Department of Applied Mechanics-SME -35- Các góc Cardan X-Y-Z ma trận quay tương ứng • Xác định góc Cardan từ ma trận cosin hướng sau: r11 r12 r13  s     s c   R  r21 r22 r23  r31 r32 r33  c c     R Nếu r13   s   c      / -34- Xác định góc Cardan từ ma trận cosin hướng z0 z1 Các góc Cardan X-Y-Z ma trận quay tương ứng s  1  c      /  0 1   R10   s(   ) c(   )   c(   ) s(   )     1   R10  s(   ) c(   )  c(   ) s(   )        atan2(r32 , r22 )   atan2(r21, r31 )      atan2(r21, r31 )   atan2(r32 , r22 )  trường hợp có vơ số nghiệm Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -36- -37- Các góc R-P-Y (roll-pitch-yaw) ma trận quay tương ứng Ma trận quay cịn mơ tả tích phép quay liên tiếp quanh trục cố định x0, y0, z0 với góc quay tương ứng , , , chúng gọi góc Roll-Pitch-Yaw (lưu ý có quy ước khác ba góc này) Ma trận quay tương ứng với ba phép quay liên tiếp quanh trục cố định tính tương đương phép quay quanh ba trục hành theo thứ tự ngược lại, nên:   x0 s c 0   c  0    s  s  1   0 c c  0 s    s  , c   c c c s s  s c c s c  s s  r r r     11 12 13  s c s s s  c c s s c  c s   r21 r22 r23   s  r r r  sc cc    31 32 33   y0 Nếu r31   s  r13  1, c    r132    atan2(r31,  r322  r332 ) Xác định góc   c c  r11, s c  r32 , c c c s s  s c c s c  s s    R1  s c s s s  c c s s c  c s   s  sc cc   s c  r21 c c  r33    atan2(r21, r11 )    atan2(r32 , r33 ) Hai nghiệm, tùy theo dấu khai (+, -) Nguyen Quang Hoang-Department of Applied Mechanics-SME Các góc R-P-Y (roll-pitch-yaw) ma trận quay tương ứng c c c s s  s c c s c  s s  r r r     11 12 13  s c s s s  c c s s c  c s   r21 r22 r23   s  r r r  sc cc    31 32 33  r31  Nếu s  1, c  0  R  0 1           / s(   ) c(   )  c(   ) s(   )  0   atan2(r12, r22 ),  atan2(r23, r13 ) Nguyen Quang Hoang-Department of Applied Mechanics-SME -39- Các góc R-P-Y (roll-pitch-yaw) ma trận quay tương ứng Xác định góc R-P-Y từ ma trận cosin hướng Xác định góc R-P-Y từ ma trận cosin hướng Nếu -38- Xác định góc R-P-Y từ ma trận cosin hướng Quay Roll-Pitch-Yaw quanh trục cố định R10  R z ( )Ry ( )R x ( ) c   s 0  z0 Các góc R-P-Y (roll-pitch-yaw) ma trận quay tương ứng R r31   s  1, 0  R 0  1       c     /2 c(   )   c(   ) s(   )  0   atan2(r12, r22 ), s(   )  atan2(r23, r13 ) tốn có vơ số nghiệm Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -40- -41- Tham số Euler – Euler parameters  u Xem xét phép quay quanh trục u góc  Sau phép quay điểm P0 chuyển đến điểm P1 Tính theo    u  (u  r ) P0 N x uT u   r  u z0  u véc tơ phương trục quay  r   r  P1 O y   u r  N P1 Q   u r  r    r   r  P1   E  uuT  uu     u  (u  r ) P0 N x Chiếu hệ cố định O    u  (u  r ) P Q  N P1 y   u r  r    u r  r O y0 x0 P0      r   OP1  ON  NQ  QP1,  ON  (r  u)u       NQ  u  (u  r ) cos      QP1  (u  r ) sin   P1   u z0    sin  u r  Ru, r r  uuT  cos  uu O N y0          r   (r  u )u  u  (u  r ) cos   (u  r ) sin           u(u  r )  cos  u  (u  r )  sin  (u  r ) Chiếu hệ cố định  r  u Xem xét phép quay quanh trục u góc  x0   (r , u,  )    u  (u  r ) P Q -42- Tham số Euler – Euler parameters   r  uuT r  cos  uur    sin  ur   uuT  cos  uu    sin  u r  R u , r  Ru ( )  E  sin  u  (1  cos  )uu Với u  [ux , uy , uz ]  T Nguyen Quang Hoang-Department of Applied Mechanics-SME Ru,  ux2 (1  c )  c   ux uy (1  c )  uzs u u (1  c )  u s y  x z ux uy (1  c )  uzs uy2 (1  c )  c uyuz (1  c )  ux s ux uz (1  c )  uys   uyuz (1  c )  uxs  uz2 (1  c )  c   Nguyen Quang Hoang-Department of Applied Mechanics-SME -43- Trục quay góc quay -44- Xác định trục quay u góc quay  từ R Ru,  ux2 (1  c )  c   ux uy (1  c )  uzs u u (1  c )  u s y  x z ux uy (1  c )  uzs u (1  c )  c y uyuz (1  c )  ux s ux uz (1  c )  uys  r11 r12 r13     uyuz (1  c )  ux s   r21 r22 r23  uz2 (1  c )  c  r31 r32 r33      r11  r22  r33    Tr (R)   1    cos  2     r32  r23    u r  r  , if   &    sin   13 31  r r  21 12    cos 1   r   r  P1 O : u véc tơ riêng ứng với trị riêng  = R  Biểu diễn chuyển động vật rắn  Tổng hợp chuyển động vật rắn  Ví dụ    u  (u  r ) P0 N x Nhận xét: trục u không thay đổi quay quanh u Ru  u  u z0 CHUYỂN ĐỘNG CỦA VẬT RẮN y   u r y0 x0 Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -45- Biểu diễn chuyển động vật rắn Xét chuyển động tổng quát vật rắn hệ quy chiếu cố định F0 Chuyển động tổng quát phân tích thành chuyển động điểm O1 thuộc vật chuyển động quay quanh điểm O1  k1 Như thế, chuyển động hệ vật hệ F0 mô tả cặp:  rO    [i1, j1, k1 ] &  k0 {F0} R10 (t )  [i1(0), j1(0), k1(0) ] rO(0)(t ) &  i0   u  O1P r (t )  r (t )  R (t )u (0) O1 (0) P  rO O  i1    {F2 }={i2 , j2 , k2 } R 12 (t )  [i(1) , j(1) , k(1) ] 2 &  rO O  u(1)  [u , u , u ]T P x y z  k2    rO O  rO O  rO O 1    rP /A  rO O  u    rP  rO O  u     rO O  rO O  u 1 {F2} rO(0)O  rO(0)O  rO(0)O r (0) O0O1  k0 R r (1) O1O2  rO O rP(1)/A  rO(1)O  R12u(2) P {F0}  i2  u O2  k1  rO O B   rO(0)  R10 rO(1)O  R12u(2) P 1   rO(0)  R10 rO(1)O  R10R12u(2) , P 1  i0  rO O  i1  j2 {F1} O1 rP(0)  rO(0)  R10 rO(1)O  R20u(2) P A  i1 A -48- Tay quay OAB quay quanh trục đứng, vật BC nối với AB lề trụ có trục nằm ngang Gọi  góc quay OAB  góc quay BC so với AB Biết OA = h, AB = b, BC = l Gắn hệ F1 = x1y1z1 vào vật OAB hệ F2 = x2y2z2 vào BC hình Hãy xác định: – Ma trận cosin hướng vật vật hệ cố định – Tọa độ điểm B C hệ cố định z1 z0 R  Rotz ,  sin  cos  0  0 , 1  R  Rotx ,90Rotz , 1 0  cos     0 1  sin  0      u(2)  [u (2), u (2), u (2) ]T P x y z Nguyen Quang Hoang-Department of Applied Mechanics-SME  cos  sin   sin  sin  cos  x2 l  b z2 B h x1 O cos     sin    C y2 A Để hệ F0 trùng song song với hệ cần thực hiện: Quay F0 quanh trục z0 góc , F0 trùng với F1 Quay F1 quanh trục x1 góc /2, sau quay tiếp quanh trục z2, F1 song song với F2 Như thế, ma trận quay sau: cos  cos   R20  R10R12   sin  cos   sin   {F1} Ví dụ A A  j1 O1 Lời giải  j1 O0 rP(0)  rO(0)  R10 rP(1) P  j2 B  rO O Nguyen Quang Hoang-Department of Applied Mechanics-SME -47- Chuyển động điểm O2 P thuộc vật B {F1} {F0}: O2  k1  rO O  i0 Nguyen Quang Hoang-Department of Applied Mechanics-SME P O0 R 20 (t )  [i(0) , j(0) , k(0) ] 2 &  i2  rO O Chuyển động tương đối B (hệ {F2}) so với hệ {F0} mô tả bởi:  rO O Tổng hợp chuyển động vật rắn  k0 {F0} R10 (t )  [i(0) , j(0) , k(0) ] 1 &  u {F2} Chuyển động tương đối A (hệ {F1}) so với hệ {F0} mô tả bởi: Vật rắn chuyển động không gian (1) P    {F1 }={i1, j1, k1 } Chuyển động tương đối B (hệ {F2}) so với A (hệ {F1}) mô tả bởi: O1  rO  j0 O0 Chuyển động điểm P thuộc vật:    rP  rO  u,  rP    {F0 }={i0, j , k }  j1  u  k2 Xét trường hợp có hai vật A B chuyển động hệ cố định F0 Gắn vào vật A B hệ trục tọa độ {F1} {F2} với véctơ đơn vị: {F1} P -46- Tổng hợp chuyển động vật rắn  x0  sin  cos  0 cos    0     sin     sin  cos  0  1 0  sin     cos     Nguyen Quang Hoang-Department of Applied Mechanics-SME -49- Ví dụ R  Rotz , cos     sin    cos  cos  0    , R20  R10R12   sin  cos   sin  1    sin  cos   cos  sin   sin  sin  cos  sin     cos     z1 z0 C y2  z2 h  x0 Vị trí điểm B C hệ cố định tính theo cơng thức sau: rB(0)   R10 u(1) , B cos   rB(0)   sin    rC(0)  rB(0)  R 20 u(2) B  sin  cos  0  b  b cos           b sin   ,  h   h       Tọa độ  Ma trận biến đổi tọa độ x1 O uC(2)  [l , 0, 0]T PHÉP BIẾN ĐỔI TỌA ĐỘ THUẦN NHẤT B Tọa độ điểm B C hệ tọa độ gắn liền vật sau: u(1)  [b, 0, h ]T , B x2 l b A -50- Ma trận biến đổi tọa độ Các ma trận quay ma trận tịnh tiến Tổng hợp chuyển động sử dụng ma trận biến đổi tọa độ Ví dụ (b  l cos  )cos     rC(0)  (b  l cos  )sin    h  l sin     Nguyen Quang Hoang-Department of Applied Mechanics-SME -51- Tọa độ z Tọa độ điểm P hệ quy chiếu véc tơ đại số phần tử  rP  rP(a )  [x P(a ), yP(a ), z P(a ) ]T yˆP(a ), rP(a ) ˆrP(a ) x P(0)  xˆ  , yP(0)  (0) P yˆ có điểm P  , z P(0)  P y {F1} x (0) P zˆ  ˆr (0) P r r (0) P (0) O u (0) O (1) r ,r ,u (0) r (0) O  , R u R  [x , y , z , 1] (a ) P (a ) P (a ) P T  [u , uy(1), uz(1), 1]T ˆrO(0)  [xO(0), yO(0), zO(0), 1]T Nguyen Quang Hoang-Department of Applied Mechanics-SME (1) 3  u  rP z0  rO {F0} O0 P y {F1} O y0 x x0 rO(0)  [xO , yO , zO ]T  ˆrO(0)  [xO , yO , zO ,1]T Tương tự có: (1) (1) x ˆ u    rP = rO + u ˆ(1)  [ux , uy , uz ,1]T u(1)  [ux , uy , uz ]T  u x0 (a ) P z Tại đưa vào tọa độ nhất? Chuyển qua tọa độ nhất: y0 -52- Phép biển đổi tọa độ O Chọn  = Quan hệ tọa độ thực tọa độ (0) P O0 zˆP(a ),  ]T Ký hiệu mũ để tọa độ => Ứng với  rO {F0}  rP  ˆrP(a )  [  x P(a ), yP(a ),  z P(a ),  ]T ,    u  rP z0 Tọa độ điểm P hệ quy chiếu véc tơ đại số phần tử định nghĩa sau:  [xˆP(a ), Nguyen Quang Hoang-Department of Applied Mechanics-SME rP(0)  [x P , yP , z P ]T  ˆrP(0)  [x P , yP , z P ,1]T Khi tọa độ điểm P tính sau:  rP(0)(t ) R(t ) rO(0)(t ) u(1)             ˆrP(0)  H(t )u ˆ(1),  R(t ) rO(0)(t ) H(t )      13  Nguyen Quang Hoang-Department of Applied Mechanics-SME -53- Phép biển đổi tọa độ z Ma trận biến đổi tọa độ (0) P ˆr ˆ ,  H(t )u (1)  R(t ) rO(0)(t ) H(t )      13   rO {F0} O0 H(t) gọi ma trận biến đổi tọa độ Ma trận chứa đựng đầy đủ thơng tin chuyển động vật rắn, chuyển động điểm O ma trận cosin hướng vật Như vậy, ta hoàn toàn biểu diễn chuyển động vật rắn thông qua ma trận biến đổi tọa độ H(t)  u  rP z0 P y {F1} O x y0 Phép biển đổi tọa độ Ma trận biến đổi tọa độ H rotx 1  cos    0 sin   0  cos  sin H rotz ( )      x0  sin  cos   sin cos 0 0  0 0  1   cos  sin     0 H roty ( )     sin  cos     0 1   0  0 0  1  1  H tran (ax ,by , cz )   0  0 R(t ) rO(0)(t ) H(t )   ,   01  A B  1 H 1     H H  E  A, B,C , D C D   1 0  cos 30  sin 30 H10   sin 30 cos 30  0 0 1  2 0  1  Nguyen Quang Hoang-Department of Applied Mechanics-SME -55- Bài tập: Cho ma trận H, vẽ hình hai hệ tọa độ tương ứng 0 ax   by  cz   0 1   H 1  ? Nguyen Quang Hoang-Department of Applied Mechanics-SME Phép biển đổi tọa độ Phép biển đổi tọa độ Bài tập: Cho ma trận H, vẽ hình hai hệ tọa độ tương ứng z0 F0 y0  cos 30  0 H1    sin 30   sin 30   2 cos 30   0 1  x0  cos 30 sin 30   sin 30 cos 30 H10   0  0  Nguyen Quang Hoang-Department of Applied Mechanics-SME -54- 1  2 0  1  z0 F0 y0 x0 z0 F0 x0 y0 Nguyen Quang Hoang-Department of Applied Mechanics-SME -56- -57- Phép biển đổi tọa độ • Bài tập: Cho hình vẽ, viết ma trận biến đổi tọa độ tương ứng  z0 F0 x1 x0 b Tìm phần tử khuyết ma trận sau: 1  ? H1   ?  ? F1 z1  y1 c y0 1  cos  H10   sin   0 0 1  ? 2 ? ? 0  0 1  0.8  ? H1   ?   0   sin  b  cos  c   1  Nguyen Quang Hoang-Department of Applied Mechanics-SME -59- Phép biển đổi tọa độ Ma trận biến đổi tọa độ nhất: chuỗi hệ tọa độ liên tiếp -60- Phép biển đổi tọa độ Ma trận biến đổi tọa độ nhất: chuỗi hệ tọa độ liên tiếp Trường hợp tổng quát: có hệ quy chiếu Fn, Fn-1, , F1, F0, điểm P thuộc hệ quy chiếu Fn, ta có: H Ri 1  i  rO(i 1)  i    (n  1) P ˆr H n 1 n ˆ u H nn 1 (n ) P zn ˆr i  (i ) i P i 1 i (i  1) i i 1 P i 1 i n 1 i i 1 n  H ˆr (0) P ˆr  H H H 1 (n ) n P ˆ Tu n 2 n 1 H T z0  H H ˆr  H H H i ˆ u (n ) P 1 R  R R R n xn 1 i 1 i R x0 O0 R Tn0   n  rO(0)  n    y0 Tn0 4 a2 Camera a1 n 1 n d3 z0 Tn0  H10 H12 Hnn 12 Hnn 1 x0 ˆ (Pn ) u n 1 n 2 yn P  u H ii  Camera z2 On (i  1) P 0.6   ? 2 ? ? 0  ? 1  Nguyen Quang Hoang-Department of Applied Mechanics-SME i 1 i -58- Phép biển đổi tọa độ O0 y0 y4 x4 z4 ˆrP(0)  Tn0 u ˆ (Pn ) Rn0  R10R12  Rii 1  Rnn 1 Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME -61- Phép biển đổi tọa độ Tay quay OAB quay quanh trục đứng, vật BC nối với AB lề trụ có trục nằm ngang Gọi  góc quay OAB  góc quay BC so với AB Biết OA = h, AB = b, BC = l Gắn hệ F1 = x1y1z1 vào vật OAB hệ F2 = x2y2z2 vào BC hình Hãy xác định: – Các ma trận biến đổi tọa độ H1 , H2, H2 – Tọa độ điểm B C hệ cố định z1 z0  B z2 h x1 O H12  Transz ,h Transx ,b Rotx ,90Rotz , cos     sin    ˆrB(0)  H10u ˆ(1) B H  Transz ,h Transx ,b Rotx ,90Rotz , 1  sin  cos  0 0 0  0 0  1  1     0 0 1 0 0  1   0 h  0   0  0 1 0 b  1  0 0 0 0   0  0 1 0 0 cos   0  sin  0   1    sin  cos  ˆ(1) u  [b, 0, h, 1]T , B  x0 Để hệ F0 trùng song song với hệ cần thực hiện: Quay F0 quanh trục z0 góc , F0 trùng với F1 Quay F1 quanh trục x1 góc /2, sau quay tiếp quanh trục z2, F1 song song với F2 Như thế, ma trận quay sau: cos   sin      x2 l b A Lời giải H  Rotz , C y2  sin  0   cos  0  0  0 1  ˆrC(0)  H20u ˆC(2) H20  H10H12 cos  cos   sin  cos    sin    b  1  ; h  1   cos  sin   sin  sin  cos  cos   sin       sin  cos  0 cos  cos   sin  cos    sin     cos  sin  sin   sin  sin  cos   cos  0 Hãy rằng:  H  H H z1 x1 x2 1m y0 z2 B h x1  x0 b cos    l  (b  l cos  )cos   (b  l cos  )cos       b sin   0  (b  l cos  )sin     (0)    r (b  l cos  )sin   C  h  0   h  l sin        h  l sin     1       Hãy viết phép biến đổi tọa độ Hk0 , k  1,2,3, Đưa ra: H23 x3 1,5 m y3 x0 z3 z2 y2 z1 y1 x2 x1 z0 x0 Nguyen Quang Hoang-Department of Applied Mechanics-SME x2  b A Phép biển đổi tọa độ z0 45 z2 l Nguyen Quang Hoang-Department of Applied Mechanics-SME 1m y2 C y2 Bài tập: Xét sơ đồ hình vẽ y1 Hãy viết phép biến đổi tọa độ H10 , H20, H12 z1 z0 O 0  b  b cos   b cos       0    b sin     (0)    r b sin   B  h   h   h        1 1        -63- Bài tập: Xét sơ đồ hình vẽ b cos     cos  b sin   h     sin  ˆC(2)  [l , 0, 0, 1]T u Nguyen Quang Hoang-Department of Applied Mechanics-SME Phép biển đổi tọa độ -62- Phép biển đổi tọa độ y0 0,8 m 1m Nguyen Quang Hoang-Department of Applied Mechanics-SME -64-