Mục lục I Giới thiệu 3 I 1 Ứng dụng 3 I 2 Kết cấu 3 I 2 1 Đặc điểm chính 3 I 2 2 Thông số kỹ thuật 3 I 2 3 Thông số cơ khí 3 II Tính toán động học thuận vị trí 4 III Tính ma trận Jacoby 8 IV Động họ. I. Giới thiệu I.1. Ứng dụng Robot MH3BM được thiết kế để ứng dụng trong những công việc mà ở đó tải nhỏ và nhẹ ví dụ như hệ thống dọn dẹp tự động, dây chuyền sản xuất thuốc hay trong các quá trình thử nghiệm lâm sàng và pha chế dược phẩm. I.2. Kết cấu I.2.1. Đặc điểm chính Robot có 6 bậc tự do, có độ linh hoạt cao Kích thước nhỏ gọn, sử dụng điều hướng bằng điện và khí nén Có thể gắn trên sàn, lên tường hoặc trần Hỗ trợ các tiêu chuẩn MotoPlus và MotomanPlus Có cấu trúc mở dễ dàng lập trình. I.2.2. Thông số kỹ thuật Lớp phủ ngoài đặc biệt, bề mặt được xử lý, ốc vít làm bằng thép không gỉ. Tiêu chuẩn bảo vệ IP65 cho phần thân và IP67 cho phần khớp Hệ thống kết nối cáp và thủy lực ở phía dưới bề mặt cố định cũng được bảo vệ Chiều rộng tối thiểu mặt phẳng để cố định robot là 200mm Tải trọng quy định là 3 kg, tầm với theo dọc chiều robot là 804mm, theo ngang chiều robot là 532 mm Có khả năng kết nối nhiều robot khác cùng lúc để tăng hiệu quả làm việc
Mục lục I Giới thiệu I.1 Ứng dụng I.2 Kết cấu I.2.1 Đặc điểm .3 I.2.2 Thông số kỹ thuật I.2.3 Thơng số khí II Tính tốn động học thuận vị trí III Tính ma trận Jacoby IV Động học đảo vị trí V Thiết kế quỹ đạo chuyển động bậc cho biến khớp 10 VI Mơ hình động lực học cho đối tượng Simmechanics .11 VI.1 Vẽ chi tiết robot SolidWorks 2010 11 VI.2 Xuất sang file Matlab 14 Nhóm – Robot MH3BM Trang | I Giới thiệu I.1 Ứng dụng Robot MH3BM thiết kế để ứng dụng cơng việc mà tải nhỏ nhẹ ví dụ hệ thống dọn dẹp tự động, dây chuyền sản xuất thuốc hay trình thử nghiệm lâm sàng pha chế dược phẩm I.2 Kết cấu I.2.1 Đặc điểm Robot có bậc tự do, có độ linh hoạt cao Kích thước nhỏ gọn, sử dụng điều hướng điện khí nén Có thể gắn sàn, lên tường trần Hỗ trợ tiêu chuẩn MotoPlus MotomanPlus Có cấu trúc mở dễ dàng lập trình I.2.2 Thơng số kỹ thuật Lớp phủ đặc biệt, bề mặt xử lý, ốc vít làm thép khơng gỉ Tiêu chuẩn bảo vệ IP65 cho phần thân IP67 cho phần khớp Hệ thống kết nối cáp thủy lực phía bề mặt cố định bảo vệ Chiều rộng tối thiểu mặt phẳng để cố định robot 200mm Tải trọng quy định kg, tầm với theo dọc chiều robot 804mm, theo ngang chiều robot 532 mm Có khả kết nối nhiều robot khác lúc để tăng hiệu làm việc I.2.3 Thơng số khí Hình chiếu mặt bên robot Nhóm – Robot MH3BM Trang | Hình chiếu mặt robot MH3BM Hình chiếu mặt trước robot MH3BM II Tính tốn động học thuận vị trí Bảng D-H: Nhóm – Robot MH3BM Trang | i αi 90 o 90 θi di θ1 a1 θ2 a2 0 90 ° +θ3 o 9 0o 90 θ4 θ5 θ6 a4 o a6 Áp dụng công thức: T i−1 i =A i =Rot ( z ,θ i ) Trans ( z , d i ) Trans ( x i , a i ) Rot ( x , ) → T i−1 i Code Matlab: function output = TinhA(al,a,t,d) % Ma tran xac tinh huong va vi tri cua mot khau theo bang DH X = [cos(t), -cos(al)*sin(t), sin(al)*sin(t), a*cos(t); sin(t), cos(al)*cos(t), -sin(al)*cos(t), a*sin(t); , sin(al), cos(al), d; , , , 1]; output = X; syms t1 t2 t3 t4 t5 t6 a1 a2 a4 a6 % sym(pi/2) de dung so pi A1 = TinhA(sym(pi/2), 0, t1, a1); A2 = TinhA(0, a2, t2, 0); A3 = TinhA(sym(pi/2), 0, t3 + sym(pi/2), 0); A4 = TinhA(sym(-pi/2), 0, t4, a4); A5 = TinhA(sym(pi/2), 0, t5, 0); A6 = TinhA(0, 0, t6, a6); Nhóm – Robot MH3BM Trang | %Tinh dong hoc thuan vi tri T06 = simplify(expand(A1*A2*A3*A4*A5*A6)); T16 = simplify(expand(A2*A3*A4*A5*A6)); T26 = simplify(expand(A3*A4*A5*A6)); T36 = simplify(expand(A4*A5*A6)); T46 = simplify(expand(A5*A6)); T56 = simplify(expand(A6)); Nhóm – Robot MH3BM Trang | T06 = Nhóm – Robot MH3BM Trang | III Tính ma trận Jacoby Để tính ma trận Jacoby cho robot bậc tự ta sử dụng thuật tốn tính toán ma trận J H Xét ma trận : ∂H px ∂ q1 H ∂ X J nH = = ∂Q H ∂ θz ∂ q1 ∂H p x ∂ qn H ∂ θz ∂ qn [ ] H Do khớp khớp quay nên ta có ma trận J có cột: ∂H X =¿ ¿ ∂θ i Xác định ma trận J= R0n H J R 0n [ ] Chương trình matlab : function output = Jh(T1, T2, T3, T4, T5, T6) %Tinh ma tran Jh Y = [jh(T1), jh(T2), jh(T3), jh(T4), jh(T5), jh(T6)]; output = Y; end function result = jh(T) nx ox ax px = = = = T(1,1); T(1,2); T(1,3); T(1,4); ny oy ay py = = = = T(2,1); nz = T(3,1); T(2,2); oz = T(3,2); T(2,3); az = T(3,3); T(2,4); X = [ny*px - nx*py; oy*px - ox*py; ay*px - ax*py; nz; oz; az]; result = X; end %Tinh ma tran Jacobi Jh JH = simplify(expand(Jh(T06, T16, T26, T36, T46, T56))); % Dung ham expand() de khai trien ma tran R06 = T06(1:3, 1:3); J_1 = [ R06 [0] [0] [0] [0] [0] [0] R06 ] ; Nhóm – Robot MH3BM Trang | J = J_1*JH ; Nhóm – Robot MH3BM Trang | IV Động học đảo vị trí Giả sử biết trước ma trận : n x o x ax p x n oy ay py T 06= A1 A A A A A 6= y nz oz az pz 0 [ ] ⇒ A−1 T 6= A A3 A A5 A (¿) R 1( 3,4) L1(3,4) px s 1− p yc ¿ Ta có : = a6 = R 1(3,3) L1(3,3) az s 1−ay c Ta giải nghiệm q1 : q1 = atan2(py-a6.ay;px-a6.az) q1 = atan2(py-a6.ay;px-a6.az) + π Sau tìm q1 ta biết ma trận L1 f x gx hx k x f g h k L1 = y y y y f z gz h z k z 0 L2 = L1 A 6−1 ; R2 =A2.A3.A4.A5 Dễ thấy L2(1,4) =kx – a6.gx L2(2,4) = ky – a6.gy R2(1,4) = a4.c23 + a2.c2 R2(2,4) = a4.s23 + a2.s2 (a c 23−kx +a gx )2 + (a s 23−ky +a gy)2 = a 22 A.c23 + B.s23 = C Do : q23 = atan2(B,A) + atan2 (± √ A +B 2−C ,C) q2 = atan2 (ky – a6.gy – a4.c23 , kx – a6.gx – a4.c23) f 1x g 1x h x k x f y g y h 1y k1 y T36 = f z g z h1 z k z 0 Đặt L3 = A 4−1 A 3−1 A 2−1 A 1−1T06 ; R3 = T46 Ta thấy : h1y.c4-h1z.s4 = nên q4 = atan2 (h1y , h1z) q4 = (h1y, h1z) + π c5 = h1z nên q5 = ± arccos(h1z) q6 = atan2(f1y.c4 – f1x.s4 , g1y.c4 -g1x.4) Nhóm – Robot MH3BM Trang | 10 Nhóm – Robot MH3BM Trang | 11 V Thiết kế quỹ đạo chuyển động bậc cho biến khớp - Công thức tổng quát trường hợp quỹ đạo bậc là: q=a t 3+ a2 t +a t+ a0 - Ta xác định hệ số a , a , a2 , a3 cho thỏa mãn điều kiện giá trị góc, vận tốc đầu cuối quỹ đạo - Tương đương với việc giải hệ phương trình: a3 θ3 +a2 θ2 +a θ+ a0=θ a3 t 3f +a t 2f + a1 t f +a0 =θf a3 θ 2+ a2 θ+ a1=θ '0 { ' a t f + 2a t f + a1=θ f - Từ phương trình (1) (3), dễ dàng suy ra: a 0=θ0 , a1=θ '0 Từ thay ngược trở lại phương trình (2) (4), ta có: a3 t 3f + a2 t 2f =θ f −θ '0 t f −θ0 a t 2f + a2 t f =θ'f −θ'0 { - Đây hệ phương trình bậc ẩn Giải hệ, ta có: θ' θ' a 2= ( θ f −θ )− − f tf tf tf −2 a 3= ( θ f −θ ) + ( θ'f +θ '0) tf tf - Từ đó, ta có kết luận: Xây dựng quỹ đạo cho biến khớp q i hàm: q i ( t )= [ −2 ' ' ' ' ' θ −θi ) + (θ if + θi 0) t + ( θ if −θi ) − (θ if + 2θi ) t +θi t+θ i ( if t tf tf tf f ] [ Nhóm – Robot MH3BM ] Trang | 12 VI Mơ hình động lực học cho đối tượng Simmechanics VI.1 Vẽ chi tiết robot SolidWorks 2010 Nhóm – Robot MH3BM Trang | 13 Nhóm – Robot MH3BM Trang | 14 Nhóm – Robot MH3BM Trang | 15 VI.2 Xuất sang file Matlab Góc quay khớp thiết lập Slider Gains Mơ Matlab: Nhóm – Robot MH3BM Trang | 16