改進(jìn)TEB算法的多機(jī)器人動態(tài)避障策略研究

陳奕梅，沈建峰，李柄棋

(天津工業(yè)大學(xué)，天津 300000)

0 引言

近年來，多機(jī)器人系統(tǒng)[1]越來越多地應(yīng)用于生產(chǎn)、生活的諸多領(lǐng)域，與之相關(guān)的智能算法也得到了不斷發(fā)展，自主規(guī)劃和環(huán)境感知作為移動機(jī)器人的重要屬性，其依賴的算法將直接影響到規(guī)劃的軌跡和避障性能的優(yōu)劣，因此，對于多機(jī)器人在復(fù)雜環(huán)境下的軌跡規(guī)劃算法和智能避障算法的研究尤為重要。

根據(jù)機(jī)器人對于環(huán)境信息側(cè)重點的不同，通常將路徑規(guī)劃算法分為全局路徑規(guī)劃算法和局部路徑規(guī)劃算法[2-3]。全局路徑規(guī)劃算法主要作用于預(yù)先知曉的全局靜態(tài)環(huán)境，處理有未知或動態(tài)障礙物的情況時則效果較差，經(jīng)典的全局路徑規(guī)劃算法主要有Dijkstra算法[4]、A*算法[5]、D*算法[6]、RRT算法[7]等；局部路徑規(guī)劃算法則是運用于部分環(huán)境已知或者環(huán)境完全未知的情況下，此時算法側(cè)重考慮局部的環(huán)境信息，應(yīng)用自身傳感器獲取的信息使機(jī)器人獲得良好的實時避障和規(guī)劃能力。因此，綜合全局和局部路徑規(guī)劃算法的優(yōu)缺點，通常將兩種算法配合使用。對于局部規(guī)劃算法，R?ESMANN等于2012年在彈性帶(Elastic Band,EB)算法[8]基礎(chǔ)上改進(jìn)并提出了時間彈性帶(Timed Elastic Band,TEB)算法[9-10]，該算法是基于多目標(biāo)優(yōu)化，使得機(jī)器人在滿足其運動學(xué)約束的前提下能夠輸出平滑的軌跡，并具有動態(tài)避障能力[11-12]，但是受限于優(yōu)化目標(biāo)的約束函數(shù)，在機(jī)器人面臨碰撞沖突或特殊情況時，該算法程序可能會將硬約束轉(zhuǎn)換為軟約束，導(dǎo)致機(jī)器人速度輸出不合理。FIORINI等于1998年提出了速度障礙(Velocity Obstacle,VO)算法[13-14]，理論上，應(yīng)用其思想可以避開所有動態(tài)障礙物，但是該算法沒有考慮機(jī)器人的運動學(xué)限制，會造成速度波動甚至速度不可達(dá)的情況。

綜上發(fā)現(xiàn)，TEB算法能在滿足機(jī)器人運動學(xué)限制的前提下優(yōu)化輸出合理的速度指令，但是算法有可能突破硬約束而造成避障不及時或者速度波動,而VO算法雖然理論上可以躲避所有的動態(tài)障礙物，但卻不能保證完全遵循機(jī)器人的運動學(xué)限制。為此，將TEB算法和VO算法相融合，提出了TEB-VO軌跡規(guī)劃算法，以優(yōu)化TEB算法在碰撞沖突時的軌跡和速度。此外，為進(jìn)一步優(yōu)化軌跡和避碰速度，又針對規(guī)劃軌跡的離散間隔和機(jī)器人最大速度設(shè)計了一個參數(shù)自適應(yīng)模塊，用于更好地處理多機(jī)器人在復(fù)雜環(huán)境下的動態(tài)避障和軌跡規(guī)劃問題。

1 基本概念

TEB算法是針對全局路徑規(guī)劃器生成路徑的后續(xù)優(yōu)化算法，其生成的局部軌跡由一系列連續(xù)的時間和位姿序列組成，算法將機(jī)器人的位姿定義為

