抗差自适应无迹Kalman滤波在GPS/INS组合导航中的应用

张 群 洪志强

(1. 广东广量测绘信息技术有限公司, 广东 东莞 523000; 2. 南昌工学院 人工智能学院, 江西 南昌 330108)

0 引言

全球卫星系统(Global Navigation Satellite System,GNSS)已成为许多测量和精密导航应用中常用的导航和定位技术[1]。然而,在动态环境中,GNSS信号可能会由于外界环境干扰而在被部分测距信号中缺失,导致定位性能降低。与GNSS相比,惯性导航系统(Inertial Navigation System,INS)是一个完全独立自主的导航系统,从这个意义上讲,它不依赖于任何外部来源。但是,INS很难维持长时间的导航定位性能。因此,将GNSS与INS进行组合,可实现两者的优势互补。

实现GNSS/INS组合的一种重要方法便是标准Kalman滤波方法[2]。然而,顾及GNSS/INS组合模型中动态模型的非线性特点(以GNSS/INS松组合为例[3]),其动态模型本身是非线性的,采用标准Kalman滤波方法需要对动态模型进行一阶求导。针对上面的问题,现有文献采用无迹变换的思路处理动态预测模型[4-5],可避免对动态方程的求导工作,也有相关文献研究了采用该种模式对导航定位精度的提升效果[6];但是采用无迹变化的思想存在计算量随着状态量维度的增加而增大的问题。另一方面,在实际导航应用中,信号部分遮挡、多路径等因素导致卫星观测值可能出现较大观测粗差以及惯导器件参数发生等,导致系统预测异常,无论采用标准Kalman滤波方法还是无迹Kalman滤波方法均无法有效处理这些异常信息。常用的针对观测值异常的抗差Kalman滤波方法[7-8]与仅针对动态异常的自适应Kalman滤波方法[9]仅能够处理单一异常出现时的情况,能够同时兼顾两者的方法不多见。因此,针对上述两方面的问题,本文根据导航系统动态模型自身的特点,即可表达为与部分状态量非线性相关(姿态四元数)而与部分状态量(速度和位置)线性相关;因此,结合边际无迹变换的思想(Marginalised Unscented Transformation,MUT)[10],在采用无迹变换时,仅需对呈现非线性相关的状态量进行sigma的计算与传播即可,大大降低了计算量;针对存在观测粗差与系统异常预测的情况,本文采用文献[11]的思想,自动更新新息向量的协方差矩阵，达到兼顾抗差性与自适应性的目的。

1 GPS/INS松组合模型

1.1 动力学模型

动力学模型中，姿态更新可采用四元数微分方程[10]

(1)

(2)

(3)

(4)

离散时间形式下的速度更新方程可写为

(5)

n系下,惯导的位置更新可表示为如下离散方程的形式

(6)

其中,rk和rk-1分别为k和k-1历元惯导中心在n系下的位置矢量。

1.2 观测模型

(7)

2 一种兼顾抗差与自适应性无迹Kalman滤波方法

2.1 结合边际无迹转换与GPS/INS松组合模型

式(1)、(4)和(6)可写为矩阵形式

(8)

可见式(8)中等号右边项对于姿态四元数、角增量及其误差向量是非线性相关的,而相对于速度和位置是线性相关的。因此,可通过条件线性变换将上式改写为

(9)

(10)

(11)

2.2 抗差与自适应因子的引入与确定

采用无迹Kalman滤波方法处理组合系统的步骤如下:

通过式(9)预测下一历元状态预测值(预测量用“-”表示,验后量用“+”表示)

(12)

并计算新息向量

(13)

(14)

计算增益矩阵Kk:

(15)

则验后状态估值及其方差阵为

(16)

(17)

文献[11]指出,当系统存在异常观测值或状态方程异常预测时,可依据Mahalanobis距离的平方满足卡方分布来检验,即须判断式(17)、(18):

(17)

(18)

(19)

因此,放大因子的求解可分别通过牛顿法求解

(20)

(21)

2.3 方法实现

(1)获得初始位置、速度和姿态的参数值及其方程阵;

