基于联邦滤波的多源融合导航算法

谭聚豪，陈安升，张博雅，陈 帅，温哲君

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

0 引言

现有的导航系统信息源种类多样，单一的导航源有GPS、BDS、GLONASS、INS、雷达测速仪、地磁匹配、图像视觉导航以及天文导航等。然而往往在复杂多变的环境下，单一的导航系统可能会失效，其导航精度将不再可靠。多源融合组合导航系统基于信息融合技术，将来自不同导航源的同构或异构导航信息按照相应的融合算法进行融合，从而得到最佳的融合结果。相对于单一导航源可能会在某一时间或地点失效，多源融合导航可以充分利用每一个导航源的优势，提高导航解算的纠错能力，增加观测的冗余度，从而提供较好的定位与导航服务。

捷联惯导系统（Strapdown Inertial Navigation System，SINS）是一种不向外部辐射能量、也不依靠外界信息的独立式导航系统，可以稳定地输出导航所需要的信息，缺点是其导航精度随时间而逐渐变差。因此，SINS常与其他导航源进行组合，从而提高长时间导航的精度和可靠性［1-2］。

我国北斗三号系统基本建成，其导航范围已经覆盖全球，其全球定位精度为10m，测速精度为0.2m/s，授时精度为20ns，高精度的导航性能可以为全球提供准确的定位、导航和授时服务；Doppler测速仪是测量载体速度的仪器，其基本原理是利用Doppler频移和速度的关系，具有测速范围较宽、测量精度较高的优点；里程计（ODO）固定在车辆传动轴上，在车辆行驶过程中，产生固定数量的脉冲，通过脉冲计数可以得到车辆行驶的里程数，进而得到车辆行驶的前向速度；气压高度计则根据气压和海拔的变化关系测得载体的高度信息。

目前，已有许多学者对不同信息源以及多种信息源的组合导航进行了研究。文献［3］提出了一种鲁棒优化的因子图算法应用在多传感器融合中；文献［4］采用了一种基于因子图的增量平滑算法在惯性导航中进行多传感器的信息融合；文献［5］提出了一种SINS/GPS/BD2/DVL组合导航联邦滤波算法，并将其应用到了船舶航行中；文献［6］提出了一种SINS/GPS/CNS组合导航联邦滤波算法，针对组合系统模型线性化、易导致滤波发散等问题，进行了研究验证；文献［7］将粒子滤波引入联邦滤波结构中，得到了一种混合联邦—粒子滤波算法，且以INS/BD/LoranC组合导航系统为对象进行了仿真；文献［8］根据组合导航Kalman滤波器中新息信息与误差的关系，利用新息信息的协方差阵对联邦滤波器的信息分配系数进行了构造；文献［9］提出了一种自适应联邦滤波在PPP/INS紧组合中的应用，证明了联邦滤波和自适应滤波的有机结合，且两者可等同使用。

本文主要采用SINS/BDS/Doppler测速仪/里程计/气压高度计的多源组合导航模型，针对多源融合非线性的特点，采用联邦滤波融合算法。算法中主滤波器不进行滤波，负责把子滤波器的估计值进行最优融合。此外，在子滤波器中引入残差故障检测理论，同时结合自适应滤波的理论，对子滤波器中的新息协方差阵进行判断。接着，利用检测结果进行联邦滤波器信息分配系数的构造，进而形成自适应容错联邦滤波器，实现多信息源的最优融合。最后，生成时空基准统一、连续、可靠的PNT服务信息。

1 联邦滤波组合导航系统模型

1.1 联邦滤波器设计方案

本文所设计的联邦滤波器中，以SINS作为公共的信息参考系统，使用了5个信息源，所以设置了4个子滤波器和1个主滤波器。每个子滤波器处理自己的量测信息得到局部估计，再在主滤波器中进行最优估计。此外，本文选用有重置的联邦滤波器，且采用了一种自适应联邦信息分配因子，在出现某一子信息源出现故障时，能够对故障很好地进行隔离，不影响滤波器的正常滤波，使系统保持较好的稳定性和鲁棒性。图1为联邦滤波器结构图。

1.2 联邦滤波器状态方程

惯性导航系统作为公共的信息参考系统，其误差模型选用通用的误差模型，并采用东北天地理坐标系作为导航坐标系。系统的公共状态变量为

图1 联邦滤波器结构图Fig.1 Structure diagram of federal filter

式（1）中，φE、φN、φU为东北天三个方向上的姿态误差角，δVE、δVN、δVU为东北天三个方向上的速度误差，δL、δλ、δh为纬度误差、经度误差、高度误差，分别为载体系下陀螺、加速度计三个轴上的随机漂移和常值偏置，则误差状态方程可以表示为

式（2）中，FSINS为SINS系统的状态转移矩阵，GSINS为SINS系统的噪声驱动矩阵，WSINS为陀螺和加速度计三个轴上均值为零、方差为QSINS的白噪声，各矩阵的具体计算形式详见文献［10］。

