基于STM32控制的平衡小車系統(tǒng)設(shè)計

陳昱 沈林濤 王凱

摘要：為了響應(yīng)節(jié)能環(huán)保、綠色出行倡議，緩解城市交通擁擠狀況，提出一種基于STM32F103C8T6芯片控制的兩輪平衡小車設(shè)計方案。以MPU-6050作為小車姿態(tài)傳感器獲取小車車體傾角和角速度，基于卡爾曼濾波算法對姿態(tài)傳感器采集到的的數(shù)據(jù)進(jìn)行濾波融合，利用霍爾編碼器測量小車車輪轉(zhuǎn)向和轉(zhuǎn)速，運(yùn)用PID算法對控制要求和采集的數(shù)據(jù)信息進(jìn)行計算分析并輸出控制PWM，經(jīng)由TB6612電機(jī)驅(qū)動模塊驅(qū)動電機(jī)，實現(xiàn)小車自主平衡并具備一定的抗干擾能力。另外小車通過藍(lán)牙模塊與手機(jī)APP通信，可通過手機(jī)端控制小車前進(jìn)、后退、轉(zhuǎn)彎等動作。

關(guān)鍵詞：STM32;PID;MPU-6050;卡爾曼濾波;自平衡

DOI：10. 11907/rjdk. 201087 開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

中圖分類號：TP319文獻(xiàn)標(biāo)識碼：A 文章編號：1672-7800（2020）008-0117-05

Abstract：In order to response to the initiatives of energy conservation and environmental protection， green travel， and alleviate the urban traffic congestion， this paper introduces the design of self-balancing two-wheeled vehicle based on STM32F103C8T6 chip control.Sensors with MPU - 6050 is used to obtain car attitude angle and velocity of the vehicle body. The Kalman filter algorithm is used to filter and fuse the data collected by the attitude sensor. The Hall encoder is used to measure the steering and rotation speed of the trolley， and the PID algorithm is used to calculate and analyze the control requirements and collected data information and output the control PWM. The motor is driven by the TB6612 motor drive module to realize the autonomous balance of the trolley which has certain anti-interference ability. In addition， the car communicates with the mobile APP via bluetooth module， and the cars forward and backward movements， turn and other actions can be controlled through the mobile end.

Key Words： STM32; PID; MPU-6050; Kalman filter; self-balance

0 引言

隨著社會發(fā)展，環(huán)境污染日益加劇，與此同時公眾環(huán)保意識逐步增強(qiáng)，綠色出行成為城市交通規(guī)劃和公眾出行首選。新能源汽車、折疊式電動車、便攜式平衡車等逐漸成為日常代步工具。其中平衡車因為輕巧便攜、轉(zhuǎn)彎半徑小，特別適用于在場地空間有限的地方運(yùn)行，如大型購物中心、辦公大樓、生活住宅小區(qū)、國際性會議或展覽場所、生態(tài)旅游風(fēng)景區(qū)等[1]。平衡車如何高效穩(wěn)定地獲取實時姿態(tài)信息、有效控制車體狀態(tài)，在復(fù)雜環(huán)境下成功執(zhí)行目標(biāo)任務(wù)等成為研究熱點。兩輪平衡小車概念順應(yīng)當(dāng)前研究浪潮，可有效解決復(fù)雜環(huán)境下小車運(yùn)動問題，同時隨著遠(yuǎn)程控制技術(shù)的發(fā)展，遠(yuǎn)程監(jiān)控平衡車在軍用、民用領(lǐng)域有廣闊的發(fā)展空間[2]。

