融合安全A＊算法和改进人工势场法的巡检机器人路径规划＊

张 强，鲁守银，张家瑞，姜 哲，于世伟，李志鹏

(1.山东建筑大学信息与电气工程学院，山东 济南 250000；2.山东建筑大学机器人技术与智能系统研究院)

0 引言

近些年来，机器人技术不断发展，具有自主导航能力的移动机器人也逐渐进入人们的视野。自主导航的首要条件是移动机器人能够规划出合理的路径，因此路径规划也越来越被人们所重视。移动机器人对路径的规划方法有很多，其中，通过搜索来找到路径的方法有JPS 算法[1]，A*算法等；也有使用数学概率来实现路径搜索的方法，例如RRT 算法[2]、RRT*算法等；此外，随着智能仿生学发展而出现的遗传算法和蚁群算法是其中的代表[3]。

总的来说，路径规划的算法一部分是针对完整的地图以及障碍物信息进行的全局规划[4]。通过规划可以找到最优的路线，其中代表的是A*算法和Dijkstra算法，但是对路径中突然出现的障碍物无法及时规避，具有一定的弊端。另一种则是针对附近的障碍物信息进行的规划，包含有动态窗口法和人工势场法等，局部路径规划的优点在于对路径上突然出现的障碍物也能有良好的避障效果，但是无法保证路径最优。

标准A*算法本质上是一种启发式的搜索算法，具有搜索时间短，可移植性和可重塑性强的特点[5]。但是A*算法规划出的路径拐点较多，难以实现机器人控制，且路径距离障碍物较近，使机器人运动存在危险。人工势场法在局部的路径寻找中使用，能够实时地避开障碍物，提高机器人的安全性，路径也易于机器人行走。然而，人工势场法存在局部极小值及目标不可达的问题，且规划出的路径往往距离较长。

针对目前方法存在的问题与不足之处，许多学者对此进行了研究改进，也取得了不错的成效。李晓露[6]等人对A*的搜索邻域进行改进，将父节点的搜索扩展到所用节点中，减少了路径的冗余度。陈晓宏[7]等人从底层表征方式出发改进A*算法，通过编码比较位的变化分段变步长寻找子节点，提高了路径的合理性。陈继清[8]等人提出了将人工势场法添加到A*搜索评价中，提高了A*搜索的避障能力。詹京吴[9]等人提出了一种融合安全A*和动态窗口法的算法，使规划的路径更符合机器人本身的运行。鲍久圣[10]等人提出了一种用于无轨胶轮车的A*和人工势场法融合算法，经实验后具有良好的效果。

本文将A*算法进行改进，在代价函数添加安全函数，规划出的路径具有更高的安全性，并选取路径的关键节点，添加到人工势场法中，作为引力点指引机器人前进。实现了实时避障和路径平滑，避免了人工势场法的部分问题，优化了路径距离。

1 安全A＊算法

1.1 A＊算法简介

A*算法是在Dijkstra 算法的基础上，与贪心算法融合而来，结合了广度优先和深度优先。在搜索时通过openlist 和closelist 两个状态表来对节点进行选择，基于栅格地图进行搜索，对每个节点进行考察，将将要考察的路径节点放到openlist表中，将已考察的节点放到closelist 表中。通过代价值评价函数对节点进行考察，找到最优的路径，路径评价函数为：

其中，f(n)为起点由待考察点到终点的代价值，g(n)为从起点到待考察点的实际代价值，h(n)为待考察点到终点的估计值。起点到终点为实际搜索的代价值，待考察点到终点的代价是由坐标估计而来的，距离计算方法有曼哈顿距离，切比雪夫距离和欧几里得距离，曼哈顿距离是横纵坐标差之和，切比雪夫距离是横纵坐标差的最大值，而欧式距离为横纵坐标差平方和的开根号，故本文采用更加精确的欧氏距离判断。

搜索过程中，由父节点向周围相邻节点扩展，传统的是向周围四节点扩展，规划出的路径只能横向或纵向移动，目前更多采用八节点扩展方法，可以向周围八个点移动。八节点扩展的方法相较于四节点的方法，规划出的路径更优，也更为平滑。

