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

杜贻群，黄 静，张华军

（1.中国人民解放军92942部队，北京 100161；2.武汉理工大学，湖北 武汉 430070）

进入21世纪，国际形势日益复杂，海上交通运输、渔业养殖、科学研究、旅游和军事等活动日益频繁，海洋资源越来越受到各国的重视，探索海洋、开发海洋逐渐成为世界各国争相研究的热门课题［1-2］。无人航行器因其机动灵活、成本低廉、全天候作业等优点受到了前所未有的关注，各类不同的无人航行器已在民用、军事和科研［3］等不同领域得到应用，在海洋资源开发、环境质量监测、无人作战等方面发挥越来越重要的作用。在民用领域中，无人航行器可用于海洋水文气象研究，进行海水温度、密度、海流、海底地形地貌的探测［4］，其可以抵达载人平台无法抵达的危险水域，为海洋环境测量提供更加准确的数据。在军事领域中，无人航行器可用于水底情报收集、战区地图绘制、协助警戒巡逻以及扫猎雷［5］等战术支持任务，为远征部队在沿海水域活动的安全提供保障。

路径规划是无人航行器的重点研究方向之一，在具有障碍物的海洋环境中按照路径最短、工作代价最小、安全性最高等性能指标［6］寻找一条从起始状态到目标状态的最优路径，是衡量其智能化水平的重要指标。许多学者在该领域中做了大量的研究工作，探索出多种有效的求解方法，使得无人航行器的规划能力不断增强。目前常用的求解方法主要有Dijkstra算法［7-10］、人 工 势 场 法［11-14］、概 率 地 图 法［15-17］、A∗算法［18-20］、蚁群算法等［21-24］，其中，Dijkstra算法是典型的单源最短路径算法，该算法的优点是运算简单并且能够规划出最短路径，但运算量较大；人工势场法的中心思想是将无人航行器所处的环境用势场定义，通过位置信息进行无人航行器的避障行驶，环境适应性较强，但存在局部最优和目标不可达问题，势场空间的设计是其应用的关键；A∗算法利用启发式函数权衡全局地图中路径最优节点的位置，迭代计算评估函数査找低成本路径，该算法通过减少搜索范围提高算法效率导致搜索空间有限，所以，最终路径并非全局最优解；概率地图法是基于启发式节点增强策略的一种路径规划方法，通过在地图中随机生成有效采样点获取概率路线图，可以有效避免对复杂环境精确建模，但该算法是概率完备型，当搜索次数限制太少或者采样点过少时，就可能找不到解；蚁群算法是一种模拟蚂蚁觅食行为的模拟优化算法，该算法在求解性能上有较强的鲁棒性且具有本质并行性，易于用计算机实现，但容易陷入局部最优，常用于解决旅行商问题。

针对水面可行域直航点之间最短路径的搜索问题，本文首先结合海图建立了路径规划问题的数学描述，其次，在采用Dijkstra算法原理基础上，对搜索方向进行扩展设计了一种具备八方向搜索的路径搜索规划算法，同时对八方向路径搜索算法进行改进，优化其扩展过程中的排序操作以提高搜索效率，最后设计了多方向路径搜索算法，使其规划的路径更短、转向角更少，使得搜索算法能够更好地应用于实际。

1 水面航行环境建模

在无人航行器的路径规划研究中，模拟海上地形是重要的组成部分，常用的环境建模法有栅格法［25］、拓扑图法［26］和自由空间法［27］等，其中，栅格法的原理是将电子海图用栅格进行划分，包含障碍物（岛屿、暗礁等）的栅格标记为不可行栅格，反之，则为可行栅格，以此为基础进行路径搜索。采用栅格来表示电子海图，可以极大地简化海图中障碍物的复杂程度和减少计算量，提高路径搜索的效率。

栅格法［28］由学者W.E.Howden于1968年提出，首先对电子海图进行图像处理，提取其中的经纬度信息，并转化为计算机能够识别的栅格化模型，利用栅格块组成电子海图中的可行区域与不可行区域，其中，可以自由航行且在此区域内不含任何障碍物的网格称为自由栅格，含有障碍物的栅格称为障碍栅格，环境Map由栅格Gr i d（xi，yi）构成：

