7. Sliding mode control based on calculated moment method

Calculating torque method is a more commonly used method in robot control. This method is based on the estimated value of each item in the robot model to design the control law.

7.1 System description

The model of the robot manipulator is:

$\tiny H\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )=\tau$ （7.20）

Among them $\tiny H\left ( q \right )$ is the positive definite mass inertia matrix, $\tiny C\left ( q,\dot{q} \right )$ Coriolis force, centrifugal force, and $\tiny G\left ( q \right )$ gravity.

7.2 Control law design

When the inertial parameters of the robot are not known, according to the calculated moment method, the control law is taken as

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

Wherein $\tiny \hat{H}\left ( q \right )$ , $\tiny \hat{C}\left ( q,\dot{q} \right )$ and $\tiny \hat{G}\left ( q \right )$ is the inertia parameter estimates $\tiny \hat{p}$ calculated $\tiny H,C$ and $\tiny G$ the estimated value.

Then the closed-loop system equation (7.20) is

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

which is

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

Among them $\tiny \tilde{H}=H-\hat{H},\tilde{C}=C-\hat{C},\tilde{G}=G-\hat{G},\tilde{p}=p-\hat{p}$ .

If the estimated value of the inertia parameter is $\tiny \hat{p}$ made $\tiny \hat{H}\left ( q \right )$ reversible, the closed-loop system equation (7.23) can be written as

$\tiny \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）

definition

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

Among them $\tiny \tilde{d}=\left [ \tilde{d}_{1},\cdots ,\tilde{d}_{n} \right ]^{T},d=\left [ d_{1},\cdots ,d_{n} \right ]^{T}$ .

Take the sliding surface

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

Among them $\tiny e=q_{d}-q,\dot{e}=\dot{q}_{d}-\dot{q},s=\left [ s_{1},\cdots ,s_{n} \right ]^{T}$ , $\tiny \Lambda$ is the diagonal matrix. then

$\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}$

take

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

Where $\tiny d$ is the vector to be designed. then

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

Select

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

$\tiny \left \| \tilde{d} \right \|\leqslant \tilde{d}$ （7.27）

Among them $\tiny \eta > 0$ . then

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

From equation (7.21) and equation (7.25), the sliding mode control law is

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

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

From the control law equation (7.28), it can be seen that if the parameter estimate is $\tiny \hat{p }$ more accurate, the $\tiny \left \|p \right \|$ smaller, and the smaller, the $\tiny \tilde{d}$ smaller the chattering generated by the synovial membrane control.

7.3 Simulation example

Choose the force arm system of the two-joint robot, and its dynamic model is:

$\tiny M\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )+F\left ( \dot{q} \right )+\tau _{d}=\tau$
Which $\tiny q=\begin{bmatrix} q_{1} & q_{2} \end{bmatrix}^{T}$ , $\tiny \tau =\begin{bmatrix} \tau _{1} & \tau _{2} \end{bmatrix}^{T}$ . take

$\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}$

Among them $\ 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} sin \ delta _ e$ .

The actual physical parameter values of the robotic arm are shown in Table 7-1.

Table 7-1 Physical parameters of dual robotic arms

$\tiny m_{1}$	$\tiny l_{1}$	$\tiny l_{e1}$	$\tiny I_{1}$	$\tiny m_{e}$	$\tiny l_{ce}$	$\tiny I_{e}$	$\tiny \delta _{e}$	$\tiny e_{1}$	$\tiny e_{2}$
1kg	1m	1 / 2m	1/12kg	3kg	1m	2/5kg	0	-7/12	9.81

The sliding mode control law (7.28) is adopted, and the position commands are respectively $\tiny q_{d1}=cos\left ( \pi t \right ),q_{d2}=sin\left ( \pi t \right ),\hat{H}=0.6H,\hat{C}=0.6C,\hat{G}=0.6G,\tilde{d}=30,\eta =0.10,\Lambda =\begin{bmatrix} 25 & 0\\ 0& 25 \end{bmatrix}$ . The simulation results are shown in Figures 7-9 and 7-10.

Figure 7.9 Position tracking of dual force arms

Figure 7.10 Dual force arm control input

Simulation program:

Simulink main program: chap7_4sim.mdl

Control law subroutine: 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);

Controlled object subroutine: 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);

Drawing subroutine: 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');

Robot dynamics and control study notes (7)-sliding mode control based on the calculated torque method

7. Sliding mode control based on calculated moment method

7.1 System description

7.2 Control law design

7.3 Simulation example

Guess you like