? 在自主移動(dòng)機(jī)器人路徑規(guī)劃的學(xué)習(xí)與開(kāi)發(fā)過(guò)程中，我接觸到Time Elastic Band算法，并將該算法應(yīng)用于實(shí)際機(jī)器人，用于機(jī)器人的局部路徑規(guī)劃。在此期間，我也閱讀了部分論文、官方文檔以及多位大佬的文章，在此對(duì)各位大佬的分享表示感謝。在本文中，我將分享Time Elastic Band算法的原理、個(gè)人對(duì)Time Elastic Band算法的理解以及在ROS下通過(guò)teb_local_planner對(duì)該算法進(jìn)行演示和講解。

算法原理概述

本文依據(jù)Christoph R?smann在論文中的描述，對(duì)eletic band進(jìn)行定義：將給定的路徑視為受內(nèi)外力影響的彈性橡皮筋，使其變形，而內(nèi)外力相互平衡，使路徑收縮，同時(shí)與障礙物保持一定的距離，其中內(nèi)外力就是對(duì)機(jī)器人運(yùn)動(dòng)的所有約束。而對(duì)于time eletic band，則在給定路徑中間插入N個(gè)控制橡皮筋形狀的控制點(diǎn)（機(jī)器人姿態(tài)），在點(diǎn)與點(diǎn)之間定義運(yùn)動(dòng)時(shí)間Time，即為T(mén)ime Elastic Band算法。

通過(guò)上述定義我們可以看出，Time Elastic Band算法把路徑規(guī)劃問(wèn)題描述為一個(gè)多目標(biāo)優(yōu)化問(wèn)題，即對(duì)最小化軌跡執(zhí)行時(shí)間、與障礙物保持一定距離并遵守運(yùn)動(dòng)動(dòng)力學(xué)約束等目標(biāo)進(jìn)行優(yōu)化。因?yàn)閮?yōu)化的大多數(shù)目標(biāo)都是局部的，只與機(jī)器人的某幾個(gè)連續(xù)的狀態(tài)有關(guān)，所以該優(yōu)化問(wèn)題為對(duì)稀疏模型的優(yōu)化。通過(guò)求解稀疏模型多目標(biāo)優(yōu)化問(wèn)題，可以有效獲得機(jī)器人的最佳運(yùn)動(dòng)軌跡。

求解稀疏模型多目標(biāo)優(yōu)化問(wèn)題，可通過(guò)構(gòu)建超圖（hyper-graph），使用g2o（通用圖優(yōu)化）框架中關(guān)于大規(guī)模稀疏矩陣的優(yōu)化算法來(lái)求解。機(jī)器人狀態(tài)和時(shí)間間隔作為nodes，目標(biāo)函數(shù)和約束函數(shù)作為edges，各nodes由edges連接構(gòu)成hyper-graph。在該hyper-graph中，每個(gè)約束為一條edge，且每條edge允許連接的nodes的數(shù)目不受限制。

Time Elastic Band算法通俗的解釋就是從給定路徑中得到一系列帶時(shí)間信息的離散位姿（pose），通過(guò)圖優(yōu)化的方法將這些離散位姿組成滿足時(shí)間最短、距離最短和遠(yuǎn)離障礙物等目標(biāo)的軌跡，同時(shí)滿足機(jī)器人運(yùn)動(dòng)動(dòng)力學(xué)的約束。需要注意的是，優(yōu)化得到的軌跡并不一定滿足所有約束，即給定的約束條件實(shí)際上都是軟約束條件。

算法演示與講解

通過(guò)閱讀teb_local_planner的源碼，我們可以知道teb_local_planner提供了許多參數(shù)和權(quán)重的配置接口，讓用戶可以為優(yōu)化問(wèn)題提供參數(shù)和權(quán)重，在不同的約束條件下指定優(yōu)化目標(biāo)。下面我們通過(guò)對(duì)teb_local_planner實(shí)現(xiàn)效果的簡(jiǎn)單演示來(lái)加深對(duì)Time Elastic Band算法的理解。

前面提到，Time Elastic Band算法可以在給定路徑的基礎(chǔ)上對(duì)軌跡進(jìn)行優(yōu)化，實(shí)現(xiàn)最小化軌跡時(shí)間、與障礙物保持距離等目標(biāo)。

其中，與障礙物保持距離可以說(shuō)是一個(gè)路徑規(guī)劃算法比較重要的功能，teb_local_planner提供了min_obstacle_dist和inflation_dist兩個(gè)參數(shù)以及相應(yīng)的權(quán)重，使用戶可對(duì)軌跡與障礙物的距離進(jìn)行調(diào)整，以此來(lái)滿足不同環(huán)境下路徑規(guī)劃的需求。以下為配置不同參數(shù)值時(shí)，teb_local_planner規(guī)劃出的不同的軌跡（為了使演示的效果更加明顯，參數(shù)權(quán)重給得比較大）：

