基于并行-交替式双向JPS算法的机器人路径规划

苗红霞，郭章旺，齐本胜，邹 杨，李成林

(1.河海大学 物联网工程学院，江苏 常州 213000;2.江苏省输配电装备技术重点实验室，江苏 常州 213000)

0 引言

路径规划是移动机器人研究领域的关键内容[1]，它是指在障碍物环境下，机器人自主地规划出一条从起始点运行至目标点的无碰撞路径。为解决机器人的路径规划问题，各国科研人员主要提出了以下四类路径规划算法[2]：1)基于智能搜索算法，如：蚁群算法、粒子群算法、快速搜索随机数算法等；2)基于人工智能算法，如：深度学习、Q-Learning算法等；3)基于几何模型算法，如：Dijkstra算法、Voronoi图等；4)基于局部避障算法，如：人工势场法、动态窗口法等。

在上述的四类路径规划算法中，基于智能搜索算法的路径规划其最大的特点是具有随机性，它首先随机生成初始解或采样点，然后通过迭代的方式来逼近最优解，因此它的解是渐进最优且非唯一的，当地图中的最优路径比较狭窄时，该方法可能会规划出较长的冗余路径。基于人工智能算法的路径规划能够让机器人自主对环境进行学习以实现自主路径规划，其通常只使用在具有大量数据样本的场景中。基于避障算法的路径规划主要适用于移动机器人在局部地图中的避障场景，主要用于提高移动机器人的安全性。基于几何模型的路径规划算法能够实时调节根据最优策略得到的可行解，其在工业、农业、医疗等领域的移动机器人路径规划中均得到广泛应用，极大地提高了生产生活的效率，适用于大规模静态场景下的移动机器人路径规划问题。

基于几何模型的路径规划先需根据已知的环境信息构建地图几何模型，再在几何模型中规划合适的路径。常见的地图模型有3种：1)语义地图；2)栅格地图；3)拓扑地图。构建语义地图[3]需要与同步定位与建图技术相结合，其主要应用于识别环境中独立个体，以应对复杂场景及完成更加智能的任务。栅格地图能够反应物体真实的物理尺寸,在实时环境中有助于机器人的定位与导航[4]。拓扑地图是对地理空间的抽象和形式化描述[5]，其采用节点和连线来表述环境信息，通常关键位置以节点的形式表示，连线表示节点之间是否允许通行。相较于只表示不同节点之间连通关系的拓扑地图而言，栅格地图能够唯一确定地表示机器人、障碍物、可行路径的确切位置信息。与此同时，栅格地图还有具有易于构建和保存的特点，因此各国的工程师和科研人员研究提出了诸多建立在栅格地图基础上的路径规划算法。

A*算法是一种可应用在栅格地图中的启发式路径规划算法，其在搜索过程中具有一定的方向性，具有运行效率较高的特点[6]。该方法广泛应用于单机器人在全局静态环境中的路径规划。文献[7]通过在估价函数中加入车身轮廓代价和障碍物距离代价对A*算法进行改进；文献[8]参考了人工势场法的引力思想，提升了A*算法的效率。即便如此，在障碍物栅格数量较少的空旷区域，A*算法仍需对栅格进行逐个搜索，这导致了冗余的搜索栅格增多，降低了算法的效率[9]。

针对A*算法在空旷区域搜索效率十分有限的问题，跳点搜索算法(JPS，jump point search)在A*算法的基础上进行了优化。文献[10]的验结果表明，由于跳点搜索算法在扩展过程中裁剪了无用的节点，其路径规划速度高于A*算法。文献[11]提出了一种正六边形栅格跳点搜索算法，并以此解决智能体在障碍物地图中的路径规划问题；文献[12]提出通过“块”操作的方法减少跳点搜索过程中不必要的节点，进一步加快跳点搜索算法的速度。然而，在障碍物位置分布随机的情况下，障碍物个数的增加导致了跳点数量的攀升，跳点搜索算法的路径规划的时间仍大幅度增加。此外，由于跳点搜索算法没有考虑机器人的体积大小，其规划出的路径存在紧贴障碍物、轨迹不平滑[13]问题，给机器人的运行带来安全风险。

