基于改进A*算法的水面无人艇路径规划

舒伟楠　赵建森　谢宗轩　张学生　马欣

摘要：为解决水面无人艇（unmanned surface vessel， USV）在碍航物比较密集水域的全局路径规划问题，提出一种基于改进A*算法的路径规划方法。利用像素阈值法剔除船载雷达图像中的冗余信息，并使用栅格法进行环境建模;提出节点碍航物检测法，用于优化无人艇路径规划，同时改进评价函数来减少遍历的节点总数;对规划路径上的冗余节点进行平滑处理。仿真结果表明：相较于A*算法，本文提出的改进A*算法能够确保USV远离碍航物，且算法遍历节点总数和规划路径上的转折点都较少。

关键词：  水面无人艇（USV）; 路径规划; A*算法; 碍航物检测

中图分类号：  U675.7; TP273文献标志码：  A

Path planning for unmanned surface vessels based on

improved A* algorithm

Abstract： To address the global path planning issue of unmanned surface vessels （USVs） in waters with dense obstacles， this paper proposes a path planning method based on the improved A* algorithm. The redundant information of shipborne radar images is removed by the pixel threshold method， and the grid method is used to carry out the environment modeling. The node obstacle detection method is proposed to optimize the path planning of USVs， and the evaluation function is improved to reduce the total number of traversal nodes. The redundant nodes on the planned path are smoothed. The simulation results show that， compared with A* algorithm， the proposed improved A* algorithm can ensure that USVs are away from obstacles， the total number of traversal nodes is fewer， and the turning nodes on the planned path are fewer.

Key words： unmanned surface vessel （USV）; path planning; A* algorithm; obstacle detection

引言

隨着人工智能技术的快速发展，路径规划在移动机器人领域的应用取得了很大的成就。水面无人艇（unmanned surface vessel， USV）是一种能够实现自主避碰的水上舰艇。当前对USV的研究主要集中在路径规划和自主避障等方面。USV在复杂水域完成全局路径规划和在特殊情况下避障的能力越突出，则说明其智能化水平越高。[1]

路径规划是指在有障碍物的情况下，在起点与终点之间寻找出一条尽可能短的安全路径[2-3]。按照对周围环境感知情况的不同，路径规划可以分为环境信息全部掌握的全局路径规划和环境信息部分未知的局部路径规划。A*算法具有搜索效率高的优点，被广泛应用于多种类型的搜索问题中。国内外学者已经对A*算法及其改进算法做了广泛的研究，并且取得了一定的成果。薛双飞[2]将人工势场法与A*算法相结合，根据不同的障碍物类型建立不同的斥力势场，并将该方法应用于东海大桥风机维护船舶的路径规划。王红卫等[4]提出平滑A*算法，解决了规划路径转折次数多的问题。陈豪等[5]基于A*算法，引入方向矢量以优化开放列表。WANG等[6]将船舶转弯半径融入A*算法，在估值函数中引入限制搜索方向的角度成本，生成与船舶实际转弯角度更加契合的路径。余必秀等[7]针对船舶避碰后无法快速回到预设航线的问题，在估值函数中增加一个代价值，让无人航道测量船在估值函数的使用上有不同的选择。王锦川[8]利用二叉堆对A*算法中的Open列表进行维护，加快了A*算法的搜索效率。杨瑶等[9]将车辆的最大转向角约束和最大曲率约束加入启发式函数中，同时提出车身轮廓代价改进拓展节点，使路径更符合车辆的运动学特性。荣少巍[10]将最小转弯半径作为约束条件对传统A*算法进行改进，同时使得A*算法可以实现高速与低速结合的应用。

USV具有惯性大、时滞大的特性，且在恶劣海况下易受风浪流的影响。USV只有与碍航物保持一定的安全距离，才能在碰撞危险出现时有足够的反应时间和操纵空间。A*算法的搜索特点可能会使规划路径贴近碍航物，影响USV航行安全。邓博文等[11]对障碍物做膨胀处理，王中玉等[12]在障碍物周围设置禁止搜索区，虽然效果都较好，但都需要对环境中的障碍物进行处理。针对A*算法应用在USV上的缺陷，本文提出碍航物检测方法：首先对A*算法遍历的候选节点进行筛选，使路径上的每个节点都能与碍航物保持一定的安全距离;然后对评价函数进行改进，使算法遍历的节点数更少;最后对所规划路径上的冗余节点进行平滑处理。

1环境模型建立

