teb_local_planner: avoid constant path replanning

edit

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, two) (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 of the robot stuck in a similar situation. However, this other video 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?

EDIT:

When re-planning, there doesn't seem to be any warning or feasibility check fail message, so both plans seems to be valid. The behavior looks like it is alternating between two very close local minima, and the noise from the sensors seems enough to make the planner jump from one minima to the other and back. Sensor noise makes the obstacles around the robot look a bit closer or farther away, which can be enough to trigger the jump ... (more)