在障碍物位置随机的栅格地图中，针对跳点搜索算法路径规划时间大幅度增加的问题，本文提出了并行-交替式双向跳点搜索(PA-BJPS，parallel alternate bidirectional jump point search)算法，其首先在障碍物位置随机的栅格地图里确定一个中心热点区域，在该中心热点区域的外部采用并行运算的方式规划路径，在中心热点区域内部采用交替运算的方式规划路径，有效地缩短了路径规划的时间。针对跳点搜索算法规划的路径存在安全性和平滑性不足的问题，本文提出迭代式路径修正方法来改良危险路径，并采用3次B-样条曲线替代拐角来平滑路径。仿真实验结果表明，本文算法有效地缩短了路径规划的时间，同时兼顾了路径的安全性和平滑性，具有良好的工程应用前景。

1 跳点搜索算法及其不足

跳点搜索算法[14]在保留A*算法框架的同时，优化了A*算法对后续节点扩展的操作。通过对扩展节点的筛选和处理，跳点搜索算法不仅缩短了路径规划的时间，而且减小了节点信息的内存占用量。然而，跳点搜索算法只将移动机器人视为一个质点，因此由跳点搜索算法规划的路径可能会由于移动机器人紧贴障碍物而产生碰撞风险。此外，若采用跳点搜索算法进行双向的路径规划，还可能会造成路径规划时间增长和路径冗余的问题。

1.1 跳点搜索算法

在进行路径规划时，跳点搜索算法对扩展过程中的无用节点进行裁剪，只保留了用于扩展的跳点。当跳点搜索算法扩展至目标点后，即得到一条从起始点到目标点的无障碍路径。

跳点搜索算法进行路径规划时，首要筛选栅格中的强迫邻居(Forced Neighbour)。当栅格节点x的八邻域中有障碍物，且节点x的父节点p经过x到达节点n的代价距离，比父节点p经过任何其他节点到达节点n的代价距离更小，则称n为x的强迫邻居，强迫邻居的筛选规则如式(1)：

len()x)

(1)

强迫邻居主要包含两种类型，横向强迫邻居如图1(a)所示，在图1(a)中画圈的节点为x的斜向强迫邻居；斜向强迫邻居如图1(b)所示，在图1(b)中画圈的节点为x的横向强迫邻居。

在筛选出强迫邻居的基础上，还需对跳点进行筛选。如果节点x满足下列3个条件之一，则该节点为跳点。

1)节点x是起始点或目标点；

2)节点x至少有一个强迫邻居；

3)在斜向搜索情况下，节点x的水平或垂直方向上有满足1)，2)的节点。

在进行节点扩展时，如果搜索到符合要求的跳点，则将该跳点加入OpenList列表。在OpenList列表中，每次扩展只选取评价函数最小的节点作为下一个待扩展的节点[15]。跳点搜索的评价函数公式定义如式(2)：

f(n)=g(n)+h(n)

(2)

式(2)中，f(n)为节点n的评价函数，它由实际代价函数g(n)和预计代价函数h(n)组成。实际代价函数g(n)反映了从起始点到当前节点的实际距离；预计代价函数h(n)估算了当前节点到目标点的剩余距离，通常预计代价函数h(n)采用欧几里得距离[16]、曼哈顿距离[17]或切比雪夫距离[18]进行计算。

跳点搜索算法的路径规划结果如图2所示。图2中，栅格s为起始点，栅格g为目标点，黑色栅格为障碍物，灰色栅格为路径规划过程中扩展的跳点，箭头标识了从起始点到目标点依次经过的路径。从图2中可知，跳点搜索算法对路径规划过程中的冗余节点进行了裁剪，实现了对节点的跳跃搜索，从而加快了路径规划的速度。

图2 跳点搜索路径规划

1.2 跳点搜索算法的不足

与A*算法相比，跳点搜索算法虽然加速了路径规划的速度，但是它仍存在一些不足。在路径规划时，跳点搜索算法只将移动机器人视为一个质点，并未考虑移动机器人的体积大小，因此由跳点搜索算法规划出来的路径存在机器人紧贴障碍物的问题[19]。紧贴障碍物的危险路径示意图如图3所示，在图3中用银白色圆圈标记了危险路径的位置，如果机器人按照图3中的路径运行，将在银白色圆圈位置与障碍物发生碰撞。此外，由跳点搜索算法规划的路径由栅格节点依次相连而成，因此路径存在不连续，拐角很多且不够平滑的缺点。在路径的拐角处，移动机器人必须停下，原地旋转至设定角度，再向前运行。频繁的急停急起操作，不仅不符合移动机器人的运动特性，还加大了移动机器人的后期维护成本。

