Stanley Controller

◀ Go to Pure Pursuit

Developed by Gabriel Hoffmann at Stanford, the Stanley Controller was designed for autonomous vehicles to track a path during the DARPA Grand Challenge. The controller achieved this by tracking both the heading error and the cross-track error.

What two error signals does the Stanley Controller track simultaneously?

A

Position error and velocity error.

B

Lookahead distance and wheelbase.

C

Heading error and cross-track error.

D

Proportional gain and integral gain.

Kinematics

What are kinematics?

Kinematics are models derived from mathematical relationships that describe an object's motion. These models are essential for understanding the motion and limitations of robots or vehicles.

Ackermann Model

The Ackermann model is typically used to represent four-wheeled vehicles. All four tires turn to create a turning radius centered at a common point, known as the center of the turning cycle. A challenge with this model is that it requires tracking four individual angular velocities.

Angular Velocity in Descending Order

Where:
b > c > a > d

B: \( \omega_1 = \frac{v}{R_b} \)
C: \( \omega_2 = \frac{v}{R_c} \)
A: \( \omega_3 = \frac{v}{R_a} \)
D: \( \omega_4 = \frac{v}{R_d} \)

The tires on the inside of a turn sweep a smaller radius than the tires on the outside, so they need a larger angular velocity to keep the whole vehicle moving as one rigid body — this is exactly why each of the four wheels needs its own steering angle and speed under a full Ackermann model.

In the diagram above, why does wheel B need a higher angular velocity than wheel A?

A

B is on a smaller turning radius, so it must spin faster to cover the same turn in the same time.

B

B is a rear wheel and rear wheels always spin faster than front wheels.

C

Angular velocity is unrelated to turning radius.

D

B has a larger wheel diameter than A.

What kinematic model will we be using?

For our Stanley Controller, we will be utilizing a simplified Ackermann model, also known as the bicycle model.
We are using this simplified model to avoid tracking four individual angular velocities. Instead, we only track the front wheel's angular velocity, which greatly simplifies the model. However, the bicycle model assumes that there is no slippage between the tire and the floor, which is only true at low speeds. It's not an effective model for high-speed applications.

Why does the Stanley Controller use the simplified bicycle model instead of the full Ackermann model?

A

Because the bicycle model is more accurate at high speeds.

B

Because it avoids tracking four separate wheel angular velocities, needing only one front steering angle.

C

Because real cars only have two wheels.

D

Because it accounts for tire slippage better than Ackermann.

Diving into the Stanley Controller

Now that we've covered the kinematics, let's dive into the actual control law used by the Stanley Controller.

Equation

\[ \delta = \theta_e + \tan^{-1} \left( \frac{K_e \cdot e(t)}{v(t)} \right) \]

Where:

\(\delta\): Steering angle
\(\theta_e\): Heading error
\(K_e\): Cross-track error gain
\(e(t)\): Cross-track error
\(v(t)\): Velocity

Why This Works

The equation is really two corrections added together:

The \(\theta_e\) term points the front wheels to match the path's direction — if the robot is already pointing the right way, this term alone would keep it on a parallel course.
The \(\arctan\left(\frac{K_e \cdot e(t)}{v(t)}\right)\) term adds an extra correction proportional to how far off the path the robot currently is, steering it back toward the path on top of the heading correction.

Dividing by velocity \(v(t)\) has an important effect: at low speed, this fraction grows large, so the arctan term saturates toward \(\pm 90°\) and the controller steers hard to close the cross-track error immediately — useful for parking-lot speeds. At high speed, the fraction shrinks toward zero, so the correction is dominated by the smoother \(\theta_e\) term instead, which avoids overcorrecting and oscillating. This is part of why Stanley tracks curves so precisely at highway speeds, which is exactly the regime it was built for during the DARPA Grand Challenge.

In practice, \(v(t)\) is clamped to a small minimum value (instead of using the raw value) to avoid dividing by zero when the vehicle is stopped.

Why does the Stanley equation divide the cross-track correction by velocity \( v(t) \)?

A

To make the robot go faster when it's off the path.

B

It's arbitrary — any constant would work the same way.

C

To convert the cross-track error from meters into degrees.

D

So the correction is aggressive at low speed and gentler at high speed, avoiding oscillation on fast, smooth tracking.

Advantages/Disadvantages

Stanley's biggest strength is that it folds two error signals — heading error and cross-track error — into a single, easy-to-tune equation with just one gain, \(K_e\). That's part of why it performed so well at highway speeds during the DARPA Grand Challenge: it corrects sharply when the vehicle drifts off the path, but settles into smooth, non-oscillating tracking once it's aligned.

