不同姿態(tài)解算在平衡車中的實現(xiàn)與應(yīng)用

貴陽學(xué)院電子與通信工程學(xué)院 古 訓(xùn) 陳 紅

雙輪平衡車是一種基于“動態(tài)穩(wěn)定”的兩輪機器人，它利用車載陀螺儀和加速度計傳感器實時檢測車身姿態(tài)，控制電機的轉(zhuǎn)速使平衡車保持動態(tài)穩(wěn)定。利用MPU6050常用的姿態(tài)解算有一階互補濾波、二階互補濾波、匹配濾波以及卡爾曼濾波等算法。本文分別對四種算法進(jìn)行分析以及實驗驗證，獲取最佳的姿態(tài)解算方案，完成平衡車姿態(tài)角度的最佳估計，實現(xiàn)平衡車直立的動態(tài)穩(wěn)定。

0 引言

隨著城市經(jīng)濟(jì)的發(fā)展，城市交通需求迅速增長。為了解決城市交通的問題，引進(jìn)了新的代步工具——平衡車，它體積小，便于攜帶，同時采用電能驅(qū)動，節(jié)能、環(huán)保，提高了人們出行的效率。平衡車主要利用車載傳感器檢測車體姿態(tài)角度的變化，然后通過控制系統(tǒng)驅(qū)動電機，達(dá)到兩輪直立自平衡的效果。

本文通過MPU6050模塊（賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計[J].河南工程學(xué)院學(xué)報(自然科學(xué)版),2014,26(01):53-57）獲取加速度和角速度，對車體姿態(tài)進(jìn)行識別。MPU6050模塊為整合性6軸運動處理組件，內(nèi)置3軸陀螺儀，3軸加速度計，如圖1所示。其中陀螺儀輸出的角速度積分就可以得到角度值，但是由于角速度存在微小的偏差和漂移，經(jīng)過積分運算之后就會形成累積誤差。加速度計恰好和陀螺儀特性相反：沒有累積誤差。但是由于平衡車在運動時，極易受到外部的干擾，采集加速度值時會產(chǎn)生很大的噪聲信號。所以單獨使用陀螺儀或加速度計估計平衡車的姿態(tài)是很難實現(xiàn)的。因此需要采用濾波算法將其加速度計和陀螺儀之間的數(shù)據(jù)進(jìn)行融合，計算出平衡車偏轉(zhuǎn)角度的最優(yōu)估計值，完成平衡車的姿態(tài)解算。

圖1 MPU6050模塊

本文采用四種不同的姿態(tài)解算算法對平衡車姿態(tài)進(jìn)行解算，對比不同姿態(tài)解算的效果以及在平衡車上的應(yīng)用，選出平衡車姿態(tài)的最優(yōu)估計，完成平衡車的姿態(tài)解算。

圖2 一階互補濾波算法框圖

1 四種姿態(tài)解算的應(yīng)用

1.1 一階互補濾波

一階互補濾波（付勇杰,丁艷紅,梁義維,行陽陽.動態(tài)傾角傳感器及其傳遞特性的研究[J].儀表技術(shù)與傳感器,2012(09):6-8）融合了陀螺儀提供的高頻角速度信號和加速度計提供的低頻角度信號，采用高通濾波算法抑制陀螺儀的積分漂移,低通濾波算法除去加速度計短時間快速變化的信號，分別濾出相應(yīng)的噪聲信號，補償陀螺儀的累積誤差和加速度的動態(tài)誤差，得到平衡車姿態(tài)的估計值。算法框圖如圖2所示。

一階互補濾波的表達(dá)式（梁光勝,杜夢楠,周子豪,劉春旭,文月.基于互補濾波的兩輪自平衡車姿態(tài)控制[J].測控技術(shù),2015,34(05):72-74）如公式(1)所示。

上式中，anglen為第n次濾波后的角度，anglen-1為第n-1次濾波的角度，a為加速度取值的權(quán)重，其取值范圍為0～1，angle_m為加速度計的采樣值，gyro_m為陀螺儀的角速度采樣值，dt為采樣時間。

經(jīng)過參數(shù)調(diào)整，最終得到當(dāng)a=0.08時，其濾波效果如圖3所示。其中曲線1表示濾波前的角度量綱值，曲線2為濾波后的角度量綱值（圖5、7、9中曲線1和曲線2的含義同圖3）。

圖3 一階互補濾波前后曲線