1.3 联邦滤波器量测方程

本文设计的联邦滤波器主滤波器无信息分配，不用主滤波器进行滤波，主滤波器只对每个子滤波器的局部估计值进行融合进而得到全局估计，下面介绍各子滤波器的量测方程。

（1）SINS/BDS的量测方程

SINS位置信息表示为

SINS的速度信息表示为

BDS的位置信息表示为

BDS的速度信息表示为

则SINS/BDS的量测方程为

式（3）～式（6）中，λRef、LRef、hRef为经度、纬度和高度的基准参考值，vRef_E、vRef_N、vRef_U是载体各轴向的速度基准参考值，PE、PN、PU为BDS东北天三个方向上的位置误差，BE、BN、BU为BDS东北天三个方向上的速度误差。式（7）中，H1和V1的具体形式如下

量测噪声V1看作是均值为零的白噪声。

（2）SINS/Doppler测速仪的量测方程

Doppler测速仪的速度信息表示为

则SINS/Doppler测速仪的量测方程为

式（11）中，DE、DN、DU为Doppler测速仪东北天三个方向上的速度误差，H2、V2的具体形式如下

量测噪声V2看作是均值为零的白噪声。

（3）SINS/ODO的量测方程

ODO 输出载体系的速度信息表示为

将ODO输出的载体速度分解到导航坐标系，并与SINS系统的速度作差，则得观测方程为

量测噪声V3看作是均值为零的白噪声。

（4）SINS/气压高度计的量测方程

气压高度计的高度信息表示为

式（17）中，AU为高度计的测量误差，可得SINS/气压高度计的量测方程为

2 联邦滤波器设计

2.1 联邦滤波器融合方法

在本文设计的联邦滤波器中，每个子滤波器的状态变量相同（SINS公共状态变量），所以不按照传统的方法在子滤波器中进行单独的时间更新以及噪声协方差阵Qf和误差状态协方差阵Pf的信息分配，而在主滤波器中进行噪声协方差阵Qf和误差状态协方差阵Pf的时间更新。子滤波器中只完成量测更新，且利用方差上界技术和信息分配方法在不改变子滤波状态估计情况下保证各子滤波器状态估计是独立的、不相关的，这样子滤波器和主滤波器均为最优估计。将上述设计的子滤波器的状态方程和量测方程进行离散化，可以得到

系统信息在主滤波器和子滤波器间的信息分配方法是遵守信息分配守恒原则的，即满足

式（23）中，βm为主滤波器信息分配系数，βi为第i个子滤波器对应的信息分配系数。

由于本文设计的联邦滤波器采用了主滤波器无信息分配，仅对子滤波器估计进行全局融合并对子滤波器进行反馈，因此信息分配又可表示为

系统信息在子滤波器中的具体分配方法，也即子滤波器中进行的量测更新方法可用下面的式子表示

式（25）中，为主滤波器的状态估计，Pf、Qf分别为主滤波器的状态误差协方差阵和系统噪声协方差阵，为子滤波器的状态估计，Pi、Qi分别为子滤波器状态估计对应的状态误差协方差阵和子滤波器系统噪声协方差阵。

由式（21）可以得到子滤波器协方差阵的时间更新过程，为

由式（27）可知，子滤波器的状态误差协方差阵Pi，k-1和噪声协方差阵Qi，k-1扩大为γi，k-1Pf，k-1和γi，k-1Qk-1后，其预测协方差阵也得到了扩大。

子滤波器的预测和滤波估计方程为

子滤波器的滤波增益为

子滤波器的估计均方差为

在子滤波器中完成了局部估计后，主滤波器根据每个子滤波器的估计值进行全局最优估计，主滤波器的最优融合方法如下

式（32）中，为系统误差状态量的全局最优估计结果，Pf为估计状态量的协方差阵。

2.2 时空基准统一方法

针对多传感器输出频率不同、数据通信延时等情况，由于惯性系统具有短时精度高的特点，采用递推法将对应时刻惯性数据的量测值变化量补偿到低频率传感器上作为融合时刻的真实值，这样就把其他传感器的滞后数据递推到了融合时刻。

空间基准统一就是将不同坐标系下的测量信息经过旋转变换统一到基准坐标系下，基准坐标系为导航坐标系。

此外，子滤波器的滤波周期以输出频率最低的传感器为准，主滤波器周期大于等于子滤波器周期。

2.3 联邦滤波器自适应信息分配方法

本文设计的联邦滤波器采用了两级滤波结构，为了得到全局最优估计，采用了对子滤波器产生的局部估计量进行信息分配。在Kalman滤波中，新息单位权重误差的计算无需利用验后信息，没有信息分配的滞后性，同时新息单位权重误差与验后残差平方和等价［10］。因此，新息单位权重误差可以表示为

