兩輪自平衡小車論文

1、2013年全國大學生電子設計競賽 兩輪自平衡小車設計 作者：楊魏，黃敏杰，夏俊逸 2015.7.17 摘 要 本文采用自制的兩輪簡易小車作為試驗平臺，以MEMS傳感MPU6050為傳感器的姿態(tài)感知系統(tǒng)，通過離散卡爾曼濾波器對兩種傳感器的數(shù)據(jù)進行濾波融合，選用32位單片機 STM32F103RB為控制核心處理器，完成對數(shù)據(jù)的采集處理和車身控制，采用PID控制算法實現(xiàn)小車兩輪自平衡。用藍牙控制前后運動。實驗結果驗證了該系統(tǒng)的性能滿足設計要求。 關鍵詞：兩輪自平衡；姿態(tài)感知；STM32F103RB；卡爾曼濾波；PID控制。 目 錄1系統(tǒng)方案11.1 姿態(tài)檢測模塊的論證與選擇11.2 電機驅動模塊的論

2、證與選擇12 系統(tǒng)硬件設計12.1 STM32F103RB 單片機系統(tǒng)22.1.1 STM32F103RB 單片機介紹22.1.2單片機最小系統(tǒng)設計32.1.3 電源管理模塊設計42.2 姿態(tài)檢測模塊MPU-605052.2.1 MPU-6050簡介52.2.3數(shù)字運動處理器（DMP）62.3速度檢測模塊設計72.3.1編碼器介紹72.3.2 編碼器電路設計82.4 電機驅動模塊82.4.1 L298N簡介82.4.2 L298N特點93 理論分析與計算93.1 兩輪平衡小車數(shù)學模型93.2 PID控制器設計103.2.1 PID控制器原理103.2.2 PID控制器設計113.2.3 PID

3、程序123.3 基于卡爾曼濾波的數(shù)據(jù)融合134 系統(tǒng)軟件設計154.1 系統(tǒng)軟件設計框架154.2 資源模塊初始化154.3 兩輪小車姿態(tài)信息檢測165 測試方案與測試結果165.1測試方案165.1.1硬件連接檢測165.1.2小車功能檢測165.2 測試分析與結論161系統(tǒng)方案本系統(tǒng)主要由姿態(tài)檢測模塊、電機驅動模塊、藍牙模塊、紅外對管模塊、電源模塊組成，下面分別論證這幾個模塊的選擇。1.1 姿態(tài)檢測模塊的論證與選擇方案一：ENC-03。一種普通陀螺儀。方案二：MPU-6050。MPU-60X0 是全球首例9軸運動處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴展的

4、數(shù)字運動處理器DMP（Digital Motion Processor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴展之后就可以通過其I2C或SPI接口輸出一個9軸的信號（SPI 接口僅在MPU-6000可用）。MPU-60X0也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。綜合以上兩種種方案，選擇方案二。1.2 電機驅動模塊的論證與選擇方案一：TB6612FNG。體積小，但容易壞。方案二：L298N。具有信號指示，轉數(shù)可調(diào)，抗干擾能力強，具有過電壓和過電流保護，可單獨控制兩臺直流電機，可單獨控制一臺步進電機。綜合以上三種方案，選擇方案二。2 系統(tǒng)硬件設計 本文設計

5、的兩輪自平衡小車采用姿態(tài)傳感器（MPU6050）監(jiān)測車身所處的俯仰狀態(tài)和狀態(tài)變化率，通過高速微控制器STM32F103RB 完成數(shù)據(jù)融合處理，得到平滑而穩(wěn)定車體姿態(tài)信息，然后驅動電動機產(chǎn)生前進或后退的加速度來控制車體保持平衡，紅外線檢測障礙物，藍牙控制前進后退。結構框圖如圖。 圖 1 系統(tǒng)設計總體結構框圖 系統(tǒng)硬件主要由STM32F103RB單片機系統(tǒng)、電源模塊、姿態(tài)檢測模塊、串口通訊模塊、電機驅動模塊等組成。 2.1 STM32F103RB 單片機系統(tǒng) 本設計中控制平臺采用ST 意法半導體的STM32F103RB微控制器，STM32系列基于專為要求高性能、低成本、低功耗的嵌入式應用專門設計的

