如何利用 teb_local_planner
改善導航功能
此指引是給那些選擇使用 teb_local_planner
的人,導航您的機器人。更多資訊,請點擊 .
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
下的整個部分。
這些是可以配置的參數,以下將解釋如何調整這些參數。
base_local_planner_params.yaml
Copy 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
velocity_setter.yaml
Copy 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. 檢查確認
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 文件中有很多參數,儘管我們只需要這兩個參數:
amcl.yaml
Copy min_particles: 50
max_particles: 1000
配置min_particles
和 max_particles
來調整精確度。
2.2. 檢查確認
3. 調整teb_local_planner
參數
本節提供,關於 TebLocalPlannerROS 的參數列表中數值的建議。 返回到相同的 base_local_planner_params.yaml 文件。
可調整的參數,可以分為以下幾類:
3.1. 機器人Robot
請根據製造商指定的配置,調整機器人相關參數。 即為速度和加速度設置的值,不應超過機器人的硬體限制。
運動學Kinematics
vel
參數限制了機器人的移動速度, acc
參數限制機器人可以加速的速度, _x
指定線性運動學,而 _theta
指定角度運動學。
Copy 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
。
Copy 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"
半徑radius
需要輸入此類型 type: "circular"
頂點vertices
需要輸入此類型 type: "polygon"
3.2. 目標容錯
Goal Tolerance
可指定您願意容忍的偏離目標點的程度。
Copy xy_goal_tolerance: 0.2 #0.2
yaw_goal_tolerance: 0.1571
xy_goal_tolerance
是距離目標可接受的直線距離,以meter為單位。
yaw_goal_tolerance
是機器人方向的偏差。 例如:目標指定機器人應直接面向牆壁,但實際上,機器人稍微面向左側。
3.3. 障礙物Obstacles
指的是決定機器人在障礙物前應如何表現。
需要進行實驗來調整planner以最適合地方式接近障礙物。 風險高的配置將允許機器人,在障礙重重的路徑中移動,例如:在狹窄的走廊,但它可能會卡在障礙物周圍或撞到障礙物;更保守的配置可能會導致機器人排除其唯一可用的路徑,因為它認為它離障礙物太近了。
最棘手的部分是,要如何在這兩種情況之間取得平衡。
Copy min_obstacle_dist: 0.05
Copy 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
是在障礙物周圍添加的緩衝區。
4. 避障參數的代價地圖costmap
除了在local planner,配置障礙物行為處理外,我們還可以配置代價圖。
代價地圖會告訴機器人,機器人移動到特定點需要多少成本。 成本越高,機器人越不應該去那裡。致命的障礙物可能損壞機器人,將會付出超高的成本。
Filepath: catkin_ws/movel_ai/config/movel/config/
File to modify: costmap_common_params.yaml
文件在同一目錄中base_local_planner_params
。
costmap_common_params.yaml
Copy 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
footprint
必須與base_local_planner_params.yaml中指定的測量相匹配。
你可以選擇將common_costmap_params
中的部分或全部圖層添加到global_costmap_params
和local_costmap_params
。
global_costmap_params.yaml local_costmap_params.yaml
Copy 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"}
Copy 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"}
4.1.膨脹層Inflation Layer
這一層膨脹了致命障礙物周圍的邊緣,且可指定你想讓障礙物膨脹多少。
Copy 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
是在膨脹期間,應用於代價縮放因子。
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
障礙層在代價地圖上標出障礙物,它會追蹤感測器數據記錄的障礙物。
Copy 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
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
。 或者您可以指定自己的方法。
Copy 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,請詳閱以下為外部資源。