CN106197405A - 地磁日变影响下的惯性地磁匹配定位方法 - Google Patents

Abstract

本发明提供了一种地磁日变影响下的惯性地磁匹配定位方法，从惯导系统读取N个时刻的待匹配点的位置测量值a_i和b_i，从磁强计获得地磁场强度信息I_i；根据惯导系统指示的N个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值I(a_i,b_i)、地磁场强度的梯度的参考值I_x,i和I_y,i；引入并初始化经纬度误差、航向误差以及地磁日变误差；计算经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM；更新经纬度误差、航向误差和地磁日变误差M；判断是否满足终止迭代条件，根据更新后的M计算参数K和δK；根据迭代计算得到经纬度误差、航向误差和地磁日变误差，将所得结果代入匹配轨迹与参考轨迹的关系方程即得匹配轨迹。

Description

地磁日变影响下的惯性地磁匹配定位方法

技术领域

本发明涉及一种地磁日变影响下的惯性地磁匹配定位方法，属于惯性地磁匹配定位方法技术领域。

背景技术

导航技术随着现代科技的发展日新月异，总的来看主要有惯性导航、卫星导航、地形匹配导航、天文导航、地磁导航等导航技术。在各式各样的导航技术中，地磁导航以其高度自主性、隐蔽性和无积累误差等优势成为当今导航领域的一大研究热门。惯性地磁匹配是指，载体上安装有惯导系统和磁传感器，载体在飞行过程中，惯导系统输出载体的运动轨迹信息，将此输出轨迹称为“参考轨迹”，由于惯性器件存在漂移，因此参考轨迹与载体运动所经历的真实轨迹存在一定误差；在惯导系统输出载体轨迹的同时，由地磁传感器测量得到载体所经历位置的地磁场信息，然后根据预存于计算机内的地球磁场信息、地磁场实时测量信息以及载体参考轨迹，采用匹配方法，得到载体的匹配轨迹。地磁场包含稳定磁场和变化磁场两部分。前者包括主磁场和异常场，是地磁场的主体，也是地磁匹配所要用到的信息；后者起源于固体地球外的空间电流体系，相当于地磁测量时的干扰。地磁日变属于后者，包括太阳静日变化和太阴日变化，其平均变化幅度约为几纳特到几十纳特。目前，地磁日变并未形成较为统一的数学模型，不同地点的地磁日变曲线有较大的差异，同一地点的地磁日变曲线不同季节也不同、白天和晚上也不同，因此在惯性地磁匹配定位过程中难以进行有效的补偿或剔除。所以，地磁日变能够对惯性地磁匹配定位的精度及可靠性产生严重的影响。

基于上述原因，对地磁日变影响下的惯性地磁匹配定位方法的需要就十分迫切。然而由于日变的不确定性、复杂性，对日变十分难于处理，现有地磁导航方法中对这方面的处理非常少。在目前的地磁导航研究中有的文献是利用复杂方法拟合地磁日变曲线、有的文献虽然强调地磁日变影响，但是没能够给出处理地磁日变的方法，直接忽略地磁日变的影响。经文献检索，授权公告号为CN103115624B、授权公告日为2014年12月10日的中国发明专利，公开了“一种基于地磁匹配的地磁日变修正方法”，该专利基于FMI方法对日变进行了拟合，而且该方法需要导航当天以及前后共三天的地磁数据为基础进行计算，不易实际应用。谢仕民等在文章“地磁匹配导航关键技术浅析”中指出了地磁变化场(也即地磁日变)对导航精度产生较大误差，但并没能够提出校正地磁日变的方法。李豫泽等在文章“基于ICCP算法的地磁匹配定位方法”(见《科学计算及信息处理》)中则没有提及地磁日变，忽略了地磁日变的影响，无法保证匹配定位精度。

发明内容

本发明的目的是为了解决上述现有技术存在的问题，进而提供一种地磁日变影响下的惯性地磁匹配定位方法。

