多方向无人水面艇路径规划算法

童心赤，张华军，郭 航

（武汉理工大学自动化学院，武汉 430070）

（∗通信作者电子邮箱zhanghj@whut.edu.cn）

0 引言

近年来，随着无人平台在军事和民用等企业中的推广，无人水面艇作为一种能够在复杂海洋环境下自主航行并完成各项任务的水面平台受到了极大关注［1-3］。路径规划是无人艇自主控制部分的关键要素，其任务是对当前海洋环境信息进行感知，对收集的信息进行分析计算，实现有效且安全的路径规划。过去，最佳路径通常与最短路径相一致，随着此问题的定义不断发展，最佳路径的选取已与最小行进距离、平均高度、燃油消耗、雷达辐射等诸多因素有关，因此相关研究工作通常选择启发式搜索算法来实现路径规划［4］。目前对无人艇路径规划的方法主要有遗传算法［5-6］、粒子群算法［7-8］、人工势场算法［9-11］、A*算法［12-14］等，其中遗传算法与粒子群算法等仿生进化算法能有效利用已获取的环境信息，同时又能不断探索新路径，避免了复杂的数学计算。另外人工势场法通过产生包括排斥力和吸引力的势场来描述障碍物和目标点对路径搜索的影响，但在解决大规模问题时易陷入局部最优解。A*算法是一类适用于全局环境信息已知的路径规划方法，它利用启发函数值来估计任意点到目标点的远近程度，从而减少搜索空间，提高搜索效率。

目前普遍使用A*算法实现无人艇在不确定的海洋环境中的路径规划，但该算法生成的路径只以路径长度为优化目标，没有考虑路径的安全性与平滑度等因素。文献［15］为避免障碍物与无人艇距离过近，对障碍物进行一定的膨胀处理，结果显示规划的路径具有较好的安全性，但该方法并不适用于复杂的海况环境；文献［16］通过评估与障碍物间应保持的最佳距离建立无人艇安全模型，并采用三次样条插值算法拟合离散的路径节点以获得平滑连续的路径；文献［17］以三阶贝塞尔曲线为基础设计了满足二阶几何连续的路径平滑算法，并对平滑后的路径进行碰撞检测，仿真结果表明该算法能够有效规划出安全平滑的路径。

针对无人水面艇路径规划问题，本文首先结合电子海图生成栅格化环境信息，建立无人艇安全区域模型并以此作为生成最优路径点的安全距离约束；其次针对常规A*算法生成的路径转折次数多且转折角大等问题，设计一种多方向A*优化算法与路径平滑算法以规划一条距离更短且更有效的受约束的全局最优路径；最后仿真结果验证了该改进算法的有效性和可行性。

1 模型建立

1.1 无人水面艇安全区域建模

为精确描述无人水面艇路径规划时的位置与状态，本文所研究的无人艇均为六自由度运动学模型，在航行过程中可产生6 个自由度的运动信息。考虑到无人艇在航行过程中需要较大的运动空间，在路径规划时不能简单地当作一个质点处理，因此设计一半径为R 的圆形区域包围无人艇作为生成最佳航路点的安全距离约束。半径R 代表安全距离，安全距离约束下的路径规划应保证无人艇与障碍物的最短距离大于安全距离，解决了无人艇在实际航行中的安全问题。其搜索原理如图1所示。

图1 基于安全区域的路径搜索原理Fig.1 Principle of path search based on safe area

1.2 环境建模

为便于实现路径规划，首先应建立无人艇的航行环境模型，将实际的海洋环境预处理为便于在计算机中表示的连接图，同时保留必要的原始信息。目前常用的环境建模方法有可视图法［18］、栅格法［19］、拓扑图法［20］等，本文采用栅格法将实际电子海图转化为二进制网格图，建立栅格化地图模型，并将以经纬度表示的导航数据转换到栅格地图坐标系中。在栅格图中，定义每一单位栅格状态为H(x，y)，其中x、y分别代表栅格的横向与纵向位置：白色部分H(x，y)=0表示可行区域；黑色部分H(x，y)=1 表示障碍物，这使得无人艇碰撞的风险度仅取决于栅格状态值。对某一海域进行栅格化处理，将环境信息离散化为一系列的二值化栅格，处理结果如图2所示。

