2.1 Mô t v trí, h ng, h quy chi u 2.1 Mô t v trí, h ng, h quy chi u 2.

1 Mô t v trí, h ng, h quy chi u

Mô t v trí m t i m P trong h quy chi u A b ng Mô t h ng c a m t v t th Các ph n t trong ma tr n quay còn g i là các cosin
vector v trí A P px py pz
T trong h quy chi u A (không k ch ph ng. L u ý BA R BAR 1 AB RT .
v trí) b ng cách g n c ng v i v t
A
P px Xˆ A pyYˆA p zZˆ A Ví d B có c b ng cách quay A m t góc
m t h quy chi u B r i mô t 3
quanh tr c xA, y A ho c zA thì các ma tr n quay là:
Robot công nghi p vector n v c a B theo h quy
chi u A b ng ma tr n quay:
A
1 0 0 c s 0
A
r11 r12 r13 B Rx 0 c s B Rz s c 0
A ˆ
A
BR X B AYˆB A ZˆB r21 r22 r23 0 s c 0 0 1
r31 r32 r33
Ch ng 2: Mô t không gian và c 0 s
Xˆ B Xˆ A YˆB Xˆ A ZˆB Xˆ A B
Xˆ TA
bi n i h quy chi u A
B Ry 0 1 0
Xˆ B YˆA YˆB YˆA ZˆB YˆA B
YˆAT s 0 c
Xˆ B Zˆ A YˆB Zˆ A Zˆ ZˆB A
B ˆT
Z A
Ch ng 2: Mô t không gian và bi n i h quy chi u 2 Ch ng 2: Mô t không gian và bi n i h quy chi u 3 Ch ng 2: Mô t không gian và bi n i h quy chi u 4

2.1 Mô t v trí, h ng, h quy chi u 2.2 Bi n i h quy chi u 2.2 Bi n i h quy chi u 2.2 Bi n i h quy chi u

Mô t m t h quy chi u (g m c h ng và v trí) Bi n i t a c a m t i m khi chuy n gi a các N u các h quy N u k t h p

A A
trong m t h quy chi u khác: B B R, PBORG
h quy chi u. chi u ch là quay c a c t nh ti n và quay:
N u các h quy chi u ch là t nh ti n c a nhau nhau: A
P APBORG BAR BP
n gi n hóa vi c ph i h p ma tr n và vector
này ta a ra khái ni m ma tr n bi n i ng nh t
A
P APBORG B P P Xˆ Yˆ Zˆ BP
B B B mô t phép
A A bi n i này g n
A
T B R PBORG Xˆ A YˆA Zˆ A A
R BP
B
B gàng b ng m t ma
0 0 0 1 A A
P R BP
B
tr n s d ng ma
L u ý BAT B
A T 1 B
A TT tr n bi n i ng
nh t ABT :
A A A B B
P B R PBORG P A P
T
B
1 0 0 0 1 1 1
A
BT

Ch ng 2: Mô t không gian và bi n i h quy chi u 5 Ch ng 2: Mô t không gian và bi n i h quy chi u 6 Ch ng 2: Mô t không gian và bi n i h quy chi u 7 Ch ng 2: Mô t không gian và bi n i h quy chi u 8

2.3 Các toán t 2.3 Các toán t 2.3 Các toán t 2.4 Tính ch t phép bi n i h quy chi u

Liên quan n vi c t nh Toán t quay m t vector quanh tr c K m t góc : Toán t bi n i t ng quát g m K t h p nhi u
ti n và quay vector trong m t A
P2 RK A
P1 c t nh ti n và quay: gi ng ma tr n phép bi n i h quy
h quy chi u. bi n i ng nh t chi u:
L u ý ma tr n RK quay m t vector trong m t h A A
Toán t t nh ti n theo quy chi u { A} gi ng h t ma tr n quay mô t m t h quy P2 RK Q A P1 A
P A
C
P
m t vector T BCT
B
A T chi u {B} trong h quy chi u { A} trong ó {B} có c do 1 0 0 0 1 1 1 1
Q qx qy qz quay { A} quanh tr c K m t góc .
A
CT
T
A A A
P2 P1 Q Toán t quay c ng có th c vi t d i d ng ma VD t nh ti n vector
T
1 1 tr n bi n i ng nh t. VD quay quanh z m t góc : A
P1 3 7 0 theo vector
cos sin 0 0 A T Có th tính c:
1 0 0 qx Q 10 5 0 r i quay 30 o quah z A
sin cos 0 0 A R CB R A
R B PCORG A
PBORG
0 1 0 qy A
P1 T B B
Rz ( ) A A A
C
0 0 0 1
0 0 1 0 P2 Rz 30 Q P1 T
0 0 1 qz 1 9.1 12.6 0 1
0 0 0 1 1 0 0 0 1 1
0 0 0 1
T
Ch DQ và
ng 2: Mô t không gian q bi n i h quy chi u 9 Ch ng 2: Mô t không gian và bi n i h quy chi u 10 Ch ng 2: Mô t không gian và bi n i h quy chi u 11 Ch ng 2: Mô t không gian và bi n i h quy chi u 12

2.4 Tính ch t phép bi n i h quy chi u 2.4 Tính ch t phép bi n i h quy chi u 2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u

