CN103344242A - 基于地磁强度和梯度的地磁匹配导航方法 - Google Patents

Abstract

本发明公开了一种基于地磁强度和梯度的地磁匹配导航方法，其特征在于：1)建立基于积分形式指标函数的正则化模型；2)采用离散化手段对正则化模型进行极小化求解，得线性方程组；3)采用迭代算法求解方程组；4)根据方程组的解修正轨迹，得到匹配结果。该技术利用了地磁场强度信息以及地磁场强度的梯度信息，采用迭代算法实现地磁匹配导航，能够有效的解决地磁等值线匹配算法中匹配精度、匹配步长和匹配算法实时性相互制约的问题。

Description

基于地磁强度和梯度的地磁匹配导航方法

技术领域

本发明涉及一种载体定位导航方法，具体涉及一种基于地磁强度和地磁强度梯度的地磁匹配导航方法。

背景技术

无论是在军事还是在民用领域，导航技术都已成为一项不可或缺的关键技术。目前导航手段较多，大致可以分为惯性器件、无线电信号和自然环境信息三类方法。惯性器件导航是应用最广泛的手段，它具有自主性强、不易受外界干扰等特点，但是存在误差累计问题。无线电信号包括地面雷达导航和GPS导航等，这类导航方法精度较高，克服了误差累积问题，但是成本较高且易受人为干扰，特别是应用在军事领域具有一定的风险。利用自然环境导航方法包括星光、太阳、地球、地形和地磁场等方法。

地磁导航作为一种无源、无辐射的导航方法，其体积小、质量轻、误差不随时间积累、不易受人为干扰、可应用范围广而且相对成本较低，其可以在诸多方面弥补其他两类导航方法的不足，因此具有极其重要的研究价值。地磁辅助导航可以有效的整合各导航方法的优点，并且实现优势互补。

地磁匹配导航算法的基本思想是在基准图中找出一组数据使其与磁传感器采集得到数据实现最佳匹配，其具有较高的捕获概率，能够断续使用，主要包括匹配准则的选取和匹配搜索方法这两个问题。匹配准则分为两类：一类是强调差别程度的平均绝对差算法(MSD)、均方差算法(MAD)、Hausdorff距离等准则；另一类是强调相似程度的互相关算法(COR)、积相关算法(PROD)和频域相关等准则。地磁等值线匹配算法(MAGCOM)因为简单、易实现成为匹配搜索方法中一种常用的算法，然而该算法要求遍历有效范围内所有平行于待匹配轨迹的序列，算法的实时性受到搜索范围和搜索步长的制约。

文献“基于增强型MAD单特征量地磁匹配导航算法”(《现代防御技术》，2012，40(1)，90-94)，研究分析了影响地磁匹配定位时间的主要因素，并采用粗精匹配结合的分层搜索策略来缩短匹配时间，然而该算法并没有从根本上解决匹配精度、匹配步长和匹配算法实时性相互制约的问题。

发明内容

基于上述不足之处，本发明提供了一种基于地磁强度和梯度的地磁匹配导航方法，本方法同时利用地磁场强度和地磁场强度的梯度信息，通过匹配实现定位导航。本方法能够在保证精度的条件下有效的提高匹配速度。

本发明的方法如下：

(1)建立基于积分形式指标函数的正则化模型：

$\∫{(\▿m\·e+(m-l))}^2+\mathrm{\λ}\left(\right||\▿e_x|{|}^2+\left|\right|\▿e_y\left|{|}^2\right)\mathrm{ds}---\left(1\right)$

其中，s为待匹配轨迹，m为待匹配轨迹上某点对应的地磁数据库中磁场强度，

为该点在地磁数据库中磁场强度的梯度，x为经度坐标，y为纬度坐标，e＝[e_x，e_y]^T为该点与真实轨迹对应点在x和y方向上的偏差，l为磁强计在该点的实际磁场测量值，为该点与真实轨迹对应点经度偏差的梯度，

为该点与真实轨迹对应点纬度偏差的梯度，λ是拉格朗日乘数，根据地磁数据库中坐标量纲的大小选择λ，使 ${(\▿m\·e+(m-l))}^2$ 与 $\mathrm{\λ}\left(\right||\▿e_x|{|}^2+\left|\right|\▿e_y\left|{|}^2\right)$ 处于同一数量级。

