自动驾驶控制算法——Stanley 算法（Stanley）

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

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

• Pure Pursuit
• Stanley

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

• LQR
• MPC

文章目录

• 1. 算法思想
• 2. 代码实现
• 3. Pure pursuit 与 Stanley 算法对比

1. 算法思想

---

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

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

图中，

2. 代码实现

---

3. Pure pursuit 与 Stanley 算法对比

---

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