陳奕梅，沈建峰，李柄棋

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

0 引言

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

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

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

1 基本概念

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

Xi=(xiyiβi)T

(1)

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

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

(2)

(3)

式中：f(B)為綜合考慮各種約束的目標函數(shù)；fk(B)為各個約束函數(shù)；γk為每項的權值；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)

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

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

(7)

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

(8)

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

(9)

3)非完整運動學約束。

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

(10)

(11)

(12)

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

4)最快路徑約束。

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

(13)

2 針對TEB算法的改進方案

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

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

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

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

V12=VT1-VT2

(14)

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

lT∩ZPT≠?

(15)

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

ZRCZ={V12|lT∩ZPT≠?}

(16)

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

ZACZ=ZRCZ⊕VT2·ΔT

(17)

(18)

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

(19)

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

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

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

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

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

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

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

(20)

(21)

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

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

(22)

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

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

3.1 仿真實驗

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

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

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

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

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

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

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

3.2 實物實驗

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

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

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

圖8 實物實驗中機器人的位置關系Fig.8 Positional relationship of robots in physical experiments

4 結論

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