TEB 調整指引

Timed Elastic Band (TEB)根據執行時間、與障礙物的距離、和運動學約束，來執行優化機器人的軌跡。

如何利用 `teb_local_planner` 改善導航功能

此指引是給那些選擇使用 teb_local_planner 的人，導航您的機器人。更多資訊，請點擊 click here.

1. 設置

1.1. 設置 local planner

Filepath: catkin_ws/movel_ai/config/movel/config/
File to modify: base_local_planner_params.yaml

請注意，local planner的配置文件位於 movel 安裝包中。在 yaml 文件本身中，裡面包含 3 個local planner，但我們只會使用其中一個。取消註釋 TebLocalPlannerROS 下的整個部分。

一種好的做法是，僅包含您需要的內容（如果之前評論過，請取消評論）並註釋掉其餘內容。

這些是可以配置的參數，以下將解釋如何調整這些參數。

TebLocalPlannerROS:

  odom_topic: /odom
  #odom_topic: /rtabmap/odom
  map_frame: map
      
  # Trajectory
    
  teb_autosize: True
  dt_ref: 0.3 #0.2
  dt_hysteresis: 0.1
  global_plan_overwrite_orientation: True
  max_global_plan_lookahead_dist: 3.0
  feasibility_check_no_poses: 5
    
  # Robot
          
  max_vel_x: 0.40 #0.25 
  max_vel_x_backwards: 0.4 #0.2
  max_vel_theta: 1.0 #0.5
  acc_lim_x: 0.5 # 0.2
  acc_lim_theta: 0.6283 #0.5, 0.26
  min_turning_radius: 0.0
  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
    type: "polygon" #"circular" 
    radius: 0.38 # for type "circular"
    #line_start: [-0.3, 0.0] # for type "line"
    #line_end: [0.3, 0.0] # for type "line"
    #front_offset: 0.2 # for type "two_circles"
    #front_radius: 0.2 # for type "two_circles"
    #rear_offset: 0.2 # for type "two_circles"
    #rear_radius: 0.2 # for type "two_circles"
    vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ]   # for type "polygon"

  # GoalTolerance
      
  xy_goal_tolerance: 0.2 #0.2
  yaw_goal_tolerance: 0.1571
  free_goal_vel: False
      
  # Obstacles
      
  min_obstacle_dist: 0.05
  inflation_dist: 0.0
  dynamic_obstacle_inflation_dist: 0.05
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 0.1
  obstacle_poses_affected: 25 #30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5

  # Optimization
      
  no_inner_iterations: 5 #5
  no_outer_iterations: 4 #4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  weight_max_vel_x: 2 #2
  weight_max_vel_theta: 1 #1
  weight_acc_lim_x: 1 # 1
  weight_acc_lim_theta: 1 # 1
  weight_kinematics_nh: 1000 #1000
  weight_kinematics_forward_drive: 1000 #1000
  weight_kinematics_turning_radius: 1 #1 #only for car-like robots
  weight_optimaltime: 1.0 #1
  weight_obstacle: 50 #50
  weight_viapoint: 5.0 #5.0 #1.0
  weight_inflation: 0.1 #0.1
  weight_dynamic_obstacle: 10 # not in use yet
  selection_alternative_time_cost: False # not in use yet

  # Homotopy Class Planner

  enable_homotopy_class_planning: False #True
  enable_multithreading: True
  simple_exploration: False
  max_number_classes: 2 #4
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_keypoint_offset: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: False

  #ViaPoints
  global_plan_viapoint_sep: 0.5 #negative if none
  via_points_ordered: False #adhere to order of via points

  #Feedback
  publish_feedback: true #false

警告：yaml禁止tab鍵。編輯 yaml 文件時不要使用tab鍵來縮排。此外，檢查縮排是否正確對齊。

1.2. 配置速度設定

使用TEB Planner去配置Seirios後，需要同步TEB Planner指定的最大速度。

Filepath: catkin_ws/movel_ai/config/velocity_setter/config/
File to modify: velocity_setter.yaml

