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
Local planning turns predicted actors and a route into a concrete, drivable trajectory. The key trick is the Frenet frame, which re-expresses position relative to a reference path (the lane centerline) as along-track distance $s$ and lateral offset $d$. In Frenet coordinates a lane change is just a smooth curve in $d$, and a comfortable trajectory is a low-jerk polynomial. For unstructured maneuvers (parking, U-turns), Hybrid A* searches a discretized space while respecting the car's turning radius.
This section develops local planning as a measurable contract: given a reference path and a Frenet start and goal, generate a trajectory that is feasible (curvature and acceleration within limits), comfortable (bounded jerk), and safe (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
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
A comfortable lateral maneuver is a polynomial $d(t)$ chosen to satisfy boundary conditions. A quintic (5th-degree) polynomial can fix position, velocity, and acceleration at both the start and the end, six conditions for six coefficients, which is the standard choice for jerk-optimal lane changes. A lower-degree (cubic) polynomial fixes fewer conditions and is enough when only endpoint positions and slopes matter. Longitudinal motion $s(t)$ is shaped the same way against speed and acceleration targets.
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.
The same lane change that is an awkward, curvature-coupled problem in $(x, y)$ becomes a one-dimensional polynomial in $d(s)$ once you adopt the Frenet frame. Picking the coordinate system that matches the constraint structure is often more powerful than picking a fancier optimizer.
Frenet planning typically samples a fan of candidate trajectories: several lateral end-offsets $d$ crossed with several longitudinal speed or time targets, each realized as a polynomial. Every candidate is checked for kinematic feasibility and collision against predicted occupancy, then scored by a cost (jerk, deviation from lane center, proximity to obstacles). The lowest-cost feasible candidate is sent to control. The conversion back from $(s, d)$ to $(x, y)$ for execution requires the reference path geometry.
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.
Production stacks use optimization-based planners rather than hand-rolled sampling: Apollo (Baidu) and Autoware ship Frenet and lattice planners plus Hybrid A* for parking. CommonRoad provides reproducible planning scenarios and a route planner. The PythonRobotics repository has readable reference implementations of Frenet optimal trajectory planning and Hybrid A*. Keep the same feasibility and cost definitions across these tools.
Practical Recipe
- Build or fetch a smooth reference path (lane centerline) and verify the Frenet mapping on known points.
- Sample candidate trajectories: a grid of lateral offsets crossed with speed or time targets.
- Reject candidates violating curvature, acceleration, or jerk limits before scoring.
- Score feasible candidates against predicted occupancy and lane-keeping cost; send the best to control.
- Use Hybrid A* only where no reference path exists (parking, unstructured maneuvers).
A poorly conditioned reference path (sharp kinks, uneven spacing) corrupts the $s$ and $d$ estimate, so a perfectly safe trajectory in Frenet maps back to a swerve in Cartesian. Always smooth and re-sample the reference path, and validate the round trip $(x,y) \to (s,d) \to (x,y)$ before trusting the planner.
For a highway lane change, fix the boundary conditions to start at the current lateral state and end centered in the target lane with zero lateral velocity and acceleration, then use a quintic in $d$ over a comfort-tuned duration. Sweeping the duration trades aggressiveness against jerk, giving a tunable family of human-like maneuvers.
s is how far down the road, d is how far off the line. Plan speed in s, plan position in d, and the curves take care of themselves.
Learning-based planners and differentiable optimization aim to replace hand-tuned cost weights with trajectories scored against human driving, while keeping the hard feasibility guarantees that sampling and Hybrid A* provide. The open challenge is certifiable safety for learned planners.
Can you say why a quintic, not a cubic, is the natural choice for a jerk-comfortable lane change, and when Hybrid A* is needed instead of Frenet sampling? If not, revisit the boundary-condition counting above.
| Tool or Library | Role in the Topic | Builder Advice |
|---|---|---|
| Apollo, Autoware | Production Frenet, lattice, and Hybrid A* planners | Reuse their feasibility checks rather than re-deriving limits. |
| CommonRoad | Reproducible planning scenarios and routes | Benchmark a planner on shared scenarios before deployment. |
| PythonRobotics | Readable Frenet and Hybrid A* references | Use to learn the algorithm before adopting a heavy stack. |
Section 48.3 supplies the predicted occupancy this planner avoids, Section 48.7 tracks the planned trajectory with control, and Section 48.8 places local planning inside route-level and behavior-level decision making.
Replace the cubic in the worked example with a quintic that also fixes lateral acceleration to zero at both ends. Plot $d(t)$, its velocity, and its acceleration, and confirm the quintic removes the acceleration discontinuity the cubic leaves at the endpoints.
Section References
Werling et al., "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame," ICRA 2010. Dolgov et al., "Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments" (Hybrid A*), IJRR 2010. Althoff et al., "CommonRoad: Composable Benchmarks for Motion Planning on Roads," IV 2017.
These define Frenet optimal trajectory planning, Hybrid A*, and a reproducible planning benchmark.
Local planning becomes tractable in the Frenet frame, where speed lives in $s$, lane position lives in $d$, and comfortable maneuvers are jerk-bounded polynomials. Reserve Hybrid A* for the unstructured cases where no reference path exists.
Build a small Frenet sampler: generate five candidate lateral offsets, realize each as a quintic over 3 s, reject any whose peak lateral acceleration exceeds 2 m/s^2, and score the rest by deviation from lane center. Report which candidate wins and why.