本文轉自 https://www.knightdusk.cn/2019/06/features-and-tuning-guide-for-teb-local-planner/
高手zxy的技術小站（做個備份記錄）
本文將介紹ROS導航中的局部規劃器TEB Local Planner在實際車輛上的應用條件和參數調試方法。同時我們也將會討論與之相關的局部耗費地圖（local costmap）和耗費地圖轉換器(costmap converter)。這裏我們將不會詳細討論如何在ROS導航棧中配置TEB Local Planner。讀者若需相關信息可閱讀其它文章。
TEB Local planner 的基本特性和使用TEB的主要原因
在討論我們爲何要使用TEB Local Planner之前，首先向讀者簡要介紹使用TEB Local Planner的理由。畢竟，若是更加簡單的局部路徑規劃器可以滿足要求，讀者便沒有必要再使用計算量大又難以調試的TEB規劃器了。
TEB Local Planner的重要特點
TEB規劃器可以處理阿克曼底盤車輛的運動學約束。阿克曼底盤即指常見的汽車底盤，一對車輪方向不可變化，一對可以轉向。ROS導航棧默認的局部規劃器base local planner只能處理萬向輪或差速轉向的車輛模型。
TEB是時間最短的最優控制器。在最大速度和加速度等參數約束的條件下，TEB規劃器將規劃出時間最短路徑和速度指令。路徑上可以表現爲明顯的切彎、靠近障礙物；速度指令上將會表現爲速度和方向指令的快速振盪。
TEB基於姿態搜索最優方案。TEB的基本思路是在車輛附近空間內散佈大量的姿態，然後通過搜索樹搜索最優方案。因此，車輛輪廓的準確描述對於TEB的工作性能有顯著影響。
在系統中使用TEB Local Planner的缺點與潛在問題
TEB規劃器對電機和舵機的動態響應性能要求很高。在給定加速度約束的情況下，TEB規劃輸出，即電機控制器的輸入是一個斜坡輸入。使用直流電機時，需要認真調試PID參數才能實現比較好的效果。
TEB規劃器計算量很高。經筆者嘗試，TEB規劃器在一款i3-7100U的小型工控機上無法發揮良好效果，默認設置下規劃週期可達1秒以上。若開啓不同拓撲結構路徑的同時規劃（homotopy class planning），規劃週期甚至可達20秒以上，完全無法使用。
TEB規劃器的穩定性一般。由於不斷重新規劃路徑，且實際車輛的定位測速不可能沒有誤差，在車輛靜止時可以觀察到舵機抖動。儘管降低規劃頻率(此頻率由move_base的配置確定)有助於改善這一問題，但這同樣導致車輛動態避障的性能大大降低。
TEB Local Planner的參數調試方法
下面我們正式開始討論如何確定TEB Local Planner的各項參數值。本文並非官方文檔的簡單重複，因此不會詳細解釋各個參數的含義。如有需要，請參考官方文檔。
-
max_vel_x和acc_lim_x 這是速度與加速度約束。注意加速度同樣約束減速過程。若電機性能良好且速度不快可以立即剎車，可直接將acc_lim_x設置爲0，表示沒有約束。若電機不能承受階躍輸入或者響應時間很長，則應當設置加速度限制。
-
max_vel_x_backwards 最大倒車速度。將此速度設置爲0或者負數將導致錯誤。禁止倒車應在penalty部分將前向行駛的權重設置得極高。後文將討論此問題。
-
max_vel_y和acc_lim_y y軸方向，即垂直於車輛正對方向的速度。對阿克曼底盤的車輛沒有意義。
-
wheelbase和cmd_angle_instead_rotvel 後者設置爲true時，ROS話題（rostopic） cmd_vel/angular/z 內的數據是舵機角度，其值滿足- \pi/2 <z< \pi/2 −π/2<z<π/2 此時應設置wheelbase即前後輪距離。前後輪距離d與速度、角速度和舵機角度之間的關係是：\varphi = arctan(d\omega /v)φ=arctan(dω/v)（注：車輛運動學中心是後輪中點，速度除以角速度所得半徑也是以這裏爲準的）
-
min_turning_radius 最小轉彎半徑。爲避免規劃出不可能實現的移動路徑，請動手測量實際車輛的轉彎半徑。此參數事實上約束了舵機的最大轉角。有些車輛轉向性能不佳，前輪實際轉過角度小於舵機角度，則應當給指令轉角乘上一增益後再控制舵機，否則車輛將總是不能實現設置的最小轉彎半徑。請注意此項應當與最大角速度配合設置。
-
max_vel_theta和acc_lim_theta 角速度和角加速度約束。正確配置下最小轉彎半徑應在低速時生效，而角速度限制應作用在車輛高速行駛時。角速度約束可以防止高重心轉彎過快翻車，角加速度限制可以避免車輪打滑，車輛失控。對於低速平穩運行的車輛可以不約束此兩項，將二者設爲一個足夠大的值即可。
-
xy_goal_tolerance和yaw_goal_tolerance 目標跟蹤誤差。根據車輛運行精度設置。例如，筆者使用一臺攀爬RC車構建導航系統，這種車輛轉向間隙特性很大，則不應設置嚴格的航向限制。
-
free_goal_vel 自由目標速度。設爲False時，車輛到達終點時的目標速度爲0。我們已經指出，TEB是時間最優規劃器。缺少目標速度約束將導致車輛“全速衝線”，即使前方有一堵牆也是如此（因爲撞牆的時刻不在規劃器考慮範圍內了）。不過此選項在競速比賽中比較有用。
-
dt_ref和dt_hysteresis 前文指出，TEB通過狀態搜索樹尋找最優路徑，而dt_ref則是最優路徑上的兩個相鄰姿態（即位置、速度、航向信息，可通過TEB可視化在rivz中看到）的默認距離。此距離不固定，規劃器自動根據速度大小調整這一距離，速度越大，相鄰距離自然越大。筆者認爲，將dt_ref設置爲與車輛長度大致是可行的。較小的值理論上可提供更高精度，但筆者並未進行驗證。對於一般的模型車而言，不需要改變默認配置。當相鄰姿態距離和dt_ref的差超過正負dt_hysteresis時，規劃器將改變這一距離。
-
global_plan_overwrite_orientation 根據官方文檔，覆蓋全局路徑的方向是預留給3D路徑規劃使用的。對於車輛的2D規劃，可以設置爲False，可實現對全局路徑的更好跟蹤。
-
global_plan_viapoint_sep 從全局規劃路徑中提取的路點的相互距離。默認設置下爲Disable，即只沿全局規劃向前尋找，找到的離開局部規劃器規劃範圍前的最後一個點作爲局部目標。若全局規劃特殊，在無障礙空間仍不走直線（例如，工作中的掃地機器人），則需要將此值設置爲設置爲一個小的正數。
-
max_global_plan_lookahead_dist 最大向前看距離。此距離1.應隨車輛最大速度的增大而增大 2.不應超過激光雷達等傳感器的可靠測量範圍 3. 不應超過局部耗費地圖的大小，即不能要求TEB對局部耗費地圖以外的部分進行規劃。
-
include_costmap_obstacles 必須設置爲True後才能規避實時探測到的、建圖時不存在的障礙物。
-
min_obstacle_dist 最小障礙物距離。若設置了車輛的輪廓，則不建議使用此參數。使用車輛輪廓配置footprint_model配合膨脹距離即可實現較好效果。障礙物懲罰權重很高，TEB不會違背這一最小距離約束，因此會把可以通過的縫隙視作不能通過，並終止運行。請注意，此處設置的機器人輪廓必須與局部耗費地圖中的一致，後文將繼續討論。
-
inflation_dist 障礙物膨脹距離。此膨脹只是降低通過這些區域的優先級，不應當用此距離迫使車輛遠離障礙物。
懲罰權重配置
在TEB規劃器中，除了撞擊障礙物不被允許外，其餘的約束在沒有可行方案時可被打破（如規劃出事實上不可行的轉彎半徑）。對於權重配置，向讀者介紹以下幾種策略：
- 提高penalty_epsilon。此項爲速度等約束提供類似膨脹層的緩衝效果。接近限速將產生一定的懲罰值。
大幅提高weight_optimaltime（最優時間權重）。提高至3-5時，車輛在直道上快速加速，並靠近路徑邊緣沿切線過彎。 - 大幅降低weight_optimaltime，設置較大的inflation_dist，並稍微增加weight_inflation。時間權重降低至0.3以下時，車輛速度明顯平穩，不會向最高速度加速。膨脹距離大時將遠離障礙物行駛。
大幅增加weight_kinematics_forward_drive（>100），禁止規劃倒車。如仍規劃倒車，說明找不到可行的前向行駛路徑。若路徑確實存在，可以檢查轉彎半徑、各種footprint是否正確，膨脹層是否合理。
TEB中的恢復措施
恢復措施可以嘗試將卡在雜物中的機器人或路徑規劃錯誤的機器人恢復至正常狀態。TEB Local Planner實現了由move_base規定的振盪恢復方法。但是，實測中控制器將高頻率振盪速度指令（>10Hz），讀者應當評估自己的電動機能否承受。（可在 move_base配置中關閉）
TEB提供路徑規劃不可行（plan not feasible）時的恢復措施，稱爲shrink horizon backup。此時TEB將以更近的點作爲規劃目標，嘗試重新規劃出可行路徑。調試時可關閉，以在可視化界面上觀察原出錯路徑。
Costmap Converter
TEB默認情況下不使用Costmap Converter。事實上，此插件可以在複雜場景下極大提高運算效率，尤其是處理激光雷達分散的測量數據時。因爲將障礙物視爲系列孤立點效率極低。配置方法官方文檔已經提供，此處不再贅述
Local Costmap 配置指導
Costmap的一般配置方法請參考相應官方文檔。本文着重介紹爲TEB規劃器配置時的注意事項。
footprint配置：配置車輛/機器人的多邊形外形。此外形應與TEB中的外形一致，或者至少在大小上沒有太大的差別。注意costmap中的footprint設置是有默認值的，因此讀者必須手動設置此配置爲合適的值。配置舉例：
local_costmap:
footprint: [[-0.305, -0.18], [-0.305, 0.18], [0.305, 0.18], [0.305, -0.18]]
costmap layers: 局部耗費地圖需要有來自地圖的靜態層和來自傳感器的動態層，缺一不可。筆者在最初嘗試配置時忽略了靜態層，此時TEB將無視全局地圖中的障礙。許多傳感器，例如2D激光雷達，不可能探測到實時障礙物（地圖中沒有的）背後的靜態障礙物（地圖中有的），TEB規劃器可能規劃出繞過前方障礙物，並穿過當前沒有見到的障礙物的錯誤路徑。速度較快時可能來不及在發現障礙物時改正。鑑於TEB已有障礙物膨脹功能，此處可以不再添加膨脹層。
局部耗費地圖的更新頻率應當不低於規劃器和costmap_conveter的轉換頻率，否則顯然是沒有意義的。同時，局部耗費地圖應當開啓滑動窗口（rolling window）模式，窗口大小也不應大於傳感器的探測距離。
關於多路徑並行規劃(Parallel Planning in distinctive Topologies)
前文已經指出，此功能將嚴重影響規劃器性能。當路徑中存在大量不連續分佈的障礙物（如錐桶）併產生大量可行路徑時，此功能有良好的效果。相反，若在單一路徑上運行，則沒有必要時用此功能。具體配置筆者並未做更多研究，還留待讀者自行探索。
TEB規劃器的性能問題
在本文的最後一部分，將總結一些可以提高TEB規劃器性能的參數調整策略，供遇到與我類似問題的讀者參考。
- 關閉多路徑並行規劃（效果非常顯著）
- 使用Costmap Converter （非常顯著）
- 降低迭代次數（no_inner/outer_iterations） （顯著）
- 降低 max_lookahead_distance （一般）
- 減小局部耗費地圖的大小 （顯著）
- 增大規劃週期和控制週期 （影響效果）
- 使用單點footprint，配合最小障礙物距離約束 （不太顯著且影響效果）