Chöông II: Ñoäng hoïc tay maùy

II-1 Vò trí vaø höôùng

II-1-1 Vị trí của vật rắn trong không gian
II-1-2 Hướng của vật rắn
II-1-3 matrận chuyển đổi
II-2 Pheùp quay moät quanh truïc baát kyø
II-2-1 Toång hôïp caùc ma traän quay
II-2-2 Pheùp quay quanh truïc baát kyø
II-3 Pheùp chuyeån ñoåi thuaàn nhaát
Chöông 2
ÑOÄNG HOÏC TAY MAÙY
2.1 VÒ TRÍ VAØ HÖÔÙNG CUÛA
VAÄT RAÉN TRONG KHOÂNG
GIAN
 II-1-1-Vị trí của vật

Z A
x
OA = y
Y z

X
O
Vò trí cuûa vaät tại điểm A ñöôïc ñaëc tröng
bôûi toaï ñoä cuûa ñieåm cuoái vectô
OA={ x,y,z}
 Z
A
OAz OAx = X
OAy = Y
Y OAz = Z

OAy
X

OAx
Phân tích véc tơ OA ra thành các thành phần
OA={ OAx,OAy,OAz}
 II-1-2-Hướng của vật rắn
• Moät heä toïa ñoä töông ñoái tuyø gaén
lieàn vôùi vaät laø X’,Y’,Z’

y’ X’

O’
 Z
A Ax
A = Ay
O’
Az
Y

X
Cho vector chỉ hướng A, hình chiếu của vec tơ này trên
các trục sẽ là :Ax,Ay,Az. Để chỉ hướng thi chỉ cần độ dài
của IAI=1. Nếu chỉ dùng 1 véctơ trục chính chỉ hướng cho
vật rắn thì không đủ, phải cần 3 véc tơ gắn với vật rắn.
 Hình sau cho một ví dụ gắn 3 véc tơ X’,Y’,Z’ chỉ hướng
Z vào vật rắn. Hệ 3 véc tơ này tạo thành hệ tọa độ cục bộ
(Hệ tọa độ tương đối)
Z’
Y’
O’ X’
Y

X
Heä toaï ñoä tuyeät ñoái X ,Y , Z

Heä toaï ñoä töông ñoái X’,Y’,Z’

• Trong khoâng gian moät vaät raén coù vò trí
ñöôïc ñaëc tröng bôûi 3 toïa ñoä OO’x, OO’y,
OO’z cuûa heä toïa ñoä tuyeät ñoái X,Y,Z
• Moät heä toïa ñoä töông ñoái tuyø gaén lieàn
vôùi vaät laø X’,Y’,Z’, đặc trưng cho hướng
của vật rắn .
• Cho các vectô chæ höôùng X’,Y’,Z’. Haõy tìm
hình chiếu của các véc tơ này trên X, Y, Z.
 Neáu veõ moät vectô ñôn vò e tren truïc X’ cuûa
toïa ñoä töông ñoái

Thì hình chieáu cuûa noù treân truïc X seõ laø

COS(α )
IeI=1 X’
Chuùng ta noùi
truïc X’ cuûa heä
toïa ñoä töông
doái quay ñi α
moät goùc α so
vôùi truïc X X
cuûa heä toa ñoä X’x=cos(α )
tuyeät ñoái.
 Neáu veõ moät vectô ñôn vò e tren truïc X’ cuûa
toïa ñoä töông ñoái

Thì hình chieáu cuûa noù treân truïc Y seõ laø

cos(β ) y
IeI=1 X’
Chuùng ta noùi truïc β
X’ cuûa heä toïa
ñoä töông doái
quay ñi moät
goùc β so vôùi
truïc Y cuûa heä X
toa ñoä tuyeät X’y=cos(β )
ñoái.
 Neáu veõ moät vectô ñôn vò e tren truïc X’ cuûa
toïa ñoä töông ñoái

Thì hình chieáu cuûa noù treân truïc Z seõ laø

cos(γ ) Z
IeI=1 X’
Chuùng ta noùi truïc
X’ cuûa heä toïa γ
ñoä töông doái
quay ñi moät
goùc γ so vôùi
truïc Y cuûa heä X
toa ñoä tuyeät X’y=cos(γ )
ñoái.
 Goïi T laø ma traän chỉ hướng T gồm ba
thành phần X’, Y’, Z’ chiếu trên X,Y,Z

X’ x Y’ x Z’ x

T X’Y Y’ Z’
= y y

X’z Y’ z Z’ z

Matrận T đặc trưng cho hướng của vật rắn

Ta gọi X’x hình chiếu vec tơ đơn vị e của X’ trên X

