Chương 3.: Chuyển Động Của Vật Rắn Và Phép Biến Đổi Tọa Độ Thuần Nhất

-2-
Nội dung
3.1 Biểu diễn vị trí
3.2 Biểu diễn chuyển động quay
3.3. Tổng hợp các phép quay
3.3.1. Quay hệ trục tọa độ hiện hành Camera
Chương 3. 3.3.2. Quay liên tiếp quanh các trục cố định z2
2 4
CHUYỂN ĐỘNG CỦA VẬT RẮN VÀ 3.4. Tham số hóa ma trận quay 1 a2
3.4.1. Các góc Euler Z-X-Z và ma trận quay tương ứng Camera
PHÉP BIẾN ĐỔI TỌA ĐỘ THUẦN NHẤT 3.4.2. Các góc Cardan và ma trận quay Cardan (XYZ-Angles)
a1
Rigid body motion and homogeneous 3.4.3.

3.4.4.
Các góc Roll-Pitch-Yaw
Trục quay và góc quay
d3
transformations 3.4.5. Các tham số Euler (Euler parameters)

z0
y0 x4
3.5. Chuyển động của vật rắn O0
y4 z4
x0
3.5.1. Biểu diễn chuyển động của vật rắn
Nguyễn Quang Hoàng 3.5.2. Tổng hợp chuyển động của vật rắn
Email: hoang.nguyenquang@hust.edu.vn
3.6. Tọa độ thuần nhất và phép biến đổi tọa độ thuần nhất
3.6.1. Tọa độ thuần nhất
3.6.2. Ma trận biến đổi tọa độ thuần nhất
Nguyen Quang Hoang-Department of Applied Mechanics-SME Nguyen Quang Hoang-Department of Applied Mechanics-SME
-3- -4-
Biểu diễn vị trí
Có hệ quy chiếu (hệ trục tọa độ, khung tọa độ) {a} và {b}: z
 
      kb {b } jb
BIỂU DIỄN VỊ TRÍ VÀ HƯỚNG 
{a } : e{a }  [i , j , k ]T , {b} :

e{b }  [ib , jb , kb ]T
P 
rP /B

Có điểm P.  r B
 Biểu diễn vị trí Vị trí điểm P trong hqc {a} k z 
 ib
 Biểu diễn phép quay 2D      {a } i
O j
r  xi  yj  zk  eT{a }rP(a ) y
 Biểu diễn phép quay 3D x
x y
Vị trí điểm P trong hqc {b}
     Biểu diễn vị trí
rP /B  x (b )ib  y (b ) jb  z (b )kb  eT{b }rP(b/)B
x P(a )  x P(b ) 
Nếu có nhiều hệ quy chiếu (nhiều hệ trục tọa độ), cần ghi    
rõ vị trí của điểm trong hệ trục tọa độ nào. r (a )
P
 yP(a )  , r(b )
P /B
 yP(b ) 
z (a )  z (b ) 
 P   P 
-5- -6-
Biểu diễn chuyển động quay/2D Biểu diễn chuyển động quay/2D
Phép quay trong mặt phẳng y0
 Tổng hợp hai phép quay quanh một trục
 j0 y0 
j1 x2  j0
Xét chuyển động quay quanh z0. M           j1
y1 y x1  e 0 =[i0 , j 0 ]T , e1 =[i1, j1 ]T , e 2 =[i2 , j2 ]T
i1 y1  
        x1
i1
F0 =[i0 , j 0 , k 0 ], F1 =[i1, j1, k1 ] rM   
 
 i0     y2  
Hướng của hệ F1 so với hệ F0, xác định bởi e 0  R10 e1, e1  R12 e2 
   i0
    O x x0  e 0  R10 R12 e2  R 20 e2 R  R ( )R ( )
0
2
0
1
1
2
 i1  i0 j  i   cos   sin   O x x0
R 10   i(0) j(0)    
 1 0    
1 1 
i j j1  j 0   sin  cos  
 1 0    Phép quay đơn
 cos  sin    cos  sin  
R10   , R12   
Ma trận cosin chỉ hướng hay ma trận quay (quanh trục z)  sin  cos    sin  cos  
   
Hướng của hệ F0 so với hệ F1, xác định bởi  cos  sin    cos  sin   cos(   )  sin(   )
R 10  (R 10 )T R10 ( )R12 ( )  
 sin 

cos     sin 
 
cos    sin(   ) cos(   ) 
        
 i0  i1 j  i   cos  sin  
R   i(1)     0 1    (R 10 )T  (R 10 )1    
 i  j 
1
j
(1)
00 0
j 0  j1    sin  cos   i  i j  i   cos(   )  sin(   )
 0 1    det(R 10 )  1 R 20   i(0) j(0)    2 0 2 0      R10 ( )R12 ( )
2 2 
i j j2  j 0   sin(   ) cos(   ) 
 2 0   
-7- -8-
 
Liên hệ giữa hai hệ véctơ đơn vị y0  j0 Cosin chỉ hướng: cosin chỉ hướng của vec tơ a trong một hệ quy chiếu F0:
      j1
e 0 =[i0 , j 0 ]T , e1 =[i1, j1 ]T M 
y1 y x1  
    
i1 a  |a | z0

i1  cos  i0  sin  j 0  rM       a
   

 a  a xe10  a ye20  a ze 30 e 3o 
j1   sin  i0  cos  j 0  i0
     
 i1   cos  sin    i0  O x x0  (a cos  )e10  (a cos  )e20  (a cos  )e 30
         
j  sin  cos    j 0 
 1     
    x   e2o y0
     
r  xi0  yj 0  [i0 j 0 ]    eT0 rM(0)    {F0}
e1  R10 e 0  (R10 )T e 0 & e 0  R10 e1 y ax e10  a e1o x0
  cos    là các cosin chỉ
a 
Liên hệ tọa độ của điểm ttrong hai hệ trục tọa độ     a  hướng của vec tơ a
   ay e20  a
 cos   e 0  a

