P. 1
Bai Giang Ki Thuat Robot 1

Bai Giang Ki Thuat Robot 1

|Views: 13|Likes:
Được xuất bản bởisimple_life09

More info:

Published by: simple_life09 on Jul 21, 2012
Bản quyền:Attribution Non-commercial

Availability:

Read on Scribd mobile: iPhone, iPad and Android.
download as PDF, TXT or read online from Scribd
See more
See less

05/10/2013

pdf

text

original

ĐỘ NG HỌ C ROBOT

Hệ tọ a độ vậ t: Hệ tọ a độ vậ t:
2.1. Vị trí và hư ớ ng củ a vậ t rắ n trong không gian
Biể u diễ n matrậ n
^ ^ ^ __
k c j b i a P
z y x
+ + ·
1
1
1
1
]
1

¸

·
w
z
y
x
P
__
1
1
1
1
]
1

¸

·
w
z
y
x
P
__
w
z
c
w
y
b
w
x
a
z y x
· · · ; ;
1
1
1
1
]
1

¸

·
1 0 0 0
0
0
0
z z z
y y y
x x x
a o n
a o n
a o n
F
Quay:
1
1
1
1
]
1

¸

·
1 0 0 0
1 0 0
0 1 0
0 0 1
z
y
x
d
d
d
T
Tịnh tiế n:
1
1
1
1
]
1

¸

·
1 0 0 0
0
0
0
z z z
y y y
x x x
a o n
a o n
a o n
F
1
1
1
1
]
1

¸

·
1 0 0 0
z z z z
y y y y
x x x x
P a o n
P a o n
P a o n
F
Quay và tịnh tiế n
z o y o x o O
z y x
' ' ' ' + + ·
Hệ tọ a độ gố c: Oyxz vớ i các véc tơ đơ n vị là x, y, z
Vị trí và định hư ớ ng củ a củ a vậ t rắ n trong không gian
gắ n lên hệ quy chiế u địa phư ơ ng O’x’y’z’
1
1
1
]
1

¸

·
z
y
x
o
o
o
o
'
'
'
'

Hư ớ ng củ a vậ t <== các
véc tơ đơ n vị x’, y’, z’:
z z y z x z z
z y y y x y y
z x y x x x x
z y x
z y x
z y x
' ' '
' ' '
' ' '
'
'
'
+ + ·
+ + ·
+ + ·
Các thành phầ n củ a các véc tơ đơ n vị (x’
x
, x’
y
, x’
z
) là cosin chỉ
phư ơ ng củ a các trụ c củ a hệ tọ a độ địa phư ơ ng so vớ i hệ quy
chiế u chung.
X’
x
= X’.cos(X’,X),…
Các thành phầ n củ a các véc tơ đơ n vị (x’
x
, x’
y
, x’
z
) là cosin chỉ
phư ơ ng củ a các trụ c củ a hệ tọ a độ địa phư ơ ng so vớ i hệ quy
chiế u chung.
X’
x
= X’.cos(X’,X),…
j [
1
1
1
]
1

¸

· ·
z z z
y y y
x x x
z y x
z y x
z y x
z y x R
' ' '
' ' '
' ' '
' ' '
Ma trậ n quay
Quay quanh trụ c z
1
1
1
]
1

¸


·
þ þ
þ þ
þ
cos 0 sin
0 1 0
sin 0 cos
) (
y
R
1
1
1
]
1

¸


·
1 0 0
0 cos sin
0 sin cos
) ( o o
o o
o
z
R
1
1
1
]
1

¸

·
1
1
1
]
1

¸


·
1
1
1
]
1

¸

·
1
0
0
' ;
0
cos
sin
' ;
0
sin
cos
' z y x o
o
o
o
1
1
1
]
1

¸


·
þ þ
þ þ
þ
cos 0 sin
0 1 0
sin 0 cos
) (
y
R
1
1
1
]
1

¸

− ·
¸ ¸
¸ ¸ ¸
cos sin 0
sin cos 0
0 0 1
) (
x
R
Quay mộ t véc tơ
1
1
1
]
1

¸

·
1
1
1
]
1

¸

·
z
y
x
z
y
x
p
p
p
p
p
p
p
p
'
'
'
' ;
j [ ' ' ' ' '
' ' ' ' ' ' '
Rp p z y x
z p y p x p p p
z y x
· ·
+ + · ·
p R p
T
· '
Hay:
p R p
T
· '
Hay:
'
1 0 0
0 cos sin
0 sin cos
p p
1
1
1
]
1

¸


· o o
o o
Quay quanh trụ c z 
Ví dụ
Quay mộ t véc tơ quanh mộ t trụ c bấ t kì:
Tổ ng hợ p các ma trậ n quay:
1
2
0
1
0
2
2
0
2
0
1
0
1
0
2
1
2
1
R R R
P R P
P R P
P R P
·
·
·
·
1
2
0
1
0
2
2
0
2
0
1
0
1
0
2
1
2
1
R R R
P R P
P R P
P R P
·
·
·
·
Phép quay quanh trụ c bấ t kì
Phép quay
quanh trụ c
bấ t kì
• 1- Biế n đổ i trụ c quay về trùng vớ i 1 trong 3 trụ c cơ bả n củ a hệ quy
chiế u:
• A2 = Rot(z, -a )
• quay v quanh trụ c z góc theo chiề u kim đồ ng hồ khi nhìn từ ngọ n về
gố c trụ c ztrùng vớ i mặ t phẳ ng xoz.
• A3 = Rot(y, -b)
• quay v quanh trụ c y góc theo chiề u kim đồ ng hồ khi nhìn từ ngọ n về
gố c trụ c yv đã trùng vớ i trụ c Oz,
• biế n đổ i v về trùng vớ i Oz :
• A1 = A2A3
• v về trùng vớ i trụ c Ox hoặ c Oy tư ơ ng tự .
Phép quay
quanh trụ c
bấ t kì
1- Biế n đổ i trụ c quay về trùng vớ i 1 trong 3 trụ c cơ bả n củ a hệ
quy chiế u:
A
2
= Rot(z, -α ) : quay r quanh trụ c z trùng vớ i mặ t phẳ ng xoz.
A3 = Rot(y, −β ): quay r quanh trụ c y trùng vớ i trụ c Oz,
biế n đổ i v về trùng vớ i Oz :
A
1
= A
2
A
3
r về trùng vớ i trụ c Ox hoặ c Oy tư ơ ng tự .
2- A
4
quay vậ t quanh trụ c r (đã trùng trụ c cơ bả n).
3- Trả kế t quả về hệ quy chiế u cũ bằ ng cách thự c hiệ n ngư ợ c lạ i
nhữ ng gì đã làm ở bư ớ c 1, ma trậ n biế n đổ i ngư ợ c là chuyể n vị
(hoặ c nghịch đả o) củ a ma trậ n biế n đổ i thuậ n.
– Quay r ngư ợ c kim đồ ng hồ quanh trụ c Oy bằ ng ma trậ n A
3
T
.
– Quay r ngư ợ c kim đồ ng hồ quanh trụ c Oz bằ ng ma trậ n A
2
T
.
T T
A A v z Rot A A v r Rot
2 3 3 2
). , ( . ) , ( ·
Mô tả tố i thiể u củ a hư ớ ng
•Góc Euler:
Ba lầ n quay quanh ba trụ c củ a ba hệ quy chiế u khác nhau
• Ví dụ mộ t bộ góc ơ le là zyz, nghĩa là quay quanh trụ c z, quay
quanh trụ c y, rồ i lạ i quay quanh trụ c z, tứ c là trong mộ t bộ góc ơ le
có thể quay quanh mộ t trụ c tố i đa 2 lầ n, song phả i là 2 lầ n không
liên tiế p. Vậ y khở i xuấ t nế u mộ t trụ c quay có thể có mặ t hai lầ n thì
ban đầ u sẽ có bộ 6 lầ n quay, quanh 6 trụ c x, y, z, x, y, z.
• Có ba khả năng chọ n trụ c quay đầ u tiên hoặ c x, hoặ c y, hoặ c z.
• Có hai khả năng chọ n trụ c quay thứ hai, chọ n 2 trong 3 trụ c trên
trừ trụ c đã chọ n ở bư ớ c trư ớ c, vì hai trụ c quay giố ng nhau không
đư ợ c thự c hiệ n liên tụ c.
• Có hai khả năng chọ n trụ c quay lầ n ba vì có thể chọ n lặ p lạ i trụ c
đầ u tiên và còn mộ t trụ c chư a dùng lầ n nào.
• Vậ y số khả năng củ a phép quay ơ le là k = 3.2.2 = 12)
• Ví dụ mộ t bộ góc ơ le là zyz, nghĩa là quay quanh trụ c z, quay
quanh trụ c y, rồ i lạ i quay quanh trụ c z, tứ c là trong mộ t bộ góc ơ le
có thể quay quanh mộ t trụ c tố i đa 2 lầ n, song phả i là 2 lầ n không
liên tiế p. Vậ y khở i xuấ t nế u mộ t trụ c quay có thể có mặ t hai lầ n thì
ban đầ u sẽ có bộ 6 lầ n quay, quanh 6 trụ c x, y, z, x, y, z.
• Có ba khả năng chọ n trụ c quay đầ u tiên hoặ c x, hoặ c y, hoặ c z.
• Có hai khả năng chọ n trụ c quay thứ hai, chọ n 2 trong 3 trụ c trên
trừ trụ c đã chọ n ở bư ớ c trư ớ c, vì hai trụ c quay giố ng nhau không
đư ợ c thự c hiệ n liên tụ c.
• Có hai khả năng chọ n trụ c quay lầ n ba vì có thể chọ n lặ p lạ i trụ c
đầ u tiên và còn mộ t trụ c chư a dùng lầ n nào.
• Vậ y số khả năng củ a phép quay ơ le là k = 3.2.2 = 12)
Rot (Z, Y, X) Euler Angles
R R R R
2
3
1
2
0
1
0
3
·
Phép quay ơ le ZYZ
1
1
1
]
1

