基于ARM的兩輪自平衡車模型系統(tǒng)設(shè)計課程設(shè)計

1、中北大學(xué)課 程 設(shè) 計 說 明 書學(xué)生姓名:*杰學(xué) 號：*學(xué) 院:儀器與電子學(xué)院專 業(yè):*題 目:基于arm的兩輪自平衡車模型系統(tǒng)設(shè)計指導(dǎo)教師： 李錦明 職稱: 副教授 2015 年 1 月 30 日摘 要 近年來，兩輪自平衡車的研究與應(yīng)用獲得了迅猛發(fā)展。本文提出了一種兩輪自平衡小車的設(shè)計方案，采用陀螺儀l3g4200以及mems加速度傳感器mma7260構(gòu)成小車姿態(tài)檢測裝置，使用卡爾曼濾波完成陀螺儀數(shù)據(jù)與加速度計數(shù)據(jù)的數(shù)據(jù)融合。系統(tǒng)選用飛思卡爾32位單片機(jī)kinetis k60為控制核心，通過濾波算法實(shí)現(xiàn)車身控制，人機(jī)交互等。整個系統(tǒng)制作完成后，各個模塊能夠正常并協(xié)調(diào)工作，小車可以在無人干預(yù)

2、條件下實(shí)現(xiàn)自主平衡。同時在引入適量干擾情況下小車能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)。關(guān)鍵詞：兩輪自平衡 陀螺儀 姿態(tài)檢測 卡爾曼濾波 數(shù)據(jù)融合目 錄1 課程設(shè)計目的12 設(shè)計內(nèi)容和要求12.1 設(shè)計要求12.2 研究意義12.3 研究內(nèi)容23 設(shè)計方案及實(shí)現(xiàn)情況23.1 兩輪平衡車的平衡原理23.2 系統(tǒng)方案設(shè)計33.3 系統(tǒng)最終方案63.4 系統(tǒng)軟件設(shè)計93.5 電路調(diào)試164 課程設(shè)計總結(jié)18參 考 文 獻(xiàn)19附 錄20致 謝211 課程設(shè)計目的（1）掌握嵌入式系統(tǒng)的一般設(shè)計方法和設(shè)計流程；（2）學(xué)習(xí)嵌入式系統(tǒng)設(shè)計，掌握相關(guān)ide開發(fā)環(huán)境的使用方法；（3）掌握arm的應(yīng)用；（4）學(xué)習(xí)掌握嵌入式

3、系設(shè)計的全過程；2 設(shè)計內(nèi)容和要求2.1 設(shè)計要求（1）學(xué)習(xí)掌握基于arm cortex-m4內(nèi)核的kinetis k60系列單片機(jī)的工作原理及應(yīng)用；（2）學(xué)習(xí)掌握加速度計、陀螺儀的工作原理及應(yīng)用；（3）設(shè)計基于pid控制的兩輪自平衡車模型系統(tǒng)的工作原理圖及pcb版圖；2.2 研究意義近年來，隨著電子技術(shù)的發(fā)展與進(jìn)步，移動機(jī)器人的研究不斷深入，成為目前科學(xué)研究最活躍的領(lǐng)域之一，移動機(jī)器人的應(yīng)用范圍越來越廣泛，面臨的環(huán)境和任務(wù)也越來越復(fù)雜，這就要求移動機(jī)器人必須能夠適應(yīng)一些復(fù)雜的環(huán)境和任務(wù)。比如，戶外移動機(jī)器人需要在凹凸不平的地面上行走，有時環(huán)境中能夠允許機(jī)器人運(yùn)行的地方比較狹窄等。如何解決機(jī)器

4、人在這些環(huán)境中運(yùn)行的問題，逐漸成為研究者關(guān)心的問題1。兩輪自平衡機(jī)器人的概念正是在這樣一個背景下提出來的，這種機(jī)器人區(qū)別于其他移動機(jī)器人的最顯著的特點(diǎn)是：采用了兩輪共軸、各自獨(dú)立驅(qū)動的工作方式(這種驅(qū)動方式又被稱為差分式驅(qū)動方式)，車身的重心位于車輪軸的上方，通過輪子的前后移動來保持車身的平衡，并且還能夠在直立平衡的情況下行駛。由于特殊的結(jié)構(gòu)，其適應(yīng)地形變化能力強(qiáng)，運(yùn)動靈活，可以勝任一些復(fù)雜環(huán)境里的工作。兩輪自平衡機(jī)器人自面世以來，一直受到世界各國機(jī)器人愛好者和研究者的關(guān)注，這不僅是因?yàn)閮奢喿云胶鈾C(jī)器人具有獨(dú)特的外形和結(jié)構(gòu)，更重要的是因?yàn)槠渥陨淼谋举|(zhì)不穩(wěn)定性和非線性使它成為很好的驗(yàn)證控制理論和

