摘要：在研究兩輪自平衡電動車的平衡控制的基礎(chǔ)上，設(shè)計(jì)了基于MEMS組合模塊的姿態(tài)測量系統(tǒng)。通過對各MEMS傳感器輸出信號特征的提取、分析，將離散化低通濾波器模型與互補(bǔ)濾波的思想相結(jié)合，巧妙地構(gòu)造出一種便于本系統(tǒng)實(shí)現(xiàn)的數(shù)據(jù)融合模型。同時，提供了一種基于MEMS組合模塊姿態(tài)檢測的低成本、可行性方案。
關(guān)鍵詞：MEMS組合模塊；姿態(tài)檢測；數(shù)據(jù)融合；MMA7260；ADXRS610

引言
    在兩輪自平衡電動車的平衡控制中，對車體的俯仰角進(jìn)行實(shí)時準(zhǔn)確的測量是整個平衡控制的前提。傳統(tǒng)的機(jī)械式姿態(tài)測量儀體積大、響應(yīng)速度慢、測量精度低，遠(yuǎn)遠(yuǎn)不能滿足雙輪自平衡電動車平衡控制的實(shí)時性和精確性要求。基于光學(xué)原理和圖像處理技術(shù)的姿態(tài)測量儀精度較高，但是價(jià)格昂貴，且需要進(jìn)行大量的復(fù)雜運(yùn)算，一般需要計(jì)算機(jī)輔助完成，不符合兩輪自平衡電動車的成本和車載性要求。本文采用ME-MS組合模塊的設(shè)計(jì)方案實(shí)現(xiàn)了對車體俯仰角的測量。本測量系統(tǒng)不但具有體積小、響應(yīng)快、成本低的特點(diǎn)外，而且經(jīng)過數(shù)據(jù)融合處理后，有效地減小了環(huán)境中震動干擾的影響，能夠在具有強(qiáng)烈震動的車體環(huán)境中達(dá)到系統(tǒng)的精度要求。

