六、机器人鲁棒自适应PD控制

6.1  问题的提出  

   对于具有强耦合性和非线性的机器人系统而言,线性PD控制是最为简单且行之有效的控制方法,在工业机器人中得到了广泛应用。但实践表明,线性PD控制往往要求驱动机构有很大的初始输出,而实际驱动机构(通常是电机)往往不可能提供过大的初始力矩,且机械臂本身所承受的最大力矩也是有限的,这将使通过增大PD控制系数来进一步提高系数的性能受到限制。鉴于此,很多非线性PD控制方法被提出了出来,但常规的非线性PD控制器只有单纯的PD项,要求比例和微分项的系数仍较大,存在输出力矩较大的问题。

        现有一种自适应鲁棒PD控制策略,避免了初始输出力矩过大的弊端。该控制器由非线性PD反馈和补偿控制两部分构成,机器人不确定动力学部分由回归矩阵构成的自适应控制器进行补偿,并针对机器人 有界扰动的上确界是否已知设计了两种不同的扰动补偿法。该控制策略的优点在于当初始误差较大时,PD反馈起主要作用,通过非线性PD控制,避免了过大初始力矩输出;当误差较小时,自适应控制器起着主要的作用,从而保证了系统具有良好的动态性能。

6.2  机器人动力学模型及其结构特性

       考虑一个N关节的机器人力臂,其动态性能可以由以下二阶非线性微分方程描述:

                                                     \tiny D\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )+\omega =\tau                              (6.7)

