高斯粒子濾波的慣性/GPS緊組合算法

于永軍，徐錦法，熊 智，張 梁

（1.直升機旋翼動力國家級重點實驗室（南京航空航天大學），210016南京；2.南京航空航天大學導航研究中心，210016南京）

高斯粒子濾波的慣性/GPS緊組合算法

于永軍1，徐錦法1，熊 智2，張 梁1

（1.直升機旋翼動力國家級重點實驗室（南京航空航天大學），210016南京；2.南京航空航天大學導航研究中心，210016南京）

為提高組合導航系統(tǒng)的可靠性，針對以偽距、偽距率殘差為量測信息的緊組合算法會帶來線性化誤差的缺點，推導了基于偽距、偽距率的非線性緊組合模型.針對緊組合系統(tǒng)狀態(tài)維數(shù)高導致粒子濾波實時性差的問題，提出基于線性非線性結構分解的高斯粒子濾波算法，對狀態(tài)方程中的非線性和線性部分利用高斯粒子濾波和經典卡爾曼濾波分別進行遞推，有效降低了系統(tǒng)的運算量.仿真結果表明，使用改進的緊組合濾波算法系統(tǒng)定位精度相比線性化緊組合算法提高一倍.

慣性導航；組合導航；緊組合；非線性濾波；高斯粒子濾波

涵道式垂直起降無人飛行器以其體積小、重量輕、噪聲低、安全性高、適應飛行器高速和小型化的發(fā)展需求等特性，目前已經成為垂直起降無人機領域研究的重點和熱點［1-3］.當前以慣性導航系統(tǒng)為基礎的基于卡爾曼濾波理論的慣性/GPS組合導航技術在無人飛行器自主導航領域得到了廣泛應用［4-5］.以速度、位置組合為特征的捷聯(lián)慣導/GPS松組合方式雖然組合方案簡單、實現(xiàn)較為容易，但當載體進行高動態(tài)機動或GPS接收機受到環(huán)境干擾時，系統(tǒng)精度將急劇下降，可靠性和抗干擾能力較差［6-7］.相對于松組合方式，緊組合中GPS接收機只需提供更為原始、直接的星歷和偽距、偽距率數(shù)據(jù)，無需直接提供載體的速度、位置信息.緊組合方式中GPS接收機和慣性導航系統(tǒng)相互輔助，從而使系統(tǒng)獲得更好的導航性能［8-9］.

隨著捷聯(lián)慣導/GPS組合導航系統(tǒng)應用條件的日益苛刻，非線性估計理論應用于SINS/GPS組合導航系統(tǒng)逐漸成為研究的熱點［10-11］.粒子濾波技術由于理論上適用于任意非線性系統(tǒng)的狀態(tài)估計，因此在非線性組合領域日益受到關注［12-13］.目前粒子濾波已在SINS/GPS組合導航系統(tǒng)的位置及姿態(tài)估計、SINS/GPS初始對準等方面得到了成功應用［14-15］.

粒子濾波雖然能夠有效解決SINS/GPS組合導航系統(tǒng)中的非線性濾波問題，但實際應用中隨著系統(tǒng)維數(shù)增加，為保證粒子濾波的收斂，對高維系統(tǒng)必須采用更多數(shù)量的粒子，粒子數(shù)量隨系統(tǒng)維數(shù)增加急劇增加［16］.因此，粒子濾波的實時性嚴重制約了其在SINS/GPS組合導航系統(tǒng)中的應用［17-18］.

針對緊組合導航系統(tǒng)特點，本文推導了基于偽距、偽距率的緊組合非線性濾波模型，去除了偽距、偽距率殘差線性化過程的線性化誤差.針對緊組合系統(tǒng)狀態(tài)量多導致粒子濾波實時性差的問題，提出了基于線性非線性結構分解的粒子濾波算法，在保證組合濾波實時性的同時，有效提高濾波精度.

1 SINS/GPS緊組合非線性模型

1.1 系統(tǒng)狀態(tài)方程

導航系選為“東北天”坐標系，捷聯(lián)慣性導航系統(tǒng)機體系（b）到導航系（n）轉動四元數(shù)Q的微分方程可以寫成［19］

其中δQ=［δq0，δq1，δq2，δq3］，U（Q）、Y（Q）詳細表述參見文獻［19］.

不考慮重力加速度誤差項，速度誤差模型可表示為