本发明的目的是通过以下技术方案实现的：

一种地磁日变影响下的惯性地磁匹配定位方法，步骤如下：

步骤一、从惯导系统读取当前时刻以及前N-1个时刻的待匹配点的位置测量值a_i和b_i，其中a_i表示经度，b_i表示纬度，下标i表示不同时刻，i＝1…N，N为整数且N＞2，i为1表示当前时刻，通过地磁传感器获得当前时刻以及前N-1个时刻的地磁场强度的测量值I_i；

步骤二、根据惯导系统指示的N个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值I(a_i,b_i)、地磁场强度的梯度的参考值I_x,i和I_y,i，其中I_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，I_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤三、引入并初始化经纬度误差、航向误差以及地磁日变误差：

M＝[Δx Δy α δ]^T＝[0 0 0 0]^T

步骤四、根据公式(1)、公式(2)和公式(3)计算迭代参数G、F和H：

G＝g(M) (1)

F＝f(M) (2)

H＝F^-1 (3)

其中：

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M)]^T；

$f\left(M\right)=\left[\begin{array}{cccc}{f}_{11}\left(M\right)& f_{12}\left(M\right)& f_{13}\left(M\right)& f_{14}\left(M\right)\\ f_{12}\left(M\right)& f_{22}\left(M\right)& f_{23}\left(M\right)& f_{24}\left(M\right)\\ f_{13}\left(M\right)& f_{23}\left(M\right)& f_{33}\left(M\right)& f_{34}\left(M\right)\\ f_{14}\left(M\right)& f_{24}\left(M\right)& f_{34}\left(M\right)& f_{44}\left(M\right)\end{array}\right];$

$\begin{array}{c}{g}_1\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_2\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_3\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}\\ +b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})\\ +I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]\end{array};$

$\begin{array}{c}{g}_4\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$f_{11}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}^2;$

$f_{12}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}{I}_{y,i};$

$f_{13}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

$f_{14}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i};$

$f_{22}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}^2;$

$f_{23}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

$f_{24}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i};$

$\begin{array}{c}{f}_{33}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})+I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]}^2\\ +\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}\\ -b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})+I_{y,i}(a_i^{\′}\sin \mathrm{\α}\\ -b_i^{\′}\cos \mathrm{\α})\]\end{array};$

$f_{34}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

f₄₄(M)＝N；

a_i′＝a_i-a₁,b_i′＝b_i-b₁,I_t,i＝I(a_i,b_i)-I_i；

步骤五、计算经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM：

δM＝-H×G (4)

步骤六、更新经纬度误差、航向误差和地磁日变误差M：

M＝M+δM (5)

步骤七、判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤十，否则跳到步骤八；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM的2范数小于设定值，即

||δM||₂＜ε (6)

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

步骤八、根据更新后的M计算参数K和δK：

K＝g(M) (7)

δK＝K-G (8)

步骤九、根据公式(9)和公式(10)更新迭代变量H和G，然后跳到步骤五，

H＝H+[(δM)-H(δK)](δM)^TH/[(δM)^TH(δK)] (9)

G＝K (10)

步骤十、根据迭代计算得到经纬度误差、航向误差和地磁日变误差Δx、Δy、α和δ，将所得结果代入匹配轨迹与参考轨迹的关系方程(11)即得匹配轨迹；

$\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\α}& sin\mathrm{\α}\\ -sin\mathrm{\α}& cos\mathrm{\α}\end{array}\right]\left[\begin{array}{c}{a}_i-a_1\\ b_i-b_1\end{array}\right]+\left[\begin{array}{c}{a}_1\\ b_1\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\end{array}\right]---\left(11\right)$

其中u_i为第i时刻匹配结果的位置经度，v_i为第i时刻匹配结果的位置纬度。

