Trang 1
Chương 5. Động học ngược robot -1- Chương 5. Động học ngược robot -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 p = [x ,y ,z ]T & R = R( ,a f y, ) E E E E 3,3 Xác định các tọa độ khớp (biến khớp): q = [q ,q ,...,q ]T Chương 5. 1 2 n 1. Động học ngược robot chuỗi y q3 E yE q 4 Inverse kinematics of serial robots 3 4 Nguyễn Quang Hoàng q2 q 3 q2 Email: hoang.nguyenquang@hust.edu.vn O q1 O q 0 0 1 x xE E [x ,y ,j ] [q ,q ,q ] [x ,y ,j ] [q ,q ,q ,q ] E E 3 1 2 3 E E 4 1 2 3 4 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -3- Chương 5. Động học ngược robot -4- Bài toán động học ngược Bài toán động học ngược Bài toán động học ngược tổng quát: cho trước ma trận biến hệ 12 pt đổi tọa độ thuần nhất của khâu cuối T0(q ,q ,...,q ) H n 1 2 n R p ta cần tìm các biến khớp H e e q ,i 1, 2,...,n T (q ,q ,...,q ) H , i 1, 2, 3, j 1, 2, 3, 4 0T 1 ij 1 2 n ij i • Do 9 phần tử của ma trận cô sin chỉ hướng chỉ có ba phần tử độ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 nên từ hệ 12 phương trình trên ta có thể viết ra đưa ra được hệ 6 cho ta hệ 12 pt phương trình độc lập. • Hệ gồm các phương trình đại số phi tuyến, việc giải hệ này tương đối T0(q ,q ,...,q ) H khó. n 1 2 n • 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ô số nghiệm. T (q ,q ,...,q ) H , i 1, 2, 3, j 1, 2, 3, 4 • Nghiệm của hệ phương trình có thể tìm được bằng các công thức giải ij 1 2 n ij tích hoặc bằng phương pháp số. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 2
Chương 5. Động học ngược robot -5- Chương 5. Động học ngược robot -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. q q (h ,h ,...,h ) i 1, 2,...,n i i 11 12 34 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 q q q , i 1, 2,...,n i,min i i,max Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -7- Chương 5. Động học ngược robot -8- Bài toán động học ngược Hàm atan2(y,x) y tan(x) Phương pháp hình học (sử dụng các hàm lượng giác) Phương pháp tách vị trí và hướng (các nhóm 3) Phương pháp dựa trên ma trận Jacobi cho tay máy dư -/2 /2 x dẫn động 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 y tan( )x x arctan( )y x 2 2 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 3
Chương 5. Động học ngược robot -9- Chương 5. Động học ngược robot -10- Hàm atan2(y,x) Giải bài toán ngược - phương pháp hình học ( ,x y) atan 2( ,y x), y0 q3 0<y O2 a3 a2 q2 a1 x<0 O q O1 0 1 0<x x Kết quả bài toán động học thuận: 0 y<0 cq sq 0 l cq l cq l cq 123 123 1 1 2 12 3 123 sq cq 0 l sq l sq l sq T0( )q 123 123 1 1 2 12 3 123 3 0 0 1 0 0 0 0 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -11- Chương 5. Động học ngược robot -12- Giải bài toán ngược - phương pháp hình học Giải bài toán ngược - phương pháp hình học Từ hình vẽ ta xác định được tọa độ điểm O2 Ma trận biến đổi tọa độ thuần y0 nhất khâu 3 (cho trước): q3 x x a c x a cq y O a3 O2 E 3 E 3 123 2 0 q y y a s y a sq 3 a O2 E 3 E 3 123 O2 a 2 q2 3 c s 0 x E a1 s c 0 y O q1 O1 r O O (x )2 (y ) ,2 a q 0 2 E x 0 2 O2 O2 2 H 0 a 0 0 1 0 1 O xO O =atan2(y ,x ) O0 q 1 1 0 0 0 1 l cq l cq l cq x 0 2 O2 O2 x 1 1 2 12 3 123 E 0 l sq l sq l sq y r2 a2 a2 1 1 2 12 3 123 E r2 a2 a2 2a a cosq cosq 2 1 : D, T0( )q H cq c 2 1 1 2 2 2 2a a 3 123 1 2 sq s sinq 1 D2 q atan2( 1 D2 ,D) 123 2 2 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 4
Chương 5. Động học ngược robot -13- Chương 5. Động học ngược robot -14- Giải bài toán ngược - phương pháp hình học Robot kiểu tay người = Anthropomorphic arm x3 O O O : Givens: [xC, yC, zC] a3 C 1 0 2 z0 sin a sinq / r, y0 q a2 q 2 2 3 3 cos (a a cosq ) / r, O2 a Find: q1,q2,q3 q zC 1 2 2 3 2 d x1 y y atan2(s,c) 1 a cq cq cq sq sq cq (l cq l cq ) C 0 2 q2 1 23 1 23 1 1 2 2 3 23 atan2(a sinq ,a a cosq ) a sq cq sq sq cq sq (l cq l cq ) r 2 2 1 2 2 1 T0( )q 1 23 1 23 1 1 2 2 3 23 , q O q O1 3 sq cq 0 l l sq l sq 1 q 0 1 23 23 1 2 2 3 23 x x’1 1 x 0 0 0 1 C 0 q q q x0 cq c, sq s q atan2(s,c) 23 2 3 123 123 123 y0 q q q q q q q q 123 1 2 3 3 123 1 2 q1 Điều gì xảy ra nếu a1 = a2? xO2 = yO2 = 0? C x0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -15- Chương 5. Động học ngược robot -16- Robot kiểu tay người = Anthropomorphic arm Robot kiểu tay người = Anthropomorphic arm Khi biết tọa độ điểm C [xC,yC,zC], từ hình chiếu trên mặt Ox0y0, ta z tính được góc Nghiệm q1 không xác định (có 0 q atan2(y ,x ) or q atan2(y ,x ) thể lấy tùy ý), khi C 1 C C 1 C C y x 0 Để giải tìm q2 và q3, ta xét hệ phẳng gồm hai khâu 2 và 3 trong mặt C C phẳng đứng. Giải hệ này như trường hợp tay máy phẳng 2 dof. tức là khi điểm C nằm trên trục y0 x3 a C z . Phương trình trên không xác z0 3 z 0 0 C a2 q s định và tay máy ở vị trí kỳ dị, q 3 1 q zC a như chỉ ra trên hình bên. Tại cấu 2 3 q x’1 d x 3 hình này, (C nằm trên z ) góc q 1 1 y y a2 0 1 x C 0 O O 2 có thể lấy giá trị tùy ý. 0 1 q r 2 d1 Một vị trí kỳ dị của robot q1 r x x’1 x’ C 1 x0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 5
Chương 5. Động học ngược robot -17- Chương 5. Động học ngược robot -18- Robot kiểu tay người = Anthropomorphic arm Robot scara 4dof Một phương án cải tiến để tránh Robot scara 4dof (RRTR) như hình vẽ, kết quả bài toán động x3 học thuận cho ta biết vị trí và hướng của bàn kẹp. kỳ dị: d2 0 a3 C a2 a y0 z q3 1 0 q zC a C 2 d 2 yC d x 1 a d1 2 1 B q q3 y r 2 q yC 4 C 2 y C y a1 r 0 r z 0 O C O 2 q x q1 q x x C 1 C 0 1 x q2 C x x 0 0 d2 y y c q( ) s q( ) 0 a cq a c q( q ) C 0 124 124 1 1 2 1 2 s q( ) c q( ) 0 a sq a s q( q ) q1 T0( )q 124 124 1 1 2 1 2 x 4 0 0 1 d q d q q q q C C 1 3 4 124 1 2 4 x 0 0 0 1 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -19- Chương 5. Động học ngược robot -20- Robot scara 4dof Robot scara 4dof a y Từ hình chiếu trên mặt (Oxy)0 cho ta xác định được 1 0 a C r2 a2 a2 2a a cosq y0 d 2 yC 2 1 1 2 2 1 a C q q3 y r 2 q r2 a2 a2 y 4 C 2 cosq 2 1 : D C C a r a2 z y 1 O 2 2a a q2 r C 0 O 2 1 2 a x q1 q x x sinq 1 D2 1 O C 1 C 0 2 q 2 O x 2 q1 C x0 x0 q atan2( 1 D2 ,D) 2 Trong thực tế hướng của khâu c s 0 q atan2(y ,x ) atan2(a sinq ,a a cosq ) 1 C C 2 2 1 2 2 cuối không thể tùy ý, ma trận R s c 0 T ( )q H q d z co sin chỉ hướng của khâu E 3,4 3,4 3 1 C 0 0 1 cuối chỉ ở dạng q q q atan2(s,c) q q q atan2(s,c) 1 2 4 4 1 2 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 6
Chương 5. Động học ngược robot -21- Chương 5. Động học ngược robot -22- 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 nhóm: nhóm 1 gồm ba phương trình tính theo tọa độ của khâu cuối; Hoặc có thể tồn tại điểm trung gian mà vị trí của nó chỉ phụ và nhóm 2 gồm các phương trình xác định hướng của khâu thuộc tối đa ba biến khớp q1, q2, q3 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 khâu cuối và ba bậc tự do cuối 4, 5, p p (q ,q ,q ) q ,q ,q 6 cho phép xác định hướng của khâu cuối. B B 1 2 3 1 2 3 p p (q ,q ,q ) q ,q ,q Đồng thời vị trí điểm trung gian này cũng được xác định từ tọa E E 1 2 3 1 2 3 độ điểm tác động cuối E và ma trận cosin chỉ hướng của bàn kẹp: R0 R0 (q ,q ,q ) q ,q ,q E E 4 5 6 4 5 6 p p R d0 (E) d(E) là véc tơ hằng số Hoặc … B E E Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -23- Chương 5. Động học ngược robot -24- Phương pháp các nhóm 3 Phương pháp các nhóm 3 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 p ,R0 R0 R0(q )R3(q ) E E E 3 1,2,3 6 4,5,6 R3(q ) [R0(q )]1R0 Tìm và tính được vị trí của điểm trung gian B 6 4,5,6 3 1,2,3 E p p R d0 (E) B E 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): R3(q ) [R0(q )]1R0 q ,q ,q p p (q ,q ,q ) q ,q ,q 6 4,5,6 3 1,2,3 E 4 5 6 B B 1 2 3 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 7
Chương 5. Động học ngược robot -25- Chương 5. Động học ngược robot -26- Phương pháp các nhóm 3 Phương pháp các nhóm 3 Ví dụ 1. Robot dạng tọa độ trụ có bàn tay (RTT-RRR) d q 3 z d6 Trong trường hợp này 3 y x y 4 q q z d6 2 2 3 x 5 E, O điểm trung gian B = O 3 3 y x y 4 q z 6 5 2 2 3 x 5 2 3 E, O6 z O z3 q z 2 q z 6 z y 3 5 6 z z q 1 4 O O 3 z q 6 2 1 5 q z y 3 q4 5 6 2 1 1 O5 x1 p p R u0 (6) p (q ) q x Bảng DH, thân robot trụ (RTT) 1 d1 q B E E B B 1,2,3 1,2,3 i d a z 1 d1 0 y z y0 1 q1 d1 0 0 0 R0 R0(q ,q ,q ) 0 0 2 /2 q2 0 /2 q1 3 3 1 2 3 x0 u(6) 0 q 3 0 q 0 0 B x 1 3 0 4 q d =0 0 /2 R0 R R0 3 d 4 4 E 3 6 6 5 q5 0 0 /2 3 0 T 0 3 6 q d 0 0 R [R ] R R (q ) q điểm trung gian ?? 6 6 6 3 E 6 4,5,6 4,5,6 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -27- Chương 5. Động học ngược robot -28- Phương pháp các nhóm 3 Phương pháp các nhóm 3 d Ví dụ 2: Stanford robot z z5 y q 3 z d6 6 6 3 E y x y 4 q x(0) (q d ) cosq q x 2 2 3 x 5 6 3 E, O6 B 3 3 1 1 q z y(0) (q d )sinq q : q 6 2 B 3 3 1 1,2,3 2 O z z z(0) d q q Bảng DH, thân robot trụ (RTT) d6 3 3 q z q6 6 B 1 2 3 x q z y 4 O 5 i d a y 4 2 1 1 5 4 z 1 q d 0 /2 4 q z(0) d x 2 B 1 1 1 x q y5 1 q (x(0))2 (y(0))2 d 2 q d 0 /2 5 z q4 5 d 3 B B 3 2 2 3 4 d1 q cosq x(0) / (q d ) 3 0 q 0 0 1 3 z 1 B 3 3 q atan2(si co, ) x y3 0 y sinq y(0) / (q d ) 1 4 q d 0 /2 3 0 1 B 3 3 4 4 y1 x q 5 q 0 0 -/2 1 3 z2 q1 5 x0 6 q d 0 0 q2 6 6 z y q 1 2 R0 R R0 3 d 1 x2 1 d E 3 6 2 z0 y R3 [R0 ]TR0 R3(q ) q điểm trung gian ? 0 6 3 E 6 4,5,6 4,5,6 x0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 8
Chương 5. Động học ngược robot -29- Chương 5. Động học ngược robot -30- Phương pháp các nhóm 3 Phương pháp các nhóm 3 z z5 y Ví dụ 3. Articulated Manipulator with spherical wrist (RRR-RRR) Trong trường hợp này điểm trung gian 6 6 B = O x6 Bảng thông số DH 4,5 q6 O i d a d z4 4,5 6 1 q d 0 /2 p p R u0 (6) p (q ) q x q5 1 1 y 4 z B E E B B 1,2,3 1,2,3 4 z 3 2 q2 0 a2 0 4 d y y 3 q 3 q3+/2 0 0 /2 x q x 2 4 q q 5 z 3 6 0 5 z 5 d 0 4 q d 0 /2 4 4 4 3 3 a q 2 q3 x2 O 5 q 0 0 /2 u(6) 0 O 2 5 B y3 1 z O2,3 6 6 q d 0 0 R0 R0(q ,q ,q ) x3 2 6 6 3 3 1 2 3 d y x q d x 6 1 1 3 z 1 1 y 2 z 0 q 1 2 O0 0 0 3 z1 y R R R q1 x 2 q1 E 3 6 d1 2 d2 R3 [R0 ]TR0 R3(q ) q z Tìm điểm trung gian ? 0 y0 x’ 6 3 E 6 4,5,6 4,5,6 1 x x0 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -31- Chương 5. Động học ngược robot -32- Phương pháp các nhóm 3 Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Trong trường hợp này Nghiệm của hệ phương trình đại số tuyến tính với ma trận hệ z O4,5 4 q điểm trung gian B = O4,5 số A dạng chữ nhật 5 z3 y d3 q p p R u0 (6) Ax y x 2 4 q B E E B z 3 6 0 p (q ) q q a2 q x A mn, y m1, x n1 2 3 2 O B 1,2,3 1,2,3 O1 O 6 z2 2,3 d x y0 1 1 0 z Trường hợp 1. 1 u(6) 0 O0 B q d m n, det(A) 0 1 R0 R0(q ,q ,q ) 6 x A1y x’1 3 3 1 2 3 x R0 R R0 3 [A vuông, không suy biến] 0 E 3 6 R3 [R0 ]TR0 R3(q ) q 6 3 E 6 4,5,6 4,5,6 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 9
Chương 5. Động học ngược robot -33- Chương 5. Động học ngược robot -34- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Ax y Nghiệm của hệ phương trình đại số tuyến tính với ma trận hệ Trường hợp 2. số A dạng chữ nhật Ax y A, mn, y m1, x n1 (1) m n, rank(A) m Ma trận A thể hiện một ánh xạ tuyến tính từ không gian X vào không gian Y. A chữ nhật số cột > số hàng, full rank, Số ẩn nhiều hơn Như vậy, nếu x* là nghiệm của số phương trình. (1) thì với x là véc tơ thuộc 0 Hệ có thể có nghiệm nếu y Î Âm không gian không của A, ta có x Î Ân x* + x cũng là nghiệm của (1). A Î Âm´n 0 rank(A) rank([A y, ]) (A) Ax* y Hệ có thể có vô số nghiệm (A) 0 Ax 0 x (A) 0 0 x ??? A x( * x ) y 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -35- Chương 5. Động học ngược robot -36- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Xét tay máy dư dẫn động, ta có quan hệ động học sau f f f 1 1 ... 1 q q q f x q( , ) 0 f 1 2 n J ... ... ... ... , q q x [x x ... x ] ,T q [q q ... q ]T f f f 1 2 m 1 2 n m m ... m q q q 1 2 n Đạo hàm phương trình trên theo thời gian ta nhận được: f f f 1 1 ... 1 J x J q 0 x x x x q f 1 2 m J ... ... ... ... x x f f f m m ... m x x x 1 2 m Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 10
Chương 5. Động học ngược robot -37- Chương 5. Động học ngược robot -38- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Giả sử ma trận J không suy biến, suy ra Jq x, (1) x J m n , q n1, x m1 J x J q 0 x q Trường hợp tay máy dư dẫn động n>m, ma trận J là ma trận chữ nhật. Phương trình đại số trên có số ẩn nhiều hơn số x J1J q Jq, J J1J . phương trình. Do đó hệ trên có thể có vô số nghiệm. x q x q Xét trường hợp tay máy không ở vị trí kỳ dị, rank(J) = m, J has a full rank. Hệ có vô số nghiệm, ta sẽ tìm nghiệm bằng cách bổ sung thêm ràng buộc. 1 g( )q (q q )T W q( q ) min, WT W 0 2 0 1 0 1 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -39- Chương 5. Động học ngược robot -40- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Jq x, (1) ( ,q ) W q( q ) JT 0 (1) q 1 0 Về mặt hình học, thì điểm qdot tìm được cần thỏa mãn (1) và gần điểm q nhất. Phương pháp nhân tử Lagrange được sử dụng để giải bài toán này:0dot ( ,q ) (x Jq) 0 (2) 1 ( ,q ) (q q )T W q( q ) T (x Jq) min Từ (1) giải được 2 0 1 0 q q W1JT (3) Từ đó dẫn đến cần giải hệ sau 0 1 Thay vào (2) nhận được ( ,q ) 0 & ( ,q ) 0 q*, * (x Jq) 0 x J q W1JT 0 (4) q 0 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 11
Chương 5. Động học ngược robot -41- Chương 5. Động học ngược robot -42- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Thay vào (3) nhận được Từ (4) giải được x J q W1JT 0 x Jq JW1JT 0 q W1JT [JW1JT ]1x I W1JT [JW1JT ]1 J q (5) 0 1 0 1 1 1 1 1 0 [JW1JT ] (1 x Jq ) 1 0 Thay vào (3) nhận được Thành phần thuộc không gian không của J q q W1JT [JW1JT ] (1 x Jq ) (5) 0 1 1 0 W1JT [JW1JT ]1x I W1JT [JW1JT ]1 J q Thành phần nghiệm có chuẩn nhỏ 1 1 1 1 0 nhất, với trọng số W1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -43- Chương 5. Động học ngược robot -44- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Ta thấy rằng 1 T 1 T 1 Thực tế, tác dụng của q0_dot là để tạo ra chuyển động bên JW J [JW J ] I trong của cấu trúc (nội chuyển động) mà không làm thay đổi 1 1 J I W1JT [JW1JT ]1 J 0 vị trí và hướng của khâu cuối, ví dụ, tay máy cấu hình lại 1 1 thành nhiều tư thế khéo léo để thực hiện một nhiệm vụ nhất định. Jq JW1JT [JW1JT ]1x J I W1JT [JW1JT ]1 J q x Jq JW1JT [JW1JT ]1x J I W1JT [JW1JT ]1 J q x 1 1 1 1 0 1 1 1 1 0 Một hệ quả trực tiếp là, trong trường hợp xdot = 0, có thể tạo Điều đó có nghĩa rằng, thành phần q không làm thay đổi ra chuyển động nội bộ được mô tả bởi 0_dot chuyển động của khâu cuối. Việc chọn véctơ này cho phép ta q I W1JT [JW1JT ]1 J q khai thác được ưu điểm (khả năng tránh vật cản, tránh va 1 1 0 chạm với giới hạn khớp, tránh cấu hình kỳ dị) của robot dư dẫn động so với robot chuẩn. cấu hình lại cơ cấu tay máy mà không thay đổi vị trí và hướng khâu cuối. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 12
Chương 5. Động học ngược robot -45- Chương 5. Động học ngược robot -46- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Cuối cùng, ta sẽ thảo luận về việc chọn véc tơ q để khai Do nghiệm dịch chuyển dọc theo gradient của hàm mục 0_dot tiêu, nên cố gắng để tối đa hóa nó tại địa phương phù hợp thác các ưu điểm của các DOFs dư. Một sự lựa chọn điển hình là với mục tiêu chính (ràng buộc động học). Các hàm mục tiêu điển hình là: æ¶w( )q öT q = k çç ÷÷ , k > 0 0 0 çç ¶q ÷÷ 0 • Số đo khả năng thao tác, định nghĩa bởi è ø w( )q = det[ ( )J q JT ( )]q w( )q là hàm mục tiêu (thứ cấp) của các biến khớp. mà triệt tiêu tại các cấu hình kỳ dị, do đó, bằng cách làm cực đại hàm này, các tọa độ suy rộng dư sẽ tái cấu trúc lại tay máy để tránh các điểm kỳ dị. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -47- Chương 5. Động học ngược robot -48- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động • Số đo khoảng cách đến các giới hạn cơ khí của khớp, định nghĩa bởi • Số đo khoảng cách đến chướng ngại vật, định nghĩa bởi æ q -q ö2 w( )q = -1 ån c ççç i i ÷÷÷ q = 1 (q +q ) w( )q min p q( ) o 2 i çèq -q ø÷÷ i 2 iM im i=1 p o, iM im qM q q q ,q 4 là ký hiệu của giới hạn lớn nhất, nhỏ nhất, trong đó o là véc tơ vị trí của điểm thích iM im Vật cản qm q hợp trên chướng ngại vật (tâm chẳng hạn, là giá trị giữa của khoảng làm việc của khớp, nếu vật cản được mô hình hóa bằng hình i q cầu), và p(q) là véc tơ vị trí của một điểm 3 c và các trọng số dương q i trên tay máy (điểm có nguy cơ va chạm 2 Do đó, cực đại của hàm này sẽ làm cho các biến khớp di chuyển về giá cao); bằng cách cực đại hóa hàm này, tính trị trung gian, tính dư dẫn động sẽ được khai thác để giữ cho các biến dư dẫn động được khai thác để tránh va O q1 0 khớp gần giá trị giữa của khoảng làm việc của mỗi khớp, tránh được va chạm vào chướng ngại vật. chạm với các giới hạn khớp. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 13
Chương 5. Động học ngược robot -49- Chương 5. Động học ngược robot -50- Động học ngược tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Xử lý vấn đề kỳ dị động học Sau khi xác định được véc tơ vận tốc khớp, nếu tích phân ta Trong phần trên ta đã giải cho trường hợp J có hạng đầy đủ. Các công sẽ nhận được véc tơ tọa độ khớp, và đạo hàm sẽ nhận được thức trên sẽ không sử dụng được nếu tay máy ở vị trí kỳ dị (J giảm hạng). gia tốc khớp. ì Khi tay máy ở cấu hình kỳ dị hay gần đó, ta sẽ không tìm đc nghiệm của ïïq( )t =q(0)+ òt q( )t dt phương trình động học. Một phương án xử lý vấn đề này là thỏa hiệp giữa ïï q = ..... í d 0 sai số động học và chuẩn của véc tơ vận tốc khớp. ïïq = q 1 1 ïïî dt g( )q (x Jq) (T x Jq) k2q qT min với x Jq 0 Như thế, ta có sơ đồ sau 2 2 Kết quả cho ta như sau: x( )t x( )t 1 q q k2I J JT 1 J xT Chú ý rằng: x( )t q = ..... s q x Jq 0 q x(0) tìm o 1 d q k2I J JT JT - damped least-squares inverse of J dt Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -51- Chương 5. Động học ngược robot -52- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Tính gia tốc khớp: d Cần tính J† 1. Sử dụng đạo hàm số từ các giá trị q_dot q = q dt J† W1JT [JW1JT ]1 1 1 2. Sử dụng công thức đạo hàm số từ biểu thức của q_dot J JW†[ 1JT ] W1JT 1 1 J JW†[ 1JT ]+J JW†[ 1JT ]+J JW†[ 1JT ] W1JT q W1JT [JW1JT ]1x I W1JT [JW1JT ]1J q 1 1 1 1 1 1 1 1 0 J x† (I J J q† ) 0 J† W1JT J JW†[ 1JT ]-J JW†[ 1JT ] [JW1JT ] .1 1 1 1 1 q J x† J x† (I J J q† ) J Jq† J Jq† 0 0 0 Cần tính J† Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 14
Chương 5. Động học ngược robot -53- Chương 5. Động học ngược robot -54- Phương pháp số giải động học ngược: pp lặp Newton-Raphson Giải bài toán vị trí tìm q(0) ứng với x(0): Phương pháp lặp Newton-Raphson f (x ,x ,...,x ) = 0 1. Chuyển thành bài toán tối ưu có ràng buộc 1 1 2 n Tìm nghiệm hệ n phương trình n ẩn f (x ,x ,...,x ) = 0 2. Sử dụng phương pháp lặp Newton-Raphson 2 1 2 n f x( ) = 0 f, Î n , x Î n ... Cho x(0) cần tìm q(0) thỏa mãn phương trình động học vị trí f (x ,x ,...,x ) = 0 n 1 2 n f x q( , ) 0 và để hàm sau đạt cực trị 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 S S( )q min (or max) toán 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 phân ly nghiệm. Một phương pháp đơn giản và hiệu quả đó Sử dụng các phương pháp giải bài toán tối ưu có ràng là phương pháp lặp Newton-Raphson. buộc để giải: pp nhân tử Lagrange, pp hàm phạt, … Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -55- Chương 5. Động học ngược robot -56- 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 Trước hết ta khai triển Taylor hàm f(x) ở lân cận x0 Giả sử x0 là một xấp xỉ ban đầu của nghiệm cần tìm và để x0 + x là một f x( ) = 0, giá trị gần nghiệm hơn (chính xác hơn), ta cho f(x0 + x) = 0. ¶f Từ đó nhận được một hệ phương trình đại số tuyến tính đối với x. f x( + dx) = f x( ) + (x )dx +O(dx)2 0 0 ¶x 0 = f x( ) + J x( )dx +O(dx)2 0 0 f x( + dx) = f x( ) + J x( )dx = 0 0 0 0 Ma trận jacobi được xác định tại điểm x é ¶f ¶f ¶f ù 0 ê 1 1 .. 1 ú J x( )dx = -f x( ) ê¶x ¶x ¶x ú 0 0 ê 1 2 n ú ê ¶f ¶f ¶f ú dx = -J-1(x )⋅ f x( ) ¶f ê 2 2 .. 2 ú J x( ) = (x ) = ê¶x ¶x ¶x ú(x ) 0 0 0 ¶x 0 ê 1 2 n ú 0 ê .. .. .. .. ú Lấy x = (x + dx) ê ú 0 0 ê¶f ¶f ¶f ú ê n n .. n ú ê¶x ¶x ¶x ú Lặp lại quá trình trên đến khi f x( ) £ e ë 1 2 n û 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 15
Chương 5. Động học ngược robot -57- Chương 5. Động học ngược robot -58- Phương pháp lặp Newton-Raphson giải hệ f(x) = 0 Phương pháp số giải động học ngược : pp lặp Newton-Raphson 1. Chọn sai số cho phép err; số bước Bắt đầu lặp tối đa M; chọn xấp xỉ ban đầu B l2 x0; khởi gán k = 0; x(0) = x0 •Chọn sai số cho phép, err •Chọn số bước lặp tối đa, M A 2. Tính f(x ) •Khởi động k = 0, chọn x0 q2 q k l 3. Kiểm tra nếu || f(x ) || <= err lấy 3 3 k l nghiệm x = x và dừng thuật toán; Tính f(xk) 1 q D k 1 trái lại tiếp tục bước 4. Yes O f(x ) £err k 4. Tính ma trận jacobi J(xk); No 5. Giải hệ J(xk)x = -f(xk); [sử dụng Tính J x( ) k ma trận tựa nghịch đảo] Giải hệ chữ nhật J x( )Dx = -f x( ) k k 6. Hiệu chỉnh x = x + x ; Chỉnh x = x + Dx y k k k k Tăng k k = k +1 Ảnh hưởng của A 7. Tăng bước k = k + 1; Yes k £ M 8. Kiểm tra số bước lặp, nếu k < M No xấp xỉ ban đầu quay lại bước 2; trái lại “Pp không Không hội tụ với Lấy nghiệm x B k M và x x hội tụ với x và M bước lặp” 0 O 0 9. Kết thúc. Kết thúc Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -59- Chương 5. Động học ngược robot -60- Ví dụ él cosq + l cos(q + q ) + l cos(q + q + q )- p ù ê 1 1 2 1 2 3 1 2 3 x ú f x( ) = êl sinq + l sin(q + q ) + l sin(q + q + q )- p ú = 0 ê 1 1 2 1 2 3 1 2 3 y ú ê q + q + q - f ú êë 1 2 3 3 úû Lưu ý: PP 2 nhóm 3 đối với tay máy robot không gian E => PP nhóm 2 + 1 đối với tay máy robot phẳng. q 3 l3 ?x = (q ,q ,q ), 1 2 3 3 Biet (p ,f ) = (p ,p ,f ) y0 E 3 x y 3 O2 l2 Tính toán với q l = 1,l = 0.8,l = 0.3 2 1 2 3 l1 O p = (p ,p ) = (1.2,1.2) 1 E x y q1 x 0 f = 90 q ? O0 3 1,2,3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 16
Chương 5. Động học ngược robot -61- Chương 5. Động học ngược robot -62- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động Giả sử tại thời điểm t ta đã có các giá trị x(t), q(t), qdot(t) ta él cosq + l cos(q + q ) + l cos(q + q + q )- p ù ê 1 1 2 1 2 3 1 2 3 x ú cần tính q(t+dt). Sử dụng các công thức tích phân gần đúng f x( ) = êl sinq + l sin(q + q ) + l sin(q + q + q )- p ú = 0 ta có thể tính được, chẳng hạn ê 1 1 2 1 2 3 1 2 3 y ú ê q + q + q - f ú t+dt êë 1 2 3 3 úû q(t +dt) =q( )t + ò q( )t dt t (1) x = [q ,q ,q ]T =q( )t +q( )t dt + 1 qdt2 1 2 3 2 é ù Vấn đề xuất hiện ở đây là các giá trị tính toán gần đúng này ê-l sq -l sq -l sq -l sq -l sq -l sq ú có thể sẽ tích lũy sai số và do đó sẽ không thỏa mãn phương ¶f x( ) ê 1 1 2 12 3 123 2 12 3 123 3 123 ú J x( ) = = ê l cq + l cq + l cq l cq + l cq l cq ú trình ràng buộc vị trí nữa, ¶x ê 1 1 2 12 3 123 2 12 3 123 3 123 ú êë 1 1 1 úû f x[ (t +dt),q(t +dt)]¹ 0 q = q + q 12 1 2 Do đó các phương án nhằm hạn chế sai số tích lũy này cần q = q + q + q được xem xét. 123 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -63- Chương 5. Động học ngược robot -64- Phương pháp dựa trên ma trận Jacobi – Tay máy dư dẫn động f x[ (t +dt),q(t +dt)]¹ 0 Phương án sử dụng phản hồi động học Ý tưởng của phương pháp xuất phát từ phương trình vi phân Do đó các phương án nhằm hạn chế sai số tích lũy này cần được xem xét. e Ke, 1. Phương án hiệu chỉnh véc tơ biến khớp sau mỗi bước tính K Là ma trận xác định dương, có các trị riêng - Coi như giá trị nhận được theo (1) là xấp xỉ ban đầu, dương, (hoặc yêu cầu K có các trị riêng có - Sử dụng phương pháp lặp Newton-Raphson để vi chỉnh phần thực dương) giá trị đó. 2. Phương án sử dụng phản hồi động học. Nghiệm của phương trình vi phân trên sẽ hội tụ về 0 e( )t 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 17
Chương 5. Động học ngược robot -65- Chương 5. Động học ngược robot -66- Đặt sai số (nếu có) của phương trình ràng buộc vị trí Sơ đồ khối phương pháp (hậu) hiệu chỉnh e f x q( , ) e J x J q x q x( )t x e Ke, e J x J q Ke x( )t q 1 q Hiệu q x q Jq x x( )t s chỉnh q J1J q x J1Ke hay Jq x J1Ke, x(0) tìm q x q x x o d q Jq x dt Như vậy thay vì tính qdot từ phương trình Ta sử dụng phương trình Jq x J Ke1 , e f x q( , ) x Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -67- Chương 5. Động học ngược robot -68- Động học ngược tay máy dư dẫn động Sơ đồ khối phương pháp phản hồi động học Phản hồi động học và phương pháp chiếu (hiệu chỉnh sau) Why? x( )t x( )t q 1 q Jq x J K f x q1 ( , ) s x( )t x q x(0) tìm q o d q How? dt Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 18
Chương 5. Động học ngược robot -69- Chương 5. Động học ngược robot -70- Ví dụ và Bài tập Một số vấn đề mở rộng • Ví dụ và Bài tập x3 E O , - Tối ưu động năng tại mỗi thời điểm (cục bộ) 3 y 3 3 - Giải bài toán động học ngược ở cấp độ gia tốc a O O , l O O 0 1 3 2 3 y z O - Tối ưu mô men / lực phát động E 3 (x ,y , ) q [q ,q ,q ]T x 3 - Giải quyết tình huống khi có khớp kẹp E E 3 1 2 3 2 q3 z1 q2 y0 O z 2 • Cho x1 2 E O , 3 O a O O 0.3, l O O 0.5 y 1 0 1 3 2 3 1 z q1 (x ,y , ) (0.8, 1.1, 60 ) 0O E E 3 0 x q [q ,q ,q ]T ?? xE 0 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -71- Chương 5. Động học ngược robot -72- Ví dụ và Bài tập • Ví dụ và Bài tập q q 3 2 E E O , a O O , q 1 3 0 1 l O O 3 2 3 (x ,y , ) q [q ,q ,q ]T E E 3 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 19
Chương 5. Động học ngược robot -73- Chương 5. Động học ngược robot -74- Ví dụ và Bài tập Ví dụ và Bài tập cho l cho l 1,2,3 1,2,3 [x ,y ,j ] [q ,q ,q ] [x ,y ,j ] [q ,q ,q ] [x ,y ,j ] [q ,q ,q ] E E 3 1 2 3 E E 3 1 2 3 E E 3 1 2 3 q3 q3 y yE yE E q2 3 3 3 y 3 E q2 q q2 3 q2 q1 O q1 O q O q1 0 0 1 0 x xE x O x E E 0 E Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -75- Chương 5. Động học ngược robot -76- Ví dụ và Bài tập Ví dụ và Bài tập l2 0.5, h 0.4 cho l 1,2,3 (x ,y , ) (1.5, 0.3, 60 ) E E 3 3 q [q ,q ,q ]T ?? q [x ,y ,j ] [q ,q ,q ] 1 2 3 1 E E 3 1 2 3 q3 q2 q3 yE h q3 q2 3 yE O 0 x y 3 E E 3 q3 q 1 q1 q q2 1 O0 O x 0 E xE O0 q 2 l 0.7,l 0.4, 2 3 x (x ,y , ) (1.2, 0.9, 80 ) E E E 3 q [q ,q ,q ]T ?? 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 20
Chương 5. Động học ngược robot -1- Chương 5. Động học ngược robot -2- Động học ngược robot song song Bàn máy động - Bàn máy cố định, giá gắn với hqc Chương 5. quán tính (CS0) CSE - Bàn máy động, (khâu cuối)-CS 2. Động học ngược robot song song E Bi Inverse kinematics of parallel robots (dịch từ bài giảng của GS. Bodo Heimann Uni Hannover, Germany) - Các chân có cấu trúc chuỗi {AiBi} - Các chân tạo thành từ các Nguyễn Quang Hoàng khâu khớp: Ai Email: hoang.nguyenquang@hust.edu.vn - khớp cầu (S) CS0 - khớp quay (bản lề) (R) - khớp trượt (P) Bàn máy cố định - Các chuỗi động quyết định đến tính chất của robot song song Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 5. Động học ngược robot -3- Chương 5. Động học ngược robot -4- Các ví dụ về robot song song 2 dof Một số robot phẳng song song 3dof Robot phẳng song song Ví dụ: 2-DOF PA-R-MA (RRR-PKM): Các ví dụ từ IWF (Institut für Parallel Redundant Manipulator Werkzeugmaschinen und Fertigungstechnik, TU TU Braunschweig Braunschweig) PARA 3 (RRR-PKM): Nanyang Technological University Singapore Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Hiển thị 20 trang đầu. Tải tài liệu để xem đầy đủ.
Bình luận (0)
Chưa có bình luận nào. Hãy là người đầu tiên!