自动驾驶控制算法——纯跟踪算法（Stanley）_基于航向误差改进的纯跟踪算法

目前主流的轨迹跟踪方法有两类：基于几何模型的跟踪方法和基于动力学模型的跟踪方法。

基于几何模型的跟踪方法：

• Pure Pursuit
• Stanley

基于动力学模型的跟踪方法:

• LQR
• MPC

1. 算法思想

前轮反馈控制(Front wheel feedback)也就是常说的Stanley方法，其核心思想是基于车辆前轴中心点的路径跟踪偏差量对方向盘转向控制量进行计算。

Stanley方法是一种基于横向跟踪误差（前轴中心B到规划轨迹最近点C的距离）的非线性反馈函数，并且能实现横向跟踪误差收敛到0。

图中，

根据车辆位姿与规划轨迹的相对几何关系可以直观的获得前轮转角的目标值，前轮转角$\delta {}_f$由两部分构成:

• 航向误差引起的转角$\delta {}_{\phi }$，即规划轨迹最近点的切线方向与车身坐标系x轴的 夹角$\delta {}_{\phi }$

  根据上图的几何关系，在不考虑横向跟踪误差（或横向误差为0）的情况下，前轮偏角和给定路径切线方向一致，所以航向误差引起的转角：
  $$\delta {}_{\phi }={\phi }_r-\phi$$
  ​其中，${\phi }_r$ 表示规划轨迹的航向角，$\phi$表示自车的航向角。

• 横向误差引起的转角$\delta {}_e$，即规划轨迹最近点的切线方向与前轮速度方向的夹角$\delta {}_e$

  根据上图的几何关系，在不考虑航向跟踪偏差（或航向误差为0）的情况下，横向跟踪误差越大，前轮转向角越大，位置误差引起的转角：
  $${\delta }_e=\arctan \frac{e_y}{l_d}$$
  $l_d$是人为给定的一个预瞄距离，通常认为和速度正相关：
  $$l_d=\frac{v}{k}$$
  代入，得到位置误差引起的转角：
  $${\delta }_e=\arctan \frac{k{e}_y}{v}$$

综上，得到Stanley算法总的前轮转角：
$$\delta {}_f=\delta {}_{\phi }+\delta {}_e$$

算法本质
使用线性自行车模型，横向误差变化率可以通过下式计算：
$$\dot{e_y}=-v\sin {\delta }_e$$

横向偏差变化率：可以理解为车速在横向上的分量，正负号也就是这个速度分量的方向符号，这里正负号和车轮偏角的正负号定义相关，例如文章第一幅图中所示，车轮偏角左偏为正，因此其正弦值也为正，此时横向偏差逐步减小，因此横向偏差变化率会有负号。

其中$\sin {\delta }_e$根据几何关系可知：
$$\sin {\delta }_e=\frac{e_y}{\sqrt{{l_d}^2+{e_y}^2}}$$
将$l_d$带入
$$\sin {\delta }_e=\frac{k{e}_y}{\sqrt{v^2+{k{e}_y}^2}}$$
所以，得到横向误差：
$$\dot{e_y}=\frac{-kv{e}_y}{\sqrt{v^2+{k{e}_y}^2}}=\frac{-k{e}_y}{\sqrt{1+{\left(\frac{k{e}_y}{v}\right)}^2}}$$
当横向误差很小时，忽略二次项，上式可以近似写成：
$$\dot{e_y}\approx -k{e}_y$$
对左右两边进行积分，得到横向误差的表达式：
$$e_y\left(t\right)=e_y\left(0\right)\times e^{-kt}$$
可以看出，当$t⟶\infty$时，横向误差以指数形式收敛于 0，而参数$k$决定了收敛速度。

2. 代码实现

3. Pure pursuit与Stanley 算法对比

1. Pure pursuit 基于对前轮转角进行控制来消除横向误差，本质是基于一个基于横向误差的P控制器。所以纯跟踪算法从原理上讲是无法消除航向误差的，而且这里的横向误差是指预瞄点的横向误差，即使横向误差为0，也无法保证自车前轴或后轴或质心处的横向误差为0；
2. Stanley 基于对前轮转角进行控制来消除横向误差和航向误差。由于是基于前轮中心进行的控制，理论上讲是可以将自车前轴中心的横向误差和航向误差都消除至0的；
3. Pure pursuit算法的关键在于预瞄点的选取：其距离越短，控制精度越高，但可能会产生震荡；预瞄距离越长，控制效果趋于平滑，震荡减弱。实际调试只需根据上述规则以及应用需求调整预瞄系数即可。
4. Stanley 算法的控制效果取决于控制增益，它缺少Pure pursuit算法的规律性，调试时需要花一定精力去寻找合适的控制增益值。