兩輪系統(tǒng)是一種高度不穩(wěn)定、非線性及多干擾系統(tǒng)，要使兩輪系統(tǒng)達(dá)到自平衡，其模型決定了系統(tǒng)的重心須在兩車輪軸線上。在兩輪小車自平衡控制過程中，小車姿態(tài)傾角測(cè)量的準(zhǔn)確性與實(shí)時(shí)性決定了小車的控制精度及穩(wěn)定性,僅僅依靠陀螺儀或者加速度計(jì)對(duì)兩輪小車姿態(tài)傾角測(cè)量，其測(cè)量誤差及振動(dòng)干擾較大，往往達(dá)不到兩輪小車的自平衡控制。本文提出了基于陀螺儀與加速度計(jì)數(shù)據(jù)融合的兩輪自平衡小車控制方法，由陀螺儀與加速度計(jì)建立姿態(tài)傾角測(cè)量系統(tǒng)，通過卡爾曼濾波器進(jìn)行數(shù)據(jù)融合得到小車高精度、高可靠性的姿態(tài)傾角，最后利用數(shù)字PID控制算法實(shí)現(xiàn)兩輪小車的自平衡控制[1]。

1 兩輪自平衡小車的動(dòng)力學(xué)模型
    兩輪自平衡小車主要由車體、核心控制板、電機(jī)驅(qū)動(dòng)板和車輪等部件構(gòu)成，小車兩輪各參數(shù)（半徑、轉(zhuǎn)動(dòng)慣量、質(zhì)量等）相同且兩輪共軸，由兩直流電機(jī)獨(dú)立驅(qū)動(dòng)。圖1為小車模型[2]，假設(shè)小車質(zhì)量為m、車輪半徑為r、車輪前進(jìn)方向移動(dòng)速度為v、車體自身水平作用力為H、車輪所受地面摩擦力為Hf、車輪對(duì)車軸的等效轉(zhuǎn)動(dòng)慣量為J、車體對(duì)車輪等效轉(zhuǎn)動(dòng)慣量為I、車輪轉(zhuǎn)速為ω、電機(jī)電磁轉(zhuǎn)矩為Tm、車體質(zhì)心位置坐標(biāo)(x,y)、車體質(zhì)量ma、車體與垂直方向所成角度為φ、車輪對(duì)車身產(chǎn)生的扭矩為T。

    對(duì)車輪受力分析可知，車輪不僅受直流電機(jī)的輸出轉(zhuǎn)矩、地面支持力和摩擦力的影響，同時(shí)由于慣性作用還受到車體自身作用力的影響。將車輪的運(yùn)動(dòng)分解為前進(jìn)方向與繞軸方向的運(yùn)動(dòng)，車輪運(yùn)動(dòng)方程可描述為[2]：
  
2 卡爾曼濾波數(shù)據(jù)融合過程
    兩輪小車姿態(tài)傾角的測(cè)量由陀螺儀與加速度計(jì)完成。陀螺儀測(cè)量的是旋轉(zhuǎn)運(yùn)動(dòng)，輸出角速度信號(hào)，旋轉(zhuǎn)越快，其輸出值越大，對(duì)角速度數(shù)據(jù)取積分，即能得到車體傾角值[3]。陀螺儀易受震動(dòng)、溫度和不穩(wěn)定力矩等影響，產(chǎn)生漂移誤差，計(jì)算小車姿態(tài)傾角時(shí)由于積分作用其測(cè)量誤差會(huì)越來越大。加速度計(jì)測(cè)量的是線性運(yùn)動(dòng)，輸出加速度信號(hào)，速度變化越快，輸出值越大，通過三角變換可得到加速度計(jì)與重力方向的夾角[4]。加速度計(jì)沒有累計(jì)誤差，但是加速度計(jì)易受震動(dòng)干擾。因此，僅單獨(dú)使用陀螺儀或者加速度計(jì)對(duì)小車姿態(tài)進(jìn)行測(cè)量，很難保證測(cè)量結(jié)果的精確性和可靠性。卡爾曼濾波是一種較好的多傳感器信息融合算法。采用卡爾曼濾波算法對(duì)加速度計(jì)與陀螺儀所測(cè)數(shù)據(jù)進(jìn)行優(yōu)化處理，補(bǔ)償加速度計(jì)的動(dòng)態(tài)誤差與陀螺儀的漂移誤差，可得到精度與可靠性更高的小車姿態(tài)傾角。
    卡爾曼濾波算法是以最小均方誤差估計(jì)的一套遞推估計(jì)算法，其算法核心思想：采用信號(hào)與偏差的狀態(tài)方程，利用前一狀態(tài)得到的估計(jì)值和當(dāng)前狀態(tài)的測(cè)量值來更新對(duì)狀態(tài)變量的預(yù)測(cè)與估計(jì)，最終求得當(dāng)前狀態(tài)的最優(yōu)估計(jì)值。
    卡爾曼濾波過程可分為狀態(tài)預(yù)測(cè)和測(cè)量修正兩個(gè)過程。狀態(tài)預(yù)測(cè)方程及時(shí)地由前一時(shí)刻的系統(tǒng)狀態(tài)和噪聲方差估計(jì)出當(dāng)前時(shí)刻系統(tǒng)狀態(tài)；測(cè)量修正方程則將當(dāng)前時(shí)刻測(cè)量值結(jié)合狀態(tài)預(yù)測(cè)過程得到的當(dāng)前估計(jì)狀態(tài)來得到系統(tǒng)最優(yōu)估計(jì)值。
   
    圖3為陀螺儀接口電路，陀螺儀選用微型角速度傳感器ENC-03。由于陀螺儀輸出電壓信號(hào)較小，其輸出信號(hào)經(jīng)TLV2211放大6倍左右。圖4為加速度計(jì)接口電路，加速度計(jì)選用3軸小量程加速度傳感器MMA7361，其輸出電壓信號(hào)是小車運(yùn)動(dòng)方向與重力方向加速度的混合值。陀螺儀、加速度計(jì)接口電路均采用+5 V電源，與單片機(jī)STC12C5608AD供電電源相匹配，簡(jiǎn)化了硬件設(shè)計(jì)。陀螺儀、加速度計(jì)輸出電壓信號(hào)均由STC12C5608AD片內(nèi)集成A/D進(jìn)行采集。

