thiết kế hệ thống cơ điện tử - thiết kế robot 2 khâu rr
ĐỒ ÁN CƠ ĐIỆN TỬ MỤC LỤC LỜI NÓI ĐẦU ……………………………………………………………………3 THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ……………………………………… Phần I Thiết kế mô hình 3D…………………………………………………… Mô hình tổng thể……………………………………………………………….4 Các khâu khớp truyền……………………………………………….4 Phần II Tính toán động học thuận, động học ngược Robot RR………………8 Động học thuận…………………………………………………………… .8 a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg………………… b Ma trận Denavit Hartenberg…………………………………………………8 c Xác định vị trí điểm tác động cuối P…………………………………………9 Động học ngược……………………………………………………………… Phần III Thiết kế quỹ đạo chuyển động cho Robot RR…………………… 10 Qũy đạo không gian khớp…………………………………………… 10 Qũy đạo không gian làm việc……………………………………… 12 a Qũy đạo điểm tác động cuối theo đường thẳng từ A đến B tc (s)… 12 b Qũy đạo điểm tác động cuối theo đường đương tròn từ A đến B tc (s) lấy đường kính AB làm đường kính…………………………………………… 13 Phần IV Khảo sát động lực học ……………………………………………….14 Phần V Thiết kế điều khiển…………………………………………………….17 Hệ thống điều khiển không gian khớp…………………………………17 1.1 Hệ thống điều khiển phản hồi…………………………………………….17 a Luật điều khiển…………………………………………………………….17 b Thiết kế cho Robot RR……………………………………………………19 1.2 Hệ thống điều khiển momen tính toán……………………………………21 a Luật điều khiển…………………………………………………………….21 b Thiết kế cho Robot RR……………………………………………………23 Hệ thống điều khiển không gian làm việc…………………………… 25 Hệ thống điều khiển ma trận Jacobien chuyển vị a Luật điều khiển…………………………………………………………… 26 b Thiết kế cho robot RR………………………………………………………27 Phần VI Các chương trình mô viết ngôn ngữ Matlab………….31 Chương trình mô kết phần hệ thống điều khiển phản hồi……… 31 Chương trình mô kết phần hệ thống điều khiển momen tính toán 35 Chương trình mô kết phần hệ thống điều khiển không gian làm việc theo đường thẳng…………………………………………………………………38 Chương trình mô kết phần hệ thống điều khiển không gian làm việc theo đường tròn từ hai điểm A, B lấy AB làm đường kính………………………41 ĐỒ ÁN CƠ ĐIỆN TỬ Sử dụng thư viện Simulink Matlab…………………………… 43 5.1 Sử dụng thư viện SimMechanics trong simulink khai báo robot RR, coi khâu mảnh đồng chất để so sánh kết quả…………….43 5.2 Sử dụng thư viện SimMechanis với mô hình robot RR xuất từ soid words ………………………………………………………………………46 5.3 Sử dụng phương trình động lực học mô điều khiển Simulink………………………………………………………………………….53 a, sơ đồ khối hệ thống điều khiển PD không bù momen trọng lượng…… 53 b, sơ đồ khối hệ thống điều khiển PD bù momen trọng lực……………….57 c, sơ đồ khối hệ thống điều khiển momen tính toán……………………….59 TÀI LIỆU THAM KHẢO……………………………………………………… 60 LỜI NÓI ĐẦU Hiện khoa học kĩ thuật phát triển mạnh, mang lại lợi ích cho người tất cảcác lĩnh vực vật chất tinh thần Để nâng cao đời sống nhân dân hòa nhập với phát triển chung giới, Đảng Nhà nước ta đề mục tiêu đưa đất nước ta lên thành nước công nghiệp hóa đại hóa Để thực điều đó, ngành cần quan tâm phát triểnlà ngành khí nói chung ngành điện tử nói riêng đóng vai trò quan trọng việc sản xuất thiết bị, công cụ ( máy móc, robot ) ngành kinh tế quốc dân Muốn thực việc phát triển ngành khí cần đẩy mạnh đào tạo đội ngũ cán kĩ thuật có trình độ chuyên môn đáp ứng yêu cầu công nghệ tiên tiến, công nghệ tự động hóa theo dây chuyền sản xuất ĐỒ ÁN CƠ ĐIỆN TỬ Tính toán thiết kế hệ thống điện tử nội dung thiếu chương trình đào tạo kỹ sư điện tử Đồ án môn học giúp cho sinh viên hệ thống hóa lại kiến thức môn học như: Lí thuyết điều khiển tử động, Robotics, Robot công nghiệp, sở máy CNC, Tính toán thiết kế Robot, Động lực học hệ nhiều vật, Chi tiết máy, Vẽ kĩ thuật, Cơ học kĩ thuật, Nguyên lí máy…Đồng thời giúp cho sinh viên làm quen với công việc thiết kế làm đồ án tốt nghiệp sau Dù cố gắng hoàn thành đồ án với cường độ làm việc cao, kỹ lưỡng hướng dẫn cụ thể thầy môn, hiểu biết hạn chế cộng với chưa có kinh nghiệm thực tiễn nên chắn đồ án không tránh khỏi khả thiết sót bất cập Vì em mong sửa chữa góp ý quý thầy cô để em rút kinh nghiệm bổ sung thêm kiến thức cho Cuối em xin chân thành cảm ơn quan tâm bảo thầy cô khoa Cơ Khí trường Đại học Bách khoa Hà Nội đặc biệt hướng dẫn tận tình thầy Đỗ Đức Nam giúp em hoàn thành đồ án Giảng viên hướng dẫn: TS.Đỗ Đức Nam Sinh viên thực THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ Nhiệm vụ thiết kế : Robot khâu RR - Coi khâu đồng chất ,tiết diện ngang không đáng kể -Thông số hình học khối lượng a1=50(cm)=0.5 (m),a2=40(cm)=0.4 (m),m1 = 2.5 (kg),m1 =1.5(kg) PhầnI Thiết kế mô hình 3D Do đồ án yêu cầu thiết kế mô hình 3D robot nên phần thiết kế em mang tính chất ý tưởng sinh viên,chứ thực tế truyền động robot nhỏ gọn, phức tạp xác ĐỒ ÁN CƠ ĐIỆN TỬ Mô hình tổng thể Hình 1.1 Mô hình tổng thể Các khâu khớp truyền 2.1 Khâu ( đế) ĐỒ ÁN CƠ ĐIỆN TỬ Hình 1.2 Khâu đế 2.2 Khâu khâu Khâu kích thước trục 500 (mm); khâu kích thước 400 (mm) ĐỒ ÁN CƠ ĐIỆN TỬ Hình 1.3 Khâu 1, khâu 2.3 Các truyền động + Bên trái truyền bánh giúp tăng momen động nên gắn khâu không tác dụng dẫn động cho khâu + Bên phải truyền bánh giúp tăng momen động nên gắn khâu không tác dụng dẫn động cho khâu với việc tạo vận tốc góc tương đối mô bánh hành tinh + Bên xilanh khí nén dẫn động cho cấu tay kẹp ĐỒ ÁN CƠ ĐIỆN TỬ Hình 1.4 truyền ĐỒ ÁN CƠ ĐIỆN TỬ Phần II Tính toán động học thuận, động học ngược Robot RR Động học thuận Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo chọn trùng với khớp 1, trục Xo chọn tùy ý cho phù hợp hình vẽ , trục Yo chọn theo quay tắc tam diện thuận Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng với khớp 2, trục X1 ta chọn theo đường vuông góc chung Zo Z1 ta chọn hình vẽ phù hợp nhất, trục Y1 chọn theo quay tắc tam diện thuậnHình 2.1 Robot RR Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 song song với Z1, X2 chọn theo đường vuông góc chung Z1 Z2, Y2 chọn theo quy tắc tam diện thuận b Ma trận trạng thái Denavit Hartenberg Từ việc chọn hệ tọa độ ta có bảng DH sau: Khâu A = d 0 − sin(q1 ) a1cos( q1 ) cos(q1 ) a1 sin( q1 ) 0 cos(q1 ) sin(q ) 0 A A Θ q1 q2 2 = = , A a a1 a2 = cos(q2 ) − sin(q2 ) sin(q ) cos( q2 ) 0 α 0 a2 cos( q2 ) a2 sin( q2 ) A A cos( q1 + q2 ) sin(q + q ) − sin( q1 + q2 ) a1cos( q1 ) + a2 cos( q1 + q2 ) cos(q1 + q2 ) a1 sin( q1 ) + a2 cos( q1 + q2 ) 0 ĐỒ ÁN CƠ ĐIỆN TỬ c.Xác định vị trí điểm tác động cuối P A Từ ma trận trạng thái X2Y2Z2 hệ cố định XoYoZo ta xác định điểm tác động cuối P khâu so với hệ cố định, ma trận cosin hướng khâu so với XoYoZo 0 r P R [ a1cos(q1 ) + a2 cos(q1 + q2 ) = = cos(q1 + q2 ) sin(q + q ) a1 sin( q1 ) + a2 sin( q1 + q2 ) 0] T − sin(q1 + q2 ) cos( q1 + q2 ) x p = a1cos(q1 ) + a2 cos(q1 + q2 ) y p = a1 sin( q1 ) + a2 sin( q1 + q2 ) z p = Chú ý: Ta có robot phẳng hai bậc tự tính toán hay điều khiển ta quan tâm đến điểm tác động cuối phẳng không quan tâm đến hướng khấu cuối ta tìm biến khớp từ điểm tác động cuối từ tìm ma trận cosin hướng R từ xác định góc Cardan hay Euler Động học ngược Khi giải toán động học thuận người ta xác định quan hệ dước dạng ma trận sau: x=f(q) Từ quan hệ ta suy cách hình thức q=f-1(x) x = [x1,x2,…,xm]T ,q = [q1,q2,…,qn]T Như ta có hai chiều hướng để giải phương pháp số phương pháp giải tích với toán RobotRR đơn giản ta dùng phương pháp giải tích đơn giản dễ giải với toán robot nhiều bậc phương pháp số phương pháp hữu hiệu từ hệ x p = a1cos( q1 ) + a2 cos( q1 + q2 ) y p = a1 sin( q1 ) + a2 sin( q1 + q2 ) z p = (2.1) ĐỒ ÁN CƠ ĐIỆN TỬ Kí hiệu C1 = cos(q1), S1 = sin(q1), C2 = cos(q2), S2 = sin(q2),C12 = cos(q1+q2), S12 = sin(q1+q2), x 2p + y 2p = a12 + a22 + 2a1a2 (C1C12 + S1S12 ) = a12 + a22 + 2a1a2C2 Từ hệ (2.1) => => x 2p + y 2p − a12 − a22 cos(q2 ) = 2a1a2 sin(q2 ) = ± − cos (q2 ) q2 = a tan 2(sin( q2 ), cos( q2 )) x p = (a1 + a2C2 )C1 − a2 S S1 y p = a2 S 2C1 + (a1 + a2C2 ) S1 Từ hệ (2.1) ta viết lại sau Giải hệ phương trình đại số tuyến tính ta : ∆= a1 + a2C2 ∆1 = ∆2 = − a2 S a1 + a2C2 a2 S2 xp − a2 S yp a1 + a2C2 a1 + a2C2 xp a2 S2 yp = a12 + a22 + 2a1a2C2 = x 2p + y 2p = a1 x p + a2 ( x pC2 + y p S ) = a1 y p + a2 ( y pC2 − x p S ) a1 x p + a2 ( x pC2 + y p S ) cos( q ) = x 2p + y 2p sin( q ) = a1 y p + a2 ( y p C2 − x p S2 ) x 2p + y 2p q1 = a tan ( sin( q1 ), cos( q1 ) ) => 10 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.8 Đồ thị X, Y đặt điều khiển Hình 6.9 Qũy đạo đặt điều khiển 53 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.10 Hình ảnh Robot RR theo đường thẳng với luật điều khiển jacobien chuyển vị b, điều khiển theo cung tròn 54 ĐỒ ÁN CƠ ĐIỆN TỬ Khối DAT có nhiệm vụ tạo tín hiệu đặt cho điều khiển PD với tín hiệu đặt đảm bảo điểm tác động cuối di chuyển theo cung tròn ( nửa đường tròn ) từ điểm ban đầu A(0.9 , 0) đến điểm B(0.5 , 0) Đã thiết kế phần thiết kế quỹ đạo Ta thu kết mô sau : Hình 6.11 Đồ thị X, Y đặt điều khiển Hình 6.12 Qũy đạo đặt điều khiển 55 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.13 Hình ảnh Robot RR theo cung tròn với luật điều khiển jacobien chuyển vị 56 ĐỒ ÁN CƠ ĐIỆN TỬ 5.3 Sử dụng phương trình động lực học mô điều khiển Simulink Với phương trình động lực học robot RR : 1 1 1 2 2 & & m2 a1a2 cos(q2 ) ÷q& + m2 a2 + M = m1a1 + m2 a2 + m2 a1 + m2 a1a2 cos(q2 ) ÷q& 3 (m + 2m2 ) 1 m2 a1 a2 sin( q2 ) q&22 + ga1 cos(q2 ) + m2 ga2 cos(q1 + q2 ) − m2 a1 a2 sin(q2 )q&& q2 − 2 1 1 & & m2 a22 q& m2 a1 a2 sin(q2 )q&12 + m2 ga2 cos(q1 + q2 ) + + M = m2 a2 + m2 a1 a2 cos(q2 ) ÷q& 2 Ta xây dựng sơ đồ khối mô Simulink sau : a, sơ đồ khối hệ thống điều khiển PD không bù momen trọng lượng M dk = K p ( qd − q) + K d (q&d − q&) = K p ε + K d ε& Với luật điều khiển : Hình 6.14 Sơ đồ khối với điều khiển PD Hình 6.15 Khối ROBOT_RR 57 ĐỒ ÁN CƠ ĐIỆN TỬ Ở Hình 6.15 hai hàm Fcn1, Fcn dùng để tính gia tốc & q& trình động với tín hiệu đầu vào gồm momen đại lượng phản hồi & q& rút từ phương q1 q2 q&1 q&2 , , , Hàm Fcn1: (1/ ((1/3)*m1*a1^2+(1/3)*m2*a2^2+m2*a1^2+m2*a1*a2*cos(u(3))))*(u(1)((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(3)))*u(6)+m2*a1*a2*sin(u(3))*u(4)*u(5) +0.5*m2*a1*a2*sin(u(3))*u(5)^2-0.5*(m1+2*m2)*g*a1*cos(u(3))0.5*m2*g*a2*cos(u(2)+u(3))) Hàm Fcn: (1/((1/3)*m2*a2^2))*(u(6)-((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(2)))*u(5)0.5*m2*a1*a2*sin(u(2))*u(3)^2-0.5*m2*g*a2*cos(u(1)+u(2))) Hình 6.16 Khối hiển thị ( hien_thi ) Hình 6.17 Khối giá trị đặt ( DAT ) File thông số mô ( với hàm DHN nói phần viết chương trình ): % -thong so robot RR -tc=2; a1=0.5; a2=0.4; 58 ĐỒ ÁN CƠ ĐIỆN TỬ m1=2.5; m2=1.5; g=9.81; % PD kp=1000; kd=500; % - gia tri dat -s0=[0.2;0.5]; sc=[0.6;0.4]; q0=DHN(s0); qc=DHN(sc); a10=q0(1);a11=0;a12=3*(qc(1)-q0(1))/tc^2;a13=-2*(qc(1)-q0(1))/tc^3; a20=q0(2);a21=0;a22=3*(qc(2)-q0(2))/tc^2;a23=-2*(qc(2)-q0(2))/tc^3; % File vẽ đồ thị figure(1); plot(tout,qd1,'c',tout,q1,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd1 va q1(rad)'); legend('qd1','q1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(2); plot(tout,vd1,'c',tout,v1,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd1 va v1 (rad/s)'); legend('vd1','v1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(3); plot(tout,qd2,'c',tout,q2,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd2 va q2(rad)'); legend('qd2','q2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(4); plot(tout,vd2,'c',tout,v2,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd2 va v2 (rad/s)'); legend('vd2','v2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on % - 59 ĐỒ ÁN CƠ ĐIỆN TỬ Với tín hiệu đặt điểm tác động cuối di chuyển từ điểm s0=[0.2;0.5] đến điểm sc=[0.6;0.4] theo quỹ đạo góc khớp bậc Ta thu kết sau : Hình 6.18 Góc khớp q1, q2 đặt điều khiển Hình 6.19 Vận tốc góc khớp 1, đặt điều khiển b, sơ đồ khối hệ thống điều khiển PD bù momen trọng lực 60 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.20 Sơ đồ khối với điều khiển PD bù trọng lực M dk = K p (qd − q ) + K d (q&d − q&) + G (q ) = K p ε + K d ε&+ G (q ) Với luật điều khiển : Hình 6.21 Bộ điều khiển PD bù trọng lực Trong Hình 6.21 có hàm sau g1: 0.5*(m1+2*m2)*g*a1*cos(u(2))+0.5*m2*g*a2*cos(u(1)+u(2)) g2: 0.5*m2*g*a2*cos(u(1)+u(2)) Kp=1000, Kd=500 Các khối ROBOT_RR, DAT, hien_thi file tham số mô phỏng, vẽ đồ thị phần a Ta thu kết sau với s0=[0.2;0.5], sc=[0.6;0.4] 61 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.22 Góc khớp q1, q2 đặt điều khiển Hình 6.23 Vận tốc góc khớp 1, đặt điều khiển c, sơ đồ khối hệ thống điều khiển momen tính toán &d + K p ε + K d ε& U dk = q& M dk = H (q )U dk + V ( q, q&) + G (q ) L uật điêu khiển: Với Hình 6.24 Sơ đồ khối với điều khiển momen tính toán 62 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.25 Khối momen tính toán Trong Hình 6.25 có hai hàm Fcn Fcn1 tính dùng để tính momen điều khiển theo luật điều nêu trên: Fcn tính M1dk: ((1/3)*m1*a1^2+(1/3)*m2*a2^2+m2*a1^2+m2*a1*a2*cos(u(2)))*u(5)+ ((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(2)))*u(6)-m2*a1*a2*sin(u(2))*u(3)*u(4)0.5*m2*a1*a2*sin(u(2))*u(4)^2+0.5*(m1+2*m2)*g*a1*cos(u(2)) +0.5*m2*g*a2*cos(u(1)+u(2)) Fcn1 tính M2dk: ((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(2)))*u(5)+ ((1/3)*m2*a2^2)*u(6)+0.5*m2*a1*a2*sin(u(2))*u(3)^2+0.5*m2*g*a2*cos(u(1)+ u(2)) Thông số Kp = 256, Kd = 16 Các khối ROBOT_RR, DAT, hien_thi file tham số mô phỏng, vẽ đồ thị phần a Ta thu kết sau với s0=[0.2;0.5], sc=[0.6;0.4] Hình 6.26 Góc khớp q1, q2 đặt điều khiển 63 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 6.27 Vận tốc góc khớp 1, đặt điều khiển Nhận xét: + Qua mô ta thấy điều khiển bù trọng lực với thông số Kp, Kd có chất lượng điều khiển tốt bám sát hơn, với lí thuyết khử G(q) + Còn với điều khiển momen tính toán có chất lượng tốt bám sắt mức độ tính toán phức tạp TÀI LIỆU THAM KHẢO [1] Động lực học hệ nhiều vật - GS TSKH Nguyễn Văn Khang 64 ĐỒ ÁN CƠ ĐIỆN TỬ [2] Điều khiển Robot công nghiệp – TS Nguyễn Mạnh Tiến [3] MATLAB & SIMULINK dành cho kĩ sư điều khiển tự động – Nguyễn Phùng Quang [4] Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang [5] Robot công nghiệp – GS TSKH Nguyễn Thiện Phúc [6] Mô hình hóa hệ thống mô – GS TS Nguyễn Công Hiền TS Nguyễn Phạm Thục Anh NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… 65 ĐỒ ÁN CƠ ĐIỆN TỬ ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Hà Nội, ngày 07 tháng 11 năm 2012 Giáo viên hướng dẫn TS Đỗ Đức Nam In màu 7,11,13,14,20,21,24,25,28,29,30,45,46,47,48,49,50,51,52 53,56,57,58,59 66 ĐỒ ÁN CƠ ĐIỆN TỬ 67 [...]... = 1 1 m1 a 12 + m2 a 22 + m2 a 12 + m2 a1 a2 cos(q2 ) 3 3 m 12 = m21 = m 22 = h11 h 12 h 21 h 22 1 1 m2 a 22 + m2 a1 a2 cos(q2 ) 3 2 1 m2 a 22 3 1 m2 a1 a2 sin(q2 )q &22 1 q2 − − m2 a1 a2 sin(q2 )q&& 2 V (q, q&) = 1 2 m2 a1a2 sin(q2 )q& 1 2 21 ĐỒ ÁN CƠ ĐIỆN TỬ 1 (m1 + 2m2 ) ga1 cos( q2 ) + m2 ga2 cos( q1 + q2 ) 2 2 G (q) = 1 m2 ga2 cos( q1 + q2 ) 2 M dk =... 1 2 2 2 2 & &2 m2 a1a2 cos(q2 ) ÷q& 1 + m2 a2 + M 1 = 3 m1 a1 + 3 m2 a2 + m2 a1 + m2 a1a2 cos(q2 ) ÷q& 3 2 (m + 2m2 ) 1 1 m2 a1a2 sin(q2 )q &22 + 1 ga1 cos(q2 ) + m2 ga2 cos(q1 + q2 ) − m2 a1a2 sin(q2 )q&& 1 q2 − 2 2 2 1 1 1 1 & 1 2 2 & & m2 a1a2 sin(q2 )q& 12 + m2 ga2 cos(q1 + q2 ) 1 + m2 a2 q2 + M 2 = 3 m2 a2 + 2 m2 a1 a2 cos(q2 ) ÷q& 3 2 2 Viết dưới dạng ma trận... m2 + cos(q2 ) ÷ = m2 a 22 + m2 a1a2 cos( q2 ) 2 2 4 3 m 22 = I 2 z + m2 2 1 a2 = m2 a 22 4 3 T => Động năng của cả hệ : 1 T = q &.M (q) q& 2 = (4.6) 1 1 1 1 1 1 m1 a 12 + m2 a 22 + m2 a 12 + m2 a1 a2 cos(q2 ) ÷q& 12 + m2 a 22 q &22 + 2 m2 a 22 + m2 a1 a2 cos(q2 ) ÷q&& 1 q2 2 3 3 3 2 3 2 Π = −∑ mi g T 0 0 r Ci i =1 Biểu thức thế năng của hệ : với go = [ 0 ,0 ,-g ]T 1 1 ... − n 2 b Thiết kế cho robot RR Các tham số như sau : a1= 0.5 (m), a2 = 0,4 (m) m1 =2. 5 (kg), m2=1.5 (kg) + Hệ phương trình động lực học có dạng như sau : &+ V ( q, q&) + G ( q) M = H ( q).q& với H=M(q)= m11 = 1 1 m1 a 12 + m2 a 22 + m2 a 12 + m2 a1 a2 cos(q2 ) 3 3 m 12 = m21 = m 22 = h11 h 12 h 21 h 22 1 1 m2 a 22 + m2 a1 a2 cos(q2 ) 3 2 1 m2 a 22 3 1 m2 a1a2 sin(q2 )q &22 1 q2 − − m2 a1a2 sin(q2... 2 z (4.4) m11 m 12 T M (q ) = ∑ ( J TiT mi J Ti + J Ri I i J Ri ) = m m i =1 21 22 2 I1 y = I1z = Thế các biểu thức (4.1),(4 .2) ,(4.3),(4.4) vào (4.5) và I2 y = I2z = 1 m2 a 22 12 (4.5) 1 m1a 12 12 , ta được tính được : 1 a2 m11 = I1z + I 2 z + m1a 12 + m2 a 12 + 2 + 2a1a2 cos( q2 ) ÷ 4 4 = 1 1 m1a 12 + m2 a 22 + m2 a 12 + m2 a1a2 cos(q2 ) 3 3 a 22 a1a2 1 1 m 12 = m21 = I 2 z + m2... q) τ = M (q).q& Với τ = [M1,M2]T, q = [ q1 , q2 ]T, M(q) ma trận khối lượng bên trên 1 m2 a1 a2 sin(q2 )q &22 1 q2 − −m2 a1 a2 sin(q2 )q&& 2 V (q, q&) = 1 2 m2 a1a2 sin(q2 )q&1 2 18 ĐỒ ÁN CƠ ĐIỆN TỬ 1 (m1 + 2m2 ) ga1 cos( q2 ) + m2 ga2 cos(q1 + q2 ) 2 2 G (q ) = 1 m2 ga2 cos(q1 + q2 ) 2 Phần V: Thiết kế điều khiển Tất cả các hệ thống điều khiển nêu dưới đây... tâm Ci trong hệ XcYc ZcCi song song với hệ tọa độ khâu ) : Khâu 1 2 xc a1 2 a2 2 yc zc mi 0 0 m1 0 0 Ixx Iyy Izz Ixy Iyz Izx 0 I1y I1z 0 0 0 m2 0 I2y I2z 0 0 I1 y = I1z = Với a1 = 0.5 (m), a2 = 0.4 (m), I2 y = I2z = 0 1 m1a 12 12 1 m2 a 22 12 Chọn tọa độ suy rộng tương đối q = [q1,q2]T Từ phần động học ta xác định được : 15 , ĐỒ ÁN CƠ ĐIỆN TỬ 0 r 0 C1 1 2 a1cos(q1 ) 1 a sin(q ) 1 2 1 0... fprintf(manh,' %2. 9f %2. 9f %2. 9f %2. 9f %2. 9f %2. 9f %2. 9f %3.9f %3.9f %2. 9f %2. 9f %2. 9f %2. 9f %2. 9f 35 ĐỒ ÁN CƠ ĐIỆN TỬ %2. 9f\n',t,qd(1),q(1),qd (2) ,q (2) ,ssq(1),ssq (2) ,M(1),M (2) ,vd(1),v(1),vd (2) ,v (2) ,s(1) ,s (2) ); end fclose(manh); fid = fopen('manhkhanh.txt', 'r'); a = fscanf(fid, '%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g', [15 inf]) fclose(fid); d1=a(1,:);%t d2=a (2, :);%qd1 d3=a(3,:);%q1 d4=a(4,:);%qd2 d5=a(5,:);%q2... a1a2 sin(q2 )q&& 2 V (q, q&) = 1 2 m2 a1a2 sin(q2 )q&1 2 1 (m1 + 2m2 ) ga cos( q ) + m2 ga2 cos( q1 + q2 ) 1 2 2 2 G (q) = 1 m2 ga2 cos( q1 + q2 ) 2 26 (5.8) ĐỒ ÁN CƠ ĐIỆN TỬ M dk = H (q )U + V ( q, q&) + G ( q ) + Phương trình luật điều khiển có dạng : , & & U = q& d + K pε + K d ε Với tín hiệu điều khiển : + Qũy đạo đặt cho hai khớp có dạng bậc ba đã thiết kế đảm... sánh kết quả với dùng thư viện simulink trong matlab) , Ta được kết quả như sau : Hình 5 .2 Góc khớp q1, q2 đặt và điều khiển 22 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 5.3 Sai số và Momen khớp 1, 2 Hình 5.4 Vận tốc khớp 1, 2 đặt và điều khiển 23 ĐỒ ÁN CƠ ĐIỆN TỬ Hình 5.5 Qũy đạo điểm tác động cuối 1 .2 Hệ thống điều khiển momen tính toán a.Luật điều khiển Hình 5.6 Sơ đồ điều khiển 24 ĐỒ ÁN CƠ ĐIỆN TỬ Phươngpháp cơ bản ... h11=((m1/3)+m2)*a1 ^2+ (1/3)*m2*a2 ^2+ m2*a1*a2*c2; h 12= (1/3)*m2*a2 ^2+ (1 /2) *m2*a1*a2*c2; h21=h 12; h 22= (1/3)*m2*a2 ^2; H=[h11,h 12; h21,h 22] ; %vector momen nhot V(q,v) j1=-m2*a1*a2*s2*x 12* x 22- 0.5*m2*a1*a2*s2*x 22^ 2; j2=0.5*m2*a1*a2*s2*x 12^ 2;... h 12= (1/3)*m2*a2 ^2+ (1 /2) *m2*a1*a2*c2; h21=h 12; h 22= (1/3)*m2*a2 ^2; H=[h11,h 12; h21,h 22] ; %vector momen nhot V(q,v) j1=-m2*a1*a2*s2*x 12* x 22- 0.5*m2*a1*a2*s2*x 22^ 2; j2=0.5*m2*a1*a2*s2*x 12^ 2; V=[j1;j2]; %... = 1 m1 a 12 + m2 a 22 + m2 a 12 + m2 a1 a2 cos(q2 ) 3 m 12 = m21 = m 22 = h11 h 12 h 21 h 22 1 m2 a 22 + m2 a1 a2 cos(q2 ) m2 a 22 m2 a1 a2 sin(q2 )q &22 q2 − − m2 a1 a2 sin(q2 )q&& V