本发明以惯导系统输出的参考轨迹、计算机中的地磁图、地磁传感器实测值作为输入，用地磁日变影响下的惯性地磁匹配定位方法解算出参考轨迹初始经纬度误差、航向误差，将解算得到的经纬度误差、航向误差代入匹配轨迹与参考轨迹的关系方程即得到匹配结果。本发明所提出的地磁日变影响下的惯性地磁匹配定位方法具有较高的定位精度，而且重复迭代后，定位精度得到了进一步提高。

附图说明

图1为以某实验跑车为例实施本发明方法的流程图。

图2为经度误差曲线图。

图3为纬度误差曲线图。

具体实施方式

下面将对本发明做进一步的详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，但本发明的保护范围不限于下述实施例。

本实施例所涉及的一种地磁日变影响下的惯性地磁匹配定位方法，包括以下步骤：

步骤一、从惯导系统读取当前时刻以及前N-1个时刻的待匹配点的位置测量值a_i和b_i，其中a_i表示经度，b_i表示纬度，下标i表示不同时刻，i＝1…N，N为整数且N＞2，i为1表示当前时刻，通过地磁传感器获得当前时刻以及前N-1个时刻的地磁场强度的测量值I_i；

步骤二、根据惯导系统指示的N个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值I(a_i,b_i)、地磁场强度的梯度的参考值I_x,i和I_y,i，其中I_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，I_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤三、引入并初始化经纬度误差、航向误差以及地磁日变误差：

M＝[Δx Δy α δ]^T＝[0 0 0 0]^T

步骤四、根据公式(1)、公式(2)和公式(3)计算迭代参数G、F和H：

G＝g(M) (1)

F＝f(M) (2)

H＝F^-1 (3)

其中：

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M)]^T；

$f\left(M\right)=\left[\begin{array}{cccc}{f}_{11}\left(M\right)& f_{12}\left(M\right)& f_{13}\left(M\right)& f_{14}\left(M\right)\\ f_{12}\left(M\right)& f_{22}\left(M\right)& f_{23}\left(M\right)& f_{24}\left(M\right)\\ f_{13}\left(M\right)& f_{23}\left(M\right)& f_{33}\left(M\right)& f_{34}\left(M\right)\\ f_{14}\left(M\right)& f_{24}\left(M\right)& f_{34}\left(M\right)& f_{44}\left(M\right)\end{array}\right];$

$\begin{array}{c}{g}_1\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_2\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_3\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}\\ +b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})\\ +I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]\end{array};$

$\begin{array}{c}{g}_4\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$f_{11}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}^2;$

$f_{12}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}{I}_{y,i};$

$f_{13}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

$f_{14}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i};$

$f_{22}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}^2;$

$f_{23}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

$f_{24}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i};$

$\begin{array}{c}{f}_{33}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})+I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]}^2\\ +\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}\\ -b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})+I_{y,i}(a_i^{\′}\sin \mathrm{\α}\\ -b_i^{\′}\cos \mathrm{\α})\]\end{array};$

$f_{34}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

f₄₄(M)＝N；

a_i′＝a_i-a₁,b_i′＝b_i-b₁,I_t,i＝I(a_i,b_i)-I_i；

步骤五、计算经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM：

δM＝-H×G (4)

步骤六、更新经纬度误差、航向误差和地磁日变误差M：

M＝M+δM (5)

步骤七、判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤十，否则跳到步骤八；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM的2范数小于设定值，即

||δM||₂＜ε (6)

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

步骤八、根据更新后的M计算参数K和δK：

K＝g(M) (7)

δK＝K-G (8)

步骤九、根据公式(9)和公式(10)更新迭代变量H和G，然后跳到步骤五，

H＝H+[(δM)-H(δK)](δM)^TH/[(δM)^TH(δK)] (9)

G＝K (10)

步骤十、根据迭代计算得到经纬度误差、航向误差和地磁日变误差Δx、Δy、α和δ，将所得结果代入匹配轨迹与参考轨迹的关系方程(11)即得匹配轨迹；

