基于联邦滤波的SINS/GNSS/OD/高度计多源组合导航算法研究

顾 涛，陈 帅，谭聚豪，王 琮，陈安升

(1.南京理工大学自动化学院，南京 210094；2.北京自动化控制设备研究所，北京 100074)

0 引言

目前，比较典型的组合导航系统有惯性/卫星、惯性/合成孔径雷达(Synthetic Aperture Radar，SAR)图像匹配组合导航系统，以及惯性/天文(Celestial Navigation System，CNS)组合导航系统等，但是这些单一模式的组合导航系统并不能提供高精度的位置和速度信息，也不能满足导航系统长时间稳定可靠运行的要求。惯性/卫星组合导航系统凭借卫星导航系统对速度和位置的精确测量可以获得高精度的位置和速度信息，但卫星信号容易受到电磁干扰或遮挡，导致卫星信号失锁，在军事应用中其精度和可靠性难以得到保证[1-2]；惯性/天文组合导航系统利用天体测量信息和惯性测量信息获取高精度导航参数，具有自主性强和姿态精度高的特点，但天文导航系统也会受到可观测星光这些自然环境的影响[3-4]。

基于以上，单一导航系统辅助惯性导航系统的组合模式已经无法满足导航系统高精度和高可靠性的需求。根据导航系统的环境特点和工作特性，以技术成熟和具有独特优势的惯性/卫星组合导航系统为基础，融合具有不同工作特点的导航传感器的输出信息组成基于多源信息融合的组合导航系统，从而提高了车载导航系统的精度和可靠性。传统联邦滤波算法中，子滤波器的信息因子分配系数采用有重置的系数平均分配法，即βm=0,βi=1/N的分配方式进行联邦滤波[5]，这种方式把每个导航子传感器的噪声特性默认为一样的。然而，系统中的传感器噪声特性各不相同，此时仍采用均匀分配会导致系统状态估计量的不准确，从而降低导航精度。文献[6]中提出了一种基于协方差矩阵迹的信息因子分配方式，由于需要经过状态转移矩阵运算后，反馈给下一时刻的协方差阵，这种方法在容错性上会存在滞后。

本文主要采用基于联邦滤波捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)/全球导航卫星系统(Global Navigation Satellite System，GNSS)/里程计(Odometry，OD)/高度计的多源联邦滤波组合导航模型，如图1所示。并设计了一种基于预测残差的自适应信息因子分配方法，使得信息因子分配系数以导航传感器的精度为基础进行实时地自适应分配，从而有效提高了联邦滤波精度。为了满足地面车载情况下的长时间复杂环境需要，设计了一种改进的χ2故障检测法，与传统χ2故障检测法相比，该算法能更快地识别检测出传感器缓变故障，在传感器出现故障时能够更迅速地隔离故障的子滤波器，并将其对组合导航系统的影响降至最低，提高了组合导航系统的精度和可靠性。

图1 SINS/GNSS/OD/高度计组合导航联邦滤波模型

1 SINS/GNSS/OD/高度计组合导航滤波器结构设计

联邦滤波器是一种如图1所示的两级滤波结构滤波器，公共参考子系统SINS的输出Xk一方面给主滤波器，另一方面给各子滤波器作为量测值[7]。而各子系统的输出只给各自的滤波器。将各子滤波器的局部估计值Xi及其协方差阵Pi送入主滤波器，和主滤波器的估计值一起进行融合得到全局估计值。

系统信息在主、子滤波器间的分配方式是基于信息分配原则的[9]，即满足

(1)

其中，βm为主滤波器信息分配系数；βi为第i个子滤波器对应的信息分配系数。

系统信息在子滤波器中的具体分配方法如下

(2)

主滤波器最优信息融合为

(3)

2 多源导航系统数学模型

2.1 联邦子滤波器基本模型

惯导的误差模型在文献[10]中有详细介绍，本文不再赘述，误差模型具体如下：

在本文的联邦滤波组合导航系统中，SINS作为公共参考系统，系统误差状态方程取

XSINS=[φEφNφUδVEδVNδVUδL

δλδhεbxεbyεbzεrxεryεrz

(4)

1)联邦滤波子滤波器状态方程为

(5)

