基于多傳感器信號融合的數(shù)字濾波方法

陳曉燕，程志江，姜波，朱玉龍

（新疆大學電氣工程學院，新疆烏魯木齊830047）

1 引言

兩輪自平衡車是一種基于倒立擺模型的自然不穩(wěn)定體，具有非線性、多變量、強耦合等特點［1］，能夠在外部的干擾下快速調整并始終保持平衡狀態(tài)是其控制的關鍵。在實際控制過程中，分別采用陀螺儀和加速度計來測量角速度和角度信號，但由于陀螺儀存在隨機漂移現(xiàn)象，且加速度計極易受車身運動干擾，因此，需要對車身的角度和角速度信號進行有效的濾波。采用簡易互補濾波法［2］對陀螺儀和加速度計進行信號融合，平滑效果好，響應靈敏，但受陀螺儀溫漂影響比較大。采用離散化低通濾波器與互補濾波相結合［3］的數(shù)據(jù)融合方法，抗干擾性能和移植性好，但是在微機電系統(tǒng)的精確度標定和溫度補償方面不完善。

本文在對兩輪自平衡車進行數(shù)學建模的基礎上，分析得出其平衡的關鍵是獲取車身穩(wěn)定的角度信號。通過對陀螺儀和加速度計的特性分析，發(fā)現(xiàn)陀螺儀是慣性器件，存在累積誤差，需要利用加速度計的角度信號對陀螺儀進行周期性地修正，而加速度計存在動態(tài)誤差，必須依靠陀螺儀積分后的角度信號進行修正［4］。因此，本文將卡爾曼濾波應用于自平衡車中，對陀螺儀與加速度計進行信號的融合。

2 自平衡車平衡原理

2.1 自平衡車動力學建模

自平衡車動力學模型如圖1 所示，兩輪自平衡車結構類似于傳統(tǒng)的倒立擺，對其進行受力分析，包含驅動電機的輸出轉矩、地面支持力、摩擦力和慣性作用力所受的自身作用力影響。其中，車輪簡化運動方程［5］可描述為

式中：m為小車質量；r為車輪半徑；ω為車輪轉速；v為車輪前進方向移動速度；J為車輪對車軸的等效轉動慣量；Tm為電機電磁轉矩；H為車體自身水平作用力。

圖1 自平衡車動力學模型Fig.1 Dynamic model of self-balanced car

對車體進行數(shù)學建模，其簡化動力學方程［5］可描述為

式中：ma為車體質量；I為車體對車輪等效轉動慣量；φ為車體與垂直方向所成角度；T 為車輪對車身產生的轉矩。

式（2）為兩輪自平衡車關于車體傾角與角加速度的微分方程，由式（2）可知精確測量自平衡車傾角和傾角速度是控制自平衡車直立穩(wěn)定的關鍵。

2.2 車身姿態(tài)檢測

加速度計可用于測量物體的線性加速度，其輸出值與傾角呈非線性關系，隨著傾角的增加而表現(xiàn)為正弦函數(shù)變化，因此對加速度計的輸出進行反正弦函數(shù)處理，才能得到其傾角值［6］。

式中：ΔV/Δg為加速度計靈敏度，mV/g；VOFFSET為加速度計的零偏電壓。

采用陀螺儀對角速度進行檢測，輸出精度較好，但其存在漂移現(xiàn)象影響信號的可靠性，因此對陀螺儀測取的當前值與陀螺儀零點偏移量的差值經過比例轉換后積分得到的數(shù)據(jù)即為角度信號，角速度計算公式為

式中，RGYRO為陀螺儀比例因子，其值選取太小波形存在滯后，選取太大則波形存在過沖現(xiàn)象，會引起車身震蕩。

3 數(shù)據(jù)融合

在實際自平衡車運行過程中，車身運動引起的加速度會產生干擾信號疊加在測量信號上，影響兩輪自平衡車角度輸出信號的可靠性。而陀螺儀角速度積分轉換為角度的過程長時間易形成積累誤差，導致電路飽和，角度信號輸出不準確?？刹捎每柭鼮V波對加速度計和陀螺儀進行有效的融合，以得到準確的角度信息。

3.1 卡爾曼濾波

卡爾曼濾波是一種最優(yōu)化自回歸數(shù)據(jù)處理方法，它可以在未知模型性質的情況下，由上一時刻的狀態(tài)值和當前的狀態(tài)觀測值來估計當前狀態(tài)的估計值，從含有噪聲的測量值中得到系統(tǒng)狀態(tài)的最優(yōu)估計。它提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方誤差最小。

在實際應用中，通常用離散化方程來描述連續(xù)系統(tǒng)，卡爾曼估計的離散時間過程包括離散隨機差分方程和量測方程：

