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

Trang 1

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.

Trang 2

• 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(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

Trang 3

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

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;

Trang 4

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)

grid

subplot(313)

grid

Trang 5

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_c1 = simplify(v_0_c1.'*v_0_c1);

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);

Trang 6

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)

% 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(' 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

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)

Trang 7

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

Trang 8

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ố Kd, Kp> 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 Kdquá 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

Trang 9

Mô hình Robot RRR

Các bạn có thể tham khảo thêm trong file simulink đính kèm

Định dạng
Số trang	9
Dung lượng	504,84 KB