1 系統(tǒng)構(gòu)成及工作原理
1．1 系統(tǒng)構(gòu)成
    本姿態(tài)測量系統(tǒng)主要由單片機(jī)PIC16F877、加速度計(jì)MMA7260和陀螺儀ADXRS610構(gòu)成。系統(tǒng)結(jié)構(gòu)框圖如圖1所示。Microchip公司提供的PIC16F877芯片作為控制芯片，負(fù)責(zé)對加速度計(jì)與陀螺儀進(jìn)行A／D采樣及后續(xù)的數(shù)據(jù)處理。

    ADXRS610是ADI公司推出的一款高性價(jià)比的單軸陀螺儀。ADXRS610是一款模擬量輸出器件，其輸出量與繞旋轉(zhuǎn)軸的旋轉(zhuǎn)角速度成線性正比，對角速度量進(jìn)行積分運(yù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 加速度計(jì)的數(shù)學(xué)應(yīng)用原理
    MMA7260是一款性價(jià)比較高的三軸模擬量輸出加速度計(jì)，模擬輸出量與其敏感軸方向上所受加速度大小成線性關(guān)系。文中利用MMA7260測得靜態(tài)加速度值，進(jìn)而計(jì)算出被測平臺與水平面的夾角值。所謂測量靜態(tài)加速度，是指在平臺靜止時，測量重力加速度在某一敏感軸上的分量的大小。由于重力加速度的方向總是垂直向下的，這正好為測量系統(tǒng)提供了一個天然的參考坐標(biāo)系。通過加速度計(jì)間接測量角度的數(shù)學(xué)原理有兩種方式。
    方式一如圖2所示。利用單一敏感軸測得重力加速度分量，其中角度θ、重力加速度g與敏感軸分量gx三者構(gòu)成反余弦關(guān)系：
   
    方式二如圖3所示。

    利用雙敏感軸測得重力加速度分量，其中角度θ、Y敏感軸分量gy與X敏感軸分量gx三者構(gòu)成反正切關(guān)系：
   
    方式一只需要單軸加速度計(jì)就能實(shí)現(xiàn)角度測量，在數(shù)據(jù)采集時也只需要單通道的ADC模塊進(jìn)行處理，在硬件成本和軟件編程的難易度方面都有優(yōu)勢。方式二采用兩敏感軸同時采集兩個方向的重力加速度分量，通過兩個軸的分量比來獲取角度值。由于兩敏感軸所處的環(huán)境因素一樣，因此對于震動、溫度變化等因素的干擾有一定的抑制效果。另外，對于不同地理位置的重力加速度常量值的變化，方式一會產(chǎn)生相應(yīng)的誤差，而方式二可以從原理上消除此類誤差。經(jīng)比較，選用第二種方式。
1．3 數(shù)據(jù)融合模型
    互補(bǔ)濾波的基本原理如圖4所示?；パa(bǔ)濾波的思想即針對不同輸出特性的傳感器，采用不同的濾波方法將所得的信號結(jié)合起來，從而克服單一傳感器失真嚴(yán)重的缺點(diǎn)，獲取到更為準(zhǔn)確的信息。使含有高頻噪聲分量的加速度計(jì)信號通過低通濾波器濾波，使具有積分漂移效應(yīng)的陀螺儀信號通過高通濾波器濾波。

    由于本系統(tǒng)的高、低通濾波過程均是通過PIC16F877進(jìn)行軟件方式處理來模擬實(shí)現(xiàn)電子電路的功能，因此需要將連續(xù)濾波模型公式離散化處理。連續(xù)型低通濾波器的模型公式如下：
   
    其中，uin(t)和uout(t)分別為輸入和輸出電壓，RC為時間常數(shù)。離散化后的數(shù)字低通濾波器模型如下：
   
    其中，，yi表示i時刻的輸出；xi表示i時刻的輸入；yi-1表示(i-1)時刻的輸出；△T為采樣周期?；诨パa(bǔ)濾波的思想并結(jié)合離散化數(shù)字低通濾波器模型，構(gòu)造出如下模型：
   
    其中，angle為系統(tǒng)最終的輸出角度值；gyr為由陀螺儀數(shù)據(jù)計(jì)算得出的角速度值，gyrdt則為一個采樣周期內(nèi)的角度增量；acc為由加速度計(jì)數(shù)據(jù)計(jì)算得出的角度值。當(dāng)選擇合適的時間常數(shù)RC和采樣周期△T時，此模型能模擬實(shí)現(xiàn)對陀螺儀的高通濾波和對加速度計(jì)的低通濾波
的互補(bǔ)組合功能。同時，時間常數(shù)和采樣周期也確定了系統(tǒng)對陀螺儀和加速度計(jì)輸出信號的置信度。當(dāng)本系統(tǒng)選擇時間常數(shù)為0．1 ms，采樣周期為5 ms時，代入上式模型有：
   

    假設(shè)初始狀態(tài)時，系統(tǒng)角度估計(jì)值angle、陀螺儀輸出角速度值gyr和加速度計(jì)輸出的角度值acc均為零。表1是當(dāng)加速度計(jì)收到一個10°數(shù)值的突變信號時，在連續(xù)10個采樣周期內(nèi)估計(jì)值angle的變化過程。

    從表中可以看出，在系統(tǒng)采樣周期設(shè)為5 ms時，如果此突變信號頻率高于200 Hz，即該信號持續(xù)時間不超過一個采樣周期，那么此信號給系統(tǒng)帶來的最大偏差不會超過0．9°。此時，可認(rèn)為系統(tǒng)對于加速度計(jì)的高頻信號不信任，而信任陀螺儀的信號。如果此突變信號頻率低于20 Hz，則該信號的持續(xù)時間會達(dá)到10個采樣周期以上，在第10個采樣周期時，系統(tǒng)的估計(jì)值angle為9．0°。此時，可認(rèn)為系統(tǒng)信任加速計(jì)的低頻信號。由此可見，此模型具有對加速度計(jì)的低通濾波功能和對陀螺儀的高通濾波功能。

2 仿真驗(yàn)證
    應(yīng)用文中構(gòu)造的數(shù)據(jù)融合模型，通過Matlab工具對實(shí)測原始數(shù)據(jù)進(jìn)行仿真，驗(yàn)證此模型的有效性。加速度計(jì)原始信號如圖5所示。

    這是加速度計(jì)在動態(tài)和靜態(tài)時的原始信號輸出對比，可見加速度計(jì)的動態(tài)輸出由于存在高頻干擾，致使輸出數(shù)據(jù)完全不能準(zhǔn)確地反映真實(shí)的姿態(tài)值。被測平臺發(fā)生傾斜時采集的加速度計(jì)和陀螺儀的原始信號值如圖6所示?？梢钥闯觯?．6 s之前被測平臺沒有受到震動干擾，此時加速度計(jì)的輸出信號振幅較小。在1．6 s之后，被測平臺受到震動干擾，加速度計(jì)的輸出信號受到嚴(yán)重干擾，而陀螺儀的輸出信號幾乎不受震動干擾。

    數(shù)據(jù)融合結(jié)果如圖7所示。其中，加速度計(jì)曲線是通過對加速度計(jì)原始信號計(jì)算得到的角度值，可見依然波動較大。陀螺儀曲線是通過對陀螺儀的輸出角速度值進(jìn)行積分運(yùn)算所得的角度值。數(shù)據(jù)融合曲線是通過文中構(gòu)造的數(shù)據(jù)融合模型進(jìn)行解算得到的角度值。

    由圖7可以看出，由單一的加速度計(jì)得出的角度值波動很大，不能準(zhǔn)確反映真實(shí)值。由單一的陀螺儀經(jīng)過積分運(yùn)算得到的角度值雖然沒有波動，但隨著時間的增大，產(chǎn)生的積分累積效應(yīng)將非常嚴(yán)重。從0 s到10 s，陀螺儀產(chǎn)生的積分累積偏差接近了50°，并以平均5°／s的速度加速擴(kuò)大。這也是不能單獨(dú)使用陀螺儀進(jìn)行平臺姿態(tài)檢測的原因。而經(jīng)過數(shù)據(jù)融合處理后得到的角度值，既克服了加速度計(jì)動態(tài)性能差的缺點(diǎn)，又很好地補(bǔ)償了陀螺儀的積分飄移。

3 系統(tǒng)實(shí)現(xiàn)
3．1 硬件設(shè)計(jì)
    在電路板設(shè)計(jì)之初，需要根據(jù)加速度計(jì)、陀螺儀的敏感軸方向確定傳感器的安裝位置和角度。陀螺儀和加速度計(jì)應(yīng)安裝在較近的位置，盡量減少兩傳感器的外部環(huán)境差異，這有利于減少后期數(shù)據(jù)處理的誤差。在平臺剛性接觸面是否采取防震處理，如加入防震墊片等措施，這對于傳感器的輸出有很大影響，尤其是對加速度計(jì)的影響尤為突出。
    為了方便調(diào)整傳感器安裝位置和角度，在硬件設(shè)計(jì)時采用了模塊化思想，將加速度計(jì)和陀螺儀設(shè)計(jì)在一塊獨(dú)立的小電路板上，并使加速度計(jì)的X敏感軸正方向指向被測平臺的水平向前方向，而陀螺儀的旋轉(zhuǎn)敏感軸則需要垂直于加速度計(jì)的X軸且平行于加速度計(jì)的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è)計(jì)
    PIC16F877中自帶一個10位精度的模／數(shù)轉(zhuǎn)換(ADC)模塊，支持8路模擬轉(zhuǎn)換通道。對于本系統(tǒng)中的加速度計(jì)和陀螺儀，其10位模／數(shù)轉(zhuǎn)換模塊的分辨率完全可以達(dá)到系統(tǒng)所需的精度要求。根據(jù)系統(tǒng)時鐘頻率和模／數(shù)轉(zhuǎn)換(ADC)模塊轉(zhuǎn)換時間，選擇1／8時鐘頻率作為模／數(shù)轉(zhuǎn)換時鐘，分別由AN0、AN1、AN2通道采集加速度計(jì)和陀螺儀的模擬量信號。由于陀螺儀的采樣結(jié)果在后續(xù)的信號處理中需要進(jìn)行積分處理，所以要求模／數(shù)轉(zhuǎn)換模塊的每次采樣間隔時間盡可能相等，以保證后續(xù)運(yùn)算的準(zhǔn)確性。在PIC16F877中的CCP2模塊設(shè)置為特殊事件觸發(fā)模式時，配合計(jì)時器1，可以實(shí)現(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三個模擬通道相互切換實(shí)現(xiàn)對加速度計(jì)和陀螺儀的采樣過程中，需要使用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)測試
    實(shí)驗(yàn)是在25℃室內(nèi)環(huán)境下進(jìn)行測試的。首先將本姿態(tài)測量系統(tǒng)固定安裝在兩輪自平衡電動車車體上，再將兩輪自平衡車的電機(jī)上電運(yùn)行以提供一種強(qiá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ù)值普遍比真實(shí)值偏大，這是由于被測試平臺安裝的初始角度比真實(shí)零初始角度偏大造成的，在對初始安裝位置進(jìn)行校正后會進(jìn)一步縮小誤差。為了達(dá)到更高精度，除了在初始安裝時對初始位置進(jìn)行校正外，還需要對MEMS器件進(jìn)行更精確的標(biāo)定和合適的溫度補(bǔ)償。

5 結(jié)論
    文中構(gòu)造了滿足本系統(tǒng)要求的數(shù)據(jù)融合模型，利用Matlab工具驗(yàn)證了該模型的合理性和有效性，并設(shè)計(jì)了本姿態(tài)測量系統(tǒng)的硬件電路。實(shí)驗(yàn)測試表明，誤差精度完全可以滿足兩輪自平衡電動車的平衡控制的需求。本姿態(tài)測量系統(tǒng)除了具有很好的抗干擾性能，還具有很好的可移植性，既可外接LCD顯示器構(gòu)成獨(dú)立的姿態(tài)測量系統(tǒng)，也可為其他平臺提供實(shí)時準(zhǔn)確的姿態(tài)數(shù)據(jù)，通過簡單的軟件擴(kuò)展還可以提供角速度、角加速度等姿態(tài)參數(shù)的測量。