融合JPS 和改进A*算法的移动机器人路径规划

张 庆，刘 旭，彭 力，朱凤增

物联网技术应用教育部工程研究中心（江南大学 物联网工程学院），江苏 无锡214122

路径规划是移动机器人完成复杂任务的前提和保证，也是移动机器人的关键技术之一[1-2]。目的是根据有障碍物的环境中的某些优化条件，找到从起点到目标点的最优或次优路径。考虑移动机器人在室内环境中的应用，如AGV（automated guided vehicle）小车[3]、智能家居[4]、无人机[5]等，要求机器人避开墙壁等障碍物，选择最优路径移动。在这个过程中，小车不仅需要具备简单的避障功能，更重要的是能够快速实现路径规划，提高效率。为此，本文研究了具有墙等障碍物的复杂室内环境下移动机器人的快速路径规划问题。

针对移动机器人的路径规划问题，国内外许多学者进行了大量的研究。根据对环境信息掌握程度的不同，可分为全局路径规划[6-9]和局部路径规划[10-13]两类。本文主要研究了全局路径规划问题。目前，用于全局路径规划的方法有蚁群算法[14]、A*算法等。蚁群算法是一种模拟自然界中蚂蚁觅食行为的仿生进化算法。然而，该算法计算量大，收敛速度慢，求解时间长，且解的质量取决于参数设置，容易陷入局部最优解。随着遗传算法种群数量的增加，也存在着计算量大、效率低的问题，容易陷入“早熟”，使目标点无法到达。A*算法相比于蚁群算法往往能更快地求出最优路径。但A*算法仍存在一些缺点。由于A*算法在寻路的过程中，要不断将每个节点及周围八个邻点添加到OpenList 和ClosedList 两个列表中进行评估，寻找代价值最低的节点，A*算法主要的计算量在于对节点的评估和代价最小节点的选择。当应用的场景较大时，由于计算量巨大导致计算速度慢，内存占用大，寻路效率低下。针对此问题，文献[15]提出了将A*算法与迭代加深算法融合，优化了状态判重和估价排序，提高了运算效率；文献[16]提出了提高评估函数的启发强度对算法进行加速，相较于传统A*算法速度大幅度提高，但存在较高的空间复杂度，使得算法对内存占用严重；文献[17]利用无限领域的思想解决了A*算法局限八运动方向的问题，但降低了运算速度；Harabor等人提出跳点搜索算法，对于A*算法路径上的节点进行修剪，然后实现较长距离的跳跃，大幅度提高了运算速度[18]。

本文基于A*算法，但传统的A*算法难以处理复杂的工业大场景，且搜索过程中存在大量冗余节点。

因此，本文从两方面对传统A*算法进行了设计和改进：

（1）改进启发函数的具体计算方式，使启发式函数精确地等于实际最佳路径，减少A*拓展的节点。

（2）使用JPS 搜索策略筛选出跳点添加到OpenList 和ClosedList 中代替A*算法中大量不必要的邻节点，通过筛选跳点不断进行拓展，减少了寻路过程拓展的节点，提高了搜索效率。

1 问题的描述

1.1 环境的表示方法

在移动机器人路径规划之前，首先建立一个机器人坐标系系统，把机器人各传感器信息传递到统一坐标系，基于不同的传感器分别建立里程计运动模型、激光雷达观测模型和IMU 运动模型，最后建立了创建地图所需的栅格地图模型。

本文将移动机器人视为二维栅格地图上的质点，移动机器人的位置以坐标表示，如图1 所示。栅格地图模型将机器人工作环境用若干连续同一尺度的栅格表示，用两种状态表示环境中的可通行区域和障碍区域。图中空白栅格表示移动机器人可以通过，即为空闲状态；黑色栅格表示不可通行的障碍物，为占据状态。

机器人实际运行中，会有多个方向的移动和控制精度导致的偏差。但考虑到模型的复杂性，A*算法中规定，移动机器人在不处于边界和四周有障碍物的情况下，可以向周围8 个方向的栅格移动。由于是二维栅格，不考虑机器人及环境高度，如图2 所示。

1.2 传统A*寻路算法引入

Fig.1 Grid map图1 栅格地图

Fig.2 Motion direction of mobile robot图2 机器人运动方向

