Following the Plan

You have a path: a sequence of waypoints from start to goal. Now what? How do you actually drive the robot along that path?

This is path following — the art of converting waypoints into velocity commands that make the robot move smoothly from point to point.

The Problem

A path planner gives you waypoints:

Path = [(x1, y1), (x2, y2), (x3, y3), ..., (xn, yn)]

Each waypoint is a position (and sometimes a heading). Your robot is at (x_robot, y_robot) with heading θ_robot. You need to output:

cmd_vel = (v, ω)  # linear velocity, angular velocity

...such that the robot follows the path. Sounds simple, but there are challenges:

1. Waypoints are discrete. The path is a set of points, but the robot moves continuously.
2. The robot has momentum. It can't teleport from waypoint to waypoint.
3. Turns take time. If waypoints form a sharp corner, the robot needs to slow down.
4. Errors accumulate. Wind, wheel slip, sensor noise — the robot drifts off the path.

A good path follower handles all of this gracefully.

Pure Pursuit: The Classic Algorithm

Pure Pursuit is the most popular path following algorithm for wheeled robots. It's simple, robust, and works beautifully for smooth paths.

The Big Idea

Imagine you're riding a bike following a trail. You don't stare at the ground right in front of you — you look ahead 5-10 meters and steer toward that point.

Pure Pursuit does exactly this:

1. Find a "lookahead point" on the path, some distance L ahead of the robot.
2. Steer toward that point using a simple geometric calculation.
3. Repeat at 10-50 Hz.

The lookahead distance L is the key tuning parameter. Small L → tight tracking, jerky motion. Large L → smooth motion, cuts corners.

The Math

Given:

• Robot position: (x_robot, y_robot, θ_robot)
• Lookahead distance: L
• Lookahead point on path: (x_goal, y_goal)

Calculate the steering angle (or angular velocity ω):

α = atan2(y_goal - y_robot, x_goal - x_robot) - θ_robot
curvature = 2 * sin(α) / L
ω = v * curvature

Where:

• α is the angle from the robot's heading to the lookahead point
• curvature is how sharply the robot needs to turn (radius = 1/curvature)
• v is the desired forward speed (constant or adaptive)
• ω is the angular velocity to send to the motors

Choosing Lookahead Distance

The lookahead distance L is typically:

Some implementations use adaptive lookahead:

L = k * v + L_min

Where k is a constant (e.g., 0.5 s) and L_min is minimum lookahead (e.g., 0.5 m). At high speeds, the robot looks farther ahead. At low speeds, it tracks tightly.

Example

Path:      ·──·──·──·──· (waypoints)
                    ↑
                    L = 2m lookahead point

Robot:     X→ (heading east at 0.5 m/s)

α = atan2(0.5, 2.0) ≈ 14°  (slight left turn)
curvature = 2 * sin(14°) / 2 = 0.24 m⁻¹
ω = 0.5 * 0.24 = 0.12 rad/s

Output: cmd_vel = (v=0.5 m/s, ω=0.12 rad/s)

The robot gently turns left to aim at the lookahead point.

Handling Waypoints: How to Find the Lookahead Point

The path is a discrete list of waypoints. How do you find a point exactly L meters ahead?

Algorithm:

1. Find the closest waypoint to the robot (current_waypoint_index)
2. Walk forward along the path, accumulating distance
3. When accumulated distance ≥ L, that's the lookahead point
4. If the path ends before reaching L, use the goal point

Optimization: Use a sliding window. Don't search the entire path — only check waypoints near where you were last time. Paths are locally smooth, so the lookahead point moves slowly.

Trajectory Tracking: Beyond Waypoints

Pure Pursuit works with discrete waypoints. But some planners generate continuous trajectories — smooth curves parameterized by time:

trajectory(t) = (x(t), y(t), θ(t), v(t))

For example, a spline-based planner might output:

• At t=0.0s: (0, 0, 0°, 0.5 m/s)
• At t=0.5s: (0.25, 0.1, 10°, 0.5 m/s)
• At t=1.0s: (0.5, 0.3, 25°, 0.4 m/s)

To follow this, you use a trajectory tracker instead of Pure Pursuit. Common approaches:

1. Time-Parameterized Pure Pursuit

Use the trajectory point at time t_current + lookahead_time as the lookahead point. Everything else is the same.

2. PID on Cross-Track Error

Measure how far the robot is from the desired trajectory (cross-track error e), then use a PID controller to steer back:

ω = Kp * e + Ki * ∫e + Kd * de/dt

This is precise but requires tuning three gains (Kp, Ki, Kd).

3. Model Predictive Control (MPC)

Predict the robot's trajectory over the next 1-2 seconds for various control inputs, then pick the input that minimizes error from the desired trajectory.

Pros: Optimal, handles constraints (max speed, max turning rate). Cons: Computationally expensive, requires accurate robot model.

Stopping at the Goal

Pure Pursuit naturally slows down as it approaches the goal (the lookahead point gets closer, curvature increases, speed decreases). But you need an explicit stopping condition:

if distance_to_goal < threshold:
    cmd_vel = (0, 0)  # stop

Typical threshold: 0.1-0.3 meters (depends on robot size and precision needs).

For precise stopping (e.g., parking), add a final alignment phase:

1. Stop when close to goal
2. Rotate in place to match goal heading
3. Creep forward to exact position

Practical Tips

Speed Control

• Slow down on curves: v = v_max / (1 + k * |curvature|)
• Slow down near goal: v = min(v_max, k * distance_to_goal)
• Respect dynamics: ramp speed smoothly (max acceleration limits)

Handling Sharp Turns

If waypoints form a 90° turn, Pure Pursuit will overshoot. Solutions:

• Insert intermediate waypoints (denser path)
• Use a path smoothing algorithm (spline fitting)
• Switch to a pivot-and-drive behavior for very sharp turns

Recovery from Drift

If the robot drifts far off the path (e.g., bumped by a person), Pure Pursuit will steer back. But if drift is extreme, you may need to:

• Replan the path from the current position
• Rotate in place to face the nearest waypoint, then resume

What's Next?

Congratulations! You've completed Module 6: Plan a Path. You now understand how robots navigate from A to B:

• Path planning: Finding routes through obstacles (A*, RRT)
• Global vs. local planning: Two-layer navigation architecture
• Path following: Steering to track waypoints (Pure Pursuit)

In the next module, we'll tackle an even harder problem: What if you don't have a map? We'll explore SLAM (Simultaneous Localization and Mapping) — how robots build maps while navigating unknown environments.