0 引　言

受未知浪涌，海冰等诸多极端恶劣环境的威胁，舰船在极区安全航行必须依靠高精度和高可靠性的导航定位信息^{[1 - 2]}。组合导航系统因其可实现各导航设备优势互补、协同超越等特点在高精度导航定位中得到广泛应用^{[3 − 5]}。而极区低温、磁暴、暴雪等因素会提高各导航设备发生故障的概率，进而影响组合导航系统的精度及可靠性，因此需对故障进行有效隔离，提升系统的鲁棒性，以保障极区舰船的航行安全。

目前隔离故障所用方法多集中于联邦滤波算法上^[6]，联邦滤波器作为分布式滤波结构的经典代表，其子滤波器相互独立，有利于故障隔离，具有良好的容错性能^{[7 − 9]}。对于联邦滤波中的故障子系统，大多采用信息分配因子优选方案进行隔离。何广军等^[10]通过对误差方差阵进行特征值分解构造联邦滤波信息分配因子，自适应调整各子系统信息分配比例。SHEN等^[11]提出了基于系统误差状态可观测度的信息分配原则，克服故障子系统污染全局最优融合的问题。XIONG等^[12]基于卡方检验对信息分配因子进行设计，自适应反应局部滤波器的性能。高社生等^{[13 - 14]}同样通过相应方法计算信息分配因子，避免故障子系统对全局估计精度的影响。但姚娅玲等^[15]通过理论证明、公式推导以及仿真实验证明信息分配因子取值不同并不会改变联邦滤波主滤波器的估计精度即最终的全局估计精度，因此上述信息分配因子优选方案所依据的理论有效性存在问题，并不能有效隔离故障子系统。

对此，本文以极区舰船惯性/多普勒/卫星（SINS/DVL/GNSS）组合导航系统为例，摒弃通过更改联邦滤波信息分配因子调整故障子系统信息比例的方法，采用异于信息分配因子优选的方案对极区SINS/DVL/GNSS组合导航系统进行故障隔离，在联邦滤波主滤波器融合过程中引入基于马氏距离的融合系数，通过真实量测与预测量测之间的马氏距离衡量子滤波器外部量测质量，降低故障子滤波器在信息融合过程中的比例，减少其对全局滤波精度的影响，提高极区组合导航系统的容错性与可靠性。

1 SINS/DVL/GNSS组合导航系统模型

以SINS/DVL/GNSS组合导航系统为例，参考文献[16]的横向惯导误差方程，建立横向组合导航系统模型，其中包含系统状态方程以及子系统量测方程。

1.1 SINS/DVL/GNSS组合导航系统的状态方程

选取适用于极区的横向地理坐标系作为系统导航坐标系^[17]，并构造系统的状态方程。由于主要针对水面船舶运载体，因此忽略天向速度和天向高度，选取横向系下载体姿态误差、速度误差、位置误差以及陀螺仪和加速度计随机常值偏置作为模型状态变量，即：

