范淇元，覃羨烘，李洪毅

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

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

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

1 雙輪平衡機器人的數(shù)學模型

1.1 雙輪平衡機器人的整體結(jié)構(gòu)

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

圖1 雙輪平衡機器人整體結(jié)構(gòu)三維圖Fig.1 3D structure of two wheeled balancing robot

1.2 平衡的實現(xiàn)

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

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

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

2 雙輪平衡機器人控制系統(tǒng)硬件設(shè)計

2.1 控制系統(tǒng)硬件總體設(shè)計方案

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

圖3 系統(tǒng)硬件結(jié)構(gòu)Fig.3 Hardware structure diagram of the system

2.2 STM32F103C8T6芯片

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

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

2.3 電源管理模塊設(shè)計

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

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

2.4 使用MPU6050測量姿態(tài)角度

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

2.5 電機及驅(qū)動設(shè)計

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

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

2.6 磁電編碼器

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

2.7 藍牙模塊選擇

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

3 控制系統(tǒng)軟件總體設(shè)計

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

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

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

4 系統(tǒng)調(diào)試結(jié)果

4.1 姿態(tài)檢測系統(tǒng)調(diào)試

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

圖5 串口輸出數(shù)據(jù)Fig.5 Serial output data

4.2 實際運行效果

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

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

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

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

5 結(jié)語

基于STM32設(shè)計的雙輪平衡機器人的控制系統(tǒng)在驗證試驗中取得了良好的結(jié)果，各功能模塊相互協(xié)調(diào)并能正常工作，在無人干預(yù)和適量干擾的情況下，機器人都能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)，因此為雙輪平衡機器人應(yīng)用到實際提供了較好的依據(jù)。

參考文獻：

[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]羅綏輝.四旋翼控制系統(tǒng)的硬件電路設(shè)計研究[J].中國機械，2014（16）：201-210.

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

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

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

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

[8]戴福全，李濟澤，朱悅涵，等.兩輪自平衡機器人轉(zhuǎn)向穩(wěn)定性優(yōu)化研究[J].機械設(shè)計與制造，2017（4）：256-258.