摘 要： 目前車模傾角一般采用陀螺儀或加速度計(jì)進(jìn)行測量。然而單純利用其中一種傳感器采集到的信號(hào)存在極大誤差，無法滿足二輪自平衡車直立控制的需要。為此，采用陀螺儀和加速度計(jì)測量傾角，并應(yīng)用卡爾曼濾波算法對(duì)上述兩種信號(hào)進(jìn)行融合，得出理想的輸出傾角。同時(shí)，將得到的卡爾曼濾波程序移植到車模直立控制程序中，進(jìn)行車模直立控制試驗(yàn)。試驗(yàn)結(jié)果表明，運(yùn)用該方法可以得到很好的濾波效果。
關(guān)鍵詞： 卡爾曼濾波；平衡車；直立控制；數(shù)據(jù)融合
二輪直立平衡車模型經(jīng)簡化可等價(jià)為一個(gè)倒立擺模型，對(duì)于其平衡的控制，需要得到車模精確的傾角。通過加速度計(jì)和陀螺儀的測量，可以得到車模的加速度和角速度，進(jìn)而可以換算出車模的傾角和傾角變化率。然而由于車模本身的運(yùn)動(dòng)會(huì)產(chǎn)生很大的干擾信號(hào)，使得加速度計(jì)無法準(zhǔn)確測量車模的加速度。同時(shí)，由于陀螺儀的溫漂效應(yīng)，也無法通過積分得到車模的準(zhǔn)確傾角。在此情況下，為得到精確的車模傾角，需要采用多傳感器進(jìn)行信號(hào)融合處理[1]。本文利用卡爾曼濾波算法，對(duì)加速度和陀螺儀采集到的信號(hào)進(jìn)行數(shù)據(jù)融合，比較精確地實(shí)現(xiàn)了車模傾角的測量。
1 陀螺儀和加速度計(jì)測量模型
1.1 陀螺儀測量角速度
陀螺儀可以用來測量物體的旋轉(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)。在車模上安裝陀螺儀，可以測量車模傾角角速度，將角速度進(jìn)行積分便可以得到車模的傾角。然而，在實(shí)際情況下，陀螺儀會(huì)發(fā)生一定程度的漂變，尤其是溫漂，給陀螺儀的測量帶來很大的干擾。因此所測得的角速度存在微小的偏差和漂移時(shí)，經(jīng)過積分作用，會(huì)產(chǎn)生累積誤差。顯然，該傾角值也不能當(dāng)作車模直立控制的控制信號(hào)。
1.2 加速度計(jì)間接測量傾角
加速度傳感器可以用來測量由地球引力作用或者物體運(yùn)動(dòng)所產(chǎn)生的加速度。本文所選用的加速度計(jì)為飛思卡爾公司生產(chǎn)的MMA8451Q，最高具有14位數(shù)據(jù)輸出精度。利用加速度計(jì)其中一個(gè)方向上的加速度值，通過計(jì)算，便可間接測量車模的傾角信號(hào)。本文利用Z軸上的加速度信號(hào)，計(jì)算得到車模的傾角。圖1是利用加速度計(jì)Z軸信號(hào)來間接測量車模傾角信號(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)無法準(zhǔn)確反映車模傾角，因此，純粹通過加速度計(jì)也無法得到需要的車模傾角信號(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í)刻的觀測值來更新對(duì)狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對(duì)需要處理的信號(hào)做出滿足最小均方誤差的估計(jì)[3]。
2.2 卡爾曼濾波器設(shè)計(jì)
2.2.1 時(shí)間更新
在卡爾曼濾波器中，時(shí)間更新的過程是一個(gè)預(yù)測過程。時(shí)間更新算法是關(guān)于卡爾曼濾波器中預(yù)測器的描述。預(yù)測器的任務(wù)是依據(jù)上一時(shí)刻的數(shù)據(jù)預(yù)測當(dāng)前時(shí)刻的數(shù)據(jù)，具體可分為狀態(tài)估計(jì)和方差估計(jì)[4-6]。
2.2.2 測量更新
測量更新的過程本質(zhì)上是一個(gè)校正過程。校正器的任務(wù)是將先驗(yàn)狀態(tài)估計(jì)值與當(dāng)前的傾角數(shù)據(jù)相融合，改善先驗(yàn)狀態(tài)估計(jì)值，從而得到當(dāng)前時(shí)刻的后驗(yàn)狀態(tài)估計(jì)值。具體可分為以下幾步：加速度計(jì)的測量參差計(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ù)。
簡單用加速度計(jì)輸出得到的角度信號(hào)如圖2所示。
由圖2可以看出，信號(hào)噪聲很大，動(dòng)態(tài)性能很差，但是靜態(tài)性能較好，如圖中箭頭所示，角度相對(duì)穩(wěn)定不變時(shí)，能正確反映車子的姿態(tài)角。
簡單用陀螺儀的角速度積分輸出的角度信號(hào)如圖3所示。
用單純的角速度積分得到的角度信號(hào)十分平滑，動(dòng)態(tài)性能非常好，但是隨著積分進(jìn)行，靜態(tà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)參過程可按下面步驟進(jìn)行，效果如圖6所示。
（1）隨便設(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測量方差比實(shí)際的小，導(dǎo)致濾波器沒能很好地濾除噪聲，這時(shí)可以選擇增大r_meas。
（2）調(diào)整參數(shù)為：
q_inc=1;
q_gyro=1;
r_meas=100 000;
效果如圖6 (b)所示，穩(wěn)態(tài)的時(shí)候曲線變得十分平滑，可見濾波器很好地抑制了加記高頻噪聲的干擾。角度跟蹤超調(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è)置過大，導(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)。
（5）繼續(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所示，由此可見濾波器效果十分理想，濾波器設(shè)計(jì)圓滿完成。
本文研究了基于卡爾曼濾波的陀螺儀和加速度計(jì)的數(shù)據(jù)融合方法，利用陀螺儀和加速度計(jì)測量車模的角速度和加速度，將采集到的數(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)角度測量[J]．西南師范大學(xué)學(xué)報(bào)（自然科學(xué)版），2011，36（4）：137-141．
[2] 競賽秘書處.第七屆全國大學(xué)生“飛思卡爾”杯智能汽車競賽電磁組直立行車參考設(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．