Stanley法—轨迹跟踪与控制—学习纪录

前轮反馈控制（Front wheel feedback）又称 Stanley 控制 ，其核心思想是基于前轮中心的路径跟踪偏差量对方向盘 转向控制量进行计算。

 选用运动学模型——单车模型

参考轨迹——若干个参考点——离散的点

P1——距离前轮中心最近的点

Stanley方法是一种基于横向跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量。

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

Stanley方法相对于纯跟踪控制方法来说要更简单一些

航向误差和横向误差构成控制变量

1.不考虑横向跟踪误差的情况——ey=0(相当于前轮中心直接平移到P1点)

车辆与轨迹的误差为航向误差

转动车轮使得车轮的方向能跟上轨迹的方向——>可减小误差

2.不考虑航向跟踪误差的情况

车身方向就是参考点的切线方向

横向跟踪误差越大，前轮转向角越大——ey越大(前轮中心离P1点距离越大)

（数学建模关系式可参考纯跟踪法预瞄点与距离的关系）

插个小知识

纯跟踪法（Pure Pursuit）和Stanley法是两种常用的路径跟踪算法，用于实现自动驾驶系统中的车辆路径跟踪。

1. 算法原理：

• 纯跟踪法：纯跟踪法基于车辆与目标点之间的一种跟踪逻辑。它使用车辆的当前位置和朝向，以及预定义的目标点，计算出一个理想的转向角度，使得车辆向目标点移动。
• Stanley法：Stanley法在纯跟踪法的基础上，引入了一种侧向误差（横向偏移）进行修正。它使用车辆的当前位置和朝向，以及路径上的目标点，计算出一个理想的转向角度，并根据车辆与路径的侧向偏差来调整此角度。

1. 处理曲线路径：

• 纯跟踪法：纯跟踪法对于曲线路径较为敏感，需要更频繁地更新目标点以保持车辆稳定在路径上。
• Stanley法：Stanley法通过侧向误差来调整转向角度，能更好地适应并处理曲线路径，车辆更稳定地跟踪路径。

1. 对误差的响应：

• 纯跟踪法：纯跟踪法对路径上的侧向误差不敏感，当车辆离开路径时，纯跟踪法不能及时修正，可能导致车辆偏离路径。
• Stanley法：Stanley法使用侧向误差进行修正，相比纯跟踪法对路径的误差更敏感，能更快地进行修正，减少车辆偏离路径的可能性。

1. 实现复杂度：

• 纯跟踪法：纯跟踪法相对简单，只需要计算车辆当前位置和目标点即可得到转向角度，实现较为简单。
• Stanley法：Stanley法相对于纯跟踪法稍微复杂一些，需要额外计算侧向误差并进行修正，实现稍微复杂。

综上所述，纯跟踪法和Stanley法在处理曲线路径、对误差的响应和实现复杂度等方面有一定的区别。选择使用哪种方法取决于具体的应用场景和需求。