基于單片機的兩輪自平衡車控制系統(tǒng)設(shè)計

1、基于單片機的兩輪自平衡車控制系統(tǒng)設(shè)計摘要兩輪自平衡車是一種高度不穩(wěn)定的兩輪機器人，就像傳統(tǒng)的倒立擺一樣，本質(zhì)不穩(wěn)定是兩輪小車的特性，必須施加有效的控制手段才能使其穩(wěn)定。本文提出了一種兩輪自平衡小車的設(shè)計方案，采用重力加速度陀螺儀傳感器MPU-6050僉測小車姿態(tài)，使用互補濾波完成陀螺儀數(shù)據(jù)與加速度計數(shù)據(jù)的數(shù)據(jù)融合。系統(tǒng)選用STC公司的8位單片機STC12C5A60S2為主控制器，根據(jù)從傳感器中獲取的數(shù)據(jù)，經(jīng)過PID算法處理后，輸出控制信號至電機驅(qū)動芯片TB6612FNG以控制小車的兩個電機，來使小車保持平衡狀態(tài)。整個系統(tǒng)制作完成后，小車可以在無人干預(yù)的條件下實現(xiàn)自主平衡，并且在引入適量干擾的情

2、況下小車能夠自主調(diào)整并迅速恢復(fù)至穩(wěn)定狀態(tài)。通過藍牙，還可以控制小車前進，后退，左右轉(zhuǎn)。關(guān)鍵詞：兩輪自平衡小車加速度計陀螺儀數(shù)據(jù)融合濾波PID算法DesignofControlSystemofTwo-WheelSelf-BalanceVehiclebasedonMicrocontrollerAbstractTwo-wheelself-balancevehicleisakindofhighlyunstabletwo-wheelrobot.Thecharacteristicoftwo-wheelvehicleisthenatureoftheinstabilityastraditionalinvert

3、edpendulum,andeffectivecontrolmustbeexertedifweneedtomakeitstable.Thispaperpresentsadesignschemeoftwo-wheelself-balancevehicle.WeneedusinggravityaccelerometergyroscopesensorMPU6050fortheinclinationangleofvehicle,andusingcomplementaryfilterforthedatafusionofgyroscopeandaccelerometer.Wechoosean8-bitmi

4、crocontrollernamedSTC12C5A60S2fromSTCCompanyasmaincontrollerofthecontrolsystem.Themaincontrolleroutputcontrolsignal,whichisbasedonthedatafromthesensors,tothemotordrivechipnamedTB6612FNGforcontrollingtwomotorsofvehicle,andkeepingthevehicleinbalance.Afterthecompletionofthecontrolsystem,thevehiclecanac

5、hieveautonomousbalanceundertheconditionsofunmannedintervention,thevehiclecanadjustautomaticallyandrestoredtoastablestatequicklyinthecaseofgivingappropriateinterferenceaswell.Inaddition,wecancontrolthevehicleforward,backwardandturnaround.Keywords:Two-WheelSelf-BalanceVehicle;Accelerometer;Gyroscope;D

6、atafusion;Complementaryfilter;PIDalgorithm1緒論自平衡小車的研究背景近幾年來，隨著電子技術(shù)的發(fā)展與進步，移動機器人的研究不斷深入，成為目前機器人研究領(lǐng)域的一個重要組成部分，并且其應(yīng)用領(lǐng)域日益廣泛，其所需適應(yīng)的環(huán)境和執(zhí)行的任務(wù)也更復(fù)雜，這就對移動機器人提出了更高的要求。比如，戶外移動機器人需要在凹凸不平的地面上行走，有時機器人所需要運行的地方比較狹窄等。如何解決機器人在這些環(huán)境中運行的問題，已成為現(xiàn)實應(yīng)用中所需要面對的一個問題。兩輪自平衡小車就是在這些的需求下所產(chǎn)生的。這種機器人相對于其他移動機器人的最顯著特點是：采用了兩輪共軸、各自獨立驅(qū)動的方式工作，

