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

算法原理概述

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

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

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

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

算法演示與講解

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

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

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

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

通過以上效果可以看出，min_obstacle_dist和inflation_dist兩個參數(shù)的值會導致teb_local_planner規(guī)劃出不同的軌跡。min_obstacle_dist可以視為比inflation_dist更為嚴格的約束條件，只有當inflation_dist的值大于min_obstacle_dist時，inflation_dist才會影響teb_local_planner規(guī)劃出的軌跡。

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

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

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

未設置viapoint

在兩個障礙物中間設置了viapoint

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

現(xiàn)在，我們再來看global_plan_viapoint_sep在局部規(guī)劃中的作用，該參數(shù)的描述為“從全局路徑中選取的每兩個連續(xù)通過點之間的最小間隔”，結合對viapoint的理解，從該描述中我們可以知道，該參數(shù)影響的是teb_local_planner規(guī)劃的最優(yōu)軌跡對全局路徑的跟隨效果。

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

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

global_plan_viapoint_sep設置為0.1

global_plan_viapoint_sep設置為5.0

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

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

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

與上述軌跡對應的線速度和角速度曲線

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

我們可以發(fā)現(xiàn)，軌跡對應的線速度曲線的最大速度大于max_vel_x，這是因為max_vel_x對應的權重設置得較小導致的。由此也可以反映出Time Elastic Band算法遵循的約束條件為軟約束，用戶可以通過權重設定算法是否嚴格遵循約束條件。

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

只規(guī)劃一條軌跡

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

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

04 總結

通過上面的介紹，可以看出Time Elastic Band算法有很多的優(yōu)點，可以滿足時間最短、距離最短和遠離障礙物等目標以及滿足機器人運動動力學的約束。那是不是Time Elastic Band算法就沒有缺點呢？答案是否定的。

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

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

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

編輯：黃飛

?