舒英利， 寇 磊， 苑全德， 皮玉珍

(1.長(zhǎng)春工程學(xué)院電氣與信息工程學(xué)院，吉林 長(zhǎng)春 130012；2.東北電力大學(xué)信息工程學(xué)院，吉林 吉林 132012)

雙超聲波數(shù)據(jù)融合的兩輪機(jī)器人平衡行進(jìn)策略研究

舒英利1， 寇 磊2， 苑全德1， 皮玉珍1

(1.長(zhǎng)春工程學(xué)院電氣與信息工程學(xué)院，吉林 長(zhǎng)春 130012；2.東北電力大學(xué)信息工程學(xué)院，吉林 吉林 132012)

針對(duì)移動(dòng)輪式自平衡機(jī)器人在生產(chǎn)應(yīng)用中存在的問(wèn)題，對(duì)其結(jié)構(gòu)特點(diǎn)與姿態(tài)平衡的控制策略進(jìn)行了研究。為了保障機(jī)器人的平衡與協(xié)調(diào)運(yùn)動(dòng)，提出了基于雙超聲波測(cè)距數(shù)據(jù)融合的平衡和行進(jìn)策略。采用雙超聲波數(shù)據(jù)融合平衡算法，在Arduino主控的機(jī)器人上實(shí)現(xiàn)了自平衡姿態(tài)的控制，對(duì)雙超聲波傳感器數(shù)據(jù)信息進(jìn)行處理，從而保證了機(jī)器人的自平衡運(yùn)動(dòng)；同時(shí)，在機(jī)器人的運(yùn)動(dòng)過(guò)程中加入了PID控制算法，以調(diào)整機(jī)器人的運(yùn)動(dòng)狀態(tài)及加快機(jī)器人對(duì)信號(hào)誤差處理的速度。試驗(yàn)與仿真結(jié)果表明，該策略設(shè)計(jì)有效可行，雙超聲波自平衡機(jī)器人可以保持較好的平衡姿態(tài)，行進(jìn)控制與基本方向控制也有效可靠，體現(xiàn)了較好的平衡穩(wěn)定性和平衡協(xié)調(diào)性。在平衡算法控制下，雙超聲波機(jī)器人不僅實(shí)現(xiàn)了平衡行進(jìn)，還實(shí)現(xiàn)了行進(jìn)策略下的平衡后退以及方向控制下的平衡轉(zhuǎn)彎，具有廣闊的應(yīng)用前景。

機(jī)器人; 傳感器； 超聲波； 數(shù)據(jù)； 控制； 平衡； 策略

0 引言

在復(fù)雜、狹窄的大轉(zhuǎn)角工作環(huán)境下，兩輪自平衡移動(dòng)式機(jī)器人可以靈活、快捷地執(zhí)行任務(wù)。這種移動(dòng)式機(jī)器人兩輪共軸，機(jī)身重心置于輪軸重心上方，可憑借前后運(yùn)動(dòng)保持機(jī)身的相對(duì)平衡，實(shí)現(xiàn)動(dòng)態(tài)平衡的直立行走。由于具有體積小、運(yùn)動(dòng)靈活、適應(yīng)地形變化能力強(qiáng)等優(yōu)點(diǎn)，兩輪自平衡移動(dòng)式機(jī)器人可完成一些狹小環(huán)境內(nèi)的任務(wù)[1-2]。在復(fù)雜環(huán)境下能對(duì)環(huán)境高效感知和及時(shí)調(diào)整自身姿態(tài)，是自平衡機(jī)器人的必備功能。由于單一的傳感器(比如超聲波傳感器)感知能力有限，因此需要由多個(gè)傳感器交互實(shí)現(xiàn)有效協(xié)調(diào)，提高工作效率，同時(shí)在不同的應(yīng)用空間與任務(wù)空間中實(shí)現(xiàn)多傳感器數(shù)據(jù)融合[3-5]。