7、車身重心位于車輪軸上方，通過車輪的前后滾動來保持車身的動態(tài)平衡，并可以在直立平衡狀態(tài)下完成前進、后退、左右轉(zhuǎn)等任務(wù)。正是由于其特殊的構(gòu)造，兩輪自平衡小車適應(yīng)地形變化的能力較強，且運動靈活，可以勝任一些復(fù)雜環(huán)境中的工作。兩輪自平衡車自面世以來，一直受到世界各國機器人愛好者和研究者的關(guān)注，這不僅是因為兩輪自平衡車具有獨特的外形和結(jié)構(gòu)，更重要的是因為其自身的本質(zhì)不穩(wěn)定性和非線性使它成為很好的驗證控制理論和控制方法的平臺，具有很高的研究價值。早在1987年，日本電信大學(xué)教授山藤一雄就提出了兩輪自平衡機器人的概念。這個基本的概念就是用數(shù)字處理器來偵測平衡的改變，然后以平行的雙輪來保持機器的平穩(wěn)。本世紀(jì)初

8、。美國發(fā)明家狄恩卡門與他的DEKA公司研發(fā)出了可以用于載人的兩輪自平衡車，并命名為賽格威，投入市場后，引發(fā)了自平衡車的流行。由于兩輪自平衡車有著活動靈活，環(huán)境無害等優(yōu)點，具被廣泛應(yīng)用于各類高規(guī)格社會活動中，目前該車已用于奧運會、世博會、機場、火車站等大型場合。自平衡小車研究意義由于兩輪自平衡小車具有結(jié)構(gòu)特殊、體積小、運動靈活、適應(yīng)地形變化能力強、能夠方便的實現(xiàn)零半徑回轉(zhuǎn)、適合在擁擠和危險的空間內(nèi)活動、可以勝任一些復(fù)雜環(huán)境里的工作。因此兩輪自平衡車有著廣泛的應(yīng)用前景，其典型應(yīng)用包括代步工具、通勤車、空間探索、危險品運輸、高科技玩具、控制理論測試平臺等方面。目前自平衡車的應(yīng)用如自平衡的代步車正在流

9、行開來。因此兩輪自平衡車的研究很有意義。論文的主要內(nèi)容本論文主要敘述了基于單片機的兩輪自平衡車控制系統(tǒng)的設(shè)計與實現(xiàn)的整個過程。主要內(nèi)容為兩輪自平衡小車的平衡原理，直立控制，藍牙控制。整個內(nèi)容分為六章，包括緒論、課題任務(wù)與關(guān)鍵技術(shù)、系統(tǒng)原理概述、系統(tǒng)硬件設(shè)計、系統(tǒng)軟件設(shè)計和系統(tǒng)的機械安裝及調(diào)試。第一章主要講解了課題的研究背景及意義，國內(nèi)外研究現(xiàn)狀。第二章主要講解了設(shè)計的主要任務(wù)與所需的關(guān)鍵技術(shù)。第三章主要講解了兩輪自平衡小車控制系統(tǒng)的直立控制原理，轉(zhuǎn)向控制原理。第四章主要講解了系統(tǒng)的硬件設(shè)計，介紹了自平衡小車控制系統(tǒng)的硬件構(gòu)成，主控芯片STC12C5A60S的結(jié)構(gòu)及組成，以及穩(wěn)壓電源模塊，傾角測

10、量模塊，直流電機驅(qū)動模塊，藍牙控制模塊和兩輪測速模塊的設(shè)計。第五章主要講解了軟件設(shè)計的算法功能與框架，主要描述了控制系統(tǒng)的程序?qū)崿F(xiàn)以及PID算法的使用。第六章主要講解了系統(tǒng)的調(diào)試與參數(shù)整定。最后總結(jié)與展望，總結(jié)本設(shè)計的各個模塊，并對兩輪自平衡小車的優(yōu)化方向進行簡要的闡述。2課題任務(wù)與關(guān)鍵技術(shù)主要任務(wù)本文研究并設(shè)計了一種基于單片機的兩輪自平衡小車控制系統(tǒng)，實現(xiàn)了兩輪小車的自主直立控制與藍牙控制功能。系統(tǒng)采用STC12C5A60sM片機作為核心控制單元，通過增加各種傳感器，設(shè)計相應(yīng)電路并編寫相應(yīng)程序完成平衡控制與藍牙控制。系統(tǒng)需要利用加速度計和陀螺儀獲得車體的傾角和角速度，并對數(shù)據(jù)進行互補濾波融合

