MPU9250傳感器的姿態(tài)檢測(cè)與數(shù)據(jù)融合

劉春陽(yáng)，徐軍領(lǐng)，程洪濤，王東方，薛玉君

(河南科技大學(xué) 機(jī)電工程學(xué)院，河南 洛陽(yáng) 471003)

?

MPU9250傳感器的姿態(tài)檢測(cè)與數(shù)據(jù)融合

劉春陽(yáng)，徐軍領(lǐng)，程洪濤，王東方，薛玉君

(河南科技大學(xué) 機(jī)電工程學(xué)院，河南 洛陽(yáng) 471003)

設(shè)計(jì)了基于MPU9250多軸姿態(tài)傳感器和MSP430F149單片機(jī)的姿態(tài)檢測(cè)系統(tǒng)。利用傳感器內(nèi)部的陀螺儀、加速度計(jì)和電子羅盤(pán)，可以對(duì)3個(gè)軸的角速度、加速度、磁感應(yīng)強(qiáng)度進(jìn)行測(cè)量，進(jìn)而解算成角度姿態(tài)。利用優(yōu)化的卡爾曼濾波算法對(duì)解算的姿態(tài)角度進(jìn)行了融合處理，融合后的數(shù)據(jù)有效地抑制了噪聲，提高了角度姿態(tài)檢測(cè)的準(zhǔn)確性。設(shè)計(jì)的姿態(tài)檢測(cè)系統(tǒng)每秒鐘可以完成100次姿態(tài)檢測(cè)與計(jì)算，具有體積小、響應(yīng)快等特點(diǎn)，并應(yīng)用于自平衡小車(chē)的姿態(tài)檢測(cè)。

姿態(tài)檢測(cè)；卡爾曼濾波；陀螺儀；加速度計(jì)；電子羅盤(pán)

0 引言

隨著無(wú)人飛行器、虛擬現(xiàn)實(shí)裝備、可穿戴裝備等技術(shù)的發(fā)展，物體姿態(tài)檢測(cè)成為獲取姿態(tài)信息、實(shí)現(xiàn)精確運(yùn)動(dòng)控制的必要手段和前提條件。機(jī)械陀螺儀及光纖陀螺儀等高精度慣性導(dǎo)航傳感器件價(jià)格昂貴[1]、體積較大，因此，在應(yīng)用中大多利用微機(jī)電系統(tǒng)(MEMS)技術(shù)制造的陀螺儀、加速度計(jì)和電子羅盤(pán)等傳感器進(jìn)行姿態(tài)檢測(cè)[2-3]。而單一傳感器的姿態(tài)檢測(cè)，容易因慣性器件的漂移和累計(jì)誤差，導(dǎo)致測(cè)量精度的下降[4]。因此，通過(guò)多個(gè)傳感器進(jìn)行姿態(tài)檢測(cè)，并借助數(shù)據(jù)融合算法提高姿態(tài)檢測(cè)精度成為目前研究的熱點(diǎn)[5]。

利用獨(dú)立的陀螺儀、加速度計(jì)等多個(gè)傳感器組合進(jìn)行姿態(tài)檢測(cè)，存在多個(gè)芯片占用空間大、姿態(tài)數(shù)據(jù)存在軸間誤差、數(shù)據(jù)融合精度欠佳等不足[6-7]。因此，本文基于9軸姿態(tài)檢測(cè)傳感器MPU9250(內(nèi)部集成陀螺儀、加速度計(jì)、電子羅盤(pán))，設(shè)計(jì)了以MSP430單片機(jī)為處理核心的姿態(tài)檢測(cè)系統(tǒng)，由陀螺儀測(cè)量物體運(yùn)動(dòng)的角速度，進(jìn)而解算成角度。利用加速度計(jì)和電子羅盤(pán)分別測(cè)量加速度和地磁場(chǎng)這兩個(gè)不相關(guān)的三維矢量，作為物體姿態(tài)的觀測(cè)矢量來(lái)校準(zhǔn)陀螺儀的測(cè)量值。在數(shù)據(jù)融合方面，考慮到互補(bǔ)濾波在動(dòng)態(tài)響應(yīng)方面的不足，采用了卡爾曼濾波算法進(jìn)行角度姿態(tài)估計(jì)，有效抑制了噪聲，并提高了姿態(tài)角檢測(cè)的準(zhǔn)確性。

1 系統(tǒng)設(shè)計(jì)

