Nội dung của báo cáo trình bày tổng quan về hệ thống robot công nghiệp; xây dựng phương trình động học thuận và ngược cho robot 3 bậc tự do; thiết kế mô hình robot 3 bậc tự do. Mời các bạn cùng tham khảo báo cáo để nắm chi tiết nội dung nghiên cứu.
TRƯỜNG ĐẠI HỌC CNTT VÀ TRUYỀN THÔNG KHOA CÔNG NGHỆ TỰ ĐỘNG HÓA BÁO CÁO THỰC TẬP CHUYÊN NGÀNH Đề tài : Nghiên cứu, thiết kế, mô robot công nghiệp bậc tự Giáo viên hướng dẫn : T.S Nguyễn Vôn Dim Họ tên SV : Lại Đức Toàn Lớp : ĐHLT- KTĐ- ĐT- K16D Thái Nguyên 3/ 2019 LỜI NĨI ĐẦU Theo q trình phát triển xã hội, nhu cầu nâng cao sản xuất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tiện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành phát triển mạnh mẽ…Vì ngày tăng nhanh nhu cầu ứng dụng người máy, cánh tay máy tự động (Robot) để tạo hệ sản xuất tự động linh hoạt Robot ứng dụng rộng rãi đóng vai trò quan trọng sản xuất đời sống Robot cấu đa chức có khả lập trình dùng để di chuyển nguyên vật liệu, chi tiết, dụng cụ thông qua truyền động lập trình trước Khoa học robot chủ yếu dựa vào phép toán đại số ma trận Robot có cánh tay với nhiều bậc tự thực chuyển động tay người điều khiển máy tính điều khiển chương trình nạp sẵn chip bo mạch điều khiển robot Chính em chọn đề tài : “Nghiên cứu, thiết kế, mô robot công nghiệp bậc tự do” Đầu tiên em xin chân thành gửi lời cảm ơn sâu sắc tới thầy cô giáo khoa Công nghệ TĐH, bạn lớp KTĐ – ĐT K16D, đặc biệt thầy giáo Tiến sĩ Nguyễn Vôn Dim giảng viên trường Đại học Công nghệ thông tin Truyền thông, người trực tiếp giảng dạy cho em kiến thức để hoàn thành đồ án Trong trình làm đề tài em nhiều cố gắng khơng tránh khỏi sai xót Rất mong thầy thông cảm giúp đỡ em nhiều Em xin chân thành cảm ơn ! LỜI NÓI ĐẦU LỜI NÓI ĐẦU .1 CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG .1 1.1 Sự đời robot công nghiệp 1.2 Các định nghĩa robot công nghiệp 1.3 Tay máy robot 1.3.1 Kết cấu tay máy .4 1.3.2 Bậc tự robot 1.3.3 Vùng làm việc robot 1.4 Ưu điểm robot công nghiệp 1.5 Tình hình tiếp cận robot Việt Nam ứng dụng robot công nghiệp 1.5.1 Tình hình tiếp cận robot Việt Nam 1.5.2 Ứng dụng robot công nghiệp CHƯƠNG 2: XÂY DỰNG CHƯƠNG TRÌNH ĐỘNG HỌC THUẬN VÀ NGƯỢC CHO ROBOT BẬC TỰ DO 10 2.1 Sơ đồ động học robot 10 15 15 CHƯƠNG 3: THẾT KẾ MƠ HÌNH ROBOT BẬC TỰ DO .19 3.1 Giới thiệu phần mềm mô 19 3.1.1 Thiết kế cấu robot SolidWorks 19 SolidWorks phần mềm CAD chiều chạy hệ điều hành Windows phát triển bởi Dassault Systèmes SolidWorks Corp, công ty Dassault Systèmes, SA ( Vélizy, Pháp) SolidWorks sử dụng 1,3 triệu kỹ sư nhà thiết kế 130.000 cơng ty tồn giới .19 Robot vẽ ghép lại phần mềm SolidWorks 2007 19 19 Hình 3.1 Cơ cấu robot 19 3.1.2 Thiết kế mơ hình robot MATLAB/ Simulink 19 MATLAB mơi trường tính tốn số lập trình, thiết kế công ty MathWorks MATLAB cho phép tính tốn số với ma trận, vẽ đồ thị hàm số hay biểu đồ thơng tin, thực thuật tốn, tạo giao diện người dùng liên kết với chương trình máy tính viết nhiều ngơn ngữ lập trình khác .19 Với thư viện Toolbox, MATLAB cho phép mơ tính tốn, thực nghiệm nhiều mơ hình thực tế kỹ thuật 20 Simulink phần chương trình mở rộng MATLAB nhằm mục đích mơ hình hóa mô khảo sát hệ thống động học Giao diện đồ họa hình Simulink cho phép thể hệ thống dạng sơ đồ tín hiệu với khối chức quên thuộc 20 Để thực toán thiết kế điều khiển cho robot, ta sử dụng thư viện Simmechanics phần mềm "SolidWorks-to-SimMechanics Translator” để chuyển đổi cấu robot từ SolidWorks sang dạng khối Simulink 20 3.2.2 Điều khiển robot theo quỹ đạo thẳng 24 3.2.3 Điều khiển robot theo quỹ đạo tròn 27 3.2.4 Kết hợp điều khiển robot theo quỹ đạo tròn điều khiển phản hồi PD .28 30 Hình 3.18 Quỹ đạo đặt- quỹ đạo ra- robot cuối hành trình 30 Nhận xét: Robot chuyển động theo quỹ đạo với độ xác cao nhiên có thời điểm robot bị quay ngược góc đặt khớp biến đổi tức thời thời gian ngắn - thời điểm kết thúc chu kì đồ thị 30 30 Hình 3.19 Tín hiệu góc quay ba khớp .30 KẾT LUẬN 31 Sau trình học tập nghiên cứu với hướng dẫn thầy giáo Tiến sĩ Nguyễn Vôn Dim cố gắng nỗ lực em hoàn thành báo cáo thực tập chuyên ngành đề tài “Nghiên cứu, thiết kế, mô robot công nghiệp bậc tự do” .31 Báo cáo thực nhiệm vụ: 31 - Thiết kế robot trục quay SolidWorks xây dựng sơ đồ khối MATLAB/Simulink 31 - Xây dựng phương trình động học thuận, động học ngược lựa chọn tham số điều khiển PD cho robot trục quay 31 - Thực mơ với kết tương đối xác 31 TÀI LIỆU THAM KHẢO 32 [1] TS Nguyễn Mạnh Tiến , Điều khiển robot công nghiệp, NXB Khoa học kỹ thuật.32 [2] Nguyễn Phùng Quang , MATLAB Simulink dành cho kĩ sư điều khiển tự động, NXB Khoa học kỹ thuật 32 [3] GS.TS Nguyễn Cơng Hiền, TS Nguyễn Phạm Thục Anh, Mơ hình hóa hệ thống mô phỏng, NXB Khoa học kỹ thuật .32 PHỤ LỤC 33 Chương trình mơ robot matlab symbolic robotics toolbox: 33 clear all % Xóa tất biến có Workspace 33 PHỤ LỤC…………………………………… … …………………………33 CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG 1.1 Sự đời robot công nghiệp Nhu cầu nâng cao suất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tiện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành Các thiết bị thay dần máy tự động “cứng” đáp ứng việc định lúc thị trường ln ln đòi hỏi thay đổi mặt hàng chủng loại, kích cỡ tính v.v… Vì ngày tăng nhanh nhu cầu ứng dụng robot để tạo hệ thống sản xuất linh hoạt Thuật ngữ “robot” lần xuất vào khoảng năm 1921 tác phẩm “Rossum’s Universal Robot” nhà viễn tưởng người Sec Karel Capek Trong kịch này, ông dùng từ “robot”, biến thể từ gốc Slavơ “Robota”, để gọi thiết bị người tạo ta Vào năm 40 nhà văn viễn tưởng người Nga, Issac Asimov, mô tả robot máy tự động, mang diện mạo người Asimov đặt tên cho ngành nghiên cứu robot Robotics, có nguyên tắc bản: - Robot không gây tổn hại cho người - Hoạt động robot phải tuân theo quy tắc người đặt Các quy tắc không vi phạm nguyên tắc thứ - Một robot cần phải bảo vệ sống mình, không vi phạm nguyên tắc trước Và năm này, ước mơ viễn tưởng Kerel Capek bắt đầu thành thực Ngay sau chiến tranh giới thứ 2, Hoa Kì xuất tay máy chép hình điều khiển từ xa phòng thí nghiệm vật liệu phóng xạ Vào năm 1950 bên cạnh tay máy chép hình khí đó, xuất loại tay máy chép hình thủy lực điện từ, tay máy Minotaur I tay máy Handyman General Electric Năm 1954 George C Devol thiết kế thiết bị có tên “cơ cấu lề dùng để chuyển hàng theo chương trình” Đến năm 1956 Devol với kĩ sư trẻ công nghiệp hàng không Joseph F.Engelber, tạo loại robot năm 1959 công ty Unimation Chỉ đến năm 1975 cơng ty Unimation bắt đầu có lợi nhuận từ sản phẩm robot Chiếc robot công nghiệp đưa vào ứng dụng năm 1961 nhà máy ô tô General Motors Trenton, New Jersey Hoa Kỳ Năm 1967 Nhật Bản nhập robot công nghiệp từ công ty AMF Hoa Kỳ (American Machine and Foundry Company) Đến năm 1990 có 40 cơng ty Nhật Bản, có cơng ty khổng lồ cơng ty Hitachi công ty Mitsubishi đưa thị trường quốc tế nhiều loại robot 1.2 Các định nghĩa robot công nghiệp Các nhà khoa học đưa nhiều định nghĩa robot: - Theo viện kỹ thuật robot Hoa Kỳ: “Robot loại tay máy nhiều chức năng, với chương trình làm việc thay đổi được, dùng để thực số thao tác sản xuất.” - Theo ISO (International Standards Organization): “Robot công nghiệp tay máy đa mục tiêu, có số bậc tự dễ dàng lập trình, điều khiển, dùng để tháo lắp phôi, dụng cụ vật dụng khác Do chương trình thao tác thay đổi nên thực nhiều nhiệm vụ đa dạng.” - Theo tiêu chuẩn AFNOR (Pháp): “ Robot công nghiệp cấu chuyển động tự động lập trình, lặp lại chương trình, tổng hợp chương trình đặt trục tọa độ; có khả định vị, định hướng, di chuyển đối tượng vật chất: chi tiết, dao cụ, gá lắp… theo hành trình thay đổi chương trình hóa nhằm thực nhiệm vụ công nghệ khác nhau.” - Theo RIA (Robot institute of America): “Robot tay máy vạn lặp lại chương trình thiết kế để di chuyển vật liệu, chi tiết, dụng cụ cácthiết bị chun dùng thơng qua chương trình chuyển động thay đổi để hồn thành nhiệm vụ khác nhau.” - Theo ΓOCT 25686-85 (Nga): “Robot công nghiệp tay máy tự động, đặt cố định di động được, liên kết tay máy hệ thống điều khiển theo chương trình, lập trình lại để hồn thành chức vận động điều khiển trình sản xuất.” - Theo tiêu chuẩn VDI 2860/BRD: “ Robot thiết bị có nhiều trục, thực chuyển động chương trình hóa nối ghép chuyển động chúng khoảng cách tuyến tính hay tuyến tính động trình Chúng điều khiển phận hợp ghép kết nối với nhau, có khả học nhớ chương trình; chúng trang bị dụng cụ phương tiện công nghệ khác để thực nhiệm vụ sản xuất trực tiếp gián tiếp.” - Theo tiêu chuẩn GHOST 1980: “Robot máy tự động liên kết tay máy cụm điều khiển chương trình hóa, thực chu trình cơng nghệ cách chủ động với điều khiển thay chức tương tự người.” Bản chất định nghĩa khác giúp ta thấy ý nghĩa quan trọng là: Riêng robot khơng thể hồn thành tốt cơng việc Nó phải liên hệ chặt chẽ với máy móc, cơng cụ thiết bị cơng nghệ tự động khác hệ thống tự động tổng hợp Do q trình phân tích thiết kế, quan niệm robot đơn vị cấu trúc biệt lập, trái lại phải thiết kế tổng thể “hệ thống tự động linh hoạt robot hóa” cho phép thích ứng nhanh đơn giản nhiệm vụ sản xuất thay đổi 1.3 Tay máy robot 1.3.1 Kết cấu tay máy Tay máy phần sở định đến khả làm việc robot Đó thiết bị khí đảm bảo cho robot khả chuyển động không gian để thực nhiệm vụ nâng hạ, vận chuyển, lắp ráp Tay máy robot thông thường cấu hở gồm chuỗi khâu liên kết với khớp, khâu nối với giá cố định Khớp tạo linh hoạt khâu với nói riêng tồn tồn tay máy robot cơng nghiệp nói chung Thơng qua khớp nối, khâu cấu tay máy chuyển động tương Tùy theo yêu cầu kết cấu robot mà ta lựa chọn loại khớp liên kết khâu khác Trong robot công nghiệp nay, người ta thường dùng chủ yếu hai loại khớp khớp quay khớp trượt Khớp quay: (thường kí hiệu R) loại khớp cho phép chuyển động quay khâu khâu khác quanh trục quay Loại khớp hạn chế năm khả chuyển động hai thành phần khớp có bậc tự Hình 1.1 Khớp quay Khớp trượt: (thường kí hiệu T) loại khớp cho phép hai khâu trượt tương theo phương trục hạn chế năm khả chuyển động khớp có bậc tự Hình Khớp trượt Ngồi số trường hợp người ta dùng khớp cầu để tăng tính linh hoạt cho robot Với loại khớp cho phép khâu thực chuyển động quay theo tất hướng qua tâm khớp, hạn chế chuyển động tịnh tiến khâu Do khớp cầu có số bậc tự ba Trong trình thiết kế tay máy robot, người ta quan tâm đến thông số ảnh hưởng lớn đến khả làm việc robot như: - Sức nâng, độ cứng vững, lực kẹp tay… - Tầm với hay vùng làm việc: kích thước hình dáng vùng mà phần cơng tác với tới - Sự khéo léo robot: thông số liên quan đến bậc tự robot 1.3.2 Bậc tự robot Bậc tự số khả chuyển động tịnh tiến quay cấu Để dịch chuyển vật thể không gian, cấu chấp hành robot phải đạt số bậc tự Nói chung hệ robot cấu hở nên số bậc tự tính theo cơng thức: w = 6n − ∑ ipi i =1 Hình 3.14 Quỹ đạo đặt- quỹ đạo ra- robot điểm cuối hành trình 3.2.4 Kết hợp điều khiển robot theo quỹ đạo tròn điều khiển phản hồi PD Hình 3.15 Mơ hình điều khiển robot 28 Hình 3.16 Khối tính sai lệch Hình 3.17 Khối phản hồi 29 Hình 3.18 Quỹ đạo đặt- quỹ đạo ra- robot cuối hành trình Nhận xét: Robot chuyển động theo quỹ đạo với độ xác cao nhiên có thời điểm robot bị quay ngược góc đặt khớp biến đổi tức thời thời gian ngắn - thời điểm kết thúc chu kì đồ thị Hình 3.19 Tín hiệu góc quay ba khớp 30 KẾT LUẬN Sau trình học tập nghiên cứu với hướng dẫn thầy giáo Tiến sĩ Nguyễn Vôn Dim cố gắng nỗ lực em hoàn thành báo cáo thực tập chuyên ngành đề tài “Nghiên cứu, thiết kế, mô robot công nghiệp bậc tự do” Báo cáo thực nhiệm vụ: - Thiết kế robot trục quay SolidWorks xây dựng sơ đồ khối MATLAB/Simulink - Xây dựng phương trình động học thuận, động học ngược lựa chọn tham số điều khiển PD cho robot trục quay - Thực mô với kết tương đối xác 31 TÀI LIỆU THAM KHẢO [1] TS Nguyễn Mạnh Tiến , Điều khiển robot công nghiệp, NXB Khoa học kỹ thuật [2] Nguyễn Phùng Quang , MATLAB Simulink dành cho kĩ sư điều khiển tự động, NXB Khoa học kỹ thuật [3] GS.TS Nguyễn Công Hiền, TS Nguyễn Phạm Thục Anh, Mơ hình hóa hệ thống mơ phỏng, NXB Khoa học kỹ thuật 32 PHỤ LỤC Chương trình mơ robot matlab symbolic robotics toolbox: clear all clc % Xóa tất biến có Workspace % Xóa dòng Command Window startup_rvc % Khởi động Robotics Toolbox (file strartup_rvc ROBOTICS TOOLBOX phải nằm thư % mục thời (Current Directory MATLAB % Khai báo symbolic variables điều kiện biến syms q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 m1 m2 m3 t g % q1, q2, q3 biến khớp % a1, a2, a3 độ dài % dq1, dq2, dq3 đạo hàm q1, q2, q3 % m1, m2, m3 khối lượng % t : biến thời gian % g : gia tốc trọng trường assume(a1,'real');assume(a1>0); % Có nghĩa ta cho MATLAB biết a1 số thực dương assume(a2,'real');assume(a2>0); % việc giúp cho việc rút gọn biểu thức hiệu assume(a3,'real');assume(a3>0); assume(q1,'real'); assume(q2,'real'); assume(q3,'real'); assume(dq1,'real'); assume(dq2,'real'); 33 assume(dq3,'real'); assume(t,'real');assume(t>0); assume(m1,'real');assume(m1>0); assume(m2,'real');assume(m2>0); assume(m3,'real');assume(m3>0); assume(g,'real');assume(g>0); q = [q1;q2;q3]; % Vector tọa độ suy rộng q dq = [dq1;dq2;dq3]; % Vector vận tốc dài % Nhập ma trận D-H A_01=[ cos(q1) sin(q1) a1*cos(q1); sin(q1) -cos(q1) a1*sin(q1); 0;0 0 1]; A_12=[ cos(q2) -sin(q2) a2*cos(q2); sin(q2) cos(q2) a2*sin(q2); 0 0;0 0 1]; A_23=[ cos(q3) -sin(q3) a3*cos(q3); sin(q3) cos(q3) a3*sin(q3); 0 0;0 0 1]; % Tính ma trận truyền R_01=A_01(1:3,1:3); A_03=A_01*A_12*A_23; A_02=simplify(A_01*A_12); % Sau tính tốn ta thu gọn kết lệnh simplify R_02=A_02(1:3,1:3); disp('Ma tran chuyen tu khau sang khau tac dong cuoi la') A_03 = simplify(A_03) R_03 = A_03(1:3,1:3); disp(' ') 34 % Tạo mơ hình Robot Robotics toolbox L(1)=Link([0,0,5,pi/2,0]); % Lệnh Link tạo khâu Robot L(2)=Link([0,0,3,0,0]); L(3)=Link([0,0,2,0,0]); rob=SerialLink(L); % Lệnh SerialLink(L) tạo robot nối tiếp gồm khâu L % Giải toán động học thuận disp('Giai bai toan dong hoc thuan') % Tìm vi trí tính vận tốc dài khâu thao tác rE = A_03(1:3,4) % Vector tọa độ khâu thao tác v_qE = simplify(jacobian(rE,q)*dq) % Tính vector vận tốc khâu tác động cuối % Tìm góc Cardan Tính tốn van toc góc cua khâu thao tác R_0E = A_03(1:3,1:3) diff_R_0E = diff(R_0E,q1)*dq1+diff(R_0E,q2)*dq2+diff(R_0E,q3)*dq3; %Tinh dao ham cua R omega_curve = diff_R_0E*R_0E.'; omega_curve = simplify(omega_curve) disp('Van toc goc:') omega = [omega_curve(3,2) omega_curve(1,3) omega_curve(2,1)] % Thay so bai toan dong hoc thuan disp(' ') disp('Thay so') % Khi thay số ta sử dụng lệnh subs disp('Vi tri diem tac dong cuoi') sub_rE = simplify(subs(rE,{q1 q2 q3 a1 a2 a3},{3*t 2*t t 2})) 35 disp('Van toc dai:') sub_diff_qE = simplify(subs(v_qE,{q1 q2 q3 dq1 dq2 dq3 a1 a2 a3},{t 2*t 3*t 2})) disp('Van toc goc:') sub_R_0E = simplify(subs(R_0E,{a1 a2 a3},{5 2})); sub_omega = simplify(subs(omega,{q1 q2 q3 dq1 dq2 dq3 a1 a2 a3},{t 2*t 3*t 2})) % Tính tốn vẽ đồ thị time=0:0.02:2*pi/6; num_rE=zeros(3,length(time)); for j=1:length(time) num_rE(:,j) = subs(sub_rE,t,time(j)); end figure(1) clf title('Quy dao cua khau tac dong cuoi bai toan thuan') hold on grid on axis([-10, 10, -10, 10 ,-5, 5]) pause for j=1:length(time) plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+'); % Vẽ quĩ đạo chuyển động MATLAB plot(rob,[3*time(j),2*time(j),time(j)]); % Vẽ hình ảnh chuyển động 3D Robot theo quĩ đạo pause(1/30) % Dùng lệnh pause để tạo cảm giác giống đoạn phim end 36 % Giải toán động học ngược figure(2) clf hold on grid on rE_solve = subs(rE,{a1 a2 a3},{5 2}); time=0:0.1:3; j=length(time); q1_num=zeros(2,j); q2_num=zeros(2,j); q3_num=zeros(2,j); xE = zeros(1,j);yE = zeros(1,j);zE = zeros(1,j); for i=1:j xE(i) = 6*sin(time(i)); yE(i) = 6*cos(time(i)); zE(i) = 3; f1 = rE_solve(1,1)-xE(i); % phương trình động học Robot f2 = rE_solve(2,1)-yE(i); f3 = rE_solve(3,1)-zE(i); f = [f1 f2 f3]; [q1_num(:,i) q2_num(:,i) q3_num(:,i)] = solve(f1,f2,f3); % Dùng lệnh solve() để tìm nghiệm hệ phương trình động học end subplot(311) plot(time,q1_num(1,:)) grid 37 subplot(312) plot(time,q2_num(1,:)) grid subplot(313) grid plot(time,q3_num(1,:)) grid figure(3) clf title('Quy dao cua khau tac dong cuoi bai toan nguoc') hold on grid on axis([-10, 10, -10, 10 ,-5, 5]) pause close(2) for i=1:length(time) plot(rob,[q1_num(1,i),q2_num(1,i),q3_num(1,i)]) plot3(xE(i),yE(i),zE(i),'b+') pause(1/5) end % Giải toán động lực học(Solve the dynamics problem) % Cac tensor quan tinh (inertial moment) I_c1 = [0 0;0 m1*a1^2/12 0;0 m1*a1^2/12]; I_c2 = [0 0;0 m1*a2^2/12 0;0 m1*a2^2/12]; I_c3 = [0 0;0 m1*a3^2/12 0;0 m1*a3^2/12]; 38 % Toa cac tam (center of mass in moving coordinate) A_c1 = [1 0 -a1/2;0 0;0 0;0 0 1]; A_c2 = [1 0 -a2/2;0 0;0 0;0 0 1]; A_c3 = [1 0 -a3/2;0 0;0 0;0 0 1]; % Chuyen sang toa co dinh (transforming into fixed coordinate) A_0_c1 = A_01*A_c1 A_0_c2 = simplify(A_02*A_c2) A_0_c3 = simplify(A_03*A_c3) % Cac ma tran vi tri disp(' ') disp('Toa cac tam he toa co dinh') r_0_c1 = A_0_c1(1:3,4) r_0_c2 = A_0_c2(1:3,4) r_0_c3 = A_0_c3(1:3,4) disp('Van toc cac khoi tam') v_0_c1 = simplify(jacobian(r_0_c1,q)*dq) v_0_c2 = simplify(jacobian(r_0_c2,q)*dq) v_0_c3 = simplify(jacobian(r_0_c3,q)*dq) v_c1 = simplify(v_0_c1.'*v_0_c1); v_c2 = simplify(v_0_c2.'*v_0_c2); v_c3 = simplify(v_0_c1.'*v_0_c3); JT1=simplify(jacobian(r_0_c1,q)); % Các Jacobian vận tốc dài JT2=simplify(jacobian(r_0_c2,q)); JT3=simplify([diff(r_0_c3,q1),diff(r_0_c2,q2),diff(r_0_c3,q3)]); 39 disp('Van toc goc cac khau') tmp = A_0_c1(1:3,1:3); C2 = A_0_c2(1:3,1:3); C3 = A_0_c3(1:3,1:3); W_c1=tmp.'*(diff(tmp,q1)*dq1+diff(tmp,q2)*dq2+diff(tmp,q3)*dq3); w_c1=simplify([W_c1(3,2);W_c1(1,3);W_c1(2,1)]) W_c2=C2.'*(diff(C2,q1)*dq1+diff(C2,q2)*dq2+diff(C2,q3)*dq3); w_c2=simplify([W_c2(3,2);W_c2(1,3);W_c2(2,1)]) W_c3=C3.'*(diff(C3,q1)*dq1+diff(C3,q2)*dq2+diff(C3,q3)*dq3); w_c3=simplify([W_c3(3,2);W_c3(1,3);W_c3(2,1)]) JR1 = [0 0;1 0;0 0]; JR2 = [sin(q2) 0;0 cos(q2) 0;0 0]; JR3 =[sin(q2+q3) 0;0 cos(q2+q3) 0;0 1]; % Các Jacobian vận tốc góc disp('Dong nang cua cac khau') % Tính tốn động khâu T1 = simplify(1/2*m1*v_c1.'*v_c1+1/2*w_c1.'*I_c1*w_c1) T2 = simplify(1/2*m1*v_c2.'*v_c2+1/2*w_c2.'*I_c2*w_c2) T3 = simplify(1/2*m1*v_c3.'*v_c3+1/2*w_c3.'*I_c3*w_c3) % Manipulator Inertia Matrix (Tính ma trận khối lượng Robot) disp('Phuong trinh dong luc hoc tong quat co dang:') disp(' M.ddq - Psi - Q = U') disp(' Trong do:') M= JT1.'*m1*JT1+JT2.'*m2*JT2+JT3.'*m3*JT3+JR1.'*I_c1*JR1+JR2.'*I_c2* JR2+JR3.'*I_c3*JR3; M = simplify(M) % Biểu thức - Potential Energy (PE) PE = (1/2*m2+m3)*a2*cos(q2)*g+1/2*m3*a3*cos(q3)*g; 40 % Centrifugal/Coriolis matrix and Psi (các ma trận lực hướng tâm/ Coriolis) tmp = sym(zeros(3)); Psi = sym(zeros(3,1)); for j=1:3 h = sym(0); for k=1:3 for l=1:3 tmp(k,l) = 1/2*(diff(M(k,l),q(l))+diff(M(l,j),q(k))diff(M(k,l),q(j)))*dq(k)*dq(l); h = h+tmp(k,l); end end Psi(j) = -h-diff(PE,q(j)); end Psi=simplify(Psi) % Statics of Robot syms Fx Fy Fz Mx My Mz % Các lực momen tác dụng vào khâucuối assume(Fx,'real');assume(Fy,'real');assume(Fz,'real'); assume(Mx,'real');assume(My,'real');assume(Mz,'real'); F_E3 = [Fx, Fy, Fz].'; M_E3 = [Mx, My, Mz].'; P_3 = [0,0,-m3*g].';P_2 = [0,0,-m2*g].';P_1 = [0,0,-m1*g].'; r_1 = [a1,0,0].'; r_2 = [a2,0,0].'; r_3 = [a3,0,0].'; r_c1 = [a1,0,0].'; r_c2 = [a2,0,0].'; r_c3 = [a3,0,0].'; F_32 = simplify(F_E3-P_3) M_32 = simplify(M_E3+R_03*MatrixCurve(r_3)*F_32R_03*MatrixCurve(r_c3)*P_3) 41 F_21 = simplify(F_32-P_2) M_21 = simplify(M_32+R_02*MatrixCurve(r_2)*F_21R_02*MatrixCurve(r_c2)*P_2) F_10 = simplify(F_21-P_1) M_10 = simplify(M_21+R_01*MatrixCurve(r_1)*F_10R_01*MatrixCurve(r_c1)*P_1) 42 ... học tập nghiên cứu với hướng dẫn thầy giáo Tiến sĩ Nguyễn Vôn Dim cố gắng nỗ lực em hoàn thành báo cáo thực tập chuyên ngành đề tài Nghiên cứu, thiết kế, mô robot công nghiệp bậc tự do” .31 Báo. .. việc robot 1.4 Ưu điểm robot công nghiệp 1.5 Tình hình tiếp cận robot Việt Nam ứng dụng robot công nghiệp 1.5.1 Tình hình tiếp cận robot Việt Nam 1.5.2 Ứng dụng robot. .. nhiều bậc tự thực chuyển động tay người điều khiển máy tính điều khiển chương trình nạp sẵn chip bo mạch điều khiển robot Chính em chọn đề tài : Nghiên cứu, thiết kế, mô robot công nghiệp bậc