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

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

1 引言

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

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

2 自平衡車平衡原理

2.1 自平衡車動(dòng)力學(xué)建模

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

式中：m為小車質(zhì)量；r為車輪半徑；ω為車輪轉(zhuǎn)速；v為車輪前進(jìn)方向移動(dòng)速度；J為車輪對(duì)車軸的等效轉(zhuǎn)動(dòng)慣量；Tm為電機(jī)電磁轉(zhuǎn)矩；H為車體自身水平作用力。

圖1 自平衡車動(dòng)力學(xué)模型Fig.1 Dynamic model of self-balanced car

對(duì)車體進(jìn)行數(shù)學(xué)建模，其簡(jiǎn)化動(dòng)力學(xué)方程［5］可描述為

式中：ma為車體質(zhì)量；I為車體對(duì)車輪等效轉(zhuǎn)動(dòng)慣量；φ為車體與垂直方向所成角度；T 為車輪對(duì)車身產(chǎn)生的轉(zhuǎn)矩。

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

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

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

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

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

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

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

在實(shí)際自平衡車運(yùn)行過(guò)程中，車身運(yùn)動(dòng)引起的加速度會(huì)產(chǎn)生干擾信號(hào)疊加在測(cè)量信號(hào)上，影響兩輪自平衡車角度輸出信號(hào)的可靠性。而陀螺儀角速度積分轉(zhuǎn)換為角度的過(guò)程長(zhǎng)時(shí)間易形成積累誤差，導(dǎo)致電路飽和，角度信號(hào)輸出不準(zhǔn)確。可采用卡爾曼濾波對(duì)加速度計(jì)和陀螺儀進(jìn)行有效的融合，以得到準(zhǔn)確的角度信息。

3.1 卡爾曼濾波

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

在實(shí)際應(yīng)用中，通常用離散化方程來(lái)描述連續(xù)系統(tǒng)，卡爾曼估計(jì)的離散時(shí)間過(guò)程包括離散隨機(jī)差分方程和量測(cè)方程：

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

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

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

狀態(tài)更新方程及時(shí)地由當(dāng)前系統(tǒng)狀態(tài)和噪聲方差估計(jì)出下一步系統(tǒng)狀態(tài)，測(cè)量更新方程負(fù)責(zé)反饋，將新的測(cè)量信號(hào)加入已由狀態(tài)更新方程得出的先驗(yàn)估計(jì)狀態(tài)，最終得到系統(tǒng)狀態(tài)的后驗(yàn)估計(jì)。

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

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

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

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

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

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

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

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

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

其協(xié)方差更新為

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

初始值P(k-1)可隨意給定（P≠0），系統(tǒng)會(huì)逐漸收斂，式（1）和式（2）則為對(duì)系統(tǒng)的預(yù)測(cè)。卡爾曼增益為

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

協(xié)方差更新為

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

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

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

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

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

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

圖5 硬件控制結(jié)構(gòu)圖Fig.5 The hardware control structure

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

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

圖6 零點(diǎn)放大圖Fig.6 Zero point enlarge figure

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

而角速度代表了角度的變化量，在角度變化的同時(shí)角速度產(chǎn)生一個(gè)脈沖信號(hào)，其高度反應(yīng)了角度變化的快慢。當(dāng)角度不再變化時(shí)，角速度則立刻回復(fù)到原點(diǎn)，兩輪自平衡車保持此傾角運(yùn)行?？柭鼮V波圖如圖8所示。

圖7 角度濾波對(duì)比圖Fig.7 Angular filtering contrast figure

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

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

5 結(jié)論

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

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

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

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

［4］江杰，朱君，豈偉楠.四旋翼無(wú)人飛行器姿態(tài)數(shù)據(jù)采集處理系統(tǒng)［J］.計(jì)算機(jī)測(cè)量與控制，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］馮智勇，曾瀚，張力，等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量［J］.西南大學(xué)師范大學(xué)學(xué)報(bào)：自然科學(xué)版，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］焦勇，楊慶堂.兩輪自平衡機(jī)器人的姿態(tài)檢測(cè)系統(tǒng)的設(shè)計(jì)［J］.船電技術(shù)應(yīng)用研究，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.