local_planner: "TebLocalPlannerROS"
parameter_name_linear: "max_vel_x"
parameter_name_angular: "max_vel_theta"

檢查local_planner在base local planner的配置中，是否匹配planner的名稱。您將配置在TEB Planner中指定的最大速度。

1.3. 檢查確認

終端裡的rostopic list 確認你是否有發佈 /odom。
Planners需要一張地圖，請確認你是否已經建置好欲導航的地圖。
啟動 rviz，將你的Fixed Frame更改為/map 。
在Seirios，從地圖管理中，載入該地圖。

2. 機器人定位

rviz：使用 2D Pose Estimator 工具，在地圖上精確定位機器人的位置。您可以使用 LaserScan 來幫助您 - 將尺寸 (m) 增加到至少 0.05，並嘗試將 LaserScan 中的線條與地圖盡可能匹配。

Seirios：請至導航定位此功能，使用鍵盤或操縱桿按鈕，將激光的線條與地圖盡可能地對齊。

2.1. AMCL 配置

Filepath: catkin_ws/movel_ai/config/movel/config/amcl.yaml
File to modify: amcl.yaml

Amcl 配置文件與 base_local_planner_params.yaml 位於同一目錄中。

同樣地，Amcl 文件中有很多參數，儘管我們只需要這兩個參數：

min_particles: 50
max_particles: 1000

配置min_particles 和 max_particles 來調整精確度。

若你想要更精準的定位，你可以增加數值。但是請根據你地圖的尺寸，來調整該數值，若在小地圖裡有太多的粒子(particles)，代表著會有多餘的粒子，會消耗太多的電腦運算能力。

更多資訊，請點擊此連結 Click here 。

2.2. 檢查確認

rviz/Seirios 中機器人的位置，必須盡可能準確地對應機器人的實際位置，不準確的定位可能會導致諸如路線錯誤、機器人進入禁區、機器人卡在尷尬的角落等問題。

3. 調整`teb_local_planner` 參數

本節提供，關於 TebLocalPlannerROS 的參數列表中數值的建議。返回到相同的 base_local_planner_params.yaml 文件。

TEB 需要做很多調整，才可達到你期望值，其中會需要許多測試與偵錯的過程。

對於不同種類的機器人調整，請參考機器人製造商的配置。您設置的任何參數都必須在您的機器人可以處理的範圍內。

小撇步: 每一次的調整，請先微調參數，去觀察機器人的變化。

可調整的參數，可以分為以下幾類：

Robot機器人
Goal Tolerance目標容錯
Trajectory機器人軌道
Obstacles障礙物
Optimisation機器人優化

3.1. 機器人Robot

請根據製造商指定的配置，調整機器人相關參數。即為速度和加速度設置的值，不應超過機器人的硬體限制。

運動學Kinematics

vel 參數限制了機器人的移動速度， acc 參數限制機器人可以加速的速度， _x 指定線性運動學，而 _theta 指定角度運動學。

  max_vel_x: 0.40 #0.25 
  max_vel_x_backwards: 0.4 #0.2
  max_vel_theta: 1.0 #0.5
  acc_lim_x: 0.5 # 0.2
  acc_lim_theta: 0.6283 #0.5, 0.26

足跡模型Footprint Model

從字面上看，就是指腳印，理想情況下，將佔地面積配置為略大於機器人的實際測量值，應該根據機器人的測量來配置footprint_model。

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
    type: "polygon" #"circular" 
    radius: 0.38 # for type "circular"
    #line_start: [-0.3, 0.0] # for type "line"
    #line_end: [0.3, 0.0] # for type "line"
    #front_offset: 0.2 # for type "two_circles"
    #front_radius: 0.2 # for type "two_circles"
    #rear_offset: 0.2 # for type "two_circles"
    #rear_radius: 0.2 # for type "two_circles"
    vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ]   # for type "polygon"

