基于改进A∗和DWA 的无人艇路径规划算法

王子静陈熙源

(1.东南大学仪器科学与工程学院，江苏 南京210096；2.微惯性仪表与先进导航技术教育部重点实验室，江苏 南京210096)

无人水面艇(unmanned surface vehicle，USV)具有自主航行、自动作业能力，成本低、环境适应性强等特点，可用于测绘、勘探、巡逻等领域[1]。 近些年来，出现了众多的USV 项目，由普利茅斯大学的海洋和工业动力分析(MIDAS)研究小组于2004 年启动研发的Springer USV 就是其中的突出代表[2]。

路径规划是USV 发展的关键技术之一，包括全局规划和局部规划。 全局规划是基于已知信息建立适当的环境模型，并寻找符合约束条件的安全路径。局部规划需要根据船载环境传感系统实时获取的周围环境信息动态调整和修正局部路径，以确保USV在航行过程中的安全。

Dijkstra 算法是经典的全局路径规划方法之一，可以在实际环境中搜索最短路径[3]。 使用启发式搜索的A∗算法在静态环境的路径规划中得到广泛使用[4]，并可以添加安全约束[5]，提高生成路径的安全性，或结合路径平滑方法[6]，消除路径中的“锯齿”以增强可行性。 此外，许多仿生物智能算法，例如遗传算法(GA)[7]，蚁群优化(ACO)[8-9]，粒子群优化(PSO)[10]，以及细菌觅食算法(BFO)[11]可用于USV 的路径规划。 近些年来，一些研究人员逐渐开始关注利用神经网络进行路径规划[12]。 文献[13]提出了一种基于模糊神经网络的最优路径规划模型，该模型将网络信息识别与海上航行路线的模式调度相结合。

局部路径规划在避障中起着至关重要的作用。人工势场(APF)是一种众所周知的局部路径规划算法，它构造了一个虚拟的引力和斥力场，而USV 的运动由吸引力和排斥力的合力决定。 但是，局部极小点和不可达点的问题限制了该方法的应用[14]。 陈彦杰等人提出的一种局部环境增量采样的路径规划算法在未知动态环境中较传统的RRT 算法具有更好的规划性能[15]。 文献[16]提出的一种使用新型约束FM方法，能够模拟运动船舶的动态行为。

尽管现有很多关于无人水面艇路径规划的研究，但其中大多数仅考虑全局规划或局部规划的单一应用场合。 针对存在动态障碍物的复杂环境中的导航和路径规划，本文提出了一种基于改进的A∗算法和动态窗口法(DWA)的混合路径规划算法，将全局规划与局部规划相结合，实现了动态环境下无人艇的路径规划，并通过仿真验证了本算法的性能。

1 全局规划

1.1 环境描述

为了进行路径规划，首先需要选择一个合适的模型来描述周围环境。 本文将USV 工作环境简化为二维平面，仅考虑该平面上USV 的偏航和位置变化，而忽略横滚和俯仰。 并使用栅格地图来表示，以二进制图像表示，其中可通行区域被视为1(白色)，而障碍物和危险区域被视为0(黑色)。

1.2 路径搜索

全局路径规划基于A∗算法实现。 本文使用8方向A∗算法，该算法每次都会搜索当前节点的8 个相邻节点，如图1 所示。启发式函数定义为:

图1 8 方向A∗算法示意图

式中:G(n)代表从初始节点通过选定节点序列到达当前节点所经过的路径长度，H(n)代表从当前节点到目标节点的估计代价。 本文使用曼哈顿距离估计H(n)，即

式中:xg和yg表示目标节点的横坐标和纵坐标，xp和yp表示当前节点的横坐标和纵坐标。

算法过程以伪代码形式描述如下:

算法1 A∗算法

传统的A∗算法每次仅搜索当前节点的8 个相邻节点。 当局部地图很大且距目标节点的距离很远时，搜索路径所需的时间将大大增加。 因此，本文提出了一种动态改变步长的快速A∗算法。

在每个搜索步骤中，检查在当前节点的8 个方向上距离为L的节点，并根据当前节点到目标节点的距离确定L:

