盐城工学院信息工程学院，江苏 盐城

收稿日期：2022年9月7日；录用日期：2022年10月8日；发布日期：2022年10月13日

摘要

针对路面移动机器人搜索最优路径慢、效率低等问题，分别选用最优性的A*算法和人工势场法作为移动机器人的全局规划和局部规划的基本算法。针对全局规划中传统的A*算法搜索节点相对较多、最优路径选择慢的问题，进行了加权启发函数的优化，达到减少节点的遍历数量、提高搜索最佳路径速度的目的。针对人工势场法会出现的局部最优解和目标不可达的问题，进行了斥力场增强因子的改动，优化了斥力场函数，引入了脱离函数，降低了出现局部最优解的可能性，避免出现目标不可达。结果表明：在相同的地图环境中对比测试，相较于传统的A*算法与利用固定系数加权启发函数的A*算法，优化了融合改进人工势场法的A*算法能够有效地减少遍历的节点数量，提高搜索的效率，缩短路径的距离，获得最优路径。

关键词

路径规划，A*算法，启发式函数，人工势场法

Optimization of A* Algorithm Integrating Improved Artificial Potential Field Method

Hengrui Zhang, Haitao Meng

College of Information Engineering, Yancheng Institute of Technology, Yancheng Jiangsu

Received: Sep. 7^th, 2022; accepted: Oct. 8^th, 2022; published: Oct. 13^th, 2022

ABSTRACT

In order to solve the problem of slow and low efficiency in searching for the optimal path for the road-mobile robot, the optimal A* algorithm and artificial potential field method were selected as the basic algorithms for global planning and local planning of mobile robots. In order to solve the problem that the traditional A* algorithm has more searching nodes and the optimal path selection is slow, the weighted heuristic function is optimized to reduce the number of nodes traversing and improve the speed of searching the optimal path. In order to solve the problem of local optimal solution and target unreachable in the artificial potential field method, the enhancement factor of the repulsive field is changed, the function of the repulsive field is optimized, and the detachment function is introduced to reduce the possibility of the local optimal solution and avoid the target unreachable. The results show that compared with the traditional A* algorithm and the A* algorithm using the fixed coefficient weighted heuristic function in the same map environment, the optimized A* algorithm combining the improved artificial potential field method can effectively reduce the number of traversed nodes, increase the efficiency of search, shorten the distance of the path, and obtain the optimal path.

Keywords:Path Planning, A* Algorithm, Heuristic Function, Artificial Potential Field Method

Copyright © 2022 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

近些年来，我国开始加快推进移动机器人的开发设计。室内小型移动机器人(如扫地机器人等)已经逐渐普及，然而在动态环境下，移动机器人的路径规划问题一直是其研究领域亟需解决的问题。路径规划是实现移动机器人自主导航功能的关键技术 [1]，该技术可以确保机器人能够在有障碍物的环境中规划出合理的路径，使机器人能够在安全的前提下，无碰撞地到达终点 [2] [3]。由地图信息的已知度，可以将移动机器人路径规划分成全局路径规划和局部路径规划两部分 [4]。全局路径规划是一种提前规划，规划路径的精确程度取决于已知地图信息的准确程度。扫描到的地图信息与实际的地图信息存在较大误差或存在噪声过大的问题时，其全局路径规划的鲁棒性会变差。局部路径规划 [5] 对硬件设备要求较高，需实时地采集周围信息用来动态地校正自身方位，使其具有较好的避障能力，对环境误差和噪声有较高的鲁棒性。局部路径规划由于缺少全局的地图信息，因此，无法保证寻路结果在全局最优，甚至出现寻路不完整和局部最优解的情况。

在全局路径规划算法之中，基于采样的快速随机扩展树法(Rapidly Random Tree, RRT) [6] 仅仅只存在概率完备性，且不能保证规划出的路径的最优性。A*算法 [7] 基于传统图搜索思维并引入了启发智能搜索，拥有出色的路径最优性、寻路完备性和搜索高效性，且计算量小、搜索速度快，故被广泛应用到静态地图下的路径规划中。但由于其启发式搜索较单一、搜索邻域受限等问题，使得在规划路径时会遍历大量不必要的搜索节点，这一直是学者们研究改进的重点和突破的难点。

