呂傳龍，曹華杰，劉浩東

(1.西南交通大學(xué) 電氣工程學(xué)院，四川 成都 610031;2.西南交通大學(xué) 信息科學(xué)與技術(shù)學(xué)院，四川 成都 611756)

自平衡機(jī)器人中數(shù)據(jù)融合算法的研究與實(shí)現(xiàn)

呂傳龍1，曹華杰2，劉浩東1

(1.西南交通大學(xué) 電氣工程學(xué)院，四川 成都 610031;2.西南交通大學(xué) 信息科學(xué)與技術(shù)學(xué)院，四川 成都 611756)

輪式自平衡機(jī)器人是一種通用的機(jī)器人，它適用于各種復(fù)雜環(huán)境中，通過(guò)陀螺儀和加速度計(jì)采集的數(shù)據(jù)可以控制其平衡性，但是不夠精確和實(shí)時(shí)。為了進(jìn)一步提高系統(tǒng)的響應(yīng)，文中通過(guò)離散卡爾曼算法將ENC03陀螺儀和MMA7260加速度計(jì)采集的數(shù)據(jù)進(jìn)行融合輸出。首先研究了傳統(tǒng)的卡爾曼算法，然后建立了適合自平衡機(jī)器人的算法模型，最后介紹了其工程實(shí)現(xiàn)。通過(guò)實(shí)時(shí)監(jiān)測(cè)可以看出，在未進(jìn)行角度融合之前，加速度計(jì)計(jì)算得出的角度在靜態(tài)和動(dòng)態(tài)都有著極大的噪聲。經(jīng)過(guò)卡爾曼濾波算法融合后的角度平滑穩(wěn)定，達(dá)到了預(yù)期效果。

自平衡機(jī)器人；加速度計(jì)；陀螺儀；卡爾曼算法；數(shù)據(jù)融合

0 引 言

輪式自平衡機(jī)器人是控制科學(xué)的一個(gè)重要研究平臺(tái)，同時(shí)也是實(shí)際應(yīng)用的重要工具。20世紀(jì)90年代，日本開(kāi)展了平衡機(jī)器人的相關(guān)研究，為后來(lái)的移動(dòng)平衡機(jī)器人學(xué)建立了相關(guān)研究基礎(chǔ)。21世紀(jì)初，自平衡機(jī)器人得到了越來(lái)越多的關(guān)注，國(guó)外高校逐漸采用其作為先進(jìn)理論的驗(yàn)證平臺(tái)，機(jī)器人公司也開(kāi)始將輪式自平衡機(jī)器人進(jìn)行商業(yè)化。比如，美國(guó)公司賽格威研究開(kāi)發(fā)兩輪自平衡代步車(chē)，除了已經(jīng)在個(gè)人用戶(hù)的使用上得到推廣外，在機(jī)場(chǎng)、高檔社區(qū)和運(yùn)動(dòng)場(chǎng)館等大型空間內(nèi)也得到使用。近年來(lái)，越來(lái)越多的互聯(lián)網(wǎng)公司加入平衡機(jī)器人的開(kāi)發(fā)中，對(duì)平衡機(jī)器人的推廣普及有著深刻影響。

1 傳感器數(shù)據(jù)采集

輪式自平衡機(jī)器人的穩(wěn)定運(yùn)行建立在正確獲得其姿態(tài)信息的基礎(chǔ)上，通過(guò)一定的控制算法驅(qū)動(dòng)電機(jī)保持平衡穩(wěn)定狀態(tài)。其姿態(tài)信息主要是指在靜止或者平衡狀態(tài)下，機(jī)器人重心與豎直方向的傾角。機(jī)器人姿態(tài)的信息需要兼顧其動(dòng)態(tài)特性和靜態(tài)精度，傾角測(cè)量需要滿(mǎn)足動(dòng)態(tài)與靜態(tài)性能指標(biāo)。靜態(tài)指標(biāo)是指在穩(wěn)定狀態(tài)下可以長(zhǎng)時(shí)間正確表示實(shí)際觀(guān)察量；動(dòng)態(tài)指標(biāo)是指當(dāng)測(cè)量的物理量在高速運(yùn)動(dòng)狀態(tài)下，仍可以快速跟隨狀態(tài)變化，并且具有一定的預(yù)測(cè)性。為了得到機(jī)器人的復(fù)合要求的姿態(tài)傾角，可以采用陀螺儀的角速度動(dòng)態(tài)特性和加速度計(jì)的靜態(tài)精度相結(jié)合的方法[1]。

