兩輪自平衡小車控制系統(tǒng)的硬件和軟件設(shè)計案例目錄TOC\o"1-3"\h\u24138兩輪自平衡小車控制系統(tǒng)的硬件和軟件設(shè)計案例 1290591研究背景 147982硬件部分的介紹與分析 2212602.1硬件系統(tǒng)的組成 2184502.2主要電路模塊的說明介紹 285422.2.1電機驅(qū)動模塊 276332.2.2STM32主控模塊 6151382.2.3MPU—6050（姿態(tài)檢測模塊） 7266572.2.4電源開關(guān)管理芯片 9234263相關(guān)算法理論 10140023.1PID算法 10224833.1.1位置型PID算法 1192933.1.2增量式PID算法 1160113.2Kalman濾波簡介 12152494軟件設(shè)計部分 13354.1兩輪自平衡小車主程序設(shè)計 13253114.2介紹所使用的軟件 1483184.3PID控制算法 1542464.4濾波算法 15302424.5數(shù)據(jù)融合算法 16209814.5.1卡爾曼濾波 16202934.5.2一階互補濾波 18170044.5.2DMP（數(shù)字運動處理器） 1839394.5兩輪自平衡車的實物展示與測試 191研究背景機器人技術(shù)是一門多學(xué)科交叉融合的多學(xué)科應(yīng)用型的技術(shù)，在幾千年前富有智慧的先民們就已經(jīng)創(chuàng)造了許多的自動更具和一些簡單記得自動機械。例如我國古代著名的一些名作有，春秋后期著名的木匠魯班發(fā)明的木鳥，東漢時期我國著名的天文學(xué)家發(fā)明的計里鼓車，三國時期諸葛亮為運糧草發(fā)明的木牛馬車等。但是現(xiàn)代機器人的真正發(fā)展要追溯與19世紀(jì)60年代美國Unimate公司發(fā)明的世界上第一臺商用的工業(yè)機器人，因此國際上也稱這一年為“機器人元年“。而到如今隨著計算機技術(shù)，電子半導(dǎo)體相關(guān)技術(shù)、力學(xué)、機械學(xué)等的相關(guān)學(xué)科的發(fā)展，從而間接推動者機器人相關(guān)學(xué)科也在快速的發(fā)展，這也使得相關(guān)機器人技術(shù)日益成熟，對人們的生活和工業(yè)的發(fā)展都產(chǎn)生了巨大的意義，它讓工廠的效率得到了極大的提高讓人們從枯燥的重復(fù)運動中解放出來。移動型機器人是整個機器人大家族中的一員，也是目前應(yīng)用比較廣泛的一種，商業(yè)價值極高，例如小米平衡車、京東物流倉庫的物流自動搬運車等。本文的研究的小車便是移動型機器人的一種。隨著現(xiàn)在得到污染問題的日益嚴(yán)重，以及國家現(xiàn)在大力推動新能源汽車。自平衡小車將會是一種很好的解決人們短途出行的方案。自平衡小車是通過兩個車輪來讓車子保持直立的狀態(tài)，同時每個車輪各自配有一個電機，從而可以通過控制兩個電機的速度從而實現(xiàn)轉(zhuǎn)彎，通過控制電機的正反轉(zhuǎn)從而控制車子的前進(jìn)和后退等，它能夠適合各種條件狀況不同的道路。2硬件部分的介紹與分析2.1硬件系統(tǒng)的組成小車系統(tǒng)硬件模塊由主控系統(tǒng)以下幾個模塊組成：STM32主控制器，6軸控制模塊（MPU-6050）、OLED模塊，電機驅(qū)動模塊，穩(wěn)壓模塊等五個模塊組成。其中穩(wěn)壓模塊負(fù)責(zé)給所有的模塊進(jìn)行供電，其余模塊均設(shè)計為最小系統(tǒng)，并為控制器提供相關(guān)數(shù)據(jù)并保持系統(tǒng)的動態(tài)平衡。如下表2-1所示表2-1硬件IO對照表單片機接口硬件PA4藍(lán)色Led燈PB8、PB9IIC接口PB3、PB4、PB5、PA15Oled接口PA9、PA10CH340接口PA5按鍵主要硬件模塊接口在介紹模塊時展示2.2主要電路模塊的說明介紹本段將介紹小車使用的主要幾個模塊，并為他們提供一定的使用說明。2.2.1電機驅(qū)動模塊LV8731模塊是一個可以同時運行四個電機的驅(qū)動模塊其電路圖如圖2-1所示，它可以很好的去控制工作的電流從而控制小車的速度，下面是該模塊的一些使用說明。圖2-1LV8731模塊運行電流調(diào)節(jié)

