[发明专利]一种基于S57标准电子海图的水下运载器多约束航路规划方法

摘要

本发明属于水下运载器三维路径规划技术领域，具体涉及一种基于S57标准电子海图的水下运载器多约束航路规划方法。本发明包括：确定规划空间和要素提取空间；提取与转换要素；航行空间生成；利用A*算法进行航路搜索；输出规划结果：将步骤四获得的规划结果存入文件。相比于大多数路径规划算法利用简单立方体模拟规划环境，本发明通过提取S57标准电子海图的数据生成三维规划环境，确保了规划环境的真实性和复杂性，进而提高了规划方法的实用性。

主权项

一种基于S57标准电子海图的水下运载器多约束航路规划方法，其特征在于：(1)确定规划空间和要素提取空间：(1.1)确定规划空间：确定规划路径的起点和终点，设为A(xa,ya,za)和G(xg,yg,zg)，确定以AG为对角线的长方体ABCD‑EFGH，即航行规划任务的理论最小空间，设为空间V0，长方体ABCD‑EFGH的边长为d，平面ABCD和平面EFGH的中心分别为O和O'，将ABCD面和EFGH面以O和O'为中心向外扩张成A'B'C'D'面和E'F'G'H'面，使扩张后平面边长d'＝d+kd,k∈[0,1]，设阈值dmin，当d＜dmin时，令d'＝dmin+kdmin航行最小深度hmin，hmin＞0，和航行最大深度hmax，hmax＞0，确定h＝hmax‑hmin，立方体A'B'C'D'‑E'F'G'H'是一个以h为高，上下平面边长为d'的长方体；(1.2)确定要素提取空间：设最大水深为h'，使要素提取空间为以h'为高，上下平面边长为dmin+k'dmin的长方体，其中k'∈[0,1]且k'＞k；(2)提取与转换要素：要素信息包括水深的经度、纬度、深度信息；点要素包括沉船、陆地区、暗礁适淹礁、倾废场、障碍物，要素信息包括点要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；线要素为陆地区，要素信息包括线要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；面要素包括陆地区、障碍物、倾废场、警告区，要素信息包括面要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；(3)航行空间生成：(3.1)计算插值网格数Ngrid=d1×d2,d1=abs(xs-xd)k1d2=abs(ys-yd)k2,]]>其中xs，xd，ys，yd分别为规划空间的起点和终点的经度值、纬度值,k1为起点和终点的经度值的宽度,k2为起点和终点的纬度值的宽度；(3.2)读取水深要素文件，提取水深要素位置数据；(3.3)通过插值获得网格水深：对步骤(3.1)获得的Ngrid个网格按位置顺序进行插值；(3.4)对插值数据进行修正：(3.4.1)叠加点要素：找到点要素所在的网格，将网格的水深更改为0；(3.4.2)叠加线要素：读取线要素中的位置点集p＝{p1,p2,...,pn}，找到每一个位置点pi所在的网格，将网格的水深更改为0，找到相邻两个位置点pi与pi+1连线经过的网格，将网格的水深更改为0；(3.4.3)叠加面要素：读取面要素中的位置点集p＝{p1,p2,...,pn}，连接点集的所有元素形成封闭图形U，以水深数据网格点为中心，以2dbuf为边长生成判断缓冲区S：dbuf=abs(pd-ps)2×divNum,]]>其中pd和ps为规划空间的起点和终点在经度或纬度方向的位置数据，divNum为水深数据网格在经度或纬度方向的网格数，计算W＝U∩S，如果W＝U∩S≠θ则修改网格点的水深值为0,θ为转向角；(3.5)修补插值失败网格：检测是否有插值失败且没有添加要素信息的区域，以检测到的网格点为中心，dbuf为半径的缓冲区搜索附近的水深值，如果缓冲区内没有水深则增加缓冲区半径使dbuf'＝kdbuf，直到搜索到水深值，如果在缓冲区内搜索到多个水深值则通过求平均数对网格点进行赋值，搜索到的一组水深点为P＝{p1,p2,....pn}，水深值Phi为第i个水深点的水深值；(3.6)将步骤(3.5)得到的网格水深写入文件以供算法规划使用；(4)利用A*算法进行航路搜索：(4.1)对水下运载器三维路径规划空间建模进一步划分规划空间，形成最初的栅格网格，经度网格数为n，纬度网格数为m，深度网格数为h，对栅格网格添加障碍物信息，即计算每个网格点是否在陆地或障碍物禁航区内，计算直接定位栅格点对应平面：其中，start_X，start_Y和start_Z是地形数据在X和Y方向上的最小值，a_x、a_y和a_z是等分网格的平均距离，通过floor函数查找节点落在地形网格的位置序号n_X、n_Y和n_Z，确定X、Y、Z方向的序号，对每个节点进行编码：NodeNum＝n×m×n_Z+n×n_Y+n_X其中NodeNum为节点编码序号，n为X方向序号的最大值，m为Y方向序号的最大值，计算出节点距离平面的距离，垂直于平面的向量栅格点为O：d=AO→·h→|h→|]]>若d＜D，则认为栅格点在禁航区内，其中D为安全距离，栅格网格形成后，对网格在X、Y、Z方向进行编码，确定节点的位置；(4.2)初始化OPEN表和CLOSED表：把遍历的节点放入一个表中保存，称为OPEN表，比较、选择后的节点放入另一个表中保存，为CLOSED表，表中的节点存有节点的信息，包括位置、启发值、父节点，子节点；(4.3)扩展子节点：(4.3.1)按照三维空间的方向关系扩展出26个节点；(4.3.2)判断并削减超过航行深度范围的节点，检测扩展子节点是否达到空间纵向的边界，最小下潜深度对应规划空间Z方向编号为z0，最大下潜深度为zn，子节点位置中Z方向的编号为zi，当zi≤z0或者zi≥zn时，删除子节点；(4.3.3)判断并削减转向角过大的节点，设当前扩展子节点为pi，pi的父节点为pi‑1，pi‑1的父节点为pi‑2，最大转向角为θmax，计算三个节点构成的两条向量的夹角的余弦cosθ：若cosθ＜cosθmax，则转向角过大，删除该子节点；(4.4)计算启发值：启发值为f(s)＝g(s)+h(s)，g(s)为从起点到当前点的实际距离，h(s)为估计值，h(s)为当前点到终点的直线距离，从父节点到扩展子节点的单位步长根据两个点之间的方位关系应设为k为垂直方向单位步长的值：g(s)=Σi=1mdih(s)=(xD-xm)×(yD-ym)×(zD-zm),]]>其中di为每个节点距离其父节点的距离，即单位步长，m代表当前子节点，D代表终点，x、y、z分别为节点在X、Y、Z方向的序号；(4.5)通过比较启发值对OPEN表按照从小到大的顺序进行排序，若新加入的节点在OPEN表和CLOSED表已经存在，则为旧节点，判断新加入OPEN表和CLOSED表中的节点的g(s)值是否小于旧节点，如果小于则更新g(s)值，同时更新父节点，如果节点还在CLOSED表中则更新节点的子节点链表；(4.6)选出OPEN表中启发值最小的节点放入CLOSED表，若为终点则停止搜索，否则，返回步骤(4.3)；(5)输出规划结果：将步骤(4)获得的规划结果存入文件。