7. Control de modo deslizante basado en el método de momento calculado

El método de cálculo de par es un método más comúnmente utilizado en el control de robots, este método se basa en los valores estimados de cada elemento en el modelo de robot para diseñar la ley de control.

7.1 Descripción del sistema

El modelo del robot manipulador es:

$\ pequeña H \ izquierda (q \ derecha) \ ddot {q} + C \ izquierda (q, \ punto {q} \ derecha) \ punto {q} + G \ izquierda (q \ derecha) = \ tau$ 7,20 （

Entre ellos se $\ tiny H \ left (q \ right)$ encuentra la matriz de inercia de masa definida positiva, $\ minúscula C \ izquierda (q, \ punto {q} \ derecha)$ la fuerza de Coriolis , la fuerza centrífuga y la $\ pequeña G \ izquierda (q \ derecha)$ gravedad.

7.2 Diseño de la ley de control

Cuando no se conocen los parámetros inerciales del robot, según el método de momento calculado, la ley de control se toma como

$\ tiny \ tau = \ hat {H} \ left (q \ right) v + \ hat {C} \ left (q, \ dot {q} \ right) \ dot {q} + \ hat {G} \ left ( q \ derecha)$ （7.21）

Donde $\ tiny \ hat {H} \ left (q \ right)$ , $\ tiny \ hat {C} \ left (q, \ dot {q} \ right)$ y $\ tiny \ hat {G} \ left (q \ right)$ es la estimación del parámetro de inercia $\ tiny \ hat {p}$ calculada $\ minúscula H, C$ y $\ pequeña G$ el valor estimado.

Entonces, la ecuación del sistema de circuito cerrado (7.20) es

$\ pequeña H \ izquierda (q \ derecha) \ ddot {q} + C \ izquierda (q, \ punto {q} \ derecha) \ punto {q} + G \ izquierda (q \ derecha) = \ hat {H} \ left (q \ right) v + \ hat {C} \ left (q, \ dot {q} \ right) \ dot {q} + \ hat {G} \ left (q \ right)$ （7.22）

cual es

$\ dpi {200} \ tiny \ hat {H} \ left (q \ right) \ ddot {q} = \ hat {H} \ left (q \ right) v- \ begin {bmatrix} \ tilde {H} \ izquierda (q \ derecha) \ ddot {q} + \ tilde {C} \ izquierda (q, \ punto {q} \ derecha) \ dot {q} + \ tilde {G} \ izquierda (q \ derecha) = \ hat {H} \ left (q \ right) vY \ left (q, \ dot {q}, \ ddot {q} \ right) \ end {bmatrix} \ tilde {p}$ （7.23）

Entre ellos $\ tiny \ tilde {H} = H- \ hat {H}, \ tilde {C} = C- \ hat {C}, \ tilde {G} = G- \ hat {G}, \ tilde {p} = p- \ hat {p}$ .

Si el valor estimado del parámetro de inercia se $\ tiny \ hat {p}$ hace $\ tiny \ hat {H} \ left (q \ right)$ reversible, la ecuación del sistema de lazo cerrado (7.23) se puede escribir como

$\ diminuto \ ddot {q} = v- \ left [\ hat {H} \ left (q \ right) \ right] ^ {- 1} Y \ left (q, \ dot {q}, \ ddot {q} \ right) \ tilde {p} = v- \ varphi \ left (q, \ dot {q}, \ ddot {q}, \ hat {p} \ right) \ tilde {p}$ 7,24 （

definición

$\ tiny \ varphi \ left (q, \ dot {q}, \ ddot {q}, \ hat {p} \ right) \ tilde {p} = \ tilde {d}$

Entre ellos $\ diminuta \ tilde {d} = \ izquierda [\ tilde {d} _ {1}, \ cdots, \ tilde {d} _ {n} \ derecha] ^ {T}, d = \ izquierda [d_ {1} , \ cdots, d_ {n} \ right] ^ {T}$ .

Toma la superficie deslizante

