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

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

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

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

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

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

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

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

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

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

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

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

小車運(yùn)動微分方程表達(dá)式為

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

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

自平衡車靜止時a()

t=0，其運(yùn)動微分方程為

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

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

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

2 硬件模塊設(shè)計(jì)

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

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

姿態(tài)檢測模塊能夠精確并穩(wěn)定地檢測出當(dāng)前的車身姿態(tài)，這是實(shí)現(xiàn)有效控制的關(guān)鍵，本設(shè)計(jì)選用慣性傳感器MEMS陀螺儀ENC-03和加速度計(jì)MMA7260作為姿態(tài)檢測傳感器.加速度MMA7260傳感器是一種低g值的傳感器，根據(jù)物件運(yùn)動改變各軸的信號輸出.各軸的信號在不運(yùn)動或不受重力作用的狀態(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)成負(fù)反饋同相放大電路，增益為R3 R4+1，即放大10倍左右.由于陀螺儀靜態(tài)輸出會隨溫度等因素變化，影響系統(tǒng)傾角檢測，為此在放大電路中利用LM358設(shè)計(jì)了零點(diǎn)偏置電壓校正電路，并將陀螺儀靜態(tài)輸出電壓調(diào)整為模擬電壓的一半（此處為1.65 V）.姿態(tài)檢測模塊電路原理如圖4和圖5所示.

圖4 MMA7260三軸加速度計(jì)電路原理圖

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

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

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

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

3.1 軟件設(shè)計(jì)框架

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

3.2 資源模塊初始化

單片機(jī)系統(tǒng)初始化主要完成以下工作：通過鎖相環(huán)模塊（PLL），設(shè)定單片機(jī)的工作頻率為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計(jì)算出陀螺儀積分出的角度，其中q bias是陀螺儀偏差.此時利用陀螺儀積分求出的angle作為系統(tǒng)的估計(jì)值，得到系統(tǒng)的觀測方程；而加速度計(jì)檢測的角度angle m當(dāng)作于系統(tǒng)中的測量值，得到系統(tǒng)狀態(tài)方程.在程序中Q angle和Q gyro分別表示系統(tǒng)對加速度計(jì)及陀螺儀的信任度.根據(jù)Pdot=AP+PA′+Q angle計(jì)算出先驗(yàn)估計(jì)協(xié)方差的微分，用于將當(dāng)前估計(jì)值進(jìn)行線性化處理.其中A為雅克比矩陣[5-7].本系統(tǒng)中雅克比矩陣為

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

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

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

圖7 車體平滑振動圖

圖8 車體劇烈振蕩圖

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

3.4 直流電機(jī)PID控制

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

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

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

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

4 總結(jié)

圖9 系統(tǒng)實(shí)物

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

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

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

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

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

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

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

江蘇省大學(xué)生實(shí)踐創(chuàng)新訓(xùn)練計(jì)劃項(xiàng)目“兩輪自平衡機(jī)器小車的設(shè)計(jì)”（jx110152011）

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

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