CN106679693A - 一种基于故障检测的矢量信息分配自适应联邦滤波方法 - Google Patents

Abstract

本发明公开了一种基于故障检测的矢量信息分配自适应联邦滤波方法，该方法通过建立惯性导航系统的误差状态方程及导航传感器的量测方程，构建卡尔曼子滤波器，在滤波器中设计双状态卡方检测函数，获得状态量中每一个分量的故障检测值。再通过故障检测值以及故障检测阈值构建联邦滤波器的矢量信息分配系数，完成联邦滤波器的信息分配，最终实现对组合导航误差状态量的最优估计。由于矢量信息分配系数是由故障检测值计算获得，利用该分配系数可以实现对量测噪声的自适应调节，并能够在子系统故障状态下将故障信息进行“软隔离”。本发明既提高了全局估计精度，又降低了故障污染。

Description

一种基于故障检测的矢量信息分配自适应联邦滤波方法

技术领域

本发明涉及一种基于故障检测的矢量信息分配自适应联邦滤波方法，属于组合导航技术领域。

背景技术

随着导航技术的发展，其应用领域越来越广泛，对导航系统的性能要求也越来越高。如何处理多种导航传感器的输出信息，获得最优的导航定位解算结果是其中的关键内容之一。在导航系统多传感器信息融合领域，Carlson提出的分散化联邦滤波器由于设计的灵活性、计算量小、容错性能好等优点而受到国内外导航界的关注。在联邦滤波器的基本结构中，无重置结构由于各子滤波器独立进行滤波，容错性能好，但是由于没有全局最优估计的重置，导致局部估计精度不高；而有重置结构中，其中的任何一个子系统发生故障，都会通过全局滤波的反馈重置而使得其他无故障的子系统局部滤波也受到影响，导致联邦滤波器的整体性能下降。因此，如何提高融合重置模式下联邦滤波器的鲁棒性具有重要的研究意义。

针对传统联邦滤波融合重置模式中子系统故障存在相互污染的缺陷，为提高联邦滤波的故障检测灵敏度和鲁棒性，国内外学者提出了许多改进方法，包括针对子滤波器估计的次优性对残差卡方故障检测算法灵敏度的影响进行故障检测算法的改进、为故障子滤波器分配较大或较小的信息分配系数等。经分析不难发现，现有的关于子系统故障条件下的信息分配系数选择问题还没有统一的结论，对于子系统故障条件下如何同时提高全局估计精度和降低故障污染的影响，还需要进行更深入的研究。

发明内容

本发明所要解决的技术问题是：提供一种基于故障检测的矢量信息分配自适应联邦滤波方法，在子系统故障条件下，既提高了全局估计精度，又降低了故障污染。

本发明为解决上述技术问题采用以下技术方案：

一种基于故障检测的矢量信息分配自适应联邦滤波方法，包括如下步骤：

步骤1，在惯性导航系统中，选取18维系统误差状态量及东北天坐标系，建立惯性导航系统的误差状态方程；

步骤2，在东北天坐标系下，根据各个导航传感器的数据输出特性，建立各组合导航子系统的量测方程；

步骤3，根据惯性导航系统的误差状态方程和各组合导航子系统的量测方程，设计各个组合导航子系统的闭环卡尔曼滤波器；

步骤4，在各个组合导航子系统的闭环卡尔曼滤波器内，加入双状态卡方检测函数，获得各导航传感器的故障检测信息；

步骤5，根据步骤3获得的各个组合导航子系统的闭环卡尔曼滤波器计算状态估计量，利用联邦主滤波器对状态估计量进行信息融合，得到全局最优估计结果；

步骤6，根据步骤4获得的各导航传感器的故障检测信息，以及故障检测阈值设计各个子滤波器对应的矢量信息分配参数，公式如下：

其中，B_i,k(j,j)表示矩阵B_i,k对角线的第j个分量，B_i,k表示第i个子滤波器k时刻的矢量信息分配参数，η_i,k表示第i个子滤波器k时刻系统误差状态量的故障检测信息，表示η_i,k的第j个分量，表示第i个子滤波器的故障检测阈值的第j个分量，i＝1,2,…,N，N为子滤波器的数目；

根据各个子滤波器对应的矢量信息分配参数完成联邦主滤波器的信息分配过程，公式如下：

其中，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，Ρ_g表示全局状态估计量的协方差矩阵，Q_i,k表示第i个子滤波器k时刻的系统噪声方差矩阵，Β_iQ为Β_i,k后9个对角线元素组成的矩阵，Q_g为主滤波器中设置的系统噪声方差矩阵，表示第i个子滤波器k时刻的系统状态估计值，表示状态量的全局最优估计结果。

