改進粒子濾波算法及其在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-），男，碩士生。