CN102322858A - 用于地磁/捷联惯导组合导航系统的地磁匹配导航方法 - Google Patents

Abstract

本发明公开了一种用于地磁/捷联惯导组合导航系统的地磁匹配导航方法。本发明方法基于递推求解思想，采用实时评价的方式，在每一步递推求解后根据匹配情况来评价是否可以输出匹配结果，从而将算法时间分散在各个采样时刻。该方法克服了现有地磁匹配算法在数据批处理上实时性的不足，能够自适应调整匹配数据序列的长度，避免了现有地磁匹配算法需要预先指定匹配序列长度的困难。

Description

用于地磁/捷联惯导组合导航系统的地磁匹配导航方法

技术领域

本发明涉及一种地磁匹配导航（Geomagnetic Matching Navigation System，简称GMNS）方法，尤其涉及一种用于地磁/捷联惯导组合导航系统的地磁匹配导航方法。

背景技术

地磁匹配导航是利用地磁场的一种基于数据库查询的自主式导航方法，它利用磁传感器采集载体位置处的地磁信息，将工作一段时间所得到的地磁信息序列与预先存储在计算机中的地磁数据库进行比较，查询出最佳匹配序列和对应的地理位置。捷联惯性导航（Strapdown Inertial Navigation System，简称SINS）是根据牛顿力学定律获得载体导航信息的一种自主式导航方法，它利用陀螺仪、加速度计等惯性元件敏感载体运动信息，然后通过计算机进行积分运算得到载体的姿态、速度和位置等导航参数。

这两种导航方式各有特点：SINS信息丰富，短时导航精度较高，但长时间工作系统精度较差；GMNS测量精度高，定位误差没有时间累积效应，但对工作区域要求地磁变化显著。因此SINS与GMNS具有很强的互补性，地磁/捷联惯导组合导航系统的性能高于单个导航系统。

目前用于地磁/捷联惯导组合导航系统中的地磁匹配导航算法有很多，主要可以分为两类，一类是相似度度量算法，其思想是指定一个准则来评价实测地磁数据序列和地磁数据库搜索空间中的待比较地磁数据序列之间的相似度，常用的有平均绝对差算法（MAD算法）、积相关法（PROD法）和Hausdorff距离度量法；另一类是等值线匹配算法（ICCP算法），其思想是借鉴图形匹配中的ICP算法，通过计算惯导输出位置序列的刚性变换，使其接近其等磁线最近点位置序列，不断迭代该过程，最终实现地磁实测数据序列与地磁数据库中地磁图的配准。

相似度度量算法原理简单可靠，适用范围广，对初始误差要求低，能得到全局最优的匹配结果，应用于地磁匹配时具有较高的匹配精度和捕获概率，但是由于一般搜索数据库较大，因此相似度度量算法耗时较长，实时性能较差。谢仕民等对几种相似度度量算法进行了仿真研究，结果表明基于Hausdorff距离的匹配算法具有更高的抗干扰能力和容错能力（见谢仕民，李邦清，刘峰，等，《基于Hausdorff距离的地磁匹配算法研究》，《战术导弹技术》，2008，(5)：59-62）。等值线匹配算法采用图形刚性变换方法，可得到比基于网格搜索的相似度度量算法更高的匹配精度，且基于等值线的搜索方式耗时相对较少，但是等值线匹配算法要求惯导初始位置误差较小，且测量数据中没有野值点，算法复杂，鲁棒性较差。郭庆等针对地磁场数据的缓变特性，研究了地磁场特征量等磁线的交叉特性，并提出了一种新的、有效的地磁匹配双等值线（DICCP）算法，仿真结果表明DICCP算法能够得到比ICCP 算法更高的匹配精度（见郭庆，魏瑞轩，胡明朗，等，《地磁匹配双等值线算法仿真研究》，《系统仿真学报》，2010，22(7)：1576-1579）。

以上所述的地磁匹配算法都是批处理方法，实时性能较差，且需要根据经验或实验结果来确定预先指定的匹配序列长度。

发明内容