目前，已经提出了很多改进A*算法：文献 [8] 为能找到最短路径，采用了新型的距离计算公式。文献 [9] 通过改变计算方式与函数权重，从而降低寻路时间和减少无效节点数量。文献 [10] 扩展20邻域并设计了安全距离，使规划出的路径更加平滑。文献 [11] 针对复杂地图，引入安全因子，使路径经过危险区域更少。文献 [12] [13] 在传统A*算法中，引入了动态窗口，使其拥有更好的避障性能。上述文献为改进A*算法的理论研究提供了思路和依据。

现有文献针对某些特定的指标对A*算法进行了改进，但大都以牺牲其它的性能指标为代价，使得其在某些特定场景下获得了期望的指标性能。为了能有效减少遍历节点的数量、降低规划的时间、缩短路径的距离、提高搜索的效率，本文提出了改进A*算法，通过对启发式函数和人工势场法进行设计，使改进算法在路径规划上有更强的启发性，且在遍历节点、路径平滑度以及避障效果上达到最优。

2. A*算法

A*算法是建立在Dijkstra算法和最佳优先搜索(BFS)算法的基础之上。它是一种启发式搜索算法。其中，Dijkstra算法会从起点遍历地图中所有可能移动到的点，最后规划出最快到达目标点的路径。不同的是，BFS算法是从起点开始逐次向外部移动，直至搜索到终点。A*算法就是Dijkstra算法与BFS算法的融合拓展，其在BFS算法的基础上提高了机器人移动到终点的引导性，这样既能提高其效率，同时又能确保图搜索概率的完备性。

A*算法最核心的是估价函数 $f\left(n\right)$，其函数表达式为：

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

式中， $f\left(n\right)$ 为起始节点到目标节点的估计代价值； $g\left(n\right)$ 为起始节点至机器人位置节点的实际代价值； $h\left(n\right)$ 为当前节点至目标点的估计代价值，也称启发式函数。

$S_n$ 表示父节点(n − 1)到当前节点n的单步实际移动距离， $S_{n+1}$ 表示当前节点n到下一节点(n + 1)的单步实际移动距离。 $g\left(n\right)$ 表示起始节点到(n − 1)节点的真实移动距离 $g_{n-1}$ 与从(n − 1)节点到当前节点n的单步实际移动距离 $S_n$ 之和，即为每一段已规划路径距离的总和。由此可得，真实移动距离函数 $g\left(n\right)$ 的表达式：

$g\left(n\right)={\displaystyle \underset{i=1}{\overset{n}{\sum }}{S}_i}$ (2)

若 $g\left(n\right)\gg h\left(n\right)$，则 $f\left(n\right)\approx g\left(n\right)$。此时，A*算法近似为Dijkstra算法，搜索的节点会增多，效率将会大大降低；若 $g\left(n\right)\ll h\left(n\right)$，此时，A*算法逐渐演变成BFS算法，路径规划的速度变快，但会出现局部最优解问题。因此，A*算法的性能好坏取决于启发式函数 $h\left(n\right)$ 的选取。常用的 $h\left(n\right)$ 预估方法有欧几里德距离(Euclidean Distance)和曼哈顿距离(Manhattan Distance)，如图1。

假设起点坐标为 $\left(S_1,S_2\right)$，终点坐标为 $\left(G_1,G_2\right)$，欧几里得距离的启发函数公式为：

$h=\sqrt{{\left(S_1-G_1\right)}^2-{\left(S_2-G_2\right)}^2}$ (3)

图1. 使用欧几里得距离的启发函数

曼哈顿距离的启发函数公式为：

$h=\left|S_1-G_1\right|+\left|S_2-G_2\right|$ (4)

考虑到欧几里德距离的计算精度高于曼哈顿距离，得出最优路径的可能性更大，故，这里选取欧几里德距离作为启发式函数 $h\left(n\right)$ 进行代价预估。

3. A*算法优化

A*算法因其易实现和方便可移植性，广泛应用于移动机器人，但将其直接应用于路面移动机器人的全局路径规划并不合适。其原因是，A*算法在遍历搜索时获得了最佳路径的较多无效点，并且该算法获得的运动路径折线多、转折数量多、弯折角度大、运动曲线不平滑等。本文综合文献 [8] - [13] 所改进思路，在设计思路时，需要避免出现因提高某一方面性能指标而导致其它性能降低的情况。通过设计修正斥力场函数增益因子、基于斥力场最大作用距离的自适应性估价函数、增设脱离函数和建立相对速度的人工势场策略以改进A*算法，确保最终规划出的路径相对最优，提升算法的鲁棒性和搜索效率，减少遍历的节点数和总转折角度。