A*算法是通过使用启发式函数来引导寻路，从而确保有效地找到最优路径。其基本思想是，通过一个估价函数来确定搜索的方向，朝着目标点开始拓展。再利用评价函数计算得到每个节点的代价值添加到OpenList 中并选择代价最小的邻节点作为下一个拓展节点，重复这一过程直到将目标点添加进OpenList中，最后通过节点的父辈关系反向生成最终路径。A*算法的评价函数f(n)表示为：

式中，n是当前正在拓展的节点；f(n)表示从起始点经过当前节点n到达目标点的评价函数；g(n)表示起点到节点n的实际代价；h(n)表示节点n到目标点的估计代价，h(n)的正确选取能够提升A*算法的效率和准确性。传统A*算法通常采用欧氏距离来计算g(n)和h(n)。

式中，(xs,ys)和(xt,yt)分别是起始点Ps坐标和目标点Pt坐标。可以从评价函数清楚看出，A*算法是将Dijkstra 算法和最佳优先搜索（best-first-search，BFS）算法相结合。当g(n)=0 的时候，A*算法就等价于使用贪心策略的BFS 算法，算法启发性更强即偏向于接近目标点的节点，虽然计算速度最快，但不一定能得到最优解；当h(n)=0 时，A*算法就等价于广度优先搜索的Dijkstra算法，更偏向于接近起点的节点，虽然一定能得到最优解，但计算量大，效率低下。A*算法结合了二者的优点，评价函数综合了g(n)和h(n)，在保证找到最优路径的前提下，提高了搜索效率。因此，合理的计算方式会调整A*算法评价函数的权重比例，能有效提高算法的效果。

1.3 A*算法的实施

A*算法在进行路径规划时会定义两个链表：一个是ClosedList，用来存放已经遍历过的节点；另一个是OpenList，用来存放未遍历并将要被遍历的节点。算法执行过程中，从OpenList中选择代价最低节点加入到ClosedList 同时把该节点周围的邻节点添加到OpenList，并且更新起点到各个节点的g(n)和各个节点到目标点的h(n)，重复以上步骤直到目标点也被添加到OpenList中，算法结束。A*算法的寻路过程如图3 所示。

Fig.3 Path-finding process of A*algorithm图3 A*算法寻路过程

2 改进A*算法

为了解决A*算法路径规划过程中存在的计算量大，内存耗费严重等问题，本文改进评价函数计算方式并结合JPS 跳点算法对传统A*算法进行改进。

2.1 改进评价函数计算方式

启发式函数作为A*搜索算法的核心，对算法的行为有重大的影响。当启发式函数h(n)始终为0 时，则将由从起点到节点n的距离g(n)决定，此时A*算法将等效于Dijkstra 算法；当h(n)始终小于等于节点n到目标点的实际代价，则A*算法一定可以求出最优解，但是h(n)的值越小，算法需要拓展的节点越多，计算量越大；当h(n)大于节点n到目标点的实际代价，则A*不能保证找到一条最短路径，但计算速度更快。

传统A*算法都是采用欧氏距离计算h(n)，但是算法定义了机器人移动是8 方向的，导致规划出的路径会大于欧氏距离计算值，如图4 所示，由于欧氏距离计算出的h(n)小于节点n到目标点的实际代价，导致拓展的节点数量增加。

Fig.4 Euclidean distance and actual path图4 欧氏距离与实际路径

本文采用切比雪夫距离代替欧氏距离，切比雪夫距离是指两个点之间的距离定义为其各坐标数值差的最大值。在向量空间中又称为L∞范数。在直角坐标系下，两点之间的切比雪夫距离为：

因此切比雪夫的启发式函数为：

式中，D是直线移动1 格的代价，(xn,yn)和(xt,yt)分别是当前节点Pn坐标和目标点Pt坐标。

图4 中，使用欧氏距离计算出的代价值低于实际路径成本，相当于在启发函数前加了一个缩放的系数，这样就导致启发性降低，算法更偏向于Dijkstra算法。使用切比雪夫距离计算的代价值等价于实际路径成本，算法的启发性增强，需要评估的节点减少，这样就能使得算法在保证最优路径的前提下也能提高速度。

如图5所示，蓝色为起始点，紫色为目标点，黄色是添加到OpenList 中的节点，红色是添加到ClosedList中参与评估的节点，绿色是最终路径。在同一规划任务下，切比雪夫距离改进的算法参与评估的节点大大减少，规划效果明显优于传统A*算法。

