本文对Pure Pursuit以及Stanley control两种算法进行了调研以及初步的性能摸底。

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型能反映车辆位置、速度、加速度等与时间的关系。基于运动学模型设计出的控制器也能保证对车辆底层执行层下发的指令是符合车辆运动学约束的。对于运动学自行车模型，我们一般做出如下假设：

上图所示为简化的车辆运动学自行车模型，其后轮中心在蓝色虚线表示的目标轨迹上。本算法通过控制前轮转角来追踪下一个路点，使车辆可以沿着经过目标预瞄点的圆弧行驶。其上涉及到的物理量如下表所示：

通过正弦定理可以推出： l d / ( s i n ⁡ ( 2 α ) ) = R / ( s i n ⁡ ( π / 2 − α ) ) l_d/(sin⁡(2α)) = R/(sin⁡(π/2-α)) ld​/(sin⁡(2α))=R/(sin⁡(π/2−α)) 即： k = 1 / R = 2 s i n α / l d k=1/R=2sinα/l_d k=1/R=2sinα/ld​ 其中k为转弯圆弧的曲率。由上图还可推出： t a n ⁡ ( δ ) = L / R tan⁡(δ)=L/R tan⁡(δ)=L/R s i n α = e / l d sinα=e/l_d sinα=e/ld​ 那么： δ = t a n − 1 ⁡ ( L / R ) = t a n − 1 ( k L ) = t a n − 1 ( 2 L s i n α / l d ) = t a n − 1 ( 2 L e / l d 2 ) δ=tan^{-1}⁡(L/R)=tan^{-1}(kL)=tan^{-1} (2Lsinα/l_d )=tan^{-1} (2Le/l_d^2 ) δ=tan−1⁡(L/R)=tan−1(kL)=tan−1(2Lsinα/ld​)=tan−1(2Le/ld2​) 由上式可知，本控制器的本质是对转角进行控制，以减少横向误差为目标的横向控制器。其中 2 L / ( l d 2 ) 2L/(l_d^2 ) 2L/(ld2​)可视为控制器的 P P P参数。 L L L为车辆轴距， l d l_d ld​为设定的预瞄距离。本控制器的控制效果主要取决于预瞄距离的选取，一般来说预瞄距离越长，控制效果会越平滑，预瞄距离越短，控制效果会越精确（同时也会带来一定的震荡）。预瞄距离的选取也和当前车速有关，以下式为例： l d = g v + l f l_d=gv+l_f ld​=gv+lf​ 其中 v v v为当前车速， g g g为一可调节的参数, l f l_f lf​为一预设前视距离。 在有些文献中[1]，预瞄距离的选取方式表现为如下形式： l d = A v 2 + B v + C l_d=Av^2+Bv+C ld​=Av2+Bv+C 上式中常数 A = 1 / ( 2 a m a x ) A=1/(2a_{max} ) A=1/(2amax​), a m a x a_{max} amax​为最大制动加速度。 A v 2 Av^2 Av2表示最短车辆制动距离。B表示车辆遇到异常时需要的反应时间， B v Bv Bv则为对应的反应距离， C C C表示车辆的最小转弯半径。

本算法在实际应用中，通常不要求跟踪的目标点到本车后轴中心的距离切实等于预瞄距离。而是会选择采样好的一系列目标点中到后轴中心距离最接近预瞄距离的那个点来近似跟踪。这样做的好处是可以不需要目标轨迹的函数方程来求解真实预瞄距离坐标，极大地提升了算法的效率。

利用python做了个简易的仿真测试，让我们来看看该控制器的效果。

由上可以看出车速越快，该控制器的跟踪效果越差。

Stanley算法也是一种直接对前轮转角进行调整来消除横向偏差的横向控制算法。其控制原理可以用如下公式来表示： δ = θ + t a n − 1 ⁡ ( k e / v ) δ=θ+tan^{-1}⁡(ke/v) δ=θ+tan−1⁡(ke/v) δ δ δ表示前轮转角， v v v表示车速， θ θ θ为最近的目标轨迹点的切线与速度方向的夹角， e e e为横向误差， k k k为可调节的控制增益量。从上式可看出当航向偏差量 θ θ θ与横向偏差量 e e e增加时，控制算法也会相应增大对前轮转角的调节，从而更快地消除较大的误差。本算法源于2005届DARPA挑战赛中的冠军车辆，Stanley robot [2]。在该次比赛中，第一次有无人驾驶车辆完成了整个越野赛道。而在所有跑完全程的五支队伍中，由Sebastian带领的斯坦福大学AI实验室凭借Stanley算法的出色发挥，以压倒性的优势摘得桂冠。

设置控制系数为10, 速度为10km/h进行仿真结果如下: 设置控制系数为10, 速度为30km/h进行仿真结果如下: 设置控制系数为10, 速度为60km/h进行仿真结果如下: 可以看到由于本算法同样是基于车辆运动学模型的缘故，其跟踪效果会随着车速的提升而变差，只不过影响程度没有像在PP算法中那样大。

设置车速为60km/h，控制系数为2进行仿真结果如下: 设置车速为60km/h，控制系数为5进行仿真结果如下: 设置车速为60km/h，控制系数为20进行仿真结果如下: 由上图可以看出无论增益系数过大还是过小都无法取得良好的控制效果。因此选取合适的控制增益系数是取得良好跟踪效果的关键。

Pure pursuit 与 Stanley method 两个方法都是基于对前轮转角进行控制来消除横向误差，因此在上文中还结合了一个P控制器用作纵向的速度控制，以达到车辆跟踪期望根轨迹的效果。由于这两种算法实际都是基于运动学模型的纯几何跟踪方法，也仅在python上使用自建的简单模型进行仿真，因此算法在高速环境下的实际表现不会像上文中这样优异。但当其用作中低速场景时，还是有比较简单可靠的。其中PP算法的关键在于预瞄点的选取：其距离越短，控制精度越高，但可能会产生震荡；预瞄距离越长，控制效果趋于平滑，震荡减弱。实际调试只需根据上述规则以及应用需求调整预瞄系数即可。相反，Stanley method的控制效果取决于控制增益且缺少PP算法的规律性，通过仿真环节可以发现这个值无论是偏大还是偏小都很难有一个较好的控制效果，调试时需要花一定精力去找那个合适的值。

[1] J. Duan., et al. Path Tracking based on Pure Pursit Algorithm for Intelligent Vehicles. IEEE Transactions on Intelligent Transportation Systems, 2018,19(2): 613-626. [2] Sebastian Thrun., et al. Stanley: The Robot That Won the DARPA Grand Challenge[J]. 2006