您必須指名機器人足跡的類型type。不同的類型包括多邊形polygon、圓形circular、two_circles、線line、點point等。為簡單起見，足跡通常是圓形或多邊形。更多資訊，請點擊此連結 Click here。

半徑radius 需要輸入此類型 type: "circular"

頂點vertices 需要輸入此類型 type: "polygon"

3.2. `目標容錯`Goal Tolerance

可指定您願意容忍的偏離目標點的程度。

  xy_goal_tolerance: 0.2 #0.2
  yaw_goal_tolerance: 0.1571

xy_goal_tolerance 是距離目標可接受的直線距離，以meter為單位。

不應將 xy 值設置得太高，否則機器人將停在非常不同的位置。但是，需要一些餘地來解決機器人漂移等問題。

yaw_goal_tolerance 是機器人方向的偏差。例如：目標指定機器人應直接面向牆壁，但實際上，機器人稍微面向左側。

不應給出太緊的偏航公差，否則機器人可能會為了獲得正確的方向而四處晃動。就效率而言，這可能並不理想。

3.3. 障礙物Obstacles

指的是決定機器人在障礙物前應如何表現。

需要進行實驗來調整planner以最適合地方式接近障礙物。風險高的配置將允許機器人，在障礙重重的路徑中移動，例如：在狹窄的走廊，但它可能會卡在障礙物周圍或撞到障礙物；更保守的配置可能會導致機器人排除其唯一可用的路徑，因為它認為它離障礙物太近了。

最棘手的部分是，要如何在這兩種情況之間取得平衡。

  min_obstacle_dist: 0.05

  inflation_dist: 0.0
  dynamic_obstacle_inflation_dist: 0.05
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 0.1
  obstacle_poses_affected: 25 #30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5

min_obstacle_dist 是您希望與障礙物保持的最小距離。

inflation_dist 是在障礙物周圍添加的緩衝區。

有關避障與罰則，請點擊此連結Click here 。

其他避障參數調整的相關問題，請點擊此連結click here。

4. 避障參數的代價地圖costmap

除了在local planner，配置障礙物行為處理外，我們還可以配置代價圖。

代價地圖會告訴機器人，機器人移動到特定點需要多少成本。成本越高，機器人越不應該去那裡。致命的障礙物可能損壞機器人，將會付出超高的成本。

Filepath: catkin_ws/movel_ai/config/movel/config/
File to modify: costmap_common_params.yaml

文件在同一目錄中base_local_planner_params。

footprint: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ]
# robot_radius: 0.38 #0.38 
# footprint_padding: 0.05 
map_type: voxel
#track_unknown_space: true

obstacle_layer:
    origin_z: -0.1
    z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar
    z_voxels: 1 
    obstacle_range: 10.0 #10.0
    raytrace_range: 15.0 #15.0
    observation_sources: laser_scan_sensor
    track_unknown_space: true
    lethal_cost_threshold: 100
    unknown_cost_value: 255 

    laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00}
#point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true}
    
lowbstacle_layer:
    origin_z: -0.1
    z_resolution: 1.8
    z_voxels: 1
    obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle
    raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy
    observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2
    publish_voxel_map: true
    track_unknown_space: true
    lethal_cost_threshold: 100
    unknown_cost_value: 255

    obs_cloud:
        data_type: PointCloud2
        topic: /obstacles_cloud
        marking: true
        clearing: true
        min_obstacle_height: 0.01
        max_obstacle_height: 0.99
    mock_scan:
        data_type: LaserScan
        topic: /obstacles_scan
        marking: false
        clearing: true 
        min_obstacle_height: 0.00
        max_obstacle_height: 1.00
        inf_is_valid: true

inflation_layer:
    enabled: true
    cost_scaling_factor: 6.0 #added in by John
    inflation_radius: 0.39 #0.45 #Minimum value: 0.379
    
dynamic_obstacle_layer:
    enabled: false
    map_tolerance: 0.2
    footprint_radius: 0.5
    range: 2.0

代價圖中添加了幾個圖層，通常我們遵循這裡的規範 here。

