呂傳龍，曹華杰，劉浩東

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

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

0 引 言

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

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

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

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

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

Δu=kgsinθ

(1)

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

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

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

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

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

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

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

2.1 卡爾曼濾波算法

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

圖2 卡爾曼濾波算法

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

(2)

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

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

(3)

測量修正方程為：

(4)

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

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

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

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

(5)

角度預(yù)測如下：

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

(6)

方差預(yù)測如下：

(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ù)融合的過程，計(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ù)測協(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ù)測協(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

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

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

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

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

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

4 結(jié)束語

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

全國大學(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