兩輪自平衡小車的PID控制系統(tǒng)設(shè)計(jì)摘要自平衡小車控制系統(tǒng)是機(jī)器人領(lǐng)域十分熱門的研究問題，它可以把人工智能領(lǐng)域和自動(dòng)控制系統(tǒng)結(jié)合在一起，這是一個(gè)很經(jīng)典的學(xué)科交叉融合的應(yīng)用。近年來，隨著現(xiàn)在微電子學(xué)的快速發(fā)展,以及自平衡小車使用的電能十分的干凈清潔，相比傳統(tǒng)能源它沒有什么大的污染十分符合國家對(duì)新能源的倡導(dǎo)。再加上自平衡小車相比傳統(tǒng)的交通工具十分的小巧輕便，容易攜帶，不像傳統(tǒng)汽車那樣的笨重。所以他的前景逐漸被人們開始挖掘，如小米推出的電動(dòng)平衡車，推出后沒有多久就受到了年輕人的喜愛，這也加速了平衡車領(lǐng)域的發(fā)展，所以平衡車的前景是十分巨大的。本文用STM32F103作為自平衡小車的主控芯片，用PID算法結(jié)合卡爾曼濾波做自平衡小車的系統(tǒng)方案。小車主要由控制模塊、OLED模塊、電機(jī)驅(qū)動(dòng)模塊，姿態(tài)檢測模塊組成。然后通過PID算法和卡爾曼濾波對(duì)采集到的數(shù)據(jù)進(jìn)行處理。在實(shí)驗(yàn)中當(dāng)小車的位姿角度為-40°到+40°之間時(shí)小車能夠進(jìn)行自平衡調(diào)節(jié)控制，當(dāng)小車角度超出這個(gè)范圍時(shí)就會(huì)自動(dòng)關(guān)閉電機(jī)讓小車停止。小車經(jīng)過實(shí)驗(yàn)發(fā)現(xiàn)小車具有良好的轉(zhuǎn)向能力和能夠很好的進(jìn)行抗干擾。目錄摘要 I1緒論 11.1引言 11.1.1研究背景 11.1.2研究意義 11.2兩輪自平衡小車的國內(nèi)外研究現(xiàn)狀 21.2.1國外研究現(xiàn)狀 21.2.2國內(nèi)研究現(xiàn)狀 41.2.3總結(jié) 41.3論文的組織結(jié)構(gòu) 52硬件部分設(shè)計(jì) 62.1硬件系統(tǒng)的組成 62.2主要電路模塊的說明介紹 62.2.1電機(jī)驅(qū)動(dòng)模塊 62.2.2STM32主控模塊 92.2.3MPU—6050（姿態(tài)檢測模塊） 102.2.4電源開關(guān)管理芯片 123相關(guān)算法理論 133.1PID算法 133.1.1位置型PID算法 133.1.2增量式PID算法 143.2Kalman濾波簡介 144軟件設(shè)計(jì)部分 154.1兩輪自平衡小車主程序設(shè)計(jì) 154.2介紹所使用的軟件 174.3PID控制算法 174.3濾波算法 174.4卡爾曼濾波 184.5兩輪自平衡車的實(shí)物展示與測試 194.6本章小結(jié) 215總結(jié)與展望 225.1總結(jié) 225.2展望 221緒論1.1引言機(jī)器人是目前比較熱門的項(xiàng)目也是實(shí)現(xiàn)中國2025工業(yè)自動(dòng)化的前提，而自平衡小車目前以及由較廣泛的商業(yè)化案例且十分成功，所以我選擇去研究它下面我將介紹我的研究背景和研究意義。1.1.1研究背景機(jī)器人技術(shù)是一門多學(xué)科交叉融合的多學(xué)科應(yīng)用型的技術(shù)，在幾千年前富有智慧的先民們就已經(jīng)創(chuàng)造了許多的自動(dòng)更具和一些簡單記得自動(dòng)機(jī)械。例如我國古代著名的一些名作有，春秋后期著名的木匠魯班發(fā)明的木鳥，東漢時(shí)期我國著名的天文學(xué)家發(fā)明的計(jì)里鼓車，三國時(shí)期諸葛亮為運(yùn)糧草發(fā)明的木牛馬車等。但是現(xiàn)代機(jī)器人的真正發(fā)展要追溯與19世紀(jì)60年代美國Unimate公司發(fā)明的世界上第一臺(tái)商用的工業(yè)機(jī)器人，因此國際上也稱這一年為“機(jī)器人元年“。而到如今隨著計(jì)算機(jī)技術(shù)，電子半導(dǎo)體相關(guān)技術(shù)、力學(xué)、機(jī)械學(xué)等的相關(guān)學(xué)科的發(fā)展，從而間接推動(dòng)者機(jī)器人相關(guān)學(xué)科也在快速的發(fā)展，這也使得相關(guān)機(jī)器人技術(shù)日益成熟，對(duì)人們的生活和工業(yè)的發(fā)展都產(chǎn)生了巨大的意義，它讓工廠的效率得到了極大的提高讓人們從枯燥的重復(fù)運(yùn)動(dòng)中解放出來。移動(dòng)型機(jī)器人是整個(gè)機(jī)器人大家族中的一員，也是目前應(yīng)用比較廣泛的一種，商業(yè)價(jià)值極高，例如小米平衡車、京東物流倉庫的物流自動(dòng)搬運(yùn)車等。本文的研究的小車便是移動(dòng)型機(jī)器人的一種。隨著現(xiàn)在得到污染問題的日益嚴(yán)重，以及國家現(xiàn)在大力推動(dòng)新能源汽車。自平衡小車將會(huì)是一種很好的解決人們短途出行的方案。自平衡小車是通過兩個(gè)車輪來讓車子保持直立的狀態(tài)，同時(shí)每個(gè)車輪各自配有一個(gè)電機(jī)，從而可以通過控制兩個(gè)電機(jī)的速度從而實(shí)現(xiàn)轉(zhuǎn)彎，通過控制電機(jī)的正反轉(zhuǎn)從而控制車子的前進(jìn)和后退等，它能夠適合各種條件狀況不同的道路。1.1.2研究意義兩輪自平衡小車是輪式機(jī)器人的一個(gè)分支，但是相比與普通的輪式機(jī)器人因?yàn)樗且粋€(gè)一個(gè)統(tǒng)合的系統(tǒng)，所以可以通過自己去檢測外部信息從而讓自己顯得更加的智能化，簡單化。所以他的系統(tǒng)又有著自己所獨(dú)有的一些優(yōu)點(diǎn)：自平衡小車的相關(guān)原理來自于傳統(tǒng)的一階倒立擺系統(tǒng)，即平衡小車是在一階倒立擺的相關(guān)原理上發(fā)展起來的。他繼承了一階倒立擺的相關(guān)優(yōu)點(diǎn)并且相比于倒立擺系統(tǒng)，他更加的靈活，移動(dòng)的角度相對(duì)來說更大，并且在實(shí)際的商業(yè)發(fā)展之中，其商業(yè)價(jià)值目前來說遠(yuǎn)高于一階倒立擺系統(tǒng)?；陲@得集成芯片例如本文中的基于STM32單片機(jī)開發(fā)設(shè)計(jì)自平衡小車，他相比以前的來說它的體積、質(zhì)量都遠(yuǎn)遠(yuǎn)低于前者，這也使得它可以輕便攜帶和通過比較狹窄的一些通道等。并且小車使用的是電能屬于國家現(xiàn)在大力提倡的清潔能源，可以很好提高能源的使用率且不會(huì)產(chǎn)生任何有害氣體 ,其次隨著現(xiàn)在微電子學(xué)的高速發(fā)展，現(xiàn)在小車的各種傳感器模塊，芯片等都已經(jīng)集成模塊化，完全可以根據(jù)自己的需要進(jìn)行相關(guān)功能的添加，且成本相對(duì)以前低很多。自平衡小車有著物理模型非線性，其系統(tǒng)不穩(wěn)定等特點(diǎn)，它屬于欠驅(qū)動(dòng)的一種復(fù)雜的系統(tǒng)，它的復(fù)雜原因主要有一下幾個(gè)點(diǎn)即：系統(tǒng)是時(shí)變的，他的各參數(shù)也是不穩(wěn)定的，它各個(gè)傳感器之間相互的耦合在一起。而這些問題在航空航天，軍工領(lǐng)域，計(jì)算機(jī)控制的其他領(lǐng)域都有很好的學(xué)習(xí)參考和借鑒作用，有著很重大的研究價(jià)值和研究意義。1.2兩輪自平衡小車的國內(nèi)外研究現(xiàn)狀1.2.1國外研究現(xiàn)狀對(duì)于自平衡小車的相關(guān)項(xiàng)目的研究，國外的進(jìn)度是領(lǐng)先于國內(nèi)的。主要研究是一些歐美國家和日本，他們從倒立擺中吸取靈感，將倒立擺中的控制方法集成與自平衡小車中，在實(shí)際的研究中，研究人員為了能夠更好的去控制小車的平衡，運(yùn)用了很多的控制理論計(jì)算，比如智能控制算法，經(jīng)典PID算法等進(jìn)行研究。他們通過相關(guān)的研究在自平衡車及平衡相關(guān)領(lǐng)域取得了大量的創(chuàng)新性突破。1996年日本筑波大學(xué)最早研制出的的YamabicoKurara倒立擺結(jié)構(gòu)的二輪機(jī)器人如圖1-1所示，左右分別有一個(gè)獨(dú)立的車輪分開進(jìn)行控制，但是因?yàn)槭窃O(shè)備和相關(guān)算法的都很少，策略也很少。所以它只能夠在自己設(shè)定的軌道保持平衡的效果而不能有進(jìn)行自由移動(dòng)。圖1-1YamabicoKurara機(jī)器人20世紀(jì)初，美國著名大發(fā)明家迪恩卡門發(fā)明了一款讓世人震驚的自平衡輪椅小車segway，如圖1-2，產(chǎn)品一經(jīng)推出，得到了很多互聯(lián)網(wǎng)大佬的爭相投資，大佬們認(rèn)為它能夠去改變?nèi)藗儸F(xiàn)有的出行方式。Segway有多個(gè)子系統(tǒng)綜合而成，其中子系統(tǒng)有，中央處理系統(tǒng)、姿態(tài)檢測系統(tǒng)，速度檢測系統(tǒng)以及動(dòng)力系統(tǒng)。這些系統(tǒng)的數(shù)據(jù)全部傳入中央處理系統(tǒng)，然后通過相關(guān)的控制算法處理相關(guān)數(shù)據(jù)，然后將結(jié)果發(fā)送給車子的動(dòng)力總成系統(tǒng)，從而控制車子的前進(jìn)后退，轉(zhuǎn)彎等操作。圖1-2Segway平衡車2007年日本早稻田大學(xué)的度亮邊教授制作可平衡車NXTway二輪平衡車如圖1-3所示，這個(gè)車子由前車子的基礎(chǔ)功能上有了音樂播放功能，且此車比起前面的車了來說，他的穩(wěn)定性的小車的機(jī)械結(jié)構(gòu)更加的合理。圖1-3NXTway平衡車2013，日本本田公司發(fā)明了一款新型平衡車U3—X如右圖1-4所示，該車采用了當(dāng)時(shí)最新的傳感器和控制原理，小車不僅具有車子前進(jìn)后退的和轉(zhuǎn)彎能力還可以播放音樂，此車可以相比與前面的車子它可以讓人坐著且依舊可以保持很好的平衡度圖1-4U3-X模型圖1.2.2國內(nèi)研究現(xiàn)狀雖然我國在自平衡車的研究上，相對(duì)與歐美國家來說要晚一些但是，我國這幾年的發(fā)展是的自平衡的商業(yè)化遠(yuǎn)遠(yuǎn)高于國外，據(jù)媒體統(tǒng)計(jì)2019美國進(jìn)口我國的平衡車已經(jīng)達(dá)到了百萬的級(jí)別，現(xiàn)在平衡車商業(yè)發(fā)展基本可以說占了全球大部分份額。我國平衡車發(fā)展速度非常之快。2003年，中國科學(xué)院技術(shù)大學(xué)研制出了，有電機(jī)進(jìn)行控制的載人二輪自平衡小車，如圖1-5所示他的形狀和Segway有一些類似，但相比于前者，他加入了各種傳感器件，如陀螺儀，加速度計(jì)等來采集他的速度，角速度等相關(guān)的信號(hào)。在根據(jù)這些數(shù)據(jù)指制定相應(yīng)的控制方法，從而運(yùn)用PWM脈沖技術(shù)來調(diào)節(jié)電機(jī)的轉(zhuǎn)速從而能夠是車子在路面上達(dá)到平衡的效果。圖1-5中國科學(xué)技術(shù)大學(xué)平衡車2012年剛剛成立的納恩伯公司，推出了現(xiàn)在的小米平衡車一代其圖片如右圖1-6所示。他相比于Segway而言，沒有了把手人只需要站在平衡車上，車子會(huì)自動(dòng)檢測人體的姿態(tài)寵兒調(diào)節(jié)車子的速度從而達(dá)到平衡，并且它還可以通過手機(jī)app來進(jìn)行控制和查尋到車子的各個(gè)數(shù)據(jù)。這個(gè)與前面的平衡車相比有了很大的升級(jí)和改造，他也有兩個(gè)電機(jī)分別來進(jìn)行控制的，車內(nèi)還有加速度計(jì)和陀螺儀等傳感器來識(shí)別人的各種姿態(tài)，從而實(shí)現(xiàn)車子的加減速和轉(zhuǎn)彎等一系列的操作。而且現(xiàn)在的最新版的平衡車已經(jīng)有了防撞預(yù)警等一系列更加智能的操作，并且車子的續(xù)航等相較于以前也有了極大的提高，使得平衡車的真正解決了短距離綠色出行的難題。也是目前比較推崇的方案。圖1-6小米第一代平衡車1.2.3總結(jié)上面兩節(jié)對(duì)于國內(nèi)外兩輪自平衡車的發(fā)展分析，控制方法越來越多而我們學(xué)習(xí)了解后對(duì)其控制理論的學(xué)習(xí)也理解的更加深刻。而目前自平衡小車使用的最多控制算法的還是我現(xiàn)在學(xué)習(xí)和使用的PID經(jīng)典控制算法。當(dāng)然隨著發(fā)展還有模糊控制，神經(jīng)網(wǎng)絡(luò)的一系列的新的控制方法。當(dāng)時(shí)這些算法的主要思路都是先用相關(guān)的傳感器收集小車的各個(gè)姿態(tài)位置，速度，溫度等一系列的數(shù)據(jù)。這些傳感器主要有，陀螺儀，加速度器，紅外傳感器，超聲波傳感器等一系列元器件來獲得小車的各個(gè)數(shù)據(jù)，但是這些傳感器都有一定的誤差，我們需要去通過一定的濾波算法來減小它的誤差值，使其系統(tǒng)的誤差范圍在我們的設(shè)計(jì)可控的誤差區(qū)間內(nèi)。目前我們的設(shè)計(jì)都是基于平面設(shè)計(jì)而言的如果在斜面等一系列曲面上，就需要重新去分析設(shè)計(jì)相關(guān)的方案。1.3論文的組織結(jié)構(gòu)本文基于PID控制算法，是二輪小車能達(dá)到自平衡的效果，通過相關(guān)算法的提高小車的穩(wěn)定性，一些是本文的組織結(jié)構(gòu)。主要對(duì)小車的研究背景，國內(nèi)外的發(fā)展進(jìn)程進(jìn)行相關(guān)的分析和介紹，并對(duì)本文的研究目標(biāo)和研究內(nèi)容進(jìn)行一定的闡述。第1章緒論介紹了國內(nèi)外研究背景和小車研究的意義及其未來的價(jià)值。第2章介紹小車的一些硬件結(jié)構(gòu)，如OLED模塊，MPU-6050模塊、穩(wěn)壓模塊等功能模塊進(jìn)行向相關(guān)的介紹和模塊的使用方法。了解這些模塊將會(huì)為我們對(duì)小車的整體架構(gòu)有更好的把握，在后面編寫相關(guān)的程序和控制算法是能夠有更加深刻的理解。第3章介紹程序的設(shè)計(jì)過程并介紹相關(guān)的一些控制方法和一些濾波的算法，如PID控制算法、卡爾曼濾波算法等第4章介紹軟件部分設(shè)計(jì)流程，并說明本文具體設(shè)計(jì)思路和核心代碼的介紹。第5章對(duì)實(shí)驗(yàn)中所解決的問題和在實(shí)驗(yàn)中與到的問題進(jìn)行相關(guān)的分析和總結(jié)，并對(duì)未來智能車的相關(guān)發(fā)展表示一些展望。

