代码详解——NMPC路径跟踪的Simulink仿真

通过上一期代码，我们可以发现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