r   i1   j1  [i1 j1 ]    eT1 rM(1) cos    trong một hệ quy
     
 
r  eT1 rM(1)  eT0 rM(0)  eT0 R 10 rM(1)  eT0 rM(0) a a  chiếu F0:   1
 
az e 30  a | a | 1   cos   e20  a
x M(0)   cos   sin     cos   
 (0)      a a  cos   e 0  a
r (0)
M
Rr 0 (1)
1 M y
 M  
sin  cos    
 
 3
-9- -10-
{F1} {F1}
Ma trận cosin chỉ hướng Tính chất của ma trận cosin chỉ hướng
    y  y
hệ quy chiếu F0: { i0 , j 0 , k 0 }
   k1 z j1    k1 z j1
hệ quy chiếu F1: { i1, j1, k1 }
R  R10  [ r1, r2, r3 ]  i1(0) j(0) k(0)   [i1, j1, k1 ] in F
  1 1  0 
Ma trận cosin chỉ hướng của hệ trục tọa độ F1 đối với hệ F0. z0 O i1 z0 O i1
       x R là ma trận trực giao  x
 i1  i0 j1  i0 k1  i0  k0 k0
       
R10  [ r1, r2, r3 ]  i1  j 0 j1  j 0 k1  j 0  i0 
{F0}
R 1  RT , RRT  RT R  E i0 
{F0}
    
i  k j1  k 0 k1  k 0 
O0 j 0 y0 1, i  j

O0 j 0 y0
 1 0    
x0 R  [ r1 r2 r3 ] cụ thể riT  rj   x0

 i1 (0)
j(0) k1(0)   [i1, j1, k1 ] in F 0, i  j
 1  0
R có một trị riêng bằng 1
Ma trận cosin chỉ hướng của hệ trục tọa độ F0 đối với hệ F1.
            T u  3 Ru  u
 i0  i1 j 0  i1 k 0  i1   i1  i0 j1  i0 k1  i0 
                 R có định thức bằng 1
R10  [i0, j0 , k 0 ] in F  i(1) j(1) k(1)   i0  j1 j 0  j1 k 0  j1   i1  j 0 j1  j 0 k1  j 0   (R10 )T
0 0 
          
i  k
0
det(R)  1
1
j 0  k1 k 0  k1  i1  k 0 j1  k 0 k1  k 0 
0 1   
-11- -12-
        
Liên hệ giữa hai hệ véc tơ đơn vị e 0  [i0 , j 0 , k 0 ]T e1  [i1, j1, k1 ]T {F1} Liên hệ giữa hình chiếu một véc tơ trong hai hệ trục u {F1}
 y              y
   k1 z j1 e 0  [i0 , j 0 , k 0 ]T e1  [i1, j1, k1 ]T e 1  RT e 0 , e 0  R 10 e 1 k1 z j1
R  R10  [ r1, r2, r3 ]  i1(0) j1(0) k1(0)   [i1, j1, k1 ] in F
  0  
z0 O i1 Trong hệ trục F1: Trong hệ trục F0: z0 O i1
Ta có các phương trình sau:  x
       x
k0   k0
          u  u x(1)i1  u y(1) j1  u z(1)k1 u  u x(0)i0  u y(0) j 0  u z(0)k 0
i1  (i1  i0 )i0  (i1  j 0 ) j 0  (i1  k 0 )k 0  {F0}    {F0}
    i0   eT1 u (1),  eT0 u (0), i0 
 i0r11  j 0r21  k 0r31  eT0 r1, O0 j 0 y0 u (1)  [u x(1), u y(1), u z(1) ]T u (0)  [u x(0), u y(0), u z(0) ]T
O0 j 0 y0
    x0 x0
j1  eT0 r2 , k 1  eT0 r3
   
Gộp lại được u  eT0 u (0)  eT1 u (1)  eT0 Ru (1) u (0)  Ru (1)  R 10 u (1)
  
eT1  eT0 [r1, r2 , r3 ]  eT0 R 10
Như vậy, ma trận cosin chỉ hướng R cho phép biến đổi
u (to )
R to
u ( from ) hình chiếu của một véctơ (hay tọa độ của một điểm)
   
 e 1  RT e 0 , e 0  R 10 e1 from
trong hai hệ trục tọa độ cùng gốc. Từ ý nghĩa đó, ma
trận cosin chỉ hướng R còn được gọi là ma trận quay
(Rotation matrix).
-13- -14-
  
Các ma trận quay cơ bản R  R 10  [ r1, r2 , r3 ]   i 1(0) j1(0) k 1(0)   [i1, j1, k1 ] in Tóm lại, ma trận quay thể hiện ba ý nghĩa hình học tương đương sau:
  F0
• Mô tả hướng giữa hai hệ trục tọa độ; các véc tơ cột là các cô sin chỉ hướng của các
z0 z0,z
trục của hệ sau khi quay đối với hệ ban đầu.
z z z0
 • Cho phép biến đối tọa độ của một điểm trong hai hệ quy chiếu cùng chung gốc.
 y
y • Cho phép tính tọa độ của một véc tơ khi nó quay quanh gốc tọa độ.
  
y0 y0, y

y0 k0
       
x0, x
x0 x x0 x  i1  i0 j1  i0 k1  i0  u
       z0
R10  [ r1, r2, r3 ]  i1  j 0

j1  j 0 k1  j 0 
    k1 
R xo ( )  rot (x o ,  ) R yo ( )  rot (yo ,  ) R zo ( )  rot (z o , ) i  k j1  k 0 k1  k 0 
z j1
1 0   cos  sin    cos   sin  0  1 0  y

0 0    j0
      
 i1 (0)
j1(0) k1(0)   [i1, j1, k1 ] in F
 0 cos   sin    0 1 0    sin  cos  0  
 y0
0
0 sin  cos     sin  0 cos    0 0 1  O

      i0 x0 x i1
