无人驾驶之车辆控制（1）纯跟踪（Pure Pursuit）算法与Stanley算法

引言

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

1 Pure Pursuit纯几何跟踪算法

1.1 车辆运动学自行车模型

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

• 不引入任何会影响到车辆动作的力
• 车辆只在xy平面上运动，在z轴（垂直方向）上的侧倾，前后俯仰等不考虑。
• 车辆结构就像自行车，左右前轮的转角和转速一致，后轮也是一样。所以两个前轮和后轮各可只用一个轮胎来描述。（转向角小）
• 前后轮的各自速度矢量和各自轮胎朝向一致，即假设轮胎无侧滑。
  而一般只有在低速状态下，轮胎产生的侧向力很小可忽略，所以该模型也主要应用于低速场景。

1.2 算法原理

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

符号	物理量
$R(m)$	转弯半径
$L(m)$	轴距
$\delta (rad)$	前轮转角
$\alpha (rad)$	车身与预瞄点夹角
$l_d(m)$	预瞄距离
$e(m)$	与预瞄点的横向偏差
$x_r(m)$	预瞄点横坐标
$y_r(m)$	预瞄点纵坐标

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

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

1.3 仿真测试

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

• 设置预瞄距离系数为0.1, 速度为20km/h:

• 设置预瞄距离系数为0.1, 速度为40km/h:

• 设置预瞄距离系数为0.1, 速度为60km/h:

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

• 设置车速为60km/h，预瞄距离系数为0.02：
  
• 设置车速为60km/h，预瞄距离系数为0.02：
  
• 设置车速为60km/h，预瞄距离系数为0.5：
  
  由上可以看出，随着预瞄距离增加，控制效果会趋向于平滑，即车辆对于期望轨迹的跟随能力会减弱。

2 Stanley算法

2.1 算法原理

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

2.2 仿真测试

设置控制系数为10, 速度为10km/h进行仿真结果如下:

设置控制系数为10, 速度为30km/h进行仿真结果如下:

设置控制系数为10, 速度为60km/h进行仿真结果如下:

可以看到由于本算法同样是基于车辆运动学模型的缘故，其跟踪效果会随着车速的提升而变差，只不过影响程度没有像在PP算法中那样大。

设置车速为60km/h，控制系数为2进行仿真结果如下:

设置车速为60km/h，控制系数为5进行仿真结果如下:

设置车速为60km/h，控制系数为20进行仿真结果如下:

由上图可以看出无论增益系数过大还是过小都无法取得良好的控制效果。因此选取合适的控制增益系数是取得良好跟踪效果的关键。

3 总结

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