為改進(jìn)兩輪自平衡移動(dòng)式機(jī)器人的性能，提出了兩輪自平衡機(jī)器人姿勢(shì)平衡和行進(jìn)策略，利用雙超聲波測(cè)距檢測(cè)平衡與行進(jìn)特征，通過(guò)不斷檢測(cè)收集超聲波傳感器與地面的距離，進(jìn)行數(shù)據(jù)濾波融合，以提高兩輪自平衡機(jī)器人的控制效果[6-7]。雙超聲波檢測(cè)提高了控制精度和集成度，在保證良好的平衡姿態(tài)和移動(dòng)控制效果的前提下，有效降低了整體生產(chǎn)成本，并運(yùn)用配套的數(shù)據(jù)融合處理算法，實(shí)現(xiàn)了對(duì)平衡姿態(tài)與行進(jìn)姿態(tài)的調(diào)控。

1 雙超聲波姿態(tài)平衡動(dòng)力學(xué)模型

兩輪自平衡移動(dòng)式機(jī)器人的基本平衡控制原理為:當(dāng)檢測(cè)到車體產(chǎn)生傾斜時(shí)，對(duì)主控系統(tǒng)通過(guò)檢測(cè)得到的數(shù)據(jù)進(jìn)行融合、得出矯正的角度值與輸出的占空比。自平衡機(jī)器人主要通過(guò)驅(qū)動(dòng)2個(gè)輪子的電機(jī)所產(chǎn)生的轉(zhuǎn)動(dòng)力矩來(lái)控制機(jī)身傾斜方向的速度v，通過(guò)控制電機(jī)控制機(jī)身平衡姿態(tài)的調(diào)整，讓兩個(gè)輪子朝機(jī)身傾斜倒下的方向運(yùn)動(dòng)，進(jìn)而保持機(jī)身的動(dòng)態(tài)平衡。平衡控制過(guò)程中，直接控制量為輪子的轉(zhuǎn)動(dòng)力矩[8]。

(1)

由式(1)得速度模型：

(2)

自平衡機(jī)器人受力分析圖如圖1所示。圖1中，O為輪軸中心，P為機(jī)身重心，機(jī)體質(zhì)量為m，重力加速度為g，機(jī)軸傾角為α，超聲波A直射到地面的距離為L(zhǎng)a，超聲波B直射到地面的距離為L(zhǎng)b，重心距機(jī)身下的平臺(tái)的距離為L(zhǎng)。設(shè)機(jī)體以加速度a向一邊傾斜，則受到水平方向的最優(yōu)慣性合力為：

∑F*=Ma

(3)

若要使機(jī)身保持平衡狀態(tài)，則∑M=0(M為力矩)。以O(shè)點(diǎn)為轉(zhuǎn)軸中心，支持力N作用在轉(zhuǎn)軸上，力矩M=0，即:

mgLsinθ+F*=0

(4)

圖1 自平衡機(jī)器人受力分析圖

2 雙超聲波測(cè)距控制方案

本研究中的自平衡機(jī)器人應(yīng)用2個(gè)超聲波傳感器模塊檢測(cè)距離數(shù)據(jù)。選用HC-SR04超聲波測(cè)距模塊，Arduino主控板通過(guò)一階濾波的數(shù)據(jù)融合換算后得出機(jī)身當(dāng)前狀態(tài)的角度值α與角速度ω，再經(jīng)過(guò)PID參數(shù)整定生成對(duì)應(yīng)的PWM輸出值，形成一個(gè)“雙超聲波測(cè)距、定位高度比較計(jì)算、角度值偏差計(jì)算、矯正數(shù)據(jù)融合、偏差調(diào)整-輸出控制”的自平衡實(shí)現(xiàn)過(guò)程。

2.1 超聲波測(cè)距原理與誤差調(diào)整

采用時(shí)間差測(cè)距的方式來(lái)完成距離測(cè)量，超聲波傳感器發(fā)射端向探頭正方向發(fā)射特定頻率的超聲波，內(nèi)部電路的定時(shí)器在發(fā)射超聲波時(shí)開(kāi)始計(jì)時(shí)。超聲波發(fā)射與回響檢測(cè)時(shí)序圖如圖2所示。

圖2 超聲波發(fā)射頻率時(shí)序圖

發(fā)射電路會(huì)根據(jù)所給的脈沖發(fā)出特定頻率的超聲波，超聲波碰到被測(cè)物界面則會(huì)被反射回接收端。文獻(xiàn)[9]采用了超聲波傳感器接收探頭接收到反射波后定時(shí)器停止計(jì)時(shí)的方法，進(jìn)而得出超聲波發(fā)射和接收回波的時(shí)間差t。超聲波在傳播介質(zhì)中的傳播速度為c(在空氣中為340 m/s)。通過(guò)計(jì)算，得出從TXD端到被測(cè)界面之間的距離s為：

