改进粒子滤波算法及其在GPS/SINS组合导航中的应用

王　林，林雪原，孙炜玮，王　萌

（1.海军航空工程学院信息融合研究所，山东烟台264001；2.92154部队，山东烟台264007）

改进粒子滤波算法及其在GPS/SINS组合导航中的应用

王林1，2，林雪原1，孙炜玮1，王萌1

（1.海军航空工程学院信息融合研究所，山东烟台264001；2.92154部队，山东烟台264007）

针对标准粒子滤波中存在的粒子退化问题，将无味卡尔曼滤波方法、线性规划方法与标准粒子滤波相结合，得到一种改进粒子滤波算法。改进粒子滤波算法中的重要性概率密度通过UKF算法获得，从而使粒子使用效率得到提升；二次采样过程中加入线性规划方法，保证了粒子的多样性。将改进粒子滤波算法应用于GPS/SINS组合导航，建立了GPS/SINS组合导航模型，通过仿真验证了该滤波算法的有效性，使用该算法可使惯性组合导航系统导航精度得到提高。

惯性组合导航；无迹卡尔曼滤波；线性规划；粒子滤波

粒子滤波是伴随计算机性能快速提升而发展起来的算法，由于其适用于非线性、非高斯环境的优点，在各领域受到越来越多人的关注[1]。粒子滤波的主要缺点是存在粒子退化问题，解决此问题的最有效方法是选择相对较好的重要性概率密度和二次采样方法，近几年出现的粒子滤波改进算法基本上都是围绕这两方面展开。[2]

本文对标准粒子滤波算法进行算法改进，重要性概率密度方面运用无味卡尔曼滤波（UKF）算法获得样本，二次采样方面加入线性规划方法。将改进后的算法应用于GPS/SINS组合导航，以微小型飞行器作为仿真对象，通过仿真验证改进算法的有效性。

1　粒子滤波算法

1.1算法概述

粒子滤波的基本思想是：选取一个重要性概率密度并且从中进行抽样，得到一组带有相应权值的随机样本，简称“粒子”，根据状态观测不断调节权值大小和位置，再使用这些样本来逼近状态后验分布，并使用这组样本的加权求和作为系统的状态估计值。当粒子数量很大时，其概率密度函数逼近于状态向量的真实后验分布函数，粒子滤波估计达到最优。[3-4]

1.2重要性概率密度

粒子滤波中的重要性概率密度分布能否逼近目标概率分布，直接影响跟踪精度。最优的重要性概率密度是后验概率密度，但是一般情况下很难直接从后验概率密度本身抽样取得样本。[4]为求解方便，标准粒子滤波算法通常选取先验概率密度来作为重要性概率密度。

1.3二次采样

二次采样的基本思想为：将权值比重相对较轻的粒子移除，将权值比重相对较重的粒子进行复制，并再次进行权值分配。

1.4标准粒子滤波的缺陷

粒子滤波的主要缺点是存在粒子退化问题。粒子退化现象是指伴随数据迭代积累，样本中粒子多样性逐渐减少的问题。[4]

解决此问题最有效的方法是使用大量的粒子，使样本的均值以最大概率（值为1）趋向数学期望。实际上，使用此方法在伴随着粒子数量增大的同时，系统资源占用量也会大幅增加，从而严重降低滤波算法实时性。因此，通常采用的2种方法是：选取好的重要性概率密度以及改进二次采样方法。

2　改进粒子滤波算法

2.1重要性概率密度选取

标准粒子滤波算法通常选取先验概率密度来作为重要性概率密度，通过采样得到的随机样本组与通过真实后验概率密度采样得到的样本组相比较存在较大偏差。

本文采用无味卡尔曼滤波（UKF）方法应用于粒子样本采样抽取，从而得到一系列的sigma采样点，这些sigma采样点在通过非线性系统后均值和方差能精确到真实的后验概率密度的二阶距，能够得到比较逼近真实后验概率密度的重要性概率密度。

2.2二次采样方法改进

标准粒子滤波的二次采样过程仅仅是移除权值比重小的粒子和复制权值比重大的粒子，从而导致权值比重大的粒子子代越来越多，粒子多样性逐步变差。

本文采用线性规划的方法，在复制权值比重大的粒子同时，将权值比重大的粒子与权值比重小的粒子进行组合，形成新的粒子。该方法使本来计划抛弃的权值比重小的粒子以一定比例参与状态转化，并且由于权值比重小的粒子的参与，粒子多样性得到了保证。

2.3改进粒子滤波算法流程

设非线性系统的系统方程及量测方程为：

式（1）、（2）中：W和V为均值为零的高斯分布白噪声，并且互不相关；方差阵分别为Q和R；u为确定性控制项。

改进后的算法流程如下。