Fig.5 Simulation results of two kinds of heuristic functions图5 两种启发函数的仿真结果

2.2 JPS 策略下改进A*

A*算法结合JPS 策略是在算法执行前增加一个预处理的过程，所谓预处理就是通过JPS 迭代跳点搜索函数，寻找当前节点的继承跳点，修剪掉中间无用的节点，连接跳点实现两点间的长距离跳跃。JPS 算法将会利用两个规则：修剪规则和跳跃规则。

2.2.1 修剪规则

修剪的基本思想就是拓展到节点x的时候，在邻居集合（即neighbours(x)）中筛选出不需要添加到OpenList 进行评估的任何节点n，以便最快地到达目标点。通过比较两条路径的代价值来进行筛选：路径L1从父节点p(x)出发经过节点x到达节点n；另一条路径L2从父节点p(x)出发不经过节点x到达节点n。此外，两条路径上的每个节点都属于neighbours(x)。

根据当前节点的邻节点是否存在障碍物，对修剪规则分两种情况进行讨论。

情况1节点周围不存在障碍物

如图6（a）所示，当前节点x是从父节点p(x)直线向右拓展而至，此时评估灰色节点是毫无意义的，因为从p(x)经过x到达这些节点的代价一定不会比从p(x)直接到这些节点的代价更低；图6（b）是当前节点x从父节点p(x)对角拓展而至，同样的，此时评估灰色节点会导致计算量大且毫无意义，为了筛选出跳点，需要修剪掉这些灰色节点。

Fig.6 Pruning rules without an adjacent obstacle图6 节点周围无障碍物时修剪规则

因此，在节点周围不存在障碍物时，对于直线移动的拓展，有以下修剪条件：

（1）直线移动时，修剪任何满足下式的节点：

（2）对角移动时，修剪任何满足下式的节点：

当A*算法拓展到当前节点x时，根据上述修剪条件对不必要邻节点进行删除操作，这时定义x的剩余邻节点为自然邻节点。

情况2节点周围存在障碍物

如图7 所示，当拓展到节点x时，若neighbours(x)存在障碍物，则无法修剪所有非自然邻节点。对于图7 中绿色的节点，不存在一条不经过节点x的代价最小路径可以从p(x)出发而达到。若要达到它们，则必须经过节点x，否则就不是代价最低路径。对于每一个这样的被迫评估的邻节点，给出筛选条件：

（1）n∈neighbours(x)且不是自然邻节点；

满足筛选条件的neighbours(x)定义为强制邻节点，在拓展过程中这些强制邻节点将作为跳点进行操作。

Fig.7 Pruning rules with adjacent obstacle图7 节点周围有障碍物时修剪规则

2.2.2 跳跃规则

根据修剪规则，筛选出了所有的自然邻节点和强制邻节点，这些就是跳点。将所有的跳点添加到OpenList中进行代价评估，这样减少了A*算法寻路过程中对大量中间节点的计算。

相比于传统A*算法每次只能拓展一格的策略，改进后的A*算法可以通过连接跳点，实现较长距离的“跳跃”。在寻路的过程中只需要花费很短的时间进行预处理，就可以减少大量不必要的节点，降低了计算过程中的内存开销，极大地提高了寻路的效率。

如图8 所示，先从起点S开始向水平和竖直方向递归拓展（从父节点到当前节点的方向是拓展的方向，若无父节点，则先向水平和竖直方向拓展），如果递归过程中遇到了障碍物，那么递归停止，且失败路径上的所有节点都被忽略，JPS 不会产生任何东西。否则，递归将继续并导向后继节点xn，xn是强制邻节点或目标点，此时搜索从当前节点x直接跳到xn，并不会将沿途的任何一个节点添加到OpenList。当水平和竖直方向都递归失败时，才会在对角线上进行递归，这样可以确保不会错过任何一个转折点。当沿着对角线递归到新的节点时，重新沿着水平和竖直方向递归拓展。

Fig.8 Path-finding process of improved A*algorithm图8 改进A*算法寻路过程

图8 中，从起始点S直接跳转到节点X，继续向水平方向递归到节点Y，由于是一个非失败的直线式跳跃，递归停止；由于节点Z的原因，若递归继续，则会失去一个转向的机会。当发现目标节点G时，停止递归，根据节点的父辈关系，反推出最终路径。