6、ARM CORTEX-M3內(nèi)核，其最高主頻72MHz，在滿足平衡控制的調(diào)節(jié)頻率的同時有更多的CPU 資源， 工作電壓為5 V。其豐富的內(nèi)置模塊資源可以為姿態(tài)檢測系統(tǒng)提供12位精度的A/D 轉換，同時使用定時器可以產(chǎn)生PWM輸出控制電機轉速和轉向。2.1.1 STM32F103RB 單片機介紹 STM32F103xB增強型系列使用高性能的ARM® Cortex-M3 32位的RISC內(nèi)核，工作頻率為72MHz，內(nèi)置高速存儲器(高達128K字節(jié)的閃存和20K字節(jié)的SRAM)，豐富的增強I/O端口和聯(lián)接到兩條APB總線的外設。所有型號的器件都包含2個12位的ADC、3個通用16位定時器和1

7、個PWM定時器，還包含標準和先進的通信接口：多達2個I2C接口和SPI接口、3個USART接口、一個USB接口和一個CAN接口。 STM32F103xx中等容量增強型系列產(chǎn)品供電電壓為2.0V至3.6V，包含-40°C至+85°C溫度范圍和-40°C至+105°C的擴展溫度范圍。一系列的省電模式保證低功耗應用的要求。STM32F103xx中等容量增強型系列產(chǎn)品提供包括從36腳至100腳的6種不同封裝形式；根據(jù)不同的封裝形式，器件中的外設配置不盡相同。下面給出了該系列產(chǎn)品中所有外設的基本介紹。這些豐富的外設配置，使得STM32F103xx產(chǎn)品容量增強型系列

8、微控制器適合于多種應用場合： 電機驅動和應用控制 醫(yī)療和手持設備 PC游戲外設和GPS平臺 工業(yè)應用：可編程控制器(PLC)、變頻器、打印機和掃描儀 警報系統(tǒng)、視頻對講、和暖氣通風空調(diào)系統(tǒng)等2.1.2單片機最小系統(tǒng)設計本設計采用STM32F103RB單片機為控制器，最小系統(tǒng)原理圖如圖3-3所示，主要包括單片機供電、復位電路、時鐘電路以及BDM接口電路。 系統(tǒng)時鐘的選擇是在啟動時進行，復位時內(nèi)部8MHz的RC振蕩器被選為默認的CPU時鐘，隨后可以選擇外部的、具失效監(jiān)控的416MHz時鐘；當檢測到外部時鐘失效時，它將被隔離，系統(tǒng)將自動地切換到內(nèi)部的RC振蕩器，如果使能了中斷，軟件可以接收到相應的中

9、斷。同樣，在需要時可以采取對PLL時鐘完全的中斷管理(如當一個間接使用的外部振蕩器失效時)。多個預分頻器用于配置AHB的頻率、高速APB(APB2)和低速APB(APB1)區(qū)域。AHB和高速APB的最高頻率是72MHz，低速APB的最高頻率為36MHz。圖3-3 單片機最小系統(tǒng)及時鐘電路原理圖 單片機的外部復位電路可以使用按鈕和電容構成，也可以使用專門的復位芯片?？紤]到產(chǎn)品經(jīng)濟型，本設計中采用了由電阻電容構成的簡易復位電路，如圖3-5所示。加電后，由于電容的充電時間，RESET保持低電平，單片機復位；一段時間后，電容電量充滿，RESET端輸出高電平，此時單片機運行。手動復位時，按下手動復位按鈕

10、，RESET端保持低電平，單片機復位；釋放手動復位按鈕后，RESET端輸出高電平，單片機工作。圖3-5 復位及按鍵接口電路2.1.3 電源管理模塊設計可靠的電源方案是整個硬件電路穩(wěn)定可靠運行的基礎。電源模塊由若干相互獨立的穩(wěn)壓電路模塊組成。這樣做可以減少各模塊之間的相互干擾。整個系統(tǒng)需要3種電源： VDD = 2.03.6V：VDD引腳為I/O引腳和內(nèi)部調(diào)壓器供電。 VSSA，VDDA = 2.03.6V：為ADC、復位模塊、RC振蕩器和PLL的模擬部分提供供電。使用ADC時，VDDA不得小于2.4V。VDDA和VSSA必須分別連接到VDD和VSS。 VBAT = 1.83.6V：當關閉VDD

