七、基于计算力矩法的滑模控制

        计算力矩法是机器人控制中较常用的方法，该方法基于机器人模型中各项的估计值进行控制律的设计。

7.1  系统描述

        机器人机械手的模型为：

其中\tiny H\left ( q \right )为正定质量惯性矩阵，\tiny C\left ( q,\dot{q} \right )为哥氏力、离心力，\tiny G\left ( q \right )为重力。

7.2  控制律设计

        当不知道机器人的惯性参数时，根据计算力矩法，取控制律为

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

其中\tiny \hat{H}\left ( q \right )、\tiny \hat{C}\left ( q,\dot{q} \right )和\tiny \hat{G}\left ( q \right )为利用惯性参数估计值\tiny \hat{p}计算出的\tiny H,C和\tiny G的估计值。

        则闭环系统方程式为

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

即

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

其中\tiny \tilde{H}=H-\hat{H},\tilde{C}=C-\hat{C},\tilde{G}=G-\hat{G},\tilde{p}=p-\hat{p}。

        若惯性参数的估计值\tiny \hat{p}使得\tiny \hat{H}\left ( q \right )可逆，则闭环系统方程式可写为

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

定义

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

其中\tiny \tilde{d}=\left [ \tilde{d}_{1},\cdots ,\tilde{d}_{n} \right ]^{T},d=\left [ d_{1},\cdots ,d_{n} \right ]^{T}。

取滑动面

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

其中\tiny e=q_{d}-q,\dot{e}=\dot{q}_{d}-\dot{q},s=\left [ s_{1},\cdots ,s_{n} \right ]^{T}，\tiny \Lambda为正对角矩阵。则

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

取

                                                                   \tiny v=\ddot{q}_{d}+\Lambda \dot{e}+d  

式中\tiny d为待设计的向量。则

                                                                           \tiny \dot{s }=\tilde{d}-d        

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

其中\tiny \eta > 0。则

                                \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

由式（7.21）和式（7.25），得滑模控制律为

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

其中\tiny v=\ddot{q}_{d}+\Lambda \dot{e}+d,d=\left ( \tilde{d}+\eta \right )sgn\left ( s \right )。

        由控制律式（7.28）可知，若参数估计值\tiny \hat{p }越准确，则\tiny \left \|p \right \|越小，\tiny \tilde{d}越小，滑膜控制产生的抖振越小。

7.3  仿真实例

        选二关节机器人力臂系统，其动力学模型为：

                                         \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
其中\tiny q=\begin{bmatrix} q_{1} & q_{2} \end{bmatrix}^{T}，\tiny \tau =\begin{bmatrix} \tau _{1} & \tau _{2} \end{bmatrix}^{T}。取

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

其中\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}l_{ce}sin\delta _{e}。

机械臂的实际物理参数值见表7-1。

采用滑模控制律式（7.28），取位置指令分别为\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}。仿真结果如图7-9和7-10所示。

图7.9  双力臂位置跟踪 

   图7.10  双力臂控制输入

仿真程序:

Simulink主程序：chap7_4sim.mdl

控制律子程序：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);

被控对象子程序：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);

绘图子程序：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');