撥碼開關(guān)S4，S5，S6用來調(diào)節(jié)運行電流，撥碼開關(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)脈沖輸入時，自動降低流過電機電流，當(dāng)步進(jìn)脈沖再次到來，電機電流立即進(jìn)入工作電流模式.S1=0保持電流為工作電流的50％S1=1保持電流為工作電流的20％撥碼開關(guān)S1撥至1時為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信號輸入端口CP+：步進(jìn)脈沖信號+極CP-：步進(jìn)脈沖信號-極EN+：使能信號+極EN-：使能信號-極DIR-：方向控制信號-極DIR-：方向控制信號-極注意：使能信號與另外兩個信號不相同，即使沒有信號輸入，使能驅(qū)動器也可以進(jìn)入工作模式；一旦有信號輸入，驅(qū)動器將馬上進(jìn)入休眠模式。上位機控制器的控制信號可以設(shè)置為高電平有效也可以設(shè)計為低電平有效。如果高電平有效時，所有控制信號的負(fù)極連在一起全部接地，反之就把所有控制信號的正極全部連在一起作為信號公共端?，F(xiàn)在以集電極開路為例子，其接口電路示意圖如圖2-2、2-3所示：圖2-2輸入接口電路共陽極接法圖2-3輸入接口電路共陰極接法1.有6個輸入端接口，有共陽或共陰兩種接法

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

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

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

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

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