图1 为A*算法两种不同的扩展搜索方式。分别为四节点搜索和八节点搜索。

图1 四节点和八节点搜索方式

1.2 安全距离函数

当搜索到目标点时搜索结束，根据搜索的父节点得到搜索出的路径，A*算法相较于Dijkstra 算法，增加了函数h(n)，在保证搜索准确度的情况下，提高了搜索效率。但是搜索的路径因为一味力求最优，所以与障碍物距离过近，许多路径甚至是贴着障碍物。故本文提出在路径评价函数中添加安全距离函数k(n)，以提高机器人运行的安全性，路径代价函数为：

引入安全距离函数k(n)评价的是节点周围距离障碍物的距离l以及障碍物的个数s。在A*的搜索中，距离l 为节点与附近八个相邻栅格障碍物的最近距离，障碍物个数s 为当前节点相邻八个栅格中障碍物个数。得到安全距离函数k(n)的公式为：

其中，μ1和μ2为函数权重，l取值为0，1或者，s取值在0到8之间。

1.3 安全A＊仿真

在MATLAB中对本文的方法进行试验分析，使用尺寸为20*20 的栅格地图，设置起点坐标为(1,39)，用圆圈来表示，将(39,1)设置为终点，用三角形进行表示。在地图中设置部分障碍物，设置A*搜索函数。由起点开始，计算起点相邻八个栅格的代价值f(n)，找到代价值最小的，即为下一个父节点。然后重复计算八个相邻节点的代价值f(n)，如果某相邻节点已经计算过，除非新的代价值更低则进行替换，否则不予改变。当节点搜索到目标点时，A*寻路结束，此时的矩阵函数包含该节点之前搜索的所有父节点，所有节点构成从起始点到目标点的路径，根据节点位置可以得到规划路径。

由图2 可以看出，传统A*算法规划的路径在所用距离上已经达到最优，只有55.25，时间也较快，1.13秒。但是最优路径有部分是紧贴障碍物行进。因此本文提出引入安全距离函数k(n)的方法，对节点代价值f(n)进行改进，利用MATLAB进行仿真，结果如图3。

图2 传统A*路径规划

图3 安全A*路径规划

根据图3 仿真结果，距离为57.59，时间为2.43 秒。虽然距离和时间有所增加，但是如图2可以看出，在引入安全距离函数后可以明显看出，规划出的路径与障碍物保留了部分的安全距离，保障了巡检机器人的安全运行。A*算法的路径优劣对栅格大小的依赖比较大，过小的栅格对路径的距离会有缩短，但这会导致搜索时间大大增加。

2 人工势场法

2.1 人工势场法简介

如果出现不在规划地图中的障碍物，机器人在运行过程中无法闪避，使用人工势场法针对机器人周围检测到的障碍物进行实时路径规划，可以解决突发障碍物的问题。因其简单效率快等优势，成为机器人领域比较常用的一种局部路径规划方法。

人工势场法的原理是求解障碍物斥力和目标点引力的合力方向作为机器人运动的方向，直至到达目标点，实现实时避障和路径规划。

由图4 看出，通过计算障碍物斥力与目标点引力的合力，作为机器人运动的方向。

图4 人工势场法原理图解

引力场函数为：

其中，k1为权重系数，d(n,ng)表示当前节点n与目标节点ng的欧氏距离。对势场的函数求解，可以得到引力函数为：

斥力场函数为：

其中，k2为权重系数，dn0表示当前节点n到障碍物的欧氏距离，d0为设定的障碍物对机器人作用的最远距离。

因此，计算移动机器人当前位置所受到的合力，判断机器人下一步运动的方向，通过设置步长，得到机器人下一步运动的位置，从而规划出移动路径。

2.2 人工势场法改进

人工势场法的缺陷有很多，当得到的合力为零时，机器人的方向无法确定，无法前进或者在当前位置振荡。当引力与斥力在一条线上时，容易在障碍物前也无法确定方向，无法到达目的地。当到达目标点但斥力仍然存在时，会出现机器人无法停在终点的情况。障碍物的速度也应考虑，只计算与障碍物之间的距离，也会出现与障碍物碰撞的情况。

