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)

Trang 1

Nguyen Quang Hoang-Department of Applied Mechanics-SME

Chương 3.

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

Nội dung

3.1 Biểu diễn vị trí 3.2 Biểu diễn chuyển động quay

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

3.4.1 Các góc Euler Z-X-Z và ma trận quay tương ứng 3.4.2 Các góc Cardan và ma trận quay Cardan (XYZ-Angles) 3.4.3 Các góc Roll-Pitch-Yaw

3.4.4 Trục quay và góc quay 3.4.5 Các tham số Euler (Euler parameters)

3.5.1 Biểu diễn chuyển động của vật rắn 3.5.2 Tổng hợp chuyển động của vật rắn 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

1

x0

d3

x4

z2

y0

z0

O0

a1

a2

z4

y4

Camera

-3-Cấu trúc

chuỗi

=

Chuỗi động

học mở

Cấu trúc lai

= Chuỗi động học đóng và mở

Cấu trúc song song = Chuỗi động học đóng

-4-Chuỗi:

Kuka-R15

Lai:

Tricept, ABB – IRB 940

Song song:

Delta-Roboter, ABB – Flexpicker

Trang 2

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

Biểu diễn vị trí

r



k 

{ } a

{ } b

/

P B r

P

O

Biểu diễn vị trí

x

y

z

x y

B

b

i 

b j



b

k 

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.

Có điểm P

Vị trí điểm P trong hqc {a}

( ) { }

a P

r   xi   yj   zk   e r 

Có hệ quy chiếu (hệ trục tọa độ, khung tọa độ) {a} và {b}:

a e  i j k   b e  i j k  

Vị trí điểm P trong hqc {b}

/

,

-7-Biểu diễn chuyển động quay/2D

Phép quay trong mặt phẳng

0

i 

0

j 

1

j



1

i 



Phép quay đơn

x 1

y 1

y 0



M





x

M

r

Xét chuyển động quay quanh z0

F =[ , , ], F =[ , , ]i j k   i j k  

Hướng của hệ F1 so với hệ F0, xác định bởi

Ma trận cosin chỉ hướng hay ma trận quay (quanh trục z)

Hướng của hệ F0 so với hệ F1, xác định bởi

1 (1) (1)

0 1

T



R

Tổng hợp hai phép quay quanh một trục

0

i 

0

j 

1

j 

1

i 



0

x 1

y 1

y 0





x

y 2



x 2



0=[ , ] ,i j0 0T 1=[ , ] ,i j1 1T 2=[ , ]i j2 2T

e   e   e  

0 1 1 1 2 2

0 1 2 2 2 2

,

e R e e R e

e R R e R e

2  1( ) ( )  2

2 0 2 0

cos( ) sin( )

( ) ( ) sin( ) cos( )

   

 

   

   

,

cos sin cos sin cos( ) sin( ) ( ) ( )

sin cos sin cos sin( ) cos( )

                              

Trang 3

Biểu diễn chuyển động quay/2D

Liên hệ giữa hai hệ véctơ đơn vị

0

i 

0

j 

1

j



1

i 



0

x 1

y 1

y 0



M





x

M

r

0=[ , ] ,i j0 0T 1=[ , ]i j1 1T

cos sin

sin cos

cos sin sin cos

 





        

1  0 0  ( 1)T 0 & 0  1 1

Liên hệ tọa độ của điểm ttrong hai hệ trục tọa độ

(1)

M

 



 

(0)

M

x

y

 

r e r e r  e R r e r

1

(0) (0)

cos sin sin cos

M M

x y

  

  



y0

x0

z0

{F0}

1

o

e 

3

o

e 

2

o

e 

a 



| |





x

y

z

a e a

0 1 0 2 0 3

cos cos cos









0 1 0 2 0 3

cos











Cosin chỉ hướng: cosin chỉ hướng của vec tơ trong một hệ quy chiếu F a  0:

là các cosin chỉ

hướng của vec tơ