Tính chất sau đây được nghiệm đúng
Rk ( )  RkT ( ), k  x , y, z u(0)  R10 u(1)
-15- -16-
Tổng hợp các phép quay
Quay liên tiếp quanh các trục hệ tọa độ hiện hành
TỔNG HỢP CÁC PHÉP QUAY F0  {Ox 0y 0z 0 } : Rotx 0 ( )  Roty 1( )  Rotz 2 ( )  Roty 3 ( ) : Fn  {Ox nyn z n } Rn0  ??
   
 Quay liên tiếp quanh các trục hệ tọa độ hiện hành R10 R12 el  [il , jl , kl ]T , l  0,1,2
 Quay liên tiếp quanh các trục hệ tọa độ cố định F0  {Ox 0y 0z 0 } F1  {Ox 1y1z1 } F2  {Ox 2y2z 2 }    
e 0  R10 e1, e1  R12e2,
 
R 20  R10R12 e 0  R 20 e2
F0  {Ox 0y 0z 0 } : Rotx 0 ( )  Roty 1( )  Rotz 2 ( )  Roty 3 ( ) : Fn  {Ox nyn z n } Rn0  ??
F0  F1  ...  Fn
R 00  E, Rk0  Rk0 1Rkk 1, k  1,2,..., n
Rk0  R10R12  Rkk 1
F0  {Ox 0y 0z 0 } : Rotx 0 ( )  Roty 1( )  Rotz 0 ( )  Roty 0 ( ) : Fn  {Ox nyn z n } Rn0  ??
Định lý Euler
Tổng hợp chuyển động quay quanh hai hay nhiều trục giao nhau được chuyển động quay quanh một trục
đi qua điểm giao đó, với ma trận quay tổng hợp bằng tích theo thứ tự quay các ma trận quay thành phần.
-17- -18-
Tổng hợp các phép quay Tổng hợp các phép quay
Quay liên tiếp quanh các trục hệ tọa độ hiện hành Ví dụ. Tổng hợp quay liên tiếp quanh trục hiện hành R10 R12
Việc tổng hợp các phép quay liên tiếp không có tính chất giao hoán. F0  {Ox 0y 0z 0 } F1  {Ox 1y1z 1 } F2  {Ox 2y2z 2 }
F0  {Ox 0y 0z 0 } : Roty 0 ( )  Rotz 1( ) : F2  {Ox 2y2z 2 }
y R 20  R10R12
y1 rot(y1,90)
z
z0 z0 z0
z Rotx 0 (90)  Roty 1(90) : z1 z1,2 z1,2
y x 
R 20  Rotx 0 (90)Roty 1(90) y2 y2
Vị trí cuối
x0 z
Vị trí đầu rot(x0,90)  
x y0, y1 y1
 y1
x0 x1  
x1 x2 x1 x2
z
rot(y0,90)
y z Vị trí cuối R 20  R10R12  Ry , Rz ,
y Roty 0 (90)  Rotx 1(90) :
 cos  0 sin   cos   sin  0   cos  cos   cos  sin  sin  
z
y R 20  Roty 0 (90)Rotx 1(90)     
x0 Vị trí đầu rot(x1,90)
 0 1 0   sin  cos  0   sin  cos  0 .
x   sin  0 cos    0 0 1    sin  cos  sin  sin  cos  
x     
-19- -20-
Quay liên tiếp quanh các trục hệ tọa độ cố định Quay liên tiếp quanh các trục hệ tọa độ cố định
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 hiện như sau:
Z0 Z0
Z0
a)
rot(y,90)
Y0 Y0 Y0
X0 rot(x,90) X0 Vị trí cuối

Vị trí đầu X0
Như thế, hai phép quay liên tiếp quanh trục x0 sau đó là y0 tương đương bốn phép quay theo thứ tự sau:
Z0 Z0 Z0
b) Rx ( )  Rx ( )  Ry ( )  R x ( )
rot(y,90)
Ma trận quay từ hệ F0 sang hệ F2 được tính như sau
Y0 Y0 Y0
R20  R x ( )Rx ( )Ry ( )Rx ( )  Ry ( )R x ( )
X0 rot(x,90) Vị trí cuối
X0 Vị trí đầu
Kết luận: Ma trận quay của các phép quay liên tiếp quanh các trục cố định được tính tương tự như quay quanh các
trục hiện hành nhưng theo thứ tự ngược lại.
-21- -22-
Quay liên tiếp quanh các trục hệ tọa độ cố định Quay liên tiếp quanh các trục hệ tọa độ cố định
Kết luận: Ma trận quay của các phép quay liên Ví dụ. Xét hai phép quay liên tiếp: quay hệ trục F0 góc  quanh trục y0 được hệ trục F1, sau đó quay tiếp F1
góc  quanh trục z0 để nhận được hệ trục F2. Ma trận quay từ F0 sang F2 được xác định như sau:
tiếp quanh các trục cố định được tính tương tự như
Z0 Z0
quay quanh các trục hiện hành nhưng theo thứ tự Z0
a)
rot(y,90) z2 z2
ngược lại. z0 z0 z0
z1 z1 z1
Tổng hợp quay
Y0 Y0 Y0 
a) Quay liên tiếp quanh trục cố định:
liên tiếp quanh trục 
y2 y2
rot(x0,90) + rot(y0,90); X0 X0 Vị trí cuối cố định
Vị trí đầu rot(x,90)
X0
y0, y  y1  y1
 
x0 x1  
x1 x2 x1 x2
b) Z0
rot(y,90)
z2
R 0  R ( )[R ( )R ( )R ( )]  R R
2 y y z y z , y ,
Y0 y
cos   sin  0   cos  0 sin    cos  cos   sin  cos  sin  
b) Quay liên tiếp quanh trục hiện hành:     
rot(x,90)
rot(y,90) + rot(x,90) X0 Vị trí đầu
z y2   sin  cos  0  0 1 0    sin  cos  cos  sin  sin   .
Vị trí cuối  0 0 1    sin  0 cos     sin  0 cos  
x x2     
-23- -24-
Các góc Euler Z-X-Z và ma trận quay tương ứng