式（33）中，ε＝Zk-Hkk/k-1为各子滤波器的量测残差，H为量测矩阵，Pk｜k-1为预测方差，Rk为观测值方差。当无故障发生时，残差ε的方差Pk为均值为零的噪声；当有故障发生时，其均值就不再为零了。这样就可以自适应对故障进行判断和隔离，提高系统的抗干扰能力。

在每次滤波前进行计算，令分配系数βi＝σ0。并且可以利用自适应滤波的原理对联邦滤波的分配系数进行确定［8］，同时结合自适应滤波的权函数和残差χ2故障检测理论对自适应分配因子进行调整，故可得

式（34）中，c1和c2两个阈值的确定，文献［11］是依靠经验值，而本文在确定时采用了残差χ2故障检测理论［12］，c1取值为0.9，c2取值为7.8。

3 实验与分析

为了对本文设计的联邦滤波器的性能进行验证，进行了基于车载运行轨迹的半实物仿真实验和车载多源融合实验。

3.1 多源融合仿真实验

在仿真实验中，将某组车载运行轨迹注入到轨迹发生器中，并根据轨迹参数对传感器的精度做限制，且模拟设置某个传感器的故障状态，再进行融合，验证算法的容错性和可靠性。

仿真中，设置BDS系统的定位误差在10m以内，测速误差在2m/s以内；Doppler测速仪的测量误差在0.5m/s；里程计的测速误差在0.8m/s以内；高度计的测量误差在5m内。SINS作为公共信息系统，仿真中采用3个子传感器与之进行不同的组合，具体仿真结果如图2～图7。

图2 纬度误差Fig.2 Errors of latitude

图3 经度误差Fig.3 Errors of longitude

图4 高度误差Fig.4 Errors of height

图5 东向速度误差Fig.5 Errors of eastward speed

图6 北向速度误差Fig.6 Errors of northbound speed

图7 天向速度误差Fig.7 Errors of sky speed

从位置误差和速度误差的曲线可以看出，不同传感器故障下的融合误差均逐渐变小且趋于平缓，水平误差稳定在1.5m 以内，高度误差稳定在1m 以内，水平速度误差稳定在 0.3m/s 左右，天向速度误差稳定在0.15m/s 左右，算法可以实现故障隔离后仍稳定、可靠地工作。

3.2 车载多源融合实验

车载实验在空旷的郊区进行，实验中采用本文前面所述的传感信息源进行融合，来验证本文算法框架的可行性和合理性。实验中以某高精度组合导航产品为基准，具体实验结果如图8 ～图16所示。

图8 车载运行轨迹Fig.8 Running track of vehicle

图9 位置误差Fig.9 Errors of position

图10 速度误差Fig.10 Errors of speed

图11 纬度对比Fig.11 Comparison of latitude

图12 经度对比Fig.12 Comparison of longitude

图13 高度对比Fig.13 Comparison of height

图14 东向速度对比Fig.14 Comparison of eastward speed

图15 北向速度对比Fig.15 Comparison of northbound speed

图16 天向速度对比Fig.16 Comparison of sky speed

从上述曲线可以看出，水平、垂直位置误差均在3m左右，且逐渐变小趋于平稳；水平速度误差在0.2m/s左右，趋于平稳；天向速度误差在0.15m/s以内，趋于平稳。整个过程的位置和速度曲线与基准相比，均能实现平稳、良好跟踪，表明车载实验下算法也能够可靠的工作。

3.3 实验结果与分析

在仿真实验中，不同传感器故障时联邦滤波器均可以有效进行隔离，自适应的融合其他子滤波器的估计值进行最优融合。定位效果较好的是“BDS+ Doppler测速仪+高度计”的组合，其纬度误差为0.7962m（1σ），经度误差为0.9495m（1σ），高度误差为1.4188m（1σ）。此外，不同传感器的组合根据其测量特性会提高载体相应测量数据的精度。比如高度计参与组合的方式中，其高度误差均较小，最小为1.4188m（1σ）。在车载实验中，各个传感器可以正常融合实现可靠定位，整个过程中的位置和速度与基准相比误差较小，其纬度误差为1.7828m（1σ），经度误差为1.5656m（1σ），高度误差为1.5026m（1σ），具体实验结果对比如表1所示。

表1 实验结果对比Table 1 Comparison of experiment results

对上述实验结果分析可知，车载实验和仿真实验的定位误差均在1.8m以内，定位精度较好。整个实验表明，本算法可以实现多传感器可靠的融合定位，且在复杂变化的环境中实现了传感器故障的有效隔离，提高了测量精度，输出了可靠的定位导航信息。

4 结论

本文提出了一种多源融合的联邦滤波组合导航算法，搭建了联邦滤波的多源融合模型，详细设计了各子滤波器和主滤波器，且对信息分配因子进行了自适应分配以及将各子滤波器的估计结果进行最优融合，并通过实验验证了多源融合的联邦滤波算法的可行性。算法能够进行不同导航传感器的自由融合且能够取得较好的定位结果，具有不错的研究意义和实际价值。