其中δV=［δVE，δVN，δVU］

位置誤差模型可表示為

其中RM，RN為地球曲率半徑.

與松組合相比，緊組合的狀態(tài)方程在松組合的基礎上，增加了GPS的誤差狀態(tài)，即:等效時鐘誤差相應的距離率δtu以及等效時鐘頻率誤差相應的距離率δtr u.δtu與δtr u的微分方程如下:

由此，系統(tǒng)狀態(tài)量如下:

其中，εbx、εby、εbz為陀螺零偏，?x、?y、?z為加速度計零偏.

分析式（2）～（4）可以看出，系統(tǒng)狀態(tài)方程非線性模型中，僅有速度誤差模型是非線性的，而姿態(tài)、位置和GPS鐘差等模型依然是線性的，因此，系統(tǒng)狀態(tài)量可以表示成如下形式:

其中:

表示系統(tǒng)的線性部分，而XNL=［δVE，δVN，δVU］表示系統(tǒng)的非線性部分.

由此，系統(tǒng)狀態(tài)方程可以表述成如下形式:

其中:FNL1，F(xiàn)NL2為系統(tǒng)非線性部分的轉移矩陣；FL1，F(xiàn)L2為系統(tǒng)線性化部分的轉移矩陣；GL，GNL為噪聲系數(shù)矩陣.

1.2 偽距量測方程

GPS接收機輸出的對應某顆衛(wèi)星i的偽距值為

其中:（x，y，z）為載體在ECEF坐標系上的位置真值；（xs i，ys i，zs i）為GPS第i顆衛(wèi)星在ECEF坐標系上的位置；vρi為偽距測量誤差.

載體真實位置（λ，L，h）與慣性導航系統(tǒng)輸出位置（λI，LI，hI）有如下關系:

根據(jù)下式:

將載體真實位置（λ，L，h）轉換到ECEF坐標系并代入式（7），可得到如式（10）的偽距量測方程.

1.3 偽距率量測方程

與偽距量測方程類似，GPS接收機輸出的對應某顆衛(wèi)星i的測量偽距率為

載體真實速度（ve，vn，vu）與慣性導航系統(tǒng)輸出速度（veI，vnI，vuI）有如下關系:

根據(jù)坐標系的轉換關系，地理系速度與ECEF坐標系的速度轉換關系為

根據(jù)式（13）將載體速度（vE，vN，vU）轉換到ECEF坐標系，并代入式（11），可得到偽距率量測方程:

根據(jù)式（10）和式（15），可以得到第i顆衛(wèi)星的量測方程為

2 基于結構分解的高斯粒子濾波算法

針對形如

的非線性系統(tǒng)，高斯粒子濾波分為抽取采樣點、時間更新和量測更新3個步驟.假設k-1時刻的后驗概率密度已知:

第一步，根據(jù)k-1時刻濾波結果抽取采樣點:

第二步，利用狀態(tài)方程進行時間更新:

并確定粒子權值

其中p（·）表示概率密度，q（·）表示重要性密度函數(shù).

第三步，根據(jù)量測方程進行量測更新:

其中Ck為歸一化因子，并利用式（20）構造新的粒子

針對由式（6）和式（16）構成的GPS/INS緊組合導航系統(tǒng)，由于系統(tǒng)方程中僅有速度誤差為非線性，因此將非線性系統(tǒng)的更新過程按照如下步驟分解為非線性方程和線性方程分別進行:

第一步，根據(jù)式（17）抽取采樣點xk，并根據(jù)式（6）將 xk分解為線性部分 xkL和非線性部分xkNL；

第二步，根據(jù)式（6）和式（18），對xkNL進行高斯粒子濾波時間更新得到x（k+1）NL，根據(jù)式（6），利用經典卡爾曼濾波對xkL進行時間更新x（k+1）L；

第三步，將x（k+1）NL和x（k+1）L合并構成非線性系統(tǒng)的狀態(tài)一步預測x（k+1），并利用UT變換求解一步預測均方誤差；

第四步，利用式（19）和式（21）進行量測更新，并對x（k+1）進行修正.

3 仿真驗證

