在MATLAB同一路径下新建两个文件。第一个命名为NMPC_main.m,作为仿真的主文件,贴入如下代码:
- %NMPC最简代码 作者北京科技大学白国星 david.gx.bai@gmail.com
- %致谢:原始框架来自北京理工大学龚建伟教授团队著作《无人驾驶车辆模型预测控制》
- clear all;
- %%车辆参数初始化
- l=1;%轴距
- %% 控制参数初始化
- Nx=3;%状态量个数
- Np=25;%预测时域
- Nc=3;%控制时域
- %% 车辆位置初始化
- State_Initial(1,1)=0;%x
- State_Initial(2,1)=0;%y
- State_Initial(3,1)=pi/6;%phi
- %% 参考轨迹参数初始化
- N=100;%参考轨迹点数量
- T=0.05;%采样周期
- %% 开始仿真
- for j=1:1:N
- %%参考轨迹生成
- for Nref=1:1:Np
- Xref(Nref,1)=(j+Nref-1)*T;
- Yref(Nref,1)=2;
- PHIref(Nref,1)=0;
- end
- %%约束条件
- for i=1:1:Nc
- lb(2*i-1)=0.8;
- lb(2*i)=-0.44;
- ub(2*i-1)=1.2;
- ub(2*i)=0.44;
- 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),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解,需要有2*Nc个0
- %%获得控制输入
- v_actual=A(1);
- deltaf_actual=A(2);
- %%车辆位置代入
- X00(1)=State_Initial(1,1);
- X00(2)=State_Initial(2,1);
- X00(3)=State_Initial(3,1);
- %%代入控制输入后,解算下一时刻车辆位置
- XOUT=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(XOUT.x);
- State_Initial(2,1)=eval(XOUT.y);
- State_Initial(3,1)=eval(XOUT.z);
- %%绘图
- figure(1)
- plot(State_Initial(1,1),State_Initial(2,1),'b*');
- hold on;
- plot([0,5],[2,2],'r--');
- hold on;
- axis([0 5 0 4])
- end
第二个命名为NMPC.m,与主文件中fmincon函数调用的文件名字保持一致,贴入如下代码:
- function cost = NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref)
- %%车辆长度
- 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)=x(1);
- delta_f(i,1)=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)=x(2*i-1);
- delta_f(i,1)=x(2*i);
- else
- v(i,1)=x(2*Nc-1);
- delta_f(i,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
运行结果: