王　林，林雪原，孫煒瑋，王　萌

（1.海軍航空工程學(xué)院信息融合研究所，山東煙臺(tái)264001；2.92154部隊(duì)，山東煙臺(tái)264007）

改進(jìn)粒子濾波算法及其在GPS/SINS組合導(dǎo)航中的應(yīng)用

王林1，2，林雪原1，孫煒瑋1，王萌1

（1.海軍航空工程學(xué)院信息融合研究所，山東煙臺(tái)264001；2.92154部隊(duì)，山東煙臺(tái)264007）

針對(duì)標(biāo)準(zhǔn)粒子濾波中存在的粒子退化問(wèn)題，將無(wú)味卡爾曼濾波方法、線性規(guī)劃方法與標(biāo)準(zhǔn)粒子濾波相結(jié)合，得到一種改進(jìn)粒子濾波算法。改進(jìn)粒子濾波算法中的重要性概率密度通過(guò)UKF算法獲得，從而使粒子使用效率得到提升；二次采樣過(guò)程中加入線性規(guī)劃方法，保證了粒子的多樣性。將改進(jìn)粒子濾波算法應(yīng)用于GPS/SINS組合導(dǎo)航，建立了GPS/SINS組合導(dǎo)航模型，通過(guò)仿真驗(yàn)證了該濾波算法的有效性，使用該算法可使慣性組合導(dǎo)航系統(tǒng)導(dǎo)航精度得到提高。

慣性組合導(dǎo)航；無(wú)跡卡爾曼濾波；線性規(guī)劃；粒子濾波

粒子濾波是伴隨計(jì)算機(jī)性能快速提升而發(fā)展起來(lái)的算法，由于其適用于非線性、非高斯環(huán)境的優(yōu)點(diǎn)，在各領(lǐng)域受到越來(lái)越多人的關(guān)注[1]。粒子濾波的主要缺點(diǎn)是存在粒子退化問(wèn)題，解決此問(wèn)題的最有效方法是選擇相對(duì)較好的重要性概率密度和二次采樣方法，近幾年出現(xiàn)的粒子濾波改進(jìn)算法基本上都是圍繞這兩方面展開(kāi)。[2]

本文對(duì)標(biāo)準(zhǔn)粒子濾波算法進(jìn)行算法改進(jìn)，重要性概率密度方面運(yùn)用無(wú)味卡爾曼濾波（UKF）算法獲得樣本，二次采樣方面加入線性規(guī)劃方法。將改進(jìn)后的算法應(yīng)用于GPS/SINS組合導(dǎo)航，以微小型飛行器作為仿真對(duì)象，通過(guò)仿真驗(yàn)證改進(jìn)算法的有效性。

1　粒子濾波算法

1.1算法概述

粒子濾波的基本思想是：選取一個(gè)重要性概率密度并且從中進(jìn)行抽樣，得到一組帶有相應(yīng)權(quán)值的隨機(jī)樣本，簡(jiǎn)稱“粒子”，根據(jù)狀態(tài)觀測(cè)不斷調(diào)節(jié)權(quán)值大小和位置，再使用這些樣本來(lái)逼近狀態(tài)后驗(yàn)分布，并使用這組樣本的加權(quán)求和作為系統(tǒng)的狀態(tài)估計(jì)值。當(dāng)粒子數(shù)量很大時(shí)，其概率密度函數(shù)逼近于狀態(tài)向量的真實(shí)后驗(yàn)分布函數(shù)，粒子濾波估計(jì)達(dá)到最優(yōu)。[3-4]

1.2重要性概率密度

粒子濾波中的重要性概率密度分布能否逼近目標(biāo)概率分布，直接影響跟蹤精度。最優(yōu)的重要性概率密度是后驗(yàn)概率密度，但是一般情況下很難直接從后驗(yàn)概率密度本身抽樣取得樣本。[4]為求解方便，標(biāo)準(zhǔn)粒子濾波算法通常選取先驗(yàn)概率密度來(lái)作為重要性概率密度。

1.3二次采樣

二次采樣的基本思想為：將權(quán)值比重相對(duì)較輕的粒子移除，將權(quán)值比重相對(duì)較重的粒子進(jìn)行復(fù)制，并再次進(jìn)行權(quán)值分配。

1.4標(biāo)準(zhǔn)粒子濾波的缺陷

粒子濾波的主要缺點(diǎn)是存在粒子退化問(wèn)題。粒子退化現(xiàn)象是指伴隨數(shù)據(jù)迭代積累，樣本中粒子多樣性逐漸減少的問(wèn)題。[4]

