Section 48.4: Route and local planning

Choose the right coordinate frame and a hard planning problem can become an easy one: drive along the road, not across the map.

On motion planning in the Frenet frame

Ask a fresh grid planner to change lanes on a curving highway and it will happily hand back a path the steering rack can never execute; the fix is not a smarter search but a change of coordinates that turns the whole maneuver into a single polynomial. That is the contract this section develops: given a reference path and a Frenet start and goal, generate a trajectory that satisfies the feasibility-comfort-safety triple (curvature and acceleration within limits, bounded jerk, and clear of predicted occupancy). The worked example converts a Cartesian waypoint into Frenet \((s, d)\) and shapes a polynomial lateral profile.

Theory

The Frenet frame

As Figure 48.4A illustrates, the Frenet frame straightens the road into a ruler so that curved-lane planning collapses into one-dimensional polynomial shaping. Given a smooth reference path \(r(s)\) parameterized by arc length \(s\), any nearby point \(p\) is described by its projection's arc length \(s\) (along-track, how far along the lane) and its signed perpendicular distance \(d\) (lateral, how far off-center, positive to the left). The mapping decouples the two problems that matter for driving: longitudinal behavior (speed, gaps) lives in \(s\), and lane positioning lives in \(d\).

Polynomial paths and boundary conditions

Figure 48.4B labels the projection point, the along-track arc length \(s\), and the lateral offset \(d\) that Figure 48.4A introduces at the scale of a full lane change.

With lane positioning now isolated in the single scalar \(d\), the planning problem reduces to choosing how that offset should evolve over time. A comfortable lateral maneuver is a polynomial \(d(t)\) that satisfies boundary conditions. A quintic (5th-degree) polynomial fixes position, velocity, and acceleration at both the start and the end. Six conditions determine six coefficients, so the quintic is the standard choice for jerk-optimal lane changes. Degree matters: a cubic lane change at 100 km/h produces a lateral jerk spike of roughly 8 m/s³ at the seam. A quintic over the same distance and duration holds jerk below 2 m/s³ throughout. That is the difference between a passenger grabbing the handle and not noticing the maneuver.

On a typical fan of 45 sampled candidates, the quintic's tighter jerk envelope eliminates roughly 38 before any collision check runs and leaves 7 to score. Without the quintic constraint the same filter passes 43 candidates, and the planner wastes most of its budget on trajectories that feel like an emergency stop. A lower-degree (cubic) polynomial fixes fewer conditions and is enough when only endpoint positions and slopes matter. The planner shapes longitudinal motion \(s(t)\) the same way, targeting speed and acceleration.

Hybrid A* for structured search

Polynomial sampling assumes a usable reference path. When there is none (a parking lot, a tight unprotected turn), Hybrid A* searches a grid of poses but expands successors using a kinematic vehicle model, so every node is reachable by a real steering command. It blends grid search (completeness) with continuous motion (feasibility) and uses a non-holonomic-with-obstacles heuristic to stay efficient.

A vehicle is non-holonomic (its steerable degrees of freedom are fewer than its position-and-heading degrees of freedom, so it cannot move sideways on demand the way it can move forward): this constraint, defined here before the search mechanics below rely on it, is what separates a car's reachable poses from an arbitrary grid cell.

This matters for embodied AI because a car cannot slide sideways: its minimum turning radius is a hard physical constraint, not a preference. Standard A* ignores this and returns paths that require zero-radius turns. The actuators cannot execute those turns. In a parking garage with 0.3 m clearance on each side, an infeasible path does not merely waste distance; it drives the car into a wall. Any planner for a non-holonomic robot must respect the turning radius during planning, not as a post-processing filter.

How the search expands

Each grid cell stores a continuous pose \((x, y, \theta)\), not just an integer index. To expand a node, Hybrid A* pushes a fixed set of steering angles (e.g., \(\{-35°, -17.5°, 0°, 17.5°, 35°\}\)) through the kinematic model for one timestep, yielding reachable child poses. Its heuristic takes the maximum of two admissible estimates: a Dubins or Reeds-Shepp cost (the shortest path length for a car-like vehicle with a fixed minimum turning radius, computed on empty ground with no obstacles; Reeds-Shepp additionally allows reverse motion) that ignores obstacles, and a 2D Dijkstra distance that ignores the turning constraint. The maximum keeps the search focused without over-estimating.

