Khảo sát robot 3 bậc tự do sử dụng MATLAB, SIMULINK và ROBOTICS (code MATLAB)

SYMBOLIC TOOLBOX của MATLAB sẽ được sử dụng để giải bài toán độnghọc Robot theo phương pháp Denavit-Hartenberg, tính toán tĩnh học, lậpphương trình động lực học.• ROBOTICS TOOLBOX là một TOOLBOX miễn phí được viết bởi Peter Corkegiúp tính toán và vẽ mô hình robot 3D một cách nhanh chóng. Các bạn có thểdownload toolbox này từ trang web của tácgiả http://www.petercorke.com/Robotics_Toolbox.html. Link downloadtoolbox: http://www.petercorke.com/RTB. Hướng dẫn sử dụngTOOLBOX: http://www.petercorke.com/RTB/robot.pdf.• Cuối cùng, ta sử dụng SIMULINK để mô phỏng Robot và thử nghiệm thuậttoán điều khiển

http://codientu.org Khảo sát Robot 3 bậc tự do sử dụng MATLAB/SIMULINK và ROBOTICS TOOLBOX Để khảo sát các Robot, hiện nay ở Đại học Bách Khoa Hà Nội sinh viên thường sử dụng hai phần mềm là MAPLE hoặc MATLAB. Bài này sẽ giới thiệu cách khảo sát một robot bằng công cụ MATLAB/SIMULINK và ROBOTICS TOOLBOX. • SYMBOLIC TOOLBOX của MATLAB sẽ được sử dụng để giải bài toán động học Robot theo phương pháp Denavit-Hartenberg, tính toán tĩnh học, lập phương trình động lực học. • ROBOTICS TOOLBOX là một TOOLBOX miễn phí được viết bởi Peter Corke giúp tính toán và vẽ mô hình robot 3D một cách nhanh chóng. Các bạn có thể download toolbox này từ trang web của tác giả http://www.petercorke.com/Robotics_Toolbox.html. Link download toolbox: http://www.petercorke.com/RTB. Hướng dẫn sử dụng TOOLBOX: http://www.petercorke.com/RTB/robot.pdf. • Cuối cùng, ta sử dụng SIMULINK để mô phỏng Robot và thử nghiệm thuật toán điều khiển. Trong hướng dẫn này, mình sẽ không nhắc lại các kiến thức cơ bản của Robot. Các kiến thức này nếu cần các bạn có thể tham khảo thêm ở cuốn sách nổi tiếng của Tsai: "Robot Analysis" Các chỉ dẫn của bài này chỉ là các comment trong chương trình tính toán Robot. 1. Đối tượng Robot RRR • Robot được sử dụng ở đây là Robot RRR 3 bậc tự do được cho trên hình sau. http://codientu.org • Các tọa độ suy rộng được chọn theo phương pháp Denavit - Hartenberg như trên hình. • Bảng D-H được cho như sau 2. Khảo sát ROBOT với MATLAB SYMBOLIC và ROBOTICS TOOLBOX % Chương trình giải bài toán ROBOT bằng MATLAB SYMBOLIC 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'); 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 D-H A_01=[ cos(q1) 0 sin(q1) a1*cos(q1); sin(q1) 0 -cos(q1) a1*sin(q1); 0 1 0 0;0 http://codientu.org 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 toá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') 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 toá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 toá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)] % 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 toán và vẽ đồ thị time=0:0.02:2*pi/6; http://codientu.org 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) 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 toá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); 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 http://codientu.org 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+') 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) 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); http://codientu.org 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 toá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) % 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) = -h-diff(PE,q(j)); 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âu cuố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_32-R_03*MatrixCurve(r_c3)*P_3) F_21 = simplify(F_32-P_2) http://codientu.org M_21 = simplify(M_32+R_02*MatrixCurve(r_2)*F_21-R_02*MatrixCurve(r_c2)*P_2) F_10 = simplify(F_21-P_1) M_10 = simplify(M_21+R_01*MatrixCurve(r_1)*F_10-R_01*MatrixCurve(r_c1)*P_1) 3. Thiết kế, mô phỏng bộ điều khiển ROBOT bằng Simulink Mô hình robot trên Simulink Từ phương trình động lực học Robot: Từ phương trình (2) ta xây dựng được mô hình Robot trên Simulink. Đầu vào gồm các giá trị góc khớp, vận tốc góc mômen tác dụng, tải và điều kiện van đầu về vị trí và vận tốc. Các giá trị này cho vào khối tính toán theo phương trình thứ 2 ở trên rồi đưa qua hai khối tích phân liên tiếp. Đầu ra là giá trị góc khớp và vận tốc góc hiện thời. Xây dựng bộ điều khiển cho robot Robot là một hệ phi tuyến mạnh, các thành phần trong mô hình toán học của nó thay đổi theo thời gian do đó việc nhận dạng và điều khiển nó là rất khó khăn. Với sự hoàn thiện của lý thuyết điều khiển tuyến tính, ta sẽ tìm cách chuyển hệ Robot phi tuyến về hệ tuyến tính để thiết kế thuật toán điều khiển. Bằng cách sử dụng phương pháp nghịch đảo mô hình, Với thao tác đơn giản này ta đã chuyển hệ MIMO phi tuyến về hệ 3 hệ SISO tuyến tính, cách ly hoàn toàn. Do đó ta chỉ phải thiết kế bộ điều khiển cho đối tượng SISO http://codientu.org có hàm truyền G(s) = 1/s^2. . Đến đây, vì trong hàm truyền đã có sẵn khâu tích phân, ta sử dụng bộ điều khiển PD quen thuộc. Suy ra phương trình vi phân sai số, mô tả hệ thống sau khi có thêm bộ điều khiển là: Phương trình đặc tính hệ kín của hệ có dạng: Ta cần xác định các tham số K d , K p > 0 sao cho hệ ổn định. Sử dụng tiêu chuẩn tối ưu động học ta tính được Ta không thể chọn K d quá lớn vì hệ sẽ dễ dao động và tín hiệu điều khiển tính ra từ bộ điều khiển ban đầu lớn có thể vượt quá khả năng đáp ứng của cơ cấu chấp hành. Mô hình robot và bộ điều khiển PD http://codientu.org Mô hình Robot RRR Các bạn có thể tham khảo thêm trong file simulink đính kèm . http://codientu.org Khảo sát Robot 3 bậc tự do sử dụng MATLAB/ SIMULINK và ROBOTICS TOOLBOX Để khảo sát các Robot, hiện nay ở Đại học Bách Khoa Hà Nội sinh viên thường sử dụng hai phần mềm là MAPLE hoặc MATLAB. Bài. r_c3 = [a3,0,0].'; F _32 = simplify(F_E3-P _3) M _32 = simplify(M_E3+R_ 03* MatrixCurve(r _3) *F _32 -R_ 03* MatrixCurve(r_c3)*P _3) F_21 = simplify(F _32 -P_2) http://codientu.org M_21 = simplify(M _32 +R_02*MatrixCurve(r_2)*F_21-R_02*MatrixCurve(r_c2)*P_2) F_10. 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); http://codientu.org 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)