具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。

本发明基于TERCOM算法匹配点阵的结构布局以及INS系统的漂移误差特性，提出了一种基于格网拓扑结构迭代最佳环域点提高水下导航精度方法，简称IOAP算法，其实现原理如下：为降低IOAP算法对初始误差的敏感性，以惯导起点位置为中心并以一定漂移误差和旋转角张成小环域匹配格点，构建环型拓扑结构的航迹起点小环域匹配定位策略，并按重力偏差绝对值最小化原则得到起点最优匹配位置；根据航迹起点的最佳匹配位置，再结合惯导航向航距信息得到大环域匹配格网的中心位置，再基于惯导累积漂移误差等以确定大环域匹配格网环数，并按中间环半径的格点基准偏转角和“内倍外半”原则，得到匹配格网点环形分布的拓扑结构，并构建航迹终点变角度三层环域的匹配定位策略，通过格网点提取重力图序列再与实测重力序列间计算均方差并匹配比较，按均方差最小化原则得到航迹终点的最佳匹配位置。

如图1，在本实施例中，该基于格网拓扑结构迭代最佳环域点提高水下导航精度方法，包括：

步骤101，通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置。

在本实施例中，考虑到SITAN和ICCP等重力匹配算法对初始误差的相对敏感性问题，同时为一定程度上改善IOAP算法对重力匹配初始误差的不敏感性(鲁棒性)，构建了航迹起点小环域格网匹配定位策略/机制(Matching Positioning strategy of thetracking Starting Point in small ring domain，SPMP)。其中，SPMP策略以惯导指示水下潜器位置为中心并以一定的漂移误差和旋转角度张成一个小型依概率覆盖航迹真实起点位置的待匹配的小环形拓扑结构格网点区域(记为小环域)，再通过待匹配点重力匹配评价指标的最优原则以确定出航迹起点的最佳匹配位置。

优选的，航迹起点小环域格网匹配定位策略的构建流程可以如下：

获取某段水下潜器航行按采样时间间隔Δt的INS航迹输出惯导序列{S₁,S₂,…,S_L}；从{S₁,S₂,…,S_L}中提取得到各航迹点对应的位置坐标和实测重力值，构建得到位置坐标序列{(x₁,y₁),(x₂,y₂),…,(x_L,y_L)}和水下潜器实测重力值序列{g₁,g₂,…,g_L}。其中，L表示INS航迹的采样序列长度，S₁、S₂、…、S_L表示惯导序列中的各航迹点。

则，SPMP策略下，以惯导序列中的航迹起点S₁的位置坐标(x₁,y₁)为中心，以3σ₀作为最大外延的搜索边界半径，确定小环域。进一步的，根据北偏东旋转角度θ₀和环半径比例均分系数λ，可以确定小环域内的待匹配格点的总数目和位置坐标，进而构建得到小环域待匹配格网点集。其中，σ₀表示采样间隔为Δt时的惯导漂移误差的标准差。例如，当θ₀＝45、λ＝1/3时，SPMP策略所张成的小环域待匹配格网点集的分布如图2所示。

进一步的，根据上述参变量的设置情况，可得小环域内各待匹配格网点的位置坐标的计算公式如下：

其中，和分别表示第i个小环上第j个格点的横纵坐标；r_i表示小环域内第i个环的半径，r_i＝3σ₀λi；β_j表示小环域各环上第j个格点的旋转角度，β_j＝j·θ₀；i＝1,2,…，且i的最大值且j的最大值

则，水下潜器航迹起点的最佳匹配位置的确定流程可以如下：

将航迹起点S₁的位置坐标(x₁,y₁)和小环域内各待匹配格网点的位置坐标作为航迹起点真实位置的待匹配点集；逐点将航迹起点真实位置的待匹配点集映射到重力基准图上，并按最近重力基准格点处的重力值作为待匹配点重力值，得到(x₁,y₁)对应的理论重力值和对应的理论重力值

为确定水下潜器航迹起点的最佳匹配位置，可按重力偏差绝对值最小化原则，得到水下潜器航迹起点的最佳匹配位置

其中，C表示重力基准图的格网分辨率，mapt(·,·)表示重力基准图按格点位置的重力值矩阵，[·]表示四舍五入取整；当i＝0、j＝0时，即为x₁、即为y₁。

需要说明的是，小范围多格点进行重力基准图映射时可能会出现多模态现象，本实施例按第一个重力偏差绝对值最小格点予以匹配，当然也可按随机选择机制得到航迹起点的最佳匹配，本实施例对此不作限制。

由上可见，SPMP策略可依概率实现水下潜器航迹起点的有效匹配定位，并弱化IOAP算法对初始误差的敏感性，同时为下一步基于惯导航向航距信息引导的航迹终点位置匹配定位提供航迹起点的位置信息基础。

步骤102，根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点。

在本实施例中，考虑到惯导航迹序列蕴藏着较好的短时高精度航向和航距信息，结合SPMP策略所得的航迹起点最佳匹配位置可进一步推得航迹终点待匹配域的中心位置O；再基于惯导系统的漂移误差统计特性而构建新的待匹配格网点环形分布的拓扑结构及其匹配定位策略(航迹终点变角度三层环域匹配定位策略/机制，MatchingPositioning mechanism of the tracking Ending Point in three-layer loopdomain，EPMP)，EPMP策略以惯导累积漂移误差标准差σ、重力基准图的格网分辨率C和中间环的半径三者间的相对关系，得到环形覆盖区域的最大环数和匹配格网点间的偏转角并张成一个大的依概率覆盖航迹真实终点位置的待匹配的变角度三层拓扑结构环型格网点区域，记为大环域；再按各格网点坐标位置提取出匹配航迹的重力图序列再与水下潜器的真实重力序列比对并按评价指标最优原则确定出航迹终点的最佳匹配位置。

优选的，航迹终点变角度三层环域匹配定位策略的构建流程可以如下：

根据惯导序列中的航迹起点S₁的位置坐标(x₁,y₁)和航迹终点S_L的位置坐标(x_L,y_L)，确定得到水下潜器航行的航向信息和航距信息：

其中，d_INS表示以h∈[0，+∞)范数刻画惯导航迹起-终点间的距离度量，设h＝2，即计算欧氏距离；α_INS表示以坐标系的横坐标为正方向的航向弧度角。

然后，根据以及水下潜器航行的航向信息和航距信息，估算得到大环域的中心位置坐标(x_O,y_O)：

则，EPMP策略下，以(x_O,y_O)为中心，以R_max作为最大外延的搜索边界半径，构建得到覆盖航迹真实终点的变角度三层拓扑结构环型格网点区域，记作大环域。同时，以重力基准图的格网分辨率C作为大环域各环跨度间隔，得到大环域的总环数

在此基础上，再给定大环域的各环上相邻格点间的偏转角度，即可张成一待匹配格网点集。若选择与SPMP策略相同的方式来确定大环域的各环上相邻格点间的偏转角度，大环域等偏转角度会保持各环相等量的格点，则会导致SPMP策略所张成的格网出现“内密外稀”现象，即内层环格点间隔过小而外侧环格点间隔则过大。故而，针对大环域，本发明实施例提出了一种新型的确定相邻格点间的偏转角度的方式，即变角度三环层格点拓扑结构：以重力基准图的格网分辨率C与中间环的半径之比作为大环域内各格点生成的基准角按“内倍外半”原则，确定大环域内层环上相邻格点间的跨度角为外层环上相邻格点间的跨度角为并结合总环数M，构建得到大环域待匹配格网点集，为一变角度三环层格点拓扑结构。例如，当M＝9时，EPMP策略所张成的大环域待匹配格网点集的分布如图3所示。

在本实施例中，考虑到自然条件下正态分布对任意系统误差的良好适用性及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机制。

则可以按照如下新方式构建EPMP策略：

将1σ-EPMP、2σ-EPMP、3σ-EPMP三种机制下大环域的总环数分别记作

考虑到不同σ原则下大环域总环数未必为3倍数，同时为进一步增强EPMP策略大环域格点对水下潜器真实位置的良好覆盖效果，可按如下公式对分别进行修正，得到修正后的三种机制下的大环域的总环数M₁′、M₂′、M₃′，以便于后续对大环域的内中外三环层的划分：

则，根据修正后的三种机制下的大环域的总环数M₁′、M₂′、M₃′，可以得到三种机制下大环域各环的半径R_1,j、R_2,j、R_3,j，以及三种机制下大环域的中间环半径

R_ξ,ζ＝ζC，ζ＝1,2,…,M′_ξ···(6)

其中，R_ξ,ζ表示第ξ种机制下的大环域的第ζ环的半径。

进而，可以得到三种机制下大环域内各格点生成的基准角

最后，根据(5)～(8)，得到不同机制下的大环域待匹配格网点。其中，各机制下的大环域待匹配格网点的计算式如下：

1σ-EPMP机制下大环域的内-中-外环层上各待匹配格网点的位置坐标的计算式为：

2σ-EPMP机制下大环域的内-中-外环层上各待匹配格网点的位置坐标的计算式为：

3σ-EPMP机制下大环域的内-中-外环层上各待匹配格网点的位置坐标的的计算式为：

