Bài tập lớn Kĩ thuật Robot: Xây dựng mô hình Robot Scara

Đại học Bách Khoa Hà Nội Trường Điện Điện tử Kĩ thuật Robot Xây dựng mô hình Robot Sacra Nhóm sinh viên thực hiện MSSV Nguyễn Đăng Lai 20181564 Nguyễn Đức Long 20181608 Trần Thanh Tùng 20181529 Nguyễn Văn Hùng 20181509 Đào Đức Thắng 20181752 Vũ Thị Thu 20181774 Lê Duy Long 20181597 Vũ Gia Lộc 20181586 Nguyễn Hữu Thắng 20181755 Lê Quang Hiếu 20141508 Giảng viên hướng dẫn PGS TS Nguyễn Phạm Thục Anh Hà Nội, 1 2022 1 Mục lục Phần 1 Giới thiệu về Robot Scara 2 I Giới thiệu về robot 2 1 Giới thiệu ch.

Đại học Bách Khoa Hà Nội Trường Điện- Điện tử Kĩ thuật Robot Xây dựng mơ hình Robot Sacra Nhóm sinh viên thực MSSV Nguyễn Đăng Lai 20181564 Nguyễn Đức Long 20181608 Trần Thanh Tùng 20181529 Nguyễn Văn Hùng 20181509 Đào Đức Thắng 20181752 Vũ Thị Thu 20181774 Lê Duy Long 20181597 Vũ Gia Lộc 20181586 Nguyễn Hữu Thắng 20181755 Lê Quang Hiếu 20141508 Giảng viên hướng dẫn: PGS.TS Nguyễn Phạm Thục Anh Hà Nội, 1-2022 Mục lục Phần Giới thiệu Robot Scara I Giới thiệu robot Giới thiệu chung 2 Robot Scara YASKAWA Model MYS450F II Các ứng dụng công nghiệp III Kết cấu khí Cấu tạo khí Thông số kỹ thuật Phần Động học Robot Scara I Động học thuận Gắn hệ tọa độ Lập bảng D- H tính tốn 10 Tính tốn matlab 12 II Động học đảo 12 1.Tính tốn động học đảo 12 1.1 Phép đảo vị trí 12 1.2 Phép phân ly 14 Tính tốn Matlab 15 Phần Ma Trận Jacobi 16 I Tính tốn ma trận Jacobi 16 II Chương trình code matlab 17 Phần Thiết kế quỹ đạo chuyển động 18 I Thiết kế quỹ đạo 18 II Xét toán cụ thể 18 Phần Xây dựng mô hình động lực học cho robot Scara 22 I Xây dựng mơ hình động lực học 22 Phần Điều khiển Robot Scara 27 Phần Giới thiệu robot Scara I Giới thiệu robot Giới thiệu chung • Scara Robot phát minh lần Giáo sư Hiroshi Makino Đại học Yamanashi, Nhật Bản vào năm 1978 Nó thiết kế để đưa vào ứng dụng lắp ráp từ năm 1981 ứng dụng dây chuyền lắp ráp cơng nghiệp Đó tay máy scara đặc biệt gồm hai khớp quay khớp trượt, ba khớp có trục song song với Kết cấu làm cho tay máy cứng, vững theo phương thẳng đứng Z cứng vững theo phương ngang XY Loại chuyên dùng công việc lắp ráp với tải trọng nhỏ theo phương thẳng đứng Hình Robot Scara thực tế • Scara hiểu theo hai định nghĩa sau: o Thứ nhất, Scara - Selective Compliance Assembly Robot Arm có nghĩa cánh tay Robot lắp ráp tuân thủ có chọn lọc Đây định nghĩa đưa từ ý nghĩa dùng phổ biến o Thứ hai, Scara - Selective Compliance Articulated Robot Arm có nghĩa cánh tay Robot khớp nối tuân thủ có chọn lọc Đây định nghĩa sau Định nghĩa đưa đưa vào áp ứng cho nhiệm vụ khác nằm nhiệm vụ lắp ráp trước mà ứng dụng • Scara Robot chủ đề quan tâm nhiều lĩnh vực tự động hóa tính ứng dụng thực tiễn xã hội nói chung ngành cơng nghiệp nói riêng Mỗi tay máy có cấu khác nhau, có phương trình động học, động lực học khác việc tính tốn điều khiển loại tay máy phức tạp Có thể thấy tay máy đề tài khơng mang tính chất khó, tính nghiên cứu ứng dụng cao • Nhờ bố trí khớp trục song song SCARA, cánh tay tuân theo trục XY cố định theo hướng Z Điều thuận lợi cho nhiều loại hoạt động lắp ráp • Thuộc tính thứ hai SCARA bố trí cánh tay hai liên kết tương tự cánh tay người chúng ta, thuật ngữ thường sử dụng, khớp nối Tính cho phép cánh tay mở rộng vào khu vực hạn chế sau rút lại "gập lại" khỏi đường Điều thuận lợi cho việc chuyển phận từ ô sang ô khác để tải / dỡ tải trạm xử lý • SCARA thường nhanh hệ thống robot Cartesian tương đương SCARA đắt hệ thống Cartesian tương đương phần mềm điều khiển yêu cầu động học ngược cho di chuyển nội suy tuyến tính • Robot Scara số hãng sản xuất: Hình Một số hãng sản xuất robot Scara Robot Scara YASKAWA Model MYS450F Robot SCARA có đặc điểm bật như: • Robot SCARA có trục lý tưởng cho hệ thống xử lý lắp ráp tốc độ cao • Cung cấp hiệu suất vượt trội ứng dụng lắp ráp, pha chế, xử lý phần nhỏ, đóng gói vỏ, tự động hóa phịng thí nghiệm, đóng gói, xử lý lượng mặt trời bán dẫn • Lý tưởng cho hệ thống đa quy trình địi hỏi khả chọn-và-đặt • Dễ dàng tích hợp với ứng dụng robot có để mở rộng quy trình tự động • Là dòng sản phẩm Yaskawa, robot 4-15 trục với trọng tải 1-800 kg • Sử dụng điều khiển giống robot Yaskawa khác Vùng làm việc thay đổi tùy thuộc vào kích thước tải trọng robot • Có tốc độ cao, hình thức nhỏ gọn,tối thiểu khơng gian lắp đặt • Có tầm với xun tâm 450 mm, hành trình trục U có tải trọng tối đa 180 mm kg • Các trục bên ngồi có sẵn, khớp linh hoạt • Độ lặp lại cao, có tuân thủ tất trục, làm cho robot lý tưởng cho nhiệm vụ lắp ráp • Có sẵn nhiều loại kết nối bus trường • Tải trọng tối đa kg • Tầm xuyên tâm 450 mm • Hành trình trục U 180 mm • Điều khiển: FS100, MXL200 • Sử dụng điều khiển giống robot Yaskawa lớn • Tích hợp tầm nhìn dễ dàng với MotoSight 2D, hệ thống quan sát dựa tầm nhìn Cognex • Mang lại hiệu cao, yêu cầu đầu tư vốn tối thiểu • Khớp nối linh hoạt, dễ dàng triển khải, lắp đặt lại nhu cầu sản xuất thay đổi II Các ứng dụng công nghiệp Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao suất dây chuyền công nghệ, giảm giá thành sản phẩm, nâng cao chất lượng khả cạnh tranh sản phẩm, đồng thời cải thiện điều kiện lao động, Điều xuất phát từ ưu điểm robot, là: • Robot cơng nghiệp thực qui trình thao tác hợp lí người thợ lành nghề cách ổn định suốt thời gian làm việc Vì robot cơng nghiệp góp phần nâng cao chất lượng khả cạnh tranh sản phẩm Hơn robot cịn nhanh chóng thay đổi cơng việc để thích nghi với thay đổi mẫu mã, kích cỡ sản phẩm theo yêu cầu thị trường cạnh tranh • Khả giảm giá thành sản phẩm ứng dụng robot làm giảm đáng kể chi phí cho người lao động • Việc áp dụng robot làm tăng suất dây chuyền công nghệ Sở dĩ tăng nhịp độ khẩn trương dây chuyền sản xuất, không thay người robot người thợ khơng thể theo kịp chóng mệt mỏi • Robot cải thiện điều kiện lao động Đó ưu điểm bật mà cần lưu tâm Vì thực tế sản xuất có nhiều nơi người lao động phải làm việc mơi trường có hại cho sức khoẻ dễ xảy tai nạn lao động • Robot công nghiệp ứng dụng nhiều lĩnh vực: đúc, gia công áp lực, hàn nhiệt luyện, gia cơng lắp ráp… III Kết cấu khí Cấu tạo khí Hình Robot Scara YASKAWA Model MYS450F Tất kích thước có đơn vị milimet để tham khảo Yêu cầu vẽ chi tiết cho tất yêu cầu thiết kế / kỹ thuật Thông số kỹ thuật Hệ (trục) Phạm vi Tốc độ cực đại Mômen Sai số Số trục điều khiển tọa độ chuyển động [mm/s] quán tinh [mm] Khối hàng [kg] 3/6 450 cực đại cho phép Phạm vi tiếp cận [mm] [𝑘𝑔 ⋅ 𝑚2 ] xuyên tâm [mm] S+L 450 mm - - ±0.015 Trục chữ U [mm] 180 S +242/-62 420 - - Khối lượng [kg] 27 L ±147.5 720 - - Công suất [kVA] 1.25 U 180 mm 1,083 mm/s - ±0.01 R ±360 2400 0.01/0.12 ±0.005 Tốc độ tối đa mômen qn tính thay đổi tùy thuộc vào tải Robot kết cấu theo dạng Scara phương án thích hợp để vận chuyển vật có khối lượng lớn Các khớp quay truyền động động điện Tốc độ quay động điện điều khiển nhờ biến tần công suất 1,5 kw Robot điều khiển nhờ lập trình PLC Các động điện có phản hồi vị trí điều khiển tốc độ quay nhờ biến tần Hệ thống điều khiển yêu cầu phản hồi đảm bảo hoạt động ổn định an tồn cho robot Thơng số kỹ thuật Tải trọng lớn khâu cuối Các giới hạn 100 kg 𝜃1 3000 𝜃2 3200 𝜃̇1 1000 𝜃̇2 1250 /𝑠 chuyển động khâu Tốc độ lớn Độ chinh xác lặp lại vị trí ±1 𝑚𝑚 Khối lượng 200 kg Phạm vi ứng dụng chủ yếu Vận chuyển đồ vật vị trí dây chuyền sản xuất Phần Động học Robot Scara I Động học thuận Gắn hệ tọa độ + Hệ tọa độ {𝑂0 } cố định gắn với thân Robot + Quan hệ hệ tọa độ {𝑂1 } với {𝑂0 }: quay góc 𝜃1 quanh trục 𝑧0 tịnh tiến đoạn 𝑎1 trục 𝑥0 + Quan hệ hệ tọa độ {𝑂2 } với {𝑂1 }: quay góc 𝜃2 quanh trục 𝑧1 , tịnh tiến đoạn 𝑎2 trục 𝑥1 quay góc 180 độ quanh trục 𝑥2 + Quan hệ hệ tọa độ {𝑂3 } với {𝑂2 }: tịnh tiến đoạn 𝑟3 trục 𝑧2 + Quan hệ hệ tọa độ {𝑂4 } với {𝑂3 }: quay góc 𝜃4 quanh trục 𝑧2 Đây hệ tọa độ gắn tay kẹp, khâu tác động cuối Hình Chọn hệ tọa độ gắn với robot Scara E= 𝑐1+2−4 , F=𝑠1+2−4 , G=𝑠1+2−4 , H=−𝑐1+2−4 Ma trận Jacobi: 𝑅0 𝐽=[ 𝑛 𝐸 𝐺 𝐻 0] 𝐽 = 𝑅𝑛 [0 𝐹 𝐻 0 0 0 0 −1 0 𝐸 𝐺 0 (𝐸𝐴 + 𝐹𝐶) 𝐺𝐴 + 𝐻𝐶 = 0 [ II Chương trình code matlab 17 0 𝐹 𝐻 0 𝐴 𝐵 𝐶 𝐷 0 ∗ 0 0 0 −1] [−1 (𝐸𝐵 + 𝐹𝐷) 𝐺𝐵 + 𝐻𝐷 0 0 0 0 0 0 −1] 0 0 0 0 0 1] Phần Thiết kế quỹ đạo chuyển động I Thiết kế quỹ đạo - Thiết kế quỹ đạo chuyển động xác định đường biểu diễn vị trí góc khớp (góc quay khớp với khớp quay & độ di chuyển khớp khớp tịnh tiến) theo thời gian di chuyển từ vị trí ban đầu 𝑞0 đến vị trí cuối 𝑞𝑐 khoảng thời gian 𝑡𝑐 với 𝑞 biến khớp tổng quát - Quỹ đạo chuyển động khớp hai vị trí phải thỏa mãn điều kiện: vị trí đầu, vị trí cuối, tốc độ vị trí đầu & tốc độ vị trí cuối Do đó, đa thức bậc phù hợp với quỹ đạo khớp robot: 𝑞(𝑡) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 + 𝑎3 𝑡 - Giả sử thời điểm đầu & cuối, vận tốc 0, ta có hệ phương trình cho khớp: 𝑞(𝑡0 ) = 𝑞0 = 𝑎0 𝑞(𝑡𝑐 ) = 𝑞𝑐 = 𝑎0 + 𝑎1 𝑡𝑐 + 𝑎2 𝑡𝑐2 + 𝑎3 𝑡𝑐3 𝑞̇ (𝑡0 ) = 𝑞̇ = 𝑎1 {𝑞̇ (𝑡𝑐 ) = 𝑞̇ 𝑐 = 𝑎1 𝑡𝑐 + 2𝑎2 𝑡𝑐 + 3𝑎3 𝑡𝑐2 = Giải hệ phương trình ta phương trình quỹ đạo: 𝑎0 = 𝑞0 𝑎1 = 𝑎2 = 3(𝑞𝑐 −𝑞0 ) { 𝑎3 = − 𝑡𝑐2 2(𝑞𝑐 −𝑞0 ) 𝑡𝑐3 II Xét toán cụ thể Thiết kế quỹ đạo chuyển động với thông số: 𝑞10 𝜋/6 𝑞1𝑐 𝜋/3 𝑞20 𝑞2𝑐 𝜋/3 - Lập phương trình cho khớp 1: 18 𝑞40 −𝜋/12 𝑞4𝑐 3𝜋/4 𝑡 𝜋 𝑎10 = 𝑞10 = 𝑎11 = 𝑎12 = 𝜋 3(𝑞1𝑐 −𝑞10 ) = 𝑡𝑐2 2(𝑞1𝑐 −𝑞10 ) { 𝑎13 = − 𝜋 𝜋 52 3( − ) 50 𝜋 𝜋 2( − ) 53 =− 𝑡𝑐3 𝑎10 = 𝑎11 = 𝜋 ⇔ 𝑎12 = 𝜋 { 𝑎13 = − 375 Ta có phương trình động học khâu 1: 𝜋 𝜋 𝜋 50 𝑞1 (𝑡) = + 𝑞̇ (𝑡) = 25 𝜋 𝑡2 − 𝜋 𝑡− 125 2𝜋 𝑡 𝜋 𝑡3 375 𝑞̈ (𝑡) = − 𝑡 25 125 { Tương tự cho khớp 2: 𝑎20 = 𝑞20 = 𝑎21 = 𝑎22 = 3(𝑞2𝑐 −𝑞20 ) = 𝑡𝑐2 { 𝑎23 = − 2(𝑞2𝑐 −𝑞20 ) 𝑡𝑐3 𝜋 3( −0) 52 =− 𝑎10 = 𝑎11 = ⇔ 𝑎12 = 𝜋 25 𝜋 2( −0) 53 2𝜋 {𝑎13 = − 375 Phương trình động học khâu 2: 𝑞2 (𝑡) = 𝜋 25 𝑞̇ (𝑡) = − { 𝑞̈ (𝑡) = − 𝜋 𝑡2 − 𝜋 125 2𝜋 125 𝑡 375 𝑡3 𝑡 • Khâu 4: 𝜋 𝑎40 = 𝑞40 = − 12 𝑎41 = 𝑎42 = 3(𝑞4𝑐 −𝑞40 ) { 𝑎43 = − 𝑡𝑐2 = 2(𝑞4𝑐 −𝑞40) 𝑡𝑐3 𝜋 3( 3𝜋 𝜋 + ) 12 52 =− 3𝜋 𝜋 2( + ) 12 53 19 𝑎40 = − 12 𝑎41 = 𝜋 ⇔ 𝑎42 = 10 𝜋 { 𝑎43 = − 75 Phương trình động học khâu 4: 𝑞4 (𝑡) = − 𝜋 𝜋 12 𝑞̇ (𝑡) = 𝑡 𝜋 { 𝜋 + 𝑡2 − 10 𝜋 − 𝑡 25 2𝜋 𝑞̈ (𝑡) = − 25 𝜋 75 𝑡3 𝑡 Thiết kế quỹ đạo matlab Code: %khớp1 syms t >> y1=pi/6+t^2*pi/50-t^3*pi/375 >> ezplot(y1,[0 5]) >> xlabel('t(s)') >> ylabel('y1') >> grid %khớp2 syms t >> y2=t^2*pi/25-t^3*2*pi/375; >> ezplot(y2,[0 5]) >> xlabel('t(s)') >> ylabel('y2') >> grid %khớp4 >> syms t >> y4=-pi/12+t^2*pi/10-t^3*pi/75; >> ezplot(y4,[0 5]) >> xlabel('t(s)') 20 >> ylabel('y4') >> grid Ta có kết mơ sau: Hình Hình dạng quay đạo khớp 1, 21 Phần Xây dựng mơ hình động lực học cho robot Scara I Xây dựng mơ hình động lực học Hàm Lagrange: L = K – P Trong K tổng động năng, P tổng hệ thống Phương trình động lực học: 𝐹𝑖 (𝑇𝑖 ) = 𝑑 𝜕𝐿 𝜕𝐿 ( )− 𝑑𝑡 𝜕𝑞̇ 𝑖 𝜕𝑞𝑖 Với • 𝑞𝑖 biến khớp: 𝑞𝑖 = 𝜃𝑖 với khớp quay 𝑞𝑖 = 𝑟𝑖 với khớp tịnh tiến • 𝐹𝑖 lực đặt lên khớp tịnh tiến • 𝑇𝑖 mơmen khớp quay 1 2 • 𝐾𝑖 = 𝑚𝑖 𝑣𝑖2 + 𝐽𝑖 𝜔𝑖2 động nối i • 𝑃𝑖 = 𝑚𝑖 𝑔ℎ𝑖 nối i Hình Mơ hình robot Scara gắn hệ trục tọa độ 22 (10) Thanh nối thứ nhất: Tọa độ khối tâm xác định: 𝑎1 𝑃𝑐1 = [ 𝑐𝜃1 𝑎1 𝑠𝜃 𝑇 0] Tốc độ khối tâm nối là: 𝑎 ̇ = [− 𝑠𝜃1 𝜃̇1 𝑃𝑐1 𝑎1 𝑐𝜃 𝜃̇ 1 𝑇 0] Tốc độ quay nối là: 𝜔𝑐1 = [0 𝜃̇1 ]𝑇 Động khối tâm nối là: 𝑇 𝑇 1 ̇ 𝑚1 𝑃𝑐1 ̇ + 𝜔𝑐1 𝐾1 = 𝑃𝑐1 𝐼𝑧𝑧1 𝜔𝑐1 = ( 𝑚1 𝑎12 + 𝐼𝑧𝑧1 ) 𝜃̇12 2 Thế năng: 𝑎1 𝑐𝜃 𝑃1 = −𝑚1 𝐺 𝑇 𝑃𝑐1 = −𝑚1 [0 −𝑔] 𝑎1 =0 𝑠𝜃1 [ ] Thanh nối thứ 2: Tọa độ khối tâm xác định: 𝑃𝑐2 = [𝑎1 𝑐𝜃1 + 𝑎2 𝑐𝜃 12 𝑎1 𝑠𝜃1 + 𝑎2 𝑠𝜃 12 𝑇 0] Tốc độ khối tâm nối 2: ̇ = [−𝑎1 𝑠𝜃1 𝜃̇1 − 𝑃𝑐2 𝑎2 𝑠𝜃12 𝜃̇12 𝑎1 𝑐𝜃1 𝜃̇1 + 𝑎2 𝑐𝜃12 𝜃̇12 Tốc độ quay nối là: 𝜔𝑐2 = [0 𝜃̇12 ]𝑇 Động khối tâm nối 2: 𝑇 𝑇 ̇ 𝑚2 𝑃𝑐2 ̇ + 𝜔𝑐2 𝐾2 = 𝑃𝑐2 𝐼𝑧𝑧2 𝜔2 2 23 𝑇 0] 𝑎22 2 ̇2 = 𝑚2 (𝑎1 𝜃1 + 𝜃̇12 + 𝑎1 𝑎2 𝜃̇1 𝜃̇12 𝑐𝜃2 ) + 𝐼𝑧𝑧2 𝜃̇12 Thế năng: a2    a1c1 + c12    a2  T P2 = − m2G Pc2 = − m2  0 − g   a1s1 + s12  =       Thanh nối thứ 3: Tọa độ khối tâm nối thứ xác định: 𝑃𝑐3 = [𝑎1 𝑐𝜃1 + 𝑎2 𝑐𝜃12 𝑎1 𝑠𝜃1 + 𝑎2 𝑠𝜃12 𝑇 𝑎3 + 𝑎4 − 𝑟3 ] Tốc độ khối tâm nối thứ là: ̇ = [−𝑎1 𝑠𝜃1 𝜃̇1 − 𝑎2 𝑠𝜃12 𝜃̇12 𝑃𝑐3 𝑎1 𝑐𝜃1 𝜃̇1 + 𝑎2 𝑐𝜃12 𝜃̇12 𝑇 𝑎3 + 𝑎4 − 𝑟3 ] Tốc độ quay nối là: 𝜔𝑐3 = [0 𝜃̇12 ]𝑇 Động khối tâm nối 3: K3 = ( ) T 1 P c3 m3 Pc3 + cT3 I zz3 3 = m3 a1212 + a22122 + 2a1a2112c + r32 + I zz3122 2 2 Thế năng:   a1c1 + a2 c12  P3 = −m3GT Pc3 = −m3  0 − g   a1s1 + a2 s12  a  + a4 − r3  24    a3  = m3 g ( + a4 − r3 )    Thanh nối thứ 4: Tọa độ khối tâm nối thứ xác định: 𝑃𝑐4 = [𝑎1 𝑐𝜃1 + 𝑎2 𝑐𝜃12 𝑇 𝑎4 − 𝑟3 ] 𝑎1 𝑠𝜃1 + 𝑎2 𝑠𝜃12 Tốc độ khối tâm nối thứ là: ̇ = [−𝑎1 𝑠𝜃1 𝜃̇1 − 𝑎2 𝑠𝜃12 𝜃̇12 𝑃𝑐4 𝑎1 𝑐𝜃1 𝜃̇1 + 𝑎2 𝑐𝜃12 𝜃̇12 −𝑟3 ]𝑇 Tốc độ quay nối là: 𝜔𝑐4 = [0 𝜃̇12 − 𝜃̇4 ]𝑇 Động khối tâm nối 4: ( ) 1 1 K = PTc4 m4 Pc4 + cT4 I zz4 4 = m4 a1212 + a22122 + 2a1a2112c + r32 + I zz4 (12 −  )2 2 2 Thế năng:    a1c1 + a2c12    a P4 = −m4GT Pc4 = −m4  0 − g   a1s1 + a2 s12  = m4 g ( − r3 )  a  − r3     Hàm Lagrange: 4 i =1 i =1 L =  K i − Pi =  a2 1  ( m1a12 + I zz1 )12 + m2  a1212 + 122 + a1a2112 c  +   ( ) a 1 I zz2122 + m3 a1212 + a22122 + 2a1a2112 c + r32 + I zz3122 − m3 g ( + a4 − r3 ) + 2 2 a 1 + m4 a1212 + a22122 + 2a1a2112 c + r32 + I zz4 (12 −  ) − m4 g ( − r3 ) 2 + ( ) 25 • Momen tác động lên nối 1: 1 = d L L ( )− dt 1 1    1  =  m1 + m2 + m3 + m4  a12 +  m2 + m3 + m4  a22 + ( m2 + 2m3 + 2m4 ) a1a2c + I zz1 + I zz2 + I zz3 + I zz4  1 +  4       1  +  m2 + m3 + m4  a22 +  m2 + m3 + m4  a1a2c + I zz2 + I zz3 + I zz4   − I zz4 −  2    − (m2 + 2m3 + 2m4 )a1a2 s 21 − ( m2 + m3 + m4 )a1a2 s 2 22 • Momen tác động lên nối 2: 2 = d L L ( )− dt      1  =  m2 + m3 + m4  a22 +  m2 + m3 + m4  a1a2 c + I zz2 + I zz3 + I zz4  1 +  2       1  +  m2 + m3 + m4  a22 + I zz2 + I zz3 + I zz4   − I zz4 +  m2 + m3 + m4  a1a2 s 212  2    • Lực tác động lên nối 3: F3 = d L L ( )− = (m3 + m4 )(r3 − g ) dt r3 r3 • Momen tác động lên nối 4: 4 = d L L ( )− = I zz4 ( − 1 −  ) dt   26 Phần Điều khiển Robot Scara I Phương pháp điều khiển phi tuyến sở mơ hình Phương trình động lực học: 𝑇 = 𝑀(𝑞)𝑞̈ + 𝑉(𝑞, 𝑞̇ ) + 𝐺(𝑞) Luật điều khiển: 𝑇 = 𝛼𝜏 + 𝛽 𝛼 = 𝑀(𝑞) Trong đó: { 𝛽 = 𝑉(𝑞, 𝑞̇ ) + 𝐺(𝑞) Và: 𝜏 = 𝑞̈ 𝑑 + 𝐾𝑝 (𝑞𝑑 − 𝑞) + 𝐾𝑑 (𝑞̇ 𝑑 − 𝑞̇ ) Hình sơ đồ điều khiển với phương pháp tuyến tính hóa xác Với 𝐾𝑝 𝐾𝑑 ma trận đường chéo xác định dương, đặt 𝑒 = 𝑞𝑑 − 𝑞 ta thu phương trình động lực học kín hệ thống: 𝑀(𝑞)𝑞̈ + 𝑉(𝑞, 𝑞̇ ) + 𝐺(𝑞) = 𝑀(𝑞)(𝑞̈ 𝑑 + 𝐾𝑝 𝑒 + 𝐾𝑑 𝑒̇ ) + 𝑉(𝑞, 𝑞̇ ) + 𝐺(𝑞)  𝑀(𝑞)(𝑒̈ + 𝐾𝑝 𝑒 + 𝐾𝑑 𝑒̇ ) = Do 𝑀(𝑞) xác định dương, ta thu phương trình vi phân bậc hai sai lệch: 𝑒̈ + 𝐾𝑝 𝑒 + 𝐾𝑑 𝑒̇ = 27 Phương trình đặc tính: 𝑠 + 𝐾𝑑 𝑠 + 𝐾𝑝 = Có: ∆= 𝐾𝑑2 − 4𝐾𝑝 Trường hợp 1: ∆> 𝑠1 ≠ 𝑠2 𝑒(𝑡) = 𝐶1 𝑒 𝑠1𝑡 + 𝐶2 𝑒 𝑠2𝑡 Trường hợp 2: ∆= 𝐾 𝑠1 = 𝑠2 = − 𝑑 𝐾𝑑 𝑒(𝑡) = (𝐶1 + 𝐶2 𝑡)𝑒 − 𝑡 Trường hợp 3: ∆< 𝑠1,2 = 𝜆 ± 𝜇𝑖 𝑒(𝑡) = 𝐶1 𝑒 𝜆𝑡 𝑐𝑜𝑠(𝜇𝑡) + 𝐶2 𝑒 𝜆𝑡 𝑠𝑖𝑛(𝜇𝑡) Như ta ln có: lim 𝑒 = → lim 𝑞 = 𝑞𝑑 𝑡→∞ 𝑡→∞ Thông sô tối ưu ta chọn là: 𝐾𝑑2 − 4𝐾𝑝 = Nhận xét: - Khi chọn ma trận đường chéo xác định dương 𝐾𝑝 , 𝐾𝑑 phù hợp cho ta kết sai số bám quỹ đạo 28 - Nhưng nhược điểm ta cần phải biết xác thơng số động học, động lực học, ước lượng khơng xác dẫn tới tồn sai lệch; khối lượng tính tốn lớn, thời gian đáp ứng chậm Áp dụng vào robot Scara Điều khiển không gian khớp: Các thông số robot Scara là: Ký hiệu 𝑚1 𝑚2 Bảng: thông số robot Scara Giá trị Ý nghĩa kg Khối lượng nối 1.5 kg Khối lượng nối 𝑚3 kg Khối lượng nối 𝑚4 𝑙1 (𝑙1 = 2𝑎1 ) 0.6 kg 0.25 kg Khối lượng nối Chiều dài nối 𝑙2 (𝑙2 = 2𝑎2 ) 0.15 kg Chiều dài nối • Giả sử ta điều khiển cho robot chạy từ vị trí đầu 𝑞𝑡 = [0 0 0] chuyển động theo quỹ đạo bậc tới vị trí 𝑞𝑡 = [40 30 0.5 40] • Với phương pháp điều khiển phi tuyến dựa mỗ hình, sau chỉnh thống số 𝐾𝑝 𝐾𝑑 ta kết sau: 30 0 225 0 0 35 0 306.25 0 𝐾𝑝 = ( ) ; 𝑣à 𝐾𝑝 = ( ) 0 10 0 25 0 0 10 0 25 Ta có kết mơ sau: 29 Hình Đáp ứng khớp quay Hình Đáp ứng khớp quay 30 Hình 10 Đáp ứng khớp tịnh tiến Hình 11 Đáp ứng khớp quay 31