基于改进A*算法的自主式机器人路径规划

Autonomous Mobile Robot Path Planning Based on Improved A* Algorithm

摘要: 针对传统的A*算法存在遍历子节点数量多、搜索自由度底、搜索方向夹角大所导致的路径搜索效率不高的问题进行了研究，提出一种改进的A*算法。首先增加节点数量，删除重复方向，建立16邻域搜索方式，扩大搜索角度。然后在新邻域搜索方式上，利用当前节点与目标点的方向信息局部调整搜索区间。改进启发函数和评价函数权重系数，在节点不陷入局部最优解的情况下，合理地缩小了搜索邻域。最后，在不同障碍物比例、不同栅格地图规模等16种模拟场景下进行实验。实验结果显示：改进A*算法相对于传统算法的搜索效率显著提高，极大地减少了搜索子节点的数量，提升了算法搜索的能力。

Abstract: Aiming at the traditional A* algorithm which has the problems of traversing a large number of child nodes, the bottom of the search degree of freedom, and the path search efficiency caused by the large angle of the search direction, the research is carried out, and an improved A* algorithm is proposed. Firstly, in increasing the number of nodes and deleting the repetitive directions, a 16-neighborhood search method is formed, which enlarges the search angle. Then, the search interval is locally adjusted by using the direction information between the current node and the target point on the new neighborhood search method. The weight coefficients of the heuristic function and the evaluation function are improved to reasonably narrow the search neighborhood without the node falling into the local optimal solution. Finally, experiments are conducted in 16 simulated scenarios with different obstacle scales and different raster map sizes. The experimental results show that the improved A* algorithm significantly improves the search efficiency compared with the traditional algorithm, greatly reduces the number of searching sub-nodes, and improves the searching ability of the algorithm.

1. 引言

近年来，国家高度重视和支持机器人创新技术领域。随着社会和自动化技术的快速进步，自主移动机器人成为了智能制造的重要工具，被广泛应用到医疗服务、物流运输、工业制造、农业生产等行业中，并逐步替代该行业的人工作业 [1] 。自主机器人的研发、制造、应用成为衡量一个国家科技创新和制造业水平的重要标志，也是国家重点发展的战略性高新技术产业 [2] 。路径规划是自主移动机器人导航的关键领域，主要目的是在障碍物的工作空间中规划出一条起始节点到目标节点的无碰撞路径，为机器人能够安全、无碰撞到达工作地点提供基础 [3] [4] 。根据对地图信息的获取方式，把路径规划分为全局路径规划和局部路径规划。全局路径规划是已知地图信息，根据自主移动机器人所在环境障碍物分布进行路径规划。局部路径规划是未知地图信息，自主机器人需要实时采集周围障碍物相对自己的分布状态，然后根据构造的局部地图规划出当前节点到目标子节点的最优路径 [5] 。局部规划没有全局的障碍物分布信息，这样得出来的路径是可行解不一定是最优解，甚至在复杂的环境中出现死锁现象无法找到最优解。

在全局路径规划算法中，基于采样的路径规划算法有快速随机扩展树法(Rapidly-Exploring Random Tree, RRT)，该算法只在概率上完备，规划出来的路径鲁棒性不强 [6] 。基于搜索的路径规划算法有Dijkstra算法 [7] ，在Dijkstra算法的评价函数中引入启发项得到A*算法 [8] ，A*算法具完备性和搜索速度快被广泛应用到静态障碍物的路径寻优中。

目前已经提出了许多改进的A*算法。文献 [9] 在评价函数中加入双重权重改善算法的寻优能力。文献 [10] 提出了一种新的邻域扩展方式。在文献 [11] 把障碍物在环境地图中的比例系数引入到启发函数中，使得启发函数因环境而改变。文献 [12] 利用目标节点和当前节点的方向引导，删除部分无效节点，提升搜索效率。文献 [13] 提出了用前一个节点和当前节点来建立评估函数，将偏离最优路径的节点不在添加到列表中，避免了盲目的扩展。文献 [14] 则混合了两种搜索方式，改变了单一的搜索方式，融合了两种搜索邻域的优点。文献 [15] 融合了欧氏距离和曼哈顿距离，提出新的估计距离函数。文献 [16] 将当前节点的父节点信息引入估计距离函数中，通过模拟退火算法来选择子节点。文献 [17] 考虑到当前节点的位置变化会使得到目标节点的估计代价与到开始节点的实际代价比例不同，通过指数衰减的方式加权。文献 [18] 简化路径的存储，只保留路径当中起点、拐点、终点，存储点直线连接就是规划的路径；最后计算每个拐点的旋转方向和最小旋转角度，利于在拐点处调整自身姿态。

基于上述文献为改进A*算法理论研究提供的思路和依据。本文提出了一种改进的A*算法，对扩展节点的方式、搜索邻域的优化、评价函数的权重调节和启发函数的设计做出改变，以面对复杂环境需求。规划路径具有搜索效率高、遍历的子节点数量少等优点。

2. 环境建模

地图是对环境空间的表达，是路径规划的前提，是定位与导航规划的基础。路径规划之前要对环境地图进行合理的建模。常见的路径规划环境建模包括点云地图、栅格地图、特征地图等。由于栅格地图具有简单、高效的特点，则采用栅格法对环境信息建模。黑色栅格为障碍物栅格，表示障碍状态；白色栅格为可通行栅格，表示自由状态，黄色栅格为机器人起始位置，红色栅格为机器人目标位置，结果如图1所示。

Figure 1. Grid map

图1. 栅格地图

为了记录所走路径位点，对其做出以下两种坐标的定义：线性索引(linear indices)坐标，规定线性索引序号从上到下、从左至右、依次递增对地图按序编号。行列索引(subscripts)坐标，规定行列索引以地图的左上角为原点，从上到下为行号、从左至右为列号、依次递增对地图按序编号。线性索引坐标便于储存数据，行列索引坐标便于考虑节点的搜索情况。

3. A*算法

3.1. 算法描述

1964年在自主机器人SHAKEY的研发工作中斯坦福大学计算机科学系教授Nilsson提出了A1算法。该算法是Dijkstra算法和BFS算法的组合算法，既保留搜索最短路径的能力，同时又具有向外扩展的贪婪策略。1966年，Nilsson和Raphael等人继续对A1算法的研究，把改进后的算法称为A2算法。1968年Hart等人证明了A2算法的完备性和存在最优性，于是算法被称为A*算法。其中，A*算法的评价函数为：

$f\left(n\right)=g\left(n\right)+h\left(n\right)$ (1)

$g\left(n\right)=\sqrt{{\left(x_s-x_n\right)}^2+{\left(y_s-y_n\right)}^2}$ (2)

$h\left(n\right)=\sqrt{{\left(x_n-x_g\right)}^2+{\left(y_n-y_g\right)}^2}$ (3)

代表当前节点；起始节点坐标是 $(x_s,y_s)$ ，当前节点坐标为 $(x_n,y_n)$ ，目标节点坐标是 $(x_g,y_g)$ 。 $f(n)$ 是评价函数，表示在静态环境地图中当前节点到起始节点和目标节点最小的成本估计， $g(n)$ 是实际距离函数，表示起始节点到达当前节点的实际代价值； $h(n)$ 是估计距离函数，表示当前节点到达目标节点的估计代价值，也被称为启发函数，是评价函数中最重要的组成部分。启发函数在A*算法中起着重要的引导作用，正确地设计函数能够A*算法找到最优解的速度最快。启发式函数与实际距离关系如下表1：

Table 1. Comparison between heuristic function and actual distance

表1. 启发函数与实际距离的比较

3.2. 算法步骤

Figure 2. A* algorithm flowchart

图2. A*算法流程图

A*算法的流程如图2所示，A*算法过程如下：

1. 定义地图中需要寻找路径的起点和终点，将定义的起点和终点加入 $open$ 开集中。

2. 判断 $open$ 开集中节点的数量，如果 $open$ 开集中节点的数量为零，则算法搜索结束表示当前地图所定义的起点和终点没有路径可达。

3. 如果 $open$ 开集中节点的数量不为零，从 $open$ 开集中取出 $f$ 值最小的节点作为当前节点，并将其加入到 $close$ 闭集中。从 $open$ 开集中移除当前节点 $n$ 。

4. 如果当前节点 $n$ 是目标节点，则搜索成功。查找当前节点的父节点，依次向上查找父节点，直到回到起点，算法搜索结束表示当前地图所定义的起点和终点有路径可达，并给出搜索路径。

5. 如果当前节点 $n$ 不是目标节点，则计算当前节点 $n$ 相邻的所有可到达节点，生成一组子节点，其中需要忽略 $close$ 闭集中的节点和障碍物节点。

5.1. 对于新的子节点，如果该节点不在 $open$ 开集中，则将其加入到 $open$ 开集，并计算其 $g$ 值、 $h$ 值和 $f$ 值，设置其父节点为当前节点 $n$ 。

5.2. 如果该节点在 $open$ 开集中，则检查其通过当前节点计算得到的 $f$ 值是否更小，如果更小则更新其 $f$ 值，并将其父节点设置为当前节点 $n$ 。如果大于则不需要更改 $f$ 值，说明此时的路径比前一节点长。

6. 转到2步骤。

4. 改进A*算法

4.1. 扩展邻域的方式和搜索策略优化

传统A*算法搜索子节点一般采用4邻域或8邻域的扩展方式。如图3所示，其中图3(a)为4邻域扩展方式，该方式会产生90˚夹角。图3(b)为8邻域扩展方式，增加了斜向运动方向，该方式会产生45˚夹角。8邻域扩展方式的优点是在搜索过程中更具有灵活性，可以考虑更多的路径选择。

Figure 3. Traditional A* algorithm expanding neighborhood method

图3. 传统A*算法扩展邻域方式

如果继续扩大搜索邻域，那么搜索方向的夹角会进一步减少。例如扩大到24或48邻域，这样规划出来的路径更为平滑。但是在复杂度高的地图环境中，一方面会使得搜索时间极具增加，另一方面会占用大量的内存空间。我们既要增加搜索方向以此来使得路径变得平滑，又要使得子节点的质量较优来减少搜索时间和内存。因此考虑到扩展邻域带来的一些优缺点，先将A*算法的8邻域扩至24邻域，针对扩展邻域数量增多导致搜索速度降低的问题，将24邻域与8邻域重合的方向删除，形成图4所示的16邻域扩展方向。图中9~16为在传统8邻域新增的搜索方向，有效的降低了搜索的产生的夹角。

Figure 4. Extension neighborhood method for improved A* algorithm

图4. 改进A*算法的扩展邻域方式

进一步为了不让子节点盲目扩展，需要对邻域的扩展方向进行引导，在引导方向上扩展必要的子节点。如图5，搜索算法不需要计算16个子节点，也能保证搜索时夹角的减少。假设目标节点坐标 $\left(x_g,y_g\right)$ ，当前节点坐标 $\left(x_n,y_n\right)$ ，通过目标节点与当前节点坐作差所得值与零进行比较来确定算法的扩展方向。当该方向的障碍物密度小时，向该方向扩展；当该方向密度比较大时，绕过该方向扩展；当该方向密度无法通行时，朝相反方向退回。

$x_g-x_n>0$ 且 $y_g-y_n>0$ ，计算当前方向障碍物的密度，当密度小于时，则可选节点为(2, 3, 6, 11, 12)；当密度在和之间时，则可选节点为(1, 2, 5, 9, 10)和(3, 4, 7, 13, 14)；当密度大于时，则可选节点为(1, 4, 8, 15, 16)。

$x_g-x_n>0$ 且 $y_g-y_n<0$ ，计算当前方向障碍物的密度，当密度小于时，则可选节点为(1, 2, 5, 9, 10)；当密度在和之间时，则可选节点为(1, 4, 8, 15, 16)和(2, 3, 6, 11, 12)；当密度大于时，则可选节点为(3, 4, 7, 13, 14)。

$x_g-x_n<0$ 且 $y_g-y_n>0$ ，计算当前方向障碍物的密度，当密度小于时，则可选节点为(3, 4, 7, 13, 14)；当密度在和之间时，则可选节点为(1, 4, 8, 15, 16)和(2, 3, 6, 11, 12)；当密度大于时，则可选节点为(1, 2, 5, 9, 10)。

$x_g-x_n<0$ 且 $y_g-y_n<0$ ，计算当前方向障碍物的密度，当密度小于时，则可选节点为(1, 4, 8, 15, 16)；当密度在和之间时，则可选节点为(1, 2, 5, 9, 10)和(3, 4, 7, 13, 14)；当当密度大于时，则可选节点为(2, 3, 6, 11, 12)。

