代码详解——S-Function模块（三）

上期已经讲解完了初始化部分，这期主要讲输出部分，本期输出部分采用的是LMPC代码，如下为输出部分:

function sys = mdlOutputs(t,x,u)

global rr;

global T;

global vd1;

global w;

i = round(t*20)+1;

%控制器参数

Nx =3;%状态量的个数

Nu =2;%控制量的个数

Np =15;%预测/控制步长

Nc=1;

l=2.7;

%系统约束条件

lv=0.05;

lw=0.0082;

dvx1=-lv;dvx2=lv;dw1=w-lw;dw2=w+lw;

if w>=0.44

dw2=0.44;

elseif w<=-0.44

dw1=-0.44;

end

lb(1)=dvx1;

lb(2)=dw1;

ub(1)=dvx2;

ub(2)=dw2;

%初始状态

vx=vd1;

w=0;

td=u(3);

%需要消除的误差

xe(1)=u(1)-rr((i-1)*10+1,1);

xe(2)=u(2)-rr((i-1)*10+1,2);

xe(3)=u(3)-rr((i-1)*10+1,3);

%权重系数

q=[0.01 0 0;0 0.01 0;0 0 0.1];

Q_cell=cell(Np,Np);

for iq=1:1:Np

for jq=1:1:Np

if iq==jq

Q_cell{iq,jq}=q;

else

Q_cell{iq,jq}=zeros(Nx,Nx);

end

end

end

Q=cell2mat(Q_cell);

R=0.000*eye(Nu*Nc,Nu*Nc);

%预测模型

a=[1 0 -vx*sin(td)*T;

0 1 vx*cos(td)*T;

0 0 1];

b=[cos(td)*T 0;

sin(td)*T 0;

tan(w)*T/l vx*T/(l*cos(w)*cos(w));];

A_cell=cell(Np,1);

B_cell=cell(Np,1);

for jl=1:1:Np

A_cell{jl,1}=a^jl;

for kl=1:1:Nc

if kl<=jl

B_cell{jl,kl}=(a^(jl-kl))*b;

else

B_cell{jl,kl}=zeros(Nx,Nu);

end

end

end

A=cell2mat(A_cell);

B=cell2mat(B_cell);

H=2*(B'*Q*B+R);

f=2*B'*Q*A*xe';

%求解

A_cons=[];

b_cons=[];

X=zeros(10,1);

[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);

dvx=X(1,1);

dw=X(2,1);

vx=vx+dvx;

w=dw;

%控制器输出

mpcout(1)=vx;

mpcout(2)=w;

mpcout(3)=rr((i-1)*10+1,1);

mpcout(4)=rr((i-1)*10+1,2);

sys=mpcout;

将所有代码整合到同一个*.m文件中后，整个S-Function控制器中的代码为：

function [sys,x0,str,ts] = mpc001(t,x,u,flag)

switch flag

case 0

[sys,x0,str,ts]=mdlInitializeSizes;

% case 2

% sys = mdlUpdate(t,x,u);

case 3

sys = mdlOutputs(t,x,u);

case {1,2,4,9}

sys=[];

otherwise

DAStudio.error('Unhandled flag=', num2st(flag));

end

function[sys,x0,str,ts]=mdlInitializeSizes

sizes=simsizes;

sizes.NumContStates=0;

sizes.NumDiscStates=3;

sizes.NumOutputs=4;

sizes.NumInputs=3;

sizes.DirFeedthrough=1;

sizes.NumSampleTimes=1;

sys=simsizes(sizes);

x0 =[0;0;0];

str = [];

ts = [0.05 0];

global rr;

global T;

global vd1;

global w;

N=30000;

T=0.05;

vd1=1;

w=0;

rr=zeros(N+10,4);

%生成产考轨迹

for ir=1:1:N

if ir<=5100

rr(ir,1)=19.5+0.1*(ir-1)*T;

rr(ir,2)=80;

rr(ir,3)=0;

rr(ir,4)=0;

elseif ir<=9787

rr(ir,1)=45+15*sin(0.0067*(ir-5100)*T);

rr(ir,2)=65+15*cos(0.0067*(ir-5100)*T);

rr(ir,3)=0-0.0067*(ir-5100)*T;

rr(ir,4)=-0.06666667;

elseif ir<=15787

rr(ir,1)=60;

rr(ir,2)=65-0.1*(ir-9787)*T;

rr(ir,3)=-1.57;

rr(ir,4)=0;

elseif ir<=20473

rr(ir,1)=75-15*cos(0.0067*(ir-15787)*T);

rr(ir,2)=35-15*sin(0.0067*(ir-15787)*T);

rr(ir,3)=-1.57+0.0067*(ir-15787)*T;

rr(ir,4)=0.06666667;

elseif ir<=30473

rr(ir,1)=75+0.1*(ir-20473)*T;

rr(ir,2)=20;

rr(ir,3)=0;

rr(ir,4)=0;

else

rr(ir,1)=150;

rr(ir,2)=20;

rr(ir,3)=0;

rr(ir,4)=0;

end

end

function sys = mdlOutputs(t,x,u)

global rr;

global T;

global vd1;

global w;

i = round(t*20)+1;

%控制器参数

Nx =3;%状态量的个数

Nu =2;%控制量的个数

Np =15;%预测/控制步长

Nc=1;

l=2.7;

%系统约束条件

lv=0.05;

lw=0.0082;

dvx1=-lv;dvx2=lv;dw1=w-lw;dw2=w+lw;

if w>=0.44

dw2=0.44;

elseif w<=-0.44

dw1=-0.44;

end

lb(1)=dvx1;

lb(2)=dw1;

ub(1)=dvx2;

ub(2)=dw2;

%初始状态

vx=vd1;

w=0;

td=u(3);

%需要消除的误差

xe(1)=u(1)-rr((i-1)*10+1,1);

xe(2)=u(2)-rr((i-1)*10+1,2);

xe(3)=u(3)-rr((i-1)*10+1,3);

%权重系数

q=[0.01 0 0;0 0.01 0;0 0 0.1];

Q_cell=cell(Np,Np);

for iq=1:1:Np

for jq=1:1:Np

if iq==jq

Q_cell{iq,jq}=q;

else

Q_cell{iq,jq}=zeros(Nx,Nx);

end

end

end

Q=cell2mat(Q_cell);

R=0.000*eye(Nu*Nc,Nu*Nc);

%预测模型

a=[1 0 -vx*sin(td)*T;

0 1 vx*cos(td)*T;

0 0 1];

b=[cos(td)*T 0;

sin(td)*T 0;

tan(w)*T/l vx*T/(l*cos(w)*cos(w));];

A_cell=cell(Np,1);

B_cell=cell(Np,1);

for jl=1:1:Np

A_cell{jl,1}=a^jl;

for kl=1:1:Nc

if kl<=jl

B_cell{jl,kl}=(a^(jl-kl))*b;

else

B_cell{jl,kl}=zeros(Nx,Nu);

end

end

end

A=cell2mat(A_cell);

B=cell2mat(B_cell);

H=2*(B'*Q*B+R);

f=2*B'*Q*A*xe';

%求解

A_cons=[];

b_cons=[];

X=zeros(10,1);

[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);

dvx=X(1,1);

dw=X(2,1);

vx=vx+dvx;

w=dw;

%控制器输出

mpcout(1)=vx;

mpcout(2)=w;

mpcout(3)=rr((i-1)*10+1,1);

mpcout(4)=rr((i-1)*10+1,2);

sys=mpcout;