图3 紧贴障碍物的危险路径

在移动机器人路径规划问题中，若采用双向搜索策略，从起始点和目标点同时规划路径，将有效地提高路径规划的效率[20]。然而，当使用跳点搜索算法进行双向的路径规划时，可能会发生以下3种可能导致冗余路径增多或路径规划时间增长的情况。

情况1：如图4所示，当障碍物形状为对称图形时，从起始点规划的正向路径从障碍物的上方经过，从目标点规划的反向路径从障碍的下方经过，正向和反向的路径有一定概率分别从两侧经过障碍物两侧，规划出了两条不相交的路径。若发生这种情况，路径规划的时间不减反增，同时还增加了移动机器人路径规划的计算量。

图4 对称障碍物下的双向路径规划

情况2：跳点搜索算法采用了跳跃搜索的方式，它裁剪了路径规划的过程中不能成为跳点的栅格。跳跃搜索的特性虽然能够达到较少内存及提高路径规划的速度的目的，但是在双向路径规划的情况下，可能会在一侧跳点扩展的过程中，越过另一侧路径正在扩展的跳点，从而导致路径冗余的产生，同时也大大增加了路径规划的时间。

情况3：在障碍物位置随机的栅格地图中，从起始点和目标点出发的跳点搜索算法的方向和距离均存在很大的不确定性。由于障碍物的位置是不规则的，且跳点搜索算法具有跳跃扩展的特性，所以从起始点和目标点规划的两条路径，其相遇节点可能距离两点连线的中心较远，这也将导致路径规划的时间大大增加。

为避免发生上述导致路径规划时间增长的情况，同时进一步提升机器人路径规划的速度，并兼顾运行路径的安全性和平滑性，本文提出了基于并行-交替式双向跳点搜索算法的机器人路径规划方法。

2 并行-交替式双向跳点搜索算法

与传统的跳点搜索算法不同，并行-交替式跳点搜索算法首先在起始点与目标点间确定了一个中心热点区域，从而增加双向并行搜索的方向性；其次，采用并行式双向跳点搜索算法，分别规划从起始点抵达中心热点区域以及目标点抵达中心热点区域的路径，在该过程中无需进行信息交换和状态同步，因此缩短了移动机器人路径规划的时间；最后，采用交替式双向跳点搜索算法，相向地规划中心热点区域内部的路径，将中心热点区域外部的路径与内部的路径拼接，即得到一条从起始点到目标点的完整路径。

2.1 中心热点区域

在双向的跳点搜索算法进行路径规划时，从起始点和目标点出发的两条路径，其搜索方向存在不确定性，因此可能会发生在1.2节中所述的导致冗余搜索节点增多、路径规划时间增加的3种情况。

为解决上述问题，本算法首先在栅格地图中确定一个如图5所示的中心热点区域，再采用并行式双向跳点搜索算法，分别规划从起始点和目标点抵达中心热点区域的路径。确定中心热点区域之后，并行式双向跳点搜索算法以中心热点区域作为全局路径规划初始目标。中心热点区域不仅能够增强双向路径规划的方向性，还能够将两侧路径的相遇位置限制在中心热点区域内部，避免了冗余路径的产生。本算法所确定的中心热点区域的形状是一个圆形，其轨迹的计算公式如式(3)：

图5 中心热点区域

(x-xcenter)2+(y-ycenter)2=R2

(3)

式(3)中，xcenter和ycenter是圆心的横坐标和纵坐标，它是起始点与目标点中点，R是圆的半径。经过多次实验测试得出，当中心热点区域的半径长度为起始点与目标点欧式距离的1/6左右时，本算法能较好地兼顾路径规划的时效性和方向性。