根據(jù)上述組合方案，對SINS/GPS緊組合算法進行仿真.設慣性導航系統(tǒng)的陀螺常值漂移為0.1°/h，加速度計零偏為3×10-4G.GPS接收機偽距白噪聲5 m，偽距率白噪聲0.2 m/s.捷聯(lián)慣導周期為5 ms，GPS輸出周期為1 s，濾波周期為1 s.仿真時間為3 600 s.粒子濾波粒子數(shù)取為1 000個.

載體的典型機動動作設置為:加速、爬升、平飛和90°轉彎.航跡如圖1所示.

圖1 三維航跡

為了使驗證具有代表性，將本文算法與基于經典卡爾曼濾波和線性化模型的SINS/GPS緊組合算法進行對比.經度誤差對比如圖2所示.由圖2可以看出，在載體無機動保持平飛的情況下，以3 000～3 500 s時間段的經度誤差為例，如圖3所示，GPF和KF兩種算法的位置穩(wěn)態(tài)估計精度相近.但在載體有機動的情況下，以圖4所示的2 500～2 700 s轉彎機動為例，從圖中可以看出，GPF濾波穩(wěn)定性要明顯高于KF濾波.

圖2 經度誤差對比

圖3 平飛時的經度誤差

速度對比以北向速度為例，北向速度全局誤差及平飛、轉彎機動的誤差對比如圖5～7所示.

由圖5和圖6可以看出，即使在平飛狀態(tài)下，GPF的速度穩(wěn)態(tài)估計精度明顯優(yōu)于KF.由圖7可知，在轉彎機動時，GPF速度穩(wěn)態(tài)估計精度和穩(wěn)定性明顯優(yōu)于KF，這與式（3）中分析的速度非線性度高的分析相吻合.

圖4 轉彎機動時的經度誤差

圖5 北向速度全局誤差

圖6 平飛時的北向速度誤差

圖7 轉彎機動時的北向速度誤差

圖8 橫滾角誤差

姿態(tài)誤差以橫滾角為例，其誤差如圖8所示.

從圖8可以看出，GPF的姿態(tài)精度略優(yōu)于KF，但相對速度和位置精度的改善程度，其精度改善不明顯，這主要是由于偽距、偽距率組合時姿態(tài)不是直接觀測量.

表1給出了兩種算法的誤差對比.仿真環(huán)境為Thinkpad T440 i5-2.6 GHz CPU，4 G內存，Matlab2013a 64位版本.

由表1可以看出，本文的算法姿態(tài)精度提高了約20%，定位精度提高一倍.同時在本文的仿真環(huán)境下，以一次KF的濾波時間作為1個基準時間，本文提出的算法耗時1.54個基準時間，時間消耗只增加50%.

表1 UKF和KF緊組合參數(shù)誤差對比

4 結 語

針對捷聯(lián)慣導/GPS松組合方式可靠性和抗干擾能力較差的缺點，本文針對緊組合系統(tǒng)的特點，推導了基于偽距、偽距率的緊組合非線性濾波模型，提出了一種基于線性非線性結構分解的高斯粒子濾波算法.改進算法將系統(tǒng)狀態(tài)方程分解為線性和非線性兩部分，對線性系統(tǒng)部分采用線性KF濾波，對非線性系統(tǒng)部分采用高斯粒子濾波，在時間增加50%的情況下，相比基于KF的緊組合算法定位精度提高一倍.本文提出的方法對SINS/GPS緊組合系統(tǒng)的研究和應用具有重要的理論和實際參考價值.

［1］REN Xiaolu，WANG Changhong，YI Guoxing.Ducted fan UAV hovering attitude control［C］//2011 International Conference on Electronic&Mechanical Engineering and Information Technology.Harbin:［s.n.］，2011，1:421-424.

［2］李遠偉，奚伯齊，伊國興，等.小型涵道式無人機的研究進展［J］.哈爾濱工業(yè)大學學報，2010，42（5）:700-704.

［3］OHANIAN O J，GELHAUSEN P A，INMAN D J.Nondimensional modeling of ducted-fan aerodynamics ［J］.Journal of Aircraft，2012，49（1）:126-140.

［4］FARHAD Samadzadegan，GHASEM Abdi.Autonomous navigation of unmanned aerial vehicles based on multisensor data fusion［C］//20th Iranian Conference on Electrical Engineering.Canada:［s.n.］，2012:868-873.

［5］于永軍，劉建業(yè)，熊智，等.非同步量測特性的慣性/星光/衛(wèi)星組合算法研究［J］.儀器儀表學報，2011，12 （3）:2761-2767.