作为本发明的一种优选方案，该方法还包括步骤7：根据步骤6获得的矢量信息分配参数，计算各个子系统的量测噪声矩阵，实现量测误差的自适应调节。

作为本发明的一种优选方案，步骤1所述惯性导航系统的误差状态方程如下：

X_k＝Φ_k|k-1X_k-1+G_k-1W_k-1

其中，X_k、X_k-1分别为k、k-1时刻系统误差状态量，Φ_k|k-1为惯性导航系统k-1到k时刻的状态转移矩阵，G_k-1为惯性导航系统k-1时刻的白噪声误差系数矩阵，W_k-1为k-1时刻白噪声随机误差向量。

作为本发明的一种优选方案，步骤2所述各组合导航子系统的量测方程如下：

Z_i,k＝H_i,kX_k+V_i,k

其中，Z_i,k为k时刻通过第i个组合导航子系统获得的量测信息，H_i,k为第i个子滤波器k时刻的量测系数矩阵，X_k为k时刻系统误差状态量，V_i,k为第i个子系统k时刻的量测噪声，i＝1,2,…,N，N为子系统或子滤波器的数目。

作为本发明的一种优选方案，步骤3所述各个组合导航子系统的闭环卡尔曼滤波器如下：

其中，表示第i个子滤波器k时刻的一步预测估计值，P_i,k|k-1表示第i个子滤波器k时刻的一步预测估计误差协方差阵，Φ_k|k-1为惯性导航系统k-1到k时刻的状态转移矩阵，P_i,k-1|k-1表示第i个子滤波器k-1时刻的系统状态估计误差协方差阵，G_k-1为惯性导航系统k-1时刻的白噪声误差系数矩阵，Q_i,k-1表示第i个子滤波器k-1时刻的系统噪声方差矩阵，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，K_i,k表示第i个子滤波器k时刻的滤波增益矩阵，H_i,k为第i个子滤波器k时刻的量测系数矩阵，R_i,k表示第i个子滤波器k时刻的量测误差方差矩阵，表示第i个子滤波器k时刻的系统状态估计值，Z_i,k为k时刻通过第i个组合导航子系统获得的量测信息，I为18×18的单位矩阵。

作为本发明的一种优选方案，步骤4所述各导航传感器的故障检测信息，表示如下：

其中，η_i,k表示第i个子滤波器k时刻系统误差状态量的故障检测信息，表示η_i,k的第j个分量，且服从χ²(1)分布，表示第i个子滤波器k时刻状态差值的第j个分量，T_i,k(j,j)表示矩阵T_i,k对角线的第j个分量，T_i,k表示第i个子滤波器k时刻状态差值协方差阵。

作为本发明的一种优选方案，步骤5所述利用联邦主滤波器对状态估计量进行信息融合，得到全局最优估计结果，表示如下：

其中，表示状态估计量的全局最优估计结果，Ρ_g表示全局状态估计量的协方差矩阵，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，表示第i个子滤波器k时刻的系统状态估计值，i＝1,2,…,N，N为子滤波器的数目。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明根据子滤波器对每个状态量的状态卡方故障检测值，设计了动态矢量信息分配系数，使得无故障子滤波器分配到较大的信息分配系数，有故障的子滤波器分配到较小的信息分配系数，减小子滤波器故障对联邦滤波全局估计的影响，避免故障子滤波器在信息重置过程中对系统造成的污染。

2、本发明根据子滤波器的状态卡方故障检测值，计算出量测噪声矩阵的修正系数，动态调节量测噪声，使得当量测信息异常时，减小对应量测信息的使用权重，提高子滤波器的估计性能。同时本发明中通过对量测噪声矩阵的动态调整，减少了一般算法结构中的故障隔离模块，提高了联邦滤波架构的鲁棒性。

附图说明

图1是本发明基于故障检测的矢量信息分配自适应联邦滤波方法的整体架构图。

图2是本发明中双状态卡方检测算法的结构原理图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，本发明的原理是：通过建立惯性导航系统的误差状态方程及传感器子系统的量测方程，构建卡尔曼子滤波器，在滤波器中设计双状态卡方检测函数，获得状态量的故障检测值。再通过故障检测值以及故障检测阈值构建联邦滤波器的矢量信息分配系数，完成联邦滤波器的信息分配，最终实现对组合导航误差状态量的最优估计。由于矢量信息分配系数是由故障检测值计算获得，通过对量测噪声的自适应调节，能够在子系统故障状态下将故障信息进行“软隔离”。

本发明的具体实施过程如下：

