CN110823217B - 一种基于自适应联邦强跟踪滤波的组合导航容错方法 - Google Patents

Abstract

本发明公开了一种基于自适应联邦强跟踪滤波的组合导航容错方法，包括如下步骤：建立组合导航系统状态模型；构建故障检测函数进行检测；构建自适应强跟踪滤波算法进行状态估计；将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值；将全局状态估计值反馈给无故障子系统，对于存在故障的子系统，需要重置子系统信息；将获得的全局状态估计值校正惯性导航系统输出的导航参数。本发明公开的方法不仅能提高系统的滤波精度，还能提高滤波器对系统状态发生突变时的跟踪能力，可以对故障系统进行检测和隔离，提升融合结果的精度和可靠性，从而增强组合导航系统的鲁棒性，本方法简单，易于实现。

Description

一种基于自适应联邦强跟踪滤波的组合导航容错方法

技术领域

本发明属于组合导航技术领域，特别涉及一种基于自适应联邦强跟踪滤波的组合导航容错方法。

背景技术

随着科学技术的进步和时代的发展，无论是军事领域还是民用领域，对高精度、高可靠性的导航定位需求都在不断增长。全球导航卫星系统(GNSS)是室外高精度定位的主要手段，具有长时测量精度高、全天候工作等特征，但在森林、城市峡谷等复杂区域中，卫星信号易被遮挡、干扰，从而导致GNSS定位设备无法正常输出导航参数。捷联惯性导航系统(SINS)是一种自主性的导航系统，能在短时间内提供精确的相对位置信息，但受限于误差积累，不适合长时间导航。多普勒计程仪(DVL)利用多普勒效应，通过对频移的测量来获取速度信息，具有较强的实时性以及一定的自主性，但因其基于声学原理和安装方面因素，在测速时会存在干扰噪声及偏差。考虑到单一系统定位精度差和稳定性差等问题，将两种或两种以上的导航系统通过某种方法组合起来构成组合导航系统，通过取长补短、综合发挥各子导航系统的优势，从而提升导航定位的精度。因此，组合导航系统己经成为一种趋势。

对于组合导航系统而言，在系统硬件性能一定的条件下，先进的滤波算法是提高其精度、实时性和可靠性，实现协同超越的有效途径。卡尔曼滤波(KF)是用于集成传感器输出的信号以产生最优解的最常用的估计技术，但只能在线性系统中使用。扩展卡尔曼滤波(EKF)简单地将所有非线性模型线性化为一阶，被广泛用于解决非线性状态估计问题。但EKF需要对方程进行泰勒级数展开来求解雅可比矩阵，计算量大，且对非线性函数进行线性化近似精度不高。UKF是UT变换与KF的结合，其核心思想是利用一组Sigma点通过UT变换对非线性系统状态的后验概率密度函数进行近似，其实现简单、滤波精度高且收敛性较好。然而，当系统状态方程或量测方程具有线性特征时，标准UKF在滤波解算中存在大量冗余计算。另外，UKF要求系统过程模型与量测模型的先验统计信息精确已知，系统模型的不确定性会导致UKF滤波精度降低甚至发散。周华东等人提出的强跟踪滤波(STF)算法，通过引入的渐消因子来增加预测误差协方差，并在线调整相应的增益矩阵，保证滤波过程中有效信息全部提取出来，让滤波器实时跟踪系统的状态，具有较强的鲁棒性。但传统的强跟踪算法中，渐消因子参数的选择通常是根据经验取值，且在系统正常情况下，强跟踪存在一定的错判概率。以上两点影响滤波的精确性。

利用滤波技术对组合导航系统进行最优组合，一般有两种方式：集中式融合和分散化融合。由于集中式融合需要将各个传感器的测量信息送到中心站集中处理，计算负担重，不利于滤波的实时运行，而且容错性能很差。分散式滤波技术中的联邦卡尔曼滤波技术，由于设计的灵活性，计算量小，容错性能好，而受到广泛的应用。在融合过程中，FKF的主滤波器需要计算大量局部与全局状态估值的协方差阵的逆矩阵，计算复杂度较高；特别当选取的组合导航系统状态的维数较高时，计算量更是急剧增加。并且，当某个子系统的观测出现较大误差或者滤波出现故障，主滤波器的融合结果也会出现较大误差，从而影响这个系统的滤波精度。因此，设计具有检错容错的自适应联邦滤波结构，降低系统计算复杂度，提升定位性能，具有实际应用价值。

