兩輪自平衡小車的設計

李榮偉，李鑫，孫傳開，張冬林，江振峰

（常熟理工學院電氣與自動化工程學院，江蘇常熟 215500）

兩輪自平衡小車的設計

李榮偉，李鑫，孫傳開，張冬林，江振峰

（常熟理工學院電氣與自動化工程學院，江蘇常熟 215500）

設計了以陀螺儀ENC-03以及MEMS加速度計MMA7260為傳感器的姿態感知系統，選用16位單片機MC9S12XS128為控制核心處理器，完成對傳感器信號的采集處理、車身控制以及人機交互的設計，實現小車自主控制平衡狀態、運行速度以及轉向角度大小等功能.實驗結果表明該系統的性能滿足設計要求.

兩輪自平衡；姿態檢測；卡爾曼濾波；數據融合；PID控制器

近年來，隨著電子技術的發展與進步，移動機器人的研究已成為目前科學研究最活躍的領域之一，移動機器人經常會遇到在較為狹窄復雜的環境中如何靈活快捷地執行任務的問題.兩輪自平衡機器人的概念就是在此背景下提出來的，這種機器人區別于其他移動機器人最顯著的特點是：采用了兩輪共軸、各自獨立驅動的工作方式（又稱差分式驅動），車身的重心位于車輪軸的上方，通過輪子的前后移動保持車身的平衡、行駛[1].因為具有體積小、運動靈活、零轉彎半徑等特點，所以在軍用和民用領域有著廣泛的應用前景，更重要的是系統具有多變量、非線性、強耦合、不穩定性的特性，使其成為很好地驗證控制理論及方法的平臺，具有很高的研究價值.

圖1 系統設計總體結構框圖

1 系統整體設計

本文設計的自平衡車采用姿態傳感器（加速度計和陀螺儀）監測車身所處的俯仰狀態和狀態變化率，通過高速微控制器（MC9S12XS128）完成數據融合處理，得到平滑而穩定車體姿態信息，然后驅動電動機產生前進或后退的加速度來控制車體保持平衡，同時系統還要根據速度的反饋量來完成對車體速度和方向的控制，微控制器還需構建相關輸入輸出模塊和人機交互設備.系統設計總體結構框圖如圖1所示.

已知自平衡車高度為l，質量為m，將其抽象為一級倒立擺，并將倒立擺置于可水平移動的小車上.假設其受外力干擾引起的車體角加速度為x(t)，沿垂直于車體方向進行受力分析（見圖2），可以得到自平衡車傾角與車輪移動加速度為a(t)以及外力干擾帶來的加速度x(t)之間的運動方程[2-3].

小車運動微分方程表達式為

圖2 外力干擾條件下的小車受力分析

當傾角θ很小的時候，可以進行線性化處理：cos(θ)≈1、sin(θ)≈θ，運動微分方程可簡化為

自平衡車靜止時a()

t=0，其運動微分方程為

由小車靜止時的運動方程可得到系統的傳遞函數：

加入比例微分反饋后的系統傳遞函數為

圖3 加入比例微分環節后的控制系統結構圖

2 硬件模塊設計

系統硬件主要由以下幾個模塊組成：MC9S12XS128單片機最小系統、電源模塊、姿態檢測模塊、電機驅動電路和速度檢測電路.

本設計采用Freescale公司16位單片機MC9S12XS128為控制器，工作電壓為5 V.其豐富的內置模塊資源可以為姿態檢測系統提供12位精度的A/D轉換，為速度檢測模塊提供脈沖計數，同時擁有獨立的PWM輸出模塊可以控制電機輸出.