s=(ct)/2

(5)

超聲波應(yīng)用中，可由式(5)得出超聲波發(fā)出速度vi與接收端接收信號(hào)所需來(lái)回越渡時(shí)間的關(guān)系為：

(6)

由于超聲波頻率較高，可滿足多種用途的測(cè)量需要。但在低維度空間中，超聲波測(cè)量會(huì)受到周邊溫度、自身響應(yīng)冗余、回波遲緩、傳播介質(zhì)等因素的影響。超聲波在固體中傳播速度最快，液體次之，在氣體中傳播速度最慢。聲速c的表征速度與傳播介質(zhì)的溫度有關(guān)。為了實(shí)現(xiàn)超聲波傳感器的精確測(cè)量，文獻(xiàn)[9]提出在硬件中設(shè)置溫度補(bǔ)償電路，以補(bǔ)償因溫度變化而帶來(lái)的聲速誤差。如果環(huán)境溫度變化不明顯，則無(wú)需設(shè)置溫度補(bǔ)償。

(7)

(8)

針對(duì)這一誤差，在滿足超聲波傳感器的主要電氣參數(shù)的條件下，本系統(tǒng)選用了具有回聲保護(hù)的HC-SR04超聲波模塊，并且盡可能地把超聲波模塊A、B隔離開(kāi)。兩輪結(jié)構(gòu)也在一定程度上阻擋了超聲波的互相干擾。

2.2 超聲波數(shù)據(jù)融合平衡算法

通過(guò)超聲波測(cè)距獲取機(jī)器人在XOY極坐標(biāo)平面內(nèi)角度θ的變化，以實(shí)現(xiàn)機(jī)身的平衡。當(dāng)兩輪機(jī)器人平衡時(shí)，需滿足以下角度值換算公式：

(9)

式中：L為機(jī)身前后的2個(gè)超聲波模塊A、B間的直線距離；λ為平衡點(diǎn)高度。

在自平衡機(jī)器人基于雙超聲波的檢測(cè)中，假設(shè)采樣周期為100ms，在k時(shí)刻獲取超聲波實(shí)測(cè)值a、b,并結(jié)合(k-1)時(shí)刻的距離預(yù)測(cè)值。根據(jù)測(cè)量的預(yù)測(cè)協(xié)方差S對(duì)k時(shí)刻的測(cè)量值進(jìn)行濾波估計(jì)修正，進(jìn)而對(duì)(k+1)時(shí)刻的距離進(jìn)行預(yù)測(cè)。

測(cè)距獲取的平衡角度模型如圖3所示。

圖3 測(cè)距獲取的平衡角度模型

在估計(jì)平衡姿態(tài)機(jī)器人的運(yùn)動(dòng)狀態(tài)時(shí)，機(jī)器人的超聲波傳感器坐標(biāo)采用極坐標(biāo)系。設(shè)系統(tǒng)的狀態(tài)向量E(k)為：

(10)

系統(tǒng)平衡E(k)的狀態(tài)表達(dá)式為：

研究結(jié)果顯示，‘索邦’花青素苷合成轉(zhuǎn)錄因子LhsorMYB12編碼區(qū)序列長(zhǎng)720 bp，核苷酸序列長(zhǎng)863 bp(登錄號(hào)：MG593164)，共編碼239個(gè)氨基酸(圖2)。

E(k)=AE(k-1)+BU(k)+W(k-1)

(11)

式中：E(k)為第k次超聲波測(cè)量時(shí)的狀態(tài)向量;A為角度值的表征參數(shù)；W為角速度的表征參數(shù)；U(k)為平衡系統(tǒng)的速度控制量；W(k-1)為上一次檢測(cè)(k-1)狀態(tài)的角速度對(duì)當(dāng)前k狀態(tài)的噪聲干擾。

由于超聲波測(cè)距的采樣周期t為300 ms，而機(jī)器人保持平衡的運(yùn)動(dòng)速度較為平緩，所以在局部時(shí)間段內(nèi)設(shè)定調(diào)整過(guò)程為近似勻速運(yùn)動(dòng)，x方向的運(yùn)動(dòng)方程為：