发明内容

为解决上述技术问题，本发明提供了一种基于自适应联邦强跟踪滤波的组合导航容错方法，以达到能提高滤波精度，提高滤波器对系统状态发生突变时的跟踪能力，具有检错容错功能的目的。

为达到上述目的，本发明的技术方案如下：

一种基于自适应联邦强跟踪滤波的组合导航容错方法，包括如下步骤：

步骤1：以SINS为参考系统，选择状态量和观测量，建立组合导航系统状态模型，以及SINS/GPS和SINS/DVL子系统量测方程；

步骤2：构建故障检测函数进行检测；

步骤3：构建自适应强跟踪滤波算法进行状态估计；

步骤4：将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值；

步骤5：将全局状态估计值反馈给无故障子系统，对于存在故障的子系统，需要重置子系统信息；

步骤6：将获得的全局误差的估计值校正惯性导航系统输出的导航参数。

上述方案中，所述步骤1具体如下：

建立组合导航系统状态模型，如下式：

式中，X(t)为系统状态向量，F(t)为状态转移矩阵，W(t)为系统白噪声；

φ_E,φ_N,φ_U分别表示捷联惯性导航系统导航坐标系内东向、北向、天向的失准角；

δv_E,δv_N,δv_U分别表示捷联惯性导航系统东向、北向、天向的速度误差；

δL,δλ,δh分别表示捷联惯性导航系统的纬度误差、经度误差、高度误差；

ε_rx,ε_ry,ε_rz分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的加速度计零偏；

SINS/GPS子系统选择位置和速度作为量测量，即

式中，υ_INSE,υ_INSN,υ_INSU,L_INS,λ_INS,h_INS分别为惯导解算的东向速度、北向速度、天向速度、纬度、经度、高度；

υ_GPSE,υ_GPSN,υ_GPSU,L_GPS,λ_GPS,h_GPS分别为GPS测得的东向速度、北向速度、天向速度、纬度、经度、高度；

H₁(t)为量测矩阵，

V₁(t)为GPS接收机在导航坐标系下的位置和速度误差噪声；

SINS/DVL子系统选择速度作为量测量，即

式中，υ_DVLE,υ_DVLN,υ_DVLU分别为多普勒测得的东向速度、北向速度、天向速度；

H₂(t)为量测矩阵，

H₂＝|0_3×3 diag|1 1 1|0_3×9|；

V₂(t)为DVL在导航坐标系下的速度误差噪声；

分别进行离散化处理得到：

式中，F_i,k|k-1,H_ik,W_i,k-1,V_ik分别为第i个子滤波器的状态转移阵，量测阵，系统噪声阵，量测噪声阵。

上述方案中，所述步骤2具体如下：

定义故障检测函数

其中：e_k为残差，即E[e_k]＝μ,E{[e_k-μ][e_k-μ]^T}＝Σ_k；

γ_k是服从自由度为m的χ²分布，即γ_k～χ²(m)，m为量测z_k的维数；

故障判定准则为:

γ_k＞T_D，出现故障，

γ_k≤T_D，未出现故障；

其中，T_D为门限阈值，T_D＝f(χ²,P_FA/m)，P_FA为误警率。

上述方案中，所述步骤3具体如下：

自适应强跟踪滤波算法主要过程包括求解渐消因子、时间更新、测量更新：使用开窗法估计残差序列协方差，并自适应调整量测噪声阵，得到改进的渐消因子；利用时间更新求出状态的一步预测用渐消因子对状态一步预测误差协方差P_k|k-1进行调整；利用测量更新求出状态的估计值/>和方差的估计值P_k；具体过程如下:

