兩輪自平衡小車的設(shè)計

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

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

兩輪自平衡小車的設(shè)計

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

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

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

兩輪自平衡；姿態(tài)檢測；卡爾曼濾波；數(shù)據(jù)融合；PID控制器

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

圖1 系統(tǒng)設(shè)計總體結(jié)構(gòu)框圖

1 系統(tǒng)整體設(shè)計

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

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

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

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

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

自平衡車靜止時a()

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

由小車靜止時的運動方程可得到系統(tǒng)的傳遞函數(shù)：

加入比例微分反饋后的系統(tǒng)傳遞函數(shù)為

圖3 加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖

2 硬件模塊設(shè)計

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

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

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

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

圖5 ENC-03放大電路設(shè)計原理圖

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

3 系統(tǒng)軟件設(shè)計

圖6 軟件流程設(shè)計

3.1 軟件設(shè)計框架

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

3.2 資源模塊初始化

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

3.3 車模姿態(tài)信息檢測

3.3.1 卡爾曼濾波器

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

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

3.3.2 姿態(tài)檢測的參數(shù)整定

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

圖7 車體平滑振動圖

圖8 車體劇烈振蕩圖

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

3.4 直流電機PID控制

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

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

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

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

4 總結(jié)

圖9 系統(tǒng)實物

本設(shè)計主要研究兩輪自平衡小車的設(shè)計與實現(xiàn).通過相應(yīng)硬件與軟件的設(shè)計，實現(xiàn)了兩輪自平衡小車的動態(tài)平衡與運動控制.系統(tǒng)實物如圖9所示.

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

[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]屠運武，徐俊艷，張培仁.自平衡控制系統(tǒng)的建模與仿真[J].系統(tǒng)仿真學報，2004（04）.

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

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

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

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

[7]耿延睿，崔中興.組合導航系統(tǒng)卡爾曼濾波衰減因子自適應(yīng)估計算法研究[J].中國慣性技術(shù)學報，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

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

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

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