改進TEB算法的局部路徑規(guī)劃算法研究

代婉玉，張麗娟，吳佳峰，馬向華

上海應(yīng)用技術(shù)大學(xué) 電氣與電子工程學(xué)院，上海 201418

移動機器人是否具有自主感知[1]、能夠進行自主導(dǎo)航[2]是當(dāng)前人工智能領(lǐng)域研究的熱點。但現(xiàn)如今移動機器人的路徑規(guī)劃不僅要求利用算法找到一條從出發(fā)點到目標(biāo)點的無碰撞路徑，還要求機器人能夠在復(fù)雜動態(tài)環(huán)境中完成自適應(yīng)避障。只使用全局路徑規(guī)劃算法顯然不能適用于復(fù)雜環(huán)境下的機器人，需要局部路徑規(guī)劃算法與之配合。但是當(dāng)前幾種常用的局部路徑規(guī)劃算法還存在一些缺陷：人工勢場法[3]會使移動機器人陷入局部極小值和不可到達點。動態(tài)窗口法[4]只適用于全向移動機器人和差速移動機器人，并不適用于所有類型的機器人。隨機搜索樹算法[5]隨機性較大，導(dǎo)致最終生成的搜索路徑具有概率性。EB（elastic band）算法[6]應(yīng)用在動態(tài)環(huán)境中，該算法根據(jù)障礙物的位置和自身位姿信息使局部變形，使機器人同時受到相互制衡的拉力和排斥力，重新規(guī)劃一條避開障礙物的路徑。后EB算法中加入了對機器人的非完整運動學(xué)約束[7]，但始終沒有引入時間信息。

基于以上的研究，近幾年R?osmann等[8]新提出的TEB（time elastic band）算法是在EB算法的基礎(chǔ)上，加入了時間信息的思想，還考慮到了機器人的動力學(xué)和非完整約束，使其在兩輪機器人[9]和四足機器人[10]的適應(yīng)性都很強。將路徑規(guī)劃問題轉(zhuǎn)為圖優(yōu)化的問題，是動態(tài)系統(tǒng)中一種有效的時間最優(yōu)模型預(yù)測方法[11]，計算效率高、對全局規(guī)劃的要求小，廣泛應(yīng)用于物流領(lǐng)域和自動駕駛領(lǐng)域[12-13]。但在實際應(yīng)用過程中還是會存在速度跳變和機器人易受沖擊等問題。國內(nèi)外許多學(xué)者對傳統(tǒng)TEB算法做出了一些改進措施。

文獻[14]在求解過程中對問題施加連續(xù)優(yōu)化約束。文獻[15]加速度的變化率過大引起機器人受到?jīng)_擊震蕩，加入了加加速度的概念。文獻[16]在TEB算法中新加入了加速度變化率約束以改變障礙物比重。文獻[17]相比TEB算法最大速度與最大角速度的約束，在原有算法的基礎(chǔ)上進一步增加了速度與角速度的聯(lián)合約束。上述文章只是加入了機器人的速度約束，沒有考慮到障礙物速度過快時易發(fā)生碰撞。文獻[18]為了改進最優(yōu)軌跡的選取方法，在基于拓?fù)浣Y(jié)構(gòu)的并行TEB軌跡優(yōu)化方法的基礎(chǔ)上，增加了候選軌跡評分函數(shù)。文獻[19]通過增加危險懲罰因子約束，增加加速度跳變抑制約束、增加末端平滑約束。上述文章增加并行軌跡和增加過多約束會導(dǎo)致算法的計算量激增，對機器人的處理器要求極高，并不適合批量生產(chǎn)，在實際復(fù)雜環(huán)境中應(yīng)用性不高。

根據(jù)上述分析中算法存在的問題，對TEB算法的進行更適合于復(fù)雜環(huán)境下的改進。在原有的TEB算法中加入遇到障礙物后的膨脹化處理，使其適用于不同形狀的障礙物環(huán)境中。針對碰撞安全性，對機器人可運行區(qū)域劃分等級。加入速度與障礙物距離的約束來降低速度跳變導(dǎo)致的規(guī)劃路徑不平滑。為驗證算法可行性，分別在仿真中設(shè)置靜態(tài)環(huán)境和動態(tài)環(huán)境用傳統(tǒng)TEB算法與改進TEB算法進行對比。

1 傳統(tǒng)TEB算法的路徑研究規(guī)劃

1.1 算法基本描述

TEB算法用機器人的n個位姿序列排列而成，x i、yi表示機器人位置，βi表示機器人的方向。機器人的位姿有序序列為：

TEB算法在EB算法的基礎(chǔ)上加入了時間間隔ΔT，是機器人從當(dāng)前位姿運動到下一個位姿需要的時間，通過合并連續(xù)位姿的時間間隔，生成一個時間間隔序列：

