基于自适应格网数字水深模型的水下地形匹配定位算法

刘现鹏，张立华，贾帅东，曹鸿博

（1. 海军大连舰艇学院 海洋测绘系，大连 116018；2. 海军大连舰艇学院 海洋测绘工程军队重点实验室，大连 116018）

基于自适应格网数字水深模型的水下地形匹配定位算法

刘现鹏，张立华，贾帅东，曹鸿博

（1. 海军大连舰艇学院 海洋测绘系，大连 116018；2. 海军大连舰艇学院 海洋测绘工程军队重点实验室，大连 116018）

为了解决当前的水下地形匹配定位算法未考虑格网大小随海底地形变化而自动调整的问题，提出了一种基于自适应格网数字水深模型的水下地形匹配定位算法。首先，引入基于四叉树的自适应格网模型，改进其局部格网的相似性评估指标与构网约束方法；然后，设计匹配区的确定及待匹配航迹的选取策略，给出待匹配航迹的水深值的计算方法，构建出目标匹配定位的地形相关组合算子。实验结果表明：提出的算法相比于基于等距离的规则格网模型的算法有以下优势：1）在地形特征丰富区域的定位精度明显提高；2）可以避免出现地形特征越明显匹配定位精度越低的情况；3）能一定程度地克服水深系统误差对定位精度的影响。

自适应网格；数字水深模型；地形相关；水下匹配

水下地形匹配由于具有自主、全天候，以及定位精度与航程无关等优点[1]，非常适用于水下潜器的自主导航。研究表明，影响水下地形匹配定位精度的主要因素有地形信息的丰富程度[2]、数字地形模型的精度[3]以及地形匹配导航算法的优劣[4]。

近年来，随着海洋测绘技术的发展，水下地形匹配导航逐渐成为研究的热点，国内外学者对地形匹配导航算法开展了大量的研究工作[1,5-6]，提出了地形相关匹配算法、基于扩展卡尔曼滤波和基于直接概率准则的匹配算法[4]。其中，基于扩展卡尔曼滤波和基于直接概率准则的算法分别受限于地形线性化和模型先验信息的获取，因而其适用条件比较苛刻。与之相比，基于地形相关性的算法原理简单，最为经典，并已被应用于巡航导弹的辅助导航。但目前此类算法采用的数字地形模型仍是等距离的规则格网模型[6-7]，这种模型不能随海底地形变化自动调节格网距离大小，从而限制了地形相关匹配算法的定位精度，而自适应格网能够有效地克服这一问题。同时，自适应格网在数字地形模型构建领域中已经取得了许多的成果，如自适应格网模型的抗差构建方法[8]、基于区域平均垂直不确定度的构建方法[9]和自适应格网模型精度的定量调控方法[10]，这些成果为将自适应格网模型应用于水下地形匹配定位奠定了一定的基础。

因此，本文提出一种基于自适应格网数字水深模型（Digital Depth Model, DDM）的水下地形相关性匹配算法（Terrain Correlation Matching, TCM）：首先，引入基于四叉树的自适应格网模型，改进其局部格网的精度评价指标与构网约束方法；然后，根据惯性导航系统（Inertial Navigation System, INS）的位置、航速、航向信息，设计匹配区及待匹配航迹的选取策略，给出待匹配航迹水深的求解方法，并构建一种选取最优匹配的地形相关组合算子，最终实现匹配定位。

1 水下地形匹配格网DDM的自适应构建

规则格网模型由于采用等距离剖分地形的方式进行构建，不能根据地形的复杂程度来自动调整局部格网的大小，因此这种模型可能造成对地形复杂区域的描述不够细致而对地形简单区域的描述过于繁琐的情形。为此，不少学者提出了构建自适应格网模型的方法[8-10]。

本节引入基于四叉树的自适应格网模型[10]，改进其局部格网模型的精度评价指标和构网约束方法，具体流程如图1所示。

图1给出了构建水下地形匹配格网DDM的基本流程，其中，已有算法的基本流程为图1中不包含虚线框的部分[9]。具体流程为：