图2 环境信息栅格处理图Fig.2 Rasterization processing chart of environment information

为实现无人艇实时路径导航，现将每一时刻的位置信息（经度与纬度值）与栅格节点一一对应。假设某一海图内的最大经度为maxlon，最小经度为minlon，最大纬度为maxlat，最小纬度为minlat，对应的栅格地图横向划分为a 个栅格，纵向划分为b个栅格，则各经纬度对应的具体栅格位置为：

其中：floor 操作为取整运算；x，y 分别为对应栅格的横向与纵向位置；lon，lat分别为当前位置的经度与纬度信息。

2 基于改进A*算法的路径规划

A*算法作为一种启发式搜索算法，根据启发函数以最小代价快速返回最优路径，保证了路径完整性以及搜索高效性，但是常规A*算法仅以路径长度作为启发函数值，规划出的路径并不能有效引导无人艇在复杂海洋环境下安全平滑的运动。

针对该问题，本文首先根据无人艇安全区域模型，建立安全距离约束下的A*启发函数来搜索最短路径。然后设计一种多方向A*搜索算法减少并修正不必要的路径节点，获得多方向A*优化路径。最后建立平滑处理安全区域，在此区域约束下采用三次样条插值算法拟合离散的关键路径节点以获得连续平滑的路径。

2.1 安全距离约束下的A*启发函数

基于上述构建的栅格化地图模型，采用具有安全距离约束的A*算法以实现路径规划。在栅格地图中，A*算法采用启发函数f(n)来估计地图上任意栅格到目标栅格的代价，从而引导搜索方向。函数f(n)表达式如下：

其中：g(n)为各单元栅格到起始栅格的距离，h(n)为各单元栅格到目标栅格的启发式代价。

如图3 所示，在栅格地图中，无人艇运动位置可由某一具体栅格表示，将A*搜索方式设置为八方向搜索，即与当前栅格相邻的八个栅格作为无人艇下一时刻的可选位置。在安全距离约束下，使用八方向A*算法搜索各栅格节点，设置各栅格对应的启发式代价为：

式中：DE为栅格到目标栅格E 的距离，l 为栅格到周围障碍物的最短距离，R 为无人艇安全半径。l ≥R 代表当前位置无人艇安全区域内不存在障碍物；相反，l ＜R 则代表安全区域内存在障碍物。

图3 安全距离约束下的八方向搜索原理Fig.3 Principle of eight-directional search under safety distance constraint

根据式（3），安全距离约束下各栅格的启发函数可定义为：

式中DS为栅格到起始栅格S的距离。

2.2 多方向A*搜索算法

八方向A*搜索算法只能以45°和90°方向作为路径搜索选项，就整体路径而言，该搜索方式并不能保证最优性。因此以路径距离最短为优化目标对已规划出的路径节点进行调整以实现多方向A*搜索，定义某一路径起点为PS、终点为PE、中间节点为Pi(i=1，2，…，n)，其具体步骤如下。

步骤1 提取安全距离约束下八方向A*算法规划的路径PS→P1→P2→…→Pi→PE中所有节点。

步骤2 从起点PS开始判断节点Pi(Pi=PS)与剩余节点的相连路径Pi→Pi+1(Pi+1=P1，P2，…，PE)是否满足安全距离约束，即判断相连路径上所有节点到障碍物的距离是否小于安全半径。记录满足约束条件的最靠后节点Pj1。若Pj1=PE则执行步骤4，若Pj1≠PE则执行步骤3。

步骤3 判断节点Pjk(Pjk=Pj1)与剩余节点的相连路径是否满足安全距离约束，记录满足约束条件的最靠后节点Pj2。若Pj2=PE则执行步骤4，若Pj2≠PE则重复执行步骤3。

步骤4 输出节点Pi的最终优化路径Path_i 为：PS→…→Pi→Pj1→Pj2→…→PE。若Pi=PE则执行步骤5，若Pi≠PE则令Pi=Pi+1并执行步骤2。

步骤5 比较所有节点优化路径长度，选择其中最短路径作为多方向A*搜索算法优化路径。

