车辆轨迹跟踪算法

车辆轨迹跟踪算法

车辆轨迹跟踪，目前的主流方法分为两类：基于几何追踪的方法和基于模型预测的方法；

几何追踪方法–pure-pursuit (纯跟踪)算法

阿克曼几何的简化版 – 车辆单轨模型（自行车模型）
采用自行车模型的一大好处就在于它简化了前轮转向角与后轴将遵循的曲率之间的几何关系，其关系如下式所示：
$tan(\delta )=\frac{L}{R}$
其中$\delta$表示前轮的转角，L为轴距，R则为在给定的转向角下后轴遵循着的圆的半径。这个公式能够在较低速度的场景下对车辆运动做估计。

纯跟踪算法

从自行车模型出发，纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角 ， 使车辆可以沿着一条经过目标路点（goal point）的圆弧行驶，如下图所示：
图中$(g_x,g_y)$是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点，$l_d$表示车辆当前位置（即后轴位置）到目标路点的距离， $\alpha$表示目前车身姿态和目标路点的夹角，那么根据正弦定理我们可以推导出如下转换式：
因为道路曲率$k=\frac{1}{R}$,上式也可以表示为：
$k=\frac{2sin(\alpha )}{l_d}$

带入上式，再把时间变量加入，得到：
这里我们把时间考虑进来，在知道t时刻车身和目标路点的夹角 α(t) 和距离目标路点的前视距离$l_d$的情况下，由于车辆轴距 L 固定，我们可以利用上式估计出应该作出的前轮转角 δ ，为了更好的理解纯追踪控制器的原理，我们定义一个新的量:$e_l$—— 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：
$sin(\alpha )=\frac{e_l}{l_d}$
则曲率可以表示为：
$k=\frac{2sin(\alpha )}{l_d}=\frac{2}{l_d^2}{e}_l$
考虑到本质是横向上的误差，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为$\frac{2}{l_d^2}$，这个P控制器受到参数$l_d$（即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说，$l_d$ 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。
一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即 $l_d=K{v}_x$ ，那么前轮的转角公式就变成了：
那么纯追踪控制器的调整就变成了调整系数k，通常来说，会使用最大，最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡)