本发明属于航天和海洋技术领域，尤其涉及一种天空海一体化水下潜器惯性/重力组合导航方法和系统。
背景技术：
天空海一体化海洋导航(融合天基、空基、海基等多源信息)是水下潜器导航系统发展的主要趋势。目前水下潜器导航系统主要为惯性导航系统(INS:Inertial Navigation System)，可为水下潜器航行和武器系统精准打击提供有利条件，但INS系统存在误差随时间积累的缺点，因此需进行外部校正。迄今为止，将地球物理场与INS系统联合构成的无源辅助导航系统始终是有效抑制INS系统误差积累问题的国际研究热点。目前无源辅助导航技术主要包括：地磁匹配、地形匹配、重力匹配等。地磁场本身存在长期和短期变化，使地磁图精度达不到较高要求，且测磁技术存在磁干扰等局限性。地形研究开展较早，特别是陆地上的地形匹配导航技术已运用于飞行器导航，但水下地形匹配导航发展相对较晚。由于需要向外发射声波，而且声纳测量地形在海况复杂条件下无法精确探测到深海地形，因此目前水下地形匹配导航技术仅适用于浅海地区。然而，海洋重力匹配导航是根据地球不同位置重力差异实现导航定位，不需要水下潜器浮出或接近水面，测量时不向外辐射能量，且地球重力场在长时间内保持稳定，因此有望实现水下潜器精确、自主和连续长航时的定位。
重力匹配技术实现的核心问题是解决高精度和高分辨率重力基准图、高精度重力测量系统、重力匹配定位算法等关键技术。自上世纪以来，众多学者先后提出了多种重力匹配算法，其中Tercom算法较为盛行。优点为计算简单可靠、定位精度较高等；缺点为采用全局遍历的搜索策略，运算量大，运算效率较低，且对载体航迹要求较高，当航向存在较大偏差时误差将急剧增大。因此，如何提高Tercom算法的定位精度和匹配效率是当前水下导航领域的研究热点。赵建虎等将基于Hausdorff距离的匹配准则引入Tercom算法中，提出通过增加旋转变化、自适应确定最佳旋转角、实现适配序列精匹配的思想和算法，进而有效提高了匹配导航精度和可靠性；闫利等基于Tercom算法开展重力匹配仿真模拟研究，并证明了地形粗糙度和坡度方差与Tercom算法的定位精度具有强相关性。综上所述，目前大部分学者主要围绕提高水下导航精度开展研究，而提高水下导航匹配效率方面研究相对较少。
技术实现要素：
本发明的技术解决问题：克服现有技术的不足，提供一种天空海一体化水下潜器惯性/重力组合导航方法和系统，旨在克服传统Tercom算法逐点遍历搜索效率较低的缺点，提高天空海一体化水下潜器惯性/重力组合导航的匹配效率。
为了解决上述技术问题，本发明公开了一种天空海一体化水下潜器惯性/重力组合导航方法，包括：
在确定的搜索范围内，以设定网格间隔数进行粗搜索匹配，得到粗搜索最佳匹配点；
根据设置阈值，对粗搜索最佳匹配点周围的若干邻域点进行匹配比较，获得各粗搜索最佳匹配点对应的粗搜索临时最佳匹配点；
对各粗搜索临时最佳匹配点进行比较，得到整个搜索范围内的最佳匹配点；
将所述最佳匹配点作为导航结果输出。
在上述天空海一体化水下潜器惯性/重力组合导航方法中，还包括：
确定当前时刻INS输出的位置信息，以及所述位置信息对应的INS估计误差；
根据所述位置信息和所述位置信息对应的INS估计误差，在重力基准图中确定搜索范围。
在上述天空海一体化水下潜器惯性/重力组合导航方法中，所述根据所述位置信息和所述位置信息对应的INS估计误差，在重力基准图中确定搜索范围，包括：
以所述位置信息所指示的位置为圆心，以所述INS估计误差的倍数为半径，在重力基准图中确定搜索范围。
在上述天空海一体化水下潜器惯性/重力组合导航方法中，所述重力基准图，包括：行和列；其中，行表示纬度，列表示经度。
在上述天空海一体化水下潜器惯性/重力组合导航方法中，所述在确定的搜索范围内，以设定网格间隔数进行粗搜索匹配，得到粗搜索最佳匹配点，包括：
在确定的搜索范围内，按照第一设定间隔，选择至少一个粗搜索行；
按照地形轮廓匹配法，对各粗搜索行，按照第二设定间隔进行粗搜索，得到各粗搜索行的最佳匹配点。
在上述天空海一体化水下潜器惯性/重力组合导航方法中，所述根据设置阈值，对粗搜索最佳匹配点周围的若干邻域点进行匹配比较，获得各粗搜索最佳匹配点对应的粗搜索临时最佳匹配点，包括：
根据设置阈值，对各粗搜索行的最佳匹配点周围的若干邻域点进行匹配比较，获得每个粗搜索行附近范围内临时最佳匹配点，作为粗搜索临时最佳匹配点。
相应的，本发明还公开了一种天空海一体化水下潜器惯性/重力组合导航系统，包括：
粗搜索模块，用于在确定的搜索范围内，以设定网格间隔数进行粗搜索匹配，得到粗搜索最佳匹配点；
筛选模块，用于根据设置阈值，对粗搜索最佳匹配点周围的若干邻域点进行匹配比较，获得各粗搜索最佳匹配点对应的粗搜索临时最佳匹配点；
比较模块，对各粗搜索临时最佳匹配点进行比较，得到整个搜索范围内的最佳匹配点；
输出模块，用于将所述最佳匹配点作为导航结果输出。
本发明具有以下优点：
1)水下惯性/重力组合导航匹配效率快；
2)水下重力匹配定位精度高；
3)水下重力匹配导航可靠性高；
4)水下重力匹配过程物理含义明确。
附图说明
图1是本发明实施例中一种天空海一体化水下潜器惯性/重力组合导航方法的步骤流程图；
图2是本发明实施例中一种水下惯性/重力组合导航流程示意图；
图3是本发明实施例中一种不同格网间隔数和邻域情况下算法定位精度和效率对比示意图；
图4是本发明实施例中一种不同格网和邻域情况下算法统计信息分析示意图；
其中，图3(a)表示3格网间隔数，图3(b)表示4格网间隔数，图3(c)表示5格网间隔数，图3(d)表示6格网间隔数；图4(a)表示平均定位精度对比，图4(b)表示定位精度标准差对比，图4(c)表示匹配重合率对比，图4(d)表示定位时间对比。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。
参照图1，示出了本发明实施例中一种天空海一体化水下潜器惯性/重力组合导航方法的步骤流程图。在本实施例中，所述天空海一体化水下潜器惯性/重力组合导航方法，包括：
步骤101，在确定的搜索范围内，以设定网格间隔数进行粗搜索匹配，得到粗搜索最佳匹配点。
步骤102，根据设置阈值，对粗搜索最佳匹配点周围的若干邻域点进行匹配比较，获得各粗搜索最佳匹配点对应的粗搜索临时最佳匹配点。
步骤103，对各粗搜索临时最佳匹配点进行比较，得到整个搜索范围内的最佳匹配点。
步骤104，将所述最佳匹配点作为导航结果输出。
在本发明的一优选实施例中，在步骤101之前，所述天空海一体化水下潜器惯性/重力组合导航方法，可以还包括：确定当前时刻INS输出的位置信息，以及所述位置信息对应的INS估计误差；根据所述位置信息和所述位置信息对应的INS估计误差，在重力基准图中确定搜索范围。其中，重力基准图，包括：行和列；行表示纬度，列表示经度。
优选的，所述根据所述位置信息和所述位置信息对应的INS估计误差，在重力基准图中确定搜索范围，具体可以包括：以所述位置信息所指示的位置为圆心，以所述INS估计误差的倍数(如3倍的INS估计误差)为半径，在重力基准图中确定搜索范围。
其中，需要说明的是，在确定搜索范围时，半径(INS估计误差的倍数)可以根据实际情况选择适当的值，本实施例对此不作限制。
在本发明的一优选实施例中，上述步骤101，具体可以包括：在确定的搜索范围内，按照第一设定间隔，选择至少一个粗搜索行；按照地形轮廓匹配法，对各粗搜索行，按照第二设定间隔进行粗搜索，得到各粗搜索行的最佳匹配点。
其中，第一设定间隔和第二设定间隔的可以根据实际情况选择适当的值，本实施例对此不作限制。
在本发明的一优选实施例中，上述步骤102，具体可以包括：根据设置阈值，对各粗搜索行的最佳匹配点周围的若干邻域点进行匹配比较，获得每个粗搜索行附近范围内临时最佳匹配点，作为粗搜索临时最佳匹配点。
基于上述实施例，下面结合一个具体实例进行说明。
在本实施例中，所述天空海一体化水下潜器惯性/重力组合导航方法，基于分层邻域阈值搜索原理实现对水下潜器惯性/重力组合导航匹配效率的提高。
本实施例中，研究数据来源于联合航天、海洋等多种技术生成的全球海陆数据库(http://topex.ucsd.edu/)，数据经纬度取值范围为(经度112°～116°E，纬度10°～11°N)，原始海底重力异常分辨率为1′×1′。
水下重力匹配精度主要由匹配算法和重力场特征参数决定。现有重力匹配算法多源于地形匹配算法，TERCOM(Terrain Contour Matching)、ICCP(Iterative Closest Contour Point)、PDAF(Probability Data Associate Filtering)等均为批处理相关类算法，SITAN(Sandia Inertial Terrain-Aided Navigation)和PF(Particle Filter)算法分别是基于扩展卡尔曼滤波(EKF)和基于直接概率准则的连续匹配算法。
重力场特征参数是影响水下导航精度和匹配概率的关键因素，主要包括：重力场标准差、坡度标准差、峰度系数、粗燥度、信息熵、相关系数等。本发明基于重力场连续分布且空间分布变化较缓的特点，提出分层邻域阈值搜索法，旨在提高Tercom算法的匹配效率。分层邻域阈值搜索法的计算原理和算法流程如图2所示。
①确定起始位置A和目标位置B的坐标，并规划航向。
②水下潜器进入匹配区后，当重力场测量序列足够长时，采用分层邻域阈值搜索法。首先，根据当前时刻INS误差估计搜索范围半径，在3σ(位置信息对应的INS估计误差)误差搜索范围内，先以较大的网格进行粗搜索匹配，获得每个粗搜索行的最佳匹配点；其次，选取粗搜索行最佳匹配点周围若干邻域点进行匹配比较，获得每个粗搜索行附近范围内临时最佳匹配点，在匹配过程中设置阈值提高匹配点的选取标准；最后，比较分析获取的若干临时最佳匹配点，确定搜索范围内最佳匹配点。
在匹配搜索过程中，假设搜索区域大小为M×M格网，g(x,y)为格网点(x,y)处的重力异常值，gs(x,y)为重力测量序列最后一个采样点值，若依据逐点遍历搜索策略，则初始匹配点总数目S0为：
S0＝M2···(1)
基于分层邻域阈值搜索法，则初始匹配点总数目S估算如下：
第一，当以较大的格网间隔数横向和纵向粗搜索时，设n为格网间隔数，M为每行格网数，则每行选取的搜索点依次是g(x,1)、g(x,n+1)、g(x,2n+1)、……、g(x,n(k-1)+1)，其中k是每行粗搜索点序号(正整数)。
每行格网数M、格网间隔数n和每行粗搜索点数k关系如下：
n(k-1)+1≤M<nk+1···(2)
所以，由公式(1)可得每行粗搜索点数k：
由于k为正整数，因而对k取整可得：
第二，由于粗搜索行与搜索列数目相同，因而搜索区域M×M格网内的粗搜索点总数目S1为：
第三，由于共有k个粗搜索行，每行都有一个最佳匹配点，选取此点周围8邻域点(或24邻域点)进行匹配，在匹配过程中阈值设置为(|g(x,y)-gs(x,y)|≤4mGal)，提高了匹配点的选点标准。由公式(4)可得8邻域时(或24邻域时)，周围附加搜索点数目S2的表达式：
第四，由公式(5)和(6)可得初始匹配点总数目S：
S＝S1+S2···(7)
其中，由于M>>n，因而估算数目S大幅度小于公式(1)中的遍历搜索数目S0。因此，基于分层邻域阈值搜索法，可通过提高匹配点的选取效率加快匹配速度，进而提高匹配导航的匹配效率。
③应用中值滤波误匹配修正法对步骤②的匹配点进行判断修正。现在的惯导误差通常是由上一时间惯导指示位置作为初值，然后对加速度积分得到。若匹配时间间隔一样，则若干相邻匹配点之间INS每次指示位置到匹配位置的修正数变化较小。因此，若某相邻点的匹配修正数偏离较大，则此点匹配结果不准确，可以用中值滤波进行匹配点估计。
④重复步骤②和③，直至到达目标区域。
下面给出了天空海一体化水下潜器惯性/重力组合导航方法的试验验证结果及具体的应用。
本发明中分辨率1′×1′的重力异常数据和海底地形数据源于加利福尼亚大学圣迭戈分校网站(http://topex.ucsd.edu/)。本发明选取南海地区重力异常数据进行研究，并与同区域海底地形数据进行对比分析。数据经纬度取值范围为(经度112°～116°E，纬度10°～11°N)，其中重力异常最大值为133.4mGal，最小值为-32.4mGal，平均值为14.81mGal。由于本课题组预期利用6～8颗GNSS-R测高星座反演高空间分辨率海洋重力基准图，进而实现高精度水下重力匹配导航，因此，本发明通过Matlab插值计算将数据转换成格网分辨率100×100m进行验证。
以研究区域100×100m格网分辨率的二维/三维海底地形基准图和研究区域100×100m格网分辨率的二维/三维重力异常基准图为例，东部及东南部重力场起伏剧烈，而西北部重力场变化较平缓。由于地球并非密度均衡球体，介质密度的不均衡性导致地形与重力值并非一一对应。二维/三维海底地形基准图中海底地形特征显著，山峰、山脊、峡谷等棱角突变特征明显；二维/三维重力异常基准图中重力场呈连续性分布，空间分布变化平滑。因此，分层邻域阈值搜索法更适用于水下重力匹配导航研究。
分层邻域阈值搜索法的特点是有利于提高匹配点的选点标准加快搜索匹配速度，可在保证Tercom算法定位精度前提下较大程度提高匹配效率。本发明验证如下：将上述重力异常基准图数据从左到右分成4块，每块大小为1°×1°。
重力场特征参数主要包括重力场标准差、坡度标准差、峰度系数、粗燥度、信息熵等。重力场标准差反映了重力场的起伏变化；重力场坡度标准差表示重力场变化的速率；峰度系数用来度量数据在中心的聚集程度，可反映频数分布曲线顶端尖峭或扁平的程度；粗糙度反映了整个区域重力场的平均光滑程度和局部起伏；信息熵用来评价重力区域信息量的丰富程度。重力场标准差越大、坡度标准差越大、粗燥度越大、信息熵越小，则信息越丰富，越有利于匹配。据4个区域重力场特征统计信息可知：首先，基准三的重力场标准差、坡度标准差和粗燥度最大，说明此区域更不平坦且重力场变化更快；其次，基准三的信息熵最小，说明此区域重力异常特征信息更丰富和更复杂；最后，基准三的峰度系数最小，由于正态分布情况下的峰度系数值为3，如果峰度系数大于3，则说明观测量更集中，有比正态分布更短的尾部；当峰度系数小于3，则说明观测量不太集中，更有利于匹配。因此，综合各特征参数判断，选择基准三适配性较好。
格网间隔数、邻域大小、阈值均为定位精度和定位时间的重要影响因素。阈值可取4σ(σ为重力仪实测数据与基准图数据差值的标准差，4σ约为99.99％)。由于格网间隔数和邻域大小对定位精度和定位时间的影响相反(随着格网间隔数增大，定位精度将降低，定位时间将减小；而随着邻域增大，定位精度将提高，定位时间将增大)，因而从定位精度与定位时间角度考虑，本发明对分层邻域阈值搜索法格网间隔数与邻域大小的优选进行了数值模拟和分析讨论。Tercom数值模拟参数设置如下：陀螺仪常值漂移0.01°/h、加速度计常值零偏10-3m/s2(惯导均方根误差服从正态分布)、航速10m/s、航向北偏东70°、初始位置误差0、速度误差0.03m/s、航向误差0.05°、重力仪实时测量数据是真实航迹在重力异常基准数据库中的采样值叠加标准差为1mGal的随机噪声(阈值取为4mGal)、采样点数220个、采样周期20s。
为验证分层邻域阈值搜索算法的稳定性，本发明在相同条件下进行了80次计算，分别在以不同格网间隔数(3个格网、4个格网、5个格网、6个格网)和邻域(8邻域、24邻域)两两组合的情况下对定位精度及效率进行了数值计算，如图3所示。其中，图3(a)、3(b)、3(c)和3(d)中的左图表示不同格网间隔数和邻域情况下算法定位精度。据图3可知，在邻域相同时，随着格网间隔数增大，算法平均定位精度明显降低；在格网间隔数相同时，24邻域情况下的平均定位精度明显高于8邻域。图3(a)、3(b)、3(c)和3(d)中的的右图表示不同格网间隔数和邻域情况下算法定位时间(定位时间由基准图加载、绘图、搜索匹配等时间组成)。据图3可知，传统Tercom算法平均单次定位用时31.79s，其中基准图加载、绘图等用时20.04s，搜索匹配用时11.75s，且各次用时较分散；而在相同条件下，本发明所提不同格网间隔数(3个格网、4个格网、5个格网、6个格网)和邻域(8邻域、24邻域)两两组合的8种情况下算法单次定位用时处于20～22s之间，则说明去掉基准图加载、绘图等公共时间，搜索匹配用时小于2s，因此快速提高了匹配效率。
据图4可知，基于80次匹配定位计算，传统Tercom算法平均定位精度为87.94m、定位精度标准差为50.66m、定位时间为31.79s、搜索匹配时间为11.75s。上述研究结果表明了提高海洋重力基准图空间分辨率对改善水下导航精度的重要性，若本课题组预期完成6～8颗GNSS-R测高卫星星座反演高空间分辨率的重力基准图，便有望实现水下重力匹配导航百米级定位精度。据图4(a)和4(b)可知，随格网间隔数增加，在8邻域情况下，其平均定位精度和定位精度标准差都有明显降低趋势，当选择3格网间隔时，其平均定位精度降低0.99m，定位精度标准差降低0.16m；但选择6格网间隔时，其平均定位精度降低74.09m，定位精度标准差降低30.45m。然而在24邻域情况下，其平均定位精度和定位精度标准差虽然亦有降低趋势，但其降低幅度较小，当选择3格网和4格网间隔时，其平均定位精度(87.94m)和定位精度标准差(50.66m)与传统Tercom算法结果一致；选择6格网间隔时，其平均定位精度也仅降低了8.61m，定位精度标准差降低5.05m。因此，如图4(c)所示，在24邻域情况下，选择3格网和4格网间隔均可达到与传统Tercom算法100％的匹配重合率，即可保持定位精度一致。80次匹配定位计算，传统Tercom算法单次平均搜索匹配时间为11.75s。随着格网间隔数增加，其定位时间呈减小趋势，8邻域情况下的定位时间略优于24邻域，但选择8邻域情况时不能保持传统Tercom算法定位精度，因而选择24邻域。当选择24邻域时，3格网间隔单次搜索匹配时间为1.40s，较相同条件下的传统Tercom算法单次搜索匹配效率提高8.48倍；然而4格网间隔单次搜索匹配时间为0.83s，比在相同条件下的传统Tercom算法单次搜索匹配时间缩短10.92s，匹配效率提高约14.14倍。
可见，以本实施例参数设置为例，本发明分层邻域阈值搜索法与传统Tercom法相比，可在保证定位精度基础上有效提高匹配效率约14.14倍。本发明仅计算了短时间水下导航匹配效率提高情况，根据INS误差随时间积累特性，若运行时间延长，INS误差估计的搜索范围将持续增大；同时采样点次数增加，均可增大传统Tercom算法的搜索匹配时间。因此，分层邻域阈值搜索法可使单次搜索匹配时间至少缩短10.92s。
综上所述，由于传统Tercom算法匹配效率较低，因而提出了新型分层邻域阈值搜索法。优点：以较大网格间隔数进行粗搜索匹配，并设置阈值且对选取的每个粗搜索行最佳匹配点周围的若干邻域点进行匹配比较，获得每个粗搜索行附近范围内临时最佳匹配点，最终对获取的所有临时最佳匹配点进行比较，进而得到整个搜索范围内最佳匹配点。
其次，基于新型分层邻域阈值搜索法，以本发明实施例参数设置为例，结果表明：①在格网间隔数相同时，24邻域情况下的平均定位精度明显高于8邻域。当选择3格网和4格网间隔时，其平均定位精度(87.94m)与传统Tercom算法定位精度一致。②在邻域相同时，随着格网间隔数增大，其平均定位精度呈降低趋势，8邻域情况下的降低变化率显著大于24邻域。当选择8邻域时，3格网间隔定位精度降低0.99m，但6格网间隔定位精度降低74.09m。然而，当选择24邻域时，3格网和4格网间隔定位精度均未降低，6格网间隔仅降低8.61m。③当采用4格网、24邻域和4mGal阈值参数，在保证定位精度前提下，单次搜索匹配时间由11.75s减少到0.83s，匹配效率提高约14.14倍。因此，分层邻域阈值搜索法有利于提高天空海一体化水下潜器惯性/重力组合导航的匹配效率。
此外，全球海洋重力基准图空间分辨率提高是目前实现高精度水下重力匹配导航的关键因素。因此，本发明数值计算以100×100m格网分辨率数据为例，基于传统Tercom算法得到平均定位精度优于百米(87.94m)。其为本课题组将来设计6～8颗GNSS-R测高卫星星座，进而实现百米级水下重力匹配导航精度提供理论支持和方法保证。
在本发明实施例中，还公开了一种天空海一体化水下潜器惯性/重力组合导航系统，包括：粗搜索模块，用于在确定的搜索范围内，以设定网格间隔数进行粗搜索匹配，得到粗搜索最佳匹配点；筛选模块，用于根据设置阈值，对粗搜索最佳匹配点周围的若干邻域点进行匹配比较，获得各粗搜索最佳匹配点对应的粗搜索临时最佳匹配点；比较模块，对各粗搜索临时最佳匹配点进行比较，得到整个搜索范围内的最佳匹配点；输出模块，用于将所述最佳匹配点作为导航结果输出。
对于系统实施例而言，由于其与方法实施例相对应，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。
本说明中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。