基于STM32的雙輪平衡機器人的控制系統設計

范淇元，覃羨烘，李洪毅

（1.華南理工大學廣州學院 機械工程學院，廣州 510800；2.廣東理工學院 工業自動化系，肇慶 526100）

伴隨著計算、傳感、控制及執行等技術的快速發展，科研工作者開始越來越重視移動機器人的研究，使得機器人學成為目前學科研究炙手可熱的領域之一。社會在發展，人們對移動機器人的應用要求也越來越高。所以機器人面臨的環境和任務也越來越復雜，這就要求移動機器人必須能夠適應一些復雜的環境和任務[1-2]。比如，戶外移動機器人需要在凹凸不平的地面上行走，有時需要在狹窄的空間運行等。如何解決機器人在這些環境中更好更平穩地運行，成為科研工作者須重點考慮的問題。

本雙輪平衡機器人完美解決了上述問題。各個模塊能夠正常并協調工作，機器人可以在無人干預條件下實現自主平衡。同時在引入適量干擾情況下機器人能夠自主調整并迅速恢復穩定狀態。本控制系統在保證機器人平穩運行的前提下，具備兩方面的優點：硬件電路總體體積小，而且其所包含的電子元器件價格低、可靠性高；程序易開發，開發耗時少，修改方便。

1 雙輪平衡機器人的數學模型

1.1 雙輪平衡機器人的整體結構

如圖1所示，是雙輪平衡機器人的機械結構三維圖。雙輪平衡機器人整體結構由機身和車輪兩大部分組成。機身由上頂板（亞克力板）與下底板（不銹鋼板）通過銅柱聯接構成，機身的底板部搭載的是控制系統板。機身下部左右各安裝1個370直流電機，2個電機各自驅動左右2個車輪，控制芯片向電機發送指令從而控制車輪實現機器人前進、后退和轉向3個動作。經測量，雙輪平衡機器人的尺寸為 150 mm×85 mm×110 mm。

圖1 雙輪平衡機器人整體結構三維圖Fig.1 3D structure of two wheeled balancing robot

1.2 平衡的實現

當雙輪平衡機器人的控制系統不工作時，不論機身向前或向后傾倒，兩輪沒有任何轉動，這時機身前后擺動與其車輪轉動是相互獨立的；當控制系統開始控制時，機身的狀態變化使機器人有前進、靜止、后退3種運動的方式，而機器人上裝載的姿態檢測系統能夠對機器人的傾斜狀況進行實時檢測并將得到的檢測數據發送給單片機，單片機接收到數據后對車輪轉動進行適當的控制，便可以抵消在這個維度上的傾斜力矩便可以保持雙輪機器人的平衡，如圖2所示。

雙輪平衡機器人的控制核心思想是，當姿態檢測系統檢測到機身產生傾斜時，控制系統會根據測得的傾角產生一個相應的力矩，通過控制電機驅動車輪朝車身要倒下的方向運動至合適距離，以保持機身的動態平衡[3-5]。

圖2 三種糾正方式Fig.2 Three types of correction

2 雙輪平衡機器人控制系統硬件設計

2.1 控制系統硬件總體設計方案

雙輪平衡機器人的硬件電路主要由控制芯片STM32F103C8T6、測量模塊、直流電機驅動模塊、藍牙模塊組成。其中測量模塊主要是MPU6050、磁電編碼器兩部分。MPU6050測量的是機身的傾角θ與角加速度x（t），安裝在左右2個電機上的磁電編碼器將電機軸轉過的角度經放大電路放大，然后經單片機處理后輸出兩路AB相脈沖信號，最后得到車輪的轉速和方向。雙輪平衡機器人硬件系統結構如圖3所示。

圖3 系統硬件結構Fig.3 Hardware structure diagram of the system

2.2 STM32F103C8T6芯片

主控模塊的STM32F103C8T6單片機是控制器的核心部分。該單片機采用ST意法半導體公司生產的32位多功能、低成本、低功耗單片機，它的內核采用ARM公司最新生產的Cortex M3架構，最高工作頻率可達72 MHz，運算速度可達1.25DMIPS/MHz，片上64 KB的閃存、20 KB RAM，擁有7個定時器。片上集成通信接口有2個I2C、3個USART、2個 SPI、1個 CAN 和 1個 USB。

