一种状态约束下的抗差自适应联邦滤波算法

冯抗洪 宋迎春 崔先强

1 中南大学地球科学与信息物理学院,长沙市麓山南路932号,410083

合理地利用先验约束信息可以提高单传感器系统和多传感器系统的导航定位服务精度及可靠性。目前,将约束信息纳入到卡尔曼滤波中来处理单传感器导航数据的研究较多[1],而在多传感器导航系统中加入先验状态约束信息的研究较少。相较于单传感器系统,多传感器系统的观测量明显增多,但不同传感器的定位原理不同,受干扰的情形也不同。如在由捷联惯导(strapdown inertial navigation system,SINS)、GNSS和里程计(odometer,ODO)组成的SINS/GNSS/ODO组合导航系统[2]中,GNSS信号易受高楼、树木等遮挡[3],车载体侧滑、滑行等现象易导致ODO的数据无效[4]。此外,即便在所有传感器都正常工作的情况下,仍有必要考虑约束信息,因为约束通常是一个硬性界限,是必须满足的条件。

本文基于对传统联邦卡尔曼滤波算法(federated Kalman filter,FKF)[5]的研究,将状态约束下的卡尔曼滤波扩展到联邦滤波中,提出状态约束下的联邦滤波(federated filtering with state constraints,CON-FKF)算法。另外,针对载体处于复杂环境时传感器可能出现故障或数据异常的情形,本文还对状态约束下的联邦滤波进行了改进。首先,在各个子滤波器中加入Huber抗差方法[6],以抵制观测异常对导航精度的影响;其次,在信息分配阶段加入基于马氏距离的自适应信息分配因子[7-8],得到一种状态约束下的自适应抗差联邦滤波算法(a robust adaptive federated filtering algorithm with state constraints,CON-RAFKF),以进一步减小子滤波器的不准确估计对全局估计的影响;最后,基于SINS/GNSS/ODO组合导航系统,利用仿真实验验证本文算法的有效性。

1 SINS/GNSS/ODO组合导航数学模型

1.1 状态方程

在多传感器组合导航中,基于SINS误差方程建立状态方程,其线性状态方程可表示为[9]:

Xk=ΦkXk-1+wk

(1)

式中,Xk=[δψ,δv,δp,bω,bf]T为15维状态误差向量,δψ、δv、δp分别为姿态误差、速度误差和位置误差,bω、bf分别为陀螺零偏和加速度计零偏,可被建模为随机常数[10];Φk为系统状态转移矩阵;wk为状态过程噪声向量,其协方差矩阵为Σwk。

1.2 观测方程

1)GNSS/SINS 观测方程。GNSS/SINS的观测值为GNSS输出的位置与SINS计算的位置之差,观测方程为[10]:

Z1k=PGNSS-PSINS=H1kXk+τ1k

(2)

式中,Z1k为k时刻滤波位置观测值;PGNSS为GNSS位置观测值;PSINS为SINS位置计算值;H1k为观测矩阵;τ1k为观测噪声,满足高斯白噪声特性,其协方差矩阵为R1k。

2)ODO/SINS观测方程。ODO/SINS的观测值为ODO输出的速度与SINS计算的速度之差,观测方程为[2]:

Z2k=VODO-VSINS=H2kXk+τ2k

(3)

式中,Z2k为k时刻滤波速度观测值;VODO为ODO速度观测值;VSINS为SINS速度计算值;H2k为观测矩阵;τ2k为观测噪声,满足高斯白噪声特性,其协方差矩阵为R2k。

2 CON-FKF算法

CON-FKF算法具有2层结构。在第1层中,SINS分别与GNSS和ODO组合,并通过并行的方式获得状态约束下SINS/GNSS和SINS/ODO的局部状态估计值;在第2层中,将各个子滤波器的状态估计进行融合,获得SINS/GNSS/ODO组合导航系统的全局状态估计。算法结构如图1所示。

图1 CON-FKF算法结构

2.1 信息融合和信息分配过程

(4)

(5)

式中,r=2为子滤波器的个数。

(6)

(7)

Σwik=1/βiΣwgk

(8)

(9)

式中,Σwgk、Σwik分别为主滤波器和第i个子滤波器的状态噪声协方差矩阵。

2.2 子滤波器状态估计

在SINS/GNSS/ODO组合导航系统中,2个子滤波器的状态方程与主滤波器的状态方程均如式(1)所示,而第i个子滤波器的观测方程可以统一表达为:

Zik=HikXk+τik

(10)

式中,Zik为第i个子滤波器的观测量;Hik为观测矩阵;τik为第i个子滤波器的观测噪声,其协方差矩阵为Σik,i=1,2。则各子滤波器的时间更新和观测更新为:

(11)

(12)

(13)

(14)

(15)

(16)

(17)

由于本文以大地坐标系位置误差[δL,δλ,δh]作为滤波的状态量,而式(16)为导航坐标系(ENU)下的位置约束,因此还需进行误差扰动和转换。对于第i个传感器,可得如下约束方程:

(18)

式中,Cik=[0,(RN+H)cos(L),0;RM+H,0,0;0,0,1],RM和RN分别为沿地球子午圈和卯酉圈的主曲率半径,H为当地高程,L为当地纬度。

为便于推导,将式(18)简化为:

DikXik=dik

(19)

(20)

通过最优化中的Lagrange乘子法求解式(20):

(21)