本发明所要解决的技术问题在于克服现有地磁匹配导航方法在数据批处理上实时性能的不足和需要预先指定匹配序列长度的困难，提供一种用于地磁/捷联惯导组合导航系统的地磁匹配导航方法，该方法借鉴了递推求解思想，克服了批处理方法在时间复杂度上的不足，同时采用实时评价的方式，即根据已有的实测地磁数据来评价是否可以输出地磁匹配结果，避免了预先指定匹配序列长度的困难。

本发明采用以下技术方案：

一种用于地磁/捷联惯导组合导航系统的地磁匹配导航方法，所述地磁/捷联惯导组合导航系统包括惯性导航系统、磁传感器及地磁数据库，该方法根据惯性导航系统输出轨迹和磁传感器输出地磁值，在惯性导航系统位置误差和磁传感器测量误差允许的范围内寻找地磁数据库中的可行解，经过时间递推和结果评价，计算偏移矩阵和偏移评价矩阵，缩小可行解空间大小，最终确定一个最优解作为匹配结果输出，具体包括以下步骤：

步骤1、载入惯性导航系统输出位置附近的地磁数据库，并根据惯性导航系统初始位置误差和磁传感器测量误差确定初始偏移矩阵和初始偏移评价矩阵，具体包括：

步骤101、根据下式计算偏移矩阵的中心坐标(O _x ⁽⁰⁾,O _y ⁽⁰⁾)，

O _x ⁽⁰⁾ =δ _x ⁽¹⁾/d _x ，O _y ⁽⁰⁾ =δ _y ⁽¹⁾/d _y  ，

式中，δ _x ⁽¹⁾、δ _y ⁽¹⁾分别为惯性导航系统在经度方向和纬度方向的初始位置误差，d _x 、d _y 分别为地磁数据库在经度方向和纬度方向的网格单位规格；

步骤102、根据下式计算偏移矩阵的大小为 N _x ⁽⁰⁾×N _y ⁽⁰⁾，

N _x ⁽⁰⁾ =2O _x ⁽⁰⁾ +1，N _y ⁽⁰⁾ =2O _y ⁽⁰⁾ +1 ；