設(shè)計(jì)的姿態(tài)檢測(cè)系統(tǒng)以MSP430F149單片機(jī)為核心，通過(guò)姿態(tài)傳感器MPU9250對(duì)運(yùn)動(dòng)姿態(tài)進(jìn)行檢測(cè)，以提供航向角、俯仰角和翻滾角等姿態(tài)信息。處理后的姿態(tài)角度信息可以顯示在液晶屏上，也可以通過(guò)無(wú)線(xiàn)數(shù)據(jù)傳輸模塊發(fā)送給遠(yuǎn)端的主機(jī)，并在上位機(jī)軟件上完成數(shù)據(jù)的顯示和運(yùn)動(dòng)曲線(xiàn)的繪制。此外，系統(tǒng)還設(shè)計(jì)了反饋控制接口，提供脈沖寬度調(diào)制(PWM)控制信號(hào)輸出，可用于不同控制算法的驗(yàn)證研究。系統(tǒng)設(shè)計(jì)的框圖如圖1所示。

MPU9250為全球首例整合9軸運(yùn)動(dòng)姿態(tài)檢測(cè)的數(shù)字傳感器[8]，消除了多傳感器組合的軸間差問(wèn)題，并減少了傳感器的體積，降低了系統(tǒng)的功耗。MPU9250內(nèi)部集成有3軸陀螺儀、3軸加速度計(jì)、3軸電子羅盤(pán)，輸出信號(hào)都是16位的數(shù)字量；可以通過(guò)集成電路總線(xiàn)(IIC)接口和單片機(jī)進(jìn)行數(shù)據(jù)交互，傳輸速率可達(dá)400 kHz/s。陀螺儀的角速度測(cè)量范圍最高達(dá)±2 000 (°)/s，具有良好的動(dòng)態(tài)響應(yīng)特性。

圖1 姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)框圖

加速度計(jì)的測(cè)量范圍最大為±16g(g為重力加速度)，靜態(tài)測(cè)量精度高。電子羅盤(pán)采用高靈敏度霍爾型傳感器進(jìn)行數(shù)據(jù)采集，磁感應(yīng)強(qiáng)度測(cè)量范圍為±4 800 μT，可用于對(duì)偏航角的輔助測(cè)量。

2 數(shù)據(jù)處理

2.1 角度的計(jì)算

傳感器傳輸回來(lái)的數(shù)據(jù)分別代表3個(gè)軸測(cè)量的角速度、加速度和磁感應(yīng)強(qiáng)度，需要解算成姿態(tài)角度信息，主要是俯仰角ρ(X軸與地面的角度)、翻滾角φ(Y軸與地面的角度)和航向角α(繞Z軸旋轉(zhuǎn)的角度)。

陀螺儀測(cè)量的是旋轉(zhuǎn)的角速度，通過(guò)積分就可以得到對(duì)應(yīng)的角度值[7]，計(jì)算公式如下：

θk=(ωk-ωbias_k)dt+θk-1,

(1)

其中：θk為當(dāng)前時(shí)刻的角度值；θk-1為前一時(shí)刻的角度值；ωk為陀螺儀測(cè)量當(dāng)前時(shí)刻的角速度；ωbias_k為當(dāng)前時(shí)刻角速度的偏移量；dt為積分時(shí)間，即角度計(jì)算的采樣周期。

加速度計(jì)測(cè)量3個(gè)軸的加速度分量，可利用三角函數(shù)關(guān)系解算角度姿態(tài)[9]：

(2)

其中：Ax、Ay、Az分別是X、Y、Z這3個(gè)軸的加速度分量；γ為Z軸與重力加速度的夾角；ρ為俯仰角；φ為翻滾角。

電子羅盤(pán)測(cè)量的是3個(gè)軸的磁感應(yīng)強(qiáng)度，由于X、Y兩軸的磁感應(yīng)強(qiáng)度合成后總指向地磁北極，因此，可通過(guò)測(cè)量敏感軸與地磁北極的夾角來(lái)實(shí)現(xiàn)航向角的檢測(cè)。電子羅盤(pán)在水平位置且無(wú)外加磁場(chǎng)干擾時(shí)，航向角可通過(guò)如下三角函數(shù)關(guān)系計(jì)算：

(3)

其中，Hx和Hy分別為X軸和Y軸輸出的磁感應(yīng)強(qiáng)度數(shù)據(jù)。

當(dāng)電子羅盤(pán)不在水平位置的時(shí)候，可通過(guò)傾斜補(bǔ)償方法，減小航向角檢測(cè)的誤差[10]。傾斜補(bǔ)償公式為：

(4)

2.2 卡爾曼濾波

在角度姿態(tài)融合中，互補(bǔ)濾波和卡爾曼濾波是常見(jiàn)的處理算法，而從濾波效果來(lái)看，互補(bǔ)濾波的動(dòng)態(tài)融合精度低，響應(yīng)速度沒(méi)有卡爾曼濾波快[11]。因此，系統(tǒng)設(shè)計(jì)中采用了卡爾曼濾波算法對(duì)角度姿態(tài)數(shù)據(jù)進(jìn)行融合處理。

