Robotics 5

-2-
Bài toán động học ngược

Bài toán: Cho biết vị trí điểm tác động cuối E và hướng của khâu cuối
pE = [x E , yE , z E ]T & R3,3 = R(a, f, y)
Xác định các tọa độ khớp (biến khớp): q = [q1, q 2 ,..., qn ]T
Chương 5.
ĐỘNG HỌC NGƯỢC ROBOT CHUỖI yE
E
q3
3
yE q4
4
Inverse kinematics of serial robots
q2 q3
q2
O0 q1 O0 q1
Nguyễn Quang Hoàng
xE xE
Email: hoang.nguyenquang@hust.edu.vn
[x E , yE , j3 ]  [q1, q2 , q 3 ] [x E , yE , j4 ]  [q1, q2 , q 3 , q 4 ]
Nguyễn Quang Hoàng-Department of Applied Mechanics-SME Nguyễn Quang Hoàng-Department of Applied Mechanics-SME
-3- -4-
Bài toán động học ngược Bài toán động học ngược
hệ 12 pt
Bài toán động học ngược tổng quát: cho trước ma trận biến đổi tọa độ thuần
nhất của khâu cuối Tn0 (q1, q 2 ,..., qn )  H
R pe  ta cần tìm các biến khớp Tij (q1, q2 ,..., qn )  H ij , i  1, 2, 3, j  1, 2, 3, 4
H   Te  qi , i  1, 2,..., n
0 1
 • Do 9 phần tử của ma trận cô sin chỉ hướng chỉ có ba phần tử độc lập, nên từ hệ 12
phương trình trên ta có thể viết ra đưa ra được hệ 6 phương trình độc lập.
So sánh H với ma biến đổi tọa độ thuần nhất của khâu cuối cho ta hệ 12 pt • Hệ gồm các phương trình đại số phi tuyến, việc giải hệ này tương đối khó.
• Bài toán này có thể có một hoặc nhiều nghiệm, hoặc không có nghiệm, hoặc có vô
Tn0 (q1, q 2 ,..., q n )  H
số nghiệm.
• Nghiệm của hệ phương trình có thể tìm được bằng các công thức giải tích hoặc
Tij (q1, q2 ,..., qn )  H ij , i  1, 2, 3, j  1, 2, 3, 4
bằng phương pháp số.
-5- -6-
Bài toán động học ngược Bài toán động học ngược
Trong một số trường hợp đơn giản, ta có thể tìm được nghiệm ở dạng • Bài toán này có thể có một hoặc nhiều nghiệm, hoặc
giải tích, tức nghiệm tìm được có dạng không có nghiệm, hoặc có vô số nghiệm.
qi  q i (h11, h12 ,..., h34 ) i  1, 2,..., n

Biểu diễn nghiệm ở dạng công thức giải tích rất có ý nghĩa trong thực tế:
vì khi xác định biến khớp theo công thức giải tích máy tính toán rất ít thời
gian, so với việc xác định nghiệm bằng tính toán số nhờ các phương
pháp lặp; với biểu thức giải tích ta đã lựa chọn được một trong nhiều
nghiệm có thể có của bài toán ngược.
Một điều cần lưu ý khi tìm nghiệm ta cần phải kiểm tra xem các nghiệm
này có phù hợp với các liên kết về mặt cơ khí hay không, chẳng hạn một
nghiệm tìm được phải nằm trong giới hạn khớp thì mới có giá trị. Bài toán ngược có 2 nghiệm Bài toán ngược có vô số nghiệm
qi,min  q i  qi,max , i  1, 2,..., n

-7- -8-
Bài toán động học ngược
1. Phương pháp hình học (sử dụng các hàm lượng giác)
2. Phương pháp tách vị trí và hướng (các nhóm 3)
PHƯƠNG PHÁP HÌNH HỌC LƯỢNG GIÁC
3. Phương pháp số (pp lặp Newton-Raphson)
4. Phương pháp dựa trên ma trận Jacobi cho tay máy dư dẫn động (PHƯƠNG PHÁP GIẢI TÍCH)
tránh điểm kỳ dị
tránh va vào giới hạn khớp
tránh va vào vật cản
-9- -10-
Hàm atan2(y,x) Hàm atan2(y,x)
y tan(x)
(x, y)    atan 2(y, x ),     
0<y
-/2 /2 x
x<0
0<x
y<0
 
y  tan(x )  x  arctan(y)  x 
2 2
-11- -12-
Giải bài toán động học ngược - phương pháp hình học Giải bài toán động học ngược - phương pháp hình học
Tay máy phẳng 3RRR Ma trận biến đổi tọa độ thuần nhất
khâu 3 (cho trước): 
yE q3
O2 a3 E
c s 0 xE 
Kết quả bài toán động học thuận:  
s c 0 yE  a2 q2
H 
cq123 0 0 1 0  a1
sq123 0 l1cq1  l2cq12  l 3cq123  y0
q3
   O1
  0 O0 q1