trong một hệ quy chiếu F0:

a 

Ma trận cosin chỉ hướng

0

1 0 1 0 1 0 0

1 1 2 3 1 0 1 0 1 0

1 0 1 0 1 0 (0) (0) (0)

[ , , ]

[ , , ]in

i i j i k i

i j j j k j

i k j k k k

i j k

R r r r



 

0 0 0

{ , , } i j k   

O y

x

y0

x0

z0

O0

0

i 

0

k 

0

j 

1

i 

1

k 

1

j



{F1}

{F0}

1 1 1

{ , , } i j k   

hệ quy chiếu F1:

Ma trận cosin chỉ hướng của hệ trục tọa độ F1 đối với hệ F0

Ma trận cosin chỉ hướng của hệ trục tọa độ F0 đối với hệ F1

1

0 1 0 1 0 1 1 0 1 0 1 0

T

T in

Tính chất của ma trận cosin chỉ hướng

0

1 [ , , ]1 2 3 1 1 1  [ , , ] i j k1 1 1 inF

z O y

x

y0

x0

z0

O0

0

i 

0

k 

0

j



1

i 

1

k 

1

j 

{F1}

{F0}

R là ma trận trực giao

R có định thức bằng 1

R có một trị riêng bằng 1

3

1 2 3



0,

T

i j



  





r r

cụ thể

det( ) 1 R 

Trang 4

Liên hệ giữa hai hệ véc tơ đơn vị

0

1 [ , , ]1 2 3 1 1 1  [ , , ] i j k1 1 1 inF

z O y

x

y0

x0

z0

O0

0

i 

0

k 

0

j 

1

i 

1

k 

1

j



{F1}

{F0}

Ta có các phương trình sau:

0  [ , , ] i j k  0 0 0T



e e 1 [ , , ] i j k  1 1 1T

0 11 0 21 0 31 0 1

,

T

 

       



Gộp lại được

0

1 0[ , , ]1 2 3 0 1

e e r r r e R

0

Liên hệ giữa hình chiếu một véc tơ trong hai hệ trục

Trong hệ trục F1:

0  [ , , ] i j k  0 0 0T



e e 1  [ , , ] i j k  1 1 1T

(1) 1 (1) (1) (1) (1)

, [ , , ]

T

x y z



e u u



0

O y

x

y0

x0

z0

O0

0

i 

0

k 

0

j



1

i 

1

k 

1

j 

{F1}

{F0}

u 

Trong hệ trục F0:

(0) 0 (0) (0) (0) (0)

, [ , , ]

T

x y z



e u u



u   e u   e u   e Ru  (0) (1) 0 (1)

1

Như vậy, ma trận cosin chỉ hướng R cho phép biến đổi

hình chiếu của một véctơ (hay tọa độ của một điểm) 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)

from



Các ma trận quay cơ bản

( ) ( , )

0 cos sin

0 sin cos

o

x  rot xo 



R

y0

x0, x

z0

z



y0, y

x0

z0 z



 x

y0

x0

z0,z



 y

x

( ) ( , ) cos 0 sin

sin 0 cos

o

y  rot yo 



o

z  rot zo



R

0

1 [ , , ]1 2 3 1 1 1  [ , , ] i j k1 1 1 inF

Tính chất sau đây được nghiệm đúng

k    k  k  x y z

Tóm lại, ma trận quay thể hiện ba ý nghĩa hình học tương đương sau:

• 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 trục của hệ sau khi quay đối với hệ ban đầu.

• 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.

• Cho phép tính tọa độ của một véc tơ khi nó quay quanh gốc tọa độ.

1



z

O y

x 1

i 

u 

1

j 

1

k 

y0

x0

z0

0

i 

0

k 

0

j 

0

1 0 1 0 1 0 0

1 1 2 3 1 0 1 0 1 0

1 0 1 0 1 0 (0) (0) (0)

[ , , ]

[ , , ]in

i i j i k i

i j j j k j

i k j k k k

i j k

