九、基于模糊自适应增益调整的机器人滑模控制

        采用自适应模糊系统,可实现机器人滑模控制中切换增益的自适应逼近,从而消除滑模控制中的抖振。本文设计一类基于模糊自适应增益调整的机器人滑模控制设计方法。

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

        设\small n关节机械手动态方程为

                                                               \small M\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )=\tau                                  (9.46)

其中\small q\in R^{n}为关节转动角度向量,\small M \left ( q \right )\small n\times n维正定惯性矩阵,\small V_{m}\left ( q,\dot{q} \right )\small n\times n维向心哥氏力矩,\small G\left ( q \right )\small n\times 1维惯性矩阵,\small \tau \in R^{n}为各个关节运动的转矩向量,即控制输入。

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

   特性1  惯性矩阵\small M \left ( q \right )是对称正定阵且有界;

        特性2  \small M\left ( q \right )-2C\left ( q,\dot{q} \right )是一个斜对称矩阵,即对任意向量\small x,有:

                                                              \small x^{T}\left ( M\left ( q \right )-2C\left ( q,\dot{q} \right ) \right )x=0                                         (9.47)

       特性3  重力项满足\small G\left ( q \right )\leqslant g_{b}\small g_{b}\small q的函数。

9.2  传统滑模控制律的设计

        定义跟踪误差为:

                                                                     \small e=q_{d}-q                                                                                           (9.48)

定义误差函数为:

                                                                         \small s=\dot{e}+\lambda e                                                      (9.49)

其中\small \lambda =diag\left [ \lambda _{1},\cdots ,\lambda _{i},\cdots ,\lambda _{n} \right ],\lambda _{i}> 0

 

        定义

                                                                      \small \dot{q}_{r}=\dot{q}-s=\dot{q}_{d}-\lambda e

                                                                      \small \ddot{q}_{r}=\ddot{q}-\dot{s}=\ddot{q}_{d}-\lambda \dot{e}                                                (9.50)

设计控制律为       

                                                                          \small \tau =\hat{\tau }-Ksgns

                                                                 \small \tau =\hat{M}\ddot{q}_{r}+\hat{C}\dot{q}_{r}+\hat{G}-As                                         (9.51)

其中\small \hat{M},\hat{C},\hat{G}分别为\small M,C,G的估计值,\small K =diag\left [ K _{11},\cdots ,K _{ii},\cdots ,K _{nn} \right ]为正定矩阵,\small A =diag\left [ a _{1},\cdots ,a _{i},\cdots ,a _{n} \right ]为正定阵。

        将式(9.51)代入式(9.46)得:

                                                         \small M\dot{s}+\left ( C+A \right )s=\Delta f-Ksgns                                  (9.52)

其中\small \Delta f=\Delta M\ddot{q}_{r}+\Delta C\ddot{q}_{r}+\Delta G,\Delta M=\hat{M}-M,C=\hat{C}-C,G=\hat{G}-G

        假设\small \left | \Delta f_{i} \right |< \left | \Delta f_{i} \right |_{bound},取

                                                                           \small K_{ii}\geqslant \left | \Delta f_{i} \right |_{bound}                                                   (9.53)

定义Lyapunov函数为

                                                                          \small V=\frac{1}{2}s^{T}Ms                                (9.54)

\small V求导,将式(9.47)和(9.52)代入,并考虑(9.53),得

                          \small \dot{V}=s^{T}M\dot{s}+\frac{1}{2}s^{T}M\dot{s}=s^{T}\left ( -\left ( C+A \right ) s+\Delta f-Ksgn\left ( s \right )+Cs\right )=s^{T}\left ( -As+\Delta f-Ksgn\left ( s \right ) \right )=s^{T}\left ( \Delta f-Ksgn\left ( s \right ) \right )-s^{T}As=\sum_{i=1}^{n}\left ( s_{i}\left ( \Delta f_{i} -K_{ii}sgn\left ( s_{i} \right )\right ) \right )-s^{T}As\leqslant -s^{T}As\leqslant 0