11、時，(通過內(nèi)部電源切換器)為RTC、外部32kHz振蕩器和后備寄存器供電。圖3-6 電源模塊電路圖2.2 姿態(tài)檢測模塊MPU-6050 姿態(tài)檢測模塊能夠精確并穩(wěn)定地檢測出當前的車身姿態(tài)，這是本設計兩輪自平衡小車實現(xiàn)有效控制的關鍵。本設計選用MEMS傳感MPU6050作為姿態(tài)檢測傳感器。 MPU-6050實物連接圖2.2.1 MPU-6050簡介MPU-60X0 是全球首例9軸運動處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴展的數(shù)字運動處理器DMP（Digital Motion Processor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴展之后就可

12、以通過其I2C或SPI接口輸出一個9軸的信號（SPI 接口僅在MPU-6000可用）。MPU-60X0也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。 MPU-60X0 對陀螺儀和加速度計分別用了三個16 位的ADC，將其測量的模擬量轉化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運動，傳感器的測量范圍都是用戶可控的，陀螺儀可測范圍為±250，±500，±1000，±2000°/秒（dps），加速度計可測范圍為±2，±4，±8，±16g。一個片上1024 字節(jié)的FIFO，有助于降低系統(tǒng)功耗。和

13、所有設備寄存器之間的通信采用400kHz 的I2C 接口或1MHz 的SPI 接口（SPI 僅MPU-6000 可用）。對于需要高速傳輸?shù)膽?，對寄存器的讀取和中斷可用20MHz 的SPI。 另外，片上還內(nèi)嵌了一個溫度傳感器和在工作環(huán)境下僅有±1%變動的振蕩器。芯片尺寸4×4×0.9mm，采用QFN 封裝（無引線方形封裝），可承受最大10000g 的沖擊，并有可編程的低通濾波器。 關于電源，MPU-60X0 可支持VDD 范圍2.5V±5%，3.0V±5%，或3.3V±5%。另外MPU-6050 還有一個VLOGIC 引腳，用來為I2

14、C 輸出提供邏輯電平。VLOGIC 電壓可取1.8±5%或者VDD。 MPU6050電路圖2.2.3數(shù)字運動處理器（DMP）DMP就是指MPU6050內(nèi)部集成的處理單元，可以直接運算出四元數(shù)和姿態(tài)，而不再需要另外進行數(shù)學運算。DMP的使用大大簡化了四軸的代碼設計。DMP是數(shù)字運動處理器的縮寫，顧名思義mpu6050并不單單是一款傳感器，其內(nèi)部還包含了可以獨立完成姿態(tài)解算算法的處理單元。如在設計中使用DMP來實現(xiàn)傳感器融合算法優(yōu)勢很明顯。首先invensense官方提供的姿態(tài)解算算法應該比像樓主這樣的小白要可靠的多。其次，由DMP實現(xiàn)姿態(tài)解算算法將單片機從算法處理的壓力中解放出來，單片

15、機所要做的是等待DMP解算完成后產(chǎn)生的外部中斷，在外部中斷里去讀取姿態(tài)解算的結果。這樣單片機有大量的時間來處理諸如電機調(diào)速等其他任務，提高了系統(tǒng)的實時性。DMP從陀螺儀、加速度計以及外接的傳感器接收并處理數(shù)據(jù)，處理結果可以從DMP寄存器讀出，或通過FIFO 緩沖。DMP有權使用MPU的一個外部引腳產(chǎn)生中斷。2.3速度檢測模塊設計兩輪自平衡小車的原理是利用地面對車輪的摩擦力抵消車受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。為實現(xiàn)速度的閉環(huán)控制，必須加入速度檢測裝置實現(xiàn)速度閉環(huán)控制中的反饋環(huán)節(jié)。本系統(tǒng)測速模塊采用OMRON(歐姆龍)公司500線增量式旋轉編碼器如

16、圖3-11。2.3.1編碼器介紹編碼器內(nèi)部為一個中心有軸的光電碼盤，其上有環(huán)形通、暗的刻線，有光電發(fā)射和接收器件讀取,獲得四組正弦波信號組合成A、B、C、D,每個正弦波相差90度相位差（相對于一個周波為360度），將C、D信號反向，疊加在A、B兩相上，可增強穩(wěn)定信號；另每轉輸出一個Z相脈沖以代表零位參考位。由于A、B兩相相差90度，可通過比較A相在前還是B相在前，以判別編碼器的正轉與反轉，通過零位脈沖，可獲得編碼器的零位參考位如圖3-12。圖3-12 編碼器A、B相輸出波形本系統(tǒng)用OMRON旋轉編碼器相關參數(shù)如表3-1所示：表3-1 編碼器參數(shù)表項目參數(shù)電源電壓DC 5V12V，紋波的峰峰值小