在中心热点区域外部，采用并行的方式规划路径，当扩展的跳点进入中心热点区域内部时，停止并行规划，并记录下由并行式双向跳点搜索算法规划的进入中心热点区域的第一个跳点。在中心热点区域内部，从记录下的跳点开始，采用双向交替的方式相向地规划路径，从而确保两条相向的路径能够连接在一起。

在障碍物位置随机的栅格地图中，非障碍物节点之间往往具有良好的连通性，因此当起始点和目标点并行规划的路径抵达中心热点区域之后，在中心热点区的内部亦能相向地规划出一条相遇的路径，最后将中心热点区域内部和外部的路径相连，就得到一条从起始点到目标点的全局路径。

2.2 并行式双向跳点搜索算法

在中心热点区域外部，并行式双向跳点搜索算法分别了从起始点和目标点规划抵达中心热点区域的路径。在进行路径规划的过程中，其采用了并行计算的思想，并且在该过程中无需进行信息交换和状态同步，因此加快了路径规划的速度。以从起始点侧出发的并行式双向跳点搜索算法为例，进行路径规划的步骤为：

步骤1：将起始点加入OpenList_S；

步骤2：寻找OpenList_S中h(n)最小的点，设置为current_S；

步骤3：判断路径1的当前节点current_S是否位于中心热点区域内部，若是则结束路径1搜索，并将current_S标记为A0，若否则进行步骤4；

步骤4：OpenList_S删除当前节点current_S，在CloseList_S加入当前节点current_S；

步骤5：根据跳点扩展规则，拓展下一个跳点point；

步骤6：判断跳点point是否在OpenList_S中，如果否则将跳点point加入OpenList_S，如果是则更新g(n)的值以及父节点的位置，并返回步骤2。

由于并行式双向跳点搜索算法在路径规划过程中不进行信息交换和状态同步，因此，为了进一步加强双向搜索的方向性，并行式跳点搜索算法对预计代价函数进行了改进。按照改进后的预计代价函数进行路径规划，能够同时兼顾中心热点区域的位置和目标点的位置。

以从起始点侧出发的并行式双向跳点搜索算法为例，其预计代价函数计算公式如式(4)：

hs(n)=p·hs1(n)+(1-p)·hs2(n)

(4)

式(4)中，p是权值，是节点n到中心热点区域圆心的欧几里得距离，节点n到目标点的欧几里得距离。

从目标点侧出发的路径，与从起始点出发的路径规划是并行运算的，其路径规划步骤与起点侧的步骤对称一致，它的预计代价函数的计算公式如式(5)：

hg(n)=q·hg1(n)+(1-q)·hg2(n)

(5)

式(5)中，q是权值，是节点到中心热点区域圆心的欧几里得距离，是节点到起始点的欧几里得距离。

2.3 交替式双向跳点搜索算法

在规划中心热点区域内部的路径之前，先读取由并行式双向跳点搜索算法规划的跳点标A0和B0，再采用交替式双向跳点搜索算法在A0点和B0点之间相向地路径规划。交替式双向跳点搜索算法的步骤为：

步骤1：将A0和B0分别放进OpenList_A和OpenList_B中；

步骤2：寻找OpenList_A中最小的点，将其设置为current_A；

步骤3：OpenList_A删除current_A节点，在CloseList_A中加入当前节点；

步骤4：根据跳点扩展规则，寻找不在CloseList_A中的跳点A1；

步骤5：判断A1是否在OpenList_A中，若否则加入OpenList_A，若是则更新的值以及父节点的位置；

步骤6：广播A1的位置；

步骤7：寻找OpenList_B中最小的点，将其设置为current_B；

步骤8：OpenList_B删除current_B节点，在CloseList_B中加入当前节点；

步骤9：根据跳点扩展规则，寻找不在CloseList_B中的跳点B1；

步骤10：判断B1是否在OpenList_B中，若否则加入OpenList_B，若是则更新的值以及父节点的位置；

步骤11：广播B1的位置；

步骤12：判断是否搜索到一个公共重合节点，若是找到完整路径，并结束搜索，若否则返回步骤2。

在交替式双向跳点搜索算法中，每进行一次跳点扩展，都需要对新跳点的位置坐标进行广播。当另一侧进行跳点扩展时，需以本侧广播的新跳点作为目标位置。