footprint必須與base_local_planner_params.yaml中指定的測量相匹配。

你可以選擇將common_costmap_params 中的部分或全部圖層添加到global_costmap_params 和local_costmap_params。

對於您決定使用的代價地圖圖層，您必須將這些圖層作為插件安裝到 global_costmap_params.yaml 和 local_costmap_params.yaml 中。更多有關global和local的代價地圖差異資訊，請點擊該連結 Click here。

  plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
    - {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"}
    - {name: dynamic_obstacle_layer, type: "dynamic_obstacle_layer::DynamicLayer"} # Uncomment to apply dynamic_obstacle_layer
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

  plugins:
    - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
    - {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
    # - {name: range_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}

本節中最重要的是，要讓機器人的足跡正確。

小撇步: 首先調整 teb_local_planner 參數，在此之後，若需要微調更多時，才需修改代價地圖。

4.1.膨脹層Inflation Layer

這一層膨脹了致命障礙物周圍的邊緣，且可指定你想讓障礙物膨脹多少。

inflation_layer:
    enabled: true
    cost_scaling_factor: 6.0 #added in by John
    inflation_radius: 0.39 #0.45 #Minimum value: 0.379

inflation_radius 是地圖將障礙物成本值膨脹到的半徑，以meter為單位。通常它是機器人的寬度，加上一些額外的空間。

cost_scaling_factors 是在膨脹期間，應用於代價縮放因子。

額外的正確配置資訊 (directly lifted from here)

The inflation_radius is actually the radius to which the cost scaling function is applied, not a parameter of the cost scaling function. Inside the inflation radius, the cost scaling function is applied, but outside the inflation radius, the cost of a cell is not inflated using the cost function.
You'll have to make sure to set the inflation radius large enough that it includes the distance you need the cost function to be applied out to, as anything outside the inflation_radius will not have the cost function applied.
For the correct cost_scaling_factor, solve the equation there ( exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)), using your distance from obstacle and the cost value you want that cell to have.

理想情況下，我們希望設置這兩個參數，使膨脹層幾乎覆蓋走廊，機器人在障礙物之間的中心移動。

（請見下圖）

4.2. 障礙物層Obstacle Layer

障礙層在代價地圖上標出障礙物，它會追蹤感測器數據記錄的障礙物。

obstacle_layer:
    origin_z: -0.1
    z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar
    z_voxels: 1 
    obstacle_range: 10.0 #10.0
    raytrace_range: 15.0 #15.0
    observation_sources: laser_scan_sensor
    track_unknown_space: true
    lethal_cost_threshold: 100
    unknown_cost_value: 255 

    laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00}
    #point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true}

若要建置2D地圖，一定要選擇laser_scan_sensor。

obstacle_range是將障礙物插入代價圖後，與機器人的最大距離。其數值為 10，表示代價地圖將標出距離機器人10 米以內的障礙物。

raytrace_range是光線追蹤障礙物的範圍（以meter為單位)，此數值必須根據您的感測器做設置。

max_obstacle_height 是添加到代價地圖的障礙物的最大高度。若此障礙物非常高，請增加此數值，並根據你的感測器去做設置。

體素層參數Voxel layer parameters

體素是在空間中具有相對位置的 3D 立方體，可用於深度相機的 3D 重建，如果您不使用 3D，請忽略以下參數。更多相關訊息，請點擊連結 here

origin_z 是地圖的 z 原點（米）。

z_resolution是立方體的高度。

z_resolution controls how dense the voxels is on the z-axis. If it is higher, the voxel layers are denser. If the value is too low (e.g. 0.01), you won’t get useful costmap information. If you set z resolution to a higher value, your intention should be to obtain obstacles better, therefore you need to increase z_voxels parameter which controls how many voxels in each vertical column. It is also useless if you have too many voxels in a column but not enough resolution, because each vertical column has a limit in height.

z_voxels 是體素的數量。

4.3. 低障礙層lowbstacle layer

