I am programming a robot to drive over variable terrain obstacles autonomously. The variable terrain could potentially knock the robot off of its initial heading, but I would like to design an autonomous sequence to correct for any change in direction. I am using a very accurate sensor with compass and yaw. What is the best way to have it correct for any changes and maintain its heading? Side to side motion does not have to stay perfect, but the heading needs to stay the same.We are currently correcting it by overpowering one side of the wheels (depending on direction of correction needed) until the heading is correct again, but this seems to be a slightly antiquated method, so I'm looking for a cleaner and more smooth method.

  • 3
    $\begingroup$ It looks like this may be a duplicate of another question that was asked recently. See my answer there, but briefly - use a PID controller whose input is heading reference, feedback is sensor heading, and output is an adjustment to motor speed signal. $\endgroup$
    – Chuck
    Commented Feb 11, 2016 at 21:49

2 Answers 2


If you have the path you want the robot to stay on, it sounds like you need Pure Pursuit. In this simple algorithm, you steer the robot to some look-ahead point on the path. The gist of it is illustrated by this image.

Figure 2. Geometry of the pure pursuit. 

This only works for non-holonomic robots, but even if you have a holonomic robot, you can do something similar.

Original algorithm proposed here: Implementation of the Pure Pursuit Path Tracking Algorithm. R. Craig Coulter, CMU, 1992.


There's not a singular, best approach to solve your problem. It's a qualitative problem (what's a good/viable solution?), not so much a quantitative one (how do I minimize drive time under all conditions? minimize path drift, again, under all conditions? minimize battery drain? etc). You could turn it into a quantative problem, but that's a huge undertaking.

@Chuck's writeup looks great for PID usage. PIDs are a great solution when you can characterize your environment, ie: properly tune your PID's variables. A PID wouldn't be my choice because I like learning algorithms more.

So here are some other approaches and a few pros/cons.

Kalman filtering

Pros: not only addresses your drift, but it accounts for the error in your sensors. You should keep in mind that regardless of how good a sensor is, it's not perfect, so not all of the drift occurs because of the terrain. Plug and play code exists.

Cons: very steep learning curve; matrix math.

Particle filering

Pros: very simple to code and tailor to your exact surroundings. path correction is very similar to the integral portion of a PID controller.

Cons: this addresses the error in your sensors as the main problem and the drift from the environment as a secondary problem.

SLAM with way points

Pros: one of the most heavily documented navigation methods

Cons: many, many variants to choose from; requires your robot to be stateful and remember its learned states from earlier runs, otherwise this approach is overkill; you'll need to hybridize your implementation by adding known way points to your map

I think that neural nets would be an interesting way to adjust the robot's immediate actions. That is, change the driving techniques to correct for slippage in mud or rain. But, again, overkill.

To throw in a few more options:

  • motion planning with potential functions (also called "gradient descent" and "vector field" calculation)
  • feed-forward / feedback reinforcement learning.

Here's a chapter on path planning.

I'd be interested to know what you choose to go with.

Good luck, Ryan


Not the answer you're looking for? Browse other questions tagged or ask your own question.