9.3  基于模糊自适应增益调整的机器人滑模控制

9.3.1  模糊系统的设计

  已经证明,采用模糊系统可以实现对任意连续函数的精确逼近。因此,可以采用模糊系统自适应逼近控制律的增益。

        采用乘积推理机、单值模糊器和中心平均解模糊器来设计模糊系统,系统的输出为:

                                                          \tiny y=\frac{\sum_{m=1}^{M}\theta ^{m}\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}{\sum_{m=1}^{M}\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}=\theta ^{T}\Psi \left ( x \right )
其中\tiny \theta =\left [ \theta ^{1},\cdots ,\theta ^{m},\cdots ,\theta ^{M} \right ]^{T}\tiny \Psi \left ( x \right )=\left [ \psi ^{1}\left ( x \right ),\cdots , \psi ^{m}\left ( x \right ),\cdots ,\psi ^{M}\left ( x \right )\right ]^{T}\tiny \psi ^{m}\left ( x \right )=\frac{\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}{\sum_{m=1}^{M}\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}\tiny M为模糊规则数量。

9.3.2  基于模糊增益自适应调整的滑模控制

        基于模糊增益调整的控制律设计为

                                                             \tiny \tau =\hat{M}\ddot{q}_{r}+\hat{C}\dot{q}_{r}+\hat{G}-As-K                                     (9.55)

其中\tiny K = \left [ k_{1},\cdots ,k_{i} ,\cdots ,k_{n}\right ]\tiny k_{i}为第\tiny i个模糊系统的输出。

  • 模糊规则的设计

        如果增益K不采用模糊逼近,定义Lyapunov函数:

                                                                       \tiny V=\frac{1}{2}s^{T}Ms

                                                                       

                                                   \tiny \dot{ V}=\frac{1}{2}\left [ \dot{s}^{T}Ms+s^{T}\dot{M}s+s^{T} M\dot{s}\right ]

                                             \tiny =\frac{1}{2}\left [ 2s^{T}M\dot{s} +s^{T}\dot{M}s\right ]=s^{T}\left [ -\left ( C+A \right ) s+\Delta f-K+Cs\right ]

                       

                                             \tiny =s^{T}\left [ -As+\Delta f-K \right ]=s^{T}\left [ \Delta f-K \right ]-s^{T}As

                                             \tiny =\sum_{i=1}^{n}\left [ s_{i}\Delta f_{i} -s_{i}k_{i}\right ]-s^{T}As

               

可见,为了保证\tiny \dot{V}为负,应使\tiny s_{i}k_{i}\geqslant 0,即保证\tiny s_{i}\tiny k_{i}符号相同。同时,考虑\tiny s_{i}\Delta f_{i}-s_{i}k_{i},当\tiny \left | s_{i} \right |较大时,为保证\tiny \dot{V}为较大的负数,希望\tiny \left | k_{i} \right |较大;当\tiny \left | s_{i} \right |较小时,\tiny \left | k_{i} \right |保持较小的值,就可以保证\tiny \dot{V}为负数。

        如果以\tiny s_{i}作为规则的输入,则模糊规则可采用如下形式:

                                                        IF    \tiny s_{i}    is    \tiny A_{i}^{M}    THEN    \tiny k_{1}    is    \tiny B_{i}^{m}

其中\tiny A_{i}^{M}\tiny B_{i}^{m}为模糊集。

        通过上述分析,将用于调整切换增益的模糊规则设计为:

                                                       IF        is    NB    THEN        is    NB

                                                       IF        is    NM    THEN        is    NM

                                                       IF        is    NS    THEN        is    NS

                                                       IF        is    ZE    THEN        is    ZE

                                                       IF        is    PS    THEN        is    PS

                                                       IF        is    PM    THEN        is    PM

                                                       IF        is    PB    THEN        is    PB