① 采用距离反比平方插值算法构建原始格网，并判断格网的整体精度是否满足预设指标，若满足要求则转②，否则转流程③；

② 判断格网各局部的精度是否满足要求，若满足则认为构建的自适应格网整体精度和局部精度均达到预设指标，输出构建的自适应格网，否则转流程③；

③ 对格网进一步剖分，剖分出的子网格被存储在四叉树的子叶节点中，然后转流程②，递归循环直至构网完成。

在这一流程的基础上，本文首先提出如图1虚线框中所示的基于地形相似性的精度指标；其次，为避免因局部格网精度阈值设置的不合理而导致构网失败[9]，提出如图1中虚线框中基于地形固有信息量的局部格网精度约束改进方法。

图1 自适应格网DDM构建的流程图Fig.1 Flow chart of adaptive digital depth model

1.1 局部格网模型的精度指标改进

在构建面向水下地形匹配的自适应格网DDM时，控制好模型面与真实海底地形的相似性是关键。长期以来，这种相似性用DDM与真实海底地形差异的中误差来描述[9-10]，具体计算公式如下：

式中，zi表示在局部格网中选取的检查点水深[9]，n表示检查点个数，表示这些检查点对应位置的模型插值水深。

然而，将这种精度表示方法应用于面向水下地形匹配的自适应格网DDM构建时，仍有一些不足。如图2a所示，黑色实线O1、O2、O3、O4表示真实海底，红色虚折线P1、P2、P3、P4和蓝色虚折线P1′、P2′、P3′、P4′分别表示构建的两组DDM模型面。显然，模型面P1′、P2′、P3′、P4′与真实海底的几何相似性要高于模型面P1、P2、P3、P4，但是若采用公式(1)表示 DDM模型的精度，则模型面P1′、P2′、P3′、P4′的精度（图要低于模型面P1、P2、P3、P4的精度（图这与实际是不相符的。

为解决这一不足，本文以模型水深与原始水深差值的标准差作为相似性评估指标，如图2b所示，其具体公式为

式中：zi表示局部格网中检查点的原始水深；zi′表示这些原始水深对应位置的模型插值水深，插值算法采用双线性插值算法[9]；表示模型插值水深与对应位置原始水深的水深差；表示其平均值。

图2 模型面与真实海底地形的差异示意图Fig.2 Difference between DDM and the seabed

1.2 局部格网模型的约束方法改进

为实现格网DDM的自适应构建，需要预先设定DDM 构建的约束方法。为此，国内外学者提出了采用局部格网 DDM 精度阈值的约束方法[9]，但这些方法大多以单一固定阈值作为局部格网DDM的约束条件，势必存在一些不足：一方面，若选择的阈值过大，则会导致DDM的整体精度偏低；另一方面，若选择的阈值过小，则可能导致在部分海区的构网失败。本文在此基础上，突破传统单一固定阈值的方法，以地形标准差表示海底地形的固有信息量[11]，重点保证地形复杂区的模型精度，并采用DDM与海底地形的相似性作为评估指标，提出一种基于地形固有信息量自适应调整的局部格网DDM约束阈值的确定方法：

式中：zΩ表示局部格网精度的约束阈值；zi表示局部格网区域包含的检查点水深值；表示其平均值；n表示局部格网区域检查点的个数；k为控制模型整体精度的协调因子，该因子由构网区海底地形标准差与局部地形标准差的比给定其中，δ为根据原始水深计算的构网区地形标准差，δn为第n个局部格网区的地形标准差。

2 基于自适应格网DDM的水下地形匹配模型

2.1 匹配区的确定及待匹配航迹的选取

