基于启发式搜索算法的水面自主无人艇避障策略

于琳文，叶 曦，钱同惠

（江汉大学 智能制造学院，湖北 武汉 430056）

0 引言

近年来，一种新型的现代化智能设备水面自主无人艇被广泛应用于军事领域，具有轻便灵活、反应迅速、自主能力强等优点，有较好的军用和民用应用前景。对于水面自主无人艇设备来说，如何规划出优良的路线以及在多种复杂环境下如何成功实现避障功能是目前讨论较多的方向［1-3］。水面自主无人艇自主进行路径规划并规避危险障碍物，能够大大减轻人们的工作量并降低人为事故发生的概率，目前已经被广泛应用于军事探测、搜索救援、民用捕鱼等领域［4］。

程向红等［5］利用栅格法建立地图模型，将地图划分为众多形状大小相同的小正方形，在算法方面采用蚁群算法验证了栅格法建模的有效性和可靠性，但路线规划精度并没有达到理想预期值。周宇杭等［6］介绍了A*算法并利用仿真实验验证了算法的可行性。Zhang等［7］利用栅格法建模，直观呈现了二维地图空间中路线的具体遍历问题，得到的结果真实可靠。Tan等［8］为解决移动目标与移动障碍物之间的冲突问题，采用网格法建立工作环境模型，该方法适用于动态环境，但在解决局部路径规划问题上尚未提出较好的思路。通过分析以上文献资料可知，路径规划系统承担着为水面自主无人艇决策的重任，目前国内外学者研究的路径规划又可以划分为全局路径规划和局部路径规划，局部路径规划主要是针对区域内的动态障碍物来说，使得质点能够在局部范围内规避障碍物；全局路径规划主要是针对区域内的静态障碍物来说，使质点能够遍历找到目标位置［9-11］。

本文着重研究了多类型障碍物共存情形下的全局路径规划问题，通过栅格法建立地图模型，从环境建模的角度阐述栅格法建模的优点，提高系统路径规划的精度［12-13］。改善传统的A*算法，设计新型的启发搜索函数，实验数据显示，利用MATLAB平台能够实时观测水面自主无人艇的运动轨迹变化，验证了避障策略的有效性。

1 栅格法建模

对于水面自主无人艇来说，建立环境区域模型是系统规划研究的一个首要环节。自主无人艇导航过程中，需要根据人为所建立的具体环境信息情况来构建区域模型，将所设置的多种障碍物放置到环境模型中。本文通过原始经典栅格建模方法，利用网格细分法将大方格均等地划分为多个小方格，缩小点与点之间的距离，进而提高算法的精确度。

栅格法作为建立地图模型的一种常用方法，它的主要原理是把一个运动的物体视为一个质点，将地图模型均等划分为多个密集的正方形方格，将此质点放置于建立好的正方形方格中。该方法需要将一大块地图区域进行网格划分，划分的一个重要标准是当方格填充不满时，需要将不完整方格填补为完整的单元方格。通过这种划分方式就可以将错综复杂的三维空间变成简单的二维模型，复杂的路径规划问题便可以看作从一个小方格到另一个小方格的问题，将复杂的问题做简单化处理，水面自主无人艇最终的运动路线便是依次经过的小方格。通过对环境划分，地图被划分成障碍物方格和自由方格，其中自由方格表示允许通行，障碍物方格表示不可通行，由于水面障碍物漂移作用的影响，水面自主无人艇在路线搜寻时遇到的障碍物形态会随机发生变化，此模型环境在计算机中用二进制0、1表示，其中0代表自由空间，1代表环境中不可通行区域。