機器人的軌跡可表示為：

機器人速度和加速度約束用幾何約束的懲罰函數(shù)實現(xiàn)的，移動線速度和角速度是根據(jù)兩個時間間隔為ΔT的連續(xù)位姿x i和x i+1之間的距離和的角度變化來計算的。

加速度的約束是根據(jù)兩個連續(xù)的平均速度計算，平移速度通過歐氏距離單位時間內(nèi)的變化量計算，需要考慮3個連續(xù)位姿，和它們相應(yīng)的兩個時間間隔：

TEB算法相較于EB算法考慮到了時間的約束，盡量減少花費的時間成本，該算法規(guī)劃出的路徑以時間最優(yōu)為目標(biāo)代替其他路徑規(guī)劃算法以路徑最短為目標(biāo)，可使機器人花費最短的時間到達目標(biāo)點。時間約束的目標(biāo)函數(shù)如式（6）所示：

1.2 TEB超圖的構(gòu)建

TEB算法中產(chǎn)生的稀疏系統(tǒng)矩陣問題用位姿和時間間隔為節(jié)點的Hyper-Graph（超圖）表示。將目標(biāo)函數(shù)和約束代價函數(shù)用邊來描述，每個節(jié)點由對應(yīng)的邊連接成為超圖。如圖1所示為軌跡優(yōu)化場景下需要的目標(biāo)函數(shù)TEB超圖，超圖中的約束主要包括障礙物、航路點、機器人自身的速度、加速度和非完整約束等。每條邊對應(yīng)的約束函數(shù)根據(jù)當(dāng)前的環(huán)境進行約束，并將權(quán)值加入整體約束函數(shù)中來進行路徑規(guī)劃。

圖1 TEB超圖Fig.1 TEB hypergraph

算法以高于機器人相應(yīng)周期的頻率不斷更新。在每一次更新中，算法都會添加著新的位姿并刪去舊的位姿。對于每一個周期檢測到的障礙物來說，算法將其視為靜態(tài)障礙物來處理。每一次的周期迭代都會產(chǎn)生一個新的超圖。

在超圖中，速度的約束連接著機器人連續(xù)的兩個位姿Si和Si+1和一個時間間隔Ti；加速度的約束連接著機器人連續(xù)的三個位姿Si-1、Si和Si+1和兩個連續(xù)的時間間隔Ti和Ti+1；障礙物約束Fob連接著障礙物和距離障礙物最近的機器人位姿節(jié)點Si；航路點約束Fpath連接著機器人當(dāng)前位姿節(jié)點Si和距離相鄰航路點。

2 改進TEB的約束條件

傳統(tǒng)的TEB路徑規(guī)劃算法是通過實時環(huán)境探測，探測到的環(huán)境信息不斷進行反饋，根據(jù)反饋結(jié)果實時優(yōu)化路線，實現(xiàn)了在未知和已知環(huán)境下的局部避障功能。但是在實際應(yīng)用過程中還是會存在部分問題。比如在復(fù)雜動態(tài)環(huán)境中，在狹窄通道內(nèi)遇到數(shù)個動態(tài)障礙物陷阱導(dǎo)致道路不可通行時，TEB算法可能存在局部最小或不可到達的情況。如果在機器人避障過程中，障礙物在與機器人距離比較接近時，運動速度突然過快，兩者有可能發(fā)生碰撞。傳統(tǒng)TEB算法沒有考慮到障礙物的形狀，在遇到不規(guī)則障礙物后，避障過程需要大量的運算，還可能對機器人的安全性造成威脅。避障過程中，過快的速度變化導(dǎo)致控制量不平滑，路徑行駛中的不流暢。

2.1 對障礙物做膨脹化處理

針對在傳統(tǒng)TEB算法中障礙物懲罰函數(shù)因不規(guī)則障礙物而不斷變化，使計算量激增，對TEB算法進行改進。建立不規(guī)則障礙物的膨脹模型，用已經(jīng)設(shè)定好的膨脹半徑對障礙物做膨脹化處理，來減少算法的計算量，使移動機器人獲得更加平穩(wěn)安全的軌跡，對障礙物的膨脹化處理如圖2所示。

圖2 將障礙物做膨脹化處理Fig.2 Expanding obstacles

2.2 區(qū)域分級策略

針對在傳統(tǒng)TEB算法中障礙物和機器人距離過近時，因障礙物速度變化可能會發(fā)生碰撞，無法保證機器人安全性的問題，提出使用一種區(qū)域分級情況將以機器人與障礙物距離為半徑，在路徑規(guī)劃中遇到障礙物的距離分成安全區(qū)域、相對碰撞區(qū)域和絕對碰撞區(qū)域。3種區(qū)域分別如圖3（a）所示。

