魏 文，段晨東，高精隆，李常磊

(長安大學 電子與控制工程學院，陜西 西安 710064)

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

1 兩輪自平衡小車的動力學模型

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

對車輪受力分析可知，車輪不僅受直流電機的輸出轉矩、地面支持力和摩擦力的影響，同時由于慣性作用還受到車體自身作用力的影響。將車輪的運動分解為前進方向與繞軸方向的運動，車輪運動方程可描述為[2]：

對上述方程組消去Hf，車輪運動方程簡化為：

對車體進行數學建模，其動力學方程可描述為[2]：

上式就是兩輪自平衡小車關于車體傾角與角加速度的微分方程。

2 卡爾曼濾波數據融合過程

兩輪小車姿態(tài)傾角的測量由陀螺儀與加速度計完成。陀螺儀測量的是旋轉運動，輸出角速度信號，旋轉越快，其輸出值越大，對角速度數據取積分，即能得到車體傾角值[3]。陀螺儀易受震動、溫度和不穩(wěn)定力矩等影響，產生漂移誤差，計算小車姿態(tài)傾角時由于積分作用其測量誤差會越來越大。加速度計測量的是線性運動，輸出加速度信號，速度變化越快，輸出值越大，通過三角變換可得到加速度計與重力方向的夾角[4]。加速度計沒有累計誤差，但是加速度計易受震動干擾。因此，僅單獨使用陀螺儀或者加速度計對小車姿態(tài)進行測量，很難保證測量結果的精確性和可靠性?？柭鼮V波是一種較好的多傳感器信息融合算法。采用卡爾曼濾波算法對加速度計與陀螺儀所測數據進行優(yōu)化處理，補償加速度計的動態(tài)誤差與陀螺儀的漂移誤差，可得到精度與可靠性更高的小車姿態(tài)傾角。

卡爾曼濾波算法是以最小均方誤差估計的一套遞推估計算法，其算法核心思想：采用信號與偏差的狀態(tài)方程，利用前一狀態(tài)得到的估計值和當前狀態(tài)的測量值來更新對狀態(tài)變量的預測與估計，最終求得當前狀態(tài)的最優(yōu)估計值。

卡爾曼濾波過程可分為狀態(tài)預測和測量修正兩個過程。狀態(tài)預測方程及時地由前一時刻的系統(tǒng)狀態(tài)和噪聲方差估計出當前時刻系統(tǒng)狀態(tài)；測量修正方程則將當前時刻測量值結合狀態(tài)預測過程得到的當前估計狀態(tài)來得到系統(tǒng)最優(yōu)估計值。

對于離散控制過程，假設系統(tǒng)當前狀態(tài)為k，前一狀態(tài)為 k-1，(k)表示當前狀態(tài)的最優(yōu)估計值，(k|k-1)表示利用前一狀態(tài)計算得到的預測值，(k-1)表示前一狀態(tài)的最優(yōu)估計值，K(x)表示系統(tǒng)卡爾曼增益，Q表示系統(tǒng)過程的協方差，P(k)為(k)對應的協方差，P(k-1)為(k-1)對應的協方差，P(k|k-1)為(k|k-1)對應的協方差，卡爾曼濾波狀態(tài)預測過程可描述為[5]：

卡爾曼濾波測量修正方程為[5]：

3 系統(tǒng)設計

3.1 硬件設計

系統(tǒng)硬件主要由主控制器、傳感器、直流電機驅動模塊等組成，傳感器包括加速度計、陀螺儀、光電編碼器，通過這些傳感器完成對小車姿態(tài)參數的測量，其中車體傾角、車體角速度分別由加速度計和陀螺儀直接測量。