陀螺儀是一種利用科里奧利力原理，對(duì)旋轉(zhuǎn)物體測(cè)量其角速度的元件。陀螺儀適應(yīng)于系統(tǒng)運(yùn)動(dòng)時(shí)的角速度信息，對(duì)其積分可以得到實(shí)際運(yùn)動(dòng)中的傾角。然而，受到陀螺漂移等因素的影響，產(chǎn)生的微小誤差經(jīng)過(guò)積分作用之后，會(huì)使測(cè)量的角度產(chǎn)生巨大的偏差。因此，應(yīng)對(duì)的主要是陀螺漂移的低頻噪聲[2]。為此，需要設(shè)計(jì)濾波電路對(duì)陀螺儀輸出信號(hào)進(jìn)行濾波，將低頻噪聲濾除，同時(shí)要保證系統(tǒng)高頻信號(hào)不能達(dá)到陀螺儀的共振頻率。

加速度計(jì)是一種測(cè)量重力加速度分量以及運(yùn)動(dòng)加速度的元件。對(duì)于輪式自平衡機(jī)器人，利用加速度計(jì)元件測(cè)量重力加速度分量，進(jìn)行反三角函數(shù)運(yùn)算得到機(jī)器人重心與豎直方向的夾角，作為機(jī)器人車(chē)體的傾角。加速度計(jì)在運(yùn)行中易受到平臺(tái)震動(dòng)這樣的高頻噪聲的影響。設(shè)計(jì)中采用飛思卡爾半導(dǎo)體公司出品的加速度計(jì)MMA7260。根據(jù)公式可以推導(dǎo)出傾角的變化量：

Δu=kgsinθ

(1)

其中，u是加速度計(jì)輸出電壓值；g是重力加速度；θ是車(chē)模傾角；k是比例系數(shù)。

2 卡爾曼數(shù)據(jù)算法融合研究

得到機(jī)器人的姿態(tài)信息是平衡車(chē)系統(tǒng)最具挑戰(zhàn)的部分之一。陀螺儀和加速度計(jì)分別為不同的角度傳感器，在該系統(tǒng)中采用雙傳感器進(jìn)行角度測(cè)量。然而，加速度計(jì)容易受到自身器件的噪聲、電機(jī)噪聲以及陀螺儀混疊的影響，而且在高速運(yùn)動(dòng)的體系下具有嚴(yán)重的滯后特性；陀螺儀噪聲小，但是存在著溫度漂移現(xiàn)象，靜態(tài)性能不足以達(dá)到要求。為了保留加速度計(jì)的靜態(tài)性能和陀螺儀的動(dòng)態(tài)性能，使兩者優(yōu)勢(shì)可以相互結(jié)合，文中采用多傳感器信息融合算法中的濾波器融合算法[3]。