式中：A為k-1時刻線性映射到k時刻的狀態(tài)矩陣；B 為控制輸入變量的增益；H 為狀態(tài)變量對測量變量的增益；wk-1，vk分別為過程激勵噪聲和觀測噪聲。

卡爾曼濾波包含狀態(tài)預測和測量修正兩部分，由濾波器估計過程某一時刻的狀態(tài)，再以測量變量的方式獲得反饋，更新流程如圖2所示。

圖2 卡爾曼更新流程圖Fig.2 Kalman update process

狀態(tài)更新方程及時地由當前系統(tǒng)狀態(tài)和噪聲方差估計出下一步系統(tǒng)狀態(tài)，測量更新方程負責反饋，將新的測量信號加入已由狀態(tài)更新方程得出的先驗估計狀態(tài)，最終得到系統(tǒng)狀態(tài)的后驗估計。

3.2 姿態(tài)信息融合

系統(tǒng)以陀螺儀、加速度計來檢測車身所處的俯仰狀態(tài)和狀態(tài)變化率，卡爾曼濾波可以根據(jù)陀螺儀和加速度計不同的數(shù)學模型，通過建立方程來消除測量誤差。由于角速度與角度存在微分關系，故將加速度計作為觀測變量，陀螺儀作為先驗估計變量，由加速度計測得角度值對陀螺儀積分后的角度信號進行對比和修正，從而把卡爾曼濾波器應用到了兩輪自平衡車姿態(tài)檢測的角度與角速度數(shù)據(jù)處理中。

首先，將陀螺儀的輸出量看成是由輸出真值和偏差組成，由陀螺儀偏差值eθ′獲取陀螺儀的角速度信號：

式中：θ′為去除偏差的角速度；θm′為陀螺儀測量角速度。

若偏差不存在，則eθ′=0。過程協(xié)方差矩陣微分方程為

由于過程的狀態(tài)不隨時間改變，本系統(tǒng)令矩陣

式中：A為系統(tǒng)參數(shù)；Q 為過程噪聲協(xié)方差；Qθ，Qθ′分別為加速度計和陀螺儀的協(xié)方差。

將去除偏差的兩輪自平衡車傾斜角速度進行積分便可以得到其傾角值：

式中：θ(k-1)為前一時刻的角度信號，其零時刻的初始值可隨意給定；θ(k)為當前時刻更新的角度值；dt為卡爾曼采樣頻率。

其協(xié)方差更新為

式中：P(k-1)為前一時刻的協(xié)方差；P(k)為當前時刻協(xié)方差。

初始值P(k-1)可隨意給定（P≠0），系統(tǒng)會逐漸收斂，式（1）和式（2）則為對系統(tǒng)的預測?？柭鲆鏋?/p>

由于陀螺儀積分得到的角度與加速度計測得的角度信號相對應，則′表示H 的轉置，矩陣R為測量噪聲協(xié)方差，由觀測得到。更新的角度偏差為eθ=θ-θ(k)，結合預測值和測量值，可以得到角度最優(yōu)化估算值：

協(xié)方差更新為

計算完時間更新方程后，將更新的角度最優(yōu)估計值和協(xié)方差作為下一次計算的先驗估計，循環(huán)估計下一步的系統(tǒng)運行狀態(tài)［6-9］，以此反復進行即可得出系統(tǒng)最優(yōu)的角度信號值，其程序流程圖如圖3所示。

圖3 卡爾曼控制流程圖Fig.3 Kalman control process

基于離散的卡爾曼濾波原理，將加速度計得到的角度信號與陀螺儀得到的角速度信號送入卡爾曼濾波器中進行信號的融合，以獲得更加精確的角度信號，再對濾波所得的角度信號進行比例和微分控制后，即可輸出作為角度控制的電壓輸出量，用于驅動電機，從而實現(xiàn)兩輪自平衡車的直立控制。其控制過程如圖4所示。

圖4 車身姿態(tài)檢測原理圖Fig.4 Attitude detection principle of self-balanced car

4 卡爾曼濾波的實現(xiàn)

在兩輪自平衡車控制的過程中，速度傳感器采用光電編碼器檢測當前車速，而傾角傳感器采用陀螺儀與加速度計控制車體角度，并將這些采集到的轉速控制信號與角度控制信號送入核心控制單元MCU 進行數(shù)據(jù)處理后，疊加加載到兩后輪電機上，控制2個電機的正反向運動，以實現(xiàn)兩輪自平衡車的靜止和直立行走。其硬件控制結構如圖5所示。

圖5 硬件控制結構圖Fig.5 The hardware control structure