¸


+ − +
− − −
·
·
0 ¢ 0 ¢ 0
0 ¢ ¢ ¢ ¢ 0 ¢ ¢ ¢ ¢ 0 ¢
0 ¢ ¢ ¢ ¢ 0 ¢ ¢ ¢ ¢ 0 ¢
¢ 0 ¢
c s s c s
s s c c s c s s c c c s
s c c s s c c s s c c c
z Rot y Rot z Rot R
EUL
) , ( ). , ( ). , (
, , ,
1
1
1
]
1

¸


+ − +
− − −
·
·
0 ¢ 0 ¢ 0
0 ¢ ¢ ¢ ¢ 0 ¢ ¢ ¢ ¢ 0 ¢
0 ¢ ¢ ¢ ¢ 0 ¢ ¢ ¢ ¢ 0 ¢
¢ 0 ¢
c s s c s
s s c c s c s s c c c s
s c c s s c c s s c c c
z Rot y Rot z Rot R
EUL
) , ( ). , ( ). , (
, , ,
• 1/ R(x, γ )
P
(1)A
= R(x, γ )∗P
B
• 2/ R(x, γ )
P
(2)A
= R(y, β )∗P
(1)A
= R(y, β )∗ R(x, γ )∗P
B
• 3/R(z, α )
P
(3)A
= R(z, α )∗P
(2)A
= R(z, α )∗ R(y, β )∗ R(x, γ )∗P
B
• 1/ R(x, γ )
P
(1)A
= R(x, γ )∗P
B
• 2/ R(x, γ )
P
(2)A
= R(y, β )∗P
(1)A
= R(y, β )∗ R(x, γ )∗P
B
• 3/R(z, α )
P
(3)A
= R(z, α )∗P
(2)A
= R(z, α )∗ R(y, β )∗ R(x, γ )∗P
B
Trong bài toán nghịch, nế u cho trư ớ c:
Có thể tính các góc quay:
Euler RPY
Quay ba lầ n quanh ba trụ c
củ a ba hệ quy chiế u khác
nhau,
Xoay vậ t liên tiế p ba lầ n
quanh ba trụ c củ a hệ quy
chiế u ban đầ u.
Quay ba lầ n quanh ba trụ c
củ a ba hệ quy chiế u khác
nhau,
Xoay vậ t liên tiế p ba lầ n
quanh ba trụ c củ a hệ quy
chiế u ban đầ u.
đố i tư ợ ng quay là hệ quy
chiế u
đố i tư ợ ng quay trong
phép quay này là vậ t thể )
• Gọ i A là ma trậ n điể m biể u diễ n điể m mút véc tơ cầ n biế n hình
trong cả hai hệ quy chiế u.
• Phép quay vậ t so vớ i hệ quy chiế u cố định liên tiế p:
(1)
• Hay gọ i A1 là ả nh củ a A qua ánh xạ đó ta có:
(2)
• Sau khi quay vậ t đi lầ n thứ nhấ t bở i phép thự c hiệ n bình thư ờ ng
vì trụ c z lúc này là trụ c cơ bả n. Lầ n quay thứ hai quanh trụ c y’
không có ma trậ n quay vì y’ lúc này là trụ c bấ t kì, ta phả i làm
trùng nó vớ i mộ t trụ c củ a hệ quy chiế u rồ i sử dụ ng phép quay có
bả n quanh trụ c y cũ , sau đó trả kế t quả lạ i như sau:
(3)
• Lúc này trụ c x” lạ i là trụ c bấ t kì, để có ma trậ n quay ta lạ i phả i
làm trùng trụ c quay trư ớ c khi quay, sau khi quay bằ ng ma trậ n
quay tiêu chuẩ n trả kế t quả lạ i như sau:
(4)
Vậ y biể u thứ c đạ t đư ợ c cuố i cùng ở đây chính là mộ t trình tự
ngư ợ c lạ i vớ i (2). Biể u thứ c (2) biể u thi phép quay RPY còn
(4) biể u thị Euler.
) , " ( ) , ' ( ) , ( ¢ 0 ¢ x R y R z R R
RPY
·
) , " ( ) , ' ( ) , ( .
1
¢ 0 ¢ x R y R z R A A ·
• Gọ i A là ma trậ n điể m biể u diễ n điể m mút véc tơ cầ n biế n hình
trong cả hai hệ quy chiế u.
• Phép quay vậ t so vớ i hệ quy chiế u cố định liên tiế p:
(1)
• Hay gọ i A1 là ả nh củ a A qua ánh xạ đó ta có:
(2)
• Sau khi quay vậ t đi lầ n thứ nhấ t bở i phép thự c hiệ n bình thư ờ ng
vì trụ c z lúc này là trụ c cơ bả n. Lầ n quay thứ hai quanh trụ c y’
không có ma trậ n quay vì y’ lúc này là trụ c bấ t kì, ta phả i làm
trùng nó vớ i mộ t trụ c củ a hệ quy chiế u rồ i sử dụ ng phép quay có
bả n quanh trụ c y cũ , sau đó trả kế t quả lạ i như sau:
(3)
• Lúc này trụ c x” lạ i là trụ c bấ t kì, để có ma trậ n quay ta lạ i phả i
làm trùng trụ c quay trư ớ c khi quay, sau khi quay bằ ng ma trậ n
quay tiêu chuẩ n trả kế t quả lạ i như sau:
(4)
Vậ y biể u thứ c đạ t đư ợ c cuố i cùng ở đây chính là mộ t trình tự
ngư ợ c lạ i vớ i (2). Biể u thứ c (2) biể u thi phép quay RPY còn
(4) biể u thị Euler.
) )R(z, (y, . ) , ( ) , ( ) , ( ) , ( .
1
¢ 0 ¢ 0 ¢ ¢ R A z R y R z R z R A ·

) , ( ) , ( ) , ( . ) , ( ) , ( ) , ( ) , ( )R (z, )R )R(z, (y, .
-1 -1
¢ 0 ¢ ¢ 0 ¢ 0 ¢ ¢ 0 z R y R x R A z R y R x R y R A ·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1 0 0 0
0 0
0 0
0 0 0 1
) , ( ;
1 0 0 0
0 0
0 0 1 0
0 0
) , ( ;
1 0 0 0
0 1 0 0
0 0
0 0
) , (
¸ ¸
¸ ¸
þ þ
þ þ
o o
o o
¸ þ o
c s
s c
x Rot
c s
s c
y Rot
c s
s c
z Rot
Phép biế n đổ i thuầ n nhấ t:
• Chuyể n độ ng = tịnh tiế n + quay
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1 0 0 0
0 0
0 0
0 0 0 1
) , ( ;
1 0 0 0
0 0
0 0 1 0
0 0
) , ( ;
1 0 0 0
0 1 0 0
0 0
0 0
) , (
¸ ¸
¸ ¸
þ þ
þ þ
o o
o o
¸ þ o
c s
s c
x Rot
c s
s c
y Rot
c s
s c
z Rot
1
1
1
1
]
1

¸

·
1 0 0 0
1 0 0
0 1 0
0 0 1
) , , (
P
N
M
P N M Trans
Bài toán độ ng họ c
thuậ n củ a tay máy
• Phép chuyể n đổ i tọ a độ đư ợ c biể u diễ n bằ ng ma trậ n chuyể n đổ i
thuầ n nhấ t:
• Trong đó là véc tơ định vị, là các véc tơ định
hư ớ ng dư ớ i dạ ng cosin chỉ phư ơ ng củ a phầ n làm việ c. Chẳ ng hạ n
vớ i ma trậ n thuầ n nhấ t có thể chọ n như sau:
• Các phầ n tử a
12
; a
13
; a
23
là các phầ n tử định hư ớ ng, các phầ n tử
a
14
; a
24
; a
34
là các phầ n tử đinh vị. Như vậ y chỉ cầ n 6 phầ n tử để
mô tả định vị và định hư ớ ng.
1
]
1

