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

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

兩輪平衡車(chē)具有廣闊的應(yīng)用前景，使其成為了當(dāng)前研究的熱點(diǎn)。其中，兩輪平衡車(chē)的姿態(tài)角度測(cè)量是研究的關(guān)鍵問(wèn)題之一。姿態(tài)角度測(cè)量是兩輪平衡車(chē)運(yùn)行和控制實(shí)現(xiàn)的前提。姿態(tài)角度測(cè)量的精度和速度，將直接影響兩輪平衡車(chē)控制算法的穩(wěn)定性和可靠性。隨著慣性測(cè)量元件的微型化與微處理器運(yùn)算能力的提高，兩輪平衡車(chē)姿態(tài)測(cè)量普遍采用低成本的慣性測(cè)量組合元件(Inertal Measurement Uint，IMU)，結(jié)合微處理器數(shù)據(jù)處理算法實(shí)現(xiàn)高精度的姿態(tài)測(cè)量。IMU主要由低成本的MEMS陀螺儀和三軸加速度計(jì)組成。MEMS陀螺儀有自主性好、功耗低、機(jī)電性能好易集成等優(yōu)點(diǎn)。但是，MEMS陀螺儀具有溫度漂移特性，其測(cè)量誤差會(huì)隨著時(shí)間的累加而不斷的累積，從而影響測(cè)量精度。加速度計(jì)會(huì)受到平衡車(chē)振動(dòng)的影響，混疊額外的振動(dòng)量干擾。所以單一的傳感器測(cè)量難以得到精確的姿態(tài)角度。需采用多傳感器信號(hào)融合的方法，來(lái)獲得準(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)算速度要求不高，且簡(jiǎn)單可靠。本文基于互補(bǔ)濾波算法，設(shè)計(jì)了兩輪平衡車(chē)姿態(tài)角度測(cè)量電路與數(shù)據(jù)處理算法，設(shè)計(jì)了信號(hào)濾波預(yù)處理，利用互補(bǔ)濾波算法融合兩種傳感器數(shù)據(jù)，分析了互補(bǔ)濾波算法中關(guān)鍵參數(shù)的計(jì)算方法。并將此方法應(yīng)用于兩輪平衡車(chē)角度測(cè)量，進(jìn)行了驗(yàn)證性試驗(yàn)，給出了實(shí)驗(yàn)測(cè)試數(shù)據(jù)。

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

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

其中，為慣性坐標(biāo)系到載體坐標(biāo)系的變換矩陣：θ為俯仰角;φ為橫滾角;g為重力加速度;可以通過(guò)測(cè)量重力加速度分量gx、gy、gz，計(jì)算出平衡車(chē)俯仰角θ1和橫滾角φ1.估計(jì)值

若使用陀螺儀來(lái)測(cè)量平衡車(chē)姿態(tài)角度，設(shè)陀螺儀測(cè)量載體相對(duì)慣性坐標(biāo)系的x、y、z三軸旋轉(zhuǎn)角速度分別為ωx、ωy、ωz.并定義O時(shí)刻平衡車(chē)直立靜止?？傻玫礁┭鼋?theta;2和橫滾角φ2估計(jì)值與ωx、ωy之間的關(guān)系如式3所示：

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

2慣性組合測(cè)量電路

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

圖1 慣性組合角度測(cè)量電路圖

3陀螺儀漂移和加速度傳感器數(shù)據(jù)的預(yù)處理

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

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

圖2 角速度與角度的曲線

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

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

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

圖3 濾波前后加速度計(jì)數(shù)據(jù)圖