$\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\α}& sin\mathrm{\α}\\ -sin\mathrm{\α}& cos\mathrm{\α}\end{array}\right]\left[\begin{array}{c}{a}_i-a_1\\ b_i-b_1\end{array}\right]+\left[\begin{array}{c}{a}_1\\ b_1\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\end{array}\right]---\left(11\right)$

其中u_i为第i时刻匹配结果的位置经度，v_i为第i时刻匹配结果的位置纬度。

前文所述地磁场强度可以选择地磁场总强度、地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量；当地磁场强度为地磁场总强度时，磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得地磁场总强度，作为地磁场强度的测量值I_i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁场总强度和地磁场总强度的梯度。当地磁场强度为地磁异常场总强度时，磁强计采用标量磁强计或者三轴矢量磁强计，从磁强计直接获得地磁场总强度，并根据地球磁场模型计算出地磁异常场总强度，作为地磁场强度的测量值I_i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁异常场总强度和地磁异常场总强度的梯度。当地磁场强度为地磁场总强度在地理坐标系下某一方向的分量时，磁强计采用三轴矢量磁强计，依据磁强计的三轴矢量测量值以及载体的姿态，计算出地磁场总强度在地理坐标系下该方向的分量，作为地磁场强度的测量值I_i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁场总强度在地理坐标系下该方向的分量及该分量的梯度。

地磁日变影响下的惯性地磁匹配定位方法还可以包括二次迭代步骤，所述二次迭代步骤的内容为：第二次执行所述步骤一到十，与第一次执行(即一次迭代)有如下两点不同：①用第一次执行所得到的匹配定位结果u_i和v_i替代所述步骤一中从惯导系统读取的a_i和b_i(如式(12)所示)，即将一次迭代得到的修正后的待匹配点位置值作为二次迭代的初始值；②用第一次执行所得到的地磁日变误差δ作为所述步骤三中的初始地磁日变误差(如式(13)所示)。由于二次迭代用的初值是经过第一次迭代获得的已经消除了大部分初始位置误差、大部分初始航向误差的值，再次通过迭代计算，其结果比第一次迭代更接近真实轨迹，使匹配定位结果精度得到进一步提高。