11、。通過編碼器獲得兩輪的速度信息。根據(jù)獲得的數(shù)據(jù)信息對速度和傾角進行閉環(huán)控制。加入藍牙通信控制，將所有輸出數(shù)據(jù)進行疊加，輸出至驅(qū)動芯片，實現(xiàn)對小車的控制。關(guān)鍵技術(shù)系統(tǒng)設(shè)計兩輪自平衡車的系統(tǒng)設(shè)計包括：車身機械結(jié)構(gòu)設(shè)計，硬件系統(tǒng)設(shè)計和軟件系統(tǒng)設(shè)計。在機械結(jié)構(gòu)上必須保持小車重心的穩(wěn)定性，才能避免控制系統(tǒng)過于復(fù)雜；硬件系統(tǒng)必須包含自平衡車所需的所有電子系統(tǒng)與電氣設(shè)備；軟件系統(tǒng)則負責(zé)車身平衡控制與目標(biāo)效果的實現(xiàn)。數(shù)學(xué)建模模型的建立有助于控制器的設(shè)計，以及控制系統(tǒng)各項參數(shù)的大概確定。模型的建立主要使用牛頓力學(xué)定律。姿態(tài)檢測兩輪自平衡車是一個本質(zhì)不平衡的系統(tǒng)，控制系統(tǒng)對小車的精確控制依賴于姿態(tài)檢測系統(tǒng)對車身姿

12、態(tài)及運動狀態(tài)的精確檢測。目前，一般采用由陀螺儀和加速度計等慣性傳感器組成的姿態(tài)檢測系統(tǒng)對車身傾角進行實時、準(zhǔn)確的檢測。但是由于慣性傳感器自身固有的特性，隨著溫度、震動等外界變化，會產(chǎn)生不同程度的噪聲與漂移，因此必須采用一些濾波算法，對加速度計和陀螺儀所采集的數(shù)據(jù)進行融合，使測量角度更加真實穩(wěn)定?？刂扑惴▋奢喿云胶廛囁鶎崿F(xiàn)的平衡是一種動態(tài)的平衡。在遇到外界干擾時，需要通過控制算法來快速將小車恢復(fù)至平衡狀態(tài)。傳統(tǒng)的PID算法在各類工業(yè)場合有著廣泛的應(yīng)用，完全可以滿足本控制系統(tǒng)的要求，因此本控制系統(tǒng)設(shè)計采用PID控制算法。3系統(tǒng)原理分析控制系統(tǒng)任務(wù)分解根據(jù)系統(tǒng)要求，小車必須能夠在沒有外界干預(yù)的情況下

13、依靠兩個同軸安裝的車輪保持平衡，并完成前進，后退，左右轉(zhuǎn)等動作。相對于四輪車，控制系統(tǒng)的任務(wù)更為復(fù)雜，為了能解決該問題，首先將復(fù)雜的問題分解成簡單的幾個問題進行討論。對系統(tǒng)要求進行分析，可知維持小車直立，并在受到外界干擾后迅速恢復(fù)穩(wěn)態(tài)，完全依賴于一對直流電機對車輪的驅(qū)動。因此本控制系統(tǒng)的設(shè)計可以從對電機的控制著手，控制電機的轉(zhuǎn)速以及轉(zhuǎn)向來實現(xiàn)對小車的控制。小車的控制任務(wù)可以分解成以下三個基本任務(wù):(1)控制小車直立：通過控制兩個電機的轉(zhuǎn)向保持小車的直立狀態(tài)。(2)控制小車車速：通過控制兩個電機的轉(zhuǎn)速實現(xiàn)車速控制。(3)控制小車轉(zhuǎn)向：通過控制兩個電機的轉(zhuǎn)速差實現(xiàn)轉(zhuǎn)向控制。以上三個任務(wù)都是通過控制

14、小車兩個車輪的驅(qū)動電機完成的。直流電機的控制最終取決于電機兩端輸入的電壓大小，將電機近似認(rèn)為處于線性狀態(tài)，因此上述三個基本任務(wù)可以等效成三種不同控制目標(biāo)的電壓，將這三種電壓進行疊加后，便可以得到最終所需的電壓，并將其施加在電機上以達到所追求的控制效果。在這三個任務(wù)中，保持小車平衡是關(guān)鍵，三個任務(wù)執(zhí)行的優(yōu)先級為：平衡控制速度才$制轉(zhuǎn)向控制。由于小車同時受到三種控制的影響，從平衡控制角度來看，其他兩個控制就成為了它的干擾。因此對小車速度、方向的控制應(yīng)該盡量保持平滑，以減少對平衡控制的干擾。上述三種控制各自獨立進行，它們各自假設(shè)其他兩個控制都已經(jīng)達到穩(wěn)定。比如控制小車加速和減速的時候，平衡控制一直在

