Oscillations around global path while using teb local planner?

[kmkharche]

I am using an ackermann drive based robot with a teb local planner. However, the robot seems to oscillate around the global path, i.e. if the robot starts off to the right of the path, then goes to the left, tries to come back and again goes to the right. I am not sure what might be causing this, since the behavior with the base local planner is pretty stable compared to this. If anyone has any suggestions, please let me know. Thanks!

[fangnan99]

Hi, My team has been encountering the same problem, and we've been trying to adjust the parameters to solve this problem. Although we haven't found a proper solution to the problem, it seems that reducing acc_lim_theta and max_vel_theta could give better results. However, by doing this the car's ability of obstacle avoidance might be affected. If you managed to overcome this problem, could you please let me know?

[kmkharche]

I agree that reducing those two values helps a bit, but the robot is not able to make a lot of the sharper turns, without having to reverse a few times, which is not ideal behavior. I thought it might be because I was setting up the base link of the robot incorrectly, but even setting it at the rear wheel correctly does not change the behavior.

[croesmann]

Hi, I was not able to reproduce it yet. Could you please try your specific parameter settings with the teb_local_planner_tutorials package? In particular roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch and its corresponding config files. Does the issue occur there as well? This might help us to localize the reason. And if so, please post the configuration such that I can reproduce it.

[kmkharche]

Hi, I will try that out. But do I use the default robot footprint, or do I need to use my robot footprint in the teb params? Because my robot footprint will be too big to maneuver through the stage world. Also, another thing I wanted to mention was that I only have a laser scan on the robot for pose estimation. I do not have motion sensors such as imu/wheel enoder for the odometry. Will that make a significant difference to the performance? And could that be responsible for the oscillations?

[fangnan99]

Hi, I think I am in a similar situation. The vehicle I am working on has a lidar for localization and a set of encoders for speed measurement. I've been tring with and without the data from the encoder, but I don't think accurate odometry information from the encoders helps a lot.

[croesmann]

Hmm, I still need a setup for reproducing this. Does anybody have a simulation setup in which these oscillations occur? Or does it only occur in real experiments right now?

@kmkharche These are all different settings which we need to try out (it is simple to change the stage world to an empty one, just use a simple bitmap painting tool ;-)). And of course, the accuracy of the position and velocity measurement is crucial for success.

[kmkharche]

@croesmann, I ran the simulation with my robot's larger footprint after cleaning up the world. You can see some oscillations in the simulation. I am attaching the screen recording, and some oscillations can be seen, especially in the second video. These oscillations seem to be even more pronounced on the real robot than the simulation though.

teb_simulation.zip

[DingFeng0103]

Hi, I met the same problem in real experiment. I am using the same parameters as that in tutorial except that for real time computation I reduce the number of iterations to 4 and 3 with respect to outer and inner iterations and make parallel planning false. Now I will try to repeat that in simulation stage.

[subodh-malgonde]

These oscillations seem to be even more pronounced on the real robot than the simulation though.

I am facing the same issue.

I tried these things:

1. Fixed the robot_base_frame from the center of my robot to the rear axle
2. Reduced both acc_lim_theta and max_vel_theta from about 0.3 to 0.12. Contrary to @Anannf's observation this actually increased the oscillations because the car would now turn by smaller angles.
3. Increased weight_kinematics_forward_drive, which penalizes backward motion, from 1.0 to 1000. The path planned by the planner remained the same.

Here is the video:

I tried visualizing the trajectories (nav_msgs/Path) in rviz. The global planner's trajectory looked fine, one which the human would have taken. The local planner's trajectory had back and forth motions.

@kmkharche Were you able to figure this one out?

[kmkharche]

Hi @subodh-malgonde, I believe your issue is a different one than mine. I think your behavior might improve by increasing the max vel theta, or decreasing the min turning radius, or maybe decreasing the min obstacle distance if those don't work. Because it seems like the robot is trying to turn on to the global path, but is not able to do so in one go. Hope that helps.

[kmkharche]

Hi @croesmann, do you have any suggestions about what might be causing the oscillations around the global path? Thanks!

[talkno]

Hi, I thought i was increase the acc_limit_x and dt_ref to resolve the oscillations around the global path.

[croesmann]

@kmkharche sorry, I was very busy during the last weeks. Can you share your stage setup (and movebase/teb config) with me?

Indeed, the issue of @subodh-malgonde seems to be different.

The oscillations might also occur due to the model mismatch (e.g. non-modelled turning rate limits etc.) It would be interesting to see if increasing dt_ref and acc_limit_x helps as suggested by TaoJY.

However, I am definitely interested in finding the cause of these oscillations.

[subodh-malgonde]

Hi @subodh-malgonde, I believe your issue is a different one than mine. I think your behavior might improve by increasing the max vel theta, or decreasing the min turning radius, or maybe decreasing the min obstacle distance if those don't work. Because it seems like the robot is trying to turn on to the global path, but is not able to do so in one go. Hope that helps.

@kmkharche You were right. My problem was different and your suggestion helped me fix it. I made the following changes:

1. Decreased inflation_dist and min_obstacle_dist
2. Increased max_vel_theta and acc_lim_theta (I should have guessed that low values for these parameters are limiting the steering angle of the car)