3.1. 斥力函数增益因子修正策略

人工势场法是设计一种虚拟力去提前干预机器人的运动。可移动区域会对机器人产生一种吸引力，相反地，障碍区域会对机器人产生排斥力，最后，依靠的两种力合成，帮助机器人完成最优路径的搜索。

在传统的人工势场法中，会引入虚拟斥力场函数 $U_r$。而相比于传统的人工势场法中斥力场函数 $U_r$，改进后的虚拟斥力场函数 $U_{repm}$ 调整了斥力势场强度系数 ${\eta }_{repm}$，并根据其与障碍物的距离进行分段，这样就能保证了激光雷达扫描建图的实时性。

${\eta }_{repm}=\{\begin{array}{ll}\alpha {\text{e}}^{|x-P_0|}\hfill & \left(0<x\le \frac{P_0}{\alpha }\right)\hfill \\ \beta x\hfill & \left(\frac{P_0}{\alpha }<x\le P_0\right)\hfill \\ 0\hfill & \left(x>P_0\right)\hfill \end{array}$ (5)

式中， $\alpha$ 为紧急区域斥力强度系数； $\beta$ 为和缓区域斥力强度系数； $P_0$ 为斥力场最大作用距离。

调整了斥力场强度系数 ${\eta }_{repm}$ 用来修正斥力场函数，添加了修正后斥力场强度系数的斥力场函数 $U_{repm}$ 为：

$U_{repm}\left(X\right)=\{\begin{array}{ll}\frac{1}{2}{\eta }_{repm}{\left(\frac{1}{X-X_0}-\frac{1}{P_0}\right)}^2{\left(X-X_g\right)}^n\hfill & \left(0<X-X_0\le P_0\right)\hfill \\ 0\hfill & \left(X-X_0>P_0\right)\hfill \end{array}$ (6)

式中， ${\eta }_{repm}$ 为斥力场强度系数； $X_0$ 为障碍物所在的位置 $\left(x_0,y_0\right)$ ； $X_g$ 为目标点所在位置 $\left(x_g,y_g\right)$ ； $X-X_g$ 为机器人与目标位置的距离； $P_0$ 为斥力场最大作用距离，为人为预设常数； $X-X_0$ 为机器人与障碍物的距离；n为修正因子，n > 0。

因此，当机器人到达目标地点时， $X=X_g$，斥力场就不会再作用到机器人，故，引入修正的增益因子不会影响机器人的势能在到达目标点时等于零。

相较于传统人工势能法的斥力势场，引入了斥力场强度系数后的斥力场将会与机器人和目标点的一致，呈现衰减总体趋势，衰减的趋势会随着n的改变而改变，如图2。

故，可以得出，引入修正因子的斥力函数为斥力势场函数的负梯度方向，即：

$F_{repm}=-grad\left[U_{repm}\left(X\right)\right]$ (7)

$F_{repm}=\{\begin{array}{ll}{\eta }_{repm}\left(\frac{1}{X-X_0}-\frac{1}{P_0}\right)\cdot {\left(X-X_g\right)}^n\cdot \frac{\partial \left(X,X_0\right)}{\partial X}\hfill & 0<X-X_0\le P_0\hfill \\ 0\hfill & X-X_0>P_0\hfill \end{array}$ (8)

图2. 不同修正因子下的斥力势场衰减趋势

关于n的取值，鲍久圣等人以有研究 [14]，此处的n取1时，机器人的运动轨迹相对平滑，故，这里我们选择n = 1。

3.2. 基于斥力场最大作用距离的自适应估价函数

A*算法在进行路径搜索时，启发函数的复杂度决定了其搜索效率，由于从当前点到目标点的实际代价值恒大于从当前节点到目标点所需的代价值的 $h\left(n\right)$，因此，在搜索的过程中需要一定量的节点以确保估价函数 $f\left(n\right)$ 在可搜索范围内的估价最小。若此时判断出当前节点对于目标点的启发函数的代价值等于其实际的代价值，到达目标点的搜索次数便只要一次。因此，如果在复杂环境中，能够提高启发函数 $h\left(n\right)$ 在整个估价函数当中的权重，那么搜索范围就会被进一步约束，算法的搜索节点会因此减少，A*算法的效率也能随之提高。基于此方法，赵真明等 [15] 设计了一种加权启发函数：

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

式中， $\alpha$ 为权重。加权后的函数确实能够帮助提升算法效率。但在实操中，当当前节点逐渐靠近目标点时，启发函数的数值 $h\left(n\right)$ 会逐渐接近真实的代价值，因此，此时权重应该接近于1。显而易见的是，采用固定权重并不能使机器人在接近目标点时，使估计代价值接近真实的代价值。所以，本文改进了文献 [15] 所提供算法基础，将启发函数 $h\left(n\right)$ 的加权系数改为使用复合斥力场增益因子 ${\eta }_{repm}$ 的指数函数，优化后的估价函数 $f\left(n\right)$ 为：

$f\left(n\right)=g\left(n\right)+{\text{e}}^{{\eta }_{repm}}h(\; n\; )$

式中， ${\eta }_{repm}$ 为改进后斥力场增益因子。随着机器人接近目标点，预估代价函数 $h\left(n\right)$ 将会接近0，而权重会根据与障碍物的距离进行改变。如靠近目标点时，此时距离障碍物较远，不受斥力场影响，那么权重值为1；当其受到斥力势场影响时，根据人为预设的 $\alpha$ 、 $\beta$ 、 $P_0$ 的不同，可使 ${\text{e}}^{{\eta }_{repm}}$ 在靠近目标点时，趋向于1的同时，又能确保机器人可以进行快速的避障。

3.3. 脱离函数

笔者在对斥力场函数修正后，解决了人工势场法的目标不可达问题，但还需解决局部最小值的问题。这里引入一个脱离程序，用于判断在机器人是否陷入了目标不可达，并解决其在引力与斥力平衡时产生的问题。当机器人陷入了目标不可达的情况时，引入一个排斥力 $F_{esp}$，帮助机器人脱离力平衡的状态，并重新寻找最优路径，这里

$F_{esp}=F_{repm}=\{\begin{array}{ll}{\eta }_{repm}\left(\frac{1}{X-X_0}-\frac{1}{P_0}\right)\cdot {\left(X-X_g\right)}^n\cdot \frac{\partial \left(X,X_0\right)}{\partial X}\hfill & 0<X-X_0\le P_0\hfill \\ 0\hfill & X-X_0>P_0\hfill \end{array}$ (9)

流程图如图3所示：

图3. 脱离函数流程图

3.4. 建立相对速度的人工势场

本文所涉及的移动机器人不仅应用于静态环境中，还可以应用在有动态障碍物的环境中，在传统人工势场法中加入了修正的增益因子和脱离程序后，解决了传统人工势场法中可能出现的局部最优解和目标不可达。为了保证其算法在动态环境中的稳定性和可靠性，这里建立相对速度斥力场函数：

$U_{repv}\left(X\right)=\{\begin{array}{ll}{\eta }_{repv}\left|V-V_0\right|\left|\sin \theta \right|\hfill & 0<X-X_0\le P_0\hfill \\ 0\hfill & X-X_0>P_0\hfill \end{array}$ (10)

式中， $\left|V-V_0\right|$ 表示机器人在某一时刻相对动态障碍物的速度大小； $\theta$ 则表示机器人在某一时刻相对于动态障碍物的位置矢量和相对速度矢量的夹角，如图4所示。

图4. 机器人及障碍物运动状态

故，机器人与动态障碍物的相对速度斥力函数为：

$F_{repv}\left(X\right)=-grad\left[U_{repv}\left(X\right)\right]=\{\begin{array}{ll}{\eta }_{repv}\frac{\left|V-V_0\right|}{\sqrt{x_{r0}^2+y_{r0}^2}}\cos \theta \left(-\sin \theta ,\cos \theta \right)\hfill & 0\le \theta \le \frac{\pi }{2}且0<X-X_0\le P_0\hfill \\ -{\eta }_{repv}\frac{\left|V-V_0\right|}{\sqrt{x_{r0}^2+y_{r0}^2}}\cos \theta \left(-\sin \theta ,\cos \theta \right)\hfill & -\frac{\pi }{2}\le \theta <0且0<X-X_0\le P_0\hfill \\ 0\hfill & 其他\hfill \end{array}$ (11)

其中， ${\eta }_{repv}$ 为相对速度势场系数(增强因子)； $\sqrt{x_{r0}^2+y_{r0}^2}$ 为机器人相对于障碍物距离； $\left(-\sin \theta ,\cos \theta \right)$ 为机器人相对于障碍物三角代换后的位置坐标。

4. 仿真与分析可行性

4.1. 静态障碍物下可行性实验验证

本小节通过与传统A*、鲍久圣等人对启发函数加权算法 [14]、以及融合斥力势场因子的A*算法数据进行比较，验证了改进人工势场法的A*算法的高效性与可行性。为了减少代价函数在相邻单位的数值由于编译器的不同可能产生的差异，这里将人工引力场和人工斥力场对于机器人的代价值全部设置为正值。

这里为了表述方便，传统A*算法称为TRA_A*，启发函数加权算法称为H_W_A*，本文改进人工势场法的A*算法称为IM_APF_A*。如表1，图5。

表1. 几种方法在静态障碍物下的性能比较

图5. 不同算法在同一静态障碍物下的轨迹比较

通过多次进行仿真静态图中，传统A*算法可以通过大量的遍历节点获取最优最短的路径规划，但代价是资源浪费严重，所消耗的时间较长。相比TRA_A*，H_W_A*由于增加了启发函数在代价函数中的权重，因此，Path_Node可能存在绕行的情况，但考虑到机器人应用于实际场景中，对于静态障碍物的避障要求不高，因此，稍微的绕行并不影响机器人在全局路径规划的收益。最后，IM_APF_A*与H_W_A*相比，Path_Node一致，原因是其根本都在于对代价函数的加权，与此同时，Searched_Node也近似相等，而时间却有了提高，因此，IM_APF_A*在静态障碍物下的有效性和可行性是可以被验证的。

4.2. 动态障碍物下可行性实验验证

下面对几种算法在动态障碍物下的可行性进行验证。考虑到TRA_A*仅用于全局路径规划，针对于动态障碍物相当于图每时每刻的变化，TRA_A*将会消耗巨大资源，因此，这里不再考虑TRA_A*。仅针对于H_W_A*和IM_APF_A*进行研究。如表2，表3，图6，图7。

这里考虑在图(12, 7)处和图(5, 5)出现动态障碍物的情况，我们发现，其总体的全局路径规划是相同的，作为本次实验的最优路径。在动态障碍物的状况下，我们将以不出现机器人碰撞作为主要目的，其次考虑算法的时间可行性。这里我们发现，机器人在此图中出现动态障碍物后，并没有发生碰撞。相比于时间上，IM_APF_A*比H_W_A*的Consumed_Time减少了0.004 s和0.007 s，这表明，IM_APF_A*其在时间上是可行的。

表2. 几种方法在一种动态障碍物下的性能比较

图6. 不同算法在同一动态障碍物下的轨迹比较

表3. 几种方法在一种动态障碍物下的性能比较

图7. 不同算法在同一动态障碍物下的轨迹比较

5. 结论

针对传统A*算法和人工势场法在全局路径规划和局部路径规划中存在的不足，本文提出了具有修正斥力场函数增益因子、基于斥力场最大作用距离的自适应性估价函数、增设脱离函数和建立相对速度的人工势场策略的改进A*算法。该算法通过斥力场函数的增益因子，构造了自适应性的估价函数，以此来提高搜索速度、减少遍历节点、最大概率地避免局部最优解和目标不可达的情况出现。

通过仿真实验比较分析，本文提出的融合改进人工势场法的A*算法不论是在静态还是在动态环境下，其遍历节点数都更少、路径更平滑、搜索效率更高，证明了算法的优越性和可行性。今后将结合其他智能算法，研究极端状况下的动态避障算法研究，以及在动态环境下的提取方法对其算法的时间复杂度及空间复杂度的影响及优化策略。

文章引用

张恒瑞,孟海涛. 融合改进人工势场法的A*算法优化
Optimization of A* Algorithm Integrating Improved Artificial Potential Field Method[J]. 软件工程与应用, 2022, 11(05): 994-1004. https://doi.org/10.12677/SEA.2022.115102

参考文献