基于STM32控制的平衡小車系統設計

陳昱 沈林濤 王凱

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

關鍵詞：STM32;PID;MPU-6050;卡爾曼濾波;自平衡

DOI：10. 11907/rjdk. 201087 開放科學（資源服務）標識碼（OSID）：

中圖分類號：TP319文獻標識碼：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 引言

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

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

1 總體設計方案

1.1 原理分析

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

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

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

1.2 系統設計方案

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

2 系統硬件設計

系統硬件主要有主控制器、姿態傳感器模塊、電源模塊、藍牙模塊、超聲波模塊、電機測速模塊、電機和電機驅動模塊等。

2.1 電源模塊

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

2.2 主控制器模塊

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

2.3 姿態傳感器模塊

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

2.4 電機驅動模塊

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

2.5 電機測速模塊

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

2.6 藍牙模塊

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

2.7 超聲波模塊

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

3 系統軟件設計

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

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

（2）數據濾波。主控制器接收小車姿態信息后，借助卡爾曼濾波算法對姿態數據進行平滑濾波輸出。

（3）PID控制。采用PID控制算法，結合優化后的姿態數據與當前電機運行信息，輸出控制數據。

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

3.1 系統初始化

在小車正式運行之前， 需對整個系統進行初始化，其流程如圖8所示。

（1）時鐘初始化。因為小車運行過程中需要每隔5ms獲取一次車體姿態信息，因此系統中需要設置時鐘，對時鐘進行初始化是保證系統正常運行的前提。

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

（3）定時器初始化。同樣是為了確保每5ms實現一次中斷，讓MPU-6050讀取車體運行姿態和傳遞參數順利進行。

（4）通信初始化。系統采用I2C通信，為了實現參數采集和數據在各模塊間的有效快速的傳輸，需要進行初始化。

（5）測速模塊初始化。實現車體穩定的平衡狀態除需要準確獲取車體傾角和角速度外，還需要準確獲取車輪的轉速和轉向，測速模塊的功能便在于實時檢測車輪運行狀態。

（6）MPU-6050初始化。姿態檢測模塊MPU-6050初始化的意義在于實現對小車的姿態檢驗。

（7）電機PWM初始化。在對車體姿態和車輪運行狀態進行解析后輸出控制PWM，控制電機不同的速度和方向轉換。

3.2 卡爾曼濾波融合算法

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

3.3 PID 控制算法

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

本文系統采用雙閉環串級PID算法進行角度環（PD）控制和速度環（PI）控制。其中將速度環輸出作為角度環輸入，將角度環輸出作為系統輸出，這是一個串級控制系統，PID調節流程如圖9所示。

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

4 系統調試

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

4.1 調試角度環

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

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

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

當Kp值為正時，小車上電啟動后有回復力驅使小車保持平衡，可確定Kp極性為正。

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

當Kp取值從300增加至500時，響應速度逐漸加快，直至Kp=500時出現大幅度抖動，表明設定的Kp值很大，抑制低頻抖動需增加微分控制，削弱比例控制。

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

當Kd值為正時，上電啟動小車后傾斜車體，車輪會跟隨傾角方向轉動，保證調節轉速達到平衡目的，可確定Kd極性為正。

（5）確定Kd大小（令Kp=500），如表4所示。

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

4.2 調試速度環

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

（1）為確定Kp、Ki極性進行第一次調試，如表5所示。

當Kp、Ki取值為正時，轉動小車其中一個車輪，另一個可以實現同向轉動，說明調節速度可實現兩個車輪同步控制，因此可確定Kp、Ki兩參數極性為正。

（2）為確定Kp、Ki極性進行第二次調試，如表6所示。

逐漸增大Kp、Ki的取值，當Kp=80、Kd=0.4時，小車整體性能良好。確定角度環和速度環后，經過調試實現小車上電后在無外力作用下自主平衡，并且在遇到一定外力作用時，小車能抗干擾，迅速回到穩定平衡狀態。

5 結語

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

參考文獻：

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

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

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

[4] 韓竺秦，張麗娜. 兩輪平衡機器人控制系統設計與仿真研究[J]. 軟件導刊，2019，18（3）：86-90.

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

[6] 束方耀，戴麗華，鄒壯志，等. 聲控自平衡小車的設計與研究[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] 李澍源，馮奇杰，蘭家祥，等. 基于超聲波測距式的自平衡車控制系統[J]. 內燃機與配件，2017（2）：9-11.

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

[10] 徐猛. 基于STM32自平衡小車的設計與制作[J]. 計算機產品與流通，2019（9）：110.

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

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

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

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

（責任編輯：江 艷）