sq cq123 0 l1sq1  l2sq12  l 3sq123  O2 0 0 1
a3  
T3 (q )   123
0

 xE
0 0 1 0 a2 q2
 
 0 0 0 1   a1 l1cq1  l2cq12  l 3cq123  x E
O0 q1 O1
T30 (q )  H
 x0 l1sq1  l2sq12  l 3sq123  yE
cq123  c
(x E , yE ,  )  (q1, q2 , q 3 ) sq123  s
-13- -14-
Từ hình vẽ ta xác định được tọa độ điểm O2   O1O0O2 :

y0 
 sin   a 2 sin q 2 / r , q3
xO 2  x E  a 3c  x E  a 3cq123 yE q3 O2
O2 cos   (a1  a2 cos q 2 ) / r , a3
a3 E
yO 2  yE  a 3s  yE  a 3sq123
a2
a2 q2
  atan2(s  , c  ) q2
r  O0O2  (xO 2 )  (yO 2 ) ,

2 2
 atan2(a 2 sin q 2 , a1  a 2 cos q 2 )  a1
 a1 O0 q1 O1
O0 q1 O1
  xO0O2 =atan2(yO2, xO2 )  xE q1      x0
cq123  c, sq123  s  q123  atan2(s, c )

r 2  a22  a12
r  a  a  2a1a2 cos q 2  cos q 2 
2 2
2
2
1
: D, q123  q1  q 2  q 3  q 3  q123  q1  q 2
2a1a2
 sin q 2   1  D 2  q 2  atan2( 1  D 2 , D ) Điều gì xảy ra nếu a1 = a2? xO2 = yO2 = 0?
-15- -16-
x3
Khi biết tọa độ điểm C [xC,yC,zC], từ hình chiếu trên mặt Ox0y0, ta tính được góc
Robot kiểu tay người = Anthropomorphic arm z0
a3 C
a2 q3 q1  atan2(yC , xC ) or q1    atan2(yC , xC )
q2 zC
d1 x1 Để giải tìm q2 và q3, ta xét hệ phẳng gồm hai khâu 2 và 3 trong mặt phẳng đứng. Giải
yC y0
Givens: [xC, yC, zC] hệ này như trường hợp tay máy phẳng 2 dof.
r
Find: q1, q2, q3 q1
x3
xC x’1 a3 C
z0 z0
cq1cq23 cq1sq23 sq1 cq1 (l2cq2  l 3cq23 ) x0 a2 q3 s
C
 
sq cq sq1sq 23 cq1 sq1 (l2cq 2  l 3cq23 ) zC
T30 (q )   1 23
q2 a3
, y0 q3
sq cq23 0 l1  l2sq2  l 3sq23  d1 x1
yC y0 a2
 23  O2
 0 0 0 1  O1
q2
q1 r
q23  q2  q 3 q1
d1
r
x0 C x’1
xC x’1
x0
-17- -18-
x
Nghiệm q1 không xác định (có thể lấy tùy Một phương án cải tiến để a3 C
z0 3
ý), khi tránh kỳ dị: d2  0 a2
z0 q3
C q2 zC
yC  xC  0 q1  atan2(yC , xC ) d1
d2 x
1 yC B
y
q2 r
0
y0 q1
tức là khi điểm C nằm trên trục z0.
xC
Phương trình trên không xác định và tay x
máy ở vị trí kỳ dị, như chỉ ra trên hình q1 0
bên. Tại cấu hình này, (C nằm trên z0) x’1 d2

góc q1 có thể lấy giá trị tùy ý. x0
yC y0
q1
 
Một vị trí kỳ dị của robot xC C
x0
-19- -20-
Robot scara 4dof (RRTR),
a1 y0
kết quả bài toán động học thuận cho ta biết vị trí và hướng của bàn kẹp.
a2 yC C
d1
a1 y0 q3 a2
q4 yC r q2
a2 yC C a1
d1 r zC C y0 O2
q3 a2 O
q4 yC r q2 xC q1 q1 xC x0
C y0 a1 q2
r zC O2 x0
xC q1 O
q1 xC x0
q2
x0
Trong thực tế hướng của khâu c s 0
c(q124 ) s(q124 ) 0 a1cq1  a2c(q1  q2 ) cuối không thể tùy ý, ma trận  
  RE  s c 0
s(q ) c(q124 ) 0 a1sq1  a2s(q1  q 2 ) co sin chỉ hướng của khâu
T40 (q )   124 cuối chỉ ở dạng
0 0 1
0 0 1 d1  q 3  d4   
  q124  q1  q2  q 4
 0 0 0 1 
-21- -22-
Giải bài toán động học ngược - phương pháp hình học
Từ hình chiếu trên mặt (Oxy)0 cho ta xác định được

r 2  a22  a12  2a1a2 cos q 2 y0
C
r 2  a22  a12 yC
 cos q2  : D a2
