CN111857164B - 一种融合海流模型的水下fm2*路径规划方法 - Google Patents

Abstract

本发明公开了一种融合海流模型的水下FM2*路径规划方法，首先，将每个障碍物视为波源，计算栅格T值，融合到达目标的成本计算T*值，用FMM方法对AUV进行初步的路径规划，使AUV能够找到一条路径到达目标；并且在安全距离外采用最大速度以提高AUV执行任务效率；其次参考海流模型，在航行起点调整AUV转向，使其转到期望路径方向；最后在航行过程中不断对AUV姿态进行实时调整，确保在海流影响下AUV依然能够按照期望的路线行驶。本发明能够实现安全避障，生成一条平滑适合实际航行的时间最短路径，消除海流负面影响，并利用海流，实时性更好，航行效率更高。

Description

一种融合海流模型的水下FM2*路径规划方法

技术领域

本发明涉及一种路径规划方法，特别是一种融合海流模型的水下FM2*路径规划方法。

背景技术

对于自主式水下潜器(AUV，Autonomous Underwater Vehicle)的路径规划，现已有许多方法。Dijkstra和A*算法已得到了广泛的应用，但是这两者规划的路径往往是不平滑的，不适合于实际的航行。另外，人工势场法也是应用较多的一种算法，但是人工势场法可能存在局部最小值，当路径规划中陷入局部最小值，则无法找出一条可行的路径。以上的方法往往是规划一条距离接近最优的路径，而不是时间接近最优的路径，而在实际航行中，尤其是AUV捕获目标时，需要规划一条时间最优的路径。

在地图中只要存在一条路径，则快速行进法(FMM，Fast Marching Method)一定能够找到它，且FMM方法生成的路径平滑，能够符合实际航行需要。然而FMM方法规划的路径可能会过于靠近障碍物，使AUV在航行过程中可能产生碰撞危险，而为了避障，AUV减速又可能导致时间不是最短。

此外，AUV在水下的航行极易受海流影响，使AUV可能偏移事先规划的路径航行。因此需要根据海流调整AUV方向角，尤其在初始点处，方向角的调整对于AUV的高效航行具有很大意义。

因此，如何改进现有的路径规划方法，使其能够安全避障，实现时间最短，并且消除海流负面影响，甚至利用海流航行是本发明的研究重点。

发明内容

发明目的：为了解决现有技术中AUV规划路径次优，航行速度受限及航行过程中受海流影响等问题，本发明提供了一种融合海流模型的水下FM2*路径规划方法。

技术方案：本发明所述的一种融合海流模型的水下FM2*路径规划方法，包括以下步骤：

(1)将每个障碍物视为波源，根据Eikonal方程计算栅格T值；

(2)计算栅格T*值，并根据T*对栅格排序；

(3)在安全距离外的点全部采用最大允许速度，直到到达终点；

(4)在行进过程中，融合海流模型，实时调整AUV航向。

进一步地，所述步骤(1)通过以下公式实现：

其中，计算波界面到达x位置所需的时间T，波源处T＝0，此处自由空间F(x)＝1，障碍物处F(x)＝0，则距离障碍物越远，T值越大。

进一步地，所述步骤(2)包括以下步骤：

(21)计算最佳成本T*的值：

其中，D为当前栅格到目标的笛卡尔距离，v_m为最大允许速度；

(22)根据T*对栅格排序，每次选择T*较小的栅格作为下一步的行进位置，初步规划出航行路径。

进一步地，所述步骤(3)包括以下步骤：

(31)以多个障碍物为源点扩展波形成的图可视为一个势场图，将势场缩放至AUV航行速度大小范围内，作为速度图，在AUV安全距离内的点即采用此速度图的速度；

(32)在AUV安全距离外的点全部采用最大允许速度。

进一步地，所述步骤(4)实现过程如下：

根据海流模型：

其中，(x,y)为当前位置，k为流函数中起伏的波数，c为相速度，B(t)为适当确定的海流幅度：

B(t)＝B₀+εcos(ωt+θ), (4)

其中，B₀为海流幅度初值，ε为调节参数，ω为角频率，θ为初始相位；

通过以下关系从流函数是获得速度场：

U(x,y,t)和V(x,y,t)是t时刻x,y方向速度矢量的分量，在每个t整数时刻进行更新；