Ngh ch o phép bi n i N u các phép M t ma tr n bi u di n phép quay m t h quy chi u Phép quay quanh các tr c X-Y-Z c nh (RPY): H
A
RT A
RT APBORG bi n i t o thành m t ch có 3 ph n t c l p. quy chi u {B} có c b ng cách quay {A} quanh Xˆ A m t
B A 1 B B
A T BT vòng kín thì có th Ma tr n quay R [ Xˆ Yˆ Zˆ ] ph i th a mãn: góc , sau ó quanh YˆA m t góc và cu i cùng quanh Zˆ A
0 0 0 1 tính m t phép bi n i
Xˆ Yˆ Zˆ 1 và Xˆ Yˆ Yˆ Zˆ Zˆ Xˆ 0 m t góc
thông qua các phép
Ví d : {B} có c b ng cách quay Ma tr n quay có nh th c b ng 1.
bi n i còn l i.
{ A } quanh tr c Zˆ A m t góc 30 o r i t nh ti n theo vector Phép quay không có tính giao hoán: BA R CB R CB R BAR
T Ví d tìm phép
4 3 0 . Tìm BAT bi n i t h quy Vì ma tr n quay ch có 3 ph n t c l p nên khai
3 2 12 0 4 3 2 12 0 4.964 chi u {C } sang {B}? báo phép quay thông qua ba ph n t s ti n h n khai báo
B
T U
T 1U A C 1 c m t ma tr n.
A 12 3 2 0 3 BT 12 3 2 0 0.598 C B AT DT DT
B T , A
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

Ch ng 2: Mô t không gian và bi n i h quy chi u 13 Ch ng 2: Mô t không gian và bi n i h quy chi u 14 Ch ng 2: Mô t không gian và bi n i h quy chi u 15 Ch ng 2: Mô t không gian và bi n i h quy chi u 16

2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u

A A
B R XYZ ( , , ) RZ ( )RY ( )RX ( ) Cho R có th tính ng c l i
B c các góc quay Phép quay theo góc Euler Z-Y-X: H quy chi u {B} Luôn có: BA R BA R BB R BB R
c c c s s s c c s c s s thành ph n. (Atan2(y,x) tr v góc có sin=y và cos=x) có c b ng cách quay {A} quanh Zˆ A góc , sau ó c c c s s s c c s c s s
Atan2 r31, r112 r212 quanh YˆB m i góc và cu i cùng quanh Xˆ B m i góc . A
RZ Y X ( , , ) s c s s s c c s s c c s
s c s s s c c s s c c s r11 r12 r13 B

s c s c c A
R XYZ ( , , ) r21 r22 r23 Atan2 r21 c , r11 c
s c s c c
B
A
Trong phép quay RPY, th t nhân ma tr n luôn r31 r32 r33 Atan2 r32 c , r33 c B RXYZ ( , , )
ng c v i th t th c hi n. Trong phép quay theo các góc Euler, th t nhân
th ng ch n trong kho ng [ 90o ;90o ]. Khi
90o thì ch tính c t ng ho c hi u c a và , ma tr n luôn gi ng v i th t th c hi n.
thông th ng khi ó nghi m ch n là: B i vì BA RZ Y X ( , , ) BA R XYZ ( , , ) nên k t qu bài
90 90 toán ng c gi ng phép quay RPY.
0 ho c 0
Atan2 r12 , r22 Atan2 r12 , r22

Ch ng 2: Mô t không gian và bi n i h quy chi u 17 Ch ng 2: Mô t không gian và bi n i h quy chi u 18 Ch ng 2: Mô t không gian và bi n i h quy chi u 19 Ch ng 2: Mô t không gian và bi n i h quy chi u 20

2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u

Phép quay theo góc Euler Z-Y-Z: Phép quay k x k xv c k x kyv kzs k x k zv kys
c c c s s c c s s c c s quanh m t tr c RK ( ) k x k yv k zs k y k yv c k y k zv k xs
Kˆ
T
A
RZ Y Z ( , , ) s c c c s s c s c c s s kx ky kzqua
B k x k zv kys k y k zv k xs k z k zv c
g ct a m t góc .
s c s s c (v 1 cos )
T A
B RZ Y Z ( , , ) có th tính , , : r11 r12 r13
Khi sin 0 thì: Khi sin 0 thì: N u cho BARK ( ) r21 r22 r23 có th tính ng c l i
Atan2 r 2
31
2
r ,r33
32
0.0 180 o r31 r32 r33
ho c
Atan2 r23 / s , r13 / s
0.0 0.0
c góc quay và tr c quay Kˆ
Atan 2 r12 , r11 Atan 2 r12 , r11
Atan2 r32 / s , r31 / s r32 r23
r r r 1 1
Có t ng s 12 phép quay quanh các tr c t a c A cos 11 22 33 và Kˆ r13 r31
nh và 12 phép quay theo các góc Euler. 2 2 sin
r21 r12

Ch ng 2: Mô t không gian và bi n i h quy chi u 21 Ch ng 2: Mô t không gian và bi n i h quy chi u 22 Ch ng 2: Mô t không gian và bi n i h quy chi u 23 Ch ng 2: Mô t không gian và bi n i h quy chi u 24

2.5 Ma tr n quay h quy chi u 2.5 Ma tr n quay h quy chi u

VD tìm mô t {B} theo {A} bi t r ng {B} có c nh

30o quanh AKˆ
T
quay {A} m t góc 0.707 0.707 0
i qua g c c a {A}?
N u tr c quay không qua g c
t a c n thêm hai phép t nh ti n
tr c và sau khi quay. VD quay {A}
quanh AKˆ 0.707 0.707 0
T
i
T
qua i m AP 1 2 3 m t góc
30o c n t nh ti n {A} theo
A T
1 2 3 t i i m P, sau ó quay
quanh A Kˆ c {B }, sau ó t nh
T
ti n ng c l i theo B 1 2 3 c {B}.

Ch ng 2: Mô t không gian và bi n i h quy chi u 25 Ch ng 2: Mô t không gian và bi n i h quy chi u 26