人工势场法的局部最优和目标不可达问题都是由于势场函数的缺陷所造成的，无法完全避免问题的出现，因此首先应对势场函数进行改进。局部最优和目标不达的情况原因主要是因为在目标点时合力不为零，而在未达到时可能会出现合力为零的情况。因此在斥力势场函数中添加距离函数，dng为当前点到目标点的距离，保证了机器人只有在到达目标点时引力和斥力同时为零。因此，改进后的斥力势场函数及斥力函数为：

2.3 改进人工势场法仿真

利用MATLAB对实验进行仿真，同样设置栅格地图，栅格大小设置为20*20，起始点坐标设置为(1,39)，用圆圈进行表示，目标点坐标设置为(39,1)，用三角形进行表示。在环境中添加与A*算法位置相同的障碍物，A*算法的障碍物为栅格，而人工势场法障碍物为以栅格为圆心直径为1 的圆。基于此环境下，根据力的公式进行设置，得到当前位置的合力，设置特定的步长，机器人根据方向和步长到达下一位置，在下一位置继续重复之前的计算，直至到达目标点并停下。

由图5 的MATLAB 运行结果可以得出，距离为57.59，时间为0.32秒。人工势场法因为省去了大量的搜索时间，因此在时间上有所提升。因为障碍物复杂度低的原因，路径也保持了较高的合理性。

图5 改进人工势场法路径规划

3 融合安全A＊的人工势场法

3.1 安全A＊关键节点

对于巡检机器人来说，所需巡检的位置基本是不变的，采用A*算法初步对路径进行规划。由于A*算法规划的结果只考虑当前存在的障碍物，且规划后无法改变，故在运行过程中采用人工势场法，以应对A*的不足。

为了提高机器人运行的安全性，本文采用设计的安全A*算法进行规划，得出在安全情况下的最优路径。对最优安全路径进行采样，选取其中部分节点，节点的选取主要为A*路径上的关键节点，同斜率的冗余节点不予采用。将提取出来的A*关键节点添加到人工势场法的地图中，在势场中添加A*关键节点的引力函数，以当前点指向A*关键节点作为引力添加到合力中，引导下一步的机器人运动。A*关键节点的添加，可以指引人工势场法向着最优路径前进，弥补了传统人工势场法规划的缺陷。

提取的关键节点数为a，在引力场中设置每个节点对机器人具有不同的吸引力，为了保证机器人向着目标点前进，因此应关键节点的引力是逐渐变大的。设当前的关键节点为第b个，则第b个节点对障碍物的引力系数为bk1/ak2，经过实验，设置为b3/a2，因此，A*关键节点对机器人的引力合力为：

3.2 融合算法仿真

在MATLAB中进行仿真，建立与之前同等障碍物的20*20 栅格地图，首先用安全A*算法进行全局仿真，提取安全A*路径关键节点，添加到引力场中，建立势场函数，将融合安全A*关键节点的人工势场法进行仿真，得到如图6所示的结果。

图6 融合算法路径规划

4 实验验证

利用纸箱模拟障碍物进行实验，采用rplidar A1M8 与下位机连接，上位机通过虚拟网络控制台观测运行状况，并获取地图。在Ubuntu18.04 系统上，采用hector slam算法[11]对现场进行建图（图7、图8）。

图7 模拟障碍物环境

图8 障碍物环境扫描建图

根据图7、图8，对雷达构建的地图使用本文融合算法进行实验，得出结果如图9所示。

图9 障碍物环境路径规划

5 结果分析

由图6 仿真结果得知，融合算法的路径长度为56.60，时间为0.31秒，在路径距离和时间进一步缩短。如表1所示。

表1 路径规划算法比较

由表1 对比得知，安全A*算法相较于传统A*算法，虽然牺牲了一部分路径距离和计算时间，但在机器人安全性上有了很大提高。融合安全A*和改进人工势场算法相则在力求路径更优的情况下做到了实时避障，在固定路径的巡检下，具有优异的性能。由图9可得，在纸箱模拟障碍物的实验中，本文所述融合算法也具有良好的避障性能。