通过确定子节点扩展方向，然后从剩余的子节点选取下一个所要扩展的节点，每次选择评价函数值最小的子节点作为下一个规划的子节点。

Figure 5. Expansion of the improved A* algorithm in the guidance direction

图5. 改进A*算法在引导方向的扩展邻域

4.2. 改进启发函数

在A*算法中，传统的启发式函数采用曼哈顿距离或欧式距离。若自主式机器人可以进行任意方向的移动，我们就可以用欧式距离来表示对应的启发函数。但是A*算法基于栅格地图，自主式机器人不能全向移动。传统算法的 $h\left(n\right)$ 函数并不能够满足实际的需要，结合欧氏距离和曼哈顿距离的优点，提出一种改进的启发函数，更真实的描述节点扩展之后的代价，假设单位节点耗散函数是 $d\left(n\right)$ ，改进后的启发式函数如下。

若 $\left|x_n-x_g\right|\ge \left|y_n-y_g\right|$ ，则

$h\left(n\right)=d\left(n\right)\left(\left|x_n-x_g\right|+\left(\sqrt{5}-1\right)\left|y_n-y_g\right|\right)$ (4)

若 $\left|x_n-x_g\right|<\left|y_n-y_g\right|$ ，则

$h\left(n\right)=d\left(n\right)\left(\left(\sqrt{5}-1\right)\left|x_n-x_g\right|+\left|y_n-y_g\right|\right)$ (5)

4.3. 优化评价函数权重系数

传统A*算法评价函数就是把实际距离函数和估计距离函数按相同的比例相加。实际上，最佳成本的评价函数中启发式成本和实际成本的比例一般是不对等。所以，在不同的复杂环境下启发函数应该有不同权重设置，合理的设置启发函数的权重，有利于A*算法的搜索效率。为减少算法扩展节点，搜索时更具有方向性，设计评价函数的权重系数如下：

$f\left(n\right)=\frac{1}{1+u}g\left(n\right)+\frac{u}{1+u}\left(1+\frac{a}{A}+\frac{H\left(n\right)}{H\left(s\right)}\right)h\left(n\right)$ (6)

其中 $\frac{1}{1+u}$ ， $\frac{u}{1+u}$ 为实际距离函数和估计距离函数的系数用于调节两个函数在评价函数中的比例。 $a$ 是障

碍物栅格在地图中的数量， $A$ 是地图中栅格总数， $H\left(n\right)$ 表示当前节点 $n$ 与目标节点之间的距离， $H\left(s\right)$ 表示指起始节点与目标节点之间的距离。在复杂的栅格地图环境中，启发函数前的比例权重应该动态调整，在开始规划的阶段由于离目标节点比较远，需要快速的向目标节点靠近，所以需要增加搜索的节点，

迅速到达目标节点的附近，此时 $\frac{H\left(n\right)}{H\left(s\right)}$ 较大。在末尾规划阶段由于目标节点就在附近，所以倾向于搜索最优路径，此时 $\frac{H\left(n\right)}{H\left(s\right)}$ 较小。

5. 仿真实验及结果分析

为了检验改进后算法的有效性和可行性，在MATLAB2022b上测试仿真。首先，通过MATLAB2022b建立不同规格和不同复杂程度(障碍物数量比重)的栅格地图。然后，在同一张栅格地图上分别运行传统A*算法和改进的A*算法。最后，从算法的路径长度和搜索路径所需要扩展子节点数量作对比。

5.1. 随机栅格地图仿真

想要验证算法的有效性和可行性，把改进后的A*算法和传统的A*算法在不同大小地图规格、不同障碍物比例的情况下进行测试和仿真实验。设置地图规格分别为30 × 30、50 × 50、100 × 100、200 × 200的栅格地图来表示小型地图、中型地图、大型地图、超大型地图，障碍物占用率为随机生成比重为30%、40%、50%、60%的栅格地图，以此生成4类16种环境地图，结果如图6~9。在所有的栅格题图环境中，我们把线性索引坐标下的第二个坐标为开始节点，倒数第三个坐标为目标节点。

Figure 6. 30% obstacle ratio map

图6. 障碍物比例30%地图

Figure 7. 40% obstacle ratio map

图7. 障碍物比例40%地图

Figure 8. 50% obstacle ratio map

图8. 障碍物比例50%地图

Figure 9. 60% obstacle ratio map

图9. 障碍物比例60%地图