本文将地图栅格细分为40×40，为了直观呈现无人艇的运动轨迹，将水面自主无人艇看作一个质点，在地图区域内布置多种不同形状、大小的障碍物模型，构建一个二维平面环境，在地图中存储无人艇的运动轨迹和障碍物等相关信息，通过实验测试得到多组水面自主无人艇搜寻路线和时间结果。为了准确模拟水面无人艇所在区域的具体位置，假设无人艇路径规划的场地为正方形，通过划分形状大小完全相等的区域，根据预先设置的质点可以运动的区域位置和大小，确定水面自主无人艇所处环境的区域大小，保证水面自主无人艇可以在已知的区域范围内遍历所有位置点并找到较优路线。模型区域的白色小圆圈代表区域环境中的障碍物，表示质点不能遍历通过的环境，其他位置的白色小网格区域表示质点能够遍历通过的环境，环境区域中小方格的划分是通过测得整片区域的长度划分成均等的40小份，使得每个小份的边长相等。

图1是建立的障碍物地图模型，方格数为40×40，水面自主无人艇与不可通行物体之间的间距为

图1 障碍物地图模型Fig.1 Obstacle map model

式中，（x i，yi）为动态水面自主无人艇运动实时方位坐标，（x j，yj）为区域中预先设置好的静态障碍物体所处的方位坐标。

2 避障策略算法

2.1 A*算法

2.1.1 算法原理 传统A*算法作为常用于车辆路径规划问题研究的一种启发性搜索算法，其原理是通过设定一个目标函数即估价函数，对区域环境下的所有小方格网络各个位置进行遍历评估，通过目标函数求得方格各个位置处的价值，对其进行评估从而判断函数的最小值，搜索到较优点，从而确定质点下一个应该到达的位置。A*算法全局路径代价为

式中，g(m)表示水面自主无人艇到达当前位置m之前质点走过的路线的总代价；h(m)表示质点从当前实时位置m到预先设置的终止位置之间还需要走的路线的总代价。通过增加h(m)的权重，可以降低h(m)对消耗路径的感应灵敏程度，从而减小无人艇的搜寻范围，减小全局路径规划的时间，h(m)的权重一般根据经验值选取。在已知的环境区域模型下，假设区域中不存在任何的障碍物，无人艇的航行没有任何干扰，此时公式中的h(m)代表开始点到目标点在直角坐标系下的距离总和，即

式中，（x1，y1）表示当前节点，（x2，y2）表示目标节点。

A*算法包含两个数据集合：一个为openlist列表，另一个为closedlist列表，openlist列表用于存储质点还没有搜索过的节点，closedlist列表用于存储质点已经搜索的节点。由（3）式可知，h(m)表示当前位置与目标位置之间的路线代价，故g(m)和h(m)的数学表达式可以根据无人艇的实时位置而定，实时位置不同，对应的质点实时坐标也不同。g(m)表达式为

式中，A i代表在区域范围内的第i个节点的位置，质点在区域中的起始位置为A0，A m表示质点在区域内的实时节点m的位置，D（A i-1，A i）代表区域内A i-1与A i两个位置之间的距离，ρi是节点（i-1）与节点i之间的环境加权系数，ρi与规划路径的易通程度与能量消耗系数有关。

2.1.2 算法流程图 传统A*算法流程图见图2。

图2 A*算法流程图Fig.2 A*algorithm flow chart

如图2所示，算法首先将起始节点存入openlist表中，计算表中所有节点的代价，选择最小代价的节点作为目标节点，计算该节点已经走过路线的代价g，将此节点作为起始节点，计算到达下一个目标节点需要的代价h，秉承质点总路径规划代价最小的原则，对openlist表中节点的g值进行更新，直至质点到达最终设置的目标节点位置，算法结束。

2.2 启发式算法改进

有障碍的搜索路径问题是在有障碍模型的情形中，按照某个或者多个判断标准，在数学上转化为求解某个目标函数极值的问题，水面自主无人艇能够成功搜寻目标位置，并且能够成功绕开区域内的所有障碍物，求解函数极小值的约束条件，目标函数数学表达式为

