韓竺秦 張麗娜

摘 要：介紹了兩輪自平衡機(jī)器人研究現(xiàn)狀，建立系統(tǒng)動(dòng)力學(xué)模型，并用MATLAB進(jìn)行仿真驗(yàn)證控制方法的有效性。設(shè)計(jì)了自平衡機(jī)器人控制系統(tǒng)，包括軟件和硬件系統(tǒng)。傳感器采用陀螺儀以及加速度檢測(cè)兩輪自平衡機(jī)器人重力方向的傾斜角度和車輪的旋轉(zhuǎn)加速度。經(jīng)過(guò)STM32控制器處理后，采用LQR最優(yōu)控制策略控制電機(jī)調(diào)整車輪狀態(tài)使機(jī)器人保持平衡。通過(guò)實(shí)驗(yàn)樣機(jī)驗(yàn)證了自平衡機(jī)器人控制策略的可行性。

關(guān)鍵詞：STM32;兩輪平衡機(jī)器人;LQR控制策略;動(dòng)力學(xué)控制;傳感器

DOI：10. 11907/rjdk. 181995

中圖分類號(hào)：TP319文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：1672-7800（2019）003-0086-05

0 引言

針對(duì)兩輪機(jī)器人運(yùn)動(dòng)平衡的控制方法研究較多[1-5]，如日本的Naoji Shiroma等[6]提出協(xié)作行為概念，應(yīng)用多倒立擺機(jī)器人協(xié)作搬運(yùn)物體，人與機(jī)器人協(xié)作實(shí)現(xiàn)物體搬運(yùn)。機(jī)器人由底部的一臺(tái)伺服電機(jī)驅(qū)動(dòng)實(shí)現(xiàn)直線運(yùn)動(dòng)，平衡控制采用極點(diǎn)配置方法。

瑞士聯(lián)邦工學(xué)院的Grasser等[7]制造了一個(gè)名為JOE的兩輪機(jī)器人。這是一款采用DSP控制的機(jī)器人，以陀螺儀作為姿態(tài)檢測(cè)傳感器，通過(guò)測(cè)得的角速度進(jìn)行積分而獲得傾角。

臺(tái)灣中興大學(xué)Shui-Chun等[8-9]設(shè)計(jì)的兩輪自平衡機(jī)器人，姿態(tài)測(cè)量采用傾角計(jì)和陀螺儀實(shí)現(xiàn)?？刂品椒ㄊ窍炔捎脗鹘y(tǒng)的PID控制，然后在同一平臺(tái)利用神經(jīng)網(wǎng)絡(luò)和自適應(yīng)滑?？刂品椒▽?shí)現(xiàn)控制。

兩輪自平衡機(jī)器人位置和姿態(tài)信息一般選擇陀螺儀、加速度計(jì)、傾角計(jì)、測(cè)距儀等傳感器獲取[10-11]，通過(guò)數(shù)據(jù)融合得到準(zhǔn)確的位姿信息。采用PID或極點(diǎn)配置法進(jìn)行平衡控制，也有少數(shù)采用模糊控制[12]。

兩輪自平衡機(jī)器人具有多變量、非線性、強(qiáng)耦合、時(shí)變、參數(shù)不確定性等特性，其平衡控制研究一直是熱點(diǎn)。由于該系統(tǒng)為欠驅(qū)動(dòng)系統(tǒng)，運(yùn)動(dòng)學(xué)方程不能完整地描述系統(tǒng)行為且系統(tǒng)不可控，因此要解決兩輪自平衡機(jī)器人系統(tǒng)的平衡問(wèn)題就必須考慮動(dòng)力學(xué)影響[5]。

本文建立系統(tǒng)的動(dòng)力學(xué)模型并進(jìn)行仿真，驗(yàn)證控制方法的有效性。兩輪自平衡機(jī)器人采用姿態(tài)檢測(cè)算法和最優(yōu)LQR控制策略算法。姿態(tài)檢測(cè)算法通過(guò)Kalman濾波器融合姿態(tài)傳感器測(cè)量數(shù)據(jù)，獲得準(zhǔn)確的姿態(tài)信息[13-14]。利用這些姿態(tài)信息，輸出控制信號(hào)控制電機(jī)轉(zhuǎn)動(dòng)使機(jī)器人得以平衡。

