基于Kalman濾波的MEMS陀螺儀濾波算法

錢華明，夏全喜，闕興濤，張 強(qiáng)

(1.哈爾濱工程大學(xué) 自動化學(xué)院，黑龍江 哈爾濱 150001;2.解放軍信息工程大學(xué) 測繪學(xué)院，河南 鄭州 450002)

微機(jī)電系統(tǒng)(micro-electro-mechanical system，MEMS)慣性器件在低成本慣性系統(tǒng)中獲得越來越廣泛的應(yīng)用，但目前MEMS陀螺儀的精度仍然較低，限制了其進(jìn)一步應(yīng)用.通過有效的降噪方法改善陀螺儀性能，以滿足較高精度的應(yīng)用需求，是目前研究的重要問題.研究表明，隨機(jī)噪聲是影響MEMS陀螺儀精度的主要因素.對其進(jìn)行模型辨識并濾波是提高陀螺儀性能的主要途徑［1］.采用神經(jīng)網(wǎng)絡(luò)、小波分析等方法對陀螺儀的隨機(jī)誤差進(jìn)行建模可以達(dá)到這個目的，但這些方法得到的模型通常具有較高的階數(shù)，并不十分適合于低成本系統(tǒng)的實(shí)時在線估計［2-3］.通過時序分析方法，采用AR模型同樣可以對陀螺隨機(jī)誤差進(jìn)行建模，其使用條件要簡單得多.在正確地建立了陀螺的隨機(jī)誤差模型之后需要選擇恰當(dāng)?shù)姆椒ㄟM(jìn)行濾波，從工程實(shí)用角度來看Kalman濾波是不錯的選擇.這種首先使用AR模型對陀螺儀隨機(jī)誤差進(jìn)行建模，然后設(shè)計Kalman濾波器來濾除噪聲的方法，文獻(xiàn)［4-6］也曾使用，這些文獻(xiàn)都對隨機(jī)誤差進(jìn)行了正確建模，然而在Kalman濾波器的設(shè)計中，建立系統(tǒng)模型時，他們都沒有區(qū)分實(shí)際角速度和噪聲，在靜態(tài)情況下，其濾波器可以有效消除隨機(jī)噪聲，但是在動態(tài)時，濾波器會出現(xiàn)問題.本文考慮了有色噪聲的因素，采用狀態(tài)擴(kuò)增法設(shè)計了Kalman濾波器，為了驗(yàn)證其正確性，進(jìn)行了仿真和試驗(yàn).針對搖擺運(yùn)動時濾波效果下降的問題提出提高采樣頻率和使用自適應(yīng)Kalman濾波2種解決辦法.

1 Kalman濾波器的設(shè)計

對陀螺儀隨機(jī)漂移信號進(jìn)行時間序列分析即對陀螺信號進(jìn)行預(yù)處理后，使其滿足平穩(wěn)隨機(jī)序列，對其建立時間序列模型，并對模型適用性進(jìn)行檢驗(yàn)，以實(shí)現(xiàn)對時間序列的預(yù)報和控制.具體的建模步驟以及方法可以參考文獻(xiàn)［4，7］.本文綜合考慮AIC準(zhǔn)則(Akaike information criterion)、模型適用性和系統(tǒng)實(shí)時性要求，選用AR(1)模型對陀螺儀隨機(jī)噪聲進(jìn)行了建模.

Kalman濾波是工程應(yīng)用中較為成熟的方法.它是一種遞推線性最小方差估計，只通過前一個時刻的估計值和現(xiàn)在的量測值來計算現(xiàn)在狀態(tài)的估計值，方法簡單，易于工程實(shí)現(xiàn).它可以實(shí)現(xiàn)最小均方估計誤差意義下的隨機(jī)信號的最優(yōu)線性濾波.建立了陀螺儀的隨機(jī)漂移模型以后，可以采用Kalman濾波方法濾除陀螺儀隨機(jī)漂移.此處系統(tǒng)噪聲為有色噪聲，量測噪聲為白噪聲.在Kalman濾波方程中，系統(tǒng)驅(qū)動噪聲和量測噪聲都必須是白噪聲，為了使用Kalman濾波器對數(shù)據(jù)進(jìn)行處理，必須對濾波器模型進(jìn)行改進(jìn)［8］.

系統(tǒng)噪聲為有色噪聲而量測噪聲為白色時的Kalman濾波，一般可以采用狀態(tài)擴(kuò)增法.以離散系統(tǒng)為例來說明.

設(shè)系統(tǒng)方程和量測方程分別為

式中，觀測噪聲Vk是零均值白噪聲序列，系統(tǒng)噪聲wk-1為有色噪聲，滿足方程:

式中:ξk為零均值白噪聲序列.

