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ả //www.petercorke.com/Robotics_Toolbox.html. Link download toolbox: //www.petercorke.com/RTB. Hướng dẫn sử dụng TOOLBOX: //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.
- Robot được sử dụng ở đây là Robot RRR 3 bậc tự do được cho trên hình sau.
- 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[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
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
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
disp['Ma tran chuyen tu khau 3 sang khau tac dong cuoi la']
% 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
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
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]
omega = [omega_curve[3,2] omega_curve[1,3] omega_curve[2,1]]
% 3. Thay so bai toan dong hoc thuan
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}]]
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}]]
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ị
num_rE=zeros[3,length[time]];
num_rE[:,j] = subs[sub_rE,t,time[j]];
title['Quy dao cua khau tac dong cuoi trong bai toan thuan']
axis[[-10, 10, -10, 10 ,-5, 5]]
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
% Giải bài toán động học ngược
rE_solve = subs[rE,{a1 a2 a3},{5 3 2}];
xE = zeros[1,j];yE = zeros[1,j];zE = zeros[1,j];
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];
[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
title['Quy dao cua khau tac dong cuoi trong bai toan nguoc']
axis[[-10, 10, -10, 10 ,-5, 5]]
plot[rob,[q1_num[1,i],q2_num[1,i],q3_num[1,i]]]
plot3[xE[i],yE[i],zE[i],'b+']
% 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_c2 = simplify[A_02*A_c2]
A_0_c3 = simplify[A_03*A_c3]
disp['Toa do cac trong tam trong he toa do co dinh']
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 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']
M = JT1.'*m1*JT1+JT2.'*m2*JT2+JT3.'*m3*JT3+JR1.'*I_c1*JR1+JR2.'*I_c2*JR2+JR3.'*I_c3*JR3;
% 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[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];
Psi[j] = -h-diff[PE,q[j]];
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]
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
Ta sử dụng luật điều khiển
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 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: R[s] = Kp + Kds
Phương trình đặc tính hệ kín của hệ có dạng:
Mô hình robot và bộ điều khiển PD