典型的數(shù)據(jù)融合算法大致分為兩類(lèi)：一類(lèi)是從頻率域來(lái)分辨、消除噪聲，不考慮信號(hào)及噪聲的統(tǒng)計(jì)特性，例如互補(bǔ)濾波器[4]；另一類(lèi)是使用狀態(tài)空間法在時(shí)域內(nèi)設(shè)計(jì)濾波器，例如卡爾曼濾波器[5]?；パa(bǔ)濾波的基本思想是通過(guò)不同傳感器具有互補(bǔ)作用的特性進(jìn)行角度融合。對(duì)于輪式機(jī)器人采用互補(bǔ)濾波的方式進(jìn)行濾波，需要實(shí)時(shí)地將時(shí)域信息轉(zhuǎn)換到頻域空間，在頻域中設(shè)計(jì)針對(duì)陀螺儀和加速度計(jì)的濾波器，最后進(jìn)行互補(bǔ)融合。

針對(duì)上述所選濾波器進(jìn)行如下設(shè)計(jì)：利用高通濾波器抑制陀螺儀積分的漂移，利用低通濾波器濾除加速度計(jì)的短時(shí)性快速變化的信號(hào)，將處理后的數(shù)據(jù)進(jìn)行融合以得到小車(chē)的實(shí)際傾角。最后將加速度濾波和陀螺儀濾波的角度各乘以一個(gè)權(quán)重，其權(quán)重和為1即可[6]。其濾波過(guò)程如圖1所示。然而在復(fù)雜的情況下，濾波器的帶寬難以動(dòng)態(tài)選擇，這給互補(bǔ)濾波器的設(shè)計(jì)帶來(lái)了困難。

圖1 復(fù)合互補(bǔ)濾波器

相對(duì)于其他濾波方式，卡爾曼濾波有著諸多優(yōu)點(diǎn)：采用狀態(tài)空間法設(shè)計(jì)濾波器，用狀態(tài)方程表征系統(tǒng)的運(yùn)動(dòng)特性，從而避免了在頻域中討論和設(shè)計(jì)濾波器的繁瑣，簡(jiǎn)單易行[7]；在遞推運(yùn)算中，實(shí)時(shí)量測(cè)信息經(jīng)提煉被濃縮在估計(jì)值中，而不必存儲(chǔ)時(shí)間過(guò)程中的量測(cè)量[8]。所以，卡爾曼濾波能適用于白噪聲激勵(lì)的任何平穩(wěn)或非平穩(wěn)隨機(jī)向量過(guò)程的估計(jì)，所得估計(jì)在線(xiàn)性估計(jì)中精度最佳。

2.1 卡爾曼濾波算法

卡爾曼濾波是一種遞推線(xiàn)性最小方差估計(jì)，其設(shè)計(jì)準(zhǔn)則與線(xiàn)性最小方差估計(jì)相同，估值同樣是量測(cè)值的線(xiàn)性函數(shù)。由于卡爾曼濾波算法使用遞推算法，啟動(dòng)時(shí)需要給定初始值在內(nèi)的系統(tǒng)初值。給定的初值在滿(mǎn)足一定的條件下，濾波過(guò)程中估計(jì)可以始終無(wú)偏[9]。文獻(xiàn)[10]中的算法利用反饋控制來(lái)實(shí)現(xiàn)對(duì)狀態(tài)的估計(jì)，它根據(jù)前一時(shí)刻的系統(tǒng)狀態(tài)，估計(jì)出當(dāng)前的系統(tǒng)狀態(tài)，然后根據(jù)當(dāng)前時(shí)刻的實(shí)際觀(guān)測(cè)值作為反饋，來(lái)修正估計(jì)的狀態(tài)。因此，卡爾曼濾波算法可以分為兩個(gè)步驟：狀態(tài)預(yù)測(cè)和測(cè)量修正。狀態(tài)預(yù)測(cè)方程由前一刻系統(tǒng)狀態(tài)和噪聲方差預(yù)測(cè)出當(dāng)前的系統(tǒng)狀態(tài)；而測(cè)量修正方程則負(fù)責(zé)修正，在狀態(tài)預(yù)測(cè)方程中得到的預(yù)測(cè)估計(jì)狀態(tài)中加入當(dāng)前實(shí)際傳感器的信號(hào)，最終得到修正后的系統(tǒng)狀態(tài)。其濾波算法如圖2所示。