其中，XI(t)同上文XSINS，为捷联惯导系统误差状态；误差系数矩阵GI(t)和状态系数矩阵FI(t)在文献[10]中均有定义，不再赘述。

2)联邦滤波子滤波器量测方程为

ZI(t)=HI(t)X(t)+VI(t)

(6)

其中，ZI为量测误差矩阵;HI和VI根据不同的组合选取有所不同。

2.2 联邦滤波子滤波器数学模型

2.2.1 SINS/GNSS松组合滤波子系统

SINS/GNSS滤波子系统采用速度、位置松组合的方式，以惯导和北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)两者的速度之差和位置之差作为子系统观测量，则SINS/BDS子系统的量测方程为

(7)

式中，vIE、vIN、vIU为惯性导航系统的东、北、天方向的速度误差，vGE、vGN、vGU为BDS的东、北、天方向的速度误差；LI、λI、hI为惯性导航系统的纬度、经度、高度，LG、λG、hG为BDS的纬度、经度、高度；VG为量测噪声。

2.2.2 SINS/OD组合滤波子系统

OD是测量车辆行驶路程的仪器，采用脉冲计数的方式测量采样时间间隔内载体的路程增量，可以提供载体的前向速度信息，脉冲数经刻度因子转换为速度测量值[12]。

本文中，以惯导与OD的组合方式选取速度组合，惯导和OD两者的速度之差作为子系统观测量，则SINS/OD子系统的量测方程为

(8)

式中，vIE、vIN、vIU为惯性导航系统的东、北、天方向的速度误差，vOE、vON、vOU为OD的东、北、天方向的速度误差；VO为量测噪声。

2.2.3 SINS/高度计组合滤波子系统

气压高度表是以气压敏感元件作为传感器感受大气压力，再根据气压与高度的函数关系确定高度值的间接测量仪器，可以测量载体的绝对高度以及标准气压高度[13]。

以惯导和高度计的组合方式选取高度组合，惯导与高度计之间的高度之差作为子系统量测值，则SINS/高度计的量测方程为

ZA(t)=hI-hA=HA(t)X(t)+VA(t)

(9)

式中，hI为惯性导航系统的高度，hA为气压高度计的高度；VA为量测噪声。

3 自适应分配联邦滤波

传统联邦滤波采用各子滤波器信息分配等分的原则进行分配，本文设计了一种基于预测残差的联邦滤波自适应分配方法，通过实时调整分配因子，提高了系统的滤波精度和性能。

Kalman滤波是否收敛可以依据状态误差的协方差阵P的对角线元素进行判断，其对角线元素与状态误差量中的相应元素相对应。如果在经过一次滤波后其相应状态误差协方差阵P中对应元素项小于设定的阈值，则认为滤波收敛。

可以基于子滤波器的残差对传感器相应的子滤波器精度进行判定。当精度高时，Kalman滤波器的残差rk是零均值白噪声；当传感器精度变低时，残差的均值就不再为0。因此对残差的检验可以确定各导航传感器的精度。

系统残差可以表示为

(10)

系统残差的协方差理论值为

(11)

系统残差的协方差理论值可以表示为

(12)

因此，通过式(11)和式(12)构建自适应分配因子

(13)

式中，c的范围为0.8～1，本文中取1，其中

(14)

根据式(14)可推导得到

(15)

为了确保信息分配因子遵循信息守恒原理，需将上述分配因子进行归一化。

4 故障检测隔离算法实现

在进行多源信息融合的过程中，需要对传感器的状态进行检测和判断，在传感器故障时进行实时隔离，防止其进入主滤波器中污染滤波结果，以保证滤波器正常工作，因此需要多源融合故障检测算法来提高系统的精度和稳定性。

本文设计了一种改进的χ2残差检测法，提出了在残差特征值的基础上对检测阈值进行模糊逻辑和加权处理，具体方法如下：

定义残差向量

(16)

从当前时刻往前选取n个残差向量构成残差矩阵

(17)

定义残差方差矩阵

H=RTR

(18)

矩阵H的特征值最大值有界，即

λmax≤S

(19)

选取N个矩阵的最大特征值以提高数据的准确性，具体为

(20)

此时，故障判断准则为