2a1a2 r q2 PHƯƠNG PHÁP TÁCH VỊ TRÍ VÀ HƯỚNG
a1
 sin q2   1  D 2 O2
O
q1 xC x0 (PHƯƠNG PHÁP CÁC NHÓM 3)
q 2  atan2( 1  D , D )2
q1  atan2(yC , xC )  atan2(a 2 sin q 2 , a1  a2 cos q 2 )
T3,4 (q )  H 3,4  q 3  d1  zC
  q1  q 2  q 4  atan2(s, c )  q 4  q1  q 2  atan2(s, c )
-23- -24-
Phương pháp các nhóm 3 (tách vị trí và hướng) Phương pháp các nhóm 3 (tách vị trí và hướng)
Trong một số trường hợp ta có thể tách hệ thành hai Hoặc có thể tồn tại điểm trung gian mà vị trí của nó chỉ phụ thuộc tối
nhóm: nhóm 1 gồm ba phương trình tính theo tọa độ đa ba biến khớp q1, q2, q3
của khâu cuối; và nhóm 2 gồm các phương trình xác
định hướng của khâu cuối. Ví dụ đối với tay máy 6
dof, ba bậc tự do 1, 2, 3 cho phép xác định tọa độ của pB  pB (q1, q 2 , q 3 )  q1, q 2 , q 3
khâu cuối và ba bậc tự do cuối 4, 5, 6 cho phép xác
định hướng của khâu cuối. Đồng thời vị trí điểm trung gian này cũng được xác định từ tọa độ
điểm tác động cuối E và ma trận cosin chỉ hướng của bàn kẹp:
pE  pE (q1, q 2 , q 3 )  q1 , q 2 , q 3
RE0  RE0 (q 4 , q 5 , q 6 )  q4 , q5, q6 pB  pE  RE0 d (E ) d (E ) là véc tơ hằng số

Hoặc …
-25- -26-
Thuật toán (phương pháp các nhóm 3) Thuật toán (phương pháp các nhóm 3)
Cho trước vị trí điểm tác động cuối E và ma trận cosin chỉ Tính ma trận quay 4 đến 6:
hướng khâu cuối E
pE , RE0 RE0  R30 (q1,2,3 )R63 (q 4,5,6 )

Tìm và tính được vị trí của điểm trung gian B  R63 (q 4,5,6 )  [R30 (q1,2,3 )]1 RE0
pB  pE  RE0 d (E )
Giải hệ phương trình đại số phi tuyến tìm ba biến khớp đầu Giải hệ phương trình đại số phi tuyến tìm ba biến khớp 4, 5, 6
tiên (có thể sử dụng phương pháp hình học):
pB  pB (q1, q 2 , q 3 )  q1 , q 2 , q 3 R63 (q 4,5,6 )  [R30 (q1,2,3 )]1 RE0  q 4 , q 5 , q 6
-27- -28-
Ví dụ 1. Robot dạng tọa độ trụ có bàn tay (RTT-RRR) d3

q3 d6
z4
y2 x2 y3 q5
d6 x3 E, O6
q3 z4 z2
y2 x2 y3
x3
q5 cho pE , RE0 tim qk , k  1,..., 6
E, O6
z2 O3 z3 q z5 q6
z6
q2 z1 y1 4
O5 Điểm trung gian B = O5
O3 z3 q z5 q6
z6
q2 z1 y1 4
O5 Bảng DH, thân robot trụ (RTT) x1
x1
i  d a  d1 q1 pB  pE  RE0 u B(6)  pB (q1,2,3 )  q1,2,3
z0
d1
1 q1 d1 0 0 y0
z0 y0 2 /2 q2 0 /2 q1  R30  R30 (q1, q 2 , q 3 )  0 
3 0 q3 0
x0  
q1
0 u (6)
B
 0 
x0
4 q4 d4=0 /2  d 
0 R RR 
0
E
0
3 6
3
 6
5 q5 0 0 /2
điểm trung gian ?? 6 q6 d6 0 0 R  [R ] R  R63 (q 4,5,6 )  q 4,5,6
3
6
0 T
3
0
E
-29- -30-
d3 x B(0)  (q 3  d 3 ) cos q1  q  Ví dụ 2: Stanford robot cho pE , RE0 tim qk , k  1,..., 6 z6 z5

q3 z4
d6   1 E
y6
y2 x2 y3 q5 yB(0)  (q 3  d3 ) sin q1   q1,2,3 : q 2  x6
x3
z2
E, O6
z B(0)  d1  q 2  q  q6
  3 Bảng DH, thân robot trụ (RTT) d6
O3 z3 q z5 q6
z6
x4
q2 z1 y1 4
O5 i  d a  y4
z4
q 2  z B(0)  d1 1 q1 d1 0 /2 x5 q 5 y5
x1 q4 d4
q 3  (x B(0) )2  (yB(0) )2  d3 2 q2 d2 0 /2 z3
d1 q1
z0 cos q1  x B(0) / (q 3  d3 ) 3 0 q3 0 0 x3
y3
y0   q1  atan2(si, co)
sin q1  yB(0) / (q 3  d3 ) 
 4 q4 d4 0 /2 y1 x1 q 3 z2