圖2 卡爾曼濾波算法

設(shè)tk時(shí)刻的被估計(jì)狀態(tài)Xk受系統(tǒng)噪聲序列Wk-1驅(qū)動(dòng)，對(duì)Xk的兩側(cè)滿(mǎn)足線(xiàn)性關(guān)系，若離散線(xiàn)性系統(tǒng)描述為：

(2)

離散型卡爾曼濾波基本方程要滿(mǎn)足如下關(guān)系[11]：

狀態(tài)預(yù)測(cè)方程為：

(3)

測(cè)量修正方程為：

(4)

當(dāng)狀態(tài)預(yù)測(cè)方程和測(cè)量修正方程循環(huán)運(yùn)算時(shí)，就能使輸出信號(hào)越來(lái)越接近真值。使用卡爾曼濾波算法能夠有效過(guò)濾信號(hào)中的噪聲。

2.2 自平衡機(jī)器人算法模型

根據(jù)卡爾曼濾波原理[12]，設(shè)計(jì)濾波算法如下：

以機(jī)器人的重心距離豎直方向傾角anglek，陀螺儀的偏差量q_biask，作為狀態(tài)向量，得到相應(yīng)的線(xiàn)性系統(tǒng)描述[13]。如公式(5)所示：

(5)

角度預(yù)測(cè)如下：

angle=angle-q_biask-1*dt+gyro_m*dt=angle+Rate*dt

(6)

方差預(yù)測(cè)如下：

(7)

角度偏量：

angle_err=incAngle-angle

(8)

濾波增益：

(9)

協(xié)方差更新：

(10)

狀態(tài)估計(jì)：

(11)

根據(jù)實(shí)際情況推導(dǎo)出工程應(yīng)用算法實(shí)現(xiàn)模型，具體的實(shí)現(xiàn)方式在下一節(jié)介紹。

2.3 卡爾曼融合算法實(shí)現(xiàn)

定義函數(shù)為KalmanFilter，入口參數(shù)為陀螺儀角速度、加速度計(jì)。輸出參數(shù)為濾波角度。該函數(shù)主要實(shí)現(xiàn)卡爾曼數(shù)據(jù)融合的過(guò)程，計(jì)算最優(yōu)估計(jì)角度。函數(shù)構(gòu)造如下：

FloatKalmanFilter(constfloat gyro_m,const float incAngle)

{

定義參數(shù)并實(shí)現(xiàn)卡爾曼融合算法；

Return angle；

}

定義參數(shù)如下：

(1)濾波增益矩陣參數(shù)。

floatK_0;floatK_1;floatY_0;floatY_1

(2)去除偏差后的角速度。

floatRate

(3)計(jì)算狀態(tài)預(yù)測(cè)協(xié)方差矩陣的中間矩陣。

floatPdot[4]

(4)定義角度偏量和計(jì)算的中間量、下時(shí)刻最優(yōu)估計(jì)值角度、陀螺儀的偏差。

floatangle_err;floatE;staticfloatangle=0;staticfloatq_bias=0

(5)定義狀態(tài)協(xié)方差。

staticfloatP[2][2]={{1,0},{0,1}}

(6)卡爾曼狀態(tài)更新。

angle+=Rate*dt

(7)計(jì)算預(yù)測(cè)協(xié)方差矩陣。

Pdot[0]=Q_angle-P[0][1]-P[1][0];

Pdot[1]=-P[1][1];Pdot[2]=-P[1][1];

Pdot[3]=Q_gyro;P[0][0]+=Pdot[0]*dt;

P[0][1]+=Pdot[1]*dt;P[1][0]+=Pdot[2]*dt;

P[1][1]+=Pdot[3]*dt

(8)計(jì)算角度偏量、卡爾曼增益。

angle_err=incAngle-angle

E=R_angle+P[0][0]