$ \begin{split} X = &\left[ {\begin{array}{*{20}{c}} {{\varphi _{E'}}}&{{\varphi _{N'}}}&{{\varphi _{U'}}}&{\delta {V_{E'}}}&{\delta {V_{N'}}}&{\delta L'}&{\delta \lambda '} \end{array}} \right. \\ & \left. {\begin{array}{*{20}{c}} {\varepsilon _x^b}&{\varepsilon _y^b}&{\varepsilon _z^b}&{\nabla _x^b}&{\nabla _y^b}&{\nabla _z^b} \end{array}} \right]。\end{split} $

(1)

式中：$ {\varphi _{E^{'}}} $、$ \varphi_{N^{'}} $和$ {\varphi _{{U^{'}}}} $分别为横向地理系下的姿态失准角；$ \delta {V_{{E^{'}}}} $、$ \delta {V_{{N^{'}}}} $分别为横向地理系下的东向和北向速度误差；$ \delta L' $、$ \delta \lambda ' $分别为横向纬度误差和横向经度误差；$\nabla _x^b$、$\nabla _y^b$和$\nabla _z^b$分别为加速度计3个轴随机常值零位误差；$\varepsilon _x^b$、$\varepsilon _y^b$和$\varepsilon _z^b$分别为陀螺仪3个轴向随机常值漂移。系统状态变量为13维，状态方程如下：

$\dot X(t) = {\boldsymbol{F}}(t)X(t) + {\boldsymbol{G}}(t){\boldsymbol{W}}(t)。$

(2)

式中：${\boldsymbol{F}}(t)$为系统状态矩阵；${\boldsymbol{G}}(t)$为系统噪声驱动阵；$ {\boldsymbol{W}}(t) $为系统噪声阵。

$ {\boldsymbol{F}}(t) = \left[ {\begin{array}{*{20}{c}} {\mathop {{F_{INS}}}\limits_{7\times7} }&{\mathop {{F_t}}\limits_{7\times6} } \\ {\mathop 0\limits_{6\times7} }&{\mathop 0\limits_{6\times6} } \end{array}} \right] ，{F_t} = \left[ {\begin{array}{*{20}{c}} { - \mathop {C_b^{g'}}\limits_{3\times3} }&{\mathop 0\limits_{3\times3} } \\ {\mathop 0\limits_{2\times3} }&{\mathop {C_b^{g'}}\limits_{2\times3} } \\ {\mathop 0\limits_{2\times3} }&{\mathop 0\limits_{2\times3} } \end{array}} \right] ，$

(3)

$ {\boldsymbol{W}}(t) = [\begin{array}{*{20}{c}} {\varepsilon _x^b}&{\varepsilon _y^b}&{\varepsilon _z^b}&{\nabla _x^b}&{\nabla _y^b}&{\nabla _z^b} \end{array}] ，$

(4)

$ {\boldsymbol{G}}(t) = \left[ {\begin{array}{*{20}{c}} { - \mathop {C_b^{g'}}\limits_{3\times3} }&{\mathop 0\limits_{3\times3} } \\ {\mathop 0\limits_{2\times3} }&{\mathop {C_b^{g'}}\limits_{2\times3} } \\ {\mathop 0\limits_{8\times3} }&{\mathop 0\limits_{8\times3} } \end{array}} \right]。$

(5)

其中由于不考虑天向速度和天向高度，因此$ {F_{INS}} $通过在原有横向惯导矩阵中去掉对应参数得到。

1.2 SINS/DVL子系统量测方程

DVL输出的速度信息是基于载体坐标系的^[18]，因此需通过载体系与横向地理系之间的转换矩阵${\boldsymbol{C}}_b^{g'}$将其转换到横向坐标系中。不考虑天向速度，将横向SINS输出的水平二维速度与转换后的DVL输出的横向水平速度之差作为系统量测量，同时在前述横向惯导状态模型的基础上，获得以水平速度误差作为观测量的SINS/DVL组合导航量测方程：

$ {Z_1} = \left[ {\begin{array}{*{20}{c}} {V_{IN{S_{E'}}}^{g'} - V_{DV{L_{E'}}}^{g'}} \\ {V_{IN{S_{N'}}}^{g'} - V_{DV{L_{N'}}}^{g'}} \end{array}} \right] = {H_1}X + {V_1} ，$

(6)

$ \text{}{{\boldsymbol{H}}_1} = \left[ {\begin{array}{*{20}{c}} {{0_{2\times3}}}&{{I_{2\times2}}}&{{0_{2\times2}}}&{{0_{2\times6}}} \end{array}} \right]。\qquad\quad $

(7)

式中：${{\boldsymbol{H}}_1}$为SINS/DVL系统量测矩阵；${V_1}$为DVL水平速度量测的零均值高斯白噪声。

1.3 SINS/GNSS子系统量测方程

将横向SINS输出的横向经纬度与卫星提供的横向经纬度的差值作为SINS/GNSS系统的量测值，其对应的量测方程为:

$ {Z_2} = \left[ {\begin{array}{*{20}{c}} {{{L'}_{INS}} - {{L'}_{GNSS}}} \\ {{{\lambda '}_{INS}} - {{\lambda '}_{GNSS}}} \end{array}} \right] = {H_2}X + {V_2}，$

(8)

$ \text{式中：}\boldsymbol{H}_2=\left[\begin{array}{*{20}{c}}0_{2\times5} & I_{2\times2} & 0_{2\times6}\end{array}\right]。\qquad\qquad\quad $

(9)

其中：${H_2}$为SINS/GNSS系统量测矩阵；${V_2}$为GNSS位置量测的零均值高斯白噪声。

2 基于马氏距离的联邦滤波融合算法

在传统联邦滤波模型的基础上，向主滤波器的最优融合中引入融合系数，该系数通过真实量测与预测量测之间的马氏距离构造，可反映各子滤波器的量测质量，自适应调整主滤波器最优融合过程中各子滤波器状态比例，形成新的联邦滤波融合算法。

2.1 联邦滤波模型结构

分别选取SINS/DVL与SINS/GNSS系统作为联邦滤波器的2个子滤波器。子滤波器之间相互独立滤波，且主滤波器没有量测更新，只进行时间更新，并且均为基本卡尔曼滤波，其中子滤波器滤波过程为：

状态一步预测

$ \hat X_{k/k - 1}^i = \phi _{k,k - 1}^i\hat X_{k - 1}^i，$

(10)

一步预测均方误差

$ P_{k/k - 1}^i = \phi _{k,k - 1}^iP_{k - 1}^i\phi _{k,k - 1}^{i{\rm{T}}} + \Gamma _{k - 1}^iQ_{k - 1}^i\Gamma _{k - 1}^{i{\rm{T}}}，$

(11)

滤波增益

$ K_k^i = P_{k/k - 1}^iH_k^{i{\rm{T}}}{(H_k^iP_{k/k - 1}^iH_k^{i{\rm{T}}} + R_k^i)^{ - 1}}，$

(12)

状态估计

$ \hat X_k^i = \hat X_{k/k - 1}^i + K_k^i(Z_k^i - H_k^i\hat X_{k/k - 1}^i)，$

(13)

估计均方误差

$ P_k^i = (I - K_k^iH_k^i)P_{k/k - 1}^i，$

(14)

主滤波器时间更新为：

状态一步预测

$ \hat X_{k/k - 1}^m = \phi _{k,k - 1}^m\hat X_{k - 1}^m，$

(15)

一步预测均方误差

$ P_{k/k - 1}^m = \phi _{k,k - 1}^mP_{k - 1}^m\phi _{k,k - 1}^{m {\rm{T}} } + \Gamma _{k - 1}^mQ_{k - 1}^m\Gamma _{k - 1}^{m {\rm{T}} }。$

(16)

式中：i为第i个子滤波器；m为主滤波器。各个子滤波器滤波后的局部状态估计$\hat X_k^i$与估计均方误差$P_k^i$将被送到主滤波器同主滤波器自身的时间更新进行最优融合。同时，主滤波器将最终融合结果结合对应的分配因子$\beta $对各滤波器进行信息分配，最终的融合结果对横向惯导进行反馈校正。该容错算法整体模型结构如图1所示。

该算法也适用于$i \geqslant 2$的联邦滤波结构，在主滤波器中增加了基于马氏距离的融合系数，具体融合公式如下：

$ \begin{aligned} & \hat X_k^g = \sum\limits_{i = 1}^N {\alpha _k^i\hat X_k^i} + \alpha _k^m\hat X_k^m，\\ & P_k^g = {\left(\sum\limits_{i = 1}^N {{{({P_k}^i)}^{ - 1}} + {{({P_k}^m)}^{ - 1}}} \right)^{ - 1}} 。\end{aligned}$

(17)

其中，$\alpha _k^i$和$\alpha _k^m$分别为子滤波器和主滤波器$k$时刻对应的融合系数。

2.2 联邦滤波信息分配因子的选取

本算法中的信息分配方案与传统的一致，遵循“信息分配”原则，将联邦滤波中的分配因子设计成固定值，使信息平均分配给主滤波器和子滤波器，各个滤波器的信息分配因子和恒为1，一个滤波器的分配因子增大，其被分配的信息量增多，其它滤波器的信息量就会相应减少，最终整体的融合信息量不变。即：

$ {(Q_k^i)^{ - 1}} = {\beta _i}{(Q_k^g)^{ - 1}},{(Q_k^m)^{ - 1}} = {\beta _m}{(Q_k^g)^{ - 1}} ，$

(18)

$ {(P_k^i)^{ - 1}} = {\beta _i}{(P_k^g)^{ - 1}},{(P_k^m)^{ - 1}} = {\beta _m}{(P_k^g)^{ - 1}} ，$

(19)

$ \hat X_k^i = \hat X_k^m = \hat X_k^g，$

(20)

其中

$ {\beta _i} = {\beta _m} = 1/3\text{，}\sum\limits_{i = 1}^N {{\beta _i} + {\beta _m} = 1}。$

(21)

式中：$N$为子滤波器数，算法中$N$取2；${\beta _i}$和${\beta _m}$分别为子滤波器和主滤波器的信息分配因子。

2.3 联邦滤波融合系数的选取

马氏距离表示2个数据的协方差距离，可以有效表示数据之间的相似度，由于其在计算过程中考虑了两者数据之间的协方差，因此不受量纲的影响，是尺度无关的。

本算法在计算融合系数中引入马氏距离来分别衡量SINS/DVL子滤波器与SINS/GNSS子滤波器的量测质量，实时选取对应子滤波器的融合系数。具体公式如下：

$ \begin{aligned} & M_k^i = \\ & \sqrt {{{(Z_k^i - {\boldsymbol{H}}_k^i\hat X_{k/k - 1}^i)}^{\rm{T}} }({\boldsymbol{H}}_k^i{\boldsymbol{P}}_{k/k - 1}^iH_k^{i {\rm{T}} } + R_k^i)(Z_k^i - {\boldsymbol{H}}_k^i\hat X_{k/k - 1}^i)} ，\end{aligned} $

(22)

$ \alpha _k^m = \frac{1}{{\sum\limits_{i = 1}^N {1/{e^{M_k^i}}} + 1}}。$

(23)

式中：$ M_k^i $为$k$时刻第$i$个子滤波器对应的马氏距离；$Z_k^i$为子滤波器量测；${\boldsymbol{H}}_k^i$为量测矩阵；${\boldsymbol{R}}_k^i$为量测噪声方差阵；$ \alpha _k^i $与$ \alpha _k^m $分别为$k$时刻子滤波器和主滤波器的融合系数。

子滤波器的量测误差越大，即量测质量越差，对应的马氏距离就越大，融合系数就越小，在主滤波器最终的最优融合过程中该滤波器的局部估计所占比例就降低，减少了故障子滤波器对整体系统的影响。此外，由于主滤波器没有量测更新，因此本算法将其对应的马氏距离默认为0。

3 仿真验证与分析

为了验证本文所提极区容错联邦滤波算法的有效性，按照前述组合导航系统模型，针对舰船进行仿真实验。

仿真条件如下：陀螺仪漂移为0.01°/h，加速度计零偏为5×10⁻⁵ g，舰船初始姿态误差为10′′，横向速度误差为0.1 m/s，横向位置误差为5 m，DVL测速误差为0.1 m/s，GNSS位置测量误差为5 m。舰船初始横向经纬度均为3°，横向水平速度均为5 m/s，舰船的摇摆运动模拟为：纵摇角：θ(t)=0.5° sin(2πt/5)；横摇角：γ(t)=2° sin(2πt/10)；航向角：ψ(t)=2° sin(2πt/6)+45°。

已有文献大多通过更改分配因子$\beta $来隔离故障，理论依据存在问题，因此对传统联邦滤波算法与所提算法进行对比仿真实验。为简便，以下将传统联邦滤波算法称为算法1，所提新算法称为算法2。

首先是无故障情况，仿真时间为3600 s，最终导航系统的横向速度误差与横向位置误差如图2和图3所示，误差均方差如表1所示。

由仿真结果对比可知，在无故障情况下，算法1与算法2的速度、位置误差结果相当，均方差值基本一致，横向速度误差在0.1 m/s之内，横向经纬误差小于5 m。

故障情况分为2个子滤波器在不同时间段分别发生故障以及在同一时间段都发生故障2种场景，仿真时间均为3 600 s。其中场景1设置为：在500～600 s时间区间内，DVL子滤波器量测发生故障，速度误差分别为5 m/s、8 m/s，在1 500～1 600 s时间区间内，GNSS子滤波器量测发生故障，位置误差分别为100 m、80 m。场景2设置为：在1 000～1 100 s时间区间内，DVL和GNSS子滤波器量测同时发生故障，速度误差均为10 m/s，位置误差均为100 m。仿真结果如图4～图7所示，误差平均均方差如表2所示。

由图4～图5可知，在500～600 s、1500～1600 s时间段内DVL和GNSS分别出现量测故障，此时算法1由于无法对故障子滤波器进行有效隔离，导致正常滤波器被污染，进而全局导航估计误差迅速增大，降低导航精度。而算法2引入的马氏距离通过衡量子滤波器量测故障从而减小故障子滤波器的融合系数降低其在全局估计过程中的比例，减少了对全局估计精度的影响，且在故障情况下该算法横向速度误差不超过0.1 m/s，横向位置误差小于6 m。

根据图6～图7及表2可知，在1000～1100 s时间段内，DVL和GNSS同时出现量测故障，此时算法相比场景1同样会出现误差迅速增大现象，不能抑制故障子滤波器对全局精度的不良影响。而在算法2中，由于此时2个子滤波器均出现量测故障，对应的融合系数均降低，此时的全局估计精度主要依靠主滤波器的时间更新，且横向速度误差不超过0.1 m/s，横向位置误差小于7 m。与算法1相比，尤其在故障情况下，本文提出的算法2导航结果更加平稳，可以有效隔离子滤波出现的量测故障，具有更好的容错性能，满足极区舰船导航的可靠性和精度需求。

4 结　语

本文提出基于马氏距离的极区容错联邦滤波算法，通过引入真实量测与预测量测之间的马氏距离来衡量子滤波器量测质量，并利用其构建对应的联邦滤波融合系数，自适应调整主滤波器最优融合过程中各子滤波器状态比例，减少故障子滤波器对全局估计的不良影响。通过实验仿真验证本算法在极区无故障情况下横向速度误差在0.1 m/s之内，横向经纬误差小于5 m，在子滤波器量测故障情况下相比传统联邦滤波算法导航结果更加平稳，可以更好地抑制故障子滤波器对整个系统的污染，且横向速度误差不超过0.1 m/s，横向位置误差小于7 m，满足极区导航需求，具有更好的容错性能，提高了极区导航系统的容错性和可靠性，对于实现极区舰船安全可靠的航行具有重要意义。