q1
x0 5 q5 0 0 -/2 q2
z1 y2
6 q6 d6 0 0 q1 x2
RE0  R30R63  d1
d2
z0 y0
R63  [R30 ]T RE0  R63 (q 4,5,6 )  q 4,5,6 điểm trung gian ?
x0
-31- -32-
Ví dụ 3. Articulated Manipulator with spherical wrist (RRR-RRR)
z6 z5
Điểm trung gian B = O4,5 y6
Bảng thông số DH
x6 cho pE , RE0 tim qk , k  1,..., 6
q6 i  d a 
d6 O4,5 1 q1 d1 0 /2
pB  pE  RE0 u B(6)  pB (q1,2,3 )  q1,2,3 y4
x4 z4
q5 2 q2 0 a2 0
z4 z3
x5 y5 y2 d3 q4
3 q3+/2 0 0 /2
 0  q4 q5 d x3 q6
z3 z0
 
4
4 q4 d3 0 /2
uB(6)  0  q2 a2 q 3 x2
y3 O6 5 q5 0 0 /2
 R30  R30 (q1, q2 , q 3 )  d  x3 O1
z2 O2,3
 6 y1 x1 q 3 z2 d 1 x1
6 q6 d6 0 0
y0
q2 z1
z1 O0 Tìm điểm trung gian ?
RE0  R30R63  d1
q1 x2
y2
q1
d2
R63  [R30 ]T RE0  R63 (q 4,5,6 )  q 4,5,6 z0 y0
x’1
x0 x0
-33- -34-
Phương pháp các nhóm 3 (tách vị trí và hướng) Ví dụ và Bài tập
 0  • Ví dụ và Bài tập
Điểm trung gian B = O4,5 x3
z4 O4,5  
uB(6)  0 
q5
pB  pE  RE0 u B(6) E  O3, y3 3
z3  d 
y2 d3 q4  6 a  O 0O1, l 3  O 2O 3 yE
z0 x3 q6
 pB (q1,2,3 )  q1,2,3 z3 O3
q2 a2 q 3 x2
O6
(x E , y E , 3 )  q  [q 1, q 2 , q 3 ]T
x2 q3
O1
z2 O2,3 z1
q2
d1 x1
y0
 R  R (q1, q 2 , q 3 )
0
3
0
3
y0
O2
z1 z2
O0
• Cho x1
q1 RE0  R30R63  E  O3,
x’1 O1
R63  [R30 ]T RE0  R63 (q 4,5,6 )  q 4,5,6 a  O 0O1  0.3, l 3  O 2O 3  0.5 y1 q1
x0 z0
(x E , y E , 3 )  (0.8, 1.1, 60) O0 xE x0
Lưu ý:
PP 2 nhóm 3 đối với tay máy robot không gian  q  [q 1, q 2 , q 3 ] ??
T
=> PP nhóm 2 + 1 đối với tay máy robot phẳng.
-35- -36-
Ví dụ và Bài tập
• Ví dụ và Bài tập
q3
q2
E
q1
E  O3, a  O 0O1 ,
l 3  O 2O 3
(x E , y E , 3 )  q  [q 1 , q 2 , q 3 ]T
-37- -38-
Ví dụ và Bài tập Ví dụ và Bài tập
cho l1,2,3 cho l1,2,3
[x E , yE , j3 ]  [q1, q2 , q 3 ] [x E , yE , j3 ]  [q1, q2 , q 3 ] [x E , yE , j3 ]  [q1, q2 , q 3 ]
E
q3 yE
yE
q2
3 3 3
E
yE
B
q2 q3 B
q2
q1
B
O0 q1 O0 q1
xE O0 xE
xE
-39- -40-
Ví dụ và Bài tập Ví dụ và Bài tập
l 2  0.5, h  0.4 cho l1,2,3
3
(x E , y E , 3 )  (1.5, 0.3,  60) q1 [x E , yE , j3 ]  [q1, q2 , q 3 ]
 q  [q 1, q 2 , q 3 ]T ?? q3
q2
q3
yE
h E
q3
O0 q2 3
E xE
yE E
E
3 yE
3
q3 q1 q1
q1 q2
O0 O0
xE
O0 q2
l 2  0.7, l 3  0.4, xE
xE
(x E , y E , 3 )  (1.2, 0.9, 80)
 q  [q 1, q 2 , q 3 ]T ??
-41- -42-
Phương pháp số giải động học ngược: pp lặp Newton-Raphson
q3
R pe  T (q1, q 2 ,..., qn )  H
0
yE
3
H   Te  n
0 1
  B