低障礙層是障礙層，但帶有附加參數。將您的observation_sources更改為，您要從中獲取數據的主題。

如果您想要 PointCloud (3D)，請使用 obs_cloud；如果您想要 LaserScan (2D)，請使用 mock_scan。或者您可以指定自己的方法。

lowbstacle_layer:
    origin_z: -0.1
    z_resolution: 1.8
    z_voxels: 1
    obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle
    raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy
    observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2
    publish_voxel_map: true
    track_unknown_space: true
    lethal_cost_threshold: 100
    unknown_cost_value: 255

    obs_cloud:
        data_type: PointCloud2
        topic: /obstacles_cloud
        marking: true
        clearing: true
        min_obstacle_height: 0.01
        max_obstacle_height: 0.99
    mock_scan:
        data_type: LaserScan
        topic: /obstacles_scan
        marking: false
        clearing: true 
        min_obstacle_height: 0.00
        max_obstacle_height: 1.00
        inf_is_valid: true

這裡有兩個重要參數，需要特別注意。

min_obstacle_height 是機器人底座下方障礙物的高度。像是包括樓梯，調整此參數，以便機器人可以檢測到下方的障礙物，並防止其掉入坑中..

max_obstacle_height 是障礙物的最大高度，通常指的是機器人的高度。

5. 問題困境Uncomfortable situations

參數調整時，一定會需要有實驗的過程，在這過程中一定會有些問題，所以建議您在修改 yaml 文件之前先在控制台中測試您的值。

rosrun rqt_reconfigure rqt_reconfigure

已知問題和可能修復的清單並不詳盡，在廣泛使用機器人的過程中，還會遇到更多的問題。因此，整理出一系列問題和解決方案的清單是一種很好的做法，這樣您就知道在問題再次出現時如何處理。

5.1. 已知問題

問題#1: 讓機器人去狹窄路徑的另一邊的問題。

從觀察中可以看出，機器人將返回目標失敗，或者機器人會將自己靠近牆壁並卡在那裡。

問題 #2: 讓機器人旋轉90度的問題。

The planner不知為何無視牆壁的存在，並指示機器人穿越過它，而不是繞過牆壁迴轉 90 度。儘管似乎大多數機器人都足夠聰明，可以識別出它們無法穿過牆壁，並且執行中止目標，或是嘗試中陷入困境。

5.2. Some attempted fix

解決方案 #1: 降低速度和加速度。直覺是，通過減慢機器人的運動和旋轉速度，The planner可能有更多時間對障礙物做出反應並做出相應的計劃。

解決方案 #2: (對於那些拒絕去走廊另一邊的機器人。)

縮小兩個代價地圖中的inflation_radius，使其不覆蓋走廊。（如右圖）觀察注意到，如果半徑覆蓋整個走廊，機器人可能會拒絕移動。

檢查機器人的足跡大小是否正確，並檢查代價地圖是否沒有阻擋到路徑。

參考資料References

如需更多資訊有關Planners和Tuning Planners，請詳閱以下為外部資源。

ROS Wiki
ROS Navigation Tuning Guide

Previous外部設備整合包External Process Handler NextPebble 調整指引

Last updated 2 years ago

如何利用 teb_local_planner 改善導航功能

1. 設置

1.1. 設置 local planner

1.2. 配置速度設定

1.3. 檢查確認

2. 機器人定位

2.1. AMCL 配置

2.2. 檢查確認

3. 調整teb_local_planner 參數

3.1. 機器人Robot

3.2. 目標容錯Goal Tolerance

3.3. 障礙物Obstacles

4. 避障參數的代價地圖costmap

4.1.膨脹層Inflation Layer

4.2. 障礙物層Obstacle Layer

4.3. 低障礙層lowbstacle layer

5. 問題困境Uncomfortable situations

5.1. 已知問題

5.2. Some attempted fix

參考資料References

如何利用 `teb_local_planner` 改善導航功能

3. 調整`teb_local_planner` 參數

3.2. `目標容錯`Goal Tolerance