¸

·
1 0 0 0
) ( ) ( ) ( ) (
) (
0 0 0 0
0
q p q a q s q n
q T
) (
0
q p
) ( ), ( ), (
0 0 0
q a q s q n
• Phép chuyể n đổ i tọ a độ đư ợ c biể u diễ n bằ ng ma trậ n chuyể n đổ i
thuầ n nhấ t:
• Trong đó là véc tơ định vị, là các véc tơ định
hư ớ ng dư ớ i dạ ng cosin chỉ phư ơ ng củ a phầ n làm việ c. Chẳ ng hạ n
vớ i ma trậ n thuầ n nhấ t có thể chọ n như sau:
• Các phầ n tử a
12
; a
13
; a
23
là các phầ n tử định hư ớ ng, các phầ n tử
a
14
; a
24
; a
34
là các phầ n tử đinh vị. Như vậ y chỉ cầ n 6 phầ n tử để
mô tả định vị và định hư ớ ng.
1
1
1
1
]
1

¸

− − −
− − −
− −

·
1
) (
34
24 23
14 13 12
0
a
a a
a a a
q T
Nguyên lý Denavit-Hartenberg (D-H)
a
i
: khoả ng cách giữ a hai khớ p theo phư ơ ng đư ờ ng
vuông góc chung.
d
i
: khoả ng cách giữ a giao điể m củ a hai đư ờ ng vuông
góc chung vớ i trụ c quay, tính theo phư ơ ng củ a đư ờ ng
vuông góc chung.
i
là góc quay quanh trụ c xi để zi-1 đế n trùng vớ i zi.
i là góc quay quanh trụ c zi-1 để xi-1 đế n trùng vớ i xi.
Biế n đổ i hệ quy chiế u O
i-1
trùng vớ i hệ quy chiế u O
i
:
• Tịnh tiế n O
i-1
Ó
i
(d
i
theo trụ c z
i-1
)
• Quay O’i quanh trụ c z’i
bằ ng ma trậ n quay R(z’
i
, θ
ι
)
1
1
1
1
]
1

¸


·
·

1 0 0 0
1 0 0
0 0
0 0
) , 0 , 0 ( ). , (
1
'
i
i i
i i
i
i
d
c s
s c
di R i Z i R A
0 0
0 0
°
•Tịnh tiế n O’i theo trụ c
x’i mộ t lư ợ ng a
i
•Quay Ói ở bư ớ c trên
quanh trụ c x’i góc để
hoàn thiệ n.
1
1
1
1
]
1

¸


·
1 0 0 0
0 0
0 0
0 0 1
'
i i
i i
i
i
i
c s
s c
a
A
o o
o o
• Ma trậ n biế n hình tổ ng hợ p đạ t đự ơ c bằ ng cách nhân hai
ma trậ n trên có dạ ng:
1
1
1
1
]
1

¸



· ·
− −
1 0 0 0
0
) (
' 1
'
1
i i i
i i i i i i i
i i i i i i i
i
i
i
i i
i
i
d c s
s a s c c c s
c a s s c s c
A A q A
o o
0 o 0 o 0 0
0 o 0 o 0 0
1
1
1
1
]
1

¸



· ·
− −
1 0 0 0
0
) (
' 1
'
1
i i i
i i i i i i i
i i i i i i i
i
i
i
i i
i
i
d c s
s a s c c c s
c a s s c s c
A A q A
o o
0 o 0 o 0 0
0 o 0 o 0 0
Mộ t số ví
dụ ứ ng
dụ ng quy
tắ c DH:
Tay máy ba khâu phẳ ng:
• Thay các thông số tư ơ ng ứ ng vào các ma trậ n mẫ u tổ ng quát nói
trên nhậ n đư ợ c ma trậ n biế n hình cho từ ng bư ớ c như sau:
• Khi nhân các ma trậ n này vớ i nhau có ma trậ n chuyể n đổ i tổ ng
hợ p:
• Ở đây kí hiệ u:
1
1
1
1
]
1

¸


·

1 0 0 0
0 1 0 0
0
0
) (
1 i i i i
i i i i
i
i
i
s a c s
c a s c
A 0
• Thay các thông số tư ơ ng ứ ng vào các ma trậ n mẫ u tổ ng quát nói
trên nhậ n đư ợ c ma trậ n biế n hình cho từ ng bư ớ c như sau:
• Khi nhân các ma trậ n này vớ i nhau có ma trậ n chuyể n đổ i tổ ng
hợ p:
• Ở đây kí hiệ u:
1
1
1
1
]
1

¸

+ +
+ + −
· ·
1 0 0 0
0 1 0 0
0
0
) (
123 3 12 2 1 1 123 123
123 3 12 2 1 1 123 123
2
3
1
2
0
1
0
3
s a s a s a c s
c a c a c a s c
A A A q T
) cos( c
3 2 1 123
ϑ + ϑ + ϑ ·
Z
0
X
0
Y
0
Z
3
X
2
Y
1
X
1
d
2
Z
1
X
3
3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
Joint i 
i
a
i
d
i

i
1 0 a
0
0 θ
0
2 -90 a
1
0 θ
1
3 0 0 d
2
θ
2
a
0
a
1
Y
2
Joint 2
1
1
1
1
]
1

¸



·

1 0 0 0
0
1
i i i
i i i i i i i
i i i i i i i
i
i
d C S
S a C S C C S
C a S S S C C
T
o o
° ° o ° o °
° ° o ° o °
1
1
1
1
]
1

¸


·
1 0 0 0
0 1 0 0
0 cosθ sinθ
0 sinθ cosθ
0 0
0 0
0
0 0
0 0
1
sin
cos
°
°
a
a
T
1
1
1
1
]
1

¸



·
1 0 0 0
0 0 0
sinθ
cosθ
1
1
1
1
sin cos 0
cos sin 0
1 1 1
1 1 1
2
° °
° °
a
a
T
) )( )( (
2 1 0
3 2 1 3
0
T T T T ·
1
1
1
1
]
1

¸


·
1 0 0 0
0 1 0 0
0 cosθ sinθ
0 sinθ cosθ
0 0
0 0
0
0 0
0 0
1
sin
cos
°
°
a
a
T
1
1
1
1
]
1

¸