15、起作用，它會自動改變小車的傾角，使小車實現(xiàn)加速和減速?？刂圃砩钪杏泻芏嘀绷⒖刂频睦?，例如一個正常人可以經(jīng)過簡單的練習(xí)，讓一根直木棒在水平的掌心中保持直立。這需要兩個條件：一是托著木棒的手掌可以移動；二是眼睛可以觀察到木棒的傾斜角和傾斜趨勢(角加速度)?？梢酝ㄟ^手學(xué)的移動抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。這兩個條件缺一不可，這就是控制中的負反饋機制圖保持木棍直立的反饋笆螞邈小車的直立也是通過負反饋實現(xiàn)的，但相對于上面的例子來說相對簡單，因為小車有兩個車輪著地，因此車體只會在一個平面內(nèi)發(fā)生傾斜?？刂栖囕嗈D(zhuǎn)動便可抵消傾斜的趨勢從而保持車體直立。車體向前傾斜,車體垂直，車車體向后傾斜

16、,圖通過車輪控制車體平衡數(shù)學(xué)模型輪自平衡小車在建模時可以將其簡化為倒立擺，便于進行受力分析并建立其數(shù)學(xué)模型，從而更好的設(shè)計控制系統(tǒng)圖單擺模型與倒立擺模型通過對單擺模型的觀察可知，當(dāng)物體離開平衡位置后會受到重力與線的合作用力，驅(qū)使重物回復(fù)至平衡位置，并進行周期運動，由于空氣阻力的存在，單擺最終會停在平衡位置?？梢缘贸?，單擺保持平衡的條件有兩點：(1)受到與位移方向相反的回復(fù)力作用；(2)受到和運動速度相反的阻尼力作用。如果沒有阻尼力的作用，單擺會在平衡位置左右晃動無法停止，如果阻尼力過小，單擺會在平衡位置震蕩，如果阻尼力過大，則單擺的回復(fù)時間將變長，因此存在一個臨界阻尼系數(shù)，使得單擺停止在平衡位

17、置所需時間最短。圖小車受力分析倒立擺在偏離平衡位置時，受到的合力與位移方向相同，因此倒立擺不能像單擺一樣穩(wěn)定在垂直位置，并且會加速偏離平衡位置直至倒下。為了讓倒立擺能像單擺一樣平衡在穩(wěn)定位置，只能通過增加額外受力使回復(fù)力與位移方向相反?？刂栖囕喿黾铀龠\動，以小車作為參考系，重心受到一個額外的慣性力，與車輪加速度大小相同，方向相反。因此倒立擺所受到的回復(fù)力為F=mgsin0-macosO(3-1)根據(jù)控制系統(tǒng)的特性，角9需要控制在很小的范圍內(nèi)，并且假設(shè)控制車輪加速度與角8成正比，比例系數(shù)為k1,因此上式可近似處理為F=mg0-mki0(3-2)此時，只要ki>g,回復(fù)力的方向便和位移方向相

18、反，此時小車可以恢復(fù)到平衡位置。為使小車能在平衡位置盡快的穩(wěn)定下來，還需要有阻尼力，阻尼力與角速度方向相反，大小成正比。式(3-2)可變?yōu)镕=mg0-mki0-m質(zhì)0'(3-3)式中，ki,k2均為比例系數(shù)，8為小車傾角，8'為角速度。只要滿足ki>g,k2>0,便可以將小車維持在直立狀態(tài)。k2是小車回到垂直位置的阻尼系數(shù)，選取合適的阻尼系數(shù)可以保證小車可以盡快穩(wěn)定在垂直位置。因此為了控制小車穩(wěn)定,需要精確的測量小車傾角9的大小和角速度9'的大小，并以此控制車輪的加速度。4系統(tǒng)硬件設(shè)計本控制系統(tǒng)主要由以下幾個模塊組成：STCi2C5A60S常片機最小系統(tǒng)、電

19、源管理模塊、車身姿態(tài)感應(yīng)模塊、電機驅(qū)動模塊、速度檢測模塊、藍牙模塊，各模塊關(guān)系圖如下所示：圖硬件設(shè)計總體框圖STC12C5A60S常片機介紹本控制系統(tǒng)采用STC12C5A60S2片機作為控制核心。該單片機是深圳宏晶科技有限公司的典型單片機產(chǎn)品，采用了增強型8051內(nèi)核，片內(nèi)集成了60KB程序Flash、1KB數(shù)據(jù)Flash（EEPROM1280字節(jié)RAM2個16位定時/計數(shù)器、44根I/O口線、兩個全雙工異步用行口（UART、高速同步通信端口（SPI）、8通道10位ADC2通道PWMW編程計數(shù)器陣列/捕獲/比較單元（PWM/PCA/CCUMAX810專用復(fù)位電路和硬件看門狗等資源。STC12C