2）重要性采样。当k时刻（k=1,2,…），对于i=1,2,…,Ns，执行以下步骤。

①采用UKF算法更新粒子。

式（5）～（7）中：n为状态向量维数；λ为尺度参数。

②时间更新。

在国内尚无同类专业烟囱软件情况下，本程序可以为设计人员进行整体悬挂式钢内筒烟囱的设计提供便利，同时可以满足现在设计人员对于工程设计智能化计算的要求，该软件还需后续优化完成自动绘图模块，并需要足够的理论支撑才能完成更加合理的软件设计。

③量测更新。

④采样粒子。

计算权重系数：

归一化权值：

采用线性规划的方法组合计算出新的粒子，χc=χa+Lχb，χc为组合后的新粒子，数量为Nc，χa为转入复制组的粒子，χb表示转入抛弃组的粒子，L为系数。

当抛弃组粒子数目少于复制组粒子时，循环使用抛弃组粒子，新粒子保留参与优化组合的复制组粒子的权值，并再次归一化权值：

4）计算滤波值。

3　GPS/SINS组合导航模型

组合导航模型采用直接式滤波，以捷联惯性导航系统机械编排方程作为状态方程，直接使用导航参数作被估状态，状态方程及量测方程无须线性化。[5]

3.1状态方程

采用“东北天”地理坐标系作为导航坐标系，惯性导航系统采用指北方位机械编排[6-7]，状态方程为：

滤波过程中，噪声进入非线性系统，参与状态转移，将系统噪声和量测噪声与状态向量一起组成增广向量Xa，维数n为21，增广的方差阵为Pa。[11-12]

状态方程依据惯性导航系统工作原理展开如下：

式（26）中：Sg,j、Sa,j为增广向量方差阵的平方根矩阵Pa的10到12行及13到15行，是陀螺仪及加速度计噪声的sigma点序列，j=0,1,…,2n，表示sigma点序号。[13-14]式中包括角增量的计算、四元数递推计算、方向余弦矩阵的计算、速度和位置的解算。[15-16]

3.2量测方程

直接式滤波的观测方程直接反映观测系统的量测原理，以GPS输出的速度和位置作为观测量，量测方程为式（27）所示，V为量测噪声：[17-18]

4　仿真验证

以微小型飞行器作为使用载体构建仿真验证系统，飞行轨迹按微小型飞行器性能特点设定，过程包括：加速—爬升—平直飞行—爬升—平直飞行—转弯—平直飞行—再一次爬升—平直飞行—转弯—下滑—盘旋状态，如图1所示。[19-21]仿真时间为3 600 s。

图1　飞行仿真的航迹Fig.1 Simulation of flight trace

飞行器的初始姿态定为：飞行器水平，航向为90°，位置（118°,29°,50 m）。[22-23]根据目前在用的惯性器件性能，设定SINS参数和GPS测量精度，如表1所示。[24-25]滤波初始参数如表2所示。

表1　SINS参数和GPS测量精度Tab.1 SINS of parameters and GPS of measure precision

表2　滤波参数Tab.2 Parameters of filter

为便于滤波性能分析，在相同仿真条件下分别使用UKF算法，基础粒子滤波和改进粒子滤波进行比较，3种滤波方法的误差曲线图如图2～4所示，稳态误差值如表3所示。

图2　基于UKF算法的误差曲线Fig.2 Error of UKF filter

图3　基于基础粒子算法的误差曲线Fig.3 Error of particle filter

图4　基于改进粒子滤波算法的误差曲线Fig.4 Error of improved particle filter

表3　组合导航系统的稳态误差值Tab.3 Steady state error of the integrated navigation system

从曲线图比较看出，UKF算法姿态曲线较差，速度曲线与位置曲线较好，这是因为UKF算法在采样点计算时，由于角度为非矢量，不能叠加计算，而是通过其他方式处理，造成姿态误差增大。由于有GPS提供速度、位置参考信号，且载体机动时间比较短，速度误差与位置误差较小；基础粒子滤波不存在UKF算法的缺点，但由于存在无法避免的粒子退化问题，滤波精度受到影响，曲线差于UKF算法；本文的改进粒子滤波算法有效克服了粒子退化问题，同时也降低了UKF算法缺点的影响，从曲线图和统计表可以看出，该方法优于UKF算法和基础粒子滤波算法。

5　结论

本文研究了粒子滤波算法，将无味卡尔曼滤波算法和线性规划方法融入标准粒子滤波算法，得到一种改进粒子滤波算法。构建了GPS/SINS组合导航仿真验证系统，仿真结果验证了该改进算法有效性。将改进粒子滤波算法仿真曲线、数据与UKF算法和基础粒子滤波的仿真曲线、数据进行对比，表明该算法优于后2种方法。采用此法可有效降低导航误差，适用于解决高机动条件下的飞行器非线性滤波与状态估计。