姿態檢測模塊能夠精確并穩定地檢測出當前的車身姿態，這是實現有效控制的關鍵，本設計選用慣性傳感器MEMS陀螺儀ENC-03和加速度計MMA7260作為姿態檢測傳感器.加速度MMA7260傳感器是一種低g值的傳感器，根據物件運動改變各軸的信號輸出.各軸的信號在不運動或不受重力作用的狀態下（0 g），其輸出為1.65 V．MMA7260的靈敏度可通過gs_1和gs_2的引腳電平來設定.分別為800 mv/g、600 mv/g、300 mv/g、200 mv/g，為了很好地檢測車體微小變化，本電路通過gs_1和gs_2腳同時接地，將其靈敏度設定為800 mv/g.本系統采用的陀螺儀為村田公司生產的ENC-03[4]，其輸出為0.67 mv/deg/sec.采用LM358構成負反饋同相放大電路，增益為R3 R4+1，即放大10倍左右.由于陀螺儀靜態輸出會隨溫度等因素變化，影響系統傾角檢測，為此在放大電路中利用LM358設計了零點偏置電壓校正電路，并將陀螺儀靜態輸出電壓調整為模擬電壓的一半（此處為1.65 V）.姿態檢測模塊電路原理如圖4和圖5所示.

圖4 MMA7260三軸加速度計電路原理圖

圖5 ENC-03放大電路設計原理圖

電源管理模塊是整個硬件電路穩定可靠運行的基礎，整個系統需要3種電源：為驅動電機供電的7.2 V電源，為單片機及相關外設供電的+5 V數字電源，為姿態傳感器和A/D基準電壓供電的3.3 V模擬電源.整個系統電源來源為7.2 V鎳氫電池，5 V電源由LM2940提供.LM2940是一種線性低壓差三端穩壓器件，其輸出紋波較小，適合單片機供電.3.3 V電源采用AMS1117轉換而得.電機驅動采用全橋式驅動電路實現電機的正反轉，系統采用兩片專用半橋驅動芯片BTS7960構成全橋式驅動電路.速度檢測模塊采用歐姆龍公司500線增量式旋轉編碼器，通過單片機的PACNT模塊對輸入脈沖進行計數，從而為處理器提供速度反饋量.輔助調試模塊由按鍵、RS-232串口通信模塊、電腦等組成.

3 系統軟件設計

圖6 軟件流程設計

3.1 軟件設計框架

系統軟件設計框架主要包括：資源模塊初始化、姿態信息采集、數據融合、速度檢測、直流電機PID控制.系統軟件流程如圖6所示.

3.2 資源模塊初始化

單片機系統初始化主要完成以下工作：通過鎖相環模塊（PLL），設定單片機的工作頻率為64 MHz；將模數轉換模塊（ATD）設置為12位精度、4通道連續采樣、采樣時間4 MHz、非中斷觸發；串行通信模塊（SCI）設置為波特率9600、8個數據位、1個停止位、允許收發；ECT中斷使能8路輸入捕捉（IC）/輸出比較（OC）通道、下降沿捕捉、使能捕捉中斷；定時器（PIT）產生10 ms的定時中斷；PWM模塊使用0通道和1通道級聯、2通道和3通道級聯產生兩路10 kHz、左對齊的PWM等.

3.3 車模姿態信息檢測

3.3.1 卡爾曼濾波器

卡爾曼濾波的程序實現是先利用angle+=（gyro m-q bias）dt計算出陀螺儀積分出的角度，其中q bias是陀螺儀偏差.此時利用陀螺儀積分求出的angle作為系統的估計值，得到系統的觀測方程；而加速度計檢測的角度angle m當作于系統中的測量值，得到系統狀態方程.在程序中Q angle和Q gyro分別表示系統對加速度計及陀螺儀的信任度.根據Pdot=AP+PA′+Q angle計算出先驗估計協方差的微分，用于將當前估計值進行線性化處理.其中A為雅克比矩陣[5-7].本系統中雅克比矩陣為

隨后計算系統預測角度的協方差矩陣P.計算估計值angle m與預測值angle間的誤差angle err.計算卡爾曼增益K_0，K_1，K_0用于最優估計值，K_1用于計算最優估計值的偏差并更新協方差矩陣P.通過卡爾曼增益計算出最優估計值angle及預測值偏差q bias，此時得到最優角度值angle及角速度值.