(1)建立惯性导航系统的状态方程

在惯性导航系统中，选取18维系统误差状态量：

其中，φ_E,φ_N,φ_U分别表示惯性导航系统误差状态量中东向、北向和天向的平台误差角状态量；δv_E,δv_N,δv_U分别表示东向、北向和天向的速度误差状态量；δL,δλ,δh分别表示纬度、经度和高度的误差状态量；ε_bx,ε_by,ε_bz和ε_rx,ε_ry,ε_rz分别表示X轴、Y轴和Z轴方向的陀螺常值漂移误差状态量以及X轴、Y轴和Z轴方向的陀螺一阶马尔可夫漂移误差状态量；分别表示X轴、Y轴和Z轴方向的加速度计一阶马尔科夫状态量。

导航坐标系选取为东北天坐标系，可以获得惯性导航系统的误差状态方程为：

X_k＝Φ_k|k-1X_k-1+G_k-1W_k-1 (1)

其中，Φ_k|k-1为惯性导航系统误差方程所对应的状态转移矩阵，G_k-1为惯性导航系统误差方程所对应的白噪声误差系数矩阵，W_k-1为惯性导航系统误差方程所对应的白噪声随机误差向量，X_k为k时刻系统误差状态量。

(2)建立组合导航子系统的量测方程

在东北天导航坐标系下，根据各个传感器的数据输出特性，建立线性化的量测方程，如式(2)所示：

Z_i,k＝H_i,kX_k+V_i,k (2)

其中，Z_i,k为k时刻通过第i个子系统获得的量测信息，H_i,k为k时刻第i个子滤波器的量测系数矩阵，V_i,k为k时刻第i个子系统的量测噪声，i＝1,2,…,N，N为子系统的数目。

(3)子系统闭环卡尔曼滤波器设计

根据(1)和(2)中建立的系统状态方程和量测方程，设计对应子系统的闭环卡尔曼滤波器，一个传感器对应一个组合导航子系统，一个组合导航子系统对应一个子滤波器，如式(3)所示：

其中，表示第i个子滤波器k时刻的一步预测估计值，表示第i个子滤波器k时刻的系统状态估计值，K_i,k表示第i个子滤波器k时刻的滤波增益矩阵，P_i,k|k-1表示第i个子滤波器k时刻的一步预测估计误差协方差阵，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，P_i,k-1|k-1表示第i个子滤波器k-1时刻的系统状态估计误差协方差阵，Q_i,k-1表示第i个子滤波器k-1时刻的系统噪声方差矩阵，R_i,k表示第i个子滤波器k时刻的量测误差方差矩阵，该参数通过信息分配矢量计算获得，在步骤(7)中详细说明，I为18×18的单位矩阵。

(4)双状态卡方检测

在第i个卡尔曼滤波子系统中，设计双状态残差卡方检测函数。定义状态变量及状态估计误差协方差阵

定义状态估计误差和

则状态的差值δe_i,k可表示为：

状态的差值协方差阵为：

当有则：

所以，与状态量X_k中每个分量对应的故障检测函数为：

其中，服从χ²(1)分布。

在式(4)的递推过程中，由于系统噪声及建模误差等的影响，将越来越偏离真实值。因此，设计了双状态递推的方法，其结构原理如图2所示，两个状态递推器交替工作，并用卡尔曼滤波的系统噪声协方差值P_i,k|k进行周期性重置。将整个时间段分为t₁∈[2nT^*,(2n+1)T^*)、t₂∈[(2n+1)T^*，2(n+1)T^*)、t₃∈{nT^*}，其中，n∈{0,1,2,…}，T^*为状态递推器的重置周期。将状态递推器设置为相同的初始值，在t∈t₁时，状态递推器A和B按各自流程进行递推，状态卡方检测函数使用计算；t∈t₂时，状态递推器A和B按各自流程进行递推，状态卡方检测函数使用计算；t∈t₃且n为奇数时，状态递推器A进行重置，即t∈t₃且n为偶数时，状态递推器B进行重置，即通过对状态递推器的交替性重置，能够避免系统误差及建模误差等引起的发散，从而使得故障检测函数值异常，提高故障检测准确率。

(5)联邦主滤波器信息融合过程

根据各个子滤波器计算获得的状态估计量，按式(10)进行融合，得到全局最优估计。

是状态量的全局最优估计结果，Ρ_g是全局估计状态量的协方差矩阵。

(6)矢量信息分配系数的设计及信息分配过程

根据(4)中计算获得各个子系统状态的故障检测值结合子系统的误警率参数按照卡方分布表选择故障检测阈值按式(11)设计各个子系统对应状态的矢量信息分配系数。

