張恒浩 唐 超 王小錠 魏 明 張 霞 吳勝寶

中國運載火箭技術研究院研發(fā)中心，北京100076

針對粒子濾波算法在組合導航系統(tǒng)應用中出現(xiàn)的粒子衰減退化問題［1-2］，引入基于概率核函數(shù)的改進粒子濾波算法。這種改進算法在設計過程中引入基于概率統(tǒng)計的核函數(shù)，通過改進粒子濾波算法中的重要性函數(shù)來獲得全面的系統(tǒng)信息;然后設計出核函數(shù)和對應的交互核函數(shù)，并將其應用在粒子濾波算法的重采樣計算中;最后利用取得的粒子集合和對應的權值生成新的粒子集。這樣改進的粒子濾波算法在阻止粒子集合退化現(xiàn)象發(fā)生的同時還可以保持粒子的多樣性，從而可以達到提高系統(tǒng)導航精度的目的。仿真結果表明，引入改進粒子濾波算法的組合導航系統(tǒng)可以有效降低系統(tǒng)的誤差，提高系統(tǒng)導航精度。

1 重要性采樣函數(shù)設計

可以看出，改進粒子濾波算法在計算重要性采樣時考慮了已知量測值條件下狀態(tài)估計概率和前一狀態(tài)值條件下的狀態(tài)估計概率。通過這2種狀態(tài)估計概率值的比例，最大程度上反映了重要性采樣函數(shù)表示的系統(tǒng)信息，避免了可能因為傳感器精度的限制而出現(xiàn)的采樣集合丟失。這樣的設計可以有效地提高系統(tǒng)粒子濾波的性能。

2 基于隨機概率統(tǒng)計的核函數(shù)設計

核函數(shù)算法主要是用來提供一種處理非線性問題的方法。粒子濾波算法中引入核函數(shù)的主要目的是希望通過核函數(shù)算法來解決粒子濾波算法在計算過程中發(fā)生的粒子退化現(xiàn)象。核函數(shù)的設計要與粒子濾波算法中得到的粒子集合的特征相對應。另外在設計核函數(shù)的過程中還要考慮粒子濾波算法的計算量和計算復雜度，避免在計算過程中出現(xiàn)獲得非線性積分數(shù)據(jù)的困難［8］。

因此，針對應用在組合導航系統(tǒng)中的粒子濾波算法改進這一特定問題，選擇什么樣的核函數(shù)是至關重要的。而且即使選擇了某一核函數(shù)，相應參數(shù)選取的不同也會影響改進的粒子濾波算法的計算精度。所以改進粒子濾波算法中引入的核函數(shù)算法必須考慮重采樣粒子的概率特征，通過概率特征來設計核函數(shù)的概率密度函數(shù)和分布函數(shù)，然后再通過重采樣計算得到新的粒子集。下面給出基于概率統(tǒng)計理論的核函數(shù)推導。

假設X作為一個隨機變量，則將函數(shù)式(5)所表示的函數(shù)稱為隨機變量X的概率分布函數(shù)［9］。

根據(jù)上述概率密度函數(shù)的定義，核函數(shù)設計的核心是要從粒子濾波得到的重要性采樣樣本數(shù)據(jù)中建立一個概率密度函數(shù)［11］，然后將這一概率密度函數(shù)引入重采樣計算中。

為了設計核函數(shù)，必須考慮粒子濾波中重采樣得到的粒子集合的特征，然后才能根據(jù)特征設計有針對性的核函數(shù)。下面分析原來典型的粒子濾波重采樣算法得到的粒子集合的性質。

重采樣粒子具有隨機性，所以它們彼此之間具有獨立性，相互之間不影響對方的采樣數(shù)值。粒子重采樣還是基于系統(tǒng)狀態(tài)方程的估計進行采樣的，這些粒子同系統(tǒng)狀態(tài)方程的分布有相同的分布，所以具有同分布性。同時滿足同分布性和隨機性的粒子集合可以近似看做是隨機抽樣樣本［12］。經過上面描述可知，粒子濾波過程中的重采樣得到的新的粒子集合滿足上述條件，在引入核函數(shù)改進粒子濾波算法的過程中可以將重采樣過程得到的粒子集合看做是隨機采樣數(shù)據(jù)集合。在設計核函數(shù)的時候利用隨機抽樣集合的特征來建立核函數(shù)方程。

假設粒子濾波計算過程中一共有n個重采樣粒子，可以看做是n個樣本估計。設對應的第i個粒子對應得到的數(shù)據(jù)為k(xi，x)。由樣本估計原理計算得到粒子集合整體的概率密度估計表達式為:

定義由重采樣計算得到的第i個粒子對應的函數(shù)表達式k(xi，x)為交互核函數(shù)，對于在粒子濾波算法中的核函數(shù)則定義其表達式為K(xi，x)。

因為交互核函數(shù)k(xi，x)實際上是重采樣算法得到的粒子中第i個粒子的概率密度函數(shù)值，所以對于任意的x，交互核函數(shù)k(xi，x)必須滿足條件:

