Planning is the cognitive core of an autonomous vehicle. Given a complete understanding of the world (perception), the vehicle’s precise location (localization), and predictions of how other agents will behave (prediction), the planner must determine what the vehicle should do — from high-level route decisions down to the exact trajectory the vehicle will follow.
Planning operates at multiple levels of abstraction:
Question: Which roads should I take to get from origin to destination?
This is essentially the same problem solved by Google Maps or Waze: find the optimal path through a road network, considering distance, traffic, and road type.
Algorithms: Dijkstra’s algorithm, A*, or their variants on a road graph. The graph nodes are intersections, edges are road segments, and edge weights encode distance, travel time, or a cost function combining both.
Question: What high-level maneuver should I execute right now?
Given the route and the current traffic situation, the behavioral planner decides:
Question: What exact path and speed profile should I follow?
Given the high-level decision, the motion planner computes a smooth, feasible, safe, and comfortable trajectory — a sequence of (x, y, heading, speed, time) waypoints.
A* is the classic informed search algorithm. It finds the shortest path in a graph by expanding the most promising nodes first:
\[f(n) = g(n) + h(n)\]where:
A* is used primarily for route planning on road networks. For motion planning in continuous space, it is adapted by discretizing the space.
Standard A* operates on a discrete grid, producing paths with sharp turns that a car cannot physically follow. Hybrid A* addresses this:
Hybrid A* is widely used for parking and low-speed maneuvering.
A state lattice pre-computes a set of feasible trajectories (primitives) that connect states at regular intervals. During planning, the algorithm searches through combinations of these primitives using A* or dynamic programming.
Advantages: all trajectories are kinematically feasible by construction. The resolution and coverage of the lattice can be tuned for different scenarios (dense lattice for parking, sparse for highway).
RRT builds a tree of feasible trajectories by randomly sampling the configuration space:
RRT*: An asymptotically optimal variant that rewires the tree when a shorter path is found.
RRT is good at finding feasible paths in complex environments quickly, but the resulting paths are typically non-smooth and suboptimal. Post-processing (smoothing, optimization) is usually required.
The dominant approach in modern autonomous driving: formulate trajectory planning as a mathematical optimization problem.
Define the planned trajectory as a sequence of states $\mathbf{s}_0, \mathbf{s}_1, …, \mathbf{s}_N$ at discrete time steps, and solve:
\[\min_{\mathbf{s}_{0:N}} \sum_{t=0}^{N} c(\mathbf{s}_t, \mathbf{a}_t) + c_\text{terminal}(\mathbf{s}_N)\]subject to:
A typical cost function includes:
Progress cost: Encourage forward progress along the route \(c_\text{progress} = -w_p \cdot s_\text{along}\)
Lane centering: Penalize deviation from lane center \(c_\text{center} = w_c \cdot d_\text{lateral}^2\)
Speed tracking: Follow the desired speed (speed limit or traffic flow) \(c_\text{speed} = w_v \cdot (v - v_\text{desired})^2\)
Smoothness: Penalize jerky motion \(c_\text{smooth} = w_j \cdot (\ddot{v}^2 + \dot{\kappa}^2)\)
where $\ddot{v}$ is longitudinal jerk and $\dot{\kappa}$ is curvature rate.
Safety: Penalize proximity to other agents \(c_\text{safety} = w_s \cdot \sum_i \max(0, d_\text{safe} - d_i)^2\)
Traffic rules: Hard or soft constraints for traffic laws (red lights, stop signs, yield rules).
The resulting optimization is typically non-convex (due to collision avoidance constraints). Common approaches:
A popular approach in structured roads: plan in the Frenet frame, which decomposes motion into:
This decoupling simplifies the problem:
Autonomous driving can be formulated as a Partially Observable Markov Decision Process (POMDP):
Solving POMDPs exactly is computationally intractable for realistic driving scenarios. Practical approaches use approximations:
Build a search tree of possible future scenarios:
Generate a discrete set of “scenarios” representing different possible behaviors of other agents:
Plan a trajectory that is safe across all scenarios, or plan the best trajectory for each scenario and select based on risk tolerance.
Developed by Mobileye, RSS provides a formal mathematical framework for safe driving:
For longitudinal safety, the safe following distance is:
\[d_\text{safe} = v_r \cdot \rho + \frac{v_r^2}{2 a_\text{min,brake}} - \frac{v_f^2}{2 a_\text{max,brake}} + \frac{1}{2} a_\text{max,accel} \cdot \rho^2 + v_r \cdot \rho \cdot a_\text{max,accel} / a_\text{min,brake}\]where $v_r$ is the rear vehicle’s speed, $v_f$ is the front vehicle’s speed, $\rho$ is the reaction time, and the acceleration parameters represent worst-case assumptions.
RSS provides a provable safety guarantee under its assumptions. However, overly conservative RSS parameters can make the vehicle too cautious to make progress in dense traffic.
A technically safe trajectory is not enough — it must also feel natural and comfortable to passengers. Key comfort metrics:
Ride comfort optimization is a significant engineering challenge. Early autonomous vehicles were often criticized for jerky, robotic behavior. Modern systems explicitly optimize for smoothness and human-like driving patterns.
The planner must produce trajectories fast enough to react to the dynamic world:
Planning is where perception meets action. The key insights:
The next chapter covers how planned trajectories are executed: vehicle control systems.
| ← Previous: Prediction and Behavior Modeling | Next: Vehicle Control Systems → |