(2)采用离散化手段对上述模型进行极小化求解，得如下方程组

$\left\{\begin{array}{c}(\mathrm{\λ}+{\left(\frac{\∂m_i}{\∂x_i}\right)}^2)e_{\mathrm{xi}}+\frac{\∂m_i}{\∂x_i}\frac{\∂m_i}{\∂y_i}{e}_{\mathrm{yi}}=\mathrm{\λ}{\stackrel{\‾}{e}}_{\mathrm{xi}}-\frac{\∂m_i}{\∂x_i}(m_i-l_i)\\ \frac{\∂m_i}{\∂x_i}\frac{\∂m_i}{\∂y_i}{e}_{\mathrm{xi}}+(\mathrm{\λ}+{\left(\frac{\∂m_i}{\∂y_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\λ}{\stackrel{\‾}{e}}_{\mathrm{yi}}-\frac{\∂m_i}{\∂y_i}(m_i-l_i)\end{array}\right.---\left(2\right)$

其中i＝1，2，…，N，N为待匹配总的点数，m_i为待匹配轨迹上第i点对应的地磁数据库中磁场强度，

为地磁数据库中磁场强度的梯度在第i点上的数值，e_xi为待匹配轨迹第i点与真实轨迹对应点的经度偏差，e_yi为待匹配轨迹第i点与真实轨迹对应点的纬度偏差，l_i为磁强计在第i点的实际磁场测量值，

和

为待匹配轨迹第i点与真实轨迹对应点经度偏差平均值和纬度偏差平均值，按如下方式计算：

$\left\{\begin{array}{c}{\stackrel{\‾}{e}}_{\mathrm{xi}}=\frac{1}{2}(e_{x(i-1)}+e_{x(i+1)})\\ {\stackrel{\‾}{e}}_{\mathrm{yi}}=\frac{1}{2}(e_{y(i-1)}+e_{y(i+1)})\end{array}\right.---\left(3\right)$

(3)对式(2)采用迭代算法求解，可以求解出待匹配轨迹第i点与真实轨迹对应点的经度偏差e_xi和纬度偏差e_yi。

(4)匹配结果为

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.---\left(4\right)$

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi待匹配轨迹第i点的经度，r_yi为待匹配轨迹第i点纬度。

本发明还具有如下特征：

(1)采用惯性导航系统、磁强计和导航计算机，通过迭代算法求解式(2)，即可实现地磁匹配，采用雅克比迭代法，具体包括以下步骤：

步骤1：在待匹配区域，根据地磁数据库中地磁强度数据，计算出地磁场梯度信息并与强度信息一起存储于导航计算机中；

步骤2：在载体运动过程中，从惯性导航系统得到载体运动的N个待匹配轨迹点，并在导航计算机中找出待匹配轨迹点对应的磁场强度和梯度数据；同时从磁强计得到载体在匹配点的磁场强度实际测量值；

步骤3：设迭代计数器k＝0，初始化轨迹的经度偏差值e_xi(k)和纬度偏差值e_yi(k)：

$\left\{\begin{array}{c}{e}_{\mathrm{xi}}\left(k\right)=e_{\mathrm{xi}}\left(0\right)=0\\ e_{\mathrm{yi}}\left(k\right)=e_{\mathrm{yi}}\left(0\right)=0\end{array}\right.----\left(5\right)$

其中i＝1，2，…，N，N为待匹配总的点数；

步骤4：计算第k次迭代经度偏差平均值

和纬度偏差平均值

首先，对第2到N-1个点中的任意一点，把与之相邻两点的偏差值的平均值作为该点的偏差平均值：

$\left\{\begin{array}{c}{\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)=\frac{1}{2}(e_{x(i-1)}\left(k\right)+e_{x(i+1)}\left(k\right))\\ {\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)=\frac{1}{2}(e_{y(i-1)}\left(k\right)+e_{y(i+1)}\left(k\right))\end{array}\right.---\left(6\right)$

其中i＝2，3，…，N-1；

然后，对第1和第N点的偏差平均值进行更新如下：

$\left\{\begin{array}{c}{\stackrel{\‾}{e}}_{x1}\left(k\right)=e_{x2}\left(k\right)\\ {\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)=e_{y2}\left(k\right)\end{array}\right.---\left(7\right)$

$\left\{\begin{array}{c}{\stackrel{\‾}{e}}_{\mathrm{xN}}\left(k\right)=e_{x(N-1)}\left(k\right)\\ {\stackrel{\‾}{e}}_{\mathrm{yN}}\left(k\right)=e_{y(N-1)}\left(k\right)\end{array}\right.---\left(8\right)$

步骤5：利用如下迭代公式求解偏差e_xi(k+1)，e_yi(k+1)

$\left\{\begin{array}{c}{e}_{\mathrm{xi}}(k+1)={\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)-\frac{\∂m_i}{\∂x_i}\frac{\frac{\∂m_i}{\∂x}{\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\∂m_i}{\∂y}{\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)+(m_i-l_i)}{\mathrm{\λ}+{\left(\frac{\∂m_i}{\∂x_i}\right)}^2+{\left(\frac{\∂m_i}{\∂y_i}\right)}^2}\\ e_{\mathrm{yi}}(k+1)={\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)-\frac{\∂m_i}{\∂x}\frac{\frac{\∂m_i}{\∂x}{\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\∂m_i}{\∂y}{\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)+(m_i-l_i)}{\mathrm{\λ}+{\left(\frac{\∂m_i}{\∂x_i}\right)}^2+{\left(\frac{\∂m_i}{\∂y_i}\right)}^2}\end{array}\right.---\left(9\right)$

其中i＝1，2，…，N；

步骤6：判断是否满足终止迭代条件，若满足则停止迭代并返回e_xi(k+1)和e_yi(k+1)作为迭代结果e_xi和e_yi，否则k＝k+1，重复执行步骤4～步骤6。

终止条件有两个，满足任意一个即终止迭代：①迭代次数达到预设次数；②迭代误差小于设定值，即

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}---\left(10\right)$

其中ε为预先设定的迭代最小误差。

步骤7：根据上步输出的迭代结果e_xi和e_yi，计算匹配结果为

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.---\left(4\right)$

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi为待匹配轨迹第i点的经度，r_yi为待匹配轨迹第i点的纬度；

(2)采用高斯赛德尔迭代法或者逐次超松弛迭代法替换雅克比迭代法，以加快收敛速度。

(3)地磁强度可选用地磁场总强度或者地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量。若地磁强度选用地磁场总强度，磁强计可以选用标量磁强计或者三轴矢量磁强计，得到的磁场总强度作为步骤2中磁强计的实际测量值。若地磁强度选用地磁异常场总强度，磁强计可以选用标量磁强计或者三轴矢量磁强计，得到磁场总强度，并根据地球磁场模型计算出地磁异常场总强度作为步骤2中磁强计的实际测量值。若地磁强度选用地磁场总强度在地理坐标系下某一方向的分量，磁强计应选用三轴矢量磁强计，依据磁强计的测量值以及载体的姿态，计算出磁强计测量值在地理坐标系下该方向的分量作为步骤2中磁强计的实际测量值。

本发明的有益效果是：本发明采用惯性导航系统、磁强计和导航计算机，同时利用了磁场强度信息以及磁场强度的梯度信息，通过迭代计算实现地磁匹配导航，该方法可以有效的解决地磁等值线匹配算法中匹配精度、匹配步长和匹配算法实时性相互制约的问题，改善导航计算机的计算搜索效率，提高匹配导航过程的实时性。

附图说明

图1是本发明的原理流程图。

图2是本发明的实现流程图。

具体实施方式

下面结合实例和附图对本发明进一步详细说明。

实施例1

1.匹配方法的原理

匹配方法的原理参见图1。

地磁图上任意点(x，y)^T所对应的磁场强度为m(x，y)，其中x为该点的经度，y为该点的纬度。设待匹配轨迹上某点为(r_x，r_y)^T，与其对应的真实轨迹上的点为(t_x，t_y)^T，则有

m(r_x+e_x，r_y+e_y)＝m(t_x，t_y)    (12)

其中e_x＝t_x-r_x和e_y＝t_y-r_y分别为待匹配点和真实点的经度偏差和纬度偏差。因为待匹配轨迹在真实轨迹附近，所以将上式左边进行泰勒展开可得

$m(r_x,r_y)+\frac{\&PartialD;m}{\&PartialD;x}{|}_{{(x,y)}^T={(r_x,r_y)}^T}{e}_x+\frac{\&PartialD;m}{\&PartialD;y}{|}_{{(x,y)}^T={(r_x,r_y)}^T}{e}_y+R_2=m(t_x,t_y)$

其中R₂代表阶数大于和等于2的项。忽略R₂，并将其写成矩阵形式

$\&dtri;m\&CenterDot;e{|}_{{(x,y)}^T={(r_x,r_y)}^T}+(m(r_x,r_y)-l(r_x,r_y))=0---\left(14\right)$

其中

为地磁数据库中磁场强度的梯度，e＝[e_x，e_y]^T为该点与真实轨迹对应点在x和y方向上的偏差，

为

在待匹配点(r_x，r_y)^T上的取值，m(r_x，r_y)为待匹配点(r_x，r_y)^T所对应地磁图库中的磁场强度，l(r_x，r_y)为在该匹配点所测得的实际磁场强度大小，即m(t_x，t_y)。

式(14)是关于经度偏差e_x和纬度偏差e_y的一个方程。此外，假设经度偏差e_x和纬度偏差e_y在整个待匹配轨迹上平滑，并将式(14)沿待匹配轨迹积分，则可以得到积分形式指标函数的正则化模型，如式(15)所示。满足使模型最小的e_x和e_yy即为所求。

$\&Integral;{(\&dtri;m\&CenterDot;e+(m-l))}^2+\mathrm{\&lambda;}\left(\right||\&dtri;e_x|{|}^2+\left|\right|\&dtri;e_y\left|{|}^2\right)\mathrm{ds}---\left(15\right)$

其中，s为待匹配轨迹，m为待匹配轨迹上某点对应的地磁数据库中磁场强度，

为该点在地磁数据库中磁场强度的梯度，e＝[e_x，e_y］^T为该点与真实轨迹对应点在x和y方向的偏差，l为磁强计在该点的实际磁场测量值，

为该点与真实轨迹对应点经度偏差的梯度，为该点与真实轨迹对应点纬度偏差的梯度，λ是拉格朗日乘数，根据地磁数据库中坐标量纲的大小选择λ，使

与

处于同一数量级。

将积分形式指标函数的正则化模型离散化处理，可以得到如下形式的指标函数

$T=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(c_i+\mathrm{\&lambda;}{s}_i)---\left(16\right)$

其中

$c_i={(\frac{\&PartialD;m_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;m_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(m_i-l_i))}^2---\left(17\right)$

$s_i=\frac{1}{2}[{(e_{\mathrm{xi}}-e_{x(i-1)})}^2+{(e_{x(i-1)}-e_{\mathrm{xi}})}^2+{(e_{\mathrm{yi}}-e_{y(i-1)})}^2+{(e_{y(i+1)}-e_{\mathrm{yi}})}^2]---\left(18\right)$

其中i＝1，2，…，N，N为待匹配总的点数，m_i为待匹配轨迹上第i点对应的地磁数据库中磁场强度，

为地磁数据库中磁场强度的梯度在第i点上的数值，e_xi为待匹配轨迹第i点与真实轨迹对应点的经度偏差，e_yi为待匹配轨迹第i点与真实轨迹对应点的纬度偏差，l_i为磁强计在第i点的实际磁场测量值。

T关于e_xi和e_yi的偏微分是

$\left\{\begin{array}{c}\frac{\&PartialD;T}{{\&PartialD;e}_{\mathrm{xi}}}=2(\frac{{\&PartialD;m}_i}{{\&PartialD;x}_i}{e}_{\mathrm{xi}}+\frac{{\&PartialD;m}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(m_i-l_i))\frac{\&PartialD;m_i}{\&PartialD;x_i}+2\mathrm{\&lambda;}(e_{\mathrm{xi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}})\\ \frac{\&PartialD;T}{\&PartialD;e_{\mathrm{yi}}}=2(\frac{\&PartialD;m_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;m_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(m_i-l_i))\frac{\&PartialD;m_i}{\&PartialD;y_i}+2\mathrm{\&lambda;}(e_{\mathrm{yi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}})\end{array}\right.---\left(19\right)$

其中

和

为待匹配轨迹第i点与真实轨迹对应点经度偏差平均值和纬度偏差平均值，按如下方式计算：

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}=\frac{1}{2}(e_{x(i-1)}+e_{x(i+1)})\\ {\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}=\frac{1}{2}(e_{y(i-1)}+e_{y(i+1)})\end{array}\right.---\left(20\right)$

当式(19)的两个偏微分为零时，即可使模型取最小值，即有

$\left\{\begin{array}{c}(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;m_i}{\&PartialD;x_i}\right)}^2)e_{\mathrm{xi}}+\frac{\&PartialD;m_i}{\&PartialD;x_i}\frac{\&PartialD;m_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}-\frac{\&PartialD;m_i}{\&PartialD;x_i}(m_i-l_i)\\ \frac{\&PartialD;m_i}{\&PartialD;x_i}\frac{\&PartialD;m_i}{\&PartialD;y_i}{e}_{\mathrm{xi}}+(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;m_i}{\&PartialD;y_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}-\frac{\&PartialD;m_i}{\&PartialD;y_i}(m_i-l_i)\end{array}\right.---\left(21\right)$

式(21)可以采用雅克比迭代、高斯赛德尔迭代、逐次超松弛迭代等迭代算法求解，得到待匹配轨迹第i点与真实轨迹对应点的经度偏差e_xi和纬度偏差e_yi。

则匹配结果为

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.---\left(22\right)$

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi为待匹配轨迹第i点的经度，p_yi为待匹配轨迹第i点的纬度。

实施例2

具体实施步骤

采用惯性导航系统、磁强计和导航计算机，通过迭代计算求解式(2)，即可实现地磁匹配，以雅克比迭代为例，参见图2，本方法的实施步骤如下：

步骤1：在待匹配区域，根据地磁数据库中地磁强度数据，计算出地磁场梯度信息并与强度信息一起存储于导航计算机中；

步骤2：在载体运动过程中，从惯性导航系统得到载体运动的N个待匹配轨迹点，并在导航计算机中找出待匹配轨迹点对应的磁场强度和梯度数据；同时从磁强计得到载体在匹配点的磁场强度实际测量值；

步骤3：设迭代计数器k＝0，初始化轨迹的经度偏差值e_xi(k)和纬度偏差值e_yi(k)：

$\left\{\begin{array}{c}{e}_{\mathrm{xi}}\left(k\right)=e_{\mathrm{xi}}\left(0\right)=0\\ e_{\mathrm{yi}}\left(k\right)=e_{\mathrm{yi}}\left(0\right)=0\end{array}\right.----\left(23\right)$

其中i＝1，2，…，N，N为待匹配总的点数；

步骤4：计算第k次迭代经度偏差平均值和纬度偏差平均值

首先，对第2到N-1个点中的任意一点，把与之相邻两点的偏差值的平均值作为该点的偏差平均值：

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)=\frac{1}{2}(e_{x(i-1)}\left(k\right)+e_{x(i+1)}\left(k\right))\\ {\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)=\frac{1}{2}(e_{y(i-1)}\left(k\right)+e_{y(i+1)}\left(k\right))\end{array}\right.---\left(24\right)$

其中i＝2，3，…，N-1；

然后，对第1和第N点的偏差平均值进行更新如下：

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{e}}_{x1}\left(k\right)=e_{x2}\left(k\right)\\ {\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)=e_{y2}\left(k\right)\end{array}\right.---\left(25\right)$

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{e}}_{\mathrm{xN}}\left(k\right)=e_{x(N-1)}\left(k\right)\\ {\stackrel{\&OverBar;}{e}}_{\mathrm{yN}}\left(k\right)=e_{y(N-1)}\left(k\right)\end{array}\right.---\left(26\right)$

步骤5：利用如下迭代公式求解偏差e_xi(k+1)，e_yi（k+1)

$\left\{\begin{array}{c}{e}_{\mathrm{xi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)-\frac{\&PartialD;m_i}{\&PartialD;x_i}\frac{\frac{\&PartialD;m_i}{\&PartialD;x}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;m_i}{\&PartialD;y}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(m_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;m_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;m_i}{\&PartialD;y_i}\right)}^2}\\ e_{\mathrm{yi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)-\frac{\&PartialD;m_i}{\&PartialD;x}\frac{\frac{\&PartialD;m_i}{\&PartialD;x}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;m_i}{\&PartialD;y}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(m_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;m_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;m_i}{\&PartialD;y_i}\right)}^2}\end{array}\right.---\left(27\right)$

其中i＝1，2，…，NN；

步骤6：判断是否满足终止迭代条件，若满足则停止迭代并返回e_xi(k+1)和e_yi(k+1)作为迭代结果e_xi和e_yi，否则k＝k+1，重复执行步骤4～步骤6。

终止条件有两个，满足任意一个即终止迭代：①迭代次数达到预设次数；②迭代误差小于设定值，即

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}---\left(28\right)$

其中ε为预先设定的迭代最小误差。

步骤7：根据上步输出的迭代结果e_xi和e_yi，计算匹配结果为

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.---\left(29\right)$

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi为待匹配轨迹第i点的经度，r_yi为待匹配轨迹第i点的纬度。

迭代算法可以采用高斯赛德尔迭代或者逐次超松弛迭代等方法替换雅克比迭代以加快收敛速度。

地磁强度可选用地磁场总强度或者地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量。若地磁强度选用地磁场总强度，磁强计可以选用标量磁强计或者三轴矢量磁强计，得到的磁场总强度作为步骤2中磁强计的实际测量值。若地磁强度选用地磁异常场总强度，磁强计可以选用标量磁强计或者三轴矢量磁强计，得到磁场总强度，并根据地球磁场模型计算出地磁异常场总强度作为步骤2中磁强计的实际测量值。若地磁强度选用地磁场总强度在地理坐标系下某一方向的分量，磁强计应选用三轴矢量磁强计，依据磁强计的测量值以及载体的姿态，计算出磁强计测量值在地理坐标系下该方向的分量作为步骤2中磁强计的实际测量值。

实施例3

一种采用上述实施步骤实现对运动载体的匹配导航的具体实施例：

以下列运动参数为例，载体位于东经16°至东经17°、北纬50°至北纬51°之间运动，载体真实初始位置的经纬度为(16.7°，50.3°)，载体在经度方向的速度为0.015°/s，载体在纬度方向的速度为0.01°/s，载体上惯性导航系统的初始经度误差为-0.01°，纬度误差为0.01°。并以匹配点总数为10，即N＝10为例实施匹配算法。匹配时，惯性导航系统采用三轴陀螺仪和三轴加速度计，磁强计采用三轴矢量磁强计，地磁强度采用地磁异常场总强度。

步骤1：根据美国国家地理数据中心(NDGC)与2008年公布的地磁异常网格EMAG3得到该待区间的地磁异常信息，采用样条插值得到分辨率为0.001°的地磁异常场总强度信息，并采用前向差分方法计算对应点的地磁异常场总强度的梯度信息，将强度信息与梯度信息一起存储于载体导航计算机中。

步骤2：根据运动参数，从惯性导航系统得到的10个位置点坐标分别为(16.710°，50.290°)，(16.725°，50.310°)，(16.740°，50.330°)，(16.755°，50.350°)，(16.770°，50.370°)，(16.785°，50.390°)，(16.800°，50.410°)，(16.815°，50.430°)，(16.830°，50.450°)，(16.845°，50.470°)。根据上述10个待匹配点，在导航计算机找出每个点所对应的地磁异常场总强度值为：m₁＝-39.82nT，m₂＝-36.15nT，m₃＝-32.43nT，m₄＝-28.88nT，m₅＝-25.78nT，m₆＝-23.38nT，m₇＝-21.86nT，m₈＝-21.36nT，m₉＝-21.98nT，m₁₀＝-23.62nT；每个点的磁场梯度值为： $\&PartialD;m_1/\&PartialD;x_1=4.02,$ $\&PartialD;m_2/\&PartialD;x_2=-6.33,$ $\&PartialD;m_3/\&PartialD;x_3=-19.98,$ $\&PartialD;m_4/\&PartialD;x_4=-36.59,$ $\&PartialD;m_5/\&PartialD;x_5=-55.99,$ $\&PartialD;m_6/\&PartialD;x_6=-77.01,$ $\&PartialD;m_7/\&PartialD;x_7=-98.72,$ $\&PartialD;m_8/\&PartialD;x_8=-119.92,$ $\&PartialD;m_9/\&PartialD;x_9=-136.54,$ $\&PartialD;m_{10}/\&PartialD;x_{10}=-146.08,$ $\&PartialD;m_1/\&PartialD;y_1=175.19,$ $\&PartialD;m_2/\&PartialD;y_2=191.38,$ $\&PartialD;m_3/\&PartialD;y_3=199.04,$ $\&PartialD;m_4/\&PartialD;y_4=195.75,$ $\&PartialD;m_5/\&PartialD;y_5=181.16,$ $\&PartialD;m_6/\&PartialD;y_6=157.35,$ $\&PartialD;m_7/\&PartialD;y_7=125.86,$ $\&PartialD;m_8/\&PartialD;y_8=87.03,$ $\&PartialD;m_9/\&PartialD;y_9=44.59,$ $\&PartialD;m_{10}/\&PartialD;y_{10}=4.59;$ 同时，从磁强计得到当地磁场总强度信息，并根据国际地磁参考场(IGRF)，得到地磁异常场的实际测量值为：l₁＝-38.02nT，l₂＝-34.10nT，l₃＝-30.17nT，l₄＝-26.50nT，l₅＝-23.38nT，l₆＝-21.02nT，l₇＝-19.62nT，l₈＝-19.32nT，l₉＝-20.20nT，l₁₀＝-22.16nT。

步骤3至步骤6：选取λ＝10000，ε＝1×10^-12，根据步骤2采集的数据以及迭代初始条件，执行公式(24)至公式(27)，实施迭代算法。并根据公式(28)判断迭代终止条件，可知，当迭代次数k＝231时，迭代终止，输出的迭代结果为：e_x1＝-0.0094°，e_x2＝-0.0094°，e_x3＝-0.0094°，e_x4＝-0.0095°，e_x5＝-0.0095°，e_x6＝-0.0095°，e_x7＝-0.0095°，e_x8＝-0.0096°，e_x9＝-0.0096°，e_x10＝-0.0097°，e_y1＝0.0105°，e_y2＝0.0104°，e_y3＝0.0104°，e_y4＝0.0104°，e_y5＝0.0104°，e_y6＝0.0103°，e_y7＝0.0103°，e_y8＝0.0103°，e_y9＝0.0103°，e_y10＝0.0103°。

步骤7：根据公式(29)计算匹配结果为：P_x1＝16.7006°，P_x2＝16.7156°，P_x3＝16.7306°，P_x4＝16.7455°，P_x5＝16.7605°，P_x6＝16.7755°，P_x7＝16.7905°，P_x8＝16.8054°，P_x9＝16.8204°，P_x10＝16.8353°，P_y1＝50.3005°，p_y2＝50.3204°，P_y3＝50.3404°，P_y4＝50.3604°，P_y5＝50.3804°，P_y6＝50.4003°，P_y7＝50.4203°，P_y8＝50.4403°，P_y9＝50.4603°，P_y10＝50.4803°。

根据匹配结果可以得出，经度方向上的匹配误差平均值为0.00049°，纬度方向上的匹配误差平均值为0.00036°。

采用计算机实现匹配导航，由步骤3运行至步骤7输出匹配结果，计算机程序运行1.7毫秒，改善了匹配导航计算机的计算搜索效率，提高了匹配导航过程的实时性。

Claims (4)

1.一种基于地磁强度和梯度的地磁匹配导航方法，其特征在于： 

(1)建立基于积分形式指标函数的正则化模型： 

其中，s为待匹配轨迹，m为待匹配轨迹上某点对应的地磁数据库中磁场强度， 

为该点在地磁数据库中磁场强度的梯度，x为经度坐标，y为纬度坐标，e＝[e_x，e_y］^T为该点与真实轨迹对应点在x和y方向上的偏差，l为磁强计在该点的实际磁场测量值，

为该点与真实轨迹对应点经度偏差的梯度，为该点与真实轨迹对应点纬度偏差的梯度，λ是拉格朗日乘数，根据地磁数据库中坐标量纲的大小选择λ，使 

与

处于同一数量级。 

(2)采用离散化手段对上述模型进行极小化求解，得如下方程组 

其中i＝1，2，…，N，N为待匹配总的点数，m_i为待匹配轨迹上第i点对应的地磁数据库中磁场强度，

为地磁数据库中磁场强度的梯度在第i点上的数值，e_xi为待匹配轨迹第i点与真实轨迹对应点的经度偏差，e_yi为待匹配轨迹第i点与真实轨迹对应点的纬度偏差，l_i为磁强计在第i点的实际磁场测量值， 

和

为待匹配轨迹第i点与真实轨迹对应点经度偏差平均值和纬度偏差平均值，按如下方式计算： 

(3)对式(2)采用迭代算法求解，求解出待匹配轨迹第i点与真实轨迹对应点的经度偏差e_xi和纬度偏差e_yi。 

(4)匹配结果为 

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi为待匹配轨迹第i点的经度，r_yi为待匹配轨迹第i点的纬度。 

2.根据权利要求1所述的基于地磁强度和梯度的地磁匹配导航方法，其特征在 于：采用惯性导航系统、磁强计和导航计算机，通过迭代计算求解式(2)，即能实现地磁匹配，采用雅克比迭代法，包括以下步骤： 

步骤1：在待匹配区域，根据地磁数据库中地磁强度数据，计算出地磁场梯度信息并与强度信息一起存储于导航计算机中； 

步骤2：在载体运动过程中，从惯性导航系统得到载体运动的N个待匹配轨迹点，并在上导航计算机中找出待匹配轨迹点对应的磁场强度和梯度数据；同时从磁强计得到载体在匹配点的磁场强度实际测量值； 

步骤3：设迭代计数器k＝0，初始化轨迹的经度偏差值e_xi(k)和纬度偏差值e_yi(k)： 

其中i＝1，2，…，N； 

步骤4：计算第k次迭代经度偏差平均值和纬度偏差平均值

首先，对第2到N-1个点中的任意一点，把与之相邻两点的偏差值的平均值作为该点的偏差平均值： 

其中i＝2，3，…，N-1； 

然后，对第1和第N点的偏差平均值进行更新如下： 

步骤5：利用如下迭代公式求解偏差e_xi(k+1)，e_yi(k+1) 

其中i＝1，2，…，N； 

步骤6：判断是否满足终止迭代条件，若满足则停止迭代并返回e_xi(k+1)和e_yi(k+1)作为迭代结果e_xi和e_yi，否则k＝k+1，重复执行步骤4～步骤6。 

终止条件有两个，满足任意一个即终止迭代：①迭代次数达到预设次数；②迭代误差小于设定值，即 

其中ε为预先设定的迭代最小误差； 

步骤7：根据上步输出的迭代结果e_xi和e_yi，计算匹配结果为 

其中p_xi为匹配结果第i点的经度，p_yi为匹配结果第i点的纬度，r_xi为待匹配轨迹第i点的经度，r_yi为待匹配轨迹第i点的纬度。 

3.根据权利要求2所述的基于地磁强度和梯度的地磁匹配导航方法，其特征在于：采用高斯赛德尔迭代法或者逐次超松弛迭代法替换雅克比迭代法，以加快收敛速度。 

4.根据权利要求2所述的基于地磁强度和梯度的地磁匹配导航方法，其特征在于：地磁强度选用地磁场总强度或者地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量；当地磁强度选用地磁场总强度，磁强计选用标量磁强计或者三轴矢量磁强计，得到的磁场总强度作为步骤2中磁强计的实际测量值；当地磁强度选用地磁异常场总强度，磁强计选用标量磁强计或者三轴矢量磁强计，得到磁场总强度，并根据地球磁场模型计算出地磁异常场总强度作为步骤2中磁强计的实际测量值；当地磁强度选用地磁场总强度在地理坐标系下某一方向的分量，磁强计应选用三轴矢量磁强计，依据磁强计的测量值以及载体的姿态，计算出磁强计测量值在地理坐标系下该方向的分量作为步骤2中磁强计的实际测量值。 

Images