王 勇 （安徽合力股份有限公司，安徽 合肥 230022）

1 概述

Pure Pursuit算法原理简明易懂，不需要高阶数学计算和繁琐过程推导，可以用于显式或隐式路径跟踪问题，特别适合移动机器人路径跟踪控制。当对移动机器人行驶方向与到达目标点无严格方向要求时，可以直接采用Pure Pursuit算法，控制移动机器人沿路径行驶或跟随某导向物体行驶。但在大多数情况下，移动机器人行驶到达路径上的目标点时，一般对机器人的朝向都有严格要求。对于采用Pure Pursuit算法的机器人必须在到目标点之前对机器人行驶方向进行调整，以满足机器人到达目标位置时的方向要求。如装卸货物的AGV叉车，不仅要求车辆准确到达目标点位置，而且还要求货叉与被装卸货物托盘的叉孔保持平行。此时，对于采用Pure Pursuit算法的移动机器人就必须对Pure Pursuit算法进行修正，以满足机器人在目标点的方向要求。

2 Pure Pursuit算法原理

Pure Pursuit算法原理如图1所示。为分析方便，假定移动机器人局部坐标系原点位于左、右两驱动轮轴线与车辆中心线交点SP上，X、Y轴按右手规则确定，Y轴正向为移动机器人前进方向。假定目标点GP位置已知，此时通过控制左、右驱动轮不同速度，使车辆始终沿Y轴切线方向按曲率γ的圆弧曲线行进，在出发点到目标点之间不断校正当前与目标位置的误差，车辆即可准确到达目标点。算法中规定曲率γ是带符号的参数，当车辆顺时针沿曲率γ行驶时γ取负值，反之取正值。

移动机器人从出发点SP到目标点GP曲率γ由下式计算得到：

式中：L——移动机器人从出发点SP到目标点GP距离；

图1 Pure Pursuit算法原理

xg——目标点GP在局部坐标系XY坐标系中的X坐标值。

3 算法修正

由Pure Pursuit算法可知，当移动机器从SP出发到GP时虽然能够满足位置要求，但是不能保证机器人与目标点的方向要求。如图2所示，移动机器从SP到达GP后方与目标点方向差异。

为了保证移动机器到达GP点后方向仍然与目标点GP要求方向相同，此时需要对Pure Pursuit算法进行适当修正。修正方法是：仍然保持Pure Pursuit算法的基本思想，在SP和GP之间增加一个过渡点TP。SP到TP移动机器人沿曲率γ1行驶，到达TP点后，按曲率γ2行驶。从SP到TP的路径曲线γ1与TP到GP的路径曲线γ2相切，γ2与目标点GP所要求的方向相同。这样修正后的Pure Pursuit算法不仅保持从SP到GP的位置，同时还能保持移动机器人到达GP后的方向（Y轴正向）与GP点要求的指向相同。对Pure Pursuit算法的修正见图3，在SP与GP之间增加中间点TP。

图2 机器人方向与目标点方向差异

事实上，在SP到GP之间满足上述修正条件要求的TP点有无穷多个，如何确定一个实用的TP点就成为修正后的Pure Pursuit算法的关键。按照图3，下面给出了一种简便切实可行求取TP点的参考方法。

图3 增加中间点TP调整移动机器人方向

首先，假定GP点在局部坐标系第一象限（第二象限时有类似方法，不再赘述），按控制律求出移动机器人从SP沿曲率为γ的路径曲线到达GP时切线方向β，判断目标点GP的方向α与β之间的关系：

（1） β+π≥α>β时，在方向轴线上方插入 TP1点，并令 γ2＞0。

（2） α=β时，不需要中间点TP点。

（3） β>α≥β-π 时，在方向轴线下方插入TP2点，并令 γ2＜0。

其次，为移动机器人从TP1（或TP2）点沿曲率γ2到GP点的半径值，这里使用SP到GP的距离为L，令：

在几何上当曲率γ2过小，从SP到达GP点既使得曲率γ1与曲率γ2两路径曲线在TP1（或TP2） 相切，同时还满足曲率γ2在GP点与目标方向相同的中间点可能并不存在。因此，依据SP到GP之间的距离来确定恰当的曲率γ2是非常必要的。一般取0.1L≥1/γ2≥0.25L。这里是通过事先确定曲率γ2来求取曲率γ1，如果先确定曲率γ1再求曲率γ2，求解过程的条件判断繁琐，不推荐不再赘述。

最后，确定曲率γ2之后（含正负），从SP到GP的中间点TP1（或TP2）就唯一确定，利用SP为原点的局部坐标系中几何关系，可推导得出以下解析式：

式中：α——目标点GP与x轴正向所成角度（逆时针为正，顺时针为负）；

Ø1——从SP点到TP1（或TP2） 转角（逆时针为正，顺时针为负）；

a,b——目标点GP的x,y坐标值；

TPx,TPy——TP1（或TP2） 的x,y坐标值。

4 程序验证

下面通过程序分别对比了未进行修正前的Pure Pursuit算法和采用中间点进行修正后的Pure Pursuit算法程序仿真验证情况。图4为未进行修正的Pure Pursuit算法的路径跟踪情况，图5为修正后的Pure Pursuit算法路径跟踪情况。从图4中可以看到，算法在未修正前虽然能很好地跟踪目标路径上的关键点，但关键点处的方向与所要求的方向并不吻合；从图5中看到采用修正后的算法，机器人不仅与目标路径曲线跟随吻合，方向与路径方向完全相同。

图4 未修正Pure Pursuit情况

图5 修正后Pure Pursuit情况

5 结论

论文对Pure Pursuit算法的原理和局限性进行了阐述，给出了算法修正的方法，并对修正后的算法进行了计算仿真，验证了修正方法的正确性。论文对采用Pure Pursuit算法进行路径跟踪的移动机器人在行驶方向有要求时提供了算法参考。