2硬件部分的介紹與分析2.1硬件系統(tǒng)的組成小車系統(tǒng)硬件模塊由主控系統(tǒng)以下幾個(gè)模塊組成：STM32主控制器，6軸控制模塊（MPU-6050）、OLED模塊，電機(jī)驅(qū)動(dòng)模塊，穩(wěn)壓模塊等五個(gè)模塊組成。其中穩(wěn)壓模塊負(fù)責(zé)給所有的模塊進(jìn)行供電，其余模塊均設(shè)計(jì)為最小系統(tǒng)，并為控制器提供相關(guān)數(shù)據(jù)并保持系統(tǒng)的動(dòng)態(tài)平衡。如下表2-1所示表2-1硬件IO對(duì)照表單片機(jī)接口硬件PA4藍(lán)色Led燈PB8、PB9IIC接口PB3、PB4、PB5、PA15Oled接口PA9、PA10CH340接口PA5按鍵主要硬件模塊接口在介紹模塊時(shí)展示2.2主要電路模塊的說明介紹本段將介紹小車使用的主要幾個(gè)模塊，并為他們提供一定的使用說明。2.2.1電機(jī)驅(qū)動(dòng)模塊LV8731模塊是一個(gè)可以同時(shí)運(yùn)行四個(gè)電機(jī)的驅(qū)動(dòng)模塊其電路圖如圖2-1所示，它可以很好的去控制工作的電流從而控制小車的速度，下面是該模塊的一些使用說明。圖2-1LV8731模塊運(yùn)行電流調(diào)節(jié)