3.3.2 姿態檢測的參數整定

姿態調試采用串口波形顯示的方式進行.XS128通過串口通訊向PC機發送濾波前與濾波后的姿態傾角數據，通過上位機波形顯示軟件（數據被放大了十倍），將離散的數據點描繪為動態波形，參數的不同可以通過前后對比直觀地看出.圖7和圖8截取了兩張相對良好且不同運動狀態的調試效果圖.

圖7 車體平滑振動圖

圖8 車體劇烈振蕩圖

由以上兩幅圖可以直觀地看出：加速度計傳感器計算出的角度一定情況下可以反應出車體的傾角，但是毛刺太多、震蕩太大不適合直接利用.卡爾曼濾波器不僅抑制了陀螺儀的零點漂移問題，還使計算的最優角度平滑、穩定，并能反映出車體的狀態，滿足設計需求.

3.4 直流電機PID控制

分析系統要求可知，保持小車直立和運動的動力都來自于小車的兩個車輪，車輪由兩個直流電機驅動.因此，從控制角度來看，可以將小車作為一個控制對象，控制輸入量是兩個車輪的驅動電壓.整個控制系統可以分為三個子系統.

（1）小車平衡控制：以小車傾角為輸入量，通過控制兩個電機的正反轉保持小車平衡.PID控制器的程序實現為：OUT Motor=Kp*Angle+Kd*Angle dot；式中OUT Motor為平衡控制輸出量，Angle為卡爾曼濾波后的傾角偏差值，Angle dot為卡爾曼濾波后的角速度值，Kp和Kd分別為比例系數及微分系數.在次采用是試湊法來整定Kp和Kd的，并用滿占空比的百分比進行描述.先設定Kd=0，根據上述小車的數學模型可知，Kp需要大于重力加速度所產生的效果才能使車模保持直立.將Kp從零逐漸增大到5%左右時，車模開始有直立的趨勢.進一步加大Kp到11%左右，使得車模保持直立并開始來回擺動.說明此時車模比例參數已經可以克服重力的影響，然后逐步增加微分參數使車模擺動減小.適當微調Kp、Kd參數使車模可以保持穩定直立，車模保持上述情況的比例和微分參數有一個相當大的空間.由于受車模重心尺寸的影響，Kp、Kd的參數不唯一，此處Kp為12.4%，Kd為0.3%.實際處理時在車模動力傳動系統中，各個環節都存在著靜摩擦力，它會降低車模靜態穩定性.所以程序中增加一個死區電壓（滿占空比的4%）.

（2）小車速度控制：在保持平衡的基礎上，通過調節小車傾角實現對速度的控制，實際上還是演變為對電機的控制實現小車的速度控制.本系統以編碼器所測得脈沖數作為速度反饋環節，形成了速度的閉環控制.速度控制的函數參數為車速的偏差，返回值為傾角，通過平衡控制達到控制車速的目的.在實際速度控制中，采用了帶死區的PID算法，防止調節過于頻繁，導致系統振蕩.設定系統平衡控制周期為10 ms，為減少干擾并且提高速度控制的力度，系統速度控制周期設定為50 ms.速度程序計算代碼為：angle speed= Speed Kp*Speed error+Speed Ki*Speed diff；式中angle speed為速度控制輸出量，Speed erro為速度脈沖的偏差，Speed Kp、Speed Ki為比例、積分項的系數.設定速度值為0，調整這些系數并觀察車模在受到外力作用時能否迅速恢復直立靜止狀態.逐步增大比例參數會發現車模能在平衡點附近來回運動，然后增加微分參數使車模能夠很快趨于靜止，微調參數后使車模抵抗外部干擾沖擊的能力逐步增強，此處取Speed Kp為滿占空比的1.22%，Speed Ki為滿占空比的0.23%.

（3）小車方向控制：通過控制兩個電機間的電壓不同實現轉向.本系統是在速度控制周期內，對兩個電機實現差速輸出，則可在保持平衡的基礎上實現轉彎.系統調試時，由其自動采集道路中的電磁場信息（由通有100 mA交變電流的導線所產生），并對路徑參數進行識別與路徑優化后，完成前進、轉彎等動作.