R r r r



 

Trang 5

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

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

0 { 0 0 0} : x0( ) y1( ) z2( ) y3( ) : n { n n n}

0 { 0 0 0} : x0( ) y1( ) z0( ) y0( ) : n { n n n}

0 ??

n

0 ??

n

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

0 { 0 0 0}

F  Ox y z

0 ??

n

1 { 1 1 1}

F  Ox y z

0 { 0 0 0} : x0( ) y1( ) z2( ) y3( ) : n { n n n}

[ , , ] ,T 0,1,2

l  i j kl l l l 

e   



2 { 2 2 2}

F  Ox y z

0

2  1 2

0



Đị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

1 2

n k

k





R R R  R

-19-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

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

x 0

x

z

y

y 1

rot(x 0 ,90)

rot(y 1 ,90)

Vị trí đầu

Vị trí cuối

x

x 0

z

z y

y y

rot(y 0 ,90)

rot(x 1 ,90)

Vị trí đầu

Vị trí cuối

0

R

0

R

Ví dụ Tổng hợp quay liên tiếp quanh trục hiện hành

y0,

y1

x0

z0

z1



x1

y1

z0

z1,2



x1 x2

y2

z0

z1,2



x1 x2

y2



0 { 0 0 0} : y0( ) z1( ) : 2 { 2 2 2}

F  Ox y z Rot   Rot  F  Ox y z F0{Ox y z0 0 0} F1{Ox y z1 1 1} F2{Ox y z2 2 2}

0

2 1 2

cos 0 sin cos sin 0 cos cos cos sin sin

y z

R R 

R R R

Trang 6

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

X 0

Y 0

rot(x,90)

rot(y,90)

Y 0

Z 0

X 0

Y 0 a)

rot(y,90)

rot(x,90)

Vị trí đầu

Vị trí cuối

Z 0

Y 0 b)

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:

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:

Ma trận quay từ hệ F0 sang hệ F2 được tính như sau

x  x   y  x

0 x( ) x() y( ) x( )  y( ) x( )

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.

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.

X 0

Y 0

rot(x,90)

rot(y,90)

Y 0

Z 0

X 0

Y 0 a)

Z 0

rot(y,90)

rot(x,90)

Vị trí đầu

Vị trí cuối

z 2

y 2 b)

2

a) Quay liên tiếp quanh trục cố định:

rot(x0,90) + rot(y0,90);

b) Quay liên tiếp quanh trục hiện hành:

rot(y,90) + rot(x,90)

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:

y0, y

x0

z0

z1



x1

y1

z0

z1



x1 x2

y2

z0

z1



x1 x2

y2





0

, ,

y  y  z y  R R z y

Tổng hợp quay liên tiếp quanh trục

cố định

Trang 7

THAM SỐ HÓA MA TRẬN QUAY

 Các góc Euler Z-X-Z và ma trận quay tương ứng

 Các góc Cardan và ma trận quay Cardan (XYZ – Angles)

 Các góc Roll-Pitch-Yaw

 Trục quay và góc quay

 Các tham số Euler (Euler parameters)

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)

j



k 

i 

y0

z0,a

x0



 O

a,b

ya

yb









z1

x1

y1

0

i 

0

j



0

k 

Xét hệ cố định F0 = {Ox0y0z0} và hệ sau khi qua F1 = {Ox1y1z1} Để 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:

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

2.Quay quanh trục x a góc  được hệ {b}; Rot(xa, )

3.Quay quanh trục z b góc  để được hệ F1 Rot(zb, )

Ba góc (, , ) là ba góc Euler theo thứ tự quay Z-X-Z

y0

x0

z0



xa

ya

z0,a

zb



xa

yb



ya

z0,a

zb,1



xa

yb

x1

y1



-27-Các góc Euler Z-X-Z và ma trận quay tương ứng

j 

k 

i 

y0

z0,a

x0



 O

a,b

ya

yb









z1

x1

y1

0

i 

0

j 

0

k 

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

