Hello,
We are working on a heavy omnidirectional platform using orientable wheels. We are using teb_local_planner for navigation. While the overall performance of the planner is good, we are facing serious issues in certain situations, particularly when there are close-by objects.
In these situations, teb planner is constantly re-planning the path, resulting in two different plans alternating quickly.
One of the main characteristics of the orientable wheels is that the platform has a pretty slow reaction to orientation changes (it has to reconfigure the wheel's orientation), so it is not able to react to the path change fast enough. The result is that the robot gets stuck in the position, with the wheels "dancing" between the two required configurations for the two different alternating paths.
Here are a coupe of sample videos ([one](https://youtu.be/zUkB8gfuFtE), [two](https://youtu.be/HgpyNAnvDok)) (rviz of the real robot) in which it can be appreciated. The plan is alternating between two apparently safe paths, but the robot is unable to configure wheels in time to follow a plan before it changes. So it gets stuck and doesn't move.
Here is [another video](https://youtu.be/yIB_y_ZXc6M ) of the robot stuck in a similar situation. However, this [other video](https://youtu.be/Qpq7MRYuxd8) shows how the robot is able to travel one path in one direction, but gets stuck in the same way in the opposite. As is seen in the video, the robot is actually capable of do it, but the planner is not able to create an adequate plan. When manually setting an additional "waypoint", the robot is able to get out. It can be seen, however, how the robot struggles with alternating re-plans several times.
We have tried to penalize angular acceleration to compensate for the slow reaction of the robot. However, it doesn't seem to have any effect on the creation of alternating plans (which is actually the source of the problem) and higher acceleration limits seems to perform better (lots of not feasible trajectories with very slow angular accelerations).
Setting `legacy_obstacle_association` to true alleviates greatly the problem. It seems that including all the obstacles in the planning makes the robot to pass further away from obstacles and makes paths more stable. However, the problem persists when the robot needs to pass close to obstacles.
So far we haven't found or guess any way or parameter to force that the planner only changes the current plan under more strict conditions to avoid it alternate between two. Is there such an option? What other parameters can we tune to alleviate this behavior?
Thank you and best regards.
Current planner configuration:
TebLocalPlannerROS:
odom_topic: odom
map_frame: map
# Trajectory
teb_autosize: true
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: false
max_global_plan_lookahead_dist: 5.0
feasibility_check_no_poses: 30
allow_init_with_backwards_motion: true
# Robot
max_vel_x: 0.2
max_vel_x_backwards: 0.2
max_vel_y: 0.2
max_vel_theta: 0.5
acc_lim_x: 0.2
acc_lim_y: 0.2
acc_lim_theta: 0.5
min_turning_radius: 0.0
wheelbase: 0.0
cmd_angle_instead_rotvel: false
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line"
line_start: [0.3, 0.0]
line_end: [-0.3, 0.0]
min_obstacle_dist: 0.45
inflation_dist: 0.75
# GoalTolerance
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.2
free_goal_vel: false
# Obstacles
include_costmap_obstacles: true
costmap_obstacles_behind_robot_dist: 2.0
obstacle_poses_affected: 30
legacy_obstacle_association: true
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: true
optimization_verbose: false
penalty_epsilon: 0.1
weight_max_vel_x: 2
weight_max_vel_y: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_y: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1 # it is an holonomic robot, must be low
weight_kinematics_forward_drive: 0
weight_kinematics_turning_radius: 0
weight_optimaltime: 1
weight_obstacle: 50
weight_dynamic_obstacle: 10 # not in use yet20.0
selection_alternative_time_cost: false
# Homotopy Class Planner
enable_homotopy_class_planning: false
enable_multithreading: true
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 100.0
selection_prefer_initial_plan: 0.95
selection_viapoint_cost_scale: 1.0
simple_exploration: false
max_number_classes: 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
# Global plan following
weight_viapoint: 10
global_plan_viapoint_sep: -1 # -1 to disable
↧