撥碼開關(guān)S4，S5，S6用來調(diào)節(jié)運(yùn)行電流，撥碼開關(guān)撥至ON表示1，反之表示0。當(dāng)S4=0，S5=0，S6=1，設(shè)定工作電流為0.3A;當(dāng)S4=0，S5=1，S6=0，設(shè)定工作電流為0.6A;當(dāng)S4=0，S5=1，S6=1，設(shè)定工作電流為0.86A;當(dāng)S4=1，S5=0，S6=0，設(shè)定工作電流為1.14A;當(dāng)S4=1，S5=0，S6=1，設(shè)定工作電流為1.43A;當(dāng)S4=1，S5=1，S6=0，設(shè)定工作電流為1.7A;當(dāng)S4=1，S5=1，S6=1，設(shè)定工作電流為2.0A;撥碼開關(guān)撥至ON表示1，反之表示0。保持電流調(diào)節(jié)撥碼開關(guān)S1用來調(diào)節(jié)保持電流，當(dāng)0.5秒內(nèi)無步進(jìn)脈沖輸入時(shí)，自動(dòng)降低流過電機(jī)電流，當(dāng)步進(jìn)脈沖再次到來，電機(jī)電流立即進(jìn)入工作電流模式.S1=0保持電流為工作電流的50％S1=1保持電流為工作電流的20％撥碼開關(guān)S1撥至1時(shí)為ON反之為OFF