控制機器人需要的端口如下：①2個PWM輸出，用于控制2個電機的轉速，用PWMA和PWMB表示。在此使用定時器3的兩個輸出端口PB0和PB1來輸出PWM信號；②4個gpio口用于控制2個電機的正反轉與停止。2個電機分別表示為motor1和motor2， 端口PA0、PA1控制motor1， 端口PB6、PB7控制motor2；③2個I2C模擬gpio端口，用于控制獲取6軸模塊的數據，使用的端口表示為SCL和SDA；④4個編碼器輸入接口，左輪的編碼器使用了定時器2的編碼器模式，需連接到定時器2的TIM2 CH1和TIM2 CH2端口（即PA0、PA1的端口）右輪的編碼器使用了定時器4的編碼器模式，使用的端口為TIM4 CH1和TIM4 CH2端口（即PB6、PB7的端口）；⑤2個藍牙模塊接口，主要用于接收和發送手機遙控信號，分別連接至單片機的PB10、PB11引腳上。

2.3 電源管理模塊設計

電源模塊由若干相互獨立的穩壓電源電路組成。這樣有利于為各模塊提供合適的電源并有利于減少模塊相互間的干擾。

整個系統的電源來源為12V 18650鋰電池，5 V電源由7805穩壓器提供。7805穩壓器用的是三端集成穩壓電路，只有3條引腳輸出，分別是輸入端、接地端和輸出端。3.3 V電源則由LM2596S提供，LM2596是3 A電流輸出降壓開關型集成穩壓芯片。利用該器件只需極少的外圍器件便可構成高效穩壓電路，提供的有3.3 V、5 V、12 V及可調（-ADJ）等多個電壓檔次產品。選擇3 V的電壓檔。

2.4 使用MPU6050測量姿態角度

MPU6050為全球首例整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器的軸間差問題，減少了大量的包裝空間以及計算誤差。MPU6050對陀螺儀和加速度計分別用了3個16位的ADC，將其測得的模擬量轉化為可輸出的數字量[6]。MPU6050的對運動狀態的檢測比較靈敏，可對快速和慢速的動作進行準確的跟蹤，使用者可對傳感器的測量范圍進行設置，陀螺儀的測量范圍為±250°/s，±500°/s，±1000°/s，±2000°/s， 加速度計的測量范圍為±2 g，±4 g，±8 g，±16 g。一個片上的 FIFO有1024 B，起到降低系統功耗的作用。本次的MPU6050的通信采用的接口為 400 MHz的I2C。另外，片上內嵌了溫度傳感器和在工作環境下僅有±1%變動的振蕩器。SDA和SCL連接到單片機，INT產生終端信號，連接至單片機。AD0接地。MPU6050綜合利用了陀螺儀和加速度計的特點，優勢互補獲得較準確的姿態角度。方法是將模塊的姿態數據輸出表示，作為系統的狀態量，模塊的姿態運動學方程作為濾波的狀態轉移方程，加速度信息作為濾波的觀察量信息，然后利用卡爾曼濾波（本次利用的是簡易卡爾曼濾波）的計算方法迭代計算更新，從而可得到精度較高的不漂移的三軸角度。

2.5 電機及驅動設計

電機型號選擇JGA25-370，采用齒輪組減速，減速比為1∶34。智能小車常用的直流電機驅動一般是L298N，但本設計使用的電機驅動是TB6612FNG，因為與L298N相比，TB6612FNG在效率上有更顯著提高，而且體積也變得更小，其大小與1元硬幣相當。

TB6612FNG內部電路是MOSFET的H橋集成電路，效率遠高于晶體管H橋驅動器。相比L298N的熱耗性和外圍二極管續流電路，它無需外加散熱片，外圍電路簡單，只需外接電源濾波電容就可以直接驅動電機，系統尺寸大幅減小。由于啟動電機的瞬間電流很大，會瞬間拉低整個系統的電壓，導致其他元器件無法正常工作，而且還有可能導致電機電源輸入端電路燒壞。所以，需要在電池電源輸入端加上較大的濾波電容C2。

2.6 磁電編碼器

使用磁電編碼器為61302H2系列霍爾永磁編碼器。該編碼器為兩通道增量式磁電編碼器，可以直接固定于電機尾部電源腳上，磁柵固定到電機尾軸上。霍爾永磁編碼器包含1個磁柵和2個磁敏檢測傳感器，輸出2個通道正交相位角90°的方波。永磁編碼器響應速度快、抗震性強、傳輸距離遠，在惡劣的環境下使用有較長的使用壽命和較高的可靠性。

2.7 藍牙模塊選擇

