于晖+苏延森

【摘 要】高质量的规划路径对自主式水下机器人（AUV）安全航行和高效成功完成任务起到了至关重要的作用。本文使用Fast Marching算法来解决AUV在三维大范围战场环境下的路径规划问题，并且在规划路径时考虑了碰撞障碍物和水雷的风险、被声纳发现的概率、路径长度、AUV下潜深度和转弯半径等多个因素的影响。最后，在一组水下地形高程图和声纳与水雷密度图上通过仿真实验验证所提出的算法在大范围战场环境下进行路径规划的可行性、安全性和高效性。

【关键词】自主式水下机器人；路径规划；多目标；战场；快速步进法。

【Abstract】Path planning plays an extremely important role in the operation of Autonomous Underwater Vehicles（AUVs） to accomplish the underwater task safely，successfully and efficiently.This paper introduces the Fast Marching method for the path planning of AUVs in 3D large-scale battlefield probability of being found， navigation security and low fuel consumption.The algorithm is able to find the optimal path between two waypoints in the work space and comprehensively takes factors such as the risk of collision with obstacles and mine，detection probability，sailing depth and path length into account.It also considers the maneuverability constraints of the AUV，including the safety depth and turning radius，to obtain the final flyable path.Finally，the proposed algorithm is tested in a large-scale area containing underwater terrain elevation map，sonar and mine density map to evaluate its feasibility， stability and efficiency.

【Key words】AUV；Path planning；Multi-objective；Battlefield；Fast Marching method

0 引言

AUV在军事领域的应用非常广泛[1]，主要用于执行水下侦查[2]、海底搜索和勘探[3-4]、导航援助[5]和潜艇的追踪和跟踪[6-7]等任务。对所有任务来说，路径规划是保证AUV能够安全航行和成功完成任务的一个重要环节。AUV的路径规划是指，在特定的约束条件下（水下地形、动态障碍物、路径长度、能源消耗等），找出一条从起始状态到目标状态的无碰平滑最优路径。本文以AUV在大范围复杂战场水下环境下执行军事任务作为研究背景，所执行的任务是在一些特定约束条件下，使用AUV航行去一个偏远位置执行侦查任务。在制定执行军事侦查任务的运动规划中，需要考虑一组内部约束条件（例如到达时间、燃料消耗、转弯半径、航行深度）和外部约束条件（地形、被敌方声纳探测到的概率、避开水雷）的限制。

Fast Marching方法是由Sethian首先提出的用来进行图像处理的一种解决波传播问题的水平集方法[8]，已经直接用于解决移动机器人的路径规划问题[9-11]，但是对于三维动态复杂水环境下AUV的路径规划，Fast Marching方法已有的研究还存在如下的问题：1）大多是在二维或者2.5维环境下进行路径规划。2）环境建模简单，障碍物很少并且为简单、规则的形状，而像水雷、敌方声纳等给 AUV执行任务带来威胁的因素都没有考虑。3）没有考虑 AUV下潜的安全深度和燃料消耗等约束，因此规划出的路径可能不可达。

本文将使用Fast Marching算法在三维大范围复杂战场水环境下对AUV进行路径规划，并且考虑了安全性、隐蔽性、任务效率、可操控性等约束条件。主要结构如下：首先介绍FM方法的基本原理，以及在三维环境下的应用。然后介绍复杂战场环境下各约束因素对AUV所执行任务的影响，以及代价函数的建立。接下来根据不同的任务要求使用Fast Marching算法对AUV执行任务的航路进行规划，并评价规划结果。最后给出本文小结。

1 三维空间Fast Marching方法

Fast Marching方法是一种基于离散网格的数值近似方法[8]。与传统的基于离散网格的算法相同，Fast Marching算法的路径规划过程也分为两步：探索过程，即建立整个地图上每个网格顶点的距离函数值；开发过程，即通过所求解到的最优代价值，从目标点向起始点回溯形成最优路径。

在自然界中，有一种物理现象与Fast Marching算法的探索过程非常相似：光的传播。假设在起始点有一个向四周发射光波的光源，那么光从起始点向目标点传播的路径（即光线轨迹）可以认为是机器人路径规划生成的最优路径。因此我们可以根据光的传播现象来建立距离函数。光在传播的过程中，在某一个瞬时时刻所到达的所有点的轨迹称为波前（形成光波的等相面）。计算初至时间T（x，y，z）可以用來描述波传播过程中的波前位，可通过呈函方程来描述：

其中D-和D+分别表示后向和前向差分算子。

利用公式（2）计算每个网格单元距离函数值的一阶近似估计之后，再通过从目标点的梯度下降方向进行回溯，形成一条从起始点到目标点的最优路径。Fast Marching算法更新过程的细节请参考文献[8]。

2 代价函数

本节讨论AUV在三维大范围复杂战场水环境下执行军事任务需要考虑的约束条件，以及通过这些约束条件构造Fast Marching算法的代价函数。

2.1 决策目标

在AUV执行军事侦察任务的路径规划中，需要考虑一组内部约束条件（例如到达时间、燃料消耗、转弯半径、航行深度）和外部约束条件（地形、被敌方声纳探测到的概率、避开水雷）的限制。我们通过这些约束条件生成每个网格单元的费用函数来表示 AUV 穿越该单元的难度。每一种约束都给出了潜在的数据来源、数据存储结构和对 AUV 运动规划的影响。针对所提出的任务背景，在对 AUV 进行运动规划时主要考虑如下四个决策目标：1）安全性；2）隐蔽性；3）任务效率；4）AUV的可操控性。

（1）安全性目标：在 AUV 的运动规划过程中，路径的安全性是需要重点考虑的约束。在 AUV 执行军事侦查任务时，在水雷区域航行和与静态障碍物或动态航行器发生碰撞会影响AUV 的安全性。因此在仿真实验中，安全目标由避碰规则和在水雷区域航行的风险性组成。

避碰规则要求 AUV 在航行过程中与静态障碍物和动态航行器之间保持安全距离。由于从导航传感器、环境感知传感器和环境电子图所获得的环境信息存在着不确定性和不精确性，这样会造成 AUV 的定位和环境的信息存在误差。这种不确定性会增加与障碍物碰撞的风险，并且离障碍物越近的单元风险越大。如果环境模型的单元尺寸很大，则这种不确定性可以被忽略。在仿真实验中，为了使所规划的路径与静态障碍物保持一定的安全距离，可以通过生成费用矩阵来表示与静态障碍物的碰撞风险。在距离静态障碍物一定范围内的区域，其对应的费用矩阵的值与到静态障碍物最短距离有关。离静态障碍物越近，穿越的费用越大；离静态障碍物越远，穿越的费用越小。而对于动态障碍物（比如其它执行任务的AUV和船只），可以将其它 AUV 建模为一个动态的球形障碍物，将船只建模为一个动态圆柱形障碍物。

在 AUV 执行任务时要尽量避免在水雷密集的区域航行，以减小碰撞水雷的风险。虽然碰撞水雷不会直接造成人员的伤亡，但是这也意味着任务的失败，并且有可能会暴露任务目标。在仿真实验中，碰撞水雷风险性的费用函数用该区域内水雷的密度表示。

（2）隐蔽性目标：AUV 的隐蔽性是其在战场环境中执行任务的一个最重要的战术指标之一，而声隐蔽是最重要的一个影响因素。如果不进行声隐蔽，AUV 有可能会被敌方声纳发现，从而会导致任务失败，甚至不可估算的损失。因此，在 AUV 的航路规划过程中需要考虑在执行任务时被敌方声纳侦查到的概率。不同的声纳有不同的探测范围。在仿真实验中，声纳的探测范围用一个圆柱形表示，圆柱形内部区域的费用为其外部区域费用的两倍，而AUV被该声纳发现的概率可以用通过该威胁区域的路径段长度表示[12]。