q2
PHƯƠNG PHÁP LẶP NEWTON-RAPHSON Cho x cần tìm q thỏa mãn phương trình động học vị trí
f (x, q)  0
GIẢI HỆ PHƯƠNG TRÌNH ĐẠI SỐ PHI TUYẾN O0 q1
xE
x = [x E , yE , j3 ]T
 q = [q1, q2, q 3 ]T
x = [x E , yE , z E , y, q, j ]T
 q = [q1, q2, q 3, q 4 , q 5, q 6 ]T
-43- -44-
Phương pháp số giải động học ngược: pp lặp Newton-Raphson Phương pháp số giải động học ngược: pp lặp Newton-Raphson
Phương pháp lặp Newton-Raphson Trước hết ta khai triển Taylor hàm f(q) ở lân cận q0
Tìm nghiệm hệ n phương trình n ẩn f (q ) = 0,
f (q ) = 0, f Î , n
qÎ n
¶f
f ( q 0 + d q ) = f (q 0 ) + (q )d q + O (d q )2
¶q 0
ì
ï f1 (q 1 , q 2 ,..., q n ) = 0
ï
ï
= f (q 0 ) + J(q 0 )d q + O (d q )2
ï
ï x = [x E , yE , z E , y, q, j ]T
ï f (q , q ,..., q n ) = 0 é ¶f ¶ f1 ¶ f1 ùú
ï2 1 2  q = [q1, q2, q 3, q 4 , q 5, q 6 ]T ê 1
í Ma trận Jacobi được xác định tại điểm q0 ê ¶q ..
ï... ê 1 ¶q 2 ¶ q n úú
ï
ï ê ¶f
ï ¶ f2 ¶ f2 ú
ï ¶f ê 2 .. ú
ï
ï fn (q 1 , q 2 ,..., q n ) = 0 J(q 0 ) = (q 0 ) = ê ¶ q 1 ¶q 2 ¶ q n úú (q 0 )
î ¶q ê
ê .. .. .. .. ú
ê ú
Việc tìm nghiệm cho hệ này sẽ khó khăn hơn rất nhiều so với việc giải bài toán một phương trình một ê ¶f ¶ fn ¶ fn ú
ẩn. Thật khó khăn đôi khi là không thể tìm được khoảng phân ly nghiệm. Một phương pháp đơn giản ê n .. ú
ê ¶q ¶q 2 ¶ q n úû
và hiệu quả đó là phương pháp lặp Newton-Raphson. ë 1
-45- -46-
Bắt đầu
Giả sử q0 là một xấp xỉ ban đầu của nghiệm cần tìm và để q0 + q là một 1. Chọn sai số cho phép err; số bước lặp
giá trị gần nghiệm hơn (chính xác hơn), ta cho f(q0 + q) = 0. tối đa M; chọn xấp xỉ ban đầu q0; khởi •Chọn sai số cho phép, err
•Chọn số bước lặp tối đa, M
gán k = 0; qk = q0 •Khởi động k = 0, chọn q0
Từ đó nhận được một hệ phương trình đại số tuyến tính đối với q.
2. Tính f(qk)
3. Kiểm tra nếu || f(qk) || <= err lấy nghiệm Tính f(qk)
q = qk và dừng thuật toán; trái lại tiếp tục Yes

f (q 0 + d q ) = f (q 0 ) + J(q 0 )d q = 0 bước 4.
f (q k ) £ err
No
4. Tính ma trận Jacobi J(qk);
J(q 0 )d q = -f (q 0 ) Tính J (q k )
5. Giải hệ J(qk)q = -f(qk); Giải hệ J (q k )D q = - f (q k )

Chỉnh q k +1 = q k + D q
-1 6. Hiệu chỉnh qk+1 = qk + q ;
d q = - J (q 0 ) ⋅ f ( q 0 ) Tăng k k = k +1
7. Tăng bước k = k + 1;
Yes
Lấy q 0 = (q 0 + d q ) 8. Kiểm tra số bước lặp, nếu k < M quay lại k £M
No
bước 2; trái lại “Pp không hội tụ với q0 và Không hội tụ với M Lấy nghiệm qk
M bước lặp” và q0
Lặp lại quá trình trên đến khi f (q 0 ) £ e 9. Kết thúc.

Kết thúc
-47- -48-
Ảnh hưởng của xấp xỉ ban đầu él cos q + l cos(q + q ) + l cos(q + q + q ) - p ù
ê1 1 2 1 2 3 1 2 3 xú
l2
B f (q ) = êê l1 sin q 1 + l 2 sin(q 1 + q 2 ) + l 3 sin(q 1 + q 2 + q 3 ) - py úú = 0 E
ê q 1 + q 2 + q 3 - f3 ú
A êë úû q3
q2
l3 q3 l3
3
l1 q1 D y0
O ? q = [q 1 , q 2 , q 3 ]T , O2
l2
Biet x = (p E , f3 ) = ( px , py , f3 ) q2
l1 O1
y
A q1 x0
Tính toán với l1 = 1, l 2 = 0.8, l 3 = 0.3 O0
B p E = (px , py ) = (1.2,1.2)
x
O
f3 = 90  q 1,2,3 ?
-49- -50-
él cos q + l cos(q + q ) + l cos(q + q + q ) - p ù