USV路径规划的前提是搭建出合适的环境模型。目前，常用的环境建模方法有栅格法、几何法和拓扑法[1]。由于采用栅格法规划的环境能够表现出一致性、规范性、简单性，所以本文使用栅格法建立环境模型。栅格法将环境空间划分为多个大小相同的栅格，每个栅格都用二进制数值表示。设整个环境空间S被划分为a×b个大小相同的栅格，每个栅格都对应着一个环境状态，用Nij表示，则S可以表示为（1）每个栅格的环境状态信息如下：Nij=0，自由空间5694CAD6-5319-4D70-9CE8-1A0831F542B3

1，碍航物（2）本文基于船载雷达图像进行路径规划，具体方法如下：（1）利用像素阈值法剔除雷达图像中的冗余信息，如背景颜色、航向线和距标圈等，只保留海冰回波（碍航物）;（2）将雷达图像转换为二值图像;（3）将二值图像栅格化。雷达图像处理前后对比示意图见图1。本文利用像素来描述环境空间中的图像栅格。图像栅格化可以把雷达图像转化成网格环境地图，给USV利用网格进行路径规划提供所需的环境信息。

基于A*算法的移动机器人运动方向通常为四邻域方向或八邻域方向。考虑到USV模型的适用性和复杂性，本文采取八邻域方向，即USV有上、下、左、右、右上、右下、左上、左下等8个运动方向可选择，见图2。

2A*算法的基本原理

A*算法是一种典型的启发式算法[13-14]，主要依据评价函数拓展节点。评价函数表示为（3）式中：f（n）表示节点n的评价函数;g（n）表示从起始點到节点n的实际代价值;h（n）表示从节点n到目标点的估计代价值。h（n）对评价函数的影响是最大的，本文采用欧几里得距离衡量h（n），即（4）式中：xn、yn分别为节点n的横、纵坐标;xg、yg分别为目标点的横、纵坐标。

A*算法根据评价函数对所有候选节点进行计算，选出f（n）值最小的节点，再将该节点作为父节点继续遍历剩余节点，直至遍历完所有节点或者成功找到目标点。具体实施步骤如下：

步骤1将起始点放入Open列表中，并对Open列表进行判断。如果Open列表为空列表，则执行步骤5，否则执行步骤2。如果Open列表包含目标点，则执行步骤4，否则执行步骤2。

步骤2从Open列表中选出具有最小f（n）值的节点，放入Closed列表中，并将其标记为父节点。

步骤3考察父节点的8个邻域节点。对于已经在Open列表中的节点，若其f（n）值小于原始值，则更新它。对于已经在Closed列表中的节点，跳过它，继续搜索。如果节点不在两个列表中，就将其放入Open列表中。跳到步骤1。

步骤4将目标点放入Closed列表中。在Closed列表中，复原从目标点到起始点的路径。

步骤5结束。

3改进的A*算法

针对A*算法规划出的路径贴近碍航物、遍历节点数较多等问题，采用碍航物检测方法将距离碍航物太近的节点剔除，同时对评价函数进行改进，使算法遍历的节点减少。最后对所规划路径上的冗余节点进行平滑处理。

3.1改进评价函数

将节点间的向量夹角值引入评价函数中，对其进行改进。如图3所示，S（xs，ys）是起始点，G（xg，yg）是目标点，N（xN，yN）是当前节点，n（xn，yn）、m（xm，ym）是当前节点的两个候选节点。假设起始点到目标点的向量为SG，起始点到两个候选节点的向量分别为Sn、Sm，则α、β分别为向量Sn、Sm与向量SG的夹角。

任意节点n的评价函数可表示为

（5）

式中：f（n）、g（n）、h（n）与式（3）定义相同;θ（n）是起始点S到任一候选节点n的向量Sn与向量SG的夹角。在评价函数中增加θ（n）的目的是使算法趋向于拓展起始点与目标点连线附近的节点。α、β越大，表明候选节点距离起始点与目标点连线越远，算法遍历的节点数会越多;α、β越小，表明候选节点距离起始点与目标点连线越近，算法遍历的节点数会越少。同时，改进评价函数可以使算法遍历的节点更加集中在起始点与目标点连线附近。候选节点n的θ（n）的计算式为

（6）

其中：（7）

（8）

（9）

（10）θ（m）也根据式（6）计算。

3.2碍航物检测

A*算法在应用于机器人路径规划时，将机器人视为一个质点，不考虑机器人的实际宽度和大小，这就使得按照规划路径行走的机器人会非常贴近碍航物。在水面航行的USV具有速度高、惯性大和不易操控的特点，如果规划出的路径太贴近碍航物，则极易导致USV与碍航物发生碰撞，因此需要让路径与碍航物保持一定的距离。

本文提出碍航物检测方法，并将该方法的实施放在A*算法步骤3之前，用来确保规划出的路径与碍航物之间保持一定的安全距离。碍航物检测就是判断每个候选节点周围一定范围内是否有碍航物，如果有碍航物，就将该候选节点剔除。碍航物检测方法的具体实施步骤如下：

