Đồ án môn học bài tập dài robot yaskawa ep4000d

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI ĐỒ ÁN MÔN HỌC BÀI TẬP DÀI : ROBOT YASKAWA EP4000D Giảng viên hướng dẫn: Bộ môn: Viện: PGS TS Nguyễn Phạm Thục Anh Tự động hóa cơng nghiệp Điện Nhóm: Thành viên: Nguyễn Thế Thiết - 20181767 Quách Cao Hiển - 20181465 Nguyễn Công Minh - 20181650 Bùi Trung Nghĩa - 20181672 Nguyễn Đình Chí - 20181350 Đào Cơng Đồn - 20181393 Trần Minh Hoàng - 20181496 Đỗ Tuấn Quang - 20181710 Vũ Mạnh Hùng - 20181513 Nguyễn Sách Dương - 20181439 HÀ NỘI, 1/2022 Lời mở đầu Trong năm gần phát triển mãnh mẽ lĩnh vực khoa học, ứng dụng chúng ngành công nghiệp, điện tử nhằm nâng cao suất chất lượng sản phẩm Đặc biệt, robot công nghiệp phận cấu thành thiếu việc tạo hệ thống tự động sản xuất có tính linh hoạt cao thị trường hàng hóa cạnh tranh Gần nửa kỷ có mặt sản xuất, robot cơng nghiệp có lịch sử phát triển hấp dẫn Ngày robot công nghiệp dùng rộng rãi nhiều lĩnh vực sản xuất Điều xuất phát từ ưu điểm loại robot chọn lựa đúc kết lại qua năm ứng dụng nhiều nước Với đề tài “Tìm hiểu robot yaskawa ep4000d”, nhóm em tìm hiểu vấn đề cấu trúc cấu động học, động lực học, lập trình quỹ đạo, điều khiển chuyển động,… Các vấn đề đề cập chương : Chương I: Giới thiệu chung robot yaskawa ep4000d Chương II: Động học thuận vị trí robot Chương III: Ma trận Jacoby Chương IV: Động học đảo vị trí robot Chương IV: Thiết kế quỹ đạo chuyển động khớp Chương IV: Mơ hình động lực học đối tượng toolbox simscape/matlab Qua việc thiết kế đồ án mơn học, giúp nhóm em hiểu rõ gỡ mắc q trình học mơn Kỹ thuật Robot Hiểu ứng dụng thực tế robot công nghiệp Tuy nhiên nội dung mẻ, nguồn tài liệu ỏi với tầm hiểu biết cịn hạn chế nên khơng tránh khỏi sai sót Em xin chân thành cảm ơn Nguyễn Phạm Thục Anh giảng dạy giúp đỡ chúng em hồn thành đồ án mơn học MỤC LỤC CHƯƠNG GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D 1.1 Giới thiệu Robot Yaskawa ep4000d 1.2 Ứng dụng Robot công nghiệp 1.3 Kết cấu khí, thơng số kĩ thuật 1.4 1.3.1 Kết cấu khí 1.3.2 Thông số kĩ thuật 1.3.3 Bộ điều khiển Robot Hình ảnh hoạt động 10 CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT 11 2.1 Tính tốn động học thuận 11 2.2 Xây dựng phần mềm giao diện động học thuận vị trí 13 CHƯƠNG MA TRẬN JACOBY 15 3.1 Tính tốn ma trận Jacoby 15 3.2 Các bước tính tốn ma trận Jacoby sử dụng thuật toán ma trận JH 15 3.3 Chương trình tính tốn Matlab 17 3.4 Xây dựng phần mềm giao diện tính tốn Jacoby 18 CHƯƠNG ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT 19 4.1 Cơ sở lý thuyết 19 4.2 Tính động học đảo 19 4.3 Xây dựng giao diện nhập xuất liệu hiển thị kết 20 CHƯƠNG 5.1 THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CÁC KHỚP 21 Các toán thiết kế quỹ đạo 21 5.1.1 Chuyển động PTP (Point to Point) 21 5.1.2 Chuyển động thông qua số điểm trung gian 21 5.1.3 Chuyển động có ràng buộc tốc độ gia tốc 22 5.2 Thiết kế quỹ đạo chuyển động PTP 22 5.3 Tính tốn thiết kế quỹ đạo chuyển động 22 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE/MATLAB 25 6.1 Thiết kế robot Solid Work 25 6.2 Mơ hình đối tượng Matlab 27 KẾT LUẬN 29 TÀI LIỆU THAM KHẢO 30 PHỤ LỤC 31 Chi tiết xây dựng giao diện động học thuận 31 Chi tiết xây dựng giao diện ma trận Jacoby 34 Chi tiết xây dựng động học nghịch 37 Chi tiết xây dựng quỹ đạo bậc 38 Chi tiết xây dựng mơ hình đối tượng Matlab 39 Nhiệm vụ thành viên nhóm 44 DANH MỤC HÌNH VẼ Hình 1.1 Robot Yaskawa EP4000d Hình 1.2 Robot nhìn từ phía trước Hình 1.3 Robot nhìn từ xuống Hình 1.4 Mặt đáy robot Hình 1.5 Mặt cắt cấu cuối Hình 1.6 Kích thước vùng làm việc robot Hình 1.7 Robot làm việc công nghiệp 10 Hình 1.8 Hình ảnh robot gắp sản phẩm 10 Hình 2.1 Hệ trục tọa độ Robot 11 Hình 2.2 Giao diện menu 13 Hình 2.3 Giao diện động học thuận 14 Hình 3.1 Xác định ma trận Tin 15 Hình 3.2 Giao diện Jacoby 18 Hình 4.1 Giao diện động học đảo 20 Hình 5.1 Chuyển động PTP 21 Hình 5.2 Chuyển động thơng qua số điểm trung gian 21 Hình 5.3 Chuyển động có ràng buộc tốc độ gia tốc 22 Hình 5.4 Đồ thị khớp 24 Hình 6.1 Phần chân đế robot 25 Hình 6.2 Phần nối thứ 25 Hình 6.3 Phần nối thứ hai 26 Hình 6.4 Phần nối thứ ba 26 Hình 6.5 Cơ cấu tác động 27 Hình 6.6 Tổng hợp mơ hình robot 27 Hình 6.7.Mơ hình điều khiển 28 DANH MỤC BẢNG BIỂU Bảng 1.1 Thông số kỹ thuật Robot Bảng 1.2 Thông số điều khiển Bảng 2.1 Bảng thông số Denavit-Hartenberg 11 Bảng 0.1 Nhiệm vụ thành viên 44 CHƯƠNG GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D 1.1 Giới thiệu Robot Yaskawa ep4000d Robot EP4000D hãng YASKAWA Nhật Bản thiết kế chế tạo Hãng thành lập năm 1915 với lịch sử 100 năm, 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 Các robot EP4000D có trọng tải 200 kg, chiều ngang ngang 3,505 mm tầm với thẳng đứng 2,629 mm Thiết kế gọn nhẹ robot cho phép làm việc không gian chật hẹp cách kinh hoạt Đây loại robot có sáu trục quay vận hành khí nén kết hợp với động điện, giúp robot vận hành cách linh hoạt, di truyển dễ dàng dến vị trí đảm bảo độ xác robot cơng việc Hình 1.1 Robot Yaskawa EP4000d 1.2 Ứng dụng Robot công nghiệp Robot EP4000D phát triển để ứng dụng vào dây truyền sản xuất công nghiệp với phù hợp với nhiều công việc khác 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) Do việc sử dụng sản phẩm robot EP4000D Yaskawa đảm bảo tính tương thích, độ quán chất lượng dịch vụ cao việc tự động hóa máy móc, dây chuyền sản xuất Yaskawa cung cấp tát loại robot để giải toán đáp ứng yêu cầu khác khách hàng 1.3 Kết cấu khí, thông số kĩ thuật 1.3.1 Kết cấu khí Hình 1.2 Robot nhìn từ phía trước Hình 1.3 Robot nhìn từ xuống Hình 1.4 Mặt đáy robot Hình 1.5 Mặt cắt cấu cuối Hình 1.6 Kích thước vùng làm việc robot Hình ảnh bên cho ta thấy kích thước robot, vùng làm việc robot vùng có màu xanh 1.3.2 Thông số kĩ thuật Bảng 1.1 Thơng số kỹ thuật Robot THƠNG SỐ KỸ THUẬT CỦA ROBOT Cấu trúc Trục điều khiển Tải trọng Tầm dọc Tầm ngang Độ lặp lại Phạm vi chuyển động tối đa Trục S Trục L Trục U Trục R Trục B Trục T EP4000D Standard Press Robot Kiểu cánh tay nối dọc 200 kg (441 lbs.) 2,614 mm (103ꞌꞌ) 3,505 mm(138ꞌꞌ) ±0.5 mm(0.02ꞌꞌ) ±150o +25o/-122o +53o/-70o ±360o ±120o ±360o Trục S Trục L Trục U Trục R Trục B Trục T Tốc độ tối đa Khối lượng Phanh Điện tiêu thụ Momen cho phép Trục R Trục B Trục T Trục R Trục B Trục T* Lực quán tính cho phép I/O Lines Air Lines *Đáng giá trục T song song với sàn 90o/s 90o/s 90o/s 80o/s 80o/s 160o/s 3,100 kg Tất trục 22 kVA 1,274 N • m 2,156 N • m 0N•m 84.5 kg • m2 330 kg • m2 80 kg • m2 34 wires (0.5 mm2) – 3/8ꞌꞌ air lines 1.3.3 Bộ điều khiển Robot Robot dùng diều khiển DX100, giúp người vận hành dễ dàng lập trình điều khiển robot, dễ dàng kết nối robot với hệ thống dây chuyền sản xuất Dưới thông số kĩ thuật điều khiển Bảng 1.2 Thông số điều khiển THƠNG SỐ BỘ ĐIỀU KHIỂN DX100** Kích thước (mm) 1,200 (w) x 1,000 (h) x 650 (d) (47.2ꞌꞌ x 39.4ꞌꞌ x 25.6ꞌꞌ) Khối lượng Hệ thống làm mát Nhiệt độ môi trường xung quanh Độ ẩm tương đối Yêu cầu điện Digital I/O NPN - Tiêu chuẩn PNP - Tuỳ chọn Tối đa 250 kg (551.3 lbs.) Làm mát gián tiếp Trong trình hoạt động: 0o đến 45o C (32o đến 113o F) Trong trình vận chuyển lưu trữ: -10o đến 60o C ( 14o đến 140o F) Tối đa 90% không ngưng tụ pha 240/480/575 VAC mức 50/60 Hz I/O tiêu chuẩn: 40 đầu vào/40 đầu bao gồm 16 đầu vào/16 đầu hệ thống 24 đầu vào/24 đầu người dùng 32 đầu Transistor, đầu Relay Tối đa I/O (tuỳ chọn): 2,048 đầu vào/2,048 đầu Phản hồi vị trí Bộ nhớ chương trình Bằng mã hoá tuyệt đối JOB: 200,000 bước, 10,000 lần truy cập Tiêu chuẩn CIO Ladder: 15,000 bước Mở rộng: 20,000 bước TÀI LIỆU THAM KHẢO [1] Đào Văn Hiệp ( 2006), “Kỹ thuật Robot”, NXB KH&KT [2] Nguyễn Thiện Phúc (2006), “Robot cơng nghiệp”, NXB KH&KT [3] Lê Hồi Quốc (2001), “Nhập môn robot công nghiệp – Tập 1”, NXB KH&KT [4] Cameron Hughes (May 22, 2016), Tracey Hughes "Robot Programming: A Guide to Controlling Autonomous Robots, 1st Edition",Que Publishing [5] Peter Mckinnon (January 28, 2016),"Robotics: Everything You Need to Know About Robotics from Beginner to Expert", CreateSpace Independent Publishing Platform 30 PHỤ LỤC Chi tiết xây dựng giao diện động học thuận - Sử dụng matlab guide, ta xây dựng giao diện menu : - Nút “Động học thuận” mở giao diện tương ứng function dht_Callback(hObject, eventdata, handles) close donghocthuan - Nút “Ma trận Jacobi” mở giao diện tương ứng - function jcb_Callback(hObject, eventdata, handles) close JACOBY Nút “Động học nghịch” mở giao diện tương ứng function dhn_Callback(hObject, eventdata, handles) close Tinh_dong_hoc_nguoc - Xây dựng giao diện tính tốn động học thuận : 31 - Nút “Tính tốn” tính tốn dựa vào giá trị Theta nhập cho ma trận 0T6 function tinhtoan_Callback(hObject, eventdata, handles) % Lay du lieu theta i1 = str2num(get(handles.theta1,'string')); i2 = str2num(get(handles.theta2,'string')); i3 = str2num(get(handles.theta3,'string')); i4 = str2num(get(handles.theta4,'string')); i5 = str2num(get(handles.theta5,'string')); i6 = str2num(get(handles.theta6,'string')); % Doi tu sang rad i1 = i1*pi/180; i2 = i2*pi/180; i3 = i3*pi/180; i4 = i4*pi/180; i5 = i5*pi/180; i6 = i6*pi/180; % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; % Tinh toan nx=(sin(i6)*(cos(i4)*sin(i1) - sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))) cos(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) cos(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))))); ny=(cos(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + cos(i5)*(cos(i1)*sin(i4) - cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) sin(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) + cos(i1)*cos(i4))); nz=(cos(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) - sin(i2 + i3)*sin(i4)*sin(i6)); 32 ox=(sin(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) cos(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))) + cos(i6)*(cos(i4)*sin(i1) - sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))); oy=(sin(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + cos(i5)*(cos(i1)*sin(i4) - cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) cos(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) + cos(i1)*cos(i4))); oz=(- sin(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) - sin(i2 + i3)*cos(i6)*sin(i4)); ax=(cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + sin(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))); ay=(cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) - sin(i5)*(cos(i1)*sin(i4) cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))); az=(sin(i2 + i3)*cos(i4)*sin(i5) - cos(i2 + i3)*cos(i5)); px=(d5*(cos(i4)*sin(i1) - sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))) + a1*cos(i1) + d3*sin(i1) + a2*cos(i1)*sin(i2) + a3*cos(i1)*cos(i2)*cos(i3) a3*cos(i1)*sin(i2)*sin(i3)); py=(a1*sin(i1) - d3*cos(i1) - d5*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) + cos(i1)*cos(i4)) + a2*sin(i1)*sin(i2) + a3*cos(i2)*cos(i3)*sin(i1) a3*sin(i1)*sin(i2)*sin(i3)); pz=(d1 + a3*sin(i2 + i3) + a2*sin(i2) - d5*sin(i2 + i3)*sin(i4)); % Hien thi set(handles.nx,'string',nx); set(handles.ny,'string',ny); set(handles.nz,'string',nz); set(handles.ox,'string',ox); set(handles.oy,'string',oy); set(handles.oz,'string',oz); set(handles.ax,'string',ax); set(handles.ay,'string',ay); set(handles.az,'string',az); set(handles.px,'string',px); set(handles.py,'string',py); set(handles.pz,'string',pz); - Nút “Quay lại” quay lại menu function quaylai_Callback(hObject, eventdata, handles) close menu - Nút “Thoát” chương trình tính function thoat_Callback(hObject, eventdata, handles) close 33 Chi tiết xây dựng giao diện ma trận Jacoby - Sử dụng công cụ guide matlab, ta xây dựng giao diện: - Ta xây dựng hàm callback cho nút TINH TOAN thực chương trình đọc liệu từ bảng nhập liệu, sau hiển thị ma trận Jacobi function tinhtoan_Callback(hObject, eventdata, handles) i1 = str2num(get(handles.i1,'string')); i2 = str2num(get(handles.i2,'string')); i3 = str2num(get(handles.i3,'string')); i4 = str2num(get(handles.i4,'string')); i5 = str2num(get(handles.i5,'string')); i6 = str2num(get(handles.i6,'string')); % Doi tu rad sang i1 = i1*pi/180; i2 = i2*pi/180; i3 = i3*pi/180; i4 = i4*pi/180; i5 = i5*pi/180; i6 = i6*pi/180; % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; % Tinh toan va hien thi J11 =d3*cos(i1) - a1*sin(i1) + d5*cos(i1)*cos(i4) - a2*cos(i2)*sin(i1) a3*cos(i2)*cos(i3)*sin(i1) + a3*sin(i1)*sin(i2)*sin(i3) d5*cos(i2)*cos(i3)*sin(i1)*sin(i4) - d5*sin(i1)*sin(i2)*sin(i3)*sin(i4); J12 =-cos(i1)*(a3*sin(i2 + i3) - (d5*cos(i2 + i3 - i4))/2 + a2*sin(i2) + (d5*cos(i2 + i3 i4))/2); J13 =-cos(i2 + i3)*cos(i1)*(a3 - d5*sin(i4)); J14 =d5*cos(i1)*cos(i4)*sin(i2)*sin(i3) d5*sin(i1)*sin(i4) d5*cos(i1)*cos(i2)*cos(i3)*cos(i4); 34 + + - J15 =0; J16 =0; J21 =a1*cos(i1) + d3*sin(i1) + a2*cos(i1)*cos(i2) + d5*cos(i4)*sin(i1) + a3*cos(i1)*cos(i2)*cos(i3) a3*cos(i1)*sin(i2)*sin(i3) d5*cos(i1)*cos(i2)*cos(i3)*sin(i4) + d5*cos(i1)*sin(i2)*sin(i3)*sin(i4); J22 =-sin(i1)*(a3*sin(i2 + i3) - (d5*cos(i2 + i3 - i4))/2 + a2*sin(i2) + (d5*cos(i2 + i3 + i4))/2); J23 =-sin(i2 + i3)*sin(i1)*(a3 - d5*sin(i4)); J24 =d5*cos(i1)*sin(i4) d5*cos(i2)*cos(i3)*cos(i4)*sin(i1) + d5*cos(i4)*sin(i1)*sin(i2)*sin(i3); J25 =0; J26 =0; J31 =0; J32 =a2*cos(i2) + a3*cos(i2)*cos(i3) - a3*sin(i2)*sin(i3) - d5*cos(i2)*cos(i3)*sin(i4) + d5*sin(i2)*sin(i3)*sin(i4); J33 =cos(i2 + i3)*(a3 - d5*sin(i4)); J34 =-d5*cos(i2 + i3)*cos(i4); J35 =0; J36 =0; J41 =0; J42 =(cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6))*(sin(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) - cos(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3)))) + cos(i6)*(cos(i4)*sin(i1) - sin(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3)))) (cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4))*(cos(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) - cos(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3)))) - sin(i6)*(cos(i4)*sin(i1) - sin(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3)))) - (cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + sin(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3))))*(cos(i2 + i3)*cos(i5) - sin(i2 + i3)*cos(i4)*sin(i5)); J43 =sin(i1); J44 =sin(i2 + i3)*cos(i1); J45 =cos(i4)*sin(i1) - cos(i1)*cos(i2)*cos(i3)*sin(i4) + cos(i1)*sin(i2)*sin(i3)*sin(i4); J46 =cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + sin(i5)*(sin(i1)*sin(i4) + cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))); J51 =0; J52 =(sin(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + cos(i5)*(cos(i1)*sin(i4) - cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) cos(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) sin(i1)*sin(i2)*sin(i3)) + cos(i1)*cos(i4)))*(cos(i4)*cos(i6) cos(i5)*sin(i4)*sin(i6)) (cos(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + cos(i5)*(cos(i1)*sin(i4) - cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) + sin(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) sin(i1)*sin(i2)*sin(i3)) + cos(i1)*cos(i4)))*(cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4)) 35 (cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) - sin(i5)*(cos(i1)*sin(i4) cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))))*(cos(i2 + i3)*cos(i5) - sin(i2 + i3)*cos(i4)*sin(i5)); J53 =-cos(i1); J54 =sin(i2 + i3)*sin(i1); J55 =sin(i1)*sin(i2)*sin(i3)*sin(i4) - cos(i2)*cos(i3)*sin(i1)*sin(i4) - cos(i1)*cos(i4); J56 =cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) - sin(i5)*(cos(i1)*sin(i4) cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))); J61 =(cos(i2 + i3)*cos(i5) - sin(i2 + i3)*cos(i4)*sin(i5))^2 + (sin(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) + sin(i2 + i3)*cos(i6)*sin(i4))^2 + (cos(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) - sin(i2 + i3)*sin(i4)*sin(i6))^2; J62 =(cos(i2 + i3)*cos(i5) - sin(i2 + i3)*cos(i4)*sin(i5))^2 - (sin(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) + sin(i2 + i3)*cos(i6)*sin(i4))*(cos(i4)*cos(i6) cos(i5)*sin(i4)*sin(i6)) + (cos(i6)*(cos(i2 + i3)*sin(i5) + sin(i2 + i3)*cos(i4)*cos(i5)) sin(i2 + i3)*sin(i4)*sin(i6))*(cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4)); J63 =0; J64 =-cos(i2 + i3); J65 =-sin(i2 + i3)*sin(i4); J66 =sin(i2 + i3)*cos(i4)*sin(i5) - cos(i2 + i3)*cos(i5); J11 = set(handles.J11,'string',num2str(J11)); J12 = set(handles.J12,'string',num2str(J12)); J13 = set(handles.J13,'string',num2str(J13)); J14 = set(handles.J14,'string',num2str(J14)); J15 = set(handles.J15,'string',num2str(J15)); J16 = set(handles.J16,'string',num2str(J16)); J21 = set(handles.J21,'string',num2str(J21)); J22 = set(handles.J22,'string',num2str(J22)); J23 = set(handles.J23,'string',num2str(J23)); J24 = set(handles.J24,'string',num2str(J24)); J25 = set(handles.J25,'string',num2str(J25)); J26 = set(handles.J26,'string',num2str(J26)); J31 = set(handles.J31,'string',num2str(J31)); J32 = set(handles.J32,'string',num2str(J32)); J33 = set(handles.J33,'string',num2str(J33)); J34 = set(handles.J34,'string',num2str(J34)); J35 = set(handles.J35,'string',num2str(J35)); J36 = set(handles.J36,'string',num2str(J36)); J41 = set(handles.J41,'string',num2str(J41)); J42 = set(handles.J42,'string',num2str(J42)); J43 = set(handles.J43,'string',num2str(J43)); 36 J44 = set(handles.J44,'string',num2str(J44)); J45 = set(handles.J45,'string',num2str(J45)); J46 = set(handles.J46,'string',num2str(J46)); J51 = set(handles.J51,'string',num2str(J51)); J52 = set(handles.J52,'string',num2str(J52)); J53 = set(handles.J53,'string',num2str(J53)); J54 = set(handles.J54,'string',num2str(J54)); J55 = set(handles.J55,'string',num2str(J55)); J56 = set(handles.J56,'string',num2str(J56)); J61 = set(handles.J61,'string',num2str(J61)); J62 = set(handles.J62,'string',num2str(J62)); J63 = set(handles.J63,'string',num2str(J63)); J64 = set(handles.J64,'string',num2str(J64)); J65 = set(handles.J65,'string',num2str(J65)); J66 = set(handles.J66,'string',num2str(J66)); - Nút QUAY LAI có chức trở menu function quaylai_Callback(hObject, eventdata, handles) menu - Nút THỐT có chức đóng giao diện function thoat_Callback(hObject, eventdata, handles) close Chi tiết xây dựng động học nghịch - Sử dụng công cụ guide matlab, ta xây dựng giao diện: - Ta xây dựng hàm callback cho nút TINH TOAN thực chương trình đọc liệu từ bảng nhập liệu, sau hiển thị ma trận Jacobi function tinhtoan_Callback(hObject, eventdata, handles) % Khai bao a,d 37 a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; %nhap du lieu vao nx=str2num(get(handles.nx,'string')); ny=str2num(get(handles.ny,'string')); nz=str2num(get(handles.nz,'string')); ox=str2num(get(handles.ox,'string')); oy=str2num(get(handles.oy,'string')); oz=str2num(get(handles.oz,'string')); ax=str2num(get(handles.ax,'string')); ay=str2num(get(handles.ay,'string')); az=str2num(get(handles.az,'string')); px=str2num(get(handles.px,'string')); py=str2num(get(handles.py,'string')); pz=str2num(get(handles.pz,'string')); %tinh toan cac goc theta1=atan2(ox,-oy); theta2=atan2(((pz-d1-a3*(ax*cos(theta1)+ay*sin(theta1)))/a2),(px*cos(theta1)a1+py*sin(theta1)-a3*(nx*cos(theta1)+ny*sin(theta1)))/a2); theta3=atan2((cos(theta2)*nzsin(theta2)*(nx*cos(theta1)+ny*sin(theta1))),cos(theta2)*(nx*cos(theta1)+ny*sin(theta1) )+sin(theta2)*nz); theta2=theta2*180/pi; theta1=theta1*180/pi; theta3=theta3*180/pi; % Hien thi set(handles.theta1,'string',num2str(theta1)); set(handles.theta2,'string',num2str(theta2)); set(handles.theta3,'string',num2str(theta3)); - Nút QUAY LAI có chức trở menu quaylai_Callback(hObject, eventdata, handles) menu - Nút THỐT có chức đóng giao diện thoat_Callback(hObject, eventdata, handles) close Chi tiết xây dựng quỹ đạo bậc - Khai báo giá trị : % giá trị ban đầu global tf Q0 Qf index index=1; 38 % Thời gian chuyển động tf=5; % vị trí điểm đầu điểm cuối Q0=[0 10 45 75 120 20]; Qf=[45 80 15 90 50 140]; plot_data(handles); - Xây dựng hàm tính tốn vẽ đồ thị function plot_data(handles,index) global Q0 Qf dQ0 dQf tf index % ràng buộc tốc độ dQ0=zeros(1,6); dQf=zeros(1,6); % Tính ma trận A0 A1 A2 A3 A0=Q0; A1=dQ0; A2=3*(Qf-Q0)/tf^2-2*dQ0/tf-dQf/tf; A3=-2*(Qf-Q0)/tf^3+(dQf+dQ0)/tf^2; t=linspace(0,tf,500); Theta=A0(index)+A1(index)*t+A2(index)*t.^2+A3(index)*t.^3; dTheta=A1(index)+2*A2(index)*t+3*A3(index)*t.^2; ddTheta=2*A2(index)+6*A3(index)*t; % Vẽ đồ thị plot(handles.axes1,t,Theta) ylabel(handles.axes1,'\theta (deg)') title(handles.axes1,sprintf('Quỹ đạo khớp %d',index-1)) grid(handles.axes1,'on') plot(handles.axes2,t,dTheta) ylabel(handles.axes2,'\theta^* (deg/s)') title(handles.axes2,sprintf('Vận tốc khớp %d',index-1)) grid(handles.axes2,'on') plot(handles.axes3,t,ddTheta) ylabel(handles.axes3,'\theta^*^* (deg/s^2)') xlabel(handles.axes3,'Time (s)') title(handles.axes3,sprintf('Gia tốc khớp%d',index-1)) grid(handles.axes3,'on') Chi tiết xây dựng mơ hình đối tượng Matlab %Initialize the RigidTransform structure array by filling in null values smiData.RigidTransform(9).translation = [0.0 0.0 0.0]; smiData.RigidTransform(9).angle = 0.0; smiData.RigidTransform(9).axis = [0.0 0.0 0.0]; smiData.RigidTransform(9).ID = ''; 39 %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(1).translation = [4.2272484300902926 0.80708661417322913]; % in smiData.RigidTransform(1).angle = 3.1415926535897931; % rad smiData.RigidTransform(1).axis = [1 0]; smiData.RigidTransform(1).ID = 'B[taythree-1:-:tayfour-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(2).translation = [2.8751445668717679e-12 5.1996185135294581e-12 1.397637795277211]; % in smiData.RigidTransform(2).angle = 3.1415926535897931; % rad smiData.RigidTransform(2).axis = [1 -1.2214635910423068e-33 -1.1393591151404146e16]; smiData.RigidTransform(2).ID = 'F[taythree-1:-:tayfour-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(3).translation = [0 0.68897637795275657]; % in smiData.RigidTransform(3).angle = 3.1415926535897931; % rad smiData.RigidTransform(3).axis = [1 0]; smiData.RigidTransform(3).ID = 'B[taythree-1:-:taytwo-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(4).translation = [-3.5433070866142451 1.5640821970919205e12 0.68897637795167932]; % in smiData.RigidTransform(4).angle = 3.1415926535897931; % rad smiData.RigidTransform(4).axis = [1 1.1586755520335646e-32 2.1547161458580124e16]; smiData.RigidTransform(4).ID = 'F[taythree-1:-:taytwo-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(5).translation = [0 2.36220472440945]; % in smiData.RigidTransform(5).angle = 3.1415926535897931; % rad smiData.RigidTransform(5).axis = [1 0]; smiData.RigidTransform(5).ID = 'B[de-1:-:tayone-1]'; %Translation Method - Cartesian 40 %Rotation Method - Arbitrary Axis smiData.RigidTransform(6).translation = [0 -0.39370078740157588]; % in smiData.RigidTransform(6).angle = 0; % rad smiData.RigidTransform(6).axis = [0 0]; smiData.RigidTransform(6).ID = 'F[de-1:-:tayone-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(7).translation = [-1.9685039370078758 0.098425196850393248 -3.7401574803149598]; % in smiData.RigidTransform(7).angle = 2.0943951023931953; % rad smiData.RigidTransform(7).axis = [0.57735026918962584 -0.57735026918962584 0.57735026918962584]; smiData.RigidTransform(7).ID = 'B[tayone-1:-:taytwo-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(8).translation = [-1.8728985376470497e-13 2.5378310564150297e-13 0.098425196850345856]; % in smiData.RigidTransform(8).angle = 2.3065952118587559e-16; % rad smiData.RigidTransform(8).axis = [-0.50244165844541766 -0.86461111481325426 5.0101167369860097e-17]; smiData.RigidTransform(8).ID = 'F[tayone-1:-:taytwo-1]'; %Translation Method - Cartesian %Rotation Method - Arbitrary Axis smiData.RigidTransform(9).translation = [-11.239139210534688 4.9424387274427328 7.6897161625927009]; % in smiData.RigidTransform(9).angle = 0; % rad smiData.RigidTransform(9).axis = [0 0]; smiData.RigidTransform(9).ID = 'RootGround[de-1]'; %Initialize the Solid structure array by filling in null values smiData.Solid(5).mass = 0.0; smiData.Solid(5).CoM = [0.0 0.0 0.0]; smiData.Solid(5).MoI = [0.0 0.0 0.0]; smiData.Solid(5).PoI = [0.0 0.0 0.0]; smiData.Solid(5).color = [0.0 0.0 0.0]; smiData.Solid(5).opacity = 0.0; smiData.Solid(5).ID = ''; 41 %Inertia Type - Custom %Visual Properties - Simple smiData.Solid(1).mass = 0.56020544176941223; % kg smiData.Solid(1).CoM = [39.17428089009119 0.17333773938144786 20.341870483340973]; % mm smiData.Solid(1).MoI = [441.49417452783126 1473.7430436286691 1725.2316392064115]; % kg*mm^2 smiData.Solid(1).PoI = [-0.55094157322391524 48.188156344247609 7.9356531860245241]; % kg*mm^2 smiData.Solid(1).color = [0.792156862745098 0.81960784313725488 0.93333333333333335]; smiData.Solid(1).opacity = 1; smiData.Solid(1).ID = 'de*:*Default'; %Inertia Type - Custom %Visual Properties - Simple smiData.Solid(2).mass = 0.054160735625397402; % kg smiData.Solid(2).CoM = [56.936177173821669 1.5046119512817814e-07]; % mm smiData.Solid(2).MoI = [8.8877884434216323 52.448108530158386]; % kg*mm^2 smiData.Solid(2).PoI = [4.5273723585127415e-05 3.0892658331279513e-07]; % kg*mm^2 smiData.Solid(2).color = [0.792156862745098 0.93333333333333335]; smiData.Solid(2).opacity = 1; smiData.Solid(2).ID = 'taythree*:*Default'; 1.5543870396311503e-07 58.068501854976212 3.9177549318348395e-07 0.81960784313725488 %Inertia Type - Custom %Visual Properties - Simple smiData.Solid(3).mass = 0.28008854593891075; % kg smiData.Solid(3).CoM = [-17.940683623248297 -42.517651535075792]; % mm smiData.Solid(3).MoI = [413.94455025844405 518.9542633925164 258.29557742804394]; % kg*mm^2 smiData.Solid(3).PoI = [0 -168.21698768807488 0]; % kg*mm^2 smiData.Solid(3).color = [0.792156862745098 0.81960784313725488 0.93333333333333335]; smiData.Solid(3).opacity = 1; smiData.Solid(3).ID = 'tayone*:*Default'; %Inertia Type - Custom %Visual Properties - Simple 42 smiData.Solid(4).mass = 0.12997490882120452; % kg smiData.Solid(4).CoM = [-42.081435526362313 0]; % mm smiData.Solid(4).MoI = [26.584164728477944 143.18802246387102 142.95748967731811]; % kg*mm^2 smiData.Solid(4).PoI = [-1.5249572191022795e-05 0]; % kg*mm^2 smiData.Solid(4).color = [0.792156862745098 0.81960784313725488 0.93333333333333335]; smiData.Solid(4).opacity = 1; smiData.Solid(4).ID = 'taytwo*:*Default'; %Inertia Type - Custom %Visual Properties - Simple smiData.Solid(5).mass = 0.14124814706907063; % kg smiData.Solid(5).CoM = [0 -50.609547717197849 14.999999999999996]; % mm smiData.Solid(5).MoI = [111.44202305400694 80.385407639234245 157.52987164921868]; % kg*mm^2 smiData.Solid(5).PoI = [0 0]; % kg*mm^2 smiData.Solid(5).color = [0.792156862745098 0.81960784313725488 0.93333333333333335]; smiData.Solid(5).opacity = 1; smiData.Solid(5).ID = 'tayfour*:*Default'; %Initialize the RevoluteJoint structure array by filling in null values smiData.RevoluteJoint(4).Rz.Pos = 0.0; smiData.RevoluteJoint(4).ID = ''; smiData.RevoluteJoint(1).Rz.Pos = -141.17685381173627; % deg smiData.RevoluteJoint(1).ID = '[taythree-1:-:tayfour-1]'; smiData.RevoluteJoint(2).Rz.Pos = 91.985183488012055; % deg smiData.RevoluteJoint(2).ID = '[taythree-1:-:taytwo-1]'; smiData.RevoluteJoint(3).Rz.Pos = -178.4983040382788; % deg smiData.RevoluteJoint(3).ID = '[de-1:-:tayone-1]'; smiData.RevoluteJoint(4).Rz.Pos = -88.259382890170727; % deg smiData.RevoluteJoint(4).ID = '[tayone-1:-:taytwo-1]'; 43 Nhiệm vụ thành viên nhóm Bảng 0.1 Nhiệm vụ thành viên Họ tên MSSV Các chương x Nguyễn Thế Thiết 20181767 Quách Cao Hiển 20181465 x x x Nguyễn Công Minh 20181650 x x x Bùi Trung Nghĩa 20181672 x x Nguyễn Đình Chí 20181350 x x x Đào Cơng Đồn 20181393 x x x Trần Minh Hồng 20181496 x x Đỗ Tuấn Quang 20181710 x Vũ Mạnh Hùng 20181513 x x Nguyễn Sách Dương 20181439 x Ghi chú: - Các bạn phân cơng làm phần đánh dấu “x” x x x 44 ... GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D 1.1 Giới thiệu Robot Yaskawa ep4000d Robot EP4000D hãng YASKAWA Nhật Bản thiết kế chế tạo Hãng thành lập năm 1915 với lịch sử 100 năm, tập đoàn hàng đầu... nên khơng tránh khỏi sai sót Em xin chân thành cảm ơn cô Nguyễn Phạm Thục Anh giảng dạy giúp đỡ chúng em hoàn thành đồ án môn học MỤC LỤC CHƯƠNG GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D 1.1... hiểu robot yaskawa ep4000d? ??, nhóm em tìm hiểu vấn đề cấu trúc cấu động học, động lực học, lập trình quỹ đạo, điều khiển chuyển động,… Các vấn đề đề cập chương : Chương I: Giới thiệu chung robot yaskawa