-
惯性导航作为水下潜器导航定位最常用的技术，具有自主性好、精度高、可靠性好以及抗干扰性强等优点。然而，该项技术也存在缺点，即在轨迹解算时，系统误差会随着时间积累，若不进行定期重调，定位误差会无限增长，从而无法适应潜器长航时、高精度自主导航的要求。为了克服惯性导航系统（inertial navigation system, INS）的系统误差随时间积累的缺陷，需要采用其他导航方式来辅助惯性导航，以修正INS产生的累积误差。利用地磁异常数据作为辅助信息是解决水下潜器长航时、高精度自主导航问题的有效途径，具有隐蔽性强，不受敌方干扰、误差不随时间积累以及磁传感器体积小、功耗低等优点，其与INS组成的组合导航系统具有无源、全天候、无辐射、全地域及能耗低等优良特性[1-2]。
早在20世纪60年代中期，E-Systems就在水上验证了地磁制导的构想，并研究得到了MAGCOM系统。经过不断科研攻关，美国海军利用Lockheed P-3得到了实验区的地磁异常数据[3]。随后，英国提出了基于地磁图的组合导航技术，猜想通过比较载体存储的地磁图和实际磁测数据来获得可靠的位置估计[4-5]。Psiaki[6]率先开展基于地磁测量的卫星导航方法，根据地磁场幅值信息，研究利用最小二乘滤波和扩展卡尔曼滤波（extended Kalman filtering, EKF）实现低轨航天器的定轨和地磁场模型误差修正。基于EKF和伪线性卡尔曼滤波的混合算法，美国宇航局戈达德飞行中心[7]在广角红外探测器上对地磁导航进行了大量验证性试验，并给出了磁强计/太阳敏感器组合导航6个月的飞行试验结果，其位置误差、速度误差、姿态角误差和姿态角速度误差分别为45~60 km、50 m/s、1°~2°以及0.003°/s~0.008°/s。Kim等[8]提出了基于粒子滤波的地磁匹配算法，解决了因初值误差过大导致的EKF算法不收敛问题，运算速度也得到了有效提高。Kwon等[9]使用i-Maro3移动机器人进行了地磁导航相关实验，采用粒子滤波技术为移动机器人提供地磁导航信息，验证了复杂磁场环境下实现室内地磁导航的可行性。Armstrong等[10]采用EKF算法提高了地磁扰动期间的导航精度，通过一系列的仿真和实验分析，验证了该算法的有效性。王向磊等[11]利用Matlab仿真了基于地磁场的自主导航系统，并引入EKF算法使得系统以地磁场矢量为观测量的导航精度明显提高。赵建虎等[12]利用多面函数优化局域地磁场模型，研究基于最近等值线迭代（the iterated closest contour point, ICCP）的水下地磁自主导航算法，有效克服了水下导航定位方法的局限性。基于地磁噪声为有色噪声的特性，文献[13-14]在子滤波器观测为实测地磁总强度下，采用EKF算法进行巡航导弹巡航段飞行过程的数据仿真分析，提出基于Sage-Husa自适应卡尔曼滤波的导航算法，通过调整测量噪声矩阵来消除磁暴影响。施桂国等[15]通过同步测量运行航迹上的地磁场和重力梯度特征，固定滑动窗口构造测量序列与基准图进行匹配来得到实时位置信息。王胜平[16]针对桑迪亚惯性地形辅助导航（Sandia inertial terrain-aided navigation, SITAN）算法在初始位置误差过大而易出现滤波发散的问题，提出了改进地形轮廓匹配与ICCP辅助的SITAN改进算法，有效提高了滤波收敛速度。解伟男等[17]针对惯性/地磁匹配组合导航系统提出了一种基于迭代计算的地磁轮廓线匹配新算法，有效修正了惯导系统的初始位置误差和初始航向误差，具有较高的实时性。
因此，匹配算法的改进成为当前地磁匹配导航研究的重点[18-19]。滤波匹配算法在重力匹配导航中已得到应用，如美国通用重力模块中的匹配计算机就是采用该算法，其本质上是一种基于EKF的最佳估计算法。EKF作为一种次优估计方法被广泛应用到众多非线性工程中，但该算法也存在不足，如线性化误差会引起不稳定，估计误差会随着系统噪声和测量噪声的统计特征不准确而增大[20-21]。为了提高EKF算法的稳定性和可靠性，本文提出了基于量测残差自适应估计观测噪声方差的地磁异常并行扩展卡尔曼滤波（residual-based adaptive estimated parallel extended Kalman filtering, RAE-PEKF）匹配算法。
-
假定INS初始位置存在误差，若将磁传感器实测地磁异常值和INS输出点处地磁图上的参考值作差，并以此差值作为唯一量测值时，有可能会因为量测不可信而导致滤波发散。通过建立一组基于地磁异常观测量的并行滤波器来增加EKF算法的可用性和鲁棒性，并行滤波器的个数由初始位置误差和地磁图分辨率来决定。利用卡尔曼滤波产生的各个滤波器解算结果构成平滑算子，用经过判断准则优化的地磁异常的对应位置信息来修正水下潜器的导航信息。
-
通常搭载在水下潜器上的测深仪可提供比较精确的深度信息，且水下潜器本身航行比较平缓。因此，本文研究是基于并行扩展卡尔曼滤波（parallel extended Kalman filtering，PEKF）匹配算法实时解算INS的平面位置误差X，即X为滤波状态变量。在采样间隔很短的情况下，结合惯导平面位置误差方程，给出如下状态转移矩阵[22]：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k, k - 1}} = \left[ {\begin{array}{*{20}{c}} 1&0\\ {\frac{{{v_e}{\rm{sin}}\varphi }}{{\left( {{R_N} + h} \right){\rm{co}}{{\rm{s}}^2}\varphi }}}&1 \end{array}} \right] $$ (1) 式中，ve为东向速度；φ为纬度；h为高度；RN为卯酉圈曲率半径；${\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k, k - 1}}$为状态转移矩阵。
假定载体真实位置的地磁异常${\rm{\Delta }}T\left( {\varphi , \lambda } \right)$在惯导指示点$\left( {{\varphi _i}, {\lambda _i}} \right)$邻域内存在一阶导数，将其进行泰勒级数展开后，得：
$${\rm{\Delta }}T\left( {\varphi , \lambda } \right) = {\rm{\Delta }}{T_M}\left( {{\varphi _i}, {\lambda _i}} \right) + \frac{{\partial {\rm{\Delta }}{T_M}}}{{\partial {\varphi _i}}}\left( {\varphi - {\varphi _i}} \right) + \frac{{\partial {\rm{\Delta }}{T_M}}}{{\partial {\lambda _i}}}\left( {\lambda - {\lambda _i}} \right) + {v_i}$$ (2) 式中，${{\varphi _i}}$和${{\lambda _i}}$分别代表惯导指示点的纬度和经度；${\rm{\Delta }}{T_M}\left( {{\varphi _i}, {\lambda _i}} \right)$代表惯导指示位置的地磁异常；${v_i}$代表高阶截断误差。
载体在实际位置$\left( {{\varphi _t}, {\lambda _t}} \right)$处的磁传感器输出经预处理后的实测地磁异常为：
$${\rm{\Delta }}T\left( {\varphi , \lambda } \right) = {\rm{\Delta }}{T_s}\left( {{\varphi _t}, {\lambda _t}} \right) + {v_s}$$ (3) 式中，${\rm{\Delta }}{T_s}\left( {{\varphi _t}, {\lambda _t}} \right)$代表实际位置处的实测地磁异常；vs为观测噪声。
由式(2)和式(3)可求得量测方程：
$$\begin{array}{*{20}{c}} {{L_k} = {\rm{\Delta }}{T_M}\left( {{\varphi _i}, {\lambda _i}} \right) - {\rm{\Delta }}{T_s}\left( {{\varphi _t}, {\lambda _t}} \right) = }\\ {\frac{{\partial {\rm{\Delta }}{T_M}}}{{\partial {\varphi _i}}}{\rm{ \mathsf{ δ} }}\varphi + \frac{{\partial {\rm{\Delta }}{T_M}}}{{\partial {\lambda _i}}}{\rm{ \mathsf{ δ} }}\lambda + {v_s} - {v_i}} \end{array}$$ (4) 式中，${{\rm{ \mathsf{ δ} }}\varphi }$和${{\rm{ \mathsf{ δ} }}\lambda }$表示水下潜器的位置误差。令${\mathit{\boldsymbol{L}}_k}$为滤波观测向量，将式（4）写成矩阵形式，则有：
$${\mathit{\boldsymbol{L}}_k} = {\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{v}}_k}$$ (5) 式中，${\mathit{\boldsymbol{A}}_k} = \left[ {\partial {\rm{\Delta }}{T_M}/\partial {\varphi _i}, \;\partial {\rm{\Delta }}{T_M}/\partial {\lambda _i}} \right]$为设计矩阵；${\mathit{\boldsymbol{X}}_k} = {\left[ {{\rm{ \mathsf{ δ} }}\varphi , {\rm{ \mathsf{ δ} }}\lambda } \right]^{\rm{T}}}$为状态向量；vk表示截断误差、观测噪声以及制图误差等。
-
由式(4)可以看出，量测方程线性化处理的关键是$\partial {\rm{\Delta }}{T_M}/\partial {\varphi _i}$和${\partial {\rm{\Delta }}{T_M}/\partial {\lambda _i}}$的求取，即地磁图水平方向梯度参数的实时求取问题。使用平面方程解析化形式往往不能准确刻画地磁异常变化剧烈的地区。因此，研究利用双二次曲面拟合法逼近地磁图上待匹配点的地磁异常水平梯度。
假定在待匹配点$({{\varphi _i}, {\lambda _i}})$的邻域Ω内，${\rm{\Delta }}T\left( {\varphi , \lambda } \right)$存在直到n+1的连续偏导数，则${\rm{\Delta }}T\left( {\varphi , \lambda } \right)$在邻域Ω内可表示为：
$$ \begin{array}{*{20}{c}} {{\rm{\Delta }}T\left( {\varphi ,\lambda } \right) = \sum\limits_{N = 0}^N {\frac{1}{{n!}}} {{[\left( {\varphi - {\varphi _i}} \right)\frac{\partial }{{\partial \varphi }} + (\lambda - {\lambda _i})\frac{\partial }{{\partial \lambda }}]}^n}{\rm{\Delta }}T\left( {{\varphi _i},{\lambda _i}} \right) + \frac{1}{{\left( {N + 1} \right)!}}[\left( {\varphi - {\varphi _i}} \right)\frac{\partial }{{\partial \varphi }} + }\\ {(\lambda - {\lambda _i})\frac{\partial }{{\partial \lambda }}{]^{N + 1}}{\rm{\Delta }}T\left( {{\varphi _i} + \theta \left( {\varphi - {\varphi _i}} \right),{\lambda _i} + \theta \left( {\lambda - {\lambda _i}} \right)} \right)} \end{array} $$ (6) 式中，N为截断阶数; $0 < \theta < 1$。式(6)等式右边第1项为泰勒级数展开逼近，第2项为逼近截断误差。且
$$ \begin{array}{l} {[\left( {\varphi - {\varphi _i}} \right)\frac{\partial }{{\partial \varphi }} + (\lambda - {\lambda _i})\frac{\partial }{{\partial \lambda }}]^n}{\rm{\Delta }}T\left( {{\varphi _i}, {\lambda _i}} \right) = \\ \sum\limits_{r = 0}^n {{{C_n^r}}} {(\varphi - {\varphi _i})^{n - r}}{(\lambda - {\lambda _i})^r}{\left. {\frac{{{\partial ^n}{\rm{\Delta }}{T_M}\left( {\varphi , \lambda } \right)}}{{\partial {\varphi ^{n - r}}\partial {\lambda ^r}}}} \right|_{\begin{array}{*{20}{l}} {\varphi = {\varphi _i}}\\ {\lambda = {\lambda _i}} \end{array}}} \end{array} $$ (7) 式(6)中，当N=1时，为平面拟合，当N=2时，为双二次曲面拟合，并且该式中已知的是等式左边一些离散点的地磁异常值。若存在多余观测，可利用最小二乘法求解式(7)中的地磁图水平方向梯度参数${\partial ^n}{\rm{\Delta }}{T_M}/\partial {\varphi _i}$和${\partial ^n}{\rm{\Delta }}{T_M}/\partial {\lambda _i}$。拟合逼近过程是在地磁异常底图上待匹配点$\left( {{\varphi _i}, {\lambda _i}} \right)$的邻域Ω内进行的，拟合半径依据惯导位置误差的置信区间大小来决定。
-
通常使用平滑加权残差平方（smoothed weighted residual squared, SWRS）最小作为选取最佳滤波器的准则[23]，用S表示。设第k时刻第j个滤波器的预测残差$\mathit{\boldsymbol{\bar V}}_k^j$为：
$$ \mathit{\boldsymbol{\bar V}}_k^j = \mathit{\boldsymbol{A}}_k^j\mathit{\boldsymbol{\bar X}}_k^j - \mathit{\boldsymbol{L}}_{_k}^j $$ (8) 定义加权残差平方（weighted residual squared, WRS），用W表示为：
$$ W_k^j = \frac{{{{\mathit{\boldsymbol{\bar V}}}_k}\mathit{\boldsymbol{\bar V}}_k^{\rm{T}}}}{{{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar V}}}_k}}}}} = \frac{{{{\mathit{\boldsymbol{\bar V}}}_k}\mathit{\boldsymbol{\bar V}}_k^{\rm{T}}}}{{{\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}}\mathit{\boldsymbol{A}}_k^{\rm{T}} + {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k}}} $$ (9) 式中，${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar V}}}_k}}}}$、${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}}}$和${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k}}$分别表示预测残差向量${{{\mathit{\boldsymbol{\bar V}}}_k}}$、预测状态向量${{{\mathit{\boldsymbol{\bar X}}}_k}}$以及观测向量Lk的协方差矩阵。则S可表示为：
$$ \left\{ {\begin{array}{*{20}{l}} {S_{k + 1}^j = \alpha S_k^j + \left( {1 - \alpha } \right)W_{k + 1}^j}\\ {S_0^j = 1} \end{array}} \right. $$ (10) 式中，α代表平滑加权因子，0 < α < 1；S值越小，表示滤波器结果与先验模型吻合得越好，即可将其作为最优滤波器。
-
观测噪声协方差矩阵的开窗估计是采用观测残差向量估计当前观测残差的协方差矩阵，即在特定的窗口宽度内，采用样本平均值的方法来确定当前历元的观测噪声协方差矩阵[24]，简称为RAE（residual-based adaptive estimated）滤波，是一种移动开窗估计法[25-27]。
设残差向量Vk和预测残差向量Vk（又称新息向量）分别为：
$$ \left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{V}}_k} = {\mathit{\boldsymbol{A}}_k}{{\hat X}_k} - {\mathit{\boldsymbol{L}}_k}}\\ {{{\mathit{\boldsymbol{\bar V}}}_k} = {\mathit{\boldsymbol{A}}_k}{{\mathit{\boldsymbol{\bar X}}}_k} - {\mathit{\boldsymbol{L}}_k}} \end{array}} \right. $$ (11) 由式(11)可得Vk和Vk的解析关系为：
$$ {\mathit{\boldsymbol{V}}_k} = \left( {\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{K}}_k}} \right){{\mathit{\boldsymbol{\bar V}}}_k} $$ (12) Vk和Vk的协方差矩阵分别为：
$$ \left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar V}}}_k}}} = {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k} + {\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}}\mathit{\boldsymbol{A}}_k^{\rm{T}}}\\ {{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\mathit{\boldsymbol{V}}_k}}} = {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k} - {\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}}\mathit{\boldsymbol{A}}_k^{\rm{T}}} \end{array}} \right. $$ (13) 假定观测噪声近似服从正态分布，观测残差向量Vk的协方差矩阵${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\mathit{\boldsymbol{V}}_k}}}}$的估值可表示为：
$$ {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_{{\mathit{\boldsymbol{V}}_k}}} = \frac{1}{N}{\sum\limits_{j = 0}^N {{{{\mathit{\boldsymbol{V}}_{k - j}}}}} }\mathit{\boldsymbol{V}}_{k - j}^{\rm{T}} $$ (14) 再根据式(13)求得第tk时刻观测向量的协方差矩阵${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k}}$的估值为：
$$ {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_k} = {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_{{\mathit{\boldsymbol{V}}_k}}} + {\mathit{\boldsymbol{A}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}}\mathit{\boldsymbol{A}}_k^{\rm{T}} $$ (15) 特别地，由式(15)自适应估计${{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_k}$需要用到第tk个历元的状态估计向量协方差矩阵${\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}}$和残差向量Vk，而求解${\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}}$和残差向量Vk又应先有${{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_k}$。为此，求解第tk个历元的${{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k}}$可以使用tk-1之前的N个历元信息求解，即：
$$ {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_{{\mathit{\boldsymbol{V}}_{k - 1}}}} = \frac{1}{N}{\sum\limits_{j = 1}^{N+1} {{{{\mathit{\boldsymbol{V}}_{k - j}}}}} }\mathit{\boldsymbol{V}}_{k - j}^{\rm{T}} $$ (16) 则式（15）变为：
$$ {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_k} = {{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_{{\mathit{\boldsymbol{V}}_{k - 1}}}} + {\mathit{\boldsymbol{A}}_{k - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_{k - 1}}}}\mathit{\boldsymbol{A}}_{k - 1}^{\rm{T}} $$ (17) 值得注意的是，特定窗口宽度m的选取是开展实验的关键。若m取得过小，所用历史信息不足，不能求得表征运动状态扰动的动态模型方差协方差矩阵；若m过大，由于使用大量历史信息进行平滑，很难反映瞬时运动状态的扰动。
-
利用水下潜器进行仿真，仿真参数的设置如表 1所示。2009年，美国国家地理数据中心综合利用卫星、海洋、航空和地面磁测数据发布EMAG2（earth magnetic anomaly grid 2）模型，分辨率为2'[28]。选取EMAG2范围为108°E~113°E、10°N~15°N的中国南海区域作为实验的地磁异常基准图，如图 1所示。
表 1 仿真参数设置
Table 1. Simulation Parameters Setting
参数 值 惯性导航初始经度误差/n mile 2 惯性导航初始纬度误差/n mile 2 陀螺零偏/（(°)·h-1） 0.01 加表零偏/（m·s-2） 1 × 10-5 潜器的匀速运动速度/（n mile·h-1） 9.7 潜器偏航角/(°) 50 潜器航行时间/h 10.29 惯性导航初始状态方差/(°)2 0.001 地磁图噪声方差/nT2 10 地磁测量噪声方差/nT2 10 
图 1 地磁异常底图
Figure 1. Geomagnetic Anomaly Grid Map
在滤波过程中，为保证载体在滤波周期内航行距离超过一个地磁图格，将步长选取为741 s，惯性导航解算步长为1 s，沿经纬度方向构造了9个并行滤波器。对于窗口宽度m的选择，经多次试算，本文给出了相应的滤波统计结果，见表 2。
表 2 不同窗口宽度下的滤波匹配结果/n mile
Table 2. Filtering Results Under Different Window Widths/n mile
方向 窗口宽度m 最大值 最小值 平均值 均方误差 纬度方向 2 7.235 31 0.043 90 1.363 82 2.056 71 5 1.304 41 0.005 07 0.648 04 0.713 55 10 4.870 52 0.068 52 0.800 95 1.814 33 20 9.568 81 0.097 15 1.456 12 2.196 72 经度方向 2 2.753 86 0.077 20 0.854 71 1.106 29 5 1.061 71 0.000 98 0.490 09 0.738 57 10 5.526 87 0.058 85 1.696 70 1.684 39 20 8.736 56 0.062 82 1.724 66 1.772 52 由表 2可得，无论是经度方向还是纬度方向，当m=5时，滤波精度最高。因此，仿真实验选择观测噪声协方差阵恒定不变的PEKF滤波和窗口宽度为5的RAE-PEKF滤波两种方法。通过比较两种滤波方法的实验结果来选取更为可靠的滤波方法。
3.1仿真实验A
选取地磁特征变化明显的实验区A开展仿真实验。在航迹仿真中，惯性导航系统起始经纬度为（110.5°E, 10.5°N），水下潜器的运动状态可描述为先进行匀加速运动，接着匀速运动，然后再转弯，最后变成匀速状态，速度参数和时间参数如表 1所示。仿真实验区域、航迹地磁异常变化及两种算法的匹配结果见图 2至图 6。
图 2 实验区A仿真航迹
Figure 2. Track in Simulation Experiment A
图 3 实验区A惯导航迹和参考航迹地磁异常变化
Figure 3. Variation of Geomagnetic Anomaly of the Inertial Navigation and the Reference Tracks in Simulation Experiment A
图 4 实验区A两种算法的匹配航迹
Figure 4. Matching Tracks of Two Algorithms in Simulation Experiment A
图 5 实验区A经纬向PEKF滤波匹配误差
Figure 5. Matching Errors Using PEKF Filtering Along Latitude and Longitude in Simulation Experiment A
图 6 实验区A经纬向RAE-PEKF滤波匹配误差
Figure 6. Matching Errors Using RAE-PEKF Filtering Along Latitude and Longitude in Simulation Experiment A
由图 4至图 6可知，经过滤波匹配后，惯导航迹随时间产生的累积误差会得到降低。通过比较，RAE-PEKF滤波算法得到的匹配航迹比PEKF滤波算法更接近于真实航迹，这种移动开窗估计法能够自适应估计观测噪声协方差矩阵，且使用该滤波算法能够较好地掌握量测信息的噪声水平，系统的导航精度也随之得到提高。
将PEKF滤波匹配算法和RAE-PEKF滤波匹配算法分别沿经度和纬度方向对两者的绝对匹配误差进行统计，并和INS的绝对匹配误差进行对比，分析结果见表 3。由表 3可得，通过RAE-PEKF滤波算法自适应估计观测噪声方差，组合导航系统的精度在纬度方向上可以提高至0.751 53 n mile，在经度方向上提高至0.778 45 n mile，并且两方向上的精度均优于INS的精度。
表 3 实验区A绝对匹配误差统计值/n mile
Table 3. Statistics of Absolute Matching Errors in Simulation Experiment A/n mile
误差方向 算法 最大值 最小值 平均值 均方误差 纬度方向 INS 3.804 66 0.134 40 1.331 36 1.571 06 PEKF 5.870 55 0.026 54 1.236 25 1.944 46 RAE-PEKF 1.304 41 0.005 07 0.648 04 0.751 53 经度方向 INS 2.037 66 0.089 70 0.841 75 1.012 46 PEKF 2.484 99 0.010 43 0.707 23 0.963 67 RAE-PEKF 1.061 71 0.000 98 0.501 39 0.778 45 -
选取地磁特征变化不明显的实验区B进行仿真来进一步验证算法的可靠性，仿真参数与仿真实验A一致，惯性导航系统的起始经纬度为（111.5° E, 10.5° N）。仿真实验区域、航迹地磁异常变化及两种算法的匹配结果见图 7至图 11，在经纬度方向上的绝对匹配误差统计值见表 4。
图 7 实验区B仿真航迹
Figure 7. Track in Simulation Experiment B
图 8 实验区B惯导航迹和参考航迹地磁异常变化
Figure 8. Variation of Geomagnetic Anomaly of the Inertial Navigation and the Reference Tracks in Simulation Experiment B
图 9 实验区B两种算法的匹配航迹
Figure 9. Matching Tracks of Two Algorithms in Simulation Experiment B
图 10 实验区B经纬向PEKF滤波匹配误差
Figure 10. Matching Errors Using PEKF Filtering Along Latitude and Longitude in Simulation Experiment B
图 11 实验区B经纬向RAE-PEKF滤波匹配误差
Figure 11. Matching Errors Using RAE-PEKF Filtering Along Latitude and Longitude in Simulation Experiment B
表 4 实验区B绝对匹配误差统计值/n mile
Table 4. Statistics of Absolute Matching Errors in Simulation Experiment B/n mile
误差方向 算法 最大值 最小值 平均值 均方误差 纬度方向 INS 5.457 78 0.030 12 2.378 78 2.679 59 PEKF 17.494 53 0.001 40 8.948 47 11.075 22 RAE-PEKF 3.581 97 0.001 75 1.028 33 1.291 23 经度方向 INS 4.120 92 1.972 80 2.958 12 3.048 32 PEKF 44.658 81 0.100 90 14.115 01 19.233 45 RAE-PEKF 3.842 53 0.001 68 1.064 13 1.351 21 由图 9至图 11可知，在地磁特征不明显的区域，观测噪声协方差阵恒定不变的PEKF滤波算法所得的匹配结果出现较大偏差，导致错误定位，可能的原因是匀加速运动阶段的噪声先验信息已经不能客观地反映当前的噪声水平。相应地，RAE-PEKF滤波算法所得的结果更为稳定，分析其原因，主要是该算法不再依靠噪声先验信息，而是通过特定窗口宽度采用样本平均值的方法确定当前历元观测向量的协方差阵和模型误差的协方差阵。
由表 4可得，同样地，通过RAE-PEKF滤波算法所得的组合导航系统精度在纬度方向上提高至1.291 23 n mile，在经度方向上提高至1.351 21 n mile，两方向上均优于INS的精度。
-
为了实现水下潜器长航时、高精度自主导航，本文采用地磁场信息限制惯导随时间产生的累积误差影响，对比分析了基于PEKF滤波和RAE-PEKF滤波两种算法的匹配结果。通过仿真实验对算法在不同地磁特征区域的可靠性进行验证，得到如下结论：
1）在地磁特征变化明显的区域，RAE-PEKF滤波匹配算法的匹配结果优于PEKF滤波匹配算法和纯惯性导航系统；在地磁特征变化不明显的区域，PEKF滤波匹配算法所得的匹配结果出现较大偏差，导致错误定位，而RAE-PEKF滤波匹配算法所得的匹配结果仍然保持较好的精度，且该算法在地磁特征变化明显的区域具有更高的匹配精度。
2）针对滤波先验噪声信息未知和复杂测量环境等问题，采用观测残差向量估计当前观测残差协方差矩阵的RAE-PEKF匹配算法能够有效提高组合导航系统的精度，智能克服外部扰动影响，对进一步研究水下潜器长航时自主导航具有重要意义。
3）特别指出的是，RAE-PEKF滤波匹配算法估计的${{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_k}$实为tk-1时刻的${{\mathit{\boldsymbol{ \boldsymbol{\hat \varSigma} }}}_{k - 1}}$，即由tk-1时刻以前的观测残差预测当前历元的观测噪声协方差矩阵。这种预测协方差矩阵的可靠性取决于当前历元观测精度与历史历元观测精度的一致性。
RAE-PEKF Matching Algorithm Based on Measurement Residuals to Estimate Residuals Covariance
-
摘要: 地磁异常数据可以为水下潜器提供高隐蔽性的外部辅助信息，修正由惯性导航产生的累积误差。滤波匹配算法作为地磁辅助惯性导航中的核心技术，能够有效削弱地磁观测噪声不确定的影响。基于航迹仿真数据，将全球地磁异常格网数据作为基准图，提出使用量测残差自适应估计观测噪声方差的地磁异常并行扩展卡尔曼滤波匹配算法。通过建立一组基于地磁异常观测量的并行滤波器，结合最优滤波器选取准则来提高算法的可用性和鲁棒性。选取地磁特征分布不同的中国南海海域进行仿真匹配实验，结果表明，该算法能够有效降低纯惯性导航经纬向的匹配误差。仿真条件下，在地磁特征变化明显的区域，地磁辅助惯性导航系统的精度在纬度方向上提高至0.751 53 n mile，在经度方向上提高至0.778 45 n mile。Abstract: Geomagnetic matching technology can provide passive external resource of correction information for underwater vehicle, which improves the long voyage navigation accuracy of inertial navigation system. The filtering matching algorithm is the core technology in geomagnetic aided inertial navigation, which can effectively mitigate the influence from the uncertainty of geomagnetic observation noise. Based on track simulation data, the earth magnetic anomaly grid 2 (EMAG2) is selected as reference map in this paper. To properly model the observation noise of geomagnetic anomaly in unpredictable geomagnetic environment and the error from measuring instruments, this paper proposes the matching algorithm of geomagnetic anomaly filter based on residual errors. The observation noise variance is estimated adaptively through the residual-based adaptive estimation (RAE) filter. Meanwhile, the availability and robustness of the algorithm are improved by combining the optimal filter selection criteria. The validation experiments of RAE filter are conducted in different maritime space of the South China Sea. It is shown that the drifting errors of inertial navigation system in longitude and latitude can be reduced based on the RAE filter. Moreover, the positioning accuracy and reliability of the aided navigation system can be improved significantly. In obvious geomagnetic fluctuating region under the simulation condition, position accuracy is raised to 0.751 53 n mile and 0.778 45 n mile respectively in latitude and longitude directions through RAE filter.
-
图 3 实验区A惯导航迹和参考航迹地磁异常变化
Figure 3. Variation of Geomagnetic Anomaly of the Inertial Navigation and the Reference Tracks in Simulation Experiment A
图 4 实验区A两种算法的匹配航迹
Figure 4. Matching Tracks of Two Algorithms in Simulation Experiment A
图 5 实验区A经纬向PEKF滤波匹配误差
Figure 5. Matching Errors Using PEKF Filtering Along Latitude and Longitude in Simulation Experiment A
图 6 实验区A经纬向RAE-PEKF滤波匹配误差
Figure 6. Matching Errors Using RAE-PEKF Filtering Along Latitude and Longitude in Simulation Experiment A
图 8 实验区B惯导航迹和参考航迹地磁异常变化
Figure 8. Variation of Geomagnetic Anomaly of the Inertial Navigation and the Reference Tracks in Simulation Experiment B
图 9 实验区B两种算法的匹配航迹
Figure 9. Matching Tracks of Two Algorithms in Simulation Experiment B
图 10 实验区B经纬向PEKF滤波匹配误差
Figure 10. Matching Errors Using PEKF Filtering Along Latitude and Longitude in Simulation Experiment B
图 11 实验区B经纬向RAE-PEKF滤波匹配误差
Figure 11. Matching Errors Using RAE-PEKF Filtering Along Latitude and Longitude in Simulation Experiment B
表 1 仿真参数设置
Table 1. Simulation Parameters Setting
参数 值 惯性导航初始经度误差/n mile 2 惯性导航初始纬度误差/n mile 2 陀螺零偏/（(°)·h-1） 0.01 加表零偏/（m·s-2） 1 × 10-5 潜器的匀速运动速度/（n mile·h-1） 9.7 潜器偏航角/(°) 50 潜器航行时间/h 10.29 惯性导航初始状态方差/(°)2 0.001 地磁图噪声方差/nT2 10 地磁测量噪声方差/nT2 10 
下载: 导出CSV
表 2 不同窗口宽度下的滤波匹配结果/n mile
Table 2. Filtering Results Under Different Window Widths/n mile
方向 窗口宽度m 最大值 最小值 平均值 均方误差 纬度方向 2 7.235 31 0.043 90 1.363 82 2.056 71 5 1.304 41 0.005 07 0.648 04 0.713 55 10 4.870 52 0.068 52 0.800 95 1.814 33 20 9.568 81 0.097 15 1.456 12 2.196 72 经度方向 2 2.753 86 0.077 20 0.854 71 1.106 29 5 1.061 71 0.000 98 0.490 09 0.738 57 10 5.526 87 0.058 85 1.696 70 1.684 39 20 8.736 56 0.062 82 1.724 66 1.772 52
下载: 导出CSV
表 3 实验区A绝对匹配误差统计值/n mile
Table 3. Statistics of Absolute Matching Errors in Simulation Experiment A/n mile
误差方向 算法 最大值 最小值 平均值 均方误差 纬度方向 INS 3.804 66 0.134 40 1.331 36 1.571 06 PEKF 5.870 55 0.026 54 1.236 25 1.944 46 RAE-PEKF 1.304 41 0.005 07 0.648 04 0.751 53 经度方向 INS 2.037 66 0.089 70 0.841 75 1.012 46 PEKF 2.484 99 0.010 43 0.707 23 0.963 67 RAE-PEKF 1.061 71 0.000 98 0.501 39 0.778 45
下载: 导出CSV
表 4 实验区B绝对匹配误差统计值/n mile
Table 4. Statistics of Absolute Matching Errors in Simulation Experiment B/n mile
误差方向 算法 最大值 最小值 平均值 均方误差 纬度方向 INS 5.457 78 0.030 12 2.378 78 2.679 59 PEKF 17.494 53 0.001 40 8.948 47 11.075 22 RAE-PEKF 3.581 97 0.001 75 1.028 33 1.291 23 经度方向 INS 4.120 92 1.972 80 2.958 12 3.048 32 PEKF 44.658 81 0.100 90 14.115 01 19.233 45 RAE-PEKF 3.842 53 0.001 68 1.064 13 1.351 21
下载: 导出CSV
-
[1] Zheng H, Wang H, Wu L, et al. Simulation Research on Gravity-Geomagnetism Combined Aided Underwater Navigation[J]. Journal of Navigation, 2013, 66(1):83-98 doi: 10.1017/S0373463312000343 [2] 彭富清, 霍立业.海洋地球物理导航[J].地球物理学进展, 2007, 22(3):759-764 doi: 10.3969/j.issn.1004-2903.2007.03.015 Peng Fuqing, Huo Liye. Marine Geophysical Navigation[J]. Progress in Geophysics, 2007, 22(3):759-764 doi: 10.3969/j.issn.1004-2903.2007.03.015 [3] Goldenberg F. Geomagnetic Navigation Beyond the Magnetic Compass[C]. Position, Location, and Navigation Symposium, Monterey, California, USA, 2006 [4] Tangye N. Magnetic Compasses and Magnetometers, by Alfred Hine[J]. Physics Bulletin, 1969, 20(6):238-239 http://www.researchgate.net/publication/275980417_Magnetic_Compasses_and_Magnetometers [5] Titterton D, Weston J. Strapdown Inertial Navigation Technology[J]. Aerospace and Electronic Systems Magazine IEEE, 2004, 20(7):33-34 http://d.old.wanfangdata.com.cn/Periodical/shjtdxxb200311038 [6] Psiaki M L. Autonomous Low-Earth-Orbit Determination from Magnetometer and Sun Sensor Data[J]. Journal of Guidance Control and Dynamics, 2012, 22(2):296-304 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=ZqpfUeDUtTOO7TLNqsw580v2OXd9N63efgOwn4Pbv+8= [7] Deutschmann J K, Bar-Itzhack I Y. Evaluation of Attitude and Orbit Estimation Using Actual Earth Magnetic Field Data[J]. Journal of Guidance, Control, and Dynamics, 2001, 24(3):616-623 doi: 10.2514/2.4753 [8] Kim S, Chun J. Satellite Orbit Determination Using a Magnetometer-Based Bootstrap Filter[C]. American Control Conference, Chicago, Illinois, USA, 2000 [9] Kwon W, Roh K S, Sung H K. Particle Filter-Based Heading Estimation Using Magnetic Compasses for Mobile Robot Navigation[C]. IEEE International Conference on Robotics and Automation, Orlando, Florida, USA, 2006 [10] Armstrong B, Wolbrecht E, Edwards D B. AUV Navigation in the Presence of a Magnetic Disturbance with an Extended Kalman Filter[C]. Oceans Conference, Sydney, Australia, 2010 [11] 王向磊, 田颜锋.基于地磁场的自主导航研究[J].地球物理学报, 2010, 53(11):2724-2731 http://d.old.wanfangdata.com.cn/Periodical/dqwlxb201011020 Wang Xianglei, Tian Yanfeng. Autonomous Navigation Based Geomagnetic Research[J]. Chinese Journal of Geophysics, 2010, 53(11):2724-2731 http://d.old.wanfangdata.com.cn/Periodical/dqwlxb201011020 [12] 赵建虎, 张红梅, 王爱学, 等.利用ICCP的水下地磁匹配导航算法[J].武汉大学学报·信息科学版, 2010, 35(3):261-264 http://ch.whu.edu.cn/CN/abstract/abstract865.shtml Zhao Jianhu, Zhang Hongmei, Wang Aixue, et al. Underwater Geomagnetic Navigation Based on ICCP[J]. Geomatics and Information Science of Wuhan University, 2010, 35(3):261-264 http://ch.whu.edu.cn/CN/abstract/abstract865.shtml [13] 郭才发, 赵星, 蔡洪.有色噪声作用下的INS/地磁组合导航算法研究[J].飞行器测控学报, 2010, 29(4):84-88 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=fxqckxb201004019 Guo Caifa, Zhao Xing, Cai Hong. Analysis of Algorithm for INS/Geomagnetic Integrated Navigation Under Colored Noise[J]. Journal of Spacecraft TT and C Technology, 2010, 29(4):84-88 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=fxqckxb201004019 [14] 郭才发, 胡正东, 赵星, 等.自适应滤波在磁暴期间地磁导航中的应用[J].宇航学报, 2010, 31(8):1927-1932 doi: 10.3873/j.issn.1000-1328.2010.08.006 Guo Caifa, Hu Zhengdong, Zhao Xing, et al. Application of Adaptive Filter in Geomagnetic Navigation Under Magnetic Storms[J]. Journal of Astronautics, 2010, 31(8):1927-1932 doi: 10.3873/j.issn.1000-1328.2010.08.006 [15] 施桂国, 周军, 葛致磊.基于无迹卡尔曼滤波的巡航导弹地磁自主导航方法[J].兵工学报, 2008, 29(9):1088-1093 doi: 10.3321/j.issn:1000-1093.2008.09.014 Shi Guiguo, Zhou Jun, Ge Zhilei. Geomagnetic Autonomous Navigation Technology of Cruise Missile Based on the Unscented Kalman Filter[J]. Acta Armamentarii, 2008, 29(9):1088-1093 doi: 10.3321/j.issn:1000-1093.2008.09.014 [16] 王胜平.水下地磁匹配导航定位关键技术研究[D].武汉: 武汉大学, 2011 http://d.wanfangdata.com.cn/Periodical_chxb201301023.aspx Wang Shengping. Research on the Key Technical of Marine Geomagnetic Matching Navigation and Positioning[D]. Wuhan: Wuhan University, 2011 http://d.wanfangdata.com.cn/Periodical_chxb201301023.aspx [17] 解伟男, 李清华, 奚伯齐, 等.基于迭代计算的地磁轮廓线匹配算法[J].中国惯性技术学报, 2015, 23(5):631-635 http://d.old.wanfangdata.com.cn/Periodical/zggxjsxb201505013 Xie Weinan, Li Qinghua, Xi Boqi, et al. Geomagnetic Contour Matching Algorithm Based on Iterative Method[J]. Journal of Chinese Inertial Technology, 2015, 23(5):631-635 http://d.old.wanfangdata.com.cn/Periodical/zggxjsxb201505013 [18] Liu Y, Wu M. Intelligent Geomagnetic Matching Method[C]. International Conference on Intelligent Computing and Integrated Systems, Guilin, China, 2010 [19] Ma T, Chen L, Wu Z, et al. An Improved Iteration Method for Downward Continuation of Potential Fields[J]. International Proceedings of Computer Science & Information Tech, 2012, 37(4):399-402 [20] Simon D. Kalman Filtering with State Constraints:A Survey of Linear and Nonlinear Algorithms[J]. IET Control Theory and Applications, 2010, 4(8):1303-1318 doi: 10.1049/iet-cta.2009.0032 [21] 黄朝艳, 田海冬, 赵华.地磁滤波导航技术的研究现状[J].科学技术与工程, 2013, 13(30):8976-8982 doi: 10.3969/j.issn.1671-1815.2013.30.021 Huang Chaoyan, Tian Haidong, Zhao Hua. Present Research Situation of Geomagnetic Filter Navigation Technologies[J]. Science Technology and Engineering, 2013, 13(30):8976-8982 doi: 10.3969/j.issn.1671-1815.2013.30.021 [22] 张国良, 曾静.组合导航原理与技术[M].西安:西安交通大学出版社, 2008 Zhang Guoliang, Zeng Jing. The Theory and Technology of Integrated Navigation System[M]. Xi'an:Xi'an Jiaotong University Press, 2008 [23] 许大欣.利用重力异常匹配技术实现潜艇导航[J].地球物理学报, 2005, 48(4):812-816 doi: 10.3321/j.issn:0001-5733.2005.04.012 Xu Daxin. Using Gravity Anomaly Matching Techniques to Implement Submarine Navigation[J]. Chinese Journal of Geophysics, 2005, 48(4):812-816 doi: 10.3321/j.issn:0001-5733.2005.04.012 [24] Mohamed A H, Schwarz K P. Adaptive Kalman Filtering for INS/GPS[J]. Journal of Geodesy, 1999, 73(4):193-203 doi: 10.1007/s001900050236 [25] 杨元喜.自适应动态导航定位[M].北京:测绘出版社, 2017 Yang Yuanxi. Adaptive Navigation and Kinematic Positioning[M]. Beijing:Surveying and Mapping Press, 2017 [26] Salmon B P, Kleynhans W, Bergh F V D, et al. Meta-Optimization of the Extended Kalman Filter's Parameters Through the Use of the Bias Variance Equilibrium Point Criterion[J]. IEEE Transactions on Geoscience & Remote Sensing, 2014, 52(8):5072-5087 [27] 王伟, 李姗姗, 邢志斌, 等. RAE并行滤波的重力异常匹配算法[J].武汉大学学报·信息科学版, 2016, 41(12):1664-1670 http://ch.whu.edu.cn/CN/abstract/abstract5620.shtml Wang Wei, Li Shanshan, Xing Zhibin, et al. A Matching Algorithm Using Gravity Anomaly Based on the RAE Parallel Filtering[J]. Geomatics and Information Science of Wuhan University, 2016, 41(12):1664-1670 http://ch.whu.edu.cn/CN/abstract/abstract5620.shtml [28] Maus S, Barckhausen U, Berkenbosch H, et al. EMAG2:A 2-Arc Min Resolution Earth Magnetic Anomaly Grid Compiled from Satellite, Airborne, and Marine Magnetic Measurements[J]. Geochemistry Geophysics Geosystems, 2009, 10(8):4918-4929 http://d.old.wanfangdata.com.cn/Periodical/dqwlxjz201204063 -
点击查看大图
计量
- 文章访问数: 1381
- HTML全文浏览量: 209
- PDF下载量: 63
- 被引次数: 0