平衡小車是一種自然不穩(wěn)定體，其原理是在倒立擺基礎(chǔ)上對普通倒立擺的發(fā)展，因此比普通倒立擺具有更大的運(yùn)動空間，使其不僅局限于實驗室，更貼近現(xiàn)實生活[3]。國內(nèi)外學(xué)者對此進(jìn)行了研究，并取得了相應(yīng)成果。韓竺秦等[4]對平衡車進(jìn)行建模，利用KL25和MPU6050搭建實驗平臺，結(jié)合四元數(shù)、互補(bǔ)濾波和PID算法實現(xiàn)小車自主平衡，并利用MATLAB仿真驗證方法有效性;陳木桂等[5]以MC9S12XS128為核心控制器，采用加速度傳感器MMA7260、ENC-03陀螺儀結(jié)合PID控制算法提出一種光電平衡智能車設(shè)計方案，該兩輪車能夠按照預(yù)定的軌跡穩(wěn)定運(yùn)行;束方耀等[6]以STM32作為主控設(shè)計了一種聲控自平衡小車，該平衡小車可在近距離內(nèi)進(jìn)行語音控制;文獻(xiàn)[7]提出了一種基于Arduino的平衡小車設(shè)計方案。綜上所述，平衡小車控制的關(guān)鍵在于車體傾斜角、角速度、車輪轉(zhuǎn)向及速度測量準(zhǔn)確性和實時性。針對該問題，本文采用STM32F103C8T6作為主控，利用霍爾編碼器獲取小車車輪轉(zhuǎn)速，將MPU-6050作為姿態(tài)傳感器采集車體傾角和角速度，結(jié)合卡爾曼濾波算法與PID控制算法實現(xiàn)小車自主平衡。

1 總體設(shè)計方案

1.1 原理分析

兩輪平衡小車受力點在底部輪子上，如果不施加外力，車體將無法保持平衡。在日常生活中該原理的應(yīng)用實例也較為常見，比如將木棒放置在手掌中，來回調(diào)整手的位置，可以實現(xiàn)木棒直立不倒[8]。在維持木棒平衡的過程中，必須滿足兩個條件：①木棒下面的手掌可來回移動;②必須實時觀察木棒傾斜角度和傾倒趨勢（角速度），憑借手掌的來回移動消除木棒傾斜角度和趨勢，保持手掌中的木棒直立不倒。木棒維持平衡采用的控制方式是閉環(huán)負(fù)反饋控制，如圖1所示。

平衡小車自動維持平衡也采用負(fù)反饋調(diào)節(jié)的控制方式，如圖2所示，由于小車底部只有兩個并排的車輪，車體上部分只能在車輪前后方向出現(xiàn)傾斜時，通過控制車輪的轉(zhuǎn)動方向和速度消除該傾斜方向上的傾角，保證小車車體平衡。將平衡小車離開垂直位置的傾角作為偏差量，憑借負(fù)反饋控制將該偏差量保持在0°附近，通過控制電機(jī)轉(zhuǎn)動方向維持小車直立狀態(tài)。

依據(jù)小車自平衡功能要求，小車底部并排的兩個車輪是保持車體直立前后移動的控制目標(biāo)，而小車輪胎的轉(zhuǎn)動受電機(jī)控制，因此從控制思維看，受控對象是小車車體，車體底部兩個電機(jī)轉(zhuǎn)動速度作為控制輸入量[9]。兩輪平衡小車通過姿態(tài)檢測模塊檢測出傾斜角，然后控制器接受姿態(tài)信息并進(jìn)行處理，最后車輪電機(jī)在主控制器的控制下實現(xiàn)變速和換向，實現(xiàn)小車動態(tài)平衡。

1.2 系統(tǒng)設(shè)計方案

本文以STM32處理器為控制核心，接收并處理傳感器傳遞的小車數(shù)據(jù)信息，MPU6050模塊負(fù)責(zé)采集小車傾角和角速度數(shù)據(jù)信息，利用霍爾編碼器獲取小車車輪轉(zhuǎn)速和轉(zhuǎn)向數(shù)據(jù)信息并反饋給主控制器。STM32結(jié)合卡爾曼濾波算法與串級PID算法將控制信號輸入至電機(jī)驅(qū)動模塊TB6612FNC，驅(qū)動電機(jī)正反轉(zhuǎn)和變速，實現(xiàn)小車上電后自主平衡。手機(jī)APP和小車通過藍(lán)牙模塊連接，實現(xiàn)小車動作控制，系統(tǒng)總體框架見圖3所示。

2 系統(tǒng)硬件設(shè)計

