在上一期的代码中，LMPC是一个固定速度的控制器，速度设置为1m/s且不便更改，本期给出可以改变速度的控制器代码。
仿真系统下载链接：
链接：https://pan.baidu.com/s/1bdVcCZWrG1svMhbiKvKVvw
提取码：msun
有效期7天，过期可以后台联系索要。
控制器代码：
代码语言：javascript复制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=2; %在此处修改参考速度
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;
vkey=vd1*10;%由于参考路径分辨率的问题，设置vd1的最低分辨率为0.1m/s。此处为新增参数。
%系统约束条件
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)*vkey 1,1); %改变速度之后需要改变相应的跟踪目标点
xe(2)=u(2)-rr((i-1)*vkey 1,2);
xe(3)=u(3)-rr((i-1)*vkey 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)*vkey 1,1);%输出也需要改变
mpcout(4)=rr((i-1)*vkey 1,2);
sys=mpcout;效果：
采用本期代码完成的路径跟踪效果较差，如何改善效果，且看下回分解。
