Ở bài toán động học thuận, vị trí và hướng của tay được xác định từ các biến khớp (góc quay ở khớp quay hoặc độ dịch chuyển ở khớp tịnh tiến) đã biết.
Để điều khiển robot di chuyển theo các vị trí mong muốn của tay trong không gian, cần xác định các giá trị biến khớp tương ứng với vị trí và hướng của tay robot mong muốn. Đây là nội dung của bài toán động học ngược. Bài toán động học ngược thông thường khó giải và không có lời giải tổng quát cho mọi robot. Để minh hoạ cho phương pháp giải bài toán động học ngược, mục này sẽ trình bày động học ngược của robot có cấu hình như hình 2.21.
Trong ví dụ 2.15 mục 2.6 đã xây dựng phương trình động học thuận của robot có dạng:
T1, =A,(,)A;(8;)A;(đ;)A,(,)A,(,)A,(9,)= yoy (2-38)
59
Ma trận T, đã biết, tức là vị trí và hướng của khung toạ độ tay robot đã biết, cần xác định giá trị các biến khớp. Nhân hai vế phương trình (2-38) ma trận nghịch đảo của ma trận A;: (A,)' nhận được phương trình sau:
(AUD TT = A;A2A¿A:A¿ = 'T (2-39)
Cac ma tran A, (i=1+6) da duoc tính ở ví dụ 2.15 mục 2.6. Thay các ma trận thành phần vào (2-39), nhận được phương trình sau:
Cễ SĐ, 0 0ẽẽn, o, a, p,
0 0 10
ny Sy ấy Py ='T, (2-40a)
Ss, =€, 0 0n, o, a, p,
| 0 0 0 I0 0 0 1
if, (n) f,(o) f,(a) f,,(p)
ft(n) f;(o) f;(a) fi, (P) | (2-40)
f;n) f;(o) f(a) fi, ©)
| 0 0 0
trong do: f,,(v) = C,.x+S,.y
f(v) =z (2-40c)
f13(v) = S¡.X- C¡.V
Với x, y, z tương ứng là các thành phần của vectơ v.
Ví dụ: f,,(n) = C,.n,+S,.n, Ma tran 'T, dugc tinh theo (2-39) sir dung cdc ma tran A,(i = 2 + 6) ở vi dụ 2.15 có dạng:
C;„C;C, 525, —C„„C;C¿—§;„C¿ CS, C;ya,+C¡;¿a; + C›a;
SạC;C, +25, —S2uC,Cu+C;JS, 2Š, Sa, +S,ya; +S,a,
-S,C, S5, C, 0
0 0 0 1
(2-41)
Cân bằng các thành phần cột 4 của hai ma trận phương trình (2-40b) với sử dụng (2-41) nhận được các phương trình sau:
fi j0) = Cua, + Ca; + C;a, (2-42a)
fj2(Ð) = S;a¿a¿+ Su;a; + Sa, (2-42b)
f;(p) =0 (2-42c)
Từ (2-40), (2-42c) ta có:
S)-Px- C¡.Py =0 (2-43a)
Suy ra: ỉ, =atan2(P,,p,) (2-43b)
Sử dụng (2-42a) và (2-42b) kết hợp với (2-40c):
ŒC,.x+S,.y= Ca + Caa¿ + Ca, (2-44a)
Pz= 5zÄ¿ † 5; + 52A; (2-44b)
Viết lại hai phương trình (2-44a) và (2-44b) như sau:
C,.x+S,.y - Ca„a¿= C;:ai + C;a, (2-45a)
Đz- 32244 Sy38;,+ Sa, (2-45b)
Bình phương hai vế của các phương trình (2-45) và cộng lại, nhận được phương trình sau:
(Cịp, +S¡Py ~Cy444)” +(p, — S544)” =a; +a; +2a,a,(8,8,, +C,C,,)
(2-46) Sử dụng hàm lượng giác, phương trình (2-46) được viết gọn lại như sau:
S,S,,; +C,C,, =cos[(0,+0,)-0,]=cosO, (2-47)
Do đó nhận được:
(C,p, +$,P, —C53444)° + (Pp, —Sy3444)° a2 a2
C,= iP iPy 3444) +(p 234) 2 3 (2-48)
2a,a, Từ đó xác định được góc Ô; theo công thức:
0, = atan2(S,,C;) (2-49a)
trong dé: S, =4/1-C? (2-49b)
Do trục 2,3,4 song song nhau nên tiếp tục nhân ma trận nghịch đảo của A, — A, sé nhan được phương trình sau:
(A,y! (Ay! (A2)" (AT, =A:A, = "Ts (2-50) Sử dung các ma trận thành phần A, — A, 6 vi du 2.15, phuong trinh (2.50) có dạng:
61
| C2„f,(n) Cạ„f(o) — C;;f,(A) Cosh (P) + Sof (P) | +5z„fy(n) +Sz„f;(0) +S,fÍ;(4) —C¡,a;TC¿a;-a,
—f¡;(n) mà — f,;(a) 0
‘T=
—8;„f(n) —S,„f(0) —S;,„f,() - Sasa (P) + Cosy fio (p) +C„f;(n) +C„„f;(0) +C;„f,(4) +S¡,a; +S,a;
L 0 0 0 I |
[C;C, -C,S, S, 0
_ S;C, -S,S, -C, 0 (2-51)
S, C, 0. 0
| 0 0 0 1
Cân bằng phần tử (3,3) của hai ma trận và sử dụng (2-40) ta có:
ơS;;„(CĂa, + S,ay) + C,,a, = 0 (2-52)
Tir (2-52) giai ra dugc 0,34:
0534 = atan2(a, (Cia, + Siay) (2-53)
Sử dung (2-53), xc dinh dugc S,,, va C34 Va sé xdc định được 0, theo (2-49).
Sử dụng các phương trình (2-44) để tính 9, như sau:
C,.x+S,-y = Cy3,a,+ Ca, + Ca, (2-54a)
Pz = Sy34a4+ S,,a,+ S,a, (2-54b)
Vi: C,, = CC, — SS, va S,, = SC, + C5; nên (2-54) có dạng:
C¡.X+Ši.V = Cạ¿a,+ (CạC; — 8;S;)a; + Ca, (2-55a)
Ðz= Š;;a4,+ (SC; + CS;)a;+ S;a, (2-55b)
Từ (2-55) giải ra được C; và S:
S.= (C;A; +a;)(p, —S;„„a„)— Š;4;(p,Ci +py8¡ —Cz„a„)
(Coa; +a;)” + Sa; (2-56)
C,= (C,a, +a,)(p,C, + PyS,; —Cy44,) +8,a, (p, —S;;„a„)
(C;a; +a;)” + Sa;
Từ (2-56), xác định được góc 0.
8, = atan2(S,,C,) (2-57)
62
Sử dụng (2-49), (2-57) và (2-53) tính được góc 0,:
0, = O54 - 8 0; (2-58)
Cân bằng các phần tử (1,3) và (2,3) của hai ma trận hai vế phương trình (2-51), và sử dụng (2-40c) ta có các phương trình sau:
Ss = Cyas(Cia, + Syay) +5444, (2-59a)
C; = -Cya, + Sa, (2-59b)
Từ (2-59) giải được:
0, = atan2(S,,C,) (2-60)
Để giải góc 0¿, nhân ma trận nghịch đảo của A; cả hai vế phương trình (2-51), nhận được phương trình sau:
[C;[C¿;,f,,(n) +S¿;„f,; (n)] C,[C;;/f¡¡(o)+S,„f„(o)]} 0 0)
—S,f,,(n) —S,f,;(0)
—$34f,, (n) + C,3, Ff) (2) ~ Sash (0) + Cay f.(0) 0 0
0 0 1 0 (2-61)
L 0 0 0 1|
[Cs -S, 0 0
S, C, 0 0
“10 0 1.0
0 0 01
Tir (2-61) giai dugc géc 0,.
0, =a tan 2([-S,,,f,,(n) + Cf}, (n)].[—Sz;,f,(o)+€C„,;„f;(o)]) 2-62)
63
Bài tập
2.1. Ma trận sau biểu diễn một khung toạ độ. Hãy xác định các phần tử chưa biết của ma trận đó:
? 0 -l15
? 0 0 3 E= ? =1 0 2
0 0 01
2.2. Một hệ toạ độ B có gốc và các trục trùng với khung toạ độ gốc A. Điểm P nằm trong hệ toạ độ B được biểu diễn bằng vectơ "5= (5,3,4]" . Xác định ma trận biểu diễn khung toạ độ B ở vị trí mới và vị trí điểm P so với khung toạ độ A:
^ 5. Biết rằng khung toạ độ mới này nhận được bằng các phép biến đổi khung toạ độ B so với khung toạ độ gốc theo thứ tự:
e Quay xung quanh trục x một góc 90”.
e Tịnh tiến theo các trục x, y, z tương ứng 5, 3, 6 đơn vị.
e Quay xung quanh trục z một góc 90!.
2.3. Một đối tượng được mô tả bằng bốn điểm trong khung toạ độ gốc:
A[ 1, 0, 0, 1];
BL 0, 0, 0, 17’;
C[ 0, 0, 1, 1";
D[ 0, 1,0, 1]".
Xác định ma trận biểu diễn phép biến đổi theo thứ tự sau so với khung toạ độ gốc.
e Quay xung quanh trục y một góc 90.
e Quay xung quanh trục z một góc 90°.
e Tịnh tiến theo các trục x 10 don vi.
Xác định đối tượng ở vị trí mới sau phép biến đổi trên.
2.4. Xác định ma trận nghịch đảo của các ma trận sau:
0527 -0,574 0,628 2 0,92 0 0,39 5 T = 0,369 0,819 0,439 5). T. = 0 | 0 6 1 | —0,766 0 0643 3 7 |-039 0 0,92 2
0 0 0 1 0 0 0 1
2.5. Xác định các góc RPY biểu diễn một khung toa độ tay robot có dạng:
0527 —0,574 0,628 4 0,369 0,819 0,439 6],
— 0,766 0 0,643 97
0 0 0 1
T =
64
2.6. Xác định các góc EULER biểu diễn hướng của khung toa độ tay robot mô tả bằng ma trận sau:
0527 -0,574 0,628 4 T = 0,369 0,819 0,439 6],
~ | — 0,766 0 0,643 97
0 0 0 1
2.7. Một robot ba bậc tự do có cấu hình như hình 2. 22.
(a) Xây dựng bảng tham số D— H.
(b) Viết các ma trận A..
(c) Xác định ma trận T biểu diễn điểm cuối cùng của thanh nối 2.
Hình 2.22. Cấu hình robot 3 bậc tự do.
2.8. Một robot bốn bậc tự do như hình 2.23 (a) Thiết kế các khung toạ độ cho các thanh nối.
(b) Xây dựng bảng tham số D-H.
(c) Viết các ma trận A,.
(d) Xác định ma trận T biểu diễn tay robot trong khung toạ độ chuẩn.
Hình 2.23. Cấu hình robot 4 bậc tự do.
65