当系统没有发生故障时，当故障发生后，增大并超过阈值 且越小，表明该子系统对应的状态量发生的故障程度越大。

根据式(11)可知，B_i,k为18×18的矩阵，且满足信息守恒原理。

联邦滤波信息分配的表达式为：

其中，Β_iQ为Β_i,k后9个对角线元素组成的矩阵，对应系统噪声W的分配系数，Q_g为主滤波器中设置的系统噪声方差矩阵。

(7)量测噪声的自适应调节方法

本步骤主要说明步骤(3)中R_i,k的计算方法，根据矢量信息分配参数自适应计算量测误差矩阵。在信息分配矢量中，各个分量与系统状态量不完全一一对应，因此需要将其转换到量测信息上来。下面分别通过姿态量测信息、位置量测信息以及速度量测信息说明转换方法，并以此计算自适应调节参数。

A.姿态量测信息转换方法

系统状态量中的采用的参数为平台误差角[φ_Eφ_Nφ_U]^T，而量测信息中采用的参数为姿态误差角[δγ δθ δψ]^T(δγ、δθ、δψ分别表示横滚角误差、俯仰角误差及航向角误差)，因此矢量分配信息不能直接使用。其转换关系为：

其中，

因此，姿态量测信息对应的分配系数为：

归一化后，得：

为转换后的姿态量测误差自适应调节参数，则修正后的姿态量测噪声方差阵为：

其中，为初始姿态量测噪声方差阵。

B.速度量测信息转换方法

在速度量测信息中，转换矩阵

状态信息与量测信息一一对应，因此

为速度量测误差自适应调节参数，则修正后的速度量测噪声方差阵为：

其中，为初始速度量测噪声方差阵。

C.位置量测信息转换方法

在位置量测信息中，转换矩阵

其中，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，L为当前纬度信息。

状态信息与量测信息一一对应，因此

为位置量测误差自适应调节参数，则修正后的位置量测噪声方差阵为：

其中，为初始位置量测噪声方差阵。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (7)

1.一种基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，包括如下步骤：

步骤1，在惯性导航系统中，选取18维系统误差状态量及东北天坐标系，建立惯性导航系统的误差状态方程；

步骤2，在东北天坐标系下，根据各个导航传感器的数据输出特性，建立各组合导航子系统的量测方程；

步骤3，根据惯性导航系统的误差状态方程和各组合导航子系统的量测方程，设计各个组合导航子系统的闭环卡尔曼滤波器；

步骤4，在各个组合导航子系统的闭环卡尔曼滤波器内，加入双状态卡方检测函数，获得各导航传感器的故障检测信息；

步骤5，根据步骤3获得的各个组合导航子系统的闭环卡尔曼滤波器计算状态估计量，利用联邦主滤波器对状态估计量进行信息融合，得到全局最优估计结果；

步骤6，根据步骤4获得的各导航传感器的故障检测信息，以及故障检测阈值设计各个子滤波器对应的矢量信息分配参数，公式如下：

$B_{i,k}(j,j)=\frac{{({\mathrm{\Λ}}_i^{\left(j\right)}/{\mathrm{\η}}_{i,k}^{\left(j\right)})}^2}{\underset{i=1}{\overset{N}{\Σ}}{({\mathrm{\Λ}}_i^{\left(j\right)}/{\mathrm{\η}}_{i,k}^{\left(j\right)})}^2},j=1,2,\mathrm{...},18$

其中，B_i,k(j,j)表示矩阵B_i,k对角线的第j个分量，B_i,k表示第i个子滤波器k时刻的矢量信息分配参数，η_i,k表示第i个子滤波器k时刻系统误差状态量的故障检测信息，表示η_i,k的第j个分量，表示第i个子滤波器的故障检测阈值的第j个分量，i＝1,2,…,N，N为子滤波器的数目；

根据各个子滤波器对应的矢量信息分配参数完成联邦主滤波器的信息分配过程，公式如下：

$P_{i,k|k}^{-1}=\sqrt{B_{i,k}}{P}_g^{-1}\sqrt{B_{i,k}}$

$Q_{i,k}^{-1}=\sqrt{B_{iQ}}{Q}_g^{-1}\sqrt{B_{iQ}}$

${\hat{X}}_{i,k|k}={\hat{X}}_g$

其中，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，Ρ_g表示全局状态估计量的协方差矩阵，Q_i,k表示第i个子滤波器k时刻的系统噪声方差矩阵，Β_iQ为Β_i,k后9个对角线元素组成的矩阵，Q_g为主滤波器中设置的系统噪声方差矩阵，表示第i个子滤波器k时刻的系统状态估计值，表示状态量的全局最优估计结果。