• Định nghĩa các góc Euler, thứ tự quay z-x-z (z0-x1-z2)  k0
THAM SỐ HÓA MA TRẬN QUAY Xét hệ cố định F0 = {Ox0y0z0} và hệ sau khi qua F1 = {Ox1y1z1}. Để
k z0,a 
z1
quay từ F0 sang F1, ta thực hiện ba phép quay liên tiếp quanh các y1 j
trục hiện hành như sau:
1.Quay quanh trục z0 góc  được hệ {a}; Rot(z0, ) 
 yb
 Các góc Euler Z-X-Z và ma trận quay tương ứng 2.Quay quanh trục xa góc  được hệ {b}; Rot(xa, ) 
3.Quay quanh trục zb góc  để được hệ F1. Rot(zb, ) y
 Các góc Cardan và ma trận quay Cardan (XYZ – Angles)  a 
O j0
 Các góc Roll-Pitch-Yaw Ba góc (, , ) là ba góc Euler theo thứ tự quay Z-X-Z. y0
 
Trục quay và góc quay 
 x0
K
i x1
 Các tham số Euler (Euler parameters)  xa,b
z0 z0,a zb,1 z0,a i0
zb
 y1
yb  y
b
ya 
 ya ya
 y0
x0 xa xa xa  x
1
-25- -26-
Các góc Euler Z-X-Z và ma trận quay tương ứng Các góc Euler Z-X-Z và ma trận quay tương ứng

 k0 • Xác định các góc Euler từ ma trận cô sin chỉ hướng
1. Quay quanh trục z0 góc  được hệ {a}; Rot(z0, )

k z0,a 
z1 c c  s c s c s  s c c s s  r11 r12 r13 
2. Quay quanh trục xa góc  được hệ {b}; Rot(xa, ) y1 j    
3. Quay quanh trục zb góc  để được hệ F1. Rot(zb, ) R E  s c  c c s s s  c c c c s   R  r21 r22 r23 
 
 yb s s s c c  r31 r32 r33 
   

• Ma trận quay ứng với ba góc Euler, thứ tự quay z-x-z: y
 a 
O j0 Nếu r33  1 c  r33  s   (r31 )2  (r32 )2  0
y0
R  R z ( )R x ( )R z ( )
0
1  
   atan2(s , c )  atan2( (r31 )2  (r32 )2 , r33 )
cos  sin 0  1 0 0  cos   sin  0 x0 i x1
    K Xác định góc 
  sin cos 0  0 cos   sin    sin  cos  0
 xa,b
i0 s s   r31 , s c  r32    atan2(r31 / s , r32 / s )
 0 0 1  0 sin  cos    0 0 1
   
c c  s c s c s  s c c s s  Xác định góc 
 
 s c  c c s s s  c c c c s  s s  r13 , c s  r23    atan2(r13 / s , r23 / s )
 s s s c c 
  Hai bộ nghiệm, tùy theo dấu khai căn (+, -)
-27- -28-
• Xác định các góc Euler từ ma trận cô sin chỉ hướng • Xác định các góc Euler từ ma trận cô sin chỉ hướng
c c  s c s c s  s c c s s  r11 r12 r13  c c  s c s c s  s c c s s  r11 r12 r13 
       
R E  s c  c c s s s  c c c c s   R  r21 r22 r23  R E  s c  c c s s s  c c c c s   R  r21 r22 r23 
 s s s c c  r31 r32 r33   s s s c c  r31 r32 r33 
       
Nếu r33   1 Nếu r33   1

c  r33  1  s  0    0 c  r33  1  s  0     Nếu r33  1 Nếu r33  1 Nếu r33  1
Ma trận cosin chỉ hướng trở thành Ma trận cosin chỉ hướng trở thành  0  
c(   ) s(   ) 0  r11 r12
  atan2( (r31 )2  (r32 )2 , r33 )
0 c(   ) s(   ) 0  r11 r12 0 chon   0
        chon   0
R10  s(   ) c(   ) 0   r21 r22 R10  s(   ) c(   ) 0   r21 r22
 0 0 1  0 0
0
1  0 0 1  0 0
0
1
  atan2(r31, r32 )     atan2(r21, r11 )     atan2(r21, r11 )
       
    atan2(r , r )     atan2(r21, r11 )   atan2(r13 , r23 )

21 11
 atan2(r12, r22 )  atan2(r12, r22 )

Lời giải không xác định rõ khi  = 0 hay  = . Đây là điểm kỳ dị của phương pháp
Chỉ một phương trình trên cho hai ẩn, nên có vô số nghiệm.
-29- -30-
• Hàm atan2(y,x), atan2(si,co) • Bài tập: Xác định 3 góc Euler từ ma trận cosin chỉ hướng sau:
y tan(x)
-/2 /2 x
 
y  tan(x )  x  arctan(y)  x 
2 2
(x, y )    atan 2(y, x ),
    
-31- -32-
Các góc Cardan X-Y-Z và ma trận quay tương ứng Các góc Cardan X-Y-Z và ma trận quay tương ứng
Xét hệ cố định F0 = {Ox0y0z0} và hệ sau khi qua F1 = {Ox1y1z1}. Để Xác định 3 góc Cardan từ ma trận cosin chỉ hướng
z1 z0
quay từ F0 sang F1, ta thực hiện ba phép quay liên tiếp quanh các
trục hiện hành như sau:  c c c s s  r11 r12 r13 
   
1. Quay quanh trục x0 góc  được hệ {a}; Rot(x0, ) y3 RC (q)  c s  s s c c c  s s s s c   R  r21 r22 r23 
z2,3 
2. Quay quanh trục ya góc  được hệ {b}; Rot(ya, ) s s  c s c s c  c s s c c  r31 r32 r33 
    
3. Quay quanh trục zb góc  được hệ F1. Rot(zb, )
 K
za 
z0 za z0 O
 y1,2 Nếu r13   1 s  r13  c   1  r132   (r23 )2  (r33 )2  0
ya y1 
zb  zb,1
 yb
yb y0   atan2(s , c )  atan2(r13 ,  (r31 )2  (r32 )2 )

y0 x0,1
y0 ya 
  x1 Xác định góc  và 
