Pure pursuit 轨迹跟踪算法

目前手头有一辆小拖车，需要用到pure pursuit算法，故拜读了大神的博客后，整理了此篇文章，首先奉上大神链接

Pure Pursuit算法最早是用于差速轮底盘小车，现在经过适配用于阿克曼底盘运动模型车辆（阿克曼运动模型这里就不多说了）。首先上个图，

PP算法以车辆后轴中心为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆按照一段圆弧经过目标路点，$l_d$表示车辆当前后轴中心到目标点的距离，$\alpha$表示当前车辆和目标路点的heading角度偏差。
根据正弦定理有，
$$\frac{l_d}{sin(2\alpha )}=\frac{R}{sin(\frac{\pi }{2}-\alpha )}$$
化简后，有
$$\frac{l_d}{sin\alpha }=2R$$
也可以表示为
$$k=\frac{2sin\alpha }{l_d}$$
式中，$k=1/R$为阿克曼运动模型车辆行驶的圆弧，那么前轮转角$\delta$的表达式为：
$$\delta =\arctan (kL)$$
$L$为车辆轴距，综上所述，有
$$\delta (t)=\arctan \frac{2Lsin(\alpha (t))}{l_d}$$
在$t$时刻，知道车身和目标点之间的夹角$\alpha (t)$和目标路点之间的前视距离$l_d$的情况下，车辆轴距是固定值$L$，可以用上式计算出当前的车辆前轮需要偏转的控制量$\alpha$。
为了便于后面的计算，定义一个新的量$e_l$，表示车辆当前位置和车辆目标点之间的横向误差，由此可得：
$$sin(\alpha )=\frac{e_l}{l_d}$$
圆弧的弧度计算可以表示为：
$$k=\frac{2}{l_d^2}{e}_l$$
可以看出PP算法的本质就是类似一个横向的P控制器，系数$P$为$\frac{2}{l_d^2}$，这个$P$系数受到参数$l_d$的影响很大，因此，引入了一个新的问题，如何选取前视距离是PP算法的关键，一般来说，Autoware中的PP算法就是使前视距离为当前车速乘以一个比例系数，在不同的时刻选取不同的前视距离。
PP算法其实比较适合用于差速轮小车的运动跟踪，在阿克曼模型中，用起来并不是很合适，尤其是在车速变化较大的情况，前视距离如何选取是个很大的问题，如果选取距离过短，会导致轨迹无法收敛，车辆横向控制发散，选取距离过长，又会导致车辆转角不足的问题。