5.2. 结果分析

如图10所示，比较A*算法、改进A*算法仿真实验的结果分析。可以看出：

(1) 改进A*算法对A*算法获得的路径更优，不管是在地图规模扩大的情况下还是在障碍物占用率增加的情况下，在16组对比实验中改进的算法对比传统的算法均可以获得较短长度的路径。

(2) 改进A*算法与传统A*算法相比，在扩展子节点方面有极大的减低，不管是在地图规模扩大的情况下还是在障碍物占用率增加的情况下，在随机的16组对比实验中每次搜索扩展的子节点数量都比传统的A*算法更加少。

(3) 随着地图规模的增加，改进A*算法对比传统A*算法在扩展子节点数量方面差异增大，即随着地图规模的增加改进A*算法更能适应复杂的环境地图。

Figure 10. Comparison of path length and number of extended sub nodes between A* algorithm and improved A* algorithm

图10. A*算法与改进A*算法路径长度和扩展子节点数量的对比

如图11~14所示，在不同规格的地图条件下从左至右障碍物比例为30%、40%、50%、60%。第一行为A*算法结果，第二行为改进A*算法的结果。

Figure 11. 30 × 30 mini map search results

图11. 30 × 30小型地图搜索结果

Figure 12. 50 × 50 medium map search results

图12. 50 × 50中型地图搜索结果

Figure 13. 100 × 100 large map search results

图13. 100 × 100大型地图搜索结果

Figure 14. 200 × 200 ultra large map search results

图14. 200 × 200超大型地图搜索结果

5.3. 公共地图数据集测试

基于栅格地图的路径规划一直是机器人和虚拟游戏等许多应用领域的核心挑战，但是这些算法没有在公共地图数据集上比较过，因此很难通过对比分析出不同算法客观的评价。MOVINGAI实验室Nathan R. Sturtevant教授创办了基于网格的路径规划比赛：The Grid-Based Path Planning Competition，简称GPPC。GPPC竞赛以MOVINGAI实验室的地图数据集为标准，该地图数据主要分为商业游戏地图、真实世界地图和人造地图 [19] 。MOVINGAI实验室的地图数据集的构成来源和数量如表2所示。图15~16是MOVINGAI实验室的地图数据集中部分示例。

Table 2. 2D Pathfinding Benchmarks set

表2. 二维寻路标准集

Figure 15. 2D Pathfinding Benchmarks dataset game scene map example

图15. 2D Pathfinding Benchmarks数据集游戏场景地图示例

Figure 16. 2D Pathfinding Benchmarks dataset artificial scene map example

图16. 2D Pathfinding Benchmarks数据集人造场景地图示例

我们在Random maps数据集中选取28张地图作为测试基准，在这28张地图中随机选择300 * 300的局部地图作为开始节点到目标节点的环境，在选择的地图中寻找开始节点到目标节点无障碍物路径。结果如图17。

(1) 改进A*算法相对传统A*算法获得的路径更优，在测试的数据集中改进A*算法比传统A*算法具有更好的寻优能力，每次的寻路中改进算法都可以稳定的找到开始节点到目标节点的规划路径，并且结果优于传统算法。

(2) 改进A*算法与传统A*算法相比，在扩展子节点数量方面有极大的改进，把偏离方向扩展的子节点删除，减少了扩展子节点的数量，提高了扩展子节点的质量。

(3) 在测试的数据集中，改进A*算法对比传统A*算法在扩展子节点数量方面更加稳定，即对于同一复杂程度的地图扩展子节点数量的波动性不会很大。

Figure 17. Improved A* algorithm vs. A* algorithm on 2D Pathfinding Benchmarks dataset

图17. 改进A*算法与A*算法在2D Pathfinding Benchmarks数据集的对比

6. 结束语

针对传统A*算法在规划导航中存在的一些问题，提出了一种改进的A*算法。主要有建立了新的节点扩展方式；同时，对启发函数进行设计以及优化评价函数的权重系数。最后通过对照实验来说明改进后的算法相对与传统的基本算法更加突出，取得了比较明显的效果。改进后的算法大幅度减少了遍历子节点的数量，提高计算效率，使得对路径规划的方向性更强，可以对大规模复杂的栅格地图做出反应。而且起始节点与目标节点距离越远，改进A*算法的优势相对于传统A*算法更加突出。

参考文献