1.2 二階互補濾波

二階互補濾波算法（南余榮,宛冬晴,潘帥,姚亞楠.二階互補濾波姿態(tài)求解器的設(shè)計[J].浙江工業(yè)大學(xué)學(xué)報,2017,45(04):416-420）主要是在一階互補濾波算法的補償環(huán)節(jié)加入積分器，消除陀螺儀漂移誤差，其原理框圖如圖4所示。

圖4 二階互補濾波原理圖

其中，ωg為陀螺儀測量的角速度，θa為加速度計測量的角度值，Kp為比例系數(shù)，Ki為積分系數(shù)，其中。

時域微分形式如公式(2)所示。

經(jīng)過參數(shù)整定，最終當(dāng)加速度計取值的權(quán)重k=0.78時，其濾波效果如圖5所示。

1.3 匹配濾波

匹配濾波（龔曉輝,薄祥岑.基于MC9S12XS128的二輪直立車設(shè)計與實現(xiàn)[J].電子設(shè)計工程,2014,22(02):137-139）主要是利用加速度計所獲得的角度θg與陀螺儀積分后的角度θ進(jìn)行比較，將比較的誤差信號經(jīng)過比例Tg放大，然后與陀螺儀輸出的角速度信號疊加，再進(jìn)行積分。對于加速度計給定的角度θg，經(jīng)過比例、積分環(huán)節(jié)使產(chǎn)生的角度θ最終等于θg。這樣就可以消除陀螺儀的累積誤差，其算法框圖如圖6所示。

圖5 二階互補濾波前后曲線

圖6 匹配濾波算法框圖

通過修改Tg，改變其濾波的效果，最終當(dāng)Tg=0.08時，得到最佳的濾波效果，如圖7所示，其中曲線1為濾波前的角度量綱值，曲線2為濾波后的角度量綱值。

圖7 匹配濾波前后曲線

1.4 卡爾曼濾波

卡爾曼濾波（彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)刊,2009,8(11):32-34）（Kalman filtering）是利用線性系統(tǒng)狀態(tài)方程通過系統(tǒng)輸入輸出觀測數(shù)據(jù)對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。其主要思想是：以最小均方誤差作為最優(yōu)估計準(zhǔn)則，利用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和當(dāng)前時刻的觀測量，更新對狀態(tài)變量的估計，并計算當(dāng)前時刻的估計值。

卡爾曼濾波的遞推過程（楊凌霄,李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測方法[J].計算機仿真,2014,31(06):406-409）如公式(3)所示。

上式中，AK為系統(tǒng)矩陣，BK為控制輸入矩陣，HK為測量矩陣，KK為卡爾曼增益，PK為誤差的協(xié)方差矩陣，Q為過程噪聲協(xié)方差矩陣，R為測量噪聲協(xié)方差矩陣。

卡爾曼濾波的系統(tǒng)模型框圖如圖8所示。

圖8 卡爾曼濾波系統(tǒng)模型框圖

通過調(diào)節(jié)Q_angle、Q_gyro、R_angle三個系數(shù)，可以修改其濾波效果，經(jīng)過一系列的參數(shù)調(diào)整、實驗最終得到如表1所示的參數(shù)，其濾波效果如圖9所示。

表1 卡爾曼可調(diào)參數(shù)

圖9 卡爾曼濾波前后曲線

2 四種姿態(tài)解算在平衡車上的實際對比

2.1 四種姿態(tài)解算的濾波曲線

圖10至13是四種濾波方案在相同情況下的濾波效果比對，曲線1為一階互補濾波，曲線2為二階互補濾波，曲線3為匹配濾波，曲線4為卡爾曼濾波方案。

將平衡車快速傾斜，靜止，然后快速回位，得到圖10的曲線。采用這種方式主要是為了測試平衡車傾角在劇烈變化時，曲線的平滑效果。從圖上可以看出，四條曲線的收斂速度差不多，但是從平滑效果上看，曲線1與曲線3的平滑效果不是很好，有嚴(yán)重的超調(diào)現(xiàn)象，其中平滑效果最好的是曲線2，曲線4的平滑效果比較適中。

圖10 快速傾斜、靜止、回位

將平衡車緩慢傾斜，靜止，然后緩慢回位，得到圖11的曲線。采取這種方式主要是為了測試平衡車傾角的跟隨性，即曲線的收斂速度。從圖上可以看出，曲線1、3、4的收斂速度差不多，響應(yīng)都很靈敏，曲線2有點滯后。