2 Quay quanh trục x a góc  được hệ {b}; Rot(xa, )

3 Quay quanh trục z b góc  để được hệ F1 Rot(zb, )

• Ma trận quay ứng với ba góc Euler, thứ tự quay z-x-z:

sin cos 0 0 cos sin sin cos 0

  

 

           



• Xác định các góc Euler từ ma trận cô sin chỉ hướng

11 12 13

21 22 23

31 32 33

E

           

s s    r s c    r    r s r  s 

s s    r  c s    r    r s   r s 

Xác định góc 

Xác định góc 

c r s  r   r  r 

atan2( ,s c ) atan2( ( )r ( ) ,r r )

Hai bộ nghiệm, tùy theo dấu khai căn (+, -)

Trang 8

11 12 13

21 22 23

31 32 33

E

           

Nếu r33   1

Ma trận cosin chỉ hướng trở thành

11 12 0

21 22

   

R

21 11

12 22

atan2( , )

r r

  

Chỉ một phương trình trên cho hai ẩn, nên có vô số nghiệm

Nếu r33   1

c r   s    

Ma trận cosin chỉ hướng trở thành

11 12 0

21 22

   

R

21 11

12 22

atan2( , )

r r

  

11 12 13

21 22 23

31 32 33

E

           

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

31 32 atan2( , r r )

 

13 23

Nếu r33   1

31 32 33

atan2( ( ) r ( ) , r r )

Nếu r33   1

0



21 11

0 atan2( , )

chon

r r







 

Nếu r33   1

 

21 11

0 atan2( , )

chon

r r







 

• Hàm atan2(y,x), atan2(si,co)

/2

y tan(x)

tan( ) arctan( )

( , ) x y  atan 2( , ), y x

  

 

  

• Bài tập: Xác định 3 góc Euler từ ma trận cosin chỉ hướng sau:

           

R

Trang 9

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} Để

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:

ya

z0

za 

x0

y0



y0

z0

zb,1

x0

yb

x1

y1



ya

zb

xa

yb



za





R

y0

z0

x0,1

x3

y3

y1,2

z1





x2



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

11 12 13

21 22 23

31 32 33

( )

C

           

Xác định góc  và 

s r c  r   r  r 

atan2( ,s c ) atan2( ,r ( )r ( ) )r

-35-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

11 12 13

21 22 23

31 32 33

( )

C

           

Nếu r13  1

trường hợp này có vô số nghiệm

0

R

32 22

21 31

atan2( , )

r r

 

  





Nếu r13   1

s  c     0

R

21 31

32 22

atan2( , )

r r

 





-36-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 sau:

R

Trang 10

Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng



0

R

y0

x0

z0







Quay Roll-Pitch-Yaw quanh các trục cố định

Ma trận quay còn có thể được mô tả là tích các phép quay liên tiếp

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

nhau về ba góc này)

Ma trận quay tương ứng với ba phép quay liên tiếp quanh các trục cố

định được tính tương đương phép quay quanh ba trục hiện hành nhưng

theo thứ tự ngược lại, nên:

Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng

21 22 23

31 32 33

Xác định các góc R-P-Y từ ma trận cosin chỉ hướng

Xác định góc  và 

s  r   c   r 

-39-Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng

11 12 13

21 22 23

s    c       

0

12 22

23 13

atan2( , ), atan2( , ).

r r

   

 





R

0

12 22

23 13

atan2( , ), atan2( , ).

r r

   

 

  





R

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

-40-Các góc R-P-Y (roll-pitch-yaw) và ma trận quay tương ứng

R

Tiêu đề	Chuyển động của vật rắn và phép biến đổi tọa độ thuần nhất
Tác giả	Nguyễn Quang Hoàng
Trường học	Đại học Bách Khoa Hà Nội
Chuyên ngành	Cơ học ứng dụng
Thể loại	Bài giảng
Thành phố	Hà Nội

Định dạng
Số trang	16
Dung lượng	1 MB