搜索算法在无人艇航迹规划中的应用

路春晖，张博，付振楷，尹美琳，张嘉琪*

1 天津理工大学 环境科学与安全工程学院，天津 300380

2 国家塔桅产品质量监督检验中心，河北 衡水 053500

0 引 言

当派遣无人艇（USV）执行危险任务时，由于操作人员的参与程度低，人员受伤的风险可大幅降低[1-2]。进行环境监控时，在高度污染的湖泊中，可以通过无人艇来收集水样和采集数据，避免将人暴露于有害元素之中。执行灾后场所的搜救任务时，采用无人艇可以有效缩短响应时间，增加救援任务的成功率[3]。简单有效的航迹规划算法是提高任务成功率的重要保证，目前，常用的航迹规划算法主要有A*算法[4-7]、粒子群算法[8-11]、人工势场算法[12]和蚁群算法[13-14]等。

A*算法作为启发式搜索算法之一，能够快速搜索出路径，但当存在多个最小值时，不能保证搜索的路径最优。粒子群优化算法具有参数较少、容易实现等优点，但存在着收敛精度低、搜索停滞等缺点。传统人工势场法存在局部最优和目标不可达的问题。蚁群算法受起止点位置和障碍分布的影响，环境复杂时容易陷入不可行点，甚至出现路径迂回和死锁的情况。

本文将在研究一般路径算法的过程中，针对传统路径算法的思路和现有缺陷，在保证最短路径和快速搜索的前提下，结合2D 扫描思想，寻求一种新型路径规划方法。在起点和终点之间存在障碍物的情况下，通过扫描获取周围障碍物信息，以获取最短路径。利用LabView2017 平台编写仿真程序，以验证规划算法的可靠性。

1 环境模型的建立

在规划无人艇的全局路径时，首先针对已知信息进行环境空间建模[15]，然后，根据建模结果获得包含障碍物信息的搜索空间，利用具体的算法对搜索空间进行搜索。本文采用栅格法模拟障碍物。将计划搜索区域划分为相同大小的网格，原始障碍物标记为不可行区域，剩余的网格则为正常条件下的可行区域。

在利用栅格法进行空间环境建模的过程中，栅格粒度的确定影响着路径规划的准确性[16]。栅格粒度的取值如果过小，会造成环境空间的信息量过大，使系统负担过重；而栅格粒度的取值如果过大，当障碍物较多时，会导致无法找到有效路径。因此，需通过环境中障碍物的疏密度来确定栅格粒度。在计算障碍物面积时，对于不是矩形的障碍区域，对障碍物边缘进行扩充，以构成矩形区域，并将扩充部分一并算为障碍物进行考虑。

通过将障碍物矩形化来确定和计算障碍物的面积。然后利用障碍物总面积所占栅格空间总面积的比例，来确定栅格粒度l。

2 搜索扫描算法原理及优化

2.1 搜索扫描路径规划原理

首先，在系统内设定起点S(xs,ys)和目标点T(xt,yt)。由起点向目标点发射射线，射线若被障碍物阻挡，则判断起点与目标点之间存在障碍物。然后，以起点为中心向Y轴正方向发射射线，被障碍物或地图边缘阻挡后，开始顺时针进行360°扫描。此时，通过式（3）计算起点S(xs,ys)至当前扫描点F(xf,yf)的距离（也即扫描射线的长度）Df(gf,gs)。Df(gf,gs)随扫描方向的变化而不断变化。Dfp(gfp,gs)为 扫描射线长度值Df(gf,gs)前一时刻扫描点p(xfp,yfp)至 起点S(xs,ys)的扫描射线长度。

扫描射线长度计算公式为

扫描射线前一扫描点的长度计算公式为

扫描射线长度变化值计算公式为当M≥l时，判断射线扫描超出障碍物阻挡范围，在最后阻挡的栅格向扫描变化方向的下一个栅格建 立 子 节 点；当 −l＜M＜l时，继 续 扫 描；当M≤−l时，判断射线扫描到新的障碍物阻挡区域，在被阻挡栅格向扫描变化反方向的下一个栅格建立子节点。

代价函数为：

若通过比较 minP发现存在2 个或多个最小代价值点，则比较这几个点的 minH。若比较minH之后仍存在2 个或多个点，则暂时随机选取其中一个。

当某代子节点向目标点发射射线，射线未被障碍物阻挡，则判断扫描到终点，扫描结束。若最后一代子节点中，存在2 个或多个点可直接扫描到目标点，则比较 minP选取最小代价值点。若还存在多个点，则比较 minH，确定最终路径。

最终确定的路径为

2.2 算法步骤

1） 建立环境模型，确定栅格粒度，设置障碍栅格、可行栅格、起始栅格和目标栅格；

2） 从起点向终点方向进行搜索，判断扫描路径上是否有障碍；

3） 碰到障碍物后，以起点为中心进行360 度扫描；