1 系統(tǒng)機(jī)械模型設(shè)計(jì)

自平衡機(jī)器人結(jié)構(gòu)種類繁多，本設(shè)計(jì)樣機(jī)采用圖1結(jié)構(gòu)，由上層電池層、中間主控層、底層電機(jī)驅(qū)動(dòng)層組成。電池層用于放置12V電池給系統(tǒng)供電，主控層由主控芯片最小系統(tǒng)和傳感器模塊組成，電機(jī)驅(qū)動(dòng)層接受單片機(jī)信號(hào)并控制電機(jī)。每層功能模塊的電路板之間用銅柱支撐固定，電機(jī)外殼與電機(jī)驅(qū)動(dòng)電路板固定，電機(jī)轉(zhuǎn)軸與兩只輪胎相連。

2 機(jī)器人動(dòng)力學(xué)模型

動(dòng)力學(xué)模型用于描述系統(tǒng)特性的數(shù)學(xué)關(guān)系，假設(shè)在純滾動(dòng)、沒(méi)有滑動(dòng)、車體和車輪等部件均為剛性[1]并忽略各種靜摩擦力的情況下，兩輪平衡機(jī)器人瞬時(shí)力平衡分析如圖2所示。

3 控制器設(shè)計(jì)

系統(tǒng)采用STM32F103C8T6為主控芯片，通過(guò)I2C接口讀取傳感器L3G4200和ADXL345的數(shù)據(jù)，陀螺儀和加速度計(jì)的數(shù)據(jù)融合測(cè)出平衡機(jī)器人的相關(guān)姿態(tài)，檢測(cè)機(jī)器人平臺(tái)的運(yùn)行姿態(tài)，最終通過(guò)PID輸出PWM電機(jī)控制信號(hào)對(duì)電機(jī)進(jìn)行有效控制，調(diào)整平臺(tái)運(yùn)行姿態(tài)和動(dòng)平衡。系統(tǒng)總體框架設(shè)計(jì)如圖4所示。

3.1 系統(tǒng)硬件設(shè)計(jì)

3.1.1 電源系統(tǒng)設(shè)計(jì)

系統(tǒng)電源采用12V電源供電，LM2596開(kāi)關(guān)電壓調(diào)節(jié)器是降壓型電源管理單片集成電路，能夠輸出3A的驅(qū)動(dòng)電流，同時(shí)具有很好的線性和負(fù)載調(diào)節(jié)特性[4-16]。STM32F103VET6以及其它外圍芯片都是3.3V供電，所以需要用到5V轉(zhuǎn)3.3V的AMS1117-3.3V電源芯片。這個(gè)電源芯片屬于線性電源芯片，比開(kāi)關(guān)電源芯片噪聲要小很多，最大輸出1A電流，給主控板供電完全夠用。電源供電系統(tǒng)如圖5所示。

3.1.2 加速度傳感器ADXL345

ADXL345是一款由ANALOG DEVICES 公司生產(chǎn)的3軸加速度計(jì)，該加速度傳感器電路如圖6所示。

ADXL345可以在傾斜檢測(cè)應(yīng)用中測(cè)量靜態(tài)重力加速度，還可測(cè)量運(yùn)動(dòng)或沖擊導(dǎo)致的動(dòng)態(tài)加速度，其高分辨率（3.9mg/LSB）能夠測(cè)量不到1.0°的傾斜角度變化[17]。主控芯片通過(guò)I2C即可初始化該芯片并讀取三軸的加速度值。單片機(jī)可通過(guò)此外部中斷觸發(fā)三軸加速度數(shù)據(jù)的讀取，用中斷觸發(fā)的方式降低系統(tǒng)計(jì)算周期，提高電機(jī)有效控制頻率。

從傳感器讀出的數(shù)據(jù)通過(guò)一定運(yùn)行求出傳感器的各類傾斜角，其中θ是平衡車要消除的角度，車身傾斜角度可通過(guò)公式（1）計(jì)算。式（1）中，A是各軸輸出的加速度分量。

3.1.3 陀螺儀傳感器L3G4200

