Động Động họ học ngượ ngược c robot ở đầu M ở
Động học ngượ c: Nhằm tính toán các thông số biến khớ p khi vị trí của đầu cuối công tác đã biết Phát biểu bài toán
Cho tr ướ ướ c một ma tr ận biến đổi homogenous 4x4 sau:
⎡n x ⎢n y K = ⎢ ⎢nz ⎢ ⎣0
ox
ax
px ⎤
oy
ay
oz
az
0
0
⎥ pz ⎥ ⎥ 1⎦
py ⎥
(1)
Tìm một hay tất cả nghiệm θ = (θ 1 , θ 2 ,..., θ n ) của phươ ng ng trình sau T
0 n
T (θ1, K, θn ) = K
(2)
trong đó 0 n
T (θ1 ,K, θn ) =10 T(θ1 ) Knn −1 T(θn )
(3)
và K biểu diễn vị trí và hướ ng ng mong muốn của cơ cấu tác động cuối Phươ ng ng trình (2) đưa đến việc gi giải 12 phươ ng ng trình phi tuyến vớ i n ẩn sau: Tij
= (θ1 ,K, θn ) = kij , i = 1,2,3 , j = 1,...,4
(4)
trong đó Tij , k ij tươ ng ng ứng là 12 phần tử có giá tr ị của T n0 và K (vì hàng cuối cùng của cà T n0 và
K đều là (0,0,0,1) , nên 4 trong số 16 phươ ng ng trình từ (2) sẽ không có giá tr ị). Có 6 phươ ng ng trình quan hệ giàng buộc giữa n x , n y , nz , ox , ... nên chỉ có 6 phươ ng ng trình độc lậ p. Ví dụ
Figure 1: Stanford robot
− s4 s6 ) − s2s5 c6 ] − s1 (s4 c5 c6 + c4 s6 ) s1 [c 2 (c 4 c5 c6 − s4 c6 ) − s2 s5s6 ] + c1 (s4 c5 c6 + c4 s6 ) − s 2 (c 4 c 5 c 6 − s 4 s 6 ) − c 2 s 5 s 6 c1 [ −c2 (c 4 c5 c6 + s4 c6 ) + s2 s5 s6 ] − s1 (−s4 c5 s6 + c4 c6 ) s1 [ −c 2 (c4 c5 c6 + s4 c6 ) + s2 s5 s6 ] + c1 (−s4 c5 s6 + c4 c6 ) s 2 (c 4 c5 c6 + s4 c6 ) + c2 s5 s6 c1 (c2 c4 s5 + s2 c5 ) − s1 s4s5 s1 (c 2 c 4s5 + s 2 c5 ) + c1s4s5 −s 2 c 4 s 5 + c 2 c 5 c1s 2 d3 − s1d 2 + d6 (c1c2 c4 s5 + c1 c5 s2 − s1s4 s5 ) s1s 2d 3 + c1d 2 + d6 (c1s4 s5 + c2 c4 s1s5 + c5 s1s2 ) c 2 d3 + d 6 (c2 c5 − c4 s2 s5 ) c1 [ c2 (c4 c5 c6
= = = = = = = = = = = =
nx ny nz ox oy oz ax ay az px py pz
Các thách thứ c của bài toàn động học ngượ c
− Các phươ ng ng trình trên r ất khó giải tr ực tiế p, và vấn đề này cũng xảy ra đối vớ i hầu hết các tay máy robot.
− Vì thế, ta cẩn phải tìm các k ỹ thuật hiệu quả có hệ thống để khai thát cấu trúc động học đặc biệt của từng tay máy.
− Trong khi vấn đề động học thuận luôn có lờ i giải duy nhất, chỉ đơ n giản thay các giá tr ị biến khớ p vào các phươ ng ng trình động học thuận, thì vấn đề
động học ngượ c có thể không có lờ i
giải.
− Ngay c ả khi tồn tại một lờ i giải, nó cũng có thể không là lờ i giải duy nhất. ng trình động h ọc thuận nói chung là các hàm phi tuy ến phức tạ p đối vớ i biến − Các phươ ng khớ p, nghiệm cho bài toán động học ngượ c có thể r ất khó giải đượ c ngay cả khi nghiệm tồn tại. Vấn đề đa nghiệm
Figure 2:
Figure 3
Các phươ ng ng pháp giải
−
Phươ ng ng pháp hình học
−
Phươ ng ng pháp đại số
−
Phươ ng ng pháp số (máy tính)
Ví dụ 1
Figure 4: Tay máy 2 khâu ph ẳng
Bảng tham số khâu cho robot 2 khâu đồng phẳng Khâu ai α i
d i
θ i
1
a1
0
0
θ 1
2
a2
0
0
θ 2
⎡cos θ1 − sin θ1 ⎢ sin θ cos θ 1 1 0 ⎢ 1T = ⎢ 0 0 ⎢ 0 ⎣ 0 ⎡cos θ2 − sin θ2 ⎢ sin θ cos θ 2 2 1 ⎢ 2T = ⎢ 0 0 ⎢ 0 ⎣ 0 Ma tr ận biến đổi của khâu cuối so vớ i hệ toạ độ gốc ⎡cos(θ1 + θ2 ) − sin(θ1 + θ2 ) ⎢ sin(θ + θ ) cos(θ + θ ) 1 2 1 2 0 0 1 T2 =1 T2 T = ⎢ ⎢ 0 0 ⎢ 0 0 ⎣ Phươ ng ng trình động học robot n x = cos(θ1 + θ2 ) o x ny
= sin(θ1 + θ2 ) nz = 0
= − sin(θ1 + θ2 ) o y = cos(θ1 + θ2 ) oz = 0
Giải phươ ng ng trình động học ngượ c
0
a1 cos θ1 ⎤
0
⎥ ⎥ 0 ⎥ 1 ⎦ a 2 co cos θ2 ⎤ ⎥ a 2 si sin θ2 ⎥ ⎥ 0 ⎥ 1 ⎦
a1 sin θ1 ⎥
1 0 0 0 1 0 0
a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎤
0
a1 sin θ1 + a 2 sin(θ1 + θ2 ) ⎥
1
0
0
1
⎥ ⎥ ⎥ ⎦
=0 ay = 0 az = 0 ax
= a1 cos θ1 + a 2 cos(θ1 + θ2 ) p y = a1 sin θ1 + a 2 sin(θ1 + θ2 ) pz = 0
px
0 2
T(θ1 , θ2 ) = K
Nghiệm chỉ tồn tại khi ma tr ận K có dạng
⎡cos ϕ − sin ϕ ⎢ sin ϕ cos ϕ K = ⎢ ⎢ 0 0 ⎢ 0 ⎣ 0 ⎡cos(θ1 + θ2 ) − sin( θ1 + θ2 ) ⎢ sin(θ + θ ) cos(θ + θ ) 1 2 1 2 ⎢ ⎢ 0 0 ⎢ 0 0 ⎣
0 0 1 0
⎧ sin 2 θ1 + cos2 θ1 = 1 ⎨ 2 2 ⎩sin θ2 + cos θ2 = 1 ⎧ x = a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎪ y = a sin θ + a sin(θ + θ ) ⎪ 1 1 2 1 2 ⎨ 2 2 sin θ1 + cos θ1 = 1 ⎪ ⎪⎩ sin 2 θ2 + cos2 θ2 = 1
x 2 + y2
− a 2 sin θ1 sin θ2
+ a 2 cos θ1 sin θ2
= a21 + a 22 + 2a1a 2 cosθ2 1
{( x
cos θ2
=
sin θ2
= ±(1 − λ 2 )1/ 2
2a1a 2
2
y⎥
1
0⎥
0
1⎦
⎥ ⎥
⎡ cos ϕ − sin ϕ ⎥ ⎢ sin ϕ cos ϕ a1 sin θ1 + a 2 sin(θ1 + θ2 ) ⎥=⎢ ⎥ ⎢ 0 0 0 ⎥ ⎢ 1 0 ⎦ ⎣ 0
Mặt khác ta cũng có
y = a1 si sin θ1 + a 2 sin θ1 cos θ2
x⎤
0
a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎤
⎧ x = a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎨ ⎩ y = a1 sin θ1 + a 2 sin(θ1 + θ2 )
x = a1 co cos θ1 + a 2 cos θ1 cos θ2
0
+ y2 ) − ( a12 + a 22 )} = λ
0 0 1 0
x⎤
⎥ ⎥ 0⎥ ⎥ 1⎦ y
cos θ1 = sin θ1
=
1 x
2
2
{ x (a
2
{ m xa
+y 1
x
2
+y
1
2
+ a 2 λ) ± ya ya 2 (1 − λ 2 )1/ 2 } (1 − λ 2 )1/ 2
+ y(a1 + a 2 λ )}
Ví dụ 2
Figure 5: Tay máy 3 kh ớ p
Figure 6
Figure 7: Chiếu tâm cổ tay lên mặt phẳng x0-y0
Từ phép chiếu trên Figure, ta có θ 1
= A tan( xc , yc )
xác định vớ i mọi ( xc , yc ) ≠ (0,0) và có nghiệm
(4.8)
cos θ1
Các nghiệm θ 1 đều hợ p lệ ngoại tr ừ xc
=
xc x c2 + yc2
; sin θ1
=
yc x c2 + yc2
(4.9)
ng hợ p này, (4.8) không xác định và ườ ng = yc = 0 . Trong tr ườ
tay máy ở vị trí k ỳ dị (singular configuration), trên hình 4.3
Hình 4.4 :
V ị trí k ỳ d ị (vô số nghiệm)
Để tìm θ 2 , θ 3 khi biết tr ướ ướ c θ 1 , ta xét mặt phẳng tạo bở i khâu 2 và khâu 3 (hình 4.8). Vi chuy ển động của khâu 2 và 3 trong mặt phẳng. (còn tiế p, trình bày sau)
Hình 4.8:
Chiế u lên mặ t phẳ ng ng t ạo bở i khâu 2 và 3
