26 Chương trình thực hiện trên matlab như sau
%thiet ke quy dao bac 3 cho cac khop robor MPL 800II
figure(1);
%quy dao vi tri khop 1
x=[ 0 :0.5:5];
y1=5.4*x.^2-0.72*x.^3; subplot(3,1,1);
plot(x,y1);
title('do thi vi tri khop 1'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao van toc khop 1
y2=10.8*x-2.16*x.^2; subplot(3,1,2);
plot(x,y2);
title('do thi van toc khop 1'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao gia toc khop 1
y3=10.8-4.32*x; subplot(3,1,3); plot(x,y3);
title('do thi gia toc khop 1'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao vi tri khop 2
figure(2);
y4=60-3.6*x.^2+0.48*x.^3; subplot(3,1,1);
plot(x,y4);
title('do thi vi tri khop 2'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao van toc khop 2
y5=-7.2*x+1.44*x.^2; subplot(3,1,2);
plot(x,y5);
title('do thi van toc khop 2'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao gia toc khop 2
y6=-7.2+2.88*x; subplot(3,1,3); plot(x,y6);
27
title('do thi gia toc khop 2'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao vi trí khop 3
figure(3);
y7=-5.4*x.^2+0.72*x.^3; subplot(3,1,1);
plot(x,y7);
title('do thi vi tri khop 3'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao van toc khop 3
y8=-10.8*x+2.16*x.^2; subplot(3,1,2);
plot(x,y8);
title('do thi van toc khop 3'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao gia toc khop 3
y9=-10.8+4.32*x; subplot(3,1,3); plot(x,y9);
title('do thi gia toc khop 3'); xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao vi tri khop 4
figure(4);
y10=30-10.8*x.^2+1.44*x.^3; subplot(3,1,1);
plot(x,y10);
title('do thi vi tri khop 4') xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao van toc khop 4
y11=-21.6*x+4.32*x.^2; subplot(3,1,2);
plot(x,y11);
title('do thi van toc khop 4') xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
%quy dao gia toc khop 4
y12=-21.6+8.64*x; subplot(3,1,3);
28
plot(x,y12);
title('do thi gia toc khop 4') xlabel('thoi gian(s)');
ylabel('bien do'); grid on;
29
CHƯƠNG 6. THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO
ROBOT MPL800 II THEO THUẬNT TOÁN PID
6.1 Tính toán động lực học cho Robot
Hàm Lagrange: L=K-P
Trong đó: K: là tổng động năng của hệ thống P: Là tổng thế năng của hệ thống.
Từ hàm Lagrange được xác định bằng biểu thức sau:
1. Nếu i là khớp quay (𝜃𝑖), khi đó momen cần đặt lên khớp i là:
𝑇𝑖 = 𝑑 𝑑𝑡 ( 𝜕𝐿 𝜕𝜃𝑖̇) − 𝜕𝐿 𝜕𝜃𝑖 (6.1) 2. Nếu i là khớp trượt (𝑟𝑖) 𝐹𝑖 = 𝑑 𝑑𝑡 ( 𝜕𝐿 𝜕𝑟𝑖) − 𝜕𝐿 𝜕𝑟𝑖 (6.2) Động năng khớp nối thứ i: 𝐾𝑖 =1 2𝑚𝑖𝑣𝑖 2+1 2𝐽𝑖𝜔𝑖 2 (6.3)
Với Ji là momen quán tính của khớp nối thứ i
Tham số Thanh nối 1 Thanh nối 2 Thanh nối 3
Chiều dài 2L1 2L2 2L3
Khối lượng M1 M2 M3
Vận tốc V1 V2 V3
Vị trí tâm khối L1 L2 L3
Tọa độ thanh nối 1:
{ 𝑥𝐶1 = 𝐿1𝐶𝑜𝑠( 𝜃1) 𝑦𝐶1 = 𝐿1𝑆𝑖𝑛( 𝜃1) 𝑧𝑐1 = 𝑑 (6.4) Tốc độ thanh nối 1: { 𝑥𝑐1 • = −𝐿1𝑆𝑖𝑛( 𝜃1)𝜃•1 𝑦•𝑐1= 𝐿1𝐶𝑜𝑠( 𝜃1)𝜃•1 𝑧𝑐1• = 0 (6.5)
Tọa độ thanh nối 2:
{
𝑥𝑐2 = (2𝐿1+ 𝐿2𝐶𝑜𝑠( 𝜃2)) 𝐶𝑜𝑠( 𝜃2) 𝑦𝑐2 = (2𝐿1+ 𝐿2𝐶𝑜𝑠( 𝜃2)) 𝑆𝑖𝑛( 𝜃2) 𝑧𝑐2 = 𝑑 + 𝐿2𝑆𝑖𝑛( 𝜃2)
30 Vận tốc thanh nối 2: { 𝑥𝑐2 • = −(2𝐿1+ 𝐿2𝐶𝑜𝑠( 𝜃2)) 𝑆𝑖𝑛( 𝜃1)𝜃•1− 𝐿2𝜃•2𝑆𝑖𝑛( 𝜃2) 𝐶𝑜𝑠( 𝜃1) 𝑦•𝑐2 = (2𝐿1+ 𝐿2𝐶𝑜𝑠( 𝜃2)) 𝐶𝑜𝑠( 𝜃1)𝜃•1− 𝐿2𝜃•2𝑆𝑖𝑛( 𝜃2) 𝑆𝑖𝑛( 𝜃1) 𝑧𝑐2• = 𝐿2𝐶𝑜𝑠( 𝜃2)𝜃•2 (6.7) Thanh nối 3: { 𝑥𝑐3 = (2𝐿1+ 2𝐿2𝐶𝑜𝑠( 𝜃2) + 𝐿3𝐶𝑜𝑠( 𝜃2+ 𝜃3)) 𝐶𝑜𝑠( 𝜃1) 𝑦𝑐3 = (2𝐿1+ 2𝐿2𝐶𝑜𝑠( 𝜃2) + 𝐿3𝐶𝑜𝑠( 𝜃2+ 𝜃3)) 𝑆𝑖𝑛( 𝜃1) 𝑧𝑐3 = 𝑑 + 𝐿2𝑆𝑖𝑛( 𝜃2) + 𝐿3𝑆𝑖𝑛( 𝜃2+ 𝜃3) (6.8) Vận tốc thanh nối 3 { 𝑥𝑐3 • = (−2𝐿2𝑆𝑖𝑛( 𝜃2)𝜃•2− 𝐿3𝑆𝑖𝑛( 𝜃2+ 𝜃3)𝜃•2− 𝐿3𝑆𝑖𝑛( 𝜃2+ 𝜃3)𝜃•3) 𝐶𝑜𝑠( 𝜃1) −(2𝐿1+ 2𝐿2𝐶𝑜𝑠( 𝜃2) + 𝐿3𝐶𝑜𝑠(𝜃2+ 𝜃3)) 𝑆𝑖𝑛( 𝜃1)𝜃•1 𝑦𝑐3• = −(2𝐿1+ 2𝐿2𝐶𝑜𝑠( 𝜃2) + 𝐿3𝐶𝑜𝑠( 𝜃3)) 𝐶𝑜𝑠( 𝜃1)𝜃•1+ (6.9) (−2𝐿2𝑆𝑖𝑛( 𝜃2)𝜃•2− 𝐿3𝑆𝑖𝑛( 𝜃2+ 𝜃3)𝜃•2− 𝐿3𝑆𝑖𝑛( 𝜃2+ 𝜃3)𝜃•3) 𝑆𝑖𝑛( 𝜃1) 𝑧𝑐3• = 𝐿2𝐶𝑜𝑠( 𝜃2)𝜃•2+ 𝐿3𝐶𝑜𝑠( 𝜃2+ 𝜃3)(𝜃•2+ 𝜃•3)
Khai báo vị trí trong Matlab:
%%vitri V1=[2*L1*cos(th1);2*L1*sin(th1);d] V2=[(2*L1+L2*cos(th2))*cos(th1);(2*L1+L2*cos(th2))*sin(th1 );L2*sin(th2)+d] V3=[(2*L1+2*L2*cos(th2)+L3*cos(th3))*cos(th1);(2*L1+2*L2*c os(th2)+L3*cos(th3))*sin(th1);2*L2*sin(th2)+L3*sin(th2+th3 )+d]
Khai báo tốc độ trong Matlab:
%%speed syms th1 L1 thc1 Pc1=[-L1*sin(th1)*thc1 L1*cos(th1)*thc1 0] Pc1=simplify(Pc1); syms L2 th2 thc2 Pc2=[(-L2*thc2*sin(th2))*cos(th1)- (2*L2+L2*cos(th2))*sin(th1)*thc1 (- L2*sin(th2)*thc2)*sin(th1)+(2*L1+L2*cos(th2))*thc1*cos(th1 ) L2*cos(th2)*thc2] Pc2=simplify(Pc2); syms L3 th3 thc3 Pc3=[(-2*L2*thc2*sin(th2)-L3*thc3*sin(th3))*cos(th1)- (2*L1+2*L2*cos(th2)+L3*cos(th3))*sin(th1)*thc1 (-
31 2*L2*thc2*sin(th2)- L3*thc3*sin(th3))*sin(th1)+(2*L1+2*L2*cos(th2)+L3*cos(th3) )*cos(th1)*thc1 2*L2*thc2*cos(th2)+L3*(thc2+thc3)*cos(th2+th3)] Pc3=simplify(Pc3);
Do các khớp đều là khớp quay, do đó Momen cần đặt lên các khớp là:
𝑀𝑖 = 𝑑 𝑑𝑡 ( 𝜕𝐿 𝜕𝜃𝑖̇) − 𝜕𝐿 𝜕𝜃𝑖 (6.10)
Khai báo 𝜃•𝑖trong Matlab là thc1
Do đó thành phần (𝜕𝐿
𝜕𝜃𝑖̇) được tính toán bằng hàm: dldthci=diff(L,thci)
𝜕𝐿
𝜕𝜃𝑖 được tính toán bằng hàm: dldthi=diff(L,thi)
%%momen dat len cac khop
%tinh dao ham lagrange theo cac bien
dldthc1=diff(L,thc1) dldthc2=diff(L,thc2) dldthc3=diff(L,thc3) dldth1=diff(L,th1) dldth2=diff(L,th2) dldth3=diff(L,th3) Với thành phần 𝑑 𝑑𝑡 (𝜕𝐿
𝜕𝜃𝑖̇) ta không lấy được đạo hàm trực tiếp bằng Matlab do các thành phần theta_i là các hàm theo thời gian. Do đó ta quy ước th2c_i là đạo hàm cấp 2 theo thời gian của các thành phần thc_i. Khi đó thành phần 𝑑
𝑑𝑡 (𝜕𝐿
𝜕𝜃𝑖̇) sẽ được tính bằng cách nhân các đạo hàm riêng theo các biến rồi nhân với đạo hàm các biến đó.
%%momen dat len cac khop( deu la khop quay)
syms th2c1 th2c2 th2c3 %%dao ham cap hai cua theta_i
M1=diff(dldthc1,th1)*thc1+diff(dldthc1,th2)*thc2+diff(dldt hc1,th3)*thc3+diff(dldthc1,thc1)*th2c1+diff(dldthc1,thc2)* th2c2+diff(dldthc1,thc3)*th2c3-dldth1 M2=diff(dldthc2,th1)*thc1+diff(dldthc2,th2)*thc2+diff(dldt hc2,th3)*thc3+diff(dldthc2,thc1)*th2c1+diff(dldthc2,thc2)* th2c2+diff(dldthc2,thc3)*th2c3-dldth2 M3=diff(dldthc3,th1)*thc1+diff(dldthc3,th2)*thc2+diff(dldt hc3,th3)*thc3+diff(dldthc3,thc1)*th2c1+diff(dldthc3,thc2)* th2c2+diff(dldthc3,thc3)*th2c3-dldth3
Kết quả thu được là biểu thức momen đặt lên các khớp:
Do kết quả thu được rất phức tạp, nên không đưa vào bài trình bày này, kết quả lưu ở m_file
32
6.2 Thuật toán điều khiển Robot
Nhóm em sẽ sử dụng bộ điều khiển PD để điều khiển cho Robot M = H.(q̅).q̈ + V(q̅ , q̅̇) + G(g)
Mđk = KP. ɛ̅ + KD. ɛ̅̇ (6.11)
Trong đó
{
ɛ̅ = q̅̅̅ − q d ̅ là sai số vị trí của khớp robot ɛ̅̇ = q̅̅̅̇ − q̅̇ là sai số tốc độ của khớp robotd KP là ma trận đường chéo các hệ số khuếch đại
KD là ma trận đường chéo các hệ số đạo hàm
Nhóm em tính toán và lựa chọn bộ ma trận Kp và Kd để điều khiển Robot là:
KP = [ 90 0 0 0 90 0 0 0 90 ] ; KD = [ 2025 0 0 0 2025 0 0 0 2025 ]
Triển khai thuật toán trong Matlab bằng Matlab funtion, phần quan trọng là thiết lập công thức tính toán mô men đặt lên các trục robot
E = W - Q; E_dot = W_dot - Q_dot;
K1 = [90 0 0; 0 90 0; 0 0 90];
K2 = [2025 0 0; 0 2025 0; 0 0 2025];
U = M*(W_2dot + K1*E + K2*E_dot) + C*Q_dot + G;
M1 = U(1); M2 = U(2); M3 = U(3);
Xây dựng giao diện để cài đặt thông số vị trí mà tay máy muốn tiếp cận, sau đó tính toán đưa ra mô men thích hợp để điều khiển tay máy đến vị trí đó:
33