步骤103、初始化偏移矩阵：记初始偏移矩阵为T ⁽⁰⁾=(t _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，其所有元素初始化为1；

步骤104、初始化偏移评价矩阵：记初始偏移评价矩阵为S ⁽⁰⁾=(s _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，其所有元素初始化为1；

步骤2、根据惯性导航系统输出位置和磁传感器输出地磁值，更新偏移矩阵和偏移评价矩阵，具体包括：

步骤201、查询地磁数据：在偏移矩阵T ^(k-1)中，寻找所有满足t _ij ^(k-1) =1，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾的索引坐标(i,j)，并求出其所对应的实际地理位置坐标p _ij ^(k)=(x _ij ^(k),y _ij ^(k))， x _ij ^(k)=x ₀ ^(k)+(i-O _x ⁽⁰⁾-1)d _x ，y _ij ^(k)=y ₀ ^(k)+(j-O _y ⁽⁰⁾-1)d _y ，然后在地磁数据库中查询出实际位置p _ij ^(k)对应的地磁值v _ij ^(k)；其中k为当前采样时刻，k=1,2,3…，x ₀ ^(k)、y ₀ ^(k)分别为当前k采样时刻惯性导航系统输出的经度方向和纬度方向的位置坐标；

步骤202、更新偏移矩阵：将偏移矩阵T ^(k-1)中值为1的元素t _ij ^(k-1)作如下处理，如果步骤201查询出的地磁值满足v _ij ^(k)∈[v ₀ ^(k) -3σ _v ,v ₀ ^(k) +3σ _v ]，则设为1，否则设为0，其中，v ₀ ^(k)为当前k采样时刻磁传感器输出的地磁值，σ _v 为磁传感器的测量标准差；令T ^(k)= T ^(k-1)；

步骤203、更新偏移评价矩阵：根据步骤202得到的偏移矩阵T ^(k)，按照下式更新偏移评价矩阵S ^(k-1)，

 s _ij ^(k-1) =s _ij ^(k-1) t _ij ^(k) g(v _ij ^(k)-v ₀ ^(k))，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，

其中，g(.)表示磁传感器测量误差的概率分布函数；

新得到的矩阵即为更新后的偏移评价矩阵S ^(k)；

步骤3、计算当前k采样时刻的结果评价因子，并判断是否输出当前匹配结果：

步骤301、计算偏移矩阵T ^(k)中元素值为1的所有元素之间关于索引坐标的最大距离，包括经度方向和纬度方向；

步骤302、利用地磁数据库的网格单位规格d _x ×d _y ，将步骤301得到的最大距离转换成实际地理长度，即为地磁匹配结果评价因子r ^(k)=[r _x ^(k),r _y ^(k)]，r _x ^(k)、r _y ^(k)分别为经度方向和纬度方向的实际地理长度；

步骤303、判断r ^(k)是否在预设允许误差范围R内，如否，返回步骤2；如是，则选出偏移评价矩阵S ^(k)中的最大元素s _mn ^(k)，(m,n)为该最大元素的索引坐标；并根据下式求出其所对应的当前地理位置，作为地磁匹配定位结果输出，

q ^(k)=(x ₀ ^(k)+(m-O _x ⁽⁰⁾-1)d _x ,y ₀ ^(k)+(n-O _y ⁽⁰⁾-1)d _y ) 。

本发明基于递推求解思想，采用实时评价的方式，在每一步递推求解后根据匹配情况来评价是否可以输出匹配结果，从而将算法时间分散在各个采样时刻。该方法克服了现有地磁匹配算法在数据批处理上实时性的不足，能够自适应调整匹配数据序列的长度，避免了现有地磁匹配算法需要预先指定匹配序列长度的困难。

附图说明

图1为本发明的地磁匹配导航方法的原理示意图；

图2为本发明的地磁匹配导航方法的算法流程图；

图3为本发明的地磁匹配导航方法的仿真程序结构图；

图4为本发明的地磁匹配导航方法的仿真实验1的结果；

图5为本发明的地磁匹配导航方法的仿真实验2的结果。

具体实施方式

下面结合附图对本发明的技术方案进行详细说明：

图1所示为本发明提出的地磁匹配导航方法的原理示意图，图中，x表示惯导输出的轨迹，m表示当前时刻的地磁测量值，p表示其测量误差，X表示偏移矩阵对应的可行解区域。采用本发明方法，随着时间递推计算，偏移矩阵对应的可行解区域越来越小（图1中的X ₁、X ₂到X _n），直到最后X _n只有一个可行解时，递推计算结束，并将该可行解作为地磁匹配结果（图1中的y _n）。

本发明的用于地磁/捷联惯导组合导航系统的地磁匹配导航方法的算法流程如图2所示，具体按照以下方法：

步骤1、算法初始化：载入惯性导航系统输出位置附近的地磁数据库，并启动地磁匹配导航算法；根据惯性导航系统初始位置误差和磁传感器测量误差确定初始偏移矩阵和初始偏移评价矩阵。

记地磁数据库的网格单位规格是d _x ×d _y （其中x表示经度，y表示纬度），磁传感器测量标准差为σ _v ，惯性导航系统初始位置误差为δ _p ⁽⁰⁾=(δ _x ⁽⁰⁾,δ _y ⁽⁰⁾)。

步骤101、按照下式计算偏移矩阵的中心坐标(O _x ⁽⁰⁾,O _y ⁽⁰⁾)：

O _x ⁽⁰⁾ =δ _x ⁽¹⁾/d _x ，O _y ⁽⁰⁾ =δ _y ⁽¹⁾/d _y ；

步骤102、根据下式计算偏移矩阵大小为 N _x ⁽⁰⁾×N _y ⁽⁰⁾：

N _x ⁽⁰⁾ =2O _x ⁽⁰⁾ +1，N _y ⁽⁰⁾ =2O _y ⁽⁰⁾ +1；

步骤103、初始化偏移矩阵：记初始偏移矩阵为T ⁽⁰⁾=(t _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，将所有元素初始化为1；

步骤104、初始化偏移评价矩阵：记初始偏移评价矩阵为S ⁽⁰⁾=(t _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，其所有元素初始化为1。

步骤2、时间递推计算：根据惯性导航系统输出位置和磁传感器输出地磁值，更新偏移矩阵和偏移评价矩阵。

记在采样时刻k(k=1,2,3…)，惯性导航系统输出位置为p ₀ ^(k)=(x ₀ ^(k),y ₀ ^(k))，磁传感器输出地磁值为v ₀ ^(k)。

步骤201、查询地磁数据：在偏移矩阵T ^(k-1)中，寻找所有满足t _ij ^(k-1) =1，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾的索引坐标(i,j)，并求出其所对应的实际地理位置坐标p _ij ^(k)=(x _ij ^(k),y _ij ^(k))， x _ij ^(k)=x ₀ ^(k)+(i-O _x ⁽⁰⁾-1)d _x ，y _ij ^(k)=y ₀ ^(k)+(j-O _y ⁽⁰⁾-1)d _y ，然后在地磁数据库中查询出实际位置p _ij ^(k)对应的地磁值v _ij ^(k)；

步骤202、更新偏移矩阵：将偏移矩阵T ^(k-1)中值为1的元素t _ij ^(k-1)作如下处理，如果步骤201查询出的地磁值满足v _ij ^(k)∈[v ₀ ^(k) -3σ _v ,v ₀ ^(k) +3σ _v ]，则设为1，否则设为0，其中，v ₀ ^(k)为当前k采样时刻磁传感器输出的地磁值，σ _v 为磁传感器的测量标准差；令T ^(k)= T ^(k-1)；

步骤203、更新偏移评价矩阵：根据步骤202得到的偏移矩阵T ^(k)，按照下式更新偏移评价矩阵S ^(k-1)，

 s _ij ^(k-1) =s _ij ^(k-1) t _ij ^(k) g(v _ij ^(k)-v ₀ ^(k))，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，

其中，g(.)表示磁传感器测量误差的概率分布函数，通常为标准正态分布函数；

新得到的矩阵即为更新后的偏移评价矩阵S ^(k)；

步骤3、匹配结果判断：计算当前采样时刻的结果评价因子，并判断是否输出当前匹配结果。

记在采样时刻k，地磁匹配结果评价因子为r ^(k)=[r _x ^(k),r _y ^(k)]，预设地磁匹配结果允许误差范围为R。

步骤301、计算偏移矩阵T ^(k)中元素值为1的所有元素之间关于索引坐标的最大距离，包括经度方向和纬度方向；

步骤302、利用地磁数据库的网格单位规格d _x ×d _y ，将步骤301得到的最大距离转换成实际地理长度，即为地磁匹配结果评价因子r ^(k)=[r _x ^(k),r _y ^(k)]，r _x ^(k)、r _y ^(k)分别为经度方向和纬度方向的实际地理长度；

步骤303、判断r ^(k)是否在预设允许误差范围R内，如否，返回步骤2；如是，则选出偏移评价矩阵S ^(k)中的最大元素，设(m,n)为该最大元素的索引坐标，则该最大元素可表示为s _mn ^(k)；并根据下式求出其所对应的当前地理位置，作为地磁匹配定位结果输出，

q ^(k)=(x ₀ ^(k)+(m-O _x ⁽⁰⁾-1)d _x ,y ₀ ^(k)+(n-O _y ⁽⁰⁾-1)d _y ) 。

本发明与现有技术相比，其优点在于：

（1）针对现有地磁匹配导航方法（包含相似度度量算法和等值线匹配算法）在数据批处理上实时性能的不足，本发明借鉴递推计算思想，将算法时间分散到各个采样时刻，避免了数据的批量处理，实时性能得到了提高。

（2）针对现有地磁匹配导航方法（包含相似度度量算法和等值线匹配算法）需要预先指定匹配数据序列长度的困难，本发明采用实时评价的方式，即设定结果评价因子，根据当前结果评价因子判断是否输出匹配定位结果。本发明的方法能够根据实际情况自适应调整匹配序列长度。

为了评价本发明提出的地磁匹配导航方法的性能，设计了仿真程序，结构如图3所示，该仿真程序包括以下步骤：

（1）产生一个预设的真实轨迹，输出随时间变化的导航参数（包括位置、速度、加速度、姿态、姿态角速率等）；

（2）在惯导仿真器中为该真实轨迹添加时间累积误差，生成惯导输出轨迹；

（3）在地磁数据库中找出真实轨迹上地理位置对应的地磁值，并添加测量误差，作为磁传感器的输出；

（4）利用磁传感器输出的地磁数据、惯导输出轨迹和地磁数据库进行地磁匹配，找出最佳的匹配轨迹，并与真实轨迹进行比较。

利用图3所示的仿真程序，进行了仿真实验1。实验中设定的仿真初始条件如下：（1）地磁数据库范围：经度为116.0~117.99°，纬度为31.0~32.99°；（2）地磁数据库的网格单位规格：经度0.01°，纬度0.01°；（3）磁传感器测量值标准差为1nT，测量误差服从标准正态分布；（4）惯导初始时间累积误差：经度方向为0.1°，纬度方向为0.1°，采用正负对称、均匀分布的随机常数；（5）运载体飞行轨迹为直线段。

在上述仿真条件下，采用本发明方法来进行地磁匹配导航，仿真结果如图4所示。从图4可以看出，经过5次递推计算，偏移矩阵中只剩下一个可行解，因此在时刻5将该可行解作为地磁匹配结果。实验结果表明本发明的地磁匹配结果与真实轨迹基本重合，证明了本发明采用递推评价算法的正确性。

同时，为了验证本发明的算法能够自适应调整匹配序列长度，进行了仿真实验2。仿真实验条件如下：（1）地磁数据库范围：经度为116.0~119.99°，纬度为31.0~34.99°；（2）地磁数据库的网格单位规格：经度0.01°，纬度0.01°；（3）磁传感器测量值标准差：0.5nT，测量误差服从标准正态分布；（4）惯导初始时间累积误差：经度方向为0.1°，纬度方向为0.1°，采用正负对称、均匀分布的随机常数；（5）运载体飞行轨迹为直线段。

在上述仿真条件下，采用本发明方法来进行地磁匹配导航，共进行500次实验，每次实验只改变惯导初始误差和磁传感器测量误差。仿真结果如图5所示。从图5可以看出，大部分情况下，采用长度为11的匹配数据序列就可以得到匹配定位结果。实验表明本发明方法能够根据惯性导航系统输出和磁传感器输出地磁值自适应调整匹配数据序列的长度，可得到有效的匹配定位结果。

Claims (1)

1.一种用于地磁/捷联惯导组合导航系统的地磁匹配导航方法，所述地磁/捷联惯导组合导航系统包括惯性导航系统、磁传感器及地磁数据库，其特征在于，该方法根据惯性导航系统输出轨迹和磁传感器输出地磁值，在惯性导航系统位置误差和磁传感器测量误差允许的范围内寻找地磁数据库中的可行解，经过时间递推和结果评价，计算偏移矩阵和偏移评价矩阵，缩小可行解空间大小，最终确定一个最优解作为匹配结果输出，具体包括以下步骤：

步骤1、载入惯性导航系统输出位置附近的地磁数据库，并根据惯性导航系统初始位置误差和磁传感器测量误差确定初始偏移矩阵和初始偏移评价矩阵，具体包括：

步骤101、根据下式计算偏移矩阵的中心坐标(O _x ⁽⁰⁾,O _y ⁽⁰⁾)，

O _x ⁽⁰⁾ =δ _x ⁽¹⁾/d _x ，O _y ⁽⁰⁾ =δ _y ⁽¹⁾/d _y  ，

式中，δ _x ⁽¹⁾、δ _y ⁽¹⁾分别为惯性导航系统在经度方向和纬度方向的初始位置误差，d _x 、d _y 分别为地磁数据库在经度方向和纬度方向的网格单位规格；

步骤102、根据下式计算偏移矩阵的大小为 N _x ⁽⁰⁾×N _y ⁽⁰⁾，

N _x ⁽⁰⁾ =2O _x ⁽⁰⁾ +1，N _y ⁽⁰⁾ =2O _y ⁽⁰⁾ +1 ；

步骤103、初始化偏移矩阵：记初始偏移矩阵为T ⁽⁰⁾=(t _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，其所有元素初始化为1；

步骤104、初始化偏移评价矩阵：记初始偏移评价矩阵为S ⁽⁰⁾=(s _ij ⁽⁰⁾)，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，其所有元素初始化为1；

步骤2、根据惯性导航系统输出位置和磁传感器输出地磁值，更新偏移矩阵和偏移评价矩阵，具体包括：

步骤201、查询地磁数据：在偏移矩阵T ^(k-1)中，寻找所有满足t _ij ^(k-1) =1，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾的索引坐标(i,j)，并求出其所对应的实际地理位置坐标p _ij ^(k)=(x _ij ^(k),y _ij ^(k))， x _ij ^(k)=x ₀ ^(k)+(i-O _x ⁽⁰⁾-1)d _x ，y _ij ^(k)=y ₀ ^(k)+(j-O _y ⁽⁰⁾-1)d _y ，然后在地磁数据库中查询出实际位置p _ij ^(k)对应的地磁值v _ij ^(k)；其中k为当前采样时刻，k=1,2,3…，x ₀ ^(k)、y ₀ ^(k)分别为当前k采样时刻惯性导航系统输出的经度方向和纬度方向的位置坐标；

步骤202、更新偏移矩阵：将偏移矩阵T ^(k-1)中值为1的元素t _ij ^(k-1)作如下处理，如果步骤201查询出的地磁值满足v _ij ^(k)∈[v ₀ ^(k) -3σ _v ,v ₀ ^(k) +3σ _v ]，则设为1，否则设为0，其中，v ₀ ^(k)为当前k采样时刻磁传感器输出的地磁值，σ _v 为磁传感器的测量标准差；令T ^(k)= T ^(k-1)；

步骤203、更新偏移评价矩阵：根据步骤202得到的偏移矩阵T ^(k)，按照下式更新偏移评价矩阵S ^(k-1)，

 s _ij ^(k-1) =s _ij ^(k-1) t _ij ^(k) g(v _ij ^(k)-v ₀ ^(k))，i=1,2,3,…,N _x ⁽⁰⁾，j=1,2,3,…,N _y ⁽⁰⁾，

其中，g(.)表示磁传感器测量误差的概率分布函数；

新得到的矩阵即为更新后的偏移评价矩阵S ^(k)；

步骤3、计算当前k采样时刻的结果评价因子，并判断是否输出当前匹配结果：

步骤301、计算偏移矩阵T ^(k)中元素值为1的所有元素之间关于索引坐标的最大距离，包括经度方向和纬度方向；

步骤302、利用地磁数据库的网格单位规格d _x ×d _y ，将步骤301得到的最大距离转换成实际地理长度，即为地磁匹配结果评价因子r ^(k)=[r _x ^(k),r _y ^(k)]，r _x ^(k)、r _y ^(k)分别为经度方向和纬度方向的实际地理长度；

步骤303、判断r ^(k)是否在预设允许误差范围R内，如否，返回步骤2；如是，则选出偏移评价矩阵S ^(k)中的最大元素s _mn ^(k)，(m,n)为该最大元素的索引坐标；并根据下式求出其所对应的当前地理位置，作为地磁匹配定位结果输出，

q ^(k)=(x ₀ ^(k)+(m-O _x ⁽⁰⁾-1)d _x ,y ₀ ^(k)+(n-O _y ⁽⁰⁾-1)d _y ) 。

Images