撥碼開關(guān)S2，和S3用來選擇細(xì)分模式，共4種模式：細(xì)分選擇S2=0，S3=0，1細(xì)分（整步）；S2=1，S3=0，2細(xì)分；S2=0，S3=1，4細(xì)分；S2=1，S3=1，16細(xì)分；碼開關(guān)撥至ON表示1，反之表示0信號(hào)輸入端口CP+：步進(jìn)脈沖信號(hào)+極CP-：步進(jìn)脈沖信號(hào)-極EN+：使能信號(hào)+極EN-：使能信號(hào)-極DIR-：方向控制信號(hào)-極DIR-：方向控制信號(hào)-極注意：使能信號(hào)與另外兩個(gè)信號(hào)不相同，即使沒有信號(hào)輸入，使能驅(qū)動(dòng)器也可以進(jìn)入工作模式；一旦有信號(hào)輸入，驅(qū)動(dòng)器將馬上進(jìn)入休眠模式。上位機(jī)控制器的控制信號(hào)可以設(shè)置為高電平有效也可以設(shè)計(jì)為低電平有效。如果高電平有效時(shí)，所有控制信號(hào)的負(fù)極連在一起全部接地，反之就把所有控制信號(hào)的正極全部連在一起作為信號(hào)公共端?，F(xiàn)在以集電極開路為例子，其接口電路示意圖如圖2-2、2-3所示：圖2-2輸入接口電路共陽極接法圖2-3輸入接口電路共陰極接法1.有6個(gè)輸入端接口，有共陽或共陰兩種接法

2.輸入的脈沖電平正常為5V，如果是12V的電平就要串聯(lián)一個(gè)1KΩ電阻，若是24V電平則需要串聯(lián)一個(gè)2.4KΩ的電阻

3.CP有脈沖時(shí)電機(jī)轉(zhuǎn)動(dòng)，如果0.5秒內(nèi)CP沒有脈沖，則進(jìn)入電流保持狀態(tài)，當(dāng)CP脈沖再次傳來時(shí)，電機(jī)電流立即進(jìn)入工作電流模式

4.DIR方向控制信號(hào)為高電平，電機(jī)在步進(jìn)脈沖觸發(fā)下正轉(zhuǎn)，為低電平或懸空則為反轉(zhuǎn)

5.電源范圍為：8-32V