藍牙模塊選擇BC04，與手機的藍牙匹配后，通過手機上裝載的遙控軟件，可實現對雙輪平衡機器人的遙控和輔助調試，驗證雙輪平衡機器人在保持平衡的同時，還能做前進、后退、轉彎的動作。

3 控制系統軟件總體設計

軟件設計是雙輪平衡機器人控制系統的關鍵。本次軟件設計實現的主要功能包括系統初始化、機器人姿態信息的采集和卡爾曼濾波處理、速度檢測、機器人運動控制（PID）、直立控制、速度控制及轉向控制、藍牙遙控和限幅控制。

圖4所示為雙輪平衡機器人的主程序流程圖，機器人通電運行后，便進行單片機的初始化。初始化包括兩部分，第一部分是對單片機所用到的模塊進行初始化；第二部分是應用程序初始化，是對于機器人控制程序中應用到的變量值進行初始化。第一部分代碼可以通過網上下載的STM32庫進行移植實現[7-8]。初始化結束后，先進入機器人直立檢測程序，并完成對機器人的姿態信息采集。該段程序中，單片機讀取MPU6050的數值判斷是否已完成對機器人的姿態信息采集。如果一旦采集完成，則啟動機器人直立控制、速度控制及轉向控制。程序在控制機器人運動的同時，也向單片機不斷地反饋機器人的運動狀態，判斷機器人是否跌倒。跌倒可通過檢測機器人的傾角是否超過一定范圍進行判斷。如果機器人跌倒，則停止機器人運行，然后重新進入姿態信息采集過程。

圖4 主程序框架Fig.4 Main program flow chart

4 系統調試結果

4.1 姿態檢測系統調試

如圖5所示，串口助手顯示的Pitch、Roll、Yaw分別代表三軸的角度，而本設計中，實際計算需要的只有Pitch（俯仰角），其大小等于機身傾角θ（穿過機身的水平坐標軸與水平面的夾角）；顯示的ACCEL、GYRO則分別表示三軸加速度、三軸角速度。通過觀察Pitch的值變化可以發現，機器人剛開始運行時機身傾角在0°附近變化，在0°附近說明控制系統能保持雙輪機器人保持直立。

圖5 串口輸出數據Fig.5 Serial output data

4.2 實際運行效果

為了更好地測試雙輪平衡機器人的動態性能，利用手機藍牙與其上的藍牙模塊匹配后對其進行遙控，檢驗機器人的運動性能。藍牙控制界面如圖6所示。

圖6 藍牙控制界面Fig.6 Bluetooth control interface

滑動界面上藍色的圓圈，可讓雙輪平衡機器人往滑動的方向移動。經過多次的測試和調整，本次設計的雙輪平衡機器人控制系統可以讓機器人在受干擾的條件下保持運動的穩定性，其穩定性及抗干擾能力較好。其運行效果如圖7所示。

圖7 運行效果圖Fig.7 Operation effect diagram

5 結語

基于STM32設計的雙輪平衡機器人的控制系統在驗證試驗中取得了良好的結果，各功能模塊相互協調并能正常工作，在無人干預和適量干擾的情況下，機器人都能夠自主調整并迅速恢復穩定狀態，因此為雙輪平衡機器人應用到實際提供了較好的依據。

參考文獻：

[1]K.Y.Cai，L.Zhang.Fuzzy reasoning as a control problem[J].IEEE Transactions on Fuzzy Systems，2008，16（3）：600-614.

[2]L.A.Zadeh.The concept of a linguistic variable and its application to approximate reasoning-I[J].Springer US，1974，8（3）：199-249.

[3]羅綏輝.四旋翼控制系統的硬件電路設計研究[J].中國機械，2014（16）：201-210.

[4]惠易佳，姜羨，朱再強.基于Arduino平臺的自平衡二輪排爆車[J].科技視界，2015（2）：22-30.

[5]胡從坤，余澤宇，陳曦晨.四旋翼飛行器控制系統研究[J].科技廣場，2014（6）：61-64.

[6]戴維，岳明，肖鵬程，等.單軸雙輪自平衡小車的設計研究[J].物理實驗，2014（12）：33-36.

[7]甘家輝.兩輪自平衡機器人的系統設計及運動控制研究[J].模式識別與智能系統，2010（22）：45-47.

[8]戴福全，李濟澤，朱悅涵，等.兩輪自平衡機器人轉向穩定性優化研究[J].機械設計與制造，2017（4）：256-258.