min_obstacle_dist為0.5，inflation_dist為0.6

min_obstacle_dist為0.5，inflation_dist為1.0

min_obstacle_dist為1.0，inflation_dist為0.6

通過(guò)以上效果可以看出，min_obstacle_dist和inflation_dist兩個(gè)參數(shù)的值會(huì)導(dǎo)致teb_local_planner規(guī)劃出不同的軌跡。min_obstacle_dist可以視為比inflation_dist更為嚴(yán)格的約束條件，只有當(dāng)inflation_dist的值大于min_obstacle_dist時(shí)，inflation_dist才會(huì)影響teb_local_planner規(guī)劃出的軌跡。

當(dāng)然，參數(shù)對(duì)應(yīng)權(quán)重的大小也會(huì)對(duì)規(guī)劃出的軌跡有一定影響。在實(shí)際的應(yīng)用過(guò)程中，需要用戶對(duì)環(huán)境以及機(jī)器人自身的定位精度進(jìn)行評(píng)估，從而設(shè)定合理的參數(shù)值，使得teb_local_planner能夠規(guī)劃出較優(yōu)的路徑。

除了與障礙物保持距離，如何合理并有效地跟隨全局路徑也是衡量一個(gè)局部規(guī)劃算法好壞的重要標(biāo)準(zhǔn)。

teb_local_planner提供global_plan_viapoint_sep參數(shù)以及相應(yīng)的權(quán)重，使用戶可以根據(jù)不同的需求對(duì)期望軌跡設(shè)定約束條件。為了更好的理解該參數(shù)的作用，我首先解釋一下viapoint，viapoint可以理解為通過(guò)點(diǎn)，即要求teb_local_planner規(guī)劃出的軌跡必須通過(guò)某個(gè)點(diǎn)。下面通過(guò)一個(gè)極端一點(diǎn)的例子來(lái)看一下viapoint的效果：

未設(shè)置viapoint

在兩個(gè)障礙物中間設(shè)置了viapoint

可以看到，上面兩幅圖中，帶紅色箭頭的軌跡為teb_local_planner規(guī)劃出的軌跡。未設(shè)置viapoint時(shí)，teb_local_planner選取了上方的軌跡為最優(yōu)軌跡，當(dāng)在兩個(gè)障礙物中間設(shè)置了viapoint，最優(yōu)軌跡從兩個(gè)障礙物中間穿過(guò)。由此可以看出，viapoint對(duì)teb_local_planner規(guī)劃選取的最優(yōu)軌跡有很大的影響。從側(cè)面也反映出，Time Elastic Band算法是盡可能地滿足設(shè)定的多個(gè)約束條件，選取出最優(yōu)的軌跡。

現(xiàn)在，我們?cè)賮?lái)看global_plan_viapoint_sep在局部規(guī)劃中的作用，該參數(shù)的描述為“從全局路徑中選取的每?jī)蓚€(gè)連續(xù)通過(guò)點(diǎn)之間的最小間隔”，結(jié)合對(duì)viapoint的理解，從該描述中我們可以知道，該參數(shù)影響的是teb_local_planner規(guī)劃的最優(yōu)軌跡對(duì)全局路徑的跟隨效果。

當(dāng)global_plan_viapoint_sep的值比較小時(shí)，從全局路徑中選取的viapoint比較密集，最優(yōu)軌跡對(duì)全局路徑的跟隨效果比較好；當(dāng)global_plan_viapoint_sep的值比較大時(shí)，從全局路徑中選取的viapoint比較稀疏，最優(yōu)軌跡對(duì)全局路徑的跟隨效果比較差，但此時(shí)的最優(yōu)軌跡可能更加平滑。

以下為使用Car-like模型時(shí)global_plan_viapoint_sep設(shè)置不同的值實(shí)現(xiàn)的路徑規(guī)劃的效果（同樣的，為了使演示效果更加明顯，適當(dāng)?shù)丶哟髤?shù)的權(quán)重）：

global_plan_viapoint_sep設(shè)置為0.1

global_plan_viapoint_sep設(shè)置為5.0

從上面的實(shí)現(xiàn)效果可以明顯看出， 當(dāng)global_plan_viapoint_sep設(shè)置為0.1時(shí)，最優(yōu)軌跡很緊密地跟隨全局路徑，Car-like模型在轉(zhuǎn)向時(shí)需要多次調(diào)整角度，當(dāng)global_plan_viapoint_sep設(shè)置為5.0時(shí)，最優(yōu)軌跡并沒(méi)有嚴(yán)格遵循全局路徑，而是更為順滑地完成轉(zhuǎn)向。這是global_plan_viapoint_sep設(shè)置不同值時(shí)，teb_local_planner選取最優(yōu)軌跡的不同效果。