①求解自适应渐消因子λ_i,k：

计算残差：

残差协方差的估计值：

残差协方差的理论值：

式中，Tr(·)表示对矩阵求迹；z_i,k为真实测量值，R_i,k为量测噪声方差阵；N为采样数或窗长度；ξ为弱化因子，一般ξ≥1；

②时间更新：

状态一步预测方程：

一步预测误差协方差方程：

③测量更新：

滤波增益方程：

状态估值计算方程：

估计误差协方差方程:P_i,k＝[I-K_i,kH_i,k]P_i,k|k-1；

式中，λ_i,k为渐消因子，λ_i,k＞1时，可以自适应调整一步预测误差协方差P_i,k|k-1，为强跟踪滤波；当λ_i,k≤1时，为标准KF滤波。

上述方案中，所述步骤4具体如下：

将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值，融合方式为：

式中，为全局状态估计，P_g为全局状态估计协方差阵；β_i为加权系数，

如果检测出子滤波器出现故障，融合过程中将故障子系统数据进行隔离，保证融合结果的准确性。

上述方案中，所述步骤5具体如下：

将全局状态估计值反馈给无故障子系统，信息分配原则为：

对于存在故障的子系统j，需要重置子系统信息，此时β_j＝1；

上述方案中，所述步骤6具体如下：将全局误差估计值反馈至惯性导航系统，用惯性导航系统输出的导航参数减去全局误差估计值，得到最终导航参数的估计值。

通过上述技术方案，本发明提供的基于自适应联邦强跟踪滤波的组合导航容错方法有益效果如下：

本发明首先对子系统的滤波状态进行检测，判断系统是否正常运行；在子系统中使用自适应强跟踪滤波技术，计算改进后的渐消因子并对一步状态预测的误差协方差值进行校正；在主滤波器融合过程中，将无故障子系统的输出数据进行自适应融合，隔离故障子系统数据，保证得到的全局估计的准确性；使用全局最优估计校正惯性导航系统输出的导航参数，并将全局估计值反馈给无故障子系统，重置故障子系统。本发明不仅能提高系统的滤波精度，还能提高滤波器对系统状态发生突变时的跟踪能力，可以对故障系统进行检测和隔离，提升融合结果的精度和可靠性，从而增强组合导航系统的鲁棒性。

附图说明

图1为本发明所公开的一种基于自适应联邦强跟踪滤波的组合导航容错方法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本发明提供了一种基于自适应联邦强跟踪滤波的组合导航容错方法，具体实施例如下：

如图1所示，一种基于自适应联邦强跟踪滤波的组合导航容错方法，包括如下步骤：

步骤1：以SINS为参考系统，选择状态量和观测量，建立组合导航系统状态模型，以及SINS/GPS和SINS/DVL子系统量测方程；

建立组合导航系统状态模型，如下式：

式中，X(t)为系统状态向量，F(t)为状态转移矩阵，W(t)为系统白噪声；

φ_E,φ_N,φ_U分别表示捷联惯性导航系统导航坐标系内东向、北向、天向的失准角；

δv_E,δv_N,δv_U分别表示捷联惯性导航系统东向、北向、天向的速度误差；

δL,δλ,δh分别表示捷联惯性导航系统的纬度误差、经度误差、高度误差；

ε_rx,ε_ry,ε_rz分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的加速度计零偏；

SINS/GPS子系统选择位置和速度作为量测量，即

式中，υ_INSE,υ_INSN,υ_INSU,L_INS,λ_INS,h_INS分别为惯导解算的东向速度、北向速度、天向速度、纬度、经度、高度；

υ_GPSE,υ_GPSN,υ_GPSU,L_GPS,λ_GPS,h_GPS分别为GPS测得的东向速度、北向速度、天向速度、纬度、经度、高度；

H₁(t)为量测矩阵，

V₁(t)为GPS接收机在导航坐标系下的位置和速度误差噪声；

SINS/DVL子系统选择速度作为量测量，即

式中，υ_DVLE,υ_DVLN,υ_DVLU分别为多普勒测得的东向速度、北向速度、天向速度；