系統(tǒng)硬件主要有主控制器、姿態(tài)傳感器模塊、電源模塊、藍(lán)牙模塊、超聲波模塊、電機(jī)測速模塊、電機(jī)和電機(jī)驅(qū)動模塊等。

2.1 電源模塊

兩輪自平衡小車正常運(yùn)行離不開電源支撐，本文設(shè)計以航模電池作為電源，相對于普通干電池，航模電池可有效改善電池漏液的安全問題，且可多次充放電，使用方便。電源模塊原理如圖4所示，12V航模電池經(jīng)過低壓差線性穩(wěn)壓器LM2596T與AMS1117模塊變壓后可降至5V和3V，為姿態(tài)傳感器MPU-6050和主控芯片和測速等模塊供電，保證各硬件模塊正常工作。

2.2 主控制器模塊

主控制器為ST公司推出的STM32F103C8T6，該微控制器內(nèi)核為Cortex-M3，CPU為32位，存儲器為SRAM， 6-64kB。其增強(qiáng)型系列處理速度達(dá)72MHz ，具有高性能、低成本等特點[10]。在該控制器的支持下，控制車輪實現(xiàn)車體平衡，主控模塊電路原理如圖5所示。

2.3 姿態(tài)傳感器模塊

車體姿態(tài)信息對于實現(xiàn)車體平衡至關(guān)重要，本文姿態(tài)檢測采用MPU-6050模塊，如圖6所示。其內(nèi)部自帶穩(wěn)壓電路可兼容3.3V/5V供電電壓，采用先進(jìn)的數(shù)字濾波技術(shù)，有效解決了測量噪聲跟隨精度上升的問題。通信方面，權(quán)限高的用戶可通過保留的I2C接口采樣底層測量數(shù)據(jù)。芯片集成了DMP數(shù)字動態(tài)處理器（實現(xiàn)平衡小車姿體平衡），從陀螺儀、加速度計及外接傳感器接收并處理數(shù)據(jù)，處理結(jié)果可從DMP寄存器讀出或通過FIFO緩沖[11]。為了滿足實時獲取姿態(tài)信息的要求，聯(lián)合使用陀螺儀與加速度計，共同構(gòu)成姿態(tài)讀取模塊。

2.4 電機(jī)驅(qū)動模塊

因為單片機(jī)IO口輸出功率較小，無法使電機(jī)正常運(yùn)行，為使電機(jī)速度和換向控制，需要一個功率放大器件。電機(jī)驅(qū)動選擇是使用東芝半導(dǎo)體生產(chǎn)的直流電機(jī)驅(qū)動TN6612FNG，相對于傳統(tǒng)的L298N電機(jī)驅(qū)動，TB6612FNG外圍電路簡單，在運(yùn)行中發(fā)熱量很小，且支持的PWM 調(diào)速頻率更高，同時也有利于減小系統(tǒng)的尺寸。

2.5 電機(jī)測速模塊

選用增量式輸出的霍爾編碼器進(jìn)行小車測速。編碼器為AB相輸出，不僅可測速，還可辨別轉(zhuǎn)向。只給編碼器電源5V供電，在電機(jī)轉(zhuǎn)動時編碼隨電機(jī)同步轉(zhuǎn)動即可通過AB相輸出存在相位差的方波信號。編碼器自帶上拉電阻，所以無需外部上拉，可直接連接單片機(jī)IO進(jìn)行讀取，這樣就可判斷電機(jī)實時轉(zhuǎn)速和轉(zhuǎn)動方向。

2.6 藍(lán)牙模塊

藍(lán)牙模塊選用HC-05，該模塊價格低、能耗低、尺寸小、收發(fā)靈敏高。HC-05有兩種工作模式：自動連接工作模與命令響應(yīng)工作模式。前者又分為3種形式：主、從、回環(huán)。當(dāng)藍(lán)牙工作在第一種模式時模塊以預(yù)先設(shè)定的步驟進(jìn)行通信;當(dāng)藍(lán)牙工作在第二種模式時，設(shè)置模塊參數(shù)需用戶向藍(lán)牙發(fā)送AT指令進(jìn)行控制。通過控制引腳電平高低，實現(xiàn)模塊工作模式轉(zhuǎn)換。

