CN104833373A - 一种基于Procrustes的地磁匹配导航方法 - Google Patents

Abstract

本发明公开了一种基于Procrustes的地磁匹配导航方法，包括以下步骤：获取载体的指示航迹序列H和真实航迹序列L的序列值；同时，利用磁传感器测得磁场值序列；将指示航迹序列H作为待匹配航迹序列H_C，然后根据磁场值序列在地磁参考图上生成磁场值等值线，并在等值线上提取最近点序列P，将最近点序列P作为匹配目标；寻找最优参数，将待匹配航迹序列H_C与匹配目标进行匹配；根据最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S计算得到匹配后的指示航迹序列，则将指示航机序列匹配到匹配点序列。本发明的一种基于Procrustes的地磁匹配导航方法，在能够修正航迹旋转和平移误差的基础上，还能够修正由于航迹形变造成的缩放误差。

Description

一种基于Procrustes的地磁匹配导航方法

技术领域

本发明属于地磁导航技术领域，涉及一种基于Procrustes的地磁匹配导航方法。

背景技术

惯性导航系统(INS，Inertial Navigation System)是目前大多数导航设备都会选择的一种导航手段，它是一种自主式的导航技术，即导航定位过程中不接收外来信息的同时也不向外辐射信息。INS具有输出数据平稳和短时间内稳定性好等优点，但是由于INS在进行导航定位时，采用的是递推计算，从而使得其导航误差会随着时间的增加而积累。因此，需要定期采用其他辅助手段对其航迹进行误差修正，否则会导致导航定位精度的降低。地磁匹配导航通过利用地球磁场所提供的天然坐标系，能够完成无源自主导航定位，具有良好的隐蔽性，并且可以消除INS航迹的累积误差。

在地磁匹配导航中不可缺少的核心技术是地磁匹配导航算法，匹配算法的性能对最终的导航定位精度和导航效率有直接影响。地磁匹配ICCP算法是目前地磁匹配导航技术中最为重要的一种匹配算法。但ICCP算法中的刚性变换运算只修正INS航迹的旋转误差和平移误差，并没有修正因航迹形变而造成的缩放误差。

发明内容

本发明的目的是提供一种基于Procrustes的地磁匹配导航方法，在能够修正航迹旋转和平移误差的基础上，还能够修正由于航迹形变造成的缩放误差。

本发明所采用的技术方案是，一种基于Procrustes的地磁匹配导航方法，包括以下步骤：

步骤1，由惯性导航系统获取载体的指示航迹序列H和真实航迹序列L的序列值，分别记为(x_i,y_i,z_i)和(X_i,Y_i,Z_i)；同时，利用磁传感器测得磁场值序列，记为m_j，i表示指示航迹序列H和真实航迹序列L中采样点的编号，j表示磁场值序列中的磁场值编号；

步骤2，将指示航迹序列H作为待匹配航迹序列H_C，然后根据磁场值序列m_j在地磁参考图上生成磁场值等值线，并在等值线上提取最近点序列P，将最近点序列P作为匹配目标；

步骤3，寻找最优参数，将待匹配航迹序列H_C与匹配目标进行匹配；

步骤4，根据最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S计算得到匹配后的指示航迹序列，则将指示航机序列匹配到匹配点序列。

本发明的特点还在于，

步骤3中寻找最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S的过程为：

步骤3.1，设置迭代次数k，并计算第k次的旋转矩阵R、缩放因子系数λ和平移向量S；1≤k≤N，k取整数；

步骤3.2，根据旋转矩阵R、缩放因子系数λ和平移向量S对待匹配航迹序列H_C进行更新，将待匹配航迹序列H_C更新后，记为H_Ck；

H_Ck＝HR^Tλ+S^T；

步骤3.3，将旋转矩阵R、缩放因子系数λ和平移向量S代入以下误差值E的函数式中：

$E(P,H)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|P_j-({\mathrm{HR}}^T\mathrm{\λ}+S^T)\left|\right|}^2;$

步骤3.4，返回步骤2继续执行，求取k＝2时的参数，即旋转矩阵R、缩放因子系数λ和平移向量S；并判断本次迭代过程中的误差与上一次迭代过程中误差的差值大小，如果两次迭代过程的误差差值大于期望值，则将H_Ck赋值给待匹配航迹序列H_C，并返回步骤2继续迭代；如果两次迭代过程中的误差差值小于或等于期望值，则结束，即寻找到最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S。

步骤3.1具体为：