K_0=P[0][0]/E;K_1=P[1][0]/E;

Y_0=P[0][0];Y_1=P[0][1]

(9)更新協(xié)方差矩陣并給出最優(yōu)估計(jì)值。

P[0][0]-=K_0*Y_0;P[0][1]-=K_0*Y_1;P[1][0]-=K_1*Y_0;P[1][1]-=K_1 *Y_1;

angle +=K_0*angle_err

(10)更新最優(yōu)估計(jì)值偏差。

q_bias+=K_1*angle_err

通過(guò)上述步驟可以實(shí)現(xiàn)卡爾曼融合算法，將輸入的陀螺儀和加速度計(jì)的數(shù)據(jù)進(jìn)行融合，得到角度信息并返回。

文中使用C語(yǔ)言對(duì)卡爾曼濾波器進(jìn)行建模得到上述程序。其中，dt代表了離散系統(tǒng)中對(duì)傳感器陀螺儀和加速度計(jì)的采樣時(shí)間。Q_angle和Q_gyro分別代表速度計(jì)和陀螺儀測(cè)量的協(xié)方差，協(xié)方差取值的物理意義是對(duì)傳感器信號(hào)品質(zhì)的一種評(píng)估。例如，如果認(rèn)為加速度計(jì)測(cè)量的數(shù)據(jù)更可靠，就可以將Q_angle的值設(shè)置的較小，從而在濾波過(guò)程中體現(xiàn)這種可靠性。R_angle矩陣代表測(cè)量誤差的協(xié)方差。同理，R取值的大小就代表測(cè)量噪聲的大小。Q和R矩陣的取值，通過(guò)大量實(shí)驗(yàn)獲取比較合理的參數(shù)值。

3 實(shí)驗(yàn)平臺(tái)驗(yàn)證

結(jié)合上述對(duì)卡爾曼濾波算法的分析，采用加速度計(jì)MMA7260和陀螺儀ENC03作為慣性傳感元件，對(duì)靜態(tài)和動(dòng)態(tài)的機(jī)器人姿態(tài)進(jìn)行平臺(tái)驗(yàn)證，結(jié)果如圖3所示。

圖3 卡爾曼數(shù)據(jù)融合后的效果

4 結(jié)束語(yǔ)

輪式自平衡機(jī)器人是一種通用的機(jī)器人。它適用于各種復(fù)雜環(huán)境中，通過(guò)陀螺儀和加速度計(jì)采集的數(shù)據(jù)可以控制其平衡性，但是效果不夠好。為了進(jìn)一步提高系統(tǒng)的性能，文中通過(guò)離散卡爾曼算法將ENC03陀螺儀和MMA7260加速度計(jì)采集的數(shù)據(jù)進(jìn)行融合輸出。分別研究了傳統(tǒng)的卡爾曼算法，并詳細(xì)推導(dǎo)了如何建立適合自平衡機(jī)器人的算法模型，最后介紹并實(shí)現(xiàn)該算法。通過(guò)實(shí)時(shí)監(jiān)測(cè)(見(jiàn)圖3)可以發(fā)現(xiàn)，在未進(jìn)行角度融合之前，加速度計(jì)計(jì)算得出的角度在靜態(tài)和動(dòng)態(tài)都有著極大的噪聲；經(jīng)過(guò)卡爾曼濾波算法融合后的角度平滑穩(wěn)定，達(dá)到了預(yù)期的效果。

[1] 王振宇.兩輪移動(dòng)機(jī)器人平衡控制系統(tǒng)的研發(fā)[D].成都:西南交通大學(xué),2010.

[2] 王小冬.卡爾曼濾波在衛(wèi)星導(dǎo)航中的應(yīng)用研究[D].大連:大連海事大學(xué),2008.

[3] 秦永元,張紅鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社,2012:33-42.

