一种基于改进水平集算法的时间最优航路规划方法

摘要

本发明提供的是一种基于改进水平集算法的时间最优航路规划方法。1.构建三维航路规划环境空间；2.初始化水平集函数为符号距离函数，将AUV航行的起点设置在零水平集上；3.构建考虑海流对AUV航行的影响的水平集演化方程，并在起点处构造窄带，设置禁航区；4.依据步骤3建立的水平集方程演化水平集函数，存储每个时间步长的零水平集界面；5.判断目标点是否在当前窄带范围内；6.重构窄带，并且利用改进快速行进法重新初始化水平集函数为符号距离函数；7.利用后向迭代方程，获得AUV的时间最优路径，输出最优路径。本发明利用海流再分析数据库生成海流场，在路径规划中充分考虑海流的影响，使得规划出的路径具有很强的实用性。

说明书

技术领域

本发明涉及的是一种水下自主航行器(AUV)的三维路径规划方法。

背景技术

AUV在水下进行航路规划时，除了要考虑路径长度、障碍物信息、路径的平滑程度之外，还需要考虑海流要素的影响。海流的速度和方向会对水下航行器的航行时间和能量消耗产生影响，尤其是当航行器速度较慢时，对其进行航路规划时务必要考虑海流对航行器运动的影响。尽量使航行器顺流航行，充分利用海流场的推动作用，减小AUV逆流航行过程中带来的能量损耗，规划出一条能量损耗较小且时间最优的路径。近年来，国内外很多学者对考虑海流影响的AUV航路规划方法进行研究。例如，王超，朱大奇等人在《基于人工势场与速度合成的AUV路径规划》一文中提出了一种基于人工势场法考虑海流影响的AUV路径规划方法。该方法的优点是解决了海流对AUV航行时间的影响，减小了能量的损耗。缺点是算法在动态的海流环境下会失效。Soulignac等人在《Feasible and Optimal PathPlanning in Strong Current Fields》一文中提出了滑动波前扩展法，该方法的优点是通过结合适当的代价函数与连续优化技术，保证了路径的有效性和全局最优性。缺点是该方法没有应用到三维情况。

S.Osher和J.A.Sethian于1988年提出了水平集方法(Level Set)。水平集方法的主要思想是将移动变形的曲线(面)作为零水平集嵌入到更高一维的函数中，由封闭超曲面的演化方程可以得到函数的演化方程，而嵌入的封闭曲线(面)总是保持为函数在零水平截面上的点集，最终只要获得演化函数在零水平截面上点集的位置，即可得到移动变形曲线(面)的演化结果。水平集方法提出以来已经被成功地应用于物理学、流体力学、材料科学、计算机图形学等多个领域，尤其在图像领域取得了巨大的发展。Ron Kimmel等人最早应用水平集方法在三维空间中寻找两点间的最短路径，此后，水平集方法在路径规划中开始得到关注。以往的航路规划算法大多数只考虑静态海流的影响，在动态的环境下效果不理想，使得这些算法的应用范围有限，而水平集方法能够很容易的从二维空间扩展到三维空间并很好的应用于动态流体运动，特别是在水下航路规划中取得了很好的效果。因此，在水下航路规划中，将水平集作为基础的航路规划算法具有良好的应用前景。

发明内容

本发明的目的在于提供一种速度快、精度高，充分考虑了海流的影响，规划出的路径具有很强的实用性的基于改进水平集算法的时间最优航路规划方法。

本发明的目的是这样实现的：

步骤1.基于电子海图和海流信息构建三维航路规划环境空间；

步骤2.初始化水平集函数为符号距离函数，将AUV航行的起点设置在零水平集上，并设置AUV的航行速度；

步骤3.构建考虑海流对AUV航行的影响的水平集演化方程，并在起点处构造窄带，设置禁航区；

步骤4.依据步骤3建立的水平集方程，在窄带内沿着零水平集梯度方向和海流方向的矢量和方向演化水平集函数，并存储每个时间步长的零水平集界面；

步骤5.判断目标点是否在当前窄带范围内，如果不在，检测新的零水平集界面是否到达窄带边缘，如果未到达窄带边缘，则返回步骤4，否则执行步骤6；如果目标点在当前窄带范围内，检测新的零水平集界面是否到达目标点，如果未到达目标点，则返回步骤4，否则转步骤7；