研究人員提出了擴展卡爾曼濾波（EKF）這一思想方法。EKF的思想方法是這樣的，假設(shè)卡爾曼濾波對系統(tǒng)當(dāng)前的值估計的誤差非常小和真實值比較接近。那么我們可以對非線性系統(tǒng)的當(dāng)前的估計值處進(jìn)行泰勒展開實現(xiàn)局部線性化。但是泰勒展開線性化本身就有一定的誤差從而導(dǎo)致EKF存在一定誤差?？柭鼮V波在本質(zhì)上利用不斷遞推運算，處理數(shù)據(jù)的原則是使誤差的均方值為最小，根據(jù)前一組數(shù)據(jù)的估計值和觀察數(shù)據(jù)的估計值來估計當(dāng)前采集信息?？柭鼮V波算法是優(yōu)點非常突出:1）在使用卡爾曼濾波時，我們對觀測數(shù)據(jù)個數(shù)的要求不高，有限的觀測數(shù)據(jù)便可以進(jìn)行相關(guān)遞推運算。2)卡爾曼濾波算法中的時間域語言是非常直觀的，在算法編寫方面它的物理意義表述的十分清楚便于理解。3)卡爾曼濾波運算中的中間變量數(shù)目相對于其他濾波算法來說要少，這也便于計算機進(jìn)行處理，加快了計算機處理的速度。4軟件設(shè)計部分4.1兩輪自平衡小車主程序設(shè)計兩輪自平衡小車的主程序設(shè)計就是根據(jù)相關(guān)的模塊，編寫系統(tǒng)的總體程序框架。以主程序為主體各個子程序按照其功能依次被調(diào)用，其程序流程圖如下圖4-1所示，首先初始化各個模塊聲明，自定義聲明，系統(tǒng)時鐘的聲明等，在所有的初始化完成之后，就可以通過編碼采集小車的速度數(shù)據(jù)和角度的數(shù)據(jù)并對數(shù)據(jù)進(jìn)行相關(guān)的濾波處理從而減小數(shù)據(jù)的誤差或者錯誤。然后進(jìn)行PID控制通過PWM來控制電機的轉(zhuǎn)速從而調(diào)節(jié)小車的速度使小車達(dá)到保持平衡。在這個整個控制流程中我使用的主控芯片是STM32f103C8T6，位姿檢測模塊為MPU-6050，兩者通過I2C進(jìn)行通信傳輸數(shù)據(jù)，主控芯片從而獲得小車的車身傾角和小車的加速度，而小車的實時車速則是通過編碼器采集到的信息轉(zhuǎn)化為小車的車速來進(jìn)行功能實現(xiàn)的，左右電機分別都有一個編碼器可以獨立讀取單個電機的速度從而實現(xiàn)小車的平衡。圖4-1自平衡小車主控制程序流4.2介紹所使用的軟件本次畢業(yè)設(shè)計中軟件部分程序編寫和編譯用到的是keil5軟件，下載所用的軟件名為Fly。Keil軟件平臺是非常強大的微型控制器軟件編譯平臺，從我們以前學(xué)的51單片機和現(xiàn)在STM32單片機都是在這上面進(jìn)行的編程，它有著很強大的功能，它把程序編寫、編譯、調(diào)試、下載集成為一體。相比于其他軟件操作起來更加簡單，伸手廣大師生及業(yè)界相關(guān)工作者的喜愛和歡迎。并且它還有一下幾大特征。1.源代碼編輯器功能很多十分的齊全且還有代碼語法實時檢測功能十分的方便和省心。2.該軟件使用人數(shù)較多在遇到很多不懂的軟件錯誤時，可以快速的找到相關(guān)的解決方法。3.該軟件有官方提供的入門教程指導(dǎo)，相對其他軟件來說上手更加的方便。4.該軟件平臺可以創(chuàng)建工程使得文件可以更好的閱讀和維護(hù)我們已經(jīng)寫好的工程文件。4.3PID控制算法在這次實驗中實驗了速度環(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;這個時PI控制器的核心算法，我先讀取我們?yōu)V波完后的速度，然后對取的速度再進(jìn)行一次低通濾波保證速度的準(zhǔn)確性，然后進(jìn)行在進(jìn)行積分得到積分值，并給積分值設(shè)置一個幅值。我再根據(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算法，他相對來說考慮的更加的周全它的偏差相對來說更小，但是我也在上面經(jīng)過兩次濾波也減小了其誤差發(fā)生的概率。因為我用的是位置型算法，所以其濾波是我才采用了一階滯后濾波的方法來進(jìn)行調(diào)節(jié)。不會使得系統(tǒng)過于滯后。PD算法因為有些參數(shù)與結(jié)構(gòu)有關(guān)就沒有進(jìn)行多加修改使用的是它的源代碼。4.4濾波算法對速度的濾波我采用的是中位值平均濾波法，它的核心是兼顧了“中位值濾波法”+“算術(shù)平均濾波法”兩種濾波法的優(yōu)點，而缺點僅僅是計算量會大一點。相對于小車自帶的平均值法來說準(zhǔn)確性更好也顯得更加平滑，核心代碼如下所示。s32Sum_Speed=0;s16Filter_Speed;statics16Speed_Buff[FILTERING_TIMES]={0};//存儲前面十個速度值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算法他的方法相對獨特，我取網(wǎng)上查找了相關(guān)資料發(fā)現(xiàn)網(wǎng)上對于MPU-6050都是使用的Kalman濾波算法，于是我決定使用卡爾曼濾波算法，在經(jīng)過一系列的調(diào)試后小車能夠讀取到傳感器的數(shù)據(jù)但是相比于它的算法我的小車在直立運行的穩(wěn)定性和小車從不平衡到平衡所需的時間更長。不過這種算法還是可以使用的，后面還要進(jìn)行一定能的優(yōu)化我會在后在接著進(jìn)行相關(guān)的調(diào)試。Kalman代碼如圖16所示。voidKalman_Filter(floatAccel,floatGyro) {angle+=(Gyro-q_bias)*dt;//先驗估計Pdot[0]=Q_angle-PP[0][1]-PP[1][0];//Pdot[1]=-PP[1][1];//PK-先驗估計誤差協(xié)方差的微分Pdot[2]=-PP[1][1];Pdot[3]=Q_gyro;PP[0][0]=PP[0][0]+Pdot[0]*dt;//PK-先驗估計誤差協(xié)方差的微分的積分PP[0][1]=PP[0][1]+Pdot[1]*dt;//先驗估計誤差協(xié)方差PP[1][0]=PP[1][0]+Pdot[2]*dt;PP[1][1]=PP[1][1]+Pdot[3]*dt;Angle_err=Accel-angle;//zk-先驗估計 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; //后驗估計誤差協(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; //后驗估計q_bias+=K_1*Angle_err; angle=Gyro-q_bias; //輸出值（后驗估計）的微分等于角速度}4.5.2一階互補濾波一階互補濾波說的是如果我們在測量兩個量時，一個量是高頻雜波信號另一個量是低頻雜波信號那么我們就可以設(shè)計一個高通濾波器和一個低通濾波器然后對這兩個濾波器進(jìn)行加權(quán)然后進(jìn)行求和處理;Y=αx1+(1-α)x2(1)