$\left\{\begin{array}{c}{a}_i=u_i\\ b_i=v_i\end{array}\right.---\left(12\right)$

M＝[0 0 0 δ]^T (13)

依照图1，以某实验跑车为例实施本发明地磁日变影响下的惯性地磁匹配定位方法的过程如下：

实验条件：地磁场选用地磁异常场总强度。选用质子磁力仪实时测量磁场信息，质子磁力仪的主要性能指标如下：分辨率：0.01nT，精度：±0.2nT。惯导系统的主要性能指标如下：陀螺零偏不稳定性：0.01°/h，陀螺随机游走：加速度计零偏不稳定性：80μg，加速度计随机游走：

将经度范围126°到129°和纬度范围44°到46°区间的地磁异常场总强度数据存入机载计算机，采用前向差分方法计算地磁异常场总强度的梯度信息并存入机载计算机；选取待匹配点个数为7个，即N＝7。

采用地磁日变影响下的惯性地磁匹配定位方法步骤如下：

步骤一：从惯性导航系统读取当前时刻以及前6个时刻的待匹配点的位置测量值a_i和b_i，如表1所示；根据质子磁力仪的测量值和地球磁场模型，得到当前时刻以及前6个时刻的磁场强度测量信息I_i如表2所示：

表1惯性导航系统测量位置坐标

i	经度ai(°)	纬度bi(°)
			1	127.1558	44.3980
2	127.1988	44.4154
			3	127.2619	44.4456
4	127.3237	44.4746
			5	127.3836	44.5033
6	127.4410	44.5324
			7	127.4946	44.5630

表2磁场强度测量信息

i	磁场强度Ii(nT)
		1	134.23
2	142.36
		3	157.68
4	148.19
		5	142.62
6	146.95
		7	162.27

步骤二：根据惯性导航系统指示的7个位置，分别从预先存储的地磁数据库中读取该位置的地磁场强度信息I(a_i,b_i)，以及该位置的地磁场强度的梯度信息I_x,i和I_y,i如表3所示：

表3地磁数据库中的地磁场强度和梯度信息

步骤三：初始化经纬度误差、航向误差以及地磁日变误差：

M＝[0 0 0 0]^T

步骤四：根据公式(1)、(2)和(3)计算迭代参数G、F和H

$G=\left[\begin{array}{c}-0.7557\\ -4.6567\\ 0.0495\\ -0.0116\end{array}\right]\×{10}^4$

$F=\left[\begin{array}{cccc}0.1878& 0.0494& 0.0237& 0.0008\\ 0.0494& 1.0446& -0.0328& 0.0014\\ 0.0237& -0.0328& 0.0092& 0.0001\\ 0.0008& 0.0014& 0.0001& 0.0001\end{array}\right]\×{10}^6$

$H=\left[\begin{array}{cccc}0.0161& 0.0013& 0.0201& 1.9347\\ 0.0013& 0.0023& 0.0112& 0.7456\\ 0.0201& 0.0112& 0.2207& 2.4284\\ 1.9347& 0.7456& 2.4284& 552.5414\end{array}\right]\×{10}^{-3}$

步骤五至步骤九：选取ε＝10^-6，预设迭代次数为50次。执行公式(4)、公式(5)、公式(7)至公式(10)，实施迭代算法。并根据公式(6)判断迭代终止条件，可知当迭代次数为7时迭代终止。迭代计算得到的经纬度误差、航向误差以及地磁日变误差M为：

M＝[-0.0325 0.0225 -0.0382 16.2869]^T

步骤十：将算得的误差值代入匹配轨迹与参考轨迹关系方程(11)，算得匹配后的经度、纬度值如表4所示：

表4匹配结果位置坐标

i	经度ui(°)	纬度vi(°)
			1	127.1233	44.4205
2	127.1656	44.4395
			3	127.2275	44.4721
4	127.2882	44.5035
			5	127.3469	44.5344
6	127.4032	44.5657
			7	127.4556	44.5983

上述迭代计算在计算机中耗时为15.5毫秒。

采用二次迭代来提高匹配定位方法的精度。二次迭代时，将步骤一中从惯导系统读取的待匹配点的位置测量值a_i和b_i由第一次迭代所得到的匹配定位结果u_i和v_i来替代，并将步骤三中初始地磁日变误差由第一次迭代所得到的地磁日变误差δ来替代。二次迭代步骤如下：

步骤一：用第一次的迭代结果(见表4)替代从惯导系统读取的7个时刻的待匹配点的位置测量值a_i和b_i，如表5所示；根据质子磁力仪的测量值和地球磁场模型，得到当前时刻以及前6个时刻的磁场强度测量信息I_i，如表2所示：

表5二次迭代时的待匹配点位置坐标初值

i	经度ai(°)	纬度bi(°)
			1	127.1233	44.4205
2	127.1656	44.4395
			3	127.2275	44.4721
4	127.2882	44.5035
			5	127.3469	44.5344
6	127.4032	44.5657
			7	127.4556	44.5983

步骤二：根据表5中的7个位置，从预先存储的地磁数据库中分别读取这7个位置的地磁场强度I(a_i,b_i)，以及该位置的地磁场强度的梯度I_x,i和I_y,i，如表6所示：

表6地磁数据库中的地磁场强度和梯度信息

步骤三：用第一次迭代所得到的地磁日变误差δ来初始化本次地磁日变误差，并初始化经纬度误差以及航向误差：

M＝[0 0 0 16.2869]^T

步骤四：根据公式(1)、(2)和(3)计算迭代参数G、F和H

$G=\left[\begin{array}{c}-4.0307\\ 0.4355\\ -1.1574\\ -0.0353\end{array}\right]\×{10}^3$

$F=\left[\begin{array}{cccc}2.8352& -0.0371& 0.5985& 0.0102\\ -0.0371& 5.3856& -0.6101& -0.0016\\ 0.5985& -0.6101& 0.2257& 0.003\\ 0.0102& -0.0016& 0.003& 0.0001\end{array}\right]\×{10}^5$

$H=\left[\begin{array}{cccc}0.0174& -0.0078& -0.0709& 0.2682\\ -0.0078& 0.0076& 0.0540& -0.9626\\ -0.0709& 0.0540& 0.5067& -9.7977\\ 0.2682& -0.9626& -9.7977& 495.3714\end{array}\right]\×{10}^{-3}$

步骤五至步骤九：选取ε＝10^-6，预设迭代次数为50次。执行公式(4)、公式(5)、公式(7)至公式(10)，实施迭代算法。并根据公式(6)判断迭代终止条件，可知当迭代次数为7时迭代终止。迭代计算得到的经纬度误差、航向误差以及地磁日变误差M为：

M＝[0.0055 -0.0086 -0.0903 24.2326]^T

步骤十：将算得的误差值代入匹配轨迹与参考轨迹关系方程(11)，算得匹配后的经度、纬度值如表7所示：

表7二次迭代匹配结果位置坐标

i	经度ui(°)	纬度vi(°)
			1	127.1288	44.4119
2	127.1693	44.4347
			3	127.2280	44.4727
4	127.2855	44.5094
			5	127.3413	44.5455
6	127.3945	44.5818
			7	127.4437	44.6190

两次迭代计算在计算机中总耗时为17.1毫秒。

为了说明校正地磁日变后的效果，我们还得到不校正地磁日变时的匹配结果位置坐标，如表8所示：

表8无地磁日变校正的匹配位置坐标

i	经度ui′(°)	纬度vi′(°)
			1	127.1841	44.4373
2	127.2299	44.4445
			3	127.2982	44.4596
4	127.3650	44.4739
			5	127.4298	44.4883
6	127.4923	44.5036
			7	127.5515	44.5213

为了验证实验结果，在实验跑车上安装GPS导航定位系统，从而可以得到7个时刻的真实位置坐标，如表9所示：

表9跑车真实位置坐标

i	经度(°)	纬度(°)
			1	127.1372	44.4094
2	127.1680	44.4314
			3	127.2232	44.4710
4	127.2786	44.5106
			5	127.3339	44.5502
6	127.3893	44.5898
			7	127.4447	44.6294

根据GPS导航定位系统的定位结果(表9)，可以绘出7个时刻的无日变校正的定位误差曲线、有日变校正的第一次迭代结果的定位误差曲线和有日变校正的第二次迭代结果的定位误差曲线，如图2和图3所示，其中图2给出了经度误差曲线，图3给出了纬度误差曲线。根据图2和图3可以看出，所提出的地磁日变影响下的惯性地磁匹配定位方法具有较高的定位精度，而且重复迭代后，定位精度会进一步提高。

以上所述，仅为本发明较佳的具体实施方式，这些具体实施方式都是基于本发明整体构思下的不同实现方式，而且本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (1)

1.一种地磁日变影响下的惯性地磁匹配定位方法，其特征在于，

步骤一、从惯导系统读取当前时刻以及前N-1个时刻的待匹配点的位置测量值a_i和b_i，其中a_i表示经度，b_i表示纬度，下标i表示不同时刻，i＝1…N，N为整数且N＞2，i为1表示当前时刻，通过地磁传感器获得当前时刻以及前N-1个时刻的地磁场强度的测量值I_i；

步骤二、根据惯导系统指示的N个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值I(a_i,b_i)、地磁场强度的梯度的参考值I_x,i和I_y,i，其中I_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，I_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤三、引入并初始化经纬度误差、航向误差以及地磁日变误差：

M＝[Δx Δy α δ]^T＝[0 0 0 0]^T

步骤四、根据公式(1)、公式(2)和公式(3)计算迭代参数G、F和H：

G＝g(M) (1)

F＝f(M) (2)

H＝F^-1 (3)

其中：

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M)]^T；

