采用 S-function 编写机器人系统的方法总结：
S-functions(系统函数)提供了一种强大的机制来扩展Simulink®环境的功能。s函数是用MATLAB®、C、c++或Fortran编写的Simulink块的计算机语言描述。
S-Function扩展了Simulink®环境的功能。S-Function是用MATLAB®、C、c++或Fortran编写的Simulink模块的计算机语言描述的。使用mex实用工具将C、c++和Fortran s函数编译到mex文件中(参见编译C mex函数)。与其他MEX文件一样，s-function是MATLAB执行引擎可以自动加载和执行的动态链接子例程。
S-Function使用一种称为S-Function API的特殊调用语法，能够与Simulink引擎交互。这种交
互非常类似于引擎和内置Simulink模块之间发生的事情。
s函数遵循一般形式，可适用于连续、离散和混合系统。通过遵循一组简单的规则，可以在S-Function中实现一个算法，并使用S-Function模块将其添加到Simulink模型中。在编写S-Function并将其名称放入S-Function模块(在用户定义函数模块库中提供)之后，可以使用封装自定义用户界面(参见创建模块封装)。
Simulink Coder™，可以在模型中使用S-Functions并生成代码。还可以通过编写目标语言编译器(TLC)文件来定制为S-Function生成的代码。更多信息请参见S-Function和代码生成(Simulink Coder)。
Matlab/Simulink S-function 机器人系统仿真：
参考【5】书籍中的模型案例。
那么，这个模型中最重要的就是S function的编写了！
第一个控制器设计 S function如下：
function [sys,x0,str,ts] = spacemodel(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
%% 以上为S function 的初始化代码，基本上是不用改动的。
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumOutputs = 2; % 控制器的输出量是两个，主要为关节驱动力矩，即关节控制量sizes.NumInputs = 2;
sizes.DirFeedthrough = 1; % 控制器系统的输出或可变采样时间是否受到输入的控制sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [];
str = [];
ts = [0 0];
function sys=mdlOutputs(t,x,u)
R1=u(1);dr1=0;
R2=u(2);dr2=0;
x(1)=u(3);
x(2)=u(4);
x(3)=u(5);
x(4)=u(6);
e1=R1-x(1);
e2=R2-x(3);
e=[e1;e2];
%% 这部分主要是为了获取参考输入轨迹和反馈位置轨迹之间的误差，然后把双关节的误差写成向量的形式！
de1=dr1-x(2);
de2=dr2-x(4);
de=[de1;de2];
%% 这部分是关节参考速度与关节实际速度之间的误差，即速度误差
Kp=[50 0;0 50];
Kd=[50 0;0 50];
%% PD 控制参数
tol=Kp*e+Kd*de;
%% 关节控制力矩描述
sys(1)=tol(1);
sys(2)=tol(2);
%% 写成S function输出的形式
控制力矩变化如下：
%%%%%%%%
第二个是机器人控制对象：
%S-function for continuous state equation
function [sys,x0,str,ts]=s_function(t,x,u,flag)
switch flag,
%Initialization
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u);
%Outputs
case 3,
sys=mdlOutputs(t,x,u);
%Unhandled flags
case {2, 4, 9 }
sys = [];
%Unexpected flags
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
%mdlInitializeSizes
function [sys,x0,str,ts]=mdlInitializeSizes
global p g
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=[];
p=[2.9 0.76 0.87 3.04 0.87];
g=9.8;
function sys=mdlDerivatives(t,x,u)
global p g
D0=[p(1)+p(2)+2*p(3)*cos(x(3)) p(2)+p(3)*cos(x(3));
p(2)+p(3)*cos(x(3)) p(2)];
C0=[-p(3)*x(4)*sin(x(3)) -p(3)*(x(2)+x(4))*sin(x(3));
p(3)*x(2)*sin(x(3)) 0];
%% 以上为机器人模型参数矩阵
tol=u(1:2); %%控制力矩作为机器人系统的输入
dq=[x(2);x(4)];
S=inv(D0)*(tol-C0*dq);
sys(1)=x(2);
sys(2)=S(1);
sys(3)=x(4);
sys(4)=S(2);
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);
%% 输出机器人实际位置变量。
那么，对于simulink中控制系统的参考输入也可以用S function进行自定义：
示例如下：
function [sys,x0,str,ts] = input(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 =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);
q2_d=sin(2*pi*t);
dq1_d=2*pi*cos(2*pi*t);
dq2_d=2*pi*cos(2*pi*t);
ddq1_d=-(2*pi)^2*sin(2*pi*t);
ddq2_d=-(2*pi)^2*sin(2*pi*t);
sys(1)=q1_d;
sys(2)=dq1_d;
sys(3)=ddq1_d;
sys(4)=q2_d;
sys(5)=dq2_d;
sys(6)=ddq2_d;
%% 描述系统输入参考轨迹
数据可视化，通过m文件实现数据可视化。
示例如下：
figure(1);
subplot(211);
plot(t,x1(:,1),'r',t,x1(:,2),'b'); %% 以时间 t 作为绘图的横坐标，纵坐标导入x1的（全部行，第一列），x2同样也是。
xlabel('time(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,x2(:,1),'r',t,x2(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 2');
figure(2);
subplot(211);
plot(t,tol(:,1),'r');
xlabel('time(s)');ylabel('tol1');
subplot(212);
plot(t,tol(:,2),'r');
xlabel('time(s)');ylabel('tol2');
复杂控制系统的示例：（鲁棒自适应控制算法）
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 p2 p3
sizes = simsizes;
sizes.NumContStates = 3;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 1;
sizes.NumInputs = 4;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0 = [0,0,0];
str = [];
ts = [];
a1=20;a0=30;b=50;
Am=[0,1;-a0,-a1];
%eig(Am)
Q=[15,0;0,15];
P=lyap(Am',Q);
p2=P(1,2);
p3=P(2,2);
function sys=mdlDerivatives(t,x,u)
global p2 p3
r=sign(sin(0.025*2*pi*t)); %Square Signal
thm=u(1);
dthm=u(2);
th=u(3);
dth=u(4);
e=thm-th;
de=dthm-dth;
ep=p2*e+p3*de;
lambda0=1.5;lambda1=1.5;lambda2=1.5;
sys(1)=lambda0*ep*r; %dk0
sys(2)=lambda1*ep*th; %dk1
sys(3)=lambda2*ep*dth; %dk2
function sys=mdlOutputs(t,x,u)
global p2 p3
thm=u(1);
dthm=u(2);
th=u(3);
dth=u(4);
e=thm-th;
de=dthm-dth;
ep=p2*e+p3*de;
r=sign(sin(0.025*2*pi*t)); %Square Signal
k0=x(1);k1=x(2);k2=x(3);
Fmax=1.0;
delta=0.1;
kk=1/delta;
if abs(ep)>delta
sats=sign(ep);
else
sats=kk*ep;
end
%v=Fmax*sign(ep);
v=Fmax*sats;
ut=k0*r+k1*th+k2*dth+v; %% k 为待调节的增益系数，v为鲁棒项
sys(1)=ut;
编程S函数，以上为鲁棒模型参考自适应控制的实现程序。
自适应控制框图：
鲁棒控制一般框架：
鲁棒模型参考自适应控制的优点：引入自适应控制，采用基于李雅普诺夫直接法的鲁棒模型参考自适应控制方法，在具有参数不确定性和未知非线性摩擦的情况下，使得使得跟踪误差趋于零。
参考文献：
【1】https://zhuanlan.zhihu.com/p/511253062
【3】https://ww2.mathworks.cn/help/simulink/slref/sfunction.html?searchHighlight=S-function&s_tid=srchtitle_S-function_1
【4】https://ww2.mathworks.cn/help/simulink/slref/custom-code-and-hand-coded-blocks-using-the-s-function-api.html
【5】刘金琨. 机器人控制系统的设计与 MATLAB 仿真: 基本设计方法[M]. 清华大学出版社, 2016.
评论（0）
您还未登录，请登录后发表或查看评论