基于格网拓扑结构迭代最佳环域点提高水下导航精度方法与流程

1.本发明属于水下导航学、海洋测绘学等交叉技术领域，尤其涉及一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法。


背景技术：

2.惯性导航系统(inertial navigation system，ins)是实现水下潜器自主导航的核心导航系统，具有短时高精度定位特性，但惯性元器件的固有误差和定位解算的多次积分等却导致ins误差随时间累积发散而难以满足潜器长航时高精度的定位目标，因此，ins需要进行定期校准以保持导航精度。
3.作为地球固有地理属性之一的重力场信息不易受气候、海浪等不确定环境影响并表现出长时间相对稳定性，故重力场信息适合被用于辅助导航，目前重力辅助导航系统作为水下辅助ins导航的一项重要技术并已成为国内外学者研究的国际热点议题。
4.匹配算法是重力辅助惯导系统的核心，目前常见的重力匹配算法主要包括桑迪亚惯性地形辅助导航算法sitan、迭代最近等值线点算法iccp和地形轮廓匹配算法tercom。相较而言，tercom算法以计算简单、对初始误差不敏感、鲁棒性强、定位精度较高等优点而得到学者的广泛关注和研究。
5.在提高tercom算法匹配精度方面，liu等提出一种由坐标变换、误匹配检测和卡尔曼滤波相组合的新型ins/tercom系统优化结构，并研究其分散式与自适应联邦滤波信息融合算法的匹配性能；yan等通过整合tercom和iccp而提出一种新的匹配算法，以tercom得到初始位置并以iccp进行精确定位；yuan等以tercom/iccp算法融合kalman滤波而提出组合水下辅助导航算法，同时精确匹配精度采用滑动窗口以提高算法效率；wang等基于tercom算法而提出旋转拼接型重力匹配算法；tong等以二维高斯基函数对局部重力基准图逼近且以拟牛顿bfgs非线性寻优方法解算相关极值匹配模型而提出基于局部重力图逼近的组合匹配算法，同时以tercom的粗匹配和以差分法的实测数据预处理来提高算法的匹配精度；zhao等将tercom算法与粒子滤波相组合而提出一种新的地形辅助导航算法以增强bitan ii算法的定位精度；wei等基于加权递减迭代而提出一种相关sitan算法以解决初始误差和线性误差问题，同时将tercom用于算法的相关性处理过程；zhang等为解决tercom算法易受高程测量误差、地形相似性等影响的误匹配问题而提出一种相关面内多参照点联合概率误匹配在线判断准则；wang等在海斗数据支持下通过tercom算法实现地球海洋最深处的无声纳定位；zhang等利用tercom算法仿真分析得出潜器航速、测深精度、初始位置偏差、水下地形特征、数字地图分辨率等主要因素对匹配精度的影响规律。在改善tercom算法匹配效率和可靠性方面，han等融合最短路径算法和新相关分析算法而构建一种改进tercom算法，同时通过空间顺序约束和决策准则限制整合机制而提出一种误匹配诊断方法下的新匹配算法，以提高tercom的可靠性和匹配精度；刘现鹏等以ins输出的速度和航向信息追踪潜器航迹而提出一种基于航迹线追踪的terpm定位算法；李钊伟等基于粗
‑
细匹配策略而提出一种新型分层邻域阈值搜索法以改善tercom算法逐点遍历搜索的匹配效率；li等通过球面几何
的最短弧原则和空
‑
海环境下姿态控制理论相耦合而提出一种新的基于测地线方法以缩小匹配区域尺度并改善算法的匹配效率；zhang等以tercom为线匹配算法以几何相似性进行面匹配而提出线面组合的水下地形匹配算法，以提高算法的稳健性与定位精度。
6.综上所述，大部分学者主要围绕tercom的应用和提高水下潜器导航性能展开研究，而鲜有关于改变tercom匹配格网拓扑结构方面的研究工作。但囿于传统tercom算法不依赖于航迹起点位置信息而仅以航迹终点处ins误差3倍作为半边长张成方型格网匹配点阵，并遍历搜索以确定水下潜器位置的最佳匹配定位，但该种搜索机制运算量大而易导致算法匹配效率低；此外，tercom算法难以有效处理观测噪声和过程噪声且对惯导航段的角度误差敏感等缺点，因此，需要进一步探讨不同于terrcom匹配格网拓扑结构的设计问题。


技术实现要素：