(2)顾及状态方程的条件非线性特性,围绕姿态参数的均值和方差采样sigma点(考虑到姿态四元素的维度不匹配问题,这里须将姿态四元素转换成罗德里格参数进行计算[10]);

(4)不引人放大因子,根据式(13)～(17)计算无迹Kalman滤波结果(结果1);

(7)将(4)～(6)中三种方法确定的验后参数估值作为基准,预测三者下一历元所对应的新息向量,并将三种预测新息所确定的Mahalanobis距离[式(17)]的最小值对应的验后状态估计作为该历元的最终估计值。

3 算例与分析

图1 车载实验轨迹

为了验证本文采用方法的有效性,采用如下三种方案进行试验:

方案1(仅含有观测粗差):在第100历元给卫星解算的位置模拟东北天(20 m,20 m,50 m)的观测粗差;

方案2(仅含有系统状态异常误差):从第200历元开始,给后续动态预测的东北天三个方向分量的速度值模拟加入0.01 m/s的误差及位置分量加入(3 m,3 m,5 m)的误差;

方案3(同时含有系统状态异常误差和观测粗差):同时按照方案1和方案2,分别在100历元模拟观测粗差，在200历元模拟动态模型粗差。

依据上述三种方案,分别采用标准UKF方法(UKF),抗差UKF方法(RUKF)、自适应UKF方法(AUKF)和本文的兼顾自适应与抗差性能的UKF滤波(本文ARUKF)进行对比分析。需要说明的是,针对GNSS/INS松组合模型中动态模型的非线性特点,上述各种UKF方法均可采用MUT思想简化动态预测的计算量,因此,本节实验验证主要探讨各方法在处理观测粗差或且动态模型粗差时的性能。

图2给出了在100 s时加入观测粗差后各方法的定位结果,从图2的结果不难发现,在仅含有观测误差时采用RUKF和本文ARUKF方法均能够有效地处理含有观测粗差历元给定位带来的影响,而UKF方法和AUKF方法在存在粗差时无法有效抵抗其影响,这是因为UKF方法本身没有针对粗差进行有效处理;而AUKF将观测粗差当成系统动态异常进行处理,其没有正确地对粗差进行归类,从而也没有得到连续可靠的定位结果。综上,方案1验证了处理存在观测粗差情况下本文ARUKF方法的有效性。

图2 在100 s时加入观测粗差情况下各方法的定位结果

图3为在200 s后系统存在状态异常误差下各方法的定位结果。在仅含有系统动态异常情况下,UKF方法和RUKF方法均未得到连续可靠的定位结果,其中RUKF的定位结果存在很大的发散;不难理解,此时RUKF方法将系统动态异常引起的新息向量异常归为由于观测异常引起的,从而错误地放大了观测向量协方差矩阵,最终导致了定位结果的严重发散。从图3还可看出,采用AUKF方法和本文的ARUKF均能够获得该情况下的连续可靠的定位结果,验证了本文ARUKF方法在处理存在系统动态模型异常预测时的有效性。

图3 在200 s后系统存在状态异常误差下各方法的定位结果

图4 同时在100 s时存在观测粗差和200 s后系统存在状态异常误差下各方法的定位结果

图4给出了同时存在观测异常和动态异常预测情况下各方法的定位结果。从图4不难发现,UKF方法、RUKF方法和AUKF方法均不能够为整个实验过程提供有效且连续的定位结果;而采用本文的ARUKF方法能够为全路段提供有效的定位服务,能够有效地对异常粗差进行归类,从而有针对性地对异常向量对应的协方差进行放大,从而减小其对最终定位结果的影响。

4 结束语

上述分析验证了本文提出的ARUKF方法在本文实验条件下是切实可行的,其能够处理观测时段非同时存在观测异常与动态异常的情形。但采用本文方法无法处理某一特定历元同时存在观测异常与动态异常的情况，这是因为新息向量无法同时对这两种异常分量进行归类,这也是目前相关方法的痛点,进一步研究可采用机器学习等方法辅助这两类异常信息进行分类,以期得到更完善有效的处理策略。