摘 要： 目前車模傾角一般采用陀螺儀或加速度計(jì)進(jìn)行測(cè)量。然而單純利用其中一種傳感器采集到的信號(hào)存在極大誤差,，無(wú)法滿足二輪自平衡車直立控制的需要,。為此，采用陀螺儀和加速度計(jì)測(cè)量?jī)A角,，并應(yīng)用卡爾曼濾波算法對(duì)上述兩種信號(hào)進(jìn)行融合,，得出理想的輸出傾角。同時(shí),，將得到的卡爾曼濾波程序移植到車模直立控制程序中,，進(jìn)行車模直立控制試驗(yàn)。試驗(yàn)結(jié)果表明,，運(yùn)用該方法可以得到很好的濾波效果,。
關(guān)鍵詞： 卡爾曼濾波；平衡車,；直立控制,；數(shù)據(jù)融合
二輪直立平衡車模型經(jīng)簡(jiǎn)化可等價(jià)為一個(gè)倒立擺模型，對(duì)于其平衡的控制,，需要得到車模精確的傾角,。通過(guò)加速度計(jì)和陀螺儀的測(cè)量，可以得到車模的加速度和角速度,，進(jìn)而可以換算出車模的傾角和傾角變化率,。然而由于車模本身的運(yùn)動(dòng)會(huì)產(chǎn)生很大的干擾信號(hào)，使得加速度計(jì)無(wú)法準(zhǔn)確測(cè)量車模的加速度,。同時(shí),，由于陀螺儀的溫漂效應(yīng)，也無(wú)法通過(guò)積分得到車模的準(zhǔn)確傾角,。在此情況下,，為得到精確的車模傾角，需要采用多傳感器進(jìn)行信號(hào)融合處理[1],。本文利用卡爾曼濾波算法,，對(duì)加速度和陀螺儀采集到的信號(hào)進(jìn)行數(shù)據(jù)融合,，比較精確地實(shí)現(xiàn)了車模傾角的測(cè)量。
1 陀螺儀和加速度計(jì)測(cè)量模型
1.1 陀螺儀測(cè)量角速度
陀螺儀可以用來(lái)測(cè)量物體的旋轉(zhuǎn)角速度,。本文所選用的陀螺儀為意法半導(dǎo)體（ST）生產(chǎn)的三軸數(shù)字陀螺儀L3G4200D,，可提供16位數(shù)據(jù)輸出。當(dāng)器件旋轉(zhuǎn)時(shí)會(huì)產(chǎn)生相應(yīng)的旋轉(zhuǎn)角速度輸出信號(hào),。在車模上安裝陀螺儀,，可以測(cè)量車模傾角角速度，將角速度進(jìn)行積分便可以得到車模的傾角,。然而,，在實(shí)際情況下，陀螺儀會(huì)發(fā)生一定程度的漂變,，尤其是溫漂,，給陀螺儀的測(cè)量帶來(lái)很大的干擾。因此所測(cè)得的角速度存在微小的偏差和漂移時(shí),，經(jīng)過(guò)積分作用,，會(huì)產(chǎn)生累積誤差。顯然,，該傾角值也不能當(dāng)作車模直立控制的控制信號(hào),。
1.2 加速度計(jì)間接測(cè)量?jī)A角
加速度傳感器可以用來(lái)測(cè)量由地球引力作用或者物體運(yùn)動(dòng)所產(chǎn)生的加速度。本文所選用的加速度計(jì)為飛思卡爾公司生產(chǎn)的MMA8451Q,，最高具有14位數(shù)據(jù)輸出精度,。利用加速度計(jì)其中一個(gè)方向上的加速度值，通過(guò)計(jì)算,，便可間接測(cè)量車模的傾角信號(hào)。本文利用Z軸上的加速度信號(hào),，計(jì)算得到車模的傾角,。圖1是利用加速度計(jì)Z軸信號(hào)來(lái)間接測(cè)量車模傾角信號(hào)的原理圖。
當(dāng)車模發(fā)生傾斜時(shí),，重力加速度g會(huì)在Z軸產(chǎn)生一個(gè)加速度分量,，使Z軸輸出信號(hào)改變。輸出關(guān)系為：
其中,，g為重力加速度,，θ為車模傾角，k為加速度傳感器靈敏度系數(shù)[2],。
當(dāng)傾角θ比較小時(shí),，輸出信號(hào)的變化可以近似與傾角成正比，然而在實(shí)際情況下,，由于車模本身的擺動(dòng)會(huì)產(chǎn)生很大的干擾,，使得加速度計(jì)輸出信號(hào)無(wú)法準(zhǔn)確反映車模傾角,，因此，純粹通過(guò)加速度計(jì)也無(wú)法得到需要的車模傾角信號(hào),。
為了獲得相對(duì)精確的車模傾角信號(hào),，需要對(duì)陀螺儀和加速度計(jì)的輸出信號(hào)進(jìn)行融合，本文采用的是卡爾曼濾波法,。
2 卡爾曼濾波原理及濾波器設(shè)計(jì)
2.1 卡爾曼濾波概述
卡爾曼濾波是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法,。它屬于一種軟件濾波方法，其基本思想是：以最小均方誤差為最佳估計(jì)準(zhǔn)則,，采用信號(hào)與噪聲的狀態(tài)空間模型,，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值,，算法根據(jù)建立的系統(tǒng)方程和觀測(cè)方程對(duì)需要處理的信號(hào)做出滿足最小均方誤差的估計(jì)[3],。
2.2 卡爾曼濾波器設(shè)計(jì)
2.2.1 時(shí)間更新
在卡爾曼濾波器中，時(shí)間更新的過(guò)程是一個(gè)預(yù)測(cè)過(guò)程,。時(shí)間更新算法是關(guān)于卡爾曼濾波器中預(yù)測(cè)器的描述,。預(yù)測(cè)器的任務(wù)是依據(jù)上一時(shí)刻的數(shù)據(jù)預(yù)測(cè)當(dāng)前時(shí)刻的數(shù)據(jù)，具體可分為狀態(tài)估計(jì)和方差估計(jì)[4-6],。
2.2.2 測(cè)量更新
測(cè)量更新的過(guò)程本質(zhì)上是一個(gè)校正過(guò)程,。校正器的任務(wù)是將先驗(yàn)狀態(tài)估計(jì)值與當(dāng)前的傾角數(shù)據(jù)相融合，改善先驗(yàn)狀態(tài)估計(jì)值,，從而得到當(dāng)前時(shí)刻的后驗(yàn)狀態(tài)估計(jì)值,。具體可分為以下幾步：加速度計(jì)的測(cè)量參差計(jì)算，殘差的協(xié)方差計(jì)算,，最優(yōu)卡爾曼增益計(jì)算,，校正先驗(yàn)狀態(tài)估計(jì)值和計(jì)算后驗(yàn)狀態(tài)估計(jì)值，校正先驗(yàn)估計(jì)協(xié)方差矩陣,，計(jì)算后驗(yàn)估計(jì)協(xié)方差矩陣[4-6],。
3 卡爾曼濾波器在車模中試驗(yàn)
小車加入了SD卡模塊，可以記錄小車運(yùn)行狀態(tài),，并用MATLAB分析設(shè)計(jì),，這樣大大方便了設(shè)計(jì)解算小車姿態(tài)角度的卡爾曼濾波器。用手晃動(dòng)車子,，使車子的姿態(tài)角度不斷發(fā)生變化,，用SD卡記錄數(shù)據(jù)，用MATLAB分析數(shù)據(jù),。
簡(jiǎn)單用加速度計(jì)輸出得到的角度信號(hào)如圖2所示,。
由圖2可以看出，信號(hào)噪聲很大，動(dòng)態(tài)性能很差,，但是靜態(tài)性能較好,，如圖中箭頭所示，角度相對(duì)穩(wěn)定不變時(shí),，能正確反映車子的姿態(tài)角,。
簡(jiǎn)單用陀螺儀的角速度積分輸出的角度信號(hào)如圖3所示。
用單純的角速度積分得到的角度信號(hào)十分平滑,，動(dòng)態(tài)性能非常好,，但是隨著積分進(jìn)行，靜態(tài)誤差越來(lái)越大,。由圖3可以看出曲線有整體向下移動(dòng)的趨勢(shì),，這是靜態(tài)誤差積累的結(jié)果。
把加速度信號(hào)和陀螺儀信號(hào)送入卡爾曼濾波器之后輸出的角度信號(hào)如圖4所示,。
x=[angle(1);drift(1)];
for i = Time(2:end)
x = A*x+B*u_gyro(i);
P = A*P*(A')+Q;
y = z_inc(i)-x(1);
s = P(1,1)-R;
K = P*[1;0]/s;
x = x + K*y;
P = ([1,0;0,1]-K*[1,0])*P;
angle(i) = x(1);
drift(i) = x(2);
end
由圖4可以看出,，濾波器輸出的角度十分平滑，動(dòng)態(tài)性能較好,，且消除了靜態(tài)誤差,。
綜合對(duì)比濾波前后的角度信號(hào)，如圖5所示,。
4 卡爾曼濾波器參數(shù)設(shè)置
調(diào)參過(guò)程可按下面步驟進(jìn)行,，效果如圖6所示。
?。?）隨便設(shè)置參數(shù)為
q_inc=1;
q_gyro=1;
r_meas=1 000;
效果如圖6 (a)所示,。可以看出,，系統(tǒng)響應(yīng)角度跟蹤時(shí)受加速度計(jì)噪聲影響很大,，在角度劇烈變化時(shí)有嚴(yán)重超調(diào)，且在穩(wěn)態(tài)時(shí),，角度有波動(dòng),，濾波器未能很好地抑制加速度計(jì)的高頻噪聲。造成這種現(xiàn)象的原因是加速度計(jì)的噪聲很大,，而設(shè)置的r_meas測(cè)量方差比實(shí)際的小,，導(dǎo)致濾波器沒(méi)能很好地濾除噪聲,，這時(shí)可以選擇增大r_meas,。
（2）調(diào)整參數(shù)為：
q_inc=1;
q_gyro=1;
r_meas=100 000;
效果如圖6 (b)所示,，穩(wěn)態(tài)的時(shí)候曲線變得十分平滑,，可見(jiàn)濾波器很好地抑制了加記高頻噪聲的干擾。角度跟蹤超調(diào)減小,，但是仍然嚴(yán)重,，試著繼續(xù)增大r_meas,。
（3）再次調(diào)整參數(shù)為：
q_inc=1;
q_gyro=1;
r_meas=10 000 000;
效果如圖6 (c)所示,，發(fā)現(xiàn)繼續(xù)增大r_meas效果改善不再明顯,，而且角度收斂變得特別緩慢，動(dòng)態(tài)性能降低,。造成這種現(xiàn)象的原因是r_meas設(shè)置過(guò)大,，導(dǎo)致系統(tǒng)把加記有用信號(hào)也當(dāng)成了噪聲。陀螺儀的噪聲是很低的,，陀螺儀零漂的噪聲也是很低的,，考慮到這個(gè)，可以選擇減小Q,，并把r_meas恢復(fù)到前面的值,。
（4）減小Q：
q_inc=0.1;
q_gyro=0.1;
r_meas=100 000;
效果如圖6 (d)所示,，系統(tǒng)動(dòng)態(tài)響應(yīng)明顯改良,，角度輸出能很好跟蹤，且不出現(xiàn)大幅度超調(diào),。
?。?）繼續(xù)減小Q：
q_inc=0.01;
q_gyro=0.01;
r_meas=100 000;
效果如圖6 (e)所示，輸出性能有小幅改善,，達(dá)到滿意效果,。再繼續(xù)減小效果不大，因此,，可把這一組參數(shù)作為最終參數(shù),。
5 卡爾曼濾波器移植到車模控制器中
把參數(shù)下載到車?？刂破髦?，查看車模直立控制效果，記錄下運(yùn)行數(shù)據(jù),，評(píng)估卡爾曼濾波器效果,。某一段時(shí)間內(nèi)系統(tǒng)濾波前后對(duì)比如圖7所示，由此可見(jiàn)濾波器效果十分理想,，濾波器設(shè)計(jì)圓滿完成,。
本文研究了基于卡爾曼濾波的陀螺儀和加速度計(jì)的數(shù)據(jù)融合方法，利用陀螺儀和加速度計(jì)測(cè)量車模的角速度和加速度,，將采集到的數(shù)據(jù)導(dǎo)入matlab中,，應(yīng)用卡爾曼濾波算法對(duì)其進(jìn)行數(shù)據(jù)融合，不斷整定參數(shù)，使得輸出傾角達(dá)到理想效果,。同時(shí),，將得到的卡爾曼濾波程序移植到車模直立控制程序中，進(jìn)行車模直立控制試驗(yàn),，試驗(yàn)結(jié)果表明,，運(yùn)用該方法可以得到很好的濾波效果。
參考文獻(xiàn)
[1] 馮智勇,，曾瀚,，張力，等．基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J]．西南師范大學(xué)學(xué)報(bào)（自然科學(xué)版）,，2011,，36（4）：137-141．
[2] 競(jìng)賽秘書(shū)處.第七屆全國(guó)大學(xué)生“飛思卡爾”杯智能汽車競(jìng)賽電磁組直立行車參考設(shè)計(jì)方案（版本1.0）[EB/OL]. [2011-12-22]（2014-07-a）http://www.smartcar.au.tsinghua.edu.cn/upload_files/atta/1383549696785_37.pdf.
[3] 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)刊，2009,，8（11）：32-34．
[4] 阮曉鋼,，蔡建慕，李欣源,，等．兩輪平衡機(jī)器人的研究與設(shè)計(jì)[M]．北京科學(xué)出版社,，2012．
[5] KALMAN R E.A new approach to linear filtering and prediction problems[C].Transaction of the ASME-Journal of Basic Engineering，1960,，82（D）：35-45.
[6] 高嵩,，潘泉，肖秦琨,，等.多傳感器自適應(yīng)濾波融合算法[J].電子與信息學(xué)報(bào),，2008，30（8）：:1901-1904．