BÀI TẬP LỚN KỸ THUẬT ROBOT: TÌM HIỂU ROBOT MPL800II

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BỘ MÔN TỰ DỘNG HÓA CÔNG NGHIỆP  BÀI TẬP LỚN MÔN HỌC KỸ THUẬT ROBOT Đề tài TÌM HIỂU ROBOT MPL800II GVHD PGS TS Nguyễn Phạm Thục Anh Nhóm 1 Lớp 129135 HÀ NỘI 12022 2 MỤC LỤC PHÂN CÔNG NHIỆM VỤ 3 DANH MỤC HÌNH VẼ 4 CHƯƠNG 1 GIỚI THIỆU ROBOT MPL800 II 5 1 1 Tổng quan về robot 5 1 2 Giới thiệu 6 1 3 Ứng dụng của Robot MPL800 II trong công nghiệp 8 1 4 Kết cấu cơ khí 8 1 5 Thông số kỹ thuật 10 CHƯƠNG 2 ĐỘNG HỌC THUẬN VỊ TRÍ CỦA ROBOT MPL800 II 12 2 1 Tín.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BỘ MƠN TỰ DỘNG HĨA CƠNG NGHIỆP - - BÀI TẬP LỚN MÔN HỌC KỸ THUẬT ROBOT Đề tài: TÌM HIỂU ROBOT MPL800II GVHD: PGS.TS Nguyễn Phạm Thục Anh Nhóm: Lớp: 129135 HÀ NỘI 1/2022 MỤC LỤC PHÂN CÔNG NHIỆM VỤ DANH MỤC HÌNH VẼ .4 CHƯƠNG GIỚI THIỆU ROBOT MPL800 II .5 1.1 Tổng quan robot: 1.2 Giới thiệu .6 1.3 Ứng dụng Robot MPL800 II công nghiệp 1.4 Kết cấu khí: .8 1.5 Thông số kỹ thuật 10 CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ CỦA ROBOT MPL800 II .12 2.1 Tính tốn cơng thức 12 2.2 Thiết kế giao diện Matlab 14 CHƯƠNG MA TRẬN JACOBY 15 3.1 Tính tốn cơng thức 15 3.2 Lập trình tính tốn phần mềm Matlab 17 CHƯƠNG ĐỘNG HỌC NGƯỢC ROBOT 18 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO CÁC KHỚP CỦA ROBOT THEO QUY ĐẠO DẠNG BẬC .21 CHƯƠNG THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO ROBOT MPL800 II THEO THUẬNT TOÁN PID 29 6.1 Tính tốn động lực học cho Robot 29 6.2 Thuật toán điều khiển Robot .32 6.3 Mô Simcape 34 PHÂN CÔNG NHIỆM VỤ STT Nhiệm vụ Thành viên MSSV Giới thiệu Robot nhóm nghiên cứu, ứng dụng cơng nghiệp, kết cấu khí, thơng số kỹ thuật Nguyễn Đắc Quang 20174132 Tính tốn động học thuận vị trí Robot, chương trình MatLab để nhập liệu, hiển thị kết Đồng Quang Vinh 20209507 Tính tốn ma trận Jacoby (thơng qua JH), viêt chương trình MatLab tính ma trận Jacobi Nguyễn Hữu Thắng 20209506 Tính tốn động học đảo vị trí Robot Hà Quang Huy 20181523 Bùi Văn Long 20181587 Thái Văn Hóa 20181479 Nguyễn Văn Đức 20181410 Trương Văn Quý 20181718 Vũ Đình Đức 20181416 Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc Thiết kế điều khiển chuyển động cho Robot theo thuật tốn PID, xây dựng mơ hình động lực học cho đối tượng DANH MỤC HÌNH VẼ Hình 1.1 Robot nhà máy .5 Hình 1.2: Một số loại Robot hãng Yaskawa Hình 1.3: Robot MPL800 II .7 Hình 1.4: Ứng dụng Robot MPL800 II cơng nghiệp Hình 1.5 Robot MPL làm việc xưởng Hình 1.6: Robot MPL800 II nhìn từ xuống Hình 1.7: Robot MPL800 II nhìn từ đằng sau Hình 1.8: Robot MPL800 II nhìn ngang 10 Hình 2.1 Mơ hình robot MPL800 II 12 Hình 3.1 Chương trình matlab tính ma trận jacoby .17 Hình 5.1 Mơ khớp 22 Hình 5.2 Mô khớp 23 Hình 5.3 Mơ khớp 24 Hình 5.4 Mơ khớp 25 Hình 6.1 Giao diện cài đặt thơng số 33 Hình 6.2 Kết mơ tọa độ tay máy theo trục 34 Hình 6.3 Mơ hình 3D chân đế 34 Hình 6.4 Khâu thứ 35 Hình 6.5 Khâu thứ hai 35 Hình 6.6 Khâu thứ ba 36 Hình 6.7 Khâu tác động cuối 36 Hình 6.8 Mơ hình hồn chỉnh robot 37 Hình 6.9 Chuyển động mô 37 Hình 6.10 Chuyển động mô 38 Hình 6.11 Chuyển động mơ 38 CHƯƠNG GIỚI THIỆU ROBOT MPL800 II 1.1 Tổng quan robot: Robot ( tiếng Anh: Robot ), "Người máy" loại máy thực công việc cách tự động điều khiển máy tính vi mạch điện tử lập trình Robot tác nhân khí, nhân tạo, ảo, thường hệ thống khí-điện tử Hình 1.1 Robot nhà máy Robot cơng nghiệp chủ yếu sử dụng nhà máy với mục đích sản xuất sản phẩm với yêu cầu xác cao, làm việc liên tục với suất không đổi Robot đem lại cho doanh nghiệp sản phẩm chất lượng cao, bị lỗi việc sản xuất liên tục làm cho sản lượng sản phẩm tăng lên để đáp ứng nhu cầu người tiêu dùng Phân loại robot công nghiệp: Bảng 1.1 Phân loại robot công nghiệp Dấu hiệu phân loại Tên gọi tay máy Theo số bậc chuyển động ( kể chuyển động rời tay máy) Có hai nhiều dạng : • Khơng di chuyển • Tự chuyển dời Theo số lượng tay máy Một, hai nhiều tay máy điều khiển động thời • Có nguồn dẫn động điều khiển riêng • Có nguồn dẫn động riêng điều khiển chung • Có chung nguồn dẫn động • Tự di chuyển Theo tải trọng nâng tay máy • • • • • Loại siêu nhẹ Loại nhẹ Loại trung Loại nặng Loại siêu nặng Theo nguồn dẫn động cấu chấp hành • • • • Khí nén Thủy lực Cơ điện Hỗn hợp Với điều khiển chương trình • Theo chu kỳ • Theo vị trí • Theo chu vi • Hỗn hợp Với điều khiển theo cảm nhận • Điều khiển khơng thích nghi • Điều khiển thích nghi Với trí tuệ nhân tạo Theo hệ thống điều khiển theo nguyên lý điều khiển • Điều khiển riêng rẽ • Điều khiển theo nhóm Theo số robot điều khiển đồng thời Theo độ xác • Gồm mức xác : 0, 1, 2, Theo kiểu bảo hiểm • • • • Kiểu thơng thường Kiểu phịng bụi Kiểu phịng nhiệt Kiểu phòng nổ 1.2 Giới thiệu Robot MPL800 II sản xuất hãng Robot tiếng Yaskawa Yaskawa tập đoàn hàng đầu giới sản xuất cung cấp sản phẩm lĩnh vực robot công nghiệp, biến tần, truyền động điện…Trong lĩnh vực robot công nghiệp, hãng sản xuất tất loại robot như: robot gắp (Handling), robot nâng bốc, đóng gói (Picking/packing, palletizing) Robot hàn, hàn điểm (Arc handling, spot welding), Robot sơn (Painting), Robot lắp ráp ( Assembly/distributing)… Hình 1.2: Một số loại Robot hãng Yaskawa Hình 1.3: Robot MPL800 II Robot thực công đoạn sản xuất thay cho người với tốc độ di chuyển nhanh xác Ứng dụng nhiều lĩnh vực khác lắp ráp tơ xe máy, đóng tàu, sắt thép, chế biến thực phẩm, điện tử bán dẫn, Độ tin cậy, tốc độ nhanh hiệu suất cao vài số đặc điểm đáng kinh ngạc robot MPL800 II Đây robot trục, hồn hảo cho việc xếp dỡ hàng hóa chọn đơn hàng với thiết kế hướng đến hiệu suất nó, dễ dàng xử lý tải khơng cân có mơmen qn tính cao cho phép MPL800 II có tầm với chiều dọc rộng 3.024 mm tầm với chiều ngang 3.159 mm mang lại cho robot khả xây dựng tải trọng cao 102 inch pallet tiêu chuẩn 48 inch x 40 inch Ngồi ra, MPL800 II có cổ tay rỗng phép thực nhiều chuyển động cổ tay mà không gặp can thiệp Cổ tay rỗng cho phép hãng hàng không cáp định tuyến từ đế đến EOAT, giúp nâng cao độ an toàn tuổi thọ cáp MPL800 II ghép nối với điều khiển DX200.( DX200 điều khiển trục đa chức năng, kết hợp hệ thống nhiều băng tải vị trí đặt pallet ) 1.3 Ứng dụng Robot MPL800 II công nghiệp Đây loại robot phù hợp với ứng dụng đóng gói (điều khiển dỡ đóng gói vào khay, hộp, thùng carton, túi, …) Robot MPL800II áp dụng rộng rãi nhiều ngành công nghiệp như: thực phẩm nước giải khát, nhà kho sản phẩm cơng nghiệp khác Tuy nhiên robot MPL800II dịng robot chuyên dụng, thích hợp ưu tiên dùng cho ứng dụng nâng bốc, di chuyển hàng có khối lượng lớn lên đến 800kg, sử dụng nhiều ngành như: nước giải khát, thực phẩm, gạch, xi măng…do đặc điểm chuyên dụng robot MPL Hình 1.4: Ứng dụng Robot MPL800 II công nghiệp 1.4 Kết cấu khí: Hình 1.5 Robot MPL làm việc xưởng Kết cấu khí robot thể hình vẽ: - Kết cấu khí robot nhìn từ xuống: Hình 1.6: Robot MPL800 II nhìn từ xuống - Kết cấu khí Rotbot MPL800 II nhìn từ đằng sau: Hình 1.7: Robot MPL800 II nhìn từ đằng sau Kết cấu khí robot nhìn ngang: Hình 1.8: Robot MPL800 II nhìn ngang 1.5 Thơng số kỹ thuật Các thơng số kỹ thuật Robot MPL800 II trình bày bảng sau: Bảng 1.2: Bảng thơng số kỹ thuật Robot MPL800 II Mẫu Kiểu Trục điều khiển Tải trọng nâng Khả lặp lại Phạm vi chuyển động Tốc độ tối đa Quán tính cho phép Khối lượng Trục S – quay Trục L – cánh tay Trục U – cánh tay Trục T – cổ tay Trục S – quay Trục L – cánh tay Trục U – cánh tay Trục T – cổ tay Trục T – cổ tay Nhiệt độ Độ ẩm Điều kiện môi trường Độ rung Khác 10 MOTOMAN-MPL800 II YR-MPL0800-J00 khớp nối theo thiều dọc 800kg ±0.5mm -180˚ – +180˚ -45˚ – +90˚ -120˚ – +15˚ -360˚ – +360˚ 1.13 rad/s, 65˚/s 1.13 rad/s, 65˚/s 1.13 rad/s, 65˚/s 2.18 rad/s, 125˚/s 500kg৹m2 2550kg 0˚C đến +45˚C 20 - 80% RH (khơng ngưng tụ) 4.9m/𝑠2 Khơng có khí chất lỏng ăn mịn, khí gây nổ − Phương trình quỹ đạo khớp với hệ số sau: a30 =3 ( A) = a31 = 3 a32 = (3 ( B) − 3 ( A)) = (−45 − 0) = −5.4 t 2 a33 = − (3 ( B) − 3 ( A)) = − ( −45 − 0) = 0.72 t Ta có phương trình quỹ đạo khớp 3: 3 (t ) = −5.4t + 0.72t (5.15) (5.16) Phương trình vận tốc: 3 (t ) = −10.8t + 2.16t (5.17) Phương trình gia tốc: 3 (t ) = −10.8 + 4.32t Kết mơ Matlab: Hình 5.3 Mơ khớp 24 (5.18) − Phương trình quỹ đạo khớp với hệ số sau: a40 = ( A) = 30 a41 = 3 a42 = ( ( B) −  ( A)) = (−60 − 30) = −10.8 t 2 a43 = − ( ( B) −  ( A)) = − (−60 − 30) = 1.44 t Ta có phương trình quỹ đạo khớp 4: 4 (t ) = 30 −10.8t + 1.44t (5.19) (5.20) Phương trình vận tốc: 4 (t ) = −21.6t + 4.32t (5.21)  (t ) = −21.6 + 8.64t (5.22) Phương trình gia tốc: Kết mơ Matlab: Hình 5.4 Mơ khớp 25 Chương trình thực matlab sau %thiet ke quy dao bac cho cac khop robor MPL 800II figure(1); %quy dao vi tri khop x=[ :0.5:5]; y1=5.4*x.^2-0.72*x.^3; subplot(3,1,1); plot(x,y1); title('do thi vi tri khop 1'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao van toc khop y2=10.8*x-2.16*x.^2; subplot(3,1,2); plot(x,y2); title('do thi van toc khop 1'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao gia toc khop y3=10.8-4.32*x; subplot(3,1,3); plot(x,y3); title('do thi gia toc khop 1'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao vi tri khop figure(2); y4=60-3.6*x.^2+0.48*x.^3; subplot(3,1,1); plot(x,y4); title('do thi vi tri khop 2'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao van toc khop y5=-7.2*x+1.44*x.^2; subplot(3,1,2); plot(x,y5); title('do thi van toc khop 2'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao gia toc khop y6=-7.2+2.88*x; subplot(3,1,3); plot(x,y6); 26 title('do thi gia toc khop 2'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao vi trí khop figure(3); y7=-5.4*x.^2+0.72*x.^3; subplot(3,1,1); plot(x,y7); title('do thi vi tri khop 3'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao van toc khop y8=-10.8*x+2.16*x.^2; subplot(3,1,2); plot(x,y8); title('do thi van toc khop 3'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao gia toc khop y9=-10.8+4.32*x; subplot(3,1,3); plot(x,y9); title('do thi gia toc khop 3'); xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao vi tri khop figure(4); y10=30-10.8*x.^2+1.44*x.^3; subplot(3,1,1); plot(x,y10); title('do thi vi tri khop 4') xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao van toc khop y11=-21.6*x+4.32*x.^2; subplot(3,1,2); plot(x,y11); title('do thi van toc khop 4') xlabel('thoi gian(s)'); ylabel('bien do'); grid on; %quy dao gia toc khop y12=-21.6+8.64*x; subplot(3,1,3); 27 plot(x,y12); title('do thi gia toc khop 4') xlabel('thoi gian(s)'); ylabel('bien do'); grid on; 28 CHƯƠNG THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO ROBOT MPL800 II THEO THUẬNT TỐN PID 6.1 Tính tốn động lực học cho Robot Hàm Lagrange: L=K-P Trong đó: K: tổng động hệ thống P: Là tổng hệ thống Từ hàm Lagrange xác định biểu thức sau: Nếu i khớp quay (𝜃𝑖 ), momen cần đặt lên khớp i là: 𝑇𝑖 = 𝑑 𝜕𝐿 𝜕𝐿 ( )− 𝑑𝑡 𝜕𝜃𝑖̇ 𝜕𝜃𝑖 (6.1) 𝐹𝑖 = 𝑑 𝜕𝐿 𝜕𝐿 ( )− 𝑑𝑡 𝜕𝑟𝑖 𝜕𝑟𝑖 (6.2) Nếu i khớp trượt (𝑟𝑖 ) Động khớp nối thứ i: 1 𝑚𝑖 𝑣𝑖 + 𝐽𝑖 𝜔𝑖 2 Với Ji momen quán tính khớp nối thứ i 𝐾𝑖 = Tham số Chiều dài Khối lượng Vận tốc Vị trí tâm khối Tọa độ nối 1: Thanh nối 2L1 M1 V1 L1 Thanh nối 2L2 M2 V2 L2 𝑥𝐶1 = 𝐿1 𝐶𝑜𝑠( 𝜃1 ) {𝑦𝐶1 = 𝐿1 𝑆𝑖𝑛( 𝜃1 ) 𝑧𝑐1 = 𝑑 (6.3) Thanh nối 2L3 M3 V3 L3 (6.4) Tốc độ nối 1: • • 𝑥𝑐1 = −𝐿1 𝑆𝑖𝑛( 𝜃1 )𝜃1 • • 𝑦𝑐1 = 𝐿1 𝐶𝑜𝑠( 𝜃1 )𝜃1 (6.5) • {𝑧𝑐1 = Tọa độ nối 2: 𝑥𝑐2 = (2𝐿1 + 𝐿2 𝐶𝑜𝑠( 𝜃2 )) 𝐶𝑜𝑠( 𝜃2 ) {𝑦𝑐2 = (2𝐿1 + 𝐿2 𝐶𝑜𝑠( 𝜃2 )) 𝑆𝑖𝑛( 𝜃2 ) 𝑧𝑐2 = 𝑑 + 𝐿2 𝑆𝑖𝑛( 𝜃2 ) 29 (6.6) Vận tốc nối 2: • • • 𝑥𝑐2 = −(2𝐿1 + 𝐿2 𝐶𝑜𝑠( 𝜃2 )) 𝑆𝑖𝑛( 𝜃1 )𝜃1 − 𝐿2 𝜃2 𝑆𝑖𝑛( 𝜃2 ) 𝐶𝑜𝑠( 𝜃1 ) • • • 𝑦𝑐2 = (2𝐿1 + 𝐿2 𝐶𝑜𝑠( 𝜃2 )) 𝐶𝑜𝑠( 𝜃1 )𝜃1 − 𝐿2 𝜃2 𝑆𝑖𝑛( 𝜃2 ) 𝑆𝑖𝑛( 𝜃1 ) (6.7) • • {𝑧𝑐2 = 𝐿2 𝐶𝑜𝑠( 𝜃2 )𝜃2 Thanh nối 3: 𝑥𝑐3 = (2𝐿1 + 2𝐿2 𝐶𝑜𝑠( 𝜃2 ) + 𝐿3 𝐶𝑜𝑠( 𝜃2 + 𝜃3 )) 𝐶𝑜𝑠( 𝜃1 ) {𝑦𝑐3 = (2𝐿1 + 2𝐿2 𝐶𝑜𝑠( 𝜃2 ) + 𝐿3 𝐶𝑜𝑠( 𝜃2 + 𝜃3 )) 𝑆𝑖𝑛( 𝜃1 ) 𝑧𝑐3 = 𝑑 + 𝐿2 𝑆𝑖𝑛( 𝜃2 ) + 𝐿3 𝑆𝑖𝑛( 𝜃2 + 𝜃3 ) (6.8) Vận tốc nối • • • • 𝑥𝑐3 = (−2𝐿2 𝑆𝑖𝑛( 𝜃2 )𝜃2 − 𝐿3 𝑆𝑖𝑛( 𝜃2 + 𝜃3 )𝜃2 − 𝐿3 𝑆𝑖𝑛( 𝜃2 + 𝜃3 )𝜃3 ) 𝐶𝑜𝑠( 𝜃1 ) • −(2𝐿1 + 2𝐿2 𝐶𝑜𝑠( 𝜃2 ) + 𝐿3 𝐶𝑜𝑠(𝜃2 + 𝜃3 )) 𝑆𝑖𝑛( 𝜃1 )𝜃1 • • 𝑦𝑐3 = −(2𝐿1 + 2𝐿2 𝐶𝑜𝑠( 𝜃2 ) + 𝐿3 𝐶𝑜𝑠( 𝜃3 )) 𝐶𝑜𝑠( 𝜃1 )𝜃1 + • • (6.9) • (−2𝐿2 𝑆𝑖𝑛( 𝜃2 )𝜃2 − 𝐿3 𝑆𝑖𝑛( 𝜃2 + 𝜃3 )𝜃2 − 𝐿3 𝑆𝑖𝑛( 𝜃2 + 𝜃3 )𝜃3 ) 𝑆𝑖𝑛( 𝜃1 ) • • • • {𝑧𝑐3 = 𝐿2 𝐶𝑜𝑠( 𝜃2 )𝜃2 + 𝐿3 𝐶𝑜𝑠( 𝜃2 + 𝜃3 )(𝜃2 + 𝜃3 ) Khai báo vị trí Matlab: %%vitri V1=[2*L1*cos(th1);2*L1*sin(th1);d] V2=[(2*L1+L2*cos(th2))*cos(th1);(2*L1+L2*cos(th2))*sin(th1 );L2*sin(th2)+d] V3=[(2*L1+2*L2*cos(th2)+L3*cos(th3))*cos(th1);(2*L1+2*L2*c os(th2)+L3*cos(th3))*sin(th1);2*L2*sin(th2)+L3*sin(th2+th3 )+d] Khai báo tốc độ Matlab: %%speed syms th1 L1 thc1 Pc1=[-L1*sin(th1)*thc1 L1*cos(th1)*thc1 0] Pc1=simplify(Pc1); syms L2 th2 thc2 Pc2=[(-L2*thc2*sin(th2))*cos(th1)(2*L2+L2*cos(th2))*sin(th1)*thc1 (L2*sin(th2)*thc2)*sin(th1)+(2*L1+L2*cos(th2))*thc1*cos(th1 ) L2*cos(th2)*thc2] Pc2=simplify(Pc2); syms L3 th3 thc3 Pc3=[(-2*L2*thc2*sin(th2)-L3*thc3*sin(th3))*cos(th1)(2*L1+2*L2*cos(th2)+L3*cos(th3))*sin(th1)*thc1 (30 2*L2*thc2*sin(th2)L3*thc3*sin(th3))*sin(th1)+(2*L1+2*L2*cos(th2)+L3*cos(th3) )*cos(th1)*thc1 2*L2*thc2*cos(th2)+L3*(thc2+thc3)*cos(th2+th3)] Pc3=simplify(Pc3); Do khớp khớp quay, Momen cần đặt lên khớp là: 𝑀𝑖 = 𝑑 𝜕𝐿 𝜕𝐿 ( )− 𝑑𝑡 𝜕𝜃𝑖̇ 𝜕𝜃𝑖 (6.10) • Khai báo 𝜃𝑖 Matlab thc1 Do thành phần ( 𝜕𝐿 𝜕𝜃𝑖 𝜕𝐿 ) 𝜕𝜃𝑖̇ tính tốn hàm: dldthci=diff(L,thci) tính tốn hàm: dldthi=diff(L,thi) %%momen dat len cac khop %tinh dao ham lagrange theo cac bien dldthc1=diff(L,thc1) dldthc2=diff(L,thc2) dldthc3=diff(L,thc3) dldth1=diff(L,th1) dldth2=diff(L,th2) dldth3=diff(L,th3) Với thành phần 𝑑 𝑑𝑡 ( 𝜕𝐿 ) 𝜕𝜃𝑖̇ ta không lấy đạo hàm trực tiếp Matlab thành phần theta_i hàm theo thời gian Do ta quy ước th2c_i đạo hàm cấp theo thời gian thành phần thc_i Khi thành phần 𝑑 𝑑𝑡 ( 𝜕𝐿 ) 𝜕𝜃𝑖̇ tính cách nhân đạo hàm riêng theo biến nhân với đạo hàm biến %%momen dat len cac khop( deu la khop quay) syms th2c1 th2c2 th2c3 %%dao ham cap hai cua theta_i M1=diff(dldthc1,th1)*thc1+diff(dldthc1,th2)*thc2+diff(dldt hc1,th3)*thc3+diff(dldthc1,thc1)*th2c1+diff(dldthc1,thc2)* th2c2+diff(dldthc1,thc3)*th2c3-dldth1 M2=diff(dldthc2,th1)*thc1+diff(dldthc2,th2)*thc2+diff(dldt hc2,th3)*thc3+diff(dldthc2,thc1)*th2c1+diff(dldthc2,thc2)* th2c2+diff(dldthc2,thc3)*th2c3-dldth2 M3=diff(dldthc3,th1)*thc1+diff(dldthc3,th2)*thc2+diff(dldt hc3,th3)*thc3+diff(dldthc3,thc1)*th2c1+diff(dldthc3,thc2)* th2c2+diff(dldthc3,thc3)*th2c3-dldth3 Kết thu biểu thức momen đặt lên khớp: Do kết thu phức tạp, nên không đưa vào trình bày này, kết lưu m_file 31 6.2 Thuật tốn điều khiển Robot Nhóm em sử dụng điều khiển PD để điều khiển cho Robot M = H.(q̅).q̈ + V(q̅ , q̅̇) + G(g) Mđk = K P ɛ̅ + K D ɛ̅̇ (6.11) ɛ̅ = q ̅̅̅d − q̅ sai số vị trí khớp robot ɛ̅̇ = q ̅̅̅ḋ − q̅̇ sai số tốc độ khớp robot Trong K P ma trận đường chéo hệ số khuếch đại { K D ma trận đường chéo hệ số đạo hàm Nhóm em tính toán lựa chọn ma trận Kp Kd để điều khiển Robot là: 90 0 2025 0 KP = [ 90 0] ; KD = [ 2025 ] 0 90 0 2025 Triển khai thuật toán Matlab Matlab funtion, phần quan trọng thiết lập cơng thức tính tốn mô men đặt lên trục robot E = W - Q; E_dot = W_dot - Q_dot; K1 = [90 0; 90 0; 0 90]; K2 = [2025 0; 2025 0; 0 2025]; U = M*(W_2dot + K1*E + K2*E_dot) + C*Q_dot + G; M1 = U(1); M2 = U(2); M3 = U(3); Xây dựng giao diện để cài đặt thơng số vị trí mà tay máy muốn tiếp cận, sau tính tốn đưa mơ men thích hợp để điều khiển tay máy đến vị trí đó: 32 Hình 6.1 Giao diện cài đặt thông số Tiến hành mô ta thu kết : 33 Hình 6.2 Kết mơ tọa độ tay máy theo trục Nhận xét: Các tay máy robot di chuyển đến bám theo giá trị đặt, thời gian xác lập nhanh ổn định khơng có sai lệch tĩnh Kết thể phương pháp tính tốn động lực học xây dựng mơ hình điều khiển đạt u cầu kĩ thuật đề ra, robot vận hành theo tính t 6.3 Mô Simcape Sau xây dựng xong phương trình động lực học xác định thơng số điều khiển, nhóm em tiến hành mơ hoạt động robot phần mềm MATLAB Những hình ảnh mơ hình 3D nối robot mơ hình hồn chỉnh Hình 6.3 Mơ hình 3D chân đế 34 Hình 6.4 Khâu thứ Hình 6.5 Khâu thứ hai 35 Hình 6.6 Khâu thứ ba Hình 6.7 Khâu tác động cuối 36 Hình 6.8 Mơ hình hồn chỉnh robot Chuyển động robot mơ : Hình 6.9 Chuyển động mơ 37 Hình 6.10 Chuyển động mơ Hình 6.11 Chuyển động mơ 38 ...