2.7 超聲波模塊

測距模塊選用HC-SR04，該超聲波模塊測距范圍2cm～400cm，測距精度達(dá)高到3mm;模塊有Trig（控制端）、Echo（接收端）、VCC和GND等4個引腳。模塊Trig（控制端）有1個10us以上的高電平就可在Echo（接收端）等待高電平輸出，一旦有輸出即可開啟模塊內(nèi)部定時器計時，當(dāng)該端口變?yōu)榈碗娖綍r可讀取定時器的值，作為該次測距時間，根據(jù)時間可算出距離。測試距離=（高電平時間*聲速（340M/S））/2。

3 系統(tǒng)軟件設(shè)計

平衡小車上電后實現(xiàn)自主平衡是一個復(fù)雜的控制過程，主要包含：系統(tǒng)初始化、按鍵檢測、車體姿態(tài)信息采集、數(shù)據(jù)濾波、PID控制算法等功能的設(shè)計，其控制流程如圖7所示。

（1）讀取姿態(tài)。MPU-6050每間隔5ms檢測一次車體傾角和角速度，與此同時，編碼器測量電機(jī)轉(zhuǎn)速和方向，然后將測量值傳遞給主控制器。

（2）數(shù)據(jù)濾波。主控制器接收小車姿態(tài)信息后，借助卡爾曼濾波算法對姿態(tài)數(shù)據(jù)進(jìn)行平滑濾波輸出。

（3）PID控制。采用PID控制算法，結(jié)合優(yōu)化后的姿態(tài)數(shù)據(jù)與當(dāng)前電機(jī)運(yùn)行信息，輸出控制數(shù)據(jù)。

（4）PWM輸出。最終在主控制器的控制下輸出PWM波控制電機(jī)轉(zhuǎn)向和速度，并再次等待定時中斷。

3.1 系統(tǒng)初始化

在小車正式運(yùn)行之前， 需對整個系統(tǒng)進(jìn)行初始化，其流程如圖8所示。

（1）時鐘初始化。因為小車運(yùn)行過程中需要每隔5ms獲取一次車體姿態(tài)信息，因此系統(tǒng)中需要設(shè)置時鐘，對時鐘進(jìn)行初始化是保證系統(tǒng)正常運(yùn)行的前提。

（2）NVIC中斷初始化。為了保證按時中斷的準(zhǔn)確性，5ms后實現(xiàn)中斷，并開始讀取MPU-6050所采集的小車姿態(tài)數(shù)據(jù)的過程，必須保證將定時中斷初始化[12]。

（3）定時器初始化。同樣是為了確保每5ms實現(xiàn)一次中斷，讓MPU-6050讀取車體運(yùn)行姿態(tài)和傳遞參數(shù)順利進(jìn)行。

（4）通信初始化。系統(tǒng)采用I2C通信，為了實現(xiàn)參數(shù)采集和數(shù)據(jù)在各模塊間的有效快速的傳輸，需要進(jìn)行初始化。

（5）測速模塊初始化。實現(xiàn)車體穩(wěn)定的平衡狀態(tài)除需要準(zhǔn)確獲取車體傾角和角速度外，還需要準(zhǔn)確獲取車輪的轉(zhuǎn)速和轉(zhuǎn)向，測速模塊的功能便在于實時檢測車輪運(yùn)行狀態(tài)。

（6）MPU-6050初始化。姿態(tài)檢測模塊MPU-6050初始化的意義在于實現(xiàn)對小車的姿態(tài)檢驗。

（7）電機(jī)PWM初始化。在對車體姿態(tài)和車輪運(yùn)行狀態(tài)進(jìn)行解析后輸出控制PWM，控制電機(jī)不同的速度和方向轉(zhuǎn)換。

3.2 卡爾曼濾波融合算法

在動態(tài)測量過程中， 測量小車傾角和角速度數(shù)值。與實際情況相比， 必定存在偏差。采用卡爾曼濾波算法將陀螺儀與加速度計采集的數(shù)據(jù)融合， 克服陀螺儀造成的漂移誤差和加速度計造成的動態(tài)誤差，得出最優(yōu)值消除誤差，可有效優(yōu)化小車姿態(tài)數(shù)據(jù)信息 [13]。