步骤6.重构窄带，并且利用改进快速行进法重新初始化水平集函数为符号距离函数，返回步骤4；

步骤7.利用后向迭代方程，获得AUV的时间最优路径，输出最优路径，路径规划结束。

本发明还可以包括：

1、所述水平集演化方程为：

其中(x,y,z)代表三维空间中某点在X轴、Y轴和Z轴的坐标，t代表水平集函数φ(x,y,z,t)到达坐标(x,y,z)的时间，AUV的运动速度大小为F，航向为海流场为V(x,y,z,t)。

2、所述窄带的宽度为22。

3、所述设置禁航区是在禁航区内将AUV的航行速度和海流速度设置为0。

4、所述改进快速行进法是采用八邻域的方式离散程函方程，同时采用双快速行进的方法，通过插值法对零水平集界面进行插值得到两条初始的零水平集同时行进。

为了解决基于传统水平集的路径规划算法中存在的速度较慢和精度不高的缺陷，本发明提出了一种基于改进水平集算法的AUV航路规划方法。本发明利用海流再分析数据库生成海流场，在路径规划中充分考虑海流的影响，使得规划出的路径具有很强的实用性。

本发明的有益效果是：

1.本发明通过修改水平集方程将海流模型结合到水平集方程中，在海流模型的基础上，规划出的路径以航行时间为最优目标且能够充分地利用海流，即使在复杂海流存在的情况下，同样能够有效的找到一条可行路径。

2.本发明提出的基于改进水平集算法的AUV航路规划方法，不仅能够充分利用海流的影响，而且通过简单的修改水平集方程能够很好的避开禁航区，这也是该算法的主要优势。

3.本发明利用改进水平集算法进行AUV全局路径规划，相比于传统水平集算法规划速度更快，规划精度更高，同时能够很好的应用于三维空间路径规划，实用性强，规划出更接近实际航行路线的AUV航行路径。

附图说明

图1为本发明所述考虑海流影响的AUV路径规划流程图。

图2为本发明所述改进快速行进法流程图。

具体实施方式

下面举例对本发明做进一步说明。

本发明是一种考虑海流影响的AUV三维路径规划方法，主要包括环境建模、水平集建模、重新初始化水平集函数、获得最优路径等关键内容，具体实现流程见图1，包括以下：

步骤1.基于电子海图和海流信息构建三维航路规划环境空间

为了真实模拟水下AUV的航行环境，对AUV航行的真实环境进行建模。在三维空间中建立以左下角顶点为坐标原点，经度增加的方向为X轴正方向，纬度增加的方向为Y轴正方向，水深增加的方向为Z轴负方向的三维空间坐标系。将空间划分为M×N×L个网格。网格划分原则为：X轴和Y轴的网格间距不大于0.02°，Z轴的网格间距不大于10米。

从电子海图中提取航行区域内的水深数据，并利用克里金插值算法将离散水深数据处理成规则网格数据；从海洋环境数据库中提取提取航行区域内的的海流数据。

步骤2.初始化水平集函数为符号距离函数，将AUV航行的起点设置在零水平集上，并设置AUV的航行速度

在三维空间中，设定一个起点X和一个终点Y，X、Y为位置向量。假设AUV总是存在于零水平集界面上，初始化水平集函数φ(x,y,z,t＝0)为符号距离函数，其零水平集界面为以栅格间距为半径的球面。

步骤3.构建考虑海流对AUV航行的影响的水平集演化方程，并在起点处构造窄带，设置禁航区

令T(Y)代表AUV从起点X到达终点Y的时间，AUV的运动速度大小为F，航向为海流场为V(x,y,z,t)，初始化的水平集函数φ(x,y,z,t)由差分方程(1)演化:

其中(x,y,z)代表三维空间中某点在X轴、Y轴和Z轴的坐标，t代表水平集函数φ(x,y,z,t)到达坐标(x,y,z)的时间。

当AUV到达终点Y时φ(Y,T(Y))＝0。为了保证为AUV从起点X到达终点Y的最短时间，从而得到一条时间最优路径，应保证AUV的航向始终垂直于零水平集曲线的梯度方向。因此方程(1)可以改写为：