3.2 軟件設(shè)計(jì)
    圖5為系統(tǒng)軟件實(shí)現(xiàn)框圖，系統(tǒng)軟件主要包括小車姿態(tài)傳感器數(shù)據(jù)采集程序、卡爾曼濾波程序、PID控制程序、直流電機(jī)PWM控制程序等部分。STC12C5608AD片內(nèi)集成A/D轉(zhuǎn)換和自帶PWM輸出功能,只需讀寫STC12C5608AD單片機(jī)內(nèi)部與A/D轉(zhuǎn)換和PWM相關(guān)的寄存器，就能完成對(duì)傳感器的數(shù)據(jù)采集與直流電機(jī)的PWM控制。因此，系統(tǒng)軟件關(guān)鍵在于卡爾曼濾波算法與數(shù)字PID算法的實(shí)現(xiàn)。

   
    采用雙閉環(huán)數(shù)字PID對(duì)兩輪系統(tǒng)進(jìn)行自平衡控制，其控制結(jié)構(gòu)如圖6所示。對(duì)光電編碼器產(chǎn)生的脈沖轉(zhuǎn)換得到兩輪實(shí)時(shí)速度信號(hào)，以該信號(hào)作為反饋量實(shí)現(xiàn)速度閉環(huán)控制；通過對(duì)陀螺儀與加速度計(jì)采集的數(shù)據(jù)進(jìn)行融合得到小車傾角信息，以小車傾角作為反饋量實(shí)現(xiàn)小車姿態(tài)閉環(huán)控制[7]。這樣就實(shí)現(xiàn)了兩輪自平衡系統(tǒng)的雙閉環(huán)數(shù)字PID控制，系統(tǒng)速度環(huán)與小車姿態(tài)環(huán)均采用位置型PID算法。

    本文介紹了基于數(shù)據(jù)融合的兩輪小車自平衡控制系統(tǒng)的設(shè)計(jì)方法，在建立兩輪自平衡小車的動(dòng)力學(xué)模型的基礎(chǔ)上，通過卡爾曼濾波算法有效地補(bǔ)償震動(dòng)、溫度及不穩(wěn)定力矩對(duì)陀螺儀與加速度傳感器的影響，得到小車最優(yōu)姿態(tài)傾角，最后通過雙閉環(huán)數(shù)字PID控制算法實(shí)現(xiàn)兩輪小車的自平衡控制。給予一定系統(tǒng)干擾，小車仍可實(shí)現(xiàn)自平衡，平衡時(shí)其傾角范圍在±10°內(nèi)。數(shù)據(jù)融合技術(shù)應(yīng)用于兩輪自平衡控制系統(tǒng)的方法，不僅提高了兩輪系統(tǒng)的穩(wěn)定性與抗干擾性，同時(shí)也為兩輪系統(tǒng)姿態(tài)控制領(lǐng)域提供了一種新的設(shè)計(jì)思路[8]。
參考文獻(xiàn)
[1] 馮智勇, 曾瀚, 張力,等. 基于陀螺儀與加速度儀新號(hào)融合的傾角姿態(tài)測(cè)量[J].西南師范大學(xué)學(xué)報(bào)(自然科學(xué)版), 2011(4):137-141.
[2] 李凡紅. 兩輪自平衡小車系統(tǒng)[D]. 北京:北京交通大學(xué), 2010.
[3] 李俊博,朱濤,鄒艷忠.陀螺穩(wěn)定系統(tǒng)參數(shù)測(cè)試儀設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制,2011,19(2):481-482.
[4] 于瑋, 封維忠, 武建軍. 基于MEMS加速度傳感器的數(shù)字水平儀[J].儀表技術(shù)與傳感器,2011(1):24-26,48.
[5] 王光林. 兩輪電動(dòng)車自平衡控制算法的研究[D]. 廣州：華南理工大學(xué), 2011.
[6] 代杰, 段晨東,李磊,等. 彩色液晶顯示模塊在智能儀表中的應(yīng)用[J].工業(yè)儀表與自動(dòng)化裝置,2012(2):27-30.
[7] 周海旭. 兩輪自平衡機(jī)器人系統(tǒng)設(shè)計(jì)[D]. 哈爾濱：哈爾濱工程大學(xué), 2010.
[8] 夏玉寶,何輔云,葛飛.基于多傳感器數(shù)據(jù)融合的漏磁信號(hào)采集與處理[J]. 電子技術(shù)應(yīng)用, 2008(2):116-118.