基于粒子滤波衰减因子抑制干扰的组合导航信息融合算法

单永志，尹健，刘少坤，孙立伟，许河川

(1.北京理工大学 机电工程学院，北京100081;2.哈尔滨建成集团有限公司，黑龙江 哈尔滨150030;3.空军装备研究院 总体所，北京100076;4.驻624 厂军事代表室，黑龙江 哈尔滨150030)

0 引言

组合导航模型一般都采用非线性模型。常用的滤波算法有扩展卡尔曼滤波(EKF)，Unscented 卡尔曼滤波(UKF)等算法［1］。但这些算法一般适用于观测噪声和过程噪声为独立或者相关的高斯白噪声。在实际应用中一般对过程和初始条件有着较高的要求，而且对已经发散的通道数据的抑制能力不足［2-4］。如果对已发散通道的数据不能有效修正，就会影响全系统的数据解算。文中首先介绍了粒子滤波的基本思想，并且在贝叶斯判据应用的条件下对算法模型做一定修改补充，旨在抑制组合导航系统单通道受干扰发散的数据。

1 粒子滤波和贝叶斯理论

1.1 贝叶斯理论

假定动态系统工作时可以表示为如下公式:

式中:xk为系统状态变量;zk为系统量测向量;ωk为系统随机噪声;vk为状态噪声。同时设定状态的初始概率密度函数为p(x0|z0)=p(x0)，所以系统的状态预测方程和状态更新方程分别为

这两个公式表示了最优贝叶斯估计的基本思想［6］。为了利用贝叶斯公式确定模型合适的解，必须计算其合适的后验分布统计，采用合适的算法模型使连续积分离散化是计算步骤的一个关键，粒子滤波就是基于这样一种近似数值解的方法。

1.2 粒子滤波

粒子滤波的关键思想是:要有一组有关联权值的随机样本去表示要求的后验概率密度，并且基于这样的一些样本和权值去计算估计值［7］。

对于非线性系统而言，粒子滤波一般采用如下计算步骤:

1)初始化:设k = 0 时，xi0∝p(x0)i = 1，2，…N，从p(xk| xk-1，yk)中抽取N 个样本。

2)逐点计算对应的p(xk| xk-1)和p(yk| xk).

3)利用公式:

计算对应的样本初值，并对其进行归一化，即:

4)计算新的粒子集:

5)输出结果:

其中L(xk+1)为滤波误差的方差阵。

6)k 变成k+1，返回步骤3).

2 滤波算法

在非线性系统中，当系统受到干扰时，考虑以下离散模型表示的非线性系统:

式中:xk∈Rn是系统状态变量;uk∈Rn为系统输入向量;zk∈Rn是观测向量;e 为观测噪声。

对于非高斯噪声下的非线性系统而言，采用EKF 算法时残差加权平方和不再服从χ2分布，为此，采用负对数似然检测办法。

假设在计算权值的过程中，得到各个粒子在j-1时刻的一步转移为预测为则可以得到由粒子表示的似然函数为

联合密度为

则负对数似然函数表示为

所以数据检测算法如下:

式中ε 为数据误差允许发生的阀值大小。对于每一步而言，在使用贝叶斯粒子滤波计算数据得到ωik后，都要计算l(θ)大小，从而判别数据是否符合系统误差要求。

1)当l(θ)≥ε 时，说明数据处于干扰状态。设xi∝q(x)，i=1，2，…N，是容易由合适的q(·)产生的样本，q(·)称为重要性密度，因此是第i 个粒子的归一化权值。

重要性函数可以分解为

将(13)式带入(4)式中计算，修正权值公式为

先验的重要性函数选择为

后验滤波密度有如下近似:

可以得到ωik∝ωik-1p(zk|xik).此时在粒子滤波的第3 步和第4 步计算中采用如上方法。观测值发现有严重干扰时，通过改变权值大小的办法消除误差，集中精力在有大权值的粒子上，然后再重采样，保证系统数据使用。

2)l(θ)＜ε 时，说明数据未受干扰，这时采用正常方法，在重要性重采样中由粒子(xik，ωik)重采样。得到N 个等权值的粒子(xik，1/N)，设定重采样后的粒子权值为1/N.

3 仿真计算

