通过上一期代码，我们可以发现LMPC在参考速度升高时，控制器无法稳定跟踪参考路径。
我们在先前的研究中发现，增大航向误差的权重系数，可以解决这个问题。
详见：
Bai G, Meng Y, Gu Q, et al. Path tracking of car-like vehicles based on variable weight model predictive control[C]//2018 Chinese Automation Congress (CAC). IEEE, 2018: 1943-1947.
但是这种方法会使得路径跟踪在转向时出现难以消除的横向误差残差。所以我们认为可以采用NMPC实现更高精度的控制。
可将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=3;
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
%初始状态
vx=vd1;
td=u(3);
State_Initial(1,1)=u(1);%x
State_Initial(2,1)=u(2);%y
State_Initial(3,1)=u(3);%phi
for Nref=1:1:Np
Xref(Nref,1)=rr((i Nref-1)*vkey,1);
Yref(Nref,1)=rr((i Nref-1)*vkey,2);
PHIref(Nref,1)=rr((i Nref-1)*vkey,3);
end
%%约束条件
lv=0.05;
lw=0.0082;
dvx1=-lv;dvx2=lv;dw1=-lw;dw2= lw;
if w>=0.44
dw2=0.0;
elseif w<=-0.44
dw1=-0.0;
end
for ir=1:2:2*Nc
lb(ir)=dvx1;
lb(ir 1)=dw1;
ub(ir)=dvx2;
ub(ir 1)=dw2;
end
%%选取求解算法
options = optimset('Algorithm','active-set'); %选择active-set为求解算法
%%求解算法预留
A=[];
b=[];
Aeq=[];
beq=[];
%%求解
[A,fval,exitflag]=fmincon(@(x)NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,vx,w),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解，需要有2*Nc个0
%%获得控制输入
vx=vx A(1);
w=w A(2);
%控制器输出
mpcout(1)=vx;
mpcout(2)=w;
mpcout(3)=rr((i-1)*vkey 1,1);
mpcout(4)=rr((i-1)*vkey 1,2);
sys=mpcout;
搭配前文中的NMPC.mat文件： 白国星，公众号：Path Tracking Letters代码详解——采用控制增量作为控制输入的NMPC路径跟踪
即可获得NMPC路径跟踪的Simulink仿真代码。
代码下载地址：
链接：https://pan.baidu.com/s/1dYni6jCsorlpfTTOhvlzxw
提取码：h4i6