圖11 緩慢傾斜、靜止、回位

圖12是平衡車在原地小弧度的高頻抖動時，得到的曲線圖。主要是為了測試平衡車直立時的高頻震蕩時，四條濾波的濾波效果。從圖12上，我們可以發(fā)現(xiàn)曲線1、3的波動很大，其濾波效果很差，曲線4有小幅度緩慢的起伏，相對于曲線1、3、4，曲線2的濾波效果是最好的。

圖12 小弧度的高頻抖動

圖13是模仿平衡車在水平面上水平移動，可以觀測到曲線1、3的波動大，濾波效果相對于曲線2、4效果不是很好。

圖13 水平移動

綜上四個圖來看，曲線1——一階互補濾波的收斂速度雖然很快，但是其平滑效果很差；曲線2——二階互補濾波的平滑效果雖然最好，但是收斂速度相對其他三種濾波方案要慢；曲線3——匹配濾波的濾波效果和一階互補濾波的濾波效果差不多，收斂速度快，但是曲線的平滑效果不是很好；與之相比，曲線4——卡爾曼濾波的收斂速度和平滑效果平衡得較好，比較適中。

2.2 四種姿態(tài)解算的時間對比

表2是四種濾波方案的濾波所執(zhí)行的時間表，采用定時器中斷的方法，測量四種算法執(zhí)行所需要的時間，由于卡爾曼濾波算法相對其它三種濾波要復(fù)雜的多，所以其濾波所需的時間相對較長，而一階互補濾波算法是最簡單的，所需的時間最短。

表2 四種濾波解算執(zhí)行時長

2.3 四種姿態(tài)解算在平衡車上應(yīng)用

本文利用四種姿態(tài)解算實現(xiàn)平衡車的動態(tài)直立穩(wěn)定，觀測四種姿態(tài)解算在平衡車動態(tài)直立時，突然受到外界干擾平衡車的動態(tài)效果。

通過這個實驗測試發(fā)現(xiàn)一階互補濾波和匹配濾波在受到外界干擾時，平衡車會在原地附近高頻的前后抖動，經(jīng)過一段時間后才能趨于穩(wěn)定。以此說明了一階互補濾波和匹配濾波響應(yīng)速度很快，但是由于存在角度超調(diào)的現(xiàn)象，使平衡車在突然受到外界干擾時會產(chǎn)生高頻的來回抖動，不能快速的回到平衡位置。相對于一階互補濾波和匹配濾波，二階互補濾波特性恰好與這兩種濾波相反。采用二階互補濾波時，如果平衡車突然受到外界干擾，平衡車回到平衡位置的收斂速度沒有階互補濾波和匹配濾快，但是二階互補不會出現(xiàn)超調(diào)、來回抖動的現(xiàn)象。而卡爾曼濾波恰好融其了三種濾波的特點，在受到外界干擾時，使用卡爾曼濾波算法的平衡車能夠快速的回到平衡位置。

綜上，從平滑角度上看，匹配濾波和一階互補濾波的平滑效果沒有卡爾曼濾波的效果好；從收斂速度上看，二階互補濾波的收斂速度沒有卡爾曼濾波的收斂速度快；從濾波所需時間上看，卡爾曼濾波相對其他曲線所需時間較長；從動態(tài)直立的效果上看，卡爾曼濾波綜合了其它三種濾波的優(yōu)勢，在受到外界干擾時快速的回到平衡位置。結(jié)合平滑效果、收斂速度和直立的動態(tài)效果，最終采用卡爾曼濾波的方案對角度進(jìn)行濾波，完成平衡車的姿態(tài)估計，實現(xiàn)平衡車的動態(tài)穩(wěn)定，如圖14所示。

圖14 平衡車的直立效果

3 結(jié)束語

本文主要對四種濾波解算在平衡車上的濾波效果做出了相應(yīng)的闡述，分析其四種濾波解算的濾波原理和實驗效果，以及對比四種濾波方案的濾波效果和動態(tài)平衡效果，選出了本次姿態(tài)解算的最佳濾波方 案，獲得平衡車姿態(tài)的最優(yōu)估計，實現(xiàn)了平衡車的動態(tài)穩(wěn)定，為后續(xù)的相關(guān)平衡車控制系統(tǒng)的研究打下基礎(chǔ)。