摘要：在研究兩輪自平衡電動車的平衡控制的基礎(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

引言
    在兩輪自平衡電動車的平衡控制中，對車體的俯仰角進行實時準(zhǔn)確的測量是整個平衡控制的前提。傳統(tǒng)的機械式姿態(tài)測量儀體積大、響應(yīng)速度慢、測量精度低，遠(yuǎn)遠(yuǎn)不能滿足雙輪自平衡電動車平衡控制的實時性和精確性要求?；诠鈱W(xué)原理和圖像處理技術(shù)的姿態(tài)測量儀精度較高，但是價格昂貴，且需要進行大量的復(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é)對加速度計與陀螺儀進行A／D采樣及后續(xù)的數(shù)據(jù)處理。

    ADXRS610是ADI公司推出的一款高性價比的單軸陀螺儀。ADXRS610是一款模擬量輸出器件，其輸出量與繞旋轉(zhuǎn)軸的旋轉(zhuǎ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)加速度值，進而計算出被測平臺與水平面的夾角值。所謂測量靜態(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模塊進行處理，在硬件成本和軟件編程的難易度方面都有優(yōu)勢。方式二采用兩敏感軸同時采集兩個方向的重力加速度分量，通過兩個軸的分量比來獲取角度值。由于兩敏感軸所處的環(huán)境因素一樣，因此對于震動、溫度變化等因素的干擾有一定的抑制效果。另外，對于不同地理位置的重力加速度常量值的變化，方式一會產(chǎn)生相應(yīng)的誤差，而方式二可以從原理上消除此類誤差。經(jīng)比較，選用第二種方式。
1．3 數(shù)據(jù)融合模型
    互補濾波的基本原理如圖4所示。互補濾波的思想即針對不同輸出特性的傳感器，采用不同的濾波方法將所得的信號結(jié)合起來，從而克服單一傳感器失真嚴(yán)重的缺點，獲取到更為準(zhǔn)確的信息。使含有高頻噪聲分量的加速度計信號通過低通濾波器濾波，使具有積分漂移效應(yīng)的陀螺儀信號通過高通濾波器濾波。

    由于本系統(tǒng)的高、低通濾波過程均是通過PIC16F877進行軟件方式處理來模擬實現(xiàn)電子電路的功能，因此需要將連續(xù)濾波模型公式離散化處理。連續(xù)型低通濾波器的模型公式如下：
   
    其中，uin(t)和uout(t)分別為輸入和輸出電壓，RC為時間常數(shù)。離散化后的數(shù)字低通濾波器模型如下：
   
    其中，，yi表示i時刻的輸出；xi表示i時刻的輸入；yi-1表示(i-1)時刻的輸出；△T為采樣周期。基于互補濾波的思想并結(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ù)進行仿真，驗證此模型的有效性。加速度計原始信號如圖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所示。其中，加速度計曲線是通過對加速度計原始信號計算得到的角度值，可見依然波動較大。陀螺儀曲線是通過對陀螺儀的輸出角速度值進行積分運算所得的角度值。數(shù)據(jù)融合曲線是通過文中構(gòu)造的數(shù)據(jù)融合模型進行解算得到的角度值。

    由圖7可以看出，由單一的加速度計得出的角度值波動很大，不能準(zhǔn)確反映真實值。由單一的陀螺儀經(jīng)過積分運算得到的角度值雖然沒有波動，但隨著時間的增大，產(chǎn)生的積分累積效應(yīng)將非常嚴(yán)重。從0 s到10 s，陀螺儀產(chǎn)生的積分累積偏差接近了50°，并以平均5°／s的速度加速擴大。這也是不能單獨使用陀螺儀進行平臺姿態(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軸進行安裝。系統(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ù)的信號處理中需要進行積分處理，所以要求模／數(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é)果并進行通道切換，在通道切換后，必須保證在下一次模／數(shù)轉(zhuǎn)換模塊觸發(fā)來臨之前有足夠的采樣時間。根據(jù)對整個系統(tǒng)需求的綜合分析，將CCP2的觸發(fā)采樣周期設(shè)為5 ms。中斷程序流程如圖9所示。

4 系統(tǒng)測試
    實驗是在25℃室內(nèi)環(huán)境下進行測試的。首先將本姿態(tài)測量系統(tǒng)固定安裝在兩輪自平衡電動車車體上，再將兩輪自平衡車的電機上電運行以提供一種強震動干擾環(huán)境。表2是將兩輪自平衡車車體分別固定在0°、30°、45°三個位置進行測量的數(shù)據(jù)。

    從測量結(jié)果可以看出，隨著角度增大，由于MEMS器件的非線性因素導(dǎo)致誤差有所增大，但在0°到30°之間誤差保持在50’以內(nèi)，誤差精度完全可以滿足兩輪自平衡電動車的控制要求。數(shù)據(jù)顯示，測量數(shù)據(jù)的數(shù)值普遍比真實值偏大，這是由于被測試平臺安裝的初始角度比真實零初始角度偏大造成的，在對初始安裝位置進行校正后會進一步縮小誤差。為了達(dá)到更高精度，除了在初始安裝時對初始位置進行校正外，還需要對MEMS器件進行更精確的標(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ù)的測量。