摘要:在研究兩輪自平衡電動車的平衡控制的基礎(chǔ)上,,設(shè)計了基于MEMS組合模塊的姿態(tài)測量系統(tǒng)。通過對各MEMS傳感器輸出信號特征的提取,、分析,,將離散化低通濾波器模型與互補濾波的思想相結(jié)合,巧妙地構(gòu)造出一種便于本系統(tǒng)實現(xiàn)的數(shù)據(jù)融合模型,。同時,,提供了一種基于MEMS組合模塊姿態(tài)檢測的低成本、可行性方案,。
關(guān)鍵詞:MEMS組合模塊,;姿態(tài)檢測;數(shù)據(jù)融合,;MMA7260,;ADXRS610
引言
在兩輪自平衡電動車的平衡控制中,對車體的俯仰角進(jìn)行實時準(zhǔn)確的測量是整個平衡控制的前提,。傳統(tǒng)的機械式姿態(tài)測量儀體積大,、響應(yīng)速度慢、測量精度低,,遠(yuǎn)遠(yuǎn)不能滿足雙輪自平衡電動車平衡控制的實時性和精確性要求,。基于光學(xué)原理和圖像處理技術(shù)的姿態(tài)測量儀精度較高,,但是價格昂貴,,且需要進(jìn)行大量的復(fù)雜運算,一般需要計算機輔助完成,,不符合兩輪自平衡電動車的成本和車載性要求,。本文采用ME-MS組合模塊的設(shè)計方案實現(xiàn)了對車體俯仰角的測量。本測量系統(tǒng)不但具有體積小,、響應(yīng)快,、成本低的特點外,而且經(jīng)過數(shù)據(jù)融合處理后,,有效地減小了環(huán)境中震動干擾的影響,,能夠在具有強烈震動的車體環(huán)境中達(dá)到系統(tǒng)的精度要求。
1 系統(tǒng)構(gòu)成及工作原理
1.1 系統(tǒng)構(gòu)成
本姿態(tài)測量系統(tǒng)主要由單片機PIC16F877,、加速度計MMA7260和陀螺儀ADXRS610構(gòu)成,。系統(tǒng)結(jié)構(gòu)框圖如圖1所示。Microchip公司提供的PIC16F877芯片作為控制芯片,,負(fù)責(zé)對加速度計與陀螺儀進(jìn)行A/D采樣及后續(xù)的數(shù)據(jù)處理,。
ADXRS610是ADI公司推出的一款高性價比的單軸陀螺儀。ADXRS610是一款模擬量輸出器件,,其輸出量與繞旋轉(zhuǎn)軸的旋轉(zhuǎn)角速度成線性正比,,對角速度量進(jìn)行積分運算即可得到旋轉(zhuǎn)的角度值。ADXRS610角速度的測量范圍是±300 rad/s,,靈敏度為6 mV/(rad·s-1),,零位輸出電壓為2.50 V,非線性誤差為±0.1%F.S.,,-3 dB帶寬為40 Hz,,固有頻率為14 kHz。
1.2 加速度計的數(shù)學(xué)應(yīng)用原理
MMA7260是一款性價比較高的三軸模擬量輸出加速度計,,模擬輸出量與其敏感軸方向上所受加速度大小成線性關(guān)系,。文中利用MMA7260測得靜態(tài)加速度值,進(jìn)而計算出被測平臺與水平面的夾角值,。所謂測量靜態(tài)加速度,,是指在平臺靜止時,測量重力加速度在某一敏感軸上的分量的大小,。由于重力加速度的方向總是垂直向下的,,這正好為測量系統(tǒng)提供了一個天然的參考坐標(biāo)系。通過加速度計間接測量角度的數(shù)學(xué)原理有兩種方式,。
方式一如圖2所示,。利用單一敏感軸測得重力加速度分量,其中角度θ,、重力加速度g與敏感軸分量gx三者構(gòu)成反余弦關(guān)系:
方式二如圖3所示,。
利用雙敏感軸測得重力加速度分量,,其中角度θ、Y敏感軸分量gy與X敏感軸分量gx三者構(gòu)成反正切關(guān)系:
方式一只需要單軸加速度計就能實現(xiàn)角度測量,,在數(shù)據(jù)采集時也只需要單通道的ADC模塊進(jìn)行處理,,在硬件成本和軟件編程的難易度方面都有優(yōu)勢。方式二采用兩敏感軸同時采集兩個方向的重力加速度分量,,通過兩個軸的分量比來獲取角度值,。由于兩敏感軸所處的環(huán)境因素一樣,因此對于震動,、溫度變化等因素的干擾有一定的抑制效果,。另外,對于不同地理位置的重力加速度常量值的變化,,方式一會產(chǎn)生相應(yīng)的誤差,,而方式二可以從原理上消除此類誤差。經(jīng)比較,,選用第二種方式,。
1.3 數(shù)據(jù)融合模型
互補濾波的基本原理如圖4所示?;パa濾波的思想即針對不同輸出特性的傳感器,,采用不同的濾波方法將所得的信號結(jié)合起來,從而克服單一傳感器失真嚴(yán)重的缺點,,獲取到更為準(zhǔn)確的信息,。使含有高頻噪聲分量的加速度計信號通過低通濾波器濾波,使具有積分漂移效應(yīng)的陀螺儀信號通過高通濾波器濾波,。
由于本系統(tǒng)的高,、低通濾波過程均是通過PIC16F877進(jìn)行軟件方式處理來模擬實現(xiàn)電子電路的功能,因此需要將連續(xù)濾波模型公式離散化處理,。連續(xù)型低通濾波器的模型公式如下:
其中,,uin(t)和uout(t)分別為輸入和輸出電壓,RC為時間常數(shù),。離散化后的數(shù)字低通濾波器模型如下:
其中,,,yi表示i時刻的輸出,;xi表示i時刻的輸入,;yi-1表示(i-1)時刻的輸出;△T為采樣周期,?;诨パa濾波的思想并結(jié)合離散化數(shù)字低通濾波器模型,構(gòu)造出如下模型:
其中,angle為系統(tǒng)最終的輸出角度值,;gyr為由陀螺儀數(shù)據(jù)計算得出的角速度值,,gyrdt則為一個采樣周期內(nèi)的角度增量;acc為由加速度計數(shù)據(jù)計算得出的角度值,。當(dāng)選擇合適的時間常數(shù)RC和采樣周期△T時,,此模型能模擬實現(xiàn)對陀螺儀的高通濾波和對加速度計的低通濾波
的互補組合功能。同時,,時間常數(shù)和采樣周期也確定了系統(tǒng)對陀螺儀和加速度計輸出信號的置信度。當(dāng)本系統(tǒng)選擇時間常數(shù)為0.1 ms,,采樣周期為5 ms時,,代入上式模型有:
假設(shè)初始狀態(tài)時,系統(tǒng)角度估計值angle,、陀螺儀輸出角速度值gyr和加速度計輸出的角度值acc均為零,。表1是當(dāng)加速度計收到一個10°數(shù)值的突變信號時,在連續(xù)10個采樣周期內(nèi)估計值angle的變化過程,。
從表中可以看出,,在系統(tǒng)采樣周期設(shè)為5 ms時,如果此突變信號頻率高于200 Hz,,即該信號持續(xù)時間不超過一個采樣周期,,那么此信號給系統(tǒng)帶來的最大偏差不會超過0.9°。此時,,可認(rèn)為系統(tǒng)對于加速度計的高頻信號不信任,,而信任陀螺儀的信號。如果此突變信號頻率低于20 Hz,,則該信號的持續(xù)時間會達(dá)到10個采樣周期以上,,在第10個采樣周期時,系統(tǒng)的估計值angle為9.0°,。此時,,可認(rèn)為系統(tǒng)信任加速計的低頻信號。由此可見,,此模型具有對加速度計的低通濾波功能和對陀螺儀的高通濾波功能,。
2 仿真驗證
應(yīng)用文中構(gòu)造的數(shù)據(jù)融合模型,通過Matlab工具對實測原始數(shù)據(jù)進(jìn)行仿真,,驗證此模型的有效性,。加速度計原始信號如圖5所示。
這是加速度計在動態(tài)和靜態(tài)時的原始信號輸出對比,,可見加速度計的動態(tài)輸出由于存在高頻干擾,,致使輸出數(shù)據(jù)完全不能準(zhǔn)確地反映真實的姿態(tài)值。被測平臺發(fā)生傾斜時采集的加速度計和陀螺儀的原始信號值如圖6所示??梢钥闯?,在1.6 s之前被測平臺沒有受到震動干擾,此時加速度計的輸出信號振幅較小,。在1.6 s之后,,被測平臺受到震動干擾,加速度計的輸出信號受到嚴(yán)重干擾,,而陀螺儀的輸出信號幾乎不受震動干擾,。
數(shù)據(jù)融合結(jié)果如圖7所示。其中,,加速度計曲線是通過對加速度計原始信號計算得到的角度值,,可見依然波動較大。陀螺儀曲線是通過對陀螺儀的輸出角速度值進(jìn)行積分運算所得的角度值,。數(shù)據(jù)融合曲線是通過文中構(gòu)造的數(shù)據(jù)融合模型進(jìn)行解算得到的角度值,。
由圖7可以看出,由單一的加速度計得出的角度值波動很大,,不能準(zhǔn)確反映真實值,。由單一的陀螺儀經(jīng)過積分運算得到的角度值雖然沒有波動,但隨著時間的增大,,產(chǎn)生的積分累積效應(yīng)將非常嚴(yán)重,。從0 s到10 s,陀螺儀產(chǎn)生的積分累積偏差接近了50°,,并以平均5°/s的速度加速擴大,。這也是不能單獨使用陀螺儀進(jìn)行平臺姿態(tài)檢測的原因。而經(jīng)過數(shù)據(jù)融合處理后得到的角度值,,既克服了加速度計動態(tài)性能差的缺點,,又很好地補償了陀螺儀的積分飄移。
3 系統(tǒng)實現(xiàn)
3.1 硬件設(shè)計
在電路板設(shè)計之初,,需要根據(jù)加速度計,、陀螺儀的敏感軸方向確定傳感器的安裝位置和角度。陀螺儀和加速度計應(yīng)安裝在較近的位置,,盡量減少兩傳感器的外部環(huán)境差異,,這有利于減少后期數(shù)據(jù)處理的誤差。在平臺剛性接觸面是否采取防震處理,,如加入防震墊片等措施,,這對于傳感器的輸出有很大影響,尤其是對加速度計的影響尤為突出,。
為了方便調(diào)整傳感器安裝位置和角度,,在硬件設(shè)計時采用了模塊化思想,將加速度計和陀螺儀設(shè)計在一塊獨立的小電路板上,并使加速度計的X敏感軸正方向指向被測平臺的水平向前方向,,而陀螺儀的旋轉(zhuǎn)敏感軸則需要垂直于加速度計的X軸且平行于加速度計的Y軸進(jìn)行安裝,。系統(tǒng)通過預(yù)留數(shù)據(jù)端口外接LCD液晶顯示屏,可以經(jīng)過簡單編程直接將姿態(tài)數(shù)據(jù)顯示出來,。系統(tǒng)的電源由一只7.5 V可充電電池提供,,并由L7805構(gòu)成的穩(wěn)壓電路轉(zhuǎn)換成穩(wěn)定的5 V電源為整個系統(tǒng)供電。采用4 MHz晶振提供系統(tǒng)時鐘,。系統(tǒng)簡化原理圖如圖8所示,。
3.2 軟件設(shè)計
PIC16F877中自帶一個10位精度的模/數(shù)轉(zhuǎn)換(ADC)模塊,支持8路模擬轉(zhuǎn)換通道,。對于本系統(tǒng)中的加速度計和陀螺儀,,其10位模/數(shù)轉(zhuǎn)換模塊的分辨率完全可以達(dá)到系統(tǒng)所需的精度要求。根據(jù)系統(tǒng)時鐘頻率和模/數(shù)轉(zhuǎn)換(ADC)模塊轉(zhuǎn)換時間,,選擇1/8時鐘頻率作為模/數(shù)轉(zhuǎn)換時鐘,,分別由AN0,、AN1,、AN2通道采集加速度計和陀螺儀的模擬量信號。由于陀螺儀的采樣結(jié)果在后續(xù)的信號處理中需要進(jìn)行積分處理,,所以要求模/數(shù)轉(zhuǎn)換模塊的每次采樣間隔時間盡可能相等,,以保證后續(xù)運算的準(zhǔn)確性。在PIC16F877中的CCP2模塊設(shè)置為特殊事件觸發(fā)模式時,,配合計時器1,,可以實現(xiàn)對模/數(shù)轉(zhuǎn)換模塊的定時中斷開啟功能。在使用特殊事件觸發(fā)啟動模/數(shù)轉(zhuǎn)換時,,需要注意幾個細(xì)節(jié)方面的處理方能得到準(zhǔn)確的模/數(shù)轉(zhuǎn)換結(jié)果:首先,,在模/數(shù)轉(zhuǎn)換模塊初始化時,需要將ADCONO寄存器中的ADON位置1,,否則即使在特殊事件觸發(fā)后也不能啟動模/數(shù)轉(zhuǎn)換,;其次,在通過AN0,、AN1,、AN2三個模擬通道相互切換實現(xiàn)對加速度計和陀螺儀的采樣過程中,需要使用ADIF中斷響應(yīng)來獲取模/數(shù)轉(zhuǎn)換的結(jié)果并進(jìn)行通道切換,,在通道切換后,,必須保證在下一次模/數(shù)轉(zhuǎn)換模塊觸發(fā)來臨之前有足夠的采樣時間。根據(jù)對整個系統(tǒng)需求的綜合分析,,將CCP2的觸發(fā)采樣周期設(shè)為5 ms,。中斷程序流程如圖9所示。
4 系統(tǒng)測試
實驗是在25℃室內(nèi)環(huán)境下進(jìn)行測試的。首先將本姿態(tài)測量系統(tǒng)固定安裝在兩輪自平衡電動車車體上,,再將兩輪自平衡車的電機上電運行以提供一種強震動干擾環(huán)境,。表2是將兩輪自平衡車車體分別固定在0°、30°,、45°三個位置進(jìn)行測量的數(shù)據(jù),。
從測量結(jié)果可以看出,隨著角度增大,,由于MEMS器件的非線性因素導(dǎo)致誤差有所增大,,但在0°到30°之間誤差保持在50’以內(nèi),誤差精度完全可以滿足兩輪自平衡電動車的控制要求,。數(shù)據(jù)顯示,,測量數(shù)據(jù)的數(shù)值普遍比真實值偏大,這是由于被測試平臺安裝的初始角度比真實零初始角度偏大造成的,,在對初始安裝位置進(jìn)行校正后會進(jìn)一步縮小誤差,。為了達(dá)到更高精度,除了在初始安裝時對初始位置進(jìn)行校正外,,還需要對MEMS器件進(jìn)行更精確的標(biāo)定和合適的溫度補償,。
5 結(jié)論
文中構(gòu)造了滿足本系統(tǒng)要求的數(shù)據(jù)融合模型,利用Matlab工具驗證了該模型的合理性和有效性,,并設(shè)計了本姿態(tài)測量系統(tǒng)的硬件電路,。實驗測試表明,誤差精度完全可以滿足兩輪自平衡電動車的平衡控制的需求,。本姿態(tài)測量系統(tǒng)除了具有很好的抗干擾性能,,還具有很好的可移植性,既可外接LCD顯示器構(gòu)成獨立的姿態(tài)測量系統(tǒng),,也可為其他平臺提供實時準(zhǔn)確的姿態(tài)數(shù)據(jù),,通過簡單的軟件擴展還可以提供角速度、角加速度等姿態(tài)參數(shù)的測量,。