17、于5%消耗電流30mA以下分辨率500 脈沖/轉輸出相A、B、Z 三相輸出狀態(tài)集電極開路輸出最高輸出響應42kHz輸出上升或下降時間1us 以下起動轉矩1mN·m以下軸允許力徑向 10N、軸向 5N允許最高轉速5000r/min質量35g2.3.2 編碼器電路設計由于編碼器采用集電極開路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡單。需要在信號輸出端接入一個上拉電阻，即可將信號提供給單片機采集數(shù)據(jù)。如圖3-13所示，PULSE引腳為編碼器A相，接單片機的脈沖計數(shù)口，通過單片機的PACNT模塊對輸入脈沖進行計數(shù)從而獲取電機轉速。DIR為編碼器B相輸出，接單片機I/O口，通過A、B相

18、位差進行軟件鑒相，從而判斷電機轉動方向。圖3-13編碼器接口電路2.4 電機驅動模塊 電機的驅動芯片選用L298N 作為驅動芯片。2.4.1 L298N簡介L298是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅動電路，是一種兩相和四相電機的專用驅動器，即內(nèi)含兩個H橋的高電壓大電流雙全橋式驅動器，接收標準的TTL邏輯電平信號，可驅動46V、2A以下的電機。本設計中直流電機的驅動信號由STM32F103RB提供，信號經(jīng)過光耦隔離后，傳至控制芯片L298N，通過L298N的輸出腳與兩個電機相連，控制電機運轉。L298N的連接方法如圖6所示 圖6理論分析與計算2.4.2 L298N特點基本特點如下： （1）

19、 具有信號指示。（2） 轉數(shù)可調(diào)。（3） 抗干擾能力強。（4） 具有過電壓和過電流保護。（5） 可單獨控制兩臺直流電機。（6） 可單獨控制一臺步進電機。（7） PWM脈寬平滑調(diào)速。（8） 可實現(xiàn)正反轉。（9） 采用光電隔離。3 理論分析與計算3.1 兩輪平衡小車數(shù)學模型 已知自平衡車高度為，質量為，將其抽象為一級倒立擺，并將倒立擺至于可水平移動的小車上。假設其受外力干擾引起的車體角加速度為，沿垂直于車體方向進行受力分析如圖2-7，可以得到自平衡車傾角與車輪移動加速度為以及外力干擾帶來的加速度之間的運動方程。圖2-7 外力干擾條件下的小車受力分析小車運動微分方程表達式如式2-8： （式2-8）當

20、傾角很小的時候，可以進行線性化處理： ，運動微分方程可簡化為： （式2-9）自平衡車靜止時,其運動微分方程為： （式2-10）3.2 PID控制器設計3.2.1 PID控制器原理當今的自動控制技術都是基于反饋的概念。反饋理論的要素包括三個部分：測量、比較和執(zhí)行。測量系統(tǒng)需要控制的變量，與期望值相比較，用這個誤差糾正調(diào)節(jié)控制系統(tǒng)的響應。在工程實際中，應用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱PID控制，又稱PID調(diào)節(jié)。PID控制器問世至今已有近70年歷史，以其結構簡單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術之一。PID控制器由比例單元（P）、積分單元（I）和微分單元（