Xi=(xiyiβi)T

(1)

式中：Xi為機(jī)器人坐標(biāo)系中第i個位姿，包括位置信息xi，yi和角度βi，相鄰位姿Xi和Xi+1之間的時間間隔用ΔTi表示，如圖1所示。

圖1 TEB算法的時間間隔和位姿序列Fig.1 Time interval and pose sequence of the TEB

優(yōu)化過程中,TEB算法應(yīng)用圖優(yōu)化的思想，將機(jī)器人相鄰的時間間隔和狀態(tài)作為圖優(yōu)化節(jié)點，將速度、加速度和機(jī)器人非完整約束作為圖優(yōu)化的邊，同時考慮障礙物信息、規(guī)劃軌跡的離散間隔和相鄰幾個時間、空間序列約束，最后用G2O求解器計算出控制量V(v,ω)(v和ω分別為機(jī)器人的線速度和角速度)，從而得到最優(yōu)的軌跡。

TEB算法通過加權(quán)多目標(biāo)優(yōu)化獲得最優(yōu)的位姿點，目標(biāo)函數(shù)的數(shù)學(xué)描述為[9]

(2)

(3)

式中：f(B)為綜合考慮各種約束的目標(biāo)函數(shù)；fk(B)為各個約束函數(shù)；γk為每項的權(quán)值；B*為最優(yōu)的TEB軌跡。

TEB算法的約束函數(shù)通常有以下4個[9]。

1)路徑跟隨和障礙物約束函數(shù)。

算法將分段連續(xù)、可微分的函數(shù)視為約束，且懲罰不符合約束的行為，具體形式為

(4)

fpath=eΓ(dmin，j,rpmax,ε,S,n)

(5)

fob=eΓ(-dmin，j,-romin,ε,S,n)

(6)

并基于此構(gòu)建懲罰函數(shù)fpath，fob。其中:xr為邊界;ε為偏移因子;S為縮放比例;n為階數(shù);dmin，j為自變量，表示路徑點與障礙物之間的距離;rpmax為軌跡偏離路徑點的最大距離;romin為軌跡與障礙物之間的最小距離。

2)機(jī)器人的速度和加速度約束。

(7)

ωi≈(βi+1-βi)/ΔTi

(8)

ai=2(vi+1-vi)/(ΔTi+ΔTi+1)。

(9)

3)非完整運動學(xué)約束。

算法仿真與實驗采用的Turtlebot3機(jī)器人為差分驅(qū)動結(jié)構(gòu)，擁有兩個移動自由度，其不能沿著機(jī)器人坐標(biāo)系的y軸方向進(jìn)行平動；機(jī)器人的兩個相鄰位姿所在圓弧的曲率可近似為不變，機(jī)器人坐標(biāo)系下相鄰位姿的轉(zhuǎn)角θi和方向向量di,i+1的外積等于轉(zhuǎn)角θi+1和方向向量di,i+1的外積，βi表示全局坐標(biāo)系下機(jī)器人的朝向，相應(yīng)的關(guān)系式和非完整運動學(xué)約束分別為

(10)

(11)

(12)

目標(biāo)函數(shù)fk(Xi,Xi+1)懲罰違反此約束的二次誤差，以此保證機(jī)器人輸出速度遵循非完整運動學(xué)約束。

4)最快路徑約束。

TEB算法加入了位姿間的時間間隔信息，總的時間為所有時間間隔的累加和，相關(guān)的目標(biāo)函數(shù)為

(13)

2 針對TEB算法的改進(jìn)方案

2.1 TEB-VO軌跡規(guī)劃算法

