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

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

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 $\tiny s_{i}$ is NB THEN $\tiny k_{1}$ is NB

IF $\tiny s_{i}$ is NM THEN $\tiny k_{1}$ is NM

IF $\tiny s_{i}$ is NS THEN $\tiny k_{1}$ is NS

IF $\tiny s_{i}$ is ZE THEN $\tiny k_{1}$ is ZE

IF $\tiny s_{i}$ is PS THEN $\tiny k_{1}$ is PS

IF $\tiny s_{i}$ is PM THEN $\tiny k_{1}$ is PM

IF $\tiny s_{i}$ is PB THEN $\tiny k_{1}$ 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');

机器人动力学与控制学习笔记（九）————基于模糊自适应增益调整的机器人滑模控制