兩輪小車控制系統(tǒng)主控制器選用STC12C5608AD微控制器，該微控制器是宏晶公司最新推出的一款高性能增強型8051 MCU，指令系統(tǒng)與傳統(tǒng)8051單片機完全兼容，運行速度卻是傳統(tǒng)8051單片機的 6～12倍，其內部集成768 B RAM，具有8路高速12位A/D轉換，同時可提供4路8位PWM輸出[6]。STC12C5608AD豐富的硬件資源以及高速運算性能，使得系統(tǒng)硬件電路簡單、可靠性高。由主控制器STC12C5608AD完成對陀螺儀、加速度計與光電編碼器的數據采集與處理，得到小車姿態(tài)傾角，然后控制直流電機驅動模塊實現小車的姿態(tài)調整，最終達到小車的自平衡控制,系統(tǒng)硬件結構如圖2所示。

圖3為陀螺儀接口電路，陀螺儀選用微型角速度傳感器ENC-03。由于陀螺儀輸出電壓信號較小，其輸出信號經TLV2211放大6倍左右。圖4為加速度計接口電路，加速度計選用3軸小量程加速度傳感器MMA7361，其輸出電壓信號是小車運動方向與重力方向加速度的混合值。陀螺儀、加速度計接口電路均采用+5 V電源，與單片機STC12C5608AD供電電源相匹配，簡化了硬件設計。陀螺儀、加速度計輸出電壓信號均由STC12C5608AD片內集成A/D進行采集。

3.2 軟件設計

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

本系統(tǒng)采用C51程序實現卡爾曼濾波算法。由卡爾曼濾波方程可知，實現卡爾曼濾波的關鍵在于確定合適的狀態(tài)向量。通過對濾波器的校正，系統(tǒng)過程的噪聲協方差矩陣Q與測量誤差的協方差矩陣R確定形式如下[7]：

其中Qangle與Qgyro分別是加速度計與陀螺儀的測量協方差,其不同取值代表卡爾曼濾波器對其的不同信任程度，Rangle是小車傾角測量誤差。通過小車實驗調試，本系統(tǒng) Qangle取值為 0.000 2，Qgyro取值為 0.001，Rangle取值為0.92。

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

本文介紹了基于數據融合的兩輪小車自平衡控制系統(tǒng)的設計方法，在建立兩輪自平衡小車的動力學模型的基礎上，通過卡爾曼濾波算法有效地補償震動、溫度及不穩(wěn)定力矩對陀螺儀與加速度傳感器的影響，得到小車最優(yōu)姿態(tài)傾角，最后通過雙閉環(huán)數字PID控制算法實現兩輪小車的自平衡控制。給予一定系統(tǒng)干擾，小車仍可實現自平衡，平衡時其傾角范圍在±10°內。數據融合技術應用于兩輪自平衡控制系統(tǒng)的方法，不僅提高了兩輪系統(tǒng)的穩(wěn)定性與抗干擾性，同時也為兩輪系統(tǒng)姿態(tài)控制領域提供了一種新的設計思路[8]。

[1]馮智勇,曾瀚,張力,等.基于陀螺儀與加速度儀新號融合的傾角姿態(tài)測量[J].西南師范大學學報(自然科學版),2011(4):137-141.

[2]李凡紅.兩輪自平衡小車系統(tǒng)[D].北京:北京交通大學,2010.

[3]李俊博,朱濤,鄒艷忠.陀螺穩(wěn)定系統(tǒng)參數測試儀設計[J].計算機測量與控制,2011,19(2):481-482.

[4]于瑋,封維忠,武建軍.基于MEMS加速度傳感器的數字水平儀[J].儀表技術與傳感器,2011(1):24-26,48.

[5]王光林.兩輪電動車自平衡控制算法的研究[D].廣州：華南理工大學,2011.

[6]代杰,段晨東,李磊,等.彩色液晶顯示模塊在智能儀表中的應用[J].工業(yè)儀表與自動化裝置,2012(2):27-30.

[7]周海旭.兩輪自平衡機器人系統(tǒng)設計[D].哈爾濱：哈爾濱工程大學,2010.

[8]夏玉寶,何輔云,葛飛.基于多傳感器數據融合的漏磁信號采集與處理[J].電子技術應用,2008(2):116-118.