x0 xa xb x0
xb x2 x3
c c  r11, c s   r12    atan2(r12 / c , r11 / c )
R10  Rx ( )Ry ( )Rz ( )
1
s c  r23 , c c  r33    atan2(r23 / c , r33 / c )
0 0   cos  0 sin    cos   sin  0   c c c s s 
     
  0 cos  sin   0 1 0   sin  cos  0   c s  s s c c c  s s s s c 
Hai bộ nghiệm, tùy theo dấu khai căn (+, -)
 0 sin cos    sin  0 cos    0 0 1  s s  c s c s c  c s s c c 
     
-33- -34-
Các góc Cardan X-Y-Z và ma trận quay tương ứng Các góc Cardan X-Y-Z và ma trận quay tương ứng
Xác định 3 góc Cardan từ ma trận cosin chỉ hướng • Xác định 3 góc Cardan từ ma trận cosin chỉ hướng sau:
 c c c s s  r11 r12 r13 
   
RC (q)  c s  s s c c c  s s s s c   R  r21 r22 r23 
s s  c s c s c  c s s

c c 

r31 r32 r33 
 
R
Nếu r13  1 Nếu r13   1

s  1  c  0     / 2 s  1  c  0     / 2
 0 0 1  0 0 1
   
R10   s(   ) c(   ) 0  R10  s(   ) c(   ) 0 
 c(   ) s(   ) 0  c(   ) s(   ) 0 
   
    atan2(r32 , r22 )     atan2(r21, r31 )

 
  atan2(r21, r31 )  atan2(r32 , r22 )

trường hợp này có vô số nghiệm
-35- -36-
Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng
Ma trận quay còn có thể được mô tả là tích các phép quay liên tiếp z0 Xác định các góc R-P-Y từ ma trận cosin chỉ hướng

quanh các trục cố định x0, y0, z0 với các góc quay tương ứng là , , ,
chúng được gọi là góc Roll-Pitch-Yaw (lưu ý rằng có các quy ước khác c c c s s  s c c s c  s s  r r r 
nhau về ba góc này).    11 12 13 
  s c s s s  c c s s c  c s   r21 r22 r23 
Ma trận quay tương ứng với ba phép quay liên tiếp quanh các trục cố  s  r r r 
sc cc
định được tính tương đương phép quay quanh ba trục hiện hành nhưng    31 32 33 
theo thứ tự ngược lại, nên: y0
x0
Quay Roll-Pitch-Yaw quanh Nếu r31   1 s  r13  1, c   1  r132  0

R10  R z ( )Ry ( )R x ( )
các trục cố định
c s 0   c 0 s  1 0 0    atan2(r31,  r322  r332 )
   
 s c 0  0 1 0  0 c s  ,
0 0 1   s 0 c  0 s c  Xác định góc  và 
   
c c  r11, s c  r21    atan2(r21, r11 )
c c c s s  s c c s c  s s 
  s c  r32 , c c  r33    atan2(r32 , r33 )
R10  s c s s s  c c s s c  c s 
 s sc cc 
  Hai bộ nghiệm, tùy theo dấu khai căn (+, -)
-37- -38-
Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng
Xác định các góc R-P-Y từ ma trận cosin chỉ hướng Xác định các góc R-P-Y từ ma trận cosin chỉ hướ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 sc cc  r r r 
   31 32 33  R
Nếu r31  1 Nếu r31   1
s  1, c  0     / 2 s  1, c  0    /2
0 s(   ) c(   ) 0 s(   ) c(   ) 

   
R  0
0
1
c(   ) s(   ) R 0
0
1
c(   ) s(   )
1 0 0   1 0 0 
   
    atan2(r12, r22 ),     atan2(r12, r22 ),
 
  atan2(r23, r13 ).   atan2(r23, r13 ).
bài toán có vô số nghiệm

-39- -40-
Tham số Euler – Euler parameters Tham số Euler – Euler parameters
 
Xem xét phép quay quanh trục u góc  u Xem xét phép quay quanh trục u góc  u
  N
  N
 z0 u z0 u
u là véc tơ chỉ phương của trục quay.    Q      Q  
x P0 N u  (u  r )    u r Chiếu trong hệ cố định được x P0 N u  (u  r )    u r
uT u  1  u  (u  r ) P P1  u  (u  r ) P P1
r  0 r  0
     
r r
 
y r y r
r  P1    r  uuT  cos  uu
   sin  u r  Ru, r r  P1   
Sau phép quay điểm P0 chuyển u r u r
đến điểm P1. O O
y0 N y0
O
Q uuT  uu
 E  O

x0 x0
   P0
Tính r theo (r , u,  ) được P1      Ru ( )  E  sin  u  (1  cos  )uu

         r   OP1  ON  NQ  QP1,
r   (r  u )u  u  (u  r ) cos   (u  r ) sin  
ON  (r  u)u
         
 u(u  r )  cos  u  (u  r )  sin  (u  r )    
 ux2 (1  c )  c ux uy (1  c )  uzs ux uz (1  c )  uys 
NQ  u  (u  r ) cos   
Chiếu trong hệ cố định được Với u  [ux , uy , uz ] 
T
Ru,  ux uy (1  c )  uzs uy2 (1  c )  c uyuz (1  c )  uxs 
    u u (1  c )  u s uyuz (1  c )  ux s uz2 (1  c )  c 
r  uuT r  cos  uur
   sin  ur 
  uuT  cos  uu
   sin  u r  R u , r  QP1  (u  r ) sin 
  x z y 
-41- -42-
Trục quay và góc quay
Xác định trục quay u và góc quay  từ R

 ux2 (1  c )  c ux uy (1  c )  uzs ux uz (1  c )  uys  r11 r12 r13 
   
Ru,  ux uy (1  c )  uzs u (1  c )  c
2
y
uyuz (1  c )  ux s   r21 r22 r23 
u u (1  c )  u s uyuz (1  c )  ux s uz2 (1  c )  c  r31 r32 r33 
 x z y    CHUYỂN ĐỘNG CỦA VẬT RẮN
 Biểu diễn chuyển động của vật rắn
r  r  r  1  Tr (R)  1 
  cos 1  11 22 33   cos 1     Tổng hợp chuyển động của vật rắn