5、控制方法的平臺，具有很高的研究價值。2.3 研究內(nèi)容本課題設(shè)計了一款兩輪自平衡小車，研究了車身姿態(tài)檢測中陀螺儀與加速度傳感器的互補(bǔ)特性，并根據(jù)其特性比較并設(shè)計濾波算法，包括卡爾曼濾波等常用濾波算法。pid控制算法的實(shí)現(xiàn)以及直流電機(jī)調(diào)速的研究。具體包括：(1) 機(jī)器人本體設(shè)計：包括機(jī)械，重心調(diào)整，電氣系統(tǒng)設(shè)計等，為進(jìn)一步研究提供良好的平臺；(2) 信號調(diào)理及控制部分電路設(shè)計：陀螺儀輸出信號需要經(jīng)過進(jìn)一步濾波放大，因此需要設(shè)計信號調(diào)理電路，同時控制核心需要構(gòu)建相關(guān)輸入輸出模塊及人際交互設(shè)備，因此需要對主控單元電路進(jìn)行設(shè)計。同時還需要設(shè)計直流電機(jī)驅(qū)動電路。(3) 基于卡爾曼濾波的數(shù)據(jù)融合：由于陀螺儀

6、測量的角速度只在短時間內(nèi)穩(wěn)定而加速度傳感器的自身白噪聲很嚴(yán)重，因此根據(jù)其互補(bǔ)特性設(shè)計卡爾曼濾波器以得到準(zhǔn)確穩(wěn)定的角度和角速度。(4) pid控制算法：包括兩路閉環(huán)控制。小車的傾角閉環(huán)控制以及直流電機(jī)的閉環(huán)速度控制。3 設(shè)計方案及實(shí)現(xiàn)情況3.1 兩輪平衡車的平衡原理控制小車平衡的直觀經(jīng)驗(yàn)來自人類日常生活經(jīng)驗(yàn)。如人類身體擁有豐富的感知器官，通過大腦調(diào)節(jié)便可以控制腰部及腿部肌肉保持人體的直立。而一般人通過簡單訓(xùn)練就可以讓一根直木棍在手指尖保持直立不倒。這需要兩個條件：一個是托著木棍的手指可以自由移動；另一個是人的眼睛可以觀察木棍的傾斜角度與傾斜趨勢(角速度)4。這兩個條件缺一不可，實(shí)際上這就是控制系

7、統(tǒng)中的負(fù)反饋機(jī)制，如圖1所示。圖1保持木棍直立的反饋控制系統(tǒng)自平衡車的控制也是通過負(fù)反饋來實(shí)現(xiàn)的，與在指尖保持木棍直立比較則相對簡單。由于小車只依靠兩個車輪著地，車輪與地面會發(fā)生相對滾動使得小車傾斜。而小車上裝載的姿態(tài)檢測系統(tǒng)能夠?qū)π≤嚨膬A斜狀況進(jìn)行實(shí)時檢測，通過控制器控制車輪轉(zhuǎn)動，抵消在這個維度上的傾斜力矩便可以保持小車平衡，如圖2所示。圖2通過車輪轉(zhuǎn)動保持小車平衡3.2 系統(tǒng)方案設(shè)計3.2.1 主控芯片方案 方案一：采用atmel公司的avr單片機(jī)avr單片機(jī)硬件結(jié)構(gòu)采取8位機(jī)與16位機(jī)的折中策略，即采用局部寄存器存堆(32個寄存器文件)和單體高速輸入/輸出的方案(即輸入捕獲寄存器、輸出比

8、 較匹配寄存器及相應(yīng)控制邏輯)。提高了指令執(zhí)行速度(1mips/mhz)，克服了瓶頸現(xiàn)象，增強(qiáng)了功能。其中的一款單片機(jī)型號為atmega128，有64個引腳，最高可達(dá)到16m主頻，iic，uart，spi接口都比較豐富，但價格高。方案二：采用宏晶科技有限公司的stc12c5a60s2增強(qiáng)型51單片機(jī)作為主控芯片。此芯片內(nèi)置adc(模數(shù)轉(zhuǎn)換)和iic總線接口，且內(nèi)部時鐘不分頻，可達(dá)到1mps。性價比低?？紤]到此系統(tǒng)的復(fù)雜度，需要與傳感器進(jìn)行iic通訊，輸出靈活可控制的pwm信號，以及進(jìn)行大量的數(shù)學(xué)運(yùn)算。從性能和價格上綜合考慮選擇方案一，即用kinetis k60作為本系統(tǒng)的主控芯片，由于外設(shè)比較

