粒子濾波在MEMS陀螺儀初始對準中的應用*

崔 銘

(中國民航大學智能信號與圖像處理天津市重點實驗室，天津 300300)

MEMS(Micro-Electro-Mechanical-Systems)即微機械電子系統［1－3］，該技術出現于20世紀80年代中后期，是隨著現代微電子與微加工工藝的迅速發展而發展起來的，并受到各國家廣泛的重視。MEMS-IMU(微機電慣性測量單元)主要包括陀螺儀和加速度計，由于體積小、成本低、重量輕和可靠性高等優點，在低成本慣導系統中廣泛應用。但由于理論和制造工藝的不完善，MEMS IMU本身存在著較大的測量誤差，其測量值包含許多因素引起的誤差，需對這些誤差進行測試和建模補償。誤差通常分為確定和隨機兩部分，確定部分可以用代數方程來表示，具有規律性容易補償。目前，國內外對這兩種誤差的補償已經非常成熟。而隨機部分具有不確定性，只能用統計規律來描述，通過濾波的方法來減少。

因而，對陀螺儀輸出數據建模后，常采用卡爾曼濾波技術［4－5］。卡爾曼濾波是一種實用的線性最小方差估計算法，適用于非平穩過程，并與遞推最小二乘估計一樣，算法采用遞推，從量測信息中提取被估計量存在估計值中。理想情況下，卡爾曼濾波能得到較好的濾波效果，但在強非線性和非高斯環境下采用卡爾曼濾波會帶來較大誤差，跟蹤性能較差，甚至會出現濾波發散。

粒子濾波［6－9］是一種基于貝葉斯估計的濾波方法。其基本思想:首先依據系統狀態向量的經驗條件分布，在狀態空間產生一組隨機樣本，這些樣本稱為粒子，然后根據觀測量，不斷地調整粒子的權重和位置，通過調整后的粒子的信息，修正最初的經驗條件分布。當粒子數目足夠多時，修正后的經驗條件分布將收斂于系統狀態向量真實的條件分布。此時，狀態向量的估計值可以通過粒子的均值得到。這種濾波算法采用遞推方式，很方便用計算機實現，并且該算法能較好地適應觀測信息出現異常突變時的情況，具有一定的魯棒性，優于標準卡爾曼濾波算法。文章較詳細地對粒子濾波進行分析，給出MEMS陀螺儀數據處理中粒子濾波具體模型和濾波算法，最后給出應用分析。

1 粒子濾波

1.1 粒子濾波基本理論

粒子濾波算法［10］是一種隨機抽樣方法，它的抽樣計劃不是始終保持不變，而是根據實際結果不斷進行調整，設計出新的抽樣計劃。從所有測量值集合Y1:k={Yi，i=1，2，…，k}中對狀態Xk進行估計，即需要構造后驗密度P(Xk|Y1:k)，由貝葉斯推論可以得到后驗密度的遞歸形式，分為預測

和更新

兩部分。

其中Xk表示狀態向量，δ()為沖激函數。則k時刻位置和速度的MMSE(最小均方誤差)估計可以近似為

根據重要度抽樣選擇權重，

根據狀態模型知

式中為Qk－1系統噪聲的協方差矩陣。

粒子濾波算法在幾次迭代之后，會出現退化問題，即歸一化權值會集中在一個粒子上面，而其他粒子的權值小到可以忽略的程度。J.Liu提出了用有效抽樣Neff來度量退化的程度［11］

Neff越小表明退化現象越嚴重。當Neff低于某個門限(比如2N/3)時，就要進行再抽樣。再抽樣的思想是消除小粒子，復制大粒子。所以我們選擇先驗分布服從正態分布p(X0)～N(u，δ2)，均值u為利用第一個抽樣點的測量信息用幾何交點法得到的結果，方差δ2為一個較大的數。相對于無信息先驗分布，這種選取方法利用了先驗信息，能夠使初始分布較接近真實分布，使得粒子集中在后驗分布附近。

1.2 算法描述流程圖

2 系統狀態方程的建立

本文采用機動目標Singer模型［12］來描述系統狀態，相關理論在參考文獻［12］中有詳細的論述。設采樣周期為T，通過典型的離散處理方法，我們可以得到Singer模型的離散狀態方程:

其中

式中Wk是離散時間白噪聲序列，并且

其中

觀測方程為

其中當僅有含噪聲的目標位置數據可觀測時，有Hk=［1 0 0］。Vk是均值為零，方差為Rk的高斯觀測噪聲。

Singer模型量測量通常是機動目標的位置，濾波估計量是目標的位置、速度和加速度。當把Singer模型用于對MEMS陀螺儀輸出數據的濾波時，量測量則是運動角速率，濾波估計量也是運動角速率。因為在對MEMS陀螺儀輸出數據的濾波過程中對角速率積分的結果角度值進行估計沒有意義，所以可將三維狀態方程簡化為只有角速率和角加速度的二維狀態方程，量測方程的量測矩陣由Hk=［1 0 0］變為 Hk=［1 0］。維數的降低必然會使計算量、計算時間減少，更適于實時濾波需要。

3 應用實例及結果分析

為了比較標準卡爾曼濾波和粒子濾波的濾波效果，我們分別用標準卡爾曼濾波算法和粒子濾波算法對MEMS陀螺儀輸出的數據進行濾波處理。

圖1所示是一組實測的GPS/SINS組合導航系統處于初始對準狀態時X軸MEMS陀螺儀輸出的5 000點數據，采樣頻率為100 Hz。

