Động học ngược robot

Mở đầu
Độ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 ox ax px ⎤
⎢n oy ay p y ⎥⎥
K=⎢ y (1)
⎢nz oz az pz ⎥
⎢ ⎥
⎣0 0 0 1⎦

Tìm một hay tất cả nghiệm θ = (θ1 , θ 2 ,..., θ n ) của phươ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 mong muốn của cơ cấu tác động cuối
Phương trình (2) đưa đến việc giải 12 phương trình phi tuyến với n ẩn sau:
Tij = (θ1 ,K , θn ) = k ij , i = 1,2,3 , j = 1,...,4 (4)

trong đó Tij , k ij tương ứng là 12 phần tử có giá trị của Tn0 và K (vì hàng cuối cùng của cà Tn0 và

K đều là (0,0,0,1) , nên 4 trong số 16 phương trình từ (2) sẽ không có giá trị). Có 6 phương trình
quan hệ giàng buộc giữa n x , n y , n z , o x ,... nên chỉ có 6 phương trình độc lập.

Ví dụ
 Figure 1: Stanford robot
c1 [ c 2 (c 4 c5c6 − s 4s 6 ) − s 2s5c6 ] − s1 (s 4 c5c6 + c 4s 6 ) = nx
s1 [ c 2 (c 4 c5c6 − s 4 c6 ) − s 2s5s 6 ] + c1 (s 4 c5c6 + c 4s 6 ) = ny
−s 2 (c 4 c5c6 − s 4s 6 ) − c 2s5s 6 = nz
c1 [ −c 2 (c 4 c5c6 + s 4 c6 ) + s 2s5s 6 ] − s1 (−s 4 c5s 6 + c 4 c6 ) = ox
s1 [ −c 2 (c 4 c5c6 + s 4 c6 ) + s 2s5s 6 ] + c1 (−s 4 c5s 6 + c 4 c6 ) = oy
s 2 (c 4 c5c6 + s 4 c6 ) + c 2s5s 6 = oz
c1 (c 2 c 4s5 + s 2 c5 ) − s1s 4s5 = ax
s1 (c 2 c 4s5 + s 2 c5 ) + c1s 4s5 = ay
−s 2 c 4s5 + c 2 c5 = az
c1s 2 d 3 − s1d 2 + d 6 (c1c2 c4s5 + c1c5s 2 − s1s 4s5 ) = px
s1s 2d 3 + c1d 2 + d 6 (c1s 4s5 + c 2 c 4s1s5 + c5s1s 2 ) = py
c 2 d 3 + d 6 (c 2 c5 − c 4s 2s5 ) = pz

Các thách thức của bài toàn động học ngược

− Các phươ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 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.
− Các phươ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
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 pháp giải

− Phương pháp hình học
− Phương pháp đại số
− Phươ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 di θi
1 a1 0 0 θ1
2 a2 0 0 θ2

⎡cos θ1 − sin θ1 0 a1 cos θ1 ⎤

⎢ sin θ cos θ1 0 a1 sin θ1 ⎥⎥
1T =
0 ⎢ 1

⎢ 0 0 1 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
⎡cos θ2 − sin θ2 0 a 2 cos θ2 ⎤
⎢ sin θ cos θ2 0 a 2 sin θ2 ⎥⎥
2T =
1 ⎢ 2

⎢ 0 0 1 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
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 ) 0 a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎤
⎢ sin(θ + θ ) cos(θ + θ ) 0 a1 sin θ1 + a 2 sin(θ1 + θ2 ) ⎥⎥
T2 =1 T2 T = ⎢
0 0 1 1 2 1 2

⎢ 0 0 1 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦

Phương trình động học robot

n x = cos(θ1 + θ2 ) o x = − sin(θ1 + θ2 ) a x = 0 p x = a1 cos θ1 + a 2 cos(θ1 + θ 2 )
n y = sin(θ1 + θ2 ) o y = cos(θ1 + θ2 ) ay = 0 p y = a1 sin θ1 + a 2 sin(θ1 + θ 2 )
nz = 0 oz = 0 az = 0 pz = 0

Giải phương trình động học ngược

 0
2 T(θ1 , θ2 ) = K
Nghiệm chỉ tồn tại khi ma trận K có dạng

⎡ cos ϕ − sin ϕ 0 x⎤
⎢ sin ϕ cos ϕ 0 y ⎥⎥
K=⎢
⎢ 0 0 1 0⎥
⎢ ⎥
⎣ 0 0 0 1⎦

⎡ cos(θ1 + θ2 ) − sin(θ1 + θ2 ) 0 a1 cos θ1 + a 2 cos(θ1 + θ2 ) ⎤ ⎡ cos ϕ − sin ϕ 0 x⎤

⎢ sin(θ + θ ) cos(θ + θ ) 0 a1 sin θ1 + a 2 sin(θ1 + θ2 ) ⎥⎥ ⎢⎢ sin ϕ cos ϕ 0 y ⎥⎥
⎢ 1 2 1 2
=
⎢ 0 0 1 0 ⎥ ⎢ 0 0 1 0⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣ 0 0 0 1⎦

⎧ x = a1 cos θ1 + a 2 cos(θ1 + θ2 )
⎨
⎩ y = a1 sin θ1 + a 2 sin(θ1 + θ2 )

Mặt khác ta cũng có

⎧ sin 2 θ1 + cos 2 θ1 = 1
⎨ 2
⎩sin θ2 + cos θ2 = 1
2

⎧ x = a1 cos θ1 + a 2 cos(θ1 + θ2 )
⎪ y = a sin θ + a sin(θ + θ )
⎪ 1 1 2 1 2
⎨
⎪ sin θ1 + cos θ1 = 1
2 2

⎪⎩ sin 2 θ2 + cos 2 θ2 = 1

x = a1 cos θ1 + a 2 cos θ1 cos θ2 − a 2 sin θ1 sin θ 2

y = a1 sin θ1 + a 2 sin θ1 cos θ2 + a 2 cos θ1 sin θ 2

x 2 + y 2 = a12 + a 22 + 2a1a 2 cosθ2

cos θ2 =
1
2a1a 2
{( x 2
}
+ y 2 ) − ( a12 + a 22 ) = λ

sin θ2 = ±(1 − λ 2 )1/ 2

 1
2 {
cos θ1 = x(a1 + a 2 λ ) ± ya 2 (1 − λ 2 )1/ 2 }
x +y
2

1
sin θ1 =
x + y2
2 {m xa 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 ) (4.8)

xác định với mọi ( xc , yc ) ≠ (0,0) và có nghiệm

 xc yc
cos θ1 = ;sin θ1 = (4.9)
x c2 + y c2 x c2 + y c2

Các nghiệm θ1 đều hợp lệ ngoại trừ xc = yc = 0 . Trong trường hợp này, (4.8) không xác định và
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 tạo bởi khâu 2 và 3