20、5A60S想有在系統(tǒng)可編程（ISP）功能和在系統(tǒng)調(diào)試（ISD）功能，可以省去價格較高的專門編程器，開發(fā)環(huán)境的搭建非常容易，并且該單片機所有指令和標(biāo)準(zhǔn)的8051內(nèi)核完全兼容，具有良好的兼容性和很強的數(shù)據(jù)處理能力。STC12C5A60S系列單片機的內(nèi)部結(jié)構(gòu)框圖如下所示，該單片機中包含中央處理器（CPU、程序存儲器（Flash）、數(shù)據(jù)存儲器（SRAM定時/計數(shù)器、UART用口、串口2、I/O接口、高速A/D轉(zhuǎn)換、SPI接口、PCA看門狗及片內(nèi)R/C振蕩器和外部晶體振蕩電路等模塊。STC12C5A60S印片機幾乎包含了數(shù)據(jù)采集和控制中所需的所有單元模塊，可稱得上一個片上系統(tǒng)。圖STC12C5A60S城

21、歹U內(nèi)部結(jié)構(gòu)框圖圖單片機最小系統(tǒng)電源管理模塊電源管理模塊為整個硬件電路提供所需的電源，其穩(wěn)定性是整個硬件電路可靠運行的基礎(chǔ)。為了減少各個模塊之間的相互干擾，電源模塊由若干相互獨立的穩(wěn)壓電路模塊組成。整個系統(tǒng)由三節(jié)的18650鋰電池串聯(lián)供電。選擇LM2596s作為穩(wěn)壓芯片，整個系統(tǒng)的供電模塊如下圖所示。LM2596SFF關(guān)電壓調(diào)節(jié)器是降壓型電源管理單片集成電路，能夠輸出3A的驅(qū)動電流，同時具有很好的線性和負載調(diào)節(jié)特性。該器件內(nèi)部集成頻率補償和固定頻率發(fā)生器，開關(guān)頻率為150KHz與低頻開關(guān)調(diào)節(jié)器相比較，可以使用更小規(guī)格的濾波元件。該器件還有其他一些特點：在特定的輸入電壓和輸出載荷的條件下，輸出電

22、壓的誤差可以保證在土4%勺范圍內(nèi)，振蕩頻率誤差在土15%勺范圍內(nèi)；可以用僅80uA的待機電流；可實現(xiàn)外部斷電；具有自我保護電路。該器件完全可以滿足系統(tǒng)需要。穩(wěn)壓電路原理圖如下圖所示。圖穩(wěn)壓電路原理圖車身姿態(tài)感應(yīng)模塊在第三章原理分析中可知，為了控制小車穩(wěn)定，需要精確的測量小車傾角9的大小和角速度8'的大小，并以此控制車輪的加速度，以此消除小車的傾角。因此小車傾角以及傾角的角速度的測量成為了控制小車直立的關(guān)鍵。測量小車傾角和角速度可以通過加速度傳感器和陀螺儀實現(xiàn)。本控制系統(tǒng)的設(shè)計使用了整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時的軸間差的問題，減少了大量的封裝空間。

23、MPU6050對陀螺儀和加速度計分別用了三個16位的ADC將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量，和所有設(shè)備寄存器之間的通信采用400kHz的I2C接口。為了精確跟蹤快速和慢速的運動，傳感器的測量范圍都是用戶可控的，陀螺儀的可測范圍為±250,±500,±1000,±2000°/秒(dps),加速度計可測范圍為土2,±4,±8,±16g。量程越大，測量精度越低。MPU605位物及坐標(biāo)軸示意圖如下圖所示。圖MPU605似物圖與對應(yīng)坐標(biāo)軸示意圖加速度計MPU6050勺加速度計部分可以測量出各軸方向上的加速度，并經(jīng)過AD轉(zhuǎn)