期望AUV方向角为：

其中，θ为期望路线的角度，因此AUV转向角为：

γ＝α-β (8)

其中，α为当前AUV方向，β为期望AUV方向；AUV始终沿着|γ|的减小方向转向，直到γ＝0。

有益效果：与现有技术相比，本发明的有益效果：1、本发明采用基于FMM方法改进的FM2*(Fast Marching Square-Star)方法，实现了AUV更快速度的航行，规划了时间上更短的路径；2、引入了海流模型，消除海流负面影响，实现了AUV严格按照所规划路径航行。

附图说明

图1为本发明的流程图；

图2为T值大小与位置关系示意图；

图3为FM2*方法栅格图分区说明；

图4为具有多个源点的迭代波扩展栅格示意图；

图5为AUV与海流方向合成示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，一种融合海流模型的水下FM2*路径规划方法，在优化FMM方法的基础上引入海流模型，使该路径规划方法更适用于水下环境。首先将每个障碍物视为波源，由Eikonal方程计算出栅格T值，离障碍物越远，T值越大，如图2所示；其次融合AUV最大允许速度，计算出T*值，再根据FMM方法进行路径规划，即每次选择较小T*值作为下一步航行方向；在此路径中，安全距离内的点采用多源点波扩展的势场图缩放的速度，安全距离外的点全部采用最大速度航行；在航行过程中，尤其是初始时，调整AUV方向使其与海流的合速度方向指向期望的路径方向。FM2*栅格图说明如图3所示，波从源点出发往四周扩展，波前尚未到达的栅格标记为未知、在下一次迭代中成为前波的一部分的候选栅格列入窄带、波已经越过的栅格标记为冻结，其中标记为冻结的栅格的T值已经固定。在本方法中，每个障碍物都被视为波源，即波从多个源头扩展，示意图如图4所示。如图5所示，融合海流的影响，由期望方向分解计算得AUV所需转向的角度，使AUV朝着角度绝对值减小的方向转向。具体包括以下步骤：

1、将每个障碍物视为波源，由Eikonal方程计算栅格T值。

本方法基于FMM方法改进，FMM方法是水平集方法的一种特殊情况，用于跟踪和建模表示物理波界面前部的运动。例如将石子丢入池塘，将会产生向四周扩展的涟漪，因为介质始终为水，所以波的形状是圆形。若存在多种介质，那么波在不同介质中扩展的速度将不同，波到达流体中一个目标点的路径必定是时间上的最短路径。在流体中，波的扩展就可以形象地理解为FMM。

此处计算的T为波前到达空间某点的时间，在波源处T＝0，距离障碍物越远，T值越大，如图2所示。

x为位置，F(x)为波在该位置的扩展速度。对于位置x，自由空间处扩展速度F(x)＝1，障碍物处F(x)＝0。

由Eikonal方程：

可计算得T值。

如图3所示，在波源处T＝0，计算波源周围栅格的T值，求解T值较小的栅格的Eikonal方程，并标记为冻结。在此过程中，波已经到达的栅格归类到冻结区；在下一次的迭代中将成为前波的一部分的栅格所在区域称为窄带；波尚未到达的栅格标记为未知。

2、计算最佳成本T*值，并根据T*对栅格排序：

其中D为当前栅格到目标的笛卡尔距离，v_m为最大允许速度。

在计算过程中，选择T*值较小的栅格计算其周围窄带栅格的T*值，由于此处引入了成本估算，类似于A*算法对于Djikstra算法的改进，减少了计算量，可以在遍历更少的栅格的基础上找到所需路径。

3、在安全距离外的点全部采用最大允许速度，直到到达终点。

将多个障碍物视为波源扩展波，扩展过程如图4所示，参考上述方法逐步计算每个栅格的T值。由此形成的图可视为一个势场图，缩放至AUV航行速度大小范围内，即可作为速度图，即T值大小对应着AUV航行速度大小。在AUV安全距离内的点即采用此速度图的速度。因此，在接近障碍物的地方速度将非常小，因为障碍物处T＝0，所以AUV永远不会碰撞到障碍物。

在安全距离外的点全部采用AUV允许的最大速度，沿着由T*排序所得路径，以此达到整条路径的最短航行时间。