7.本发明的技术解决问题：克服现有技术的不足，提供一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法和系统，旨在提高水下潜器重力辅助导航的匹配精度。
8.为了解决上述技术问题，本发明公开了一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法，包括：
9.通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置；
10.根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点；
11.迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正ins系统控制参数并辅助完成水下潜器长航时长航距的航行目标。
12.在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，航迹起点小环域格网匹配定位策略的构建流程如下：
13.获取某段水下潜器航行按采样时间间隔δt的ins航迹输出惯导序列{s1,s2,
…
,s
l
}；其中，l表示ins航迹的采样序列长度，s1、s2、
…
、s
l
表示惯导序列中的各航迹点；
14.从{s1,s2,
…
,s
l
}中提取得到各航迹点对应的位置坐标和实测重力值，构建得到位置坐标序列{(x1,y1),(x2,y2),
…
,(x
l
,y
l
)}和水下潜器实测重力值序列{g1,g2,
…
,g
l
}；
15.以惯导序列中的航迹起点s1的位置坐标(x1,y1)为中心，以3σ0作为最大外延的搜索边界半径，构建得到小环域；其中，σ0表示采样间隔为δt时的惯导漂移误差的标准差；
16.根据北偏东旋转角度θ0和环半径比例均分系数λ，确定小环域内的待匹配格点的总数目和位置坐标，构建得到小环域待匹配格网点集。
17.在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，小环域内各待匹配格网点的位置坐标的计算公式如下：
[0018][0019]
其中，和分别表示第i个小环上第j个格点的横纵坐标，r
i
表示小环域内第i个
环的半径，β
j
表示小环域各环上第j个格点的旋转角度。
[0020]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，
[0021]
r
i
＝3σ0λi
[0022]
β
j
＝j
·
θ0[0023]
其中，i＝1,2,
…
，且i的最大值且j的最大值
[0024]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置，包括：
[0025]
将航迹起点s1的位置坐标(x1,y1)和小环域内各待匹配格网点的位置坐标作为航迹起点真实位置的待匹配点集；
[0026]
逐点将航迹起点真实位置的待匹配点集映射到重力基准图上，并按最近重力基准格点处的重力值作为待匹配点重力值，得到(x1,y1)对应的理论重力值和对应的理论重力值
[0027][0028]
其中，c表示重力基准图的格网分辨率，mapt(
·
,
·
)表示重力基准图按格点位置的重力值矩阵，[
·
]表示四舍五入取整；
[0029]
按重力偏差绝对值最小化原则，得到水下潜器航迹起点的最佳匹配位置
[0030][0031]
其中，当i＝0、j＝0时，即为x1、即为y1。
[0032]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，航迹终点变角度三层环域匹配定位策略的构建流程如下：
[0033]
根据惯导序列中的航迹起点s1的位置坐标(x1,y1)和航迹终点s
l
的位置坐标(x
l
,y
l
)，确定得到水下潜器航行的航向信息和航距信息：
[0034]
[0035][0036]
其中，d
ins
表示以h∈[0， ∞)范数刻画惯导航迹起
‑
终点间的距离度量，设h＝2，即计算欧氏距离；α
ins
表示以坐标系的横坐标为正方向的航向弧度角；
[0037]
根据以及水下潜器航行的航向信息和航距信息，估算得到大环域的中心位置坐标(x
o
,y
o
)：
[0038][0039]
以(x
o
,y
o
)为中心，以r
max
作为最大外延的搜索边界半径，构建得到覆盖航迹真实终点的变角度三层拓扑结构环型格网点区域，记作大环域；
[0040]
以重力基准图的格网分辨率c作为大环域各环跨度间隔，得到大环域的总环数
[0041]
将重力基准图的格网分辨率c与中间环的半径之比作为大环域内各格点生成的基准角按“内倍外半”原则，确定大环域内层环上相邻格点间的跨度角为外层环上相邻格点间的跨度角为并结合总环数m，构建得到大环域待匹配格网点集；其中，大环域待匹配格网点集为变角度三环层格点拓扑结构。
[0042]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，大环域待匹配格网点的生成方式如下：
[0043]
将1σ
‑
epmp、2σ
‑
epmp、3σ
‑
epmp三种机制下大环域的总环数分别记作其中，σ表示惯导累积漂移误差标准差；
[0044]
对分别进行修正，得到修正后的三种机制下的大环域的总环数m
′1、m
′2、m
′3：
[0045][0046]
其中，ξ＝1,2,3，分别对应1σ
‑
epmp机制、2σ
‑
epmp机制、3σ
‑
epmp机制；
[0047]
根据修正后的三种机制下的大环域的总环数m1′
、m2′
、m3′
，得到三种机制下大环域各环的半径r
1,j
、r
2,j
、r
3,j
：
[0048]
r
ξ,ζ
＝ζc，ζ＝1,2,
…
,m
′
ξ
···
(6)
[0049]
其中，r
ξ,ζ
表示第ξ种机制下的大环域的第ζ环的半径；
[0050]
根据修正后的三种机制下的大环域的总环数m
′1、m
′2、m
′3，得到三种机制下大环域的中间环半径
[0051][0052]
根据三种机制下大环域的中间环半径得到三种机制下大环域内各格点生成的基准角
[0053][0054]
根据(5)～(8)，得到不同机制下的大环域待匹配格网点。
[0055]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，
[0056]
1σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的计算公式如下：
[0057][0058]
2σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的计算公式如下：
[0059]
[0060]
3σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的计算公式如下：
[0061][0062]
在上述基于格网拓扑结构迭代最佳环域点提高水下导航精度方法中，迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，包括：
[0063]
确定第ξ种机制下大环域的内
‑
中
‑
外环层上的待匹配格网点的总数目n
ξ
；
[0064]
将第ξ种机制下大环域的第r个待匹配格网点作为待评估航迹终点，记作点其中，r∈{1,2,
…
,n
ξ
}；
[0065]
将点在重力基准图中对应的位置(x
r
,y
r
)与重力基准图的格网分辨率c相比，并按四舍五入原则得到重力基准图上最近邻于点的格网点base
l
；
[0066]
将格网点base
l
对应的重力值作为点重力值的近似；
[0067]
根据格网点base
l
在重力基准图上的位置坐标、水下潜器航行的航速和航向，提取得到点对应的重力图航迹序列及对应的最近邻重力序列
[0068]
将与水下潜器实测重力值序列{g1,g2,
…
,g
l
}进行对比，并计算匹配效能评价指标，记作msd
r
；
[0069]
依次计算得到第ξ种机制下大环域的内
‑
中
‑
外环层上的各待匹配格网点的匹配效能评价指标，得到第ξ种机制下大环域的内
‑
中
‑
外环层上的所有待匹配格网点的匹配效能评价指标集和{msd
r
|r＝1,2,
…
,n
ζ
}；
[0070]
基于{msd
r
|r＝1,2,
…
,n
ζ
}，按最优原则筛选得到大环域范围内水下潜器航迹终点的最佳匹配位置
[0071]
相应的，本发明还公开了一种基于格网拓扑结构迭代最佳环域点提高水下导航精
度系统，包括：
[0072]
解算模块，用于通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置；
[0073]
生成模块，用于根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点；
[0074]
迭代确定模块，用于迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正ins系统控制参数并辅助完成水下潜器长航时长航距的航行目标。
[0075]
本发明具有以下优点：
[0076]
为突破传统重力匹配算法固有格网结构的限制并改善水下重力匹配导航精度，本发明提出一种新型格网拓扑结构迭代最佳环域点法(ioap)，其原理如下：首先，根据惯导起点位置和漂移误差及旋转角等构建航迹起点的小环域格网的匹配定位策略，通过小环域格网匹配点的匹配比较，得到航迹起点的最佳匹配定位并增强算法对初始位置误差的不敏感性；其次，利用航迹起点最佳匹配位置，再结合惯导航向航距信息、累积漂移误差等构造航迹终点变角度三层环域的匹配定位机制，以生成环型拓扑结构的环域匹配点；最后，迭代计算环域匹配点的匹配指标并按最优原则获得环域范围内航迹终点的最佳匹配位置。
[0077]
综合考虑匹配精度统计指标、平均匹配时间和匹配成功率等作为匹配优劣的分析依据，验证了本发明所述的方法在不同惯导累积误差倍数或基准角环半径下的匹配性能差异性及其良好鲁棒性。
[0078]
此为，通过对不同区域起点且终点落于不同重力区间的航迹进行重力匹配测试比较，证明了：本发明所述的方法具有匹配精度高、不同重力区段定位适用性强等优点，其平均匹配精度和最差匹配精度分别相对于tercom算法最高提升了40.39％和72.16％。
附图说明
[0079]
图1是本发明实施例中一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法的步骤流程图；
[0080]
图2是本发明实施例中一种基于spmp策略的小环域待匹配点分布示意；
[0081]
图3是本发明实施例中一种基于epmp策略的大环域待匹配点分布示意；
[0082]
图4是本发明实施例中一种研究区域卫星遥感及局部放大区域的重力异常分布示意；其中，4(a)为卫星遥感图，4(b)为重力异常基准图；
[0083]
图5是本发明实施例中一种不同σ准则下重力匹配算法的匹配测试效果对比示意图；其中，5(a)为tercom算法，5(b)为1σ
‑
ioap算法，5(c)为2σ
‑
ioap算法，5(d)为3σ
‑
ioap算法；
[0084]
图6是本发明实施例中一种不同定位精度下4种算法成功匹配概率的柱状对比图；
[0085]
图7是本发明实施例中一种tercom算法匹配位置与真实位置对比及其格网点的类别划分示意图；其中，7(a)为tercom算法100次测试，7(b)为tercom算法匹配位置点分类；
[0086]
图8是本发明实施例中一种不同σ
‑
ioap算法的匹配位置与真实位置对比示意图；其中，8(a)为1σ
‑
ioap算法100次测试，8(b)为2σ
‑
ioap算法100次测试，8(c)为3σ
‑
ioap算法100次测试；
[0087]
图9是本发明实施例中一种不同环半径基准角下ioap算法的匹配效果对比示意图；其中，9(a)为tercom算法，9(b)为1
‑
ioap算法，9(c)为1.5
‑
ioap算法，9(d)为2
‑
ioap算法，9(e)为2.5
‑
ioap算法；
[0088]
图10是本发明实施例中一种不同环半径基准角下ioap算法匹配成功概率的柱状对比图；
[0089]
图11是本发明实施例中一种不同航迹起点下算法匹配定位效果的对比示意图；其中，11(a)为tercom算法(航迹起点a)，11(b)为tercom算法(航迹起点b)，11(c)为tercom算法(航迹起点c)，11(d)为1.5
‑
ioap算法(航迹起点a)，11(e)为1.5
‑
ioap算法(航迹起点b)，11(f)为1.5
‑
ioap算法(航迹起点c)。
具体实施方式
[0090]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。
[0091]
本发明基于tercom算法匹配点阵的结构布局以及ins系统的漂移误差特性，提出了一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法，简称ioap算法，其实现原理如下：为降低ioap算法对初始误差的敏感性，以惯导起点位置为中心并以一定漂移误差和旋转角张成小环域匹配格点，构建环型拓扑结构的航迹起点小环域匹配定位策略，并按重力偏差绝对值最小化原则得到起点最优匹配位置；根据航迹起点的最佳匹配位置，再结合惯导航向航距信息得到大环域匹配格网的中心位置，再基于惯导累积漂移误差等以确定大环域匹配格网环数，并按中间环半径的格点基准偏转角和“内倍外半”原则，得到匹配格网点环形分布的拓扑结构，并构建航迹终点变角度三层环域的匹配定位策略，通过格网点提取重力图序列再与实测重力序列间计算均方差并匹配比较，按均方差最小化原则得到航迹终点的最佳匹配位置。
[0092]
如图1，在本实施例中，该基于格网拓扑结构迭代最佳环域点提高水下导航精度方法，包括：
[0093]
步骤101，通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置。
[0094]
在本实施例中，考虑到sitan和iccp等重力匹配算法对初始误差的相对敏感性问题，同时为一定程度上改善ioap算法对重力匹配初始误差的不敏感性(鲁棒性)，构建了航迹起点小环域格网匹配定位策略/机制(matching positioning strategy of the tracking starting point in small ring domain，spmp)。其中，spmp策略以惯导指示水下潜器位置为中心并以一定的漂移误差和旋转角度张成一个小型依概率覆盖航迹真实起点位置的待匹配的小环形拓扑结构格网点区域(记为小环域)，再通过待匹配点重力匹配评价指标的最优原则以确定出航迹起点的最佳匹配位置。
[0095]
优选的，航迹起点小环域格网匹配定位策略的构建流程可以如下：
[0096]
获取某段水下潜器航行按采样时间间隔δt的ins航迹输出惯导序列{s1,s2,
…
,s
l
}；从{s1,s2,
…
,s
l
}中提取得到各航迹点对应的位置坐标和实测重力值，构建得到位置坐标序列{(x1,y1),(x2,y2),
…
,(x
l
,y
l
)}和水下潜器实测重力值序列{g1,g2,
…
,g
l
}。其中，l表示ins航迹的采样序列长度，s1、s2、
…
、s
l
表示惯导序列中的各航迹点。
[0097]
则，spmp策略下，以惯导序列中的航迹起点s1的位置坐标(x1,y1)为中心，以3σ0作为最大外延的搜索边界半径，确定小环域。进一步的，根据北偏东旋转角度θ0和环半径比例均分系数λ，可以确定小环域内的待匹配格点的总数目和位置坐标，进而构建得到小环域待匹配格网点集。其中，σ0表示采样间隔为δt时的惯导漂移误差的标准差。例如，当θ0＝45、λ＝1/3时，spmp策略所张成的小环域待匹配格网点集的分布如图2所示。
[0098]
进一步的，根据上述参变量的设置情况，可得小环域内各待匹配格网点的位置坐标的计算公式如下：
[0099][0100]
其中，和分别表示第i个小环上第j个格点的横纵坐标；r
i
表示小环域内第i个环的半径，r
i
＝3σ0λi；β
j
表示小环域各环上第j个格点的旋转角度，β
j
＝j
·
θ0；i＝1,2,
…
，且i的最大值且j的最大值
[0101]
则，水下潜器航迹起点的最佳匹配位置的确定流程可以如下：
[0102]
将航迹起点s1的位置坐标(x1,y1)和小环域内各待匹配格网点的位置坐标作为航迹起点真实位置的待匹配点集；逐点将航迹起点真实位置的待匹配点集映射到重力基准图上，并按最近重力基准格点处的重力值作为待匹配点重力值，得到(x1,y1)对应的理论重力值和对应的理论重力值
[0103][0104]
为确定水下潜器航迹起点的最佳匹配位置，可按重力偏差绝对值最小化原则，得到水下潜器航迹起点的最佳匹配位置
[0105][0106]
其中，c表示重力基准图的格网分辨率，mapt(
·
,
·
)表示重力基准图按格点位置的重力值矩阵，[
·
]表示四舍五入取整；当i＝0、j＝0时，即为x1、即为y1。
[0107]
需要说明的是，小范围多格点进行重力基准图映射时可能会出现多模态现象，本实施例按第一个重力偏差绝对值最小格点予以匹配，当然也可按随机选择机制得到航迹起点的最佳匹配，本实施例对此不作限制。
[0108]
由上可见，spmp策略可依概率实现水下潜器航迹起点的有效匹配定位，并弱化ioap算法对初始误差的敏感性，同时为下一步基于惯导航向航距信息引导的航迹终点位置匹配定位提供航迹起点的位置信息基础。
[0109]
步骤102，根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点。
[0110]
在本实施例中，考虑到惯导航迹序列蕴藏着较好的短时高精度航向和航距信息，结合spmp策略所得的航迹起点最佳匹配位置可进一步推得航迹终点待匹配域的中心位置o；再基于惯导系统的漂移误差统计特性而构建新的待匹配格网点环形分布的拓扑结构及其匹配定位策略(航迹终点变角度三层环域匹配定位策略/机制，matching positioning mechanism of the tracking ending point in three
‑
layer loop domain，epmp)，epmp策略以惯导累积漂移误差标准差σ、重力基准图的格网分辨率c和中间环的半径三者间的相对关系，得到环形覆盖区域的最大环数和匹配格网点间的偏转角并张成一个大的依概率覆盖航迹真实终点位置的待匹配的变角度三层拓扑结构环型格网点区域，记为大环域；再按各格网点坐标位置提取出匹配航迹的重力图序列再与水下潜器的真实重力序列比对并按评价指标最优原则确定出航迹终点的最佳匹配位置。
[0111]
优选的，航迹终点变角度三层环域匹配定位策略的构建流程可以如下：
[0112]
根据惯导序列中的航迹起点s1的位置坐标(x1,y1)和航迹终点s
l
的位置坐标(x
l
,y
l
)，确定得到水下潜器航行的航向信息和航距信息：
[0113][0114][0115]
其中，d
ins
表示以h∈[0， ∞)范数刻画惯导航迹起
‑
终点间的距离度量，设h＝2，即计算欧氏距离；α
ins
表示以坐标系的横坐标为正方向的航向弧度角。
[0116]
然后，根据以及水下潜器航行的航向信息和航距信息，估算得到大环域的中心位置坐标(x
o
,y
o
)：
[0117][0118]
则，epmp策略下，以(x
o
,y
o
)为中心，以r
max
作为最大外延的搜索边界半径，构建得到覆盖航迹真实终点的变角度三层拓扑结构环型格网点区域，记作大环域。同时，以重力基准图的格网分辨率c作为大环域各环跨度间隔，得到大环域的总环数
[0119]
在此基础上，再给定大环域的各环上相邻格点间的偏转角度，即可张成一待匹配格网点集。若选择与spmp策略相同的方式来确定大环域的各环上相邻格点间的偏转角度，
大环域等偏转角度会保持各环相等量的格点，则会导致spmp策略所张成的格网出现“内密外稀”现象，即内层环格点间隔过小而外侧环格点间隔则过大。故而，针对大环域，本发明实施例提出了一种新型的确定相邻格点间的偏转角度的方式，即变角度三环层格点拓扑结构：以重力基准图的格网分辨率c与中间环的半径之比作为大环域内各格点生成的基准角按“内倍外半”原则，确定大环域内层环上相邻格点间的跨度角为外层环上相邻格点间的跨度角为并结合总环数m，构建得到大环域待匹配格网点集，为一变角度三环层格点拓扑结构。例如，当m＝9时，epmp策略所张成的大环域待匹配格网点集的分布如图3所示。
[0120]
在本实施例中，考虑到自然条件下正态分布对任意系统误差的良好适用性及3σ准则99.73％的高概率覆盖特性，同时为后续测验不同σ(σ，表示惯导累积漂移误差标准差)原则下epmp策略的水下重力匹配效能差异性，则根据不同σ原则分别构建了基于1σ
‑
epmp、2σ
‑
epmp、3σ
‑
epmp机制的三种ioap算法，简记为1σ
‑
ioap、2σ
‑
ioap、3σ
‑
ioap。其中，采用ξ＝1,2,3来表示1σ
‑
epmp、2σ
‑
epmp、3σ
‑
epmp三种机制，即，ξ＝1，表示1σ
‑
epmp机制；ξ＝2，表示2σ
‑
epmp机制；ξ＝3，表示3σ
‑
epmp机制。
[0121]
则可以按照如下新方式构建epmp策略：
[0122]
将1σ
‑
epmp、2σ
‑
epmp、3σ
‑
epmp三种机制下大环域的总环数分别记作
[0123]
考虑到不同σ原则下大环域总环数未必为3倍数，同时为进一步增强epmp策略大环域格点对水下潜器真实位置的良好覆盖效果，可按如下公式对分别进行修正，得到修正后的三种机制下的大环域的总环数m1′
、m2′
、m3′
，以便于后续对大环域的内中外三环层的划分：
[0124][0125]
则，根据修正后的三种机制下的大环域的总环数m1′
、m2′
、m3′
，可以得到三种机制下大环域各环的半径r
1,j
、r
2,j
、r
3,j
，以及三种机制下大环域的中间环半径
[0126]
r
ξ,ζ
＝ζc，ζ＝1,2,
…
,m
′
ξ
···
(6)
[0127][0128]
其中，r
ξ,ζ
表示第ξ种机制下的大环域的第ζ环的半径。
[0129]
进而，可以得到三种机制下大环域内各格点生成的基准角
[0130][0131]
最后，根据(5)～(8)，得到不同机制下的大环域待匹配格网点。其中，各机制下的大环域待匹配格网点的计算式如下：
[0132]
1σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的计算式为：
[0133][0134]
2σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的计算式为：
[0135][0136]
3σ
‑
epmp机制下大环域的内
‑
中
‑
外环层上各待匹配格网点的位置坐标的的计算式为：
[0137][0138]
步骤103，迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正ins系统控制参数并辅助完成水下潜器长航时长航距的航行目标。
[0139]
在本实施例中，考虑到重力基准图中格网分辨率处重力值的较高准确性而插值法推算的重力值未必能真实反映匹配点处的实际重力，因此，可以采用类似于传统tercom算法的匹配过程，以确定epmp大环域待匹配点集中水下潜器终点的最佳匹配定位。
[0140]
优选的，一种可行的迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置的方式如下：
[0141]
确定第ξ种机制下大环域的内
‑
中
‑
外环层上的待匹配格网点的总数目n
ξ
。
[0142]
将第ξ种机制下大环域的第r(r∈{1,2,
…
,n
ξ
})个待匹配格网点作为待评估航迹终点，记作点将点在重力基准图中对应的位置(x
r
,y
r
)与重力基准图的格网分辨率c相比，并按四舍五入原则得到重力基准图上最近邻于点的格网点base
l
。将格网点base
l
对应的重力值作为点重力值的近似；根据格网点base
l
在重力基准图上的位置坐标、水下潜器航行的航速和航向，提取得到点对应的重力图航迹序列及对应的最近邻重力序列将与水下潜器实测重力值序列{g1,g2,
…
,g
l
}进行对比，并计算匹配效能评价指标，记作msd
r
。其中，需要说明的是，在计算匹配效能评价指标时，可以选择任意一种适当效能评价指标进行计算匹配，本实施例中是以均方差msd为例进行的说明。
[0143]
按照上述第r个待匹配格网点的计算过程，可以依次计算得到第ξ种机制下大环域的内
‑
中
‑
外环层上的各待匹配格网点的匹配效能评价指标，得到第ξ种机制下大环域的内
‑
中
‑
外环层上的所有待匹配格网点的匹配效能评价指标集和{msd
r
|r＝1,2,
…
,n
ζ
}。
[0144]
最后，基于{msd
r
|r＝1,2,
…
,n
ζ
}，按最优原则筛选得到大环域范围内水下潜器航迹终点的最佳匹配位置
[0145]
综上，通过上述步骤101～103，实现了水下潜器终点的有效匹配定位，将得到的水
下潜器航迹终点的最佳匹配位置作为水下潜器航迹终点，以修正ins系统控制参数，并辅助完成水下潜器长航时长航距的航行目标。
[0146]
在上述实施例的基础上，下面对上述实施例所述的基于格网拓扑结构迭代最佳环域点提高水下导航精度方法进行验证。
[0147]
为验证该ioap算法在水下潜器重力导航应用中的有效性和优越性，共设计3组测试：
[0148]
测试1，验证不同σ准则下ioap算法的匹配性能差异；
[0149]
测试2，以不同环半径下的基准角验证其对算法匹配性能的不同影响；
[0150]
测试3，以不同区域航迹起点验证所提ioap算法对水下重力匹配导航的良好适用性。
[0151]
实例数据源自加利福尼亚大学圣迭戈分校网站(http://topex.ucsd.edu/)，其分辨率为1
′×1′
的重力异常数据。如图4(a)所示，本发明选取南海地区重力异常数据进行研究，其数据经纬度取值范围为(经度113
°
e
–
115
°
e，纬度10
°
n
–
12
°
n)。本发明通过双线性插值法将重力异常基准数据转换成100m
×
100m的格网分辨率重力数据，如图4(b)所示，该区域重力异常最大值为130.57mgal，最小值为
‑
33.53mgal，平均值为15.43mgal。
[0152]
(1)不同σ准则下ioap算法的匹配性能差异性验证
[0153]
模拟样本区块内重力异常格网分辨率为100m
×
100m、加速度计常值零偏10
‑3m/s2(惯导均方根误差服从正态分布)、航速10m/s1、航向北偏东70
°
、初始位置误差0m、速度误差0.04m/s1、航向误差0.05
°
、重力仪实时测量数据为真实航迹在重力异常数据库中的采样值叠加标准差为1mgal的随机噪声，采样点数目110个、采样周期20s。其中，本发明定义匹配定位精度为l，则匹配位置与真实位置的绝对值之差在闭区间[0,l]内即为有效匹配，故可得n次实验测试下算法的有效匹配次数n和算法的匹配成功率同时记录n次测试匹配定位精度的平均值(mean)、标准差(std)和最差值(max)以及航迹平均匹配时间t(不含环境配置时间)作为重力匹配算法性能评价指标。
[0154]
为测试分析不同σ准则的ioap算法在水下潜器重力匹配导航中的应用性能，以1σ
‑
ioap、2σ
‑
ioap和3σ
‑
ioap算法进行100次独立实验，同时以tercom算法作为对比算法，以重力基准图格网点坐标(1400，1500)作为水下潜器航行的模拟起点，则直观匹配定位精度对比效果如图5所示。
[0155]
不同σ作用下ioap算法的匹配定位性能有所差异，以3σ
‑
ioap算法的匹配效果表现最为优异且显著优于传统tercom算法。在t指标上，1σ
‑
ioap算法虽然平均运行时间最小但匹配效果较差而难以有效应用于实际水下潜器的导航；2σ
‑
ioap算法平均运行时间约是tercom算法的一半、平均匹配精度小于1个格网分辨率，且在定位精度l＝100条件下其匹配概率也优于tercom算法(88％>82％)，表明2σ
‑
ioap算法在匹配效率和匹配精度的双目标条件下具有一定的实际导航应用价值，可根据实际导航需求适定性选择该重力导航机制；3σ
‑
ioap算法则与tercom的t指标相差不大，而在匹配精度的mean值、std值和max值以及匹配成功概率等指标上均优于tercom算法及其他算法的大部分指标值，充分表明所提ioap算法在水下潜器重力辅助导航的较好匹配性能和良好潜在实用价值。
[0156]
为进一步分析不同定位精度l约束条件下4种算法的成功匹配效果差异，分别以l
为20、40、60、80、100和的约束条件下统计100次测试的成功匹配概率对比结果的直观对比柱状图，如图6所示。
[0157]
不同定位精度约束下不同σ
‑
ioap算法的成功匹配概率差异明显，且3种σ
‑
ioap算法在l≤40时已有一定次数的成功匹配但tercom却均匹配失效；综合分析不同定位精度l下算法的成功匹配概率，以3σ
‑
ioap算法的匹配性能表现最好、2σ
‑
ioap次之且优于传统tercom算法，但1σ
‑
ioap算法定位精度约束下则弱于tercom；图6则进一步直观展示了3σ
‑
ioap算法优异的成功匹配性能，上述结论仍有效验证了所提3σ
‑
ioap算法在水下重力匹配导航中的优异性能。
[0158]
为进一步探究并剖析3σ
‑
ioap算法优异于tercom及其他σ
‑
ioap算法匹配效率和精度的原因，绘制tercom算法匹配位置与水下潜器实际位置的散点对比示意如图7所示(以惯导位置作为图像坐标原点以保证100次测试结果均可绘制在同一幅图像)。
[0159]
由图7(a)分析可知，tercom算法100次测试的水下潜器实际位置几乎均位于惯导位置的3σ误差格网范围内(图7(a)中虚线圆外侧的实线框)，同时所有位置点也几乎均位于以惯导位置为中心的3σ圆环边界虚线内部(图7(a)中内切实线框的虚线圆)，即在一定程度上表明tercom算法矩形格网待匹配点存在一定量的小概率被匹配点，见图7(b)中实线框与虚线圆间的点，即这些3σ圆环外侧点虽被匹配概率较小但却明显影响着tercom的匹配效率，因此，删除3σ环域外匹配点的重力匹配算法可在不显著影响算法匹配精度的同时可有效改善其匹配效率，这也从侧面有效佐证了本发明基于环域拓扑结构设计格网点匹配机制的可行性和有效性。
[0160]
为进一步剖析3σ
‑
ioap算法优异于其他σ
‑
ioap匹配精度的原因，绘制不同σ
‑
ioap算法100次测试的匹配效果示意如图8所示。由图8分析可知，基于不同σ环域的ioap算法对水下潜器真实位置的覆盖匹配效果有所差异：3σ
‑
ioap算法以大环域格点覆盖性实现了水下潜器的高精度匹配定位；而2σ
‑
ioap和1σ
‑
ioap仅实现了大环域内真实位置的良好匹配，但对域外真实位置的最优匹配往往只是散布于其边界环上、难以寻得更优的匹配位置而影响了算法的匹配精度，进而在一定程度上解释了不同σ
‑
ioap算法成功匹配概率的差异及其3σ
‑
ioap算法的良好匹配精度，表明3σ
‑
ioap算法在水下潜器重力匹配导航中的重要潜在应用价值。
[0161]
(2)不同环半径基准角下ioap算法匹配性能的影响差异性验证
[0162]
为进一步探究基于不同环半径r所定基准角对3σ
‑
ioap算法匹配性能的影响效果，分别以3σ
‑
ioap算法的内环层最大环半径r1＝r
m/3
、“内中”环层中间环半径中间环层最大环半径r2＝r
2m/3
、“中外”环层中间环半径来确定3σ
‑
ioap算法的基准角并张成变角度三环层待匹配格点集，相应算法分别记为1
‑
ioap、1.5
‑
ioap(即2.1节的3σ
‑
ioap)、2
‑
ioap和2.5
‑
ioap算法。根据式(6)分析可知，环半径r越大则3σ
‑
ioap算法的大环域基准角越小，生成的待匹配格网点总数目n越多，故理论上1
‑
ioap算法的匹配效率最快而2.5
‑
ioap算法的执行最慢。
[0163]
模拟样本区块、航迹起点格网坐标等参数设置均同上(1)，100次独立测试的算法匹配定位统计结果分别如图9和图10所示。3σ
‑
ioap算法的匹配精度和成功概率几乎均随基准角环半径r的增大呈现匹配效果改善的特点，且以2.5
‑
ioap性能最优、2
‑
ioap次之，1
‑
ioap匹配效果相对较差但在6/7指标上仍优于terctom算法，表明基于不同环半径的基准角会影响3σ
‑
ioap算法的匹配性能；而根据t指标分析可知，不同环半径基准角的ioap算法随环半径r增大而表现出“匹配效率降低”的现象，且该结果与其理论分析结论相一致，表明不同环半径基准角对应的待匹配格网点总量不同并导致算法匹配效率的差异。因此，在实际水下潜器导航应用中，可根据具体匹配目标需求以适定性选择合适的环半径r所对应的3σ
‑
ioap算法并完成特定航行匹配任务，说明本发明所提的3σ
‑
ioap算法在水下重力匹配导航中具有较高算法鲁棒性和良好应用价值。
[0164]
2.5
‑
ioap算法的匹配精度和不同尺度下匹配成功概率几乎均优越于其他ioap算法和tercom算法，但其匹配t指标值最高且几乎为tercom匹配时间的2倍，故在匹配效率要求不高且更重视匹配精度的水下导航情境下，2.5
‑
ioap算法不失为重力匹配算法的最佳选择；而在匹配效率和匹配精度双目标要求情境下，1
‑
ioap算法则是重力匹配算法的最佳选择，因此，进一步佐证了3σ
‑
ioap算法具有较好的水下重力匹配导航鲁棒性。此外，1.5
‑
ioap算法(即2.1节的3σ
‑
ioap算法)在匹配效率和匹配精度间表现出较好的折中效果，同时考虑到其与tercom算法的匹配效率相差不大，故下节按1.5
‑
ioap算法进一步测试以验证其在不同起点下的良好匹配性能。
[0165]
(3)不同区域航迹起点下ioap算法的良好匹配性能验证
[0166]
为进一步验证1.5
‑
ioap算法在水下潜器匹配导航中的良好匹配适用性，本发明以区域格网坐标a(1660，1410)、b(1550，740)和c(1400，350)分别作为水下潜器航迹起点的位置。在匹配效率(t指标)相差不大的前提下，1.5
‑
ioap算法在平均匹配精度(mean)、匹配精度标准差(std)和最差匹配精度(max)等4项指标上均显著优于传统tercom算法；与tercom相比较，3种情境下1.5
‑
ioap的最差匹配精度分别相对提高了47.24％、63.96％和72.16％，而平均匹配精度则相对提高20.37％、40.39％和13.88％，表明本发明所提ioap算法具有较高的匹配精度和多次测试良好的同步寻优性能；同时在不同成功匹配尺度l下，本发明算法相较于tercom算法表现出相对更为优异的匹配成功概率，特别是匹配精度小于40时1.5
‑
ioap算法仍一定概率实现水下潜器位置的成功匹配，有效验证了所提ioap算法在不同区域起点条件下对水下重力匹配导航的良好匹配适用性。
[0167]
为直观展示1.5
‑
ioap算法和tercom算法在3个测试位置下的匹配效果差异，绘制100次测试中最差匹配定位对比示意，如图11所示。由图11分析可知，在不同区域航迹起点下水下重力匹配定位中，1.5
‑
ioap算法相较于tercom算法具有更高的匹配定位精度，同时这3条航迹终点落于不同的重力区间段内，在一定程度上有效验证了所提ioap算法在不同重力区段均具有较好的匹配适应性，进一步有效佐证了本发明基于新型格网拓扑结构的迭代最佳环域点算法在提高水下潜器重力匹配精度的有效性和可靠性。
[0168]
在上述实施例的基础上，本发明还公开了一种基于格网拓扑结构迭代最佳环域点提高水下导航精度系统，包括：解算模块，用于通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置；生成模块，用于根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点；迭代确定模块，用于迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正ins系统控制参数并辅助完成水下潜器长航时长航距的航行目标。
[0169]
对于系统实施例而言，由于其与方法实施例相对应，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。
[0170]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。
[0171]
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。