卡爾曼濾波是以系統(tǒng)的觀測(cè)量作為濾波器的輸入量，以系統(tǒng)狀態(tài)量的估計(jì)值作為濾波器的輸出量，利用系統(tǒng)噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性進(jìn)行最優(yōu)估計(jì)的數(shù)據(jù)處理方法[12]。假設(shè)系統(tǒng)的狀態(tài)方程和測(cè)量方差可用如下線(xiàn)性公式描述[13]：

(5)

其中：xk為狀態(tài)向量；yk為觀測(cè)向量；A為從k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；Uk為系統(tǒng)輸入控制向量；B為輸入控制向量的增益矩陣；H為狀態(tài)量到觀測(cè)量的增益矩陣；wk為輸入噪聲；vk為觀測(cè)噪聲。假定輸入噪聲和觀測(cè)噪聲都是服從正態(tài)分布，輸入噪聲的協(xié)方差為Q，觀測(cè)噪聲的協(xié)方差為R，則卡爾曼濾波遞推公式可概括為5個(gè)核心公式[13]：

狀態(tài)量的預(yù)估計(jì)，

(6)

誤差協(xié)方差預(yù)估計(jì)，

(7)

卡爾曼增益更新，

(8)

現(xiàn)狀態(tài)估計(jì)值更新，

(9)

誤差協(xié)方差更新，

(10)

對(duì)于姿態(tài)檢測(cè)系統(tǒng)，陀螺儀的角度狀態(tài)可用式(1)描述，加速度計(jì)和電子羅盤(pán)測(cè)量的對(duì)應(yīng)角度值可作為觀測(cè)反饋值。為了抑制陀螺儀的漂移，系統(tǒng)還需要對(duì)偏移量進(jìn)行估計(jì)處理，所以系統(tǒng)的狀態(tài)方程可用如下公式描述：

(11)

其中：Ts為采樣周期；ωk和ωk-1分別為陀螺儀測(cè)量k時(shí)刻以及k-1時(shí)刻的角速度。

R和Q的取值會(huì)影響濾波的效果和響應(yīng)速度：R取值越小，響應(yīng)越快，收斂越迅速；Q取值越小，抑制噪聲的能力越強(qiáng)，但收斂和響應(yīng)變慢。在系統(tǒng)的軟件處理程序中，優(yōu)化后的卡爾曼濾波的各項(xiàng)系數(shù)設(shè)定初始值如下：

3 實(shí)驗(yàn)與分析

系統(tǒng)完成角度姿態(tài)數(shù)據(jù)的采集和處理后，將角度信息通過(guò)無(wú)線(xiàn)傳輸模塊傳輸給主機(jī)，主機(jī)再傳輸給上位機(jī)，并利用自主設(shè)計(jì)的上位機(jī)軟件進(jìn)行顯示和運(yùn)動(dòng)曲線(xiàn)繪制，便于對(duì)角度姿態(tài)的變化進(jìn)行分析。

圖2是對(duì)已知角度姿態(tài)的靜態(tài)測(cè)量數(shù)據(jù)。利用實(shí)驗(yàn)室的角度分度裝置，設(shè)置了0°和35°兩個(gè)測(cè)試位置，利用系統(tǒng)采集傳感器的數(shù)據(jù)，將卡爾曼濾波后所得數(shù)據(jù)與陀螺儀、加速度計(jì)分別測(cè)量的角度數(shù)據(jù)進(jìn)行了比較。從圖2a中可以看出：在0°位置，加速度計(jì)存在較明顯的噪聲，最大的噪聲達(dá)到了±0.9°；而陀螺儀則存在較明顯的漂移，隨著時(shí)間的延長(zhǎng)，測(cè)量角度累計(jì)誤差增大；卡爾曼濾波后得到的俯仰角，在一定程度上抑制了加速度計(jì)的噪聲，角度誤差降低到±0.5°范圍內(nèi)，并且對(duì)陀螺儀的漂移也進(jìn)行了修正，提高了角度姿態(tài)檢測(cè)的準(zhǔn)確性。圖2b是在35°位置靜止時(shí)俯仰角的測(cè)量曲線(xiàn)，卡爾曼濾波同樣起到了很好的修正作用，降低了測(cè)量誤差。測(cè)量結(jié)果驗(yàn)證了系統(tǒng)的測(cè)量姿態(tài)角度的可行性和準(zhǔn)確性。

圖2 靜止?fàn)顟B(tài)下俯仰角的測(cè)量結(jié)果

圖3 運(yùn)動(dòng)狀態(tài)下卡爾曼濾波前后俯仰角檢測(cè)結(jié)果比較