使用全局路徑規(guī)劃算法配合TEB局部軌跡規(guī)劃算法進(jìn)行多機(jī)器人動態(tài)避障且發(fā)生碰撞沖突時，由于算法是對約束函數(shù)的組合優(yōu)化，考慮優(yōu)化效率的情況下不免會出現(xiàn)小的違規(guī)行為，即采用較小懲罰因子的成本函數(shù)組合時,算法的約束函數(shù)會被突破成軟約束，那么機(jī)器人則有可能出現(xiàn)速度波動大、走走停?；蛘哕壽E較差甚至碰撞的情況。而VO算法[13]作為移動機(jī)器人用于處理動態(tài)障礙物的方法,其理論上可以避開所有動態(tài)障礙物，直觀地理解,速度障礙就是機(jī)器人當(dāng)前速度方向與移動障礙物速度方向相交的部分，即未來規(guī)劃的時間間隔內(nèi)會發(fā)生碰撞的區(qū)域。因此，結(jié)合TEB算法和VO算法提出了TEB-VO軌跡規(guī)劃算法，進(jìn)一步對全局路徑下的軌跡進(jìn)行優(yōu)化，在碰撞沖突時對TEB算法計算輸出的速度進(jìn)行二次約束。

圖2 速度障礙示意圖Fig.2 Schematic diagram of velocity obstacle

改進(jìn)算法核心在于計算TEB算法輸出的速度是否在碰撞沖突區(qū)內(nèi)，若在則剔除，轉(zhuǎn)而進(jìn)行再規(guī)劃，選擇一個次優(yōu)速度，使速度一直處于安全范圍之內(nèi)，從而做到完全避碰。圖中,T1,T2分別為兩個機(jī)器人，VT1，VT2分別是兩個機(jī)器人的速度，則定義T1,T2的相對速度為V12，即

V12=VT1-VT2

(14)

此時，機(jī)器人T2相對于T1靜止，ZPT區(qū)域則為機(jī)器人T1的位置障礙，設(shè)ΔT內(nèi)相對速度V12保持不變，以機(jī)器人T1為基準(zhǔn)向機(jī)器人T2和其膨脹區(qū)域做射線lT,則機(jī)器人T1，T2碰撞的條件是

lT∩ZPT≠?

(15)

滿足上式的V12的集合定義為相對沖突區(qū)(Relative Conflict Zone,RCZ)，即

ZRCZ={V12|lT∩ZPT≠?}

(16)

將相對沖突區(qū)RCZ與VT2做閔可夫斯基矢量和運算，即在ΔT內(nèi)把RCZ區(qū)域以速度VT2進(jìn)行平移，得到的區(qū)域定義為絕對沖突區(qū)(Absolute Conflict Zone,ACZ)，即

ZACZ=ZRCZ⊕VT2·ΔT

(17)

(18)

要使機(jī)器人避免碰撞，則要求TEB算法規(guī)劃輸出的速度VT1new滿足

(19)

以保證規(guī)劃的速度始終安全。同時,為減小計算量，VO算法的計算僅在機(jī)器人之間距離小于一定閾值距離時才會被觸發(fā)，設(shè)置閾值距離為0.5 m。機(jī)器人應(yīng)用TEB-VO軌跡規(guī)劃算法流程如圖3所示。

圖3 TEB-VO算法流程圖Fig.3 Flow chart of the TEB-VO algorithm

應(yīng)用速度障礙的思想去二次約束TEB算法的輸出速度，將做到在碰撞沖突時完全避免機(jī)器人相撞，且可以彌補(bǔ)VO算法未考慮機(jī)器人運動學(xué)限制的問題，并能使沖突時的速度平穩(wěn)安全。

2.2 自適應(yīng)參數(shù)動態(tài)調(diào)節(jié)模塊

針對機(jī)器人在碰撞沖突時速度過快且軌跡不平滑的問題，設(shè)計了一個對于改進(jìn)算法的自適應(yīng)參數(shù)動態(tài)調(diào)節(jié)模塊。經(jīng)過大量的仿真調(diào)試發(fā)現(xiàn)，TEB算法規(guī)劃軌跡時兩個相鄰姿態(tài)間的離散距離對于輸出軌跡影響較大，該參數(shù)為dtref，圖4為其示意圖。