步骤1确定待检测的候选节点。图4a中，紫色的点为当前节点，将8个候选节点放入序列N（={n1，n2，…，n8}）中。

步骤2依次选取序列N中的候选节点。图4b中，n1为候选节点之一。为保证USV与碍航物之间有足够的安全距离，本文以两个栅格单位为安全距离，确定节点n1周围的8个邻域节点序列M（={m1，m2，…，m8}）。

步骤3碍航物检测。判断n1与节点序列M中每一个节点的连线是否经过碍航物。如果任何一条连线通过的区域都有碍航物存在，则将节点n1从序列N中剔除。

步骤4对没有被剔除的候选节点继续执行A*算法剩余程序。

碍航物检测

在算法中增加了碍航物检测之后，算法计算量会变大，因此对已经遍历过的节点进行标记，不再对其进行筛选，以提高算法搜索效率。如果水域中碍航物比较多，在确保USV航行安全的情况下，可以将检测范围适当减小。比如，在本文仿真所选择的北极冰区，碍航物比较多，如果节点检测范围设置得太大，就会出现找不到最优路径的情况，因此可将检测范围适当调小。

3.3路径平滑

用改进的A*算法规划出的路径会存在转折次数多、冗余点多等问题。在实际应用中，这种频繁的转向会导致USV产生大幅度的航向改变，对USV的控制系统产生不利的影响。因此，需要对规划出的路径进行平滑处理，具体步骤如下：5694CAD6-5319-4D70-9CE8-1A0831F542B3

步骤1对节点进行共线判断，如果任一节点与其前后两个节点共线，就删除该节点。比如，图5a中的节点c与其前后节点b和d共线，则节点c会被删除，结果见图5b。

步骤2删除多余的转折点，处理方法是：遍历航线中剩余的所有节点，判断任一节点（起始点、目标点除外）前后两个节点连线上有没有碍航物。如果没有碍航物，就将该中间节点直接删除。比如，图5b中节点b的前后节点a与d的连线上没有碍航物，则节点b会被删除，结果见图5c。

3.4改进的A*算法路径规划流程

改进的A*算法路径规划流程见图6。

4仿真实验

为验证改进A*算法的有效性和适用性，在不同的雷达图像上进行实验。经处理后每张雷达栅格图像尺寸为400×300像素，即横坐标和纵坐标范围分别是[0，400]和[0，300]。每张雷达图像上，黄色五角星为USV的起始点，蓝色三角形为USV的目标点。

4.1改進评价函数的A*算法仿真

在两种具有不同碍航物的雷达图像中，给USV设置不同的起始点和目标点，用来验证改进A*算法的适用性。图7中蓝色连线是用A*算法搜索得到的路径，黑色连线是用改进评价函数的A*算法得到的路径。由图7可以很直观地看出，改进了评价函数的A*算法趋向于选择起始点与目标点连线附近的节点。

由表1可知，与A*算法相比，改进了评价函数的A*算法遍历节点总数和节点向量夹角总和都是较小的。这表明，本文对A*算法的改进具有明显的效果，能够使得算法遍历的节点更少，且具有目的性。

4.2进行碍航物检测的A*算法仿真

为验证碍航物检测方法能够使USV航线远离碍航物，在两种不同的雷达图像上进行仿真实验。从图7b和7d可以看出，A*算法规划出的路径（蓝色连线）容易出现距离碍航物较近的情况，而对候选节点进行碍航物检测的改进A*算法规划出的路径（黑色连线）能够远离碍航物，保证了USV航行安全。表1改进评价函数的A*算法与A*算法数据对比图7子图

序号起始点，目标点A*算法遍历

节点总数DA*算法遍历

节点总数A*算法节点向量

夹角总和/（°）DA*算法节点向量

夹角总和/（°）a（350，100）， （200，150）1 8401 2162 121479b（320，200）， （120，100）1 6481 6003 7901 342c（350，100）， （200，150）2 9761 2402 1091 325d（380，120）， （80，250）5 5842 7205 0053 067注：改进评价函数的A*算法用DA*算法表示

4.3路径平滑

用改进的A*算法进行USV路径规划，并对规划出的路径进行平滑处理。平滑处理前后的路径见图8。由图8可以看出，平滑处理能够将多余的节点删除，最终剩余的转折点也仅剩6个。本例中，改进A*算法生成路径所用时间是103 ms，改进A*算法生成路径后再进行平滑处理所用时间是112 ms。这说明改进A*算法可以在不增加太多时间的情况下，生成能够与碍航物保持安全距离的路径，而且遍历节点更少，转折点也大幅减少，更加适用于USV航行。