3 仿真及实验验证

3.1 仿真分析

为了验证改进算法的效果，本文对传统A*算法和JPS 策略下改进的A*算法进行仿真分析。选取了5个不同尺寸、障碍物密度相同的栅格地图，在CPU 为i7-9750，操作系统为Windows 10，主频2.6 GHz，运行内存为16 GB 的计算机上进行仿真。

如图9 所示，两种算法在40×40 栅格地图上进行仿真实验，左上角的蓝色格点为起始节点，右下角的紫色格点为目标节点，黑色为障碍物节点，绿色节点为最终生成的路径。在图9（a）中，黄色节点为A*算法添加到ClosedList 评估的节点，灰色节点为添加到OpenList 的节点；图9（b）中，黄色节点为JPS 算法筛选出的跳点，灰色节点为修剪掉的节点。可以看出，相同环境下两种算法生成的路径区别不大，且JPS 策略下改进的A*算法在寻路过程中参与评估的节点数量远少于A*算法。

Fig.9 Simulation results of A*algorithm and improved A*algorithm图9 A*算法和改进A*算法的仿真结果

表1 给出了两种算法在不同尺寸栅格地图中寻路的搜索时间和评估节点数量的对比。可以清楚地看出，改进后的A*算法生成的路径长度和A*算法生成的路径长度基本相等，且寻路过程中评估的节点数量大大减少，计算时间也明显降低，减少了寻路过程中对内存的耗费，随着地图尺寸的扩大，这种效果更加明显。

仿真结果表明，JPS 策略下改进的A*算法不仅寻路速度远超传统A*算法，对内存的消耗也远低于传统A*算法。

3.2 实验验证

为了验证改进后的A*算法在移动机器人实际运行中的有效性，将改进后的算法应用到本人研发的麦克纳姆移动机器人上，如图10（b）所示。移动机器人采用激光测距雷达RPLIDAR A1 获取二维点云数据，融合陀螺仪姿态和里程计数据后利用Gmaping 功能包完成定位和二维地图的构建，最后利用Move_base 功能包中的global_planner 完成机器人的全局路径规划。本文的实验场景为实验室内一段过道，如图10（b）所示。

如图11 所示，麦克纳姆机器人在实验室场地进行两次路径规划，规划结果利用可视化工具Rviz 显示出来，其中蓝色方块为小车模型，红色点为激光雷达数据，绿色线条为规划出的路径。

表2 给出了4 种算法在实验室环境下两次路径规划的寻路时间对比。可以清楚地看出，本文改进后的算法相较于传统A*算法，同样能规划出近似的路径，但是速度却远超传统A*算法；ACO 和GA 算法规划的路径虽然更优，但规划的时间太长，不能满足移动机器人的实时路径规划，因此本文算法更适合移动机器人路径规划。

实验结果表明，在JPS 策略下改进的A*算法能够有效提高移动机器人路径规划的速度，减少了内存的消耗，提高了寻路效率。

Table 1 Comparison between traditional A*algorithm and improved A*algorithm in this paper表1 传统A*算法和本文改进A*算法的对比

Fig.10 Mobile robots and experimental environments图10 移动机器人与实验环境

Fig.11 Path planning result of improved A*algorithm图11 改进A*算法的路径规划结果

Table 2 Comparison of pathfinding time of 4 algorithms under 2 paths表2 2 种路径下4 种算法寻路时间对比

4 结束语

本文针对传统A*算法在工厂场景较大的栅格地图路径规划时，遍历很多冗余的节点导致寻路算法内存消耗大、计算速度慢等问题，提出了一种融合JPS 跳点算法与A*算法的改进策略。通过对不同尺寸栅格地图的路径规划分析与对比，证明了改进算法的有效性。仿真结果表明，本文算法不仅能很大程度地加速A*算法，而且使系统运行时消耗的内存也明显地减少。

本文也尚存在不足之处，例如本文改进的A*算法虽然在路径规划的速度上有了巨大的提升，但最终生成的路径不够平滑。在实际机器人工作时，平滑的路径可以减少电机的加减速，进而提高机器人的能耗。针对这个问题将会引入不同的平滑函数对算法进行改进，进一步提高该算法的实际应用价值。