4） 扫描过程中，当产生的扫描新向量比旧向量长度差超过1 个格子长度时，立即判断此处有缺口；

5） 在判断为缺口的位置，将被阻挡的格子，在搜索射线方向或反方向的下一个格子上建立子起点；

6） 以起点为中心扫描出第1 代子节点，并判断第1 代各子节点到目标点是否存在障碍物阻挡；

7） 判断子节点到目标点均有障碍物后，通过代价函数得出第1 代最优子节点；

8） 在第1 代最优子节点的基础上，按以上步骤扫描出第2 代子节点；

9） 按上述方法，直至扫描到终点，扫描过程结束，寻路完成（图1）。

图1 扫描算法最终路径图Fig. 1 Final path diagram of the scanning algorithm

2.3 辐射扫描算法优化

通过上述分析可以看出，搜索扫描算法可以顺利规划出可行路径。但在某些情况下，存在可优化空间。在多次实验中发现，算法所规划的路径中某些栅格可以略过，以达到减少路径长度的目的。具体优化方法：从路径第1 个栅格开始，向每个中间栅格扫描，判断是否穿过障碍物。在未穿过障碍物的前提下，计算直线路径与原始路径的长度。当直线路径长度小于原始路径长度时，将直线路径代替原始路径。以此类推，直至目标点栅格。

另外，当比较 minP后，若发现存在2 个或多个最小代价值点，则比较这几个点的 minH。若比较 minH之后仍存在2 个或多个点，暂时随机选取其中一个，则在某些情况下将会得出不同的路径。通过算法随路径进行多次规划，并通过路径长度比较出最短路径。在此基础上，进行下一轮规划，至相邻两轮规划的路径完全一样，则判断结果为最佳路径（图2）。

图2 算法优化前后路径对比图Fig. 2 Path comparison before and after algorithm optimization

改进算法后，开始分析规划次数与所用时间的关系，以及路径长度随规划次数的走势。可以看出，前期所用时间随规划次数的增多升高较快。在规划次数超过15 次之后，所用时间呈稳定增长的趋势（图3）。路径长度从第3 次开始收敛，之后趋于稳定（图4）。

3 仿真实验与实地测试

为了验证搜索扫描算法的正确性，利用Lab-View 2017 开发环境编写仿真程序。将开发的航迹规划程序与无人艇岸基站控制软件相连接，通过软件环境模拟和无人艇实地检验验证程序的可行性。将理工湖地形图导入仿真程序进行测试，仿真软件主界面如图5 所示，理工湖仿真试验如图6 所示。

图3 规划次数与所用时间关系图Fig. 3 Relationship between planned times and used time

图中红点是路径规划的起点和目标。可以看出，在理工湖仿真测试中，规划的路径避开了障碍物，得到了平稳和较短的路径。

图4 路径长度收敛趋势图Fig. 4 Path length convergence trend

图5 仿真软件界面Fig. 5 Simulation software interface

图6 理工湖仿真实验Fig. 6 Science and technology lake simulation experiment

为方便使用，在软件内部转换GPS 坐标和栅格模型的序号，因此软件的输入和输出是GPS 坐标。无需用户手动输入目标所在网格的起点和坐标。在实际情况下，无人艇的航程往往达数公里至数十公里。为了比较搜索扫描算法在远距离条件下的性能，相较蚁群算法在湖中进行了远程路径规划仿真。仿真实验中，算法的起点和目标是相同的（图7）。从图中可以看出，搜索扫描路径规划算法的表现较优。

为了验证搜索扫描算法的实地应用能力，分别在理工湖和渤海湾进行了实地测试。在理工湖测试（图8）中，无人艇能够根据软件规划的路径顺利到达终点。在渤海湾测试（图9）中，无人艇在PID 控制系统的辅助下，能够在风浪中按规划路径稳定前进。

图7 两种算法在长距离下规划路径效果Fig. 7 Two algorithms plan path effects over long distances

图8 理工湖测试图Fig. 8 Science and technology lake test

图9 渤海湾测试图Fig. 9 Bohai bay test

4 结 语

本文针对海上无人艇执行任务的特点及存在的问题，通过仿真程序对几十种不同地形图进行了仿真实验，实验结果表明，所提的搜索扫描算法能避开障碍物，在环境模拟场所规划的路径安全可行，并能提高生成路径的质量。

该改进算法主要有以下特点：1）建立了一种新的程序思路用于生成路径；2）对算法中未能实现最短路径的情况提出了新的自适应更新策略；3）通过仿真实验，验证了本文所提射线搜索算法在进一步提高二维空间中生成路径解质量的有效性。

对于无人艇的导航问题，路径规划虽不可或缺，但也不是问题的全部。路径规划为无人艇提供航线，具体如何航行还需要导航避障系统和控制器协作配合完成。本文所研究的路径规划算法仅在该领域进行了初步探讨。