[1]刘建业，曾庆华，赵伟，等.导航系统理论与应用[M].西安：西北工业大学出版社，2010：296-366. LIU JIANYE，ZENG QINGHUA，ZHAO WEI，et al.Theory and application of guidance[M].Xi’an：Northwestern Polytechnical University Press，2010：296-366.（in Chinese）

[2]秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].西安：西北工业大学出版社，1998：198-221. QIN YONGYUAN，ZHANG HONGRONG，WANG SHUHUA.Kalman filtering and combined navigation principle[M].Xi’an：Northwestern Polytechnical University Press，1998：198-221.（in Chinese）

[3]高社生，何鹏举，杨波，等.组合导航原理及应用[M].西安：西北工业大学出版社，2012：92-108. GAO SHESHENG，HE PENGJU，YANG BO，et al.Combined navigation principle and application[M].Xi’an：Northwestern Polytechnical University Press，2012：92-108.（in Chinese）

[4]朱志宇.粒子滤波算法及其应用[M].北京：科学出版社，2010：19-38. ZHU ZHIYU.Particle filtration algorithm and application [M].Beijing：Science Press，2010：19-38.（in Chinese）

[5]林雪原，刘华章，史佩.一种低成本、低精度SINS/GPS组合导航系统级试验研究[J].武汉大学学报：信息科学版，2006，22（10）：1158-1162. LIN XUEYUAN，LIU HUAZHANG，SHI PEI.A low cost and low accuracy SINS/GPS integrated navigation system[J].Geomatics and Information Science of Wuhan University，2006，22（10）：1158-1162.（in Chinese）

[6]林雪原，林炳荣.基于量测多尺度预处理的GPS/SINS组合导航系统[J].兵工自动化，2010，15（7）：914-919. LIN XUEYUAN，LIN BINGRONG.A GPS/SINS integrated navigation system based on capacity surveyed many scales pretreatment[J].Ordnance Industry，2010，15（7）：914-919.（in Chinese）

[7]林雪原.GPS/SINS组合导航系统与实现[J].计算机测量与控制，2007，9（12）：1055-1058. LIN XUEYUAN.GPS/SINS integrated navigation system and realizing[J].Computer Measurement&Control，2007，9（12）：1055-1058.（in Chinese）

[8]林雪原，骆卉子，范文强.一种改进的GPS动态定位滤波方法[J].海军航空工程学院学报，2010，25（4）：402-407. LIN XUEYUAN，LUO HUIZI，FAN WENQIANG.An improved filtration method fixed position to one kind of GPS trends[J].Journal of Naval Aeronautical and Astronautical University，2010，25（4）：402-407.（in Chinese）

[9]林雪原.GPS/SINS组合导航系统的多尺度融合算法研究[J].电子科技大学学报，2011，31（9）：1259-1263. LIN XUEYUAN.Capacity surveyed many scales pretreatment of SINS/GPS integrated navigation system[J].Journal of the University of Electronic Science and Technology of China，2011，31（9）：1259-1263.（in Chinese）

[10]陆海勇.捷联惯性导航系统中UKF滤波技术的应用研究[D].南京：南京航空航天大学，2008. LU HAIYONG.Research on application of UKF in strapdown inertial navigation system[D].Nanjing：Nanjing University of Aeronautics and Astronautics，2008.（in Chinese）

[11]徐佳.INS/GPS组合导航系统滤波算法的研究[D].沈阳：东北大学，2006. XU JIA.Research on the filtering algorithm of the GPS/ SINS integrated navigation system[D].Shenyang：Northeast University，2006.（in Chinese）

[12]刘铮.UKF算法及其改进算法的研究[D].长沙：中南大学，2009. LIU ZHENG.UKF algorithm and an improved UKF algorithm[D].Changsha：Central South University，2009.（in Chinese）

[13]周翟和，刘建业，赖际舟，等.一种改进的UGPF算法及其在导航问题中的应用[J].南京航空航天大学学报，2010，42（6）：727-730. ZHOU ZHAIHE，LIU JIANYE，LAI JIZHOU，et al.Improved UGPF algorithm and its application for navigation problem[J].Nanjing University of Aeronautics and Astronautics，2010，42（6）：727-730.（in Chinese）

[14]邹国辉，敬忠良，胡洪涛.基于优化组合二次采样的粒子滤波算法[J].上海交通大学学报，2006，12（7）：821-826. ZOU GUOHUI，JING ZHONGLIANG，HU HONGTAO. An improved particle filtering algorithm based on linear optimization method[J].Journal of Shanghai Jiaotong University，2006，12（7）：821-826.（in Chinese）