式中:L是搜索步长的最大值，[ ]表示向下取整，ρ是比例因子，xg和yg表示目标节点的X坐标和Y坐标，xp和yp表示当前节点的X坐标和Y坐标。

由于快速A∗算法增加了搜索步长，所以路径上的两个节点之间可能会有障碍，如图2 所示。 因此，有必要检查路径上两个相邻节点之间是否有障碍。 如果两个相邻节点之间有障碍物，对该段进行一次重规划。

图2 搜索步长较大时相邻节点间存在障碍

为了评估动态改变搜索步长的快速A∗算法相对于传统A∗算法的改进，进行仿真对比如图3 所示，路径长度和运行时间列入表1。

图3 快速A∗与传统A∗路径规划结果

表1 路径长度和运行时间对比

计算机操作系统为Windows 10，处理器为Intel® CoreTMi7-6700HQ，主频率为2.60 GHz，内存为16G。 地图尺寸为2 000 m×2 000 m，比例因子ρ＝0.05，最大搜索步长为ML＝50。 通过传统A∗算法得到的路径如图3 中实线所示，长度为2 453.708 m，耗时63.464 24 s。 而通过快速A∗算法得到的路径如图3 中虚线所示，长度为2 474.696 m，耗时仅为2.291 18 s。 可以看出，通过快速A∗算法获得的路径长度可能略大于传统A∗算法的结果，但在比例因子和最大搜索步长设定合理的情况下，路径总长度的增加不足1%，而算法效率大大提高，时间消耗仅为传统A∗算法的1/30。 当需要在较短的时间内完成一次大范围的路径搜索时，快速A∗算法是十分有效的。

1.3 路径平滑

通过A∗算法生成的路径是由多段折线构成，如图4 中虚线所示，而受限于USV 本身的运动性能，航向不可能突变，因此需要进行平滑处理。 平滑分为两步，第一次去除路径中的部分冗余节点，只保留关键节点，第二次对保留下来的关键节点进行分段Hermite 插值，获得可行的平滑路径。

图4 A∗算法生成路径及平滑前后对比

剔除冗余节点，筛选关键节点的方法如下:设A、B、C为通过A∗算法搜索得到的初始路径上连续的三个节点，若AC段上不存在障碍物，且满足

dist(A-B-C)表示从A经过B到达C的路径长度，dist(A-C)表示从A到C的路径长度，即可将节点B视为冗余节点，将其剔除。 从初始路径的起点开始，遍历所有路径点，直到提取出所有的关键节点，此时的路径如图4 中点线所示。

Hermite 插值多项式的数学定义如下:

设f为[a，b]上充分光滑函数，对给定的插值节点，及相应的重数标号，当N＋1 时，若有H(x)∈Pn满足

则称H(x)为f(x)关于节点及重数标号的Hermite 插值多项式。

本文中使用分段三次Hermite 插值进行平滑处理，即在每个节点处都有:

一阶导数相等的物理意义是USV 的速度连续变化，符合USV 的运动约束。

对关键节点构成的路径进行分段三次Hermite插值平滑，得到最终路径如图4 中实线所示。

2 局部规划

动态窗口方法(DWA)用于局部规划。 该方法是基于速度采样的局部规划方法。 它在可行空间中对多组速度(v，ω)进行采样，并在一定时间内模拟USV 的轨迹。 评估功能用于评估这些轨迹，并选择与最佳轨迹相对应的速度来执行。

2.1 速度采样

根据USV 本身的限制和环境限制，可以将导航系统输出速度的采样空间限制在一定范围内。 第一个约束是每个USV 的固有属性，定义为

式中:vmin，vmax，ωmin和ωmax分别表示速度和角速度的最小值和最大值。

由于USV 的加减速性能的限制，有一个动态窗口，其定义为

式中:vc和ωc为当前速度和角速度，和为最大加速度和角加速度，和为最大减速度和角减速度，dt是时间间隔。

由于欠驱动USV 通常不具有随时停止的能力，因此，为了使其在到达目标点时能够停上，对速度进行限制

式中:dist 是当前位置到目标点的距离。