H₂(t)为量测矩阵，

H₂＝|0_3×3 diag|1 1 1|0_3×9|；

V₂(t)为DVL在导航坐标系下的速度误差噪声；

分别进行离散化处理得到：

式中，F_i,k|k-1,H_ik,W_i,k-1,V_ik分别为第i个子滤波器的状态转移阵，量测阵，系统噪声阵，量测噪声阵。

步骤2：构建故障检测函数进行检测；

定义故障检测函数

其中：e_k为残差，即E[e_k]＝μ,E{[e_k-μ][e_k-μ]^T}＝Σ_k；

γ_k是服从自由度为m的χ²分布，即γ_k～χ²(m)，m为量测z_k的维数；

故障判定准则为：

γ_k＞T_D，出现故障，

γ_k≤T_D，未出现故障；

其中，T_D为门限阈值，T_D＝f(χ²,P_FA/m)，P_FA为误警率。

步骤3：构建自适应强跟踪滤波算法进行状态估计；

自适应强跟踪滤波算法主要过程包括求解渐消因子、时间更新、测量更新：使用开窗法估计残差序列协方差，并自适应调整量测噪声阵，得到改进的渐消因子；利用时间更新求出状态的一步预测用渐消因子对状态一步预测误差协方差P_k|k-1进行调整；利用测量更新求出状态的估计值/>和方差的估计值P_k；具体过程如下:

①求解自适应渐消因子λ_i,k：

计算残差：

残差协方差的估计值：

残差协方差的理论值：

式中，Tr(·)表示对矩阵求迹；z_i,k为真实测量值，R_i,k为量测噪声方差阵；N为采样数或窗长度；ξ为弱化因子，一般ξ≥1；

②时间更新：

状态一步预测方程：

一步预测误差协方差方程：

③测量更新：

滤波增益方程：

状态估值计算方程：

估计误差协方差方程:P_i,k＝[I-K_i,kH_i,k]P_i,k|k-1；

式中，λ_i,k为渐消因子，λ_i,k＞1时，可以自适应调整一步预测误差协方差P_i,k|k-1，为强跟踪滤波；当λ_i,k≤1时，为标准KF滤波。

步骤4：将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值，

融合方式为：

式中，为全局状态估计，P_g为全局状态估计协方差阵；β_i为加权系数，

如果检测出子滤波器出现故障，融合过程中将故障子系统数据进行隔离，保证融合结果的准确性。

步骤5：将全局状态估计值反馈给无故障子系统，

信息分配原则为：

对于存在故障的子系统j，需要重置子系统信息，此时β_j＝1；

步骤6：用获得的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过多传感器融合滤波系统，得到误差的估计值，然后再反馈至惯性导航系统，用惯性导航系统输出的导航参数减去全局误差的估计值，就得到最终导航参数的估计值。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (5)

1.一种基于自适应联邦强跟踪滤波的组合导航容错方法，其特征在于，包括如下步骤：

步骤1：以SINS为参考系统，选择状态量和观测量，建立组合导航系统状态模型，以及SINS/GPS和SINS/DVL子系统量测方程；

步骤2：构建故障检测函数进行检测；

步骤3：构建自适应强跟踪滤波算法进行状态估计；

步骤4：将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值；

步骤5：将全局状态估计值反馈给无故障子系统，对于存在故障的子系统，需要重置子系统信息；

步骤6：用获得的全局状态估计值校正捷联惯性导航系统SINS输出的导航参数；

所述步骤2具体如下：

定义故障检测函数

其中：e_k为残差，即E[e_k]＝μ,E{[e_k-μ][e_k-μ]^T}＝Σ_k；

γ_k是服从自由度为m的χ²分布，即γ_k～χ²(m)，m为量测z_k的维数；

故障判定准则为：

γ_k＞T_D，出现故障，

γ_k≤T_D，未出现故障；

其中，T_D为门限阈值，T_D＝f(χ²,P_FA/m)，P_FA为误警率；

所述步骤3具体如下：