(12)

式中:tk為第k次超聲波測(cè)量狀態(tài)時(shí)的采樣周期。

(13)

同理，可得y方向的運(yùn)動(dòng)方程為：

(14)

由式(14)可得平衡系統(tǒng)的狀態(tài)方程為：

E(k)=A×E(k-1)+W(k-1)

(15)

(16)

經(jīng)過(guò)PID數(shù)據(jù)整定后的PWM輸出方程為:

PWM|out=kapA+kadW+ksp×S+ksi×P

(17)

基于Arduino的雙超聲波機(jī)器人平衡驅(qū)動(dòng)系統(tǒng)經(jīng)過(guò)PID調(diào)整后，輸出實(shí)現(xiàn)姿態(tài)的平衡。具體的超聲波雙向測(cè)距定位平衡模型如圖4所示。由模型圖可得測(cè)量值Z(k)為：

圖4 雙向測(cè)距定位平衡模型

由于各狀態(tài)變量的誤差符合均值為0的高斯分布，故平衡策略下的測(cè)量方程為：

Z(K)=CE(k)+V(k)

(18)

式中:C為測(cè)量系統(tǒng)的表征參數(shù)，當(dāng)系統(tǒng)為多測(cè)量系統(tǒng)時(shí)，C為矩陣;V(k)為測(cè)量系統(tǒng)在k狀態(tài)下的噪聲誤差。由平衡系統(tǒng)在極坐標(biāo)下的雙超聲波測(cè)距徑向距離和融合后偏差方向角的測(cè)量數(shù)據(jù)可得:

(19)

測(cè)速碼盤對(duì)電機(jī)編碼器產(chǎn)生的PWM脈沖進(jìn)行記數(shù)，監(jiān)測(cè)機(jī)身的位移與輪子的速度。根據(jù)超聲波傳感器和姿態(tài)換算監(jiān)測(cè)車體運(yùn)動(dòng)姿態(tài)信號(hào)，經(jīng)過(guò)雙超聲波數(shù)據(jù)融合流程，文獻(xiàn)[11]通過(guò)相關(guān)的控制算法計(jì)算出輸出控制電壓的數(shù)值大小，對(duì)雙超聲波的2個(gè)驅(qū)動(dòng)電機(jī)進(jìn)行控制，進(jìn)而調(diào)整車體平臺(tái)的行進(jìn)姿態(tài)與平衡位置，從而使機(jī)身保持良好的運(yùn)動(dòng)協(xié)調(diào)性和平衡狀態(tài)[11-12]。

數(shù)據(jù)優(yōu)化融合前的PWM輸出近似波形與融合優(yōu)化后的實(shí)測(cè)波形如圖5所示。從波形對(duì)比中可以看出，經(jīng)過(guò)數(shù)據(jù)優(yōu)化融合后的波形更為穩(wěn)定、平緩，姿態(tài)控制效果更好。

圖5 數(shù)據(jù)優(yōu)化融合前后PWM波形對(duì)比圖

2.3 控制方案與行進(jìn)策略

Arduino如今已被運(yùn)用到各個(gè)領(lǐng)域，是一款使用Atmel Atmega328微處理控制器的開(kāi)源控制板。其開(kāi)發(fā)操作界面和搭建外設(shè)環(huán)境簡(jiǎn)易，有0～13個(gè)數(shù)字模擬I/O、0～5個(gè)Analog I/O，控制效果顯著。自平衡機(jī)器人采用超聲波電路Arduino為核心控制模塊，電機(jī)驅(qū)動(dòng)模塊為L(zhǎng)298N，電機(jī)為TT馬達(dá)，并以2個(gè)HC-SR04作為超聲波測(cè)距模塊，采用7.4 V/2 200 mAh鋰電池供電。電位器用于平衡點(diǎn)設(shè)置與PID中kp、ki和kd系數(shù)的調(diào)試，方便數(shù)據(jù)的更新與調(diào)試，有效減少了開(kāi)發(fā)時(shí)間與生產(chǎn)成本。

雙超聲波自平衡機(jī)器人的行進(jìn)控制主要通過(guò)PWM占空比輸出控制電機(jī)轉(zhuǎn)速大小，進(jìn)而調(diào)整角度值的偏差，使得角度值趨近系統(tǒng)平衡零點(diǎn)。