[4] 席 峰,劉 中.基于信息矩陣加權(quán)一致策略的分布式Kalman濾波器[J].信息與控制,2010,39(2):194-199.

[5] 楊元喜.動(dòng)態(tài)Kalman濾波模型誤差的影響[J].測(cè)繪科學(xué),2006,31(1):17-18.

[6] 王學(xué)斌,徐建宏,張 章.卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究[J].計(jì)算機(jī)應(yīng)用與軟件,2012,29(6):212-215.

[7] 陸如華,徐傳玉,張 玲,等.卡爾曼濾波的初值計(jì)算方法及其應(yīng)用[J].應(yīng)用氣象學(xué)報(bào),1997,8(1):34-43.

[8] 邱 愷,黃國(guó)榮,陳天如,等.卡爾曼濾波過(guò)程中的穩(wěn)定性研究[J].系統(tǒng)工程與電子技術(shù),2005,27(1):33-35.

[9] 潘 泉,于 昕,程詠梅,等.信息融合理論的基本方法與進(jìn)展[J].自動(dòng)化學(xué)報(bào),2003,29(4):599-615.

[10] 崔平遠(yuǎn),鄭黎方,裴?？?等.基于卡爾曼/粒子組合濾波器的組合導(dǎo)航方法研究[J].系統(tǒng)仿真學(xué)報(bào),2009,21(1):220-223.

[11] Kalman R E.A new approach to linear filtering and prediction problems[J].Transaction of the ASME-Journal of Basic Engineering,1960,82:35-45.

[12] Saber R O,Shamma J S.Consensus filters for sensor networks and distributed sensor fusion[C]//Proc of IEEE conference on decision and control.Seville:IEEE Press,2005:6698-6703.

[13] Ren W,Beard R W,Kingston D B.Multi-agent Kalman consensus with relative uncertainty[C]//Proc of American control conference.Oregon:IEEE Press,2005:1865-1870.

[14] Marco C,Giorgio F,Riccardo G,et al.Real-time data fusion and MEMS sensors fault detection in an aircraft emergency attitude unit based on Kalman filtering[J].IEEE Sensors Journal,2012,12(10):2984-2992.

Research and Implementation of Data Fusion Algorithm for Self-balancing Robot

LV Chuan-long1,CAO Hua-jie2,LIU Hao-dong1

(1.School of Electrical Engineering,Southwest Jiaotong University,Chengdu 610031,China;2.School of Information Science and Technology,Southwest Jiaotong University,Chengdu 611756,China)

Self-balancing robot is general and uses the data collected by gyroscope and accelerometer to control its balance,which can be applied to various complex environments.In order to improve the response of the system because of its limited accuracy and punctuality,the discrete Kalman algorithm is utilized for fusion of the data acquired from the gyroscope (ENC-03) and accelerometer (MMA-7260).Research of the traditional Kalman algorithm is carried on,then establishment of its model for self balancing robot,finally introduction of its engineering implementation.According to the real-time monitoring,it can be seen that the angle calculated from the accelerometer without angle infusion has great noise both in its dynamic and static aspects.The angle acquired from Kalman filtering algorithm fusion is smooth and stable,achieving the desired effect.

self balancing robot;accelerometer;gyroscope;Kalman algorithm;data fusion

2015-10-15

2016-01-21

時(shí)間：2016-06-22

全國(guó)大學(xué)生創(chuàng)新基金資助重點(diǎn)項(xiàng)目(201310292023Z)；江蘇省“挑戰(zhàn)杯-卓越杯”基金資助項(xiàng)目(201310292023Z)

呂傳龍(1992-)，男，碩士研究生，CCF會(huì)員，研究方向?yàn)闄C(jī)器人控制技術(shù)。

http://www.cnki.net/kcms/detail/61.1450.TP.20160622.0842.006.html

TP301.6

A

1673-629X(2016)08-0035-04

10.3969/j.issn.1673-629X.2016.08.007