不同姿態解算在平衡車中的實現與應用

貴陽學院電子與通信工程學院 古 訓 陳 紅

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

0 引言

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

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

圖1 MPU6050模塊

本文采用四種不同的姿態解算算法對平衡車姿態進行解算，對比不同姿態解算的效果以及在平衡車上的應用，選出平衡車姿態的最優估計，完成平衡車的姿態解算。

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

1 四種姿態解算的應用

1.1 一階互補濾波

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

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

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

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

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

1.2 二階互補濾波

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

圖4 二階互補濾波原理圖

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

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

經過參數整定，最終當加速度計取值的權重k=0.78時，其濾波效果如圖5所示。

1.3 匹配濾波

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

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

圖6 匹配濾波算法框圖

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

圖7 匹配濾波前后曲線

1.4 卡爾曼濾波

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

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

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

卡爾曼濾波的系統模型框圖如圖8所示。

圖8 卡爾曼濾波系統模型框圖

通過調節Q_angle、Q_gyro、R_angle三個系數，可以修改其濾波效果，經過一系列的參數調整、實驗最終得到如表1所示的參數，其濾波效果如圖9所示。

表1 卡爾曼可調參數

圖9 卡爾曼濾波前后曲線

2 四種姿態解算在平衡車上的實際對比

2.1 四種姿態解算的濾波曲線

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

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

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

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

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

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

圖12 小弧度的高頻抖動

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

圖13 水平移動

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

2.2 四種姿態解算的時間對比

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

表2 四種濾波解算執行時長

2.3 四種姿態解算在平衡車上應用

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

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

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

圖14 平衡車的直立效果

3 結束語

本文主要對四種濾波解算在平衡車上的濾波效果做出了相應的闡述，分析其四種濾波解算的濾波原理和實驗效果，以及對比四種濾波方案的濾波效果和動態平衡效果，選出了本次姿態解算的最佳濾波方 案，獲得平衡車姿態的最優估計，實現了平衡車的動態穩定，為后續的相關平衡車控制系統的研究打下基礎。