4、在行进过程中，尤其是初始航行时，融合海流模型，实时调整AUV航向。

由AUV期望航向分解为海流速度矢量和一个大小为最大允许速度的AUV期望速度矢量，计算出AUV期望速度矢量与当下AUV速度矢量之间的偏差角，使AUV朝偏差角绝对值减小的方向转向，直到偏差角为零，即转到期望速度方向。

根据海流模型：

其中，k为流函数中起伏的波数，c为相速度，(x,y)为当前位置，B(t)为适当确定的海流幅度：

B(t)＝B₀+εcos(ωt+θ), (4)

其中，B0为海流幅度初值，ε为调节参数，ω为角频率，θ为初始相位；

通过以下关系从流函数是获得速度场：

U(x,y,t)和V(x,y,t)是t时刻x，y方向速度矢量的分量，在每个t整数时刻进行更新。

根据海流速度与AUV最大允许速度的矢量合成，可得期望AUV方向角为：

其中，θ为期望路线的角度，因此AUV转向角，即偏差角为：

γ＝α-β (8)

其中，α为当前AUV方向，β为期望AUV方向；AUV始终沿着|γ|的减小方向转向，直到γ＝0。

海流与AUV速度的融合如图5所示。例如，此时AUV以最大航行速度20海里/小时航行，角度为45°；海流速度大小为2海里/小时，方向为60°；而此时的期望航向为30°。由AUV期望航向分解得海流速度矢量和一个大小为最大航行速度20海里/小时的AUV期望速度矢量。将数据代入(7)式：

则AUV需要转向的角度为γ＝α-β＝17.87°。在航行过程中不断对AUV姿态进行实时调整，确保在海流影响下AUV依然能够按照期望的路线行驶。本发明能够实现安全避障，生成一条平滑适合实际航行的时间最短路径，消除海流负面影响，并利用海流，实时性更好，航行效率更高。

Claims (4)

1.一种融合海流模型的水下FM2*路径规划方法，其特征在于，包括以下步骤：

(1)将每个障碍物视为波源，根据Eikonal方程计算栅格T值；

(2)计算栅格T*值，并根据T*对栅格排序；

(3)在安全距离外的点全部采用最大允许速度，直到到达终点；

(4)在行进过程中，融合海流模型，实时调整AUV航向；

所述步骤(4)实现过程如下：

根据海流模型：

其中，(x,y)为当前位置，k为流函数中起伏的波数，c为相速度，B(t)为适当确定的海流幅度：

B(t)＝B₀+εcos(ωt+θ), (4)

其中，B₀为海流幅度初值，ε为调节参数，ω为角频率，θ为初始相位；

通过以下关系从流函数是获得速度场：

U(x,y,t)和V(x,y,t)是t时刻x,y方向速度矢量的分量，在每个t整数时刻进行更新；

期望AUV方向角为：

其中，θ为期望路线的角度，因此AUV转向角为：

γ＝α-β (8)

其中，α为当前AUV方向，β为期望AUV方向；AUV始终沿着|γ|的减小方向转向，直到γ＝0。

2.根据权利要求1所述的一种融合海流模型的水下FM2*路径规划方法，其特征在于，所述步骤(1)通过以下公式实现：

其中，计算波界面到达x位置所需的时间T，波源处T＝0，此处自由空间F(x)＝1，障碍物处F(x)＝0，则距离障碍物越远，T值越大。

3.根据权利要求1所述的一种融合海流模型的水下FM2*路径规划方法，其特征在于，所述步骤(2)包括以下步骤：

(21)计算最佳成本T*的值：

其中，D为当前栅格到目标的笛卡尔距离，v_m为最大允许速度；

(22)根据T*对栅格排序，每次选择T*较小的栅格作为下一步的行进位置，初步规划出航行路径。

4.根据权利要求1所述的一种融合海流模型的水下FM2*路径规划方法，其特征在于，所述步骤(3)包括以下步骤：

(31)以多个障碍物为源点扩展波形成的图可视为一个势场图，将势场缩放至AUV航行速度大小范围内，作为速度图，在AUV安全距离内的点即采用此速度图的速度；

(32)在AUV安全距离外的点全部采用最大允许速度。

Images