解決此問(wèn)題最有效的方法是使用大量的粒子，使樣本的均值以最大概率（值為1）趨向數(shù)學(xué)期望。實(shí)際上，使用此方法在伴隨著粒子數(shù)量增大的同時(shí)，系統(tǒng)資源占用量也會(huì)大幅增加，從而嚴(yán)重降低濾波算法實(shí)時(shí)性。因此，通常采用的2種方法是：選取好的重要性概率密度以及改進(jìn)二次采樣方法。

2　改進(jìn)粒子濾波算法

2.1重要性概率密度選取

標(biāo)準(zhǔn)粒子濾波算法通常選取先驗(yàn)概率密度來(lái)作為重要性概率密度，通過(guò)采樣得到的隨機(jī)樣本組與通過(guò)真實(shí)后驗(yàn)概率密度采樣得到的樣本組相比較存在較大偏差。

本文采用無(wú)味卡爾曼濾波（UKF）方法應(yīng)用于粒子樣本采樣抽取，從而得到一系列的sigma采樣點(diǎn)，這些sigma采樣點(diǎn)在通過(guò)非線性系統(tǒng)后均值和方差能精確到真實(shí)的后驗(yàn)概率密度的二階距，能夠得到比較逼近真實(shí)后驗(yàn)概率密度的重要性概率密度。

2.2二次采樣方法改進(jìn)

標(biāo)準(zhǔn)粒子濾波的二次采樣過(guò)程僅僅是移除權(quán)值比重小的粒子和復(fù)制權(quán)值比重大的粒子，從而導(dǎo)致權(quán)值比重大的粒子子代越來(lái)越多，粒子多樣性逐步變差。

本文采用線性規(guī)劃的方法，在復(fù)制權(quán)值比重大的粒子同時(shí)，將權(quán)值比重大的粒子與權(quán)值比重小的粒子進(jìn)行組合，形成新的粒子。該方法使本來(lái)計(jì)劃拋棄的權(quán)值比重小的粒子以一定比例參與狀態(tài)轉(zhuǎn)化，并且由于權(quán)值比重小的粒子的參與，粒子多樣性得到了保證。

2.3改進(jìn)粒子濾波算法流程

設(shè)非線性系統(tǒng)的系統(tǒng)方程及量測(cè)方程為：

式（1）、（2）中：W和V為均值為零的高斯分布白噪聲，并且互不相關(guān)；方差陣分別為Q和R；u為確定性控制項(xiàng)。

改進(jìn)后的算法流程如下。

2）重要性采樣。當(dāng)k時(shí)刻（k=1,2,…），對(duì)于i=1,2,…,Ns，執(zhí)行以下步驟。

①采用UKF算法更新粒子。

式（5）～（7）中：n為狀態(tài)向量維數(shù)；λ為尺度參數(shù)。

②時(shí)間更新。

在國(guó)內(nèi)尚無(wú)同類專業(yè)煙囪軟件情況下，本程序可以為設(shè)計(jì)人員進(jìn)行整體懸掛式鋼內(nèi)筒煙囪的設(shè)計(jì)提供便利，同時(shí)可以滿足現(xiàn)在設(shè)計(jì)人員對(duì)于工程設(shè)計(jì)智能化計(jì)算的要求，該軟件還需后續(xù)優(yōu)化完成自動(dòng)繪圖模塊，并需要足夠的理論支撐才能完成更加合理的軟件設(shè)計(jì)。

③量測(cè)更新。

④采樣粒子。

計(jì)算權(quán)重系數(shù)：

歸一化權(quán)值：

采用線性規(guī)劃的方法組合計(jì)算出新的粒子，χc=χa+Lχb，χc為組合后的新粒子，數(shù)量為Nc，χa為轉(zhuǎn)入復(fù)制組的粒子，χb表示轉(zhuǎn)入拋棄組的粒子，L為系數(shù)。

當(dāng)拋棄組粒子數(shù)目少于復(fù)制組粒子時(shí)，循環(huán)使用拋棄組粒子，新粒子保留參與優(yōu)化組合的復(fù)制組粒子的權(quán)值，并再次歸一化權(quán)值：

4）計(jì)算濾波值。

3　GPS/SINS組合導(dǎo)航模型

組合導(dǎo)航模型采用直接式濾波，以捷聯(lián)慣性導(dǎo)航系統(tǒng)機(jī)械編排方程作為狀態(tài)方程，直接使用導(dǎo)航參數(shù)作被估狀態(tài)，狀態(tài)方程及量測(cè)方程無(wú)須線性化。[5]