其中x,y,z代表三维空间中某点在X轴、Y轴和Z轴的坐标，t代表水平集函数φ(x,y,z,t)到达坐标(x,y,z)的时间，F代表AUV的航行速度，V(x,y,z,t)代表三维海流场的速度，φ(x,y,z,t)代表水平集函数。

由方程(2)可知，当AUV的航行速度始终垂直于水平集函数的梯度方向时，AUV的航行时间由水平集函数的演化速度决定，当水平集函数演化至终点后，所有时间步Δt的总和即为AUV的最优航行时间。

窄带设置：根据窄带宽度，以初始的零水平集曲线为基准，将所有距离零水平集不超过λ的点加入零水平集的窄带范围，构造宽度为2λ的窄带。

禁航区的设置：出于安全性的考虑，AUV在航行过程中需要避开某些区域，比如障碍物，此区域即为禁航区，禁航区内AUV的速度为0，此时可以修改水平集演化方程(2)，在规划空间中的禁航区内令AUV的航行速度和海流速度为零，因此修改后的水平集方程在演化过程中顺着水平集函数的梯度演化即可避开禁航区。

步骤4.依据步骤3建立的水平集方程，在窄带内沿着零水平集梯度方向和海流方向的矢量和方向演化水平集函数，存储每个时间步长的零水平集界面。界面由点集组成，此点集是当前时刻形成的一个可达集轮廓。初始条件下，零水平集是以栅格边长为半径的球面

某一时刻AUV能够到达的点集称为一个可达集。AUV从起点开始航行，在t时刻存在一个可达集，AUV以垂直于零水平集曲线的梯度方向继续航行到达下一个可达集，通过可达集的不断演化直至到达终点。

AUV在航行过程中的速度与零水平集界面演化的速度相同，AUV运动时所受的和速度由水平集函数梯度方向的速度和海流速度组成，令X(t)代表AUV的运动轨迹，因此有下式：

其中x,y,z代表三维空间中某点在X轴、Y轴和Z轴的坐标，t代表水平集函数φ(x,y,z,t)到达坐标(x,y,z)的时间，代表零水平集上任意一点X的速度，F代表AUV的航行速度，代表航向，V(x,y,z,t)代表三维海流场的速度，φ(x,y,z,t)代表水平集函数。

最后，AUV的时间最优航行路径可通过式(4)从终点开始后向迭代求得。

其中x,y,z代表三维空间中某点在X轴、Y轴和Z轴的坐标，t代表水平集函数φ(x,y,z,t)到达坐标(x,y,z)的时间，代表零水平集上任意一点X的速度，F代表AUV的航行速度，代表航向，V(x,y,z,t)代表三维海流场的速度，φ(x,y,z,t)代表水平集函数。

步骤5.判断目标点是否在当前窄带范围内。如果不在，检测新的零水平集界面是否到达窄带边缘，如果未到达窄带边缘，则返回步骤4，否则进行步骤6；如果目标点在当前窄带范围内，检测新的零水平集界面是否到达目标点，如果未到达目标点，则返回步骤4，否则进行步骤7

窄带边缘检测方法如下：计算出距离窄带边缘σ的一组点，将这组点和窄带边缘点一起加入到一个数组中，时刻监视这些点的符号。因为位于零水平集外的点的符号为正，位于零水平集内的点的符号为负，所以初始时数组内点的符号均为正。当零水平集距离窄带的边缘小于σ，甚至超出窄带时，数组内的点至少存在一个点的符号为负，此时表示零水平集已经接近窄带边缘。

步骤6.重构窄带，并且利用改进快速行进法重新初始化水平集函数为符号距离函数，返回步骤4

本发明采用改进快速行进法作为重新初始化方法，改进的快速行进法采用八邻域的方式代替传统的四邻域方式离散程函方程|▽T|F＝1，提高了算法精度。离散后的程函方程为：

其中DT代表差分格式，T称为到达时间函数，h代表网格间距，F代表行进速度。

同时，由于传统快速行进法只能单向传播，重新初始化水平集函数的时间较长。为了加快窄带内水平集函数的快速重构，采用双快速行进的方法，通过插值法对零水平集界面进行插值得到两条初始的波前同时行进，进一步提高算法的效率，具体处理流程见图2。

