路径跟踪之Stanley控制算法

Stanley控制算法是一种基于横向误差（cross-track error）的跟踪算法，对该算法的相关解释可以参考下图。

上图中相关变量的含义如下：

1）为横向误差，是车辆前轮到给定轨迹最近点的距离；

2）为给定轨迹上点的切线方向与车身航向之间的夹角；

3）为前轮线速度方向与车身航向之间的夹角。

如果不考虑横向误差，要保持车辆沿给定轨迹运动，则前轮转角需要时刻保持与给定轨迹上点的切线方向一致，即前轮转角应为。

如果不考虑航向误差，要保持车辆沿给定轨迹运动，则前轮转角需要消除横向误差，前轮转角应为。

因而，如果同时考虑横向误差和航向误差，前轮转角应为：

由上图不难得出：

当时，可以保证横向误差收敛，此时有：

当很小时，上式近似为：

所以，前轮转角为：

如上图所示，在计算前轮转角的时候，可以先计算BC'的方向角，然后直接减去车身航向的方向角即可。在计算BC'方向角的时候，需要知道，根据上图，有：

所以，

也就是说，前视距离和前轮线速度成正比。

利用Matlab可以得到该算法跟踪一个圆形轨迹的仿真效果如下图所示。

上述仿真时，没有对前轮转角作限制，此时该算法可以较好地跟踪给定轨迹。但是实际车辆的前轮转角有一定的范围限制，代码中增加对前轮转角的限制后，车辆出现了“掉头”的情况。仿真结果如下图所示。