·
1 0 0 0
0 sinθ cosθ
2 2
2
2
2 2 3
1 0 0
0 0 cos sin
0
d
T
° °
Tay máy tọ a độ cầ u:
Sơ đồ độ ng và bả ng thông số DH cho thấ y như
hình vẽ :
• Vì z0 và z1 cắ t nhau nên d1 = 0. Từ bả ng thông số DH có các
ma trậ n chuyể n vị thành phầ n như sau:
• Nhân các ma trậ n trên vớ i nhau có ma trậ n chuyể n vị tổ ng
hợ p:
1
1
1
1
]
1

¸



·
1 0 0 0
0 0 1 0
0 0
0 0
) (
1 1
1 1
1
0
1
c s
s c
A 0
1
1
1
1
]
1

¸


·
1 0 0 0
0 1 0
0 0
0 0
) (
2
2 2
2 2
2
1
2
d
c s
s c
A 0
1
1
1
1
]
1

¸

·
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) (
3
3
2
3
d
d A
• Vì z0 và z1 cắ t nhau nên d1 = 0. Từ bả ng thông số DH có các
ma trậ n chuyể n vị thành phầ n như sau:
• Nhân các ma trậ n trên vớ i nhau có ma trậ n chuyể n vị tổ ng
hợ p:
1
1
1
1
]
1

¸


+
− −
· ·
1 0 0 0
0
) (
3 2 2 2
2 1 3 2 1 2 1 1 2 1
2 1 3 2 1 2 1 1 2 1
2
3
1
2
0
1
0
3
d c c s
d c d s s s s c c s
d s d s c s c s c c
A A A q T
Robot Stanford
Z1
X2
Z2
X3
Z3
X4
X5
X6
Z4
Z5
Z6
X7
Z7
X1
Y1
• DH Parameters of Stanford Arm
i ai di oi ° i
1 0 d1 90° ° 1
2 0 d2 90° ° 2 2 0 d2 90° ° 2
3 0 d3 (var) 90° 90°
4 0 0 90° ° 4
5 0 d5 0° ° 5
6 0 d6 0 ° 6
Fig. 2.31 The frames of t he St anford Arm.
# ° d a o
1
°
1
0 0 -90
2
°
2 d
1
0 90
3
0
d
1
0 0
4
°
4
0 0 -90
5
°
5
0 0 90
6
°
6
0 0 0
Robot Scara
Khâu
i
°
i
o L
i
D
i
1
*
1
°
0 L1 H
2
*
2
°
180
0
L2 0
3 0 0 0
*
3
d
4
*
4
°
0 0 d
4
1
1
1
1
]
1

¸


· ·
1 0 0 0
1 0 0
0
0
) 0 , 0 , ( ). , (
1 1 1 1
1 1 1 1
1 1 01
H
S L C S
C L S C
l T z R T °
1
1
1
1
]
1

¸



· ·
1 0 0 0
0 1 0 0
0
0
) 0 , 0 , ( ). , (
2 2 2 2
2 2 2 2
2 2 12
S L C S
C L S C
l T z R T °
1
1
1
1
]
1

¸

· ·
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) , 0 , 0 (
3
3 23
d
d T T
1
1
1
1
]
1

¸

· ·
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) , 0 , 0 (
3
3 23
d
d T T
1
1
1
1
]
1

¸


· ·
1 0 0 0
1 0 0
0 0
0 0
) , (
4
4 4
4 4
4 34
d
C S
S C
z R T °
1
1
1
1
]
1

¸


+
+ −
· ·
1 0 0 0
1 0 0
0
0
.
12 2 1 1 12 12
12 2 1 1 12 12
12 01 02
H
S L S L C S
C L C L S C
T T T
1
1
1
1
]
1

¸

+ − −
+
+ −
· ·
1 0 0 0
1 0 0
0
0
. .
3
12 2 1 1 12 12
12 2 1 1 12 12
23 12 01 03
H d
S L S L C S
C L C L S C
T T T T
1
1
1
1
]
1

¸

+ − −
+
+ −
· ·
1 0 0 0
1 0 0
0
0
. .
3
12 2 1 1 12 12
12 2 1 1 12 12
23 12 01 03
H d
S L S L C S
C L C L S C
T T T T
1
1
1
1
]
1

¸

+ − − −
+
+ −
· ·
1 0 0 0
1 0 0
0
0
. . .
4 3
12 2 1 1 123 123
12 2 1 1 123 123
34 23 12 01 04
H d d
S L S L C S
C L C L S C
T T T T T
Puma
Example: Puma 560
Joint i 
i

i
a
i
(mm) d
i
(mm)
1 θ
1
0 0 0
2 θ
2
-90 0 d
2
3 θ
3
0 a
2
d
3
4 θ
4
90 a
3
d
4
5 θ
5
-90 0 0
6 θ
6
0 0 0
Example: Puma 560
Joint i 
i

i
a
i
(mm) d
i
(mm)
1 θ
1
0 0 0
2 θ
2
-90 0 d
2
3 θ
3
0 a
2
d
3
4 θ
4
90 a
3
d
4
5 θ
5
-90 0 0
6 θ
6
0 0 0
Forearm of a PUMA
a
3
x
3
y
3
x
4
z
4
d
4
a
3
x
5
y
5
x
6
z
6
y
3
z
4
Spherical j oint
Example: Puma 560
Different Configurat ion
Link Coordinate Parameters
Joint i

i

i
a
i
(mm) d
i
(mm)
1
θ
1
-90 0 0
2
θ
2
0 431.8 149.09
3
θ
3
90 -20.32 0
4
θ
4
-90 0 433.07
5
θ
5
90 0 0
6
θ
6
0 0 56.25
PUMA 560 robot arm link coordinate parameters
Joint i

i

i
a
i
(mm) d
i
(mm)
1
θ
1
-90 0 0
2
θ
2
0 431.8 149.09
3
θ
3
90 -20.32 0
4
θ
4
-90 0 433.07
5
θ
5
90 0 0
6
θ
6
0 0 56.25
Example: Puma 560
Độ ng họ c ngư ợ c
I n v e r s e K i n e m a t i c s
From Position to Angles
Độ ng họ c ngư ợ c
I n v e r s e K i n e m a t i c s
From Position to Angles
Ví dụ
(x , y)
Tìm:
)
x
y
arctan( ·
Trong tính toán:
)
x
y
( 2 arctan ·

1
X
Y
S
)
x
y
( 2 arctan ·
Kế t quả :
) y (x S
2 2
+ ·

2
(x , y)
l
2
Cơ cấ u hai khâu phẳ ng:
Given: l
1
, l
2
, x , y
Find: 
1
, 
2
(x , y)

1
l
1
Phư ơ ng pháp hình họ c
l
1
l
2

2

1

(x , y) Sử dụ ng định luậ t Cosines:

,
_
,
,
¸
¸
− − +
·
− − +
·
− · −
− − + · +
− + ·
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 2 2
2
arccos
2
) cos(θ
) cos(θ ) 180 cos(
) 180 cos( 2 ) (
cos 2
l l
l l y x
l l
l l y x
l l l l y x
C ab b a c
2
2
2 2
2

,
_
,
,
¸
¸
− − +
·
− − +
·
− · −
− − + · +
− + ·
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 2 2
2
arccos
2
) cos(θ
) cos(θ ) 180 cos(
) 180 cos( 2 ) (
cos 2
l l
l l y x
l l
l l y x
l l l l y x
C ab b a c
2
2
2 2
2
Sử dụ ng định luậ t Cosines:

,
_
,
¸
¸
·
+ ·
+
·
+

·
·
x
y
2 arctan
α
y x
) sin(θ
y x
) sin(180 sin
sin sin
1 1
2 2
2
2 2
2
2
1
l
c
C
b
B

,
_
,
¸
¸
+

,
_
,
,
¸
¸
+
·
x
y
2 arctan
y x
) sin(θ
arcsin
2 2
2 2
1
l
θ
2
có thể ở góc tọ a độ đầ u tiên hay
thứ tư
Phư ơ ng pháp đạ i số
l
1
l
2

2

1

(x , y)
2 1
2 1 2 1 1
2 1 2 1 1
1 2 2 1
1 1
θ (3)
sin s y (2)
c c x (1)
) cos( θ c
cos θ c
+ ·
+ ·
+ ·
+ ·
·
+
+
+
l l
l l
, ) , )
, )