2 2
z0 u
      
r32  r23  x P0 N u  (u  r )
 Ví dụ
1   r  
u r  r  , if   0 &     y
2 sin   13 31  r  P1  
r r u r
 21 12 
O y0
Nhận xét: trục u không thay đổi khi quay quanh u.
Ru  u : u là véc tơ riêng ứng với trị riêng  = 1 của R.

x0
-43- -44-
Biểu diễn chuyển động của vật rắn Tổng hợp chuyển động của vật rắn

Xét chuyển động tổng quát của vật rắn đối với hệ quy chiếu cố định F0. Xét trường hợp có hai vật A và B chuyển động đối với hệ cố định F0. k2
Chuyển động tổng quát này có thể được phân tích thành chuyển động  Gắn vào vật A và B các hệ trục tọa độ {F1} và {F2} với các véctơ đơn vị:
của điểm O1 thuộc vật và chuyển động quay quanh điểm O1. k1 {F1}         
{F2}
 P
P {F0 }={i0, j 0 , k 0 } {F1 }={i1, j1, k1 } {F2 }={i2 , j2 , k2 } u
Như thế, chuyển động của hệ vật đối với hệ F0 được mô tả bởi cặp:    
    u j1 k0 i2 O2
  Chuyển động tương đối của B (hệ {F2}) so với A (hệ {F1}) mô tả bởi:  
rO & [i1, j1, k1 ] k0 rP rO O
  
B j2
R (t )  [i , j , k ]
1
{F0} rO O & 1 (1) (1) (1) 0 2
rO O
 O1 1 2 2 2 2 2 {F0} k1
(0)
r (t ) & R (t )  [i , j , k ]
0 (0) (0) (0)
O0 rO 
1 2

O1 1 1 1 1
 i1 Chuyển động tương đối của A (hệ {F1}) so với hệ {F0} mô tả bởi: O0 j1
 j0  {F1}
Chuyển động của điểm P thuộc vật:   rO O O1
i0 rO O & R10 (t )  [i(0) , j(0) , k(0) ] 0 1
1 1 1 i0 A
 
0 1
  
rP  rO  u, u  O1P Vật rắn chuyển động không gian Chuyển động tương đối của B (hệ {F2}) so với hệ {F0} mô tả bởi:  A
1
 i1
rO O & R (t )  [i , j , k ]
0
2
(0)
2
(0)
2
(0)
2
0 2
rP(0)(t )  rO(0)(t )  R10 (t )u(1) u(1)  [u , u , u ]T
1 P P x y z
-45- -46-
Tổng hợp chuyển động của vật rắn Ví dụ
 Tay quay OAB quay được quanh trục đứng, vật BC nối với AB bằng bản lề trụ
Chuyển động của điểm O2 và P thuộc vật B đối với {F1} và {F0}: k2 có trục nằm ngang. Gọi  là góc quay của OAB và  là góc quay của BC so với z1 y2
C x2
l
  
{F2}
 P
AB. Biết OA = h, AB = b, BC = l. z0
rO O  rO O  rO O rO(0)O  rO(0)O  rO(0)O u Gắn hệ F1 = x1y1z1 vào vật OAB và hệ F2 = x2y2z2 vào BC như trên hình. Hãy
xác định:
b 
0 2 0 1 1 2 0 2 0 1 1 2
 A
r (0)
R r
0 (1)  – Ma trận cosin chỉ hướng của vật 2 đối với vật 1 và đối với hệ cố định. B
O0O1 1 O1O2 k0 i2 O2  – Tọa độ các điểm B và C trong hệ cố định.
z2
h
 j2
rO O B
   0 2   Lời giải x1
rP /A  rO O  u rP(1)/A  rO(1)O  R12u(2)
P
{F0} k1 rO O O

1 2 1 2 1 2
 Để hệ F0 về trùng hoặc song song với các hệ cần thực hiện:
O0 j1 Quay F0 quanh trục z0 góc , F0 trùng với F1. x0
Quay F1 quanh trục x1 góc /2, sau đó quay tiếp quanh trục z2, F1 sẽ song song với F2.
   
rP  rO O  u rP(0)  rO(0)  R10 rP(1) rO O O1
{F1} Như thế, các ma trận quay sẽ như sau:
  
 
0 2 1
 rO O  rO O  u  0 1
A cos  0 1 0 0  cos  0 cos  0
0 1 1 2
 rO(0)  R10 rO(1)O  R12u(2)
P i0 
 sin 
  
 sin 
 
 sin 

1 1 2
 A R10  Rotz ,   sin  cos  0 , R12  Rotx ,90Rotz ,  0 0 1  sin  cos  0   0 0 1
 rO(0)  R10 rO(1)O  R10R12u(2), i1  0
1 1 2 P 0 1 0 1 0   0 0 1   sin  cos  0
      
rP(0)  rO(0)  R10 rO(1)O  R20u(2)
P
cos  cos   cos  sin  sin  
1 1 2  
R20  R10R12   sin  cos   sin  sin   cos  
u(2)  [u (2), u (2), u (2) ]T  sin  cos  0 
P x y z  
-47- -48-
Ví dụ
cos   sin  0 cos  cos   cos  sin  sin   z1 C x2

    y2
l
R10  Rotz ,   sin  cos  0  , R20  R10R12   sin  cos   sin  sin   cos   z0
 0   sin  0  

0 1
 
cos 
 A
b
PHÉP BIẾN ĐỔI TỌA ĐỘ THUẦN NHẤT
z2 B
h
 Tọa độ thuần nhất
Tọa độ các điểm B và C trong các hệ tọa độ gắn liền vật như sau:
x1
O
u(1)
B
 [b, 0, h ]T , uC(2)  [l , 0, 0]T   Ma trận biến đổi tọa độ thuần nhất