在栅格图中，假设某一路径的路径节点为P1，P2，…，Pm，则该路径长度DP可计算为：其中：Pi(x)、Pi(y)表示节点的横坐标和纵坐标，m为路径的节点数。

其次在多方向优化过程中，需判断两节点相连路径是否安全，因此需计算路径所经过的所有栅格是否满足安全距离约束。假设两栅格P(i1，j1)和P(i2，j2)之间的坐标位置差异为Δi、Δj，且i1＜i2、j1＜j2。若Δi ＜Δj 则路径所有栅格坐标位置计算方法如下：

同理，若Δi ＞Δj 则被路径所有栅格坐标位置计算方法如下：

如图4 所示，现基于传统八方向A*算法得到路径1 为PS→P1→P2→P3→P4→P5→P6→P7→PE，对 该路径进行多方向搜索优化处理，其具体优化步骤如下。为便于描述优化过程，图中栅格为实际栅格放大所得，并不代表相连路径与障碍物间的实际距离。

图4 多方向A*搜索算法示例图Fig.4 Example of multi-directional A*search algorithm

取出路径所有节点，从节点PS开始依次判断该点与节点P1，P2，P3，P4，P5，P6，P7，PE的相连路径是否满足安全距离约束，取出满足约束条件的最靠后节点P4。然后依次判断节点P4与节点P5，P6，P7，PE的相连路径是否满足约束条件，同样取出满足约束条件的最靠后节点PE，最后输出节点PS的优化路径为Path_S：PS→P4→PE。

路径剩余节点P1，P2，P3，P4，P5，P6，P7的优化步骤与节点PS一致，依次得到各节点的优化路径为：

Path_1：PS→P1→P4→PE

Path_2：PS→P1→P2→P4→PE

Path_3：PS→P1→P2→P3→P6→P7→PE

Path_4：PS→P4→PE

Path_5：PS→P4→P5→P6→P7→PE

Path_6：PS→P4→P5→P6→PE

Path_7：PS→P4→P5→P6→P7→PE

最后依次比较各优化路径长度大小并取出其中较短路径，得到最终优化路径2为PS→P4→PE，明显相较于路径1该路径长度大大减小且规划更为合理。

2.3 多方向A*搜索算法

路径由多方向A*算法优化后，虽删除并整合了不必要的节点，但优化路径仍由许多剩余节点相连的短线段组成，导致路径转折角较大。针对该问题，本文采用与原始路径偏差较小的三次样条插值算法进一步改善路径的平滑性。该算法的主要原理是使用由三阶多项式组成的样条来连接一组路径节点，通过求解此多项式以拟合原路径。若给定n个路径节点，则存在n -1个节点区间，设每个区间[xk，xk+1]的三次样条函数为：

为保证生成的路径光滑连续，该多项式应满足以下约束条件：

根据式（9），对所有路径节点区间进行平滑处理可获得原路径的拟合平滑曲线。而多方向A*搜索算法新生成的路径相邻节点距离较远，导致平滑曲线与原路径拟合程度较差。因此本研究仅对路径关键节点即航向改变节点进行平滑处理，假设某一关键节点为Pi(xi，f(xi))，则取该节点前后相邻四个节点Pi-2、Pi-1、Pi+1、Pi+2生成4 个节点区间以获得关键节点平滑曲线。

为保障路径的安全性，消除平滑处理所造成的路径变化影响，设计一半径为RS的圆形区域包围路径关键节点作为平滑区域。如图5 所示，结合安全半径R，将关键节点的安全区域扩大为半径R+RS的圆形区域。因此对关键节点平滑处理应满足条件：

式中l为关键节点到障碍物的最短距离。

图5 关键节点平滑处理的安全区域示例图Fig.5 Example diagram of safe area smoothed by key nodes

3 基于改进A*算法的路径规划

本实验采用微软Core i5-4200M 处理器，主频为2.5 GHz，内存为8 GB。在Python 3.7 环境下，为验证上述算法的可行性，现提取经度范围为112.513 926°E ∼112.700 695°E、纬度范围为21.570 176°N ∼21.735 468°N 的实际海上环境信息，并对其进行栅格化处理，设置每个栅格代表的横向与纵向实际距离均为20 m，处理结果如图2 所示。根据无人艇操纵特性与航行环境，将安全半径R 设置为20 m、60 m 与100 m，分别对应1个、3个、5个栅格距离。