21、D）組成。其輸入e (t）與輸出u (t）的關系為： (式2-11)其中為比例系數(shù)；為積分時間常數(shù)；為微分時間常數(shù)。PID控制器具有原理簡單、使用方便、適應性強、魯棒性強、對模型依賴少等特點，因此使用PID控制器實現(xiàn)兩輪自平衡車的控制是完全可行的。3.2.2 PID控制器設計 由小車靜止時其運動方程可得到系統(tǒng)輸入輸出傳遞函數(shù)： (式2-12) 此時系統(tǒng)具有兩個極點：。其中一個極點位于s平面的右半平面。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車在靜止狀態(tài)不能保持平衡56。由小車受力分析可知小車平衡的條件是提供額外的回復力及阻尼，其來源為車輪與地面的摩擦力。由式2-7可知，車輪提供的加速度的大小

22、是根據(jù)角度及角速度的反饋得出，因此需要在控制系統(tǒng)中引入角度及角速度構成比例(P)微分(D)反饋環(huán)節(jié)，如圖2-8所示。圖2-8 加入比例微分環(huán)節(jié)后的控制系統(tǒng)結構圖加入比例微分反饋后的系統(tǒng)傳遞函數(shù)為： (式2-13)此時，系統(tǒng)的兩個極點為。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，系統(tǒng)穩(wěn)定需要兩個極點都位于s平面的左半平面。要滿足這一點，需要>g, >0。由此可得出結論，但>g, >0時，小車可以保持平衡，這也與上文中小車受力分析的結果相符。在反饋環(huán)節(jié)中，與角度成比例的控制量稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此上面系數(shù)，分別稱為比例和微分控制參數(shù)。其

23、中微分參數(shù)相當于阻尼力，可以有效抑制自平衡車振蕩?？刂葡到y(tǒng)的輸出量為電機控制量，因而小車平衡控制的PID控制器的輸出方程可寫為：OUT_Motor=Kp*Angle+Kd*Angle_dot (式2-14)式2-14中，OUT_Motor為PID控制輸出量，Angle為反饋傾角值，Angle_dot為反饋角速度值，Kp和Kd分別為比例系數(shù)及微分系數(shù)。3.2.3 PID程序#include <stm32f10x.h>#include "math.h"#include <pwm.h>float PID_realize();void pwm(void);e

24、xtern float Pitch;float SetSpeed=-1.04;/定義目標值float ActualSpeed=0.0;/反饋值float err=0.0;/偏差值float err_last=0.0;/上次偏差值float Kp=135;float Ki=2.7;float Kd=135;float output=0.0;/電壓值控制執(zhí)行器的變量float integral=0.0;/積分量float PID_realize()ActualSpeed=Pitch;err=SetSpeed-ActualSpeed;integral+=err;output=Kp*err+Kd*(e

25、rr-err_last)+Ki*integral;err_last=err;return output;3.3 基于卡爾曼濾波的數(shù)據(jù)融合 卡爾曼濾波器是一種高效率的遞歸濾波器(自回歸濾波器)，能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)?？柭鼮V波器不僅能估計信號的過去和當前狀態(tài)，甚至能估計將來的狀態(tài)?？柭鼮V波器解決離散時間控制過程的一般方法，首先定義模型線性隨機微分方程。假設卡爾曼濾波模型k時刻真實狀態(tài)是從（k-1）時刻推算出來，如下式 (式2-15)式2-15中，是k時刻狀態(tài)；A是k-1時刻狀態(tài)變換模型；B是作用在控制器向量上的輸入控制模型；是過程噪聲，假設其均值為零，協(xié)方

26、差矩陣符合多元正態(tài)分布： (式2-16)k時刻對應真實狀態(tài)的測量滿足下式： (式2-17)式2-17中是觀測模型，將真實控制映射為觀測空間；為觀測噪聲，其均值為零，協(xié)方差矩陣符合正態(tài)分布： (式2-18)初始狀態(tài)以及每一時刻的噪聲都認為是互相獨立的?？柭鼮V波器的操作主要包括兩個階段：預估與更新。在預估階段，濾波器根據(jù)上一時刻狀態(tài)，估算出當前時刻狀態(tài)；在更新階段，濾波器利用當前時刻觀測值優(yōu)化在預估階段獲得的測量值，以獲得一個更準確的新估計值111213。卡爾曼濾波器迭代過程如下：1. 先驗狀態(tài)估計：2. 先驗估計誤差協(xié)方差 3. 卡爾曼增益 (式2-21)4. 后驗狀態(tài)估計 (式2-22)5. 后驗誤差協(xié)方差 (式2-23)在上面各式中：A：作用在上的n階矩陣；B：作用在控制向量上的n×1輸入控制矩陣；H：m×n觀測模型矩陣，將真實狀態(tài)空間映射為觀測空間；：n×n先驗估計誤差協(xié)方差矩陣；：n×n后驗估計誤差協(xié)方差矩陣；Q：n×n過程噪聲協(xié)方差矩陣；R：m×m過程噪聲協(xié)方差矩陣；I：n階單位矩陣；：n×m矩陣，稱之為卡爾曼增益。4 系統(tǒng)軟件設計 4.1 系統(tǒng)軟件設計框架系統(tǒng)軟件設計框架主要