3.1狀態(tài)方程

采用“東北天”地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，慣性導(dǎo)航系統(tǒng)采用指北方位機(jī)械編排[6-7]，狀態(tài)方程為：

濾波過(guò)程中，噪聲進(jìn)入非線性系統(tǒng)，參與狀態(tài)轉(zhuǎn)移，將系統(tǒng)噪聲和量測(cè)噪聲與狀態(tài)向量一起組成增廣向量Xa，維數(shù)n為21，增廣的方差陣為Pa。[11-12]

狀態(tài)方程依據(jù)慣性導(dǎo)航系統(tǒng)工作原理展開(kāi)如下：

式（26）中：Sg,j、Sa,j為增廣向量方差陣的平方根矩陣Pa的10到12行及13到15行，是陀螺儀及加速度計(jì)噪聲的sigma點(diǎn)序列，j=0,1,…,2n，表示sigma點(diǎn)序號(hào)。[13-14]式中包括角增量的計(jì)算、四元數(shù)遞推計(jì)算、方向余弦矩陣的計(jì)算、速度和位置的解算。[15-16]

3.2量測(cè)方程

直接式濾波的觀測(cè)方程直接反映觀測(cè)系統(tǒng)的量測(cè)原理，以GPS輸出的速度和位置作為觀測(cè)量，量測(cè)方程為式（27）所示，V為量測(cè)噪聲：[17-18]

4　仿真驗(yàn)證

以微小型飛行器作為使用載體構(gòu)建仿真驗(yàn)證系統(tǒng)，飛行軌跡按微小型飛行器性能特點(diǎn)設(shè)定，過(guò)程包括：加速—爬升—平直飛行—爬升—平直飛行—轉(zhuǎn)彎—平直飛行—再一次爬升—平直飛行—轉(zhuǎn)彎—下滑—盤(pán)旋狀態(tài)，如圖1所示。[19-21]仿真時(shí)間為3 600 s。

圖1　飛行仿真的航跡Fig.1 Simulation of flight trace

飛行器的初始姿態(tài)定為：飛行器水平，航向?yàn)?0°，位置（118°,29°,50 m）。[22-23]根據(jù)目前在用的慣性器件性能，設(shè)定SINS參數(shù)和GPS測(cè)量精度，如表1所示。[24-25]濾波初始參數(shù)如表2所示。

表1　SINS參數(shù)和GPS測(cè)量精度Tab.1 SINS of parameters and GPS of measure precision

表2　濾波參數(shù)Tab.2 Parameters of filter

為便于濾波性能分析，在相同仿真條件下分別使用UKF算法，基礎(chǔ)粒子濾波和改進(jìn)粒子濾波進(jìn)行比較，3種濾波方法的誤差曲線圖如圖2～4所示，穩(wěn)態(tài)誤差值如表3所示。

圖2　基于UKF算法的誤差曲線Fig.2 Error of UKF filter

圖3　基于基礎(chǔ)粒子算法的誤差曲線Fig.3 Error of particle filter

圖4　基于改進(jìn)粒子濾波算法的誤差曲線Fig.4 Error of improved particle filter

表3　組合導(dǎo)航系統(tǒng)的穩(wěn)態(tài)誤差值Tab.3 Steady state error of the integrated navigation system

從曲線圖比較看出，UKF算法姿態(tài)曲線較差，速度曲線與位置曲線較好，這是因?yàn)閁KF算法在采樣點(diǎn)計(jì)算時(shí)，由于角度為非矢量，不能疊加計(jì)算，而是通過(guò)其他方式處理，造成姿態(tài)誤差增大。由于有GPS提供速度、位置參考信號(hào)，且載體機(jī)動(dòng)時(shí)間比較短，速度誤差與位置誤差較?。换A(chǔ)粒子濾波不存在UKF算法的缺點(diǎn)，但由于存在無(wú)法避免的粒子退化問(wèn)題，濾波精度受到影響，曲線差于UKF算法；本文的改進(jìn)粒子濾波算法有效克服了粒子退化問(wèn)題，同時(shí)也降低了UKF算法缺點(diǎn)的影響，從曲線圖和統(tǒng)計(jì)表可以看出，該方法優(yōu)于UKF算法和基礎(chǔ)粒子濾波算法。

5　結(jié)論