对于飞行器的GPS/INS 组合导航系统采用改进的粒子滤波方法进行导航计算。设飞行器初始状态为:北、东、天3 向速度初始偏差均为0，速度测量误差为0.01 m/s，加速度计初始偏差为:东向初始误差为1.0 ×10-4m/s2，北向和天向初始误差均为0。随机偏差为1.0 × 10-5，刻度误差为0.000 1，经度λ 的初始误差量为1'，纬度L 的初始误差量为1'，高度初始误差为100 m.飞行器组合导航系统开始工作时的初始位置为:北纬30°，东经120°，高度为5 000 m;飞行器处于初始状态时其初始速度表示为:北向速度和天向速度为0，东向速度为300 m/s.εL= ελ= 0.01'，εh= 10 m，ενN=ενE=ενD=0.5 m/s.

则在滤波解算中，系统状态方程的初始矩阵数据P0、Q 和R 分别为P0=diag{(1')2(1')210020 0 0}，Q = diag{0 0 0 (1.0 × 10-5)2(1.0 × 10-5)2(1.0 × 10-5)2}，R = diag {(0.01)2(0.01)2(0.01)2}.

在飞行器工作过程中，对组合导航系统进行干扰，使卫星导航系统提供的数据进行单通道发散，这时引入改进粒子滤波算法并将其应用在组合导航系统中。利用其他通道的正常信息消除因单通道数据发散而引起的误差。

在组合导航系统中，当卫星GPS 信号正常时，对卫星的动态模型采用常速模型。状态转移矩阵Φk，k-1和干扰矩阵Γk-1定义如下:

图1 加速度方向误差对比Fig.1 Comparison of the acceleration errors

图2 速度方向误差对比Fig.2 Comparison of the speed errors

从仿真结果来看，在系统的单一通道发生误差发散后，系统将粒子滤波中的衰减因子应用在改进的粒子滤波算法和贝叶斯判断中，可以通过减小经度通道干扰数据的权值使得重要性权值分布变得倾斜而抑制系统单个通道发散干扰而带来的影响。这样可以利用粒子滤波的衰减性抑制系统的误差发散并使系统误差在允许的范围内进行调节，恢复允许误差的工作状态。缺点是当粒子滤波向其他通道进行权值重新分配时，有时衰减因子会向正常通道内部倾斜，与正常情况相比，其他稳定通道相对应的误差值会有所增加。但是，只要把误差控制在系统误差允许范围内，还是可以保证系统正常工作的。

图3 位置方向误差对比Fig.3 Comparison of the position errors

4 结论

针对导航系统中单通道数据受干扰发散的情况，本文使用了负对数似然方法判断数据是否受到干扰。当数据正常时，引入UKF 重采样解决粒子衰退现象;当数据受到干扰时，提出了基于贝叶斯理论的粒子滤波算法，利用粒子滤波中的“退化”现象消除了干扰数据带来的影响。仿真结果表明，当单一通道受到干扰而严重发散时，该算法可有效抑制干扰，提高精度，从而使得系统仍能保持较好的工作状态。文中的研究结果为复杂噪声下组合导航系统单通道干扰的抑制提供了一种可行的解决方法。

References)

［1］ Sheen J Bishop R.Adaptive nonlinear control of spaceraft［J］.The Journal of the Astronautical Sciences，1994，42(4):451 -472.

［2］ 潘泉，杨峰，叶亮，等.一类非线性滤波器——UKF 综述［J］.控制与决策，2005，20(5):481 -489.PAN Quan，YANG Feng，YE Liang，et al.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481 -489.(in Chinese)

［3］ Julier S，Unlmann J.Unscented filtering and nonlinear estimation［J］.Proceedings of the IEEE，2004，92(3):401 -422.

［4］ 贾英红.航天器姿态与能量一体化控制研究［D］.哈尔滨:哈尔滨工业大学，2004.JIA Ying-hong.Research on the spacecraft attitude and energy integration control［D］.Harbin:Harbin Institute of Technology，2004.(in Chinese)

［5］ Rudolph V M，Arnaud D.The unscented particle filter CU ED/FInfeng/Tr380［R］.UK:Cambridge University Engineering Department，2001.

［6］ 张磊，李行善，于劲松，等.一种基于高斯混合模型粒子滤波的故障预测算法［J］.航空学报，2009，30(2):319 -324.ZHANG Lei，LI Xing-shan，YU Jin-song，et al.A fault prognostic algorithm based on gaussian mixture model particle filter［J］.Acta Aeronautica et Astronautica Sinica，2009，30(2):319 -324.(in Chinese)

［7］ 胡士强，敬忠良.粒子滤波原理及其应用［M］.北京:科学出版社，2010.HU Shi-qiang，JING Zhong-liang.Principle and application of particle filtering［M］.Beijing:Science Press，2010.(in Chinese)