核函數(shù)K(xi，x)表示的含義是在粒子濾波重采樣中得到的第i個粒子的分布函數(shù)值。核函數(shù)和交互核函數(shù)之間的關系如下:

為了在計算中方便求解，一般在設計交互核函數(shù)時要盡可能地將已知量xi的函數(shù)表達式和未知量x的函數(shù)表達式分開。這樣在計算過程中可以更方便地求出核函數(shù)，這種設計方法可以使粒子濾波算法減小計算量，有利于滿足系統(tǒng)的實時性要求。

根據(jù)這一思路，設計的交互核函數(shù)一般將其改進為:

式(12)中，因為設計的交互核函數(shù)k(xi，x)已將關于xi的函數(shù)表達式和x的函數(shù)表達式分開，在求解核函數(shù)K(xi，x)時只對x做積分運算。所以在計算過程中可以將函數(shù)ψi(xi)看做常數(shù)。設改進后的交互核函數(shù)ψi(xi)φi(x)對x求積分得到的核函數(shù)為:

設計出核函數(shù)K(xi，x)和交互核函數(shù)k(xi，x)后，將它們引入粒子濾波算法中的重采樣計算中，利用取得的粒子集合和對應的權值生成新的粒子集，在阻止粒子集合退化現(xiàn)象發(fā)生的同時保持粒子的多樣性，這樣可以提高粒子濾波算法的計算精度。

3 引入核函數(shù)的改進粒子濾波算法設計

因為傳統(tǒng)的粒子濾波算法所依據(jù)的信息主要是離散的樣本粒子及其在后驗概率分布中占有比重權值的粒子。在傳統(tǒng)的粒子濾波算法中引入重采樣方法的主要目的就是利用粒子權值來區(qū)別對待所獲得的采樣粒子。它通過保留或者復制粒子權值較大的粒子、放棄刪除粒子權值較小的粒子來解決粒子退化問題。由于大量的粒子在迭代過程中逐漸由相同的大權值粒子復制而來，這樣的結果必然導致粒子樣本的多樣性喪失，樣本粒子逐漸枯竭。

改進粒子濾波算法根據(jù)前面推導得到的樣本粒子集合以及對應的權值，通過設計的核函數(shù)來模擬擬合離散分布，估計出后驗概率密度的連續(xù)模型分布函數(shù)，再根據(jù)這個模型對粒子濾波算法進行重采樣，這樣可以得到更具多樣性的粒子，從而保證了粒子集合的多樣性，并能有效地抑制粒子退化現(xiàn)象。

因此，推導改進的核函數(shù)粒子濾波算法的計算步驟為:

步驟5:狀態(tài)輸出估計:當系統(tǒng)的改進粒子濾波在步驟4中進行重采樣計算后，則狀態(tài)輸出估計表達式為:

步驟6:設k=k+1，并返回步驟2繼續(xù)計算。

這6個步驟給出了引入核函數(shù)的粒子濾波改進算法的計算推導過程。在這一改進算法中，首先，對重要性函數(shù)設計所作的改進會使算法能通過采樣粒子更全面地反映系統(tǒng)狀態(tài)，從而提高粒子濾波器的工作性能;其次，在重采樣過程中引入基于概率統(tǒng)計的核函數(shù)算法，計算生成新的粒子集合。核函數(shù)的引入可以使粒子濾波算法在計算過程中保持粒子集合的多樣性，不因粒子濾波算法的迭代而使粒子性質變得單一;最后由于核函數(shù)計算過程中引入了隨機抽樣算法的平均計算思路，這樣如果計算過程中出現(xiàn)粒子失真現(xiàn)象時可通過加權平均的思想將其變得穩(wěn)定。所以在計算粒子濾波過程中，核函數(shù)解算可使粒子濾波中得到的重采樣粒子集合變得平滑。

4 仿真驗證

為驗證改進粒子濾波算法的性能，在組合導航系統(tǒng)中應用改進的粒子濾波核函數(shù)算法和傳統(tǒng)的粒子濾波算法進行組合導航系統(tǒng)的濾波設計，通過兩者結果的比較來驗證改進的粒子濾波算法的有效性和可行性。

組合導航系統(tǒng)由慣性導航系統(tǒng)和GPS衛(wèi)星定位系統(tǒng)組成，組合導航系統(tǒng)的狀態(tài)方程和觀測方程表達式如式(24)和(25)所示:

組合導航系統(tǒng)工作時的初始參數(shù)如下:

初始狀態(tài)的速度誤差和位置誤差均為0，計算過程中的迭代時間為300s;

粒子算法中計算的粒子數(shù)目設為40，重采樣的粒子限制值Nth設為20。

在計算過程中通過對組合導航系統(tǒng)內部的狀態(tài)誤差進行估計，并得到計算結果。通過比對2種算法的計算結果來判斷改進算法的優(yōu)劣。

組合導航系統(tǒng)在應用改進的粒子濾波核函數(shù)算法時需要設計核函數(shù)，根據(jù)推導核函數(shù)時給出的核函數(shù)相關信息，選擇核函數(shù)K(xi，x)的表達式為:

為了簡化計算，在核函數(shù)式(27)中設參數(shù)λ的值為1。

