反馈粒子滤波在GPS_INS组合导航系统中的应用_秦岭.pdf

第 34 卷第 4 期 2015 年 12 月 武 Journal 汉 轻 工 大 学 学 报 of Wuhan Polytechnic University Vol． 34No． 4 Dec． 2015 文章编号： 2095-7386（ 2015） 04-0056-04 DOI： 10． 3969 / j． issn． 2095-7386． 2015． 04． 014 反馈粒子滤波在 GPS / INS 组合导航系统中的应用 秦 岭，刘晨曦 （ 武汉轻工大学 电气与电子工程学院，湖北 武汉 430023） 摘 要： GPS / INS 组合导航系统的性能主要是由滤波器的精度和实时性决定，由于组合导航 系统的状态维数较高，导致滤波器的计算量大，因而制约了粒子滤波与高斯粒子滤波算法在实 际工程中的应用。针对惯性组合导航系统的特点，基于反馈粒子滤波算法，对位置 / 速度误差 进行了滤波估计，同时利用组合滤波器的输出对 INS 系统的误差进行修正，最后对相应的算法 进行了仿真验证。仿真结果表明，与混合高斯粒子滤波算法相比，反馈粒子滤波算法的滤波精 度和实时性都有一定的提高。 关键词： 组合导航系统； 反馈粒子滤波； 高斯粒子滤波 中图分类号： U 666． 1 文献标识码： A Usage of Feedback Particle Filter algorithm in GPS / INS integrated navigation system QIN Ling，LIU Chen-Xi （ School of Electrical and Electronic Engineering，Wuhan Polytechnic University，Wuhan 430023，China） Abstract： The performance of GPS / INS integrated navigation system will directly be determinedby the precision and real-time performance of the filters． The dimension of the integrated navigation system is so high that the filter needs the large amount of computation． The result is that the application of particle filter and the Gaussian particle filter algorithms is restricted in practice． According to the characteristics of integrated navigation system，the errors of position and velocity are estimated based on feedback particle filter． And the use of the integration method allows the integration navigation system correcting the errors of INS． At last the filter algorithm is verified by simulation． The simulation results show that the precision and real-time performance of feedback particle filter can be improved， compared with those of Gaussian particle hybrid filter． Key words： integrated navigation system； Feedback Particle Filter； Gaussian Particle Filter 1 周期性校正，有效的减少了系统误差的影响，提高了 引言 导航系统的整体性能。目前，常用的信息融合算法 针对 GPS 和 INS 的 是 Kalman 滤波算法［2］，在理想情况下，Kalman 滤波 误差特性，采用 GPS 的位置与速度信息对 INS 进行 效果较好，但是当载体作大幅度机动或者 GPS 信号 ［1］ GPS / INS 组合导航系统 收稿日期： 2015-08-14． 修回日期： 2015-11-27． 作者简介： 秦岭（ 1976-） ，男，副教授，E-mail： popularqin@ 163． com． 4期 57 秦岭，刘晨曦： 反馈粒子滤波在 GPS / INS 组合导航系统中的应用 受到干扰 或 遮 挡 时，Kalman 滤 波 会 产 生 较 大 的 误 其中系统动态矩阵 F（ t） 和过程误差系数矩阵 G（ t） 差，进而导致滤波失败。基于 Monte Carlo 采样方法 的非零元素详见文献［8］。 的粒子 滤 波 （ Particle Filter，PF） 和 高 斯 粒 子 滤 波 将 INS 的输出位置信息与 GPS 接收机计算出 （ Gaussian Particle Filter，GPF） 算法可以有效的解决 的相应位置信息进行相减，其差值作为组合导航系 ［3-6］ ，然而组合导航系统的状态变量维数 上述问题 高，为了提高粒子的采样效率，保证滤波收敛及其精 统的量测信息，同时将量测噪声看为高斯白噪声，则 组合导航系统的量测方程为： 度，必须采用更多数量的粒子来逼近概率密度函数， 这就导致计算量急剧增加，制约了粒子滤波和高斯 粒子滤波算法在组合导航系统中的应用。为此，文 献［4］和［6］采用 Kalman 滤波分别结合粒子滤波与 Z（ t） = H（ t） X（ t） + v（ t） ． 其中量测系数矩阵 H（ t） = [ 0 3 ×6 I3 ×3 0 3 ×10 ] 。 3 高斯粒子 滤 波 算 法，对 组 合 系 统 的 线 性 部 分 采 用 Kalman 滤波进行递推估计，而对系统非线性部分则 分别利用粒子滤波与高斯粒子滤波估计，有效的减 少了滤波的计算量，加快了滤波的收敛速度。反馈 粒子滤波（ Feedback Particle Filter，FPF） ［7］ 是一种 新型的免重采样非线性滤波算法，它将每个粒子都 反馈粒子滤波 假设系统状态方程和量测方程分别为式（ 1） 和 式（ 2） ，且估计状态的后验概率密度函数服从高斯 分布，即 p t ～ N（ x； μ t ，P t ） ，于是反馈增益矩阵 K t 可 以由 Euler-Lagrange 方程解得 视为一个能控的随机系统，通过求解 Euler-Lagrange  1  （ ｛ p K ｝ ） = HＲ t－1 ． x p t x t t 并且满足以下边界条件 方程，得到在 K-L 距离最小意义下的最优反馈控制 lim p t K t = 0 ， 律，进而基于每个粒子的自身状态进行校正和更新。 由于反馈粒子滤波不需要重采样，因此有效的避免 了因估计状态维数的增加而导致的计算量增大，同 － 由此可以解得 K t = P t H T Ｒ t－1 。则反馈粒子滤波的 最优控制由下式给出： dx it = Fx it dt + Gdw t + 应用。 P t H T Ｒ t－1 （ dZ t － HF 位置信息融合进行仿真研究，利用 GPS 观测信息通 过反馈粒子滤波算法修正 INS 的位置误差信息，以 x it + μ t dt） ． 2 （ 4） 其中 { x it }Ni= 1 表示 t 时刻样本粒子集，{ w t } 表示与状 态相互独立的维纳过程噪声。则后验概率密度函数 提高导航精度。最后通过算例仿真表明，与混合高 的条件均值和条件方差满足以下状态方程 斯粒子滤波相比，基于反馈粒子滤波算法的 GPS / d T －1 dZ t － Hμ t ） ． μ t = Fμ t + P t H Ｒ t （ dt dt INS 组合导航系统精度较高，实时性较好，可以满足 （ 5） d P = P t F + F T P t + GQ t G T － P t H T Ｒ t－1 HP t ． dt t 实际的导航要求。 2 （ 3） x→! 时反馈粒子滤波实现简单，实时性好，易于工程实际 本文主要对松耦合 GPS / INS 组合导航系统的 （ 2） GPS / INS 松耦合滤波模型 （ 6） 取地理坐标系为导航坐标系，则惯性导航系统 状态变量定义为： X（ t） = ［φ e φ n φ u δv e δv n δv u δL δλ δh 在算法具体实现时，可以采用样本粒子的均值 和方差对其进行估计。即 － μt ≈ μt = T 1 x it ． ∑ N i （ 7） ε bx ε by ε bz ε rx ε ry ε rz b x b y b z δt］ ． 矩阵各元素分别代表坐标系中 INS 在各方向的平台 Pt ≈ Pt = 角度误差、速度误差、位置误差、陀螺仪漂移、一阶马 此时，对第 i 个粒子的迭代方程 （ 4 ） 可以改写 尔可夫过程以及加速度计零漂等，共计 18 维行向 量。同时，由于采用 GPS 的量测信息作为 INS 的修 正信息，因此将 GPS 的接收机钟差信息扩充到组合 导航系统的状态向量。进而组合导航系统的误差系 统状态方程可以表示为： · X （ t） = F（ t） X（ t） + G（ t） w（ t） ． － 1 （ x it － μ t ） （ x it － μ t ） T ． （ 8） ∑ N －1 i 为： dx it = Fx it dt + Gdw t + 珔t H T Ｒ t－1 （ dZ t － HF P x it + 珔 μt dt） ． 2 （ 9） 由此，可以给出基于反馈粒子滤波的组合导航 （ 1） 滤波算法步骤如下： 58 武 汉 轻 工 大 学 学 报 2015 年 Step1． 在初始时刻 t = 0 ，假设初始状态的先 图可以看出，GPF 和 FPF 算法的位置误差曲线均能 验概率密度服从高斯分布为 p（ x， 0） ～ N（ u0 ，P0 ） ， 收敛，其中 FPF 滤波后各个方向的位置误差均减小 对其进行采样得到初始粒子样本集 { x i }N 0 i =1 ； 到约 20 m 以内，滤波精度明显均优于 GPF 滤波后 Step2． 假设每次迭代采样时间间隔为 Δ t ，则在 度误差都是发散的，但是 FPF 滤波后水平方向的速 从 t → t + Δ t 时，计算一步预测均值，即 N ^ ht = 1 （ HFx it ） ． N∑ i =1 （ 10） Step3． 对每一个样本粒子分别按下列式子计算 新息误差和反馈增益； i 度误差均减小至 1 m / s 以内，滤波精度也较 GPF 更 高。同时，表 1 给出了两种算法在不同情况下的运 行时间，由此看出，FPF 的实时性也优于 GPF，表明 FPF 在粒子数目较少的情况下，不仅保证了滤波精 ^ ΔI t = ΔZ t － 各个方向的位置误差。两种算法对于垂直方向的速 （ HFx it + h t ） ·Δ t ． 2 ^ K（ x，t） = P t H T Ｒ t－1 ． （ 11） 度，而且提高了导航系统的实时性。 （ 12） 其中样本粒子的均值和协方差可以分别按式（ 7） 和 式（ 8） 进行统计。 Step4 随机采样 Δ V ～ N（ 0， 1） ，然后对每一个 样本粒子按照下式进行校正更新。 x it +Δt = x it + Fx it Δ t + Δ V 槡GQ t G T ·Δ t + K（ x，t） Δ I it ． （ 13） 然后令 t = t + Δ t ，返回 Step2 进行下一次迭代， 更新各个粒子值。 4 算例与分析 取一组 GPS / INS 导航数据进行仿真实验。仿 真参数设置如下： 设惯性导航系统的陀螺随机常值 为 0． 1（ °） / h ，陀螺仪一阶马尔可夫过程驱动白噪 声为 0． 1（ °） / h ，相关时间为 500 s； 加速度计一阶马 尔柯夫过程驱动白噪声为 1 × 10 － 3 g，相关时间常数 1 000 s。GPS 接收机位置白噪声 30 m，速度白噪声 1 m / s，仿真时间为 50 s，采样时间为 Δt = 0． 1 s。 基于 Monte Carlo 方法的粒子滤波和高斯粒子 滤波算法的滤波精度是随着粒子数目的增加而增大 的，估计状态的维数越高，需要的粒子数目就越多， 滤波算法的计算量也就越大，而反馈粒子滤波算法 由于不需要在每次迭代时对粒子进行采样。只需要 计算粒子的反馈增益，因此有效的减少了算法的复 杂度，在保证滤波收敛的同时也提高了算法的精度。 因此，为了评价反馈粒子滤波算法的性能，在仿真中 对同一组测试数据分别利用高斯粒子混合滤波算法 和反馈粒子滤波算法进行组合导航系统的信息融 合，粒子数目分别选取为 N = 1 000 和 N = 10 000 两种情况进行比较。 图 1—2 给 出 了 1 000 个 粒 子 情 况 下，GPF 和 FPF 算法的位置和速度在各方向的误差曲线图。由 图1 位置误差曲线图（ N = 1 000 ） 图 3—4 给出了 10 000 个粒子情况下，GPF 和 FPF 的位置和速度在各方向的误差曲线图。由图可 以看出，对于 GPF，水平方向的位置误差虽然可以收 敛至 20 m 范围以内，但是波动大，精度较差，速度误 差也呈现发散趋势。而经过反 FPF 融合后，组合导 航系统的位置误差均能收敛在 20 m 以内，同时水平 方向的速度误差均能减小至 1 m / s 以内，与 GPF 相 比，曲线变化更为平缓。由此可见，在相同粒子数目 4期 59 秦岭，刘晨曦： 反馈粒子滤波在 GPS / INS 组合导航系统中的应用 图2 速度误差曲线图（ N = 1 000 ） 图4 速度误差曲线图（ N = 10 000 ） 的情况下，采用 FPF 实现 GPS / INS 组合导航系统的 位置和速度信息融合，其误差精度均优于采用 GPF 滤波算法。并且，当粒子数量减少后，FPF 滤波精度 虽然有所降低，但是提高了组合系统的实时性。 表1 FPF 和 GPF 运行时间比较 数目 算法 时间 / s 5 1000 FPF 10000 GPF FPF GPF 5． 945 999 18． 167 292 46． 891 639 178． 200 659 结论 本文基于反馈粒子滤波算法，对 GPS / INS 松组 合导航系统进行了信息融合。并将滤波结果进行对 比分析，仿真结果表明，该方法可以在粒子数目较少 的情况下，较为准确的估计惯性导航系统的误差信 息，并且随着粒子数目的增加，滤波精度会有所提 高，但是实时性降低。与高斯粒子滤波混合算法相 比，反馈粒子滤波不需要进行重采样，计算量小，易 （ 下转第 64 页） 图3 位置误差曲线图（ N = 10 000 ） 64 武 汉 轻 工 大 学 学 报 2015 年 表明，对于一个具体的符号序列进行编码，为了得到 ［3］ 邵军花，刘玉红，周东梅． 香农编码的优化算 最快的编码速度，只需要从大概率优先查找和折半 法研究 ［J］． 兰 州 交 通 大 学 学 报，2010，29 查找这两种算法中选择平均查找长度最短的编码算 （ 6） ： 110 － 113． 法即可． ［4］ 郭蕾，李东辉 ，缪志甫． 结合压缩感知理论的 参考文献： 快速分形编码［J］． 计算机工程与设计， 2012， ［1］ 叶芝慧，沈克勤． 信息论与编码［M］． 北京： 电 9， 33（ 9） ： 3494 － 3497． 子工业出版社 ， 2013． ［2］ 程佳佳，熊志斌． 哈夫曼算法在数据压缩中的 应用［J］科学技术与工程，2010， 2： 35 － 37． ［5］ 汪大勇，孙世新，杨浩淼． 一种基于残差系数 的快 速 编 码 算 法［J］． 电 子 与 信 息 学 报， 2010， 11， 32（ 11） ： 2541 － 2546．  （ 上接第 59 页） 于实现，在保证滤波精度的同时，也能有效的克服由 ［4］ 袁俊刚，范胜林，刘建业，等． 卡尔曼 / 粒子组 于组合导航系统维数过高而导致粒子数目增加滤波 合滤波 在 GPS / INS 组 合 导 航 中 的 应 用 研 究 计算量增大的缺点，更加适用于组合导航系统信息 09（ 4） ： 37-40． ［J］． 导航与控制，2010， 的实时融合。 ［5］ 周翟和，刘建业，赖际舟，等． 混合高斯粒子滤 参考文献： 波在组合导航中应用的计算量分析［J］． 中国 ［1］ Casper E S． INS and GPSintegration［D］． Lyn- 惯性技术学报，2010，18（ 5） ： 595-599． gby： Technical University of Denmark，2006： 60-72． ［2］ 鲍其莲，周媛嫒． 基于 UKF 的 GPS / SINS 伪距 （ 伪距率） 组合导航系统设计［J］． 中国惯性 技术学报，2008，16（ 1） ： 78-82． ［3］ 周 翟 和，刘 建 业，赖 际 舟，等． Ｒao-Blackwellized 粒子滤波在 SINS / GPS 深组合导航系 统中的应用研究［J］． 宇航学报，2009 （ 2） ： 515-520． ［6］ 熊剑，郭杭，熊智，等． GPS / INS 组合导航系统 中的高斯粒子滤波混和算法［J］． 中国惯性 技术学报， 2012， 20（ 2） ： 225-229． ［7］ Yang T，Mehta P G，Meyn S P． Feedback particle filter［J］． IEEE Trans． onAutomatic Control，2013，58（ 10） ： 2465-2480． ［8］ 王惠南． GPS 导航原理与应用［M］． 北京： 科 学出版社，2003．