Oscillation using TEB local planner with car-like setup

asked 2019-08-16 04:42:13 -0500

mysqo gravatar image

I've come across other topics mentioning similar problems, oscillation when using the TEB local planner for a car-like robot. But nothing has helped me so far, this topic yet remains unanswered, which I think experiences the same problem: https://answers.ros.org/question/3070...

Our robot uses a GPS, IMU and encoders for the wheels. All is fused together with robot_navigation package.

When I simulating the same robot in Gazebo everything works fine, and I do NOT see this oscillation when navigating. However, when I use the real robot, and give it a goal, it immediately turns left, and then after a while turns right, making up this snake like path.

Oscillation when navigating

I've tried multiple configurations in teb_local_planner_params. This is the current config:

    base_local_planner: teb_local_planner/TebLocalPlannerROS

TebLocalPlannerROS:

#### Miscellaneous Parameters ####
  odom_topic: "odometry/imu_filtered"
  map_frame:  "/map" 

#### Robot Configuration Parameters ####
  acc_lim_x:           0.15
  acc_lim_theta:       0.3
  max_vel_x:           0.15
  max_vel_x_backwards: 0.15 
  max_vel_theta: 0.2 

# ********************** Carlike robot parameters ********************
  min_turning_radius: 1       # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
  wheelbase: 0.4525                  # Wheelbase of our robot
  cmd_angle_instead_rotvel: true 
# ********************************************************************

  max_vel_y: 0.0
  acc_lim_y: 0.0 

  footprint_model/type:         "point"     

  #### Goal Tolerance Parameters ####
  #   2.5  degrees: 0.0436332313
  #   5.0  degrees: 0.0872664626
  #   10.0 degrees: 0.174532925
  xy_goal_tolerance:  0.2
  yaw_goal_tolerance: 0.2 
  free_goal_vel:      false 

  #### Trajectory Configuration Parameters ####
  dt_ref:                            0.3 
  dt_hysteresis:                     0.1
  min_samples:                       3  
  global_plan_overwrite_orientation: true
  global_plan_viapoint_sep:         -0.1
  max_global_plan_lookahead_dist:    10
  force_reinit_new_goal_dist:        1.0   
  feasibility_check_no_poses:        4 
  publish_feedback:                  false
  shrink_horizon_backup:             true
  allow_init_with_backwards_motion:  false
  exact_arc_length:                  false
  shrink_horizon_min_duration:       10.0
  teb_autosize:                      true


  #### Obstacle Parameters ####
  min_obstacle_dist:                   0.5
  include_costmap_obstacles:           false
  costmap_obstacles_behind_robot_dist: 0.1
  obstacle_poses_affected:             1
  inflation_dist:                      0.0 

  include_dynamic_obstacles:                   false
  legacy_obstacle_association:                 false
  obstacle_association_cutoff_factor:          5.0 

#  The following parameters are relevant only if costmap_converter plugins are desired (see tutorial):
#  "costmap_converter::CostmapToPolygonsDBSMCCH"
#  "costmap_converter::CostmapToLinesDBSRANSAC"
#  "costmap_converter::CostmapToLinesDBSMCCH"
#  "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  costmap_converter_plugin:      "" 
  costmap_converter_spin_thread: true 
  costmap_converter_rate:        5.0  

  #### Optimization Parameters ####
  optimization_activate: true
  optimization_verbose: false
  no_inner_iterations:              5  
  no_outer_iterations:              4  
  penalty_epsilon:                  0.1 
  weight_max_vel_x:                 2.0 
  weight_max_vel_theta:             1.0 
  weight_acc_lim_x:                 1.0 
  weight_acc_lim_theta:             1.0 
  weight_kinematics_nh:             1000.0 
  weight_kinematics_forward_drive:  1000.0
  weight_kinematics_turning_radius: 10.0 
  weight_optimaltime:               100.0
  weight_obstacle:                  1.0
  weight_viapoint:                  1.0 
  weight_inflation:                 0.1 
  weight_adapt_factor:              2.0 

  #### 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:       100.0 
  selection_viapoint_cost_scale:   1.0  
  selection_alternative_time_cost: false 
  roadmap_graph_no_samples:        15
  roadmap_graph_area_width:        6.0 
  h_signature_prescaler:           1.0 
  h_signature_threshold:           0.1 
  obstacle_heading_threshold:      1.0
  visualize_hc_graph:              false 
  viapoints_all_candidates:        false
  switching_blocking_period:       0.0

This is the global costmap params:

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 1.0
  static_map: true

  static_map: true
  rolling_window: true
  width: 25.0
  height: 25.0
  resolution: 0.010
  transform_tolerance: 10.0
  cost_scaling_factor: 0.0
  unknown_cost_value: 253
  inflation_radius: 0.0

And this is the local_costmap_params:

local_costmap:
  global_frame: /world
  robot_base_frame: /base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: true

  static_map: false
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.01
  transform_tolerance: 10.0
  cost_scaling_factor: 0.0
  unknown_cost_value: 253
edit retag flag offensive close merge delete