一种多传感器组合导航系统的自适应分布式滤波算法

2023-11-28孙炜玮潘新龙乔玉新韩真真

大地测量与地球动力学 2023年12期

王玮孙炜玮潘新龙乔玉新韩真真

1 烟台南山学院智能科学与工程学院,山东省龙口市大学路12号,265713 2 海军航空大学,山东省烟台市二马路188号,264001 3 山东东海热电有限公司,山东省龙口市东海工业园区,265713

集中式卡尔曼滤波器是实现多传感器组合导航系统的最优估计方法,但是由于其状态维数高,存在计算负担重、容错性差、通信负担重等缺点。随着计算机技术的发展,计算负担重的困难得到缓解,但对容错和估计精度的要求却越来越高[1]。基于分布式滤波方法,联邦滤波器可增强系统的容错性与可靠性[2],且设计灵活,便于工程实现,但其精度低于集中式卡尔曼滤波器[3]。为解决系统模型误差导致的滤波精度下降的问题,扩展卡尔曼滤波、无迹卡尔曼滤波和容积卡尔曼滤波等非线性滤波相继被提出[4],然而相对常规的线性卡尔曼滤波,这些方法存在工程实现难的缺陷。为克服量测噪声统计特性不准确导致的滤波精度下降的问题,自适应卡尔曼滤波技术不断发展,主要包括基于新息的自适应估计方法(IAE)和贝叶斯方法[5]。变分贝叶斯方法是对实际过程中实现难度较大的贝叶斯方法的改进,其核心思想是对贝叶斯密度分布函数进行近似,可以实现量测噪声统计特性的次优估计。

本文在集中式卡尔曼滤波的基础上推导了分布式卡尔曼滤波的一种表达式方法,该方法不仅保留了集中式卡尔曼滤波技术的最优性,而且具有故障识别与自动隔离、系统自动重组的特点。以此为基础,结合变分贝叶斯估计方法提出一种组合导航系统的自适应分布式滤波算法。

1 自适应分布式滤波模型

1.1 集中式卡尔曼滤波

综合考虑主导航系统及各子导航系统,设多传感器组合导航系统的状态方程为:

Xk+1=Φk+1,kXk+ΓkWk

(1)

各子导航系统的量测方程可表示为:

(2)

故集中式卡尔曼滤波器量测方程为:

Zk+1=Hk+1Xk+1+Vk+1

(3)

且

其中,

(4)

集中式卡尔曼滤波器的状态估计方程为:

(5)

(6)

(7)

(8)

(9)

其中,式(8)中Pk+1|k+1也可写为:

Pk+1|k+1=[I-Kk+1Hk+1]Pk+1|k

(10)

综合以上,得到多传感器系统的集中状态估计表达式为:

(11)

1.2 分布式滤波器的一种表达形式

根据式(1)和式(2),与第i个子导航传感器对应的卡尔曼滤波方程为:

(12)

(13)

(14)

(15)

(16)

展开式(5)右边,合并后有:

(17)

由式(10)可得:

(18)

从式(12)可推出:

(19)

假定所有出现的矩阵的逆矩阵都是存在的,并且P0为非奇异的。由式(7)可得:

(20)

(21)

类似于式(18),有:

(22)

将式(22)代入式(21):

(23)

将式(18)、(20)、(23)代入式(17):

(24)

以上分析说明,式(24)的分布模型不仅具有集中卡尔曼滤波器的最优特性,同时具有联邦卡尔曼滤波器的较好的容错特性。

1.3 变分贝叶斯原理

当量测噪声方差未知、系统噪声方差已知,包含量测噪声的最优贝叶斯分预测与更新2步[7-8]。

p(Xk-1,Rk-1|Z1:k-1)dXk-1dRk-1

(25)

p(Xk,Rk|Z1:k)∝

p(Zk|Xk,Rk)p(Xk,Rk|Z1:k-1)

(26)

虽然上述贝叶斯滤波是最优的,但后验密度函数在实际中很难求解。变分贝叶斯原理是一种近似方法,对难以求解的后验分布利用多个已知分布进行近似。由变分贝叶斯原理,式(25)和式(26)可转换为如下的高斯分布和逆Gamma分布:

(27)

(28)

式中,m为量测方差的维数。

基于上述分析,第i个子导航传感器对应的自适应卡尔曼滤波的预测过程为:

(29)

量测更新过程为:

(30)

式中,·/为点除运算。

基于式(29)、(30),利用式(24)可完成多传感器组合导航系统的自适应分布式滤波。从式(29)、(30)看出,量测噪声方差的估计与状态估计相似,也包括预测和更新2个过程,避免了IAE方法中的滑动窗和信息的存储,并可对协方差阵内独立变化的噪声进行单独调整。综上,本文提出的自适应分布式组合导航系统方案见图1。

图1 自适应分布式组合导航系统方案Fig.1 Adaptive distributed integrated navigation system scheme

2 仿真与分析

2.1 仿真算例模型

以SINS/GNSS/CNS/ADS多传感器组合导航系统为例进行仿真与分析。选取式(1)中的系统状态向量为X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,∇rx,∇ry,∇rz]T,其中,φE、φN、φU分别为平台东、北、天向的误差角;δvE、δvN、δvU分别为东、北、天向的速度误差;δL、δλ、δh分别为纬度、经度、高度误差;εbx、εby、εbz与εrx、εry、εrz分别为陀螺沿x、y、z轴的常值漂移误差和一阶Markov漂移误差;∇rx、∇ry、∇rz分别为加速度计沿x、y、z轴的一阶Markov漂移误差。

SINS/CNS子系统的量测方程采用姿态组合(转换为地理系下的横滚角、俯仰角、航向角)的模式,SINS/GNSS量测方程采用三维位置及三维速度的组合模式,SINS/ADS量测方程采用高度的组合模式以增强对高度通道的精确测量。

2.2 仿真条件设置

SINS/GNSS/CNS/ADS导航系统的仿真参数设置见表1。表中,CNS、GNSS和ADS的测量误差均为正常情况下的测量误差,选取仿真时间为3 600 s,SINS的输出速率为100 Hz,GNSS、CNS和ADS的输出速率为1 Hz,量测误差均为高斯噪声。

表1 仿真参数设置

本文仅考虑量测噪声均方差异常的2种极端情况,即突变和缓变。为此,在GNSS子导航系统的各导航参数中分别加入量测噪声均方差阶跃变化和斜坡变化,而假设CNS及ADS的量测噪声无异常,见表2,其中,t代表仿真时间。

表2 GNSS量测误差均方差变化设置

与GNSS对应的子滤波器中,ρ、α、β的初始值选取为:ρ=[0.990.990.990.990.990.99]、α=[111111]、β=[2220.10.10.1]。在设定包含起飞、爬升、俯仰、加速、转弯等各种机动飞行轨迹的基础上,针对表2中GNSS量测噪声均方差异常的2种情况,对算法性能进行仿真分析。

2.3 仿真结果分析

分别对联邦卡尔曼滤波器(FKF)与本文所提的自适应分布式滤波(ADKF)算法进行仿真实验。

其中,FKF方法采用基于残差卡方检测的、具有容错功能的、带有重置式的信息融合模式。

图2为当GNSS量测噪声均方差发生突变时2种方法的仿真结果。可以看出,当GNSS量测噪声均方差突变时,FKF算法的滤波精度非常差,主要原因是其相关子滤波器在处理GNSS时得到的故障检测结果大于门限值,此时认为与GNSS相连的子滤波器发生故障进而进行隔离。因为组合导航系统引入了ADS,增加了高度通道的可观测性,故其高度与天向速度的精度较好。