9、簡單，只需要iic和pwm通道，因此具體型號定位為k60n512vm100。方案三：采用freescale公司kinetis k60系列單片機(jī)作為主控芯片。kinetis k60系列單片機(jī),工作電壓1.71-3.6v,閃存的寫電壓為1.71-3.6v,采用arm cortex-m4內(nèi)核,其性能可達(dá)1.25dhrystonemips/mhz。該系列提供高達(dá)180mhz的性能和ieee1588以太網(wǎng)mac，用于工業(yè)自動化環(huán)境中的精確的、實(shí)時的時間控制。硬件加密支持多個算法，以最小的cpu負(fù)載提供快速、安全的數(shù)據(jù)傳輸和存儲。系統(tǒng)安全模塊包括安全密鑰存儲和硬件篡改檢測，提供用于電壓、頻率、溫度和外部傳

10、感（用于物理攻擊檢測）的傳感器2。3.2.2 姿態(tài)檢測傳感器方案本系統(tǒng)采用的加速度計是三軸加速度計mma7260。該加速度傳感器是一種低g值的傳感器，輸出信號很大，不需要再進(jìn)行放大。通過gsel1和gsel2腳選擇靈敏度，本系統(tǒng)設(shè)置其靈敏度為800mv/g4。電路如圖3所示。圖3 加速度計mma7260接口電路圖 本方案采用的陀螺儀傳感器為l3g4200。l3g4200d 是意法半導(dǎo)體(st)近日推出一款業(yè)界獨(dú)創(chuàng)、采用一個感應(yīng)結(jié)構(gòu)檢測3條正交軸向運(yùn)動的3軸數(shù)字陀螺儀。該3軸數(shù)字陀螺儀讓用戶可以設(shè)定全部量程，量程范圍從 250 dps2000 dps，低量程數(shù)值用于高精度慢速運(yùn)動測量，而高量程則

11、用于測量超快速的手勢和運(yùn)動4。這款器件提供一個16位數(shù)據(jù)輸出，以及可配置的低通和高通濾波器等嵌入式數(shù)字功能。與加速度傳感器的數(shù)字接口一致，也是通過用sda和scl與主控芯片的硬件iic接口進(jìn)行通訊，采用3.3v供電，其應(yīng)用電路如圖4所示。圖4 陀螺儀傳感器電路3.2.3 電機(jī)選擇方案 方案一：直流無刷電機(jī)。直流無刷電機(jī)具有直流有刷電機(jī)機(jī)械特性好、調(diào)速范圍寬等優(yōu)點(diǎn)，而且無刷電機(jī)沒有換向器和電刷，可靠性高，壽命長。但是無刷電機(jī)的驅(qū)動電路復(fù)雜，而且在本設(shè)計中小車為實(shí)驗(yàn)性質(zhì)，車身較小，市面上很難找到大小合適的直流無刷電機(jī)。 方案二：步進(jìn)電機(jī)。步進(jìn)電機(jī)的選擇角度正比于脈沖數(shù)，有較寬的調(diào)速范圍，可以采用開