圖4 dtref示意圖Fig.4 Schematic diagram of dtref

理論上該參數(shù)越小則TEB規(guī)劃軌跡越細(xì)膩，相應(yīng)的計算量也越大，為了權(quán)衡計算量以及規(guī)劃效果，設(shè)置移動機(jī)器人之間的歐氏距離d作為評價依據(jù)，且當(dāng)機(jī)器人距離在一定閾值區(qū)間內(nèi)時模塊才會被激活，設(shè)置的閾值區(qū)間為[0.2，1.2]，單位為m，并將參數(shù)與距離做正比計算，這樣既保證了移動機(jī)器人在距離彼此較近時可獲得較為細(xì)膩的軌跡，又能避免在整個避障過程中整體計算量過大。相關(guān)算式為

(20)

(21)

經(jīng)過大量的仿真及實驗可知，當(dāng)α=0.1,β=0.18時，機(jī)器人在避障時的軌跡平滑度會有明顯的提高。

同時,又對機(jī)器人距離落在一定閾值區(qū)間內(nèi)時的最大速度進(jìn)行了自適應(yīng)調(diào)節(jié)，設(shè)置的閾值區(qū)間為[0.15，0.6]，單位為m，當(dāng)機(jī)器人之間的距離較近時，則機(jī)器人可達(dá)到的最大速度越小，以此來避免機(jī)器人在動態(tài)避障時可能面臨的速度過快問題。算法中表示機(jī)器人最大線速度的參數(shù)為mvel,算式如下

(22)

依據(jù)大量的仿真和實驗數(shù)據(jù)分析，當(dāng)η=0.05,γ=0.19時，機(jī)器人在碰撞沖突時能有效地控制速度。

3 仿真實驗及實物實驗驗證

3.1 仿真實驗

為驗證改進(jìn)的自適應(yīng)TEB-VO算法，設(shè)計了雙機(jī)器人在復(fù)雜環(huán)境下的沖突避免實驗，并與經(jīng)典的TEB算法進(jìn)行對比。實驗中采用2個Turtlebot3機(jī)器人(Wiffle Pi)作為平臺，機(jī)器人的出廠性能參數(shù)和實驗中用到的配置參數(shù)見表1。

表1 機(jī)器人出廠參數(shù)和配置參數(shù)值Table 1 Factory default parameters and configuration parameters of the robot

首先在ubuntu16.04下的機(jī)器人操作系統(tǒng)(Robot Operating System,ROS)里搭建仿真環(huán)境，使用三維物理仿真軟件Gazebo模擬了一個邊長為9 m的正方形房間，其中每個方格代表1 m,并設(shè)置了一些障礙物，如圖5所示。

圖5 模擬的復(fù)雜仿真環(huán)境Fig.5 A complex simulation environment

仿真實驗中依次啟動Gazebo物理仿真環(huán)境和二維代價地圖，利用Rviz可視化工具為機(jī)器人分別設(shè)置目標(biāo)點，其中,未改進(jìn)的原始對照組中機(jī)器人A,B均采取TEB算法，改進(jìn)后的實驗中機(jī)器人A采取自適應(yīng)TEB-VO算法、機(jī)器人B采取自適應(yīng)模塊結(jié)合的TEB算法。機(jī)器人A的起點為(1.00,-2.50,3.14),終點為(-0.50,3.80,3.14),機(jī)器人B的起點為(-2.50,1.00,-1.57)，終點為(3.90,0.00,-1.57)。機(jī)器人到達(dá)既定的目標(biāo)點后記錄2組機(jī)器人的線速度和運動軌跡，得到數(shù)據(jù)如圖6所示。