x0
1. Ma trận biến đổi tọa độ thuần nhất
Vị trí các điểm B và C trong hệ cố định được tính theo các công thức sau:
2. Các ma trận quay cơ bản thuần nhất và ma trận tịnh tiến thuần nhất
3. Tổng hợp chuyển động sử dụng ma trận biến đổi tọa độ thuần nhất
rB(0)  0  R10 u(1)
B
, rC(0)  rB(0)  R 20 u(2)
B
4. Ví dụ
cos   sin  0  b  b cos   (b  l cos  )cos  

      
rB(0)   sin  cos  0   0   b sin   , rC(0)  (b  l cos  )sin  
 0 0 1  h   h   h  l sin  
      
-49- -50-
Tọa độ thuần nhất Phép biển đổi tọa độ thuần nhất
Tọa độ điểm P trong một hệ quy chiếu là véc tơ đại số 3 phần tử z Tại sao đưa vào tọa độ thuần nhất? z
P y P y
     
rP  rP(a )  [x P(a ), yP(a ), z P(a ) ]T u rP = rO + u r (0)
r (0)
u (0)
r (0)
R u 0 (1)
u
z0  {F1} P O O 1
z0  {F1}
rP rP
Tọa độ thuần nhất của điểm P trong một hệ quy chiếu là véc tơ đại số 4
(0)
r ,r ,u
P
(0)
O
(1)
 , 3
R   3 3
phần tử định nghĩa như sau:  O  O
{F0} rO Chuyển qua tọa độ thuần nhất: {F0} rO

rP  ˆrP(a )  [  x P(a ), yP(a ),  z P(a ),  ]T ,   0 x x
O0 y0 u(1)  [ux , uy , uz ]T  u
ˆ(1)  [ux , uy , uz ,1]T O0 y0
x0 x0
 [xˆP(a ), yˆP(a ), zˆP(a ),  ]T rO(0)  [xO , yO , zO ]T  ˆrO(0)  [xO , yO , zO ,1]T
Ký hiệu mũ để chỉ tọa độ thuần nhất. Chọn  = 1 rP(0)  [x P , yP , z P ]T  ˆrP(0)  [x P , yP , z P ,1]T
=> Ứng với mỗi rP(a ) hoặc ˆrP(a ) có duy nhất một điểm P.
(a )
ˆr
P  [x , y , z , 1]
(a )
P
(a )
P
(a )
P
T
Khi đó tọa độ thuần nhất của điểm P được tính như sau:
Tương tự có:
Quan hệ giữa tọa độ thực và tọa độ thuần nhất
ˆ
u(1)
 [u , uy(1), uz(1), 1]T
(1)  rP(0)(t ) R(t ) rO(0)(t ) u(1)   R(t ) rO(0)(t )

x
xˆ (0)
yˆ (0) (0)
zˆ    ˆrP(0)  H(t )u
ˆ(1), H(t )   
x P(0)  P
, yP(0)  P
, z P(0)  P
ˆrO(0)  [xO(0), yO(0), zO(0), 1]T  1   0 1  1   013 1 

    
-51- -52-
Phép biển đổi tọa độ thuần nhất Phép biển đổi tọa độ thuần nhất
Ma trận biến đổi tọa độ thuần nhất z Ma trận biến đổi tọa độ thuần nhất cơ bản
P y
 1 0 0 0  cos  0 sin  0 
 R(t ) rO(0)(t ) u    
 {F1} 0 cos   sin  0 0 1 0 0
 H(t )u H(t )   z0  H roty ( )  
(0) (1)
ˆr ˆ ,  rP H rotx
0 sin  cos  0   sin  0 cos  0 
 013 1 
P
 
   
O 0 0 0 1
  0 0 0 1

{F0} rO
x cos  sin 0 0 1 0 0 ax 
H(t) được gọi là ma trận biến đổi tọa độ thuần nhất. Ma trận này O0 y0
   
chứa đựng đầy đủ thông tin về chuyển động của vật rắn, đó là sin cos 0 0 0 1 0 by 
x0 H rotz ( )   H tran (ax ,by , cz )  
chuyển động của điểm O và ma trận cosin chỉ hướng của vật.  0 0 1 0 0 0 1 cz 
Như vậy, ta hoàn toàn biểu diễn được chuyển động của vật rắn    
thông qua ma trận biến đổi tọa độ thuần nhất H(t).  0 0 0 1
 0 0 0 1

R(t ) rO(0)(t )
H(t )   ,  H 1  ?
 01 3 1 
 A B 
H 1   1
  H H  E  A, B,C , D
C D
 
-53- -54-
Bài tập: Cho ma trận H, hãy vẽ hình hai hệ tọa độ tương ứng Bài tập: Cho ma trận H, hãy vẽ hình hai hệ tọa độ tương ứng
1 0 0 1 z0 z0
   cos 30 0 sin 30 1 
0 cos 30  sin 30 2  
H10   H10  
0 1 0 2
F0
0 sin 30 cos 30 0 F0  sin 30 0 cos 30 0 
  y0  
0 0 0 1 0 0 0 1
  
x0 y0
x0
z0
 cos 30 sin 30 0 1 F0
 
 sin 30 cos 30 0 2
H1  
0
0 0 1 0
  y0

0 0 0 1
 x0
-55- -56-
Bài tập: Cho hình vẽ, hãy viết ra các ma trận biến đổi tọa độ thuần nhất tương ứng. • Tìm các phần tử khuyết của các ma trận sau:
F1 1 0 0 1
z1   0.8 0 0.6 1 
 ? 0 ? 2  
H1  
0
? 1 ? 2
z0  y1 ? ? ? 0 H1  
0
  ? ? ? 0
? 0 0 1  
F0 x1 c   0 ? 0 1

x0 b y0 1 0 0 0
 
0 cos   sin  b 
H10  
0 sin  cos  c 
 
0 0 0 1

-57- -58-
Ma trận biến đổi tọa độ thuần nhất: chuỗi các hệ tọa độ liên tiếp Ma trận biến đổi tọa độ thuần nhất: chuỗi các hệ tọa độ liên tiếp
Trường hợp tổng quát: có các hệ quy chiếu Fn, Fn-1, ..., F1, F0, điểm P thuộc hệ quy chiếu Fn, ta có: Camera
z2
zn 2 4
Ri 1 rO(i 1)  H nn 1 P yn
H i 1
 i  (n  1)