步骤A，根据步骤1获得的指示航迹序列H和真实航迹序列L的序列值，求取缩放因子系数λ和旋转矩阵R；具体过程如下：

步骤a，分别按照以下方法求取指示航迹序列H的重心坐标

$\stackrel{\‾}{H}=\frac{\mathrm{\ΣH}}{n};$

和按照以下方法求取真实航迹序列L的重心坐标

$\stackrel{\‾}{L}=\frac{\mathrm{\ΣL}}{n};$

步骤b，以重心坐标作为平移量对指示航迹序列H进行平移，得到平移后的重心坐标L_S：

$L_S=L-\stackrel{\‾}{L};$

和以重心坐标作为平移量对真实航迹序列L进行平移，得到平移后的重心坐标H_S：

$H_S=H-\stackrel{\‾}{H};$

步骤c，根据平移后的重心坐标L_S、H_S来构造指示航迹序列H匹配到真实航迹序列L的矩阵表达式，如下式所示：

F＝H_S ^T*L_S；

步骤d，对步骤c中的表达式进行奇异值分解，得到：

F＝UDiag(σ₁,σ₂,σ₃)V^T；

步骤e，根据步骤d中的公式得到旋转矩阵R＝UV^T；

步骤f，根据求得的旋转矩阵R求解缩放因子系数λ，具体按照以下公式求解：

$\mathrm{\λ}=\frac{{\mathrm{trL}}^T{\mathrm{CHR}}^T}{{\mathrm{trH}}^T\mathrm{CH}};$

其中，n为航迹序列中的采样点的数量；

步骤B，根据步骤A中求得的旋转矩阵R和缩放因子系数λ计算平移向量S，具体过程为：

首先，根据旋转矩阵R和缩放因子系数λ计算偏移量矩阵W，具体为：

$W=L-{\mathrm{HR}}^T\mathrm{\λ}=\left[\begin{array}{c}\begin{array}{ccc}{X}_{01}& Y_{01}& Z_{01}\end{array}\\ \begin{array}{ccc}{X}_{02}& Y_{02}& Z_{02}\end{array}\\ ............\\ \begin{array}{ccc}{X}_{0n}& Y_{0n}& Z_{0n}\end{array}\end{array}\right];$

然后根据上式中的偏移量平均值，计算平移向量S：

$S=[\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_{0i},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Y}_{0i},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Z}_{0i}].$

本发明的有益效果是通过在传统的ICCP算法中加入Procrustes来代替传统的刚性变换，使得匹配参数的获取不仅考虑了旋转和平移，而且也考虑了缩放误差的因素，从而达到在能够修正航迹旋转和平移误差的基础上，还能够修正由于航迹形变造成的缩放误差。

附图说明

图1是本发明一种基于Procrustes的地磁匹配导航方法的流程示意图；

图2是本发明实验中所采用的地磁基准图；

图3是本发明的方法中缩放因子系数为0.6时与传统ICCP算法的匹配结果对比图；

图4是本发明的方法中缩放因子系数为0.8时与传统ICCP算法的匹配结果对比图；

图5是本发明的方法中缩放因子系数为1.0时与传统ICCP算法的匹配结果对比图；

图6是本发明的方法中缩放因子系数为1.2时与传统ICCP算法的匹配结果对比图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种基于Procrustes的地磁匹配导航方法，如图1所示，包括以下步骤：

步骤1，由惯性导航系统获取载体的指示航迹序列H和真实航迹序列L的序列值，分别记为(x_i,y_i,z_i)和(X_i,Y_i,Z_i)；同时，利用磁传感器测得磁场值序列，记为m_j，i表示指示航迹序列H和真实航迹序列L中采样点的编号，指示航迹序列H和真实航迹序列L中采样点一一对应；j表示磁场值序列中的磁场值编号；

步骤2，将指示航迹序列H作为待匹配航迹序列H_C，然后根据磁场值序列m_j在地磁参考图上生成磁场值等值线，并在等值线上提取最近点序列P_j，将最近点序列P_j作为匹配目标；

步骤3，寻找最优参数，将待匹配航迹序列H_C与匹配目标进行匹配；

寻找最优参数的过程为：

步骤3.1，设置迭代次数k，并计算第k次的旋转矩阵R、缩放因子系数λ和平移向量S；1≤k≤N，k取整数；

步骤A，根据步骤1获得的指示航迹序列H和真实航迹序列L的序列值，求取缩放因子系数λ和旋转矩阵R；具体过程如下：

步骤a，分别按照以下方法求取指示航迹序列H的重心坐标