应用八邻域求解中心点的前提是每一维度的网格间距相等，设h代表网格间距，则利用八个邻近点方式求解程函方程的方法如下：

设存在一阶算子和将其带入程函方程(5)中可得：

同样存在一阶算子和将其带入程函方程(5)中可得：

当点(i,j)周围邻域点只有点(i,j-1)或者点(i-1,j)单一邻接点，且对角点存在时，则利用式(6)或者式(7)更新点(i,j)水平集函数值。当点(i,j)周围邻域点同时存在点(i,j-1)和点(i-1,j)两个邻接点，且两邻接点相邻的对角点存在时，则利用式(8)更新点(i,j)水平集函数值。

改进快速行进法中采用的数据结构定义如下：

结构体Coordinate：主要存储算法行进过程中，点的位置坐标(横坐标和纵坐标)以及标志位(标识点是否处于零水平集内部)。

结构体Node：存储待处理的更新点的位置信息和水平集函数值。该结构体包含位置坐标和走时值两个成员变量。位置坐标类型为Coordinate，走时值代表当前更新点的距离值。

C++标准模板类Vector<Node>：代表一个优先队列，存储待处理的更新点。

已知点Accepted：类型为map<Coordinate，double>。该变量存储对零水平集插值后的初始外部波前点和初始内部波前点。其中键存储的是初始波前点的坐标和标识信息，值存储的是当前点的水平集函数值。

更新点tentative：类型为map<Coordinate，Node*>。该变量存储零水平集外部和内部的待更新点。其中键存储的待更新点的坐标和标识信息，值存储的是当前点的位置信息和水平集函数值。

窄带内改进快速行进法重构水平集函数的具体步骤为：

步骤6.1初始化：

a)通过最邻近插值法对零水平集曲线插值，同时得到零水平集外部的初始波前点和零水平集内部的初始波前点，此时这些点的水平集函数值已经得到，标记这些点为已知点，同时设置标志位，标志已知点是否为零水平集内部点。

b)遍历检查每个已知点的四个邻域点，如果此点标记不是已知点，则记此点为更新点，通过方程(8)计算更新点此时的水平集函数值，同时通过已知点标志位，确认更新点是否位于零水平集内部。将所有的更新点加入一个优先队列中，并依据更新点的水平集值升序排列；

步骤6.2外部和内部同时推进：

a)输出优先队列中的首元素φ_min(i,j)，标记φ_min(i,j)为已知点，并从优先队列中删除；

b)依次检查φ_min(i,j)的四邻域点(i-1,j)，(i+1,j)，(i,j-1)，(i,j+1)。如果原来标记为已知点或者更新点则忽略此邻域点，维持水平集函数值不变。否则，若该邻域点的水平集函数值不大于窄带宽度，将该邻域点记为更新点，根据其周围邻点的情况，由方程(8)求得该邻域点的水平集函数值，并将该点加入到优先队列的尾部；若该邻域点的水平集函数值大于窄带宽度，则忽略该邻域点，继续检查下一个邻域点，当φ_min(i,j)的四个邻域点均检查完，检查优先队列是否为空，若为空，停止推进，转到步骤3.3，否则转到a)步骤。

步骤6.3此时，窄带内所有点均为已知点，根据已知点的标志位输出零水平集内外点新的水平集函数值。

步骤7.利用后向迭代方程(4)，获得AUV的时间最优路径，输出最优路径，路径规划结束

当零水平集界面第一次到达终点Y时，首先确定终点Y是否在此时刻的零水平集界面上，若Y不在零水平集界面上，则确定终点Y在零水平集曲线上的投影，记为Y₁，从点Y₁开始以式(9)反向计算最优路径。

具体步骤如下：

步骤7.1.在t时刻，寻找距离终点Y最近的三个点构成一个三角平面，根据点和平面的位置关系，确定终点Y在零水平集界面上的投影Y₁。

步骤7.2.求取投影Y₁垂直于零水平集界面的垂向如果Y₁在零水平集界面上，则垂向为三角平面的垂直方向；如果Y₁不再零水平集平面上，则垂向为距离Y₁最近的点周围三角平面垂向的平均值。

步骤7.3.计算

步骤7.4重复步骤7.1到步骤7.3直至回溯到起点，这样从终点到起点能够回溯出一系列路径点，将这些路径点连接起来，输出最优路径。