Pure Persuit算法基础

纯跟踪（Pure Pursuit）算法是一种用于移动机器人或自动驾驶车辆的路径跟踪方法。其基本目标是让机器人沿着预定路径行驶，同时保持一定的参考速度。在给定线速度的前提下，它计算移动的角速度令机器人从其当前位置到达机器人前方的某个前瞻点（lookahead）。该算法根据机器人的当前位置不断地追着它前面的一个点，直到路径的最后一个点。
根据模型的不同，Pure Pursuit算法需要使用不同的控制公式，例如：

自行车模型：

如上图所示，点C为路径上的一个点（前视点/预瞄点），Pure Pursuit算法的目标则是控制B的角速度，使得A点可以经过C点。
假设我们以AC两点画一个圆弧，注意，由于要考虑到运动学约束，这段圆弧应当与直线AB相切，也就是角OAB=90度。同时我们知道对于圆弧，OA=OC，三角形AOC为等腰三角形。
$$\angle OAC=\angle OCA$$
同时有：
$$\angle OAC+\angle CAB=\frac{\pi }{2}$$
即：
$$2\ast （\angle OAC+\angle CAB）=\pi$$
于是可以得到：
$$\angle OAC+\angle OCA+2\ast \angle CAB=\pi$$
通过三角形内角和可以知道:
$$\angle AOC=\pi -\angle OAC-\angle OCA=2\ast \angle CAB$$
过O点做AOC的角平分线，根据等腰三角形原理，其垂直于AC且平分AC。于是有：
$$sin\alpha =\frac{l_d}{2R}$$
然后，我们连接OB，需要求出B点在当前位置绕O点的旋转角度，则B点轮子的朝向应垂直于OB，于是根据几何图形原理我们可以知道：
$$\angle \delta =\angle AOB$$
即：
$$tan\delta =\frac{L}{R}$$
而根据前面我们知道：
$$R=\frac{l_d}{2sin\alpha }$$
联立上式得到：
$$tan\delta =\frac{2Lsin\alpha }{l_d}$$
推出：
$$\delta =\arctan (\frac{2L\sin (\alpha )}{l_d})$$
根据上式，在知道t时刻的车身和目标路径点的夹角\alpha以及距离目标路径点的前视距离l_d的情况下，轴距一定，可以利用上式估计出需要的前轮转角。

差速模型：

对于差速模型的机器人，其没有前后轮的距离L这个参数，其运动由以下方程控制：
$$v=R\ast w$$
上式：
$v$是机器人的线速度
$w$是机器人的角速度
$R$是机器人的旋转半径
在自行车模型中我们知道：
$$R=\frac{l_d}{2sin\alpha }$$
联立方程可以得到：
$$w=\frac{v}{R}=\frac{2vsin\alpha }{l_d}$$

双舵轮模型：

双舵轮模型与自行车模型类似，相当于两个自行车模型的拼接，同样可以按照自行车模型进行分析。

算法测评

在较低速度下给定一条三阶贝塞尔曲线，使用Pure Persuit算法进行跟踪，实测效果图如下：
起始：
结束：

上图中红色线条为贝塞尔的输出轨迹，蓝色线条为机器人实际运动轨迹，基本能完成较平滑曲线的跟随效果。

个人总结

Pure Persuit算法本身的原理不难理解，它主要基于几何原理进行的设备控制，本身不考虑太多的运动学约束。同时机器人的路径跟踪效果应该会比较依赖于预瞄点。对于曲线部分而言，如果曲率很小而预瞄点很远的话，对于曲线的跟随性应该会很差。对于直线而言，如果设备在开始时与轨迹有横向偏差，则在运动过程中机器人会逐渐收敛到曲线，但是收敛速度取决于预瞄点距离。同时这个算法本身虽然跟随曲线，但是本身并不会很大程度上考虑轨迹曲线，而是以离散点的形式存在，所以它的横向到点精度受曲线本身影响，不能保证机器人到点时完全消除横向误差。
参考：
《车辆控制纯跟踪算法（pure pursuit）》
《纯跟踪算法（Pure Pursuit）在差速机器人上的应用》
《双舵轮AGV轨迹跟踪Pure Pursuit算法模型分析、python代码实现》
《ROS实现无人驾驶控制算法（一）——pure pursuit 》