代码详解——铰接车的NMPC路径跟踪控制

根据铰接车的运动学模型：

对NMPC.m进行一系列的修改：

function cost = NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,THETAref,v1)

%%车辆长度

lf=2;

lr=2;

%%预测时域内初始位置代入

X=State_Initial(1,1);

Y=State_Initial(2,1);

THETA=State_Initial(3,1);

GAMMA=State_Initial(4,1);

%%循环，获得预测位姿

for i=1:1:Np

if i==1

v(i,1)=v1+x(1);

omega(i,1)=x(2);

X_predict(i,1)=X+T*v(i,1)*cos(THETA);

Y_predict(i,1)=Y+T*v(i,1)*sin(THETA);

GAMMA_predict(i,1)=GAMMA+T*omega(i,1);

L=lf*cos(GAMMA)+lr;

THETA_predict(i,1)=THETA+T*(v(i,1)*sin(GAMMA)/L+omega(i,1)*lr/L);

else

if i<Nc

v(i,1)=v(i-1,1)+x(2*i-1);

omega(i,1)=omega(i-1,1)+x(2*i);

else

v(i,1)=v(i-1,1)+x(2*Nc-1);

omega(i,1)=omega(i-1,1)+x(2*Nc);

end

X_predict(i,1)=X_predict(i-1)+T*v(i,1)*cos(THETA_predict(i-1));

Y_predict(i,1)=Y_predict(i-1)+T*v(i,1)*sin(THETA_predict(i-1));

GAMMA_predict(i,1)=GAMMA_predict(i-1,1)+T*omega(i,1);

L=lf*cos(GAMMA_predict(i-1,1))+lr;

THETA_predict(i,1)=THETA_predict(i-1,1)+T*(v(i,1)*sin(GAMMA_predict(i-1,1))/L+omega(i,1)*lr/L);

end

%%解算预测位姿和参考轨迹的误差

X_error(i,1)=X_predict(i,1)-Xref(i,1);

Y_error(i,1)=Y_predict(i,1)-Yref(i,1);

THETA_error(i,1)=THETA_predict(i,1)-THETAref(i,1);

end

cost=Y_error'*Y_error+X_error'*X_error;

% cost=Y_error'*Y_error+X_error'*X_error+0.01*THETA_error'*THETA_error;

end

mpc001文件也进行相应地修改：

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=0;

sizes.NumOutputs=4;

sizes.NumInputs=4;

sizes.DirFeedthrough=1;

sizes.NumSampleTimes=1;

sys=simsizes(sizes);

x0 =[];

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;

%控制器参数

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

Nc=1;

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);%theta

State_Initial(4,1)=u(4);%gamma

for Nref=1:1:Np

Xref(Nref,1)=rr((i+Nref-1)*vkey,1);

Yref(Nref,1)=rr((i+Nref-1)*vkey,2);

THETAref(Nref,1)=rr((i+Nref-1)*vkey,3);

end

%%约束条件

lv=0.00;

lw=0.21;

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

if u(4)>=0.687

dw2=0.0;

elseif u(4)<=-0.687

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,THETAref,vx),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解，需要有2*Nc个0

%%获得控制输入

vx=vx+A(1);

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;

当然，被控的运动学模型也需要改为铰接车运动学模型，此处不再赘述。

源代码

PS：本期代码与Path tracking of mining vehicles based on nonlinear model predictive control中的代码有所区别，本期代码是最简代码，论文中的代码进行了额外的优化，优化的部分正在申请专利，专利公开之后代码也将公开。