其中xi＝1、2、…n，yi＝1、2、…n，Gr i d（xi，yi）＝0表示该栅格为自由栅格，Gr i d（xi，yi）＝1表示该栅格为障碍栅格。

在电子海图的栅格化过程中除了自由栅格与障碍栅格外还存在第三种栅格，该栅格内一部分是自由区域，另一部分是障碍物区域。为此，本文按照图像形态学扩充理论对其进行障碍物膨胀处理，当障碍物占据某一栅格时，无论占据栅格的面积为多少均将其视为完全不可行区域。图1为不规则障碍物边缘膨化处理方式，为使结果显示更明显，将栅格进行放大处理。

图1 不规则障碍物边缘膨化处理

根据上述建模方法对某一海域进行栅格化处理，图2将环境信息离散化为一系列的二值化栅格，处理后的栅格化环境模型如图2b）所示。

图2 电子海图的栅格化处理

2 无人艇最短路径求解

狄克斯特拉算法由荷兰计算机科学家狄克斯特拉于1959年提出，用于解决有权图中单源点最短路径问题。即在一个图G中，选取一个顶点V，然后以V为起点找出顶点V到图G中其余各点的最短距离。其主要特点是利用广度优先搜索的思想，以起始点为中心向外层层扩展，直到终点为止，该算法一直被学者广泛采用，作为计算最短路径问题时的基础理论，并结合实际问题对其进行改进，具体算法流程图如图3所示。

图3 Dijkstra算法原理图

Step1：初始化起点V1与其他各点的直接距离，没有边直接相连的节点之间距离为∞，d i s［0，∞，10，∞，30，100］；

Step2：从d i s［］中找出节点Vi，让V1到Vi的距离为最小，且把Vi规划到T｛｝中，所以d i s［0］＝0（V1→V1），那么d i s［0，∞，10，∞，30，100］，T｛V1｝；

Step3：继续Step1，此时d i s［3］＝10，T｛V1，V3｝；

Step4：T｛｝中每加入一个新节点，需要对边进行松弛判断，判断V1通过新节点到达其他节点的距离是否比V1直接到达其他节点的距离小，若小则替换V1与其他节点间的直达距离。此时d i s［4］＝60（（V1→V3→V4）＞（V1→V4）），所以d i s［0，∞，10，60，30，100］，T｛V1，V3｝；

Step5：循环Step2与Step3，此时d i s［5］＝30，所以T｛V1，V3，V5｝，又因为d i s［4］＝50，d i s［6］＝90，所以d i s［0，∞，10，50，30，90］；

Step6：循环搜索，直到求出V1与其他所有的点之间的最短距离，并以图3b）所示的邻接矩阵表示。

2.1 四方向路径搜索算法

在处理完毕的栅格地图中设置起点V（xs，ys）与终点V（xe，ye），并获取地图的横向与纵向栅格数m、n，其次，利用Dijkstra算法的思想以起始点为中心向外层层扩展，扩展方向为正东、正西、正南、正北四个方向，设定距离矩阵d i s（xi，yi）存储起点栅格到各扩展栅格之间的距离，初始化起点栅格的距离为0，其他栅格距起点栅格的距离为∞，如公式（2）所示：

其次，由起点向四个方向进行第一次扩展搜索，搜索过程中将遇到可行栅格与不可行栅格，令可行栅格与起点的距离为1，不可行栅格与起点的距离为∞，如公式（3）所示，同时用数组S D P_num和S_d i s存储此时的最短距离栅格个数与最短距离：

当由起点向四周的第一次扩散搜索完成后，保存扩展的四个栅格坐标及其父栅格坐标，因为当算法搜索到终点时，需要根据各栅格的父栅格进行路径回溯从而得到起点与终点之间的最短路径。以向正东方向扩展为例，令数组S D_Poi nt_x与S D_P oi n t_y分别存储扩散的最短距离栅格节点的横纵坐标，如公式（4）所示；令数组P_x（xi，yi）与P_y（xi，yi）分别存储扩散的最短距离栅格节点的父节点的横纵坐标，如公式（5）所示：