[15]杨璐，李明，张鹏.一种新的改进粒子滤波算法[J].电子科技大学学报，2010，30（5）：666-669. YANG LU，LI MING，ZHANG PENG.An improved particle filtering algorithm[J].Journal of the University of Electronic Science and Technology of China，2010，30（5）：666-669.（in Chinese）

[16]向礼.非线性滤波方法及其在导航中的应用研究[D].哈尔滨：哈尔滨工业大学，2009. XIANG LI.Research on the nonlinear filtering algorithm of the GPS/SINS integrated navigation system[D].Harbin：Harbin Institute of Technology，2009.（in Chinese）

[17]赵俊梅.GPS/SINS组合导航系统滤波算法研究[D].太原：中北大学，2006：21-22. ZHAO JUNMEI.Research on the filtering algorithm of the GPS/SINS integrated navigation system[D].Taiyuan：North University of China，2006：21-22.（in Chinese）

[18]管波.用于SINS/GPS组合导航的粒子滤波算法及硬件实现研究[D].杭州：浙江大学，2012：7-14. GUAN BO.The particle filtering algorithm and hardware of the GPS/SINS integrated navigation system[D].Hangzhou：Zhejiang University，2012：7-14.（in Chinese）

[19]吴太旗，孙付平，易维勇.GPS/INS组合导航系统的Matlab/Simulink仿真[J].测绘学报，2004，21（3）：172-174. WU TAIQI，SUN FUPING，YI WEIYONG.The Matlab/ simulink simulation of the gps/sins integrated navigation system[J].Acta Geodaetica et Cartographica Sinica，2004，21（3）：172-174.（in Chinese）

[20]崔平远，郑黎方，裴福俊.基于卡尔曼/粒子组合滤波器的组合导航方法研究[J].系统仿真学报，2009，21（1）：220-223. CUI PINGYUAN，ZHENG LIFANG，PEI FUJUN.Research on the GPS/SINS integrated navigation system based on kalman filtering and particle filtering[J].Journal of System Simulation，2009，21（1）：220-223.（in Chinese）

[21]袁俊刚.GPS/惯性组合导航系统研究[D].南京：南京航空航天大学，2011：21-22. YUAN JUNGANG.Research on the GPS/SINS integrated navigation system[D].Nanjing：Nanjing University of Aeronautics andAstronautics，2011：21-22.（in Chinese）

[22]常明飞.GPS/惯性组合导航系统研究[D].重庆：重庆大学，2004：25-27. CHANG MINGFEI.Research on the GPS/SINS integrated navigation system[D].Chongqing：Chongqing University，2004：25-27.（in Chinese）

[23]郑黎方.SINS/GPS组合导航系统新算法的研究与实现[D].北京：北京工业大学，2008：28-31. ZHENG LIFANG.Research on the new filtering algorithm of the SINS/GPS integrated navigation system[D]. Beijing：Beijing University of Technology，2008：28-31.（in Chinese）

[24]吴韦华.卫星/SINS组合导航关键技术研究[D].西安：电子科技大学，2008：33-34. WU WEIHUA.Research on the critical technology of the SINS/GPS integrated navigation system[D].Xi’an：University of Electronic Science and Technology of China，2008：33-34.（in Chinese）

[25]周健.组合导航系统信息融合算法研究与可视化仿真[D].北京：北京工业大学，2009：32-34. ZHOU JIAN.Research on the capacity surveyed pretreatment of integrated navigation system[D].Beijing：Beijing University of Technology，2009：32-34.（in Chinese）

Improved Particle Filtering Algorithm and Its Application of the GPS/SINS Integrated Navigation

WANG Lin1，2,LIN Xueyuan1,SUN Weiwei1,WANG Meng1
（1.Research Institute of Information Fusion,NAAU,Yantai Shandong 264001,China; 2.The 92154thUnit of PLA,Yantai Shandong 264007,China）

Due to the degenerac existing in the particle filter,the particle filter algorithm was combined with unscented Kal⁃man filter(UKF)algorithm and linear optimization method in this paper.Unscented Kalman filter algorithm was used to generate the importance proposal distribution,and the linear optimization method was used to enhance the diversity of the samples.A demonstration system for the GPS/SINS integrated navigation was constructed according to the characteristics of GPS/SINS integrated navigation system.Simulation results verified the effectiveness of the filtering method and the filter⁃ing method could improve the navigation accuracy.

inertial integrated navigation;unscented Kalman filter;linear optimization;particle filter

V249.32+8

A

1673-1522（2016）01-0051-07

10.7682/j.issn.1673-1522.2016.01.010

2015-07-15；

2015-12-29

国家自然科学基金资助项目（60874112）

王林（1982-），男，硕士生。