（3）任务效率目标：航路规划也需要考虑任务本身效率目标的最优化。这些目标包括航行时间和燃料消耗。假设AUV单位时间的能耗是常数，也就是能量消耗是正比于航行时间，则能量消耗和航行时间两个目标等价。因此在 AUV的任务效率费用矩阵中，障碍物所在区域为不可通过，其它区域的费用值反比于AUV穿越该区域的速度。

（4）可操控性约束：为了提高所规划航路的可达性，需要考虑 AUV 的可操控性，包括最小转弯半径和安全深度。因此，所规划路径的曲率半径的下确界必须大于 AUV 的最小转弯半径，并且所穿越区域的最大深度必须在安全深度之上。Fast Marching算法规划的路径已经非常平滑，一般可以满足 AUV 对转弯半径的要求。如果所规划的路径曲率半径的下确界没有达到要求，可以使用均值滤波或增加补偿的方式通过改变总费用函数来改善，详细内容见文献[10]。但是为了不改变总费用函数的值所代表的每个决策目标的意义，使用增加补偿的方式更加合适。

2.2 代价函数矩阵

在AUV的航路规划中，需要满足并且最优化上述提到的所有决策目标，但是要使所有目标都达到最优是不可能的，因为某些目标就是矛盾的，比如安全性和到达时间。通常的做法是使用加权求和法或者模糊映射法。

碰撞风险、水雷风险、隐蔽性和燃料消耗等目标分别用费用矩阵Cr、Mr、C和Fc表示，并且全部进行归一化处理。这四个矩阵的阶数与网格单元矩阵相同，每个元素代表环境模型上对应网格单元的费用函数。三维网格模型每个单元的总费用函数W是通过加权求和法将这四个目标融合在一起得到，则所生成的路径代价为：

最后，如果某些约束因素（例如地形、障碍物、禁航区域）是需要避免的，则对应的这些网格单元的总费用函数为∞。为了达到仿真的目的，假设所有的环境信息都是已知的。

3 仿真实验

仿真模型包含了水底地形图、水雷密度图和敌方声纳探测范围等信息。首先需要对水底地形环境进行建模。本算法通过高度图对水底地形环境进行描述，如图1所示。然后使用规则网格对整个搜索空间进行建模，则其它环境信息（比如水雷密度图和声纳探测区域）也可以通过网格单元表示。

所有的仿真实验都按照下面的步骤进行：

1）分别计算每个网格单元的碰撞风险费用Cr、水雷风险费用Mr、反侦察费用C和燃料消耗费用Fc。

2）根据任务要求通过经验选择每个决策目标的加权系数，并通过费用函数生成每个顶点的费用？棕。

3）选择起始点和目标点。

4）通过Fast Marching算法計算每个网格单元的距离函数值，直到达到目标点停止。

5）通过从目标点距离函数梯度下降的方向进行回溯，直到到达起始点。最优路径形成。

6）评价路径质量。

下面根据不同任务要求改变总费用函数W中加权系数？棕i的值，进行了几组路径规划的仿真实验。所有仿真实验使用的环境模型、水雷密度图、声纳探测范围、其它动态航行器的运动状态以及起始点、目标点都是相同的。起始点和目标点分别位于一个山脉的两边。圆柱形表示声纳的探测区域，轴心在水平坐标（110NM，170NM）处，探测半径为5NM ，探测高度为 200M。水雷密度图使用颜色图表示。

首先给出航行时间为最优的路径规划仿真实验的结果。图2和图3给出了从起始点到目标点航行时间最短的路径。由图可见，所生成的路径越过山脉，穿越高密度水雷区域和敌方声纳探测区域，直接到达目标点。该路径的评价结果如表 1所示，其中Ttotal为路径总航行时间，Lsonar表示在敌方声纳探测区域内路径段的长度，Lobs为路径靠近障碍物的最短距离，Rmin表示路径曲率半径的下确界，Dmax表示整条路径的最大深度。由表可见，该路径离障碍物非常近，有很大的碰撞风险，并且穿越声纳区域的路径段很长，被敌方发现的概率很大。

