卡爾曼濾波在MEMS慣性姿態(tài)測(cè)量中的應(yīng)用

張彤　孫玉國(guó)

摘要：由于測(cè)控成本和有效載荷的限制，一般采用微機(jī)電系統(tǒng)（MEMS）慣性傳感器來測(cè)量小型無人機(jī)的飛行姿態(tài)。在MC9S12XS128單片機(jī)上通過嵌入式軟件編程實(shí)現(xiàn)了卡爾曼濾波算法，并在JZJ-1型自準(zhǔn)直儀轉(zhuǎn)臺(tái)上對(duì)MEMS加速度計(jì)和陀螺儀的輸出信號(hào)進(jìn)行了數(shù)據(jù)融合試驗(yàn)，較好地解決了MEMS慣性測(cè)量系統(tǒng)的零漂和機(jī)械振動(dòng)干擾問題。

關(guān)鍵詞：姿態(tài)測(cè)量； 微機(jī)電系統(tǒng)； 慣性傳感器； 卡爾曼濾波

中圖分類號(hào)： TP 275 文獻(xiàn)標(biāo)志碼： A doi： 10.3969/j.issn.1005-5630.2015.01.007

Abstract：Micro-electronic mechanical system （MEMS） inertial sensors are usually used to measure the attitude of the small unmanned aerial vehicle （UAV）， in order to reduce the controlling cost and save the effective payload. In this paper， the Kalman filtering algorithm was implemented on MCU MC9S12XS128 by embedded software. The Data fusion of the MEMS accelerometer and gyroscope output signals was executed by Kalman filter. The experimental results show that the zero drift and mechanical vibration interference problem of the MEMS inertial sensors could be reduced dramatically by the Kalman filter.

Keywords：attitude determination； MEMS； inertial sensor； Kalman filter

引 言

卡爾曼濾波器是Kalman于1960年提出的一種應(yīng)用于離散線性濾波的迭代算法[1]，其目的是盡可能地減少測(cè)量噪聲的影響，并從含有噪聲的測(cè)量值中得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。在小型無人機(jī)自動(dòng)駕駛儀的設(shè)計(jì)中，受有效載荷重量和安裝體積的限制，業(yè)界多采用微機(jī)電系統(tǒng)（MEMS）慣性測(cè)量單元來獲取無人機(jī)的飛行姿態(tài)參數(shù)[2-4]。同時(shí)，受MEMS傳感器制造工藝的限制，由MEMS加速度計(jì)和陀螺儀組合而成的慣性測(cè)量系統(tǒng)不可避免地存在零漂誤差，且測(cè)試數(shù)據(jù)極易受到機(jī)體振動(dòng)的干擾。為此，本文著重對(duì)小型四軸飛行器縱搖和橫搖姿態(tài)（分別對(duì)應(yīng)俯仰角和橫滾角）測(cè)量進(jìn)行研究。

1 卡爾曼濾波器的構(gòu)造

2 卡爾曼濾波實(shí)驗(yàn)與結(jié)果分析

如圖1所示，采用三軸MEMS陀螺儀L3G4200D，三軸MEMS加速度計(jì)MMA8451Q和16位Freescale單片機(jī)MC9S12XS128構(gòu)成慣性姿態(tài)測(cè)量單元。其中，加速度計(jì)和陀螺儀通過IIC總線與單片機(jī)GPIO口的E2、E3引腳相連，經(jīng)卡爾曼濾波后的數(shù)據(jù)通過Tx、Rx引腳發(fā)送到PC機(jī)。

軟件流程圖如圖2所示，流程包括傳感器初始化、IIC數(shù)據(jù)采集、卡爾曼數(shù)據(jù)融合和串口數(shù)據(jù)發(fā)送等4個(gè)主要步驟。

首先將JZJ-1轉(zhuǎn)臺(tái)的俯仰角保持在22°附近，采樣時(shí)間為50 s左右。在此期間用橡皮錘對(duì)轉(zhuǎn)臺(tái)底座施加機(jī)械沖擊載荷，測(cè)試曲線如圖3所示。由圖3可見，MEMS陀螺儀輸出的俯仰角度值隨著時(shí)間的推移明顯偏離實(shí)際值，這是典型的零漂現(xiàn)象。在機(jī)械沖擊載荷作用的時(shí)刻，由加速度值換算得到的角度值會(huì)明顯偏離實(shí)際值。而經(jīng)卡爾曼濾波后輸出的角度值零漂現(xiàn)象不明顯，且對(duì)外界沖擊載荷的抗干擾能力較強(qiáng)。

當(dāng)轉(zhuǎn)臺(tái)從-5°附近開始產(chǎn)生動(dòng)態(tài)俯仰運(yùn)動(dòng)，在測(cè)試過程中對(duì)底座施加1次錘擊，測(cè)試曲線如圖4所示。由圖可見，在動(dòng)態(tài)工況下，卡爾曼濾波后的動(dòng)態(tài)角度曲線比單純依靠加速度計(jì)測(cè)得的角度曲線更平滑且無偏移。此時(shí)，卡爾曼濾波器同樣對(duì)機(jī)械沖擊載荷有較好的抑制作用。

3 結(jié) 論

本文利用卡爾曼濾波器對(duì)MEMS陀螺儀和角速度計(jì)的輸出信號(hào)進(jìn)行了數(shù)據(jù)融合，較好地解決了零漂和機(jī)械振動(dòng)干擾問題。其基本原理是利用MEMS陀螺儀的信號(hào)作為預(yù)測(cè)值，利用加速度計(jì)輸出信號(hào)對(duì)其校正。所設(shè)計(jì)的卡爾曼濾波器可運(yùn)行在16位飛思卡爾單片機(jī)上，受單片機(jī)浮點(diǎn)運(yùn)算能力的限制，采樣時(shí)間間隔設(shè)置為0.1 s。如何在32位ARM芯片上實(shí)現(xiàn)多通道卡爾曼濾波，并提高采樣頻率將是今后研究工作的重點(diǎn)。

參考文獻(xiàn)：

[1] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社.1998：110-135.

[2] 付勇杰，丁艷紅，梁義維，等.動(dòng)態(tài)傾角傳感器及其傳遞特性的研究[J].儀表技術(shù)與傳感器，2012（9）：6-8.

[3] 周升良，孫玉國(guó)，任強(qiáng).基于MEMS的搖擺振動(dòng)測(cè)試方法研究[J].信息技術(shù)，2014（1）：15-16.

[4] 任強(qiáng)，周升良，孫玉國(guó).基于MEMS的角位置無線隨動(dòng)控制系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng)，2013，32（9）：122-124.

（編輯：劉鐵英）