2.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，该方法还包括步骤7：根据步骤6获得的矢量信息分配参数，计算各个子系统的量测噪声矩阵，实现量测误差的自适应调节。

3.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，步骤1所述惯性导航系统的误差状态方程如下：

X_k＝Φ_k|k-1X_k-1+G_k-1W_k-1

其中，X_k、X_k-1分别为k、k-1时刻系统误差状态量，Φ_k|k-1为惯性导航系统k-1到k时刻的状态转移矩阵，G_k-1为惯性导航系统k-1时刻的白噪声误差系数矩阵，W_k-1为k-1时刻白噪声随机误差向量。

4.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，步骤2所述各组合导航子系统的量测方程如下：

Z_i,k＝H_i,kX_k+V_i,k

其中，Z_i,k为k时刻通过第i个组合导航子系统获得的量测信息，H_i,k为第i个子滤波器k时刻的量测系数矩阵，X_k为k时刻系统误差状态量，V_i,k为第i个子系统k时刻的量测噪声，i＝1,2,…,N，N为子系统或子滤波器的数目。

5.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，步骤3所述各个组合导航子系统的闭环卡尔曼滤波器如下：

${\hat{X}}_{i,k|k-1}=0$

$P_{i,k|k-1}={\mathrm{\Φ}}_{k|k-1}{P}_{i,k-1|k-1}{\mathrm{\Φ}}_{k|k-1}^T+G_{k-1}{Q}_{i,k-1}{G}_{k-1}^T$

$P_{i,k|k}=(I-K_{i,k}{H}_{i,k})P_{i,k|k-1}{(I-K_{i,k}{H}_{i,k})}^T+K_{i,k}{R}_{i,k}{K}_{i,k}^T$

$K_{i,k}=P_{i,k|k-1}{H}_{i,k}^T{(H_{i,k}{P}_{i,k|k-1}{H}_{i,k}^T+R_{i,k})}^{-1}$

${\hat{X}}_{i,k|k}=K_{i,k}{Z}_{i,k}$

其中，表示第i个子滤波器k时刻的一步预测估计值，P_i,k|k-1表示第i个子滤波器k时刻的一步预测估计误差协方差阵，Φ_k|k-1为惯性导航系统k-1到k时刻的状态转移矩阵，P_i,k-1|k-1表示第i个子滤波器k-1时刻的系统状态估计误差协方差阵，G_k-1为惯性导航系统k-1时刻的白噪声误差系数矩阵，Q_i,k-1表示第i个子滤波器k-1时刻的系统噪声方差矩阵，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，K_i,k表示第i个子滤波器k时刻的滤波增益矩阵，H_i,k为第i个子滤波器k时刻的量测系数矩阵，R_i,k表示第i个子滤波器k时刻的量测误差方差矩阵，表示第i个子滤波器k时刻的系统状态估计值，Z_i,k为k时刻通过第i个组合导航子系统获得的量测信息，I为18×18的单位矩阵。

6.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，步骤4所述各导航传感器的故障检测信息，表示如下：

${\mathrm{\η}}_{i,k}^{\left(j\right)}=\frac{{\left({\mathrm{\δe}}_{i,k}^{\left(j\right)}\right)}^2}{T_{i,k}(j,j)},j=1,2,\mathrm{...},18$

其中，η_i,k表示第i个子滤波器k时刻系统误差状态量的故障检测信息，表示η_i,k的第j个分量，且服从χ²(1)分布，表示第i个子滤波器k时刻状态差值的第j个分量，T_i,k(j,j)表示矩阵T_i,k对角线的第j个分量，T_i,k表示第i个子滤波器k时刻状态差值协方差阵。

7.根据权利要求1所述基于故障检测的矢量信息分配自适应联邦滤波方法，其特征在于，步骤5所述利用联邦主滤波器对状态估计量进行信息融合，得到全局最优估计结果，表示如下：

$P_g={\left(\underset{i=1}{\overset{N}{\Σ}}{P}_{i,k|k}^{-1}\right)}^{-1}$

${\hat{X}}_g=P_g\left(\underset{i=1}{\overset{N}{\Σ}}{P}_{i,k|k}^{-1}{\hat{X}}_{i,k|k}\right)$

其中，表示状态估计量的全局最优估计结果，Ρ_g表示全局状态估计量的协方差矩阵，P_i,k|k表示第i个子滤波器k时刻的系统状态估计误差协方差阵，表示第i个子滤波器k时刻的系统状态估计值，i＝1,2,…,N，N为子滤波器的数目。