为了减少计算量，n和N的取值不宜过大，本文中设为n=2,N=3。

式中，λ是服从自由度为m的χ2分布，即λ～χ2(m) (m根据滤波器量测方程的维数进行确定)；TD为预先设置的门限值，这样就可以根据χ2分布表、信息的置信度以及实际滤波计算情况设置滤波发散的门限值。如此一来，既可以保证判断的灵敏度，又可以防止出现误判的现象，有效地保证了滤波的可靠性和系统的安全性。

当某一个子滤波器检测出故障时，系统舍弃该滤波器的信息，主滤波器的估计值舍弃故障滤波器的信息而使用其他子滤波器的值进行最优估计。

当系统加入故障检测算法后，联邦滤波自适应分配系统形成如图2所示的两级检测系统进行工作。第一级为子滤波器将滤波结果送入故障检测算法进行判断，对故障的子滤波器进行隔离；第二级为判断为无故障的滤波器送入主滤波器进行自适应信息因子分配。

图2 联邦滤波自适应分配两级结构

5 实验与分析

为了验证本文设计的基于联邦滤波的SINS/GNSS/OD/高度计多源组合导航性能，进行了车载多源融合实验。

5.1 实验条件

本文使用的导航系统基于自研BDS，SINS采用MSI3200微惯性测量单元(Micro Inertial Measurement Unit，MIMU)，OD采用某车载里程计，气压高度计采用BMP280气压传感器。由于BDS目前收星情况不如全球定位系统(Global Positioning System，GPS)，本次实验将自研BDS作为联邦滤波中的GNSS传感器，而高精度UBLOX卫星导航系统作为基准进行参照。

实验地点为南京麒麟科技园周边，跑车路线选取直线，测试路线如图3所示。

图3 车载运行轨迹

本次实验分为2组，第1组在传感器无故障条件下进行车载实验，通过对采集数据进行仿真，验证信息因子自适应分配的精度；第2组在仿真中加入突变故障和缓变故障，用于验证改进的残差故障检测算法的性能。

5.2 实验结果与分析

第1组跑车实验的行驶路径如图3所示，将450s运动过程中所得到的误差情况分别列于图4～图9中，位置和速度的平均误差(1σ)如表2和表3所示；第2组实验中，在一段时间内加入突变故障与缓变故障，主要对GPS发生故障时进行仿真验证，故障具体设置如表1所示，算法的故障检测值如图10和图11所示。

图4 东向速度误差对比

图5 北向速度误差对比

图6 天向速度误差对比

图7 纬度误差对比

图8 经度误差对比

图9 高度误差对比

表1 故障设置表

表2 平均分配组合误差信息(1σ)

表3 自适应分配组合误差信息(1σ)

图10 传统残差χ2故障检测值

图11 改进残差χ2故障检测值

由表2～表3和图4～图9可知，由于在车载条件下，各导航传感器在不同时刻根据环境的变化精度有所不同，当加入基于预测残差的自适应分配后，位置和速度信息的精度明显得到提高，其中，位置精度较平均分配提升约1～2m，速度精度较平均分配提升约0.1m/s左右。

由表1和图10～图11可知,在加入突变故障后，传统和改进残差χ2故障检测法均能迅速且准确地检测出故障;当加入缓变故障时，传统χ2残差检测法在缓变故障发生后延迟58s才检测出来，而改进χ2残差检测法则在故障发生后延迟30s检测出来，所以该算法较传统χ2残差检测法灵敏度得到提升，能够提前检测出缓变故障。

6 结论

本文主要提出了一种基于联邦滤波的SINS/GNSS/OD/高度计多源组合导航算法：

1)针对各导航传感器精度不同的问题，设计了一种基于预测残差的信息因子自适应分配算法，使得分配系数可以反映各传感器状态变量的特性；并对算法进行仿真实验，结果分析显示了该算法能够充分利用各子系统的工作特性，从而提高了多源组合导航系统的导航精度。

2)针对实际情况下导航传感器易出现突变故障和缓变故障的情况，设计了一种改进的残差故障检测算法；并通过分析验证，该算法较传统故障检测算法能够更快地识别缓变故障，从而提高了多源组合导航系统的容错性。