為了獲得穩(wěn)定準確的數(shù)據(jù)，通過單片機軟件多次采集自平衡車靜止時模擬通道的AD采樣數(shù)據(jù)，取平均值作為傾角信號的零偏電壓，將陀螺儀與加速度計的測量數(shù)據(jù)分別與相應零偏電壓做差，經過16位精度比例轉換為角速度與角度信號后，送入MK60 嵌入式系統(tǒng)進行控制。上位通過無線串口示波器實時觀察車體的濾波情況。

當兩輪自平衡車處于直立狀態(tài)時，由于噪聲的干擾，濾波前角度與角速度信號曲線均在零度水平線±10°上下波動，波形震蕩頻率高，車身穩(wěn)定性差；經過卡爾曼濾波后波形保持在零度水平直線狀態(tài)，曲線光滑平穩(wěn)，車身穩(wěn)定性高，零點放大圖如圖6所示。

圖6 零點放大圖Fig.6 Zero point enlarge figure

車身傾角變化時，由于兩輪自平衡車自身穩(wěn)定性差，濾波前波形存在過沖現(xiàn)象，過沖角度高達±8°，會引起車身震蕩。經過卡爾曼濾波后，車身角度信號過沖現(xiàn)象消除，且跟隨性好，實時反應了車身傾角變化情況。濾波對比圖如圖7所示。

而角速度代表了角度的變化量，在角度變化的同時角速度產生一個脈沖信號，其高度反應了角度變化的快慢。當角度不再變化時，角速度則立刻回復到原點，兩輪自平衡車保持此傾角運行。卡爾曼濾波圖如圖8所示。

圖7 角度濾波對比圖Fig.7 Angular filtering contrast figure

圖8 卡爾曼濾波圖Fig.8 Kalman filtering

由上述分析可知，加速度計得到的角度信號反應了兩輪自平衡車角度的變化情況，隨時跟隨角度的變化；而陀螺儀得到的角速度信號反應了角度變化的靈敏情況，兩輪自平衡車傾角改變后陀螺儀信號變化，隨即會立即回復到原處。經過卡爾曼濾波后的角度信號基本不存在諧波，且波形平滑，濾波效果好，提高了兩輪自平衡車的穩(wěn)定性。

5 結論

針對兩輪自平衡車姿態(tài)檢測傳感器陀螺儀輸出角速度存在隨機漂移誤差，且極易受外界噪聲干擾的問題，本文采用卡爾曼濾波算法將陀螺儀和加速度計輸出的信號進行數(shù)據(jù)融合，來估計系統(tǒng)的最佳狀態(tài)。在動態(tài)數(shù)據(jù)采集試驗的基礎上，通過卡爾曼濾波有效地補償傳感器漂移與測量噪聲等因素對加速度計與陀螺儀的影響，減小了姿態(tài)角度測量誤差，提高了運算精度，因而有效地提高了兩輪自平衡車的穩(wěn)定性。

［1］姜香菊，劉二林.兩輪自平衡機器人角度檢測數(shù)據(jù)融合算法［J］.計算機工程與應用，2013，49（8）：203-205.

［2］張吉昌，程凱，鄭榮兒.單軸雙輪自平衡車姿態(tài)檢測方案設計［J］.中國海洋大學學報，2009，39（S）：467-470.

［3］夏圣，許勇.基于MEMS 組合模塊的姿態(tài)檢測系統(tǒng)設計［J］.單片機與嵌入式系統(tǒng)應用，2011（4）：52-55.

［4］江杰，朱君，豈偉楠.四旋翼無人飛行器姿態(tài)數(shù)據(jù)采集處理系統(tǒng)［J］.計算機測量與控制，2012，20（6）：1703-1706.

［5］Han I，Park H S，Jeong Y K，et al.An Integrated Home Server for Communication，Broadcast Reception and Home Automation［J］.IEEE Transactions on Consumer Electronics，2006，52（1）：104-109.

［6］馮智勇，曾瀚，張力，等.基于陀螺儀及加速度計信號融合的姿態(tài)角度測量［J］.西南大學師范大學學報：自然科學版，2011，36（4）：137-141.

［7］Kalman R E.A New APProach to Linear Filtenng and Prediction Problems［J］.Transaction of the ASME-Journal of Basic Engineering，1960，82（Series D）：35-45.

［8］焦勇，楊慶堂.兩輪自平衡機器人的姿態(tài)檢測系統(tǒng)的設計［J］.船電技術應用研究，2011，31（5）：45-49.

［9］LIU Kun，BAI Ming，NI Yuhua.Two-wheel Self-balanced Car Based on Kalman Filtering and PID Algorithm［C］//Proceeding of 2011IEEE the 18thInternational Conference on Industrial Engineering and Engineering Management（Volume1）2011：281-285.