通過圖6(a)和圖6(b)可以看出，在機(jī)器人A,B面臨碰撞沖突的情況下，應(yīng)用自適應(yīng)TEB-VO算法的機(jī)器人A能夠在滿足運動學(xué)限制的前提下規(guī)劃輸出更為平緩的線速度，在機(jī)器人A,B距離較近時能夠合理降速避障，避免了避障時出現(xiàn)走走停停甚至速度過快發(fā)生碰撞的情況，而且速度波動較??；由圖6(c)和圖6(d)可知,改進(jìn)后的算法相較于TEB算法擁有更平滑的軌跡，而且軌跡長度也較短，其中，紅色線代表機(jī)器人A的運動軌跡，紫色線代表機(jī)器人B的運動軌跡。經(jīng)過多次仿真實驗得出的數(shù)據(jù)顯示，改進(jìn)后的自適應(yīng)TEB-VO算法與經(jīng)典的TEB算法相比，該算法能夠在更短的時間內(nèi)完成任務(wù)，具有時間上的優(yōu)越性。

圖6 TEB算法和改進(jìn)算法的仿真實驗對比圖Fig.6 Comparison diagram of the TEB algorithm and the improved algorithm

3.2 實物實驗

為了進(jìn)一步驗證改進(jìn)算法能夠在真實環(huán)境下完成預(yù)期的軌跡規(guī)劃和避碰效果，將算法移植到真實機(jī)器人上，應(yīng)用了2個Turtlebot3機(jī)器人進(jìn)行了實物實驗驗證。實物實驗采取的機(jī)器人參數(shù)和算法均與仿真實驗相同，實驗得到的機(jī)器人線速度曲線和運動軌跡如圖7所示。

受電機(jī)性能、實際環(huán)境下的摩擦力和控制信號傳輸?shù)木W(wǎng)絡(luò)延時影響，實際情況下機(jī)器人的響應(yīng)速度不如仿真環(huán)境下的響應(yīng)速度快，這反而造成使用原始TEB算法時的實物實驗比仿真實驗速度波動小，此時機(jī)器人幾乎趨近停止?fàn)顟B(tài)，但是這并不是合理的避讓策略。應(yīng)用改進(jìn)的自適應(yīng)TEB-VO算法且面臨碰撞沖突時，機(jī)器人能夠主動協(xié)調(diào)自身速度，做到梯度降速以實現(xiàn)合乎機(jī)器人運動學(xué)約束的避障。由圖7可以看出，實物實驗和仿真實驗得到的結(jié)果基本一致，改進(jìn)后的算法擁有波動較小的線速度曲線和更為平滑的軌跡，機(jī)器人運動的連貫性高，而且避障所用的時間較短，同時在避障時機(jī)器人之間的最小距離也能夠保持在安全距離之內(nèi)，避障的安全性有一定的保障，如圖8所示。

圖7 TEB算法和改進(jìn)算法的實物實驗對比圖Fig.7 Physical experimental comparison diagram of the TEB algorithm and the improved algorithm

圖8 實物實驗中機(jī)器人的位置關(guān)系Fig.8 Positional relationship of robots in physical experiments

4 結(jié)論

本文將TEB算法和VO算法相融合，提出了TEB-VO軌跡規(guī)劃算法，對優(yōu)化輸出的速度進(jìn)行了二次約束，使機(jī)器人能夠合理高效地解決碰撞沖突問題；與此同時，又設(shè)計了一個自適應(yīng)參數(shù)動態(tài)調(diào)節(jié)模塊，提高了機(jī)器人避障時運動軌跡的平滑度，并且對避障時的速度進(jìn)行了約束。最后，在ROS上進(jìn)行了仿真，之后將改進(jìn)算法移植到了Turtlebot3機(jī)器人上進(jìn)行實物實驗，得出的速度曲線和軌跡圖等數(shù)據(jù)均表明了改進(jìn)算法的有效性和優(yōu)越性。今后的工作重點是把改進(jìn)算法應(yīng)用于3個及以上的機(jī)器人環(huán)境里，并針對速度障礙區(qū)域的大小和形狀做進(jìn)一步優(yōu)化，使機(jī)器人的可選速度更多，避免出現(xiàn)因機(jī)器人數(shù)量增多而造成無合理輸出速度可選的情況。