12、環(huán)方式控制；步進(jìn)電機(jī)有較大的輸出轉(zhuǎn)矩；有優(yōu)秀的起制動性能；控制精度較高，誤差不會累積。但是步進(jìn)電機(jī)步距角固定，分辨率缺乏靈活性，而且步進(jìn)驅(qū)動時容易造成車體震蕩，不利于小車的穩(wěn)定。步進(jìn)電機(jī)雖然可以使用細(xì)分驅(qū)動方式克服上述缺點(diǎn)，但是細(xì)分驅(qū)動電路結(jié)構(gòu)復(fù)雜，而且功耗增大不適合用于電池供電的應(yīng)用上。方案三：直流電機(jī)。直流有刷電機(jī)具有機(jī)械特性硬，響應(yīng)速度快，調(diào)速范圍寬的特點(diǎn)，滿足兩輪自平衡小車對靈敏性、快速性等要求，雖然電機(jī)的電刷會是電機(jī)的壽命縮短，還會引發(fā)電磁干擾。但是由于本設(shè)計負(fù)載較輕，換向器和電刷的損耗較低。小車采用多層機(jī)械結(jié)構(gòu)，電機(jī)驅(qū)動電路與其他電路分離，有效降低電磁干擾5。綜上所述，本設(shè)計使用兩

13、個7.2v的直流有刷電機(jī)驅(qū)動兩輪自平衡小車模型。3.3 系統(tǒng)最終方案使用k60為主控芯片，通過iic接口讀取陀螺儀傳感器l3g4200和加速度傳感器mma7260的數(shù)據(jù)，再將兩者數(shù)據(jù)融合測出小車的姿態(tài)，最終通過pid輸出pwm電機(jī)控制信號，由電機(jī)驅(qū)動完成對電機(jī)的控制。此系統(tǒng)方框圖如圖5所示。 電機(jī)驅(qū)動 k60主控芯片 加速度傳感器mma7260 編碼器電路狀態(tài)顯示陀螺儀l3g4200圖5 系統(tǒng)方框圖3.3.1 主控電路 本設(shè)計的兩輪自平衡小車采用k60n512vm100單片機(jī)為主控芯片。kinetis k60是基于arm cortex-m4具有超強(qiáng)可擴(kuò)展性的低功耗、混合信號微控制器。主控及其外

14、圍電路如圖6所示圖6 主控芯片k60n512vm100 芯片電源類引腳，bga封裝22個，lqfp封裝27個，其中 bga 封裝 的芯片有五個引腳未使用（a10、b10、c10、m5 和 l5）。芯片使用多組電源引腳分別為內(nèi) 部電壓調(diào)節(jié)器、i/o 引腳驅(qū)動、a/d 轉(zhuǎn)換電路等電路供電，內(nèi)部電壓調(diào)節(jié)器為內(nèi)核和振蕩器 等供電。為了電源穩(wěn)定，mcu 內(nèi)部包含多組電源電路，同時給出多處電源引出腳，便于外 接濾波電容。為了電源平衡，mcu 提供了內(nèi)部相連的地的多處引出腳，供電路設(shè)計使用5。k60系列mcu具有ieee1588以太網(wǎng)、全速和高速usb2.0otg、硬件解碼能力和干預(yù)發(fā)現(xiàn)能力。芯片從帶有25

15、6kbflash的100引腳的lqfp封裝到1mbflash的256引腳的mapbga封裝，具有豐富的電路、通信、定時器和控制外圍電路。高容量的k60系列帶有一個可選擇的單精度浮點(diǎn)處理單元、nand控制單元和dram控制器。這是一款非常穩(wěn)定且有潛力的arm控制系列的微控制器3。3.3.2 電機(jī)驅(qū)動電路本設(shè)計中使用直流有刷電機(jī)作為兩輪自平衡車的驅(qū)動電機(jī)，電機(jī)采用h橋驅(qū)動方式，使用脈寬調(diào)制方式調(diào)節(jié)電機(jī)兩端電壓有效值，達(dá)到調(diào)速的目的。電機(jī)驅(qū)動電路采用4片bts7970搭載成兩個h橋來驅(qū)動平衡車的兩個電機(jī)，具有輸出功率大，穩(wěn)定性好，保護(hù)措施好等優(yōu)點(diǎn)8。一個h橋電機(jī)驅(qū)動電路如圖7所示。圖7 電機(jī)驅(qū)動電路

