轨迹跟踪算法——Stanley法

Stanley法核心思想：

Stanley法又称为前轮反馈控制法（Front wheel feedback)，核心在于基于前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算。该算法基于横向跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数，并且能实现横向跟踪误差指数收敛为0。根据车辆位姿与给定路径的几何关系可以直观的获得控制车辆方向盘转角的控制量。

前轮转角控制量由两部分组成：1.由航向误差引起的转角，即当前车身方向与参考轨迹最近的点的切线方向的夹角。2.由横向误差引起的转角，即前轮中心到参考轨迹最近点的横向距离。

 模型推导：

跟踪结果图：

 纯追踪及LQR的跟踪见：

https://blog.csdn.net/m0_50888394/article/details/115588154