圖3為運(yùn)動(dòng)狀態(tài)下系統(tǒng)測(cè)量的俯仰角。隨著傳感器姿態(tài)的調(diào)整，陀螺儀和加速度計(jì)都及時(shí)跟蹤了角度變化。從圖3的局部放大數(shù)據(jù)可以看出：陀螺儀的動(dòng)態(tài)響應(yīng)更好；加速度計(jì)的噪聲比較明顯，并存在一定滯后現(xiàn)象；卡爾曼濾波對(duì)數(shù)據(jù)進(jìn)行了有效的融合處理，滿(mǎn)足了系統(tǒng)對(duì)姿態(tài)檢測(cè)的要求。

通過(guò)實(shí)驗(yàn)分析，設(shè)計(jì)的姿態(tài)檢測(cè)系統(tǒng)工作良好，經(jīng)過(guò)卡爾曼濾波算法融合的數(shù)據(jù)能及時(shí)跟蹤系統(tǒng)姿態(tài)的變化，并有效抑制了加速度計(jì)的噪聲，降低了陀螺儀的漂移，提高了姿態(tài)檢測(cè)的準(zhǔn)確度。

4 結(jié)論

利用姿態(tài)檢測(cè)傳感器MPU9250和MSP430單片機(jī)搭建的姿態(tài)檢測(cè)系統(tǒng)，實(shí)現(xiàn)了姿態(tài)角度檢測(cè)的功能；并通過(guò)卡爾曼濾波算法對(duì)陀螺儀、加速度計(jì)、電子羅盤(pán)解算的角度信息進(jìn)行了融合處理，提高了姿態(tài)角度檢測(cè)的準(zhǔn)確性。該系統(tǒng)具有響應(yīng)快、接口豐富、實(shí)時(shí)性好等特點(diǎn)，并可應(yīng)用于機(jī)器人、無(wú)人機(jī)姿態(tài)檢測(cè)、慣性導(dǎo)航、虛擬現(xiàn)實(shí)裝備等領(lǐng)域。

[1] 王巍.慣性技術(shù)研究現(xiàn)狀及發(fā)展趨勢(shì)[J].自動(dòng)化學(xué)報(bào),2013,39(6):723-729.

[2] 秦勇,臧希喆,王曉宇,等.基于MEMS慣性傳感器的機(jī)器人姿態(tài)監(jiān)測(cè)系統(tǒng)的研究[J].傳感技術(shù)學(xué)報(bào),2007,20(2):298-301.

[3] 夏圣,許勇.基于MEMS組合模塊的姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用,2011(4):52-55.

[4] 梁建宏,田偉程,王田苗.基于ARM與低成本MEMS的AHRS設(shè)計(jì)[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用,2012(5):1-4.

[5] 畢盛,閔華清,李淳,等.姿態(tài)傳感器采集測(cè)試系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)測(cè)量與控制,2011，19(7):1562-1564.

[6]FosterCC,ElkaimGH,SantaCUC.ExtensionofaTwo-StepCalibrationMethodologytoIncludeNonorthogonalSensorAxes[J].IEEETransactionsonAerospaceandEledtronicSystems,2008,44(3):1070-1078.

[7] 李偉,何鵬舉,高社生.多傳感器加權(quán)信息融合算法研究[J].西北工業(yè)大學(xué)學(xué)報(bào),2010,28(5):674-678.

[8] MPU9250數(shù)據(jù)手冊(cè)[EB/OL].[2015-01-23].http://www.invensense.com/mems/gyro/mpu9250.html.

[9] Kimberly T.Tilt Sensing Using Linear Accelerometers[J].Freescale Semiconductor Application Note,2007,6:1-7.

[10] 邵婷婷,馬建倉(cāng),胡士峰,等.電子羅盤(pán)的傾斜與羅差補(bǔ)償算法研究[J].傳感技術(shù)學(xué)報(bào),2007,20(6):1335-1337.

[11] 郜園園,阮曉鋼,宋洪軍,等.兩輪自平衡機(jī)器人慣性傳感器濾波問(wèn)題的研究[J].傳感技術(shù)學(xué)報(bào),2010,23(5):696-700.

[12] 付夢(mèng)印,鄧志紅,閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京:科學(xué)出版社,2010:17-18.

[13] 楊凌霄,李曉陽(yáng).基于卡爾曼濾波的兩輪自平衡車(chē)姿態(tài)檢測(cè)方法[J].計(jì)算機(jī)仿真,2014,3(6):406-409.

河南省基礎(chǔ)與前沿技術(shù)研究基金項(xiàng)目(142300413217);河南省教育廳基金項(xiàng)目(14B460028);河南科技大學(xué)青年基金項(xiàng)目(2012QN0025)

劉春陽(yáng)(1982-),男,滿(mǎn)族,河北承德人，講師,博士,主要研究方向?yàn)榫軠y(cè)試技術(shù).

2014-12-01

1672-6871(2015)04-0014-04

TP216

A