在地形匹配导航过程中，根据潜器的速度和航向等信息实时计算潜器的INS指示位置，从而获得其水平航迹（如图3中红色折线所示）；然后，以开始匹配定位时3倍的INS定位误差标准差为搜索范围[1]（如图3中±3σ所示虚线圆），确定匹配区的范围（如图3中虚线包围的区域）；最后，通过平移变换，在匹配区内均匀地选取m条与潜器的INS指示航迹平行的轨迹作为待匹配航迹序列。需要说明的是：在地形匹配过程中，随着INS累积误差的增大，匹配区的范围必然有所增大，本文重点探讨基于自适应格网DDM水下地形匹配定位的实现，且文献[12]表明：现代惯性导航陀螺和加速度计的精度可以分别达到 10-4(°)/h 和10-6g水平，因此在匹配过程中匹配区发散的量比较小，本文参考了文献[1]的处理方法，暂不考虑匹配区增大的问题。此外，匹配区中选取的待匹配航迹数m的数值越大，匹配区划分得越细致，匹配定位的精度也更高，因此m的数值需要根据定位任务的规划精度来确定，其中，σ表示INS定位误差标准差，0σ表示潜航器任务规划中定位需求的最低精度。

在获取这些待匹配航迹序列时，需要预先确定INS指示航迹每次移动的方位及距离。以图3中红色O点为例，该点将被平移至Oi，其中Oi为在虚线圆中均匀分布的第i个位置。为便于计算，我们将该点的采样范围由虚线圆拓展为如图3中的实线矩形。此时，匹配区的范围被扩充至图3中P1、P2、P3、P4、P5、P6包围的区域，并可得待匹配航迹序列的计算公式：

图3 匹配区域选取示意图Fig.3 Schematic diagram of the matching area

2.2 待匹配航迹的水深值计算

在获得待匹配航迹后，还需进一步求解待匹配航迹的水深序列Z′。如图3所示，图中红色、黄色方块表示的是构建自适应格网的初始格网。

式中，px、py分别表示初始格网编号，xmin、ymin表示自适应格网DDM的地理坐标下限，dx、dy分别表示初始格网x和y方向的格网距离。

然后，判断该初始格网是否被剖分，如果该格网未被剖分，则以双线性内插算法[9]计算航迹点的水深zi,j：

表1 最终格网的寻找伪代码Tab.1 Pseudo-codes used to find the last grid

2.3 实现目标定位的地形相关组合算子

用式(2)和式(3)求解待匹配航迹水深与实测水深插值的标准差，并用该标准差表示待匹配航迹水深剖面与实测水深剖面的相似性。为方便描述，我们将这种相似性的表示方法简称为 SDD算子（Standard Deviation of Depth distance, SDD）。采用SDD算子对待匹配航迹进行预处理，具体步骤为：开始水下地形匹配定位时，不断采集潜航器下方的水深信息并计算潜航周围的地形相关长度λ，待潜器航迹水深值的序列满足L=βλ时开始进行匹配定位（参照文献[13]，β的值取4），采用2.2节所提方法计算待匹配水深值序列；然后以SDD算子计算待匹配航迹水深与实测水深的相似性，并按照由小到大的顺序对这些航迹进行排序，构成航迹序列假设这些航迹为误匹配的概率分别为则前k条航迹均为误匹配的概率为

式中，m表示待匹配航迹总数。

举例说明：当k=15且m>50时，可计算得出，即假设超过50条待匹配航迹中选取的前15条航迹均为误匹配的概率非常小。因此，只需挑选适当数量的待匹配航迹数m和前k条航迹数即可保证误匹配为小概率事件。

在此基础上，考虑到SDD算法仅顾及了待匹配航迹水深与实测水深的相似性，没有考虑二者的平均水深差异，本文进一步以应用比较成熟、定位精度较高的 MSD算子[8]对排序后的待匹配航迹进行处理：从SSD算子的输出结果中选取前k条待匹配航迹，然后分别计算这些航迹水深与实测水深的MSD，将具有最小 MSD值的航迹作为最优匹配，航迹末端的地理坐标即被认为是潜航器结束匹配采样时的真实位置。其中，MSD算子的计算公式为

式中，zi表示局部格网中检查点的原始水深，表示这些原始水深对应位置的模型插值水深，n表示水深点个数。

3 实验与分析

3.1 实验区及构建的DDM

图4所示为实验区的海底地形显示效果图。该海区的经度跨度为 0.046°，纬度跨度为 0.047°，最大水深68.4 m，最小水深16.8 m。本文选择图中地形较复杂的红色矩形a与地形较简单的绿色矩形b所示的两个区域作为匹配实验区。