最终采样空间是Vm，Vd和Vs的交集。

在对多组速度进行采样之后，根据预设的时间间隔和仿真时间，对相应的多组轨迹进行仿真，从中剔除不安全的轨迹，并计算出剩余轨迹的评估函数。

2.2 轨迹模拟

基于USV 的运动方程和采样速度估算轨迹。前向仿真时间分为多个小时间窗口。 由于相邻两个时刻之间的时间间隔较短，因此可以将USV 的运动简化为线性运动，并根据式(11)更新状态。

式中:x，y和φ代表USV的X坐标，Y坐标和航向角，dt代表时间间隔，v和ω代表艏向线速度和角速度。

2.3 评价函数

评估函数用于评估每个轨迹，定义为:

其中:①G表示总的评价函数值，即期望最大化的目标函数。 ②heading ＝180-θ，其中θ是USV 的航向角与目标点方向之间的夹角，该分量的物理意义是使USV 朝向目标行进。 ③distance 是当前轨迹上每个点与最近障碍物之间的最小距离。 为了防止此项的影响过大，应设置一个最大阈值，即只考虑一定范围之内的障碍物。 ④velocity 是USV 的艏向线速度，在确保安全的前提下，期望USV 能够更快地达到目标点。 ⑤deviation 用于评估当前轨迹与从全局规划获得的轨迹之间的偏差。α，β，γ和δ是各项的权重系数。

值得注意的是，在计算每个轨迹的总评估函数之前，需要对评估函数中的四个组成部分进行归一化，如下所示:

算法流程如图5 所示。

图5 动态窗口法算法流程

3 仿真分析

为验证本算法的可行性，使用实际场景进行仿真实验，图6 是新安江水库附近水域的卫星地图，图中部分对应的实际大小为1 500 m×1 100 m，对其进行处理得到栅格地图如图7 所示，分辨率为1 m/pixel。仿真所用无人艇运动参数如表2 所示。

图6 新安江水库卫星地图

图7 栅格地图(1 m/pixel)

表2 无人艇运动参数

3.1 静态环境仿真

图8 路径规划结果(静态环境)

第一组仿真环境中不存在动态障碍，所有地图信息均为先验已知，规划结果如图8 所示，图8 中实线为通过A∗算法进行全局规划生成的预规划路径，长度为1 220.882 m，虚线为通过DWA 进行实时局部规划得到的最终路径，长度为1 219.901 m。 显然两者近乎完全重合，表明在没有未知障碍物的情况下，无人艇将沿着预规划路径前行。 同时，将本文算法与人工势场法(APF)进行对比，APF 的规划结果如图8 中点线所示，路径长度为1 247.941 m。 显然本文提出的算法规划得到的路径长度更短，也更加平滑。

3.2 动态环境仿真

第二组仿真在环境中添加了两个未知的动态障碍，一个以速度v1＝7 m/s 从左向右移动，另一个以速度v2＝2 m/s 从上向下移动。 在仿真时间t＝20 s和t＝50 s 两个时刻，无人艇分别遭遇了两个动态障碍，并偏离预规划的路径(图9 和图10 中实线)进行规避，避障完成后继续沿预规划路径前行，成功躲避了两个动态障碍，到达目标位置，得到最终路径如图11 中虚线所示。

图9 t＝20 s 躲避第1 个障碍

图10 t＝50 s 躲避第2 个障碍

图11 路径规划结果(动态环境)

4 结论

本文提出了一种基于改进的快速平滑A∗算法和动态窗口法的无人艇路径规划方法。 在传统A∗算法的基础上，动态改变搜索步长，将大范围搜索时的算法效率提升了约30 倍，之后剔除路径中的冗余节点，根据关键节点采用分段Hermite 插值方法进行平滑处理，得到了更加平滑的路径。 通过在动态窗口法的评价函数中新增路径偏差项，将全局规划与局部规划相结合，实现了动态环境中无人艇的路径规划。 仿真实验表明，该算法切实有效，可以满足无人艇的运动约束，快速规划出可行路径，并在行进过程中，适时修正路径，躲避动态障碍，使无人艇安全到达目标位置。