0 引　言

水下机器人（AUV）是一个包含运动控制、目标探测、图像识别、水下通信、图像传输、定位导航与避障、路径规划与跟踪等多个子系统的水下无人平台。AUV作为重要的海洋作业机器，广泛应用于资源勘探开发、水下维修等工程中。路径规划作为AUV的关键技术之一，近年来逐渐成为了各国学者的研究热点^[1]。程志等^[2]引入机器人移动方向向量，对人工势场法进行改进，避免陷入局部最小点。刘贵杰等^[3]考虑AUV能耗会受到海流等因素的影响，对蚁群算法作出优化，引入路径能耗来引导蚁群进化，提高了AUV续航能力。Hui等^{[4]中AUV规划路径时考虑软、硬约束，使用了2种改进的粒子群算法，对比后发现选择性差分进化混合量子粒子群算法计算效率更高，生成的路径更为光滑。陈实等[5]通过随机函数布置搜索节点，形成搜索空间，显著提高了计算效率。吴鹏等[6]考虑双向搜索思想，对A*算法同时进行正向和反向的路径搜索，缩短了计算时间，大大提升了算法效率。}

本文出于多种因素的考虑，对A*算法进行了一定的改进。引入障碍物系数，同时考虑搜索节点距目标点的距离，对评价函数进行改进；考虑AUV自身体积，改进搜索节点选取规则，防止发生碰撞；划分象限，根据目标点所处位置减少不必要的搜索节点；去除路径冗余拐点，改善路径平滑度，使其更适合AUV实际航行。

1 环境建模与传统A*算法 1.1 环境建模

AUV在规划路径前，需要获取水下静态环境的信息构建地图，包括障碍物的大小和所处位置等，环境建模就是用抽象化的方式将水下实际的环境信息表达出来^[7]。

水下环境是一个复杂的三维环境，但三维环境建模方法难度较大且在其中的路径规划算法复杂，计算量大，计算速度慢，并且考虑到AUV实际工作时，航行的纵深变化范围一般较小，往往采取定深度航行。因此本文采取分层的思想，将水下三维环境按一定的水深分成多层环境，将每一层的环境投影到二维平面中，再用栅格法建立环境地图模型。

栅格法的原理是将二维环境划分为栅格网络，然后根据环境中有无障碍物对单元栅格进行标记。由于环境中的障碍物形状不规则，不能充满整个单元栅格，因此在进行栅格化后需要对障碍物进行膨胀处理，如图1所示。最终得到的地图如图2所示，其中黑色区域表示存在障碍物的区域，不可通过，白色区域表示AUV可以通行的区域。

1.2 传统A*算法

A*算法广泛应用于已知全局环境地图信息的二维路径规划^[8]。算法原理是从起始节点开始，通过启发函数搜索并选择周围最优节点作为下一个扩展点，不断重复这个操作，直到到达目标点，最终从目标点原路返回到起始点，生成最终路径。由于在整个过程中，每个节点代价都最小，因此得到的路径也是代价最小的。传统A*算法的评价函数为：

$ F\left( n \right) = G\left( n \right) + H\left( n \right)。$

(1)

式中：F(n)表示经过当前节点n从起点到目标点预估的代价总和；G(n)表示起点到节点n的实际代价；H(n)表示节点n到目标点的估计代价。A*算法路径规划过程中，每次都需要通过启发函数计算确定最优扩展节点，该过程会生成大量与最终路径无关的节点，或导致能够通过但花费代价较高的情况，从而使搜索时间变长，所以提高A*算法路径规划效率十分重要。

2 改进A*算法 2.1 评价函数改进

传统 A*算法的评价函数中，启发函数H(n)的选取决定了搜索性能，当H(n)=0时，A*算法退化为Dijkstra算法。选取欧氏距离作为启发函数，引入障碍物系数K，同时考虑搜索节点距目标点的距离，改变H(n)的权重，从而对评价函数作出改进：

$ K = \frac{N}{M}，$

(2)

$ G\left( n \right) = \sum\limits_{m = 1}^{m = n} {\sqrt {{{\left( {{x_m} - {x_{m - 1}}} \right)}^2} + {{\left( {{y_m} - {y_{m - 1}}} \right)}^2}} }，$

(3)

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

(4)

$ F\left( n \right) = G\left( n \right) + \left( {1 + {e^{ - K}}} \right)\left( {1 + \frac{r}{R}} \right)H\left( n \right)。$

(5)

式中：N表示有障碍物，不可通行的栅格数，M表示地图栅格总数；x₀，y₀，x_n，y_n，x_g，y_g分别表示起点、当前节点、目标点的横、纵坐标；r表示当前节点距目标点的距离；R表示起点距目标点的距离。

根据式（5）可以看出，当障碍物系数较小，当前节点距目标点距离较长时，启发函数的权重增大，从而减小搜索空间，提高路径规划效率；当障碍物权重系数较大，当前节点距目标点距离较短时，启发函数的权重减小，从而增大搜索空间，避免算法陷入局部最优。通过改进A*算法的评价函数，能够根据环境中障碍物数量与当前节点距目标点的距离，自适应地改变启发函数的权重，对规划最优路径有很大的帮助。

2.2 考虑AUV自身体积

在使用传统A*算法进行路径规划时，为了算法更为简便，往往会增加一条将AUV视为一个理想化质点的规则，从而忽略AUV本身的体积问题，但在这种规则下规划出来的路径存在一定的安全风险。传统A*算法规划得到的路径通过了障碍物栅格顶点，甚至从2个斜接的障碍物之间通过，但是由于AUV有自身的体积，它在航行到这些位置时有极大的可能与障碍物发生碰撞，无法继续前进甚至会损坏到AUV本体。