如果任务的要求仅需要保障 AUV 的安全性，要求所生成的路径尽可能的远离障碍物，并且要绕开高密度的水雷区域，则我们构建总费用函数为W=0.2*Cr+0.8*Mr。生成的安全性最优的路径如图4和图3所示。表1给出了该路径的评价结果。与航行时间最优的路径相比较，所生成的路径远离了障碍物，并且绕开了高密度水雷区域。

当仅考虑 AUV 执行军事任务的隐蔽性，也就是费用函数 W = C，生成了另一条最优路径，如图5和 图3 所示。生成的路径完全绕开了敌方声纳的探测区域。

最后，制定路径规划的目标如下：Ttotal？燮13h，Lsonar？燮1NM，最大下潜深度Dmax<150M，曲率半径的下确界Rmin？叟10M，在满足上述要求前提下，尽量选择与障碍物距离远和水雷密度较小的区域穿越。通过经验设定费用函数为W=0.1*Cr+0.5*Mr+0.1*C+0.4*Fc时，生成的最优路径如图6和图3所示。表1给出了该路径的属性参数。

需要注意，当任务需求发生改变，可通过修改费用的权重？棕i来生成一条满足约束条件的最优路径。

4 结论

本文将Fast Marching算法用于解决AUV在大范围复杂战场环境下的路径规划问题。在进行路径规划时考虑了安全性、隐蔽性、任务效率和可操控性四个决策目标。实验结果表明，根据每个决策目标在任务中的优先级顺序和它们对任务的影响程度建立合理的费用函数，可以规划出满足任务要求的最优路径。

【参考文献】

[1]Fletcher B.UUV master plan：a vision for navy UUV development.in： Proceedings of OCEANS 2000 MTS/IEEE Conference and Exhibition.IEEE，2000，65-71.

[2]Le Bouffant N，Pidsley P，Malkasse J P，et al.Automatic MCM mission control for AUV systems.in：Proceedings of Oceans 2005-Europe.IEEE，2005，930-936.

[3]Smith WH，Marks K M.Sea floor in the Malaysia Airlines Flight MH370 Search Area.Eos，Transactions American Geophysical Union，2014，95（21）：173-174.

[4]Kohut J，Roarty H，Licthenwalner S，et al.The Mid-Atlantic regional coastal ocean observing system：Serving coast guard needs in the Mid-Atlantic Bight.in： Proceedings of US/EU-Baltic International Symposium，2008 IEEE/OES.IEEE，2008，1-5.

[5]Freitag L E，Grund M，Partan J，et al.Multi-band acoustic modem for the communications and navigation aid AUV.in：Proceedings of OCEANS，2005. Proceedings of MTS/IEEE.IEEE，2005，1080-1085.

[6]Wernli R L.Low Cost UUVs for Military Applications：Is the Technology Ready？ Technical report，DTIC Document，2000.

[7]Nguyen C，Samuel R，Nguyen H，et al.Unmanned Systems Network-Centric Operations.Technical report，DTIC Document，2005.

[8]Sethian J A.Level set methods and fast marching methods：evolving interfaces in computational geometry，uid mechanics， computer vision， and materials science[M].（volume 3）.U.K.：Cambridge university press，1999.

[9]Petres，Clement and Pailhas， Yan and Patron，Pedro and Petillot，Yvan and Evans， Jonathan and Lane，David，“Path planning for autonomous underwater vehicles”， IEEE Transactions on Robotics，23（2）：331-341，2007.

[10]G′omez，Javier V and Lumbier，Alejandro and Garrido，Santiago and Moreno， Luis，“Planning robot formations with fast marching square including uncertainty conditions”，Robotics and Autonomous Systems，61（2）：137-152， 2013.

[11]Garrido，Santiago and Malfaz，Mar′ ？覦a and Blanco，Dolores，“Application of the fast marching method for outdoor motion planning in robotics”，Robotics and Autonomous Systems，61（2）：106-114，2013.

[12]Lin T ZH.Research on submarine counter-detection probability by threat surface ship sonar.Computer Science，2009.

[責任编辑：田吉捷]