That said, it comes with real tradeoffs:

Like Pure Pursuit, it relies on the non-holonomic bicycle model, so it assumes no tire slip — accuracy degrades on loose or slippery surfaces.
The \(v(t)\) term in the denominator means the controller needs special handling (like clamping to a minimum speed) to avoid dividing by zero when the vehicle is stopped or moving very slowly.
Cross-track error is measured relative to the front axle, which makes the controller track curves tightly when driving forward, but that same geometry makes it behave poorly in reverse.

What is the main practical advantage of the Stanley Controller over separately tuning a heading controller and a cross-track controller?

A

It combines both error sources into one equation with a single gain to tune.

B

It eliminates the need to know the robot's heading.

C

It works equally well in reverse and forward driving.

D

It requires no tuning at all.

Algorithm Steps

The Stanley Controller follows these steps at every timestep:

Localize the robot to get its position \((x, y)\) and heading (yaw).
Find the nearest point on the reference path and compute the cross-track error (\(e(t)\)) — the signed perpendicular distance from the front axle to that point.
Compute the heading error (\(\theta_e\)) — the difference between the robot's current heading and the path's direction at that nearest point.
Plug \(e(t)\), \(\theta_e\), the current velocity \(v(t)\), and the gain \(K_e\) into the Stanley equation to get the steering angle \(\delta\).
Command the steering servo/motors to angle \(\delta\) and move forward.
Repeat at each timestep until reaching the destination.

At each timestep, which of these does the Stanley Controller need to compute before it can find the steering angle \( \delta \)?

A

Only the cross-track error.

B

Both the cross-track error and the heading error.

C

Only the wheelbase.

D

Only the lookahead distance.

Demo

Drag the blue waypoints below to reshape the path, then watch the robot correct its heading and cross-track error using the Stanley equation. The panel on the right shows the live cross-track error (\(e\)), heading error (\(\theta_e\)), and resulting steering angle (\(\delta\)).

Things to try:

Increase Ke — the robot corrects cross-track error more aggressively, which can cause oscillation if pushed too high.
Decrease Ke — the robot relies more on matching the path's heading and drifts back toward the path more gently.
Increase speed — watch the cross-track term shrink (since it's divided by velocity), so heading error dominates more, just like on a real highway.
Drag a waypoint to create a sharp turn — watch the heading error spike as the robot approaches it.

Path View (drag the waypoints)

Live Readout (from the Stanley Equation)

e (cross-track error)0.0

θ_e (heading error)0.0°

δ (steering angle)0.0°

Driving straight

Ke = 0.5

speed = 2.0

The Code

View the Stanley controller class on GitHub

KEY SNIPPETS OF CODE

The navigate method below computes the cross-track error from the vector to the current target point, computes the heading error from that same vector, and combines them with the Stanley equation to get a steering command:

class Stanley():
    def __init__(self, path, k=0.1, previous_error=0, target_index=0, debug=False):
        self.k = k
        self.previous_error = previous_error
        self.path = path
        self.target_index = target_index

    def navigate(self, x, y, yaw):
        curr_x, curr_y, curr_yaw = x, y, yaw

        curr_target = self.path[self.target_index]
        while True:
            dx = curr_target[0] - curr_x
            dy = curr_target[1] - curr_y
            distance = math.sqrt(dx ** 2 + dy ** 2)
            if distance < 0.5:
                # within 0.5 meters of target point, switch to next target point
                if self.target_index < len(self.path) - 1:
                    self.target_index += 1
                    curr_target = self.path[self.target_index]
                else:  # reached final target point
                    break
            else:
                break

        # Calculate cross track error (cte).
        cte = dy * math.cos(curr_yaw) - dx * math.sin(curr_yaw)

        # Calculate the heading error (theta_e)
        desired_yaw = math.atan2(curr_target[1] - curr_y, curr_target[0] - curr_x)
        theta_e = desired_yaw - curr_yaw
        theta_e = math.atan2(math.sin(theta_e), math.cos(theta_e))

        # Compute velocity command (constant velocity in this case)
        velocity = 13

        # Calculate the steering angle using the Stanley controller equation
        steering = -(theta_e + math.atan(self.k * cte / (velocity + 0.001)))

        # limit steering angle to range [-1, 1]
        steering = max(-1.0, min(1.0, steering))

        return velocity, steering

Note the + 0.001 in the denominator — this is the same divide-by-zero guard described in the math section above, just implemented as a small constant instead of a speed floor.