Ta gọi X’y hình chiếu vec tơ đơn vị e của X’ trên Y

Ta gọi X’z hình chiếu vec tơ đơn vị e của X’ trên Z

 II-1-3-Ma trận chuyển đổi
A-Hệ tọa độ

• Hệ tuyệt đối gắn liền với hệ không chuyển động

• Hệ tương đối gắn liền với vật chuyển động

Trong điều khiển robot tự hành, thường hệ tọa độ tuyệt đối gắn với
các camera theo dõi robot, hệ định vi tòan cầu v.v…Hệ tọa độ tương
đối gắn theo hướng đầu xe.
Trong điều khiển robot cố định hệ tọa độ tuyệt đối gắn với đế máy.
Hệ tọa độ tương đối gắn liền với từng khâu của tay máy.
 Chuyển đổi không gian
Z
D Dx
D = Dy
Dz
Y

X
Cho véc tơ D trong không gian
 Z
D Dx
Z’ Y’
D = Dy
O’ X’ Dz
Y

O
Dx’
X D’ = Dy’
Heä toaï ñoä tuyeät ñoái X,Y,Z Dz’

Heä toaï ñoä töông ñoái X’,Y’,Z’

 Dx’ X’
IeI=1

α
X
X’x=cos(α )

Hình chieáu cuûa e treân truïc X laø cos(α )

Hình chieáu cuûa Dx’ treân truïc X laø Dx

Dx = cos(α ) Dx’
 Töông quan hai heä toïa ñoä
Dx x’x Y’x Z’x Dx’
Dy X’y Y’y Z’y Dy’
=
Dz X’z Y’z Z’z Dz’

IeI=1 X’

Heä soá chuyeån X

ñoåi X’x=cos(α )
 Goïi T laø ma traän chuyeån ñoåi

x’x y’x z’x

T x’y y’y z’y
=
x’z y’z z’z

Ta coù D = T. D’
 Trong coâng thöùc treân cho D’ tìm D và ngược lại :
Neáu cho tröôùc:
+ ma traän hệ số 3x3, khoâng suy bieán
+vaø vectô coät D
Thì chuùng ta luoân tìm ñöôïc moät lôøi giaûi D’

YÙ nghóa vaät lyù cuûa ñònh lyù treân laø :

Moãi vectô trong toïa ñoä töông ñoái ñeàu coù theå tính
toaùn quy ñoåi veà toïa ñoä tuyeät ñoái vaø ngöôïc laïi .
 T
D’

E T-1 E’

R’
R
Ta coù theå choïn matraän T làm công cụ cho phép ánh xạ từ
không gian tương đối về không gian tuyệt đối và T-1 cho chiều
 Ví dụ về tính 1 đối 1 của bài tóan này

y
Y’
R

X’

α
X
Chất điểm trên ngọn cây trong thực tại là duy nhất. Nếu
chiếu vào không gian X,Y thì có tọa độ này song khi
Ví dụ về tính tóan ánh xạ xuôi và ngược

y
y’ A X’

X
Ví dụ về tính tóan ánh xạ ngược
X Cos(α) -Sin (α)
X’
=
Y Sin (α) Cos (α)
Y’

Cos(α) -Sin (α)

T=
Sin (α) Cos (α)
T.T-1=1

T-1= Cos(α) Sin (α)

-Sin (α) Cos (α)
Hình thành tọa độ tương đối từ một
tọa độ tuyệt đối

1- Phép quay

2- Phép tịnh tiến

 II.2.1 Ma traän quay
• A-Quanh Moät truïc Z

Z
Z’ Y’
y

X’

α
X
Quanh Moät truïc Z (tieáp)
Heä soá chuyeån ñoåi X’ => X
Z
Z’ Y’
y

X’
IeI=1
α
X
X’x=cos(α )
Heä soá chuyeån ñoåi X’ => Y

Z
Z’ Y’
y

X’
IeI=1
α
X
X’y =sin(α )
Heä soá chuyeån ñoåi Y’ =>
Y

Z
y
Z’ Y’
IeI=1
α
X’

α
X
Y’y =cos(α )
Heä soá chuyeån ñoåi Y’ =>
X

Z
Z’ Y’ y
IeI=1
α
X’

α
X
Y’y = -sin(α )
 Z=Z’ vì truïc z vaø z’ song song
Vaäy
: X Cos(α) -Sin (α)
X’
=
Y Sin (α) Cos (α)
Y’
Chuyeån sang khoâng gian 3 chieàu ta coù coâng
thöùc chuyeån ñoåi töø toaï ñoä töông ñoái sang
toaï ñoä tuyeät ñoái laø :

