Đang tải... (xem toàn văn)
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 CHUN NGÀNH Đề tài : Nghiên cứu, thiết kế, mơ phỏng robot cơng nghiệp 3 bậc tự do Giáo viên hướng dẫn : T.S Nguyễn Vơn Dim Họ và tên SV : Lại Đức Tồn Lớp : ĐHLT KTĐ ĐT K16D Thái Ngun 3/ 2019 LỜI NĨI ĐẦU Theo q trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người máy, các cánh tay máy tự động (Robot) để tạo ra các hệ sản xuất tự động linh hoạt Robot ứng dụng rộng rãi và đóng vai trò quan trọng sản xuất cũng như trong đời sống. Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di chuyển ngun vật liệu, các chi tiết, các dụng cụ thơng qua các truyền động được lập trình trước. Khoa học robot chủ yếu dựa vào các phép tốn về đại số ma trận Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot. Chính vì vậy em chọn đề tài : “Nghiên cứu, thiết kế, mơ phỏng robot cơng nghiệp 3 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 các thầy cơ giáo trong khoa Cơng nghệ TĐH, cùng các bạn trong lớp KTĐ – ĐT K16D, đặc biệt là 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 và Truyền thơng, người đã trực tiếp giảng dạy và cho em kiến thức để hồn thành đồ án này. Trong q trình làm đề tài mặc dù em đã nhiều cố gắng nhưng vẫn khơng tránh khỏi sai xót. Rất mong các thầy thơng cảm và giúp đỡ em nhiều hơn Em xin chân thành cảm ơn ! LỜI NÓI ĐẦU LỜI NÓI ĐẦU CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG 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 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 11 2.1 Sơ đồ động học robot 11 16 17 CHƯƠNG 3: THẾT KẾ MƠ HÌNH ROBOT BẬC TỰ DO 20 3.1 Giới thiệu phần mềm mô 20 3.1.1 Thiết kế cấu robot SolidWorks 20 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 toàn giới 20 Robot vẽ ghép lại phần mềm SolidWorks 2007 20 21 Hình 3.1 Cơ cấu robot 21 3.1.2 Thiết kế mơ hình robot MATLAB/ Simulink 21 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 toá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 21 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 21 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 21 Để 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 22 3.2.2 Điều khiển robot theo quỹ đạo thẳng 26 3.2.3 Điều khiển robot theo quỹ đạo tròn 29 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 .30 32 Hình 3.18 Quỹ đạo đặt- quỹ đạo ra- robot cuối hành trình 32 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ị 32 32 Hình 3.19 Tín hiệu góc quay ba khớp 32 KẾT LUẬN 33 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” .33 Báo cáo thực nhiệm vụ: 33 - Thiết kế robot trục quay SolidWorks xây dựng sơ đồ khối MATLAB/Simulink 33 - 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 33 - Thực mơ với kết tương đối xác 33 TÀI LIỆU THAM KHẢO 34 [1] TS Nguyễn Mạnh Tiến , Điều khiển robot công nghiệp, NXB Khoa học kỹ thuật 34 [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 34 [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 34 PHỤ LỤC 35 Chương trình mơ robot matlab symbolic robotics toolbox: 35 clear all % Xóa tất biến có Workspace 35 PHỤ LỤC…………………………………… … …………………………33 CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG 1.1. Sự ra đời của robot cơng nghiệp Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng tạo ra những dây chuyền về thiết bị tự động có tính linh hoạt cao đang hình thành. Các thiết bị này đang thay thế dần các máy tự động “cứng” chỉ đáp ứng một việc nhất định trong lúc thị trường ln ln đòi hỏi thay đổi mặt hàng về chủng loại, về kích cỡ và về tính năng v.v… Vì thế ngày càng tăng nhanh nhu cầu ứng dụng robot để tạo ra các hệ thống sản xuất linh hoạt Thuật ngữ “robot” lần đầu tiên xuất hiện vào khoảng năm 1921 trong tác phẩm “Rossum’s Universal Robot” của nhà viễn tưởng người Sec Karel Capek. Trong vở kịch này, ông dùng từ “robot”, biến thể của từ gốc Slavơ “Robota”, để gọi một thiết bị do con người tạo ta. Vào những năm 40 nhà văn viễn tưởng người Nga, Issac Asimov, mô tả robot là một chiếc máy tự động, mang diện mạo người Asimov đặt tên cho ngành nghiên cứu về robot là Robotics, trong đó có 3 ngun tắc cơ bản: Robot khơng gây tổn hại cho con người Hoạt động của robot phải tn theo các quy tắc do con người đặt ra. Các quy tắc này khơng được vi phạm ngun tắc thứ nhất Một robot cần phải bảo vệ sự sống của mình, nhưng khơng được vi phạm 2 ngun tắc trước. Và cũng trong những năm này, ước mơ viễn tưởng của Kerel Capek đã bắt đầu thành hiện thực. Ngay sau chiến tranh thế giới thứ 2, ở Hoa Kì đã xuất hiện những tay máy chép hình điều khiển từ xa trong các phòng thí nghiệm về vật liệu phóng xạ Vào những năm 1950 bên cạnh các tay máy chép hình cơ khí đó, đã xuất hiện các loại tay máy chép hình thủy lực và điện từ, như tay máy Minotaur I hoặc tay máy Handyman của General Electric. Năm 1954 George C. Devol đã thiết kế một thiết bị có tên là “cơ cấu bản 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 ra loại robot đầu tiên năm 1959 cơng ty Unimation. Chỉ đến năm 1975 cơng ty Unimation mới bắt đầu có lợi nhuận từ các sản phẩm robot đầu tiên này. Chiếc robot cơng nghiệp được đưa vào ứng dụng đầu tiên năm 1961 ở một nhà máy ơ tơ của General Motors tại Trenton, New Jersey Hoa Kỳ. Năm 1967 Nhật Bản mới nhập chiếc robot cơng nghiệp từ cơng ty AMF của Hoa Kỳ (American Machine and Foundry Company). Đến năm 1990 có hơn 40 cơng ty Nhật Bản, trong đó có những cơng ty khổng lồ như cơng ty Hitachi và cơng ty Mitsubishi đã đưa ra thị trường quốc tế nhiều loại robot 1.2. Các định nghĩa về robot cơng nghiệp Các nhà khoa học đã đưa ra rất nhiều định nghĩa về robot: Theo viện kỹ thuật robot của Hoa Kỳ: “Robot là 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 hiện một số thao tác sản xuất.” Theo ISO (International Standards Organization): “Robot cơng nghiệp là một tay máy đa mục tiêu, có một số bậc tự do dễ dàng lập trình, điều khiển, dùng để tháo lắp phơi, dụng cụ hoặc các vật dụng khác. Do chương trình thao tác có thể thay đổi nên thực hiện nhiều nhiệm vụ đa dạng.” Theo tiêu chuẩn AFNOR (Pháp): “ Robot cơng nghiệp là một cơ cấu chuyển động tự động có thể lập trình, lặp lại các chương trình, tổng hợp các chương trình đặt ra trên các trục tọa độ; có khả năng định vị, định hướng, di chuyển các đối tượng vật chất: chi tiết, dao cụ, gá lắp… theo những hành trình thay đổi đã chương trình hóa nhằm thực hiện các nhiệm vụ cơng nghệ khác nhau.” Theo RIA (Robot institute of America): “Robot là một tay máy vạn năng có thể lặp lại các chương trình được thiết kế để di chuyển vật liệu, chi tiết, dụng cụ hoặc cácthiết bị chun dùng thơng qua các chương trình chuyển động có thể thay đổi để hồn thành các nhiệm vụ khác nhau.” Theo ΓOCT 2568685 (Nga): “Robot cơng nghiệp là một tay máy tự động, được đặt cố định hoặc di động được, liên kết giữa một tay máy và một hệ thống điều khiển theo chương trình, có thể lập trình lại để hồn thành các chức năng vận động và điều khiển trong q trình sản xuất.” Theo tiêu chuẩn VDI 2860/BRD: “ Robot là một thiết bị có nhiều trục, thực hiện các chuyển động có thể chương trình hóa và nối ghép các chuyển động của chúng trong những khoảng cách tuyến tính hay tuyến tính của động trình. Chúng được điều khiển bởi các bộ phận hợp nhất ghép kết nối với nhau, có khả năng học và nhớ các chương trình; chúng được trang bị dụng cụ hoặc các phương tiện cơng nghệ khác để thực hiện các nhiệm vụ sản xuất trực tiếp và gián tiếp.” Theo tiêu chuẩn GHOST 1980: “Robot là máy tự động liên kết giữa một tay máy và một cụm điều khiển chương trình hóa, thực hiện một chu trình cơng nghệ một cách chủ động với sự điều khiển có thể thay thế chức năng tương tự của con người.” Bản chất của các định nghĩa khác nhau trên đây giúp ta thấy được một ý nghĩa quan trọng là: Riêng một mình robot thì khơng thể hồn thành tốt được cơng việc. Nó phải được liên hệ chặt chẽ với máy móc, cơng cụ và các thiết bị cơng nghệ tự động khác trong một hệ thống tự động tổng hợp. Do đó trong q trình phân tích và thiết kế, khơng thể quan niệm robot như một đơn vị cấu trúc biệt lập, trái lại đó phải là những thiết kế tổng thể của “hệ thống tự động linh hoạt robot hóa” cho phép thích ứng nhanh và đơn giản khi nhiệm vụ sản xuất thay đổi 1.3. Tay máy robot 1.3.1. Kết cấu của tay máy Tay máy là phần cơ sở nó quyết định đến khả năng làm việc của robot. Đó là thiết bị cơ khí đảm bảo cho robot khả năng chuyển động trong khơng gian để thực hiện các nhiệm vụ như nâng hạ, vận chuyển, lắp ráp Tay máy robot thơng thường là cơ cấu hở gồm một chuỗi các khâu liên kết với nhau bằng các khớp, khâu đầu tiên được nối với giá cố định. Khớp tạo sự linh hoạt giữa các khâu với nhau nói riêng và tồn bộ tồn bộ tay máy robot cơng nghiệp nói chung. Thơng qua khớp nối, các khâu trong cơ cấu tay máy được chuyển động tương đối với nhau. Tùy theo u cầu về kết cấu của robot mà ta lựa chọn loại khớp liên kết giữa các khâu khác nhau. Trong robot cơng nghiệp hiện nay, người ta thường dùng chủ yếu hai loại khớp là khớp quay và khớp trượt Khớp quay: (thường được kí hiệu là R) loại khớp này cho phép chuyển động quay của khâu này và khâu khác quanh một trục quay. Loại khớp này hạn chế năm khả năng chuyển động giữa hai thành phần khớp do đó có một bậc tự do Hình 3.16. Khối tính sai lệch Hình 3.17. Khối phản hồi 31 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 độ chính xác cao tuy nhiên có thời điểm robot bị quay ngược do góc đặt khớp biến đổi tức thời trong thời gian ngắn thời điểm kết thúc chu kì trong đồ thị dưới Hình 3.19. Tín hiệu góc quay của ba khớp 32 KẾT LUẬN Sau một q trình học tập và nghiên cứu cùng với sự hướng dẫn của thầy giáo Tiến sĩ Nguyễn Vơn Dim và sự cố gắng nỗ lực em đã hồn thành báo cáo thực tập chun ngành về đề tài “Nghiên cứu, thiết kế, mơ phỏng robot cơng nghiệp 3 bậc tự do” Báo cáo đã thực hiện các nhiệm vụ: Thiết kế robot 3 trục quay trên SolidWorks và xây dựng sơ đồ khối trên MATLAB/Simulink Xây dựng phương trình động học thuận, động học ngược và lựa chọn các tham số bộ điều khiển PD cho robot 3 trục quay Thực hiện mơ phỏng với kết quả tương đối chính xác 33 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 và kỹ thuật [2]. Nguyễn Phùng Quang , MATLAB và Simulink dành cho kĩ sư điều khiển tự động, NXB Khoa học và 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 và mơ phỏng, NXB Khoa học và kỹ thuật 34 PHỤ LỤC Chương trình mơ phỏng robot trên matlab symbolic và robotics toolbox: clear all % Xóa tất cả các biến hiện có ở Workspace clc % Xóa mọi dòng trên Command Window startup_rvc % Khởi động Robotics Toolbox (file strartup_rvc của ROBOTICS TOOLBOX phải đang nằm ở thư % mục hiện thời (Current Directory của MATLAB % Khai báo các symbolic variables cùng các điều kiện của biến syms q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 m1 m2 m3 t g % q1, q2, q3 là các biến khớp % a1, a2, a3 là độ dài các thanh % dq1, dq2, dq3 là đạo hàm của q1, q2, q3 % m1, m2, m3 là khối lượng các thanh % t : biến thời gian % g : gia tốc trọng trường assume(a1,'real');assume(a1>0); % Có nghĩa là ta cho MATLAB biết a1 là số thực dương assume(a2,'real');assume(a2>0); % việc này sẽ giúp cho việc rút gọn biểu thức được hiệu quả hơn assume(a3,'real');assume(a3>0); assume(q1,'real'); assume(q2,'real'); 35 assume(q3,'real'); assume(dq1,'real'); assume(dq2,'real'); 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 các tọa độ suy rộng q dq = [dq1;dq2;dq3]; % Vector các vận tốc dài % Nhập các ma trận DH A_01=[ cos(q1) 0 sin(q1) a1*cos(q1); sin(q1) 0 cos(q1) a1*sin(q1); 0 1 0 0;0 0 0 1]; A_12=[ cos(q2) sin(q2) 0 a2*cos(q2); sin(q2) cos(q2) 0 a2*sin(q2); 0 0 1 0;0 0 0 1]; A_23=[ cos(q3) sin(q3) 0 a3*cos(q3); sin(q3) cos(q3) 0 a3*sin(q3); 0 0 1 0;0 0 0 1]; % Tính các 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 khi tính tốn ta thu gọn kết quả ngay bằng lệnh simplify R_02=A_02(1:3,1:3); disp('Ma tran chuyen tu khau 3 sang khau tac dong cuoi la') 36 A_03 = simplify(A_03) R_03 = A_03(1:3,1:3); disp(' ') % Tạo mơ hình Robot trong Robotics toolbox L(1)=Link([0,0,5,pi/2,0]); % Lệnh Link tạo một khâu của 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 một robot nối tiếp gồm các khâu của L % Giải bài tốn động học thuận disp('Giai bai toan dong hoc thuan') % 1. Tìm vi trí và tính vận tốc dài của 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 % 2. Tìm góc Cardan và 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)] 37 % 3. 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 5 3 2})) 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 3 2 1 5 3 2})) disp('Van toc goc:') sub_R_0E = simplify(subs(R_0E,{a1 a2 a3},{5 3 2})); sub_omega = simplify(subs(omega,{q1 q2 q3 dq1 dq2 dq3 a1 a2 a3},{t 2*t 3*t 3 2 1 5 3 2})) % 4. Tính tốn và 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 trong bai toan thuan') hold on grid on axis([10, 10, 10, 10 ,5, 5]) pause for j=1:length(time) 38 plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+'); % Vẽ quĩ đạo chuyển động bằng MATLAB plot(rob,[3*time(j),2*time(j),time(j)]); % Vẽ hình ảnh chuyển động 3D của Robot theo quĩ đạo pause(1/30) % Dùng lệnh pause để tạo cảm giác giống như một đoạn phim end % Giải bài tốn động học ngược figure(2) clf hold on grid on rE_solve = subs(rE,{a1 a2 a3},{5 3 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); % các phương trình động học Robot f2 = rE_solve(2,1)yE(i); 39 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 của hệ phương trình động học end subplot(311) plot(time,q1_num(1,:)) grid 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 trong 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+') 40 pause(1/5) end % Giải bài toán động lực học(Solve the dynamics problem) % Cac tensor quan tinh (inertial moment) I_c1 = [0 0 0;0 m1*a1^2/12 0;0 0 m1*a1^2/12]; I_c2 = [0 0 0;0 m1*a2^2/12 0;0 0 m1*a2^2/12]; I_c3 = [0 0 0;0 m1*a3^2/12 0;0 0 m1*a3^2/12]; % Toa do cac trong tam (center of mass in moving coordinate) A_c1 = [1 0 0 a1/2;0 1 0 0;0 0 1 0;0 0 0 1]; A_c2 = [1 0 0 a2/2;0 1 0 0;0 0 1 0;0 0 0 1]; A_c3 = [1 0 0 a3/2;0 1 0 0;0 0 1 0;0 0 0 1]; % Chuyen sang toa do 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 do cac trong tam trong he toa do 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) 41 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)]); 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 0;1 0 0;0 0 0]; JR2 = [sin(q2) 0 0;0 cos(q2) 0;0 0 0]; JR3 =[sin(q2+q3) 0 0;0 cos(q2+q3) 0;0 1 1]; % Các Jacobian vận tốc góc disp('Dong nang cua cac khau') % Tính tốn động năng từ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) 42 % Manipulator Inertia Matrix (Tính ma trận khối lượng của 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 thế năng Potential Energy (PE) PE = (1/2*m2+m3)*a2*cos(q2)*g+1/2*m3*a3*cos(q3)*g; % 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) = hdiff(PE,q(j)); 43 end Psi=simplify(Psi) % Statics of Robot syms Fx Fy Fz Mx My Mz % Các lực và 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_E3P_3) M_32 = simplify(M_E3+R_03*MatrixCurve(r_3)*F_32 R_03*MatrixCurve(r_c3)*P_3) F_21 = simplify(F_32P_2) M_21 = simplify(M_32+R_02*MatrixCurve(r_2)*F_21 R_02*MatrixCurve(r_c2)*P_2) F_10 = simplify(F_21P_1) M_10 = simplify(M_21+R_01*MatrixCurve(r_1)*F_10 R_01*MatrixCurve(r_c1)*P_1) 44 45 ... 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” .33 Báo. .. 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 ... đờ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 1.3.2 Bậc tự robot 1.3.3 Vùng làm việc robot