[zengxiaolei]

Hi， I'm facing the same problem, problems are very similar with that in the video by @kmkharche. But this problem is only with real robot, not in Sage Simulation. I checked that the robot's optimization model was correct through Rviz visualization, and tried both two circles and line type robot model. Since there is no such problem in the simulation, One important reason may be the execution of velocity command. In the following figure, you can see the velocity command from TEB is smooth, but the execution not. Fig. 1: velocity recording when using TEB

What I have done is to increase the weights to limit the velocity acceleration and the maximal velocity (especially for rotational part), things got better: Oscillation remains, however, is less obvious. Fig. 2: velocity recording when using TEB after tuning

Hope it helps.

It is worth noting that just when I thought it might be a problem caused by execution, I tried the DWA algorithm. Execution is still not perfect, but I can't see the oscillation. Fig. 3: velocity recording when using DWA

Therefore, the problem has not been solved in essence. Hope this is a useful reference for you. @croesmann

[shiningjuly]

Hi， I'm facing the same problem，do you have any progress ? @zengxiaolei

[croesmann]

@zengxiaolei thanks a lot for your analysis. Is it a car-like robot you are using?

[bababuluba]

@croesmann Hi! I have the same problem. I can reduce a lot when I set dt_ref to0.4 and acc_lim_theta=0.1, but this while limit the turning speed. The acc_lim_theta has a very big impact on this. When I set it to 1. you can see the angular velocity oscillated before and after a sharp turn in the simulator. When the feedback has some delay will increase this oscillation. pic1 is the 1, pic2 is 0.1. Do you think I can make some change on acceleration edge to improve this? Or add another edge?

[nirwester]

Hi, I have a possible fix for this that shifts the reference control point in the trajectory to a defined amount of poses in the future (instead of always using the second pose). This virtually lowers the "control bandwidth", reducing the impact of actuation and localization delays in the control loop. I'll PR as soon as the mbf pull request is merged or rejected to avoid re-adapting my code :)

[croesmann]

Cool, I am going to check and merge recent PRs on Friday :)

[croesmann]

I just merged the mbf PR. Moving the reference control point to the future seems to me like a workaround rather than a real fix. Did you run tests with real robots? To my mind, the oscillations may be caused by the model mismatch. Ignoring or merging multiple control intervals in the front potential removes degrees of freedoms that could be necessary for other scenarios, e.g. for parking maneuvers (the planner provides a plan which cannot be followed). However, in case this really resolves the oscillations, I am open for this feature ;-)

[nirwester]

Thankyou very much :) I will PR this evening or Monday.

I agree that moving it to the future is conceptually wrong in the ideal case, and indeed when testing it on a simulator I didn't notice any relevant oscillation. However, when moving to real hardware (that usually has control and position feedback delay), a shift of 1 or 2 poses ahead helped me increasing the control-dt without sacrificing the resolution of the path (that would happen increasing the dt_ref). As commented above by others, reducing the angular acceleration constrain below the real limits of the platform also helps reducing the oscillations, but of course limits the overall performance.

[zengxiaolei]

Sorry for the later reply. Based on my testing experience, differential-drive, omni-directional and car-like robots can all have this oscillation, especially when the robot inertia is large or when the delay between velocity command and execution is long. When the inertia is large, small acc_lim_theta and large weight_acc_lim_theta can reduce oscillations. When delay is also large, relative higher control rate can help some in addition. @croesmann

[r91andersson]

I might think that I'm experiencing the same issue, I posted this in the ROS forum: https://answers.ros.org/question/330868/oscillation-using-teb-local-planner-with-car-like-setup/

However, I haven't pulled the latest update if there have been some changes in the repo. I will try with the suggestions above and reply with the results! @croesmann

[nirwester]

If you could give a try to the fix in this pr

https://github.com/rst-tu-dortmund/teb_local_planner/pull/164/commits

setting the parameter "control_look_ahead_poses" to a value bigger than the default one (for example 3), it would help to understand if the approach generalizes to different platforms or has other side-effects @r91andersson @zengxiaolei

[r91andersson]

I'm using Kinetic version, I saw that this PR is for melodic. Is there an simple way to make a PR for Kinetic so that I can test? @nirwester

[nirwester]

I'll port the PR to Kinetic too

[nirwester]

Done @r91andersson https://github.com/rst-tu-dortmund/teb_local_planner/pull/166

[r91andersson]

Thanks! Unfortunately, our GPS got broke, so we're waiting for the replacement board. However, when I ran the teb_local_planner but excluded the GPS, the robot did not oscillate, but follow ed the global plan just perfectly! I'm really confused.. When the GPS worked, the odometry was really good, and I couldn't see any problem with it. I verified the GPS and the odometry by driving the robot approx. 100 m in different directions and then back to the initial position, and it was spot on, so I can't really see why the GPS could cause this.

I will test the PR as soon as I get back the GPS!! Thanks @nirwester

[r91andersson]

Ok, we've finally solved the problem with the oscillation around the global plan. It was a missing transfomration between base_link and navsat_link. The strange thing is that in simulation it worked, so I guess it could handle the transformation internally, but when running in real environment, it simply couldn't. Thanks for all the help! @nirwester