Path Following Simulator: Pure Pursuit and Stanley

Controls

Controller switches between pure pursuit and Stanley. Lookahead is pure pursuit’s only knob and is disabled for Stanley, which does not use one — that is itself the point.

Speed and lookahead interact, which is the thing worth playing with: set 1 m/s and find the best lookahead, then set 3 m/s and find it again.

Position from swaps perfect odometry for a 1 Hz fix with metre-scale noise, held between updates. Error is always scored against the truth, so the penalty is real rather than hidden.

Drag any waypoint, or press 1–8 to select one and move it with the arrow keys. The whole run recomputes as you drag.

Theory

Both controllers answer the same question — given where I am and where the path is, what steering angle? — and answer it completely differently.

Pure pursuit is geometry. Find the point on the path a lookahead distance L ahead, and drive the circular arc through it:

κ = 2y / L_d²          δ = atan(2 b sin α / L_d)

with y the lateral offset of the goal point in the robot’s frame and b the wheelbase. One parameter, and it is a distance rather than a gain — so it transfers between robots.

Stanley is control. Reference the front axle and add two terms:

δ = θ_e + atan(k · e / v)

heading error plus an arctangent of cross-track error. Dividing by speed is what lets one gain work across a speed range. k is a true control gain and does not transfer.

The lab adds what textbook derivations leave out: 0.4 s of total lag, as a 0.25 s first-order steering response plus 0.15 s of sensing and compute delay. Without it, pure pursuit gets monotonically better as the lookahead shrinks and the whole tuning question disappears.

Algorithm

Each step, for both controllers:

1. Project onto the path, searching a window around the arc length last reached rather than the whole path. A global search matches whichever segment is nearest, so a course that doubles back teleports the robot’s progress to a corner it passed a minute ago.
2. Resolve a tie at a vertex to the outgoing segment. Past a corner, both adjacent segments project to the same point at the same distance; keeping the incoming one hands the controller a heading error and a lateral offset that are both near zero while it drives straight off the path, silently.
3. Compute the steering angle from the controller’s own rule, and clamp it to the mechanical limit.
4. Apply lag, then integrate the bicycle model.
5. Score against the truth, never the estimate.

Step 2 is not an optimisation. It was the difference between Stanley working and Stanley confidently leaving the course at exactly 78% every single run.

Source code

Pure pursuit, complete, in the form it takes on a real robot:

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

The three lines that matter are the schedule floor, the lastS monotonicity, and the ld clamp. Everything else is the formula.

Assumptions and hardware differences

The lab gives the robot a perfect map of the path, a perfectly known wheelbase, no wheel slip, and lag that is a clean first-order model. A real robot has none of those.

Wheel slip makes the position estimate wrong in a way no controller can detect. Gearbox backlash adds dead travel at every steering reversal, which behaves like extra lag and pushes the optimal lookahead up. And a differential drive has no steering angle at all — it has a commanded rotation rate, so ω = v κ replaces the atan and the wheelbase only matters for how tightly it can turn.

What survives all of it is the shape: an interior optimum, moving right with speed, and error at that optimum growing with speed regardless.

Circuit diagram

The controller is the cheap part. What it needs from the rest of the robot is the expensive part.

Position. Quadrature encoders on both wheels and differential-drive odometry integrating them, at the loop rate. Indoors that is enough for a few tens of metres.

Heading. A gyro for the short term, a magnetometer for absolute reference, or both. Heading error is the term both controllers weight most heavily, and it is the one odometry loses first.

An absolute correction, outdoors. A GPS module fused into the estimate rather than followed directly — the difference is 0.09 m of RMS error against 1.25.

For the theory in full, see pure pursuit and cross-track error; to build it, the GPS waypoint rover starts indoors on encoders and goes outside only once the follower works.