$f\left(M\right)=\left[\begin{array}{cccc}{f}_{11}\left(M\right)& f_{12}\left(M\right)& f_{13}\left(M\right)& f_{14}\left(M\right)\\ f_{12}\left(M\right)& f_{22}\left(M\right)& f_{23}\left(M\right)& f_{24}\left(M\right)\\ f_{13}\left(M\right)& f_{23}\left(M\right)& f_{33}\left(M\right)& f_{34}\left(M\right)\\ f_{14}\left(M\right)& f_{24}\left(M\right)& f_{34}\left(M\right)& f_{44}\left(M\right)\end{array}\right];$

$\begin{array}{c}{g}_1\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_2\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$\begin{array}{c}{g}_3\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}\\ +b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})\\ +I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]\end{array};$

$\begin{array}{c}{g}_4\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}\left(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x\right)\\ +I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}-b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\end{array};$

$f_{11}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}^2;$

$f_{12}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}{I}_{y,i};$

$f_{13}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i}\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})+I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\];$

$f_{14}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{x,i};$

$f_{22}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}^2;$

$f_{23}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i}\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})+I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\];$

$f_{24}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{I}_{y,i};$

$\begin{array}{c}{f}_{33}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}{\[I_{x,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α})+I_{y,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})\]}^2\\ +\[I_{x,i}(a_i^{\′}\cos \mathrm{\α}+b_i^{\′}\sin \mathrm{\α}-a_i^{\′}+\mathrm{\Δ}x)+I_{y,i}(-a_i^{\′}\sin \mathrm{\α}+b_i^{\′}\cos \mathrm{\α}\\ -b_i^{\′}+\mathrm{\Δ}y)+I_{t,i}+\mathrm{\δ}\]\×\[I_{x,i}(-a_i^{\′}\cos \mathrm{\α}-b_i^{\′}\sin \mathrm{\α})+I_{y,i}(a_i^{\′}\sin \mathrm{\α}\\ -b_i^{\′}\cos \mathrm{\α})\]\end{array};$

