PHƯƠNG TRỈNH ĐỘNG HỌC CỦA ROBOT
6.5. GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC CỦA ROBOT STANFORD
Đối với robot Stanford, Tft là tích cùa 6 ma trận A trình bày trên toán đồ ở hình 6.2.
N. A١
0
Ai
Ai،؛
A A٦
؟ A
'ư
'Te
'.TI
T
؛؛6 0
H ình 6.2. Toán đồ ma trận biến đổi của robot Stanford
Tft = A|A2A١A4A٩A(١ Liên tục nhân (6.35) với các ma trận nghịch đảo. ta được:
(6.35)
A p T , = ' T ,
A ĩ A | ٠T(١ = ' T , a ١'a; 'a í-'t. = ١١T6
a - 'a : 'a; 'a- 't٠١ = ■.T.S
a ^'A 4 'a : 'a ; 'a - 't, = ■١ T6
Các phần tử ma trận ở vế trái cùa phương trình này là hàm sô' của các phần tử và các biến số của khớp (n-1). Trong khi dó các phần tử ma trận ở vế phải hoặc bằng 0, bằng const hoặc là hàm số cùa biến sô thứ n cùa khớp thứ 6. Từ mỗi phưctng trình ma trận, cho cân bằng các phần tử tương đương ta nhận được 12 phương trình. Mỗi phương trình là một phần tử của 4 vectơ n١ 0, a và p. Điều này bạn đọc đã được biết đến trong mục 5.8.
Bây giờ hãy trở lại phương trình (6.35).
* Nếu nhân (6.35) với A f ' ta có:
A،| 'Tfi = A2A١A4A؟Ae = ' Tf, Vế trái là:
A r'T .
c , s , 0 0 "x 0 ١ a١ Px
٠ 1
f , , ( 0 ) ؛.n ( a) f i i ( p ) 0 0 - 1 0 n؛, 0 ١ a١ p.١ f | 2 ( n ) f٠2 ( 0 ) f i2( a ) f . 2 ( p ) - s , c , 0 0 ٠١/ 0 ’ a ’ P/ fi:١(n) f ,٥)١) f i١( a ) f |١( p )
0 0 0 0 ٥ 0 0 1 0 0 0 1
trong đó:
fii = c ,x + s,y f|2 = -z
fi:.= - s ,x + c ,y Vế phải là;
T.
؛ Sf
<؛
S2S + ( S 4C٨ +
١ C2(C4C١S(
— S2S٩Sf ؛
— 848(؛)
—
؛
C2(C4C٩Cf C2C4S١+S2C, Sid,
S2(C4C ,C ,-S4S٠١)+C2S١C ,- S2(C4C٩S,+S4C٨)-C2S٩S, S2C4S؟+C-)C١ —C2d١
؛.
S4C5Cft+C4S -S4C,S,+C4C, 848, ٥2
0 0 0 1
Các phần tử của ma trận vế phải đều là hàm số của 9 ,, 6١,, 84, 60 ,؟ft, ngoại trừ phần tử hàng 3 cột 4, đó là;
fi.١(p) = d,
hoặc -S |P ١ + C|P١, = d,
Để giải phương trình ở dạng này ta có thể thay thế bời các hàm lượng giác sau đây:
p١ = r.coscị) p١, = r.sinệ
r = + ١/ p ; + p Ị
trong đó:
(ị) = arctg^^١
vPxy Thay thế p١ và p١, vào phương trình -S |P ١ + C|P = d, ١ ta có:
^
٥
٢7
٧ ؛0 < — < 1
sinộcosGị - cos(ị)sin0ị = — vớd->
r Hay là:
sin((Ị) - 0|) = — v ớ i O < ( ị) - 0d ٠<7i
٩ r
Từ đó ta có:
cos((ị) - 0|) = ±١/l - ( d 2 / r ) .
Irong đó dấu phù hợp với hình thể vai trái của robot và dấu “+ ٠١ phù hợp với hình thê vai phải của robot. Cuối cùng:
0| = arctg
v P x y
arctg ^ 2 (6.36)
Nếu tính được 6, thì vế trái của phương trình A['T(١ = ' T(؛ được xác định.
Cho cân bằng các phần tử hàng 1, cột 4 và hàng 2, cột 4, ta có:
“ ؛-iPx ■٠■ ^iPy
— C2d١ = —p٩
Ta yêu cầu d, là dịch chuyển dài ra của khớp tịnh tiến phải lớn hcm 0, ta sẽ định lượng được tỷ số sin và cos của 0, và tìm được giá trị duy nhất của 02 là:
C | p ١ + S i P y
02 = arctg
p. (6.37)
* Nếu nhân (6.35) với A2. A ؛"' ta được:
A ;'A 7 ' \ = A,A،٠A١A ,= ؛ T, Khai triển phương trình này ta có:
f2|(n) ؛21(0) ؛2i(a) 0 C4C٩C ,-S ,S ٠١ -C4C,S,-S4C, C4S5 0
(") 22
؛ ؛22؛^.) Í22(a) 0 S4C١C,+C4S, -S4C,S,+C4C, S4S5 0 f2,١(n) f2.٥ )١) f2١(a) f2.١(p) S5S. c٩ d٩
0 0 0 1 0 0 0 1
trong đó :
f ji = C2(C|X + s,y) - S2Z Í22 = - s.x + c ,y
fj, = S2(C|X + s,y) + C2Z
Từ cân bằng phần tử hàng 3, cột 4 ta được:
d١ = S,(C|P١ + s ؛p١,) -٠■ CịP, (6.38) Nếu nhân (6.35) với A4'A ١'Aị. Aị ' Ff, = ■.T^ và lượng hoá các phần tử ina trận, ta được:
Í 4 | ( n ) ؛41(0) Í 4 i ( a ) 0 C . Q - c ١s . s ؛ 0
) 4 2 ) n
؛ ؛42(0) ؛a )42) 0 s ١c . - s ٩s ٨ - c ١ 0
f4١( n ) f , , ( o ) ؛a ) 4 ١) 0 s . Q 0 0
. 0 0 0 1 0 0 0 1
Từ cân bằng phần tử hàng 3, cột 3 ta được một hàm số của 04 đó là f4,(a) = 0.
Hay là:
- S4[C2(C|X + s,y) - s,z] + Cj(-S|X + c ,y ) = 0
Phương trình này có dạng -.sin(ị) a١ + cos(ị) a١, = 0 như đã giải trong lời giải của phép biến đối Euler, mục 6.2, ta nhận được hai nghiệm:
S.a^ “i"C(a١, Ỡ4 = arctg — — --- — - ^ ٠- --- (6.39)
0 4 = 0 4 + 180'. (6.40)
Nếu các yếu tố tử số và mẫu sô' của (6.39) tiến tới 0 thì robot rơi vào tình trạng suy biến mà bạn đọc đã biết ở mục 6.2.
Ta hãy kiểm tra lại bằng cách cho cân bằng các phần tử hàng 1, cột 3 và hàng 2, cột 3 của phương trình ma trận:
a؛ 'aị■' T٠١ = A,A4A٩A٨ = -T,
và giải Ô4 thông qua mối quan hệ S4S،; và C 4 S 5, ta dược:
C4S5 = C 2 (C ,a١ + S|a١.) - Sja, S4S5 = - S |a١ + C |a١
Với 0, > 0 ta được:
S |a١ + (2|ay Ô4 = arctg
؛؛
S ^a — (.S|a١ +
C7(C|a١ đúng như (6.39).
Và với 0 0 > ؟ ta được :
04 = 04 + 0 ؟؟ ؛ ’ đúng như (6.40).
Kết quả này phù hợp với hai hình thê của robot:
Khi s ٩ = 0, 0 0 = ؟. robot có suy biến do cả hai trục của khớp 4 và khớp 6 nằm thảng hàng, ở trạng thái này chí có tổng của 04 và là có ý nghĩa.
Nếu 0 ١ = 0 la cú thể tự do lựa chọn giỏ trị của 04ã Giỏ trị xột đến thường dược chi định trước.
Từ vế phái ciia phương trình:
A4.A3 A ؛ A| T(-١ = Tf١ = A١A٨
ta có thể có các phương trình của s ؟ c١ ؟ . và Q bằng cách tra xét các phần tử ma trận thích hợp. Khi cá hai hàm sin và cos được xác định, ta nhận được nghiệm duy nhất của góc ở khớp tương ứng. Chẳng hạn, khi cân bằng các phần tử ma trận hàng 1, cội 3 và hàng 2, cột 3. ta có;
s , = C4[C2(C,a, + s,a ؛,) - s^a,] + S4(-S,a١ + c , a١)
c١= S2(C,a,، + S|a١.) + c^a, Từ đó suy ra:
C4[C2(C |a^ + S| ) — S ia ؛؛ ] + S4 (—S| a^ + C |a ١,) 0١ = arctg
S -)(C |a١ + S |a ١, ) + C-)a؛, (6.41)
Các phương trình có liên quan đến nằm ở cột 1 của phương trình ma trận, đó là vectơ n của T(؛. Vectơ này thường không có ý nghĩa hữu ích trong tính toán. Ta thường dùng máy tính để giải tích chéo của các vectơ 0 và a vì như đã rõ n = 0 X a.
* Nếu nhân (6.35) với A 5 . A 4 'A 4 'A ĩ 'A ؛"' T٠١= ٩Tfi = A٠١ và lượng hoá các phần tử ma trận, ta có:
T |(n) T,(0) 0 0 c,١ - s . 0 0
T:(.!) f٩2 (٥) 0 0 Sr١ 0 0
T,(n) f٥)١١) 1 0 0 0 1 0
0 0 0 1 0 0 0 1
trong đó:
f٩i = C٩|C4[C2(C|X + s,y) - 8 2 2 ] + S4(-S|X + c .y )! + S٩[-S2(C,X + s,y ) - C2Z]
T2 = -St[C2(C|X + s,y) - 822] + C4(-S|X + c ,y ) Tí = S٩|C4[C2(C|X + S|y) - 8 3 2] + S4(-S|X + C|y)l + S؟[S2(C|X + S|y) - C2Z)
Cân bằng các phần tử hàng 1, cột 2 và hàng 2, cột 2 ta nhận được các giá trị của s ؛؛ và Cf,:
s, = -C ,|C4[C,(C,0١ + s ,0١.) - S,OJ + S4(-S |0١ + C|0١.) í +
+ S٩[-S2(C,0, + s,0١.) + C٦O J
c, = - S4[C2(C,0١ + S|0,) - SjOJ + C4(-s٠0, + c,0١.)
Từ đó tính được: = arctg
c .١ (6.42)
Trong mọi trường hợp khi 04 không xác định do hình thể robot bị suy biến, một khi chí định một giá trị cho 04 thì các giá trị đúng của 0؛ và ١ Qf, luôn được xác định bởi (6.41) và (6.42).
Để tính toán cần tới 14 hàm siêu việt, 31 phép nhân và 15 phép cộng.