$\ tiny s = \ dot {e} + \ Lambda e$

Entre ellos $\ tiny e = q_ {d} -q, \ dot {e} = \ dot {q} _ {d} - \ dot {q}, s = \ left [s_ {1}, \ cdots, s_ {n} \ right] ^ {T}$ , se $\ diminuto \ Lambda$ encuentra la matriz diagonal. luego

$\ tiny \ dot {s} = \ ddot {e} + \ Lambda \ dot {e} = \ left (\ ddot {q} _ {d} - \ ddot {q} \ right) + \ Lambda \ dot {e } = \ ddot {q} _ {d} -v + d + \ Lambda \ dot {e}$

tomar

$\ diminuto v = \ ddot {q} _ {d} + \ Lambda \ dot {e} + d$ （7.25）

¿Dónde $\ diminuto d$ está el vector a diseñar? luego

$\ diminuto \ dot {s} = \ tilde {d} -d$ （7.26）

Seleccione

$\ tiny d = \ left (\ tilde {d} + \ eta \ right) sgn \ left (s \ right)$

$\ pequeña \ izquierda \ | \ tilde {d} \ derecha \ | \ leqslant \ tilde {d}$ 7,27 （

Entre ellos $\ tiny \ eta> 0$ . luego

$\ tiny \ dot {s} s = \ left (\ tilde {d} -d \ right) s = \ tilde {d} s- \ tilde {d} sgn \ left (s \ right) s- \ eta sgn \ izquierda (s \ derecha) s \ leqslant - \ eta \ izquierda | s \ right | \ leqslant 0$

De la ecuación (7.21) y la ecuación (7.25), la ley de control del modo deslizante es

$\ tiny \ tau = \ hat {H} \ left (q \ right) v + \ hat {C} \ left (q, \ dot {q} \ right) \ dot {q} + \ hat {G} \ left ( q \ derecha)$ 7,28）

Entre ellos $\ tiny v = \ ddot {q} _ {d} + \ Lambda \ dot {e} + d, d = \ left (\ tilde {d} + \ eta \ right) sgn \ left (s \ right)$ .

De la ecuación de la ley de control (7.28), se puede ver que si la estimación del parámetro es $\ tiny \ hat {p}$ más precisa, cuanto $\ pequeña \ izquierda \ | p \ derecha \ |$ más pequeña y más pequeña, $\ tiny \ tilde {d}$ más pequeña es la vibración generada por el control de la membrana sinovial.

7.3 Ejemplo de simulación

Elija el sistema de brazo de fuerza del robot de dos articulaciones, y su modelo dinámico es:

$\ minúscula M \ izquierda (q \ derecha) \ ddot {q} + C \ izquierda (q, \ punto {q} \ derecha) \ punto {q} + G \ izquierda (q \ derecha) + F \ izquierda (\ punto {q} \ derecha) + \ tau _ {d} = \ tau$
Lo cual $\ tiny q = \ begin {bmatrix} q_ {1} & q_ {2} \ end {bmatrix} ^ {T}$ , $\ tiny \ tau = \ begin {bmatrix} \ tau _ {1} & \ tau _ {2} \ end {bmatrix} ^ {T}$ . tomar

$\ tiny M \ left (q \ right) = \ begin {bmatrix} \ alpha +2 \ varepsilon cosq_ {2} +2 \ eta sinq_ {2} & \ beta + \ varepsilon cosq_ {2} + \ eta sinq_ {2 } \\ \ beta + \ varepsilon cosq_ {2} \ eta sinq_ {2} & \ beta \ end {bmatrix}$

$\ tiny C \ left (q, \ dot {q} \ right) = \ begin {bmatrix} \ left (-2 \ varepsilon sinq_ {2} +2 \ eta cosq_ {2} \ right) \ dot {q} _ {2} & \ left (- \ varepsilon sinq_ {2} + \ eta cosq_ {2} \ right) \ dot {q} _ {2} \\ \ left (\ varepsilon sinq_ {2} - \ eta cosq_ {2 } \ right) \ dot {q} _ {1} & 0 \ end {bmatrix}$