3.3 PID 控制算法

在一些簡單的PID控制系統(tǒng)中，一般加入單獨的P控制或PI控制即可實現(xiàn)要求，但在要求提高干擾響應(yīng)速度情況下，需加入D控制。

本文系統(tǒng)采用雙閉環(huán)串級PID算法進(jìn)行角度環(huán)（PD）控制和速度環(huán)（PI）控制。其中將速度環(huán)輸出作為角度環(huán)輸入，將角度環(huán)輸出作為系統(tǒng)輸出，這是一個串級控制系統(tǒng)，PID調(diào)節(jié)流程如圖9所示。

由式（6）和式（7）得到讓小車保持直立且速度為給定值的控制算法。由一個負(fù)反饋的直立PD控制器與一個正反饋的速度PI控制器組成平衡和速度的雙閉環(huán)控制串級PID控制算法。

4 系統(tǒng)調(diào)試

在軟件調(diào)試前先組裝平衡小車硬件，在設(shè)計安裝車體硬件各部分的過程中必須使車體重心盡可能在小車中心點，后續(xù)平衡調(diào)試才更容易。組裝好車體后，小車通過下載線連接到電腦USB串口，經(jīng)過軟件Kile5編譯燒錄程序到主控芯片，軟件調(diào)試主要有角度環(huán)調(diào)試和速度環(huán)調(diào)試，在傾斜角度和車輪速度控制共同作用下實現(xiàn)小車平衡。

4.1 調(diào)試角度環(huán)

將小車放在光滑地面上，調(diào)試角度環(huán)時，需屏蔽速度環(huán)，角度環(huán)調(diào)試步驟包括：確定小車平衡中值、確定 Kp、Kd極性及其大小。

（1）確定機(jī)械中值。將小車放在光滑地面，小車在零度附近有直立趨勢，因此可確定小車機(jī)械中值為0。

（2）確定Kp極性（令 Kd=0），如表1所示。

當(dāng)Kp值為正時，小車上電啟動后有回復(fù)力驅(qū)使小車保持平衡，可確定Kp極性為正。

（3）確定Kp大小，如表2所示。

當(dāng)Kp取值從300增加至500時，響應(yīng)速度逐漸加快，直至Kp=500時出現(xiàn)大幅度抖動，表明設(shè)定的Kp值很大，抑制低頻抖動需增加微分控制，削弱比例控制。

（4）確定Kd極性（令Kp=0），如表3所示。

當(dāng)Kd值為正時，上電啟動小車后傾斜車體，車輪會跟隨傾角方向轉(zhuǎn)動，保證調(diào)節(jié)轉(zhuǎn)速達(dá)到平衡目的，可確定Kd極性為正。

（5）確定Kd大?。頚p=500），如表4所示。

Kp=500，Kd=1.7是Kp和Kd的最大值，超過該數(shù)值會影響小車平衡狀態(tài)。根據(jù)工程經(jīng)驗可知，將調(diào)試得到的最大值分別乘以0.6，然后取整得Kp=300，Kd=1，這是實現(xiàn)最好平衡狀態(tài)的理想?yún)?shù)值[14]。此時小車能保持一定時間的平衡，但是十幾秒后小車仍會向一個方向倒下，這是因為需依靠直立控制和速度控制兩者的共同作用才能實現(xiàn)小車長時間平衡，因此需進(jìn)行速度環(huán)參數(shù)調(diào)試。

4.2 調(diào)試速度環(huán)

將小車放在光滑地面上，在調(diào)試速度環(huán)時，需屏蔽角度環(huán)，通過確定 Kp、 Ki極性及大小完成速度環(huán)調(diào)試。

（1）為確定Kp、Ki極性進(jìn)行第一次調(diào)試，如表5所示。

當(dāng)Kp、Ki取值為正時，轉(zhuǎn)動小車其中一個車輪，另一個可以實現(xiàn)同向轉(zhuǎn)動，說明調(diào)節(jié)速度可實現(xiàn)兩個車輪同步控制，因此可確定Kp、Ki兩參數(shù)極性為正。

