Lập trình quỹ đạo và điều khiển chuyển động của robot công nghiệp

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI CAO XN HỒ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ố qn tính ly tâm Cơriơlis mơ hình thực 14 ˆ ( q, q& ) C Ma trận hệ số qn 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 tố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 tố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 q 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 tố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 tố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 qn 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 tố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 dqd de dde dq Ks momen dqr s -K- momen dq dqr lambda ds -K- q ddqr ddqr mo hinh lambda1 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 Hình 3.10 Mơmen điều khiển khớp 90 ... (hoặc quỹ đạo theo chương trình) Mục tiêu lập trình quỹ đạo tạo tham số đầu vào cho hệ thống điều khiển chuyển động để đảm bảo rôbốt thực theo quỹ đạo lập trình Nhiệm vụ tốn lập trình quỹ đạo xác... vị khâu chuyển động quỹ khâu quanh điểm định vị Đơi tốn lập trình quỹ đạo gọi thiết kế quỹ đạo Các phương pháp lập trình quỹ đạo phân thành hai nhóm: Lập trình trực tuyến (on-line) lập trình ngoại... 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 tốn lập trình quỹ