$\stackrel{\‾}{H}=\frac{\mathrm{\ΣH}}{n};$

和按照以下方法求取真实航迹序列L的重心坐标

$\stackrel{\‾}{L}=\frac{\mathrm{\ΣL}}{n};$

步骤b，以重心坐标作为平移量对指示航迹序列H进行平移，得到平移后的重心坐标L_S：

$L_S=L-\stackrel{\‾}{L};$

和以重心坐标作为平移量对真实航迹序列L进行平移，得到平移后的重心坐标H_S：

$H_S=H-\stackrel{\‾}{H};$

步骤c，根据平移后的重心坐标L_S、H_S来构造指示航迹序列H匹配到真实航迹序列L的矩阵表达式，如下式所示：

F＝H_S ^T*L_S；

式中，H_S ^T为H_S的转置矩阵；

步骤d，对步骤c中的表达式进行奇异值分解，得到：

F＝UDiag(σ₁,σ₂,σ₃)V^T；

步骤e，根据步骤d中的公式得到旋转矩阵R＝UV^T；

步骤f，根据求得的旋转矩阵R求解缩放因子系数λ，具体按照以下公式求解：

$\mathrm{\λ}=\frac{{\mathrm{trL}}^T{\mathrm{CHR}}^T}{{\mathrm{trH}}^T\mathrm{CH}};$

其中，n为指示航迹序列中的采样点的总数；

步骤B，根据步骤A中求得的旋转矩阵R和缩放因子系数λ计算平移向量S，具体过程为：

首先，根据旋转矩阵R和缩放因子系数λ计算偏移量矩阵W，具体为：

$W=L-{\mathrm{HR}}^T\mathrm{\λ}=\left[\begin{array}{c}\begin{array}{ccc}{X}_{01}& Y_{01}& Z_{01}\end{array}\\ \begin{array}{ccc}{X}_{02}& Y_{02}& Z_{02}\end{array}\\ ............\\ \begin{array}{ccc}{X}_{0n}& Y_{0n}& Z_{0n}\end{array}\end{array}\right];$

然后根据上式中的偏移量平均值，计算平移向量S：

$S=[\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_{\mathrm{oi}},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Y}_{\mathrm{oi}},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Z}_{\mathrm{oi}}];$

步骤3.2，根据旋转矩阵R、缩放因子系数λ和平移向量S对待匹配航迹序列H_C进行更新，将待匹配航迹序列H_C更新后，记为H_Ck；

H_Ck＝HR^Tλ+S^T；

步骤3.3，将旋转矩阵R、缩放因子系数λ和平移向量S代入以下误差值E的函数式中：

$E(P,H)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|P_j-({\mathrm{HR}}^T\mathrm{\λ}+S^T)\left|\right|}^2$

步骤3.4，返回步骤2继续执行，求取k＝2时的参数，即旋转矩阵R、缩放因子系数λ和平移向量S；并判断本次迭代过程中的误差与上一次迭代过程中误差的差值大小，如果两次迭代过程的误差差值大于期望值，则将H_Ck赋值给待匹配航迹序列H_C，并返回步骤2继续迭代；如果两次迭代过程中的误差差值小于或等于期望值，则结束，即寻找到最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S；

上述步骤3中的期望值为在具体的工程应用中，根据需求进行设置；

步骤4，根据最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S计算得到匹配后的指示航迹序列，则将指示航机序列匹配到匹配点序列。

下面通过实验对本发明的方法的效果进行说明：

首先选用大小为80×100的地磁图作为待匹配的基准图，仿真参数依次设置为采样时间间隔为1s，采样点数为30，基准噪声、高斯噪声、均匀噪声和基准图噪声分别为7dB、11dB、15dB和10dB。

然后由磁传感器采集到磁场值，与此同时，惯性导航系统输出相应采样时刻的位置序列，以此构成匹配序列和待匹配序列。进而对所选用地磁基准图进行判断，判断其是否是磁异常区，判断结果如图2所示，然后在缩放因子系数分别为0.6、0.8、1.0和1.2的情况下分别采用传统ICCP算法和本发明的基于Procrustes的地磁匹配导航方法对惯导真实航迹和惯导指示航迹的进行匹配，如图3、图4、图5和图6所示，可以看到传统ICCP算法由于缩放误差的存在而导致匹配轨迹发散，而基于Procrustes的地磁匹配导航方法则能够很好地匹配上真实轨迹。

Claims (3)

1.一种基于Procrustes的地磁匹配导航方法，其特征在于，包括以下步骤：