图4 实验区海底示意图Fig.4 Schematic diagram of the experimental seabed

对整个海区的水深数据进行处理，分别构建三组DDM，如表2所示，其中DDM1、DDM2为规则格网模型，DDM3为本文所提的算法构建的自适应格网模型，DDM1和DDM3的初始格网间距均为800m×800m，DDM2与DDM3的格网节点数基本相同。

表2 构建DDM选定的技术参数Tab.2 Technical parameters in building DDM

3.2 算法的定位精度分析

参照文献[1]，将地形匹配定位实验的基本参数设置为：INS的初始位置误差为1060 m；潜航器初始航速为3 m/s并作匀速运动；INS的东向陀螺漂移为0.85(°)/h，北向陀螺漂移为0.5(°)/h，天向陀螺漂移为0.75(°)/h；东、北、天向加速度计零篇均为1 mg；将潜航器所在位置的原始水深作为实测水深，水深采样频率为1 Hz，测量时间为330s，结合当前多波束测深系统的精度[14]，给实测水深附加2.2 m的高斯白噪声和1 m的系统误差。

在以上参数设置的基础上，分别在实验区a与实验区b中均匀地选取50个位置作为INS的初始坐标；随机给定任意航向，并保证匹配区始终落在实验区 a与实验区b中，从而在实验区a与实验区b中各设置50组实验；分别采用如表3所示的技术参数对每组实验独立匹配定位60次。

图5所示为50组实验独立进行60次匹配定位的平均定位误差，其中，图5a表示实验区a中的50组实验结果，图5b表示实验区b中的50组实验结果。

分析实验区a的定位结果，对比实验1与实验2的平均定位误差可知：通过细化DDM的格网后可以显著提高 MSD算法的定位精度，经进一步计算，其平均定位误差由681.1 m降低至169.4 m；由实验2与实验3的平均定位误差可知：通过构建自适应格网模型，可以进一步降低MSD算法的定位误差，经计算，其平均定位误差降低至139.4 m；由实验1、2、3与实验4的平均定位误差可知：使用本文提出的基于自适应格网DDM的匹配算法进行匹配定位时，其定位精度优于基于规则格网DDM的算法，SDD+MSD组合算子的定位精度普遍高于传统 MSD算子，经计算，SDD+MSD的平均定位精度约为23.6 m。

表3 匹配实验的技术参数Tab.3 Technical parameters for terrain matching

分析实验区b的定位结果可知：在地形较为简单的实验区 b进行匹配定位时，自适应格网模型在降低匹配算法定位误差的优势并不明显，甚至在某些区域的定位误差要略大于规则格网模型。这是由于实验区 b的地形较为简单，水深噪声掩盖了真实地形信息造成的。

图5 50组实验的平均定位误差Fig.5 Mean location errors in 50 sets of experiments

3.3 定位精度随地形特征变化的一致性分析

为进一步分析新算法的匹配定位能力，对该算法定位精度与地形特征的关系进行一致性分析。如图 6所示：实验1-a和实验1-b分别表示实验1在实验区a与实验区b中的50组定位结果；实验2-a和实验2-b分别表示实验2在实验区a与实验区b中的50组定位结果；实验3-a和实验3-b分别表示实验3在实验区a与实验区b中的50组定位结果；实验4-a和实验4-b分别表示实验4在实验区a与实验区b中的50组定位结果。由图6a和5d可知：实验1在实验区a中的平均定位误差普遍大于在实验区b中的平均定位误差，而实验4在实验区a中的平均定位误差要普遍小于在实验区b中的平均定位误差。这说明由于DDM1的格网间距过大，对地形复杂区的描述过于简单，未能提供准确的地形信息；与之相比，本文所提基于自适应格网模型的地形匹配算法，由于采用 SDD算子和MSD算子作为组合算子，首先由SDD算子挑选出与实测水深剖面形态相似的若干组待匹配水深剖面，然后由 MSD算子选取与实测水深剖面整体差异最小的待匹配水深剖面，并将该剖面对应航迹的末端位置作为最优匹配，从而避免了出现地形特征越明显的区域匹配定位精度越低的情况，定位精度随地形特征的变化具有更好的一致性。由图6b与图6c可知，实验2和实验3仍存在实验区a中的定位误差大于在实验区b中的定位误差的情况（如图6b和图6c中黑色虚线矩形），这有可能是由于MSD算子单纯采用是实测水深剖面与待匹配模型剖面的整体差异作为最优判别标准，从而引入了实测水深中可能存在的水深系统误差，导致匹配精度下降，为了验证这一可能，需要进一步分析水深系统误差对算法定位精度的影响。