（2）為確定Kp、Ki極性進(jìn)行第二次調(diào)試，如表6所示。

逐漸增大Kp、Ki的取值，當(dāng)Kp=80、Kd=0.4時，小車整體性能良好。確定角度環(huán)和速度環(huán)后，經(jīng)過調(diào)試實現(xiàn)小車上電后在無外力作用下自主平衡，并且在遇到一定外力作用時，小車能抗干擾，迅速回到穩(wěn)定平衡狀態(tài)。

5 結(jié)語

本文設(shè)計以STM32為主控制器， 以MPU-6050和霍爾編碼器為數(shù)據(jù)采集模塊，結(jié)合卡爾曼濾波算法與PID算法對姿態(tài)數(shù)據(jù)進(jìn)行處理，系統(tǒng)上電后按下啟動按鈕，可實現(xiàn)小車在無外力作用下自主平衡。通過藍(lán)牙連接手機(jī)APP后，可在手機(jī)端控制小車前進(jìn)、后退、轉(zhuǎn)彎等動作。在一定范圍內(nèi)的外力干擾下，小車能維持車體不倒且在短時間內(nèi)恢復(fù)到平衡狀態(tài)。通過對平衡小車的研究，為進(jìn)一步學(xué)習(xí)倒立擺及對更復(fù)雜的控制系統(tǒng)（如四軸飛行器）研究打下了堅實基礎(chǔ)。

參考文獻(xiàn)：

[1] 季鵬飛. ?基于STM32的兩輪自平衡小車控制系統(tǒng)設(shè)計[J]. 電子科技，2014，27（11）：96-105.

[2] 王素青，熊維堂. ?基于STM32的兩輪自平衡小車系統(tǒng)設(shè)計[J]. 實驗室研究與探索，2016，35（5）：146，150.

[3] 李滿. ?基于積分分離PID 控制的自平衡車設(shè)計[J]. 武漢交通職業(yè)學(xué)院學(xué)報，2016，18（2）：82-86.

[4] 韓竺秦，張麗娜. 兩輪平衡機(jī)器人控制系統(tǒng)設(shè)計與仿真研究[J]. 軟件導(dǎo)刊，2019，18（3）：86-90.

[5] 陳木桂，張謙，黃睿，等. ?一種光電平衡智能車設(shè)計[J]. 教育教學(xué)論壇，2019（39）：76-80.

[6] 束方耀，戴麗華，鄒壯志，等. 聲控自平衡小車的設(shè)計與研究[J]. 電子制作，2018（17）：17-18，16.

[7] HOU Z Y. Design of intelligent balancing vehicle control system based on arduino[J]. International Journal of Plant Engineering and Management，2018，23（3）：164-167.

[8] 李澍源，馮奇杰，蘭家祥，等. 基于超聲波測距式的自平衡車控制系統(tǒng)[J]. 內(nèi)燃機(jī)與配件，2017（2）：9-11.

[9] 王宇坤，陳沃源. 基于PI控制器的兩輪平衡小車設(shè)計[J]. 信息記錄材料，2018，19（7）：215-216.

[10] 徐猛. 基于STM32自平衡小車的設(shè)計與制作[J]. 計算機(jī)產(chǎn)品與流通，2019（9）：110.

[11] 熊躍軍，史明政，林金勇，等. 一種兩輪自平衡小車的設(shè)計與實現(xiàn)[J]. 長沙大學(xué)學(xué)報，2016，30（2）：39-42.

[12] 李詳鵬，陳春，周子文，等. 基于MEMS傳感器的兩輪平衡小車設(shè)計[J]. 科技創(chuàng)新與應(yīng)用，2019（22）：27-29.

[13] 楊柳. 兩輪自平衡小車控制系統(tǒng)探討與分析[J]. 中國新通信，2018，20（19）：152-152.

[14] 林偉捷，黃唯佳，蔡劍卿. 基于四元數(shù)互補(bǔ)濾波和PID控制算法的兩輪自平衡車系統(tǒng)設(shè)計[J]. 軟件導(dǎo)刊，2016，15（6）：80-82.

（責(zé)任編輯：江 艷）