ĐỒ án môn học đề tài THIẾT kế và điều KHIỂN ROBOT BA bậc tự DO

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ ĐỒ ÁN MÔN HỌC ĐỀ TÀI: THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BA BẬC TỰ DO Giảng viên hướng dẫn: TS Nguyễn Kiên Trung Sinh viên thực hiện: Bùi Văn Mạnh MSSV : 20152383 Hà Nội, 06/2017 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung LỜI MỞ ĐẦU Robot công nghiệp lĩnh vực nghiên cứu phát triển mạnh giới nước ta, lĩnh vực mẻ Ngày trình sản xuất đại, hầu hết quy trình thực máy chuyên dùng Các phương pháp làm giảm rõ rệt chi phí sản xuất sản phẩm công nghiệp phù hợp với đa số người tiêu dùng Tuy nhiên, máy công cụ thiết kế để thực nguyên công cho trước, cần thay đổi kiểu mẫu sản phẩm, tồn dây chuyền sản xuất phải cải tạo lại Việc sửa đổi tốn Chính lý thúc đẩy phát triển phươnng pháp sản xuất tiên tiến, chế tạo loại robot đưa vào quy trình chế tạo để thực nhiều nguyên công như: chuyển tải vật tư thiết bị dây truyền đại, hàn điểm, sơn phun lắp ráp cơng nghiệp tơ Đây kiểu tự động hóa linh hoạt mang lại hiệu kinh tế cao, cần thiết hoàn cảnh đất nước ta tiến tới cơng nghiệp hóa đại hóa tồn diện Robot cơng nghiệp ứng dụng rộng rãi dây truyền sản xuất đại có linh hoạt cao Vì việc nghiên cứu chế tạo robot cần thiết vô quan trọng Chúng em xin chân thành cảm ơn hướng dẫn tâm huyết thầy TS.Nguyễn Kiên Trung– Bộ môn Công nghệ chế tạo máy - Viện khí – Đại học Bách khoa Hà Nội tận tình bảo, giúp đỡ chúng em suốt thời gian hồn thành đồ án Do kiến thức cịn hạn hẹp thời gian thực không nhiều nên q trình thực khơng tránh khỏi thiếu sót Đề tài mong góp ý quý thầy cô Em xin chân thành cảm ơn! Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung MỤC LỤC LỜI MỞ ĐẦU CHƯƠNG 1:PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC ĐỀ TÀI I Tổng quan đề tài 1) Khái niệm nguyên lý hoạt động Robot 2) Đối tượng thao tác, dạng thao tác II Phân tích thơng số kỹ thuật Robot 1) Các thành phần kết cấu Robot 2) Thông số kỹ thuật 3) Phân tích nguyên lý hoạt động CHƯƠNG 2: Tính tốn quỹ đạo chuyển động 10 1) Bảng D-H 10 2) Thiết lập phương trình động học Robot 11 3) Bài toán động học thuận 13 4) Bài toán động học ngược 18 CHƯƠNG 3: Bài toán tĩnh học 21 1) Tính tốn lý thuyết 21 2) Thay số liệu thực tế 26 CHƯƠNG 4: Bài toán động lực học 28 1) Cơ sở lý thuyết 28 2) Thiết lập phương trình động học 30 CHƯƠNG 5: Kiểm nghiệm bền 39 CHƯƠNG 6: Thiết kế quỹ đạo chuyển động Robot 41 1) Khái niệm, yêu cầu nhiệm vụ thiết kế quỹ đạo 41 2) Không gian làm việc Robot 41 3) Thiết kế quỹ đạo không gian khớp 41 Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung CHƯƠNG 7: Xây dựng mơ hình tốn điều khiển động 48 I Giới thiệu chung 48 1) Động Bước ? 48 2) Hoạt động 48 3) Ứng dụng 48 II Điều khiển động bước 49 1)Giới thiệu phương pháp điều khiển tần số , băm xung cho động step 49 2) Nguyên lý 50 3) Thiết kế điều khiển PID mô MATLAB 52 CHƯƠNG 8: Thiết kế hệ thống điều khiển bám quỹ đạo Robot 61 1) Cơ sở lý thuyết 61 2) Mô phần mềm Matlab-simulink 64 PHỤ LỤC 70 TÀI LIỆU THAM KHẢO 76 Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung CHƯƠNG Phân tích lựa chọn kết cấu đề tài I Tổng quan đề tài 1) Khái niệm nguyên lý hoạt động Robot Thuật ngữ “Robot” sử dụng lần Karel Capek kịch ông Rosum’s Universal Robots xuất vào năm 1921 Co lẽ gợi ý ban đầu cho nhà sáng chế kỹ thuật việc sáng chế cấu, máy móc bắt chước hoạt động người Về mặt kỹ thuật, robot cơng nghiệp ngày có nguồn gốc từ hai lĩnh vực kỹ thuật đời sớm cấu điều khiển từ xa (Teleoperator) máy công cụ điều khiển số (NC-Numerially Controoled Machine Toll)  Robot thiết bị linh hoạt phục vụ người – Có hình dạng (cơ cấu) gần giống tay người – Có khả thao tác tự động – Có khả bắt chước thao tác người Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Các phận cánh tay robot  – Cơ cấu chấp hành – Hệ thống dẫn động – Hệ thống điều khiển theo chương trình có khả lập trình linh hoạt – Hệ thống thông tin giám sát Tay máy hoạt động nhờ động thông qua truyền • như: hộp giảm tốc bánh răng, bánh - răng, trục vít, khí nén, … Kết cấu tay robot vững chắc, hoạt động tin cậy, dễ dàng đạt đến vị trí • vật thể tác động 2) - Một số ứng dụng robot Lắp ráp thử nghiệm chi tiết khác nhau, lắp ráp linh kiện điện tử - Kiểm tra bảng PC, tự động làm thành phần mạch - Đóng gói sản phẩm - Ứng dụng tự động y tế Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử II GVHD:TS.Nguyễn Kiên Trung PHÂN TÍCH CÁC THƠNG SỐ KỸ THUẬT CỦA ROBOT 1) Các thành phần kết cấu Robot Các khâu Tay robot có bậc tự do, thiết kế khí dạng khớp xoay  Thân robot: Là khâu cố định,đặt thẳng đứng giữ robot cố định làm việc, gắn với khâu động qua khớp xoay với trục z01 thẳng đứng  Khâu 1: khâu dẫn động nằm ngang vng góc với trục thẳng đứng suốt trình làm việc robot,có khả quay xung quanh trục Z0 qua khớp xoay  Khâu 2: Khâu động có khả xoay mặt phẳng chứa trục Z0  Khâu 3: Giống khâu , có khả quay mặt phẳng chứa trục Z0 Hệ dẫn động  Khớp xoay : hệ bánh răng(hộp giảm tốc), truyền động đai…  Khớp xoay , : truyền động bánh vi sai ăn khớp  Động truyền động : Servo , Step  Sử dụng hộp giảm tốc , biến tần PWM để điều khiển tốc độ Hệ điều khiển  Dùng biến tần kết nối với PLC  Dùng mạch nguyên lý mạch băm xung PMW  Dùng Driver để điều khiển : WHD24 2) Thông số kỹ thuật L1 = 240 mm L2 = 350mm Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung L3 = 325 mm Chọn: - θ1=[0; - θ2=[0; ] - θ3=[0; ] ] Hình 2: Các thơng số kỹ thuật Robot DOF RRR 3) Phân tích nguyên lý hoạt động Mơ hình 3D robot : Khâu 0: Khâu cố định Khâu 1: Khâu động quay quanh trục thẳng đứng Khâu 2: Khâu động quay quanh trục nằm ngang vng góc với khâu Khâu 3: Khâu thao tác quay quanh trục nằm ngang vng góc khâu Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Mơ hình 3D robot Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung CHƯƠNG Tính tốn quỹ đạo chuyển động Khảo sát động học thuận, khảo sát động học ngược 1) Bảng D-H Joint Bảng 1: Các tham số Denavit-Hartenberg Trong : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 10 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Và hệ số Kd , Kp ma trận đường chéo vuông cấp n (n số tham số động học mơ hình robot), phần sau xem xét chi tiết việc chọn hệ số Kd , Kp theo điều kiện ràng buộc mục tiêu tốn Ta tìm biểu thức đặc trưng cho hệ điều khiển vịng kín robot: ̇ + ̇+ Với: = e=0 − ̇=− ̇=− Hệ phương trình bao gồm phương trình độc lập, ma trận Kd , Kp ma trận đường chéo, phương trình viết riêng cho khớp: ̇+ + ð =0 Theo lí thuyết dao động kĩ thuật, nhìn vào phương trình ta nhận xét phương trình dao động tự có cản, ứng xử hệ thống phụ thuộc vào hệ số kv i kpi Trong mục tiêu đưa đáp ứng hệ sát với giá trị mong muốn tốt, khoảng thời gian đạt đáp ứng nhanh tốt Do đó, giá trị nghiệm hướng tới trường hợp nghiệm kép, giá trị thực, trường hợp gọi trường hợp tới hạn k d kP Từ nội dung ta xây dựng sơ đồ khối mơ hình hệ thống sau: Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 63 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Từ sơ đồ nhận thấy điều khiển hệ thống không gian khớp theo thời gian Tín hiệu đặt tín hiệu phản hồi tương ứng với khớp (vị trí, vận tốc,gia tốc) 2) Mô phần mềm Matlab- simulink Từ sơ lí thuyết ta tiến hành xây dựng chương trình mơ điều khiển hệ thống Toolbox Simulink phần mêm Matlab : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 64 Đồ Án Thiết Kế Hệ Thống Cơ điện tử  GVHD:TS.Nguyễn Kiên Trung Khối đầu vào :  Khối PD : Với Kp=[625 625 625] Kd=[50 50 50]  Khối Robot : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 65 Đồ Án Thiết Kế Hệ Thống Cơ điện tử  GVHD:TS.Nguyễn Kiên Trung Kết nhận : • q q_dat : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 66 Đồ Án Thiết Kế Hệ Thống Cơ điện tử • GVHD:TS.Nguyễn Kiên Trung dq dq_dat : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 67 Đồ Án Thiết Kế Hệ Thống Cơ điện tử Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 GVHD:TS.Nguyễn Kiên Trung Page 68 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Đáp ứng quỹ đạo thiết kế chương : Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 69 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung Phụ lục %CODE TINH LUC DAN DONG U function u = dieukhien_phituyen(u1) %thong so dau vao m=3; %[kg] g=9.81; %[m/s2] n=3; %so bac tu q=u1(1:n);q1=q(1);q2=q(2);q3=q(3); dq=u1(n+1:2*n);dq1=dq(1);dq2=dq(2);dq3=dq(3); ddq=u1(2*n+1:3*n);ddq1=ddq(1);ddq2=ddq(2);ddq3=ddq(3); % khau m1=15.52; %[kg] I1x=.13; %[kg m2] I1y=.18; I1z=.14; a1=.17; L1=.24; m2=6.44; I2x=0.01; I2y=0.09; I2z=0.1; a2=.18; L2=0.350; m3=6.64; I3x=.01; I3y=.08; I3z=0.09; a3=.15; L3=0.325; %MA TRAN KHOI LUONG M m11=I1y + I3y*cos(q2 + q3)^2 + I3x*sin(q2 + q3)^2 + I2y*cos(q2)^2 + I2x*sin(q2)^2 + m3*sin(q1)^2*(L3*cos(q2 + q3) - a3*cos(q2 + q3) + L2*cos(q2))^2 + m3*cos(q1)^2*(L3*cos(q2 + q3) a3*cos(q2 + q3) + L2*cos(q2))^2 + m2*cos(q2)^2*sin(q1)^2*(L2 - a2)^2 + m2*cos(q1)^2*cos(q2)^2*(L2 - a2)^2; m12=0;m13=0;m21=0;m31=0; m22= I2z + I3z + L2^2*m2 + L2^2*m3 + L3^2*m3 + a2^2*m2 + a3^2*m3 - 2*L2*a2*m2 2*L3*a3*m3 + 2*L2*L3*m3*cos(q3) - 2*L2*a3*m3*cos(q3); m23=m3*L3^2 - 2*m3*L3*a3 + L2*m3*cos(q3)*L3 + m3*a3^2 - L2*m3*cos(q3)*a3 + I3z; m32=m23; m33= m3*L3^2 - 2*m3*L3*a3 + m3*a3^2 + I3z; M= [m11,m12,m13;m21,m22,m23;m31,m32,m33]; Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 70 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung %MA TRAN C c11=(I2x*dq2*sin(2*q2))/2 - (I2y*dq2*sin(2*q2))/2 + (I3x*dq2*sin(2*q2 + 2*q3))/2 + (I3x*dq3*sin(2*q2 + 2*q3))/2 - (I3y*dq2*sin(2*q2 + 2*q3))/2 (I3y*dq3*sin(2*q2 + 2*q3))/2 - (L2^2*dq2*m2*sin(2*q2))/2 - (L2^2*dq2*m3*sin(2*q2))/2 (a2^2*dq2*m2*sin(2*q2))/2 - (L3^2*dq2*m3*sin(2*q2 + 2*q3))/2 (L3^2*dq3*m3*sin(2*q2 + 2*q3))/2 - (a3^2*dq2*m3*sin(2*q2 + 2*q3))/2 (a3^2*dq3*m3*sin(2*q2 + 2*q3))/2 + L2*a2*dq2*m2*sin(2*q2) + L3*a3*dq2*m3*sin(2*q2 + 2*q3) + L3*a3*dq3*m3*sin(2*q2 + 2*q3) (L2*L3*dq3*m3*sin(q3))/2 + (L2*a3*dq3*m3*sin(q3))/2 L2*L3*dq2*m3*sin(2*q2 + q3) - (L2*L3*dq3*m3*sin(2*q2 + q3))/2 + L2*a3*dq2*m3*sin(2*q2 + q3) + (L2*a3*dq3*m3*sin(2*q2 + q3))/2; c12=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) + I2y*sin(2*q2) + a3^2*m3*sin(2*q2 + 2*q3) + L2^2*m2*sin(2*q2) + L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c13=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 + 2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3) L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) - L2*a3*m3*sin(2*q2 + q3) 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c21=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) + I2y*sin(2*q2) + a3^2*m3*sin(2*q2 + 2*q3) + L2^2*m2*sin(2*q2) + L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) - 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c22=-L2*dq3*m3*sin(q3)*(L3 - a3); c23=L2*m3*sin(q3)*(dq2 + dq3)*(L3 - a3); c31=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 + 2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3) - L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) L2*a3*m3*sin(2*q2 + q3) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c32=L2*dq2*m3*sin(q3)*(L3 - a3); c33=0; C=[c11,c12,c13;c21,c22,c23;c31,c32,c33]; %MA TRAN LUC SUY RONG CO THE G G=[0;g*m3*(cos(q2 + q3)*(L3 - a3) + L2*cos(q2)) + g*m2*cos(q2)*(L2 a2);g*m3*cos(q2 + q3)*(L3 - a3)]; %LUC KO THE Q=[ 0;-m*g*(L3*cos(q2 + q3) + L2*cos(q2));-m*g* L3*cos(q2 + q3)]; % PHUONG TRINH VI PHAN u=M*ddq+C*dq+G-Q; Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 71 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung % CODE TINH GIA TOC KHOP DDQ function ddq = robot_model(u) %thong so dau vao m=3; %[kg] g=9.81; %[m/s2] n=3; %so bac tU % Thong so tinh toan q=u(1:n);q1=q(1);q2=q(2);q3=q(3); dq=u(n+1:2*n);dq1=dq(1);dq2=dq(2);dq3=dq(3); U=[u(2*n+1);u(2*n+2);u(2*n+3)]; % khau m1=15.52; %[kg] I1x=.13; %[kg m2] I1y=.18; I1z=.14; a1=.17; L1=.24; m2=6.44; I2x=0.01; I2y=0.09; I2z=0.1; a2=.18; L2=0.350; m3=6.64; I3x=.01; I3y=.08; I3z=0.09; a3=.15; L3=0.325; %MA TRAN KHOI LUONG m11=I1y + I3y*cos(q2 + q3)^2 + I3x*sin(q2 I2x*sin(q2)^2 + L2*cos(q2))^2 + a3*cos(q2 + q3) + L2*cos(q2))^2 + m2*cos(q2)^2*sin(q1)^2*(L2 - a2)^2 + m2*cos(q1)^2*cos(q2)^2*(L2 - a2)^2; m12=0; m13=0; m21=0; m22= I2z + I3z + L2^2*m2 + L2^2*m3 + L3^2*m3 + a2^2*m2 + a3^2*m3 - 2*L2*a2*m2 2*L3*a3*m3 + 2*L2*L3*m3*cos(q3) - 2*L2*a3*m3*cos(q3); m23=m3*L3^2 - 2*m3*L3*a3 + L2*m3*cos(q3)*L3 + m3*a3^2 - L2*m3*cos(q3)*a3 + I3z; m31=0; m32=m23; m33= m3*L3^2 - 2*m3*L3*a3 + m3*a3^2 + I3z; M= [m11,m12,m13;m21,m22,m23;m31,m32,m33]; %MA TRAN C c11=(I2x*dq2*sin(2*q2))/2 - (I2y*dq2*sin(2*q2))/2 + (I3x*dq2*sin(2*q2 + 2*q3))/2 + (I3x*dq3*sin(2*q2 + 2*q3))/2 - (I3y*dq2*sin(2*q2 + 2*q3))/2 (I3y*dq3*sin(2*q2 + 2*q3))/2 Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 72 TIEU LUAN MOI download : skknchat@gmail.com Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung - (L2^2*dq2*m2*sin(2*q2))/2 - (L2^2*dq2*m3*sin(2*q2))/2 (a2^2*dq2*m2*sin(2*q2))/2 - (L3^2*dq2*m3*sin(2*q2 + 2*q3))/2 (L3^2*dq3*m3*sin(2*q2 + 2*q3))/2 - (a3^2*dq2*m3*sin(2*q2 + 2*q3))/2 (a3^2*dq3*m3*sin(2*q2 + 2*q3))/2 + L2*a2*dq2*m2*sin(2*q2) + L3*a3*dq2*m3*sin(2*q2 + 2*q3) + L3*a3*dq3*m3*sin(2*q2 + 2*q3) (L2*L3*dq3*m3*sin(q3))/2 + (L2*a3*dq3*m3*sin(q3))/2 L2*L3*dq2*m3*sin(2*q2 + q3) - (L2*L3*dq3*m3*sin(2*q2 + q3))/2 + L2*a3*dq2*m3*sin(2*q2 + q3) + (L2*a3*dq3*m3*sin(2*q2 + q3))/2; c12=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) + I2y*sin(2*q2) + a3^2*m3*sin(2*q2 + 2*q3) + L2^2*m2*sin(2*q2) + L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c13=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 + 2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3) L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) - L2*a3*m3*sin(2*q2 + q3) 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c21=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) + I2y*sin(2*q2) + a3^2*m3*sin(2*q2 + 2*q3) + L2^2*m2*sin(2*q2) + L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) - 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c22=-L2*dq3*m3*sin(q3)*(L3 - a3); c23=L2*m3*sin(q3)*(dq2 + dq3)*(L3 - a3); c31=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 + 2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3) - L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) L2*a3*m3*sin(2*q2 + q3) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2; c32=L2*dq2*m3*sin(q3)*(L3 - a3); c33=0; C=[c11,c12,c13;c21,c22,c23;c31,c32,c33]; %MA TRAN LUC SUY RONG CO THE G G=[0; g*m3*(cos(q2 + q3)*(L3 - a3) + L2*cos(q2)) + g*m2*cos(q2)*(L2 - a2); g*m3*cos(q2 + q3)*(L3 - a3)]; %LUC KO THE Q=[ 0; -m*g*(L3*cos(q2 + q3) + L2*cos(q2)); -m*g* L3*cos(q2 + q3)]; % PHUONG TRINH VI PHAN ddq=M\(U-G-C*dq+Q); Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 73 Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung %CODE LAP TRINH QUY DAO function [x,y,z]= fcn(t) %#codegen r=sqrt(0.2^2+0.4^2); if (t2)&&(t