［6］de MARINA H G，PEREDA F J，GIRON-SIERRA J M.UAV attitude estimation using unscented Kalman filter and TRIAD［J］.IEEE Trans.Ind.Electron.，2012，59 （11）:4465-4474.

［7］張海，常艷紅，車歡.基于GPS/INS不同測量特性的自適應卡爾曼濾波算法［J］.中國慣性技術學報，2010，18（6）:696-701.

［8］KONDO S，KUBO N，YASUDA A.Evaluation of the pseudo range performance by using software GPS receiver［J］.Journal of Global Positioning System，2005 （4）:215-222.

［9］李榮冰，劉建業(yè)，賴際舟，等.Sigma-Point直接式卡爾曼濾波慣性組合導航算法［J］.控制與決策，2009，24 （7）:1018-1022.

［10］ALI J，MIRZA MRUB.Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution［J］.Nonlinear Dynamics，2010，61（3）:491-502.

［11］熊劍，魏林生，郭杭，等.基于加性四元數(shù)的 SINS/ CNS非線性緊組合方法［J］.中國慣性技術學報，2012，20（5）:596-600.

［12］LIM Jaechan，HONG Daehyoung.Gaussian particle filtering approach for carrier frequency offset estimation in OFDM systems［J］.IEEE Signal Processing Letters，2013，20（4）:367-370.

［13］段苛苛，李鄧化.粒子濾波在光纖陀螺四位置尋北中的應用研究［J］.儀器儀表學報，2013，34（8）:1749-1755.

［14］劉建業(yè)，熊劍，賴際舟，等.采用粒子濾波的捷聯(lián)慣導非線性快速初始對準算法［J］.中國慣性技術學報，2010，18（15）:527-532.

［15］周翟和，劉建業(yè)，賴際舟，等.一種新的改進高斯粒子濾波算法及其在SINS/GPS深組合導航系統(tǒng)中的應用［J］.控制與決策，2011，26（1）:85-88.

［16］文志強，朱艷輝，彭召意.粒子濾波目標跟蹤中的有效粒子數(shù)控制方法［J］.控制與決策，2013，28（9）: 1349-1360.

［17］于春娣，丁勇，李偉，等.一種基于改進重采樣的粒子濾波算法［J］.計算機應用與軟件，2013，30（2）:296-299.

［18］周翟和，劉建業(yè)，賴際舟，等.Rao-Blackwellized粒子濾波在SINS/GPS深組合導航系統(tǒng)中的應用研究［J］.宇航學報，2009（2）:515-520.

［19］劉建業(yè)，曾慶化，趙偉，等.導航系統(tǒng)理論與應用［M］.西安:西北工業(yè)大學出版社，2010.

（編輯 張 宏）

SINS/GPS tightly integrated algorithm with gaussian particle filter

YU Yongjun1，XU Jinfa1，XIONG Zhi2，ZHANG Liang1
（1.Science and Technology on Rotorcraft Aeromechanics Laboratory（Nanjing University of Aeronautics and Astronautics），210016 Nanjing，China；2.Navigation Research Center，Nanjing University of Aeronautics and Astronautics，210016 Nanjing，China）

To improve the reliability of integrated navigation，a tightly coupling nonlinear model based on pseudo range and rate is proposed for reducing the estimation error of tradi-tional algorithm using linear measurement model in this work.For the application of particle filtering to SINS/GPS tightly integrated navigation system，the dimension of the state variables has been a major constraint for the Real-time system.In this new arithmetic，a linear KF deduction and nonlinear GPF method have been employed for the linear part and the non-linear part to improve the precision and real time performance，respectively.Results from the simulation show that the hybrid algorithm can effectively improve the performance of the integrated navigation system，and the precision increases one time.

inertial navigation；integrated navigation；tightly coupling；non-linear filter；gaussian particle filter

V249.3

A

0367-6234（2015）05-0081-05

10.11918/j.issn.0367-6234.2015.05.014

2014-05-19.

中國博士后科學基金（2013M541668）；江蘇高校優(yōu)勢學科建設工程資助；江蘇省博士后基金（1401041B）.

于永軍（1982—），男，博士后；

徐錦法（1963—），男，教授，博士生導師；

熊 智（1976—），男，研究員，博士生導師.

徐錦法，xjfae@nuaa.edu.cn.