基于互補濾波器的兩輪平衡車姿態(tài)角度測量

摘要：微機電系統(tǒng)(Micro—Electro—Mechanical Systems，MEMS)陀螺儀和MEMS加速度計在兩輪平衡車姿態(tài)測量中存在擾動和噪聲，引起姿態(tài)角度測量誤差。通過對陀螺儀和加速度計輸入信號進行滑動扣除均值方法來抑制直流分量，利用滑動濾波算法抑制加速度計高頻噪聲，引入互補濾波算法將預處理后的陀螺儀和加速度計信號進行融合，得到更加準確穩(wěn)定的角度測量值，分析了融合算法中加權因子與濾波頻率特征之間的關系。該方法應用到兩輪平衡車的運行姿態(tài)角度控制中，提高了對姿態(tài)角度測量的精度。

兩輪平衡車具有廣闊的應用前景，使其成為了當前研究的熱點。其中，兩輪平衡車的姿態(tài)角度測量是研究的關鍵問題之一。姿態(tài)角度測量是兩輪平衡車運行和控制實現(xiàn)的前提。姿態(tài)角度測量的精度和速度，將直接影響兩輪平衡車控制算法的穩(wěn)定性和可靠性。隨著慣性測量元件的微型化與微處理器運算能力的提高，兩輪平衡車姿態(tài)測量普遍采用低成本的慣性測量組合元件(Inertal Measurement Uint，IMU)，結合微處理器數據處理算法實現(xiàn)高精度的姿態(tài)測量。IMU主要由低成本的MEMS陀螺儀和三軸加速度計組成。MEMS陀螺儀有自主性好、功耗低、機電性能好易集成等優(yōu)點。但是，MEMS陀螺儀具有溫度漂移特性，其測量誤差會隨著時間的累加而不斷的累積，從而影響測量精度。加速度計會受到平衡車振動的影響，混疊額外的振動量干擾。所以單一的傳感器測量難以得到精確的姿態(tài)角度。需采用多傳感器信號融合的方法，來獲得準確的姿態(tài)角度量。

多傳感器數據的融合方法有神經網絡、小波分析、卡爾曼濾波等姿態(tài)解算算法，但這些方法建立穩(wěn)定可靠的更新方程通常具有較高的階數，且計算量大，不適合于低運算能力系統(tǒng)的實時計算。相比以上方法，互補濾波算法對處理器運算速度要求不高，且簡單可靠。本文基于互補濾波算法，設計了兩輪平衡車姿態(tài)角度測量電路與數據處理算法，設計了信號濾波預處理，利用互補濾波算法融合兩種傳感器數據，分析了互補濾波算法中關鍵參數的計算方法。并將此方法應用于兩輪平衡車角度測量，進行了驗證性試驗，給出了實驗測試數據。

1 姿態(tài)角度測量原理

沿平衡車3個機體軸即直立時正前、正右、正上方向定義為x、y、z三軸參考坐標系。所受的3軸重力加速度分量定義為gx、gy、gz。假設兩輪平衡車處于靜止或勻速運行的狀態(tài)。得到重力加速度與平衡車姿態(tài)角度的關系如式1所示：

若使用陀螺儀來測量平衡車姿態(tài)角度，設陀螺儀測量載體相對慣性坐標系的x、y、z三軸旋轉角速度分別為ωx、ωy、ωz。并定義O時刻平衡車直立靜止。可得到俯仰角θ2和橫滾角φ2估計值與ωx、ωy之間的關系如式3所示：

在實際應用中，由于平衡車機體運行時存在運動加速度、測量噪聲，以及陀螺儀本身存在漂移等因素的影響，式(2)、(3)姿態(tài)角度測量方法失效，為了準確的獲得姿態(tài)角度?？蓪⒁陨系?種姿態(tài)角度測量得到的姿態(tài)角度信息相融合。

2 慣性組合測量電路

該系統(tǒng)中慣性組合測量電路如圖1所示，由加速度計MMA7361、陀螺儀ENC-03及放大電路組成。實現(xiàn)對加速度計和陀螺儀測得信號進行放大。加速度計和陀螺儀信號經放大，分別由angle引腳和gyro引腳輸出后，信號通過AD采樣轉換為數字信號，傳遞到微控制器中，再利用互補濾波算法，得到姿態(tài)角度。

3 陀螺儀漂移和加速度傳感器數據的預處理

MEMS陀螺儀的漂移誤差由常值漂移、隨機漂移組成，漂移信號不符合平穩(wěn)、正態(tài)、零均值的時間序列特征。圖2為陀螺儀采集到的一段數據，對其積分后得到的俯仰角θ2，表現(xiàn)為誤差不斷累加，逐漸增大。通過式(4)所示算法進行均值扣除。