,
_
,
,
¸
¸
− − +
· ∴
+ + ·
+ + + ·
+ + + + + ·
· + · +
+ +
+ + + +
2 1
2
2
2
1
2 2
2
2 2 1
2
2
2
1
2 1 1 2 1 1 2 1
2
2
2
1
2 1 1 2 1
2
2 1
2
2
2
1
2
1 2 1 1 2 1
2
2 1
2
2
2
1
2
1
2 2 2 2
2
y x
arccos
c 2
) (sin s ) (c c 2
) (sin s 2 ) (sin s ) (c c 2 ) (c c
y x ) 2 ( (1)
l l
l l
l l l l
l l l l
l l l l l l l l
2 1
2 1 2 1 1
2 1 2 1 1
1 2 2 1
1 1
θ (3)
sin s y (2)
c c x (1)
) cos( θ c
cos θ c
+ ·
+ ·
+ ·
+ ·
·
+
+
+
l l
l l
Only
Unknown
) )(sin (cos ) )(sin (cos ) sin(
) )(sin (sin ) )(cos (cos ) cos(
:
a b b a b a
b a b a b a
Note
+

+


+
+

·
·
) )(sin (cos ) )(sin (cos ) sin(
) )(sin (sin ) )(cos (cos ) cos(
:
a b b a b a
b a b a b a
Note
+

+


+
+

·
·
) c ( s ) s ( c
c s c s s
sin s y
) ( ) c ( c
c c c
c c x
2 2 1 1 2 2 1
1 2 2 2 1 2 1 1
2 1 2 1 1
2 2 1 2 2 1 1
2 1 2 2 1 2 1 1
2 1 2 1 1
l l l
l l l
l l
s l s l l
s s l l l
l l
+ + ·
+ + ·
+ ·
− + ·
− + ·
+ ·
+
+
Chúng ta biế t rằ ng θ
2
đã xác định ở
trên slide. Chúng ta cầ n tìm θ
1
.
, )
2 2
2 2 2 2 1
1
2 2 1
2
2
2
1 1 2 2
2 2 1
2 2 1 1 2 2
2 2 1
2 2 1
2 2 1
2 2 1
1
y x
x ) c ( y
s
) c 2 ( s x
) c (
1
) c ( s ) s (
) c (
) ( x
y
) c (
) ( x
c
+
− +
·
+ + +
+
·
+ +
+
+
·
+
+
·
s l l l
l l l l s l
l l
l l l
l l
s l s
l l
s l s
, )
2 2
2 2 2 2 1
1
2 2 1
2
2
2
1 1 2 2
2 2 1
2 2 1 1 2 2
2 2 1
2 2 1
2 2 1
2 2 1
1
y x
x ) c ( y
s
) c 2 ( s x
) c (
1
) c ( s ) s (
) c (
) ( x
y
) c (
) ( x
c
+
− +
·
+ + +
+
·
+ +
+
+
·
+
+
·
s l l l
l l l l s l
l l
l l l
l l
s l s
l l
s l s
Thay thế cho c1 và đơ n giả n hóa
nhiề u lầ n
Chú ý đay là định luạ t cosin và có
thể đư ợ c thay thể bằ ng x
2
+ y
2

,
_
,
,
¸
¸
+
− +
·
2 2
2 2 2 2 1
1
y x
x ) c ( y
arcsin
s l l l
Phư ơ ng pháp đạ i số
Khả o sát mộ t t ay máy có 3 khâu
T T T T T
B
W
2
3
1
2
0
1
0
3
· ·
Chúng t a t hu đư ợ c
phư ơ ng t rình độ ng họ c:
Phư ơ ng pháp đạ i số
Khả o sát mộ t t ay máy có 3 khâu
T T T T T
B
W
2
3
1
2
0
1
0
3
· ·
Chúng t a t hu đư ợ c
phư ơ ng t rình độ ng họ c:
3

Y
1

Y
2

X 2

Y
3

X
0

Y
Phư ơ ng pháp đạ i số
D-H t ransfaormat ion
i o
i-1
o
i-1
d
i
°
i
1 0 0 0 °
1
2 0 L
1
0 °
2
3 0 L
2
0 °
3
1

X
0

X
Phư ơ ng pháp đạ i số
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸

− −

·
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0 0
1 0 0 0
0
3 3
2 3 3
2
3
2 2
1 2 2
1
2
1 1
1 1
1 0 0 0 1 0 1
1 0 0 0 1 0 1
0 1 1
0
1
° °
° °
° °
° °
° °
° °
o o o ° o °
o o o ° o °
° °
c s
L s c
T
c s
L s c
T
c s
s c
d c c s c s s
d s s c c c s
a s c
T
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸


·
1
1
1
1
]
1

¸

− −

·
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0 0
1 0 0 0
0
3 3
2 3 3
2
3
2 2
1 2 2
1
2
1 1
1 1
1 0 0 0 1 0 1
1 0 0 0 1 0 1
0 1 1
0
1
° °
° °
° °
° °
° °
° °
o o o ° o °
o o o ° o °
° °
c s
L s c
T
c s
L s c
T
c s
s c
d c c s c s s
d s s c c c s
a s c
T
Phư ơ ng pháp đạ i số
Ta có:
1
1
1
1
]
1

¸

+
+ −
· ·
1 0 0 0
0 1 0 0
0
0
12 2 1 1 123 123
12 2 1 1 123 123
0
3
s l s l c s
c l c l s c
T T
B
W
1
1
1
1
]
1

¸

+
+ −
· ·
1 0 0 0
0 1 0 0
0
0
12 2 1 1 123 123
12 2 1 1 123 123
0
3
s l s l c s
c l c l s c
T T
B
W
Giả sử
1
1
1
1
]
1

¸


