粒子濾波在MEMS陀螺儀初始對(duì)準(zhǔn)中的應(yīng)用*

崔 銘

(中國(guó)民航大學(xué)智能信號(hào)與圖像處理天津市重點(diǎn)實(shí)驗(yàn)室，天津 300300)

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

因而，對(duì)陀螺儀輸出數(shù)據(jù)建模后，常采用卡爾曼濾波技術(shù)［4－5］?？柭鼮V波是一種實(shí)用的線性最小方差估計(jì)算法，適用于非平穩(wěn)過(guò)程，并與遞推最小二乘估計(jì)一樣，算法采用遞推，從量測(cè)信息中提取被估計(jì)量存在估計(jì)值中。理想情況下，卡爾曼濾波能得到較好的濾波效果，但在強(qiáng)非線性和非高斯環(huán)境下采用卡爾曼濾波會(huì)帶來(lái)較大誤差，跟蹤性能較差，甚至?xí)霈F(xiàn)濾波發(fā)散。

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

1 粒子濾波

1.1 粒子濾波基本理論

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

和更新

兩部分。

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

根據(jù)重要度抽樣選擇權(quán)重，

根據(jù)狀態(tài)模型知

式中為Qk－1系統(tǒng)噪聲的協(xié)方差矩陣。

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

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

1.2 算法描述流程圖

2 系統(tǒng)狀態(tài)方程的建立

本文采用機(jī)動(dòng)目標(biāo)Singer模型［12］來(lái)描述系統(tǒng)狀態(tài)，相關(guān)理論在參考文獻(xiàn)［12］中有詳細(xì)的論述。設(shè)采樣周期為T(mén)，通過(guò)典型的離散處理方法，我們可以得到Singer模型的離散狀態(tài)方程:

其中

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

其中

觀測(cè)方程為

其中當(dāng)僅有含噪聲的目標(biāo)位置數(shù)據(jù)可觀測(cè)時(shí)，有Hk=［1 0 0］。Vk是均值為零，方差為Rk的高斯觀測(cè)噪聲。

Singer模型量測(cè)量通常是機(jī)動(dòng)目標(biāo)的位置，濾波估計(jì)量是目標(biāo)的位置、速度和加速度。當(dāng)把Singer模型用于對(duì)MEMS陀螺儀輸出數(shù)據(jù)的濾波時(shí)，量測(cè)量則是運(yùn)動(dòng)角速率，濾波估計(jì)量也是運(yùn)動(dòng)角速率。因?yàn)樵趯?duì)MEMS陀螺儀輸出數(shù)據(jù)的濾波過(guò)程中對(duì)角速率積分的結(jié)果角度值進(jìn)行估計(jì)沒(méi)有意義，所以可將三維狀態(tài)方程簡(jiǎn)化為只有角速率和角加速度的二維狀態(tài)方程，量測(cè)方程的量測(cè)矩陣由Hk=［1 0 0］變?yōu)?Hk=［1 0］。維數(shù)的降低必然會(huì)使計(jì)算量、計(jì)算時(shí)間減少，更適于實(shí)時(shí)濾波需要。

3 應(yīng)用實(shí)例及結(jié)果分析

為了比較標(biāo)準(zhǔn)卡爾曼濾波和粒子濾波的濾波效果，我們分別用標(biāo)準(zhǔn)卡爾曼濾波算法和粒子濾波算法對(duì)MEMS陀螺儀輸出的數(shù)據(jù)進(jìn)行濾波處理。

圖1所示是一組實(shí)測(cè)的GPS/SINS組合導(dǎo)航系統(tǒng)處于初始對(duì)準(zhǔn)狀態(tài)時(shí)X軸MEMS陀螺儀輸出的5 000點(diǎn)數(shù)據(jù)，采樣頻率為100 Hz。

對(duì)MEMS陀螺儀輸出數(shù)據(jù)進(jìn)行濾波前的預(yù)處理的，提取出隨機(jī)常數(shù)和趨勢(shì)項(xiàng)后數(shù)據(jù)如圖2所示。

圖1 陀螺X軸初始對(duì)準(zhǔn)狀態(tài)輸出數(shù)據(jù)

圖2 提取出隨機(jī)常數(shù)和趨勢(shì)項(xiàng)后陀螺X軸數(shù)據(jù)