自适应强跟踪滤波算法主要过程包括求解渐消因子、时间更新、测量更新：使用开窗法估计残差序列协方差，并自适应调整量测噪声阵，得到改进的渐消因子；利用时间更新求出状态的一步预测用渐消因子对状态一步预测误差协方差P_k|k-1进行调整；利用测量更新求出状态的估计值/>和方差的估计值P_k；具体过程如下：

①求解自适应渐消因子λ_i,k：

计算残差：

残差协方差的估计值：

残差协方差的理论值：

式中，Tr(·)表示对矩阵求迹；z_i,k为真实测量值，R_i,k为量测噪声方差阵；N为采样数或窗长度；ξ为弱化因子，ξ≥1；

②时间更新：

状态一步预测方程：

一步预测误差协方差方程：

③测量更新：

滤波增益方程：

状态估计值计算方程：

估计误差协方差方程：P_i,k＝[I-K_i,kH_i,k]P_i,k|k-1；

式中，λ_i,k为渐消因子，λ_i,k＞1时，自适应调整一步预测误差协方差P_i,k|k-1，为强跟踪滤波；当λ_i,k≤1时，为标准KF滤波。

2.根据权利要求1所述的一种基于自适应联邦强跟踪滤波的组合导航容错方法，其特征在于，所述步骤1具体如下：

建立组合导航系统状态模型，如下式：

式中，X(t)为系统状态向量，F(t)为状态转移矩阵，W(t)为系统白噪声；

φ_E,φ_N,φ_U分别表示捷联惯性导航系统导航坐标系内东向、北向、天向的失准角；

δv_E,δv_N,δv_U分别表示捷联惯性导航系统东向、北向、天向的速度误差；

δL,δλ,δh分别表示捷联惯性导航系统的纬度误差、经度误差、高度误差；

ε_rx,ε_ry,ε_rz分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

分别表示捷联惯性导航系统载体坐标系内X、Y、Z三个方向的加速度计零偏；

SINS/GPS子系统选择位置和速度作为量测量，即

式中，υ_INSE,υ_INSN,υ_INSU,L_INS,λ_INS,h_INS分别为惯导解算的东向速度、北向速度、天向速度、纬度、经度、高度；

υ_GPSE,υ_GPSN,υ_GPSU,L_GPS,λ_GPS,h_GPS分别为GPS测得的东向速度、北向速度、天向速度、纬度、经度、高度；

H₁(t)为量测矩阵，

V₁(t)为GPS接收机在导航坐标系下的位置和速度误差噪声；

SINS/DVL子系统选择速度作为量测量，即

式中，υ_DVLE,υ_DVLN,υ_DVLU分别为多普勒测得的东向速度、北向速度、天向速度；

H₂(t)为量测矩阵，

H₂＝|0_3×3 diag|1 1 1|0_3×9|；

V₂(t)为DVL在导航坐标系下的速度误差噪声；

分别进行离散化处理得到：

式中，F_i,k|k-1,H_ik,W_i,k-1,V_ik分别为第i个子滤波器的状态转移阵，量测阵，系统噪声阵，量测噪声阵。

3.根据权利要求1所述的一种基于自适应联邦强跟踪滤波的组合导航容错方法，其特征在于，所述步骤4具体如下：

将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合，得到全局状态估计值，

融合方式为：

式中，为全局状态估计，P_g为全局状态估计协方差阵；β_i为加权系数，

如果检测出子滤波器出现故障，融合过程中将故障子系统数据进行隔离，保证融合结果的准确性。

4.根据权利要求1所述的一种基于自适应联邦强跟踪滤波的组合导航容错方法，其特征在于，所述步骤5具体如下：

将全局状态估计值反馈给无故障子系统，信息分配原则为：

对于存在故障的子系统j，需要重置子系统信息，此时β_j＝1。

5.根据权利要求1所述的一种基于自适应联邦强跟踪滤波的组合导航容错方法，其特征在于，所述步骤6具体如下：将全局状态估计值反馈至惯性导航系统，用惯性导航系统输出的导航参数减去全局状态估计值，得到最终导航参数的估计值。