其中NB,NM,NS,ZE,PS,PM,PB为7个模糊集,分别表示负大,负中,负小,零,正小,正中,正大。

        用于表示模糊集的隶属函数设计为:

                                                         \tiny \mu _{A }\left ( x_{i} \right )=exp\left [ -\left ( \frac{x_{i}-\alpha }{\sigma } \right ) ^{2}\right ]                                 (9.56)

模糊系统的输出为:

                                                      \tiny k_{i}=\frac{\sum_ {m=1}^{M}\theta _{k_{i}}^{m} \mu A^{m}\left ( s_{i} \right )}{\sum_ {m=1}^{M}\mu A^{m}\left ( s_{i} \right )}=\theta _{k_{i}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )                (9.57)

其中\tiny \theta _{k_{i}}=\left [\theta _{k_{i}}^{1},\cdots , \theta _{k_{i}}^{m},\cdots ,\theta _{k_{i}}^{M} \right ]^{T},\Psi _{k_{i}}\left ( s_{i} \right )=\left [ \psi _{k_{i}}^{1}\left ( s_{i} \right ),\cdots , \psi _{k_{i}}^{m}\left ( s_{i} \right ),\cdots ,\psi _{k_{i}}^{M}\left ( s_{i} \right )\right ]^{T}\tiny \psi ^{m}\left ( x \right )=\frac{\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}{\sum_{m=1}^{M}\prod_{i=1}^{n}\mu A_{i}^{m}\left ( x_{i}^{*} \right )}\tiny M为模糊规则数量。

  • 自适应模糊滑模控制律的设         将式(9.55)代入式(9.46)中,得:

                                                              \tiny M\dot{s}=-\left ( C+A \right )s+\Delta f-k                               (9.58)

k_{i}=\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )为理想\Delta f_{i}的逼近,根据万能逼近定理,存在\omega _{i}> 0,有

                                                                        \left | \Delta f_{i}-\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right |\leqslant \omega _{i}                                   (9.59)        

自适应律取为:                   

                                                                            \dot{\tilde{\theta}}_{k_{i}}=s_{i}\psi _{k_{i}}\left ( s_{i} \right )                                              (9.60)

定义Lyapunov函数:

                                                                  V=\frac{1}{2}s^{T}Ms+\frac{1}{2}\sum_{i=1}^{n}\left ( \tilde{\theta }_{k_{i}}^{T} \tilde{\theta }_{k_{i}}\right )

其中\tilde{\theta }_{k_{i}}={\theta }_{k_{i}}-{\theta }_{k_{id}}。则

                                           \dot{V}=\frac{1}{2}\left [ \dot{s}^{T}Ms+s^{T}\dot{M}s+s^{T}M\dot{s}\right ]+\frac{1}{2}\sum_{i=1}^{n}\left ( \dot{\tilde{\theta }}_{k_{i}}^{T}\tilde{\theta }_{k_{i}}+\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}\right ) 

                                               =\frac{1}{2}\left [ 2s^{T}M\dot{s}+s^{T}\dot{M}s \right ]+\frac{1}{2}\sum_{i=1}^{n}2\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}=s^{T}\left [ M\dot{s}+Cs \right ]+\sum_{i=t}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}

                                               =s^{T}\left [ -\left ( C+A \right )s+\Delta f-k+Cs \right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}} 

                                               =s^{T}\left [ -As+\Delta f-k \right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}

                                               =-s^{T}As+s^{T}\left [ \Delta f-k \right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}} 

                                               =-s^{T}As+\sum_{i=1}^{n}s_{i}\left [ \Delta f_{i}-k_{i} \right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}