从A0点一侧规划的路径，在进行跳点扩展时，其预计代价函数的计算公式如式(6)：

从B0点一侧规划的路径，在进行跳点扩展时，其预计代价函数的计算公式如式(7)：

式(6)和式(7)中，xA_cur、yA_cur为A0点一侧当前跳点位置坐标，xB_cur、yB_cur为B0点一侧当前跳点位置坐标。

得到中心热点区域内部的完整路径后，将中心热点区域外部的路径与内部的路径拼接，即可得到一条从起始点到目标点的完整路径。

3 局部路径优化

跳点搜索算法在进行路径规划时，没有考虑移动机器人的体积，因此规划的路径存在移动机器人与障碍物发生碰撞的风险。此外，跳点搜索算法规划的路径由多个跳点栅格依次相连而成，因此规划出来的路径具有不连续、拐角多、不平滑的缺陷。这不仅不符合机器人的运动学特征，同时也给机器人带来安全隐患。针对上述问题，提出了迭代式路径修正方法，并采用3次B-样条曲线对路径进行平滑处理。

3.1 迭代式路径修正

在基于跳点搜索算法的路径规划过程中，根据跳点扩展的规则，路径仅先保留了跳点的信息，再将跳点依次相连，继而得到从起始点到目标点路径。因此，由跳点搜索算法规划的路径具有稀疏性，要获得路径的完整坐标信息，需要对路径跳点进行双线性插值[21]处理。

相邻跳点之间的栅格数是不固定的，因此将插值点间的插值步长设定为单个栅格的长度。双线性插值完成后，得到一条从起始点到目标点的完整路径，该条路径中每个栅格的坐标信息均已悉知，双线性插值的公式如式(8)：

(8)

式(8)中，和是插值点两端跳点的坐标，是插值点的坐标。

在获取全局路径的信息之后，采用迭代式路径修正方法对存在安全隐患的局部路径进行修正，迭代式路径修正方法的步骤为：

步骤1：循环遍历路径route；

步骤2：判断路径route中第k个元素的上下左右邻域中是否存在障碍物obstacle，如果是执行步骤3，如果否退出当前路径修正；

步骤3：判断路径route中第k+1个元素的上下左右邻域中是否存在步骤2中得到的障碍物obstacle，如果是执行步骤4，如果否则退出当前路径修正；

步骤4：根据式(9)，计算路径转角周围需要检测栅格的位置；

P1={(xk,yk+1)}

P2={(xk+1,yk)}

(9)

式(9)中，xk和yk是路径中第k个元素的横坐标和纵坐标，xk+1和yk+1是路径中第k+1个元素的横坐标和纵坐标。

步骤5：将路径修正到P1或P2对应的位置，并返回步骤1。

修正前路径如图6(a)所示，修正后的路径如图6(b)所示。修正后的路径与障碍物的最短距离维持在半个栅格长度以上，从而有效地避免了移动机器人在运行过程中与障碍物发生碰撞的问题。

图6 迭代式路径修正示意图

3.2 路径平滑

移动机器人若按照经过迭代式修正后的路径运行，虽可以有效地避免与障碍物发生碰撞，但是路径中拐角数量的增多限制了移动机器人的运行效率。在栅格地图中，如果按照跳点搜索算法规划的路径运行，机器人不得不在拐角处停下，原地旋转至设定角度，再继续向前运行，这不符合机器人的运动特性。

非均匀B-样条具有仿射不变性等优势[22]，因此广泛应用于计算机辅助设计、制造等工程应用中。递推式B-样条曲线的基函数的定义如下：

设U={u0,u1，…，un}为一不减的实数序列，以U作为样条节点，第i个p次B-样条基函数Ni,p(u)的定义如式(10)，并约定，当式(10)出现0/0形式的商时，其比值为0。

(10)

本文采用3次B-样条曲线对路径进行平滑处理。平滑前后的路径对比如图7所示，从图7中可以看到，平滑后的路径对拐角进行了优化，移动机器人按照平滑后的路径运行，在拐角处可以柔滑地通过，无需停下并原地旋转，更加符合移动机器人的运动特性。

图7 平滑前后的路径对比示意图

4 实验结果分析