控制部分由無(wú)線藍(lán)牙遙控的代碼實(shí)現(xiàn)，功能實(shí)現(xiàn)為“前進(jìn)狀態(tài)”、“后退狀態(tài)”、“左轉(zhuǎn)狀態(tài)”、“右轉(zhuǎn)狀態(tài)”。從程序初始化參數(shù)設(shè)置可以看出，雙超聲波自平衡機(jī)器人的行進(jìn)控制方案是先調(diào)整平衡點(diǎn)和PID設(shè)定系數(shù)，然后調(diào)整左右電機(jī)的PWM輸出，從而實(shí)現(xiàn)行進(jìn)角度的控制與電機(jī)控制[13]。

3 試驗(yàn)與仿真

雙超聲波機(jī)器人運(yùn)用了主控Arduino，以簡(jiǎn)單、快捷的編程實(shí)現(xiàn)與I/O接口連接，通過(guò)雙超聲波傳感器進(jìn)行環(huán)境測(cè)距定位，并利用控制算法進(jìn)行機(jī)器人姿態(tài)平衡控制。在試驗(yàn)驗(yàn)證方面，通過(guò)仿真顯示超聲波采集到的回聲檢測(cè)數(shù)據(jù)。由超聲波接收端接收到的回聲檢測(cè)波形圖可知，平衡系統(tǒng)的姿態(tài)搖擺幅度不大，基本處于平衡零點(diǎn)上下，具備基本穩(wěn)定性與數(shù)據(jù)的可計(jì)算性。

假設(shè)2個(gè)超聲波傳感器經(jīng)過(guò)互補(bǔ)濾波后，換算出的角度數(shù)據(jù)為M1，超聲波采集數(shù)據(jù)為s，超聲波傳感器測(cè)距換算出的偏差角度值相對(duì)加速度的因子為k，前一次超聲波傳感器經(jīng)過(guò)濾波后的角度數(shù)據(jù)為M2，增益選擇固定值為g。濾波后的角度數(shù)據(jù)為realM，最后得出融合出的角度數(shù)據(jù)。

試驗(yàn)結(jié)果證明，雙超聲波自平衡機(jī)器人能有效保持平衡，在平衡算法控制下能平衡行進(jìn)；可實(shí)現(xiàn)行進(jìn)策略下的平衡后退和方向控制下的平衡轉(zhuǎn)彎，穩(wěn)定效果好。

4 結(jié)束語(yǔ)

為實(shí)現(xiàn)基于Arduino的雙超聲波機(jī)器人的姿態(tài)平衡與行進(jìn)控制，提出了雙超聲波傳感器數(shù)據(jù)融合策略。試驗(yàn)與仿真結(jié)果表明，該策略有效可行，雙超聲波自平衡機(jī)器人可以保持較好的平衡姿態(tài)，行進(jìn)控制與基本方向控制有效可靠，體現(xiàn)出了較好的平衡穩(wěn)定性和平衡協(xié)調(diào)性。該策略整體硬件開(kāi)發(fā)周期相對(duì)較短，選用材料性價(jià)比高，能滿足在較好控制效果的前提下降低生產(chǎn)成本的要求。在接下來(lái)的工作中，將對(duì)多超聲波定位自平衡機(jī)器人在上下坡和不平整復(fù)雜行動(dòng)界面上實(shí)現(xiàn)姿態(tài)平衡進(jìn)行研究。

[1] 劉斌.兩輪自平衡小車軟硬件研發(fā)與基于模糊線性化模型的變結(jié)構(gòu)控制研究[D].西安：西安電子科技大學(xué)，2009.

[2] 陳偉，延文杰，周超英，等.兩輪自平衡機(jī)器人控制系統(tǒng)的設(shè)計(jì)[J].傳感器與微系統(tǒng)，2008，27(4):117-120.

[3] 閆飛，莊嚴(yán)，王偉.移動(dòng)機(jī)器人基于多傳感器信息融合的室外場(chǎng)景理解[J].控制理論與應(yīng)用，2011，28(8):1093-1098.