由于k_{i}=\tilde{\theta }_{k_{i}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )+{\theta }_{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right ),则

                            \dot{V}=-s^{T}As+\sum_{i=1}^{n}s_{i}\left [ \Delta f_{i}-\left ( \tilde{\theta }_{k_{i}}^{T} \Psi _{k_{i}}\left ( s_{i} \right )+{\theta }_{k_{id}}^{T} \Psi _{k_{i}}\left ( s_{i} \right )\right ) \right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}

                                =-s^{T}As+\sum_{i=1}^{n}s_{i}\left [ \Delta f_{i} -\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right ]+\sum_{i=1}^{n}\left ( -s_{i} \tilde{\theta }_{k_{i}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right )+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}

                                =-s^{T}As+\sum_{i=1}^{n}s_{i}\left [ \Delta f_{i} -\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right ]+\sum_{i=1}^{n}\left ( -s_{i} \tilde{\theta }_{k_{i}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )+\tilde{\theta }_{k_{i}}^{T}\dot{\tilde{\theta }}_{k_{i}}\right )

                                =-s^{T}As+\sum_{i=1}^{n}s_{i}\left [ \Delta f_{i} -\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right ]+\sum_{i=1}^{n}\tilde{\theta }_{k_{i}}^{T}\left ( -s_{i}\Psi _{k_{i}}\left ( s_{i} \right ) +\dot{\tilde{\theta }}_{k_{i}}\right )

将自适应律式(9.60)代入上式,得

存在很小的正实数\gamma _{i},使得式(9.59)满足

                                                               \left | \Delta f_{i}-\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right |\leqslant \omega _{i}\leqslant \gamma _{i}\left | s_{i} \right |

其中0< \gamma _{i}< 1。则

                                                           s_{i}\left | \Delta f_{i}-\theta _{k_{id}}^{T}\Psi _{k_{i}}\left ( s_{i} \right )\right |\leqslant \gamma _{i}\left | s_{i} \right |^{2}=\gamma _{i}s_{i}^{2}

                                   \dot{V}\leqslant -s^{T}As+\sum_{i=1}^{n}\gamma _{i}s_{i}^{2}=\sum_{i=1}^{n}\left ( -a_{i}s_{i}^{2}+\gamma _{i}s_{i}^{2}\right )=\sum_{i=1}^{n}\left ( \gamma _{i}-a_{i} \right )s_{i}^{2}\leqslant 0  (9.61)

其中\gamma =diag\left [ \gamma _{1},\cdots ,\gamma _{i},\cdots ,\gamma _{n} \right ],a_{i}> \gamma _{i}

        由式(9.61)可见,仅当s=0时,\dot{V}=0,自适应律式(9.60)渐进收敛。得出结论为:

                                                    \lim_{t\rightarrow \infty }=\lim_{t\rightarrow \infty }\left ( \dot{e}+\lambda e \right )=0,即\lim_{t\rightarrow \infty }q=q_{d}\lim_{t\rightarrow \infty }\dot{q}=\dot{q}_{d}         

9.3.4  仿真实例

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

                                      \small M\left ( q \right )\ddot{q}+V_{m}\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )+F\left ( \dot{q} \right )+\tau _{d}=\tau

其中\small q=\begin{bmatrix} q_{1} & q_{2} \end{bmatrix}^{T}\small \tau=\begin{bmatrix} \tau _{1} & \tau _{2} \end{bmatrix}^{T}

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

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

\small 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_{c1}^{2}+I_{ee}+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}
        机械臂的实际物理参数值见表9-1

            表9-1  双机械臂物理参数

\tiny m_{1} \tiny l_{1} \tiny l_{c1} \tiny I_{1} \tiny m_{e} \tiny l_{ce} \tiny I_{e} \tiny \delta _{e} \tiny e_{1} \tiny e_{2}
1 1 1/2 1/12 3 1 2/5 0 -7/12

9.81

                                                

M=1时为采用固定增益的传统滑模控制律式(9.51),M=2时为采用基于模糊自适应增益调整的滑模控制律式(9.55)。仿真结果如图所示。 

 

仿真程序:chap7_7sim.mdl

 

控制律子程序:chap7_7ctrl.m                                               