式中:\tiny q\in R^{n}为关节角位移量,\tiny D\left ( q \right )\in R^{n\times n}为机器人的惯性矩阵,\tiny C\left ( q,\dot{q} \right )\in R^{n}表示离心力和哥氏力,\tiny G\left ( q \right )\in R^{n}为重力项,\tiny \tau \in R^{n}为控制力矩,\tiny \omega \in R^{n}为各种误差和扰动。

  机器人系统的动力学特性如下:

  •  特性1   D\left ( q \right )-2C\left ( q,\dot{q} \right )是一个斜对称矩阵。
  •  特性2   惯性矩阵D\left ( q \right )是对称正定矩阵,存在正数m_{1},m_{2}满足如下不等式:

                                                               m_{1}\left \| x \right \|^{2}\leqslant x^{T}D\left ( q \right )x\leqslant m_{2}\left \| x \right \|^{2}                                      (6.8)

    • 特性3   存在一个依赖于机械手参数的参数向量,使得D\left ( q \right ),C\left ( q,\dot{q} \right ),G\left ( q \right )满足线性关系:

                                                           D\left ( q \right )\theta +C\left ( q,\dot{q} \right )\rho +G\left ( q \right )=\Phi \left ( q,\dot{q},\rho ,\theta \right )P                         (6.9)

    其中,\Phi \left ( q,\dot{q},\rho ,\theta \right )\in R^{n\times m}为已知关节变量函数的回归矩阵,它是机器人广义坐标及其各阶导数的已知函数矩阵;P\in R^{n}是描述机器人质量特性的未知定常参数向量。

        假设1  q_{d}\in R^{n}为期望的关节角位移,q_{d}的一阶导数和二阶导数存在。

            假设2  误差和扰动\omega的范数满足:

                                                                \left \| \omega \right \|\leqslant d_{1}+d_{2}\left \| e \right \|+d_{3}\left \| \dot{e} \right \|                                             (6.10)

    其中,d_{1},d_{2},d_{3}分别为正常数,e=q=q_{d},\dot{e}=\dot{q}-\dot{q}_{d}分别为跟踪误差和跟踪误差导数。

    6.3  控制器的设计

            分别引入变量\tiny y\tiny q_{r},并令:

                                                                   \tiny y=\dot{e}+\gamma e                                                                 (6.11)

                                                                  \tiny \dot{q_{r}}=\dot{q_{d}}-\gamma _{e}                                                              (6.12)

     其中常数\tiny \gamma >0,则可推出:

                                                                    \tiny y=\dot{q}-\dot{q_{r}}                                                                 (6.13)

            由式(6.9)中的机器人线性关系特性,取\theta =\ddot{q}_{r},\rho =\dot{q}_{r},得:  

                                              D\left ( q \right )\ddot{q}_{r} +C\left ( q,\dot{q} \right )\dot{q}_{r} +G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P                              (6.14)

       由式(6.13)得\dot{q}_{r}=\dot{q}-y,将其代入上式,得:

                                      D\left ( q \right )\left (\ddot{q}-\dot{y} \right )+C\left ( q,\dot{q} \right )\left (\dot{q}-y \right ) +G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P                                   

            将上式结合式(6.7),得:

                                             D\left ( q \right )\dot{y}+C\left ( q,\dot{q} \right )y =\tau -\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P-\omega                                (6.15)

    1.扰动信号的上确界已知时控制器的设计

            对于式(6.7)所示的机器人系统,在误差扰动信号的上确界已知时,采用以下控制器和自适应律,可保证系统全局渐进稳定。

                                                  \tiny \tau =-K_{p}e-K_{v}\dot{e}+\Phi \left ( q,\dot{q},\dot{q}_{r},\ddot{q}_{r} \right )\hat{P}+u                         (6.16)

                                        \tiny u=\begin{bmatrix} u_{1}\cdots u_{n} \end{bmatrix}^{T}\tiny u_{i}=-\left ( d_{1}+d_{2}\left \| e \right \|+d_{3}\left \| \dot{e} \right \| \right )sgn\left ( y_{i} \right )   (6.17)

     \tiny \hat{p}的参数估计律取:

                                                                      \tiny \dot{\hat{p}}=-\Gamma \Phi ^{T}\left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r}\right )y                                    (6.18)

    式中

                                                \tiny K_{p}=K_{p1}+K_{p2}B_{p}\left ( e \right ),K_{v}=K_{v1}+K_{v2}B_{v}\left ( \dot{e} \right )

                          \tiny K_{p1}=diag\left ( k_{p11}, k_{p12},\cdots ,k_{p1n}\right ),K_{p2}=diag\left ( k_{p21}, k_{p22},\cdots ,k_{p2n}\right )

                         \tiny K_{v1}=diag\left ( k_{v11}, k_{v12},\cdots ,k_{v1n}\right ),K_{v2}=diag\left ( k_{v21}, k_{v22},\cdots ,k_{v2n}\right )

                                        \tiny B_{p}\left ( e \right )=diag\left ( \frac{1}{\alpha _{1}+\left | e_{1} \right |} , \frac{1}{\alpha _{2}+\left | e_{2} \right |}, \cdots ,\frac{1}{\alpha _{n}+\left | e_{n} \right |}\right )

                                       \tiny B_{v}\left ( e \right )=diag\left ( \frac{1}{\beta _{1}+\left | \dot{e}_{1} \right |} , \frac{1}{\beta _{2}+\left | \dot{e}_{2} \right |}, \cdots ,\frac{1}{\beta _{n}+\left | \dot{e}_{n} \right |}\right )

    其中\tiny k_{p1i},k_{p2i},k_{v1i},k_{v2i},\alpha _{i},\beta _{i}\left ( i=1,2,\cdots ,n \right )均大于零,\tiny \Gamma为正定对称阵。

    证明过程略。

    2.扰动信号的上确界未知时控制器的设计

    定理  当误差扰动信号\tiny \omega的上确界为未知时,设计控制器为:

                                                      \tiny \tau =-K_{p}e-K_{v}\dot{e}+\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )\hat{P}+u                      (6.21)

                                                                     \tiny u=-\frac{\left ( \hat{d} f\right )^{2}}{\hat{d} f\left \| y \right \|+\varepsilon ^{2}}y                                                 (6.22)

                                                                  \tiny \dot{\hat{d}}=\gamma _{1}f\left \| y \right \|,\hat{d}\left ( 0 \right )=0                                               (6.23)

                                                                     \tiny \dot{\varepsilon }=-\gamma _{2}\varepsilon ,\varepsilon \left ( 0 \right )=0                                                  (6.24)

    其中\tiny K_{p},K_{v}的取值同式(6.16),并保证满足式(6.19),\tiny P的估计值\tiny \hat{P}通过式(6.18)求得,\tiny d=d_{1}+d_{2}+d_{3}\tiny \tilde{d}=d-\hat{d}\tiny f=max\left ( 1,\left \| e \right \| \right,\left \| \dot{e} \right \| )\tiny \hat{d}\tiny d的估值,\tiny \gamma _{1},\gamma _{2}均为任意的正常数。

            对式(6.7)所示的机器人系统,并且误差扰动信号的上确界未知时,采用式(6.21)和式(6.22)的控制律可保证系统全局渐进稳定。

    证明过程略。

    6.4  机器人动态方程的线性化推导

            考虑动态方程:

                                        \tiny D\left ( q \right )\ddot{q_{r}}+C\left ( q,\dot{q} \right )\dot{q_{r}}+G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )P              

          为了实现控制律式(6.16)、式(6.17)和自适应律式(6.18),必须给出式(6.28)中\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )\tiny P的表达。

            针对双力臂机器人动态方程:

                 \tiny \begin{bmatrix} D_{11} \left ( q_{2} \right )&D_{21} \left ( q_{2} \right ) \\ D_{12} \left ( q_{2} \right )&D_{22} \left ( q_{2} \right ) \end{bmatrix}\begin{bmatrix} \ddot{q_{1}} \\ \ddot{q_{2}} \end{bmatrix}+\begin{bmatrix} -C_{12} \left ( q_{2} \right )\dot{q_{2}}&-C_{12} \left ( q_{2} \right )\left ( \dot{q_{1}} +\dot{q_{2}} \right ) \\ C_{12} \left ( q_{2} \right )\dot{q_{1}}&0 \end{bmatrix}\begin{bmatrix} \dot{q_{1}} \\ \dot{q_{2}} \end{bmatrix}+\begin{bmatrix} G_{1} \left ( q_{1},q_{2} \right )g\\ G_{2} \left \left (q_{1},q_{2} \right )g \end{bmatrix}=\begin{bmatrix} \tau _{1} \\ \tau _{2} \end{bmatrix}             (6.26)

    其中

                                     \tiny D_{11}\left ( q_{2} \right )=\left ( m_{1}+m_{2}\right )r_{1}^{2}+m_{2}r_{2}^{2}+2m_{2}r_{1}r_{2}cosq_{2} 

                                                      \tiny D_{12}\left ( q_{2} \right )=m_{2}r_{2}^{2}+m_{2}r_{1}r_{2}cosq_{2}

                                                                      \tiny D_{22}\left ( q_{2} \right )=m_{2}r_{2}^{2}

                                                             \tiny C_{12}\left ( q_{2} \right )=m_{2}r_{2}r_{2}sinq_{2}

                                    \tiny G_{1}\left ( q_{1},q_{2} \right )=\left ( m_{1}+m_{2} \right )r_{1}cosq_{2}+m_{2}r_{2}cos\left ( q_{1}+q_{2} \right )

                                                      \tiny G_{2}\left ( q_{1},q_{2} \right )=m_{2}r_{2}cos\left ( q_{1}+q_{2} \right )

    则式(6.25)中的第一个关节方程为:

                                \begin{bmatrix} D\left ( 1,1 \right ) & D\left ( 1,2 \right ) \end{bmatrix}\ddot{q}_{r}+\begin{bmatrix} C\left ( 1,1 \right ) & C\left ( 1,2 \right ) \end{bmatrix}\dot{q}_{r}+G\left ( 1 \right )=\begin{bmatrix} \phi _{11}& \phi _{12} & \phi _{13} \end{bmatrix}\begin{bmatrix} p_{1}\\ p_{2}\\ p_{3} \end{bmatrix}

    则式(6.25)中的第二个关节方程为:

                                \begin{bmatrix} D\left ( 2,1 \right ) & D\left ( 2,2 \right ) \end{bmatrix}\ddot{q}_{r}+\begin{bmatrix} C\left ( 1,1 \right ) & C\left ( 1,2 \right ) \end{bmatrix}\dot{q}_{r}+G\left ( 2 \right )=\begin{bmatrix} \phi _{21}& \phi _{22} & \phi _{23} \end{bmatrix}\begin{bmatrix} p_{1}\\ p_{2}\\ p_{3} \end{bmatrix}

            根据机器人系统的动力学特性3,取回归矩阵\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )的表达式为:

                                                                 \tiny \phi _{11}=\ddot{q}_{1r}+e_{1}cosq_{2}

                                                                    \tiny \phi _{12}=\ddot{q}_{1r}+\ddot{q}_{2r}

                  \tiny \phi _{13}=2\ddot{q}_{1r}cosq_{2}+\ddot{q}_{2r}cosq_{2}-\dot{q}_{2}\dot{q}_{1r}sinq_{2}-\left ( \dot{q_{1}}+ \dot{q_{2}}\right )\dot{q}_{2r}sinq_{2}+e_{1}cos\left ( q_{1}+q_{2} \right )

                                                                         \tiny \phi _{21}=0

                                                                       \tiny \phi _{22}=\phi _{12}

                                      \tiny \phi _{23}=\dot{q}_{1}\dot{q}_{1r}sinq_{2}+\ddot{q}_{1r}cosq_{2}+e_{1}cos(q_{1}+q_{2})

    其中\tiny e_{1}=\frac{g}{r_{1}}\tiny g=9.8

            可见,由于\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )的表达式不含有\tiny m_{1}\tiny m_{2},故本控制算法可实现变负载的机械手控制。

    6.5  仿真实例

            被控对象为双力臂机器人动态方程式(6.26),\tiny r_{1}=1,r_{2}=0.8,m_{1}=0.5,m_{2}=0.5

            误差扰动、位置指令和系统的初始状态分别为:

                                     \tiny d_{1}=2,d_{2}=3,d_{3}=6,\omega =d_{1}+d_{2}\left \| e \right \|+d_{2}\left \| \dot{e} \right \|

                                                                \tiny \left\{\begin{matrix} q_{1d}=sin(2\pi t)\\ q_{2d}=sin(2\pi t) \end{matrix}\right.,\begin{bmatrix} q_{1}\\ \dot{q}_{1}\\ q_{2}\\ \dot{q}_{2} \end{bmatrix}=\begin{bmatrix} 0.1\\0\\0.1\\0 \end{bmatrix}

           控制参数取

                                    \tiny K_{p1}=diag\left ( 180,190 \right ),K_{p2}=diag\left ( 150,150 \right )

                                    \tiny K_{v1}=diag\left ( 180,180 \right ),K_{v2}=diag\left ( 150,150 \right )

                                    \tiny \alpha _{i}=\beta _{i}=1\left ( i=1,2 \right ),\gamma =5,\Gamma =\begin{bmatrix} 5 & 0 &0 \\ 0 & 5 &0 \\ 0 &0 & 5 \end{bmatrix}

            采用S函数进行控制器和被控对象的设计。按扰动上确界已知和未知两种情况进行仿真。

    仿真之一

        仿真程序

            控制主程序:chap2_2sim.mdl

      

            指令信号子程序:chap2_2input.m

    function[sys,x0,str,ts] = chap2_2input(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(falg)]);
    end
     
    function[sys,x0,str,ts] = mdlInitializeSizes
    sizes = simsizes;
    sizes.NumOutputs = 6;
    sizes.NumInputs = 0;
    sizes.DirFeedthrough = 0;
    sizes.NumSampleTimes = 0;
    sys = simsizes(sizes);
    x0 =[];
    str =[];
    ts =[];
     
    function sys = mdlOutputs(t,x,u)
    q1_d = sin(2*pi*t);                    %关节1期望的角位移
    q2_d = sin(2*pi*t);                    %关节2期望的角位移
    dq1_d = 2*pi*cos(2*pi*t);              %关节1期望的角位移的导数
    dq2_d = 2*pi*cos(2*pi*t);              %关节1期望的角位移的导数
    ddq1_d = -(2*pi)^2*sin(2*pi*t);        %关节1期望的角位移的二阶导数
    ddq2_d = -(2*pi)^2*sin(2*pi*t);        %关节1期望的角位移的二阶导数
     
    sys(1) = q1_d;                         %赋值给sys,一共6个输入
    sys(2) = dq1_d;                      
    sys(3) = ddq1_d;
    sys(4) = q2_d;
    sys(5) = dq2_d;
    sys(6) = ddq2_d;

    控制器子程序:chap2_2ctrl.m

    %本文件是控制策略
    function [sys,x0,str,ts] = chap2_2ctrl(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
    sizes = simsizes;
    sizes.NumOutputs      = 2;
    sizes.NumInputs       = 13;
    sizes.DirFeedthrough  = 1;
    sizes.NumSampleTimes  = 0;
    sys = simsizes(sizes);
    x0 = [];
    str =[];
    ts = [];
     
    function sys = mdlOutputs(t,x,u)
    q1_d = u(1);dq1_d = u(2);ddq1_d = u(3);                %关节1期望的角位移,导数,二阶导数
    q2_d = u(4);dq2_d = u(5);ddq2_d = u(6);                %关节2期望的角位移,导数,二阶导数
    q1 = u(7);dq1 = u(8);                                  %关节1实际的角位移及导数
    q2 = u(9);dq2 = u(10);                                 %关节2实际的角位移及导数
     
    dq_d = [dq1_d,dq2_d]' ;                               %关节1和关节2期望的角位移的导数矩阵
    ddq_d = [ddq1_d,ddq2_d].';                              %矩阵加单引号表示共轭转置,不仅交换行列,还改变虚部符号
      
    e = [q1-q1_d,q2-q2_d].';                                %跟踪误差
    de = [dq1-dq1_d,dq2-dq2_d].';                           %跟踪误差导数
     
    gama = 5*eye(2);                                       %eye(2)表示生成2*2单位矩阵
    y = gama.*e+de;                                        %变量y
     
    dqr = dq_d-gama.*e;                                    %变量dqr是二维列向量
    ddqr = ddq_d-gama.*de;                                  
     
    %扰动信号的上确界已知时控制器的设计
    d1 = 2;d2 = 3;d3 = 6;
    u1 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(1));      %norm([e(1),e(2)])是范数,求e(1)的平方加e(2)的平方和再开根号
    u2 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(2));
     
    Kp1 = [180,0;0,190];
    Kp2 = [150,0;0,150];
    Kv1 = [180,0;0,190];
    Kv2 = [150,0;0,150];
     
    alfa1 = 1;alfa2 = 1;
    beta1 = 1;beta2 = 1;
     
    p1 = u(11);
    p2 = u(12);
    p3 = u(13);
    P = [p1 p2 p3].';
    r1 = 1;g = 9.8;e1 = g/r1;
    fai11 = ddqr(1)+e1*cos(q2);
    fai12 = ddqr(1)+ ddqr(2);
    fai13 = 2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);
    fai21 = 0;
    fai22 = fai12;
    fai23 = dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);
    FAI = [fai11 fai12 fai13;fai21 fai22 fai23];
    R = FAI * P;
    tol(1) = -(Kp1(1,1)+Kp2(1,1)/(alfa1+abs(e(1))))*e(1)-(Kv1(1,1)+Kv2(1,1)/(beta1+abs(de(1))))*de(1)+R(1)+u1;                            %控制力矩tol
    tol(2) = -(Kp1(2,2)+Kp2(2,2)/(alfa2+abs(e(2))))*e(2)-(Kv1(2,2)+Kv2(2,2)/(beta2+abs(de(2))))*de(2)+R(2)+u2;                            %控制力矩tol
    sys(1) = tol(1);
    sys(2) = tol(2);

    自适应算法子程序:chap2_2adapt.m

    %机器人自适应控制律
    function [sys,x0,str,ts] = chap2_2adapt(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    = 3;
    sizes.NumDiscStates   = 0;
    sizes.NumOutputs      = 3;
    sizes.NumInputs       = 10;
    sizes.DirFeedthrough  = 1;
    sizes.NumSampleTimes  = 0;
     
    sys = simsizes(sizes);
    x0 = [4.1,1.9,1.7];
    str =[];
    ts = [];
    function sys = mdlDerivatives(t,x,u)
    q1_d = u(1);dq1_d = u(2);ddq1_d = u(3);
    q2_d = u(4);dq2_d = u(5);ddq2_d = u(6);
     
    q1 = u(7);dq1 = u(8);                            
    q2 = u(9);dq2 = u(10);                          
    q_error  = [q1-q1_d,q2-q2_d]';
    dq_error = [dq1-dq1_d,dq2-dq2_d]';
     
    gama = 5*eye(2);
    y = gama*q_error+dq_error;
    ddq_d = [ddq1_d,ddq2_d]';
    dq_d  = [dq1_d,dq2_d]';
     
    dqr = dq_d-gama*q_error;
    ddqr = ddq_d-gama*dq_error;
     
    g = 9.8; r1 =1;
    e1 = g/r1;
    fai11 = ddqr(1)+e1*cos(q2);
    fai12 = ddqr(1)+ddqr(2);
    fai13 = 2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);
     
    fai21 = 0;
    fai22 = fai12;
    fai23 = dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);
    FAI =[fai11 fai12 fai13;fai21 fai22 fai23];
    Gama = 5*eye(3);
    S = -Gama*FAI'*y;
    for i=1:1:3
      sys(i)=S(i);
    end
    function sys = mdlOutputs(t,x,u)
    sys(1) = x(1);
    sys(2) = x(2);
    sys(3) = x(3);

    被控对象子程序:chap2_2plant.m

    %机器人平台
    function [sys,x0,str,ts] = chap2_2plant(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.10,0,0.10,0];
    str =[];
    ts = [];
    function sys = mdlDerivatives(t,x,u)
    q1_d = sin(2*pi*t);                           %给定关节1的期望轨迹
    q2_d = sin(2*pi*t);                           %给定关节2的期望轨迹
    dq1_d = 2*pi*cos(2*pi*t);                     %给定关节1的期望轨迹的导数
    dq2_d = 2*pi*cos(2*pi*t);                     %给定关节2的期望轨迹的导数
    %ddq1_d = -(2*pi)^2*sin(2*pi*t);               %给定关节1的期望轨迹的二阶导数
    %ddq2_d = -(2*pi)^2*sin(2*pi*t);               %给定关节2的期望轨迹的二阶导数
      
    e(1) = x(1)-q1_d;                             %关节1角位移的跟踪误差
    e(2) = x(3)-q2_d;                             %关节2角位移的跟踪误差
    de(1) = x(2)-dq1_d;                           %关节1角位移的跟踪误差的导数
    de(2) = x(4)-dq2_d;                           %关节2角位移的跟踪误差的导数
     
    tol = [u(1);u(2)];
    q1  = x(1);
    dq1 = x(2);
    q2  = x(3);
    dq2 = x(4);
     
    m1 = 0.5;m2 = 0.5;                            %机器人的双臂连杆的质量
    r1 = 1;r2 = 0.8;                              %机器人的双臂连杆端点到质心之间的距离
    g = 9.8;                                      %重力加速度g
     
    D11 = (m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*cos(q2);           %机器人的惯性矩阵      
    D12 = m2*r2^2+m2*r1*r2*cos(q2);                          %机器人的惯性矩阵   
    D22 = m2*r2^2;                                           %机器人的惯性矩阵   
    D = [D11 D12;D12 D22];                                   %机器人的惯性矩阵   
     
    C12 = m2*r1*r2*sin(q2);                                  %机器人的离心力和哥氏力
    C = [-C12*dq2 -C12*(dq1+dq2);C12*dq1 0];                 %机器人的离心力和哥氏力
      
    g1 = (m1+m2)*r1*cos(q2)+m2*r2*cos(q1+q2);                %机器人的重力项
    g2 = m2*r2*cos(q1+q2);                                   %机器人的重力项
    G = [g1*g;g2*g];                                         %机器人的重力项
    w = [1.5;1.5]+2.0*[e(1);e(2)]+5.0*[de(1);de(2)];         %其余误差和扰动
     
    S = inv(D).*(tol-[dq1;dq2].*C-G-w);                      %S为给定关节的期望轨迹的二阶导数,即ddq
     
    sys(1)=x(2);                                             %给定关节1的实际轨迹的导数
    sys(2)=S(1);                                             %
    sys(3)=x(4);                                             %给定关节2的实际轨迹的导数
    sys(4)=S(2);                                             %
     
    function sys = mdlOutputs(t,x,u)
    sys(1) = x(1);                                           %给定关节1的实际轨迹
    sys(2) = x(2);                                           %给定关节1的实际轨迹的导数
    sys(3) = x(3);                                           %给定关节1的实际轨迹
    sys(4) = x(4);                                           %给定关节1的实际轨迹的导数

    作图子程序:chap2_2plot.m

    close all;
     
     
    t = out.t.Data;
    qd = out.qd.Data;
    q = out.q.Data; 
    p = out.p.Data;
     
    figure(1);
    subplot(211);
    plot(t,qd(:,1),'r',t,q(:,1),'b');
    xlabel('time(s)');
    ylabel('position tracking of link 1');
    subplot(212);
    plot(t,qd(:,2),'r',t,q(:,2),'b');
    xlabel('time(s)');
    ylabel('velocity tracking of link 1');
     
    figure(2);
    subplot(211);
    plot(t,qd(:,4),'r',t,q(:,3),'b');;
    xlabel('time(s)');
    ylabel('position tracking of link 2');
    subplot(212);
    plot(t,qd(:,5),'r',t,q(:,4),'b');
    xlabel('time(s)');
    ylabel('velocity tracking of link 2');
     
    m1 = 0.5;m2 = 0.5;
    r1 = 1; r2 = 0.8;
    p1 = (m1+m2)*r1^2;
    p2 = m2*r2^2;
    p3 = m2*r1*r2;
     
    figure(3);
    subplot(311);
    plot(t,p(:,1),'r',t,p1,'b');
    xlabel('time(s)');
    ylabel('p1 estimate value');
    subplot(312);
    plot(t,p(:,2),'r',t,p2,'b');
    xlabel('time(s)');
    ylabel('p2 estimate value');
    subplot(313);
    plot(t,p(:,3),'r',t,p3,'b');
    xlabel('time(s)');
    ylabel('p3 estimate value');