由式(19)可知DikXik-dik=0,则:

(22)

(23)

求出各子滤波器在状态约束下的状态估计值及其协方差矩阵后,按照式(4)和式(5)融合,即可得到状态约束下联邦滤波的全局状态估计值及其协方差矩阵。

3 CON-RAFKF算法

当载体处于一些复杂环境时,传感器可能会出现观测数据缺失或因受到外界干扰使观测量出现异常的情况。为进一步提高导航系统的定位精度和可靠性,对上节提出的CON-FKF算法进行改进。首先,在各个子滤波器中加入Huber抗差方法,以抵制观测异常对导航精度的影响;其次,在信息分配阶段加入基于马氏距离的自适应信息分配因子,以进一步减小子滤波器的不准确估计对全局估计的影响[8]。

3.1 Huber抗差方法

Huber抗差方法通过结合l1、l2两种范数来构建代价函数,其抗差性优于基于l2范数的估计方法。Huber代价函数表达式为[6]:

(24)

Huber权值函数表达式如下[11]:

(25)

式中,φ(vj)由ρ(vj)求导所得。

(26)

利用上式对观测噪声矩阵Σik进行修正,以提高系统的抗差性,修正公式为:

(27)

3.2 计算信息分配因子

在传统的联邦滤波中,信息分配因子被设置为固定值,当子滤波器出现异常情况时,其错误的估计信息会被带入到主滤波器中,导致系统的容错性降低。因此,自适应确定信息分配因子对提高系统性能具有重要意义[10,12]。本文选取第i个子滤波的状态估计和融合后的状态估计的马氏距离作为评价子滤波器滤波效果的指标,并以此来计算信息分配因子。具体公式为:

(28)

(29)

4 仿真实验及分析

初始误差协方差矩阵和过程噪声矩阵设置为:

ΣX0=diag[(1′)2,(1′)2,(1.5′)2,

(0.2 m/s)2I3×3,(10 m)2,(10 m)2,(20 m)2

0.5(°)/h)2I3×3,(2 000μg)2I3×3]

(30)

(31)

观测噪声设置为:

R1k=diag[(5 m)2,(5 m)2,(10 m)2]

(32)

R2k=diag[(0.1 m/s)2,

(0.1 m/s)2,(0.1 m/s)2]

(33)

4.1 测试1

测试1主要分析加入约束条件对FKF状态估计的影响。由图2和3可知,加入约束条件后,状态估计精度有所提高。为了防止随机性,对该系统进行50次蒙特卡洛仿真,计算50次仿真的RMSE,得到图4和5。可以看出,加入约束条件后,水平方向位置的平均RMSE分别下降12.46%、34.27%,速度的平均RMSE分别下降10.87%、34.80%;高程方向CON-FKF与FKF相比,位置和速度的RMSE变化都很小,这是由于本文采用的约束信息仅针对水平方向,在高程方向没有约束信息。

图2 SINS/GNSS/ODO组合导航位置误差

图4 FKF和CON-FKF位置RMSE

图5 FKF和CON-FKF速度RMSE

4.2 测试2

测试2主要是分析本文算法在SINS/GNSS/ODO组合导航系统出现异常时的性能(图6和7)。在200～250 s,模拟ODO观测异常,将10σv加入到ODO观测的速度上,σv=[0.1 m/s,0.1 m/s,0.1 m/s];在400～450 s,模拟GNSS观测异常,将10σp加入GNSS观测的位置上,σp=[5 m,5 m,10 m]。

图6 添加误差后 SINS/GNSS/ODO组合导航位置误差

图7 添加误差后 SINS/GNSS/ODO组合导航速度误差

由图6和7可知,FKF无法处理观测情况,会把误差信息带入到最终融合解中,致使导航系统的准确性下降;CON-FKF由于加入了先验约束信息,在一定程度上抑制了观测异常;CON-RAFKF在CON-FKF的基础上进一步加入了Huber抗差方法和自适应信息分配因子,与前2种算法相比,能够更好地应对观测异常的情况。

同样,对观测异常情况下的导航系统进行50次蒙特卡洛仿真,其RMSE结果见图8和9,平均RMSE统计结果见表1。可以看出,与FKF相比,CON-RAFKF三方向的位置平均RMSE分别下降53.84%、68.12%、43.37%,速度平均RMSE分别下降40.05%、62.25%、39.21%;与CON-FKF相比,CON-RAFKF三方向的位置平均RMSE分别下降40.58%、38.60%、43.30%,速度平均RMSE分别下降26.11%、44.75%、39.07%,可见CON-RAFKF显著提高了系统的定位精度和可靠性。

表1 3种算法的平均均方根误差

图8 添加误差后3种算法的位置均方根误差对比

图9 添加误差后3种算法的速度均方根误差对比

5 结 语

针对多传感器导航系统中状态量之间存在先验约束信息的情形,本文提出一种状态约束的联邦卡尔曼滤波算法。此外,当观测量出现异常时,本文将Huber抗差方法及自适应信息分配策略加入到状态约束下的联邦卡尔曼滤波中。理论分析和仿真实验表明,在联邦卡尔曼滤波中,考虑先验约束条件能够有效提高组合导航系统的定位精度;在带有约束条件的联邦卡尔曼滤波中,加入Huber抗差方法及自适应信息分配策略能够进一步提高复杂环境下组合导航系统的定位精度和可靠性。