MPC轨迹跟踪算法

双移线绘制问题

双移线作为一种经典工况，在测试车辆稳定性和无人驾驶轨迹跟踪仿真分析中应用广泛。本人在学习龚老师《无人驾驶车辆模型预测控制》第五章时，首先发现配套代码中没有绘制出参考轨迹，这样就不明确追踪效果如何。因此，参考网上案例，调制出了双移线参考轨迹。

双移线参考轨迹

这里，本人是将X_predict作为参考双移线的横坐标，纵坐标是Y_ref。

figure(1);
  plot(X_predict, Y_ref,'r--','LineWidth',1);
  plot(X, Y,'cx','LineWidth',1);
  title('追踪结果对比');
  xlabel('横向位置X（米）');
  axis([0 150 -2 4]);  %坐标轴
  ylabel('纵向位置Y（米）');
  legend('追踪轨迹','参考轨迹');
  hold on;

网上也有博主写到参考双移线的横坐标是X_phi,见下图：

按照网上博主的介绍，如果将X_phi替换X_predict, 总是会出现报错，原因是参考网上的设置很难保证X_phi与Y_ref的向量长度保持一致，原代码中的（X_predict, Y_ref）是在迭代中同时计算出来的，这里本人还没有找出它到底输出的数据长度是多少，如果有同学清楚，烦请指点。

绘制横坐标为X_phi的双移线

首先，本人是在MATLAB中将单独绘制出以X_phi为横坐标，Y_ref为纵坐标的双移线，代码如下：

  clc;
  clear all;
  N=300;
  T=0.05;

  shape=2.4;%参数名称，用于参考轨迹生成
  dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
  dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
  Xs1=27.19;Xs2=56.46;%参数名称
  phi_ref=zeros(N,1);%用于保存预测时域内的期望轨迹
  Y_ref=zeros(N,1);%用于保存预测时域内的期望轨迹
  X_phi=0.05:0.5:150;
  X_ref=X_phi';

for p=1:1:N
    %%双移线追踪
      z1=shape/dx1*(X_ref(p,1)-Xs1)-shape/2;
      z2=shape/dx2*(X_ref(p,1)-Xs2)-shape/2;
      Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
      phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));
      Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
end

   figure(1);
  plot(X_ref, Y_ref,'b--','LineWidth',1);
  title('双移线图例');
  xlabel('横向位置X（米）');
  axis([0 150 -2 4]);  %直线工况
  % axis([-50 50 0 100]); 圆形工况
  ylabel('纵向位置Y（米）');
  legend('参考轨迹');
  hold on;

绘制出的参考轨迹如图所示：

结果对比

这里单独生成的双移线会和之前绘制的以X_predict为横坐标的双移线有差别吗？感兴趣的同学可以把上述代码添加到书上第五章的代码中，这里本人绘制后发现两个参考轨迹并无差别，所以原代码中，直接可以用X_predict绘制横坐标，这样能直接保证它和纵坐标的向量长度保持一致。