为验证并行-交替式双向跳点搜索算法的有效性，分别在栅格地图中放置不同比例的随机障碍物进行仿真实验。仿真使用的设备配置为：Windows10操作系统，四核Core i5-6300处理器，主频为2.3 GHz，内存大小为8 GB，仿真运行平台为MATLAB R2016a。

4.1 全局路径规划结果

在大小为100*100，随机障碍物比例为1.5%栅格地图中，使用并行-交替式双向跳点搜索算法跳点搜索算法进行路径规划结果如图8所示。在图8中，中心热点区域外部的是由并行式跳点搜索算法规划的，A0和B0分别是起始点侧和目标点侧的路径抵达中心热点区域的第一个跳点。A0与B0之间的路径，是采用交替式双向跳点搜索算法规划的。对贴近障碍物的局部路径进行放大观察，优化后的路径(实线)与优化前的路径(虚线)相比，优化后的路径更好地兼顾了安全性和平滑性，按照优化后的路径运行，移动机器人不仅避免了与障碍物发生碰撞风险，同时也较少了在拐角处急停急起的次数，确保了移动机器人安全平稳运行。在中心热点区域外部，并行式双向跳点搜索算法无需进行信息交换与状态同步，在没有冗余路径的前提下，加快了路径规划的速度。在中心热点区域内部，交替式双向跳点搜索算法相向地规划到了重合的节点，得到了区域内部最短路径。将中心热点区域内外的路径相连，得到从起始点到目标点的全局路径。

图8 路径规划仿真结果

4.2 路径规划的时效性

不同随机障碍物比例下的路径规划时间见表1。从表1可以看出，随着障碍物比例逐渐升高，A*算法的路径规划时间基本保持不变，这是由于A*算法需要逐个计算路径中每个节点的信息，而障碍物比例升高对路径中节点数量的影响较小。对于跳点搜索算法而言，障碍物比例的升高导致了跳点数量的攀升，因此跳点搜索算法路径规划时间也随之增长。与跳点搜索算法相比，并行-交替式双向跳点搜索算法在中心热点区域外部，采用并行的方式进行路径规划，有效地缩短了路径规划的时间。

表1 不同障碍物比例下的路径规划时间 s

4.3 路径规划长度

不同随机障碍物比例下的路径规划长度见表2。从表2可以看出，在不同障碍物比例下，基于并行-交替式双向跳点搜索算法路径规划长度都略大于A*算法和跳点搜索算法。通过对全局路径位置信息进行逐一分析可知，路径长度增加是由于对局部路径进行修正和平滑造成的。相较于移动机器人可能存在的安全隐患而言，微量的路径长度增加几乎可以忽略不计。

表2 不同障碍物比例下的路径规划距离 m

4.4 路径的平滑性

路径的曲率K描述了移动机器人运行过程中法向加速度关于弧长参数的变化情况，曲率越大，路径越不平滑。根据式(11)，计算从起始点到目标点完整路径的曲率变化情况。

(11)

使用3次B-样条曲线进行平滑处理前后，路径的曲率变化情况如图9所示。如图9(a)所示，平滑处理前的路径曲率数值很大，其变化过程也十分尖锐。经过3次B-样条曲线平滑处理后的路径曲率如图9(b)所示，路径的最大曲率相较于处理前大幅度降低，变化的过程也比平滑前更加平缓，按照平滑处理后的路径运行，移动机器人无需在拐角处停下，避免了急停急起的问题。

图9 平滑处理前后曲率变化对比

5 结束语

针对在障碍物随机的栅格地图中，跳点搜索算法路径规划时间较长的问题，本文提出了基于并行-交替式双向跳点搜索算法的机器人路径规划算法，并行式双向跳点搜索算法采用了并行运算的思想，无需进行信息交互和状态同步，加快了路径规划的速度。交替式双向跳点搜索算法每进行一次跳点扩展，需实时更新跳点的位置信息，以确保找到中心热点区域内部的最短路径。为避免机器人紧贴障碍物而产生的安全问题，本文提出迭代式路径修正方法，并使用3次B-样条曲线对路径进行平滑处理。仿真实验结果表明，本文所提算法能够有效地缩短路径规划时间，同时所规划路径兼顾了安全性和平滑性，具有良好的学术研究和工程应用价值。