基于改进A*算法的AUV 路径规划研究

潘世瑛

（上海交通大学，上海 201100）

1 基本模型和算法

1.1 A*算法原理

A*算法因其寻优能力强、场景适应度高等特性，在路径规划领域有着广泛应用发展[1]。陈若男等[2]结合距离及角度信息改进传统A*算法代价函数，设计邻域选择策略，提升了算法效率和路径安全性。冯来春等[3]将A* 算法和快速搜索随机树（Rapid-Exploring Random Tree，RRT）算法结合，降低了RRT 算法的采样盲目性。孟珠李等[4]将A*算法与B 样条曲线结合，改善了A*算法规划路径的平滑性。余文凯等[5]采用K-均值（K-Means）聚类算法对地图进行预处理，量化不同区域地图复杂度，依据复杂度设置搜索权重，应用弗洛伊德（Floyd）算法对路径进行平滑，提高了路径规划效率和路径平滑性。

A*算法是一种快速简洁的启发式算法，其核心思想是通过代价函数的数值评估各路径点，从而对路径做出规划。

代价函数的表达式通常为

式中：f（n）为评估从n 点到目标点消耗的代价函数；g（n）为从起点到下一路径点的消耗；h（n）为从当前点到目标点的直观消耗。

A*算法利用Dijkstra 算法思想，每次运算都会将已处理的点加入关闭列表（Close），并从待选列表（Open）中选出代价函数最小的点作为下一路径点，重复该过程，直至抵达目标。改进算法将各栅格的活性值作为A*算法的代价函数运算，快速规划出当前场景下的全局路径，并用父节点优化产生下一路径点坐标。

1.2 传统A*算法实现过程

1.2.1 代码整体思路

（1）对环境进行设定：首先设定生成的环境的大小，生成的方格数为n*n，也就是对参数n 的设定。然后设定障碍物，也就是对参数wallpercent 进行设定。

（2）设定（1）的2个参数后，调用编写的initialize原Field 函数来随机生成包含障碍物，起始点，终点等信息的矩阵（图1）。

图1 随机生成的环境矩阵

（3）调用编写的createFigure 函数，利用initialize原Field 函数生成的数据进行创建环境，也就是绘制随机生成的要进行路径规划的场景，包括障碍物，起始点，终止点等。

（4）开始规划路径前，要对路径的一些矩阵进行初始化。

（5）开始路径规划，从起始点开始，利用循环进行迭代来寻找终止点，在进行点的拓展时调用编写的findFValue 函数来拓展寻找子节点以及子节点的代价。

（6）如果在上一步的路径规划中找到了从起始点到终止点的路，就调用编写的findWayBack 函数进行路径回溯，并绘制出从起始点到终止点的路径曲线。1.2.2 环境中点的设置

环境中的一个点他起初的身份是有障碍物点，没有障碍物点，以及特殊的点——起始点和终止点，然后在路径规划进行点的拓展时，首先是当被父节点拓展出来后放到矩阵posinds 中，如果符合要求，就升级为待选点，放到setOpen 矩阵中，每次循环都从se原tOpen 矩阵中找一个最优的点，升级为下一次进行拓展的父节点，放到矩阵setClosed 中。最后在找到终止点后，通过回溯出来的点放到矩阵p 中，利用这些点可以找到规划的路径，并将其绘制出来。

1.2.3 路径规划的实现过程

首先通过initializeField 函数生成矩阵costchart，在矩阵costchart 中起始点位置存放的内容为0，其他位置存放的内容为NaN，生成的field 矩阵起始点和终止点存放的内容为0，没有障碍物的点存放的内容为1 到11 的随机数，有障碍物的点存放的内容为最大值Inf，生成了一个元胞数组fieldpointers 里面在有障碍物的地方存放的内容为0，在起始点处存放的内容为S，终止点处存放G，其他位置为空，除此之外还得到了起始点的索引值startposind，终止点的索引值goalposind。

在进行路径规划时，需要初始化一些矩阵，se原tOpen 矩阵用来存放待选点的索引值，初始化为起始点的索引值，一开始的时候只能从起始点出发，因此刚开始的时候只有起始点一个待选点；setOpenCosts矩阵存放该待选点与起始点的代价，由于目前该待选点就是起始点，所以初始化为0，setOpenHeuristics 矩阵存放该待选点与终止点的距离，因为目前还不知道能不能找到到终止点的路，所以先初始化为最大值Inf，初始化的时候，这3个矩阵内都只存放一个待选点，但是，随着路径规划的进行会不断将符合要求的待选点串到（新增）到矩阵中，这三个矩阵是配套使用的，也就是说这三个矩阵的同一行中存放的是同一个待选点的信息，分别存放该待选点的索引值，到起始点的代价，到终止点的代价，因此要往setOpen 中增加或者删除待选点时，必须同时对另外两个矩阵进行增加或删除，来保证其对应关系。