24、換后可輸出數(shù)字信號。加速度檢測的基本原理如下圖所示。圖加速度檢測的基本原理通過微機械加工技術(shù)在硅片上加工形成了一個機械懸臂。它與相鄰的電極形成了兩個電容。由于加速度使得機械懸臂與兩個電極之間的距離發(fā)生了變化，從而改變了兩個電容的參數(shù)。通過集成的開關(guān)電容放大電路測量電容參數(shù)的變化，形成了與加速度成正比的電壓輸出。只需要測量出一個軸上的加速度，便可計算出小車的傾角。如下圖所示，設(shè)小車前進方向是小車直立時MPU6050勺Y軸正向。mgsin0圖小車受力分析當(dāng)小車前傾時，小車重心在Y軸上所受的力便是重力在Y軸上的分力，為mgsin8,因止匕MPU605/Y軸上所獲得的加速度為gsinB。似乎只需要獲得

25、加速度數(shù)據(jù)就可以獲得小車的傾角，但在實際小車的運行過程中，由于小車本身的運動所產(chǎn)生的加速度會產(chǎn)生很大的干擾信號疊加在上述測量信號上，使得輸出信號無法準(zhǔn)確的反映小車的傾角，如下圖所示。圖加速度計信號波動小車運動所產(chǎn)生的振動加速度使得輸出電壓在實際傾角電壓附近波動，可以使用低通濾波將其過濾，但也會使得信號無法實時反映小車的傾角變化，從而影響對小車的控制，使得小車無法保持平衡。陀螺儀陀螺儀可以用來測量物體的旋轉(zhuǎn)角速度，它利用了旋轉(zhuǎn)坐標(biāo)系中的物體會受到克里利奧力的原理，在器件中利用壓電陶瓷做成振動單元。當(dāng)器件旋轉(zhuǎn)時會改變振動頻率從而反映出物體旋轉(zhuǎn)的角速度。將MPU605酸裝在小車上時，可以測量出小車傾

26、斜的角速度，將角速度信號進行積分便可得到小車的傾角如下圖所示測圖小車的角速度和角度由于陀螺儀輸出的是車模的角速度，不會受到車體振動的影響，因此該信號中的噪聲很小，小車的傾角數(shù)據(jù)又是由所測角速度積分得來，進一步使信號變得平滑，從而使得角度信號更加穩(wěn)定。但是在實際情況中，測量所得的角速度信號存在微小的誤差，經(jīng)過積分運算之后，會形成累計誤差，并會隨著時間的延長逐步增加，最終導(dǎo)致電路飽和，無法形成正確的角度信號。如下圖所示。圖角度積分漂如上所述，加速度計對加速度很敏感，所獲得的數(shù)據(jù)會由于小車的運動產(chǎn)生高頻噪聲。而陀螺儀所測得的數(shù)據(jù)受到車體振動影響很少，但是隨著時間延長，容易存在積分漂移。因此可以使用互

27、補濾波，使得這兩個傳感器正好能彌補相互的缺點。簡而言之，互補濾波就是在短時間內(nèi)采用陀螺儀得到的角度作為最優(yōu)，定時對加速度轉(zhuǎn)化而來的角度進行取平均值處理來校正陀螺儀所得到的角度。具體實現(xiàn)方法如下圖所示。陀螺圖互補濾波原理框圖利用加速度計所獲得的角度信息9g與陀螺儀積分后的角度9進行比較，將比較的誤差信號經(jīng)過比例Tg放大之后與陀螺儀輸出的角速度信號疊加之后再進行積分。從上圖的框圖可以看出，對于加速度計給定的角度8g,經(jīng)過比例、積分環(huán)節(jié)之后產(chǎn)生的角度9必然最終等于9g。由于加速度計獲得的角度信息不會存在積累誤差，所以最終將輸出角度8中的積累誤差消除了。加速度計所產(chǎn)生的角度信息8g中會疊加很強的噪聲信

28、號。為了避免該噪聲信號對于角度8的影響，比例系數(shù)Tg應(yīng)該非常小。這樣，加速度的噪聲信號經(jīng)過比例、積分后，在輸出角度信息中就會變得很小。由于存在積分環(huán)節(jié)，所以無論Tg多小，最終輸出角度8必然與加速度計測量的角度8g相等，但是這個調(diào)節(jié)過程會隨著Tg的減小而延長。為了避免輸出角度8跟著8g過長，可以采取以下兩個方面的措施:g(1) 仔細調(diào)整陀螺儀的放大電路，使得它的零點偏置盡量接近于設(shè)定值，并且穩(wěn)定。(2) 在控制電路和程序運行的開始，盡量保持小車處于直立狀態(tài)，這樣一開始就使得輸出角度9和9g相等。此后，加速度計的輸出只是消除積分的偏移，輸出角度不會出現(xiàn)很大的偏差。電機驅(qū)動模塊本控制系統(tǒng)采用了TB6

