最近在看控制的东西，想动手实践一下，看到有书名曰《机器人控制系统和MATLAB simulink仿真》，于是学习一下做做记录。
考虑一个关节机器人，其动态性能可由二阶非线性微分方程描述：
M ( q ) q ¨ + C ( q , q ˙ ) q ˙ + G ( q ) + F ( q ˙ ) + τ d = τ \boldsymbol{M}(\boldsymbol{q}) \ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \dot{\boldsymbol{q}}+\boldsymbol{G}(\boldsymbol{q})+\boldsymbol{F}(\dot{\boldsymbol{q}})+\boldsymbol{\tau}_{\mathrm{d}}=\boldsymbol{\tau} M(q)q¨+C(q,q˙)q˙+G(q)+F(q˙)+τd=τ
其中， q ∈ R n \boldsymbol{q}\in \mathbb{R} ^n q∈Rn为关节角位移量， M ( q ) ∈ R n × n \boldsymbol{M}(\boldsymbol{q})\in\mathbb{R} ^{n\times n} M(q)∈Rn×n为机器人的惯性矩阵， C ( q , q ˙ ) q ˙ ∈ R n \boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}}\in \mathbb{R}^n C(q,q˙)q˙∈Rn表示离心力和哥氏力， G ( q ) ∈ R n \boldsymbol{G}(\boldsymbol{q})\in \mathbb{R}^n G(q)∈Rn为重力项， F ( q ˙ ) ∈ R n \boldsymbol{F}(\dot{\boldsymbol{q}})\in\mathbb{R}^n F(q˙)∈Rn表示摩擦力矩， τ ∈ R n \boldsymbol{\tau}\in\mathbb{R}^n τ∈Rn为控制力矩， τ d ∈ R n \boldsymbol{\tau}_{\mathrm{d}}\in \mathbb{R}^n τd∈Rn为外加扰动。
机器人系统的动力学特性为：
（书中没有对这些特性严格地证明，后面还得再琢磨一下）
S函数是Simulink的重要部分，它为Simulink环境下的仿真提供了强有力的拓展能力。它用文本方式输入公式和方程，适合复杂动态系统的数学描述，并且在仿真过程中可以对仿真参数进行更精确的描述。机器人控制系统的Simulink仿真中，可以使用S函数来实现控制律、自适应律和被控对象的描述。
一般而言，S函数的使用步骤如下：
(1)创建S函数源文件。创建S函数源文件有多种方法，Simulink提供了很多S函数模板和例子，用户可以根据自己的需要修改相应的模板或例子即可。
(2)在动态系统的Simulink模型框图中添加S-function模块，并进行正确的设置。
(3)在Simulink模型框图中按照定义好的功能连接输入输出端口。为了方便S函数的使用和编写，Simulink的Functions&.Tables模块库还提供了S-functiondemos模块组，该模块组为用户提供了编写S函数的各种例子，以及S函数模板模块。
S函数的基本功能及重要参数设定如下：
(1)S函数功能模块：各种功能模块完成不同的任务，这些功能模块（函数）称为仿真例程或回调函数(call-back functions),包括初始化(initialization)、导数(mdlDerivative)、输出(mdlOutput)等。
(2)NumContStates表示S函数描述的模块中连续状态的个数。
(3)NumDiscStates表示离散状态的个数。
(4)NumOutputs和NumInputs分别表示模块输出和输入的个数。
(5)直接馈通(dirFeedthrough)为输入信号是否在输出端出现的标识，取值为0或1.例如，形如
y
=
k
×
u
y=k×u
y=k×u的系统需要输入（即直接反馈），其中，
u
u
u是输入，
k
k
k是增益，
y
y
y是输出，形如等式
y
=
x
,
x
=
u
˙
y=x,x=\dot{u}
y=x,x=u˙的系统不需要输入（即不存在直接反馈），其中，
x
x
x是状态，
u
u
u是输入，
y
y
y为输出。
(6)NumSampleTimes为模块采样周期的个数，S函数支持多采样周期的系统。
除了sys外，还应设置系统的初始状态变量 x o x_o xo、说明变量 s t r str str和采样周期变量 t s t_s ts。t变量为双列矩阵，其中每一行对应一个采样周期。对连续系统和单个采样周期的系统来说，该变量为 [ t 1 , t 2 ] [t_1,t_2] [t1,t2], t 1 t_1 t1为采样周期， t 1 = − 1 t_1=-1 t1=−1表示继承输入信号的采样周期， t 2 t_2 t2为偏移量，一般取为 0 0 0。对继承输入信号的采样周期的连续系统来说， t t t取为 [ 一 1 , 0 ] [一1,0] [一1,0]。
有关采样周期变量的含义更详细的摘录官方文档的内容：
采样周期的取值
如何选择采样时间：
关于以上dirFeedthroug和NumSampleTimes的内容可以参考官方文档理解得更详细：
以下面的公式为例
J θ ¨ = u + d ( t ) J \ddot{\theta}=u+d(t) Jθ¨=u+d(t)
u u u为控制输入, d ( t ) d(t) d(t)为加在控制输人端的扰动,模型输出为角度 θ \theta θ和角速度 θ ˙ \dot{\theta} θ˙， J J J是转动惯量，模型描述为：
x ˙ 1 = x 2 x ˙ 2 = 1 J ( u + d ( t ) ) \begin{array}{l} \dot{x}_{1}=x_{2} \\ \dot{x}_{2}=\frac{1}{J}(u+d(t)) \end{array} x˙1=x2x˙2=J1(u+d(t))
其中 x 1 = θ , x 2 = θ ˙ x_{1}=\theta, x_{2}=\dot{\theta} x1=θ,x2=θ˙是状态变量。
上述动力学方程是1输入2输出系统，角度 θ \theta θ和角速度 θ ˙ \dot{\theta} θ˙的初始值为0，模型初始化参数为[0,0]，模型初始化S函数为：
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 2;%连续状态个数为2：x_1和x_2，因为采用连续采样周期仿真
sizes.NumDiscStates = 0;%离散状态个数为0
sizes.NumOutputs = 2;%输出量为2个：角度和角速度
sizes.NumInputs = 1;%输入量为1个，u
sizes.DirFeedthrough = 0;%方程中输入没有直馈于输出
sizes.NumSampleTimes = 1;%采样周期数是1
sys = simsizes(sizes);
x0=[0,0]:%初始化输出值角度和角速度都是0
str=[]:%无其他说明
ts=[0 0]:%连续采样，第一个值为0，没有偏移，第二个值也设为0
该函数可用于描述微分方程并实现数值求解。在控制系统中，可采用该函数来描述，被控对象和自适应律等，并通过Simulink环境下选择数值分析方法(如ODE方法)实现模型的数值求解。取 J = 2 J=2 J=2, d ( t ) = sin t d(t)=\sin t d(t)=sint,则采用S函数可实现该模型角度 θ \theta θ和角速度 θ ˙ \dot{\theta} θ˙的求解，描述如下：
function sys=mdlDerivatives(t,x,u)%sys
J=2;
dt=sin(t);
ut=u(1);
sys(1)=x(2):%左边的sys是状态变量的导数
sys(2)=1/J (ut+dt);
S函数的mdlOutput函数通常用于描述控制器或模型的输出。采用S函数的mdlOutput模块来描述模型角度 θ \theta θ和角速度 θ ˙ \dot{\theta} θ˙的输出：
function sys=mdlOutputs(t,x,u)
sys(1)=x(1):
sys(2)=x(2):
其他关于S函数的详细案例可以参考我的另一个博客Matlab simulink上手控制仿真学习笔记3-常用模块S Function及使用案例