CN103900573B - 一种基于s57标准电子海图的水下运载器多约束航路规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于水下运载器三维路径规划技术领域，具体涉及一种基于S57标准电子海图的水下运载器多约束航路规划方法。

背景技术

电子海图是一种把需要向航海人员显示和解释的各类信息融成一体的数字海图，与简单的用颜色显示的纸海图相比,电子海图包括了更多的地理和文字信息。电子海图一般应用于一些专业的应用系统,其表示内容、数据格式和显示方式可以由用户或设计者自行定义。基于S57标准的电子海图符合《数字化海道测量数据传输标准》（S-57）标准，该标准是国际通用的，且是最具权威性的数据标准。由于基于S57标准的电子海图须按S-57标准进行特征物标编码、几何图形制作，具有标准的数据封装形式，因此保证了数据传输的正确性，有利于不同系统的兼容。电子海图包括安全航行需要的全部海图信息，也可以包含纸质海图上没有的而对安全航行认为是需要的补充信息。对于航路规划，电子海图不只提供了海域和陆地位置信息，还有警告区、倾废场、沉船等附加信息，而附加信息决定了路径规划的实用性。目前水下运载器航路规划大多把研究重点放在给定模拟环境的规划方法上，其规划环境过于简单，导致规划的航路实用性不足。基于S57标准电子海图的水下运载器三维航路规划通过S57标准电子海图提取规划空间信息，获得的空间更加复杂、更接近真实航行环境，因此规划出的路径具有较好的实用性。

发明内容

本发明的目的在于提供一种提高了规划方法实用性的基于S57标准电子海图的水下运载器多约束航路规划方法。

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

（1）确定规划空间和要素提取空间：

（1.1）确定规划空间：

确定规划路径的起点和终点，设为A(x_a,y_a,z_a)和G(x_g,y_g,z_g)，确定以AG为对角线的长方体ABCD-EFGH，即航行规划任务的理论最小空间，设为空间V₀，长方体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]，设阈值d_min，当d<d_min时，令d'=d_min+kd_min航行最小深度h_min(h_min>0)和航行最大深度h_max(h_max>0)，确定h=h_max-h_min，立方体A'B'C'D'-E'F'G'H'是一个以h为高，上下平面边长为d'的长方体；

（1.2）确定要素提取空间：

设最大水深为h'，使要素提取空间为以h'为高，上下平面边长为d_min+k'd_min的长方体，其中k'∈[0,1]且k'>k；

（2）提取与转换要素：

要素信息包括水深的经度、纬度、深度信息；点要素包括沉船、陆地区、暗礁适淹礁、倾废场、障碍物，要素信息包括点要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；线要素为陆地区，要素信息包括线要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；面要素包括陆地区、障碍物、倾废场、警告区，要素信息包括面要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；

（3）航行空间生成：

（3.1）计算插值网格数