function [sys,x0,str,ts] = spacemodel(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
global nmn
nmn = 10*eye(2);
sizes = simsizes;
sizes.NumContStates     =10;
sizes.NumDiscStates     =0;
sizes.NumOutputs        =4;
sizes.NumInputs         =6;
sizes.DirFeedthrough    =1;
sizes.NumSampleTimes    =1;
sys = simsizes(sizes);
x0 = [0.1*ones(10,1)];
str = [];
ts = [0 0];
 
function sys =mdlDerivatives(t,x,u)
q1 = x(1);dq1 = x(2);
q2 = x(3);dq2 = x(4);
dq = [dq1;dq2];
 
 
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)
global nmn
qd1 = u(1);qd2 = u(2);
dqd1 = 0;dqd2 = 0;
ddqd1 = 0;ddqd2 = 0;
dqd = [dqd1;dqd2];
ddqd = [ddqd1;ddqd2];
q1 = u(3);dq1 = u(4);
q2 = u(5);dq2 = u(6);
dq = [dq1;dq2];
e1 = q1-qd1;
e2 = q2-qd2;
e =[e1;e2];
de1 = dq1-dqd1;
de2 = dq2-dqd2;
de =[de1;de2];
%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)*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.5*M;
C0 = 0.5*C;
G0 = 0.5*G;
s = de+nmn*e;
dqr = dqd-nmn*e;
ddqr = ddqd-nmn*de;
for i = 1:1:5
   thta1(i,1) = x(i);
end
for i = 1:1:5
   thta2(i,1) = x(i+5);
end
fsd1 = 0; fsd2 = 0;
for l1 = 1:1:5
     gs1 = -[(s(1)+pi/6-(l1-1)*pi/12)/(pi/24)]^2;
     u1(l1) = exp(gs1);
end
for l1=1:1:5
     fsu1(l1) = u1(l1);
     fsd1 = fsd1+u1(l1);
end
fs1 = fsu1/(fsd1+0.001);
k1 = thta1'*fs1';
for l2=1:1:5
    gs2 = -[(s(2)+pi/6-(l2-1)*pi/12)/(pi/24)]^2;
    u2(l2) = exp(gs2);
end
for l2=1:1:5
     fsu2(l2) = u2(l2);
     fsd2 = fsd2+u2(l2);
end
fs2 = fsu2/(fsd2+0.001);
k2 = thta2'*fs2';
A = 50*eye(2);
S = 1;
if S == 1
    K = [100 0;100 0];
    tol = M0*ddqr+C0*dqr+G0-A*s-K*sign(s);
elseif S == 2 %Adaptive fuzzy gain
    K = l*[k1 k2]'
    tol = M0*ddqr+C0*dqr+G0-A*s-K;
end
sys(1) = tol(1);
sys(2) = tol(2);
sys(3) = K(1);
sys(4) = K(2);

被控对象子程序:chap7_7plant.m

function [sys,x0,str,ts] = s_function(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         =4;
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)*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_7plot.m

close all;
 
figure(1);
subplot(211);
plot(t,y1(:,1),'r',t,y1(:,2),'b');
xlabel('time(s)');
ylabel('Position tracking of joint 1');
subplot(212);
plot(t,y2(:,1),'r',t,y2(:,2),'b')
xlabel('time(s)');
ylabel('Position tracking of joint 2');
 
figure(2);
subplot(211);
plot(t,y1(:,1)-y2(:,2),'r');
xlabel('time(s)');
ylabel('Position tracking error of joint 1');
subplot(212);
plot(t,y2(:,1)-y2(:,2),'r')
xlabel('time(s)');
ylabel('Position tracking error of joint 2');
 
figure(3);
subplot(211);
plot(t,P(:,1),'r');
xlabel('time(s)');
ylabel('Control input 1');
subplot(212);
plot(t,P(:,2),'r');
xlabel('time(s)');
ylabel('Control input 2');