0 引　言

由于惯性器件误差的存在，惯性导航系统误差会随着时间迅速累积，因此惯导不能单独用于舰艇的长时间导航与定位。全球卫星定位系统能够提供较精确的当地位置信息，且不会随着时间发散，但是存在输出频率较低，容易受到干扰的缺点，所以常把全球卫星定位系统和惯导系统组合起来用于载体的导航与定位^[1-2]。

当前舰艇上的航向传感器主要包括：平台罗经、天文导航系统、惯性导航系统以及卫星导航系统。它们均有各自的缺点，比如平台罗经和惯导航向误差会随着时间的积累而增大；天文导航系统容易受到天气环境的影响，在可见度低的条件下难以正常工作^[3]，卫导系统在静止和大机动条件下航向指示误差大。因此这些航向传感器也不适合单独使用。

为了提高导航系统的可靠性和航向测量的精度，本文在惯导/卫导组合导航系统中加入了平台罗经和天文导航系统，并通过基于传感器输出误差协方差的权值分配方法实现航向信息的融合，从而更加准确地测量得到舰艇的航向信息。

图1为该组合导航系统的原理图。除了包含航向传感器和导航系统外，该结构还包含优化航向信息融合和故障容错卡尔曼滤波器2个模块。本文主要对这2个系统作特别的说明，并且通过船舶航行试验验证设计方案的可行性。

1 最优航向信息融合

本文通过权值法对多个位置传感器的数据进行融合：

${k_O}{\rm{ = }}{ W}MK = \left[ {{w_g},{w_c},{w_e}} \right]\left[ {diag{m_g},{m_c},{m_e}} \right]\left[ {{k_g},{k_c},{k_e}} \right] \text{。}$

(1)

式中：W为权值矩阵，其中 ${w_g}$ ， ${w_c}$ ， ${w_e}$ 分别为GPS、天导、电罗经航向对应的权值；M为故障矩阵，其中 ${m_g}$ ， ${m_c}$ ， ${m_e}$ 分别为GPS、天导、电罗经航向对应的故障状态； ${ K}$ 为航向信息矩阵，其中 ${k_g}$ ， ${k_c}$ ， ${k_e}$ 分别为GPS、天导、电罗经航向对应的航向值。

权值的分配基于误差协方差，以电罗经输出的航向最优信息为例，其权重为：

${w_e} = \frac{{{m_e}}}{{\sigma _e^2({m_e}/\sigma _e^2 + {m_c}/\sigma _c^2 + {m_g}/\sigma _g^2)}}\text{。}$

(2)

式中： $\sigma _e^2$ ， $\sigma _c^2$ ， $\sigma _g^2$ 分别为电罗经、天文导航系统、GPS对应的输出协方差。通过这样的权值分配方案，一旦检测出某个航向传感器发生故障，就可以自动的将该传感器对应的航向信息进行隔离，并对权值进行重新分配。

在k时刻，如果电罗经输出的航向信息为 ${\tilde k_e}(k)$ ，按照下式计算得到电罗经的输出协方差：

