无人驾驶之车辆控制（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 / ( s i n ⁡ ( 2 α ) ) = R / ( s i n ⁡ ( π / 2 − α ) ) l_d/(sin⁡(2α)) = R/(sin⁡(π/2-α)) l