图6 两块实验区内的平均定位误差对比Fig.6 Comparison on mean location errors in two experimental zones

3.4 水深系统误差的影响分析

为了进一步分析水深系统误差对算法定位精度的影响，设置如表4所示的验证实验。

如表4所示：实验1采用模型DDM2；实验2与实验3采用模型DDM3；实验1与实验2的水深系统误差设置为0 m，匹配算子采用MSD算子；实验3的水深系统误差设置为1 m，匹配算子采用SDD+MSD算子。实验结果如图7所示。

由图7可知，去掉水深系统误差后，三组实验的定位误差都被限定在70 m以内，定位误差都比较小。这说明系统误差确实是导致 MSD算子定位误差较大的重要因素，本文所提SDD+MSD算子能够一定程度上克服水深系统误差对定位的影响。

表4 水深系统误差影响实验选定的技术参数Tab.4 Influences of water depth systematic errors on selection of algorithms

图7 系统误差对平均定位误差影响的验证实验Fig.7 Influence of systematic error on mean location error

4 结 论

通过分析、计算及实验比对，得结论如下：

1）在相同的惯性导航系统误差条件下，本文所提的水下地形匹配定位算法在地形特征明显区域的定位精度明显提高；

2）基于自适应格网模型的新算法可避免出现地形特征越明显匹配定位精度越低的情况；

3）本文所提SDD+MSD组合算子综合考虑了实测水深剖面与DDM模型剖面的相似性和差异性，能够一定程度上克服水深系统误差对定位精度的影响。

由于文章篇幅所限，本文仅初步验证了自适应格网DDM应用于水下地形匹配定位的可行性，因而在设计实验时并未考虑INS的航向误差、速度误差在采集实测水深时的变化。此外，文中也没有针对SDD算子与 MSD算子的定位性能进行单独的探讨。这些问题都有待于在以后的研究中进一步完善。

（References）：

[1]程传奇, 郝向阳, 张振杰, 等. 鲁棒性地形匹配/惯性组合导航算法[J]. 中国惯性技术学报, 2016, 24(2):202-207.Cheng Chuan-qi, Hao Xiang-yang, Zhang Zhen-jie, et al.Robust integrated navigation algorithm of terrain aided navigation/INS[J]. Journal of Chinese Inertial Technology,2016, 24(2): 202-207.

[2]Péter F, Marco H. A universal grid map library: implementtation and use case for rough terrain navigation[J].Robot Operating System, 2016, 625: 99-120.

[3]Zhou Ling, Cheng Xiang-hong, Zhu Yi-xian. Terrain aided navigation for autonomous underwater vehicles with coarse maps[J]. Measurement Science and Technology, 2016, 27(9): 095002.

[4]张静远, 谌剑, 李恒, 等. 水下地形辅助导航技术的研究与应用进展[J]. 国防科技大学学报, 2015, 37(3):128-135.Zhang Jing-yuan, Shen Jian, Li Heng, et al. Research and application progress on underwater terrain-aided navigation technology[J]. Journal of National University of Defense Technology, 2015, 37(3): 128-135.

[5]宋子奇, 卞红雨, Zielinski Adam, 等. 基于地形图像特征与模糊决策的海底地形可导航性分析方法[J]. 中国惯性技术学报, 2016, 24(2): 164-169.Song Zi-qi, Bian Hong-yu, Adam Z, et al. Underwater terrain navigability analysis based on image processing and fuzzy decision[J]. Journal of Chinese Inertial Technology, 2016, 24(2): 164-169.