ADXL345主要用來(lái)測(cè)量靜態(tài)重力加速度，但系統(tǒng)的動(dòng)態(tài)加速度值是干擾，用L3G4200測(cè)量出平衡車的角速度值能在一定程度上消除動(dòng)態(tài)干擾，利用ADXL345和L3G4200能夠解算出車的傾角信息。L3G4200提供一個(gè)16位數(shù)據(jù)輸出[18]，可配置低通和高通濾波器等嵌入式功能。與加速度傳感器的數(shù)字接口一致，通過(guò)SDA和SCL與主控芯片的硬件I2C接口進(jìn)行通訊，采用3.3V供電。應(yīng)用電路如圖7所示。

3.1.4 電機(jī)驅(qū)動(dòng)電路

使用減速比為1∶30 的GB37電機(jī)作為兩輪自平衡機(jī)器人的驅(qū)動(dòng)電機(jī)，電機(jī)采用H橋驅(qū)動(dòng)方式。電機(jī)驅(qū)動(dòng)器件采用TB6612FNG，具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)兩個(gè)GB37電機(jī)，外圍驅(qū)動(dòng)電路簡(jiǎn)單[19]。對(duì)于PWM信號(hào)輸入頻率范圍，高達(dá)100kHz的頻率足以滿足需求。

3.2 系統(tǒng)軟件設(shè)計(jì)

軟件程序設(shè)計(jì)流程如圖8所示。系統(tǒng)上電復(fù)位后，先初始化各個(gè)功能模塊，并進(jìn)行平衡機(jī)器人的姿態(tài)檢測(cè)和PID控制[20]。為查看前期調(diào)試數(shù)據(jù)，使用STM32主控串口，將程序中產(chǎn)生的數(shù)據(jù)估算出最終角度，通過(guò)UART串口上傳到上位機(jī)，可在串口測(cè)試軟件看到數(shù)據(jù)特性。

4 系統(tǒng)仿真

采用LQR控制策略對(duì)系統(tǒng)進(jìn)行控制，建立機(jī)器人模型如圖9所示。

圖10為兩輪自平衡機(jī)器人的速度控制曲線，反映了兩輪機(jī)器人的角度、傾角以及傾角角速度變化情況。從圖10可以看出，仿真曲線經(jīng)過(guò)大約2s的自調(diào)整最終恢復(fù)到0值，達(dá)到了動(dòng)態(tài)穩(wěn)定狀態(tài)，取得了較好效果。

在時(shí)間 t = 0時(shí)，機(jī)器人傾角[θ]角為正，平衡車整體向前傾。要讓機(jī)器人重新回到平衡狀態(tài)，在控制的作用下向前運(yùn)行重新回到直立狀態(tài)，[θ]角重新回零。在 0.4s時(shí)間內(nèi)，平衡機(jī)器人的相關(guān)參數(shù)均達(dá)到其峰值。經(jīng)過(guò)不斷調(diào)整，大約 2 s后系統(tǒng)最終達(dá)到新的動(dòng)態(tài)平衡狀態(tài)。

系統(tǒng)運(yùn)行穩(wěn)定后，在5s時(shí)間給平衡機(jī)器人突然加一擾動(dòng)，人為給平衡機(jī)器人加一個(gè)外力，如圖11所示。從圖11可以清楚地看出，傾角變化曲線有明顯峰值，速度曲線也隨之變化，經(jīng)過(guò)0.6s的自我調(diào)整，機(jī)器人趨于平衡穩(wěn)定，達(dá)到一個(gè)新的動(dòng)態(tài)平衡，本文構(gòu)建的兩輪平衡車實(shí)現(xiàn)穩(wěn)定控制和抗干擾目的。

5 結(jié)語(yǔ)

兩輪自平衡機(jī)器人是一個(gè)多變量、非線性、強(qiáng)耦合系統(tǒng)。本文分析了自平衡機(jī)器人的主要控制原理，對(duì)該系統(tǒng)的平衡瞬間動(dòng)力學(xué)進(jìn)行了分析，建立數(shù)學(xué)模型，采用最優(yōu)控制LQR控制策略，使平衡車系統(tǒng)的位移、角度、速度和加速度等變量趨于穩(wěn)定。用MATLAB進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明本文設(shè)計(jì)的兩輪平衡機(jī)器人能實(shí)現(xiàn)穩(wěn)定控制和抗干擾目的，滿足設(shè)計(jì)要求。