6.通電前要仔細(xì)檢測2.2.2STM32主控模塊主控模塊采用的主控芯片是STM32F103C8T6，其該模塊的最小系統(tǒng)如下圖2-4所示，這款單片機(jī)屬于M3內(nèi)核，其最高工作頻率為72MHZ，它能夠完成收集信號(hào)，處理信號(hào)和發(fā)送信號(hào)等多種功能。它的計(jì)算速度比較快、范圍廣和使用的時(shí)間很長。官方提供具體的使用說明和庫函數(shù)，網(wǎng)上有豐富的內(nèi)容解答。具體的硬件發(fā)面有如下需求他均能滿足。1.STM32具有12位的數(shù)模轉(zhuǎn)換器，其電壓轉(zhuǎn)換空間為0到3.6v，其轉(zhuǎn)換的時(shí)間僅為1us，它的最高的頻率可以達(dá)到1MHZ，完全滿足我們對(duì)于要采集電源電壓的要求。不需要我們重新再去設(shè)計(jì)AD轉(zhuǎn)換電路節(jié)省很多時(shí)間。2.STM32的主頻高達(dá)72MHZ遠(yuǎn)高于51單片機(jī)，使得它的運(yùn)算速度十分的快，且因?yàn)樗茿RM架構(gòu)所以它的功耗會(huì)較低，且ST公司提高了強(qiáng)大DMA功能，使得AD轉(zhuǎn)換以及串口通信的時(shí)間大大下降。3.STM32具有多個(gè)通道，對(duì)于串口的寄存器存取操作不需要經(jīng)運(yùn)算單元，使得操作起來相對(duì)簡單，且串口通信時(shí)不需要考慮CPU的時(shí)序問題。4.STM32提供了七個(gè)定時(shí)器，多個(gè)中斷系統(tǒng)。并且提供了多種通信方式,如SPI，IIC等，且就有4個(gè)PWM脈沖技術(shù)。圖2-4STMF103最小系統(tǒng)2.2.3MPU—6050（姿態(tài)檢測模塊）小車的平衡非常以為與小車的位置姿態(tài)，如果不知道位姿，那么平衡也就無從說起，所以說小車的位姿檢測傳感器時(shí)小車保持平衡的重要因素，也是小車設(shè)計(jì)過程中最重要的一個(gè)模塊，本文實(shí)驗(yàn)中檢測位姿就是檢測到小車的傾角和小車的加速度。要使小車保持平衡小車的傾角至關(guān)重要，這也表示我們必須要選取對(duì)于傾角相對(duì)來說比較敏感的傳感器。位姿檢測需要用到兩個(gè)傳感器分別是陀螺儀和加速度計(jì)我們先來看看他們的設(shè)計(jì)原理及區(qū)別。加速度計(jì)：主要測的時(shí)以平面為X，Y軸水平向上為Z軸，其中Y軸與小車保持平行，但是隨著小車的運(yùn)動(dòng)，其X軸和Z軸在不斷地發(fā)生變化，但是我們始終可以讀取到三個(gè)軸的加速度，而且我們已知小車平行時(shí)的角加速度，所以我們把問題轉(zhuǎn)化為一個(gè)求斜坡角度的一個(gè)物理模型，而這個(gè)模型就是我們高中做了無數(shù)次的小球求角度問題，那么我們可以直接用它做為陀螺儀測量它的角加速度呢，我設(shè)計(jì)了這個(gè)物理模型其圖形如圖2-5所示設(shè)向下的加速度恒為g，其斜坡處加速度為g1那么其角度就有公式就有。圖2-5斜坡物理模型g1=g*sin(α)（2-1）a=arcsin(g1/g)（2-2）這樣雖然可以求得小車的傾角，但是因?yàn)樗慕嵌仍诟咚俨煌５陌l(fā)生變化所以，它的姿勢在不停的抖動(dòng)所以它的加速度也在抖動(dòng)，那么實(shí)時(shí)處理需要很大的計(jì)算量而且數(shù)據(jù)也不好處理，對(duì)于小車保持平衡會(huì)有很大的不利之處，所以我認(rèn)為還是需要用的陀螺儀來準(zhǔn)確讀取小車的傾角。陀螺儀是時(shí)刻測量物體角速度的傳感器，即把陀螺儀放在水平面上小車經(jīng)過一段時(shí)間運(yùn)動(dòng)后小車便與水平面產(chǎn)生了一定的角度θ我們采集它在運(yùn)動(dòng)到一定位置過程中的三次角速度數(shù)據(jù)分別命名為ω1、ω2、ω3+取它們的對(duì)應(yīng)時(shí)間分別為t1、t2、t3。納那么我就可以算出他在運(yùn)動(dòng)中總的傾角θ。即θ=ω1*t1+ω2*t2+ω3*t3·（2-3）那么我們就可以通過這個(gè)公式得到車身的傾角，但是我們通過數(shù)學(xué)公式可以很好的看到這個(gè)公式在不停的經(jīng)行積分的過程而我們每一次讀取的數(shù)據(jù)都是有著一定的誤差的。所以隨著不停的積分那么每個(gè)數(shù)據(jù)中的誤差也在不停的進(jìn)行積分從而導(dǎo)致系統(tǒng)的誤差會(huì)越來越大，最后的結(jié)果表現(xiàn)為數(shù)據(jù)會(huì)有嚴(yán)重的失真行為，即我們只能夠去取它短期的數(shù)據(jù)。剛剛介紹了兩種傳感器的原理和他們?cè)跍y量角度時(shí)都有著各自的誤差，所以我們?cè)趯?shí)驗(yàn)中就需要去考慮制作相關(guān)的算法取得的平衡。MPU-6050：在文獻(xiàn)中我們發(fā)現(xiàn)MPU-6050完美的平衡了相關(guān)的算法，它是一個(gè)6軸的傳感器，其電路圖和原理圖如圖2-6、2-7所示，集成了陀螺儀和加速度計(jì)且精度和速度的表現(xiàn)相對(duì)都表現(xiàn)得比較優(yōu)秀。許多電子電子設(shè)備都有著它的身影，如我們現(xiàn)在做的平衡小車，四輪無人機(jī)等，它由一個(gè)三軸MEMS陀螺儀，一個(gè)三軸MEMS加速度計(jì)和一個(gè)數(shù)字融合處理引擎組成，其數(shù)據(jù)通過I2C接口進(jìn)行傳輸操作簡單。它的數(shù)據(jù)傳輸能力最高可以到達(dá)400Khz且工作電壓為2.6～3.3v之間都可以正常運(yùn)行。圖2-6MPU-6050實(shí)物圖（左）圖2-7MPU-6050電路圖（右）2.2.4電源開關(guān)管理芯片我們選取的穩(wěn)電源開關(guān)管理片為LM2596T其電路內(nèi)部結(jié)構(gòu)圖和電路圖如圖2-8、2-9所示，LM2596系列是由德州儀器（TI）生產(chǎn)的標(biāo)準(zhǔn)為電流3A的輸出降壓開關(guān)型集成穩(wěn)壓芯片，它自己帶有固定頻率振蕩器（150KHZ）和基準(zhǔn)穩(wěn)壓器（1.23v），并具有十分完善的電路保護(hù)、電流限制、過熱自動(dòng)關(guān)斷電路等。利用該器件只需要在使用極少的外圍器件就可以構(gòu)成非常高效的穩(wěn)壓電路。該芯片提供了工作狀態(tài)下的外部控制引腳。其主要特性如下：1、輸出電壓：3.3V、5V、12V及（ADJ）等，最大輸出電壓37V2、工作模式：支持低功耗運(yùn)行和正常運(yùn)行兩種模式?？赏ㄟ^外部接口自行控制3、工作模式控制：TTL電平相容4、所需外部組件：僅四個(gè)（不可調(diào)）；六個(gè)（可調(diào)）5、器件保護(hù)：過熱自動(dòng)關(guān)以及電流保護(hù)6、封裝形式：5腳（TO-220(T)；TO-263(S))圖2-8LM2596T內(nèi)部電路結(jié)構(gòu)圖圖2-9LM2596T電路圖3相關(guān)算法理論3.1PID算法PID算法是平衡小車保持平衡的核心算法整個(gè)小車的控制思路都是為他展開十分重要，PID中P是比例環(huán)節(jié)、I是積分環(huán)節(jié)、D是微分環(huán)節(jié)線性組合的總體。其控制流程原理圖如圖3-1所示.輸入量通過PID控制器作用于被控對(duì)象，然后檢測被控對(duì)象的輸出值并把輸出值與設(shè)定的目標(biāo)值進(jìn)行比較從而形成閉環(huán)負(fù)反饋系統(tǒng)。圖3-1PID控制系統(tǒng)原理圖在PID控制系統(tǒng)中e（t）是偏差值最后實(shí)際值與實(shí)際輸出值y（t）的差值，其數(shù)學(xué)表達(dá)式為ut=Kpe公式中Kp是比例系數(shù)，T1是時(shí)間積分常數(shù)，TD是時(shí)間微分常數(shù)。由此可見PID控制是有三方面分別進(jìn)行矯正的。比例部分就是通過檢測偏差值，并進(jìn)行相關(guān)的控制來減少偏差值，積分環(huán)節(jié)主要作用是去除靜態(tài)誤差，而微分部分就對(duì)超前校正即提前加入一個(gè)矯正信號(hào)，從而加快系統(tǒng)調(diào)節(jié)速度。上述公式所描述為連續(xù)型的函數(shù)，但是在實(shí)際使用中都是離散型的數(shù)據(jù)。下面我介紹兩種離散型的PID控制方法。3.1.1位置型PID算法在這個(gè)方法中把時(shí)間常數(shù)用采樣時(shí)刻序列來進(jìn)行代替，把微分用求和來進(jìn)行代替，把積分用向后差分來代替，在采樣頻率足夠高時(shí)（即采樣時(shí)間很短），即可得到離散化后的數(shù)學(xué)公式：uK=Kpek+T3.1.2增量式PID算法增量式PID是指數(shù)字控制器的輸出只是控制量的增量Δu(k)。采用增量式算法時(shí)，計(jì)算機(jī)輸出的控制量Δu(k)對(duì)應(yīng)的是本次執(zhí)行機(jī)構(gòu)位置的增量，而不是對(duì)應(yīng)執(zhí)行機(jī)構(gòu)的實(shí)際位置，因此要求執(zhí)行機(jī)構(gòu)必須具有對(duì)控制量增量的累積功能，才能完成對(duì)被控對(duì)象的控制操作。執(zhí)行機(jī)構(gòu)的累積功能可以采用硬件的方法實(shí)現(xiàn)；也可以采用軟件來實(shí)現(xiàn)，如利用算式