$\ tiny G \ left (q \ right) = \ begin {bmatrix} \ varepsilon e_ {2} cos \ left (q_ {1} + q_ {2} \ right) + \ eta e_ {2} sin \ left (q_ {1} + q_ {2} \ right) + \ left (\ alpha - \ beta + e_ {1} \ right) e_ {2} cosq_ {1} \\ \ varepsilon e_ {2} cos \ left (q_ { 1} + q_ {2} \ right) + \ eta e_ {2} sin \ left (q_ {1} + q_ {2} \ right) \ end {bmatrix}$

Entre ellos $\ tiny \ alpha = I_ {1} + m_ {1} l_ {e1} ^ {2} + I_ {e} + m_ {e} l_ {ce} ^ {2} + m_ {e} l_ {1} ^ {2}, \ beta = I_ {e} + m_ {e} l_ {ce} ^ {2}, \ varepsilon = m_ {e} l_ {1} l_ {ce} cos \ delta _ {e}, \ eta = m e l 1 {ce} sen \ delta _ e$ .

Los valores reales de los parámetros físicos del brazo robótico se muestran en la Tabla 7-1.

Tabla 7-1 Parámetros físicos de brazos robóticos duales

$\ tiny m_ {1}$	$\ diminuto l_ {1}$	$\ tiny l_ {e1}$	$\ pequeña I_ {1}$	$\ tiny m_ {e}$	$\ tiny l_ {ce}$	$\ tiny I_ {e}$	$\ tiny \ delta _ {e}$	$\ tiny e_ {1}$	$\ tiny e_ {2}$
1 kilogramo	1 m	El 1/2 m	1/12 kg	3 kg	1 m	2/5 kg	0	-7/12	9,81

Se adopta la ley de control del modo deslizante (7.28) y los comandos de posición son respectivamente $\ tiny q_ {d1} = cos \ left (\ pi t \ right), q_ {d2} = sin \ left (\ pi t \ right), \ hat {H} = 0.6H, \ hat {C} = 0.6 C, \ hat {G} = 0.6G, \ tilde {d} = 30, \ eta = 0.10, \ Lambda = \ begin {bmatrix} 25 & 0 \\ 0 & 25 \ end {bmatrix}$ . Los resultados de la simulación se muestran en las Figuras 7-9 y 7-10.

Figura 7.9 Seguimiento de posición de brazos de doble fuerza

Figura 7.10 Entrada de control de brazo de fuerza doble

Programa de simulación:

Programa principal de Simulink: chap7_4sim.mdl

Subrutina de ley de control: chap7_4ctrl.m

function [sys, x0,str,ts]= chap7_1ctrl(t,x,u,flag)
switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 3
        sys=mdlOutputs(t,x,u);
    case {2,4,9}
        sys=[];
    otherwise
        error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts] = mdlInitializeSizes

global nmn
nmn = 25*eye(2);
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 2;
sizes.NumInputs  = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[];
str =[];
ts = [0 0];
function sys = mdlOutputs(t,x,u)
global nmn
qd1 = u(1);
dqd1 = -pi*sin(pi*t);
ddqd1 = -pi^2*cos(pi*t);
qd2 = u(2);
dqd2 = pi*cos(pi*t);
ddqd2 = -pi^2*sin(pi*t)
ddqd = [ddqd1;ddqd2];

dqd = [dqd1;dqd2];
ddqd = [ddqd1;ddqd2];

q1=u(3);dq1=u(4);
q2=u(5);dq2=u(6);
dq=[dq1;dq2];

e1 = qd1-q1;
e2 = qd2-q2;
e=[e1;e2];
de1 = dqd1 - dq1;
de2 = dqd2 - dq2;
de = [de1;de2];