·
1 0 0 0
0 1 0 0
0
0
y c s
x s c
T
B
W
¢ ¢
¢ ¢
Từ đó ta có
12 2 1 1
12 2 1 1
123 123
s l s l y
c l c l x
s s c c
+ ·
+ ·
· ·
¢ ¢
Bình phư ơ ng 2 phư ơ ng
trình cuố i ta có:
Phư ơ ng pháp đạ i số
Bình phư ơ ng 2 phư ơ ng
trình cuố i ta có:
2 2 1
2
2
2
1
2 2
2 c l l l l y x + + · +
Từ đây ta có biể u thứ c
cho c
2
2 1
2
2
2
1
2 2
2
2 l l
l l y x
c
− − +
·
). , ( 2 tan 1
2 2 2
2
2 2
c s A c s · ⇒ − ± · °
Và cuố i cùng:
Ta có c
12
=c
1
c
2
-s
1
s
2
và s
12
= c
1
s
2
-c
2
s
1
:
1 2 1 1
1 2 1 1
c k s k y
s k c k x
+ ·
− ·
Phư ơ ng pháp đạ i số
1 2 1 1
1 2 1 1
c k s k y
s k c k x
+ ·
− ·
Trong đó k
1
=l
1
+l
2
c
2
và k
2
=l
2
s
2
.
Đặ t
r=+\ k
1
2
+k
2
2
and γ=Atan2(k
2
,k
1
).
Phư ơ ng pháp đạ i số
k
1
k
2
θ
2
l
1
l
2
γ
Trong đó: k
1
=r cos ¸ , k
2
=r sin ¸ và chúng ta
có thể viế t
x/r= cos ¸ cos °
1
- sin ¸ sin °
1
y/r= cos ¸ sin °
1
+ sin ¸ cos °
1
hoặ c: cos(¸+°
1
) = x/r, sin(¸+°
1
) =y/r
Trong đó: k
1
=r cos ¸ , k
2
=r sin ¸ và chúng ta
có thể viế t
x/r= cos ¸ cos °
1
- sin ¸ sin °
1
y/r= cos ¸ sin °
1
+ sin ¸ cos °
1
hoặ c: cos(¸+°
1
) = x/r, sin(¸+°
1
) =y/r
Do đó
γ+θ
1
= atan2(y/r,x/r) = atan2(y,x)
Suy ra :
θ
1
= atan2(y,x) - atan2(k
2
,k
1
)
Cuố i cùng, θ
3
có thể đư ợ c rút ra từ
θ
1

2
+ θ
3
= φ
Phư ơ ng pháp đạ i số
Do đó
γ+θ
1
= atan2(y/r,x/r) = atan2(y,x)
Suy ra :
θ
1
= atan2(y,x) - atan2(k
2
,k
1
)
Cuố i cùng, θ
3
có thể đư ợ c rút ra từ
θ
1

2
+ θ
3
= φ

Biểu diễn matrận
__

P = a x i + by j + c z k

^

^

^

x  y __ P=  z    w
x y z a x = ; by = ; c z = w w w

1 Tịnh tiến: 0 T = 0  0
Quay:

0 0 dx  1 0 dy   0 1 dz   0 0 1

n x n  y F= nz  0

ox oy oz 0

ax 0   ay 0  az 0   0 1

Quay và tịnh tiến

 nx n  y F=  nz  0

ox oy oz 0

ax ay az 0

Px   Py  Pz   1

Hệ tọa độ gốc: Oyxz với các véc tơ đơn vị là x. y. z Vị trí và định hướng của của vật rắn trong không gian  gắn lên hệ quy chiếu địa phương O’x’y’z’ O ' = o' x x + o' y y + o' z z   o' x  o '  o' =  y   o' z    .

x’z) là cosin chỉ phương của các trục của hệ tọa độ địa phương so với hệ quy chiếu chung.X). z’: ' ' x' = x x x + x 'y y + x z z ' ' y ' = y x x + y 'y y + y z z ' ' z ' = z x x + z 'y y + z z z Các thành phần của các véc tơ đơn vị (x’x. y’. x’y.cos(X’.… R = [x'  x' x  x' y ' z '] =  y  x' z  y' x y' y y' z z' x   z' y  z' z   .Hướng của vật <== các véc tơ đơn vị x’. X’x = X’.

Ma trận quay Quay quanh trục z cos − sin 0 sin cos 0 Rz ( ) =    0 0 1    cos  R y ( ) =  0   − sin   0 1 0 sin   0   cos    0 1 cos   − sin   0 R ( ) = 0 cos x   sin  . y ' =  cos  . z ' = 0 x' =  0 sin        0      0   1   0   − sin  cos   .

p ' =  p ' y  p   p'   z  z = [x' p = p ' = p ' x x'+ p ' y y '+ p ' z z ' y ' z '] p ' = Rp' Hay: p' = R T p Quay quanh trục z  cos  sin  p=  0  − sin  cos 0 0  p' 0 1  .Quay một véc tơ  px   p' x      p =  p y .

Ví dụ .

.

Quay một véc tơ quanh một trục bất kì: Tổng hợp các ma trận quay: P = R2 P 1 1 0 0 0 0 2 1 2 1 P = R1 P 0 0 P = R2 P R2 = R1 R2 .

Phép quay quanh trục bất kì .

 biến đổi v về trùng với Oz : A1 = A2A3  v về trùng với trục Ox hoặc Oy tương tự. . A3 = Rot(y.Biến đổi trục quay về trùng với 1 trong 3 trục cơ bản của hệ quy chiếu: A2 = Rot(z. -b) quay v quanh trục y góc theo chiều kim đồng hồ khi nhìn từ ngọn về gốc trục yv đã trùng với trục Oz. -a ) quay v quanh trục z góc theo chiều kim đồng hồ khi nhìn từ ngọn về gốc trục z trùng với mặt phẳng xoz.Phép quay quanh trục bất kì • • • • • • • • 1.

-α ) : quay r quanh trục z trùng với mặt phẳng xoz. −β ) quay r quanh trục y trùng với trục Oz. :  biến đổi v về trùng với Oz : A1 = A2A3  r về trùng với trục Ox hoặc Oy tương tự.Biến đổi trục quay về trùng với 1 trong 3 trục cơ bản của hệ quy chiếu: A2 = Rot(z.Phép quay quanh trục bất kì 1. A3 = Rot(y. .

A4  quay vật quanh trục r (đã trùng trục cơ bản). – Quay r ngược kim đồng hồ quanh trục Oy bằng ma trận A3T. 3. A3 A2 T T .2.Trả kết quả về hệ quy chiếu cũ bằng cách thực hiện ngược lại những gì đã làm ở bước 1. v) = A2 A3 . Rot (r . v). ma trận biến đổi ngược là chuyển vị (hoặc nghịch đảo) của ma trận biến đổi thuận.Rot ( z . – Quay r ngược kim đồng hồ quanh trục Oz bằng ma trận A2T.

2. chọn 2 trong 3 trục trên trừ trục đã chọn ở bước trước. quanh 6 trục x.Mô tả tối thiểu của hướng •Góc Euler: Ba lần quay quanh ba trục của ba hệ quy chiếu khác nhau • Ví dụ một bộ góc ơle là zyz.2 = 12) . • Có ba khả năng chọn trục quay đầu tiên hoặc x. quay quanh trục y. Vậy khởi xuất nếu một trục quay có thể có mặt hai lần thì ban đầu sẽ có bộ 6 lần quay. y. vì hai trục quay giống nhau không được thực hiện liên tục. z. • Có hai khả năng chọn trục quay lần ba vì có thể chọn lặp lại trục đầu tiên và còn một trục chưa dùng lần nào. x. hoặc z. • Vậy số khả năng của phép quay ơle là k = 3. y. hoặc y. • Có hai khả năng chọn trục quay thứ hai. tức là trong một bộ góc ơle có thể quay quanh một trục tối đa 2 lần. song phải là 2 lần không liên tiếp. z. nghĩa là quay quanh trục z. rồi lại quay quanh trục z.

Rot (Z. X) Euler Angles . Y.

0 3 R = 0 1 R 1 2 R 2 3 R .

 ). c c c − s s  =  s c c + c s  − s c  − c c s − s c − s c s + c c s s c s   s s  c   .Rot ( z .. ) .Rot ( y .  ). .Phép quay ơle ZYZ REUL = Rot ( z .

.

.

α )∗ β )∗ (x. γ ) P(1)A= R(x. γ )∗PB • 2/  R(x. α )∗ (2)A= R(z. γ )∗PB P R(y.• 1/  R(x. γ ) P(2)A= R(y. α ) P(3)A= R(z. γ )∗PB P R • 3/ R(z. β )∗ (x. R . β )∗ (1)A= R(y.

nếu cho trước:  Có thể tính các góc quay: .Trong bài toán nghịch.

Euler Quay ba lần quanh ba trục của ba hệ quy chiếu khác nhau. chiếu RPY Xoay vật liên tiếp ba lần quanh ba trục của hệ quy chiếu ban đầu. phép quay này là vật thể) đối tượng quay là hệ quy đối tượng quay trong .

để có ma trận quay ta lại phải làm trùng trục quay trước khi quay. ) ( . ) ( . ) )R Vậy biểu thức đạt được cuối cùng ở đây chính là một trình tự ngược lại với (2). -1  -1 yRx RyRz  =A (x RyRz  (4) . sau đó trả kết quả lại như sau: (3) A (z. ) ( . Lần quay thứ hai quanh trục y’ không có ma trận quay vì y’ lúc này là trục bất kì. Biểu thức (2) biểu thi phép quay RPY còn (4) biểu thị Euler.R1(z. • Phép quay vật so với hệ quy chiếu cố định liên tiếp: RRPY = R( z.)R( x".R   (z. .)R(z. ) (2) • Sau khi quay vật đi lần thứ nhất bởi phép thực hiện bình thường vì trục z lúc này là trục cơ bản. =A (y.RyRz. ) (1) • Hay gọi A1 là ảnh của A qua ánh xạ đó ta có: A1 = A. sau khi quay bằng ma trận quay tiêu chuẩn trả kết quả lại như sau: A (y.)R( . ) ( ) .R ) − ) ( .R . ) ( .)R(z. )R( y' .R( z. .• Gọi A là ma trận điểm biểu diễn điểm mút véc tơ cần biến hình trong cả hai hệ quy chiếu. ) ( .)R( x". ) .R   ) • Lúc này trục x” lại là trục bất kì. )R( y' . ta phải làm trùng nó với một trục của hệ quy chiếu rồi sử dụng phép quay có bản quanh trục y cũ.

Roty. N .) = ( s 1 0   0 1 0 0 −s 0 1 0 1 0 0 . P) =  0  0 0 0 M 1 0 N  0 1 P  0 0 1 .) =   ( 0 0  0 0 1 0 Trans(M .) = ( 0 0 c 0   0 0 1 0 0 0 c −s s c 0 0 0 0  0  1 c −s s c Rotz.Phép biến đổi thuần nhất: • Chuyển động = tịnh tiến + quay 0 0 c 0 0 0 .Rotx.

Bài toán động học thuận của tay máy

• Phép chuyển đổi tọa độ được biểu diễn bằng ma trận chuyển đổi thuần nhất: n0(q) s0(q) a0(q) p0(q) T0(q) =  0 0 1   0
) • Trong đó p0(q là véc tơ định vị, n0 (q), s 0 (q), a0 (q) là các véc tơ định hướng dưới dạng cosin chỉ phương của phần làm việc. Chẳng hạn với ma trận thuần nhất có thể chọn như sau:
T
0

− − (q ) =  −  −

a 12 − − −

a 13 a
23

− −

a 14  a 24   a 34   1 

Các phần tử a12; a13; a23 là các phần tử định hướng, các phần tử a14; a24; a34 là các phần tử đinh vị. Như vậy chỉ cần 6 phần tử để mô tả định vị và định hướng.

Nguyên lý Denavit-Hartenberg (D-H)

ai: khoảng cách giữa hai khớp theo phương đường vuông góc chung. i là góc quay quanh trục zi-1 để xi-1 đến trùng với xi. di : khoảng cách giữa giao điểm của hai đường vuông góc chung với trục quay. tính theo phương của đường vuông góc chung. i là góc quay quanh trục xi để zi-1 đến trùng với zi. .

di i i ) 0 0 d 1 i =  c   s    0   0 − c s 0 0  i  i 0 0 1 0       . R ( 0 . 0 . θι) A i − 1 i ' = R ( Zi . Biến đổi hệ quy chiếu Oi-1 trùng với hệ quy chiếu Oi: • Tịnh tiến Oi-1  Ói (di theo trục zi-1) • Quay O’i quanh trục z’i bằng ma trận quay R(z’i.  i ).

•Tịnh tiến O’i theo trục x’i một lượng ai •Quay Ói ở bước trên quanh trục x’i góc để hoàn thiện. Ai i' 1 0 =  0  0 0 ci si 0 0 − si ci 0 ai  0  0   1  .

• Ma trận biến hình tổng hợp đạt đựơc bằng cách nhân hai ma trận trên có dạng: ci − si ci si si ai ci  s  ci ci − ci si ai si  i i−1 i−1 i ' Ai (qi ) = Ai' Ai =  0 si ci di    0 0 1  0 .

Một số ví dụ ứng dụng quy tắc DH: .

.

Tay máy ba khâu phẳng: .

• Thay các thông số tương ứng vào các ma trận mẫu tổng quát nói trên nhận được ma trận biến hình cho từng bước như sau: ci −si s c i− 1 i i A () = i i 0 0  0 0 0 0 1 0 aici   aisi  0  1 • Khi nhân các ma trận này với nhau có ma trận chuyển đổi tổng hợp: c −s 0 ac +a c +a c  123 123 11 2 12 3 123 s  123 0 as +a s +a s  11 2 12 3 123 0 0 1 2  123 c T (q =A A A = ) 1 2 3 3 0 0 1  0   1 0 0 0  • Ở đây kí hiệu: c123 = cos( 1 + ϑ2 + ϑ3 ) ϑ .

Z3 Z0 Y0 Z1 Y1 Joint 3 O3 X3 d2 Joint 1 O0 X0 Joint 2 O1 X1 O2 X2 Y2 a0 a1 Joint i 1 2 3 i 0 -90 0 ai a0 a1 0 di 0 0 d2 i θ0 θ1 θ2 .

T i i −1 C  i  S i =   0   0 − C  iS i C  iC  i S i 0 S iS i − S  iC  i Ci 0 cosθ1  sinθ 1 T 2=  1  0   0 a iC  i  aiS i   di   1  0 0 −1 0 − sin 1 cos 1 0 0 a1 cos 1  a1 sin 1   0   1  cosθ0 − sinθ0 sinθ cosθ0 0 1  T 0=  0 0  0  0 0 a0 cos0  0 a0 sin0   1 0   0 1  cosθ2 − sinθ2 sin cos2 2 3  T 2=  0 0  0  0 0 0 0 0  1 d2   0 1 1 3 T03 = (T 0)(T 2)(T 2) 1 .

Tay máy tọa độ cầu: Sơ đồ động và bảng thông số DH cho thấy như hình vẽ: .

Từ bảng thông số DH có các ma trận chuyển vị thành phần như sau: c1 0 − s1 s 0 c 1 A0 (1) =  1 1  0 −1 0  0 0 0 c2 0 s  1 0 A ( ) =  2 2 2 0 0   0 1 0 s2 0 0 − c2 0   1 0 d2   0 0 1 1 0 A2(d3) =  3 0  0 0 0 0 1 0 0  0 1 d3  0 0 1 • Nhân các ma trận trên với nhau có ma trận chuyển vị tổng hợp: c c2 −s1 c s2 c s2d3 −s1d2 1 1 1 s c c s s s s d +c d  1 1 2 1 2 3 1 2 0 0 1 2 1 2 T (q) = A A A = 3 1 2 3 −s2 0 c2 c2d3    1 0 0 0  .• Vì z0 và z1 cắt nhau nên d1 = 0.

Robot Stanford .

Z7 X7 X6 Z6 Z5 Z4 X4 Z3 X5 X2 Z1 X3 Z2 X1 Y1 .

• DH Parameters of Stanford Arm i 1 2 3 4 5 6 ai 0 0 0 0 0 0 di d1 d2 d3 (var) 0 d5 d6 i 90° 90° 90° 90° 0° 0 i 1 2 90° 4 5 6 .

# 1 2 3 4 5 Fig.31 The frames of the Stanford Arm. 2.  1 2 0 d 0 a 0 0 0 0 0 0  -90 90 0 -90 90 0 d1 d1 0 0 0 4 5 6 6 .

Robot Scara Khâu 1 2 3 4 i 1* * 2 i Li L1 L2 0 0 Di H 0 * d3 0 1800 0 0 0 * 4 d4 .

0 . 0 .T 01 C1 S = R ( z . 4 ) =  4 0  0 − S4 C4 0 0 0 0 1 0 0 0  d4  1 . 0 ) =  2 0  0 − S2 C2 0 0 0 0 −1 0 L2C 2  L2 S 2   0   1  T 34 C 4 S = R ( z .T ( l 2 . d 3 ) =  0  0 0 1 0 0 0 0 1 0 0 0  d3  1 C 2 S = R ( z .T ( l1 . 0 .  1 ). 0 ) =  1 0  0 − S1 C1 0 0 0 0 1 0 L1 C 1  L1 S 1   H   1  T12 T 23 1 0 = T ( 0 .  2 ).

T12 =  0   0 − S12 C12 0 0 L1C1 + L2C12  L1 S1 + L2 S12    −1 H  0 1  0 0 C12 − S12 0 L1C1 + L2C12 S C12 0 L1S1 + L2S12   T03 = T01.T .T12.T .T = 04 01 12 23 34 0 0  0 0 0 LC +L C  1 1 2 12 0 LS1 +L S12  1 2  −1 −d3 −d4 +H  0 1  .C12 S  12 T02 = T01.T23 =  12 0 0 −1 − d3 + H    0 0 1 0  C −S123 123 S 123 C 123  T =T .

.

Puma .

Example: Puma 560 .

Example: Puma 560 Joint i 1 2 3 4 5 6 i θ1 θ2 θ3 θ4 θ5 θ6 i 0 -90 0 90 -90 0 ai(mm) di(mm) 0 0 a2 a3 0 0 0 d2 d3 d4 0 0 .

Forearm of a PUMA d4 x3 a3 y3 x5 y5 x4 z4 x6 z6 Spherical joint .

Example: Puma 560 Different Configuration .

Link Coordinate Parameters PUMA 560 robot arm link coordinate parameters Joint i 1 2 3 4 5 6 i θ1 θ2 θ3 θ4 θ5 θ6 i -90 0 90 -90 90 0 ai(mm) di(mm) 0 -20.32 0 0 0 0 0 433.25 431.09 .07 0 56.8 149.

Example: Puma 560 .

Động học ngược Inverse Kinematics From Position to Angles .

y) Trong tính toán: y θ = arctan2( ) x S Kết quả: Y 1 X S = (x2 + y2 ) .Ví dụ Tìm : y θ = arctan( ) x (x .

y) 1 l1 .Cơ cấu hai khâu phẳng: (x . x . y) 2 Given: l1. y Find: 1. 2 l2 (x . l2 .

Phương pháp hình học 2 l2 (x . y) Sử dụng định luật Cosines: c 2 = a 2 + b2 − 2ab cosC ( x + y ) = l + l2 − 2l1l2 cos(180 − θ2 ) 2 2 2 1 2 cos(180 − θ2 ) = − cos(θ2 ) l1 1  x 2 + y 2 − l1 − l2 cos(θ2 ) = 2l1l2 2 2 Sử dụng định luật Cosines:  x 2 + y 2 − l12 − l2 2   θ2 = arccos   2l1l2   θ 2 có thể ở góc tọa độ đầu tiên hay thứ tư sin B sin C = b c sinθ1 sin(180− θ2 ) sin(θ2 ) = = 2 2 l2 x +y x 2 + y2 θ1 = θ1 + α  y α = arctan2  x  l sin(θ )   y 2   2 θ1 = arcsin + arctan2  2 2   x +y x   .

y)  c 1 = cos θ 1 c 1 + 2 = cos( θ 2 + θ 1 ) l1 1 (1) x = l 1 c 1 + l 2 c 1 + 2 (2) y = l 1 s 1 + l 2 sin (3) θ = θ 1 + θ 2 1+ 2 (1)2 + (2) 2 = x 2 + y 2 = = l1 c1 + l2 (c1+2 ) 2 + 2l1l2 c1 (c1+2 ) + l1 s1 + l2 (sin1+2 ) 2 + 2l1l2 s1 (sin1+2 ) 2 2 2 2 2 2 = l + l2 + 2l1l2 (c1 (c1+2 ) + s1 (sin1+2 )) 2 1 2 2 2 ( ) ( ) = l1 + l2 + 2l1l2 c2  x 2 + y 2 − l12 − l2 2   ∴θ 2 = arccos   2l1l2   Only Unknown Note : sin( −b) = (cos )(sin )+(cos )(sin ) a+ a b− b a cos( −b) = (cos )(cos )−(sin )(sin ) a+ a b+ a b .Phương pháp đại số 2 l2 (x .

cos( −b) = (cos )(cos )−(sin )(sin ) a+ a b+ a b = l1 s 1 + l 2 s 1 c 2 + l 2 s 2 c 1 = c 1 ( l 2 s 2 ) + s 1 ( l1 + l 2 c 2 ) x + s1 (l 2 s 2 ) c1 = ( l1 + l 2 c 2 ) x + s1 (l 2 s 2 ) y = ( l 2 s 2 ) + s 1 ( l1 + l 2 c 2 ) ( l1 + l 2 c 2 ) = Thay thế cho c1 và đơn giản hóa nhiều lần 1 x l 2 s 2 + s 1 ( l1 2 + l 2 2 + 2 l1 l 2 c 2 ) ( l1 + l 2 c 2 ) y ( l1 + l 2 c 2 ) − x l 2 s 2 x2+ y2 ( ) Chú ý đay là định luạt cosin và có thể được thay thể bằng x2+ y2 s1 =  y ( l1 + l 2 c 2 ) − x l 2 s 2 θ 1 = arcsin   x2+ y2      .x = l1 c 1 + l 2 c 1 + 2 = l1 c 1 + l 2 c 1 c 2 − l 2 s 1 s 2 = c 1 ( l1 + l 2 c 2 ) − s 1 ( l 2 s 2 ) y = l 1 s 1 + l 2 sin 1+ 2 Note : sin( −b) = (cos )(sin )+(cos )(sin ) a+ a b− b a Chúng ta biết rằng θ2 đã xác định ở trên slide. Chúng ta cần tìm θ1 .

Phương pháp đại số Khảo sát một tay máy có 3 khâu Chúng ta thu được phương trình động học: B W T= T= T T T 0 3 0 1 2 1 2 3 .

Phương pháp đại số Khảo sát một tay máy có 3 khâu Chúng ta thu được phương trình động học: B W T= T= T T T 0 3 0 1 2 1 2 3 .

Phương pháp đại số D-H transfaormation ∧ ∧ X Y 1 3 ∧ ∧ 0 Y ∧ 3 Y ∧ 2 ∧ Y X X 1 2 ∧ X 0 i i-1 0 i-1 0 di 0 i 1 2 3 1 2 0 L1 L2 0 3 0 0 .

Phương pháp đại số  c 1  s c 0 T = 1 0 1  s1 s 0   0  c 2  s 1 T = 2 2  0   0   c 1 c  1c  0 − s  0 − s  0 d 1   s  1 = c 1 s 0 c 0 c 0 d1   0   0 0 1   0 − s 2 0 L1   c 3 − s 3  s c 2 0 0  2   3 c 3 3T =  0 0 1 0 0   0 0 1 0  0 0 a0 − s1 − s1 c 1 0 0 0 0 1 0 L2  0  0  1 0 0 0 0  1 0  0 1 .

Phương pháp đại số Ta có: c123 − s123 s c123 123 B 0  WT = 3T = 0 0  0 0 0 l1c1 + l2c12  0 l1s1 + l2 s12    1 0  0 1  Giả sử c s B  WT = 0  0 − s c 0 0 0 x  0 y 1 0  0 1 .

c2 ). .Phương pháp đại số Từ đó ta có c = c123 s = s123 x = l1c1 + l2c12 y = l1s1 + l2 s12 Bình phương 2 phương 2 2 2 2 x + y = l1 + l2 + 2l1l2c2 trình cuối ta có: Từ đây ta có biểu thức cho c2 Và cuối cùng: x + y −l −l c2 = 2l1l2 2 2 2 1 2 2 2 2 s2 = ± 1 − c ⇒ 2 = A tan 2(s2 .

Đặt r=+ k12+k22 and γ=Atan2(k2.Phương pháp đại số Ta có c12=c1c2-s1s2 và s12= c1s2-c2s1: x = k1c1 − k2 s1 y = k1s1 + k2c1 Trong đó k1=l1+l2c2 và k2=l2s2.k1). .

sin  sin 1 y/r= cos  sin 1 + sin  cos 1 hoặc: cos(+1) = x/r.Phương pháp đại số k1 l2 θ2 γ l1 k2 Trong đó: k1=r cos  . sin(+1) =y/r . k2=r sin  và chúng ta có thể viết x/r= cos  cos 1 .

x/r) = atan2(y.x) .atan2(k2.k1) Cuối cùng.Phương pháp đại số Do đó Suy ra : γ+θ1 = atan2(y/r.x) θ1 = atan2(y. θ3 có thể được rút ra từ θ1+ θ2+ θ3 = φ .

You're Reading a Free Preview

Tải về
scribd
/*********** DO NOT ALTER ANYTHING BELOW THIS LINE ! ************/ var s_code=s.t();if(s_code)document.write(s_code)//-->