3.

Điều khiển Robot

 Phương trình động lực có dạng:

M (q)q  C (q, q)q  G (q)  Q  U (1)

 Chọn luật điều khiển

U   u   (2)
Trong đó:
   M (q)
   C ( q, q ) q  G ( q )  Q ( q )
 u  qd ( n1)  Kv (nn ) E(n1)  K p (nn ) E(n1)
ở đây qd ( n1) là giá trị gia tốc mong muốn (giá trị đặt)
E( n1) , E( n1) sai lệch vị trí, vận tốc giá trị đặt so với giá trị thực
e1  e1 
e  e 
E( n1)  qd ( n1)  q( n1)   2  ; E( n1)  qd ( n1)  q( n1)   2  .
:  : 
   
en  en 

 Phương trình vi phân hệ kín đối với sai lệch vị trí

Thay (2) vào (1), chọn bộ thông số Kv, Kp. Ta được:

E  Kv E  K p E  0
Trong đó:
 E   e1 , e2 , e3  ; với ei  qdi  qi
T

 E   e1 , e2 , e3  ;
T

 E   e1 , e2 , e3  .
T

 kv1 0 0   20 0 0 
 Kv  0 kv 2 0    0 20 0 

   
 0 0 kv 3   0 0 20
 k p1 0 0  100 0 0 
  
 K p   0 k p 2 0    0 100 0  .
0 0 k p 3   0 0 100 

 Mô phỏng:

 Khối điều khiển PD:

 Khối phần tử phi tuyến tính lực điều khiển:

Code MATLAB Function:

function y = fcn(u2)
%thong so
a3=0.4;
d1=0.6;
a2=0.6;
m1=16;
m2=12;
m3=6;
q =u2(1:3);
dq = u2(4:6);
u=u2(7:9);
%tinh M(alpha)
m11=-(1/4)*cos(q(3))^2*(a3^2)*m3+(1/3)*m3*(a3^2)-
m3*a3*sin(q(3))*q(2)+m2*(q(2)^2)+m3*(q(2)^2)-
m2*a2*q(2)+(1/3)*m2*(a2^2)+(1/12)*m1*(d1^2);
m22=m2+m3;
m23=-(1/2)*m3*a3*cos(q(3));
m32=-(1/2)*m3*a3*cos(q(3));
m33=-(1/12)*m3*(a3^2)*(cos(q(1))^2-4);
M=[m11 0 0;0 m22 m23;0 m32 m33];
%tinh beta
beta1=-7.2*dq(1)*dq(2)+36.*dq(1)*dq(2)*q(2)-
2.4*dq(1)*dq(2)*sin(q(3))+0.48*dq(1)*dq(3)*cos(q(3))*sin(q(3))-
2.4*dq(1)*dq(3)*cos(q(3))*q(2)-0.08*sin(q(1))*cos(q(1))*(dq(3)^2)-
16*cos(q(1))*sin(q(3))+80*cos(q(1))*q(2);
beta2=3.60*(dq(1)^2)-
18.*(dq(1)^2)*q(2)+1.20*(dq(1)^2)*sin(q(3))+1.20*(dq(3)^2)*sin(q(3))+80.*sin(
q(1));
beta3=-
0.24*(dq(1)^2)*cos(q(3))*sin(q(3))+1.2*(dq(1)^2)*cos(q(3))*q(2)+0.16*sin(q(1)
)*cos(q(1))*dq(1)*dq(3)+0.24*sin(q(3))-16*sin(q(1))*cos(q(3))-20*cos(q(1));

Beta=[beta1;beta2;beta3];

y = M*[u(1);u(2);u(3)]+Beta;

 Khối cơ cấu chấp hành:

 Intergrator2: Khối tích phân gia tốc(ddq) => output là vận tốc dq.

 Intergrator1: Khối tích phân vận tốc(dq) => output là vị trí q.

 Code Dynamic Function:

function ddq = RobotDynamic(t)

%thong so
a3=0.4;
d1=0.6;
a2=0.6;
m1=16;
m2=12;
m3=6;
q=t(1:3);
dq= t(4:6);
u=t(7:9);
%tinh M(alpha)
m11=-(1/4)*cos(q(3))^2*(a3^2)*m3+(1/3)*m3*(a3^2)-
m3*a3*sin(q(3))*q(2)+m2*(q(2)^2)+m3*(q(2)^2)-
m2*a2*q(2)+(1/3)*m2*(a2^2)+(1/12)*m1*(d1^2);
m22=m2+m3;
m23=-(1/2)*m3*a3*cos(q(3));
m32=-(1/2)*m3*a3*cos(q(3));
m33=-(1/12)*m3*(a3^2)*(cos(q(1))^2-4);
M=[m11 0 0;0 m22 m23;0 m32 m33];
%tinh beta
beta1=-7.2*dq(1)*dq(2)+36.*dq(1)*dq(2)*q(2)-
2.4*dq(1)*dq(2)*sin(q(3))+0.48*dq(1)*dq(3)*cos(q(3))*sin(q(3))-
2.4*dq(1)*dq(3)*cos(q(3))*q(2)-0.08*sin(q(1))*cos(q(1))*(dq(3)^2)-
16*cos(q(1))*sin(q(3))+80*cos(q(1))*q(2);
beta2=3.60*(dq(1)^2)-
18.*(dq(1)^2)*q(2)+1.20*(dq(1)^2)*sin(q(3))+1.20*(dq(3)^2)*sin(q(3))+80.*sin(
q(1));
beta3=-
0.24*(dq(1)^2)*cos(q(3))*sin(q(3))+1.2*(dq(1)^2)*cos(q(3))*q(2)+0.16*sin(q(1)
)*cos(q(1))*dq(1)*dq(3)+0.24*sin(q(3))-16*sin(q(1))*cos(q(3))-20*cos(q(1));

Beta=[beta1;beta2;beta3];

% Returning Robot's accelerations

ddq = inv(M)*([u(1);u(2);u(3)]-Beta);

 Đồ thị:

Đồ thị lực điều khiển U

Đồ thị vị trí khâu 1 so với giá trị đặt Đồ thị sai lệch khâu 1

Đồ thị vị trí khâu 2 so với giá trị đặt Đồ thị sai lệch khâu 2