$f_{34}\left(M\right)=\underset{i=1}{\overset{N}{\Σ}}\[I_{x,i}(-a_i^{\′}sin\mathrm{\α}+b_i^{\′}cos\mathrm{\α})+I_{y,i}(-a_i^{\′}cos\mathrm{\α}-b_i^{\′}sin\mathrm{\α})\];$

f₄₄(M)＝N；

a′_i＝a_i-a₁，b′_i＝b_i-b₁，I_t,i＝I(a_i,b_i)-I_i；

步骤五、计算经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM：

δM＝-H×G (4)

步骤六、更新经纬度误差、航向误差和地磁日变误差M：

M＝M+δM (5)

步骤七、判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤十，否则跳到步骤八；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②经纬度误差的增量、航向误差的增量和地磁日变误差的增量δM的2范数小于设定值，即

||δM||₂＜ε (6)

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

步骤八、根据更新后的M计算参数K和δK：

K＝g(M) (7)

δK＝K-G (8)

步骤九、根据公式(9)和公式(10)更新迭代变量H和G，然后跳到步骤五，

H＝H+[(δM)-H(δK)](δM)^TH/[(δM)^TH(δK)] (9)

G＝K (10)

步骤十、根据迭代计算得到经纬度误差、航向误差和地磁日变误差Δx、Δy、α和δ，将所得结果代入匹配轨迹与参考轨迹的关系方程(11)即得匹配轨迹；

$\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\α}& sin\mathrm{\α}\\ -sin\mathrm{\α}& cos\mathrm{\α}\end{array}\right]\left[\begin{array}{c}{a}_i-a_1\\ b_i-b_1\end{array}\right]+\left[\begin{array}{c}{a}_1\\ b_1\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\end{array}\right]---\left(11\right)$

其中u_i为第i时刻匹配结果的位置经度，v_i为第i时刻匹配结果的位置纬度。