式中，Rn表示预设区域环境中的所有可以搜寻的位置；X表示水面自主无人艇在区域里的实时位置；gi（X）表示所要求解问题的目标值，本文通过计算搜寻路线的时间长短来判断目标函数解的好坏；W i表示权值，在这里对应的是第i个目标节点位置在整体区域环境中的权值；f(X)表示求得的目标函数值大小；minf(X)表示多次实验后求出的目标函数的最小值，在本文对应的即为搜寻路线的最优解。

A*算法在路径规划中，利用启发函数f(m)，当水面自主无人艇距离终点位置越来越近时，从当前实时节点到终止节点的估计代价h(m)便会越来越小，实时节点与起始节点的距离越来越远，意味着从起始节点到当前实时节点位置处质点走过的路线总代价越来越大，由于无人艇可以搜索环境区域中的所有节点，故质点并不会直接搜索目标节点，环境区域内的所有节点对质点都会产生干扰，使得算法在搜寻中会求解许多无效节点，花费很多不必要的时间，进而得到的路径并不是最优路径。

本文采用了一种新的启发式信息函数，如图3所示，在预先构建好的环境方格地图中，和之间的夹角为和之间的夹角为b，A、B分别为开始节点和终止节点。当算法使用曼哈顿距离作为启发信息，假设在算法求解代价函数过程中求得的C点和D点的代价值相同，此时C点相对于D点来说便可以看作干扰节点，即水面自主无人艇会对C、D两点都进行搜寻，由于D点距离目标节点更近，所以应该优先考虑D点，而C点虽然被搜索但是会成为无效节点而被放弃。

图3 不同位置的向量夹角余弦Fig.3 Cosine of vector angle at different positions

根据向量夹角余弦公式可知：

基于上述分析，设计节点m处的启发函数为

式中，hcos（m）代表节点m和开始位置构成的向量和节点m与终止节点构成的向量的夹角p的余弦值，W表示目标节点m处的权值。可以求出

由余弦角度定理可得，cosa＞cosb，hcos(C)＞hcos(D)，得fcos(D)＜fcos(C)，C点位置的代价值要高于D点，所以算法会优先考虑D点，导致C点位置不会被考虑，水面自主无人艇便会放弃C点直接去搜寻D点，从而减少全局搜寻目标的时间，对整体路径起到了优化的作用。

3 仿真实验与结果

通过仿真实验分析，可以进一步理解算法改进的思路，同时对系统仿真结果进行进一步验证。采用网格细分化操作将区域精确划分为40×40的小方格，作为水面自主无人艇路径规划的实验区域，为了将复杂问题简单化，在此进行理想化处理，在仿真实验中将水面自主无人艇、起始点位置、目标点位置等全部看成质点，通过利用x、y组成的二维坐标将所有质点显示在栅格地图中，分两步分别分析、验证算法的准确性以及改进后算法的优越性。

3.1 算法的准确性

在MATLAB仿真环境中，设置相关的质点信息，障碍物用白色圆圈表示，代表水面自主无人艇不能通过的区域，其余的栅格部分表示水面自主无人艇的自由空间。随机设置不同的障碍物形状以及大小，设置水面自主无人艇的开始位置坐标start和目标位置坐标end，为了避免实验偶然性，在不同起始节点、目标节点位置以及不同障碍物形状、大小情况下，每组情况做3次仿真实验验证，实验结果表明在不同的障碍模型下水面自主无人艇均能够找到目标。

3.1.1 不同起始位置的路径规划 在MATLAB测试中，首先测试算法的准确性，具体步骤如下：

设置开始位置坐标为（40，40），目标位置坐标为（370，370），在MATLAB仿真平台上验证实验结果如图4所示。

设置开始位置坐标为（30，30），目标位置坐标为（220，220），在MATLAB仿真平台上验证实验结果如图5所示。

图4（40，40）—（370，370）路径规划Fig.4 （40，40）-（370，370）path planning

