车辆运动学相关的控制算法

1.Pure Pursuit（纯追踪法）

        Pure Pursuit（纯追踪法）是一种广泛使用的基于几何的追踪方法，它在低速的条件下有比较好的效果，其是一种基于自行车模型和阿克曼转向几何推导出的控制算法，如下图所示。

 

Pure Pursuit算法最基本的思想：

        以车的后轴中心为切点，以车的纵向车身为切线，通过控制前轮的转角，使得车辆能够沿着一条过目标点的圆弧来行驶。

目标点的选取：

         选择距离自车后轴中心ld目标轨迹上的一个点，目标点与自车的角度为α，可计算出自车沿着圆弧行驶到目标点处的角度为2α。

如何计算出自车的转向角δ？

由三角形定理可得，

 化简得：

 已知，

 所以：

         Pure Pursuit的主要影响因素为ld的选取，通常小一点的ld追踪会更加准确，因为它更靠近车辆本身，所以会更好的追踪，由于会不断更新目标点，所以系统的稳定性会随之降低。如果是大一点的ld，由于它会追踪较长的时间，所以系统相对比较稳定，但是准确性可能会随之降低。

2.Stanley

        Stanley也是一种基于几何的轨迹追踪算法，它的主要思想是以前轮中心为参考点，不需要预瞄距离，直接计算前轮中心到目标轨迹的横向误差和航向误差。

 

算法步骤：

（1）假设当前横向误差e= 0，所以自车只需转过θe的角度，则自车可一直沿着目标轨迹行驶，此时令

（2）为了消除横向误差，构建三角形，可得

 

（3）

（4）

（5）低速情况下，为了解决噪音，通常加入一个正的常数Ks

（6） 在高速的情况下，当我们有转向的命令时，我们需要更缓慢的变化，以确保侧向力不会过大，即使Stanley里面有速度的考量，但在高速的情况下，反应还是很剧烈，因此，我们常常会在θe(t)上加一个阻尼项，相当于对θe(t)设计一个PD控制器。

（7）根据路径的曲率，设计一个前馈项

（8）最终的转角为