When the task point is close to the obstacle, the teb generates a strange path and the control frequency drops.

[berleyzheng]

As shown in the figure below, my simulation environment is tested on gazebo. When the task point is close to the obstacle, the teb generates a strange path and the control frequency drops. (How much is the control frequency related to the obstacle? In other cases, When the number of points of the obstacles exceeds 140, the control frequency will be lower than 4hz). The following is a screenshot of rviz when I test. The red path in Figure 2 is the global path, and the green is the path generated by the teb.

If someone encounters a similar situation and has a solution, please let me know, thank you very much! I will continue to understand this problem and try to solve it. If there is a solution, it will be posted. Here are the teb configuration parameters: #http://www.ros.org/wiki/teb_local_planner TebLocalPlannerROS: odom_topic: odom map_frame: /odom

Trajectory

teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1

min_samples: 3

global_plan_overwrite_orientation: False
allow_init_with_backwards_motion: False max_global_plan_lookahead_dist: 6.0 feasibility_check_no_poses: 2 via_points_ordered: True # path following. false increases oscillation around the path

max_vel_x: 1.5 max_vel_x_backwards: 0.16 max_vel_theta: 0.41 acc_lim_x: 0.16 acc_lim_theta: 0.3 min_vel_x: 0.1 min_vel_y: 0.0 min_vel_theta: 0 min_vel_x_backwards: 0.0

Carlike robot parameters

wheelbase: 0.61 cmd_angle_instead_rotvel: False min_turning_radius: 0

max_vel_y : 0.0 #(double, default: 0.0)

acc_lim_y : 0.5 #(double, default: 0.5)

footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "polygon" vertices: [[-0.35, -0.28], [-0.35, 0.28], [0.35, 0.28], [0.35, -0.28]] # for type "polygon"

radius: 0.2 # 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"

GoalTolerance

xy_goal_tolerance: 0.3 yaw_goal_tolerance: 6.14 free_goal_vel: False

Obstacles

min_obstacle_dist: 0.35
include_costmap_obstacles: costmap_obstacles_behind_robot_dist: 2.0 obstacle_poses_affected: 3 obstacle_association_force_inclusion_factor: 1.5 inflation_dist: 0.5 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5

Optimization

no_inner_iterations: 5 no_outer_iterations: 4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.15 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 1 weight_acc_lim_theta: 1 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 20 weight_dynamic_obstacle: 10 selection_alternative_time_cost: False weight_viapoint: 10

Homotopy Class Planner

enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: False max_number_classes: 4 roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 #（0.2 < value <= 1） h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False

[jpcpontes]

I also have a similar issue with a car-like robot around obstacles. The path generated by TEB gets very messy, and the control frequency drops drastically.

My parameter config is as follows:

TebLocalPlannerROS: odom_topic: /odom # /odom map_frame: /map # change to map if we want to take into account map obstacles

Trajectory

teb_autosize: True dt_ref: 0.3 # temporal resolution dt_hysteresis: 0.1 # Hysteresis for automatic resizing depending on the current temporal resolution, usually approx. 10% of dt_ref is recommended global_plan_overwrite_orientation: True # Overwrite orientation of local subgoals provided by the global planner (since they often provide only a 2D path) allow_init_with_backwards_motion: True max_global_plan_lookahead_dist: 10 # Specify the maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization. feasibility_check_no_poses: 4 # Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval. global_plan_viapoint_sep: 1.0 # 1.0, If positive, via-points are extrected from the global plan (path-following mode). The value determines the resolution of the reference path (min. separation between each two consecutive via-points along the global plan, if negative: disabled) publish_feedback: False oscillation_recovery: True shrink_horizon_backup: True exact_arc_length: True #If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations (-> increased cpu time), otherwise the Euclidean approximation is used.

Robot

max_vel_x: 0.4 #0.4 max_vel_x_backwards: 0.3 #0.25 max_vel_y: 0.0 max_vel_theta: 0.2 #0.2; largest feasible theta vel when planning for a car: (max_vel_x / wheelbase) acc_lim_x: 0.4 # 0.4 acc_lim_theta: 0.2 # 0.15 max_steering_rate: 2.0 # 2.0 min_turning_radius: 0.7 # 0.7 max_steering_angle=atan(wheelbase/min_turning_radius) (-> solve w.r.t. min_turning_radius) (set to zero for a diff-drive robot). cmd_angle_instead_rotvel: True # Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle [-pi/2,pi/2]. wheelbase: 1.31 #Distance between the rear axle and the front axle. (only required if cmd_angle_instead_rotvelis set to true).

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

Goal Tolerance

xy_goal_tolerance: 0.15 # 0.15 Allowed final euclidean distance to the goal position in meters yaw_goal_tolerance: 0.15 # 0.15 Allowed final orientation error in radians free_goal_vel: False # Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed

Obstacles

min_obstacle_dist: 0.5 #0.5 inflation_dist: 1.0 #0.6 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 6.0 # Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters). obstacle_poses_affected: 500 # Increase the value if the trajectory is not smooth enough close to obstacles costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 #obstacle_association_force_inclusion_factor: 2.0

# Optimization

no_inner_iterations: 5 no_outer_iterations: 4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.05 weight_max_vel_x: 1.0 weight_max_vel_theta: 1.0 weight_acc_lim_x: 1.0 weight_acc_lim_theta: 1.0 weight_max_steering_rate: 1.0 weight_kinematics_nh: 10000.0 weight_kinematics_forward_drive: 1.0 weight_kinematics_turning_radius: 1.0 weight_obstacle: 5.0 weight_dynamic_obstacle: 5.0 weight_inflation: 0.1 weight_viapoint: 1.0 # Optimization weight for minimzing the distance to via-points selection_alternative_time_cost: False

Parallel Planning in distinctive Topologies

enable_homotopy_class_planning: True enable_multithreading: True max_number_classes: 4 selection_cost_hysteresis: 1.0 selection_obst_cost_scale: 2.0 selection_alternative_time_cost: False roadmap_graph_no_samples: 15 roadmap_graph_area_width: 7 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False

[rdeniza]

Hey, did you able to find a solution? I'm having the same problem near obstacles.

[phurkornt]

hi. can you solve this problem . i have a similar problem

[jcmonteiro]

Divergence can be caused by a multitude of reasons. Usually, it will be due to poor numerical conditioning. A way around this, while you do not reach a better set of tuning parameters, is to enable divergence detection #259