ê1 1 2 1 2 3 1 2 3 xú E
f (q ) = êê l1 sin q 1 + l 2 sin(q 1 + q 2 ) + l 3 sin(q 1 + q 2 + q 3 ) - py úú = 0
q3
ê q 1 + q 2 + q 3 - f3 ú l3
êë úû 3
T
q = [q 1 , q 2 , q 3 ] y0
O2
l2
é ù
J(q ) =
¶ f (q )
, éJ ù = ê ¶ fi (q ) ú q2
¶q êë ij úû ê ¶q ú
êë j úû
l1 O1
é-l sq - l sq - l sq -l 2sq 12 - l 3sq 123 -l 3sq 123 ùú q1 x0
ê 1 1 2 12 3 123
O0
= êê l1cq 1 + l 2cq 12 + l 3cq 123 l 2cq 12 + l 3cq 123 l 3cq 123 úú
ê 1 1 1 úú
êë û
q 12 = q1 + q 2
q 123 = q 1 + q 2 + q 3
-51- -52-
Chuyển bài toán động học ngược về bài toán tối ưu
Bài toán động học ngược, dẫn đến giải hệ 12 phương trình đại số phi tuyến
Tn0 (q)  H  Tn0[i, j ](q)  H [i, j ], i  1,2, 3, j  1,2, 3, 4

CHUYỂN BÀI TOÁN ĐỘNG HỌC NGƯỢC
VỀ BÀI TOÁN TỐI ƯU CÓ RÀNG BUỘC Bài toán trên được chuyển thành bài toán tối ưu có ràng buộc như sau:
Tìm cực tiểu của hàm sau đây:
1 3 4
 
2
Sử dụng các phương pháp giải bài toán tối ưu có ràng min S (q) 
q

2 i 1 j 1
cij Tn0[i, j ](q)  H [i, j ] , ci, j  0
buộc để giải: pp nhân tử Lagrange, pp hàm phạt, …
với các ràng buộc về giới hạn khớp:
S  S (q)  min (or max)
qim  qi  qiM , i  1,2,..., n
Rõ ràng là nghiệm của bài toán tối ưu trên cũng chính x = [x E , yE , z E , y, q, j ]T

là nghiệm của bài toán động học ngược.  q = [q1, q2, q 3, q 4 , q 5, q 6 ]T
-53-
Chuyển bài toán động học ngược về bài toán tối ưu
Thiết lập bài toán tối ưu cho bài toán đọng học ngược tay máy phẳng 4 khớp quay sau:
Tọa độ điểm cuối tính theo các góc khớp như sau: y0 E
q3 q4
 f1(q)  a1 cos q1  a2 cos q12  a 3 cos q123  a 4 cos q1234  x E  0, B
(1)  C
 f2 (q)  a1 sin q1  a2 sin q12  a 3 sin q123  a 4 sin q1234  yE  0.
q2
a2
a1
q1 A
Đây là hệ hai phương trình bốn ẩn nên có thể có vô số nghiệm. O x0
Để nghiệm tìm được nằm trong phạm vi giới hạn khớp càng gần giá trị
giữa càng tốt, ta đưa vào hàm cần tối ưu sau đây:
Tay máy phẳng RRRR
2
1  q  qi 
4
(3) min S (q)   ci  i ,
q 2 j 1  qiM  qim 
với giá trị trung bình của biến khớp: Bài toán tối ưu: tìm min của hàm
qi  (qim  qiM ) / 2 S(q) với các ràng buộc (1)
Nguyễn Quang Hoàng-Department of Applied Mechanics-SME

-2-
Ma trận trạng thái khâu cuối
{ Fn }
Ký hiệu các thông số xác định vị trí và hướng của khâu cuối:

e 3n
 
rE : r(0)
E
 [x , y , z
(0)
E
(0)
E
(0) T
E
] rO

e 2n

n e1(n )
(0)
ˆr
E
 [x , y , z , 1]
(0)
E
(0)
E
(0)
E
T
{F0 }
PHƯƠNG PHÁP LẶP NEWTON RAPHSON
Chẳng hạn sử dụng 3 góc Euler, hoặc RPY O0
q  [ ,  ,  ]T
Véc tơ thể hiện vị trí và hướng khâu cuối
x E  p E  [rE(0)T , qT ]T
 [x E(0), y E(0), z E(0),  ,  ,  ]T
Nguyen Quang Hoang-Department of Applied Mechanics Nguyen Quang Hoang-Department of Applied Mechanics
-3- -4-
Ma trận trạng thái khâu cuối Phương pháp lặp Newton-Raphson
z2
Ma trận trạng thái khâu cuối biểu diễn theo vector 2 4 • ĐỘNG HỌC THUẬN
1 a2
x E  p E  [rE(0)T , qT ]T a1
Cho véc tơ biến khớp q  Ma trận
 [x E(0), y E(0), z E(0),  ,  ,  ]T d3