29、612FN弊為直流電機驅(qū)動器件，該器件具有很高的集成度，同時能提供足夠的輸出能力，運行性能和能耗方面也具有優(yōu)勢，因此在集成化、小型化的電機控制系統(tǒng)中，它可以作為理想的電機驅(qū)動器件。TB6612FN0東芝半導(dǎo)體公司生產(chǎn)的一款直流電機驅(qū)動器件，它具有大電流MOSFET-橋結(jié)構(gòu)，雙通道電路卒U出，可同時驅(qū)動2個電機。該器件每通道輸出最高的連續(xù)驅(qū)動電流，啟動峰值電流達2A/(連續(xù)脈沖/單脈沖)；4種電機控制模式：正轉(zhuǎn)/反轉(zhuǎn)/制動/停止；PWMC持頻率高達100kHz;待機狀態(tài)；片內(nèi)低壓檢測電路與熱停機保護電路；工作溫度：-2085C；SSOP24b型貼片封裝。VM1PWMAAIN2AIN1YccSiB

30、YGWBIU1BIN2P甲MBVM3VM2AOIAOIP6ND1PGND14期A況502B02PGND2PGN02B01BOI圖TB6612FNGE片功能示意圖如上圖所示，TB6612FNG勺主要弓|腳功能：AIN1/AIN2、BIN1/BIN2、PWMA/PWMB為控制信號輸入端；AO1/AO2BO1/BO刻2路電機控制輸出端；STBY%正常工作/待機狀態(tài)控制引腳；VM15V）口VCC分別為電機驅(qū)動電壓輸入和邏輯電平輸入端。TB6612FNO基于MOSFETH橋集成電路，具效率高于晶體管H橋驅(qū)動器，并且外圍電路簡單，只需外接電源濾波電容就可以直接驅(qū)動電機，利于減小系統(tǒng)尺寸。對于PWMt號，它

31、支持高達100kHz的頻率。TB6612FN。本控制系統(tǒng)中的電路連接如下圖所示。圖TB6612FNG6路連接示意圖如上圖所示，AIN1/AIN2,BIN1/BIN2以及STBY1接直單片機的普通I/O口，STBY空制器件的工作狀態(tài)，AIN1/AIN2和BIN1/BIN2的輸入決定電機的正反轉(zhuǎn)。單片機的PCA模塊產(chǎn)生PWMB出作為電機轉(zhuǎn)速的才5制手段，連接至TB6612FNG的PWMA/PWMB路采用耐壓值25V的10uF電解電容和的電容進行電源濾波,使用功率MOSFETVMff口VCO供電源反接保護。TB6612FNG勺邏輯真值表如下圖所示表1TB6612FN哭輯真值表輸入輸出IN1IN2PW

32、MSTBYO1O2模式狀態(tài)HHH/LHLL制動LHHHLH反轉(zhuǎn)LHLHLL制動HLHHHL止轉(zhuǎn)HLLHLL制動LLHHOFF停止H/LH/LH/LLOFF待機速度檢測模塊本系統(tǒng)采用安華高公司的L15D11型光電編碼器作為車速檢測元件，其精度達到車輪每旋轉(zhuǎn)一周，旋轉(zhuǎn)編碼器產(chǎn)生448個脈沖，可滿足控制精度的要求。圖光電編碼器由于光電管器件直接輸出數(shù)字脈沖信號，因此可以直接將這些脈沖信號連接到單片機的計數(shù)器或外部中斷端口。編碼器每個光電管輸出兩個脈沖信號，它們波形相同，相位相差90°。如果電機正轉(zhuǎn)，第二個脈沖落后90°如果電機反轉(zhuǎn)，第二個脈沖超前90。可以通過這個關(guān)系判斷電機是否

33、正反轉(zhuǎn)，但是在實際電路中，只檢測一路脈沖信號，通過該信號得到電機轉(zhuǎn)速，電機的轉(zhuǎn)向是通過控制程序最后輸出值的正負來判斷的。5系統(tǒng)軟件設(shè)計設(shè)計好系統(tǒng)硬件之后，剩下的任務(wù)便是系統(tǒng)的軟件設(shè)計和調(diào)試。小車能否正常運行，需要通過軟件的編寫和調(diào)試來完成。軟件編寫與調(diào)試的主要任務(wù)包括：(1)建立軟件工程，配置單片機資源，初步編寫程序的主框架；(2)建立軟件編譯、下載、調(diào)試的環(huán)境；(3)編寫實現(xiàn)各個模塊，并測試各個模塊的功能；(4)逐步完成各個待定參數(shù)的整定；(5)綜合測試。開發(fā)STC12C5A60S2片機應(yīng)用程序可以使用Keil集成開發(fā)環(huán)境。軟件系統(tǒng)總體結(jié)構(gòu)根據(jù)控制系統(tǒng)的要求，整個軟件的功能模塊包括：(1)小