图9是基于本文提出的改进A*算法得到的其他路径规划结果。由图9可知，改进A*算法规划出的路径均能远离碍航物，且效果较好。

5结论

针对A*算法规划路径存在较多冗余点、贴近碍航物等问题，提出采用碍航物检测方法筛选出比较安全的候选节点，同时在评价函数中引入节点向量夹角值进行改进，并通过对规划路径的平滑处理去除冗余点，最终提出一种基于改进A*算法的水面无人艇（USV）路径规划方法。仿真实验表明，本文的改进方法能够生成远离碍航物的USV路径，改进后的A*算法趋向于选择起始点与目标点连线附近的节点，遍历的节点总数明显减少，平滑处理后生成的路径更符合USV实际航行特性，能够解决USV在内河航道和沿海等碍航物密集水域的路径规划问题。

参考文献：

[1]庄佳园， 万磊， 廖煜雷， 等. 基于电子海图的水面无人艇全局路径规划研究[J]. 计算机科学， 2011， 38（9）： 211214， 219. DOI： 10.3969/j.issn.1002137X.2011.09.049.

[2]薛双飞. 基于改进A*算法的近海船舶路径规划[D]. 武汉： 武汉理工大学， 2018.

[3]ZUO L， GUO Q， XU X， et al. A hierarchical path planning approach based on A* and leastsquares policy iteration for mobile robots[J]. Neurocomputing， 2015， 170： 257266. DOI： 10.1016/j.neucom.2014.09.092.

[4]王红卫， 马勇， 谢勇， 等. 基于平滑A*算法的移动机器人路径规划[J]. 同济大学学报（自然科学版）， 2010， 38（11）： 16471650， 1655. DOI： 10.3969/j.issn.0253374x.2010.11.016.

[5]陈豪， 李勇， 罗靖迪. 基于改进A*算法优化的移动机器人路径规划研究[J]. 自动化与仪器仪表， 2018（12）： 14. DOI： 10.14016/j.cnki.10019227.2018.12.001.

[6]WANG Z， XIANG X B. Improved A star algorithm for path planning of marine robot[C]//2018 37th Chinese Control Conference. IEEE， 2018： 54105414. DOI： 10.23919/ChiCC.2018.8483946.5694CAD6-5319-4D70-9CE8-1A0831F542B3

[7]余必秀， 初秀民， 柳晨光， 等. 基于改進A*算法的无人航道测量船路径规划方法[J]. 武汉大学学报（信息科学版）， 2019， 44（8）： 12581264. DOI： 10.13203/j.whugis20170239.

[8]王锦川. 自主式水面航行器导航与制导算法的研究[D]. 大连： 大连海事大学， 2014.

[9]杨瑶， 付克昌， 蒋涛， 等. 改进A*算法的智能车路径规划研究[J]. 计算机测量与控制， 2020， 28（10）： 170176. DOI： 10.16526/j.cnki.114762/tp.2020.10.035.

[10]荣少巍. 基于改进A*算法的水下航行器自主搜索航迹规划[J]. 电子科技， 2015， 28（4）： 1719， 22. DOI： 10.16180/j.cnki.issn10077820.2015.04.005.

[11]邓博文， 张春华， 李娟， 等. 局部路径规划在无人工程机械作业中的应用[J]. 兵工自动化， 2016， 35（10）： 7073， 79. DOI： 10.7690/bgzdh.2016.10.018.

[12]王中玉， 曾国辉， 黄勃， 等. 改进A*算法的机器人全局最优路径规划[J]. 计算机应用， 2019， 39（9）： 25172522. DOI： 10.11772/j.issn.10019081.2019020284.

[13]CHEN J C， LI M Y， YUAN Z Y， et al. An improved A* algorithm for UAV path planning problems[C]//2020 IEEE 4th Information Technology， Networking， Electronic and Automation Control Conference. IEEE， 2020： 958962. DOI： 10.1109/ITNEC48623.2020.9084806.

[14]陆皖麟， 雷景森， 邵炎. 基于改进A*算法的移动机器人路径规划[J]. 兵器装备工程学报， 2019， 40（4）： 197201. DOI： 10.11808/bqzbgcxb2019.04.048.

（编辑贾裙平）

收稿日期： 20210429修回日期： 2021-06-18

基金项目： 国家自然科学基金（51709167）;国家重点研发计划（2019YFB1600605）;上海市科技创新行动计划（18DZ1206101）

作者简介： 舒伟楠（1995—），男，安徽宿州人，硕士研究生，研究方向为船舶导航与海上通信，（Email）15755782120@163.com;

赵建森（1983—），男，黑龙江牡丹江人，副教授，博士，研究方向为智能通信、微波和天线，（Email）7230981@163.com05694CAD6-5319-4D70-9CE8-1A0831F542B3