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

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

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

0 引言

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

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

圖1 MPU6050模塊

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

圖2 一階互補(bǔ)濾波算法框圖

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

1.1 一階互補(bǔ)濾波

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

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

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

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

圖3 一階互補(bǔ)濾波前后曲線

1.2 二階互補(bǔ)濾波

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

圖4 二階互補(bǔ)濾波原理圖

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

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

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

1.3 匹配濾波

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

圖5 二階互補(bǔ)濾波前后曲線

圖6 匹配濾波算法框圖

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

圖7 匹配濾波前后曲線

1.4 卡爾曼濾波

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

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

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

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

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

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

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

圖9 卡爾曼濾波前后曲線

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

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

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

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

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

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

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

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

圖12 小弧度的高頻抖動(dòng)

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

圖13 水平移動(dòng)

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

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

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

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

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

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

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

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

圖14 平衡車的直立效果

3 結(jié)束語

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