這樣將設置好參數(shù)的核函數(shù)和交互核函數(shù)引入改進粒子濾波算法的重采樣算法中，分別將改進的粒子濾波核函數(shù)算法和原來的經典粒子濾波算法應用到組合導航系統(tǒng)中。這2種算法都是通過粒子解算估計出組合導航狀態(tài)X的估計值，再通過濾波算法得到下一狀態(tài)的推測值。對比2種算法得到的誤差結果，從而判斷改進的粒子濾波算法是否能起到改善組合導航系統(tǒng)性能的要求。

圖1～5為2種算法的結果比較。圖中的實線表示引進核函數(shù)的改進粒子濾波算法得到的結果，虛線表示傳統(tǒng)的粒子濾波算法得到的結果。

從仿真結果可以看出，當粒子集合退化嚴重，粒子集合向單一性發(fā)展的時候，傳統(tǒng)的粒子濾波算法會產生較大的導航誤差，并且誤差有發(fā)散趨勢。引入核函數(shù)的改進粒子濾波算法在工作中可以降低由粒子集合退化產生的組合導航系統(tǒng)誤差，并且使系統(tǒng)誤差的變化趨于平穩(wěn)。組合導航系統(tǒng)在工作時誤差的變化率變得平穩(wěn)，這樣有利于系統(tǒng)在工作中達到穩(wěn)定狀態(tài)。

圖1 正北方向位置誤差比較

圖2 正東方向位置誤差比較

圖3 高度位置誤差比較

圖4 北向速度誤差比較

圖5 東向速度誤差比較

5 結論

針對傳統(tǒng)粒子濾波算法中出現(xiàn)的粒子衰減退化問題，引入了基于概率核函數(shù)設計的改進粒子濾波算法。并將這一算法應用在組合導航系統(tǒng)中。該算法在粒子濾波算法出現(xiàn)粒子衰減退化現(xiàn)象時能夠有效抑制粒子衰減并保持粒子的多樣性。仿真結果表明，這種基于概率核函數(shù)設計的改進粒子濾波算法可以很好地解決粒子濾波算法在計算過程中出現(xiàn)的粒子衰減退化問題，提高組合導航系統(tǒng)的計算精度。

［1］ Julier S，Unlmann J.Unscented Filtering and Nonlinear Estimation［J］.Proceedings of IEEE，2004，192(3):401-422.

［2］ Rudolph V M，Arnaud D.The Unscented Particle Filter［R］.CU ED/F-Infeng/Tr380.Cambridge University Engineering Department，2001.

［3］ Erik B，Peter J A，Nils C，John M S.Monte Caulo filter for Non-linear State Estimation［J］.Automatica，2001，37(2):177-183.

［4］ 莊澤森，張建秋，尹建君.Rao—Blackwellizes粒子概率假設密度濾波算法［J］.航空學報，2009，30(4):698-705.(ZHUANG Zesen， ZHANG Jianqiu， YIN Jianjun.Rao-Blackwellized Particle Probability Hypothesis Density Filter［J］.Acta Aeronautica ET Astronautica Sinica，2009，30(4):698-705.)

［5］ 張磊.一種基于高斯混合模型粒子濾波的故障預測算法［J］.航空學報，2009，30(2):319-324.(ZHANG Lei.A Fault Prognostic Based on Gaussian Mixture Model Particle Filter.Acta Aeronautica ET Astronautica Sinica，2009，30(2):319-324.

［6］ Petovello M G，Lachapelle G.Comparison of Vectorbased Software Receiver Implementations with Application to Ultra-tight GPS/INS Integration［C］.ION GNSS 2006.Fort Worth TX，2006:1-10.

［7］ Rankin J.An Errormodel for Sensor Simulation GPS and Differential GPS［C］.IEEE Position Location and Navigation Symposium，1994:260-266.

［8］ Sumit Roy，Ronald A.Iltis Decentralized Linear Estimation in Correlated Measurement Noise［J］.IEEE Transaction on Aerospace and Electronic Systems，1999，27(6):939-941.

［9］ 葛哲學，楊擁民.非高斯噪聲下基于Unscented粒子濾波器的非線性系統(tǒng)故障診斷方法［J］.兵工學報，2007，28(3):332-335.(GE Zhemin，YANG Yongmin.Unscented Particle Filter-based Fault Diagnosis of Nonlinear System with Non-Gaussian Noises［J］.Actaarmamentarll，2007，28(3):332-335.)

［10］ 于飛，唐小勇，潘洪悅.改進粒子群算法在三維水下導航規(guī)劃中的應用［J］.北京理工大學學報，2010，30(9):1059-1064.(YU Fei，TANG Xiaoyong，PAN Hongyue.The Application of an Improved PSO to the Submersible Path-Planning［J］.Transactions of Beijing Institute of Technology，2010，30(9):1059-1064.)

［11］ Thierry Chateau.M2SIR:A Multi Modal Sequential Importance Resampling Algorithm for Particle Filters［C］.IEEE ICIP2009:4073-4076.

［12］ Wataru Sawabe.Application of Particle Filter to Autonomous Navigation System for Outdoor Environment［C］.SICE Annual Conference 2008:93-96.