如圖3（b）所示，傳統(tǒng)TEB算法規(guī)劃路徑時，與障礙物距離過近，考慮到機器人在實際復(fù)雜環(huán)境中的不確定性，提出對TEB的改進算法。改進后的TEB算法進行動態(tài)避障時，優(yōu)先考慮可在安全區(qū)域規(guī)劃的路徑。如果在安全區(qū)域沒有可行路徑，則考慮相對碰撞區(qū)域的可行路徑。如果還是沒有可行路徑，路徑規(guī)劃失敗。

圖3 障礙物所在區(qū)域Fig.3 Obstacle area

實現(xiàn)分級的意義有以下幾個方面：（1）增加機器人避開障礙物的可能性，保證機器人運動過程中的安全性；（2）在機器人檢測到障礙物的過程中，優(yōu)先選擇安全區(qū)域的路徑，可以大量減少TEB算法的計算量，提高實時性；（3）在狹窄通道中，如遇動態(tài)障礙物，機器人大部分情況下都會處于相對碰撞區(qū)域和絕對碰撞區(qū)域，通過改進的算法速度權(quán)重，更加智能地避開障礙物，增加機器人通過動態(tài)障礙物場景的可行性。

2.3 增加速度的約束

如果障礙物出現(xiàn)在相對碰撞區(qū)域內(nèi)，就會認(rèn)為將來一段時間內(nèi)機器人會與障礙物發(fā)生碰撞，雖通過速度迅速下降直至停下，等障礙物離開相對碰撞距離后再繼續(xù)行進的策略會實現(xiàn)避障，但是這樣會導(dǎo)致機器人在遇到動態(tài)障礙物后的速度跳變，速度控制量不平滑，增加不必要的消耗?？紤]到機器人的成本和安全性，本文加入速度與障礙物距離約束關(guān)系，現(xiàn)將障礙物與速度在超圖中連接，構(gòu)建新的超圖，如圖4所示。

圖4 改進后的TEB超圖Fig.4 Improved TEB ultra-graph

線速度在加入障礙物的約束后可表示為：

同時，角速度也可表示為：

2.4 改進TEB算法實現(xiàn)過程如下：

改進TEB算法的路徑規(guī)劃過程如下所示：

（1）初始化運行參數(shù)，輸入全局路徑，給出起始點和終點；

（2）調(diào)用一段全局路徑，得到初始化軌跡，機器人的位姿狀態(tài)，每次迭代的時間間隔；

（3）根據(jù)這一段初始化徑規(guī)劃路線，插入/刪除TEB狀態(tài)；關(guān)聯(lián)TEB狀態(tài)到路徑點和障礙物點；

（4）生成超圖并優(yōu)化超圖，找出最優(yōu)軌跡；

（5）判斷當(dāng)前軌跡是否在安全區(qū)域；

（6）如在安全區(qū)域就繼續(xù)行進，不在安全區(qū)域就判斷安全區(qū)域的可行性；

（7）安全區(qū)域沒有可行性就在相對碰撞區(qū)域找最短軌跡路線，同時加入機器人速度與障礙物的約束；

（8）根據(jù)相鄰的導(dǎo)航點計算機器人的行進速度和轉(zhuǎn)向角等控制變量；

（9）更新環(huán)境地圖中機器人與障礙物位置信息；

（10）是否到達終點；

（11）到達目標(biāo)點則結(jié)束算法，沒有到達則返回步驟（2）。

3 仿真實驗與分析

為驗證上述改進TEB算法可行性與有效性，在ROS系統(tǒng)中設(shè)置了仿真程序，分別對TEB傳統(tǒng)算法和改進算法仿真驗證。仿真平臺為Ubuntu系統(tǒng)中的Rviz，ROS導(dǎo)航功能包通過配置文件完成相應(yīng)的參數(shù)配置，相關(guān)參數(shù)如表1所示。仿真所用的機器人參數(shù)通過多次調(diào)試獲得。

表1 機器人運動學(xué)參數(shù)Table 1 Robot kinematics parameters

改進后的TEB算法的局部路徑規(guī)劃實驗主要分成兩種情況進行仿真：（1）靜態(tài)環(huán)境；（2）動態(tài)環(huán)境，其中包括簡單動態(tài)環(huán)境和復(fù)雜動態(tài)環(huán)境。主要目的是驗證改進TEB算法在靜態(tài)和動態(tài)環(huán)境中是否能更安全有效地規(guī)劃路徑，并且能夠減少機器人受到的沖擊。

3.1 靜態(tài)環(huán)境中的軌跡安全性分析