另一种方法是对障碍物采用膨化处理时，以AUV的半径大小进行膨化，然后将AUV视作一个质点进行规划。这种方法虽然能有效地解决AUV与障碍物发生碰撞的问题，但是划分的单元栅格变大了很多，整个栅格网络栅格数量也变少，这样建模生成的环境精度较低，同时栅格地图中可供AUV航行的区域也变小，因而规划生成的路径精度也相应降低。

本文考虑AUV自身体积，增加一条节点选取规则，当障碍物位于当前节点上下左右4个方向之一时（见图3），当前节点为O，栅格B为障碍物节点，此时A和C节点不考虑作为下一扩展点，因此不会生成OA和OC路径，避免与障碍物产生碰撞，其他几个方向同理。引入节点选取规则前后路径变化如图4所示。

2.3 减少搜索节点

用A*算法规划路径的过程中，需要对周围8个节点进行搜索，通过计算比较各节点代价，选择最小的作为下一扩展节点，在搜索过程中不可避免地会搜索大量与最终路径无关甚至与目标点方向相反的节点，因此通过减少搜索节点的方式，可以直观地降低搜索时间，进而提高规划效率。

基于减少A*算法搜索节点进而缩短算法时间的思想，以当前节点作为坐标原点，建立坐标系，根据目标点所处象限，去掉3个相反方向的节点，保留5个搜索节点，如图5所示。具体规则为：当目标点位于第一象限时，待搜索节点为1～5，去掉6～8三个节点；当目标点位于第二象限时，待搜索节点为7，8，1，2，3，去掉4，5，6三个节点；当目标点位于第三象限时，待搜索节点为5，6，7，8，1，去掉2，3，4三个节点；当目标点位于第四象限时，待搜索节点为3，4，5，6，7，去掉8，1，2三个节点。

2.4 路径平滑处理

A*算法由于其扩展方向只有周围8个节点方向，因此规划出来的路径不平滑、存在很多冗余节点且很多拐点处转向困难，导致AUV跟踪路径时实际航行效率变低。对路径进行平滑处理，简化路径，使其适合AUV航行非常有必要。

Floyd算法原理如图6所示，常用于得出两点间最短的路径，L(S, G)表示当前点S距目标点G的最短距离，当节点S和G之间有障碍时，L(S, G)=∞，表示节点S和G之间不可直达。因为L(S, A)+L(A, G)<L(S,G)，此时L(S, G)=L(S, A)+L(A, G)，节点S到目标点G的路径为S→A→G；进一步有L(S, B)+L(B, G)<L(S, A)+L(A, G)，那么此时L(S, G)=L(S, B) +L(B, G)，路径为S→B→G。节点S到目标点G间的最优路径为S→B→G。

通过Floyd算法思想对传统A*算法规划出来的路径进行平滑优化处理，可以有效减少路径中冗余节点，提高AUV航行效率，有利于AUV的实际航行需求。通过Floyd算法对路径平滑优化前后对比如图7所示。采用传统A*算法规划的路径为S→1→2→3→4→5→G，图中路径拐点较多，并且存在冗余节点，整体路径平滑度较差。Floyd算法通过判断2个节点之间是否有障碍物，剔除路径中冗余节点，优化为S→3→4→G，通过对比优化前后路径可以看出，Floyd算法能够有效减少路径的转折次数、缩短路径长度，改善路径的平滑度，使其适合AUV实际跟踪航行。同样的，优化后的路径也需要考虑AUV自身体积，因而设置安全距离D，防止AUV与障碍物发生碰撞。障碍物中心点到路径的距离记为d_i，通过比较d_i与D的大小来判断路径是否可行。若d_i ≤ D，那么放弃该路径，若d_i > D，那么选择该路径。

3 仿真验证及分析

仿真实验在Matlab 2018b环境下进行，验证本文改进A*算法的有效性以及对复杂环境的适应性，同时在相同栅格地图中对比Dijkstra算法、传统A*算法与改进A*算法，验证改进A*算法的优越性。仿真实验所用的栅格地图栅格数为20×20，地图逐渐复杂，障碍物权重逐渐增大。栅格地图中的单位栅格边长为1 m，空白区域为AUV可通行区域，黑色区域为不可通过的障碍物区域，仿真起始节点设置为左下角单元栅格，目标点设置为右上角单元栅格，安全距离D设为0.8 m。仿真结果如图8～图10所示，各算法在不同地图中的算法时间、搜索节点数、路径长度与拐点个数如表1所示。

由此可知，Dijkstra算法是最简单的路径规划算法，但是搜索节点太多，导致占用内存较多，算法花费时间长，效率低下，不适合将其用于地图范围大、环境复杂的场合。对比传统A*算法和改进A*算法，能够明显看出改进后的算法减少的搜索节点数量十分可观，算法时间平均缩短了20%以上，路径长度也得到了一定的缩短，拐点个数显著减少。改进A*算法得到的路径更为平滑，适合AUV实际运动。

4 结　语

传统A*算法存在算法时间较长，搜索效率低，路径冗余拐点较多、平滑度差等问题，本文针对这些不足，考虑多种因素，对其做出改进。引入障碍物系数，同时考虑搜索节点距目标点的距离，实现评价函数的自适应调整；考虑AUV自身体积，改进搜索节点选取规则，防止碰撞；划分象限，根据目标点所处位置减少搜索节点；剔除路径冗余拐点，改善路径平滑度。仿真实验结果表明，改进A*算法极大地减少了搜索节点数量，降低算法时间，提高规划效率，同时路径长度得到一定缩短，拐点个数显著减少。结果表明改进A*算法规划得到的路径更为平滑，符合AUV实际航行需求。