步骤103，迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正INS系统控制参数并辅助完成水下潜器长航时长航距的航行目标。

在本实施例中，考虑到重力基准图中格网分辨率处重力值的较高准确性而插值法推算的重力值未必能真实反映匹配点处的实际重力，因此，可以采用类似于传统TERCOM算法的匹配过程，以确定EPMP大环域待匹配点集中水下潜器终点的最佳匹配定位。

优选的，一种可行的迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置的方式如下：

确定第ξ种机制下大环域的内-中-外环层上的待匹配格网点的总数目N_ξ。

将第ξ种机制下大环域的第r(r∈{1,2,…,N_ξ})个待匹配格网点作为待评估航迹终点，记作点将点在重力基准图中对应的位置(x_r,y_r)与重力基准图的格网分辨率C相比，并按四舍五入原则得到重力基准图上最近邻于点的格网点base_L。将格网点base_L对应的重力值作为点重力值的近似；根据格网点base_L在重力基准图上的位置坐标、水下潜器航行的航速和航向，提取得到点对应的重力图航迹序列及对应的最近邻重力序列将与水下潜器实测重力值序列{g₁,g₂,…,g_L}进行对比，并计算匹配效能评价指标，记作MSD_r。其中，需要说明的是，在计算匹配效能评价指标时，可以选择任意一种适当效能评价指标进行计算匹配，本实施例中是以均方差MSD为例进行的说明。

按照上述第r个待匹配格网点的计算过程，可以依次计算得到第ξ种机制下大环域的内-中-外环层上的各待匹配格网点的匹配效能评价指标，得到第ξ种机制下大环域的内-中-外环层上的所有待匹配格网点的匹配效能评价指标集和{MSD_r|r＝1,2,…,N_ζ}。

最后，基于{MSD_r|r＝1,2,…,N_ζ}，按最优原则筛选得到大环域范围内水下潜器航迹终点的最佳匹配位置

综上，通过上述步骤101～103，实现了水下潜器终点的有效匹配定位，将得到的水下潜器航迹终点的最佳匹配位置作为水下潜器航迹终点，以修正INS系统控制参数，并辅助完成水下潜器长航时长航距的航行目标。

在上述实施例的基础上，下面对上述实施例所述的基于格网拓扑结构迭代最佳环域点提高水下导航精度方法进行验证。

为验证该IOAP算法在水下潜器重力导航应用中的有效性和优越性，共设计3组测试：

测试1，验证不同σ准则下IOAP算法的匹配性能差异；

测试2，以不同环半径下的基准角验证其对算法匹配性能的不同影响；

测试3，以不同区域航迹起点验证所提IOAP算法对水下重力匹配导航的良好适用性。