$N_{\mathrm{grid}}=d_1\&times;d_2,\left\{\begin{array}{c}{d}_1=\frac{\mathrm{abs}(x_s-x_d)}{k_1}\\ d_2=\frac{\mathrm{abs}(y_s-y_d)}{k_2}\end{array}\right.,$

其中x_s，x_d，y_s，y_d分别为规划空间的起点和终点的经度值、纬度值；

（3.2）读取水深要素文件，提取水深要素位置数据；

（3.3）通过插值获得网格水深：

对步骤（3.1）获得的N_grid个网格按位置顺序进行插值；

（3.4）对插值数据进行修正：

（3.4.1）叠加点要素：

找到点要素所在的网格，将网格的水深更改为0；

（3.4.2）叠加线要素：

读取线要素中的位置点集p={p₁,p₂,...,p_n}，找到每一个位置点p_i所在的网格，将网格的水深更改为0，找到相邻两个位置点p_i与p_i+1连线经过的网格，将网格的水深更改为0；

（3.4.3）叠加面要素：

读取面要素中的位置点集p={p₁,p₂,...,p_n}，连接点集的所有元素形成封闭图形U，以水深数据网格点为中心，以2d_buf为边长生成判断缓冲区S：

$d_{\mathrm{buf}}=\frac{\mathrm{abs}(p_d-p_s)}{2\&times;\mathrm{div}\mathrm{Num}},$

其中p_d和p_s为规划空间的起点和终点在经度或纬度方向的位置数据，divNum为水深数据网格在经度或纬度方向的网格数，计算W=U∩S，如果W=U∩S≠θ则修改网格点的水深值为0；

（3.5）修补插值失败网格：

检测是否有插值失败且没有添加要素信息的区域，以检测到的网格点为中心，d_buf为半径的缓冲区搜索附近的水深值，如果缓冲区内没有水深则增加缓冲区半径使d_buf'=kd_buf，直到搜索到水深值，如果在缓冲区内搜索到多个水深值则通过求平均数对网格点进行赋值，搜索到的一组水深点为P={p₁,p₂,....p_n}，水深值P_hi为第i个水深点的水深值；

（3.6）将步骤（3.5）得到的网格水深写入文件以供算法规划使用；

（4）利用A*算法进行航路搜索：

（4.1）对水下运载器三维路径规划空间建模

进一步划分规划空间，形成最初的栅格网格，经度网格数为n，纬度网格数为m，深度网格数为h，对栅格网格添加障碍物信息，即计算每个网格点是否在陆地或障碍物禁航区内，计算直接定位栅格点对应平面：

$\left\{\begin{array}{c}n\_X=\mathrm{floor}((X(l,j)-\mathrm{start}\_X)/a\_x)+1\\ n\_Y=\mathrm{floor}((Y(l,j)-\mathrm{start}\_Y)/a\_y)+1\\ n\_Z=\mathrm{floor}((Z(l,j)-\mathrm{start}\_Z)/a\_z)+1\end{array}\right.$

其中，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=\frac{\stackrel{\&RightArrow;}{\mathrm{AO}}\&CenterDot;\stackrel{\&RightArrow;}{h}}{\left|\stackrel{\&RightArrow;}{h}\right|}$

若d<D，则认为栅格点在禁航区内，其中D为安全距离，栅格网格形成后，对网格在X、Y、Z方向进行编码，确定节点的位置；

（4.2）初始化OPEN表和CLOSED表：

把遍历的节点放入一个表中保存，称为OPEN表，比较、选择后的节点放入另一个表中保存，为CLOSED表，表中的节点存有节点的信息，包括位置、启发值、父节点，子节点；

（4.3）扩展子节点：

（4.3.1）按照三维空间的方向关系扩展出26个节点；

（4.3.2）判断并削减超过航行深度范围的节点，检测扩展子节点是否达到空间纵向的边界，最小下潜深度对应规划空间Z方向编号为z₀，最大下潜深度为z_n，子节点位置中Z方向的编号为z_i，当z_i≤z₀或者z_i≥z_n时，删除子节点；

（4.3.3）判断并削减转向角过大的节点，设当前扩展子节点为p_i，p_i的父节点为p_i-1，p_i-1的父节点为p_i-2，最大转向角为θ_max，计算三个节点构成的两条向量的夹角的余弦cosθ：

$\cos \mathrm{\&theta;}=\frac{\stackrel{\&RightArrow;}{p_{i-2}{p}_{i-1}}\&CenterDot;\stackrel{\&RightArrow;}{p_{i-1}{p}_i}}{\left|\stackrel{\&RightArrow;}{p_{i-2}{p}_{i-1}}\right|\&CenterDot;\left|\stackrel{\&RightArrow;}{p_{i-1}{p}_i}\right|}$

若cosθ<cosθ_max，则转向角过大，删除该子节点；

（4.4）计算启发值：

启发值为f(s)=g(s)+h(s)，g(s)为从起点到当前点的实际距离，h(s)为估计值，h(s)为当前点到终点的直线距离，从父节点到扩展子节点的单位步长根据两个点之间的方位关系应设为k,k为垂直方向单位步长的值：

$\left\{\begin{array}{c}g\left(s\right)=\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_i\\ h\left(s\right)=\sqrt{(x_D-x_m)\&times;(y_D-y_m)\&times;(z_D-z_m)}\end{array}\right.,$

其中d_i为每个节点距离其父节点的距离，即单位步长，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）输出规划结果：将步骤四获得的规划结果存入文件。

本发明的有益效果在于：

相比于大多数路径规划算法利用简单立方体模拟规划环境，本发明通过提取S57标准电子海图的数据生成三维规划环境，确保了规划环境的真实性和复杂性，进而提高了规划方法的实用性。本发明对A*算法进行改进，使算法可以根据实际约束条件进行三维的航路规划，并且满足计算的快速性。另外，本发明将改进的A*搜索算法应用于基于电子海图的路径规划中，具有重要的实际意义。

附图说明

图1为本发明航路规划总流程图。

图2为本发明中航行空间生成总流程图。

图3为本发明提出的改进三维A*算法流程图。

具体实施方式

下面结合附图对本发明做进一步描述：

本发明属于水下运载器三维路径规划技术领域，具体涉及一种基于S57标准电子海图的水下运载器多约束航路规划方法。具体包括：确定规划空间和要素提取空间、要素提取和数据转换、航行空间生成、A*算法航路规划以及规划结果输出五个基本步骤。相比于大多数路径规划方法利用简单立方体模拟规划环境，本发明通过提取S57标准电子海图的数据生成三维规划环境，确保了规划环境的真实性，进而提高了规划方法的实用性。同时，本发明对A*算法进行改进，使算法可以在考虑约束条件的情况下进行三维航路规划，并且能够满足计算的快速性。

步骤一：确定规划空间和要素提取空间。根据水下运载器的航行任务确定航行的起点和终点，进而确定路径规划空间和要素提取空间。

步骤二：要素提取和数据转换。在要素提取空间中提取航路规划需要的要素信息，如水深、陆地、障碍物等。提取的要素数据经转换后写入文件中，供算法读取使用。

步骤三：航行空间生成。基于步骤二提取的数据通过插值算法生成基本的水深网格数据，并叠加陆地、障碍物等要素信息，最后通过修正形成适合航路规划的空间数据。

步骤四：利用A*算法进行航路规划。利用改进的A*算法进行满足规划约束条件的三维航路规划。

步骤五：规划结果输出。将规划结果存入文件，以备使用。

本发明进一步详细说明如下

步骤一：确定规划空间和要素提取空间。

航路规划需要先确定航行环境，并根据规划任务确定一个满足规划要求的最小空间，这是实现快速有效的路径规划的第一步。另外，规划空间需要环境要素信息的填充，环境要素信息的丰富程度决定了规划结果的准确性，为了获得信息量足够丰富的要素信息，还需要确定要素提取空间。具体方法如下：

步骤1.1规划空间的确定

当确定航行任务后，就可以得到规划路径的起点和终点，分别设为A(x_a,_ya,z_a)和G(x_g,y_g,z_g)，则可以确定一个以AG为对角线的立方体ABCD-EFGH，该空间是航行规划任务的理论最小空间，设为空间V₀。在实际应用时，由于环境的复杂性，规划路径往往要超出V₀，即对于实际规划空间V，有因此要基于空间V₀扩展出空间V。以正方体ABCD-EFGH为例，长方体方法类似，设正方体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]，为了保证规划空间足够大且尽量减少计算量，一般建议k取值为0.4。需要注意的是，如果起点和终点在某一维度跨度很小，会导致规划空间在该维长度过小，不符合实际情况，因此要给出d_min，当d<d_min时，令d'=d_min+kd_min。另一方面，路径规划中可能会遇到路径中段有障碍物导致路径中段的高度高于起点和终点的高度的情况，因此应有规划空间的高度h>d，设航行最小深度h_min(h_min>0)和航行最大深度h_max(h_max>0)，则可以确定h=h_max-h_min。因此扩张后的立方体A'B'C'D'-E'F'G'H'是一个以h为高，上下平面边长为d'的立方体，由此确定了航路的规划空间，算法规划的路径不会超过该空间的范围。

步骤1.2要素提取空间的确定

确定规划空间后，考虑到规划空间中每一点的信息生成是基于附近环境要素的信息提取，为了保证规划空间边缘的准确性，要素提取的空间V'应该大于规划空间V，即有因此按照规划空间的扩展办法扩展立方体A'B'C'D'-E'F'G'H'，设平面A'B'C'D'在海图上对应区域的最大水深为h'，使新的立方体为一个以h'为高，上下平面边长为d_min+k'd_min的立方体，其中k'∈[0,1]且k'>k，为了保证空间生成的精度和计算速度并满足规划的需要，一般建议k'取0.8。

步骤二：要素提取与转换

算法进行正确有效的航路规划的前提是必须要获得尽可能多的环境要素信息，如水深、陆地、沉船、警告区等。S57标准将电子海图要素类型分为水深要素、点要素、线要素、面要素等四类要素。每一类要素中与路径规划相关的要素总结如下：

（1）水深要素。要素信息包括水深的经度、纬度、深度信息。

（2）点要素。点要素包括沉船、陆地区、暗礁适淹礁、倾废场、障碍物。要素信息包括点要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息。

（3）线要素。线要素主要为陆地区。要素信息包括线要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息。

（4）面要素。面要素包括陆地区、障碍物、倾废场、警告区等。要素信息包括面要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息。

在确定要素提取空间后，通过判断要素是否与航行空间有交集，对海图信息进行要素的分类提取，并转换为数据文件，为进一步生成航行空间数据做准备。四种要素的文件格式设计如下：

（1）点要素文件内容如下：

第一行：点要素数目，如：50

第二行：第一个点要素名称经度纬度，如：灯塔123.523.4

第三行：第二个点要素名称经度纬度

……

第N+1行：第N个点要素名称经度纬度

（2）线要素文件

第一行：要素数目，如：50

第二行：第一个线要素名称位置点个数N，如：陆地区10

第三行：第一个线要素第一个位置点的经度纬度，如123.523.4

第四行：第一个线要素第二个位置点的经度纬度，如123.523.4

……

第N+3行：第二个线要素名称位置点个数

……

（3）面要素文件

第一行：要素数目，如：50

第二行：第一个面要素名称位置点个数N，如：障碍区100

第三行：第一个面要素第一个位置点的经度纬度，如123.523.4

第四行：第一个面要素第二个位置点的经度纬度，如123.523.4

……

第N+3行：第二个面要素名称位置点个数

……

（4）水深要素文件

第一行：水深要素数目，如：50

第二行：第一个水深要素名称经度纬度，如：灯塔123.523.4

第三行：第二个水深要素名称经度纬度

……

第N+1行：第N个水深要素名称经度纬度

步骤三：航行空间生成

通过步骤一和步骤二可以获得建模空间框架和空间内的要素信息数据，但是这些离散的数据点还不能构成航行空间，算法需要读入叠加有要素信息的均匀网格数据，因此需要对要素数据进行插值、叠加、修正处理，形成适合路径规划算法读取的数据形式。航行空间生成的总体思想是通过水深插值形成基本的环境数据网格，再通过添加要素信息完成障碍物的判断和添加，然后通过判断每一个插值点与危险要素的关系进行航行空间修补，最后形成叠加有要素信息的、逼近真实连续变化地形环境的数据网格。具体步骤如下：

步骤3.1计算插值网格数

插值网格的疏密决定了插值的效果，插值网格数过大，不仅不会提高插值效果还会浪费计算时间，而插值网格数过小则降低精度，达不到插值的效果。因此插值网格数的确定应考虑插值效果，而插值效果应与最小插值网格对应的实际经纬度宽度相关联，即一个插值网格对应k₁宽度的经度值和k₂宽度的纬度值，k₁和k₂的取值根据水深要素数据的丰富程度和实际规划需要决定，一般建议k₁和k₂取值在0.03°～0.1°之间。依此则可以计算出插值的网格数N_grid=d₁×d₂，公式如下：

$\left\{\begin{array}{c}{d}_1=\frac{\mathrm{abs}(x_s-x_d)}{k_1}\\ d_2=\frac{\mathrm{abs}(y_s-y_d)}{k_2}\end{array}\right.$

其中x_s，x_d，y_s，y_d分别为规划空间的起点和终点的经度值、纬度值。

步骤3.2读取水深要素文件，提取水深要素位置数据。

将水深要素从数据文件中读入内存中，供插值使用。存储水深数据的数据结构定义如下：

struct_Depth

{

double lon;//水深点经度

double lat;//水深点纬度

double depth;//水深值

}

步骤3.3通过插值获得网格水深

为了获得每个网格所在位置的水深数据，以步骤3.2提取的离散水深数据为基础，对步骤3.1获得的N_grid个网格按位置顺序进行插值，从而获得插值后的水深数据阵列，插值算法采用基于自然邻点的插值法。

步骤3.4根据危险要素对插值数据进行修正

由于海洋中还存在很多障碍物，如沉船、禁航区等，步骤3.3生成的插值数据并不能完全反映真实的海洋环境，因此需要根据步骤二提取出的不利于航行的点、线、面要素信息对步骤3.3生成的水深数据网格进行修正。修正方法为将步骤二获得的陆地、障碍物等要素叠加到步骤3.3获得的网格水深中。具体方法如下：

（1）点要素的叠加

首先找到点要素所在的网格，然后将该网格的水深更改为0，表示该网格不可航。

（2）线要素的叠加

线要素的叠加分为两步完成：首先读取线要素中的位置点集p={p₁,p₂,...,p_n}，找到每一个位置点p_i所在的网格，将该网格的水深更改为0，表示该网格不可航；然后找到相邻两个位置点p_i与p_i+1连线经过的网格，将该网格的水深更改为0，表示该网格不可航。

（3）面要素的叠加

读取面要素中的位置点集p={p₁,p₂,...,p_n}，连接该点集的所有元素形成一个封闭图形U，再以某个水深数据网格点为中心，以2d_buf为边长生成判断缓冲区S，d_buf的计算公式如下：

$d_{\mathrm{buf}}=\frac{\mathrm{abs}(p_d-p_s)}{2\&times;\mathrm{div}\mathrm{Num}},$

其中p_d和p_s为规划空间的起点和终点在经度或纬度方向的位置数据，divNum为水深数据网格在经度或纬度方向的网格数。计算W=U∩S，如果W=U∩S≠θ则修改该网格点的水深值为0，表示该网格不可航。

步骤3.5对插值失败网格进行修补

在插值过程中，不可避免会存在因可参考海图水深数据较少而造成的插值失败现象。该问题的存在不利于进行路径规划，必须进行修补。具体方法为：检测是否有插值失败且没有添加要素信息的区域，如果检测到这样的网格点则以该点为中心，d_buf为半径的缓冲区搜索附近的水深值，如果缓冲区内没有水深则增加缓冲区半径使d_buf'=kd_buf，直到搜索到水深值，如果在缓冲区内搜索到多个水深值则通过求平均数的方法对该网格点进行赋值。设搜索到的一组水深点为P={p₁,p₂,....p_n}，该点水深值其中，P_hi为第i个水深点的水深值。

步骤3.6将步骤3.5得到的网格水深写入文件以供算法规划使用。

步骤四：利用A*算法进行航路搜索

A*算法是一种启发式搜索算法，由于其对空间的探索机制非常适合于寻路，二维A*算法已广泛应用于路径规划问题。2000年，Robert J.Szczerba等提出了一种改进的A*算法，称为稀疏A*算法(SAS)。该算法根据约束条件有效削减搜索空间，实现算法快速有效的规划。A*算法的主要搜索原理为从起点出发，扩展子节点，通过比较各节点的启发值选择下一步要走的节点，直到找到终点则停止搜索。本发明以二维A*算法为基础，构建基于三维环境的栅格网格，以用户设定的约束条件削减需要遍历的子节点，构造算法启发值，实现快速有效的三维路径规划，具体步骤如下：

步骤4.1对水下运载器三维路径规划空间建模

由于通过步骤三已经得到规划空间，可根据实际需要进一步划分规划空间，形成最初的栅格网格，设经度网格数为n，纬度网格数为m，深度网格数为h，则A*算法的栅格网格为n×m×h的三维网格。接着对栅格网格添加障碍物信息，即计算每个网格点是否在陆地或障碍物等禁航区内。由于步骤三生成的环境数据也是一种网格数据，因此假设禁航区是由一定数量的单位多边形平面包裹而成的，可以先寻找栅格网格点对应的单位多边形平面，再计算栅格点到单位多边形平面的距离即可判断栅格点是否在障碍物内。由于规划空间数据中经度纬度都是由小到大顺次排列，因此可以通过计算直接定位栅格点对应平面，定位公式如下：

$\left\{\begin{array}{c}n\_X=\mathrm{floor}((X(l,j)-\mathrm{start}\_X)/a\_x)+1\\ n\_Y=\mathrm{floor}((Y(l,j)-\mathrm{start}\_Y)/a\_y)+1\\ n\_Z=\mathrm{floor}((Z(l,j)-\mathrm{start}\_Z)/a\_z)+1\end{array}\right.$

其中，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方向序号的最大值。

确定了栅格点对应的平面后，即可以计算出节点距离平面的距离。设有平面ABCD，有垂直于平面的向量栅格点设为O，则有计算公式如下：

$d=\frac{\stackrel{\&RightArrow;}{\mathrm{AO}}\&CenterDot;\stackrel{\&RightArrow;}{h}}{\left|\stackrel{\&RightArrow;}{h}\right|}$

若d<D，则认为栅格点在禁航区内，其中D为安全距离。栅格网格形成后，对网格在X、Y、Z方向进行编码，用于确定节点的位置。

步骤4.2初始化OPEN表和CLOSED表

A*算法的原理是通过遍历周围节点进行比较、选择，从而达到寻优目的。因此A*算法中把遍历的节点放入一个表中保存，称为OPEN表，而把比较、选择后的节点放入另一个表中保存，成为CLOSED表。表中的节点存有节点的各项信息，包括位置、启发值、父节点，子节点，具体结构体形式如下：

struct NODE

{

int x;//x方向的位置序号

int y;//y方向的位置序号

int z;//z方向的位置序号

int h;//当前点到终点的估计值

int g;//起点到当前点的实际长度

int f;//启发值

int NodeNum;//节点编码序号

NODE*Parent;//父节点结构体指针

NODE*Child[26];//扩展子节点结构体指针数组

};

注意，初始化时，把第一节点即起始点放入OPEN表和CLOSED表中。

步骤4.3扩展子节点。

水下运载器实际航行中，由于本身性能的限制以及航行任务的需要，会受到多种条件的约束。为了保证规划出的航路实际可用，在航路规划过程中必须考虑水下运载器的约束。本专利考虑的约束条件为最大下潜深度、最小下潜深度和最大转向角。其中，最大下潜深度是指水下运载器可以承受的最大航行深度；最小下潜深度是指任务要求水下运载器必须达到的最小航行深度；对于水下运载器而言，频繁而大角度的转向不仅会增加航行时间而且容易使其失控而产生危险，因此应尽量减少大角度的转向角。最大转向角是指为了保证航行安全而要求水下运载器可以达到的最大的转向角度。

由于水下运载器航路规划是三维航路规划，每一步扩展的子节点有26个，若考虑航行深度和最大转向角则可以根据约束条件删减扩展的子节点，大大减少搜索空间，提高搜索速度且可以使规划的路径满足实际航行要求。

子节点具体扩展方法如下：

（1）按照三维空间的方向关系扩展出26个节点。

（2）判断并削减超过航行深度范围的节点。由于在步骤1规划空间建模时利用最大下潜水深和最小下潜水深确定航行空间深度，因此可以直接检测扩展子节点是否达到空间纵向的边界即可。设最小下潜深度对应规划空间Z方向编号为z₀，最大下潜深度为z_n，子节点位置中Z方向的编号为z_i，当z_i≤z₀或者z_i≥z_n时，删除该子节点。

（3）判断并削减转向角过大的节点。设当前扩展子节点为p_i，p_i的父节点为p_i-1，p_i-1的父节点为p_i-2，最大转向角为θ_max，计算三个节点构成的两条向量的夹角的余弦cosθ。公式如下：

$\cos \mathrm{\&theta;}=\frac{\stackrel{\&RightArrow;}{p_{i-2}{p}_{i-1}}\&CenterDot;\stackrel{\&RightArrow;}{p_{i-1}{p}_i}}{\left|\stackrel{\&RightArrow;}{p_{i-2}{p}_{i-1}}\right|\&CenterDot;\left|\stackrel{\&RightArrow;}{p_{i-1}{p}_i}\right|}$

若cosθ<cosθ_max，则说明转向角过大，删除该子节点。

步骤4.4计算启发值。

A*算法的启发值公式为f(s)=g(s)+h(s)，g(s)为从起点到当前点的实际距离，h(s)为估计值，这里h(s)为当前点到终点的直线距离。相应的，从父节点到扩展子节点的单位步长根据两个点之间的方位关系应设为k,k为垂直方向单位步长的值，一般情况下取1。g(s)和h(s)的计算公式如下：

$\left\{\begin{array}{c}g\left(s\right)=\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_i\\ h\left(s\right)=\sqrt{(x_D-x_m)\&times;(y_D-y_m)\&times;(z_D-z_m)}\end{array}\right.,$

其中d_i为每个节点距离其父节点的距离，即单位步长，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。

步骤五：输出规划结果。将步骤四获得的规划结果存入文件，以备使用。

Claims (1)

1.一种基于S57标准电子海图的水下运载器多约束航路规划方法，其特征在于：

(1)确定规划空间和要素提取空间：

(1.1)确定规划空间：

确定规划路径的起点和终点，设为A(x_a,y_a,z_a)和G(x_g,y_g,z_g)，确定以AG为对角线的长方体ABCD-EFGH，即航行规划任务的理论最小空间，设为空间V₀，长方体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]，设阈值d_min，当d＜d_min时，令d'＝d_min+kd_min航行最小深度h_min，h_min＞0，和航行最大深度h_max，h_max＞0，确定h＝h_max-h_min，立方体A'B'C'D'-E'F'G'H'是一个以h为高，上下平面边长为d'的长方体；

(1.2)确定要素提取空间：

设最大水深为h'，使要素提取空间为以h'为高，上下平面边长为d_min+k'd_min的长方体，其中k'∈[0,1]且k'＞k；

(2)提取与转换要素：

要素信息包括水深的经度、纬度、深度信息；点要素包括沉船、陆地区、暗礁适淹礁、倾废场、障碍物，要素信息包括点要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；线要素为陆地区，要素信息包括线要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；面要素包括陆地区、障碍物、倾废场、警告区，要素信息包括面要素数目，名称，位置点个数以及每个位置点的经度、纬度、深度信息；

(3)航行空间生成：

(3.1)计算插值网格数

$N_{grid}=d_1\&times;d_2,\left\{\begin{array}{c}{d}_1=\frac{abs(x_s-x_d)}{k_1}\\ d_2=\frac{abs(y_s-y_d)}{k_2}\end{array}\right.,$

其中x_s，x_d，y_s，y_d分别为规划空间的起点和终点的经度值、纬度值,k₁为起点和终点的经度值的宽度,k₂为起点和终点的纬度值的宽度；

(3.2)读取水深要素文件，提取水深要素位置数据；

(3.3)通过插值获得网格水深：

对步骤(3.1)获得的N_grid个网格按位置顺序进行插值；

(3.4)对插值数据进行修正：

(3.4.1)叠加点要素：

找到点要素所在的网格，将网格的水深更改为0；

(3.4.2)叠加线要素：

读取线要素中的位置点集p＝{p₁,p₂,...,p_n}，找到每一个位置点p_i所在的网格，将网格的水深更改为0，找到相邻两个位置点p_i与p_i+1连线经过的网格，将网格的水深更改为0；

(3.4.3)叠加面要素：

读取面要素中的位置点集p＝{p₁,p₂,...,p_n}，连接点集的所有元素形成封闭图形U，以水深数据网格点为中心，以2d_buf为边长生成判断缓冲区S：

$d_{buf}=\frac{abs(p_d-p_s)}{2\&times;divNum},$

其中p_d和p_s为规划空间的起点和终点在经度或纬度方向的位置数据，divNum为水深数据网格在经度或纬度方向的网格数，计算W＝U∩S，如果W＝U∩S≠θ则修改网格点的水深值为0,θ为转向角；

(3.5)修补插值失败网格：

检测是否有插值失败且没有添加要素信息的区域，以检测到的网格点为中心，d_buf为半径的缓冲区搜索附近的水深值，如果缓冲区内没有水深则增加缓冲区半径使d_buf'＝kd_buf，直到搜索到水深值，如果在缓冲区内搜索到多个水深值则通过求平均数对网格点进行赋值，搜索到的一组水深点为P＝{p₁,p₂,....p_n}，水深值P_hi为第i个水深点的水深值；

(3.6)将步骤(3.5)得到的网格水深写入文件以供算法规划使用；

(4)利用A*算法进行航路搜索：

(4.1)对水下运载器三维路径规划空间建模

进一步划分规划空间，形成最初的栅格网格，经度网格数为n，纬度网格数为m，深度网格数为h，对栅格网格添加障碍物信息，即计算每个网格点是否在陆地或障碍物禁航区内，计算直接定位栅格点对应平面：

$\left\{\begin{array}{c}n\_X=floor\left(\right(X(l,j)-start\_X)/a\_x)+1\\ n\_Y=floor\left(\right(Y(l,j)-start\_Y)/a\_y)+1\\ n\_Z=floor\left(\right(Z(l,j)-start\_Z)/a\_z)+1\end{array}\right.$

其中，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=\frac{\stackrel{\&RightArrow;}{AO}\&CenterDot;\stackrel{\&RightArrow;}{h}}{\left|\stackrel{\&RightArrow;}{h}\right|}$

若d＜D，则认为栅格点在禁航区内，其中D为安全距离，栅格网格形成后，对网格在X、Y、Z方向进行编码，确定节点的位置；

(4.2)初始化OPEN表和CLOSED表：

把遍历的节点放入一个表中保存，称为OPEN表，比较、选择后的节点放入另一个表中保存，为CLOSED表，表中的节点存有节点的信息，包括位置、启发值、父节点，子节点；

(4.3)扩展子节点：

(4.3.1)按照三维空间的方向关系扩展出26个节点；

(4.3.2)判断并削减超过航行深度范围的节点，检测扩展子节点是否达到空间纵向的边界，最小下潜深度对应规划空间Z方向编号为z₀，最大下潜深度为z_n，子节点位置中Z方向的编号为z_i，当z_i≤z₀或者z_i≥z_n时，删除子节点；

(4.3.3)判断并削减转向角过大的节点，设当前扩展子节点为p_i，p_i的父节点为p_i-1，p_i-1的父节点为p_i-2，最大转向角为θ_max，计算三个节点构成的两条向量的夹角的余弦cosθ：

若cosθ＜cosθ_max，则转向角过大，删除该子节点；

(4.4)计算启发值：

启发值为f(s)＝g(s)+h(s)，g(s)为从起点到当前点的实际距离，h(s)为估计值，h(s)为当前点到终点的直线距离，从父节点到扩展子节点的单位步长根据两个点之间的方位关系应设为k为垂直方向单位步长的值：

$\left\{\begin{array}{c}g\left(s\right)=\underset{i=1}{\overset{m}{\&Sigma;}}{d}_i\\ h\left(s\right)=\sqrt{(x_D-x_m)\&times;(y_D-y_m)\&times;(z_D-z_m)}\end{array}\right.,$

其中d_i为每个节点距离其父节点的距离，即单位步长，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)获得的规划结果存入文件。