采用狀態(tài)擴(kuò)增法進(jìn)行Kalman濾波方程的推導(dǎo).將wk也列為狀態(tài)，則擴(kuò)增后的狀態(tài)為

擴(kuò)增狀態(tài)后的系統(tǒng)方程和觀測方程為

式中:Vk是零均值白噪聲序列，符合Kalman基本濾波方程的要求，可以按照相關(guān)推導(dǎo)方法推導(dǎo)相應(yīng)的濾波方程.

2 濾波器性能的檢驗(yàn)

為了驗(yàn)證誤差模型的準(zhǔn)確性和濾波器的有效性，針對ADIS16350慣性測量組件進(jìn)行了試驗(yàn)，該組件是一個包含有三軸陀螺儀和三軸加速度計慣性傳感器，其零偏不穩(wěn)定性分別是陀螺儀為0.015(°)/s，加速度計為0.7mg，測量噪聲是 0.60(°)/s 和 35 mg.此處只對其中一個軸向上的陀螺儀進(jìn)行分析研究.

2.1 靜態(tài)試驗(yàn)

把慣導(dǎo)組件平放在地面上，保持靜止，以100 Hz采樣頻率采集并保存1 min數(shù)據(jù)，然后進(jìn)行離線分析.

由于陀螺儀的分辨率較低，不能敏感到地球自轉(zhuǎn)角速度，此處可以把陀螺儀所采集的數(shù)據(jù)看作是誤差，對其進(jìn)行預(yù)處理然后進(jìn)行建模.利用滑動平均法剔除陀螺儀的野值，采用逆序法進(jìn)行平穩(wěn)性檢驗(yàn)，選取顯著性水平為0.05，則|u|表明數(shù)據(jù)平穩(wěn)，由采樣數(shù)據(jù)算得u為0.110 4，表明數(shù)據(jù)無趨勢項，滿足平穩(wěn)性要求;根據(jù)AIC準(zhǔn)則對模型階數(shù)進(jìn)行選擇，綜合考慮硬件實(shí)現(xiàn)的方便，此處選擇AR(1)模型;利用Matlab中“aryule”函數(shù)計算模型參數(shù)得 φ=0.107;最后，進(jìn)行模型適用性檢驗(yàn)，此處通過計算殘差εn的自相關(guān)函數(shù)和xn與εn的互相關(guān)函數(shù)來完成.經(jīng)計算，結(jié)果分別為0.052和 -0.085，滿足為小量的要求，說明殘差信號為白噪聲，即證明所構(gòu)建的AR(1)模型符合系統(tǒng)要求.

如圖1所示，濾波后誤差的均值和標(biāo)準(zhǔn)差分別由濾波前的 0.635 0(°)/s 和 -0.313 1(°)/s降低到0.086 9(°)/s和 -0.196 5(°)/s.結(jié)果顯示，在靜態(tài)條件下，Kalman濾波器可以大幅降低陀螺儀的隨機(jī)噪聲，對提高陀螺儀性能有顯著作用.

圖1 靜態(tài)試驗(yàn)結(jié)果Fig.1 Result of static test

2.2 速率試驗(yàn)

由于缺少轉(zhuǎn)臺等試驗(yàn)設(shè)備，此處進(jìn)行速率試驗(yàn)的仿真.在所采集的靜態(tài)數(shù)據(jù)的基礎(chǔ)上疊加一個恒定的角速率來模擬速率試驗(yàn).角速率值設(shè)定為2、5、10、100(°)/s.和靜態(tài)數(shù)據(jù)一樣，首先進(jìn)行AR建模，然后使用Kalman濾波器濾波.如圖2所示，濾波前后誤差的均值和標(biāo)準(zhǔn)差數(shù)值與靜止?fàn)顟B(tài)相同，說明在恒定角速率下，該濾波方法仍然適用.

圖2 速率試驗(yàn)結(jié)果(輸入角速率為10(°)/s)Fig.2 Result of rate test(input angular rate is 10(°)/s)

2.3 搖擺試驗(yàn)

在所采集的靜態(tài)數(shù)據(jù)基礎(chǔ)上疊加一個正弦信號來模擬搖擺試驗(yàn)，分別以 5°、15°、50°、150°為振幅，10 s為周期進(jìn)行仿真，所得結(jié)果見表1和表2.圖3為振幅為50°時濾波前后的曲線圖.

表1 搖擺試驗(yàn)角速率誤差均值(100 Hz)Table 1 mean value of angular rate in the oscillating test(100 Hz)

表2 搖擺試驗(yàn)角速率誤差標(biāo)準(zhǔn)差(100 Hz)Table 2 Standard deviation of angular rate in the oscillating test(100 Hz)

圖3 搖擺試驗(yàn)結(jié)果(振幅為50°，采樣頻率為100 Hz)Fig.3 Result of oscillating test(amplitude is 50 °，sampling frequency is 100 Hz)