16、3.3.3 編碼器電路為了使用閉環(huán)控制，我們在模型上附加了編碼器。和其他元件相比，選用編碼器可以使電路更加完善，信號更加精確。編碼器功耗低，重量輕，抗沖擊抗震動，精度高，壽命長，非常實(shí)用。k60自身具有正交解碼功能，因此這里無需使用任何外圍計數(shù)輔助器件，只需要將接口連接到單片機(jī)上相應(yīng)的接口即可3，接口如圖8所示。 圖8 編碼器的接口部分電路3.3.4 供電電路可靠的電源方案是整個硬件電路穩(wěn)定可靠運(yùn)行的基礎(chǔ)。電源模塊由若干相互獨(dú)立的穩(wěn)壓電路模塊組成。這樣做可以減少各模塊之間的相互干擾，另外為了進(jìn)一步減小單片機(jī)的5v電源噪聲，可以單獨(dú)使用一個5v的穩(wěn)壓芯片，與其它接口電路分開7。整個系統(tǒng)需要3種電

17、源： (1) 7.2v電源，為驅(qū)動電機(jī)供電。(2) 5v電源，為編碼器及相關(guān)外設(shè)供電。(3) 3.3v電源，為單片機(jī)、陀螺儀及加速度計供電。整個系統(tǒng)電源來源為7.2v鎳氫電池，5v電源由lm2940提供。lm2940是一種線性低壓差三端穩(wěn)壓器件，其輸出紋波較小，適合單片機(jī)供電。3.3v電源采用lm1117。電機(jī)供電直接采用電池供電如下圖所示。 圖9 5v電源模塊 圖10 3.3v電源模塊3.4 系統(tǒng)軟件設(shè)計3.4.1 編譯環(huán)境概述嵌入式軟件開發(fā)有別于桌面軟件開發(fā)的一個顯著的特點(diǎn)，是它一般需要一個交叉編譯和調(diào)試環(huán)境，即編輯和編譯軟件在通常的pc機(jī)上進(jìn)行，而編譯好的軟件需要通過寫入工具下載到目標(biāo)機(jī)

18、上執(zhí)行，如mk60n512vmd100的目標(biāo)機(jī)上。在開發(fā)過程中我們使用iarsystems公司的iarembeddedworkbenchforarm6.30集成開發(fā)環(huán)境（簡稱iar環(huán)境）j-link-arm-v8仿真器。embeddedworkbenchforarm6.30是iarsystems公司為arm微處理器開發(fā)的一個集成開發(fā)環(huán)境。比較其他的arm開發(fā)環(huán)境，iar具有入門容易、使用方便和代碼緊湊等特點(diǎn)2。iar中包含一個全軟件的模擬程序(simulator)。用戶不需要任何硬件支持就可以模擬各種arm內(nèi)核、外部設(shè)備甚至中斷的軟件運(yùn)行環(huán)境。從中可以了解和評估iar的功能和使用方法。調(diào)試界面

19、如圖所示：圖11 iar界面示意圖3.4.2 系統(tǒng)軟件總體流程系統(tǒng)軟件總體流程如圖12所示。圖12 系統(tǒng)軟件總流程圖系統(tǒng)上電復(fù)位后便開始初始化各個功能模塊，并啟動了1ms定時，每1ms進(jìn)行一次姿態(tài)估算和pid控制，即1s內(nèi)系統(tǒng)進(jìn)行了1000次姿態(tài)調(diào)整。同時為了前期調(diào)試已經(jīng)查看數(shù)據(jù)，使用了主控的串口將程序中產(chǎn)生的數(shù)據(jù)如估算出的最終角度等，上傳到電腦，以觀察數(shù)據(jù)的特性，上傳周期為16ms。3.4.3 系統(tǒng)初始化系統(tǒng)初始化完成kinetis k60單片機(jī)的初始化設(shè)置，初始化程序流程圖如圖13所示。圖13 系統(tǒng)初始化流程圖3.4.4 pid控制器當(dāng)今的自動控制技術(shù)都是基于反饋的概念。反饋理論的要素包括

