matlab---s函数讲解之二连杆动力学仿真

matlab虽然后simulink,但是再复杂系统的仿真的时候简单的simulink中模块不能满足要求,因此需要自己建立s函数,作为仿真中的一个模块

在控制系统中分为控制器和被控对象。

matlab---s函数讲解之二连杆动力学仿真

  • 零、原理
  • 一、控制器
    • 1、功能选择函数(flag)
    • 2、初始化函数
    • 3、输出函数
  • 二、被控对象
    • 1、功能选择函数(flag)
    • 2、初始化函数
    • 3、连续状态变量更新函数
    • 4、输出函数
  • 三、控制框图
  • 四、代码

零、原理

1、定点控制时,期望为常数,所以可以利用导数为0的特性构造误差动力学,然后构造李雅普诺夫函数,证明稳定性。
matlab---s函数讲解之二连杆动力学仿真_第1张图片

一、控制器

1、功能选择函数(flag)

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(['Unhandlede flag = ',num2str(flag)]);
    end
end

在s函数中,这是必须的一个函数,s函数中的标志为flag有10个,据说目前支持的有0、1、2、3、4、9等几个数值。matlab中S函数的概念及使用

1)flag=0。进行系统的初始化过程,调用mdlInitializeSizes函数,对参数进行初始化设置,比如离散状态个数、连续状态个数、模块输入和输出的路数、模块的采样周期个数、状态变量初始数值等。2)flag=1。进行连续状态变量的更新,调用mdlDerivatives函数。3)flag=2。进行离散状态变量的更新,调用mdlUpdate函数。4)flag=3。求取系统的输出信号,调用mdlOutputs函数。5)flag=4。调用mdlGetTimeOfNextVarHit函数,计算下一仿真时刻,由sys返回。6)flag=9。终止仿真过程,调用mdlTerminate函数。

同时,需要注意对于离散状态更新,每个仿真步内必然调用该子函数,不论是否有意义。Matlab S-function 使用总结

2、初始化函数

function [sys,x0,str,ts] = mdlInitializeSizes
    sizes = simsizes;
    sizes.NumOutputs = 2;
    sizes.NumInputs = 6;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0 = [];
    str = [];
    ts = [0 0];
end

可以看到在控制器中一般没有状态变量,而状态变量一般都在被控对象中。这也就导致了没有连续状态变量的定义,也就没有了连续状态更新的函数定义(状态方程)。(ps:虽然离散状态更新每个仿真中都会调用,flag=2.)

所以x0不需要定义值,只需要定义空就可以。
ts的值表示每个连续的采样时间步都运行。
sizes.DirFeedthrough = 1:只要输出函数中有u就需要=1。

3、输出函数

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 = [30 0;0 30];
    Kd = [30 0;0 30];

    tol = Kp*e+Kd*de;

    sys(1) = tol(1);
    sys(2) = tol(2);
end

控制流程图如下:
matlab---s函数讲解之二连杆动力学仿真_第2张图片

控制器的输入信号之前加了一个Mux,作用是可以将输入向量拼接成一个向量。根据图上的顺序,u1,u2就是输入信号(关节值),u3,u4,u5,u6是那个反馈信号向量中包含的值,也就是被控对象的输出信号。

这里关注demux的输出,正如上面所说,被控对象输出4个元素,双击在块设置中如下设置
matlab---s函数讲解之二连杆动力学仿真_第3张图片
但是输出连接到mux的只有x1和x3.

u3-u6在后面分析中可以知道,分别是q1,q2以及他们的导数。
然后根据R和q构造e;根据dr和dq构建de,这里因为dr=0,所以设为常数。

二、被控对象

1、功能选择函数(flag)

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

因为后面设置了连续状态变量,所以这里需要有状态更新函数设置。

2、初始化函数

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;

这里定义的全局变量p在后面的状态更新函数中有用到。
这里因为定义连续状态变量,所以x0不能定义空数组,输入是控制器输出的tol,输出是4个状态变量,虽然最后只连接存储两个。

3、连续状态变量更新函数

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);

这里是整个系统中最重要的部分—动力学方程。也就是在这个函数中更新状态变量。这里用到的状态变量有关节角度,以及关节角度的导数。
这里的输出是状态变量的导数,因为这个函数的名称就“求导”。
所以就要先手动求解q的二阶导数。然后和q的一阶导数共同组成“求导的结果。”
目测是当给出导数以后,状态变量的值就会自动更新。

4、输出函数

function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);

输出状态变量

三、控制框图

matlab---s函数讲解之二连杆动力学仿真_第4张图片

四、代码

1、m

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

function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 6;
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];

tol=Kp*e+Kd*de;

sys(1)=tol(1);
sys(2)=tol(2);

2、m

%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);

你可能感兴趣的:(机器人,matlab,#,机械臂,matlab,仿真,机器人,s函数)