组合导航数据融合自适应UKF算法研究

吴 涛，戴 卿，匡秀梅，王炎川

(1.中测新图(北京)遥感技术有限责任公司 航空遥感技术自然资源部重点实验室，北京 100039；2.邢台市勘察测绘院 测绘部，河北 邢台 054000)

0 引言

微惯性导航系统(MEMS-INS)与全球卫星导航系统(GNSS)近年来在导航与位置服务领域发展迅速，将两者结合的MEMS-INS/GNSS组合导航技术，优势互补、性价比高，并发展为航空、交通、农业、测绘等领域的应用热点[1]。由于导航系统方程的非线性，且在高动态环境下工作时，导航结果会受到噪声统计特征时变性的影响，因此，需要对时变噪声情况下的非线性导航系统在信息融合算法上提出更高的要求，这是现代测绘导航数据处理研究的热门话题[2-3]。

扩展卡尔曼滤波(Extended Kalman Filter, EKF)是测绘导航数据处理领域中被广泛采用的一种非线性滤波算法，但由于算法中使用函数模型近似来逼近非线性系统，会产生泰勒高阶项截断误差，并最终引起滤波精度下降甚至发散[4]。为克服EKF局部线性化和Jacobin矩阵计算繁琐的问题，无迹卡尔曼滤波(Unscented Kalman Filter , UKF)中借鉴sigma点对概率密度函数的逼近易于非线性函数本身的思路，构造了一种基于确定性采样的滤波算法，以二阶泰勒精度进行逼近，但当系统噪声统计特征不确定时，滤波精度则会受到显著影响[5]。抗差自适应UKF算法的出现，有效提升了噪声影响下的滤波鲁棒性，但自适应因子的选取受到经验性制约[6]。于此同时，为改善UKF算法数值稳定性方面的问题，平方根无迹卡尔曼滤波(Square Root Unscented Kalman Filter，SRUKF)的提出有效避免了舍入误差影响和矩阵求逆计算[7]。

为进一步改善SRUKF算法的稳定性和计算效率，本文在现有研究基础上，提出了一种基于最大后验概率估计(Maximum A Posteriori, MAP)的快捷UKF算法。新算法将奇异值分解(Singular Value Decomposition, SVD)引入传统平方根状态协方差计算，然后采用基于MAP的时变噪声估计器改善算法在时间更新和量测更新阶段的自适应能力，提高算法的鲁棒性，最后通过MEMS-INS/GNSS组合导航系统数据测试，结果表明，新算法在确保导航输出结果的同时，能有效降低计算成本，提高导航的实时性。

1 MEMS-INS/GNSS松散耦合导航模型

松散耦合导航是一种应用较广的低成本组合导航方式。松散耦合导航工作流程如图1所示，MEMS-INS模块和GNSS卫星导航模块相互独立工作，输出位置、速度和姿态信息后进行数据融合计算，并对MEMS-INS闭环修正，是一种GNSS辅助惯导的工作模式[8]。

图1 松散耦合导航工作流程

1.1 导航系统状态方程

MEMS-INS/GNSS组合导航系统中，建模状态方程为

1.2 导航系统量测方程

松散耦合导航系统中，惯性装置和GNSS卫星模块输出的位置和速度之差一并构成系统量测值，因此组合导航的量测方程表示如下

其中，

2 基于MAP噪声估计器的UKF算法

2.1 基本算法框架

设时间离散系统的非线性模型为

xk=Mk,k-1(xk-1)+uk(5)

yk=Hk(xk)+vk(6)

其中，Mk,k-1表示状态转移矩阵，Hk表示观测矩阵，均表现为一定的非线性特征，uk和vk分别表示状态方程和量测方程的噪声项，xk为k时刻状态量；yk为量测向量。

产生2lk-1+1个sigma点

进行时间更新：

(13)

进行量测更新：

(18)

其中，Γk表示阈值，并限制lk范围区间为ll≤lk≤lu。

时变噪声估计统计器是一种次优无偏MAP估计器，如文献[4-5]所述:

由上述推导发现，基于MAP噪声估计器的UKF算法通过在线实时估计状态方程和量测方程的噪声项进一步精化了噪声统计模型，可改善算法的估计性能，基于MAP噪声估计器的UKF算法流程如图2所示。

图2 基于MAP噪声估计器的UKF算法流程图

2.2 算法性能分析

3 导航数据测试分析

利用MATLAB编译MEMS-INS/GNSS组合导航系统仿真平台，仿真数据采用动态仿真加组合导航误差融合的方式获得，其中量测误差由MEMS-INS惯导单元和GNSS卫星接收机模块在实测数据中获得，具有统计不确定性和时变性。模拟GNSS测速误差 0.5 m/s，定位误差5 m，MEMS陀螺噪声均方根 0.5° /s，加速度计噪声均方根10-3g。GNSS数据输出1 Hz卫星可见显示数如图3所示。惯导装置数据输出100 Hz，横滚角量测输出、俯仰角量测输出、航偏角量测输出分别如图4、图5、图6所示。导航系统采用1 s周期的闭环反馈校正方式，工作时间共1800 s。

图3 卫星可见显示数

图4 横滚角量测输出

图5 俯仰角量测输出

图6 航偏角量测输出

粒子滤波算法(Particle Filter, PF)是在贝叶斯框架准则下推导的一种高性能滤波，虽然算法计算量较大易出现维度灾难，但在非线性非高斯系统中，仍是一种输出精度较高的滤波算法。因此为验证本文提出的新算法估计精度，将PF计算结果设为实验参考值。

方案1：标准PF算法

方案2：基于MAP的快捷UKF算法

方案3：SRUKF算法

由实验结果可以看出：

(1)平面位置输出误差如图7、图8所示，三种滤波方案均能基本满足组合导航定位计算，而方案3的精度略低于方案1和方案2，因为SRUKF相比PF和基于MAP的快捷UKF算法缺少对量测项的降噪处理。

图7 平面位置输出误差(E)

图8 平面位置输出误差(N)

(2)速度输出误差如图9、图10所示，姿态输出误差如图11、图12所示。方案1和方案2输出精度明显优于方案3。说明基于标准PF算法和MAP的快捷UKF算法对噪声统计特性不确定的动态导航系统具有一定的鲁棒性。

图9 速度输出误差(E)

图10 速度输出误差(N)

图11 姿态输出误差(X)

图12 姿态输出误差(Y)

(3)不同滤波方案的RMSE结果如表1所示。分析表1数据可知，方案2所用新算法稳定性较好，输出位置、速度与姿态误差较小，相比传统UKF算法在滤波精度上有所提升。新算法虽然相比方案1所用PF在精度上有所损失，但计算效率较高，计算时间上大大减小，方案3相比于方案2计算效率提升了16%，在组合导航系统数据融合实时性上有很大的提升。

表1 不同滤波方案的RMSE结果

4 结语

为消除UKF算法在组合导航数据融合中输出精度和计算效率之间的矛盾，本文提出了一种带时变噪声估计的快捷UKF算法。通过MAP准则对噪声时变统计特性进行实时跟踪估计，可有效提高滤波鲁棒性，同时引入SVD分解技术较好的缓解算法中sigma点的计算负担，有利于组合导航UKF算法的实时自适应计算。将新算法应用于MEMS-INS/GNSS测绘导航数据实验中，结果表明，该算法在受时变噪声影响的高机动导航环境下，能有效改善滤波输出精度，提高算法的计算效率，对低成本MEMS-INS/GNSS的工程应用具有一定参考价值。