參考文獻(xiàn)：

[1] 路珺. 獨(dú)輪自平衡車設(shè)計(jì)方法研究[D] . 哈爾濱：黑龍江大學(xué)，2011.

[2] CHAN R P M，STOL K，HALKYARD? R. Review of modeling and control of two-wheeled robots[J]. Annual Re views in Control，2013（37）： 89-103.

[3] GRASSER F，D'ARRIGO A，COLOMBI S，et al. A mobile，inverted pendulum[J]. IEEE Trans， Ind Electron （90278-0046），2002，49（1）：107-114.

[4] 康冬亮，戰(zhàn)玉博，李憬宇. 基于自由擺的平板精確控制與激光追蹤系統(tǒng)[J]. 單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2012（10）：55-58.

[5] 茅力非. 兩輪自平衡移動(dòng)機(jī)器人建模與控制研究[D]. 武漢：華中科技大學(xué)， 2013.

[6] FENG T，LIU T，WANG X，et al. Modeling and imple- mentation of two-wheeled self-balancing robot equipped with sup- porting arms[C].? ICIEA，2011： 713-718.

[7] 詹長(zhǎng)書(shū)，邢博坤，樸光宇，等. 兩輪自平衡小車的控制策略設(shè)計(jì)[J]. 森林工程，2015，31（4）：73-76.

[8] LIN S L，TSAI C C，HUANG H C. Nonlinear adaptive sliding-mode control design for two-wheeled human transportation vehicle[C]. In 2009 IEEE International Conference on Systems， Man and Cybernetics， Piscataway， NJ， USA， 2009 ：1965-1970.

[9] TSAI C C，HUANG H C，LIN S C. Adaptive neural network control of a self-balancing? two-wheeled? scooter[J]. IEEE Transactions on Industrial Electronics， Apr，2010， 57（4）：1420-1428.

[10] 屠運(yùn)武. 自平衡控制系統(tǒng)的建模與仿真[J]. 系統(tǒng)仿真學(xué)報(bào)，2004，16（4）：938-841.

[11] 蔡建羨，馬洪蕊，程麗娜. 基于仿生策略的機(jī)器人自主導(dǎo)航方法研究[J]. 計(jì)算機(jī)仿真，2014，31（ 1） ： 333-338.

[12] 何述東. 多層前向神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)的研究進(jìn)展[J]. 控制理論與應(yīng)用，1998，15（3）：313-319.

[13] KAMEN，DEAN. Segway[EB/OL].? http：//www.segway.com/.

[14] 黃永平，聞雙云，相文超，等. 卡爾曼濾波在單神經(jīng)元PID控制中的應(yīng)用[J]. 吉林大學(xué)學(xué)報(bào)：理學(xué)版，2016，54（6）：1350-1354.

[15] 李亞文，翟金玲，彭曉邦. 一種位置式PID控制的四旋翼飛行器的設(shè)計(jì)與實(shí)現(xiàn)[J]. 自動(dòng)化儀器與儀表，2016（7）：96-100.

[16] 梁振濤，樊澤明，任永亮. 基于單片機(jī)的移動(dòng)監(jiān)控系統(tǒng)硬件設(shè)計(jì)[J]. 微型機(jī)與應(yīng)用，2014，33（2）：25-28.

[17] 周明安，郭天天，程小冬，等. 數(shù)碼手榴彈引信研究與設(shè)計(jì)[J]. 四川兵工學(xué)報(bào)，2014（8）：8-10.

[18] 王琪，呂雪霜. 基于MSP430F5438A單片機(jī)的無(wú)線定位裝置[J]. 技術(shù)與市場(chǎng)，2011（9）：163-164.

[19] 王建平，盧杉，武歡歡. TB6612FNG在直流電機(jī)控制設(shè)計(jì)中的應(yīng)用[J]. 電子設(shè)計(jì)工程，2010（6）：65-67.

[20] 趙智. 基于STM32的二輪自平衡電動(dòng)車系統(tǒng)研制[D]. 武漢：華中師范大學(xué)，2013.

（責(zé)任編輯：杜能鋼）