图6 不同安全距离下的路径规划对比Fig.6 Comparison of path planning under different safety distances

将所设置的三种不同安全半径作为无人艇的安全距离约束，设置路径起始点为（100，280），目标点为（320，20），重复进行20次路径规划。图6显示了不同安全距离约束下的路径规划结果，三种路径存在明显差别，其中安全半径越大的路径距障碍物越远。

各路径上的节点到周围障碍物的最短距离如图7 所示，横坐标代表路径节点位置，纵坐标代表各路径节点到周围障碍物的最短距离。其中所标记的黑色节点为各路径上距障碍物最近的节点，该点纵坐标值均等于所设置的安全半径大小，可见路径节点均符合安全距离约束。

图7 不同路径节点与障碍物的最短距离对比Fig.7 Comparison of the shortest distances between different path nodes and obstacles

因此由图6、图7 可知：安全距离约束下的A*启发函数能够准确搜索到一条从起点到终点的安全路径。

在不同安全距离约束下，各路径的总节点数、路径长度与规划时间具体对比结果如表1 所示。随着安全半径的增加，路径总节点数以及路径长度均小幅度地增加。另外安全半径为R=60 m 与R=100 m 的规划时间较R=20 m 分别增加了0.4 s 和1.77 s，这是由于路径规划过程中安全半径设置得越大，需搜索的栅格节点增加得越多，所需的计算成本也更高。

表1 不同安全距离下的路径规划结果Tab.1 Path planning results under different safety distances

另外，为验证多方向A*搜索算法以及路径平滑算法性能，本文在上述仿真的基础上设置平滑区域半径RS=40 m，并对R=60 m 的常规A*搜索路径重复进行20 次优化平滑处理。常规A*搜索路径经多方向A*搜索算法优化后，仅由起始点、目标点以及三个关键节点相连而成。多方向优化路径上各节点到障碍物的最短距离如图8 所示，其中横坐标代表节点位置，纵坐标代表节点距障碍物的最短距离。另外所标记的黑色实心节点代表优化路径上距离障碍物最近的节点，黑色空心节点则为路径上的三个关键节点。

图8 多方向优化路径节点与障碍物最短距离图Fig.8 The shortest distance between nodes of multi-directional optimized path and obstacles

由图8 可知：多方向优化路径上距障碍物的最短距离为63.2 m，符合半径R=60 m 的安全约束。另外路径上关键节点距障碍物的最短距离分别为174.1 m、72.1 m、72.6 m，而关键节点平滑处理安全半径为R+RS=100 m，可见仅第一个关键节点满足平滑处理约束条件。对该关键节点进行平滑处理后的最终优化路径及优化结果如图9以及表2所示。

图9 不同算法优化的路径Fig.9 Optimized paths of different algorithms

由图9、表2 可知：改进A*多方向搜索算法可以有效地整合并去除传统路径中多余的节点，减少路径长度，其中路径节点数与路径长度均明显小于其余三种算法。同样改进A*算法消除了路径转折次数多且转折角较大的问题，转向次数减少至3 次，在一定程度上达到最优。另外改进A*算法与其余三种算法时间复杂度均为O(n2)，虽然搜索节点数的增加导致路径规划时间小幅度地提高，但它始终提供一条距离成本更低且转向次数更少的路径，这更加符合无人艇路径规划时的实际需求。

4 结语

针对无人水面艇路径规划时的安全性、经济性及实用性要求，本研究改进A*算法取得了较好的效果。通过建立无人艇安全区域模型，使该模型作为生成最优路径点的安全距离约束，并在八方向A*算法上设计一种多方向搜索算法以及路径平滑算法以获得更符合无人艇操纵特性的全局最优路径。所提出的改进A*算法经过对比分析，结果表明该方法可生成更安全、距离更短、平滑度更好的路径，非常适用于复杂海洋环境下无人艇的实时路径规划。