若起点周围没有障碍物栅格，第一次扩散搜索将得到四个距离为1的可行栅格，然后，由存储的四个最短距离栅格依次进行第二次扩展搜索，为避免某些栅格被重复搜索，本文利用两个距离矩阵d i s（xi，yi）和d i s′（xi，yi）对栅格扩散搜索时的距离更新进行合理的规则设计，如公式（6）所示：

若第二次扩散搜索时未遇到障碍物栅格，将得到八个距离为2的可行栅格，此时最短距离栅格个数S D P_num的值更新为8，最短距离S_d i s的值更新为2，并将新的最短距离栅格横纵坐标分别存储于数组S D_P oi n t_x与S D_Poi nt_y中，其父节点的横纵坐标分别存储于数组P_x（xi，yi）与P_y（xi，yi）中。 在第二次获得的八个栅格基础上不断向四周进行扩散搜索直至终点，最后，利用存储的父栅格坐标进行路径回溯，找到一条由起点到终点的全局避障路径，如图4所示。

图4 二维栅格地图中四方向搜索步骤

2.2 八方向路径搜索算法

在上述四方向路径规划算法中搜索方向仅为横向与纵向，而水面无人艇在实际航行过程中不仅能够进行直角转弯，还能进行斜向转弯，所以，需要对上述四方向搜索算法进行优化，增加斜向栅格的检测使得从出发点到目的地的搜索路径更短。八方向搜索算法可行性分析如图5a）所示，搜索原理如图5b）所示，具体步骤如下。

图5 八方向搜索算法原理

Step1：第一次扩散搜索如图5b）中蓝色栅格所示，由d i s（xs，ys）＝0的起点扩散得到四个d i s（xi，yi）＝1与四个d i s（xi，yi）＝1.414的栅格，并对距离进行排序［1，…，1；1.414，…，1.414］；

Step2：以四个距离为1的栅格为中心按搜索顺序进行第二次扩展搜索，如图5b）中绿色栅格所示，得到四个距离为d i s（xi，yi）＝2的栅格与八个距离为d i s（xi，yi）＝2.414的栅格，同时对距离数组进行更新与排序［1.414，…，1.414；2，…，2；2.414，…，2.414］；

Step3：以距离为四个1.414的栅格为中心进行第三次扩散搜索，如图5b）中黄色栅格所示，得到四个距离为d i s（xi，yi）＝2.828的栅格，再次对距离数组进行更新与排序操作［2，…，2；2.414，…，2.414；2.828，…，2.828］，得到新的路径节点；

Step4：如此循环，直到遍历到终点时跳出循环，最后，通过父栅格坐标进行路径回溯找到最短路径。

3 无人艇路径搜索算法优化

扩展八方向搜索算法虽然能够得到距离更优的路径，但在搜索过程中排序操作会耗费较多时间，因此需要对其进行优化，设计出一种无须在每次扩展搜索时进行距离排序的改进八方向路径搜索算法。为使航行器满足实际转向的要求，在改进八方向搜索算法的基础上对搜索方向进行进一步扩展，设计了一种多方向的路径规划搜索算法。

3.1 改进八方向搜索算法

改进的八方向路径搜索算法是在四方向搜索的基础上，每一次更新栅格距离数据时增加对角栅格的判断，判断从当前栅格到其扩展栅格的对角直达距离是否小于通过横向与纵向结合的方式到达该扩展栅格的总距离，如果是，则将该扩展栅格的父栅格设置为斜对角栅格从而完成无须排序的八方向栅格搜索，改进方式如图6所示。

图6 八方向搜索算法改进方式

3.2 多方向搜索算法

由图6可知，改进的八方向搜索算法虽然简化了每次扩展搜索时的排序过程，转向角也从90°直角扩展为45°的斜向角，但就整体而言并不是真正意义上的最短路径，实际航行过程中无人航行器的转向角可以是多方向的。因此，本文以路径距离最短为优化目标，在改进八方向搜索的基础上，利用局部搜索的方法从其获得的路径节点中提取最佳路径点，使得水面无人艇可向任意航行角方向前进，以此获得最短路径。

