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

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

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

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

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

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

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

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

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

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

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

    由圖7可以看出，由單一的加速度計(jì)得出的角度值波動(dòng)很大，不能準(zhǔn)確反映真實(shí)值。由單一的陀螺儀經(jīng)過積分運(yùn)算得到的角度值雖然沒有波動(dòng)，但隨著時(shí)間的增大，產(chǎn)生的積分累積效應(yīng)將非常嚴(yán)重。從0 s到10 s，陀螺儀產(chǎn)生的積分累積偏差接近了50°，并以平均5°／s的速度加速擴(kuò)大。這也是不能單獨(dú)使用陀螺儀進(jìn)行平臺(tái)姿態(tài)檢測(cè)的原因。而經(jīng)過數(shù)據(jù)融合處理后得到的角度值，既克服了加速度計(jì)動(dòng)態(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ù)處理的誤差。在平臺(tái)剛性接觸面是否采取防震處理，如加入防震墊片等措施，這對(duì)于傳感器的輸出有很大影響，尤其是對(duì)加速度計(jì)的影響尤為突出。
    為了方便調(diào)整傳感器安裝位置和角度，在硬件設(shè)計(jì)時(shí)采用了模塊化思想，將加速度計(jì)和陀螺儀設(shè)計(jì)在一塊獨(dú)立的小電路板上，并使加速度計(jì)的X敏感軸正方向指向被測(cè)平臺(tái)的水平向前方向，而陀螺儀的旋轉(zhuǎn)敏感軸則需要垂直于加速度計(jì)的X軸且平行于加速度計(jì)的Y軸進(jìn)行安裝。系統(tǒng)通過預(yù)留數(shù)據(jù)端口外接LCD液晶顯示屏，可以經(jīng)過簡(jiǎn)單編程直接將姿態(tài)數(shù)據(jù)顯示出來。系統(tǒng)的電源由一只7．5 V可充電電池提供，并由L7805構(gòu)成的穩(wěn)壓電路轉(zhuǎn)換成穩(wěn)定的5 V電源為整個(gè)系統(tǒng)供電。采用4 MHz晶振提供系統(tǒng)時(shí)鐘。系統(tǒng)簡(jiǎn)化原理圖如圖8所示。

3．2 軟件設(shè)計(jì)
    PIC16F877中自帶一個(gè)10位精度的模／數(shù)轉(zhuǎn)換(ADC)模塊，支持8路模擬轉(zhuǎn)換通道。對(duì)于本系統(tǒng)中的加速度計(jì)和陀螺儀，其10位模／數(shù)轉(zhuǎn)換模塊的分辨率完全可以達(dá)到系統(tǒng)所需的精度要求。根據(jù)系統(tǒng)時(shí)鐘頻率和模／數(shù)轉(zhuǎn)換(ADC)模塊轉(zhuǎn)換時(shí)間，選擇1／8時(shí)鐘頻率作為模／數(shù)轉(zhuǎn)換時(shí)鐘，分別由AN0、AN1、AN2通道采集加速度計(jì)和陀螺儀的模擬量信號(hào)。由于陀螺儀的采樣結(jié)果在后續(xù)的信號(hào)處理中需要進(jìn)行積分處理，所以要求模／數(shù)轉(zhuǎn)換模塊的每次采樣間隔時(shí)間盡可能相等，以保證后續(xù)運(yùn)算的準(zhǔn)確性。在PIC16F877中的CCP2模塊設(shè)置為特殊事件觸發(fā)模式時(shí)，配合計(jì)時(shí)器1，可以實(shí)現(xiàn)對(duì)模／數(shù)轉(zhuǎn)換模塊的定時(shí)中斷開啟功能。在使用特殊事件觸發(fā)啟動(dòng)模／數(shù)轉(zhuǎn)換時(shí)，需要注意幾個(gè)細(xì)節(jié)方面的處理方能得到準(zhǔn)確的模／數(shù)轉(zhuǎn)換結(jié)果：首先，在模／數(shù)轉(zhuǎn)換模塊初始化時(shí)，需要將ADCONO寄存器中的ADON位置1，否則即使在特殊事件觸發(fā)后也不能啟動(dòng)模／數(shù)轉(zhuǎn)換；其次，在通過AN0、AN1、AN2三個(gè)模擬通道相互切換實(shí)現(xiàn)對(duì)加速度計(jì)和陀螺儀的采樣過程中，需要使用ADIF中斷響應(yīng)來獲取模／數(shù)轉(zhuǎn)換的結(jié)果并進(jìn)行通道切換，在通道切換后，必須保證在下一次模／數(shù)轉(zhuǎn)換模塊觸發(fā)來臨之前有足夠的采樣時(shí)間。根據(jù)對(duì)整個(gè)系統(tǒng)需求的綜合分析，將CCP2的觸發(fā)采樣周期設(shè)為5 ms。中斷程序流程如圖9所示。

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

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

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