Hybrid A* exists because the 2007 DARPA Urban Challenge exposed exactly this failure: Stanford's Junior entry needed feasible motion in unstructured zones (parking lots, U-turns) where a lane-relative planner has no reference path to follow. Dolgov, Thrun, and colleagues built Hybrid A* for that vehicle so every expanded node was reachable by a real steering command, and the same algorithm now ships in Apollo and Autoware for production parking. A standard grid A* would happily return a path with a zero-radius pivot that the steering rack physically cannot execute.

Worked Example

The example converts a Cartesian waypoint to Frenet \((s, d)\) against a reference path, then generates a 3rd-degree polynomial that moves \(d\) from 0 to 1 m over 3 seconds (a gentle nudge toward the lane's left).

import numpy as np

# Reference path as densely sampled (x, y) points (here a gentle arc).
s_vals = np.linspace(0, 50, 501)               # arc-length samples (m)
ref = np.stack([s_vals, 0.02 * s_vals**1.0], axis=1)  # slowly rising centerline

def to_frenet(point, ref, s_vals):
    """Cartesian (x, y) -> Frenet (s, d) against a polyline reference path."""
    p = np.asarray(point, dtype=float)
    d2 = np.sum((ref - p) ** 2, axis=1)        # squared distance to each ref point
    i = int(np.argmin(d2))                      # nearest reference index
    s = float(s_vals[i])                        # along-track arc length
    # Lateral offset: signed perpendicular distance using the local tangent.
    tan = ref[min(i + 1, len(ref) - 1)] - ref[max(i - 1, 0)]
    tan = tan / (np.linalg.norm(tan) + 1e-9)
    normal = np.array([-tan[1], tan[0]])        # left-hand normal
    d = float(np.dot(p - ref[i], normal))       # signed lateral distance
    return s, d

wp = (20.0, 1.5)                                # a Cartesian waypoint near the path
s, d = to_frenet(wp, ref, s_vals)
print(f"Frenet of {wp}: s={s:.2f} m, d={d:.2f} m")

def cubic_lateral(d0, d1, T):
    """3rd-degree d(t): d(0)=d0, d'(0)=0, d(T)=d1, d'(T)=0."""
    # Coefficients for d(t) = a0 + a1 t + a2 t^2 + a3 t^3 with zero end slopes.
    a0, a1 = d0, 0.0
    a2 = 3 * (d1 - d0) / T**2
    a3 = -2 * (d1 - d0) / T**3
    return lambda t: a0 + a1 * t + a2 * t**2 + a3 * t**3

d_of_t = cubic_lateral(0.0, 1.0, 3.0)           # move 0 -> 1 m over 3 s
for t in [0.0, 0.75, 1.5, 2.25, 3.0]:
    print(f"t={t:4.2f}s  d={d_of_t(t):.3f} m")

Expected output: the waypoint maps to roughly s=20.0 m, d=1.10 m (the offset measured perpendicular to the arc), and the cubic profile rises smoothly from 0 to 1 m, passing through 0.5 m at the midpoint with zero slope at both ends, the signature of a comfortable, jerk-bounded nudge.

This section's contract was to derive both planners, implement the Frenet trajectory sampler, and wire it to occupancy predictions; the Cartesian-to-Frenet conversion and the cubic sampler are covered in the worked example above, and the wiring step is completed here for clarity: the algorithm's step 7 rejects any candidate whose inflated Cartesian tube overlaps an occupied cell in the section 48.3 occupancy grid \(\mathcal{O}\), so the same predicted occupancy that section 48.3 produces becomes a hard constraint on which Frenet candidate the planner is allowed to select, not merely an input it consults informally.

Practical Recipe

1. Build or fetch a smooth reference path (lane centerline) and verify the Frenet mapping on known points.
2. Sample candidate trajectories: a grid of lateral offsets crossed with speed or time targets.
3. Reject candidates violating curvature, acceleration, or jerk limits before scoring.
4. Score feasible candidates against predicted occupancy and lane-keeping cost; send the best to control.
5. Use Hybrid A* only where no reference path exists (parking, unstructured maneuvers).