[6]Han Yu-rong, Wang Bo, Deng Zhi-hong, et al. An improved TERCOM-based algorithm for gravity-aided navigation[J]. IEEE Sensors Journal, 2016, 16(8): 2537-2544.

[7]Zhao Long, Gao Nan, Huang Bao-qi, et al. A novel terrain-aided navigation algorithm combined with the TERCOM algorithm and particle filter[J]. IEEE Sensors Journal, 2015, 15(2): 1124-1131.

[8]Chen Chuan-fa, Liu Feng-ying, Li Yan-yan, et al. A robust interpolation method for constructing digital elevation models from remote sensing data[J]. Geomorphology,2016, 268: 275-287.

[9]Jia Shuai-dong, Zhang Li-hua, Song Guo-da, et al. A method for constructing an adaptive grid digital depth model based on mean vertical uncertainty of area[J]. Acta Geodaetica Et CartographicaSinica, 2012, 41(3): 454-460.

[10]Zhang Li-hua, Jia Shuai-dong, Peng Ren-can, et al. A quantitative method to control and adjust the accuracy of adaptive grid depth modeling[J]. Marine Geodesy, 2013,36(4): 408-427.

[11]Li Ye, Ma Teng, Chen Peng-yun, et al. Autonomous underwater vehicle optimal path planning method for seabed terrain matching navigation[J]. Ocean Engineering, 2017, 133: 107-115.

[12]王巍. 惯性技术研究现状及发展趋势[J]. 自动化学报.2013, 39(6): 723-729.Wang Wei. Status and development trend of inertial technology[J]. Acta Automatica Sinica, 2013, 39(6): 723-729.

[13]李雄伟, 刘建业, 康国华. TERCOM 地形高程辅助导航系统发展及应用研究[J]. 中国惯性技术学报, 2006,14(1): 34-40.Li Xiong-Wei, Liu Jian-ye, Kang Guo-hua. Development and application of TERCOM elevation-aided navigation system[J]. Journal of Chinese Inertial Technology, 2006,14(1): 34-40.

[14]Zhao Jian-hu, Yan Jun, Zhang Hong-mei, et al. A new method for weakening the combined effect of residual errors on multibeam bathymetric data[J]. Marine Geophysical Research, 2014, 35(4): 379-394.

Underwater terrain matching algorithm based on adaptive grid digital depth model

LIU Xian-peng, ZHANG Li-hua, JIA Shuai-dong, CAO Hong-bo
(1. Department of Hydrography and Cartography, Dalian Naval Academy, Dalian 116018, China;2. Key Laboratory of Hydrographic Surveying and Mapping of PLA, Dalian Naval Academy, Dalian 116018, China)

The underwater terrain aided matching algorithm does not take into account that the grid space of reference map should automatically be adjusted with seabed terrains. To solve this problem, a new terrain matching algorithm based on adaptive Digital Grid Model is presented. First, an adaptive grid model based on quadtree is put forward to improve its local grid evaluation index and the construction method. Then, the strategy used to select the matching regions is designed, and the algorithm for calculating the track depths ready to be matched is given. Finally, the topographic correlation combination operator for object matching and localization is established. Experiment results show that the proposed method is superior to the traditional algorithm based on regular Digital Depth Model in that: 1) the new method can capture more precious location in the areas with substantial features; 2) the new method can avoid the situation that more obvious terrain features lead to more lower location precision; and 3) the new method can significantly overcome the system error’s influence on the location precision.

adaptive grid; digital depth model; terrain correlation; underwater matching

1005-6734(2017)04-0488-07

10.13695/j.cnki.12-1222/o3.2017.04.012

P229.5

A

2017-04-10；

2017-07-28

国家自然科学基金（41471380）；国家自然科学基金（41601498）

刘现鹏（1991—），男，博士研究生，从事水下地形匹配定位研究。E-mail: ouc_lxp@163.com

联 系 人：张立华（1973—），男，教授，博士生导师。E-mail: zlhua@163.com