Summary of Pure Pursuit trajactory tracking and Stanley trajactory tracking

Geometric Vehicle Model

这里使用阿克曼转向系，车辆模型简化为自行车模型。pure persuit and Stanley均使用了车辆运动学进行车辆横向控制，忽略车辆动力学的考虑，在未知车辆参数的情况下可以被迫使用。 2007DARPA 以后，使用较少。

此处可见，四轮车辆被简化为了二轮车辆，另外一个简化为，车辆仅可以在二维平面运动。由于简化，车辆的转向角和轴距和转弯半径遵循以下关系：

Pure Pursuit

如上，pure pursuit 需要给出预瞄距离Ld, 因此后期当工作效果不好的时候可以对这个预瞄距离进行tuning,但是过程繁琐，在某些情况下使用该模型效果不佳。

给定预瞄距离后在预定轨迹上获得采样点g, 本车的后轴中心，采样点g, 以及车辆的运动学半径中心行程三角形，根据sin正弦函数三角公式，获得如下：

也可以写成关于曲率半径k的关系：

根据上面的公式，车辆转角改写为：

以上两式合并写为：

此时，在任何情况下，根据给定的预瞄距离ld, 以及常数L， 以及计算出的alpha(t)，就可以实时就算期望车辆转角。

为了更好地理解该式：
定义一个新的变量，e_ld, 为车辆头指向与目标点的矢量的横向距离，于是有：

曲率半径可以被重新写为：

这里的2,/ld^2可以被当成一个纯P 控制的增益，值的注意的是，通常情况下我们选取Ld的原则是与车速成线性关系，当然，这个选择可以根据设计更改，也可以是非线性的。

现在选取线性关系，车辆期望转角写为：

此式即为pure pursuit 的最后表达式，需要使用者tuning的部分就是这里的分母K值。

Stanley

Stanley 考虑到了车辆后轴中心与预定轨迹的横向偏差e_fa, 车辆头指向与预定轨迹heading angle 方向的夹角，stanley车辆期望转角的第一项为

这里，theta为车辆的真实heading angle, theta_p为预定轨迹点的heading angle, 差值就是车辆需要被纠正的heading angle.
如果横向偏差不为零，车辆期望转角会产生第二个控制量，这个控制量和pure pursuit很相似，由前轴中心对应的车辆预定轨迹点C出，往切线防线延伸预瞄距离L, 这里的预瞄距离仍然与车速成正比。于是，两项控制量可合并为：

分析
1，pure pursuit预瞄距离系数K越大，跟随效果会越差，随着K减少，效果变好，继续减少，跟随效果会突然变差，最终跟随发散，无法工作。
2，曲率或曲率半径对pure pursuit工作效果影响很大，某个曲率下，给定的pure模型可能工作的很好，在另一个曲率下，工作效果会突然变差，这是因为pure仅仅考虑了curvature，因为对curvature的依赖性就很强。并且这种效果会随着车速的增加越发增强。
3，对于stanley, 比pure好的地方是考虑到了预定点heading angle 和真是的车辆heading angle只差， 作为一个前馈控制，提前补足了在转弯过程中的车辆转角，设计者可以根据需求，对前馈值进一步增加增益系数。同时还推荐使用与速度相关的可变增益系数，可以更好的弥补此类仅考虑车辆运动学，而忽略车辆动力学的模型的不足。增益系数的设定方法可以参考PID系数整定法。