如圖5所示，（a）、（c）代表在靜態(tài)環(huán)境中傳統(tǒng)TEB算法的運行軌跡，雖路徑短，但是會貼著障礙物運動，無法保證機器人的安全性能。圖5（b）、（d）代表在靜態(tài)環(huán)境中改進的TEB算法的運行軌跡?？梢钥闯?，改進的TEB算法為了保證器人的安全性，使機器人與障礙物之間保持安全距離，優(yōu)先考慮在安全區(qū)域的路線規(guī)劃。

圖5 靜態(tài)環(huán)境中的局部路徑規(guī)劃Fig.5 Local path planning in a static environment

3.2 動態(tài)環(huán)境中的路徑規(guī)劃

在靜態(tài)環(huán)境中的仿真實驗完成后，為了更好地表現(xiàn)出機器人在車間環(huán)境下的可行性。在ROS仿真環(huán)境中，分別構(gòu)建一個單障礙物動態(tài)環(huán)境和一個多障礙物有動態(tài)環(huán)境，該環(huán)境中行駛的機器人設(shè)定為雙輪差速機器人，加入提前構(gòu)建的動態(tài)障礙物，進行周期性的上下移動，以模擬機器人的動態(tài)工作環(huán)境。為了能更加清晰直觀地表達實驗效果，同時在stage平臺上進行同步模擬仿真。

在構(gòu)建的仿真環(huán)境中，給機器人一個導(dǎo)航的目標(biāo)點，用傳統(tǒng)的TEB算法和改進后的TEB算法分別在簡單動態(tài)環(huán)境和復(fù)雜動態(tài)環(huán)境進行仿真實驗。如圖6（a）和圖7（a）所示，傳統(tǒng)的TEB算法規(guī)劃的路徑會貼著障礙物運動，機器人與障礙物碰撞的可能性較大。而改進的TEB算法中，優(yōu)先選取在安全區(qū)域狀態(tài)下的局部路徑，增大與障礙物之間的距離，保證機器人運行的安全性。在無安全路徑時選擇最短路徑，同時增加障礙物與機器人速度的約束，規(guī)劃出新的局部路徑。通過圖6（b）和圖7（b）所示，機器人與障礙物保持著相對安全的距離，提高了機器人運行過程中的安全性。

圖6 簡單動態(tài)環(huán)境中的局部路徑規(guī)劃Fig.6 Local path planning in simple dynamic environment

圖7 復(fù)雜動態(tài)環(huán)境中的局部路徑規(guī)劃Fig.7 Local path planning in complex dynamic environment

3.3 動態(tài)環(huán)境中的速度變化曲線

如圖8所示，藍線為圖6中動態(tài)障礙物循環(huán)往復(fù)運動速度的變化圖。紅線為圖6中機器人運行中的速度變化圖。圖8（a）為在傳統(tǒng)TEB算法中形成的速度曲線，圖8（b）為在改進后的TEB算法中形成的速度曲線。在傳統(tǒng)TEB算法和改進后的TEB算法仿真中設(shè)定相同的起止點后，兩種算法生成的速度變化圖有明顯區(qū)別，改進后的算法在應(yīng)用過程中速度變化明顯變慢。具體數(shù)值如表2所示。

圖8 機器人與障礙物的速度變化圖Fig.8 Velocity variation diagram of robot and obstacle

表2 速度曲線分析Table 2 Velocity curve analysis

仿真測試結(jié)果表明：在傳統(tǒng)TEB算法的基礎(chǔ)上，通過對速度進行一個新的約束，即增加了速度與障礙物距離的約束，形成了改進的TEB算法。通過在靜態(tài)環(huán)境中和動態(tài)環(huán)境中的仿真可以看出來，使用改進的TEB算法后，速度震蕩次數(shù)在X軸和Y軸分別減少40%和81%，速度差值也分別減少了62.5%和22.2%。速度變化曲線明顯平滑，速度變化減慢，基本達到預(yù)期要求，證明本實驗設(shè)計的合理性。

4 結(jié)論與展望

本文針對障礙物在動態(tài)環(huán)境中機器人的局部路徑規(guī)劃問題，對TEB算法局部路徑規(guī)劃算法進行了優(yōu)化，提出了一種改進的TEB算法。所提出的算法改進考慮到了障礙物不規(guī)則的形狀及速度過快導(dǎo)致碰撞的可能性，對障礙物膨脹化處理并對機器人可行區(qū)域分級處理。然后用障礙物距離對機器人速度進行約束。通過在靜態(tài)和動態(tài)環(huán)境中的仿真對比實驗發(fā)現(xiàn)，改進的TEB算法增加了機器人行進過程中的安全性考慮，減少速度跳變導(dǎo)致的速度控制量不平滑，提高機器人動態(tài)環(huán)境中運行的平滑性，得到的軌跡可有效避開障礙物。