alfa = 6.7;beta = 3.4;
epc = 3.0;eta = 0;
m1 = 1;l1 = 1;
lc1 = 1/2;I1 = 1/12;
g = 9.8;
e1 = m1*l1*lc1-I1-m1*l1^2;
e2 = g/l1;
M = [alfa+2*epc*cos(q2)+2*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);
    beta+epc*cos(q2)+eta*sin(q2),beta];
C = [(-2*epc*sin(q2)+2*eta*cos(q2))*dq2,(-epc*sin(q2)+eta*cos(q2))*dq2;
    (epc*sin(q2) - eta*cos(q2))*dq1,0];
G = [ epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)+(alfa-beta+e1)*e2*cos(q1);
    epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)];
M0 = 0.6*M;
C0 = 0.6*C;
G0 = 0.6*G;

s = de+nmn*e;
d_up = 30;
xite = 0.10;
d = (d_up+xite)*sign(s);
v = ddqd +nmn*de+d;

tol = M0*v+C0*dq+G0;

sys(1) =tol(1);
sys(2) =tol(2);

Subrutina de objeto controlado: chap7_4plant.m

function [sys, x0,str,ts]= chap7_1plant(t,x,u,flag)
switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 1
        sys =mdlDerivatives(t,x,u);
    case 3
        sys=mdlOutputs(t,x,u);
    case {2,4,9}
        sys=[ ];
    otherwise
        error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts] = mdlInitializeSizes

sizes = simsizes;
sizes.NumContStates = 4;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 4;
sizes.NumInputs  = 2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0 =[0;0;0;0];
str =[];
ts = [];

function sys = mdlDerivatives(t,x,u)
q1 = x(1);dq1 = x(2);
q2 = x(3);dq2 = x(4);
dq = [dq1;dq2];

% The model is given by Slotine and Weiping Li(MIT 1987)
alfa = 6.7;beta = 3.4;
epc = 3.0;eta = 0;
m1 = 1;l1 = 1;
lc1 = 1/2; I1=1/12;
g = 9.8;
e1 = m1*l1*lc1-I1-m1*l1^2;
e2 = g/l1;


M = [alfa+2*epc*cos(q2)+2*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);
    beta+epc*cos(q2)+eta*sin(q2),beta];
C = [(-2*epc*sin(q2)+2*eta*cos(q2))*dq2,(-epc*sin(q2)+eta*cos(q2))*dq2;
    (epc*sin(q2) - eta*cos(q2))*dq1,0];
G = [ epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)+(alfa-beta+e1)*e2*cos(q1);
    epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)];

tol(1)=u(1);
tol(2)=u(2);

ddq = inv( M )*( tol'-C*dq-G );
sys(1) =x(2);
sys(2) =ddq(1);
sys(3) =x(4);
sys(4) =ddq(2);

function sys = mdlOutputs(t,x,u)
sys(1) = x(1);
sys(2) = x(2);
sys(3) = x(3);
sys(4) = x(4);

Subrutina de dibujo: chap7_4plot.m

close all;

t = out.t.Data;
y1 = out.y1.Data;
y2 = out.y2.Data; 
tol = out.tol.Data;

figure(1);
subplot(211);
plot(t,y1(:,1),'r',t,y1(:,2),'b','linewidth',2);
xlabel('time(s)');ylabel('Angle tracking of joint 1');
subplot(212);
plot(t,y2(:,1),'r',t,y2(:,2),'b','linewidth',2);
xlabel('time(s)');ylabel('Angle tracking of joint 2');

figure(2);
subplot(211);
plot(t,tol(:,1),'r','linewidth',2);
xlabel('time(s)');ylabel('Control input 1');
subplot(212);
plot(t,tol(:,2),'r','linewidth',2);
xlabel('time(s)');ylabel('Control input 2');

Notas de estudio de control y dinámica del robot (7): control de modo deslizante basado en el método de par calculado

7. Control de modo deslizante basado en el método de momento calculado

7.1 Descripción del sistema

7.2 Diseño de la ley de control

7.3 Ejemplo de simulación

Supongo que te gusta