绝尘子-白景辰的个人资料 - RAT

绝尘子-白景辰 b249347683

直男猫一只！

关注数: 214 粉丝数: 223 发帖数: 32,013 关注贴吧数: 76

贴吧，一代人的记忆基佬们

代码详解——最简NMPC路径跟踪仿真代码敬请关注公众号Path Tracking Letters 在MATLAB同一路径下新建两个文件。第一个命名为NMPC_main.m，作为仿真的主文件，贴入如下代码： %NMPC最简代码作者北京科技大学白国星 [email protected] %致谢：原始框架来自北京理工大学龚建伟教授团队著作《无人驾驶车辆模型预测控制》 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 运行结果：

代码详解——NMPC路径跟踪复杂参考路径设置原创白国星 Path Tracking Letters 今天来自专辑代码详解在先前的代码中，参考路径在每一个仿真循环内设置，因此只能设置为直线。详见白国星，公众号：Path Tracking Letters代码详解——最简NMPC路径跟踪仿真代码为设置更加复杂的参考路径，我们可以借助全局变量。即在初始化部分，通过全局变量设置参考路径，然后将在每一个仿真循环中读取参考路径即可。 NMPC_main.m的代码修改如下，黄绿背景部分为改后代码： %NMPC参考路径设置作者北京科技大学白国星 [email protected] %致谢：原始框架来自北京理工大学龚建伟教授团队著作《无人驾驶车辆模型预测控制》 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=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 %读取参考路径 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 %%约束条件 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); %%代入控制输入后，解算下一时刻车辆位置 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),'b*'); hold on; plot(Xrefg(j,1),Yrefg(j,1),'ro'); hold on; axis([-5 25 -5 25]) end NMPC.m文件保持不变。运行结果为：PS：后面会持续输出干货，希望大家能多多关注、转发，谢谢！

公众号推荐Path Tracking Letters

还有在的少年，加微信群一起玩耍撒

无人驾驶车辆路径跟踪控制交流借宝地打个广告欢迎各路大佬

1 下一页