两个矩阵setClosed = []和setClosedCosts = []分别用来存放下一次进行拓展的父节点的索引值，以及改点到起始点的代价，其实这两个矩阵在进行路径规划的时候并没有用到，也没有发挥任何作用，可以用来帮助进行分析。

初始化结束后，接下来进行拓展，从setOpen 矩阵中的待选点中选取一个最优点进行拓展，选取标准就是这个待选点距离起始点和终止点的代价的和最小，那么这个待选点就认为他是最优的待选点，那么就将这个点作为下一次拓展的父节点，每次拓展把拓展出来的符合要求的点放到矩阵setOpen 中作为待选点，这样不断的迭代循环，直到没有可拓展的点或者直到终止点，结束拓展，路径规划结束。

2 改进A*算法

2.1 启发函数改进

启发式函数可以控制A*的行为：

（1）一种极端情况，如果h（n）是0，则只有g（n）起作用。

（2）如果h（n）经常都比从n 移动到目标的实际代价小（或者相等），则A * 保证能找到一条最短路径。h（n）越小，A*扩展的结点越多，运行就得越慢。

（3）如果h（n）精确地等于从n 移动到目标的代价，则A * 将会仅仅寻找最佳路径而不扩展别的任何结点，这会运行得非常快。尽管这不可能在所有情况下发生，你仍可以在一些特殊情况下让它们精确地相等。只要提供完美的信息，A * 会运行得很完美。

（4）如果h（n）有时比从n 移动到目标的实际代价高，则A* 不能保证找到一条最短路径，但它运行得更快。

（5）另一种极端情况，如果h（n）比g（n）大很多，则只有h（n）起作用。

传统的A * 算法的总代价计算公式为f（n）= g（n）+ h（n），与传统的A 星不同的是，改进后变成了w（n）* h（n），其中w（n）>= 1 。w（n）可以影响评估值。在搜索过程中，可以通过改变w（n）来影响搜索过程如上面提到的h（n）对A 星算法的影响。

2.2 路径规划拐角优化

无论是传统的A 星算法还是动态衡量启发式的他找出来的路只是数学上的最优路径（或者说近似最优路径，随着搜索速度的提高，找到的路可能不是最短的），规划出来的路线有一些转弯是完全可以省去的（用一个转弯去取代多个转弯），因为最终的目标还是要控制实物去移动，所以要尽量减少转弯的次数，使其在保证不增加路程的基础上尽量减少转弯次数。

如图2 所示，第一种情况：如上图的标号于处所示，可以将其优化成白线所示的轨迹；第二种情况：如上图的标号淤处所示，这种转弯，则不能进行类似的操作，因为A 星算法在进行拓展的时候是对总代价最小的点来拓展，在淤号标注处向下拓展比向左拓展要花费的总代价小，所以类似这种远离终止点的拐角无法得到优化。

图2 拐角优化的不同情况

具体实现方法：从setOPen 矩阵中，找到最小总代价的点在setOPen 中的索引值ii 时，也就是得到将要拓展的点在fieldpointers 元胞数组中的索引值se原tOPen（ii），然后通过该点在元胞数组fieldpointers 中储存的方向信息，就可以知道这个点是由那个点拓展出来的，进而得到其父节点的索引值，也就可以知道其父节点在元胞数组fieldpointers 中储存的方向信息，近而知道了期望的下一步要走的点的位置。得到这个期望要走的点位置后，就查看这个点是否位于待选点矩阵setOPen 中，如果在，那么计算一下这个点要花费的总代价，如果跟原来要走的点代价相同，则用这个期望的点代替原来的点进行优化，否则不进行优化。

3 仿真过程

通过对比传统A*算法、改进启发函数后的A*算法以及进行拐角优化的A* 算法完成同等状态的路径规划情况，得到具体路径规划图如图3 和图4，具体计算时间见表1。第一种情况中右上角为起点，左上角为终点，第二种则反之。通过迭代产生的探索区域可知，改进后的A*算法明显减少了迭代次数，规划时间得到优化，拐角优化后的计算时间并未明显优化，但是路径的转弯次数有一定程度的减少，达到了预期结果。

图3 传统A*算法、改进启发式函数算法、拐角优化后的路径规划情况1

图4 传统A*算法、改进启发式函数算法、拐角优化后的路径规划情况2

表1 路径规划时间对比

4 结语

传统A*算法存在算法时间较长，搜索效率低，路径冗余拐点较多、平滑度差等问题，针对这些不足，考虑多种因素，对作做出改进。通过改进A*算法，将传统的A*算法改为动态衡量启发式A*算法，并对拐角次数进行了一定程度的优化，在不同复杂度场景下对比进行仿真验证，经过验证，改进的A* 算法能够更快更好地完成路径规划任务。