34、車姿態(tài)信息采集；(2)小車的直立控制、速度控制、方向控制;(3) 電機PWIMJ出;(4) 程序初始化、參數(shù)設(shè)定等；上述功能中，1-3需要在精確的時間周期內(nèi)完成，因此可以在一個周期定時中斷里完成。功能4的執(zhí)行不需要精確的時間周期，因此可以放在程序的主程序中完成。這兩類任務(wù)之間可以通過全局變量實現(xiàn)相互的通訊。關(guān)鍵程序流程圖如下圖所示,圖程序流程圖主程序是程序編譯的入口，是程序運行的開始。如上圖所示，單片機上電運行后，便運行初始化程序，該工作包括所應(yīng)用到模塊的初始化和應(yīng)用程序的初始化兩部分。前者是對單片機所應(yīng)用到的各個模塊進行初始化，這部分的代碼由STM片機燒錄工具STC-ISP自動生成。后者是應(yīng)

35、用程序的初始化，是對控制程序中應(yīng)用到的變量值進行初始化。初始化工作完成后，開啟總中斷，等待10ms定時器中斷，中斷產(chǎn)生后，進入中斷服務(wù)子程序，該程序負責(zé)調(diào)用控制系統(tǒng)關(guān)鍵部分的函數(shù)，是整個程序流程的關(guān)鍵所在。在該子程序中，MCUS過IIC與MPU605城行通訊，獲取小車的加速度值和角速度值；分別使用外部中斷與單片機計數(shù)器對左右輪編碼器產(chǎn)生的脈沖進行計數(shù)，從而獲取小車行駛速度相關(guān)的數(shù)據(jù)。對從傳感器所獲得的數(shù)據(jù)使用PID算法進行分析與計算后，將速度控制、角度控制、轉(zhuǎn)向控制的結(jié)果進行疊加，并將最后程序運算結(jié)果通過PWMI/O口的高低電平輸出至電機。實現(xiàn)對小車的閉環(huán)控制。程序在主循環(huán)中只運行藍牙控制函數(shù)

36、，實時通過審口獲取上位機通過藍牙發(fā)送的數(shù)據(jù)，并將此數(shù)據(jù)作為轉(zhuǎn)向控制的依據(jù)。本控制系統(tǒng)程序共使用了三個中斷：定時器溢出中斷、計數(shù)器溢出中斷、外部中斷。定時器溢出中斷負責(zé)采樣周期的設(shè)定，在此中斷子程序中完成控制程序的絕大部分功能功能。計數(shù)器溢出中斷負責(zé)右車輪編碼器產(chǎn)生脈沖的計數(shù)。外部中斷負責(zé)左車輪編碼器產(chǎn)生脈沖的計數(shù)。單片機的硬件資源配置時鐘頻率設(shè)置：該控制系統(tǒng)中，將晶振頻率設(shè)置為CPUT作頻率，為20ML定時/計數(shù)器設(shè)置定時/計數(shù)器的核心是一個加1計數(shù)器。加1計數(shù)器的脈沖有兩個來源，一個是外部脈沖源，另一個是系統(tǒng)的時鐘振蕩器。計數(shù)器對這兩個脈沖源之一進行輸入計數(shù)，每輸入一個脈沖，計數(shù)值加1。當(dāng)計數(shù)器全為1時，再輸入一個脈沖就使計數(shù)值回0,同時，從最高位溢出一個脈沖使特殊功能寄存器TCON勺TF0或TF1置1,作為計數(shù)器的溢出中斷標(biāo)志。如果定時/計數(shù)器工作于定時狀態(tài),則表示定時時間到；若工作于計數(shù)狀態(tài)，則表示計數(shù)回00所以，加1計數(shù)器的基本功能是對輸入脈沖進行計數(shù)，至于其工作于定時還是計數(shù)狀態(tài)，則取決于脈沖源。16位的加1計數(shù)器由兩個8位的特殊功能寄存器THX高8位）和TLX（低8位）組成（X=R1）。TMODTCO洞AUXFffl來確定定時/