$\left\{ \begin{array}{l} \Delta {{\tilde k}_e}(k) = {{\tilde k}_e}(k) - {{\hat k}_e}(k); \\ \Delta {{\bar k}_e} = \frac{1}{N}\sum\limits_{k = 1}^N {\Delta {{\tilde k}_e}(k),{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} k = 1,2,...,N}; \\ {\sigma _e}(k) = \frac{1}{N}\sum\limits_{k = 1}^N {(\Delta {{\tilde k}_e}(k) - \Delta {{\bar k}_e})} \text{。} \\ \end{array} \right.$

(3)

式中， ${\hat k_e}(k)$ 为k时刻卡尔曼滤波器的估计航向， $\Delta {\tilde k_e}(k)$ 为k时刻电罗经的滤波残差， $\Delta {\bar k_e}$ 为电罗经滤波残差的平均值。

按照以下方式得到航向融合解：

${k_o} = {w_e}{m_e}{k_e} + {w_c}{m_c}{k_c} + {w_g}{m_g}{k_g}\text{。}$

(3)

2 故障容错卡尔曼滤波器设计 2.1 卡尔曼滤波器

图2为故障容错卡尔曼滤波器的原理框图。其中滤波器O为标准SINS/GNSS组合导航系统，采用文献[4]的状态方程与观测方程，状态变量为 ${X_O} = {[{\varphi _x}},{\varphi _y},{\varphi _z},\delta {v_x}, $ $\delta {v_y},\delta {v_z},{\delta _x},{\delta _y},{\delta _z},{\varepsilon _x},{\varepsilon _y},{\varepsilon _z},{\nabla _x},{\nabla _y},{\nabla _z}]^{{\rm{T} }}$ ，其中 ${\varphi _x},{\varphi _y},{\varphi _z}$ 均为航姿误差， $\delta {v_x},\delta {v_y},\delta {v_z}$ 均为速度误差， ${\delta _x},{\delta _y},{\delta _z}$ 均为位置误差， ${\varepsilon _x},{\varepsilon _y},{\varepsilon _z}$ 均为陀螺零偏， ${\nabla _x},{\nabla _y},{\nabla _z}$ 均为加速度计零偏；滤波器C，E，G分别为SINS与电罗经、天文导航、GPS以航向差为观测量组成的卡尔曼滤波器。本文选用的卡尔曼滤波器均为标准滤波器，以卡尔曼滤波器G为例，卡尔曼滤波器的状态向量为：X_g= $ {[{\varphi _x}},{\varphi _y},{\varphi _z},{\varepsilon _x}, $ ${\varepsilon _y},{\varepsilon _z}]^{{\rm{T}} }\text{。}$

其中 ${\varphi _x},{\varphi _y},{\varphi _z}$ 为SINS的姿态误差， ${\varepsilon _x},{\varepsilon _y},{\varepsilon _z}$ 为陀螺漂移的常值项。

卡尔曼滤波器G的离散化状态方程为：

${X_{g,k}} = {F_{g,k}}{X_{g,k - 1}} + {w_{g,k}},$

(5)

${F_{g,k}} = \left[ {\begin{array}{*{20}{c}} {{I_3}}&{\hat C_b^n{\tau _s}} \\ {{0}}&{{I_3}} \end{array}} \right], $

(6)

其中： ${I_3}$ 为 $3 \times 3$ 单位矩阵； $\hat C_b^n$ 为载体坐标系到导航坐标系转换矩阵的估计； ${\tau _s}$ 为滤波的间隔； ${w_{g,k}}$ 为过程噪声；假设为零均值方差 ${Q_{g,k}}$ 的高斯白噪声。

观测量为：

${Y_g} = {k_s} - {k_g},$

(7)

式中， ${k_s}$ 为SINS的航向输出。

观测方程为：

${Y_{g,k}} = {H_{g,k}}{X_{g,k}} + {\upsilon _{g,k}},$

(8)

${H_{g,k}} = \left[ {\begin{array}{*{20}{c}} {{I_3}}&{{0_3}} \end{array}} \right]{\text{。}}$

(9)

其中， ${\upsilon _{g,k}}$ 为测量噪声，假设为零均值方差 ${R_{g,k}}$ 的高斯白噪声。

卡尔曼滤波器C以及卡尔曼滤波器E与卡尔曼滤波G具有完全相同的结构，状态矩阵也一样，观测量分别为CNS、电罗经与SINS的航向差。

2.2 状态传播

状态传播器只对状态进行预测，不进行量测更新，所以不受故障观测值的影响，因此状态传播器的预测结果可作为子滤波器的参考信号，用于检测观测数据中的故障数据。状态传播器的状态传播方程为：

$\left\{ \begin{array}{l} {{\hat X}_{i,k|k - 1}} = {F_{i,k}}{X_{i,k - 1|k - 1}} \\ {P_{i,k|k - 1}} = {F_{i,k}}{P_{i,k - 1|k - 1}}{F_{i,k}}^{\rm{T}} + {Q_{i,k}} \end{array} \right.,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i = 1,2,\cdots \text{，}$

(10)

$ {F_{i,k}} = {F_{g,k}}{\text{。}} $

(11)

其中，下标i为第i个状态传播器。为了避免状态传播器发散，每隔一段时间利用主滤波器的信息交替对状态传播器进行校准。

2.3 故障检测

检验信号和参考信号之间一致性的卡方检验被广泛使用于随机动态系统中的故障检测。通过使用不同的方式来计算检验统计量，卡方检验可以分为3类^[5-7]：1）使用滤波器残差的残差卡方检验，尽管该方法可以容易地检测传感器中的故障，但几乎不可能检测到渐变的故障；2）通过使用具有一个状态传播器的状态向量卡方检验，该方法可以检测到渐变故障，但信号的幅度随着运行时间增加最终发散；3）通过使用具有2个状态传播器的状态向量卡方检验，该方法可以通过在一段时间间隔后根据卡尔曼滤波器的输出交替地重置2个状态传播器，从而避免发散。但是，状态传播器的数量是滤波器数量的2倍，这可能严重影响计算速度。

本文采用文献[7]的基于残差 ${\chi ^2}$ 检测和状态 ${\chi ^2}$ 检测的并行结构故障检测方法。只要故障检测器中的一个检测到故障，便可以认为该传感器是故障传感器。此外，由于所有卡尔曼滤波器的状态向量相同，所以只需要使用2个状态传播器，从而减少了计算量。

为了得到状态 ${\chi ^2}$ ，定义误差状态向量为：

$\left\{ \begin{array}{l} {e_{i,k}} = {X_k} - {{\hat X}_{i,k}}, \\ {e_{i,g}} = {X_k} - {{\hat X}_{g,k}}, \end{array} \right.$

(12)

其中： ${X_k}$ 为状态向量的真值， ${\hat X_{g,k}}$ 为卡尔曼滤波器G估计的状态， ${\hat X_{i,k}}$ 为状态传播器i的估计状态。

本文采用下述变量作为卡尔曼滤波器G的故障检测的检测统计量：

${\beta _{g,k}} = {e_{i,k}} - {e_{i,g}} = {\hat X_{g,k}} - {\hat X_{i,k}},$

(13)

该变量的协方差为：

$\begin{split} {T_{g,k}} \!=\!& E\left\{ {{\beta _{g,k}}\beta _{g,k}^{\rm T}} \right\} \!=\! E\left\{ {e_{g,k}}e_{g,k}^{\rm T} - {e_{g,k}}e_{i,k}^{\rm T} - {e_{i,k}}e_{g,k}^{\rm T} \!+\! \right. \\ &\left. {e_{i,k}}e_{i,k}^{\rm T} \right\} = \;\;{P_{g,k}} - {P_{gi,k}} - {P_{ig,k}} + {P_{i,k}} , \end{split} $

(14)

其中， ${P_{gi,k}}$ 为卡尔曼滤波器与状态传播器i的协方差。

假定卡尔曼滤波器G与状态传播器i的初始状态相同，可以得到 ${P_{gi,k}} = {P_{ig,k}} = {P_{i,k}}$ ，所以变量的协方差可以写成：

${T_{g,k}} = {P_{g,k}} - {P_{i,k}},$

(15)

所以状态 ${\chi ^2}$ 的检验统计量可以写成：

${\lambda _{g,k}} = \beta _{g,k}^TT_{g,k}^{ - 1}{\beta _{g,k}},$

(16)

检测统计量 ${\lambda _{g,k}}$ 服从自由度为6的 ${\chi ^2}$ 分布^[8]，故障检测规则为：

$\left\{ \begin{array}{l} {\lambda _{g,k}} > {\varepsilon _\beta },{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\rm fault}; \\ {\lambda _{g,k}} < {\varepsilon _\beta },{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\rm no}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\rm fault} \text{。} \end{array} \right.$

(17)

其中阈值 ${\varepsilon _\beta }$ 由误警率函数确定。

同理，对于卡尔曼滤波器G定义残差 ${\chi ^2}$ 检验的误差观测向量为：

${d_{g,k}} = \Delta {k_{g,k}} - \Delta {\hat k_{i,k}},$

(18)

残差协方差为：

${S_{g,k}} = {H_{g,k}}{P_{g,k}}H_{g,k}^{\rm T} + {R_{g,k}},$

(19)

所以残差 ${\chi ^2}$ 的检验统计量可以写成：

${\gamma _{g,k}} = d_{g,k}^{\rm T}S_{g,k}^{ - 1}{d_{g,k}},$

(20)

如果状态 ${\chi ^2}$ 或残差 ${\chi ^2}$ 检验检测到了故障，那么 ${m_g} = 0$ ，否则 ${m_g} = 1$ 。

3 试验结果与比较分析

为了验证设计方案的可行性，本文以高精度天文导航系统的航向为参考基准，在船上做了验证试验。试验采集了几个设备3 h左右的输出数据，试验结果如下：

1）无故障检测的融合

采用无故障检测融合算法计算出的航向如图3所示。与卫导输出的航向相比，航向值的波动幅度明显减小，从原来的 $ \pm {250^ {\circ} }$ 下降到 $ \pm {60^ {\circ} }$ 。与惯导/卫导组合输出的航向相比，在船运动过程中，野值点的数量明显减少。

滤波器G，E，C的残差及状态卡方检测统计量分别如图4和图5所示。从图4可以看出，滤波器G在船静止状态下的残差卡方检测统计量大多数都很大，而在运动状态下的数值较小，这说明卫导在静止条件下得到的航向不可用，动态性能较好，但也存在一些跳跃点。而滤波器E，C的故障检测统计量在整个航行过程中的数值都较小，只存在几个数值较大的点，这说明电罗经与天导系统输出的航向比较稳定，只含有一些幅值较大的故障信息。从图5可以看出，卫导航向数据中含有缓变干扰，而电罗经和天导航向不含有缓变干扰。

2）含有故障检测与隔离的融合算法

使用含有故障检测与隔离的融合算法，选取误警率为1%，则 ${\lambda _k} = 0.872$ ， ${\gamma _k} = 0.115$ 。得到的融合结果如图6和图7所示。

对比图3和图6，可以看出，经过故障检测与隔离算法后，融合得到的航向信息更加平滑，野值点得到抑制。从图7可以看出，加入故障检测与隔离后，融合出的航向，其相对误差明显减小，从原来的 $ \pm {1.7^ {\circ} }$ 减小到 $ \pm {0.5^ {\circ} }$ 。

4 结　语

本文提出了一种适用于舰艇的基于最优信息融合与容错卡尔曼滤波器的多航向传感器信息融合算法，该方法利用了基于协方差的权值分配方法以及状态卡方与残差卡方并行的故障检测策略，能够同时检测到突变故障与缓变故障，为了防止状态传播器的发散，本文还引入了双状态传播器，进一步提高了系统的可靠性，然后利用故障检测结果对故障传感器进行自动隔离和权值的重新分配，最后通过实际试验验证方案的可行性。试验结果表明，本文提出的航向融合算法能够明显的提高系统的可靠性以及航向精度，为工程实践提供参考。