經(jīng)統(tǒng)計(jì)分析發(fā)現(xiàn)，在SINS進(jìn)行初始對(duì)準(zhǔn)時(shí)，陀螺儀輸出的數(shù)據(jù)是在常數(shù)的基礎(chǔ)上疊加了噪聲成分。在這種情況下，機(jī)動(dòng)頻率是很小的正數(shù)，角加速度絕對(duì)值的最大值也不會(huì)很大?；谶@樣的事實(shí)，設(shè)定機(jī)動(dòng)頻率 α=0.001，最大正角加速度可設(shè)為1.5 d/s2，絕對(duì)值最大的負(fù)角加速度可設(shè)為－1.5 d/s2。

3.1 用標(biāo)準(zhǔn)卡爾曼濾波對(duì)輸出數(shù)據(jù)的濾波實(shí)現(xiàn)

將圖2所示數(shù)據(jù)直接運(yùn)用標(biāo)準(zhǔn)卡爾曼濾波算法進(jìn)行濾波，效果如圖3所示。

圖3 卡爾曼濾波前與濾波后的數(shù)據(jù)

3.2 用粒子濾波對(duì)輸出數(shù)據(jù)的濾波實(shí)現(xiàn)

將圖2所示數(shù)據(jù)直接運(yùn)用粒子濾波算法進(jìn)行濾波，效果如圖4所示。

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

圖4 粒子濾波前與濾波后的數(shù)據(jù)

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

3.3 用粒子濾波對(duì)動(dòng)態(tài)數(shù)據(jù)的濾波實(shí)現(xiàn)

當(dāng)機(jī)體處于運(yùn)動(dòng)狀態(tài)時(shí)，角加速度的絕對(duì)值的最大值通常是比較大的正數(shù)。為了研究粒子濾波性能，對(duì)圖5所示的實(shí)測(cè)MEMS陀螺儀輸出的典型動(dòng)態(tài)數(shù)據(jù)進(jìn)行濾波。

圖5 實(shí)測(cè)MEMS陀螺儀輸出的典型動(dòng)態(tài)數(shù)據(jù)

根據(jù)實(shí)際中機(jī)體的角運(yùn)動(dòng)情況，機(jī)動(dòng)狀態(tài)時(shí)，機(jī)動(dòng)頻率相對(duì)于機(jī)體靜止時(shí)的機(jī)動(dòng)頻率要大一些，考慮到這些因素的影響，設(shè)定機(jī)動(dòng)頻率α=0.01，正負(fù)最大角加速度分別設(shè)定為10 d/s2和－10 d/s2。根據(jù)這些設(shè)定，粒子濾波效果的局部放大圖如圖6所示。

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

從圖6可以看出，采用粒子濾波算法對(duì)MEMS陀螺儀輸出的動(dòng)態(tài)數(shù)據(jù)進(jìn)行濾波，可以獲得較為理想的濾波效果。應(yīng)該注意的是，實(shí)際中加速度絕對(duì)值的最大值通常是未知的，機(jī)動(dòng)頻率通常也難以準(zhǔn)確確定，這就需要根據(jù)機(jī)體的具體運(yùn)動(dòng)情況根據(jù)經(jīng)驗(yàn)進(jìn)行確定，從而制約該算法的通用性。但如果我們能夠?qū)δ撤N機(jī)動(dòng)的最大加速度和機(jī)動(dòng)頻率有適當(dāng)?shù)牧私?，運(yùn)用Singer模型和粒子濾波算法能夠獲得比較理想的濾波效果。

4 結(jié)論

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

［1］祝燕華，蔡體菁，楊卓鵬.MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的實(shí)現(xiàn)［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17(5):552－556.

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

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

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

［5］付夢(mèng)印，鄧志紅，張繼偉.kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京:科學(xué)出版社，2003，65－104.

［6］田世君，陳俊，皮亦鳴.粒子濾波在高動(dòng)態(tài)GPS定位中的應(yīng)用［J］.測(cè)繪學(xué)報(bào)，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］程水英，張劍云.粒子濾波評(píng)述［J］.宇航學(xué)報(bào)，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］周宏仁，敬忠良，王培德.機(jī)動(dòng)目標(biāo)跟蹤［M］.北京:國(guó)防工業(yè)出版社，1991，109－153.