图5（30，30）—（220，220）路径规划Fig.5 （30，30）-（220，220）path planning

比较上述两种不同情形下的路径规划图，图中的白色圆圈表示所处环境中的障碍物，彩色线条表示水面自主无人艇的运动轨迹。由图4和图5仿真结果可以看出，在障碍物形状大小已知不变的情况下，通过改变水面自主无人艇的起始节点位置坐标与终止节点位置坐标，发现水面自主无人艇能够从起始节点出发，成功绕开范围内的所有障碍物到达终止节点，能够在区域范围内规划出一条有效路径。

3.1.2 不同的障碍物的路径规划 在实际应用中，考虑到海面上的物体时刻处于漂浮状态，在水上障碍物漂移作用下，障碍物会呈现出不同的形态、大小。在本次实验中只考虑静态障碍物的情形，在预设的区域范围内随机放置多种不同种类、形状的障碍模型，用三角形、直线、梯形等形状模拟障碍物在水面的不同形态，区域范围内白色圆圈代表不同形状的障碍物类型，将原始梯形障碍物参数由1∶9调整为1∶15，从而改变梯形障碍物直角边的长度，水面自主无人艇运动路线在MATLAB仿真平台上验证实验结果如图6所示。仿真结果表明，当梯形障碍物大小发生改变时，水面自主无人艇能够重新规划出新的路径到达目标点位置。

图6 梯形障碍物大小变化时的路径规划Fig.6 Path planning when the trapezoidal obstaclesize changes

将地图模型的高度和宽度参数由5调整为10，将地图区域范围内的障碍物等比例放大，水面自主无人艇运动路线在MATLAB仿真平台上验证结果如图7所示。仿真实验分析发现，水面自主无人艇仍能够在区域范围内从起始节点到达终止节点，并且成功躲避所有障碍物规划出一条准确的路径。

图7 障碍物形状变化时的路径规划Fig.7 Path planning when obstacle shape changes

3.2 算法的优化性能对比

在二维地图模型中预先放置障碍物模型，设置障碍物的几何形状以及参数大小，设置开始位置坐标为（30，30），目标位置坐标为（390，390），对改进前后的算法分别做3次仿真实验测试结果，对比算法的优化效果。

没有对启发式函数进行改进之前，水面自主无人艇从开始位置到达目标位置的运动轨迹如图8所示。对启发式函数改进之后，水面自主无人艇从起始节点到达终止节点的运动轨迹如图9所示。从图中可以看出，算法改进前后都可以使质点规划出一条从起始节点到达终止节点的路线。

图8 算法优化前路径规划Fig.8 Path planning before algorithm optimization

图9 算法优化后路径规划Fig.9 Path planning after algorithm optimization

本文判断路径规划的好坏主要采用搜寻路线的时间长短来体现，从仿真图片上可以直观地看出算法优化前后的路径规划情况，本文算法优化前后路径规划的时间见表1。

由表1可知，设定相同的起始节点位置以及终止节点位置，发现改进前算法路径规划所用的平均时间为36.194 2 s，改进后算法路径规划所用的平均时间为35.034 2 s，结果显示算法在时间上平均可以节约1.16 s，优化后的算法消耗的时间更短，较明显地提高了算法效率。

表1 算法优化前后路径规划需要的时间Tab.1 The time required for path planning before and after algorithm optimization

4 结语

研究水面自主无人艇避障策略，往往存在无法避障或者路径不是最优的问题，本文基于改进的启发式搜索算法，采用经典栅格法建模，通过对场地内的栅格进行细分，提高系统路径规划的精度。针对传统A*算法存在的精度以及复杂程度问题，本文在原始算法的基础上提出了一种新的启发式函数。仿真实验结果表明，改进后的算法能够使水面自主无人艇成功躲避障碍物并找到目标点，通过数据对比分析得出改进后算法比改进前算法通过障碍区域耗时更短，减少了搜寻无效节点的数量，提高了工作效率。