【導(dǎo)讀】本文基于互補(bǔ)濾波算法，設(shè)計(jì)了兩輪平衡車姿態(tài)角度測量電路與數(shù)據(jù)處理算法，設(shè)計(jì)了信號(hào)濾波預(yù)處理，利用互補(bǔ)濾波算法融合兩種傳感器數(shù)據(jù)，分析了互補(bǔ)濾波算法中關(guān)鍵參數(shù)的計(jì)算方法。并將此方法應(yīng)用于兩輪平衡車角度測量，進(jìn)行了驗(yàn)證性試驗(yàn)，給出了實(shí)驗(yàn)測試數(shù)據(jù)。
兩輪平衡車具有廣闊的應(yīng)用前景， 使其成為了當(dāng)前研究的熱點(diǎn)。其中，兩輪平衡車的姿態(tài)角度測量是研究的關(guān)鍵問題之一。姿態(tài)角度測量是兩輪平衡車運(yùn)行和控制實(shí)現(xiàn)的前提。姿態(tài)角度測量的精度和速度，將直接影響兩輪平衡車控制算法的穩(wěn)定性和可靠性。隨著慣性測量元件的微型化與微處理器運(yùn)算能力的提高，兩輪平衡車姿態(tài)測量普遍采用低成本的慣性測量組合元件（Inertial Measurement Uint，IMU），結(jié)合微處理器數(shù)據(jù)處理算法實(shí)現(xiàn)高精度的姿態(tài)測量。IMU 主要由低成本的MEMS 陀螺儀和三軸加速度計(jì)組成。MEMS 陀螺儀有自主性好、功耗低、機(jī)電性能好易集成等優(yōu)點(diǎn)。但是，MEMS 陀螺儀具有溫度漂移特性，其測量誤差會(huì)隨著時(shí)間的累加而不斷的累積，從而影響測量精度。加速度計(jì)會(huì)受到平衡車振動(dòng)的影響，混疊額外的振動(dòng)量干擾。所以單一的傳感器測量難以得到精確的姿態(tài)角度。需采用多傳感器信號(hào)融合的方法，來獲得準(zhǔn)確的姿態(tài)角度量。
多傳感器數(shù)據(jù)的融合方法有神經(jīng)網(wǎng)絡(luò)、小波分析、卡爾曼濾波等姿態(tài)解算算法，但這些方法建立穩(wěn)定可靠的更新方程通常具有較高的階數(shù)，且計(jì)算量大，不適合于低運(yùn)算能力系統(tǒng)的實(shí)時(shí)計(jì)算。相比以上方法，互補(bǔ)濾波算法對(duì)處理器運(yùn)算速度要求不高，且簡單可靠。本文基于互補(bǔ)濾波算法，設(shè)計(jì)了兩輪平衡車姿態(tài)角度測量電路與數(shù)據(jù)處理算法，設(shè)計(jì)了信號(hào)濾波預(yù)處理，利用互補(bǔ)濾波算法融合兩種傳感器數(shù)據(jù)，分析了互補(bǔ)濾波算法中關(guān)鍵參數(shù)的計(jì)算方法。并將此方法應(yīng)用于兩輪平衡車角度測量，進(jìn)行了驗(yàn)證性試驗(yàn)，給出了實(shí)驗(yàn)測試數(shù)據(jù)。
1 姿態(tài)角度測量原理
沿平衡車3 個(gè)機(jī)體軸即直立時(shí)正前、正右、正上方向定義為x、y、z 三軸參考坐標(biāo)系。所受的3 軸重力加速度分量定義為gx、gy、gz。假設(shè)兩輪平衡車處于靜止或勻速運(yùn) 行的狀態(tài)。得到重力加速度與平衡車姿態(tài)角度的關(guān)系如式1所示：
其中，Cbn為慣性坐標(biāo)系到載體坐標(biāo)系的變換矩陣；θ 為俯仰角；φ為橫滾角；g 為重力加速度； 可以通過測量重力加速度分量gx、gy、gz，計(jì)算出平衡車俯仰角θ1和橫滾角φ1估計(jì)值
若使用陀螺儀來測量平衡車姿態(tài)角度，設(shè)陀螺儀測量載體相對(duì)慣性坐標(biāo)系的x、y、z 三軸旋轉(zhuǎn)角速度分別為ωx、ωy、 ωz。并定義0 時(shí)刻平衡車直立靜止。可得到俯仰角θ2和橫滾角φ2估計(jì)值與ωx、ωy之間的關(guān)系如式3 所示:
在實(shí)際應(yīng)用中，由于平衡車機(jī)體運(yùn)行時(shí)存在運(yùn)動(dòng)加速度、測量噪聲， 以及陀螺儀本身存在漂移等因素的影響， 式（2）、（3）姿態(tài)角度測量方法失效，為了準(zhǔn)確的獲得姿態(tài)角度?？蓪⒁陨系? 種姿態(tài)角度測量得到的姿態(tài)角度信息相融合。
2 慣性組合測量電路
該系統(tǒng)中慣性組合測量電路如圖1 所示， 由加速度計(jì)MMA7361、陀螺儀ENC-03 及放大電路組成。實(shí)現(xiàn)對(duì)加速度計(jì)和陀螺儀測得信號(hào)進(jìn)行放大。加速度計(jì)和陀螺儀信號(hào)經(jīng)放大，分別由angle 引腳和gyro 引腳輸出后，信號(hào)通過AD 采樣轉(zhuǎn)換為數(shù)字信號(hào)，傳遞到微控制器中，再利用互補(bǔ)濾波算法，得到姿態(tài)角度。
圖1 慣性組合角度測量電路圖
[page]
3 陀螺儀漂移和加速度傳感器數(shù)據(jù)的預(yù)處理
MEMS 陀螺儀的漂移誤差由常值漂移、隨機(jī)漂移組成，漂移信號(hào)不符合平穩(wěn)、正態(tài)、零均值的時(shí)間序列特征。圖2 為陀螺儀采集到的一段數(shù)據(jù)， 對(duì)其積分后得到的俯仰角θ2，表現(xiàn)為誤差不斷累加，逐漸增大。通過式（4）所示算法進(jìn)行均值扣除。
圖2 角速度與角度的曲線
去掉陀螺儀信號(hào)的均值（即常值分量）后，這樣陀螺儀的漂移只含有隨機(jī)漂移，此時(shí)的陀螺儀信號(hào)將作為互補(bǔ)濾波融合的輸入。
加速度計(jì)受機(jī)體振動(dòng)的影響，對(duì)其采用滑動(dòng)均值濾波的方法對(duì)加速度傳感器原始數(shù)據(jù)進(jìn)行處理，濾波原理如式（5）所示：
圖3 濾波前后加速度計(jì)數(shù)據(jù)圖
[page]
4 基于互補(bǔ)濾波器的姿態(tài)角度測量設(shè)計(jì)
陀螺儀動(dòng)態(tài)響應(yīng)特性優(yōu)良，解算姿態(tài)角時(shí)，由于陀螺儀低頻漂移的影響，積分后低頻擾動(dòng)會(huì)產(chǎn)生較大誤差；加速度計(jì)解算的姿態(tài)角會(huì)受到平衡車運(yùn)行中機(jī)體高頻振動(dòng)的影響， 輸出角度中攜帶較大分量的高頻干擾。二者在頻域上具有互補(bǔ)特性，采用互補(bǔ)濾波器對(duì)這兩種傳感器數(shù)據(jù)融合，可提高姿態(tài)角度測量的精度和動(dòng)態(tài)響應(yīng)的性能。
互補(bǔ)濾波器的基本原理圖如圖4 所示。
圖4 互補(bǔ)濾波器的原理圖
其中θ 為實(shí)際的角度值，ω 為陀螺儀測量的角速度， 互補(bǔ)濾波算法后估計(jì)的角度值為互補(bǔ)濾波算法后估計(jì)的角度值， 加速度計(jì)測量中引入的高頻噪聲n1，陀螺儀測量中引入的低頻噪聲n2，用低通濾波器G1（s）消除加速度計(jì)中的高頻噪聲n1，用高通濾波器G2（s）消除陀螺儀中的低頻噪聲n2。兩個(gè)濾波器的傳遞函數(shù)， 被設(shè)計(jì)為（6）、（7）所示，圖4 結(jié)構(gòu)可化簡為圖5 結(jié)構(gòu)。
圖5 互補(bǔ)濾波器頻域圖
選用的濾波傳遞函數(shù)需滿足G1（s）+G2（s）=1，由加速度計(jì)得到θ1經(jīng)低通濾波器和陀螺儀得到θ2經(jīng)高通濾波器后的數(shù)據(jù)融合為角度估計(jì)值互補(bǔ)濾波算法后估計(jì)的角度值，適當(dāng)?shù)倪x取權(quán)重因子K 值，可以使系統(tǒng)中高、低通濾波器具有合適的截止率。得到穩(wěn)定的姿態(tài)角度。
由（8）式可看出，在小于截止頻率的低頻段，加速度計(jì)對(duì)姿態(tài)解算結(jié)果起主要作用；在大于截止頻率的高頻段，陀螺儀對(duì)姿態(tài)解算結(jié)果起主要作用。通過調(diào)整時(shí)間常數(shù)，改變?yōu)V波器的截止頻率τ，實(shí)現(xiàn)對(duì)陀螺儀和加速度計(jì)權(quán)重的調(diào)整。
[page]
5 實(shí)驗(yàn)驗(yàn)證
為了驗(yàn)證上述設(shè)計(jì)方案的可行性， 利用直立兩輪小車為實(shí)驗(yàn)驗(yàn)證平臺(tái)。本實(shí)驗(yàn)使用慣性測量組合元件（IMU）中的慣性傳感器分別選用了ENC-03（測量范圍：±300（deg/s））陀螺儀，采樣頻率為1.25 kHz 和MMA7361 加速度計(jì)（測量范圍：±1.5 g）。角度更新頻率為1.25 kHz。互補(bǔ)濾波器截止頻率為138 Hz。以俯仰角（θ）為例，進(jìn)行了測試。
5.1 角速度和角度用互補(bǔ)濾波算法融合的分析
把陀螺儀測得的角速度數(shù)據(jù)和加速度計(jì)測得的角度數(shù)據(jù)通過（10）式進(jìn)行融合后的波形如圖6 所示，從圖6 中可以看出， 經(jīng)互補(bǔ)濾波算法融合后得到角度消除了陀螺儀的漂移和加速度計(jì)的高頻擾動(dòng)，可得以下結(jié)論：
1）單從陀螺儀獲取的角速度積分后得到的角度是不正確的， 要把加速度計(jì)測得的角度值和陀螺儀測得角速度積分后的角度進(jìn)行互補(bǔ)濾波算法融合，提高角度精度。
2）經(jīng)互補(bǔ)濾波后陀螺儀的隨機(jī)漂移得到較為明顯的抑制，表現(xiàn)出了此互補(bǔ)濾波算法的有效性和優(yōu)越性。
圖6 角速度與角度的融合曲線圖
5.2 經(jīng)互補(bǔ)濾波處理后的角度與沒有經(jīng)處理后的角度比較分析
經(jīng)互補(bǔ)濾波處理后的角度與直接由加速度計(jì)測得的角度時(shí)域比較，對(duì)其互補(bǔ)濾波處理前后的信號(hào)進(jìn)行FFT 變換其頻譜圖如圖7 所示， 通過互補(bǔ)濾波算法可以降低隨機(jī)噪聲的干擾，可以使測得角度的波形更加的平滑。
圖7 融合前后角度比較圖
6 結(jié)論
文中分析了兩輪平衡車姿態(tài)角度解算時(shí)陀螺儀漂移和加速度計(jì)高頻擾動(dòng)的影響，針對(duì)陀螺儀漂移和加速度計(jì)高頻擾動(dòng)采用互補(bǔ)濾波融合加速度計(jì)和陀螺儀信號(hào)?；パa(bǔ)濾波能有效消除陀螺儀的漂移，抑制加速度計(jì)的高頻擾動(dòng)，減少輸出姿態(tài)角的動(dòng)態(tài)誤差，提高了角度測量精度，能夠滿足兩輪平衡車的姿態(tài)控制需要。實(shí)驗(yàn)結(jié)果表明了該方法的有效性，可推廣應(yīng)用于車載導(dǎo)航、兩輪平衡車、微小型機(jī)器人的姿態(tài)角度測量系統(tǒng)。
