Trang 1
-1- -2- Động học robot: thuận và ngược 1. Mở đầu về động học robot Chương 3. Cấu trúc robot và Mô hình cơ học Các loại khớp Động học: vị trí, ma trận trạng thái Bậc tự do khâu Robot chuỗi, robot song song Robot chuẩn, robot dư dẫn động 2. Ma trận trạng thái khâu Nguyễn Quang Hoàng Cách xác định ma trận trạng thái khâu Email: hoang.nguyenquang@hust.edu.vn Phương pháp Denavit-Hartenberg 3. Động học thuận, động học ngược robot chuỗi Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -3- -4- Cấu trúc robot và mô hình cơ học Serielle Kinematik Parallele Kinematik MỞ ĐẦU VỀ ĐỘNG HỌC Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 2
-5- -6- Cấu trúc robot và mô hình cơ học Cấu trúc robot và mô hình cơ học Cấu trúc song song = Chuỗi động học đóng Cấu trúc lai Cấu trúc = chuỗi Chuỗi động = học đóng và Song song: mở Chuỗi: Lai: Chuỗi Delta-Roboter, động học Kuka-R15 Tricept, ABB – IRB 940 ABB – Flexpicker mở Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -7- -8- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Định nghĩa chuỗi động Phân loại tô pô Chuỗi động là một hệ nhiều vật kết nối với nhau bằng các khớp, đó là Hệ nhiều vật có cấu trúc cây (chuỗi động hở), các mô hình cơ học cho một hệ nhiều vật như cơ cấu máy, robot, ... Hệ nhiều vật có cấu trúc mạch vòng (chuỗi động kín), Như đã quen sử dụng từ chuỗi, các vật thể rắn, hay các khâu, Hệ nhiều vật có cấu trúc hỗn hợp, thường bị ràng buộc bởi các kết nối của nó với vật thể khác hoặc nối Hệ nhiều vật có cấu trúc thay đổi. với giá cố định. Một ví dụ là chuỗi mở đơn giản được hình thành bởi các vật kết nối liên tiếp, như chuỗi thông thường, đó là mô hình động Hệ nhiều vật có cấu trúc cây: học cho một tay máy Robot điển hình. • Số khớp n = p số vật thể J chuyển động (khâu động) Hệ nhiều • Có một đường duy nhất nối từ vật có liên khâu k đến giá cố định kết với Hệ nhiều vật có cấu mạch vòng: giá cố • Số vòng động học n = n - p định L J Hệ nhiều vật có chuỗi động • Có nhiều hơn một đường nối từ không liên kết với giá cố định khâu k đến giá cố định Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 3
-9- -10- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Phân loại tô pô Hệ nhiều vật có cấu trúc cây (chuỗi động hở), HỆ NHIỀU VẬT Hệ nhiều vật có cấu trúc mạch vòng (chuỗi động kín), Hệ nhiều vật có cấu trúc hỗn hợp, Hệ nhiều vật có cấu trúc thay đổi. CẤU TRÚC CÂY CẤU TRÚC MẠCH VÒNG CẤU TRÚC HỖN HỢP (CHUỖI HỞ, SERIAL (CHUỖI KÍN, CLOSED (GỒM CẤU TRÚC STRUCTRURES) STRUCTRURES) VÒNG VÀ CHUỖI HỞ) Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -11- -12- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Định nghĩa cơ cấu Cơ cấu là một chuỗi động kín trong đó có một khâu cố định là giá đỡ. [Cơ cấu là một hệ nhiều vật có cấu trúc mạch vòng] y q2 q u 3 q 4 2 q1 d y1 O x dx1 q 5 u1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 4
-13- -14- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Sự phân loại động học Sự phân loại động học 1. Chuỗi động phẳng (các điểm chuyển động 3. Chuỗi động cầu (các điểm thuộc hệ chuyển động trên trong các mặt phẳng song song với một mặt các mặt cầu đồng tâm, trục của các khớp quay đồng quy) quy chiếu) 2. Chuỗi động không gian Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -15- -16- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Các khớp động trong hệ nhiều vật Các khớp động và bậc tự do của khớp động: Khớp và bậc tự do của khớp Một vật rắn tự do trong không gian có 6 bậc tự do (3 tịnh tiến và 3 quay) Nếu hai vật không kết nối với nhau giữa chúng sẽ có 6 chuyển động Vật thể tự do trong không gian có 6 bậc tự do (3 TT và 3 Q). Khớp động là nơi hai vật thể kết nối với nhau cho phép hai vật tương đối. chuyển động tương đối với nhau được. Khi hai vật liên kết với nhau bằng một khớp, chuyển Khớp hạn chế sự tự do tương đối giữa hai vật. [Khớp cản trở một số động tương đối giữa hai vật bị hạn chế. Bậc tự do chuyển động tương đối giữa hai vật]. của khớp i f = 6 - r Bậc tự do của khớp i ký hiệu f , Ji i Ji Một số loại khớp: f = 6 - r , 1 £ f <6 r = số bậc tự do bị hạn chế khớp i. Khớp trượt (khớp tịnh tiến), khớp bản lề trụ (1dof) Ji i Ji i Khớp đinh ốc (2dof), khớp cầu (3dof), khớp Cardan (2dof), khớp Phân loại khớp: phẳng, Khớp thấp: khớp có tiếp xúc mặt Khớp cao: khớp có tiếp xúc điểm hoặc đường Hai loại khớp hay được sử dụng: khớp quay và khớp tịnh tiến. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 5
-17- -18- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Một số loại khớp thấp sử dụng trong hệ nhiều vật Khớp Cardan (2 dof) Khớp quay = Khớp bản lề trụ Khớp trượt = Khớp tịnh tíến f = 1 f = 1 J J Khớp trụ Khớp xoắn (đinh ốc) f = 2 f = 1 J J Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -19- -20- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Khớp bản lề cầu (khớp cầu): f = 3 Khớp phẳng: f = 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 6
-21- -22- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Các khớp cao: tại khớp hai vật tiếp xúc đường hoặc điểm với nhau. Ví dụ: khớp trong cơ cấu cam, bánh răng Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -23- -24- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Số bậc tự do và các tọa độ suy rộng Khớp cầu, p = 3 f = 1, Định lý Gruebler 3 dof Khớp quay, J 1 n n n = 4 f = 3, åJ åJ 1 dof f = 6p - (6 - f )- f = 6(p -n )+ f - f J J 2 Ji th J Ji th Khớp cardan, f = 2, i=1 i=1 2 dof J 3 p Số khâu động n Số khớp động f Số bậc tự do của khớp i f = 1 J Ji J 4 f f = 0 Số bậc tự do thừa [bậc tự do mà chuyển động của nó không th th làm thay đổi cấu hình của hệ] Khớp trượt, n åJ Đối với hệ nhiều vật phẳng 1 dof f = 6(p - n ) + f - f n Khớp cầu Khớp cầu J Ji th åJ f = 3p - (3 - f )- f i=1 Ji th = 6(3 - 4) + (1 + 3 + 2 + 1)- 0 i=1 n åJ = 3(p -n )+ f - f bậc tự do thừa = 1 J Ji th i=1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 7
-25- -26- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động z Cơ cấu 4 khâu phẳng (3 khâu động) p = 3 f = 1, Khớp cầu J 1 Cơ cấu tay quay con trượt Cơ cấu 4 khâu bản lề Khớp quay n = 4 f = 3, J J 2 bậc tự do thừa f = 3, J 3 Khớp cầu f = 1 J 4 y f = 1 x th Khớp trượt n p = 3 n åJ åJ f = 6(p - n ) + f - f f = 3(p - n ) + f - f J Ji th n = 4 J Ji th i=1 J i=1 = 6(3 - 4) + (1 + 3 + 3 + 1)-1 f = f = f = f = 1 = 3(3 - 4) + (1 + 1 + 1 + 1)- 0 J 1 J 2 J 3 J 4 = 1 f = 0 = 1 th Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -27- -28- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động Các tọa độ suy rộng (xét hệ hô lô nôm) Các tọa độ suy rộng (xét hệ hô lô nôm) Các tọa độ suy rộng là bộ các thông số để xác định vị trí của cơ hệ trong một Khi sử dụng tọa độ suy rộng dư: hệ quy chiếu nào đó. Phân các tọa độ suy rộng thành hai tập: các tọa độ độc lập và các tọa Gọi số các tọa độ suy rộng là m, m f = số bậc tự do của hệ. độ phụ thuộc Ký hiệu T q = [q ,q ,....,q ] ,T m ³ f q = [q ,q ,....,q ] , m > f 1 2 m 1 2 m = [qT ,qT ]T = [qT ,zT ]T là véc tơ các tọa độ suy rộng i d i q = [q ,q ,....,q ] ,T Tọa độ suy rộng đủ Tọa độ suy rộng dư i 1 2 f m = f m > f q = [q ,q ,....,q ]T =: [z ,z ,....,z ]T =: z d f +1 f +2 m 1 2 r Khi sử dụng tọa độ suy rộng dư: Các phương trình ràng buộc Số các ràng buộc đặt lên các tọa độ suy rộng = Số phương trình ràng buộc r = m - f f ( )q = f (q ,q ) = f (q ,z) = 0, k = 1, 2,...,r k k i d k i Số phương trình ràng buộc f ( )q = f (q ,q ,....,q ) = 0, i = 1, 2,...,r q = z = z q( ) i i 1 2 m d i Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 8
-29- -30- Các mối quan hệ cơ bản của chuỗi động Các mối quan hệ cơ bản của chuỗi động q = [q ,q ,...,q ] ,T dim( )q = f, q Î Âf 1 2 f 4 DOF Robot Arm, RRRR 3 DOF Robot Arm, RRT 4 DOF Robot Arm, RRRR 3 DOF Robot Arm, RRT Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -31- -32- Không gian biến khớp – không gian thao tác Không gian biến khớp – không gian thao tác Ký hiệu các thông số xác định vị trí và hướng của khâu cuối: Không gian biến khớp: (0) (0) (0) (0) T q q q , q Q r : r [x , y , z ] R min max R E E E E E n n ˆr(0) [x(0), y(0), z(0), 1]T E E E E n Không gian thao tác n e e 3 3 r n r n Chẳng hạn sử dụng 3 góc Euler, hoặc RPY e2 e2 On e(n) x x x , x X On e(n) 1 min max 1 q [, , ]T R R 0 0 O O Véc tơ thể hiện vị trí và hướng khâu cuối 0 0 x p [r(0)T ,qT ]T E E E [x(0), y(0), z(0), , , ]T E E E Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 9
-33- -34- Các mối quan hệ cơ bản của chuỗi động Robot chuẩn và robot dư dẫn động • Tọa độ suy rộng (generalized joint coordinates) hay tập các biến • Số bậc tự do khâu tác động khớp (joint variables), ký hiệu q: cuối là: q = [q ,q ,...,q ] ,T dim( )q = f, q Î Âf o 6 dof (robot không gian) 1 2 f o 3 dof (robot phẳng) q4 B E • Robot chuẩn không gian: q q o Robot 6 dof, dim(q) = 6 3 2 q3 • Robot dư dẫn động không gian: y q o Robot > 6 dof, dim(q) > 6 q 2 1 • Robot chuẩn phẳng: A O q1 o Robot 3 dof, dim(q) = 3 x q = [q ,q ,q ]T q = [q ,q ,q ,q ]T • Robot dư dẫn động phẳng: 1 2 3 1 2 3 4 o Robot > 3 dof, dim(q) > 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -35- -36- Robot chuẩn và robot dư dẫn động Robot chuẩn và robot dư dẫn động • robot dư dẫn động: có độ linh hoạt cao, nhưng động học ngược q4 B robot dư dẫn động phức tạp hơn, có thể cho vô số nghiệm. E q q 3 2 q3 y q q2 1 A O q1 x q = [q ,q ,q ]T q = [q ,q ,q ,q ]T 1 2 3 1 2 3 4 • Động học ngược robot dư dẫn động phức tạp hơn, có thể cho vô số nghiệm. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 10
-37- -38- Robot chuẩn và robot dư dẫn động • robot dư dẫn động: có độ linh hoạt cao, nhưng động học ngược robot dư dẫn động phức tạp hơn, có thể cho vô số nghiệm. MA TRẬN TRẠNG THÁI KHÂU BẤT KÌ VÀ KHÂU CUỐI Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Ma trận trạng thái khâu bất kì và khâu cuối -39- -40- Ma trận trạng thái khâu bất kì và khâu cuối • Thông số xác định vị trí và hướng khâu cuối: Ai Khâu An 1 Khâu cuối, • Hệ tọa độ gắn vào khâu cuối, khâu n: (Oxyz) i1 i+1 n n khâu n • Vị trí gốc On, ma trận cosin chỉ hướng khâu cuối Khâu i r(0) [x(0),y(0),z(0) ] ,T R0 T0 On On On On n i z 0 T0 n Vị trí của khâu cuối, khâu • Nếu sử dụng tọa độ thuần nhất: R0 ( )q r(0)( )q T0 n, trong hệ cố định: n O y T0( )q n On 0 0 n 0 1 Vị trí của khâu i trong hệ T0 x0 13 cố định: i Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 11
-41- Ma trận trạng thái khâu bất kì và khâu cuối -42- Ma trận trạng thái khâu bất kì và khâu cuối • Thiết lập ma trận trạng thái khâu cuối Ma trận trạng thái của khâu cuối n, (vị trí gốc O và hướng của khâu) n – Xây dựng các hệ trục tọa độ gắn liền các khâu 0-1-2-…-n trong hệ cố định: – Xác định ma trận biến đổi tọa độ thuần nhất tương đối giữa hai R0 ( )q r(0)( )q hệ trục liên tiếp T0( )q n On – Ma trận trạng thái khâu thứ i trong chuỗi được tính bằng tích n 0 1 13 các ma trận biến đổi tọa độ thuần nhất các khâu từ 0 đến i. Ma trận trạng thái của khâu i, (vị trí gốc Oi và hướng của khâu) trong hệ cố định: T0( )q A0( )q 1 1 R0( )q r(0)( )q T0( )q T0 ( )q Ai1( )q A0( )q A1( )q Ai1( )q T0( )q i Oi i i1 i 1 2 i i 0 1 13 T0( )q A0( )q A1( )...q Ai1( )...q An 1( )q n 1 2 i n q [q ,q , ...,q ]T 1 2 n là véc tơ chứa các biến khớp Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Ma trận trạng thái khâu bất kì và khâu cuối -43- Ma trận trạng thái khâu bất kì và khâu cuối -44- Tọa độ điểm N thuộc khâu i trong hệ hệ cơ sở R Tọa độ điểm tác động cuối E thuộc khâu n (EE) trong hệ hệ cơ sở R0 0 R r r u r r u i N E On E R N O N n E i (0) (0) (0) u (0) (0) (0) r r u N r r u u N O N r E O E E i n r(0) R u0 ( )i N r(0) R u0 (n) r O i N R O O n E E i i n O 0 r R n Tọa độ điểm N thuộc khâu i (tính trong Oi Tọa độ thuần nhất của điểm E thuộc 0 r O O hệ cơ sở R ) khâu n (tính trong hệ cơ sở R ) n 0 0 0 O 0 ˆr(0) T0( )q uˆ( )i ˆr(0) T0( )q uˆ(n) N i N E n E uˆ( )i [x( )i , y( )i , z( )i , 1]T tọa độ thuần nhất của N uˆ(n) [x(n), y(n), z(n), 1]T tọa độ thuần nhất của E N N N N trong hệ gắn liền khâu i. E E E E trong hệ gắn liền khâu n. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 12
-45- -46- Ma trận trạng thái khâu cuối Ma trận trạng thái khâu cuối Áp dụng cho robot z Ví dụ q [q ,q , ...,q ]T là véc tơ chứa các biến khớp An 1 n P y 1 2 n chuỗi, (chuỗi động n n Ai hở) u i1 n O e (n) 3 e n en 1 T0 2 i xn é ù z R0 ( )q p(0)( )q 0 T0( )q = ê n e ú n ê 0 1 ú êë úû T0 ée(0)( )q e(0)( )q e(0) ( )q p(0)( )q ù n = ê 1,n 2,n 3,n e ú O y ê 0 0 0 1 ú 0 0 êë úû x0 éR0 ( )q p(0)( )q ù T0 T0 An 1, T0 A A0 1 Ai An 1 T0( )q = ê n e ú n n 1 n n 1 2 i1 n n ê 0 1 ú êë úû é ù s ên(0)( )q s(0)( )q a(0)( )q p(0)( )q ú ˆr(0) T u0ˆ(n) A A0 1 Ai An 1uˆ(n) e a = e e e e n e ê 0 0 0 1 ú P n P 1 2 i1 n P e êë úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -47- -48- Ví dụ: Ví dụ: Ví dụ: Cho sơ đồ rôbốt không gian ba bậc tự 1. Dựng các hệ trục tọa độ gắn liền các khâu: do như hình vẽ. Xác định: R0 - cố định x3 1. tọa độ điểm E và R1 – gắn liền khâu 1 2. ma trận cosin chỉ hướng của các khâu. R2 – gắn liền khâu 2 z2 L3 E z3 x3 R3 – gắn liền khâu 3 q 3 L2 x y y3 2 z2 L3 E 2. Các ma trận biến đổi tọa độ 2 z3 x’ q thuần nhất tương đối q2 1 x 3 Số khâu n = 3, số bậc tự do f = 3. L 2 L1 2 z z y2 y3 0 1 q x’ R R : rot (q ) Chọn véc tơ biến khớp (tọa độ 2 1 0 1 z0 1 x1 L q1 suy rộng) 1 z z R R : tran (L ) rot (-q ) 0, 1 x 1 2 z1 1 y 1 2 0 f = 3, q = [q ,q ,q ]T x1 1 2 3 q1 R R : tran (L ) rot (-q ) x 2 3 x 2 2 y 2 3 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 13
-49- -50- Ví dụ: Ví dụ: Ma trận biến đổi tọa độ thuần nhất tương đối Ma trận biến đổi tọa độ thuần nhất tương đối x3 x3 R R : z2 L 1 2 z2 L R R : rotz(q ) z3 3 E z 3 E tran (L ) roty(-q ) 3 0 1 1 q q 3 z 1 2 3 L2 x2 L2 x y y y y3 2 2 3 é ù 2 q x’ ê cos(-q ) 0 sin(-q ) 0 ú q x’ 2 1 2 2 2 1 ê 0 1 0 0 ú L1 A1 = ê ú L1 z0 z1 2 ê-sin(-q ) 0 cos(-q ) L ú z0 z1 écosq -sinq 0 0ù ê 2 2 1 ú ê 1 1 ú ê 0 0 0 1 ú êsinq cosq 0 0ú x1 êë úû x1 0 0 ê 1 1 ú q1 q1 R R : T = A = ê ú écos(q ) 0 -sin(q ) 0 ù 0 1 1 1 ê 0 0 1 0ú x ê ú x 0 2 2 0 ê 0 0 0 1ú ê 0 1 0 0 ú êë úû = ê ú êsin(q ) 0 cos(q ) L ú ê 2 2 1 ú ê 0 0 0 1 ú êë úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -51- -52- Ví dụ: Ví dụ: Ma trận biến đổi tọa độ thuần nhất tương đối Các ma trận biến đổi tọa độ thuần nhất của khâu i x3 R R : tran (L ) roty(-q ) T0( )q A0( )q R0( ),q r(0) 2 3 x 2 3 1 1 1 O1 z2 L E T0( )q T0( )q A1( )q R0( ),q r(0) z3 3 2 1 2 2 O2 R R : q3 T0( )q T0( )q A2( )q R0( ),q r(0) 2 3 L x2 x3 2 3 2 3 3 O 3 é cos(-q ) 0 sin(-q ) L ù y y3 2 ê 3 3 2 ú q x’ z2 L E ê 0 1 0 0 ú 2 1 z 3 A2 = ê ú L 3 ê-sin(-q ) 0 cos(-q ) 0 ú 1 q3 3 ê ú z z 3 3 0 1 Tọa độ thuần nhất điểm E trong hệ R3 L2 x2 ê 0 0 0 1 ú y2 y3 êë úû x1 uˆ(3) = [L , 0, 0, 1]T é ù q q x’1 cos(q ) 0 -sin(q ) L 1 E 3 2 ê 3 3 2 ú x L ê 0 1 0 0 ú 0 Tọa độ thuần nhất điểm E trong hệ R0 1z z = ê ú 0 1 êsin(q ) 0 cos(q ) 0 ú ê 3 3 ú ˆr(0) T0( )q uˆ(3) x1 ê 0 0 0 1 ú q êë úû E 3 E 1 x0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 14
-53- -54- Ví dụ: Ma trận trạng thái khâu cuối MAPLE: Matran_thuannhat_vidu_proc Ký hiệu các thông số xác định vị trí và hướng của khâu cuối: (0) (0) (0) (0) T r : r [x , y , z ] R E E E E E n ˆr(0) [x(0), y(0), z(0), 1]T E E E E n e 3 r n Chẳng hạn sử dụng 3 góc Euler, hoặc RPY e2 O e(n) n 1 q [, , ]T R 0 x3 z2 L E O z3 3 Véc tơ thể hiện vị trí và hướng khâu cuối 0 q3 L2 x y2 y3 2 x p [r(0)T ,qT ]T q x’ E E E 2 1 Lz1 z [x(0), y(0), z(0), , , ]T 0 1 E E E x1 q1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanicsx0 Department of Applied Mechanics -55- -56- Ma trận trạng thái khâu cuối Ma trận trạng thái khâu cuối biểu diễn theo vector • ĐỘNG HỌC THUẬN x p [r(0)T ,qT ]T R Cho véc tơ biến khớp q Ma trận E E E n trạng thái khâu cuối [x(0), y(0), z(0), , , ]T E E E en q T0( )q T0( )q 3 r E n en O (n) 2 n e 1 • ĐỘNG HỌC NGƯỢC R 0 Cho ma trận trạng thái khâu cuối R( )q r O Tìm véc tơ biến khớp q T ( )x T ( )p E 0 E E 0 1 T ( )x T ( )p q E E Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 15
-57- -58- T ( )x T ( )p q • Nếu chuyển động của khâu cuối cho dạng hàm của thời gian, E E éa ( )q a ( )q a ( )q x( )q ù éc (a b g, , ) c (a b g, , ) c (a b g, , ) x ù ta viết lại các phương trình thành: ê 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 ( ),t y ( ),t z ( ),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 ú ê ú ê ú E E E ê 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 f = x( )q - x ( )t = 0, f = a ( )q - c ( )t = 0 phương trình liên quan cosin chỉ hướng. 1 E 4 11 11 f = y( )q - y ( )t = 0, f = a ( )q - c ( )t = 0 f = x( )q - x = 0, f = a ( )q - c (a b g, , ) = 0 2 E 5 22 22 1 E 4 11 11 f = y( )q - z ( )t = 0, f = a ( )q - c ( )t = 0 f = y( )q - y = 0, f = a ( )q - c (a b g, , ) = 0 3 E 6 33 33 2 E 5 22 22 f = y( )q - z = 0, f = a ( )q - c (a b g, , ) = 0 3 E 6 33 33 f (x ,x ,...,x ) = 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? j 1 2 6 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -59- -60- Phương pháp lặp Newton-Raphson Phương pháp lặp Newton-Raphson 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, f (x ,x ,...,x ) = 0 1 1 2 n ¶f f x( + dx) = f x( ) + (x )dx +O(dx)2 f (x ,x ,...,x ) = 0 0 0 ¶x 0 2 1 2 n f x( ) = 0 f, Î n , x Î n = f x( ) + J x( )dx +O(dx)2 ... 0 0 Ma trận jacobi được xác định tại điểm x0 f (x ,x ,...,x ) = 0 n 1 2 n é ¶f ¶f ¶f ù ê 1 1 .. 1 ú ê¶x ¶x ¶x ú ê 1 2 n ú ê ¶f ¶f ¶f ú 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 2 .. 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( ) = (x ) = ê¶x ¶x ¶x ú(x ) 0 ¶x 0 ê 1 2 n ú 0 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 ¶f ¶f ú ê n n .. n ú ê¶x ¶x ¶x ú ë 1 2 n û Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 16
-61- -62- Phương pháp lặp Newton-Raphson Phương pháp lặp Newton-Raphson 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. Từ đó nhận được một hệ phương trình đại số tuyến tính đối với x. 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 f x( k ) f x( k ) £ e Bước 2. Tính . Nếu thì dừng, nếu không thì tiếp tục từ 3. f x( + dx) = f x( ) + J x( )dx = 0 Bước 3. Tính ma trận Jacobi tại xk , tức là tính ma trận J x( k ) 0 0 0 Bước 4. Giải hệ J x( k )dx = -f x( k ) dx = -J-1(xk ) (f xk ) J x( )dx = -f x( ) 0 0 Bước 5. Lấy xk+1 = xk + dx dx = -J-1(x )⋅ f x( ) 0 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 = (x + dx) 0 0 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( ) £ e 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -63- -64- Phương pháp lặp Newton-Raphson Sơ đồ khối thuật giải của phương Bắt đầu pháp lặp Newton-Raphson giải hệ phương trình đại số phi tuyến n Chọn , M, k = 0, phương trình, n ẩn Chọn xấp xỉ ban đầu x(0) Tính f(x(k)) f x( ) = 0 f, Î n , x Î n Yes || f(x(k)) || No f (x ,x ,...,x ) = 0 Tính:Giải hệ: JJ((xx(k)(k)))x = - f(x(k)) Phương pháp Denavit-Hartenberg 1 1 2 n Lấy: x(k+1) := x(k)+ x f (x ,x ,...,x ) = 0 Tăng k, k:= k + 1 2 1 2 n ... Yes k M f (x ,x ,...,x ) = 0 No Lấy nghiệm n 1 2 n không hội tụ với x(*) := x(k) x(0) và M lần lặp Kết thúc Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 17
-65- -66- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg Xét hệ robot có cấu trúc chuỗi (đơn) hở chỉ gồm các khớp quay • Tham số thứ nhất , được gọi là góc khớp, góc quay quanh i và khớp trượt. Quan hệ vị trí giữa hai khâu liên tiếp có thể được zi-1 để xi-1 về song song với xi. xác định bởi hai tham số: • Tham số thứ hai di là khoảng cách từ x‘i đến xi. zi-1 • Tham số thứ nhất , được i zi-1 i i gọi là góc khớp, góc quay khâu i khớp i quanh z để x về song xi khâu i i-1 i-1 song với xi. O d khớp i • Tham số thứ hai d là i-1 i x’ xi i khâu i-1 i khoảng cách từ x‘i đến xi. xi-1 O di i i-1 khâu i-1 x’i Khớp quay: - biến, d - hằng, x i i i-1 i Khớp tịnh tiến: - hằng, d - biến. i i Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -67- -68- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg Denavit-Hartenberg (năm 1955) đã đề xuất quy tắc dựng các hệ trục tọa Biểu diễn các tham số Denavit-Hartenberg độ gắn liền các khâu như sau: z’ • Trục z(i-1) là trục của khớp (i) nối khâu (i-1) với khâu (i); zi-1 i-1 z i i • Trục x(i-1) là đường vuông góc chung của hai trục z(i-2) và z(i-1), hướng từ z(i-2) sang z(i-1), gốc O(i-1) là giao điểm z(i-1) với x(i-1); z i-2 i-1 • Trục y(i-1) được chọn sao cho hệ trục tọa độ là thuận. khớp i khâu i i khâu i+1 z z’i-1 khâu i-1 i-1 zi i z khớp i-1 khớp i+1 i-2 i-1 a khớp i khâu i i khâu i+1 i khâu i-1 xi O’ Oi i khớp i+1 i khớp i-1 d ai i O xi x’i O’ i i i d Oi-1 i x i x’ Hệ trục tọa độ (Oxyz) i-1 i i O gắn liền khâu i. i x i-1 i-1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 18
-69- -70- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg Biểu diễn các tham số Denavit-Hartenberg Theo quy tắc này, trong các trường hợp sau đây việc xây dựng hệ tọa z z’ i-1 i-1 z i i Khớp tịnh tiến : độ khâu là không duy nhất. zi-2 d = d (t) • Hệ tọa độ 0 mới chỉ có trục z(0) xác định, còn lại gốc O(0) và i-1 i i khớp khâu i i khâu i+1 Khớp quay: hướng trục x(0) có thể chọn tùy ý. khâu i-1 i = (t) i i • Hệ tọa độ n, do không có khớp (n+1), trục z(n) có thể chọn tùy ý và khớp i-1 khớp i+1 ai 1 Khớp quay trục x(n) phải vuông góc với z(n-1). Thông thường nếu khớp n là xi O’ Oi i i 0 Khớp tịnh tiến khớp quay, nên chọn z(n) song song với z(n-1). i di • Nếu hai trục khớp liên tiếp song song, thì đường vuông góc chung x’i O là không duy nhất. i x i-1 i-1 • Nếu hai trục khớp liên tiếp giao nhau, thì hướng của trục x là tùy ý. Bảng thông số DH Biến khớp hay tọa độ suy rộng q i d a i • Nếu khớp i là tịnh tiến, chiều trục z(i) là tùy ý. i i i i 1 q (1 )d , i i i i i 2 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -71- -72- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg z z’i-1 Chuyển hệ (i-1) về trùng với hệ (i) z z’i-1 i-1 z i-1 zi i i thông qua 4 phép dịch chuyển sau: i i i+1 z khâu i+1 i-2 i-1 1. Quay hệ (i-1) quanh trục z(i-1) góc khớp i khâu i i khâu i+1 khâu i (i) để trục x(i-1) trở thành x‘(i) khớp i khâu i-1 song song với x(i) khớp i+1 2. Tịnh tiến dọc trục z(i-1) đoạn d(i) để khớp i-1 khớp i+1 khâu i-1 ai x‘(i) về trùng với x(i) x ai O’ O i i x 3. Tịnh tiến dọc trục x(i) đoạn a(i) để i i O i d O’ i gốc O‘(i) về trùng với O(i) i i i x’i d 4. Quay quanh trục x(i) góc (i) để i z’(i-1) về trùng với trục z(i); O i x x’i i-1 i-1 Oi-1 i x Ma trận biến đổi tọa độ thuần nhất tương ứng i-1 Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) i i z i z i x i x i i i z i z i x i x i Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 19
-73- -74- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg 1. Quay hệ (i-1) quanh trục z(i-1) góc (i) để trục x(i-1) trở thành x‘(i) Các bước áp dụng phương pháp D-H đối với chuỗi động mở n khâu, song song với x(i) n bậc tự do gồm các khớp quay (1dof) và tịnh tiến (1dof) 2. Tịnh tiến dọc trục z(i-1) đoạn d(i) để x‘(i) về trùng với x(i) 1. Đánh số thứ tự các khâu từ 0 đến n: khâu 0 là giá cố định, khâu 3. Tịnh tiến dọc trục x(i) đoạn a(i) để gốc O‘(i) về trùng với O(i) cuối (bàn kẹp) có số thứ tự là n. 4. Quay quanh trục x(i) góc (i) để z’(i-1) về trùng với trục z(i); 2. Số thứ tự các khớp từ 1 đến n tính từ khớp nối với giá (khâu 0). Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) 3. Xây dựng các hệ trục tọa độ gắn liền khâu theo quy tắc D-H i i z i z i x i x i 4. Lập bảng thông số D-H écq -sq 0 0ù é1 0 0 0 ù é1 0 0 a ù é1 0 0 0ù 5. Viết ra các ma trận biến đối thuần nhất tương đối giữa hai khâu ê i i ú ê ú ê i ú ê ú êsq cq 0 0ú ê0 1 0 0 ú ê0 1 0 0 ú ê0 ca -sa 0ú liên tiếp = ê i i ú ê ú ê ú ê i i ú ê 0 0 1 0ú ê0 0 1 d ú ê0 0 1 0 ú ê0 sa ca 0ú 6. Tính toán ma trận thuần nhất của khâu cuối ê ú ê i ú ê ú ê i i ú ê 0 0 0 1ú ê0 0 0 1 ú ê0 0 0 1 ú ê0 0 0 1ú 7. Viết ra kết quả: vị trí và hướng của khâu cuối êë ú êû ë ú êû ë ú êû ë ûú écq -sqca sq sa a cq ù T0 A A0 1 Ai1 An 1 ê i i i i i i i ú êsq cqca -cq sa a sq ú n 1 2 i n = ê i i i i i i i ú ê 0 sa ca d ú ê i i i ú ˆr(0) T u0ˆ(n) A A0 1 Ai1 An 1uˆ(n) ê 0 0 0 1 ú P n P 1 2 i n P êë úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -75- -76- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg Ví dụ. Robot ba khâu phẳng ba khớp quay Ví dụ. Robot ba khâu phẳng ba khớp quay 1. Đánh số các khâu và khớp 1-2-3 x3 2. Dựng các hệ trục tọa độ khâu theo quy tắc khâu d a i i i i O DH q q 0 l 0 y 3 3 1 A0( )q 3 x l 1 1 1 q 2 3. Lập bảng DH 3 l l 3 4. Từ bảng DH, tính các ma trận biến đổi tọa 2 q2 0 2 0 A1( )q 3 2 y2 độ thuần nhất 3 q 0 l3 0 A2( )q O2 l 3 3 y y0 2 1 l q2 y 2 Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) 0 q 2 x i i z i z i x i x i l 1 1 Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) q i i z i z i x i x i l1 O khâu d a 1 1 i i i i O q1 0 x écq -sqca sq sa a cq ù O 1 0 ê i i i i i i i ú 0 x êsq cqca -cq sa a sq ú 0 = ê i i i i i i i ú 2 ê 0 sa ca d ú ê i i i ú 3 ê 0 0 0 1 ú êë úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 20
-77- -78- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg x3 O cq sq 0 l cq l cq l cq c s 0 l c 3 123 123 1 1 2 12 3 123 i i i i y3 x sq cq 0 l sq l sq l sq s c 0 l s q 2 T 0 ( )q 123 123 1 1 2 12 3 123 x3 Ai 1(q ) i i i i , i 1, 2, 3 l 3 3 0 0 1 0 0 0 1 0 3 O i i y 0 0 0 1 3 2 y3 x 0 0 0 1 O q3 2 2 l y1 3 y l2 y 0 q 2 2 x l cq l cq l cq O2 1 1 1 2 12 3 123 y l O r(0)( )q l sq l sq l sq 1 l 1 q 1 O 3 1 1 2 12 3 123 y0 2 q O 1 0 2 x 0 1 x0 l O 1 1 cq sq 0 l cq l cq l cq cq sq 0 q1 123 123 1 1 2 12 3 123 123 123 O sq cq 0 l sq l sq l sq q q q , R0( )q sq cq 0 0 x T 0 ( )q 123 123 1 1 2 12 3 123 3 123 123 0 3 0 0 1 0 12 1 2 q q q q 0 0 1 0 0 0 1 123 1 2 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics -79- -80- Phương pháp Denavit-Hartenberg Phương pháp Denavit-Hartenberg Ví dụ. Robot ba khâu phẳng ba khâu (RTR) y x3 3 1. Đánh số các khâu và khớp 1-2-3, z O 2. Dựng các hệ trục tọa độ khâu theo q3 3 3 y0 x q quy tắc DH, 2 3 q z1 2 q 3. Lập bảng DH, 2 4. Từ bảng DH, tính các ma trận biến z y z O2 2 0 x 2 đổi tọa độ thuần nhất. 1 z 1 Ai-1(q ) = Rot (q )Tran (d Tran) (a )Rot (a ) O i i z i z i x i x i z q1 d a y 1 q 0 khâu i i i i 1 z 1 d a O0 0O khâu i i i i x0 1 0 x 0 1 2 2 3 3 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!