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

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

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

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

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

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

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

1.1 算法基本描述

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

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

機(jī)器人的軌跡可表示為：

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

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

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

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

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

圖1 TEB超圖Fig.1 TEB hypergraph

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

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

2 改進(jìn)TEB的約束條件

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

2.1 對(duì)障礙物做膨脹化處理

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

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

2.2 區(qū)域分級(jí)策略

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

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

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

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

2.3 增加速度的約束

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

圖4 改進(jìn)后的TEB超圖Fig.4 Improved TEB ultra-graph

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

同時(shí)，角速度也可表示為：

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

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

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

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

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

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

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

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

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

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

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

（10）是否到達(dá)終點(diǎn)；

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

3 仿真實(shí)驗(yàn)與分析

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

表1 機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)Table 1 Robot kinematics parameters

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論與展望

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