X Cos(α) -Sin (α) 0 X’

=
Y Sin (α) Cos (α) 0 Y’
Z 0 0 1 Z’
 Kí hieäu Ma traän quay quanh truc Z laø :

Cos(α) -Sin (α) 0

R(z, α) =
Sin (α) Cos (α) 0
0 0 1
 Quy taéc vaën nuùt chai Z,Z’

α
X X’

Y’
 Quy taécxaùc ñònh chieàu quay
Y’ y

X’

X
Ngöôïc chieàu kim ñoàng hoà α > 0
Xuoâi chieàu kim ñoàng hoà α < 0
 B-Quanh Moät truïc X

Z
Y’
Z’ y
β β

X’

X
 B-Quanh Moät truïc X

Z
Y’
Z’ y
β β

X’

Y’y=cosβ Y’z=sinβ
 B-Quanh Moät truïc X

Z
Y’
Z’ y
β β

X’

Z’z=cosβ Z’y=-sinβ
Truïc X vaø X’ truøng nhau

X 1 0 0 X’
= 0 Cos(α) -Sin (α)
Y Y’
0 Sin (α) Cos (α)
Z Z’
 Kí hieäu Ma traän quay quanh truc x laø :

1 0 0
R(X, α) = 0 Cos(α) -Sin (α)
0 Sin (α) Cos (α)
 C-Quanh Moät truïc Y
y
Z α
Z’ Y’

X
α

X’
X’x=cos(α ) X’z=-sin(α )
 y
Z α
Z’ Y’

X
α

X’
Z’z=cos(α ) Z’x=sin(α )
 Quanh Moät truïc Y(tieáp)

Kí hieäu Ma traän quay quanh truc Y laø :

X Cos(α) 0 Sin (α) X’

= 0 1 0
Y Y’
-Sin (α) 0 Cos (α)
Z Z’

R(Y, α)
 Quanh Moät truïc Y(tieáp)

Kí hieäu Ma traän quay quanh truc Y laø :

Cos(α) 0 Sin (α)

R(Y, α) = 0 1 0
-Sin (α) 0 Cos (α)
1 0 0
Yaw Chong chanh
0 Cos(α) -Sin (α)
R(X, α)
0 Sin (α) Cos (α)
Cos(β) 0 Sin (β) 0
1 0 pitch Len xuong

-Sin (β) 0 Cos (β) R(Y, β)

Z’
roll Lac lu
Cos(γ) -Sin (γ) 0
X’
Y’ R(Z, γ)
Sin (γ) Cos (γ) 0

Y-P-R 0 0 1
 D – Ví duï
• Cho chaát ñieåm chuyeån ñoäng P
• Trong heä toaï ®é di ®éng m . Gi¶ sö P lµ
toaï ®é töông ñoái :
[ P]m = [ 2, 0, 3 ]
Goùc quay quanh truc X cua heä toa ñoä töông
ñoái la 600

• Tìm täa ®é cè ®Þnh [P]f = ?.

 [P]F = R(X, α) [p]m

1 0 0
R(X, α) = 0 Cos(α) -Sin (α)
0 Sin (α) Cos (α)

Thay so
 Chuyeån ñoåi theo chieàu ngöôïc lai:

[ P]f = [ 3, 4, 0 ]

[p]m = R(X, α) -1 [P]F

1 0 0
R(X, α) -1 =
0 Cos(α) Sin (α)
0 -Sin (α) Cos (α)
Thay so
=[3 2 -3.464]
 II-1-4 Ma traän quay

A-Quay đồng phẳng (truc Z)

y

A’
A

α X

X Cos α+β Cos(α) -Sin (α) Cos β

= L = L
Y Sin α+β Sin (α) Cos (α) Sin β
 Quay đồng phẳng
X Cos α+β Cos(α) -Sin (α) Cos β
= L = L
Y Sin α+β Sin (α) Cos (α) Sin β
X’
R(Z,α)
Y’

X X’
= R(Z,α)
Y Y’
Như vậy có thể coi X’,Y’ là hình chiếu trên vị trí cuối cùng
của véctơ A
 Ma traän quay lieân tuïc
y
A’’
A’
γ
A

α X

X Cos α+β+γ Cos(α +β) -Sin (α+β) Cos γ

= L = L Sin (α+β) Cos (α+β)
Y Sin α+β+γ Sin γ
 Quay đồng phẳng
X Cos α+β+γ Cos(α+β) -Sin (α+β)
Cos γ
=L = L
Y Sin α+β+γ Sin γ
Sin (α+β) Cos (α+β)
X’’
R(Z, α+β)
Y’’

