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