[发明专利]一种基于改进水平集算法的时间最优航路规划方法

摘要

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

主权项

1.一种基于改进水平集算法的时间最优航路规划方法，其特征是包括如下步骤：步骤1.基于电子海图和海流信息构建三维航路规划环境空间：在三维空间中建立以左下角顶点为坐标原点，经度增加的方向为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，方程(1)改写为：当水平集函数演化至终点后，所有时间步△t的总和即为AUV的最优航行时间；窄带设置：根据窄带宽度，以初始的零水平集曲线为基准，将所有距离零水平集不超过λ的点加入零水平集的窄带范围，构造宽度为2λ的窄带；禁航区的设置：禁航区内AUV的速度为0，修改水平集演化方程(2)，在规划空间中的禁航区内令AUV的航行速度和海流速度为零，修改后的水平集方程在演化过程中顺着水平集函数的梯度演化避开禁航区；步骤4.依据步骤3建立的水平集方程，在窄带内沿着零水平集梯度方向和海流方向的矢量和方向演化水平集函数，存储每个时间步长的零水平集界面，界面由点集组成，此点集是当前时刻形成的一个可达集轮廓，初始条件下，零水平集是以栅格边长为半径的球面：某一时刻AUV能够到达的点集称为一个可达集，AUV从起点开始航行，在t时刻存在一个可达集，AUV以垂直于零水平集曲线的梯度方向继续航行到达下一个可达集，通过可达集的不断演化直至到达终点，AUV在航行过程中的速度与零水平集界面演化的速度相同，AUV运动时所受的和速度由水平集函数梯度方向的速度和海流速度组成，令X(t)代表AUV的运动轨迹，则：其中，代表零水平集上任意一点X的速度，最后，AUV的时间最优航行路径通过式(4)从终点开始后向迭代求得，步骤5.判断目标点是否在当前窄带范围内，如果不在，检测新的零水平集界面是否到达窄带边缘，如果未到达窄带边缘，则返回步骤4，否则进行步骤6；如果目标点在当前窄带范围内，检测新的零水平集界面是否到达目标点，如果未到达目标点，则返回步骤4，否则进行步骤7 ：窄带边缘检测方法如下：计算出距离窄带边缘σ的一组点，将这组点和窄带边缘点一起加入到一个数组中，时刻监视这些点的符号，位于零水平集外的点的符号为正，位于零水平集内的点的符号为负，初始时数组内点的符号均为正；当零水平集距离窄带的边缘小于σ，甚至超出窄带时，数组内的点至少存在一个点的符号为负，此时表示零水平集已经接近窄带边缘；步骤6.重构窄带，并且利用改进快速行进法重新初始化水平集函数为符号距离函数，返回步骤4 ：采用改进快速行进法作为重新初始化方法，改进的快速行进法采用八邻域的方式代替四邻域方式离散程函方程离散后的程函方程为：其中DT代表差分格式，T称为到达时间函数，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)水平集函数值，窄带内改进快速行进法重构水平集函数的具体步骤为：步骤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直至回溯到起点，从终点到起点回溯出一系列路径点，将这些路径点连接起来，输出最优路径。