【Matlab】使用Level 1MS编写PID控制关节的S-Function

我太难了………………干啥啥不行

1.PID控制器设计

代码如下：

function [sys,x0,str,ts,simStateCompliance] = my_pid_ctrl(t,x,u,flag,Kp,Kd) switch flag, case 0, [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); case 2, sys=mdlUpdate(t,x,u); case 3, sys=mdlOutputs(t,x,u,Kp,Kd); case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); case 9, sys=mdlTerminate(t,x,u); otherwise DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); end function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 0; sizes.NumOutputs = 2; sizes.NumInputs = 6; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = []; str = []; ts = [0,0]; simStateCompliance = 'UnknownSimState'; function sys=mdlDerivatives(t,x,u) sys = []; function sys=mdlUpdate(t,x,u) sys = []; function sys=mdlOutputs(t,x,u,Kp,Kd) e1 = u(1) - u(3); e2 = u(2) - u(5); de1 = 0 - u(4); de2 = 0 - u(6); e = [e1; e2]; de = [de1; de2]; tol = Kp*e+Kd*de; sys(1) = tol(1); sys(2) = tol(2); function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime = 1; sys = t + sampleTime; function sys=mdlTerminate(t,x,u) sys = [];

解析：

1.设置系统参数信息，输入输出数量可以根据自己实际需求写，注意，sizes.DirFeedthrough是1还是0？可以通过观察输出是否含有输入来判断。

2.因为PID没用使用状态变量，故直接在output中编写控制算法：

function sys=mdlOutputs(t,x,u,Kp,Kd) e1 = u(1) - u(3); e2 = u(2) - u(5); de1 = 0 - u(4); de2 = 0 - u(6); e = [e1; e2]; de = [de1; de2]; tol = Kp*e+Kd*de; sys(1) = tol(1); sys(2) = tol(2);

其中，u为输入信号，sys为输出信号，可以通过在simulink中连线将输入输出信号与实际信号相对应

2.被控对象机械臂动力学方程

代码如下：

function [sys,x0,str,ts,simStateCompliance] = my_dynamics(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); case 2, sys=mdlUpdate(t,x,u); case 3, sys=mdlOutputs(t,x,u); case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); case 9, sys=mdlTerminate(t,x,u); otherwise DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); end function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 6; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [0 0 0 0]; str = []; ts = [0,0]; simStateCompliance = 'UnknownSimState'; function sys=mdlDerivatives(t,x,u) global ddq p = [2.9 0.76 0.87 3.04 0.87]; g = 9.8; M = [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)]; V = [-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); u(2)]; dq = [x(2); x(4)]; ddq = inv(M)*(tol - V*dq); sys(1) = x(2); sys(2) = ddq(1); sys(3) = x(4); sys(4) = ddq(2); function sys=mdlUpdate(t,x,u) sys = []; function sys=mdlOutputs(t,x,u) global ddq sys(1) = x(1); sys(2) = x(2); sys(3) = x(3); sys(4) = x(4); sys(5) = ddq(1); sys(6) = ddq(2); function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime = 1; sys = t + sampleTime; function sys=mdlTerminate(t,x,u) sys = [];

此处较复杂，且有些莫名其妙的问题我也没搞懂，故以后可能只有老老实实按照这种模板来写

1.首先对所有的微分量进行编写，包括高阶微分

function sys=mdlDerivatives(t,x,u) global ddq p = [2.9 0.76 0.87 3.04 0.87]; g = 9.8; M = [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)]; V = [-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); u(2)]; dq = [x(2); x(4)]; ddq = inv(M)*(tol - V*dq); sys(1) = x(2); sys(2) = ddq(1); sys(3) = x(4); sys(4) = ddq(2);

其中x为系统的状态变量组，分别为：x1、dx1、x2、dx2……

注意：在mdlDerivatives中对ddq进行设计，故要使用global ddq，否则如果都写在output会出现莫名其妙的问题

2.再编写输出部分的控制算法，因为本系统不需要控制算法，故直接sys输出

level 1MS编写就告一段落了，学不懂了，确实有点绕，接下来将全力以赴C mex