兩輪自平衡小車(chē)系統(tǒng)設(shè)計(jì)

兩輪自平衡小車(chē)是一種非線(xiàn)性、多變量、強(qiáng)耦合、參數(shù)不確定的復(fù)雜系統(tǒng)，他體積小、結(jié)構(gòu)簡(jiǎn)單、運(yùn)動(dòng)靈活，適合在狹小空間內(nèi)工作。兩輪自平衡小車(chē)系統(tǒng)是一種兩輪左右并行布置的系統(tǒng)。像傳統(tǒng)的倒立一樣，其工作原理是依靠?jī)A角傳感器所檢測(cè)的位姿和狀態(tài)變化率結(jié)合控制算法來(lái)維持自身平衡。本設(shè)計(jì)通過(guò)對(duì)倒立擺進(jìn)行動(dòng)力學(xué)建模，類(lèi)比得到小車(chē)平衡的條件。從加速度計(jì)和陀螺儀傳感器得出的角度。運(yùn)用卡爾曼濾波優(yōu)化，補(bǔ)償陀螺儀的漂移誤差和加速度計(jì)的動(dòng)態(tài)誤差，得到更優(yōu)的傾角近似值。通過(guò)光電編碼器分別得到車(chē)子的線(xiàn)速度和轉(zhuǎn)向角速度，對(duì)速度進(jìn)行PI控制。根據(jù)PID控制調(diào)節(jié)參數(shù)，實(shí)現(xiàn)兩輪直立行走。通過(guò)調(diào)節(jié)左右兩輪的差速實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向。實(shí)驗(yàn)前期，對(duì)各個(gè)模塊進(jìn)行了解，例如電機(jī)、STM32、陀螺儀、藍(lán)牙模塊、避障模塊等等，對(duì)這些進(jìn)行學(xué)習(xí)程序并編寫(xiě)。接著研究了小車(chē)的自平衡、手機(jī)藍(lán)牙遙控、超聲波避障。其主要工作內(nèi)容包括：小車(chē)機(jī)械本體設(shè)計(jì)、硬件設(shè)計(jì)、電氣原理圖、小車(chē)的直立、速度、轉(zhuǎn)向平衡和小車(chē)的手機(jī)藍(lán)牙控制試驗(yàn)、超聲波避障實(shí)驗(yàn)。后期進(jìn)行對(duì)小車(chē)的機(jī)械結(jié)構(gòu)進(jìn)行完善，并進(jìn)行三維圖二維圖的繪制。制作完成小車(chē)樣品后，小車(chē)實(shí)現(xiàn)了前進(jìn)、后退、和左右轉(zhuǎn)向的基本動(dòng)作。此外小車(chē)能在正常條件下達(dá)到自主平衡狀態(tài)。并且在適量干擾下，小車(chē)能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)。小車(chē)能夠?qū)崿F(xiàn)手機(jī)藍(lán)牙控制前后左右移動(dòng)的功能，以及能夠智能實(shí)現(xiàn)避障功能。關(guān)鍵詞：自平衡 藍(lán)牙遙控 STM32 避障 前言移動(dòng)機(jī)器人是機(jī)器人學(xué)的一個(gè)重要分支，對(duì)于移動(dòng)機(jī)器人的研究，包括輪式、腿式、履帶式以及水下式機(jī)器人等，可以追溯到20世紀(jì)60年代。移動(dòng)機(jī)器人得到快速發(fā)展有兩方面原因：一是其應(yīng)用范圍越來(lái)越廣泛；二是相關(guān)領(lǐng)域如計(jì)算、傳感、控制及執(zhí)行等技術(shù)的快速發(fā)展。移動(dòng)機(jī)器人尚有不少技術(shù)問(wèn)題有待解決，因此近幾年對(duì)移動(dòng)機(jī)器人的研究相當(dāng)活躍。近年來(lái)，隨著移動(dòng)機(jī)器人研究不斷深入、應(yīng)用領(lǐng)域更加廣泛，所面臨的環(huán)境和任務(wù)也越來(lái)越復(fù)雜。機(jī)器人經(jīng)常會(huì)遇到一些比較狹窄，而且有很多大轉(zhuǎn)角的工作場(chǎng)合，如何在這樣比較復(fù)雜的環(huán)境中靈活快捷的執(zhí)行任務(wù)，成為人們頗為關(guān)心的一個(gè)問(wèn)題。雙輪自平衡機(jī)器人概念就是在這樣的背景下提出來(lái)的。兩輪自平衡小車(chē)是一個(gè)高度不穩(wěn)定兩輪機(jī)器人，是一種多變量、非線(xiàn)性、強(qiáng)耦合的系統(tǒng)，是檢驗(yàn)各種控制方法的典型裝置。同時(shí)由于它具有體積小、運(yùn)動(dòng)靈活、零轉(zhuǎn)彎半徑等特點(diǎn)，將會(huì)在軍用和民用領(lǐng)域有著廣泛的應(yīng)用前景。因?yàn)樗扔欣碚撗芯恳饬x又有實(shí)用價(jià)值，所以?xún)奢喿云胶庑≤?chē)的研究在最近十年引起了大量機(jī)器人技術(shù)實(shí)驗(yàn)室的廣泛關(guān)注。II目錄摘要I前言II第一章 設(shè)計(jì)綜述11.1 自平衡車(chē)發(fā)展現(xiàn)狀及市場(chǎng)分析11.1.1 市場(chǎng)調(diào)查分析11.1.2 市場(chǎng)背景11.1.3 目標(biāo)市場(chǎng)的確定21.2 智能機(jī)器人系統(tǒng)設(shè)計(jì)任務(wù)概述21.3 課程設(shè)計(jì)主要內(nèi)容2第二章 方案設(shè)計(jì)22.1 機(jī)械結(jié)構(gòu)方案設(shè)計(jì)22.2驅(qū)動(dòng)方案選擇32.3 傳感器的選擇32.3.1主芯片的選擇42.3.2電機(jī)驅(qū)動(dòng)的選擇42.3.3 陀螺儀芯片的選擇52.3.4 降壓模塊的選擇52.3.5超聲波模塊的選擇62.3.6藍(lán)牙模塊的選擇62.4 結(jié)構(gòu)的合理性7第三章 機(jī)械系統(tǒng)設(shè)計(jì)73.1 機(jī)械系統(tǒng)總體方案73.2關(guān)鍵零部件結(jié)構(gòu)尺寸設(shè)計(jì)73.3 驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)軌跡規(guī)劃83.3.1運(yùn)動(dòng)學(xué)方程83.3.2速度分析93.3.3 運(yùn)動(dòng)學(xué)反解10第四章 控制系統(tǒng)設(shè)計(jì)104.1 控制系統(tǒng)總體方案104.2 選型114.3 STM32使用的資源114.4 電機(jī)驅(qū)動(dòng)的電路設(shè)計(jì)114.5 傳感檢測(cè)外圍電路設(shè)計(jì)124.6 電源供電電路設(shè)計(jì)134.7 總電路圖14第五章 軟件系統(tǒng)設(shè)計(jì)145.1 軟件系統(tǒng)總體方案145.2位姿控制方案145.2.1 PID控制145.2.2 應(yīng)用現(xiàn)狀155.2.3 PID調(diào)節(jié)規(guī)律155.3 位姿控制流程圖175.4 速度控制方案185.5 手機(jī)藍(lán)牙遙控流程圖195.6 超聲波避障流程圖20第六章 裝配與調(diào)試216.1 機(jī)器人系統(tǒng)整體裝配216.2系統(tǒng)調(diào)試流程與狀況216.3 設(shè)計(jì)創(chuàng)新點(diǎn)216.4 系統(tǒng)缺陷與改進(jìn)21第七章 市場(chǎng)應(yīng)用前景分析227.1 市場(chǎng)前景227.2 產(chǎn)品優(yōu)勢(shì)22項(xiàng)目心得22參考文獻(xiàn)23附錄一 成本分析24附錄二 程序源代碼25 第一章 設(shè)計(jì)綜述1.1 自平衡車(chē)發(fā)展現(xiàn)狀及市場(chǎng)分析1.1.1 市場(chǎng)調(diào)查分析我國(guó)人口眾多，交通擁堵問(wèn)題嚴(yán)重，電動(dòng)平衡車(chē)輕便、靈活的特點(diǎn)非常好的滿(mǎn)足了年輕學(xué)生族、上班族對(duì)于解決短途出行的需求，不僅節(jié)省了金錢(qián)成本，也有效的縮短了上下班在路途花費(fèi)的時(shí)間和體力，而且兩輪自平衡電動(dòng)車(chē)較廣泛的被用來(lái)商用和警用，起到了非常不錯(cuò)的效果，因此電動(dòng)平衡車(chē)市場(chǎng)需求很大。 圖1.1 市場(chǎng)平衡車(chē)產(chǎn)品1.1.2 市場(chǎng)背景隨著智能硬件技術(shù)的快速發(fā)展，電動(dòng)平衡車(chē)產(chǎn)業(yè)如雨后春筍，行業(yè)內(nèi)生產(chǎn)商從數(shù)十家增長(zhǎng)至萬(wàn)余家，國(guó)內(nèi)平衡車(chē)產(chǎn)能從2011年的3.2萬(wàn)臺(tái)激增至2015年近1600萬(wàn)臺(tái)。電動(dòng)平衡車(chē)在2015年經(jīng)歷了爆發(fā)式的增長(zhǎng)，其高產(chǎn)量的背后也反映出了市場(chǎng)對(duì)產(chǎn)品極大的需求，消費(fèi)者對(duì)這種出行方式的認(rèn)可度和接受能力在不斷提高。目前，國(guó)內(nèi)的平衡車(chē)主要是經(jīng)出口銷(xiāo)售到海外市場(chǎng)，2015年的高峰期，電動(dòng)平衡車(chē)的月出口量就超過(guò)200萬(wàn)臺(tái)。同時(shí)，國(guó)內(nèi)消費(fèi)市場(chǎng)也在崛起，越來(lái)越多的消費(fèi)者開(kāi)始嘗試使用這一新穎的短途出行工具。但由于電動(dòng)平衡車(chē)的安全性問(wèn)題，2016年，中國(guó)電動(dòng)平衡車(chē)行業(yè)不僅未得秋收反而提早入冬，訂單銳減。從在美國(guó)為首安全事故至亞馬遜網(wǎng)站下架，從三次美國(guó)ITC“337調(diào)查”至京滬的禁行政策，多方面接踵襲擊電動(dòng)平衡車(chē)行業(yè)。而且，我國(guó)兩輪平衡行業(yè)發(fā)展滯后，以次充好現(xiàn)象嚴(yán)重，成本無(wú)法控制，市場(chǎng)平衡車(chē)價(jià)格高居不下，因此，平衡車(chē)市場(chǎng)需要安全性高讓人放心的產(chǎn)品出現(xiàn)，具有極大的拓展空間。 1.1.3 目標(biāo)市場(chǎng)的確定兩輪自平衡電動(dòng)車(chē)是一種新型代步工具，是依靠重力操控、電力驅(qū)動(dòng)的智能交通工具，在安保巡邏、廣告宣傳、短途代步等領(lǐng)域具有廣泛的用途。因此目標(biāo)市場(chǎng)主要集中在政府機(jī)構(gòu)、保安公司和廣告商，以及學(xué)生族和上班族等。1.2 智能機(jī)器人系統(tǒng)設(shè)計(jì)任務(wù)概述a. 具備自平衡功能，能夠?qū)崿F(xiàn)直立、速度、轉(zhuǎn)向平衡等基本動(dòng)作b. 具備完整檢測(cè)系統(tǒng)，機(jī)器人能夠充分檢測(cè)外部接觸環(huán)境（超聲波避障）；c. 具備足夠負(fù)載能力，可攜帶移動(dòng)電源；d. 具備無(wú)線(xiàn)通訊功能，能夠?qū)崿F(xiàn)小車(chē)的無(wú)線(xiàn)遠(yuǎn)程控制（手機(jī)藍(lán)牙控制）。1.3 課程設(shè)計(jì)主要內(nèi)容（1）資料分析：查閱相關(guān)文獻(xiàn)資料，對(duì)資料進(jìn)行分析總結(jié)。（2）機(jī)器人總體設(shè)計(jì)：根據(jù)機(jī)器人的具體任務(wù)要求，初步擬定小車(chē)的技術(shù)參數(shù)、電路部分設(shè)計(jì)、機(jī)械結(jié)構(gòu)設(shè)計(jì)、控制方案選擇等，制造出樣品。（3）機(jī)器人機(jī)械結(jié)構(gòu)設(shè)計(jì)：將機(jī)器人分解為車(chē)身結(jié)構(gòu)、運(yùn)動(dòng)機(jī)構(gòu)等若干部分，繪制機(jī)器人總裝圖和關(guān)鍵零部件圖。（4）各個(gè)芯片的選擇與調(diào)試：根據(jù)任務(wù)要求，選擇各個(gè)模塊進(jìn)行相關(guān)信息檢測(cè)、處理，并完成信息的正確傳輸。（5）運(yùn)動(dòng)控制方案設(shè)計(jì)：基于傳感信息，采用STM32單片機(jī)，進(jìn)行小車(chē)控制系統(tǒng)硬件和軟件程序的設(shè)計(jì)和系統(tǒng)調(diào)試。（6）編制課程設(shè)計(jì)說(shuō)明書(shū)。 第二章 方案設(shè)計(jì)2.1 機(jī)械結(jié)構(gòu)方案設(shè)計(jì) 本文是對(duì)本質(zhì)不穩(wěn)定兩輪小車(chē)自平衡控制問(wèn)題的研究。實(shí)現(xiàn)小車(chē)直立、速度、轉(zhuǎn)向平衡等基本動(dòng)作以及藍(lán)牙遙控、自動(dòng)避障的功能。整個(gè)小車(chē)由機(jī)械部分和控制系統(tǒng)兩大部分組成。機(jī)械部分包括3個(gè)亞克力板、兩個(gè)驅(qū)動(dòng)輪。它們是電池以及控制系統(tǒng)的載體。機(jī)器人的底板大小為15CM8CM，機(jī)器人有兩個(gè)電機(jī)對(duì)稱(chēng)聯(lián)接于機(jī)器人底板上，底板上方則時(shí)放置超聲波避障模塊、電池等，中間亞克力板則放置控制模塊、陀螺儀芯片等。2.2驅(qū)動(dòng)方案選擇 利用PWM原理來(lái)控制直流減速電機(jī)的轉(zhuǎn)動(dòng) 對(duì)于電機(jī)的轉(zhuǎn)速調(diào)整，我們是采用脈寬調(diào)制（PWM）辦法，控制電機(jī)的時(shí)候，電源并非連續(xù)地向電機(jī)供電，而是在一個(gè)特定的頻率下以方波脈沖的形式提供電能。不同占空比的方波信號(hào)能對(duì)電機(jī)起到調(diào)速作用，這是因?yàn)殡姍C(jī)實(shí)際上是一個(gè)大電感，它有阻礙輸入電流和電壓突變的能力，因此脈沖輸入信號(hào)被平均分配到作用時(shí)間上，這樣，改變?cè)谑鼓芏松陷斎敕讲ǖ恼伎毡染湍芨淖兗釉陔姍C(jī)兩端的電壓大小，從而改變了轉(zhuǎn)速。PWM不管是高電平還是低電平時(shí)電機(jī)都是轉(zhuǎn)動(dòng)的，電機(jī)的轉(zhuǎn)速取決于平均電壓。脈沖寬度越大即占空比越大，提供給電機(jī)的平均電壓越大，電機(jī)轉(zhuǎn)速就高。 圖2.1 12V直流減速電機(jī)2.3 傳感器的選擇 2.3.1 主芯片的選擇 STM32單片機(jī)作為主控芯片。此芯片是以ARM的Cortex-M系列為內(nèi)核的單片機(jī)，相對(duì)其他單片機(jī)，外設(shè)豐富，主頻高，價(jià)格便宜，有專(zhuān)門(mén)的軟件庫(kù)，操作簡(jiǎn)單，調(diào)試方便，低功耗。強(qiáng)型系列時(shí)鐘頻率達(dá)到72MHz，是同類(lèi)產(chǎn)品中性能最高的產(chǎn)品；基本型時(shí)鐘頻率為36MHz，以16位產(chǎn)品的價(jià)格得到比16位產(chǎn)品大幅提升的性能，是16位產(chǎn)品用戶(hù)的最佳選擇。 圖2.2 STM32芯片 2.3.2電機(jī)驅(qū)動(dòng)的選擇 TB6612FNG是東芝半導(dǎo)體公司生產(chǎn)的一款直流電機(jī)驅(qū)動(dòng)器件，它具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)2個(gè)電機(jī)。 TB6612FNG每通道輸出最高1.2 A的連續(xù)驅(qū)動(dòng)電流，啟動(dòng)峰值電流達(dá)2A/3.2 A(連續(xù)脈沖/單脈沖);4種電機(jī)控制模式：正轉(zhuǎn)/反轉(zhuǎn)/制動(dòng)/停止;PWM支持頻率高達(dá)100 kHz;待機(jī)狀態(tài);片內(nèi)低壓檢測(cè)電路與熱停機(jī)保護(hù)電路;工作溫度：-2085;SSOP24小型貼片封裝。 圖2.3 驅(qū)動(dòng)模塊TB6612FNG 2.3.3 陀螺儀芯片的選擇 MPU-6050集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP，可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過(guò)其I2C或SPI接口輸出一個(gè)9軸的信號(hào)。MPU-6050也可以通過(guò)其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-6050對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶(hù)可控的，陀螺儀可測(cè)范圍為250，500，1000，2000/秒（dps），加速度計(jì)可測(cè)范圍為2，4，8，16g。另外，片上還內(nèi)嵌了一個(gè)溫度傳感器和在工作環(huán)境下僅有1%變動(dòng)的振蕩器。芯片尺寸440.9mm，采用QFN封裝（無(wú)引線(xiàn)方形封裝），可承受最大10000g的沖擊，并有可編程的低通濾波器。關(guān)于電源，MPU-6050可支持VDD范圍2.5V5%，3.0V5%，或3.3V5%。另外MPU-6050還有一個(gè)VLOGIC引腳，用來(lái)為I2C輸出提供邏輯電平。VLOGIC電壓可取1.85%或者VDD。 圖2.4 Mpu6050陀螺儀 其有以下優(yōu)點(diǎn)：（）集角度測(cè)量與加速度測(cè)量于一體（2）其那同時(shí)測(cè)量三軸上的角度與加速度測(cè)量（3）其輸出為數(shù)字信號(hào)，便于處理于存儲(chǔ)與傳輸（4）測(cè)量范圍大，反應(yīng)快。 2.3.4 降壓模塊的選擇 LM2596數(shù)顯可調(diào)降壓模塊是直流降壓穩(wěn)壓模塊，帶有電壓顯示功能，按鍵可切換輸入或輸出電壓測(cè)量；具有輸入反接保護(hù)，輸出短路保護(hù)，過(guò)流過(guò)熱保護(hù)等措施；模塊全部采用優(yōu)質(zhì)元器件，用料講究，布局合理，外觀精美，性能穩(wěn)定可靠。帶電壓表顯示，電壓表量程040V，誤差0.1V；輕觸按鍵（短按）即可切換測(cè)量輸入或輸出電壓，并有指示燈顯示正在測(cè)量的是哪路電壓；特有的輸入和輸出自動(dòng)轉(zhuǎn)換顯示功能，可實(shí)時(shí)監(jiān)控電壓；帶電源指示燈，可指示是否有電源輸入；帶反接保護(hù)，反接不會(huì)損壞模塊；帶接線(xiàn)端子，方便使用； 圖2.5 降壓模塊2.3.5超聲波模塊的選擇DYP-ME007超聲波測(cè)距模塊可提供3cm-3.5m的非接觸式距離感測(cè)功 能，圖1為DYP-ME007外觀，包括超聲波發(fā)射器、接收器與控制電路。其基本工作原理為給予此超聲波測(cè)距模塊一觸發(fā)信號(hào)后發(fā)射超聲波，當(dāng)超聲波投射到物體而反射回來(lái)時(shí)，模塊輸出一回響信號(hào)，以觸發(fā)信號(hào)和回響信號(hào)間的時(shí)間差，來(lái)判定物體的距離。 圖2.6 超聲波測(cè)距模塊 2.3.6藍(lán)牙模塊的選擇 HC-05 無(wú)線(xiàn)藍(lán)牙串口透?jìng)髂K引出接口包括VCC,GND,TXD,RXD,預(yù)留LED狀態(tài)輸出腳，單片機(jī)可通過(guò)該腳狀態(tài)判斷藍(lán)牙是否已經(jīng)連接，KEY引腳對(duì)從機(jī)無(wú)效。底板3.3V LDO，輸入電壓3.66V，未配對(duì)時(shí)電流約30mA，配對(duì)后約10mA，輸入電壓禁止超過(guò)7V。空曠地有效距離10米，超過(guò)10米也是可能的，但不對(duì)此距離的連接質(zhì)量做保證。配對(duì)以后當(dāng)全雙工串口使用，無(wú)需了解任何藍(lán)牙協(xié)議，但僅支持8位數(shù)據(jù)位、1位停止位、無(wú)奇偶校驗(yàn)的通信格式，這也是最常用的通信格式，不支持其他格式。 圖2.7 無(wú)線(xiàn)藍(lán)牙模塊2.4 結(jié)構(gòu)的合理性本機(jī)構(gòu)采用兩輪驅(qū)動(dòng)，通過(guò)調(diào)節(jié)左右兩輪的差速實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向，能更好的實(shí)現(xiàn)小車(chē)的換向。 第三章 機(jī)械系統(tǒng)設(shè)計(jì)3.1 機(jī)械系統(tǒng)總體方案雙輪自平衡小車(chē)身上裝有2個(gè)電機(jī)，用來(lái)帶動(dòng)車(chē)輪轉(zhuǎn)動(dòng)能夠?qū)崿F(xiàn)直立、速度、轉(zhuǎn)向平衡等基本動(dòng)作，小車(chē)車(chē)身裝有藍(lán)牙模塊、陀螺儀模塊、驅(qū)動(dòng)模塊、降壓模塊、和超聲波模塊。小車(chē)電機(jī)控制通過(guò)單片機(jī)來(lái)實(shí)現(xiàn)控制。底面的陀螺儀模塊作為角度檢測(cè)傳感器，可以實(shí)現(xiàn)自平衡功能。還有藍(lán)牙模塊，實(shí)現(xiàn)手機(jī)藍(lán)牙控制，超聲波模塊實(shí)現(xiàn)避障功能。 圖3.1 小車(chē)初步設(shè)計(jì)3.2關(guān)鍵零部件結(jié)構(gòu)尺寸設(shè)計(jì)通過(guò)在查閱相關(guān)資料和比較現(xiàn)有平衡車(chē)產(chǎn)品，初步設(shè)定平衡車(chē)機(jī)器人的外形機(jī)構(gòu)如圖3.1所示，其中小車(chē)的3個(gè)亞克力板大小均為150mm80mm。 圖3.2 亞克力板 3.3 驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)軌跡規(guī)劃 3.3.1運(yùn)動(dòng)學(xué)方程圖 3.2 小車(chē)的受力分析圖如圖 3.2所示，小車(chē)的本質(zhì)不穩(wěn)定，控制系統(tǒng)根據(jù)測(cè)得的傾角產(chǎn)生一個(gè)相應(yīng)的力矩，通過(guò)控制電機(jī)驅(qū)動(dòng)兩個(gè)輪朝車(chē)身要倒下的方向運(yùn)動(dòng)，以保持小車(chē)自身的動(dòng)態(tài)平衡?？刂屏渴禽喿拥霓D(zhuǎn)動(dòng)力矩。小車(chē)在水平方向上位移為： 豎直方向上位移為： 在各個(gè)方向上的運(yùn)動(dòng)方程可以寫(xiě)為： 3.3.2速度分析1.驅(qū)動(dòng)電機(jī)為直流減速電機(jī)，電機(jī)的動(dòng)態(tài)方程為： 電機(jī)轉(zhuǎn)角與小車(chē)的位置關(guān)系，電機(jī)轉(zhuǎn)子的角位移為，電機(jī)的齒輪比為，則小車(chē)的驅(qū)動(dòng)輪可以表示為： 設(shè)小車(chē)的質(zhì)量為M, 為電機(jī)提供給方向上的驅(qū)動(dòng)力，則小車(chē)的運(yùn)動(dòng)學(xué)方程為： 聯(lián)立上式可以得：為了保持平衡，小車(chē)驅(qū)動(dòng)力，必須大于或等于重力沿水平方向上的分力 將兩輪的轉(zhuǎn)動(dòng)力矩作為系統(tǒng)輸入量u，建立平衡小車(chē)的系統(tǒng)方程為： 3.3.3 運(yùn)動(dòng)學(xué)反解假設(shè)小車(chē)作勻加速直線(xiàn)運(yùn)動(dòng)，小車(chē)的輪子在地面上作純滾動(dòng)，由此可得小車(chē)的加速度為： 時(shí)小車(chē)運(yùn)動(dòng)的位置為，車(chē)與豎直方向的夾角為，由此可得，車(chē)輪的輸入角度為： 第四章 控制系統(tǒng)設(shè)計(jì)4.1 控制系統(tǒng)總體方案 自平衡小車(chē)由多個(gè)功能模塊共同組成，這幾個(gè)模塊共同工作、相互協(xié)調(diào)、相互作用，保證了小車(chē)的直立、速度、轉(zhuǎn)向平衡。具體的工作原理如下：由STM32作為控制芯片，它對(duì)其它各個(gè)功能模塊進(jìn)行控制。信息采集模塊負(fù)責(zé)采集周?chē)h(huán)境以及機(jī)器人本身的各種信息。其中陀螺儀、角速度編碼器，主要負(fù)責(zé)小車(chē)的平衡的功能，超聲波測(cè)距模塊實(shí)現(xiàn)避障。藍(lán)牙遙控接收模塊可以接收人們對(duì)機(jī)器人的控制信息，然后把信息傳給STM32進(jìn)行處理。4.2 選型1) 電機(jī)選擇12V直流減速電機(jī)2) HC-05藍(lán)牙模塊3) DYP-ME007超聲波測(cè)距模塊4) LM2596數(shù)顯可調(diào)降壓模塊5) MPU-6050陀螺儀6) TB6612FNG電機(jī)驅(qū)動(dòng)4.3 STM32使用的資源 表4-1 STM32資源TIM15ms定時(shí)中斷 采集小車(chē)位姿TIM2初始化為正交編碼器模式，硬件采集編碼器1數(shù)據(jù)（采集超聲波脈寬）TIM3初始化為PWM輸出，CH3 ，CH4輸出20KHZ的PWM控制電機(jī)TIM4初始化為正交編碼器模式，硬件采集編碼器2數(shù)據(jù)USTART3通過(guò)串口3接受藍(lán)牙遙控?cái)?shù)據(jù)，接受方式為中斷接收IIC利用IO模擬IIC接收MPU6050數(shù)據(jù)GPIO 輸入和輸出4.4 電機(jī)驅(qū)動(dòng)的電路設(shè)計(jì) 圖4.1 TB6612FNG引腳 STBY口接單片機(jī)的IO口清零電機(jī)全部停止，置1則通過(guò)AIN1 AIN2，BIN1，BIN2 來(lái)控制正反轉(zhuǎn)，VM接12V以?xún)?nèi)電源，VCC接5V電源，GND接地，PWMA和PWMB分別接接單片機(jī)的PWM口，A01和AO2接電機(jī)1的兩個(gè)腳。BO1和BO2接電機(jī)2的兩個(gè)腳。表4-2 電機(jī)1真值表 AIN1 0 0 1 AIN2 0 1 0 停止正轉(zhuǎn)反轉(zhuǎn)表4-3 電機(jī)2真值表 BIN1 0 0 1 BIN2 0 1 0 停止正轉(zhuǎn)反轉(zhuǎn) 4.5 傳感檢測(cè)外圍電路設(shè)計(jì) 圖4.2 驅(qū)動(dòng)電機(jī)控制電路 圖4.3 藍(lán)牙 圖4.4 超聲波 圖4.5 陀螺儀 4.6 電源供電電路設(shè)計(jì) 圖4.6 電源電路4.7 總電路圖 圖4.7 總電路圖 第五章 軟件系統(tǒng)設(shè)計(jì)5.1 軟件系統(tǒng)總體方案1. 通過(guò)陀螺儀與編碼器實(shí)現(xiàn)小車(chē)的自平衡功能。2. 通過(guò)藍(lán)牙模塊實(shí)現(xiàn)手機(jī)端的藍(lán)牙遙控，手動(dòng)實(shí)現(xiàn)小車(chē)的前后左右。3. 通過(guò)超聲波模塊實(shí)現(xiàn)小車(chē)的避障功能。5.2位姿控制方案5.2.1 PID控制控制技術(shù)是運(yùn)動(dòng)控制的核心，各種先進(jìn)控制技術(shù)的研究不斷推動(dòng)著運(yùn)動(dòng)控制的發(fā)展，比如自適應(yīng)控制技術(shù)和以神經(jīng)網(wǎng)絡(luò)和模糊控制為代表的智能控制技術(shù)，但在實(shí)際生產(chǎn)實(shí)踐中應(yīng)用最普遍的還是各種以PID為代表的基本控制技術(shù)按照偏差的比例、積分和微分進(jìn)行控制的調(diào)節(jié)器，簡(jiǎn)稱(chēng)為PID調(diào)節(jié)器，是連續(xù)系統(tǒng)中技術(shù)成熟且應(yīng)用廣泛的一種調(diào)節(jié)器。本節(jié)將對(duì)系統(tǒng)用到的PID控制技術(shù)做相應(yīng)的介紹和研究，當(dāng)小車(chē)開(kāi)始傾斜時(shí)，陀螺儀及時(shí)地將采集的小車(chē)傾斜角度、瞬時(shí)角加速度傳給單片機(jī)，同時(shí)，編碼器也將車(chē)子的實(shí)時(shí)速度采集后傳給單片機(jī)，單片機(jī)系統(tǒng)收集到以上三組數(shù)據(jù)，對(duì)數(shù)據(jù)進(jìn)行量化處理后，在PID平衡算法的控制下，控制電機(jī)及時(shí)地做出前進(jìn)或后退或加速或減速的反應(yīng)，使車(chē)子在一個(gè)小角度范圍內(nèi)做平衡地來(lái)回?cái)[動(dòng)，以保持車(chē)子的不倒。5.2.2 應(yīng)用現(xiàn)狀在電機(jī)伺服系統(tǒng)的控制中，經(jīng)典的PID控制具有其結(jié)構(gòu)簡(jiǎn)單、魯棒性強(qiáng)以及現(xiàn)場(chǎng)對(duì)Pro的廣泛使用積累了豐富的經(jīng)驗(yàn)等優(yōu)點(diǎn)，在無(wú)刷直流電動(dòng)機(jī)的控制方面一直占有很重要的地位。PID的引入保證了其系統(tǒng)響應(yīng)的快速性，穩(wěn)定了閉環(huán)控制器，補(bǔ)償了由逆變器引起的控制誤差。PID控制器就是將偏差的比例(Proportional)、積分(Integral)和微分(Differential)通過(guò)線(xiàn)性組合構(gòu)成控制量，用這一控制量對(duì)被控對(duì)象進(jìn)行控制。 PID算法是目前工業(yè)過(guò)程控制中應(yīng)用最廣泛的控制算法。PID算法應(yīng)用如此廣泛，是因?yàn)樗哂腥缦聝?yōu)點(diǎn):(1)算法較為簡(jiǎn)單，易于實(shí)現(xiàn)；(2)基于線(xiàn)性控制理論，具備許多成熟的穩(wěn)定性分析方法，有較高的可靠性；(3)可以在很寬的操作條件內(nèi)保持較好的魯棒性，對(duì)于控制對(duì)象模型參數(shù)小范變化不敏感；(4)不要求了解控制對(duì)象的精確數(shù)學(xué)模型。利用許多成熟的參數(shù)整定方法，可以根據(jù)控制對(duì)象的實(shí)際響應(yīng)曲線(xiàn)來(lái)計(jì)算PID控制器的參數(shù)；(5)允許工程技術(shù)人員以一種簡(jiǎn)單直接的方式來(lái)調(diào)節(jié)控制系統(tǒng)，以達(dá)到希望得到的控制性能，如上升時(shí)間、最大超調(diào)量和穩(wěn)態(tài)誤差等。當(dāng)被控對(duì)象的結(jié)構(gòu)和參數(shù)不能完全掌握，或得不到精確的數(shù)學(xué)模型時(shí)，系統(tǒng)控制器的結(jié)構(gòu)和參數(shù)必須依靠經(jīng)驗(yàn)和現(xiàn)場(chǎng)調(diào)試來(lái)確定，這時(shí)應(yīng)用PID控制技術(shù)最為方便。即當(dāng)我們不完全了解一個(gè)系統(tǒng)和被控對(duì)象，或不能通過(guò)有效的測(cè)量手段來(lái)獲得系統(tǒng)參數(shù)時(shí)，最適合用PID控制技術(shù)。5.2.3 PID調(diào)節(jié)規(guī)律PID控制分為兩大類(lèi)，一個(gè)是模擬PID控制，一個(gè)是數(shù)字PM控制。在模擬控制系統(tǒng)中，PID是最常用的控制方法。圖5.1所示為模擬PID控制系統(tǒng)原理框圖。比例系數(shù)Kp積分系數(shù)Ki微分系數(shù)Kd被控對(duì)象r(t)c(t) + _ + + u(t) y(t) + 圖 圖5.1 模擬PID控制系統(tǒng)原理框圖計(jì)算機(jī)控制系統(tǒng)中，使用的是數(shù)字PID控制器。將連續(xù)的模擬量進(jìn)行離散化處理，則可得在數(shù)字PID控制中，采樣周期相對(duì)于系統(tǒng)的時(shí)間常數(shù)來(lái)說(shuō)一般是很短。因此其參數(shù)可按模擬PID控制器中的方法來(lái)選擇。由于要保持動(dòng)態(tài)的平衡，則小車(chē)的傾角在一定的范圍內(nèi)要求可控。故本文選擇系統(tǒng)的小車(chē)的傾角作為輸出量，利用PID技術(shù)對(duì)其進(jìn)行分析。由上述理論分析可以看出:其比例、積分、微分三者是彼此影響；同時(shí)要使其系統(tǒng)保持穩(wěn)定，PID三參數(shù)必須滿(mǎn)足上述關(guān)系；為了讓系統(tǒng)能夠?qū)崿F(xiàn)其動(dòng)態(tài)的平衡，需要通過(guò)反復(fù)試湊的方法來(lái)解決，而且整定的參數(shù)多導(dǎo)致反復(fù)試湊的次數(shù)極大的增加，控制器的參數(shù)較難選取。加之系統(tǒng)經(jīng)過(guò)線(xiàn)性化處理，即使所選參數(shù)滿(mǎn)足上述的關(guān)系式，仍需要根據(jù)系統(tǒng)的實(shí)際模型進(jìn)行調(diào)整，這大大增加了調(diào)試的難度。5.3 位姿控制流程圖 圖5.2 位姿控制流程圖5.4 速度控制方案利用PWM原理來(lái)控制直流減速電機(jī)的轉(zhuǎn)動(dòng)，PWM主要就是控制頻率和占空比的：這兩個(gè)因素分別通過(guò)兩個(gè)寄存器控制：TIMX_ARR和TIMX_CCRX。ARR寄存器就是自動(dòng)重裝寄存器，也就是計(jì)數(shù)器記到這個(gè)數(shù)以后清零再開(kāi)始計(jì)，這樣PWM的頻率就是tim_frequency/（TIMX_ARR-1）。在計(jì)數(shù)時(shí)會(huì)不停的和CCRX寄存器中的數(shù)據(jù)進(jìn)行比較，如果小于的話(huà)是高電平或者低電平，計(jì)數(shù)值大于CCRX值的話(huà)電平極性反相。所以這也就控制了占空比。5.5 手機(jī)藍(lán)牙遙控流程圖 圖 5.3 手機(jī)藍(lán)牙遙控流程圖5.6 超聲波避障流程圖 圖5.4 超聲波避障流程圖 第六章 裝配與調(diào)試6.1 機(jī)器人系統(tǒng)整體裝配 圖6.1 小車(chē)實(shí)物裝配圖6.2系統(tǒng)調(diào)試流程與狀況1) 根據(jù)PID控制調(diào)節(jié)參數(shù)，如和來(lái)實(shí)現(xiàn)兩輪直立平衡。同理，需調(diào)節(jié)相應(yīng)參數(shù)實(shí)現(xiàn)速度與轉(zhuǎn)向平衡。由于平衡的角度中值和機(jī)械相關(guān)，把平衡小車(chē)放在可以讓輪子懸空的臺(tái)子上，需要通過(guò)不斷調(diào)試找出小車(chē)接近平衡的角度。2) 由于陀螺儀的漂移誤差和加速度計(jì)的動(dòng)態(tài)誤差、相應(yīng)參數(shù)調(diào)試的不完美、齒輪減速器存在死區(qū)、小車(chē)存在比較大的慣性等原因?qū)е滦≤?chē)不能完全靜止平衡，產(chǎn)生抖動(dòng)。通過(guò)互補(bǔ)濾波可以得到更優(yōu)的傾角近似值，有利于平衡。3) 超聲波模塊的響應(yīng)與避障，設(shè)定距離不能太短，使小車(chē)在一定速度下可以避免碰撞。4) 藍(lán)牙模塊與手機(jī)連接的時(shí)候，藍(lán)牙遙控的有效距離在空曠地為10米左右，所以實(shí)際情況下超過(guò)10米或小于10米也是可能的。6.3 設(shè)計(jì)創(chuàng)新點(diǎn) 在小車(chē)平衡的基礎(chǔ)上附加了遙控和避障功能，以及通過(guò)調(diào)節(jié)左右兩輪的差速實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向，能更好的實(shí)現(xiàn)小車(chē)的換向。6.4 系統(tǒng)缺陷與改進(jìn) 小車(chē)平衡性能還是不完美，存在抖動(dòng)，同時(shí)速度過(guò)快很容易摔倒，還需后續(xù)的調(diào)試與改進(jìn)。21 第七章 市場(chǎng)應(yīng)用前景分析7.1 市場(chǎng)前景 電動(dòng)平衡車(chē)是我國(guó)新興產(chǎn)業(yè)之一，開(kāi)始較廣泛的被用來(lái)商用和警用，起到了非常不錯(cuò)的效果，其次，自平衡電動(dòng)車(chē)在中國(guó)的民用銷(xiāo)量也是很不錯(cuò)的，很多人對(duì)它的特性所著迷。總體來(lái)說(shuō)，自平衡電動(dòng)車(chē)在中國(guó)的市場(chǎng)還是比較樂(lè)觀的，隨著民眾生活水平的提高，自平衡電動(dòng)車(chē)的銷(xiāo)量一定會(huì)出現(xiàn)成倍的增加，這都是完全可能的。但由于電動(dòng)平衡車(chē)在國(guó)內(nèi)的技術(shù)已經(jīng)成熟，我們需要對(duì)其進(jìn)行不斷地改進(jìn)，不斷地創(chuàng)新，爭(zhēng)取把產(chǎn)品最優(yōu)化，實(shí)現(xiàn)人們的需求。7.2 產(chǎn)品優(yōu)勢(shì)目前大部分市面上的電動(dòng)平衡車(chē)，占道面積等同中等體型人的寬度，因此對(duì)于現(xiàn)代擁擠的交通狀況來(lái)說(shuō)確實(shí)滿(mǎn)足最初的設(shè)計(jì)理念：流暢平順的人類(lèi)運(yùn)輸器。平衡車(chē)采用鋰電池能量驅(qū)動(dòng)，由普通民用電進(jìn)行補(bǔ)足，很方便人們使用。相對(duì)傳統(tǒng)代步工具，電動(dòng)平衡車(chē)的機(jī)動(dòng)性比較強(qiáng)。 項(xiàng)目心得本設(shè)計(jì)主要研究?jī)奢喿云胶庑≤?chē)的設(shè)計(jì)與實(shí)現(xiàn)。通過(guò)相應(yīng)硬件與軟件的設(shè)計(jì)，最終實(shí)現(xiàn)了對(duì)兩輪自平衡小車(chē)電機(jī)的的動(dòng)態(tài)平衡與運(yùn)動(dòng)控制。系統(tǒng)硬件結(jié)構(gòu)以stm32為控制核心，采用mpu6050構(gòu)成了姿態(tài)檢測(cè)系統(tǒng)，當(dāng)我們輕輕將陀螺儀裝懂一個(gè)角度時(shí)，電機(jī)就會(huì)帶動(dòng)車(chē)輪轉(zhuǎn)動(dòng)，當(dāng)我們向著反方向輕輕轉(zhuǎn)動(dòng)時(shí)，電機(jī)會(huì)帶動(dòng)車(chē)輪向相反的方向轉(zhuǎn)動(dòng)，陀螺儀在次處于水平，電機(jī)會(huì)停止轉(zhuǎn)動(dòng)。 最終，我們要完成實(shí)現(xiàn)了兩輪自平衡小車(chē)的平衡控制及運(yùn)動(dòng)控制。本設(shè)計(jì)的控制策略采用PID控制算法。通過(guò)對(duì)小車(chē)的運(yùn)動(dòng)建模，構(gòu)建了小車(chē)運(yùn)動(dòng)控制的PD控制算法，并對(duì)PID參數(shù)進(jìn)行了整定,使小車(chē)能夠穩(wěn)定地平衡。并且在適量干擾下，小車(chē)能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)。小車(chē)能夠?qū)崿F(xiàn)手機(jī)藍(lán)牙控制前后左右移動(dòng)的功能，以及能夠智能實(shí)現(xiàn)避障功能。設(shè)計(jì)過(guò)程中，在老師、助教以及同學(xué)的幫助和配合下，才使得項(xiàng)目如期基本完成，這體現(xiàn)了團(tuán)隊(duì)精神的重要性。本次課程實(shí)踐項(xiàng)目無(wú)論在專(zhuān)業(yè)知識(shí)還是在學(xué)習(xí)能力上都對(duì)我們有很大的啟發(fā)，也非常感謝老師和助教的指導(dǎo)。 參考文獻(xiàn)1 張洋 劉軍 嚴(yán)漢寶.原子教你玩STM32（庫(kù)函數(shù)版）M.北京航空航天大學(xué)出版社,20072 劉軍 張洋 原子教你玩STM32（寄存器版）M.北京：北京航空航天大學(xué)出版社，20133 熊有倫.機(jī)器人技術(shù)基礎(chǔ),華中科技大學(xué)出版社,19954 李凡紅 兩輪自平衡小車(chē)系統(tǒng)J 北京交通大學(xué)學(xué)報(bào)，2011年03期附錄一 成本分析 表1 成本電機(jī)2個(gè)STM32芯片1個(gè)驅(qū)動(dòng)模塊1個(gè)陀螺儀1個(gè)降壓模塊2個(gè)車(chē)輪及聯(lián)軸器2個(gè)亞克力板3個(gè)電機(jī)支架2個(gè)電路板2個(gè)手工費(fèi)其他藍(lán)牙模塊 超聲波模塊總計(jì)49元/個(gè)29.8元/個(gè)12.9元/個(gè)6.7元/個(gè)20元/個(gè)20元/個(gè)9元/個(gè)3.8元/個(gè)2元/個(gè)50元15元10元/個(gè)10元/個(gè)356元附錄二 程序源代碼主程序：u8 Way_Angle=2; /獲取角度的算法，2：卡爾曼 可以改成3 即互補(bǔ)濾波，但是在頂配版程序中無(wú)法使用DMP,因?yàn)闀r(shí)序和超聲波有沖突 u8 Flag_Qian,Flag_Hou,Flag_Left,Flag_Right,Flag_sudu=2; /藍(lán)牙遙控相關(guān)的變量u8 Flag_Stop=1,Flag_Show=1; /停止標(biāo)志位和 顯示標(biāo)志位 默認(rèn)停止 顯示打開(kāi)int Encoder_Left,Encoder_Right; /左右編碼器的脈沖計(jì)數(shù)int Moto1,Moto2; /電機(jī)PWM變量 Motorint Temperature; /顯示溫度int Voltage; /電池電壓采樣相關(guān)的變量float Angle_Balance,Gyro_Balance,Gyro_Turn; /平衡傾角 平衡陀螺儀 轉(zhuǎn)向陀螺儀float Show_Data_Mb; /全局顯示變量，用于顯示需要查看的數(shù)據(jù)u32 Distance; /超聲波測(cè)距int main(void) Stm32_Clock_Init(9); /系統(tǒng)時(shí)鐘設(shè)置delay_init(72); /延時(shí)初始化JTAG_Set(JTAG_SWD_DISABLE); /=關(guān)閉JTAG接口JTAG_Set(SWD_ENABLE); /=打開(kāi)SWD接口 可以利用主板的SWD接口調(diào)試LED_Init(); /初始化與 LED 連接的硬件接口KEY_Init(); /按鍵初始化uart_init(72,115200); /初始化串口1uart3_init(36,9600); /串口3初始化Adc_Init(); /ADC初始化MiniBalance_PWM_Init(7199,0); /=初始化PWM 10KHZ 高頻可以防止電機(jī)低頻時(shí)的尖叫聲#if MODE_BIZHANG=1 TIM2_Cap_Init(0XFFFF,72-1); /=如果MODE_BIZHANG置1，則把TIM2初始化為超聲波接口 然后不采集左路編碼器，通過(guò)右路編碼器近似左路車(chē)輪速度#else Encoder_Init_TIM2(); /=如果MODE_BIZHANG置0，則把TIM2初始化為編碼器接口#endifEncoder_Init_TIM4(); /初始化編碼器2 IIC_Init(); /模擬IIC初始化 MPU6050_initialize(); /=MPU6050初始化DMP_Init(); /初始化DMP Timer1_Init(49,7199); /=5MS進(jìn)一次中斷服務(wù)函數(shù)#if MODE_BIZHANG=1 Read_Distane(); /BIZHANG_MODE=1才能讀取超聲波#endif角度測(cè)量int Balance_Pwm,Velocity_Pwm,Turn_Pwm;u8 Flag_Target;int TIM1_UP_IRQHandler(void) if(TIM1-SR&0X0001)/5ms定時(shí)中斷 TIM1-SR&=(10); /=清除定時(shí)器1中斷標(biāo)志位 Flag_Target=!Flag_Target; if(Flag_Target=1) /=5ms讀取一次陀螺儀和加速度計(jì)的值，更高的采樣頻率可以改善卡爾曼濾波和互補(bǔ)濾波的效果Get_Angle(Way_Angle); /=更新姿態(tài)return 0; /=10ms控制一次，為了保證M法測(cè)速的時(shí)間基準(zhǔn)，首先讀取編碼器數(shù)據(jù)Encoder_Right=Read_Encoder(4); /=讀取編碼器的值#if MODE_BIZHANG=1 Encoder_Left=Encoder_Right; /=避障模式，左路編碼器直接由右路編碼器近似，并取消轉(zhuǎn)向環(huán)中的編碼器使用，只用Z軸陀螺儀#else Encoder_Left=-Read_Encoder(2); /=讀取編碼器的值，因?yàn)閮蓚€(gè)電機(jī)的旋轉(zhuǎn)了180度的，所以對(duì)其中一個(gè)取反，保證輸出極性一致 #endif Get_Angle(Way_Angle); Led_Flash(100); /=LED閃爍;指示單片機(jī)正常運(yùn)行 Voltage=Get_battery_volt(); /=獲取電池電壓 Key(); /=掃描按鍵狀態(tài) 單擊雙擊可以改變小車(chē)運(yùn)行狀態(tài) Balance_Pwm =balance(Angle_Balance,Gyro_Balance); /=平衡PD控制 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); /=速度環(huán)PI控制 記住，速度反饋是正反饋，就是小車(chē)快的時(shí)候要慢下來(lái)就需要再跑快一點(diǎn) Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); /=轉(zhuǎn)向環(huán)PD控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; /=計(jì)算左輪電機(jī)最終PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; /=計(jì)算右輪電機(jī)最終PWM Xianfu_Pwm(); /=PWM限幅 if(Turn_Off(Angle_Balance,Voltage)=0) /=如果不存在異常 Set_Pwm(Moto1,Moto2); /=賦值給PWM寄存器 return 0; 直立PD控制int balance(float Angle,float Gyro) float Bias,kp=390,kd=1.2; int balance; Bias=Angle+9; /=求出平衡的角度中值 和機(jī)械相關(guān) balance=kp*Bias+Gyro*kd; /=計(jì)算平衡控制的電機(jī)PWM PD控制 kp是P系數(shù) kd是D系數(shù) return balance;速度PI控制int velocity(int encoder_left,int encoder_right) static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral; float kp=-80,ki=-0.4; /遙控前進(jìn)后退部分if(1=Flag_Qi