trạng thái khâu cuối
z0
O0 y0 x4 q  TE0 (q )  Tn0 (q )
y4 z4
x0
Hình 9-13. Tay máy SCARA, { Fn } • ĐỘNG HỌC NGƯỢC

RRTR
 R(q ) rE  
e 3n Cho ma trận trạng thái khâu cuối 
TE (x)  TE (p)     
Tìm véc tơ biến khớp q
0 1 rO

e 2n
 n

e1(n )
TE (x)  TE (q )  q
{F0 }
O0
-5- -6-
Phương pháp lặp Newton-Raphson Phương pháp lặp Newton-Raphson
TE (x)  TE (p)  q Nếu chuyển động của khâu cuối cho dạng hàm của thời gian, ta viết lại các
phương trình thành:
éa (q ) a (q ) a (q ) x (q )ù é c (a, b, g ) c (a, b, g ) c (a, b , g ) x ù
ê 11 12 13 ú ê 11 12 13 Eú
êa (q ) a (q ) a (q ) y (q )ú ê c (a, b , g ) c (a, b , g ) c (a, b , g ) y ú
ê 21 22 23 ú ê 21 22 23 E ú x E (t ), y E (t ), z E (t ), a(t ), b (t ), g (t )
êa (q ) a (q ) a (q ) z (q )ú = ê c (a, b , g ) c (a, b , g ) c (a, b , g ) z ú
ê 31 32 33 ú ê 31 32 33 E ú
ê 0 0 0 1 úú êê 0 0 0 1 úú
êë û ë û
Đối với robot chuẩn không gian (6 dof), ta chọn ra 6 phương trình
để giải bà toán động học ngược. Gồm 3 phương trình vị trí và 3 f1 = x (q ) - x E (t ) = 0, f4 = a 11 (q ) - c11 (t ) = 0
phương trình liên quan cosin chỉ hướng. f2 = y (q ) - y E (t ) = 0, f5 = a 22 (q ) - c 22 (t ) = 0
f1 = x (q ) - x E = 0, f4 = a 11 (q ) - c11 (a, b , g ) = 0
f3 = y (q ) - z E (t ) = 0, f6 = a 33 (q ) - c 33 (t ) = 0
f2 = y (q ) - y E = 0, f5 = a 22 (q ) - c22 (a, b , g ) = 0
f3 = y (q ) - z E = 0, f6 = a 33 (q ) - c 33 (a, b , g ) = 0
f j (x 1 , x 2 ,..., x 6 ) = 0, j = 1, 2,..., 6
Có thể chọn 3 phần tử trên cùng hàng hoặc cùng cột của ma trận cosin chỉ hướng?
-7- -8-
Trước hết ta khai triển Taylor hàm f(x) ở lân cận x0

Tìm nghiệm hệ n phương trình n ẩn
f (x) = 0,
f1 (x 1, x 2 ,..., x n ) = 0
¶f
f (x 0 + d x ) = f (x 0 ) + (x )d x + O (d x)2
f2 (x 1, x 2 ,..., x n ) = 0 ¶x 0
f (x) = 0, f Î n , x Î n ...
= f (x 0 ) + J(x 0 )d x + O (d x)2
Ma trận jacobi được xác định tại điểm x0
fn (x 1, x 2 ,..., x n ) = 0
é ¶f ¶ f1 ¶ f1 ùú
ê 1 ..
ê ¶x ¶x 2 ¶ x n úú
ê 1
ê ¶f ¶ f2 ¶ f2 ú
Việc tìm nghiệm cho hệ này sẽ khó khăn hơn rất nhiều so với việc giải bài toán ¶f ê 2 .. ú
một phương trình một ẩn. Thật khó khăn đôi khi là không thể tìm được khoảng J(x 0 ) = (x 0 ) = ê ¶ x 1 ¶x 2 ¶ x n úú (x 0 )
¶x ê
phân ly nghiệm. Một phương pháp đơn giản và hiệu quả đó là phương pháp ê .. .. .. .. ú
ê ú
lặp Newton-Raphson. ê ¶f ¶ fn ¶ fn ú
ê n .. ú
ê ¶x ¶x 2 ¶ x n úû
ë 1
-9- -10-
Giả sử x0 là một xấp xỉ ban đầu của nghiệm cần tìm và để x0 + x Thuật giải phương pháp Newton-Raphson
là một giá trị gần nghiệm hơn (chính xác hơn), ta cho f(x0 + x) = 0.
Bước 1. Khởi gán k = 0, chọn xấp xỉ ban đầu x0, sai số , số lần lặp tối đa M
Từ đó nhận được một hệ phương trình đại số tuyến tính đối với x.
k
Bước 2. Tính f (x k ) . Nếu f (x ) £ e thì dừng, nếu không thì tiếp tục từ 3.
Bước 3. Tính ma trận Jacobi tại x k