实例数据源自加利福尼亚大学圣迭戈分校网站(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。

(1)不同σ准则下IOAP算法的匹配性能差异性验证

模拟样本区块内重力异常格网分辨率为100m×100m、加速度计常值零偏10^-3m/s²(惯导均方根误差服从正态分布)、航速10m/s¹、航向北偏东70°、初始位置误差0m、速度误差0.04m/s¹、航向误差0.05°、重力仪实时测量数据为真实航迹在重力异常数据库中的采样值叠加标准差为1mGal的随机噪声，采样点数目110个、采样周期20s。其中，本发明定义匹配定位精度为l，则匹配位置与真实位置的绝对值之差在闭区间[0,l]内即为有效匹配，故可得N次实验测试下算法的有效匹配次数n和算法的匹配成功率同时记录N次测试匹配定位精度的平均值(mean)、标准差(std)和最差值(max)以及航迹平均匹配时间T(不含环境配置时间)作为重力匹配算法性能评价指标。

为测试分析不同σ准则的IOAP算法在水下潜器重力匹配导航中的应用性能，以1σ-IOAP、2σ-IOAP和3σ-IOAP算法进行100次独立实验，同时以TERCOM算法作为对比算法，以重力基准图格网点坐标(1400，1500)作为水下潜器航行的模拟起点，则直观匹配定位精度对比效果如图5所示。

不同σ作用下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算法在水下潜器重力辅助导航的较好匹配性能和良好潜在实用价值。

为进一步分析不同定位精度L约束条件下4种算法的成功匹配效果差异，分别以l为20、40、60、80、100和的约束条件下统计100次测试的成功匹配概率对比结果的直观对比柱状图，如图6所示。

不同定位精度约束下不同σ-IOAP算法的成功匹配概率差异明显，且3种σ-IOAP算法在l≤40时已有一定次数的成功匹配但TERCOM却均匹配失效；综合分析不同定位精度l下算法的成功匹配概率，以3σ-IOAP算法的匹配性能表现最好、2σ-IOAP次之且优于传统TERCOM算法，但1σ-IOAP算法定位精度约束下则弱于TERCOM；图6则进一步直观展示了3σ-IOAP算法优异的成功匹配性能，上述结论仍有效验证了所提3σ-IOAP算法在水下重力匹配导航中的优异性能。

为进一步探究并剖析3σ-IOAP算法优异于TERCOM及其他σ-IOAP算法匹配效率和精度的原因，绘制TERCOM算法匹配位置与水下潜器实际位置的散点对比示意如图7所示(以惯导位置作为图像坐标原点以保证100次测试结果均可绘制在同一幅图像)。

由图7(a)分析可知，TERCOM算法100次测试的水下潜器实际位置几乎均位于惯导位置的3σ误差格网范围内(图7(a)中虚线圆外侧的实线框)，同时所有位置点也几乎均位于以惯导位置为中心的3σ圆环边界虚线内部(图7(a)中内切实线框的虚线圆)，即在一定程度上表明TERCOM算法矩形格网待匹配点存在一定量的小概率被匹配点，见图7(b)中实线框与虚线圆间的点，即这些3σ圆环外侧点虽被匹配概率较小但却明显影响着TERCOM的匹配效率，因此，删除3σ环域外匹配点的重力匹配算法可在不显著影响算法匹配精度的同时可有效改善其匹配效率，这也从侧面有效佐证了本发明基于环域拓扑结构设计格网点匹配机制的可行性和有效性。

为进一步剖析3σ-IOAP算法优异于其他σ-IOAP匹配精度的原因，绘制不同σ-IOAP算法100次测试的匹配效果示意如图8所示。由图8分析可知，基于不同σ环域的IOAP算法对水下潜器真实位置的覆盖匹配效果有所差异：3σ-IOAP算法以大环域格点覆盖性实现了水下潜器的高精度匹配定位；而2σ-IOAP和1σ-IOAP仅实现了大环域内真实位置的良好匹配，但对域外真实位置的最优匹配往往只是散布于其边界环上、难以寻得更优的匹配位置而影响了算法的匹配精度，进而在一定程度上解释了不同σ-IOAP算法成功匹配概率的差异及其3σ-IOAP算法的良好匹配精度，表明3σ-IOAP算法在水下潜器重力匹配导航中的重要潜在应用价值。

(2)不同环半径基准角下IOAP算法匹配性能的影响差异性验证

为进一步探究基于不同环半径R所定基准角对3σ-IOAP算法匹配性能的影响效果，分别以3σ-IOAP算法的内环层最大环半径R¹＝R_M/3、“内中”环层中间环半径中间环层最大环半径R²＝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算法的执行最慢。

模拟样本区块、航迹起点格网坐标等参数设置均同上(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算法在水下重力匹配导航中具有较高算法鲁棒性和良好应用价值。

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算法进一步测试以验证其在不同起点下的良好匹配性能。

(3)不同区域航迹起点下IOAP算法的良好匹配性能验证

为进一步验证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算法在不同区域起点条件下对水下重力匹配导航的良好匹配适用性。

为直观展示1.5-IOAP算法和TERCOM算法在3个测试位置下的匹配效果差异，绘制100次测试中最差匹配定位对比示意，如图11所示。由图11分析可知，在不同区域航迹起点下水下重力匹配定位中，1.5-IOAP算法相较于TERCOM算法具有更高的匹配定位精度，同时这3条航迹终点落于不同的重力区间段内，在一定程度上有效验证了所提IOAP算法在不同重力区段均具有较好的匹配适应性，进一步有效佐证了本发明基于新型格网拓扑结构的迭代最佳环域点算法在提高水下潜器重力匹配精度的有效性和可靠性。

在上述实施例的基础上，本发明还公开了一种基于格网拓扑结构迭代最佳环域点提高水下导航精度系统，包括：解算模块，用于通过航迹起点小环域格网匹配定位策略，得到水下潜器航迹起点的最佳匹配位置；生成模块，用于根据水下潜器航迹起点的最佳匹配位置，通过航迹终点变角度三层环域匹配定位策略，生成大环域待匹配格网点；迭代确定模块，用于迭代计算大环域待匹配格网点的匹配效能评价指标，并按最优原则获得大环域范围内水下潜器航迹终点的最佳匹配位置，以实现水下潜器航迹终点的有效匹配定位，进而修正INS系统控制参数并辅助完成水下潜器长航时长航距的航行目标。

对于系统实施例而言，由于其与方法实施例相对应，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。