BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI CAO XUÂN HOÀNG LẬP TRÌNH QUỸ ĐẠO VÀ ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA ROBOT CÔNG NGHIỆP CHUYÊN NGÀNH: CƠ ĐIỆN TỬ LUẬN VĂN THẠC SỸ KHOA HỌC NGÀNH: CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC: GS.TSKH NGUYỄN VĂN KHANG Hà nội – Năm 2011 MỤC LỤC LỜI CAM ĐOAN DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT .4 DANH MỤC CÁC BẢNG DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ .7 MỞ ĐẦU .9 Chương - THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA RÔBỐT CÔNG NGHIỆP BẰNG DẠNG THỨC LAGRANGE II 11 1.1 Thiết lập phương trình vi phân Lagrange loại II cho hệ nhiều vật 11 1.2 Các thí dụ áp dụng .15 Chương - LẬP TRÌNH QUỸ ĐẠO 33 2.1 Mở đầu .33 2.1.1 Một vài thuật ngữ sử dụng lập trình quỹ đạo .33 2.1.2 Nhiệm vụ lập trình quỹ đạo rôbốt 34 2.1.3 Các bước toán lập trình quỹ đạo 35 2.1.4 Một vài tiêu chuẩn điều kiện phụ 36 2.2 Lập trình quỹ đạo động học .37 2.2.1 Mô tả đường cong không gian .38 2.2.2 Prôphin vận tốc quỹ đạo 40 2.2.3 Việc xây dựng quỹ đạo tổng quát đa thức nội suy 44 2.3 Lập trình quỹ đạo tối ưu động lực học ngược 47 2.3.1 Tham số hóa phương trình vi phân chuyển động 48 2.3.2 Các ý lập trình quỹ đạo 50 Chương - ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA RÔBỐT 77 3.1 Thiết lập toán .77 3.1.1 Điều khiển chuyển động rôbốt không gian khớp .78 3.1.2 Điều khiển chuyển động rôbốt không gian thao tác 78 3.2 Phương pháp điều khiển mômen .79 3.3 Phương pháp điều khiển trượt 81 3.3.1 Tổng quan 81 3.3.2 Xây dựng luật điều khiển .82 3.3.3 Điều khiển trượt rôbốt Scara 87 KẾT LUẬN .92 TÀI LIỆU THAM KHẢO 94 PHỤ LỤC 97 LỜI CAM ĐOAN Tôi xin cam đoan công trình khoa học Các kết nghiên cứu luận văn trung thực có nguồn gốc cụ thể, rõ ràng Các kết luận văn chưa công bố công trình khoa học Hà Nội, tháng 05 năm 2011 Người cam đoan Cao Xuân Hoàng DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT STT Ký hiệu Diễn giải Ci cos(qi) Cij cos(qi + qj) Si sin(qi) Sij sin(qi + qj) ph h Ci Ma trận Craig khâu thứ i so với hệ R0 Ki Ma trận Craig khâu thứ i so với hệ Ri-1 mi Khối lượng khâu thứ i 10 M (q ) Ma trận khối lượng suy rộng thực tế 11 ) M (q ) Ma trận khối lượng suy rộng điều khiển 28 % (q ) M Sai số ma trận khối lượng suy rộng 12 J Ti Ma trận Jacobi tịnh tiến khâu i 13 C ( q, q& ) Ma trận hệ số quán tính ly tâm Côriôlis mô hình thực 14 ˆ ( q, q& ) C Ma trận hệ số quán tính ly tâm Côriôlis mô hình điều khiển 15 % ( q, q& ) C Sai số C ( q, q& ) Cˆ ( q, q& ) 16 qi Tọa độ suy rộng thứ i (biến khớp i) 17 q Véc tơ tọa độ suy rộng 18 q&i Vận tốc suy rộng thứ i 19 q& Véc tơ vận tốc suy rộng 20 q&&i Gia tốc suy rộng thứ i 21 && q Véc tơ gia tốc suy rộng 22 ei Sai số biến khớp thứ i 23 e Véc tơ sai số véc tơ tọa độ suy rộng 24 e&i Sai số vận tốc suy rộng thứ i r,r r Tọa độ vật lý điểm Tọa độ điểm STT Ký hiệu Diễn giải 25 e& Véc tơ sai số vận tốc suy rộng 26 e&&i Sai số gia tốc suy rộng thứ i 27 &e& Véc tơ sai số gia tốc suy rộng 28 si Sai số suy rộng véc tơ tọa độ suy rộng thứ i 29 s Véc tơ sai số suy rộng tọa độ suy rộng 30 s& Véc tơ sai số suy rộng vận tốc suy rộng 31 &s& Véc tơ sai số suy rộng gia tốc suy rộng 32 n Số tọa độ khâu thao tác 33 m Số bậc tự hệ 34 T Động tay máy 35 Π Thế tay máy 36 x Véc tơ tọa độ khâu thao tác 37 x& Véc tơ vận tốc khâu thao tác 38 &x& Véc tơ gia tốc khâu thao tác 39 x = f (q) Phương trình động học rôbôt 40 τi Mômen lực suy rộng tác dụng lên khớp i 41 τ Véc tơ mômen điều khiển 42 kPi Hệ số điều khiển quán tính cho động khớp i 43 kDi Hệ số điều khiển vi phân cho động khớp i 44 kIi Hệ số điều khiển tích phân cho động khớp i 45 KSi Hệ số điều khiển trượt cho động khớp i 46 KS Véc tơ hệ số điều khiển trượt 47 ϕ,ψ , θ Ba góc quay theo trục khâu 48 T Ma trận biến đổi DANH MỤC CÁC BẢNG Bảng 2.1 Các tham số động học Craig rôbốt Scara 67 Bảng 2.2 Các tham số động lực rôbốt Scara 67 Bảng 3.1 Các tham số Craig rôbốt Scara 88 Bảng 3.2 Các tham số động lực rôbốt Scara 88 Bảng 3.3 Thông số rôbốt Scara 88 DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ Hình 1.1 Vị trí vật rắn không gian 11 Hình 1.2 Vị trí vật rắn không gian 13 Hình 1.3 Mô hình lắc kép 16 Hình 1.4 Cơ cấu phẳng 19 Hình 1.5 Mô hình rôbốt Scara bậc tự .24 Hình 1.6 Hình chiếu mô hình rôbốt Scara bậc tự 25 Hình 1.7 Mô hình rôbốt Scara bậc tự .28 Hình 1.8 Hình chiếu rôbốt Scara bậc tự 29 Hình 2.1 Mô tả đường cong không gian 38 Hình 2.2 Mô tả tham số đường tròn không gian 40 Hình 2.3 Các prôphin vận tốc quỹ đạo điển hình 41 Hình 2.4 Prôphin vận tốc dạng hình thang đối xứng .43 Hình 2.5 Xấp xỉ đa thức 44 Hình 2.6 Nối quỹ đạo thẳng đa thức bậc ba .47 Hình 2.7 Miền cho phép (với n=3 f Ri = ) 50 Hình 2.8 Tay máy cực 52 Hình 2.9 Miền cho phép với s = 56 R Hình 2.10 Miền cho phép với s = 1.5 .56 R Hình 2.11 Vận tốc quỹ đạo prôphin vận tốc cho phép 57 Hình 2.12 Tính toán thời gian làm việc .58 Hình 2.13 Mô hình cấu tay máy hai khâu phẳng 59 Hình 2.14 Miền giới hạn cho phép với s =1 65 Hình 2.15 Miền giới hạn cho phép với s =1.25 65 Hình 2.16 Rôbốt Scara có khâu thao tác quay 66 Hình 2.17 Miền cho phép với s = 0.5 75 Hình 2.18 Miền cho phép với s =0.5 75 Hình 2.19 Miền cho phép với s = 1.0 76 Hình 2.20 Miền cho phép với s =1.0 76 Hình 3.1 Ví dụ quỹ đạo chuyển động rôbôt 77 Hình 3.2 Sơ đồ phương pháp điều khiển không gian khớp 78 Hình 3.3 Sơ đồ phương pháp điều khiển không gian thao tác 78 Hình 3.4 Hàm sgn 83 Hình 3.5 Mô hình toán mô điều khiển rôbôt 85 Hình 3.6 Hàm sat .86 Hình 3.7 Hàm artan có chỉnh tỉ lệ 86 Hình 3.8 Rôbôt Scara .87 Hình 3.9 Sơ đồ khối hệ điều khiển trượt rôbôt Scara 90 Hình 3.10 Mômen điều khiển khớp .90 Hình 3.11 Quỹ đạo điểm đầu khâu thao tác .91 MỞ ĐẦU Trong nghiệp công nghiệp hóa đại hóa đất nước ta, lĩnh vực điều khiển tự động hóa có vai trò quan trọng Các rôbốt công nghiệp có vị trí đặc biệt quan trọng trình tự động hóa Mục tiêu ứng dụng rôbốt công nghiệp sản xuất nhằm nâng cao suất dây chuyền công nghiệp, nâng cao chất lượng sản phẩm, cải thiện điều kiện lao động Ở nước ta từ năm 90 kỷ 20 đến nay, nhiều sở sản xuất sử dụng nhiều loại rôbốt công nghiệp phục vụ cho trình sản xuất Ở nhiều trường đại học cao đẳng kỹ thuật đưa môn học “Rôbốt công nghiệp” vào giảng dạy cho sinh viên ngành Cơ điện tử, Công nghệ chế tạo máy, Ô tô xe chuyên dụng,v v [2,3,4,5] Việc nghiên cứu vấn đề động học, động lực học điều khiển rôbốt công nghiệp quan tâm trường đại học Bách khoa Hà Nội, Học viện kỹ thuật quân sự, đại học Bách khoa TP Hồ Chí Minh, đại học Bách khoa Đà Nẵng, Viện khoa học Việt Nam [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 24, 25, 26] Trong luận văn này, sở lý thuyết động lực học hệ nhiều vật [1] tác giả trình bày số vấn đề lập trình quỹ đạo điều khiển chuyển động rôbốt công nghiệp Công cụ tin học dùng để tính toán phần mềm Maple 9.5 phần mềm Matlab 6.5 Trong lĩnh vực điện tử, người ta thường quan tâm nhiều đến toán ngược: cho biết chuyển động khâu thao tác, tìm chuyển động tọa độ trạng thái lực phát động Trong luận văn theo hướng trên, nghĩa thiết lập phương trình chuyển động rôbốt công nghiệp dạng thức Lagrange II lập trình quỹ đạo chuyển động Luận văn gồm có chương sau: Chương 1: Dựa lý thuyết động lực học hệ nhiều vật, xây dựng giải toán “Thiết lập phương trình Lagrange II cho hệ nhiều vật”, sau áp dụng thiết lập phương trình vi phân cho số dạng chuyển động cụ thể Chương 2: Lập trình quỹ đạo chuyển động rôbốt công nghiệp, cho trước cụ thể điểm tác động cuối rôbốt, cách giải toán động học ngược động lực học ngược, ta tìm qũy đạo chuyển động rôbốt tìm miền giới hạn đại lượng dẫn động rôbốt Các tham số Craig rôbôt bảng 3.3 Khâu α i −1 −1 di θi qi 0 θ1 q1 = θ1 l11 l12 θ2 q2 = θ 180 l2 d3 q3 = d3 0 θ4 q4 = θ Bảng 3.1 Các tham số Craig rôbốt Scara Các tham số động lực rôbôt cho bảng 3.4 Khâu Ma trận mômen quán tính I Khối Vị trí trọng tâm xC yC zC lượng I xx I yy I zz I xy I yz I zx lC1 l12 m1 I1 x I1y I1 z 0 lC 0 m2 I2x I2 y I2z 0 0 lC m3 I3x I3 y I3z 0 0 −lC m4 I4x I4 y I4z 0 Bảng 3.2 Các tham số động lực rôbốt Scara Giải toán thiết lập phương trình vi phân chuyển động giống thí dụ 2.7 Chọn tham số rôbôt Scara bảng 3.5 Khâu m[kg] I x [kg.m ] I y [kg.m ] I z [kg.m ] lC [m] Thông số khác 1.25 1.25 0.2 0.35 l11 =0.5;l12 =0.5 0.36 0.12 0.3 l2 =0.4 0.08 0.08 0.01 0.2 - 0.01 0.01 0.01 0.1 - Bảng 3.3 Thông số rôbốt Scara 88 Suy ma trận tham số ⎡ 2.4C2 + 3.54 1.2C2 + 0.98 ⎢1.2C + 0.98 0.96 M=⎢ ⎢ 0 ⎢ −0.01 ⎣ −0.01 −1.2 S2 q&2 ⎡ −2.4 S2 q&2 ⎢0.6 S (2q& − q& ) 0.6 S q& 2 C=⎢ ⎢ 0 ⎢ 0 ⎣ −0.01⎤ −0.01⎥ ⎥ ⎥ ⎥ −0.01⎦ 0⎤ 0⎥ ⎥ 0⎥ ⎥ 0⎦ (3.36) Phương trình quỹ đạo theo biến khớp theo thời gian cho phương trình: ⎡ xE ⎤ ⎡ 0.5C1 + 0.5C12 ⎤ ⎢ y ⎥ ⎢ 0.5S + 0.5S ⎥ 12 ⎥ x(q) = f (q) = ⎢ E ⎥ = ⎢ ⎢ z E ⎥ ⎢ 0.5 − q3 ⎥ ⎥ ⎢ ⎥ ⎢ ⎣ ϕ ⎦ ⎣ q1 + q2 − q4 ⎦ ⎡0.5 + 0.2cos(2t ) ⎤ ⎢ 0.5 + 0.2sin(2t ) ⎥ ⎥ x(t ) = ⎢ ⎢ 0.3 + 0.1sin(4t ) ⎥ ⎢ ⎥ ⎣ ⎦ (3.37) Giải toán ngượi phương pháp hiệu chỉnh gia lượng véc tơ tọa độ suy rộng, ta && xác định giá trị q,q& , q Mômen điều khiển xác định theo công thức && r + C(q,q& )q& r − g$ (q) − K pd s − K s sgn(s) τ = M (q)q (3.38) Sơ đồ điều khiển xây dựng theo (3.36) hình 3.9 Hàm bão hòa sat dung thay cho hàm dấu sgn để giảm tượng chattering Thông số điều khiển: K pd = diag (5 2) K s = diag (50 35 20 5) λ = diag (80 80 50 30) 89 -KKpd DK Truot - Scara q ddq -KSaturation e qd de dqd dq Ks momen dqr s -K- momen dq dqr lambda ds q ddqr -K- ddqr mo hinh lambda1 dde M.ddqr+C.dqr+g+d ddqd dau vao Hình 3.9 Sơ đồ khối hệ điều khiển trượt rôbôt Scara Mômen khớp cho hình 3.10 60 50 60 40 40 30 20 20 10 -20 -10 -40 -20 10 10 3.10b: Mômen khớp 3.10a: Mômen khớp -20 -25 -30 -35 -2 -40 -4 -45 -50 -6 10 3.10d: Mômen khớp 3.10c: Lực khớp Hình 3.10 Mômen điều khiển khớp 90 10 Quỹ đạo khâu thao tác hình 3.11 z x y Hình 3.11 Quỹ đạo điểm đầu khâu thao tác Ta thấy khâu thao tác nhanh chóng vào quỹ đạo chuyển động theo mong muốn 91 KẾT LUẬN Việc nghiên cứu ứng dụng lý thuyết động lực học hệ nhiều vật vào việc lập trình quỹ đạo chuyển động rôbốt công nghiệp nước ta nhiều mẻ Song ứng dụng việc hiệu quả, giúp cho dễ dàng tiếp cận giải toán việc thiết lập phương trình chuyển động rôbốt công nghiệp Từ việc áp dụng lý thuyết động lực học hệ nhiều vật, người ta xây dựng phương trình vi phân chuyển động rôbốt cách dễ dàng nhanh chóng Trên sở phương trình vi phân chuyển động, dễ dàng giải toán điều khiển toán lập trình quỹ đạo động lực học Việc áp dụng phần mềm MAPLE, MATHEMATICAL giúp ta dễ dàng tự động hóa việc thiết lập phương trình vi phân chuyển động rôbốt Việc lập trình quỹ đạo chuyển động rôbốt công nghiệp giúp ta tìm miền cho phép quỹ đạo cho trước điểm tác động Việc áp dụng phương pháp tối ưu vào toán giúp cho ta lựa chọn cấu hình thích hợp cho cấu Bài toán động lực học ngược phương tiện giúp cho việc lựa chọn độ lớn điều khiển lực dẫn động hệ điện tử Phần mềm MATLAB giúp ta tính toán số nhanh chóng giải toán ngược điều khiển rôbốt Luận văn “ Lập trình quỹ đạo điều khiển chuyển động rôbốt công nghiệp” gồm phần mở đầu, kết luận ba chương Trong chương áp dụng lý thuyết Động lực học hệ nhiều vật, tiến hành thiết lập phương trình vi phân chuyển động rôbốt công nghiệp theo dạng thức Lagrange loại II Áp dụng khái niệm ma trận Jacobi tịnh tiến ma trận Jacobi quay thiết lập biểu thức tính ma trận khối lượng suy rộng Sau áp dụng phương trình Lagrange loại II để thành lập phương trình vi phân chuyển động rôbốt Trong luận văn bốn mô hình rôbốt cụ thể khảo sát, xây dựng phương trình vi phân Chương nghiên cứu việc lập trình quỹ đạo rôbốt công nghiệp Cho biết chuyển động khâu thao tác x = x ( s (t )) Phần động học tiến hành xác định chuyển 92 động tọa độ khâu rôbốt q = q (t ) Trong phần động lực học, sở giải toán động học ngược xác định mômen phát động khâu Luận văn sử dụng phương pháp tham số hóa phương trình chuyển động Phương pháp cho phép xác định tương đối thuận tiện lực phát động biết chuyển động khâu thao tác Chương trình bày sơ điều khiển rôbốt công nghiệp Các chuyển động theo chương trình có ý nghĩa chúng ổn định Luận văn trình bày khái niệm thiết lập toán điều khiển không gian khớp không gian thao tác Sau trình bày phương pháp điều khiển mômen phương pháp điều khiển trượt Đây vấn đề tương đối khó, thời gian hạn chế, nên chưa có điều kiện nghiên cứu đầy đủ toán Các kết luận văn nghiên cứu bước đầu lập trình quỹ đạo điều khiển rôbốt công nghiệp Việc tương quan hai toán vấn đề có ý nghĩa khoa học thực tế Do cần nghiên cứu tiếp 93 TÀI LIỆU THAM KHẢO Nguyễn Văn Khang: Động lực học hệ nhiều vật NXB Khoa học Kỹ thuật, Hà Nội 2007 B Heimann, W.Gerth, K Popp: Cơ điện tử (Nguyễn Văn Khang, Nguyễn Phong Điền, Nguyễn Quang Hoàng, Hoàng Minh Sơn dịch) NXB Khoa học Kỹ thuật, Hà Nội 2007 Nguyễn Thiện Phúc: Rôbôt công nghiệp NXB Khoa học Kỹ thuật, Hà Nội 2004 Nguyễn Văn Khang, Chu Anh Mỹ: Cơ sở rôbốt công nghiệp NXB Giáo dục Việt Nam, Hà Nội 2011 Phạm Đăng Phước: Rôbốt công nghiệp NXB Xây dựng, Hà Nội 2007 Đỗ Thành Trung: Mô hình hóa mô số động lực học rôbôt công nghiệp Luận văn thạc sĩ khí, Đại học Bách khoa Hà Nội, Hà Nội 2003 Ngô Minh Dũng: Động học cấu hình kỳ dị Rôbôt Đồ án tôt nghiệp đại học Bách khoa Hà Nội, Hà Nội 2005 Nguyễn Mạnh Tiến: Điều khiển rôbốt công nghiệp NXB Khoa học Kỹ thuật, Hà Nội 2007 Phạm Thượng Cát: Một số phương pháp điều khiển đại rôbốt công nghiệp NXB Đại học Thái Nguyên, Thái Nguyên 2009 10 Nguyễn Văn Minh: Góp phần nghiên cứu động lực học điều khiển rôbốt công nghiệp Luận văn thạc sỹ, Đại học Bách khoa Hà Nội, Hà Nội 2005 11 Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng Nam: Về thuật toán điều khiển trượt rôbốt dư dẫn động Tạp chí tin học điều khiển học, Tập 24 (2008), No.3, Trang 269-280 12 Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: Về toán ngược động lực học rôbốt dư dẫn động Kỷ yếu hội nghị khoa học công nghệ khí chế tạo toàn quốc lần thứ NXB Khoa học Kỹ thuật, Hà Nội 2010 Trang 276 – 287 13 Nguyễn Trung Tuấn: Tính toán động học, động lực học điều khiển rôbốt phương pháp ma trận Craig Đồ án tốt nghiệp, Đại học Bách khoa Hà Nội, Hà Nội 2010 94 14 Chu Hoàng Anh: Tính toán động lực học mô số rôbốt công nghiệp dựa chương trình robodyn cải tiến Luận văn thạc sỹ, Đại học Bách khoa Hà Nội, Hà Nội 2010 15 Phạm Duy Điền: Tính toán, lập trình giảng dạy toán học Maple NXB Giáo dục Việt Nam, Hà Nội 2011 16 Phạm Minh Hoàng: Maple toán ứng dụng NXB Khoa học Kỹ thuật, TP Hồ Chí Minh 2008 17 Đinh Văn Phong: Mô số điều khiển hệ học NXB Giáo dục Việt Nam, Hà Nội 2010 18 Nguyễn Doãn Phước: Lý thuyết điều khiển tuyến tính NXB Khoa học Kỹ thuật, Hà Nội 2007 19 Lưu Văn Hiệu, Lương Văn Hưng: Ứng dụng SimMechanics-Matlab mô cần trục quay Đồ án tốt nghiệp đại học Bách khoa Hà Nội, Hà Nội 2005 20 J.J Craig: Introduction to Robotics (3rd edition), Prentice Hall, Upper Saddle River, New Jersey 2005 21 B.Siciliano.O Khatib: Springer Handbook of Robotics Springer – Verlag Berlin 2008 22 R.K Mittal, I.J Nagrath: Robotics and Control Tata McGraw-Hill publishing company limited, New Delhi 23 B Siciliano, L Sciavicco, L Villani, G Oriolo: Robotics, Modeling, Planning and Control Springer – Verlag, Lodon 2009 24 Nguyen Van Khang, Do Thanh Trung: A Contribution to the Dynamic Simulation of Robot Manipulator with software robodyn Vietnam Journal of Mechanics, VAST, Vol26, No.4 (2004), Paper 215-225 25 Nguyen Van Khang: Partial derative of matrix functions with respect to a vector variable Vietnam Journal of Mechanics, 30(2008), N.4, pp 269-279 26 Nguyen Van Khang: Consistent definition of partial derivatives of matrix functions in dynamics of mechanical systems Mechanism and Machine Theory 45 (2010), Elsevier 27 Waterloo Maple Inc: Maple help Maple version 13.0, release 2009 95 28 The MathWorks, inc: Matlab help Matlab version 7.7.0.471 (R2008b), release 2008 96 PHỤ LỤC Mã nguồn Maple chương trình rôbốt cực > restart: r:=sqrt(x^2+s^2)-b: theta:=arctan(s/x): q[1]:=r/R: q[2]:=theta: A:=simplify(diff(q[1],s)): B:=simplify(diff(q[2],s)): C:=simplify(diff(q[1],s,s)): E:=simplify(diff(q[2],s,s)): > `VE DO THI VOI S/R =0.5`; s:=0.5: x:=1: b:=0.1: R:=1: Q[11]:=0.7: Q[12]:=-0.7: Q[21]:=1: Q[22]:=-1: D(S[chambinh11]):=simplify(2*Q[11]/A+2*((q[1]*B^2-C)*S[chambinh])/A): D(S[chambinh12]):=simplify(2*Q[12]/A+2*((q[1]*B^2-C)*S[chambinh])/A): D(S[chambinh21]):=simplify(2*Q[21]/((1+q[1]^2)*B)2*(((1+q[1]^2)*E+2*q[1]*A*B)*S[chambinh])/((1+q[1]^2)*B)): D(S[chambinh22]):=simplify(2*Q[22]/((1+q[1]^2)*B)2*(((1+q[1]^2)*E+2*q[1]*A*B)*S[chambinh])/((1+q[1]^2)*B)): plot([D(S[chambinh11]),D(S[chambinh12]),D(S[chambinh21]),D(S[chambinh22])],S[ chambinh]=0 10); Mã nguồn Maple chương trình rôbốt hai khâu > restart: with(linalg): q1:=theta[1]: q2:=theta[2]: q:=matrix(2,1,[q1,q2]): Xc1:=l[1]*cos(q1): Yc1:=l[1]*sin(q1): Zc1:=0: Xc2:=a[1]*cos(q1)+l[2]*cos(q2): Yc2:=a[1]*sin(q1)+l[2]*sin(q2): Zc2:=0: rc1:=matrix(3,1,[Xc1,Yc1,Zc1]): rc2:=matrix(3,1,[Xc2,Yc2,Zc2]): 97 JT1:=matrix(3,2,[diff(Xc1,q1),diff(Xc1,q2),diff(Yc1,q1),diff(Yc1,q2),diff(Zc1,q1),dif f(Zc1,q2)]): JT2:=matrix(3,2,[diff(Xc2,q1),diff(Xc2,q2),diff(Yc2,q1),diff(Yc2,q2),diff(Zc2,q1),dif f(Zc2,q2)]): omega11:=0: omega12:=0: omega13:=Diff(q1,t): omega1:=matrix(3,1,[omega11,omega12,omega13]): omega21:=0: omega22:=0: omega23:=Diff(q1,t): omega2:=matrix(3,1,[omega21,omega22,omega23]): omega13:=C: omega23:=E: JR1:=matrix(3,2,[diff(omega11,C),diff(omega11,E),diff(omega12,C),diff(omega12,E) ,diff(omega13,C),diff(omega13,E)]): JR2:=matrix(3,2,[diff(omega21,C),diff(omega21,E),diff(omega22,C),diff(omega22,E) ,diff(omega23,C),diff(omega23,E)]): I1:=matrix(3,3,[I[11],0,0,0,I[12],0,0,0,I[13]]): I2:=matrix(3,3,[I[21],0,0,0,I[22],0,0,0,I[23]]): JT1T:=transpose(JT1): JT2T:=transpose(JT2): JR1T:=transpose(JR1): JR2T:=transpose(JR2): m1:=matrix(3,3,[m[1],0,0,0,m[1],0,0,0,m[1]]): m2:=matrix(3,3,[m[2],0,0,0,m[2],0,0,0,m[2]]): M11:=simplify(multiply(JT1T,m1,JT1)+ multiply(JR1T,I1,JR1)): M12:=simplify(multiply(JT2T,m2,JT2)+ multiply(JR2T,I2,JR2)): M:=evalm(M11 + M12): G:=map(diff,q(t),t): G_T:=transpose(G): T:=simplify(1/2*multiply(G_T,M,G)): PI:=-m[1]*l[1]*g*cos(theta[1])-m[2]*g*(a[1]*cos(theta[1])+l[2]*cos(theta[2])): > Q[11]:=4.2: Q[12]:=-4.2: Q[21]:=1.35: Q[22]:=-1.35: m[1]:=0.8: m[2]:=1.2: a[1]:=0.8: a[2]:=1.2: l[1]:=a[1]/2: l[2]:=a[2]/2: L[13]:=m[1]*a[1]^2/12: L[23]:=m[2]*a[2]^2/12: g:=0.1: 98 x:=1: a[1]:=0.8: a[2]:=1.2: theta[3]:=arccos((x^2+y^2-a[1]^2-a[2]^2)/(2*a[1]*a[2])): theta[2]:=arccos((a[1]*x*cos(theta[3])a[1]*y*sin(theta[3])+a[2]*x)/(2*a[1]*a[2]*cos(theta[3])+a[1]^2+a[2]^2)): theta[1]:=theta[2]-theta[3]: A:=diff(theta[1],y): B:=diff(theta[2],y): C:=diff(theta[1],y,y): H:=diff(theta[2],y,y): > `VE DO THI VOI y = 1`; y:='y': y:=1: A1:=L[13]+m[1]*l[1]^2+m[2]*a[1]^2: A2:=a[1]*l[2]*m[2]*cos(theta[1]-theta[2]): A3:=a[1]*l[2]*m[2]*sin(theta[1]-theta[2]): A4:=L[23]+m[2]*l[2]^2: A5:=(m[1]*l[1]+m[2]*a[1])*g*sin(theta[1]): A6:=m[2]*l[2]*g*sin(theta[2]): D(S[chambinh11]):=2*(Q[11]-A5)/(A1*A+A2*B)2*(A1*C+A2*H+A3*B)*S[chambinh]/(A1*A+A2*B): D(S[chambinh12]):=2*(Q[12]-A5)/(A1*A+A2*B)2*(A1*C+A2*H+A3*B)*S[chambinh]/(A1*A+A2*B): D(S[chambinh21]):=2*(Q[21]-A6)/(A2*A+A4*B)-2*(A2*C+A4*HA3*A)*S[chambinh]/(A2*A+A4*B): D(S[chambinh22]):=2*(Q[22]-A6)/(A2*A+A4*B)-2*(A2*C+A4*HA3*A)*S[chambinh]/(A2*A+A4*B): plot([D(S[chambinh11]),D(S[chambinh12]),D(S[chambinh21]),D(S[chambinh22])],S[ chambinh]=-10 10); Mã nguồn Maple chương trình rôbốt Scara có khâu thao tác quay > restart: L11:= 1.0: L12:= 1.2: LC1:= 0.5: LC2:= 0.6: m1:= 1: m2:= 1.2: m3:= 0.5: m4:= 1: I1z:= 0.2: I2z:= 0.12: I3z:= 0.015: I4z:= 0.015: 99 X0:= 0.4: Y0:= 0.4: R:= 0.5: xc3:= X0 + R*cos(s): yc3:= Y0 + R*sin(s): q[2]:= arccos((xc3^2 + yc3^2 - L11^2 - L12^2)/2/L11/L12): q[1]:= arccos((xc3*(L11+L12*cos(q[2]))+yc3*L12*cos(q[2]))/((L11+L12*cos(q[2]))^2+(L1 2*cos(q[2]))^2)): q[4]:= -s: q11:= simplify(diff(q[1],s)): q21:= simplify(diff(q[2],s)): q41:= simplify(diff(q[4],s)): q12:= simplify(diff(q[1],s,s)): q22:= simplify(diff(q[2],s,s)): q42:= simplify(diff(q[4],s,s)): > `VE DO THI VOI s = 1.0`; s:=1.0: T1min:= -5: T1max:= 5: T2min:= -3: T2max:= 3: T4min:= -2: T4max:= 2: A1:= I1z+I2z+I3z+I4z + m1*LC1^2 + m2*(LC2^2+L11^2+2*LC2*L11*cos(q[2])) + (m3+m4)*(L11^2+L12^2+2*L11*L12*cos(q[2])): A2:= I2z+I3z+I4z + m2*(LC2^2+LC2*L11*cos(q[2])) + (m3+m4)*(L12^2+L11*L12*cos(q[2])): A4:= I4z: a12:= -2*(m2*LC2+m3*L12+m4*L12)*L11*sin(q[2]): a22:= -(m2*LC2+m3*L12+m4*L12)*L11*sin(q[2]): B1:= I2z+I3z+I4z + m2*(LC2^2+LC2*L11*cos(q[2])) + (m3+m4)*(L12^2+L11*L12*cos(q[2])): B2:= I2z+I3z+I4z + m2*LC2^2 + (m3+m4)*L12^2: B4:= I4z: b11:= (m2*LC2+m3*L12+m4*L12)*L11*sin(q[2]): D1:= I4z: D2:= I4z: D(S[chambinh1min]):=simplify(2/(A1*q11+A2*q21+A4*q41)*(T1min (A1*q12+A2*q22+A4*q42+a12*q11*q21+a22*q21^2)*S[chambinh])): D(S[chambinh1max]):=simplify(2/(A1*q11+A2*q21+A4*q41)*(T1max (A1*q12+A2*q22+A4*q42+a12*q11*q21+a22*q21^2)*S[chambinh])): D(S[chambinh2min]):=simplify(2/(B1*q11+B2*q21+B4*q41)*(T2min (B1*q12+B2*q22+B4*q42+b11*q11^2)*S[chambinh])): D(S[chambinh2max]):=simplify(2/(B1*q11+B2*q21+B4*q41)*(T2max (B1*q12+B2*q22+B4*q42+b11*q11^2)*S[chambinh])): 100 D(S[chambinh4min]):=simplify(2/(D1*q11+D2*q21)*(T4min (D1*q12+D2*q22)*S[chambinh])): D(S[chambinh4max]):=simplify(2/(D1*q11+D2*q21)*(T4max (D1*q12+D2*q22)*S[chambinh])): plot([D(S[chambinh1min]),D(S[chambinh1max]),D(S[chambinh2min]),D(S[chambin h2max])],S[chambinh]=-5 5); plot([D(S[chambinh4min]),D(S[chambinh4max])],S[chambinh]=-5 5); Mã nguồn Matlab chương trình điều khiển trượt rôbốt Scara có khâu thao tác quay Momen.m function [sys,x0,str,ts]=M(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case {1,2,4,9} sys=[]; case 3, sys=mdlOutputs(t,x,u); end %Ham mdlInitializeSizes function [sys,x0,str,ts]=mdlInitializeSizes sizes=simsizes; sizes.NumContStates =0; sizes.NumDiscStates =0; sizes.NumOutputs =4; sizes.NumInputs =16; sizes.DirFeedthrough=1; size.NumSampleTimes =-1; sys=simsizes(sizes); x0=[]; str=[]; ts=[]; function sys=mdlOutputs(t,x,u) sys(1) =(0.2400000000e1 * cos(u(2)) + 0.3542500000e1) * u(13) + (0.9800000000e0 + 0.1200000000e1 * cos(u(2))) * u(14) + (-0.1e1) * 0.1000000000e-1 * u(16) + (0.1e1) * 0.2400000000e1 * u(6) * sin(u(2)) * u(9) + (-0.1e1) * 0.1200000000e1 * u(6) * sin(u(2)) * u(10); sys(2) =(0.9800000000e0 + 0.1200000000e1 * cos(u(2))) * u(13) + 0.9800000000e0 * u(14) + (-0.1e1) * 0.1000000000e-1 * u(16) + 0.6000000000e0 * sin(u(2)) * (0.2e1 * u(5) + (-0.1e1) * 0.1e1 * u(6)) * u(9) + 0.6000000000e0 * u(5) * sin(u(2)) * u(10); sys(3) = 0.3e1 * u(15) - 0.2943000000e2; sys(4) =(-0.1e1) * 0.1000000000e-1 * u(13) + (-0.1e1) * 0.1000000000e-1 * u(14) + 0.1000000000e-1 * u(16); 101 Mohinh.m function [sys,x0,str,ts]=M(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case {1,2,4,9} sys=[]; case 3, sys=mdlOutputs(t,x,u); end %Ham mdlInitializeSizes function [sys,x0,str,ts]=mdlInitializeSizes sizes=simsizes; sizes.NumContStates =0; sizes.NumDiscStates =0; sizes.NumOutputs =4; sizes.NumInputs =16; sizes.DirFeedthrough=1; size.NumSampleTimes =-1; sys=simsizes(sizes); x0=[]; str=[]; ts=[]; function sys=mdlOutputs(t,x,u) sys(1) = (-0.1e1) * 0.3200000000e1 * (0.485e3 * (u(9)-u(13)) + 0.1164e4 * u(6) * sin(u(2)) * u(5) + 0.582e3 * u(6) ^ * sin(u(2)) + (-0.1e1) * 0.485e3 * (u(10)-u(14)) + 0.582e3 * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.600e3 * cos(u(2)) * (u(10)-u(14)) + 0.720e3 * cos(u(2)) * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.600e3 * (u(12)-u(16)) * cos(u(2))) / (-0.3977e4 + 0.2304e4 * cos(u(2)) ^ 2); sys(2) = 0.8000000000e0 * (0.1940e4 * (u(9)-u(13)) + 0.4656e4 * u(6) * sin(u(2)) * u(5) + 0.2328e4 * u(6) ^ * sin(u(2)) + 0.2400e4 * cos(u(2)) * (u(9)-u(13)) + 0.5760e4 * cos(u(2)) * u(6) * sin(u(2)) * u(5) + 0.2880e4 * cos(u(2)) * u(6) ^ * sin(u(2)) + (-0.1e1) * 0.4800e4 * cos(u(2)) * (u(10)-u(14)) + 0.5760e4 * cos(u(2)) * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.7065e4 * (u(10)-u(14)) + 0.8478e4 * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.2400e4 * (u(12)-u(16)) * cos(u(2)) + (-0.1e1) * 0.5125e4 * (u(12)-u(16))) / (-0.3977e4 + 0.2304e4 * cos(u(2)) ^ 2); sys(3) = 0.3333333333e0 * (u(11)-u(15)) + 0.9809999999e1; sys(4) = 0.4e1 * (0.480e3 * cos(u(2)) * (u(9)-u(13)) + 0.1152e4 * cos(u(2)) * u(6) * sin(u(2)) * u(5) + 0.576e3 * cos(u(2)) * u(6) ^ * sin(u(2)) + (-0.1e1) * 0.480e3 * cos(u(2)) * (u(10)-u(14)) + 0.576e3 * cos(u(2)) * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.1025e4 * (u(10)-u(14)) + 0.1230e4 * sin(u(2)) * u(5) ^ + (-0.1e1) * 0.100450e1 * (10 ^ 5) * (u(12)-u(16)) + 0.57600e5 * (u(12)-u(16)) * cos(u(2)) ^ 2) / (-0.3977e4 + 0.2304e4 * cos(u(2)) ^ 2); 102 ... Chương - LẬP TRÌNH QUỸ ĐẠO 33 2.1 Mở đầu .33 2.1.1 Một vài thuật ngữ sử dụng lập trình quỹ đạo .33 2.1.2 Nhiệm vụ lập trình quỹ đạo rôbốt 34 2.1.3 Các bước toán lập trình. .. thiết lập phương trình vi phân cho số dạng chuyển động cụ thể Chương 2: Lập trình quỹ đạo chuyển động rôbốt công nghiệp, cho trước cụ thể điểm tác động cuối rôbốt, cách giải toán động học ngược động. .. 50 Chương - ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA RÔBỐT 77 3.1 Thiết lập toán .77 3.1.1 Điều khiển chuyển động rôbốt không gian khớp .78 3.1.2 Điều khiển chuyển động rôbốt không gian