摘 要： 目前車模傾角一般采用陀螺儀或加速度計(jì)進(jìn)行測量。然而單純利用其中一種傳感器采集到的信號存在極大誤差，無法滿足二輪自平衡車直立控制的需要。為此，采用陀螺儀和加速度計(jì)測量傾角，并應(yīng)用卡爾曼濾波算法對上述兩種信號進(jìn)行融合，得出理想的輸出傾角。同時(shí)，將得到的卡爾曼濾波程序移植到車模直立控制程序中，進(jìn)行車模直立控制試驗(yàn)。試驗(yàn)結(jié)果表明，運(yùn)用該方法可以得到很好的濾波效果。
關(guān)鍵詞： 卡爾曼濾波；平衡車；直立控制；數(shù)據(jù)融合
二輪直立平衡車模型經(jīng)簡化可等價(jià)為一個(gè)倒立擺模型，對于其平衡的控制，需要得到車模精確的傾角。通過加速度計(jì)和陀螺儀的測量，可以得到車模的加速度和角速度，進(jìn)而可以換算出車模的傾角和傾角變化率。然而由于車模本身的運(yùn)動會產(chǎn)生很大的干擾信號，使得加速度計(jì)無法準(zhǔn)確測量車模的加速度。同時(shí)，由于陀螺儀的溫漂效應(yīng)，也無法通過積分得到車模的準(zhǔn)確傾角。在此情況下，為得到精確的車模傾角，需要采用多傳感器進(jìn)行信號融合處理[1]。本文利用卡爾曼濾波算法，對加速度和陀螺儀采集到的信號進(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í)會產(chǎn)生相應(yīng)的旋轉(zhuǎn)角速度輸出信號。在車模上安裝陀螺儀，可以測量車模傾角角速度，將角速度進(jìn)行積分便可以得到車模的傾角。然而，在實(shí)際情況下，陀螺儀會發(fā)生一定程度的漂變，尤其是溫漂，給陀螺儀的測量帶來很大的干擾。因此所測得的角速度存在微小的偏差和漂移時(shí)，經(jīng)過積分作用，會產(chǎn)生累積誤差。顯然，該傾角值也不能當(dāng)作車模直立控制的控制信號。
1.2 加速度計(jì)間接測量傾角
加速度傳感器可以用來測量由地球引力作用或者物體運(yùn)動所產(chǎn)生的加速度。本文所選用的加速度計(jì)為飛思卡爾公司生產(chǎn)的MMA8451Q，最高具有14位數(shù)據(jù)輸出精度。利用加速度計(jì)其中一個(gè)方向上的加速度值，通過計(jì)算，便可間接測量車模的傾角信號。本文利用Z軸上的加速度信號，計(jì)算得到車模的傾角。圖1是利用加速度計(jì)Z軸信號來間接測量車模傾角信號的原理圖。
當(dāng)車模發(fā)生傾斜時(shí)，重力加速度g會在Z軸產(chǎn)生一個(gè)加速度分量，使Z軸輸出信號改變。輸出關(guān)系為：
其中，g為重力加速度，θ為車模傾角，k為加速度傳感器靈敏度系數(shù)[2]。
當(dāng)傾角θ比較小時(shí)，輸出信號的變化可以近似與傾角成正比，然而在實(shí)際情況下，由于車模本身的擺動會產(chǎn)生很大的干擾，使得加速度計(jì)輸出信號無法準(zhǔn)確反映車模傾角，因此，純粹通過加速度計(jì)也無法得到需要的車模傾角信號。
為了獲得相對精確的車模傾角信號，需要對陀螺儀和加速度計(jì)的輸出信號進(jìn)行融合，本文采用的是卡爾曼濾波法。
2 卡爾曼濾波原理及濾波器設(shè)計(jì)
2.1 卡爾曼濾波概述
卡爾曼濾波是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法。它屬于一種軟件濾波方法，其基本思想是：以最小均方誤差為最佳估計(jì)準(zhǔn)則，采用信號與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測值來更新對狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計(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)角度的卡爾曼濾波器。用手晃動車子，使車子的姿態(tài)角度不斷發(fā)生變化，用SD卡記錄數(shù)據(jù)，用MATLAB分析數(shù)據(jù)。
簡單用加速度計(jì)輸出得到的角度信號如圖2所示。
由圖2可以看出，信號噪聲很大，動態(tài)性能很差，但是靜態(tài)性能較好，如圖中箭頭所示，角度相對穩(wěn)定不變時(shí)，能正確反映車子的姿態(tài)角。
簡單用陀螺儀的角速度積分輸出的角度信號如圖3所示。
用單純的角速度積分得到的角度信號十分平滑，動態(tài)性能非常好，但是隨著積分進(jìn)行，靜態(tài)誤差越來越大。由圖3可以看出曲線有整體向下移動的趨勢，這是靜態(tài)誤差積累的結(jié)果。
把加速度信號和陀螺儀信號送入卡爾曼濾波器之后輸出的角度信號如圖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可以看出，濾波器輸出的角度十分平滑，動態(tài)性能較好，且消除了靜態(tài)誤差。
綜合對比濾波前后的角度信號，如圖5所示。
4 卡爾曼濾波器參數(shù)設(shè)置
調(diào)參過程可按下面步驟進(jìn)行，效果如圖6所示。
?。?）隨便設(shè)置參數(shù)為
q_inc=1;
q_gyro=1;
r_meas=1 000;
效果如圖6 (a)所示?？梢钥闯觯到y(tǒng)響應(yīng)角度跟蹤時(shí)受加速度計(jì)噪聲影響很大，在角度劇烈變化時(shí)有嚴(yán)重超調(diào)，且在穩(wěn)態(tài)時(shí)，角度有波動，濾波器未能很好地抑制加速度計(jì)的高頻噪聲。造成這種現(xiàn)象的原因是加速度計(jì)的噪聲很大，而設(shè)置的r_meas測量方差比實(shí)際的小，導(dǎo)致濾波器沒能很好地濾除噪聲，這時(shí)可以選擇增大r_meas。
?。?）調(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。
?。?）再次調(diào)整參數(shù)為：
q_inc=1;
q_gyro=1;
r_meas=10 000 000;
效果如圖6 (c)所示，發(fā)現(xiàn)繼續(xù)增大r_meas效果改善不再明顯，而且角度收斂變得特別緩慢，動態(tài)性能降低。造成這種現(xiàn)象的原因是r_meas設(shè)置過大，導(dǎo)致系統(tǒng)把加記有用信號也當(dāng)成了噪聲。陀螺儀的噪聲是很低的，陀螺儀零漂的噪聲也是很低的，考慮到這個(gè)，可以選擇減小Q，并把r_meas恢復(fù)到前面的值。
?。?）減小Q：
q_inc=0.1;
q_gyro=0.1;
r_meas=100 000;
效果如圖6 (d)所示，系統(tǒ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 卡爾曼濾波器移植到車?？刂破髦?/strong>
把參數(shù)下載到車?？刂破髦?，查看車模直立控制效果，記錄下運(yùn)行數(shù)據(jù)，評估卡爾曼濾波器效果。某一段時(shí)間內(nèi)系統(tǒng)濾波前后對比如圖7所示，由此可見濾波器效果十分理想，濾波器設(shè)計(jì)圓滿完成。
本文研究了基于卡爾曼濾波的陀螺儀和加速度計(jì)的數(shù)據(jù)融合方法，利用陀螺儀和加速度計(jì)測量車模的角速度和加速度，將采集到的數(shù)據(jù)導(dǎo)入matlab中，應(yīng)用卡爾曼濾波算法對其進(jìn)行數(shù)據(jù)融合，不斷整定參數(shù)，使得輸出傾角達(dá)到理想效果。同時(shí)，將得到的卡爾曼濾波程序移植到車模直立控制程序中，進(jìn)行車模直立控制試驗(yàn)，試驗(yàn)結(jié)果表明，運(yùn)用該方法可以得到很好的濾波效果。
參考文獻(xiàn)
[1] 馮智勇，曾瀚，張力，等．基于陀螺儀及加速度計(jì)信號融合的姿態(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．