X X’’
= R(Z, α+β)
Y Y’’
Như vậy có thể coi X’’,Y’’ là hình chiếu trên vị trí cuối cùng
của véctơ A’’
 Ma traän quay lieân tuïc
Ví dụ :Quay đồng phẳng
Cho robot SCARA có hinh chiếu bằng sau:
y X’’
D Dx’ Dx’’
= R(Z,β)
Dy’ Dy’’
Y’’ B
Dx Dx’
Y’ β = R(Z,α)
Dy Dy’
X’
L1

A
Dx Dx’’
α X = R(Z,α+β)
Dy
O Dy’’
 T1
T2 D’’
D’
D

T-11
E E’ E’’
T-12

R’ R’’
R
Ta coù theå choïn matraän T làm công cụ cho phép ánh xạ từ
không gian tương đối về không gian tuyệt đối và T-1 cho chiều
Ta coù coâng thöùc chuyeån ñoåi töø toaï ñoä
töông ñoái sang toaï ñoä tuyeät ñoái laø :

X Cos(α) -Sin (α) 0 X’

=
Y Sin (α) Cos (α) 0 Y’
Z 0 0 1 Z’
 B-Quay không đồng phẳng
Cho robot scorbot

Z
Z’ Y’
D X’’
Z’’
y
β X’

α
X
 Kí hieäu Ma traän quay quanh truc Z laø :

Cos(α) -Sin (α) 0

R(z, α) =
Sin (α) Cos (α) 0
0 0 1
Z
Z’
Y’
D X’’
Z’’
y
β X’
α
X
 Cos(β) 0 Sin (β)
R(Y’, β) = 0 1 0
-Sin (β) 0 Cos (β)

Z
Z’ Luu y chieu quay xuoi chieu
Y’ kim dong ho
D X’’
Z’’
y
β X’
α
X
 Quay lien tuc khong dong phang
X X’’
Y = R(z, α) R(Y’, β) Y’’
Z Z’’
Dx’’
0
=
Ví dụ: α=450 β= - 450 D= Dy’’ 0
Dz’’
1

Dx
Cos(α) -Sin (α) 0 Cos(β) 0 Sin (β)
0
Dy Sin (α) Cos (α) 0 0 1 0
0
=
-Sin (β) 0 Cos (β)
Dz 0 0 1
1
Dx √2 - √2 0 √2 √2
2 2 0 -
0
Dy = 2 2
√2 √2 0 1 0 0
2 0
Dz 2
0 0 1 √2 0 √2 1
2 2
α=450 β= - 450 D=?
D
- 1
Z
Dx 2
y

Dy = - 1
2
Dz
√2 β X’
2 α
X
 C- Quay quanh trục bất kỳ
Cho hướng X’’ quay vectơ A quanh trục này một góc ω
X’’ Ax’’
Z
A A = Ay’’
ω Az’’
O’

β
Y

X’
O
α
X
 Quay quanh trục bất kỳ
Hệ tọa độ tương đối X’’,Y’’,Z’’ hình thành từ phép quay của
X,Y,Z lần lượt góc α, β .Véctơ A sau khi quay quanh trục X’’
sẽ là A* ta có:

Ax Ax’’
Z X’’
Ay =R(Z, α) R(Y, β) Ay’’
A
ω Az Az’’
O
’
β Y
Ax’’
A*x’’
X’ Ay’’
A*y’’ =R(X, ω)
O
α Az’’
X A*z’’
 Quay quanh trục bất kỳ
A*x Ax’’
A*y =R(Z, α). R(Y, β). R(X, ω). Ay’’

A*z Az’’

Z X’’ Một cách khác trả về một trục nào đó ,

A quay đi theo yêu cầu rồi quay trả lại
ω
O Ax
’
Y Ay =R(Y, α).R(Z, β ).R(X, ω).
β
Az Ax’’
X’
O R(Z, -α) .R(Y, -β). Ay’’
α
X Az’’
D-Một số tính chất của phép quay liên tục

+ Ý nghĩa của matrận quay:

- Biểu diễn hướng tương đối giữa hai hệ

tọa độ
-Matrận chuyển đổi các đại lượng của
của một véc tơ từ không gian tương đối
sang tuyệt đối và ngược lại.

-Là tóan tử biểu diển phép quay véc tơ

trong cùng một không gian
Một số tính chất của phép quay liên tục

Tính chất 1: Về nghich đảo

RT (Z, α)= R-1 (Z, α)

Tính chất 2: Về tính bắc cầu

R13= R12 . R23

Quay lien tuc R(Z,α).R(Y’,β)=R(Y, β ).R(Z, α)
Z Y’

Y Z