u(k)=u(k-1)+Δu(k)程序化來完成。Δu(k)=KP[e(k)-e(k-1)]+k1e(k)+KD[e(k)-2e(k-1)+e(k-2)](3-3)兩者的區(qū)別：（1）位置式PID控制的輸出與整個(gè)過去的狀態(tài)有關(guān)，用到了誤差的累加值;而增量式PID的輸出只與當(dāng)前狀態(tài)和前兩次狀態(tài)的誤差有關(guān)，因此位置式PID控制的累積誤差相對(duì)來說更大;（2）增量式PID控制輸出的是控制量增量，沒有積分作用，因此該方法適用于執(zhí)行機(jī)構(gòu)帶積分部件的對(duì)象，如步進(jìn)電機(jī)等，但位置式PID適用于執(zhí)行機(jī)構(gòu)不帶積分部件的對(duì)象，如電液伺服閥。（3）由于增量式PID輸出的是控制量增量，如果計(jì)算機(jī)出現(xiàn)故障，其誤動(dòng)作影響較小且執(zhí)行機(jī)構(gòu)本身有記憶功能恢復(fù)后可保持原位不變，不會(huì)嚴(yán)重影響系統(tǒng)的工作，但位置式的輸出與對(duì)應(yīng)對(duì)象的輸出有著直接關(guān)系，因此對(duì)系統(tǒng)影響比較大?；谝陨先c(diǎn)情況我們?cè)谑褂肞ID算法的一般偏向于使用增量型PID算法。3.2Kalman濾波簡介濾波的含義是傳感器除了產(chǎn)生我所需要的信號(hào)外還有含有別的雜波信號(hào)但是這些雜波信號(hào)不是我所需要的所以我們需要去除那些雜波信號(hào)這個(gè)過程就叫做濾波。通常來說濾波指的是將不需要的信號(hào)通過一定的硬件或者軟件算法進(jìn)行過濾去除，留下或放大我自己所需要的目標(biāo)信號(hào)。雜波信號(hào)也可以稱作為稱作噪聲，它對(duì)于整個(gè)系統(tǒng)有著一定干擾作用使得系統(tǒng)偏離了自己的預(yù)期。濾波理論建立的基礎(chǔ)是測量得到系統(tǒng)的可以觀測的信號(hào)，然后種用某種方法對(duì)狀態(tài)進(jìn)行估計(jì)計(jì)算。這種濾波算法的應(yīng)用十分廣泛，在理論研究上卡爾曼濾波是建立在系統(tǒng)是線性的以及系統(tǒng)中的狀態(tài)變量和噪聲干擾都是呈高斯分布的45。但是在實(shí)際生活中要滿足這兩個(gè)條件非常的困難，這也因此限制了它在現(xiàn)實(shí)生活中的應(yīng)用。為了解決它的這兩現(xiàn)在，

