Ứng dụng cánh tay robot 6 bậc tự do trong vẽ tranh

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA ỨNG DỤNG CÁNH TAY ROBOT BẬC TỰ DO TRONG VẼ TRANH GVHD: THS LÊ HOÀNG LÂM SVTH: PHẠM TRẦN NGUN VŨ TRẦN HỒNG KHA SKL011226 Tp Hồ Chí Minh, tháng 06/2023 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM VÀ KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO —————————₸♣₸————————— ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: ỨNG DỤNG CÁNH TAY ROBOT BẬC TỰ DO TRONG VẼ TRANH GVHD: ThS Lê Hoàng Lâm SVTH: Phạm Trần Nguyên Vũ Trần Hoàng Kha 19151200 19151021 Thành phố Hồ Chí Minh, tháng 06 năm 2023 TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP – TỰ DO – HẠNH PHÚC ————————— Tp HCM, ngày tháng 06 năm 2023 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Phạm Trần Nguyên Vũ Mã số sinh viên: 19151200 Họ tên sinh viên 1: Trần Hoàng Kha Mã số sinh viên: 19151021 Chuyên ngành: Công nghệ kỹ thuật điều khiển tự động hóa Mã ngành: 151 Hệ đào tạo: Đại học quy Mã hệ: 19 Khóa: 2019 Lớp: 19151CL I II TÊN ĐỀ TÀI: ỨNG DỤNG CÁNH TAY ROBOT BẬC TỰ DO TRONG VẼ TRANH NHIỆM VỤ Các số liệu ban đầu: o Thông số (điện áp, dòng điện, ) động Servo Driver MR – J4 o Thông số nhà sản xuất PLC Mitsubishi Q03UD CPU o Thông số nhà sản xuất Motion CPU Mitsubishi Q173DS o Thông số nhà sản xuất thông số Robot Denso o Tỉ số truyền hộ số trục Robot bậc tự o Tài liệu lý thuyết động học, qui hoạch quĩ đạo điều khiển Robot tác giả John J Craig o Bài báo nghiên cứu ứng dụng cánh tay Robot công nghiệp bậc tự vào việc vẽ tranh Nội dung thực hiện: o Kết nối giao tiếp Driver – Servo với hệ Multiple CPU để thực điều khiển Robot o Xây dựng toán động học, quy hoạch quỹ đạo không gian làm việc Robot o Thiết kế thi cơng lắp ráp khí (thiết kế tay cầm bút vẽ cho Robot xây dựng lắp ráp tụ điện) o Xây dựng toán điều khiển Robot đến ô định bảng vẽ o Xây dựng tốn vẽ nét (đường thẳng, hình trịn) o Xây dựng tốn vẽ hình từ nét (hình trịn nội tiếp hình vng) III IV V o Xây dựng tốn vẽ theo hình mong muốn người dùng Sản phẩm dự kiến: o Hệ Robot bậc tự (bao gồm: cánh tay Robot bậc tự do, tủ điện, bàn vẽ) o Chương trình điều khiển Robot o Chương trình kiểm chứng lý thuyết động học Robot o Giao diện điều khiển Robot o Giao diện xử lý ảnh đưa vào để Robot vẽ tranh o Quyển báo cáo đồ án tốt nghiệp o Poster giới thiệu đồ án NGÀY GIAO ĐỀ TÀI: 06/02/2023 NGÀY HOÀN THÀNH ĐỀ TÀI: 30/06/2023 HỌ VÀ TÊN GIẢNG VIÊN HƯỚNG DẪN: ThS Lê Hoàng Lâm TRƯỞNG KHOA TRƯỞNG NGÀNH (Ký ghi rõ họ tên) (Ký ghi rõ họ tên) GIẢNG VIÊN HƯỚNG DẪN (Ký ghi rõ họ tên) TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO BỘ MƠN ĐIỀU KHIỂN TỰ ĐỘNG CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP – TỰ DO – HẠNH PHÚC ————————— Tp HCM, ngày tháng 06 năm 2023 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Giảng viên hướng dẫn: ThS Lê Hoàng Lâm Họ tên sinh viên 1: Phạm Trần Nguyên Vũ Mã số sinh viên: 19151200 Họ tên sinh viên 1: Trần Hoàng Kha Mã số sinh viên: 19151021 Tên đề tài: Ứng dụng cánh tay Robot bậc tự vẽ tranh Tuần/Ngày 13/02/2023 (1 tuần) 20/02/2023 (1 tuần) 27/02/2023 (1 tuần) 06/03/2023 (1 tuần) 13/03/2023 (1 tuần) 03/04/2023 (3 tuần) 10/04/2023 (1 tuần) 17/04/2023 (1 tuần) 24/04/2023 (1 tuần) 05/05/2023 (1 tuần) Nội dung thực Tìm hiểu điều khiển cấu Robot Tiến hành lắp đặt phần cứng, dây thiết bị Kiếm tra Servo, tìm tỉ số Gear thực điều khiển Servo Tìm hiểu phần mềm MT Developer2, cài đặt thông số Servo phần mềm Tiến hành dây module ngõ vào ngõ PLC, giao tiếp CPU Q03UDCPUvới Motion CPU Q173DSCPU Tính động học thuận, động học nghịch Robot kiểm chứng kết tốn sử dụng mơ Thiết kế chương trình điều khiển dựa vào kết động học Robot PLC Xây dựng chương trình điều khiển Robot chạy điểm Thiết kế khí gia cơng tay cầm bút cho Robot Xây dựng chương trình cho Robot vẽ đường (đường thẳng đường tròn) Xác nhận từ GVHD 12/05/2023 (1 tuần) 26/05/2023 (2 tuần) 02/06/2023 (1 tuần) 09/06/2023 (1 tuần) 16/06/2023 (1 tuần) Xây dựng chương trình cho Robot vẽ từ đường (hình trịn nội tiếp hình vng) Xây dựng kiểm chứng chương trình xử lí ảnh Kết hợp chương trình xử lí ảnh với Robot để vẽ hình phức tạp Tối ưu chương trình xử lí ảnh thiết kế app người dùng Kiểm tra vận hành hệ thống hoàn chỉnh GIẢNG VIÊN HƯỚNG DẪN (Ký ghi rõ họ tên) CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự do– Hạnh phúc PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Họ tên sinh viên 1: Phạm Trần Nguyên Vũ MSSV: 19151200 Họ tên sinh viên 2: Trần Hoàng Kha MSSV: 19151021 Tên đề tài: ỨNG DỤNG CÁNH TAY ROBOT BẬC TỰ DO TRONG VẼ TRANH Ngành đào tạo: Công nghệ kỹ thuật điều khiển tự động hóa - Khóa 2019-2023 Họ tên GV hướng dẫn: ThS Lê Hoàng Lâm 269 NHẬN XÉT Về đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: Đề nghị cho bảo vệ hay không? Đánh giá loại: Điểm………….(Bằng chữ:………………………………………………….……) Tp.Hồ Chí Minh, ngày tháng Giáo viên hướng dẫn năm 20… CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự do– Hạnh phúc PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Họ tên sinh viên 1: Phạm Trần Nguyên Vũ MSSV: 19151200 Họ tên sinh viên 2: Trần Hoàng Kha MSSV: 19151021 Tên đề tài: ỨNG DỤNG CÁNH TAY ROBOT BẬC TỰ DO TRONG VẼ TRANH Ngành đào tạo: Công nghệ kỹ thuật điều khiển tự động hóa - Khóa 2019-2023 Họ tên GV hướng dẫn: ThS Lê Hoàng Lâm 269 NHẬN XÉT Về đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: Đề nghị cho bảo vệ hay không? Đánh giá loại: Điểm………….(Bằng chữ:………………………………………………….……) Tp.Hồ Chí Minh, ngày tháng Giáo viên phản biện năm 20… LỜI CAM ĐOAN Nhóm tác giả cam kết đề tài nhóm tác giả thực dựa vào tài liệu tham khảo có sẵn khơng chép lạm dụng tài tài liệu hay cơng trình nghiên cứu trước Nhóm tác giả thực đề tài tinh thần tuân thủ quy định nhà trường đề ra, xin chịu trách nhiệm vi phạm nhằm đảm bảo nguyên tắc đạo đức tính minh bạch Nhóm tác giả thực Phạm Trần Ngun Vũ Trần Hồng Kha LỜI CẢM ƠN Nhóm chúng em xin gửi lời chân thành cảm ơn đến thầy cô khoa Đào tạo Chất lượng cao, khoa Điện – Điện tử, môn Tự động điều khiển trường Đại học Sư phạm Kỹ thuật Thành phố Hồ Chí Minh truyền đạt kiến thức kinh nghiệm cho chúng em suốt trình rèn luyện học tập để áp dụng vào trình thực luận văn Nhóm chúng em xin gửi lời cảm ơn sâu sắc đến ThS Lê Hoàng Lâm tận tâm hướng dẫn nhóm suốt trình nghiên cứu viết luận văn Từ đóng góp ý tưởng hình thành, đưa phương án cải tiến với lời khích lệ tinh thần từ thầy giúp đỡ nhóm chúng em nhiều để hồn thành luận văn tốt nghiệp Sau nhóm chúng em muốn bày tỏ lịng biết ơn đến gia đình bạn bè Những người ln động viên ủng hộ xuyên suốt trình thực luận văn tốt nghiệp Sự tin tưởng niềm hi vọng nguồn động lực cho chúng em phấn đấu, nỗ lực đường phát triển nghiệp sau Nhóm chúng em xin chân thành cảm ơn! i PHỤ LỤC case %% theta1 theta1 = atan2d(yc, xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(-sqrt(a^2 + b^2 n^2), n); %% theta3 k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, -sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)); %% theta5 BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 105 PHỤ LỤC theta5 = atan2d(-sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); case %% theta1 theta1 = atan2d(yc, xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(-sqrt(a^2 + b^2 n^2), n); %% theta3 k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, -sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 106 PHỤ LỤC alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)) + 180; %% theta5 theta5 = atan2d(sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); case %% theta1 theta1 = atan2d(-yc, -xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(sqrt(a^2 + b^2 n^2), n); %% theta3 BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 107 PHỤ LỤC k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)); %% theta5 theta5 = atan2d(-sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); case %% theta1 theta1 = atan2d(-yc, -xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 108 PHỤ LỤC u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(sqrt(a^2 + b^2 n^2), n); %% theta3 k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)) + 180; %% theta5 theta5 = atan2d(sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 109 PHỤ LỤC cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); case %% theta1 theta1 = atan2d(-yc, -xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(-sqrt(a^2 + b^2 n^2), n); %% theta3 k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, -sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)); %% theta5 BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 110 PHỤ LỤC theta5 = atan2d(-sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); case %% theta1 theta1 = atan2d(-yc, -xc); %% theta2 t = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (l1 - zc)^2 + l2^2; u = d2^2 + l3^2; n = (t - u)/(2 * l2); a = cosd(theta1)*xc + sind(theta1)*yc - d1; b = l1 - zc; theta2 = atan2d(b, a) - atan2d(-sqrt(a^2 + b^2 n^2), n); %% theta3 k = (cosd(theta1)*xc + sind(theta1)*yc - d1)^2 + (zc - l1)^2; q = d2^2 + l3^2 + l2^2; m = (k - q)/(2 * l2); theta3 = atan2d(d2, l3) - atan2d(m, -sqrt(l3^2 + d2^2 - m^2)); %% beta alpha gamma beta = atan2d(sqrt((sind(theta2 + theta3))^2 + (cosd(theta2 + theta3))^2), 0); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 111 PHỤ LỤC alpha = atan2d(cosd(theta2 + theta3)/sind(beta), sind(theta2 + theta3)/sind(beta)); gamma = atan2d(-cosd(theta1)/sind(beta), sind(theta1)/sind(beta)); %% theta4 theta4 = atan2d(-cosd(beta), cosd(alpha)*sind(beta)) + 180; %% theta5 theta5 = atan2d(sqrt(1 (sind(alpha)*sind(beta))^2), sind(alpha)*sind(beta)); %% theta6 theta6 = atan2d((sind(alpha)*cosd(beta)*sind(gamma) cosd(alpha)*cosd(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2), (sind(alpha)*cosd(beta)*cosd(gamma) + cosd(alpha)*sind(gamma))/sqrt(1 (sind(alpha)*sind(beta))^2)); otherwise disp('out of corner set') theta1 = 0; theta2 = 0; theta3 = 0; theta4 = 0; theta5 = 0; theta6 = 0; end Quy hoạch quỹ đạo bậc function [x, y, z, vx, vy, vz] = trajectory_planning(t, Po, Pf, tf) xo = Po(1); yo = Po(2); zo = Po(3); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 112 PHỤ LỤC xf = Pf(1); yf = Pf(2); zf = Pf(3); A = [1 0 0 0 0 0 0 0 0 tf^3 3*tf^2 0 1 tf 0 0 0 0; 0 0; tf tf^2 tf^3 0; 2*tf 3*tf^2 0; 0 0 0; 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0; 0 0 0; 0 tf^2 tf^3; 0 2*tf 3*tf^2]; 0 0 0 0 0 0 0 0 0 0 0 tf tf^2 0 0 0 0 0 0 0 0 2*tf B = [xo; 0; xf; 0; yo; 0; yf; 0; zo; 0; zf; 0]; X = inv(A)*B; a0 a1 a2 a3 = = = = X(1,1); X(2,1); X(3,1); X(4,1); b0 b1 b2 b3 = = = = X(5,1); X(6,1); X(7,1); X(8,1); c0 = X(9,1); c1 = X(10,1); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 113 PHỤ LỤC c2 = X(11,1); c3 = X(12,1); x = a0 + a1*t + a2*t^2 + a3*t^3; y = b0 + b1*t + b2*t^2 + b3*t^3; z = c0 + c1*t + c2*t^2 + c3*t^3; vx = a1 + 2*a2*t + 3*a3*t^2; vy = b1 + 2*b2*t + 3*b3*t^2; vz = c1 + 2*c2*t + 3*c3*t^2; end Quy hoạch quỹ đạo bậc function [x, y, z, vx, vy, vz] = trajectory_planning_2(t, Po, Pf, tf) xo = Po(1); yo = Po(2); zo = Po(3); xf = Pf(1); yf = Pf(2); zf = Pf(3); % X A = [0 tf^5 5*tf^4 20*tf^3 tf^4 4*tf^3 12*tf^2 tf^3 3*tf^2 6*tf tf^2 2*tf 0 tf 0 1; 0; 0; 0; 0; 0;]; B = [xo; (xf - xo); 0; 0; 0; 0]; C = inv(A)*B; a1 a2 a3 a4 a5 a6 = = = = = = C(1,1); C(2,1); C(3,1); C(4,1); C(5,1); C(6,1); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 114 PHỤ LỤC x = a1*t^5 + a2*t^4 + a3*t^3 + a4*t^2 + a5*t + a6; vx = 5*a1*t^4 + 4*a2*t^3 + 3*a3*t^2 + 2*a4*t + a5; % Y D = [0 tf^5 5*tf^4 20*tf^3 tf^4 4*tf^3 12*tf^2 tf^3 3*tf^2 6*tf tf^2 2*tf 0 tf 0 1; 0; 0; 0; 0; 0;]; E = [yo; (yf - yo); 0; 0; 0; 0]; F = inv(D)*E; b1 b2 b3 b4 b5 b6 = = = = = = F(1,1); F(2,1); F(3,1); F(4,1); F(5,1); F(6,1); y = b1*t^5 + b2*t^4 + b3*t^3 + b4*t^2 + b5*t + b6; vy = 5*b1*t^4 + 4*b2*t^3 + 3*b3*t^2 + 2*b4*t + b5; % Z K = [0 tf^5 5*tf^4 20*tf^3 tf^4 4*tf^3 12*tf^2 tf^3 3*tf^2 6*tf tf^2 2*tf 0 tf 0 1; 0; 0; 0; 0; 0;]; L = [zo; (zf - zo); 0; 0; 0; 0]; M = inv(K)*L; c1 c2 c3 c4 c5 c6 = = = = = = M(1,1); M(2,1); M(3,1); M(4,1); M(5,1); M(6,1); BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 115 PHỤ LỤC z = c1*t^5 + c2*t^4 + c3*t^3 + c4*t^2 + c5*t + c6; vz = 5*c1*t^4 + 4*c2*t^3 + 3*c3*t^2 + 2*c4*t + c5; end Mô Robot không gian chiều function plot_robot_2D(corner_set, theta1, theta2, theta3, theta4, theta5, theta6) [d1, d2, l1, l2, l3, l4] = robot_data; r3 = -cosd(theta4)*sind(theta5)*cosd(theta2 + theta3) cosd(theta5)*sind(theta2 + theta3); r6 = sind(theta4)*sind(theta5); r9 = sind(theta2 + theta3)*cosd(theta4)*sind(theta5) cosd(theta2 + theta3)*cosd(theta5); p1 = d1 + d2*cosd(theta2 + theta3) - l3*sind(theta2 + theta3) + l2*cosd(theta2); p2 = 0; p3 = -l3*cosd(theta2 + theta3) - d2*sind(theta2 + theta3) - l2*sind(theta2); x0 = 0; y0 = 0; z0 = 0; x1 = 0; y1 = 0; z1 = l1; x2 = d1*cosd(theta1); y2 = d1*sind(theta1); z2 = l1; x3 = cosd(theta1)*(d1 + l2*cosd(theta2)); y3 = sind(theta1)*(d1 + l2*cosd(theta2)); x4 = cosd(theta1)*p1 - sind(theta1)*p2; y4 = sind(theta1)*p1 + cosd(theta1)*p2; xee = l4*(cosd(theta1)*r3 - sind(theta1)*r6) + cosd(theta1)*p1 - sind(theta1)*p2; BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 116 PHỤ LỤC yee = l4*(sind(theta1)*r3 + cosd(theta1)*r6) + sind(theta1)*p1 + cosd(theta1)*p2; plot([x0 x1 x2 x3 x4 xee], [y0 y1 y2 y3 y4 yee], '-o', LineWidth = 2, Color = 'b') grid on xlim([-1000 1000]); ylim([-1000 1000]) xlabel('x'); ylabel('y') name1 = "theta1: " + theta1 + " theta2: " + theta2 + " theta3: " + theta3; name2 = "theta4: " + theta4 + " theta5: " + theta5 + " theta6: " + theta6; title("corner set: " + corner_set) subtitle([name1, name2, 'unit: degree']) end Mô Robot không gian chiều function plot_robot(corner_set, theta1, theta2, theta3, theta4, theta5, theta6) [d1, d2, l1, l2, l3, l4] = robot_data; r3 = -cosd(theta4)*sind(theta5)*cosd(theta2 + theta3) cosd(theta5)*sind(theta2 + theta3); r6 = sind(theta4)*sind(theta5); r9 = sind(theta2 + theta3)*cosd(theta4)*sind(theta5) cosd(theta2 + theta3)*cosd(theta5); p1 = d1 + d2*cosd(theta2 + theta3) - l3*sind(theta2 + theta3) + l2*cosd(theta2); p2 = 0; p3 = -l3*cosd(theta2 + theta3) - d2*sind(theta2 + theta3) - l2*sind(theta2); x0 = 0; y0 = 0; z0 = 0; x1 = 0; y1 = 0; z1 = l1; BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 117 PHỤ LỤC x2 = d1*cosd(theta1); y2 = d1*sind(theta1); z2 = l1; x3 = cosd(theta1)*(d1 + l2*cosd(theta2)); y3 = sind(theta1)*(d1 + l2*cosd(theta2)); z3 = l1 - l2*sind(theta2); x4 = cosd(theta1)*p1 - sind(theta1)*p2; y4 = sind(theta1)*p1 + cosd(theta1)*p2; z4 = l1 + p3; xee = l4*(cosd(theta1)*r3 - sind(theta1)*r6) + cosd(theta1)*p1 - sind(theta1)*p2; yee = l4*(sind(theta1)*r3 + cosd(theta1)*r6) + sind(theta1)*p1 + cosd(theta1)*p2; zee = l4*r9 + l1 + p3; plot3([x0 x1 x2 x3 x4 xee], [y0 y1 y2 y3 y4 yee], [z0 z1 z2 z3 z4 zee], '-o', LineWidth = 2, Color = 'b') grid on xlim([-1000 1000]); ylim([-1000 1000]); zlim([0 1000]) xlabel('x'); ylabel('y'); zlabel('z') name1 = "theta1: " + theta1 + " theta2: " + theta2 + " theta3: " + theta3; name2 = "theta4: " + theta4 + " theta5: " + theta5 + " theta6: " + theta6; title("corner set: " + corner_set) subtitle([name1, name2, 'unit: degree']) end BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 118 S K L 0