[4] 蘇志毅，趙偉，黃松嶺.多傳感器信息融合技術(shù)在現(xiàn)代測(cè)量領(lǐng)域的地位和重要作用[J].電測(cè)與儀表，2013，50(3):1-5.

[5] 王洪波.物聯(lián)網(wǎng)信息融合技術(shù)及存在的問(wèn)題研究[J].計(jì)算機(jī)應(yīng)用研究，2013，30(8):2252-2255，2261.

[6] 陳輝，鄧記才，吳曉輝，等.多傳感器信息融合在輪式機(jī)器人運(yùn)動(dòng)控制中的應(yīng)用[J].傳感技術(shù)學(xué)報(bào)，2011，24(6):915-918.

[7] 張偉民，段曉明，趙艷花.兩輪自平衡小車控制研究[J].自動(dòng)化技術(shù)與應(yīng)用，2011，30(4):10-13.

[8] 孫建勤，何雅靜，孟祥忠.兩輪不平衡小車的優(yōu)化模糊控制研究[J].計(jì)算機(jī)仿真，2011，28(2):184-186，406.

[9] 張劍，王維，何俊峰，等.基于雙超聲波模塊的新型定位方法[J].傳感器與微系統(tǒng)，2012，31(9):22-24，31.

[10]蔣林，閆繼宏，臧希喆，等.一種新的超聲波絕對(duì)定位方法[J].吉林大學(xué)學(xué)報(bào)(工學(xué)版)，2009，39(1):188-193.

[11]蔡建羨，阮曉鋼.OCPA仿生自主學(xué)習(xí)系統(tǒng)及在機(jī)器人姿態(tài)平衡控制上的應(yīng)用[J].模式識(shí)別與人工智能，2011，24(1),138-146.

[12]朱效洲，李宇波，盧惠民，等.基于視覺(jué)的移動(dòng)機(jī)器人可通行區(qū)域識(shí)別研究綜述[J].計(jì)算機(jī)應(yīng)用研究，2012，28(6):1135-1137.

[13]阮曉鋼，蔡建羨，陳靜.強(qiáng)化學(xué)習(xí)規(guī)則的兩輪機(jī)器人自平衡控制[J].計(jì)算機(jī)測(cè)量與控制，2009，17(2):321-323.

Research on the Balanced Traveling Strategy Based on Dual Ultrasonic Data Fusion for Two Wheeled Robot

SHU Yingli1,KOU Lei2,YUAN Quande1,PI Yuzhen1

(1.School of Electrical and Information Engineering， Changchun Institute of Technology， Changchun 130012，China; 2.School of Information Engineering，Northeast Dianli University， Jilin 132012，China)

Aiming at the problems in production and application of the mobile self-balanced robot,its structural features and control strategy of posture balancing are researched.In order to ensure balanced and coordinated movement of the robot,the balanced traveling strategy based on dual ultrasonic distance measurement of data fusion is proposed.By utilizing the dual ultrasonic data fusion balancing algorithm,the control of self-balanced pose can be achieved in Arduino-controlled robots.In order to ensure the self-balanced motion of the robot,the dual ultrasonic sensor data information is processed;at the same time,the PID control algorithm is added in robot motion process,to adjust the motion status of robot and the speed of error processing is greatly improved.The results of experiment and simulation show that the proposed method is effective and feasible.The dual ultrasonic self balancing robot can maintain a good posture balance,and the moving control and basic direction control are effective and reliable,which reflects the better balance stability and coordination.Under control of balancing algorithm,the dual ultrasonic robot is not only balanced traveling,but also implementing balanced retreat,and balanced turning around under marching strategy,this possesses wide application prospects.

Robot； Sensor； Ultrasonic； Data； Control; Balance; Strategy

吉林省教育廳“十二五”科學(xué)技術(shù)研究項(xiàng)目(2014324)

舒英利(1965—)，男，碩士，高級(jí)試驗(yàn)師，主要從事電力電子應(yīng)用等研究。E-mail：shuyl@scitimes.com。 皮玉珍(通信作者)，女，碩士，主要從事智能電網(wǎng)技術(shù)、物聯(lián)網(wǎng)技術(shù)等方向的研究和應(yīng)用。E-mail：piyuzhen@gmail.com。

TH-3；TP27

A

10.16086/j.cnki.issn 1000-0380.201702010

修改稿收到日期:2016-06-22