由表1、2中數(shù)據(jù)和圖3可見，隨著振幅的增大，誤差均值和標(biāo)準(zhǔn)差逐漸增大，濾波器不再適用.其原因在于:在設(shè)計Kalman濾波器時，系統(tǒng)狀態(tài)方程中Φk，k-1選為單位陣，這就隱含著默認(rèn)角速率值在一個采樣周期內(nèi)沒有明顯變化，而搖擺環(huán)境下，這個假設(shè)不再成立.Kalman濾波雖然只通過前一個時刻的估計值和現(xiàn)在的量測值來計算現(xiàn)在狀態(tài)的估計值，而因?yàn)樯弦粋€時刻的估計是利用上一時刻和以前的量測得到的，所以，實(shí)際上是利用前一時刻和以前的量測得到的.若系統(tǒng)做搖擺運(yùn)動，各個時刻的真實(shí)角速率值不同，仍然用此模型會產(chǎn)生較大的誤差，這也可以解釋在恒定角速率轉(zhuǎn)動和靜態(tài)時濾波效果相同.

為了解決此問題，首先想到的辦法是提高采樣頻率，此處仿真1 000 Hz采樣頻率下的濾波效果.所得結(jié)果見表3、4.圖4所示為振幅為50°，搖擺周期為10 s，采樣頻率為1 000 Hz條件下仿真所得濾波效果曲線.

對比表1～4，可見提高采樣頻率后，誤差的均值和標(biāo)準(zhǔn)差都有了顯著下降，尤其是標(biāo)準(zhǔn)差下降最為明顯，在任何振幅下都穩(wěn)定在一個較低的水平.說明前面分析正確而且此方法有效.

表3 搖擺試驗(yàn)角速率誤差均值(1 000 Hz)Table 3 mean value of angular rate in the oscillating test(1 000 Hz)

表4 搖擺試驗(yàn)角速率誤差標(biāo)準(zhǔn)差(1 000 Hz)Table 4 Standard deviation of angular rate in the oscillating test(1 000 Hz)

圖4 搖擺試驗(yàn)結(jié)果(振幅為50°，采樣頻率為1 000 Hz)Fig.4 Result of oscillating test(amplitude is 50 °，sampling frequency is 1 000 Hz)

3 自適應(yīng)Kalman濾波

2.3節(jié)證明通過提高采樣頻率可以有效改善濾波器效果，然而采樣頻率的提高對系統(tǒng)硬件及處理器計算能力提出了較高的要求，在現(xiàn)有條件下并不完全適用.

由前文可知，搖擺狀態(tài)下濾波效果下降的原因是上一時刻狀態(tài)的預(yù)測在狀態(tài)估計中占有的比重太大.本文進(jìn)一步提出利用漸消因子自適應(yīng)Kalman濾波來進(jìn)行改進(jìn)，通過減小以往估計值在估計值中的比重來提高濾波效果.

自適應(yīng)Kalman濾波方程描述如下［9］:

漸消因子α由先驗(yàn)知識來確定，此處通過改變α的大小進(jìn)行仿真觀察濾波效果來選擇α的值.選擇 α 為0.9、0.8、0.7 和0.6 來對比濾波前后誤差的均值和標(biāo)準(zhǔn)差，仿真結(jié)果見表5和6.

表5 搖擺試驗(yàn)角速率誤差均值(自適應(yīng)Kalman濾波100 Hz)Table 5 Mean value of angular rate in the oscillating test(adaptive Kalman filter 100 Hz)ALPHA

表6 搖擺試驗(yàn)角速率誤差標(biāo)準(zhǔn)差(自適應(yīng)Kalman濾波100 Hz)Table 6 Standard deviation of angular rate in the oscillating test(adaptive Kalman filter 100 Hz)ALPHA

由表中數(shù)據(jù)可見隨著α值的減小，誤差標(biāo)準(zhǔn)差逐漸趨于穩(wěn)定，而其值逐漸增大，在均值方面，小振幅時α=0.7處有極小值，隨后增大，大振幅時誤差均值隨著α的減小而減小.綜合在小振幅和大振幅情況下誤差均值和標(biāo)準(zhǔn)差的濾波效果，此處選擇α=0.7作為固定的漸消因子.圖5所示為α=0.7時濾波前后數(shù)據(jù)和濾波后誤差曲線.

圖5 搖擺試驗(yàn)結(jié)果(振幅為50°，采樣頻率為100 Hz，α =0.7)Fig.5 Oscillating test(amplitude is50°，sampling freqnency is 100 Hz，α =0.7)

4 結(jié)束語