4 總結

圖9 系統實物

本設計主要研究兩輪自平衡小車的設計與實現.通過相應硬件與軟件的設計，實現了兩輪自平衡小車的動態平衡與運動控制.系統實物如圖9所示.

系統硬件以飛思卡爾公司16位單片機MC9S12XS128為控制核心，采用壓電陀螺儀ENC-03及MEMS加速度計MMA7260構成了慣性姿態檢測系統，通過H橋電機驅動及旋轉編碼器實現了直流電機的閉環調速.本系統結合硬件構架設計了以卡爾曼濾波器為核心的數據融合算法.通過卡爾曼濾波器將陀螺儀與加速度計的輸出融合為準確的傾角與角速度輸出，為系統的控制提供了有力保障.本設計的控制策略采用PID控制算法.通過對小車的運動建模，構建了小車運動控制的PD控制算法，并對PID參數進行了整定.本設計最終實現了兩輪自平衡小車的平衡控制及運動控制.小車在保持兩輪平衡的基礎上可以實現前進、后退、轉彎等基本動作.

[1]He Kezhong,Sun Haihang,Guo Mtthe.Research of intelligent mobile robot key techniques[J].IEEE International Conference on IndusIrial Technology,1996(7):503-507.

[2]屠運武，徐俊艷，張培仁.自平衡控制系統的建模與仿真[J].系統仿真學報，2004（04）.

[3]霍亮.兩輪自平衡電動車的關鍵技術研究[D].哈爾濱：哈爾濱工程大學，2010.

[4]鄒學鋒，盧新艷.基于Allan方差的MEMS陀螺儀性能評價方法[J].微納電子技術，2010，47（8）：490-493.

[5]趙杰，王曉宇，秦勇，等.基于UKF的兩輪自平衡機器人姿態最優估計研究[J].機器人，2006（11）：605-609.

[6]王曉宇，閆繼宏，臧希喆，等.兩輪自平衡機器人多傳感器數據融合方法研究[J].傳感器技術學報，2007（3）：668-672

[7]耿延睿，崔中興.組合導航系統卡爾曼濾波衰減因子自適應估計算法研究[J].中國慣性技術學報，2001（04）.

A Design of Two-Wheel Self-Balance Vehicle

LI Rong-wei,LI Xin,SUN Chuan-kai,ZHANG Dong-lin,JIANG Zhen-feng
(School of Electrical and Automation Engineering,Changshu Institute of Technology,Changshu 215500,China)

A two-wheel self-balancing electric vehicle is an important branch of moving robots,because of its dynamic system with multi-variables,nonlinear,strong coupling,time-varying,with parametric uncertainties, and other characteristics,and the complex research can make the experiment device of a control theory become an ideal platform.This paper summarizes the current situation of research on a two-wheel self-balancing robot. And gyroscope ENC-03,MMA7260 for attitude sensing sensors and MEMS accelerometer system are designed in the paper.By means of Freescale 16-bit single-chip Microcomputer Mc9s12xs128 for the control of core processor,the paper completes the acquisition of sensor signal processing,body control,as well as features such as human-computer interaction.Besides,the paper realizes the independent control balance of the car features such as size,speed and steering angle.Experimental results show that performance meets the design requirements of the system.

two-wheel self-balance;gesture detection;Kalman filter;data fusion;PID controller

TP242.6

A

1008-2794（2012）10-0070-06

2012-09-07

江蘇省大學生實踐創新訓練計劃項目“兩輪自平衡機器小車的設計”（jx110152011）

李榮偉（1989—），男，江蘇東海人，常熟理工學院電氣與自動化工程學院測控技術與儀器專業2009級學生.

李鑫（1983—），男，安徽亳州人，實驗師，碩士，研究方向：智能控制技術與現代檢測技術，E-mail:lixin_yy@yahoo.com.cn．