一种基于S57标准电子海图的水下运载器多约束航路规划方法技术

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

【技术实现步骤摘要】
一种基于S57标准电子海图的水下运载器多约束航路规划方法
本专利技术属于水下运载器三维路径规划
，具体涉及一种基于S57标准电子海图的水下运载器多约束航路规划方法。
技术介绍
电子海图是一种把需要向航海人员显示和解释的各类信息融成一体的数字海图，与简单的用颜色显示的纸海图相比,电子海图包括了更多的地理和文字信息。电子海图一般应用于一些专业的应用系统,其表示内容、数据格式和显示方式可以由用户或设计者自行定义。基于S57标准的电子海图符合《数字化海道测量数据传输标准》（S-57）标准，该标准是国际通用的，且是最具权威性的数据标准。由于基于S57标准的电子海图须按S-57标准进行特征物标编码、几何图形制作，具有标准的数据封装形式，因此保证了数据传输的正确性，有利于不同系统的兼容。电子海图包括安全航行需要的全部海图信息，也可以包含纸质海图上没有的而对安全航行认为是需要的补充信息。对于航路规划，电子海图不只提供了海域和陆地位置信息，还有警告区、倾废场、沉船等附加信息，而附加信息决定了路径规划的实用性。目前水下运载器航路规划大多把研究重点放在给定模拟环境的规划方法上，其规划环境过于简单，导致规划的航路实用性不足。基于S57标准电子海图的水下运载器三维航路规划通过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）计算插值网格数其中xs，xd，ys，yd分别为规划空间的起点和终点的经度值、纬度值；（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：其中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<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，计算三个节点构成的两条向量的本文档来自技高网...

【技术保护点】
一种基于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分别为规划空间的起点和终点的经度值、纬度值；（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，对栅格网格添加障碍物信息，即计算每个网格点是否在陆地或障碍物禁航区内，计算直接定位栅格点对应平面：n_X=floor((X(l,j)-start_X)/a_x)+1n_Y=floor((Y(l,j)-start_Y)/a_y)+1n_Z=floor((Z(l,j)-start_Z)/a_z)+1]]>其中，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方向进行编码，确定节点的位置；...

【技术特征摘要】
1.一种基于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)计算插值网格数其中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：其中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)得...

【专利技术属性】
技术研发人员：刘厂，赵玉新，金娜，沈志峰，李刚，齐昭，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23