本文研究了粒子濾波算法，將無(wú)味卡爾曼濾波算法和線性規(guī)劃方法融入標(biāo)準(zhǔn)粒子濾波算法，得到一種改進(jìn)粒子濾波算法。構(gòu)建了GPS/SINS組合導(dǎo)航仿真驗(yàn)證系統(tǒng)，仿真結(jié)果驗(yàn)證了該改進(jìn)算法有效性。將改進(jìn)粒子濾波算法仿真曲線、數(shù)據(jù)與UKF算法和基礎(chǔ)粒子濾波的仿真曲線、數(shù)據(jù)進(jìn)行對(duì)比，表明該算法優(yōu)于后2種方法。采用此法可有效降低導(dǎo)航誤差，適用于解決高機(jī)動(dòng)條件下的飛行器非線性濾波與狀態(tài)估計(jì)。

[1]劉建業(yè)，曾慶華，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，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]秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，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]高社生，何鵬舉，楊波，等.組合導(dǎo)航原理及應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，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]朱志宇.粒子濾波算法及其應(yīng)用[M].北京：科學(xué)出版社，2010：19-38. ZHU ZHIYU.Particle filtration algorithm and application [M].Beijing：Science Press，2010：19-38.（in Chinese）

[5]林雪原，劉華章，史佩.一種低成本、低精度SINS/GPS組合導(dǎo)航系統(tǒng)級(jí)試驗(yàn)研究[J].武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，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]林雪原，林炳榮.基于量測(cè)多尺度預(yù)處理的GPS/SINS組合導(dǎo)航系統(tǒng)[J].兵工自動(dòng)化，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組合導(dǎo)航系統(tǒng)與實(shí)現(xiàn)[J].計(jì)算機(jī)測(cè)量與控制，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]林雪原，駱卉子，范文強(qiáng).一種改進(jìn)的GPS動(dòng)態(tài)定位濾波方法[J].海軍航空工程學(xué)院學(xué)報(bào)，2010，25（4）：402-407. LIN XUEYUAN，LUO HUIZI，F(xiàn)AN 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組合導(dǎo)航系統(tǒng)的多尺度融合算法研究[J].電子科技大學(xué)學(xué)報(bào)，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]陸海勇.捷聯(lián)慣性導(dǎo)航系統(tǒng)中UKF濾波技術(shù)的應(yīng)用研究[D].南京：南京航空航天大學(xué)，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ǎo)航系統(tǒng)濾波算法的研究[D].沈陽(yáng)：東北大學(xué)，2006. XU JIA.Research on the filtering algorithm of the GPS/ SINS integrated navigation system[D].Shenyang：Northeast University，2006.（in Chinese）

[12]劉錚.UKF算法及其改進(jìn)算法的研究[D].長(zhǎng)沙：中南大學(xué)，2009. LIU ZHENG.UKF algorithm and an improved UKF algorithm[D].Changsha：Central South University，2009.（in Chinese）

[13]周翟和，劉建業(yè)，賴際舟，等.一種改進(jìn)的UGPF算法及其在導(dǎo)航問(wèn)題中的應(yīng)用[J].南京航空航天大學(xué)學(xué)報(bào)，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]鄒國(guó)輝，敬忠良，胡洪濤.基于優(yōu)化組合二次采樣的粒子濾波算法[J].上海交通大學(xué)學(xué)報(bào)，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ìn)粒子濾波算法[J].電子科技大學(xué)學(xué)報(bào)，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ǎo)航中的應(yīng)用研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，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ǎo)航系統(tǒng)濾波算法研究[D].太原：中北大學(xué)，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ǎo)航的粒子濾波算法及硬件實(shí)現(xiàn)研究[D].杭州：浙江大學(xué)，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組合導(dǎo)航系統(tǒng)的Matlab/Simulink仿真[J].測(cè)繪學(xué)報(bào)，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]崔平遠(yuǎn)，鄭黎方，裴?？?基于卡爾曼/粒子組合濾波器的組合導(dǎo)航方法研究[J].系統(tǒng)仿真學(xué)報(bào)，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ǎo)航系統(tǒng)研究[D].南京：南京航空航天大學(xué)，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ǎo)航系統(tǒng)研究[D].重慶：重慶大學(xué)，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ǎo)航系統(tǒng)新算法的研究與實(shí)現(xiàn)[D].北京：北京工業(yè)大學(xué)，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]吳韋華.衛(wèi)星/SINS組合導(dǎo)航關(guān)鍵技術(shù)研究[D].西安：電子科技大學(xué)，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ǎo)航系統(tǒng)信息融合算法研究與可視化仿真[D].北京：北京工業(yè)大學(xué)，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

國(guó)家自然科學(xué)基金資助項(xiàng)目（60874112）

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