研究人員提出了擴(kuò)展卡爾曼濾波（EKF）這一思想方法。EKF的思想方法是這樣的，假設(shè)卡爾曼濾波對(duì)系統(tǒng)當(dāng)前的值估計(jì)的誤差非常小和真實(shí)值比較接近。那么我們可以對(duì)非線性系統(tǒng)的當(dāng)前的估計(jì)值處進(jìn)行泰勒展開實(shí)現(xiàn)局部線性化。但是泰勒展開線性化本身就有一定的誤差從而導(dǎo)致EKF存在一定誤差?？柭鼮V波在本質(zhì)上利用不斷遞推運(yùn)算，處理數(shù)據(jù)的原則是使誤差的均方值為最小，根據(jù)前一組數(shù)據(jù)的估計(jì)值和觀察數(shù)據(jù)的估計(jì)值來估計(jì)當(dāng)前采集信息。卡爾曼濾波算法是優(yōu)點(diǎn)非常突出:1）在使用卡爾曼濾波時(shí)，我們對(duì)觀測數(shù)據(jù)個(gè)數(shù)的要求不高，有限的觀測數(shù)據(jù)便可以進(jìn)行相關(guān)遞推運(yùn)算。2)卡爾曼濾波算法中的時(shí)間域語言是非常直觀的，在算法編寫方面它的物理意義表述的十分清楚便于理解。3)卡爾曼濾波運(yùn)算中的中間變量數(shù)目相對(duì)于其他濾波算法來說要少，這也便于計(jì)算機(jī)進(jìn)行處理，加快了計(jì)算機(jī)處理的速度。4軟件設(shè)計(jì)部分4.1兩輪自平衡小車主程序設(shè)計(jì)兩輪自平衡小車的主程序設(shè)計(jì)就是根據(jù)相關(guān)的模塊，編寫系統(tǒng)的總體程序框架。以主程序?yàn)橹黧w各個(gè)子程序按照其功能依次被調(diào)用，其程序流程圖如下圖4-1所示，首先初始化各個(gè)模塊聲明，自定義聲明，系統(tǒng)時(shí)鐘的聲明等，在所有的初始化完成之后，就可以通過編碼采集小車的速度數(shù)據(jù)和角度的數(shù)據(jù)并對(duì)數(shù)據(jù)進(jìn)行相關(guān)的濾波處理從而減小數(shù)據(jù)的誤差或者錯(cuò)誤。然后進(jìn)行PID控制通過PWM來控制電機(jī)的轉(zhuǎn)速從而調(diào)節(jié)小車的速度使小車達(dá)到保持平衡。在這個(gè)整個(gè)控制流程中我使用的主控芯片是STM32f103C8T6，位姿檢測模塊為MPU-6050，兩者通過I2C進(jìn)行通信傳輸數(shù)據(jù)，主控芯片從而獲得小車的車身傾角和小車的加速度，而小車的實(shí)時(shí)車速則是通過編碼器采集到的信息轉(zhuǎn)化為小車的車速來進(jìn)行功能實(shí)現(xiàn)的，左右電機(jī)分別都有一個(gè)編碼器可以獨(dú)立讀取單個(gè)電機(jī)的速度從而實(shí)現(xiàn)小車的平衡。圖4-1自平衡小車主控制程序流4.2介紹所使用的軟件本次畢業(yè)設(shè)計(jì)中軟件部分程序編寫和編譯用到的是keil5軟件，下載所用的軟件名為Fly。Keil軟件平臺(tái)是非常強(qiáng)大的微型控制器軟件編譯平臺(tái)，從我們以前學(xué)的51單片機(jī)和現(xiàn)在STM32單片機(jī)都是在這上面進(jìn)行的編程，它有著很強(qiáng)大的功能，它把程序編寫、編譯、調(diào)試、下載集成為一體。相比于其他軟件操作起來更加簡單，伸手廣大師生及業(yè)界相關(guān)工作者的喜愛和歡迎。并且它還有一下幾大特征。1.源代碼編輯器功能很多十分的齊全且還有代碼語法實(shí)時(shí)檢測功能十分的方便和省心。2.該軟件使用人數(shù)較多在遇到很多不懂的軟件錯(cuò)誤時(shí)，可以快速的找到相關(guān)的解決方法。3.該軟件有官方提供的入門教程指導(dǎo)，相對(duì)其他軟件來說上手更加的方便。4.該軟件平臺(tái)可以創(chuàng)建工程使得文件可以更好的閱讀和維護(hù)我們已經(jīng)寫好的工程文件。4.3PID控制算法在這次實(shí)驗(yàn)中實(shí)驗(yàn)了速度環(huán)和控制環(huán)即PD算法和PI算法speed=Mean_Filter(velocity_left,velocity_right); Encoder*=0.7; Encoder+=speed*0.3; Encoder_Integral+=Encoder; if(Encoder_Integral>320000) Encoder_Integral=320000; if(Encoder_Integral<-320000) Encoder_Integral=-320000; Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/100;這個(gè)時(shí)PI控制器的核心算法，我先讀取我們?yōu)V波完后的速度，然后對(duì)取的速度再進(jìn)行一次低通濾波保證速度的準(zhǔn)確性，然后進(jìn)行在進(jìn)行積分得到積分值，并給積分值設(shè)置一個(gè)幅值。我再根據(jù)使用位置式PI算法，其公式為u(t)=Kpe(k)+KiTe(k)dt（4-1）可得Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/100;其中KP和KI為比例系數(shù)而小車原來使用的是增量型的PID算法，他相對(duì)來說考慮的更加的周全它的偏差相對(duì)來說更小，但是我也在上面經(jīng)過兩次濾波也減小了其誤差發(fā)生的概率。因?yàn)槲矣玫氖俏恢眯退惴?，所以其濾波是我才采用了一階滯后濾波的方法來進(jìn)行調(diào)節(jié)。不會(huì)使得系統(tǒng)過于滯后。PD算法因?yàn)橛行﹨?shù)與結(jié)構(gòu)有關(guān)就沒有進(jìn)行多加修改使用的是它的源代碼。4.4濾波算法對(duì)速度的濾波我采用的是中位值平均濾波法，它的核心是兼顧了“中位值濾波法”+“算術(shù)平均濾波法”兩種濾波法的優(yōu)點(diǎn)，而缺點(diǎn)僅僅是計(jì)算量會(huì)大一點(diǎn)。相對(duì)于小車自帶的平均值法來說準(zhǔn)確性更好也顯得更加平滑，核心代碼如下所示。s32Sum_Speed=0;s16Filter_Speed;statics16Speed_Buff[FILTERING_TIMES]={0};//存儲(chǔ)前面十個(gè)速度值for(i=1;i<FILTERING_TIMES;i++){Speed_Buff[i-1]=Speed_Buff[i];}Speed_Buff[FILTERING_TIMES-1]=moto1+moto2;for(i=0;i<FILTERING_TIMES;i++){Sum_Speed+=Speed_Buf[i];}Filter_Speed=(s16)(Sum_Speed/FILTERING_TIMES);4.5數(shù)據(jù)融合算法4.5.1卡爾曼濾波MPU-6050的噪聲非常的大，所以我需要進(jìn)行濾波處理才能去讀取數(shù)據(jù)，我參考了小車的源代碼發(fā)現(xiàn)，它使用的是DMP算法他的方法相對(duì)獨(dú)特，我取網(wǎng)上查找了相關(guān)資料發(fā)現(xiàn)網(wǎng)上對(duì)于MPU-6050都是使用的Kalman濾波算法，于是我決定使用卡爾曼濾波算法，在經(jīng)過一系列的調(diào)試后小車能夠讀取到傳感器的數(shù)據(jù)但是相比于它的算法我的小車在直立運(yùn)行的穩(wěn)定性和小車從不平衡到平衡所需的時(shí)間更長。不過這種算法還是可以使用的，后面還要進(jìn)行一定能的優(yōu)化我會(huì)在后在接著進(jìn)行相關(guān)的調(diào)試。Kalman代碼如圖16所示。voidKalman_Filter(floatAccel,floatGyro) {angle+=(Gyro-q_bias)*dt;//先驗(yàn)估計(jì)Pdot[0]=Q_angle-PP[0][1]-PP[1][0];//Pdot[1]=-PP[1][1];//PK-先驗(yàn)估計(jì)誤差協(xié)方差的微分Pdot[2]=-PP[1][1];Pdot[3]=Q_gyro;PP[0][0]=PP[0][0]+Pdot[0]*dt;//PK-先驗(yàn)估計(jì)誤差協(xié)方差的微分的積分PP[0][1]=PP[0][1]+Pdot[1]*dt;//先驗(yàn)估計(jì)誤差協(xié)方差PP[1][0]=PP[1][0]+Pdot[2]*dt;PP[1][1]=PP[1][1]+Pdot[3]*dt;Angle_err=Accel-angle;//zk-先驗(yàn)估計(jì) PCt_0=C_0*PP[0][0];PCt_1=C_0*PP[1][0];E=R_angle+C_0*PCt_0K_0=PCt_0/E;K_1=PCt_1/Et_0=PCt_0;t_1=C_0*PP[0][1];PP[0][0]=PP[0][0]-K_0*t_0; //后驗(yàn)估計(jì)誤差協(xié)方差PP[0][1]=PP[0][1]-K_0*t_1;PP[1][0]=PP[1][0]-K_1*t_0;PP[1][1]=PP[1][1]-K_1*t_1; angle+=K_0*Angle_err; //后驗(yàn)估計(jì)q_bias+=K_1*Angle_err; angle=Gyro-q_bias; //輸出值（后驗(yàn)估計(jì)）的微分等于角速度}4.5.2一階互補(bǔ)濾波一階互補(bǔ)濾波說的是如果我們?cè)跍y量兩個(gè)量時(shí)，一個(gè)量是高頻雜波信號(hào)另一個(gè)量是低頻雜波信號(hào)那么我們就可以設(shè)計(jì)一個(gè)高通濾波器和一個(gè)低通濾波器然后對(duì)這兩個(gè)濾波器進(jìn)行加權(quán)然后進(jìn)行求和處理;Y=αx1+(1-α)x2(1)(4-1)MPU6050中的陀螺儀和加速度計(jì)剛好一個(gè)是含有低頻雜波信號(hào)一個(gè)是含有高頻雜波信號(hào)所以剛好可以使用一階互補(bǔ)濾波算法來讀取角速度和傾角。一階互補(bǔ)濾波算法的代碼為：angle=K1*angle_m+(1-K1)*(angle+gyro_m*0.005);其中K1為加權(quán)系數(shù)。這種算法相比于卡爾曼濾波來說小車的不穩(wěn)定性更大，保持平衡相對(duì)來說更加的困難，但是也可以保持平衡。4.5.2DMP（數(shù)字運(yùn)動(dòng)處理器）MPU6050本身就一個(gè)數(shù)字運(yùn)動(dòng)處理器DMP，它可以直接輸出小車的姿態(tài)解數(shù)據(jù)而不需要我們自己再去求取小車的姿態(tài)解，它的算法是用的四元法，誤差相對(duì)更小、計(jì)算量也減小許多而且官方提供相應(yīng)的庫文件以及教程從而大大降低了開發(fā)的難度。我們只需要在進(jìn)行調(diào)用即可。其核心代碼如下所示longquat[4];dmp_read_fifo(gyro,accel,quat,&sensor_timestamp,&sensors,&more); if(sensors&INV_WXYZ_QUAT){q0=quat[0]/q30;q1=quat[1]/q30;q2=quat[2]/q30;q3=quat[3]/q30;Pitch=arcsin(2*q0*q2-2*q1*q3)*57.3; Roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3;4.5兩輪自平衡車的實(shí)物展示與測試圖2-2為小車的側(cè)面圖，圖2-3為小車保持自立的正面圖。小車第一層有下車的主控芯片STM32，OLED液晶顯示屏，MPU-6050芯片，第二層為小車的電機(jī)驅(qū)動(dòng)電路，TTL轉(zhuǎn)換電路的一系列電路設(shè)計(jì)模塊和導(dǎo)熱版進(jìn)行導(dǎo)熱。電池放在下車的最下面可以增加小的穩(wěn)定增加地盤重量使得小車機(jī)械結(jié)構(gòu)更加的穩(wěn)定。圖2-2小車側(cè)面圖圖2-3小車保持直立時(shí)正面圖我在燒錄程序后將小車放在水平地面上觀察小車狀況發(fā)現(xiàn)小車存在些許抖動(dòng)的情況，但是能夠保持穩(wěn)定，然后我輕輕推動(dòng)小車后仍然能夠保持平衡，實(shí)驗(yàn)結(jié)果基本能夠達(dá)到開題時(shí)的要求，但是小車在運(yùn)動(dòng)時(shí)會(huì)有嚴(yán)重的發(fā)熱現(xiàn)象，我嘗試使用小車自帶源代碼進(jìn)行測試小車發(fā)熱也比較嚴(yán)重，我認(rèn)為這是小車的硬件設(shè)計(jì)結(jié)構(gòu)的問題暫時(shí)無法解決所以小車運(yùn)行時(shí)間不能過長，大概一分鐘左右小車導(dǎo)熱板就感到燙手，且電路板溫度較高。但對(duì)對(duì)小車平衡沒有太大影響，我分析我同學(xué)小車應(yīng)該該原因出現(xiàn)損壞。4.6本章小結(jié)本章首先介紹了主程序設(shè)計(jì)流程圖并介紹相關(guān)的運(yùn)行流程，其次介紹了作者在小車使用中的一些控制算法和多種濾波算法并貼出相關(guān)的源代碼進(jìn)行講解和分析各種算法對(duì)小車穩(wěn)定性的影響，最后我拍攝了小車的實(shí)物照片和小車保持直立狀態(tài)的照片。驗(yàn)證了這些算法是可以在實(shí)際中是可靠的可以運(yùn)行的。最后感謝我的父母對(duì)于我的支持。5總結(jié)與展望5.1總結(jié)解決我們最后一公里路程一直是人們