本文對MEMS陀螺儀的隨機(jī)誤差特性進(jìn)行研究，設(shè)計Kalman濾波器進(jìn)行濾波，通過對ADIS16350的試驗(yàn)和仿真，以誤差均值和標(biāo)準(zhǔn)差為衡量指標(biāo)，驗(yàn)證了在靜態(tài)和恒定角速率狀態(tài)下，該方法可以有效提高陀螺儀精度.然而在搖擺情況下，濾波效果會隨著振幅的增大而逐漸降低.通過分析原因，進(jìn)一步提出了2種改進(jìn)方案:提高采樣頻率和使用自適應(yīng)Kalman濾波.仿真結(jié)果表明，2種方法都可以有效提高濾波效果，但是考慮到系統(tǒng)硬件實(shí)現(xiàn)中處理器計算速度和采樣頻率的限制，提高采樣頻率法并不十分可取，而自適應(yīng)Kalman濾波有更高的實(shí)用價值.

［1］REN Yafei，KE Xizheng，LIU Yijie.MEMSgyroscope performance estimate based on Allan［C］//The Eighth International Conference on Electronic Measurement and Instruments(ICEMI2008).Xi＇an，China，2008:1326-1330.

［2］CHEN Xiyuan.Modeling randomgyro driftby time series neural Networks and by traditionalmethod［C］//IEEE Int Conf Neural Networks ＆ Signal Processing.Nanjing，China，2003:810-813.

［3］袁廣民，李曉瑩，常洪龍，等.MEMS陀螺隨機(jī)誤差補(bǔ)償在提高姿態(tài)參照系統(tǒng)精度中的應(yīng)用［J］.西北工業(yè)大學(xué)學(xué)報，2008，26(6):777-781.YUAN Guangmin，LI Xiaoying，CHANG Honglong，et al.Compensating for randomnoises in a low-precision MEMS gyroscope for improving accuracy of attitude reference system［J］.Journal of Northwestern Polytechnical University，2008，26(6):777-781.

［4］吉訓(xùn)生，王壽榮.MEMS陀螺儀隨機(jī)漂移誤差研究［J］.宇航學(xué)報，2006，27(4):640-642.JI Xunsheng，WANG Shourong.The Research on randomdrift of MEMS gyroscope［J］.Journal of Astronautics，2006，27(4):640-642.

［5］ZHANG Hua，KE Xizheng，JIAO Rong.Experimental research on feedback Kalman model of MEMS gyroscope［C］//The Eighth Int Conf on Electronic Measurement and Instruments.Xi'an，China，2007:253-256.

［6］ZHANG Yunong，LEITHEADwE，LEITH D J.Time-series Gaussian process regression based on computation of O(N2)operations and O(N)-level storage［C］//Proceedings of the 44th IEEE Conference on Decision and Control.Seville，Spain，2005:987-991.

［7］楊叔子，吳雅.時間序列分析的工程應(yīng)用［M］.武漢:華中理工大學(xué)出版社，2007:87-93.YANG Shuzi，WU Ya.Time series analysis in engineering application［M］.Wuhan:Huazhong University Press，2007:87-93.

［8］付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在慣性導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京:科學(xué)出版社，2003:65-127.FU Mengyin，DENG Zhihong，ZHANG Jiwei.The theory of Kalman filter and the application of inertial navigation［M］.Beijing:Science Press，2003:65-127.

［9］JIXunsheng，WANG Shourong，XU Yishen，et al.Application of the digital signal procession in the MEMS gyroscope de-drift［C］//Proceedings of the 1st IEEE International Conference on Nano/Micro Engineered and Molecular Systems.Zhuhai，China，2006:218-221.

［10］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安:西北工業(yè)大學(xué)出版社，1998:33-41.QIN Yongyuan，ZHANG Hongyue，Wang Shuhua.Principle of Kalman filtering and integrate navigation ［M］.Xi＇an:Northwestern Polytechnical University Press，1998:33-41.

［11］SEONG Y C，BYUNGD K.Adaptive IIR/FIR fusion filter and its application to the INS/GPS integrated system［J］.Automatica，2008，44(8):2040-2047.

［12］耿延睿，崔中興.組合導(dǎo)航系統(tǒng)卡爾曼濾波衰減因子自適應(yīng)估計算法研究［J］.中國慣性技術(shù)學(xué)，2001，9(4):8-10.GENG Yanrui，CUI Zhongxing.Adaptive estimation of Kalman filter fading factor in integrated navigation system［J］.Journal of Chinese Inertial Technology，2001，9(4):8-10.

［13］熊海林，鄧方林.陀螺靜態(tài)漂移系數(shù)的兩種估計方法［J］.上海航天，2002，1:39-42.XIONG Hailin，DENG Fanglin.Two estimation methods of gyroscope static drift coefficients［J］.Aerospace Shanghai，2002，1:39-42.