對MEMS陀螺儀輸出數據進行濾波前的預處理的，提取出隨機常數和趨勢項后數據如圖2所示。

圖1 陀螺X軸初始對準狀態輸出數據

圖2 提取出隨機常數和趨勢項后陀螺X軸數據

經統計分析發現，在SINS進行初始對準時，陀螺儀輸出的數據是在常數的基礎上疊加了噪聲成分。在這種情況下，機動頻率是很小的正數，角加速度絕對值的最大值也不會很大。基于這樣的事實，設定機動頻率 α=0.001，最大正角加速度可設為1.5 d/s2，絕對值最大的負角加速度可設為－1.5 d/s2。

3.1 用標準卡爾曼濾波對輸出數據的濾波實現

將圖2所示數據直接運用標準卡爾曼濾波算法進行濾波，效果如圖3所示。

圖3 卡爾曼濾波前與濾波后的數據

3.2 用粒子濾波對輸出數據的濾波實現

將圖2所示數據直接運用粒子濾波算法進行濾波，效果如圖4所示。

本文選取MEMS陀螺儀初始對準狀態時X軸的輸出數據，進行經典卡爾曼濾波與粒子濾波對比實驗。從圖3和圖4的濾波結果可直觀的看出，經典卡爾曼濾波算法和粒子濾波算法都能較好完成濾波任務。進一步對兩種算法濾波前后的數據進行計算分析，如表1所示，由于兩種算法應用同一組MEMS陀螺儀輸出數據，故對兩種濾波前后的方差和均值進行對比，發現兩種算法所得均值相差不大，方差比濾波前減小兩個數量級，而粒子濾波的性能要稍好。

圖4 粒子濾波前與濾波后的數據

表1 卡爾曼濾波與粒子濾波前后的均值與方差對比

3.3 用粒子濾波對動態數據的濾波實現

當機體處于運動狀態時，角加速度的絕對值的最大值通常是比較大的正數。為了研究粒子濾波性能，對圖5所示的實測MEMS陀螺儀輸出的典型動態數據進行濾波。

圖5 實測MEMS陀螺儀輸出的典型動態數據

根據實際中機體的角運動情況，機動狀態時，機動頻率相對于機體靜止時的機動頻率要大一些，考慮到這些因素的影響，設定機動頻率α=0.01，正負最大角加速度分別設定為10 d/s2和－10 d/s2。根據這些設定，粒子濾波效果的局部放大圖如圖6所示。

圖6 粒子濾波效果局部放大圖

從圖6可以看出，采用粒子濾波算法對MEMS陀螺儀輸出的動態數據進行濾波，可以獲得較為理想的濾波效果。應該注意的是，實際中加速度絕對值的最大值通常是未知的，機動頻率通常也難以準確確定，這就需要根據機體的具體運動情況根據經驗進行確定，從而制約該算法的通用性。但如果我們能夠對某種機動的最大加速度和機動頻率有適當的了解，運用Singer模型和粒子濾波算法能夠獲得比較理想的濾波效果。

4 結論

粒子濾波和卡爾曼濾波是兩種重要的數據處理方法。本文中，首先采用粒子濾波和卡爾曼濾波分別對靜態數據進行處理，粒子濾波后得到效果比卡爾曼濾波稍好。而當機體處于運動狀態時，角加速度的絕對值是比較大的正數時，采用粒子濾波算法的同樣得到比較理想的濾波效果。但該算法也有其局限性，在實際當中的應用效果將在日后做進一步研究。

［1］祝燕華，蔡體菁，楊卓鵬.MEMS-IMU/GPS組合導航系統的實現［J］.中國慣性技術學報，2009，17(5):552－556.

［2］吉訓生，王壽榮.硅微陀螺漂移數據濾波方法研究［J］.傳感技術學報，2008，21(2):333－336.

［3］李杰，張文棟，劉俊.基于時間序列分析的Kalman濾波方法在MEMS陀螺儀隨機漂移誤差補償中的應用研究［J］.傳感技術學報，2006，19(5):2215－2219.

［4］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理［M］.西安:西北工業大學出版社，1998，1－5.

［5］付夢印，鄧志紅，張繼偉.kalman濾波理論及其在導航系統中的應用［M］.北京:科學出版社，2003，65－104.

［6］田世君，陳俊，皮亦鳴.粒子濾波在高動態GPS定位中的應用［J］.測繪學報，2007，36(3):274－278.

［7］Pau Closasy，Carles Fern’andez-Prades，Juan A Fern’andez-Rubio.A Particle Filtering Tracking Algorithm for GNSS Synchronization Using Laplace’s Method［C］//IEEE International Conference on Acoustics，Speech and Signal Processing，2008:3409－3412.

［8］Javad Rezaie，Behzad Moshiri，Araabi Babak N，et al.GPS/INS Integration Using Nonlinear Blending Filters［C］//Annual Conference on SICE，2007:1674－1680.

［9］Wang Fasheng，Lin Yuejin.Improving Particle Filter with A New Sampling Strategy［C］//Nanning，China:4th International Conference on Computer Science ＆ Education，2009:408－412.

［10］程水英，張劍云.粒子濾波評述［J］.宇航學報，2008，29(4):1099－1109.

［11］LIU V J，COLAJANNI.Dynamic Load Balancing on Web-server System［J］.IEEE Internet Computing，1999，3(3):28－39.

［12］周宏仁，敬忠良，王培德.機動目標跟蹤［M］.北京:國防工業出版社，1991，109－153.