机器人路径跟踪 代码 博客_代码详解——最简NMPC路径跟踪仿真代码

敬请关注公众号Path Tracking Letters

在MATLAB同一路径下新建两个文件。第一个命名为NMPC_main.m，作为仿真的主文件，贴入如下代码：

1. %NMPC最简代码 作者北京科技大学白国星 david.gx.bai@gmail.com
2. %致谢：原始框架来自北京理工大学龚建伟教授团队著作《无人驾驶车辆模型预测控制》
3. clear all;
4. %%车辆参数初始化
5. l=1;%轴距
6. %% 控制参数初始化
7. Nx=3;%状态量个数
8. Np=25;%预测时域
9. Nc=3;%控制时域
10. %% 车辆位置初始化
11. State_Initial(1,1)=0;%x
12. State_Initial(2,1)=0;%y
13. State_Initial(3,1)=pi/6;%phi
14. %% 参考轨迹参数初始化
15. N=100;%参考轨迹点数量
16. T=0.05;%采样周期
17. %% 开始仿真
18. for j=1:1:N
19. %%参考轨迹生成
20. for Nref=1:1:Np
21. Xref(Nref,1)=(j+Nref-1)*T;
22. Yref(Nref,1)=2;
23. PHIref(Nref,1)=0;
24. end
25. %%约束条件
26. for i=1:1:Nc
27. lb(2*i-1)=0.8;
28. lb(2*i)=-0.44;
29. ub(2*i-1)=1.2;
30. ub(2*i)=0.44;
31. end
32. %%选取求解算法
33. options = optimset('Algorithm','active-set'); %选择active-set为求解算法
34. %%求解算法预留
35. A=[];
36. b=[];
37. Aeq=[];
38. beq=[];
39. %%求解
40. [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
41. %%获得控制输入
42. v_actual=A(1);
43. deltaf_actual=A(2);
44. %%车辆位置代入
45. X00(1)=State_Initial(1,1);
46. X00(2)=State_Initial(2,1);
47. X00(3)=State_Initial(3,1);
48. %%代入控制输入后，解算下一时刻车辆位置
49. 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)');
50. t=T;
51. %%更新车辆位置
52. State_Initial(1,1)=eval(XOUT.x);
53. State_Initial(2,1)=eval(XOUT.y);
54. State_Initial(3,1)=eval(XOUT.z);
55. %%绘图
56. figure(1)
57. plot(State_Initial(1,1),State_Initial(2,1),'b*');
58. hold on;
59. plot([0,5],[2,2],'r--');
60. hold on;
61. axis([0 5 0 4])
62. end

第二个命名为NMPC.m，与主文件中fmincon函数调用的文件名字保持一致，贴入如下代码：

1. function cost = NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref)
2. %%车辆长度
3. l=1;
4. %%预测时域内初始位置代入
5. X=State_Initial(1,1);
6. Y=State_Initial(2,1);
7. PHI=State_Initial(3,1);
8. %%循环，获得预测位姿
9. for i=1:1:Np
10. if i==1
11. v(i,1)=x(1);
12. delta_f(i,1)=x(2);
13. X_predict(i,1)=X+T*v(i,1)*cos(PHI);
14. Y_predict(i,1)=Y+T*v(i,1)*sin(PHI);
15. PHI_predict(i,1)=PHI+T*v(i,1)*tan(delta_f(i,1))/l;
16. else
17. if i<Nc
18. v(i,1)=x(2*i-1);
19. delta_f(i,1)=x(2*i);
20. else
21. v(i,1)=x(2*Nc-1);
22. delta_f(i,1)=x(2*Nc);
23. end
24. X_predict(i,1)=X_predict(i-1)+T*v(i,1)*cos(PHI_predict(i-1));
25. Y_predict(i,1)=Y_predict(i-1)+T*v(i,1)*sin(PHI_predict(i-1));
26. PHI_predict(i,1)=PHI_predict(i-1)+T*v(i,1)*tan(delta_f(i,1))/l;
27. end
28. %%解算预测位姿和参考轨迹的误差
29. X_error(i,1)=X_predict(i,1)-Xref(i,1);
30. Y_error(i,1)=Y_predict(i,1)-Yref(i,1);
31. PHI_error(i,1)=PHI_predict(i,1)-PHIref(i,1);
32. end
33. %cost=Y_error'*Y_error+X_error'*X_error; 也可以不加航向误差
34. cost=Y_error'*Y_error+X_error'*X_error+0.01*PHI_error'*PHI_error;
35. end

运行结果：