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