图7描述了多方向路径搜索的原理，该算法在八方向路径规划算法的基础上进行改进，其中，黑色线段为八方向搜索算法规划路径，红色线段为多方向搜索算法规划路径，对比可知多方向搜索算法规划的路径比八方向搜索算法规划的路径更短、转向角更少且路径更加平滑，具体原理如下。

图7 多方向搜索算法原理

Step1：在栅格地图的基础上采用改进的八方向搜索算法进行路径规划，得到一条满足要求的全局避障路径，获取其中的最佳避障路径节点如图7中黑色线段VS→V1→V2→V3→V4→V5→V6→V7→VE所示；

Step2：从节点V（xs，ys）开始，依次判断该节点与V1、V2、V3、V4、V5、V6、V7、VE的连线经过的栅格是否存在障碍物，并计算所经过栅格的坐标值，设两栅格V（xs，ys）和V（xi，yi）的横坐标与纵坐标之差分别为Δx与Δy，如公式（7）所示。当Δx≥Δy时，被测栅格坐标位置计算方法如公式（8）所示、当Δx＜Δy时，计算方法如公式（9）所示，式中，r oun d操作为近似取整运算，若存在障碍物，则路径不变：

当VS与其他节点连线经过的栅格之间不存在障碍物时，说明两节点可以直接相连，那么，V（xs，ys）将作为与之直连节点V（xi，yi）的父节点，两节点间的其他节点将被剔除，如公式（10）所示：

如图7中红色虚线所示，获取满足条件的最靠后节点V3，并将其与起点VS直接相连，然后，依次判断节点V3与V4、V5、V6、V7、VE的相连路径是否满足约束条件，同时获取满足条件的最后节点Vi。如此循环，直到找到最优路径即停止。图7中经过优化后的路径为VS→V3→V5→VE，与改进八方向搜索算法得到的路径节点连线相比长度有所减短、转向角数量减少、航行方向的可选择性也相对增加，更加适用于实际航行的要求。

4 实例分析

本文以某群岛为例，在依据电子海图建立的栅格环境模型上分别对四方向搜索算法、改进的八方向搜索算法、多方向搜索算法进行测试，并与PRM路径规划算法进行对比与分析。水面无人艇的起点与终点如表1所示，为避免规划的路径与障碍物边缘距离过近，测试时将障碍物的栅格边缘均向外扩张一个长度单位；表2为四种路径规划算法所得路径的总节点数对比；图8描述了水面无人艇在不同起始点和终点的路径搜索结果。

图8 不同水域中四种搜索算法路径对比图

表1 四种路径规划算法起点终点位置汇总

表2 四种路径规划算法总节点数对比／个

在图8所示的水域图中，水面无人艇利用PRM搜索、四方向搜索、八方向搜索与多方向搜索算法都能找到一条全局避障路径。

在图8a）所示的开阔水域中由于起点与终点的直线上无障碍物并且角度为45°，所以，八方向搜索与多方向搜索规划的路径均为最短，但多方向搜索算法的节点数更少，PRM算法路径长度次之，四方向搜索规划的路径最长；在图8b）所示的小型障碍物集中水域，四方向搜索、八方向搜索与多方向搜索都具有沿边搜索的特性，其中，多方向搜索算法规划的路径最短，而PRM算法具有随机性，路径长度比八方向搜索算法更长，但路径节点更少；当无人艇的起点与终点分布在图8c）、d）所示的较为复杂的水域中，四方向搜索算法规划的路径最长并且与障碍物边缘最为贴近，PRM算法规划的路径与八方向搜索算法相比更长，但路径节点数较少，多方向搜索算法在八方向搜索算法的基础上进一步优化，规划的路径最短，路径节点数最少，转向次数也最少，更符合时间应用需求。

5 结束语

本文在传统Dijkstra算法的四方向搜索基础上设计了一种多方向路径搜索算法，通过模拟海洋环境进行无人艇路径规划测试，仿真结果表明本文所提出的多方向路径搜索算法比基本的四方向搜索算法获得的路径更短、转折角更少、转向更平滑，能够更好满足无人艇实际航行的要求，具有实际推广应用的可行性。