20、三個部分：測量、比較和執(zhí)行。測量系統(tǒng)需要控制的變量，與期望值相比較，用這個誤差糾正調(diào)節(jié)控制系統(tǒng)的響應(yīng)。在工程實(shí)際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱pid控制，又稱pid調(diào)節(jié)。pid控制器問世至今已有近70年歷史，以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一6。pid控制器由比例單元（p）、積分單元（i）和微分單元（d）組成。其輸入e (t）與輸出u (t）的關(guān)系為： (式1)其中為比例系數(shù)；為積分時間常數(shù)；為微分時間常數(shù)。pid控制器具有原理簡單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對模型依賴少等特點(diǎn)，因此使用pid控制器實(shí)現(xiàn)兩輪自平衡車的控制是完

21、全可行的。 由小車靜止時其運(yùn)動方程可得到系統(tǒng)輸入輸出傳遞函數(shù)： (式2) 此時系統(tǒng)具有兩個極點(diǎn)：。其中一個極點(diǎn)位于s平面的右半平面。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車在靜止?fàn)顟B(tài)不能保持平衡9。由小車受力分析可知小車平衡的條件是提供額外的回復(fù)力及阻尼，其來源為車輪與地面的摩擦力。由式2可知，車輪提供的加速度的大小是根據(jù)角度及角速度的反饋得出，因此需要在控制系統(tǒng)中引入角度及角速度構(gòu)成比例(p)微分(d)反饋環(huán)節(jié)，如圖14所示。圖14 加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖加入比例微分反饋后的系統(tǒng)傳遞函數(shù)為： (式3)此時，系統(tǒng)的兩個極點(diǎn)為。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，系統(tǒng)穩(wěn)定需要兩個極點(diǎn)都位于

22、s平面的左半平面。要滿足這一點(diǎn)，需要g, 0。由此可得出結(jié)論，但g, 0時，小車可以保持平衡，這也與上文中小車受力分析的結(jié)果相符。在反饋環(huán)節(jié)中，與角度成比例的控制量稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此上面系數(shù)，分別稱為比例和微分控制參數(shù)。其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車振蕩?？刂葡到y(tǒng)的輸出量為電機(jī)控制量，因而小車平衡控制的pid控制器的輸出方程可寫為：out_motor=kp*angle+kd*angle_dot (式4)式4中，out_motor為pid控制輸出量，angle為反饋傾角值，angle_dot為反饋角速度值，kp和kd分別為

23、比例系數(shù)及微分系數(shù)。3.4.5濾波器單獨(dú)使用加速度計或者陀螺儀，都無法提供一個有效而可靠的信息來保證車體的平衡。采用一種簡易互補(bǔ)濾波方法來融合陀螺儀和加速度計的輸出信號，補(bǔ)償陀螺儀的漂移誤差和加速度計的動態(tài)誤差，便可以得到一個最優(yōu)的傾角近似值。該濾波器的實(shí)現(xiàn)算法如圖15所示。 圖15 互補(bǔ)濾波器算法流程圖在使用互補(bǔ)濾波器前，先對加速度的值進(jìn)行低通濾波。加速度的值里包含了一些短時性快速變化的信號，這些信號對計算角度有較大的干擾，因此應(yīng)使用低通濾波器將其濾除掉，濾除后的加速度值具有長時性緩慢變化的平滑效果。由圖可看出該濾波器的輸入有兩個數(shù)據(jù)，一是加速度計測得的角度，二是陀螺儀測得的角速度，兩者經(jīng)過

24、一定的融合運(yùn)算后得出一個穩(wěn)定可靠的傾角。以下將介紹數(shù)據(jù)的融合過程。首先建立一個以主控板為基準(zhǔn)的空間坐標(biāo)系，如圖16所示。 圖16 傳感器直角坐標(biāo)系r代表電路板的加速度矢量，rn(n可以是x，y，z)代表r在各個軸的分量，ann(a代表角度angle，nn可以是xz，yz，如axz)代表r與各個平面的夾角。因此，從加速度計測出來的rxacc，ryacc，rzacc(acc代表accelerometer)便可合成矢量racc，即racc=raccx，raccy，raccz。由racc的分量即可計算出axz，ayz，但如前文討論的，此時的axz，ayz并不穩(wěn)定可靠。假設(shè)最后計算出的穩(wěn)定可靠的矢量(實(shí)

25、際上是單純的重力加速度矢量)為rest = rxest,ryest,rzest(est代表：estimate)。程序中，第一次計算rest直接取值于加速度計的racc，即： (式5)此后的rest(如rest(1),rest(2),rest(3)rest(n)則由陀螺儀參與調(diào)整。在計算rest(n)時，可以通過rxest(n-1) 和rzest(n-1)計算出axz(n-1),即： (式6) 接著由axz(n-1)和rateaxz(n)可計算出axz(n)，即： (式7)其中rateaxz(n)為繞y軸的陀螺儀角速度，t為計算axz(n-1)到計算axz(n)的時間差值。同樣道理可得： (式8

26、)再由以下公式可得： (式9)同理得： (式10) (式11)到此，計算出了兩個單位為1的向量：racc(n)，rgyro(n)，通過一個加權(quán)平均公式便可得到rest(n)，即： (式12)令w2/w1 = wgyro，可得 (式13)即： (式14) (式15) (式16)而小車的傾角為： (式17)3.5 電路調(diào)試3.5.1 加速度計測試結(jié)果 理論上只需要加速度就可以獲得車模的傾角，再對此信號進(jìn)行微分便可以獲得傾角速度。但在實(shí)際車模運(yùn)行過程中，由于車模本身的擺動所產(chǎn)生的加速度會產(chǎn)生很大的干擾信號，它疊加在上述測量信號上使得輸出信號無法準(zhǔn)確反映車模的傾角，如圖17所示。 圖17 車運(yùn)動引起的

27、加速度信號波動下圖是實(shí)際測量安裝在車模上mma7260的z軸信號。車模傾角在兩個角度位置過渡，看到除了角度變化信號之外，還存在由于運(yùn)動引起的電壓波動，這個電壓波動隨著車模運(yùn)動速度增加會變得很大。 圖18 實(shí)際測量加速度計z軸信號3.5.2 陀螺儀測試結(jié)果 在車模上安裝陀螺儀，可以測量車模傾斜角速度，將角速度信號進(jìn)行積分便可以得到車模的傾角，如圖19所示。由于陀螺儀輸出的是車模的角速度，不會受到車體運(yùn)動的影響，因此該信號中噪聲很小。車模的角度又是通過對角速度積分而得，這可進(jìn)一步平滑信號，從而使得角度信號更加穩(wěn)定。因此車?？刂扑枰慕嵌群徒撬俣瓤梢允褂猛勇輧x所得到的信號。 圖19 角速度積分得到

28、角度 由于從陀螺儀角速度獲得角度信息，需要經(jīng)過積分運(yùn)算。如果角速度信號存在微小的偏差和漂移，經(jīng)過積分運(yùn)算之后，變化形成積累誤差。這個誤差會隨著時間延長逐步增加，最終導(dǎo)致電路飽和，無法形成正確的角度信號，如圖20所示。 圖20角速度積分漂移現(xiàn)象 消除上述誤差通過上面的加速度傳感器獲得的角度信息對此進(jìn)行校正。通過對比積分所得到的角度與重力加速度所得到的角度，使用它們之間的偏差改變陀螺儀的輸出，從而積分的角度逐步跟蹤到加速度傳感器所得到的角度，如圖21所示。 圖21通過重力加速度來矯正陀螺儀的角度漂移4 課程設(shè)計總結(jié)本設(shè)計主要研究兩輪自平衡小車的設(shè)計與實(shí)現(xiàn)。通過相應(yīng)硬件與軟件的設(shè)計，實(shí)現(xiàn)了兩輪自平衡

29、小車的動態(tài)平衡與運(yùn)動控制。系統(tǒng)硬件結(jié)構(gòu)以飛思卡爾公司32位單片機(jī)k60為控制核心，采用壓電陀螺儀l3g4200及mems加速度計mma7260構(gòu)成了慣性姿態(tài)檢測系統(tǒng)，通過h橋電機(jī)驅(qū)動及旋轉(zhuǎn)編碼器實(shí)現(xiàn)了直流電機(jī)的閉環(huán)調(diào)速，最終實(shí)現(xiàn)了兩輪自平衡車的姿態(tài)檢測與平衡控制。系統(tǒng)軟件設(shè)計上比較了各類濾波器優(yōu)缺點(diǎn)，結(jié)合本系統(tǒng)硬件構(gòu)架設(shè)計了以卡爾曼濾波器為核心的數(shù)據(jù)融合算法。通過卡爾曼濾波器將陀螺儀與加速度計的輸出融合為準(zhǔn)確的傾角與角速度輸出，為系統(tǒng)的控制提供了有力保障。本設(shè)計的控制策略采用pid控制算法。通過對小車的運(yùn)動建模，構(gòu)建了小車運(yùn)動控制的pd控制算法，并對pid參數(shù)進(jìn)行了整定。本設(shè)計最終實(shí)現(xiàn)了兩輪自平衡小車的平衡控制及運(yùn)動控制。由于時間及個人能力原因，本設(shè)計完成的兩輪自平衡小車只是一種簡單的模型。本設(shè)計還有以下方