, tức là tính ma trận J(x k )
f (x 0 + d x) = f (x 0 ) + J(x 0 )d x = 0
Bước 4. Giải hệ J(x k )d x = -f (x k )  d x = - J-1 (x k )f (x k )
J(x 0 )d x = -f (x 0 )
Bước 5. Lấy x k +1 = x k + d x
-1
d x = - J (x 0 ) ⋅ f ( x 0 )
Bước 6. Tăng k = k +1 . Nếu k > M với M là số bước lặp đã chọn trước, thì
Lấy x 0 = (x 0 + d x )
dừng. Nếu không, tiếp tục từ Bước 2.
Lặp lại quá trình trên đến khi f (x 0 ) £ e
-11-
Phương pháp lặp Newton-Raphson
Bắt đầu
Sơ đồ khối thuật giải của phương
pháp lặp Newton-Raphson giải hệ Chọn , M, k = 0,
phương trình đại số phi tuyến n Chọn xấp xỉ ban đầu x(0)
phương trình, n ẩn
Tính f(x(k))
Yes
f (x) = 0, f Î  n , x Î n || f(x(k)) ||  
No
Tính: J(x(k))
f1 (x 1, x 2 ,..., x n ) = 0 Giải hệ: J(x(k))x = - f(x(k))
Lấy: x(k+1) := x(k)+ x
f2 (x 1, x 2 ,..., x n ) = 0 Tăng k, k:= k + 1
... Yes
kM
fn (x 1, x 2 ,..., x n ) = 0 No Lấy nghiệm
không hội tụ với x(0) x(*) := x(k)
và M lần lặp
Kết thúc
Nguyen Quang Hoang-Department of Applied Mechanics

Robotics 5

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics 5

Uploaded by

Copyright:

Available Formats

-2-

Bài toán động học ngược

qi  q i (h11, h12 ,..., h34 ) i  1, 2,..., n

qi,min  q i  qi,max , i  1, 2,..., n

Từ hình vẽ ta xác định được tọa độ điểm O2   O1O0O2 :

r  O0O2  (xO 2 )  (yO 2 ) ,

  xO0O2 =atan2(yO2, xO2 )  xE q1      x0

cq123  c, sq123  s  q123  atan2(s, c )

 sin q 2   1  D 2  q 2  atan2( 1  D 2 , D ) Điều gì xảy ra nếu a1 = a2? xO2 = yO2 = 0?

bên. Tại cấu hình này, (C nằm trên z0) x’1 d2

Từ hình chiếu trên mặt (Oxy)0 cho ta xác định được

q1  atan2(yC , xC )  atan2(a 2 sin q 2 , a1  a2 cos q 2 )

RE0  RE0 (q 4 , q 5 , q 6 )  q4 , q5, q6 pB  pE  RE0 d (E ) d (E ) là véc tơ hằng số

pE , RE0 RE0  R30 (q1,2,3 )R63 (q 4,5,6 )

pB  pB (q1, q 2 , q 3 )  q1 , q 2 , q 3 R63 (q 4,5,6 )  [R30 (q1,2,3 )]1 RE0  q 4 , q 5 , q 6

Ví dụ 1. Robot dạng tọa độ trụ có bàn tay (RTT-RRR) d3

d3 x B(0)  (q 3  d 3 ) cos q1  q  Ví dụ 2: Stanford robot cho pE , RE0 tim qk , k  1,..., 6 z6 z5

=> PP nhóm 2 + 1 đối với tay máy robot phẳng.

cho l1,2,3 cho l1,2,3

[x E , yE , j3 ]  [q1, q2 , q 3 ] [x E , yE , j3 ]  [q1, q2 , q 3 ] [x E , yE , j3 ]  [q1, q2 , q 3 ]

q = qk và dừng thuật toán; trái lại tiếp tục Yes

5. Giải hệ J(qk)q = -f(qk); Giải hệ J (q k )D q = - f (q k )

Lặp lại quá trình trên đến khi f (q 0 ) £ e 9. Kết thúc.

él cos q + l cos(q + q ) + l cos(q + q + q ) - p ù

Tn0 (q)  H  Tn0[i, j ](q)  H [i, j ], i  1,2, 3, j  1,2, 3, 4

Rõ ràng là nghiệm của bài toán tối ưu trên cũng chính x = [x E , yE , z E , y, q, j ]T

Nguyễn Quang Hoàng-Department of Applied Mechanics-SME

Véc tơ thể hiện vị trí và hướng khâu cuối

Hình 9-13. Tay máy SCARA, { Fn } • ĐỘNG HỌC NGƯỢC

Trước hết ta khai triển Taylor hàm f(x) ở lân cận x0

Bước 3. Tính ma trận Jacobi tại x k

Lặp lại quá trình trên đến khi f (x 0 ) £ e

Nguyen Quang Hoang-Department of Applied Mechanics

You might also like