Để xác định dịch chuyển vi sai, hoặc tốc độ cần thiết của các khớp robot tương ứng với dịch chuyển vi sai hoặc tốc độ của tay robot, cần phải tính ma trận Jacobien nghịch đảo theo phương trình sau:
D=1.D,
Từ đó nhận được: Dạ =J'”.D (3-32)
93
Tương tự: Dạ= "J*'D (3-33)
Phương pháp tính theo (3-32) và (3-33) phức tạp và thời gian tính toán lớn, nên ít được dùng trong thực tế. Sau đây trình bày phương pháp tính toán các dịch chuyển vi sai hoặc tốc độ của robot có cấu hình ở hình 2.21 bằng sử dụng phương trình động học ngược đã dẫn ra ở mục 2.7.
Sử dụng phương trình (2-43a):
SĂ.P,-CĂ.Py = ệ (3-34)
Vi phân phương trình (3-34), sẽ tìm được dÕ,,
Sị.D, = C¡-Py C;.p,dỉ; + S,dp, = -S,.p,d0, + C, dp,
Suy ra: d@, = nh —Š4P, (3-35)
iP, + S.dp,
Su dung phuong trinh (2-52):
Sy(Ca, + Say) = Ca, = 0 — (3-36)
Vị phân (3-36) ta có:
C,3,(d8, + dO, + dO,) (Cya, + Syay) + Sj,4(-S,a,d8, + C,a,d0, + C,da, + S,da,)
= -S,,,(d@, + dO, + dO,)a, + C,,,da, (3-37) Do đó ta có:
d0, + d0, + đô, = S23„(—S,a,dÓ, + Ca,d0, + C,da, + S,da,) + C.;„đda,
S934, + Cy34(Cia, + S,a,)
(3-38) Trong phương trình (3-38), các vi phân da,, da, và da, đã được xác định từ phương trình biểu diễn vi phân đT:
dn, do, da, dp dn, do, da, dp
dT = y y ỳ (3-39)
Tiếp theo vi phân phương trình (2-48) dé tìm d0.
2a,a,C, =(C\p, +S\p, —C),,a,)’ +(p, -Syya,)? -a2-a? — 3-40)
94
—2a,a,8,d0, = 2(C\p, +S,p, - C3444)
x[C,dp, —-S,p,d0, +S,dp, +C,p, +4,S,,,(d0, + dO, + d@, )]
+2(p, ~S.44,) [dp, —a,C,;,(d0, + d0, + dO, )]
(3-41)
Tương tự, vi phân phương trình (2-56) sẽ xác định được d0;,
Sz[(Csa; +a2)” +S3a1]=(Cạa; +a2)J(P; —S2;4a4)—S34;(PxC¡ +Py§¡ —t 23444)
(3-42)
C;d62[(C¿ay +a;)2 +S3a1]+ 2S2[(Caa +5 )(—a3$3d03) + 2a3S,C3d05]
= —a383d03(p, — 48234) + (a3C3 + 42 )[dp, — a4C234(d82 + dO; + dO,)]
—a3C3d03(p,C, + pyS; ~ C3444)
~a3S3[C,dp, —p,S,d0, + Sydp, + pyC,d®, +a4S234(d02 + d03 + dO, )]
(3-43) Str dung (3-38), (3-41) va (3-43) sé x4c dinh dugc d®,, dO, va d@,.
Vi phân phương trình (2-59b) sẽ nhận được công thức tính dÔ.,
C; = -C¡ay + S4, (3-44)
-5;d6; = a,S,d0, —C,da, + a,C¡d9; +5¡da, (3-45) Cuối cùng sử dụng phương trình cân bằng hai phần từ (2,1) hai vế để tính ra dé, :
Se = - Sp3a(Cin, + Syny) + Cysgt, (3-46)
Vị phân phương trình (3-46) nhận được phương trình để giải được dÔ,.
Czd0; = -C;„(C,n, + S,n,)(dÔ; +dÕ; + dÕ„)
- Sa;(-5,n,d9, +C,dn, + C,n d0, +S,dn,) (3-47)
- 1Sy,4(d0, +d0, + d9,) + C,;;dn, Như vậy bằng 6 phương trình vi phân sẽ xác định 6 giá trị vi sai vị trí của các khớp. 6 phương trình đó sẽ được sử dụng để lập trình điều khiển robot và tính tốc độ của các khớp robot.
Ví dụ 3.7: Cho robot có cấu hình như hình 3.8 (xem ví dụ 2.15). Tính tốc độ của khớp 1 khi biết tốc độ dài và tốc độ góc của tay robot như sau:
‘dx/dt = 2,4 cm/s ; dy/dt = 4,8 cm/s ; 6x/dt = 0,1 rad/s.
0,=0°;0;=90°; 0;= 0°; 0,= 90; 0;= 0”; = 45” - a, = 0,5 m; a; = 0,5 m; a¿ = 0,1 m.
95
Hình 3.8. Cấu hình và các khung toa dé của thanh nối
robot dạng khớp nối 6 bậc tự do.
Bảng 3.2. Bảng D — H của robot
Khung toa độ 8 d a œ
1 9, 0 0 90°
2 6, 0 a 0
3 0, 0 a 0
4 0, 0 -90°
5 0; 0 0 90°
6 06 0 0
Sử dụng kết quả của ví dụ 2.15, thay giá trị các vị trí các khớp ở bảng
D — H vào phương trình động học thuận robot (xem ví dụ 2.15), với giả thiét vi trí của robot được lấy mốc theo trục x, sẽ tính được ma trận FT;,, biểu diễn vị trí và hướng cuối cùng của tay robot:
Ti, = “Ai. 1A, 7A Ay As Ag
96
C¡(C2¿C;C¿ —S¿2;zS¿) Cy(—Cr34CsCo -S234Co) CyCy348s5 Cy (Ca34-84 +
~$,85Ce +$,85S¢ $S\Cg +C;sa; +C3;)
Si(C2a¿CzCs —S2zzS¿) S1(-Caz4CsCo -Sa34Co) S,C2jzŠ3; S¡(C234-324 +
+C¡S;S ~C¡SsS -C\Cg +Ca;ay+C;3¿)
Sz;.C;C¿ + C23454) -Sy34CsCg +Cy34Ce) - S25; 52344 †
+§Š2‡4: +Š¿.4;
0 0 0
ny Ox ax Px | |-9,707 0,707 0 -01
_|My Oy ay Py |_ 0 0, -1 0
“\n, 0, a Pz| |-9707 -0,707 0 1
0 1 0 0 0 1
Từ các giá trị tốc độ dịch chuyển tịnh tiến và dịch chuyển quay, tính được toán tử vi sai theo công thức (3-6):
0 -ðz dy dx 0 0 0 0,024
A= ỗz 0 -ðx dy - 0 0 -01 -0/048
-ðy 5x 0 dz 0 01 0 0
0 0 0 0 90 0 0 0
Từ đó xác định được ma trận dịch chuyển vi sai của T:
0 0 0 0,024
0/0707 0,0707 0 -0,148
dT=AT= 0 0 0 0
0 0 0 0
Do d6: dp, = 0,024; dp, = -0,148 ; dp, = 0;
Theo công thức (3-35) tính được:
C,dp, —S¡d ~ _
d6, __1 Py —53I8Px _ 1á 0,148) 040/024) _ 1 4§rad/s dt Cip, + SPy 1.(-0,1) + 0.(-0,148)
97
Bài tập
1.Vị trí và hướng tay robot được mô tả bởi ma trận ŸT;, dưới đây. Thực hiện quay vi sai xung quanh trục x một góc 0,15 rad; tiếp theo là phép dịch chuyển tịnh tiến vi sai [(0,1),(0,1),(0,3)]”. Xác định vị trí mới của tay robot.
0 0 1 2 1 007 0 1 0 5 0 0 0 1
2. Cho ma trận A biểu diễn 1 khung toạ độ. Khung toạ độ A dịch chuyển tịnh tiến bởi vectơ d = [(),(0),(0,5)]ẽ và vectơ quay 5 =[(0),(0,1),(0)]"
+ Xác định toán tử dịch chuyển vi sai so với khung toạ độ gốc.
+ Xác định toán tử dịch chuyển vi sai so với khung toa độ A.
0 0 1 10 J1 00 5 _l0 100
000 1
3. Cho ma trận biểu diễn khung toạ độ tay robot và ma trận Jacobien tương ứng.
Vectơ dịch chuyển nhỏ của các khớp là D. Xác định toán tử dịch chuyển vi sai và độ dịch chuyển của khung toạ độ tay và vị trí mới của khung toạ độ tay.
Biết:
§ 00000 0
0 1 0 10 -3 01000 0,1
10 0 5 T6 0 10 0 0 0 0; ~ - 0,1
T, = = D =
00 -1 0 0 1 001 0 1 0,2
00 0 1 0 00100 0,2
-1 0 0000 0
4. Cho ma trận T biểu diễn khung toạ độ T va đT là ma tran dich chuyển vi sai của T. Xác định vectơ dịch chuyển tịnh tiến đ, vectơ dịch chuyển quay 5 va toán tử dịch chuyển vi sai A.
98
Biết:
1 0 0 5 0 -0,1 -01 0,6
0 0 1 3
T= dT = 0,1 0 0 0,5
0 -1 0 8 -0,1 0 0 -0,5
0 0 01 0 0 0 0
5. Ma tran T,, T, biéu diến khung toạ độ tay robot tương ứng ở vị trí ban đầu và vị trí cuối cùng.
10 0 5 1 0 OL 48 0 0 -1 3 01 0 -1 3,5 T, = 01 0 6 T, = 0 1 0 62
00 01 000 1
+ Xác định ma trận biến đổi Q.
+ Giả sử các biến là nhỏ, xác định toán tử dịch chuyển vi sai.
+ Xác định vectơ dịch chuyển tịnh tiến và quay vi sai.
99