步骤1，由惯性导航系统获取载体的指示航迹序列H和真实航迹序列L的序列值，分别记为(x_i,y_i,z_i)和(X_i,Y_i,Z_i)；同时，利用磁传感器测得磁场值序列，记为m_j，i表示指示航迹序列H和真实航迹序列L中采样点的编号，j表示磁场值序列中的磁场值编号；

步骤2，将指示航迹序列H作为待匹配航迹序列H_C，然后根据磁场值序列m_j在地磁参考图上生成磁场值等值线，并在等值线上提取最近点序列P，将最近点序列P作为匹配目标；

步骤3，寻找最优参数，将待匹配航迹序列H_C与匹配目标进行匹配；

步骤4，根据最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S计算得到匹配后的指示航迹序列，则将指示航机序列匹配到匹配点序列。

2.根据权利要求1所述的一种基于Procrustes的地磁匹配导航方法，其特征在于，所述步骤3中寻找最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S的过程为：

步骤3.1，设置迭代次数k，并计算第k次的旋转矩阵R、缩放因子系数λ和平移向量S；1≤k≤N，k取整数；

步骤3.2，根据旋转矩阵R、缩放因子系数λ和平移向量S对待匹配航迹序列H_C进行更新，将待匹配航迹序列H_C更新后，记为H_Ck；

H_Ck＝HR^Tλ+S^T；

步骤3.3，将旋转矩阵R、缩放因子系数λ和平移向量S代入以下误差值E的函数式中：

$E(P,H)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|P_j-({\mathrm{HR}}^T\mathrm{\λ}+S^T)\left|\right|}^2;$

步骤3.4，返回步骤2继续执行，求取k＝2时的参数，即旋转矩阵R、缩放因子系数λ和平移向量S；并判断本次迭代过程中的误差与上一次迭代过程中误差的差值大小，如果两次迭代过程的误差差值大于期望值，则将H_Ck赋值给待匹配航迹序列H_C，并返回步骤2继续迭代；如果两次迭代过程中的误差差值小于或等于期望值，则结束，即寻找到最优旋转矩阵R、最优缩放因子系数λ和最优平移向量S。

3.根据权利要求2所述的一种基于Procrustes的地磁匹配导航方法，其特征在于，所述步骤3.1具体为：

步骤A，根据步骤1获得的指示航迹序列H和真实航迹序列L的序列值，求取缩放因子系数λ和旋转矩阵R；具体过程如下：

步骤a，分别按照以下方法求取指示航迹序列H的重心坐标

$\stackrel{\‾}{H}=\frac{\mathrm{\ΣH}}{n};$

和按照以下方法求取真实航迹序列L的重心坐标

$\stackrel{\‾}{L}=\frac{\mathrm{\ΣL}}{n};$

步骤b，以重心坐标作为平移量对指示航迹序列H进行平移，得到平移后的重心坐标L_S：

$L_S=L-\stackrel{\‾}{L};$

和以重心坐标作为平移量对真实航迹序列L进行平移，得到平移后的重心坐标H_S：

$H_S=H-\stackrel{\‾}{H};$

步骤c，根据平移后的重心坐标L_S、H_S来构造指示航迹序列H匹配到真实航迹序列L的矩阵表达式，如下式所示：

F＝H_S ^T*L_S；

步骤d，对步骤c中的表达式进行奇异值分解，得到：

F＝UDiag(σ₁,σ₂,σ₃)V^T；

步骤e，根据步骤d中的公式得到旋转矩阵R＝UV^T；

步骤f，根据求得的旋转矩阵R求解缩放因子系数λ，具体按照以下公式求解：

$\mathrm{\λ}=\frac{{\mathrm{trL}}^T{\mathrm{CHR}}^T}{{\mathrm{trH}}^T\mathrm{CH}};$

其中，n为航迹序列中的采样点的数量；

步骤B，根据步骤A中求得的旋转矩阵R和缩放因子系数λ计算平移向量S，具体过程为：

首先，根据旋转矩阵R和缩放因子系数λ计算偏移量矩阵W，具体为：

$W=L-{\mathrm{HR}}^T\mathrm{\λ}=\left[\begin{array}{ccc}{X}_{01}& Y_{01}& Z_{01}\\ X_{02}& Y_{02}& Z_{02}\\ ....& ....& ....\\ X_{0n}& Y_{0n}& Z_{0n}\end{array}\right];$

然后根据上式中的偏移量平均值，计算平移向量S：

$S=[\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_{0i},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Y}_{0i},\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Z}_{0i}].$