在與障礙物保持距離和跟隨全局路徑的情況下，Time Elastic Band算法遵循運(yùn)動(dòng)動(dòng)力學(xué)約束。teb_local_planner提供了max_vel_x和max_vel_theta等多個(gè)參數(shù)以及相應(yīng)的權(quán)重，用戶可根據(jù)實(shí)際機(jī)器人的性能進(jìn)行設(shè)置。下面我們通過(guò)一個(gè)示例來(lái)簡(jiǎn)單看一下Time Elastic Band算法對(duì)速度約束的效果：

存在障礙物的情況下，teb_local_planner選取的最優(yōu)軌跡

與上述軌跡對(duì)應(yīng)的線速度和角速度曲線

max_vel_x為0.4m/s，max_vel_theta為0.3rad/s

我們可以發(fā)現(xiàn)，軌跡對(duì)應(yīng)的線速度曲線的最大速度大于max_vel_x，這是因?yàn)閙ax_vel_x對(duì)應(yīng)的權(quán)重設(shè)置得較小導(dǎo)致的。由此也可以反映出Time Elastic Band算法遵循的約束條件為軟約束，用戶可以通過(guò)權(quán)重設(shè)定算法是否嚴(yán)格遵循約束條件。

說(shuō)到這里，可能也有小伙伴發(fā)現(xiàn)了，在上面演示的過(guò)程中，teb_local_planner并不僅僅規(guī)劃出一條路徑，而是從多條路徑中選取最優(yōu)軌跡，這就不得不提到teb_local_planner中的Homotopy Class Planner，用戶可以將enable_multithreading參數(shù)設(shè)置為T(mén)rue來(lái)開(kāi)啟同時(shí)規(guī)劃多條路徑，并從中選取最優(yōu)軌跡。下面我們也來(lái)看一下只規(guī)劃一條軌跡與同時(shí)規(guī)劃多條路徑并選取最優(yōu)軌跡的對(duì)比：

只規(guī)劃一條軌跡

同時(shí)規(guī)劃多條路徑并選取最優(yōu)軌跡

從上面的對(duì)比可以看出，在某些極端條件下，同時(shí)規(guī)劃多條路徑并選取最優(yōu)軌跡得到的軌跡更符合全局最優(yōu)，也更合理。當(dāng)然，同時(shí)規(guī)劃多條路徑也將消耗更多的機(jī)器性能。在實(shí)際應(yīng)用過(guò)程中，用戶應(yīng)當(dāng)根據(jù)具體情況合理取舍。

04 總結(jié)

通過(guò)上面的介紹，可以看出Time Elastic Band算法有很多的優(yōu)點(diǎn)，可以滿足時(shí)間最短、距離最短和遠(yuǎn)離障礙物等目標(biāo)以及滿足機(jī)器人運(yùn)動(dòng)動(dòng)力學(xué)的約束。那是不是Time Elastic Band算法就沒(méi)有缺點(diǎn)呢？答案是否定的。

從前文我們可以知道，Time Elastic Band算法的大多數(shù)約束都是軟約束條件。若參數(shù)和權(quán)重設(shè)置不合理或者環(huán)境過(guò)于苛刻，都有可能導(dǎo)致Time Elastic Band算法規(guī)劃失敗，出現(xiàn)非常奇怪的軌跡。因此，teb_local_planner中包含了檢測(cè)沖突的部分，判斷軌跡上的點(diǎn)是否與障礙物存在沖突，此時(shí)需要考慮機(jī)器人的實(shí)際輪廓。

在實(shí)際的開(kāi)發(fā)過(guò)程中，更多地需要考慮機(jī)器人自身其他模塊的性能，例如電機(jī)能夠提供的最大加速度，定位算法的精度等，同時(shí)也要考慮具體的環(huán)境以及選擇Time Elastic Band算法是否合理，如此才能將其性能發(fā)揮出更好的效果。

在本文中，我分享了我對(duì)Time Elastic Band算法的一點(diǎn)理解，但畢竟認(rèn)知有限，對(duì)算法的部分內(nèi)容的理解還是比較粗淺的，望大家多多包涵，有什么問(wèn)題也希望能夠與大家多交流。在后續(xù)的文章中，我會(huì)分享ROS自主移動(dòng)機(jī)器人的開(kāi)發(fā)以及各類算法的應(yīng)用，有機(jī)會(huì)的話也會(huì)對(duì)Time Elastic Band算法的原理進(jìn)行更加深入的解析，希望大家多多支持。

編輯：黃飛

?