ˆr H n 1
ˆ
u (n )  1 a2
u
i
Camera
i
 0 1  P n P H ii  1 a1
 R  R R R
0 0 1 i 1
R n 1
n 1 2 i n
On d3
z0
(i  1)
ˆr  H ˆr i  1 (i ) Ti0 xn
P i P z0 Tn0  H10 H12 ...Hnn 12 Hnn 1 x4
i 1 i (i  1)
 H H ˆr
i i 1 P
O0 y0
y4 z4
x0
i 1 i n 1
 H H ...H ˆ (Pn )
u
i i 1 n Tn0 ˆrP(0)  Tn0 u
ˆ (Pn )
y0
x0 O0
n 2 n 1
(0)
ˆr
P
 H H ...H0 1
1 2 n 1
H n
ˆ
u (n )
P
R 0 rO(0) 
Tu
ˆ 0 (n )
n P Tn0   n n
 Rn0  R10R12  Rii 1  Rnn 1
 0 1 

-59- -60-
Tay quay OAB quay được quanh trục đứng, vật BC nối với AB bằng bản lề C H12  Transz ,h Transx ,b Rotx ,90Rotz , H20  H10H12 C
z1 x2 z1 x2
trụ có trục nằm ngang. Gọi  là góc quay của OAB và  là góc quay của BC y2 y2
z0 l cos   sin  0 b cos  cos   cos  sin  sin  b cos   z0 l
so với AB. Biết OA = h, AB = b, BC = l.    
 0 0 1 0  sin  cos   sin  sin   cos  b sin   
   
Gắn hệ F1 = x1y1z1 vào vật OAB và hệ F2 = x2y2z2 vào BC như trên hình. b b
;
Hãy xác định: A sin  cos  0 h sin  cos  0 h  A
0 1 0 z2 B     z2 B
– Các ma trận biến đổi tọa độ thuần nhất H1 , H2, H2 h  0 0 0 1  0 0 0 1  h
 
– Tọa độ các điểm B và C trong hệ cố định.
x1 x1
O ˆ(1)
u  [b, 0, h, 1]T , ˆC(2)  [l , 0, 0, 1]T
u O
Lời giải 
B

Để hệ F0 về trùng hoặc song song với các hệ cần thực hiện: x0 x0
Quay F0 quanh trục z0 góc , F0 trùng với F1.
cos   sin  0 0  b  b cos  
     b cos  
Quay F1 quanh trục x1 góc /2, sau đó quay tiếp quanh trục z2, F1 sẽ song song với F2. sin  cos  0 0   0  b sin    
ˆrB(0)  H10u
ˆ(1)     rB  b sin  
(0)
Như thế, các ma trận quay sẽ như sau: B
0 0 1 0  h   h 
      h 
 0 0 0 1 1   1   
H10  Rotz , H12  Transz ,h Transx ,b Rotx ,90Rotz ,    
cos   sin  0 0 1 0 0 0  1 0 0 b  1 0 0 0 cos   sin  0 0  cos  cos 
        cos  sin  sin  b cos    l  (b  l cos  )cos  
sin  cos  0 0 0 0 1 0  0 0 1 0 0 0 1 0  sin  cos  0 0       (b  l cos  )cos  
     
sin  cos   sin  sin   cos  b sin   0  (b  l cos  )sin    
0 h  0 0 0 0  0 1 0 ˆrC(0)  H20u
ˆC(2)   r  (b  l cos  )sin  
(0)
0 0 1 0 1 0 1 0 1 0 0 sin  cos  0 h  0   h  l sin   C
             h  l sin  
 0 0 0 1
 0 0 0 1  0

0 0 1  0

0 0 1  0

0 0 1
 0 0 0 1  1   1  
    
-61- -62-
Bài tập: Xét sơ đồ như trên hình vẽ. y1 Bài tập: Xét sơ đồ như trên hình vẽ.
Hãy viết ra các phép biến đổi tọa độ thuần nhất H10 , H20, H12 z1 Hãy viết ra các phép biến đổi tọa độ thuần nhất Hk0 , k  1,2,3,..
Hãy chỉ ra rằng: H 0  H 0 H1 Đưa ra: H23
2 1 2 x1
1m
x3
y3 z3
z0
y2 1,5 m
45 x0 z2
x2 y0
z2 1m y2
z1 y1 x2
x1
z0
y0
x0 0,8 m 1m

Chương 3.: Chuyển Động Của Vật Rắn Và Phép Biến Đổi Tọa Độ Thuần Nhất

Uploaded by

Copyright:

Available Formats

You might also like

Chương 3.: Chuyển Động Của Vật Rắn Và Phép Biến Đổi Tọa Độ Thuần Nhất

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Chương 3.: Chuyển Động Của Vật Rắn Và Phép Biến Đổi Tọa Độ Thuần Nhất

Uploaded by

Copyright:

Available Formats

-2-

Rigid body motion and homogeneous 3.4.3.

transformations 3.4.5. Các tham số Euler (Euler parameters)

0 sin  cos     sin  0 cos    0 0 1  O

Quay liên tiếp quanh các trục hệ tọa độ hiện hành

X0 rot(x,90) X0 Vị trí cuối

1. Quay quanh trục z0 góc  được hệ {a}; Rot(z0, )

Nếu r33   1 Nếu r33   1

    atan2(r , r )     atan2(r21, r11 )   atan2(r13 , r23 )

 atan2(r12, r22 )  atan2(r12, r22 )

Nếu r13  1 Nếu r13   1

    atan2(r32 , r22 )     atan2(r21, r31 )

Quay Roll-Pitch-Yaw quanh Nếu r31   1 s  r13  1, c   1  r132  0

0 s(   ) c(   ) 0 s(   ) c(   ) 

bài toán có vô số nghiệm

Xác định trục quay u và góc quay  từ R

Ru  u : u là véc tơ riêng ứng với trị riêng  = 1 của R.

cos   sin  0 cos  cos   cos  sin  sin   z1 C x2

cos   sin  0  b  b cos   (b  l cos  )cos  

You might also like