其中，ωgy(n)為第n次的陀螺儀角速度值;ω(n)為第n次去除均值后的角速度值;N為緩沖數據個數。

去掉陀螺儀信號的均值(即常值分量)后，這樣陀螺儀的漂移只含有隨機漂移，此時的陀螺儀信號將作為互補濾波融合的輸入。

加速度計受機體振動的影響，對其采用滑動均值濾波的方法對加速度傳感器原始數據進行處理，濾波原理如式(5)所示：

其中，θocc(n)為加速度計第n次測得的角度值;θ1(n)為第n次滑動濾波后的角度值：N為滑動窗階數。圖3給出了滑動濾波前后的加速度計測得的角度值，從圖3中可以看出，滑動濾波算法對高頻擾動具有濾除效果。

4 基于互補濾波器的姿態(tài)角度測量設計

陀螺儀動態(tài)響應特性優(yōu)良，解算姿態(tài)角時，由于陀螺儀低頻漂移的影響，積分后低頻擾動會產生較大誤差;加速度計解算的姿態(tài)角會受到平衡車運行中機體高頻振動的影響，輸出角度中攜帶較大分量的高頻干擾。二者在頻域上具有互補特性，采用互補濾波器對這兩種傳感器數據融合，可提高姿態(tài)角度測量的精度和動態(tài)響應的性能。

互補濾波器的基本原理圖如圖4所示。

其中θ為實際的角度值，ω為陀螺儀測量的角速度，θ為互補濾波算法后估計的角度值，加速度計測量中引入的高頻噪聲n1，陀螺儀測量中引入的低頻噪聲n2，用低通濾波器G1(s)消除加速度計中的高頻噪聲n1，用高通濾波器G2(s)消除陀螺儀中的低頻噪聲n2。兩個濾波器的傳遞函數，被設計為(6)、(7)所示，圖4結構可化簡為圖5結構。

選用的濾波傳遞函數需滿足G1(s)+G2(s)=1，由加速度計得到θ1經低通濾波器和陀螺儀得到θ2經高通濾波器后的數據融合為角度估計值，適當的選取權重因子K值，可以使系統(tǒng)中高、低通濾波器具有合適的截止率。得到穩(wěn)定的姿態(tài)角度。

由(8)式可看出，在小于截止頻率的低頻段，加速度計對姿態(tài)解算結果起主要作用;在大于截止頻率的高頻段，陀螺儀對姿態(tài)解算結果起主要作用。通過調整時間常數，改變?yōu)V波器的截止頻率τ，實現(xiàn)對陀螺儀和加速度計權重的調整。

5 實驗驗證

為了驗證上述設計方案的可行性，利用直立兩輪小車為實驗驗證平臺。本實驗使用慣性測量組合元件(IMU)中的慣性傳感器分別選用了ENC—03(測量范圍：±300(deg/s))陀螺儀，采樣頻率為1.25 kHz和MMA7361加速度計(測量范圍：±1.5 g)。角度更新頻率為1.25 kHz?；パa濾波器截止頻率為138 Hz。以俯仰角(θ)為例，進行了測試。

5.1 角速度和角度用互補濾波算法融合的分析

把陀螺儀測得的角速度數據和加速度計測得的角度數據通過(10)式進行融合后的波形如圖6所示，從圖6中可以看出，經互補濾波算法融合后得到角度消除了陀螺儀的漂移和加速度計的高頻擾動，可得以下結論：

1)單從陀螺儀獲取的角速度積分后得到的角度是不正確的，要把加速度計測得的角度值和陀螺儀測得角速度積分后的角度進行互補濾波算法融合，提高角度精度。

2)經互補濾波后陀螺儀的隨機漂移得到較為明顯的抑制，表現(xiàn)出了此互補濾波算法的有效性和優(yōu)越性。

5. 2 經互補濾波處理后的角度與沒有經處理后的角度比較分析

經互補濾波處理后的角度與直接由加速度計測得的角度時域比較，對其互補濾波處理前后的信號進行FFT變換其頻譜圖如圖7所示，通過互補濾波算法可以降低隨機噪聲的干擾，可以使測得角度的波形更加的平滑。

6 結論

文中分析了兩輪平衡車姿態(tài)角度解算時陀螺儀漂移和加速度計高頻擾動的影響，針對陀螺儀漂移和加速度計高頻擾動采用互補濾波融合加速度計和陀螺儀信號。互補濾波能有效消除陀螺儀的漂移，抑制加速度計的高頻擾動，減少輸出姿態(tài)角的動態(tài)誤差，提高了角度測量精度，能夠滿足兩輪平衡車的姿態(tài)控制需要。實驗結果表明了該方法的有效

性，可推廣應用于車載導航、兩輪平衡車、微小型機器人的姿態(tài)角度測量系統(tǒng)。