NMPC_main.m更新为：
NMPC参考路径设置 作者北京科技大学白国星 david.gx.bai@gmail.com
致谢：原始框架来自北京理工大学龚建伟教授团队著作《无人驾驶车辆模型预测控制》
代码语言：javascript复制clear all;
%%车辆参数初始化
l=1;%轴距
global v1;
global deltaf1;
v1=1;
deltaf1=0;
%% 控制参数初始化
Nx=3;%状态量个数
Np=25;%预测时域
Nc=3;%控制时域
%% 车辆位置初始化
State_Initial(1,1)=0;%x
State_Initial(2,1)=1.9;%y
State_Initial(3,1)=pi/36;%phi
%% 参考轨迹参数初始化
N=1000;%参考轨迹点数量
T=0.05;%采样周期
global Xrefg;
global Yrefg;
global PHIrefg;
%%参考轨迹生成
for k=1:1:N Np
if k<200
Xrefg(k,1)=k*T; %横坐标
Yrefg(k,1)=2; %纵坐标
PHIrefg(k,1)=0; %航向角
elseif k<514
Xrefg(k,1)=10 10*sin( 0.1*(k-200)*T);
Yrefg(k,1)=12-10*cos(0.1*(k-200)*T);
PHIrefg(k,1)=0.1*(k-200)*T;
elseif k<714
Xrefg(k,1)=20;
Yrefg(k,1)=12 (k-514)*T;
PHIrefg(k,1)=1.57;
else
Xrefg(k,1)=20;
Yrefg(k,1)=22;
PHIrefg(k,1)=1.57;
end
end
%% 开始仿真
for j=1:1:N-300
%读取参考路径
Xref=zeros(Np,1);
Yref=zeros(Np,1);
PHIref=zeros(Np,1);
for Nref=1:1:Np
Xref(Nref,1)=Xrefg(j Nref-1,1);
Yref(Nref,1)=Yrefg(j Nref-1,1);
PHIref(Nref,1)=PHIrefg(j Nref-1,1);
end
%%约束条件
lv=0.05;
lw=0.0082;
dvx1=-lv;dvx2=lv;dw1=-lw;dw2= lw;
if deltaf1>=0.44
dw2=0.0;
elseif deltaf1<=-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,v1,deltaf1),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解，需要有2*Nc个0
%%获得控制输入
v_actual=v1 A(1);
deltaf_actual=deltaf1 A(2);
v1=v_actual;
deltaf1=deltaf_actual;
%%车辆位置代入
X00(1)=State_Initial(1,1);
X00(2)=State_Initial(2,1);
X00(3)=State_Initial(3,1);
%%代入控制输入后，解算下一时刻车辆位置
Xref=dsolve('Dx-v_actual*cos(z)=0','Dy-v_actual*sin(z)=0','Dz-v_actual*tan(deltaf_actual)/l=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
t=T;
%%更新车辆位置
State_Initial(1,1)=eval(Xref.x);
State_Initial(2,1)=eval(Xref.y);
State_Initial(3,1)=eval(Xref.z);
%%绘图
figure(1)
plot(State_Initial(1,1),State_Initial(2,1),'r*');
hold on;
plot(Xrefg(j,1),Yrefg(j,1),'k.');
hold on;
axis([-5 25 -5 25])
end
NMPC.m更新为：
function cost = NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,v1,delta_f1)
%%车辆长度
l=1;
%%预测时域内初始位置代入
X=State_Initial(1,1);
Y=State_Initial(2,1);
PHI=State_Initial(3,1);
%%循环，获得预测位姿
for i=1:1:Np
if i==1
v(i,1)=v1 x(1);
delta_f(i,1)=delta_f1 x(2);
X_predict(i,1)=X T*v(i,1)*cos(PHI);
Y_predict(i,1)=Y T*v(i,1)*sin(PHI);
PHI_predict(i,1)=PHI T*v(i,1)*tan(delta_f(i,1))/l;
else
if i<Nc
v(i,1)=v(i-1,1) x(2*i-1);
delta_f(i,1)=delta_f(i-1,1) x(2*i);
else
v(i,1)=v(i-1,1) x(2*Nc-1);
delta_f(i,1)=delta_f(i-1,1) x(2*Nc);
end
X_predict(i,1)=X_predict(i-1) T*v(i,1)*cos(PHI_predict(i-1));
Y_predict(i,1)=Y_predict(i-1) T*v(i,1)*sin(PHI_predict(i-1));
PHI_predict(i,1)=PHI_predict(i-1) T*v(i,1)*tan(delta_f(i,1))/l;
end
%%解算预测位姿和参考轨迹的误差
X_error(i,1)=X_predict(i,1)-Xref(i,1);
Y_error(i,1)=Y_predict(i,1)-Yref(i,1);
PHI_error(i,1)=PHI_predict(i,1)-PHIref(i,1);
end
%cost=Y_error'*Y_error X_error'*X_error; 也可以不加航向误差
cost=Y_error'*Y_error X_error'*X_error 0.01*PHI_error'*PHI_error;
end仿真结果：
在这组仿真中，前轮转角增量约束采用的是《无人驾驶车辆模型预测控制》中通过试验测出的系统约束，不过由于该测试由人工完成，所以无人驾驶车辆线控转向系统的前轮转角速度上限可能大于书中测得的上限，具体数值取决于无人驾驶车辆线控转向系统的性能。
