Week 6 — Planning: Metrics, Physical Feasibility, and Graph Search

← Course 1 syllabus

Overview

Prediction tells you what others will do; planning decides what you will do. This week builds a first motion planner and, just as importantly, the clean contract between prediction and planning. The planner must produce a trajectory that is physically feasible (respects curvature, acceleration, and jerk limits from Week 2), safe with respect to predicted agent futures (Week 4), and good according to an explicit cost function. You will use graph search over a discretized space of motion candidates — the workhorse of real planning stacks — applying Course 5’s graph algorithms to a robotics problem.

The design lesson is interface discipline: prediction and planning are separate concerns with a narrow, well-defined boundary. Get that contract right and each can be developed and tested independently; blur it and you get a tangled stack that no one can reason about.

Readings

  • DPV (review/apply): graph search, shortest paths, Dijkstra/A*, and dynamic programming. Extract: how to turn a continuous planning problem into a graph and search it.
  • Morin: dynamics, constraints, and feasibility. Extract: the acceleration/friction limits that bound a feasible trajectory.
  • Pressley: curvature and smoothness. Extract: curvature limits as steering constraints.
  • Embedded AI: real-time and failsafe framing. Extract: planning under a time budget with a safe fallback.

Key Concepts

The prediction→planning contract

The planner consumes: ego state, a set of predicted agent futures with probabilities, and the map. It produces: a feasible, scored trajectory plus a fallback. Defining this interface explicitly (a typed message) is what lets Week 5’s predictor and this planner evolve independently and lets the Week 10 runtime swap either side.

Feasibility as hard constraints

A candidate trajectory is feasible only if it respects kinematic and dynamic limits: max curvature (min turning radius), max lateral acceleration \(v^2\kappa \le a_\text{max}\), longitudinal accel/decel limits, and jerk bounds for comfort. Infeasible candidates are pruned before scoring — feasibility is a filter, cost is a ranking.

Search over motion candidates

Discretize the action/state space (e.g. a lattice of (speed, lateral-offset) or a set of spline candidates), build a graph, and use A* / dynamic programming to find the minimum-cost feasible path. The cost function combines progress, comfort (jerk), safety margin to predicted agents, and lane adherence. A* with an admissible heuristic gives optimal paths efficiently; the heuristic design is where domain knowledge enters.

Safety against predicted futures

Because predictions are multimodal and probabilistic, the safety cost must integrate over modes weighted by \(\pi_k\) (or take a worst-case over high-probability modes). This is where Week 4’s representation pays off — the planner reasons about several weighted futures, not one.

Theory Exercises

  1. Specify the prediction→planning message contract as a typed interface; justify each field.
  2. Given \(a_\text{max}\) and speed \(v\), derive the max feasible curvature and the implied min turning radius.
  3. Formulate candidate-trajectory planning as a shortest-path problem on a lattice; define nodes, edges, and edge costs.
  4. Prove A* returns an optimal path given an admissible heuristic; propose an admissible heuristic for this problem.
  5. Define a safety cost that integrates over \(K\) weighted predicted modes; show how it changes the chosen trajectory vs ignoring mode probabilities.

Implementation

Build a lattice/spline-candidate planner in Python that takes the Week 4 prediction output and Week 2 scene, prunes infeasible candidates, scores the rest, and returns the best trajectory plus a fallback. Implement A* or DP over the candidate graph. Visualize the chosen path against agents and predicted futures.

Benchmark

On the scenario set, measure planning latency (p50/p90/p99), feasibility rate, and a safety metric (min distance to predicted agents) vs a naive “follow lane center” baseline. Ablate: planning against the top mode only vs all weighted modes.

Expected baselines: the planner stays within a real-time budget on most scenarios; planning against all weighted modes yields larger safety margins at decision points than top-mode-only, at some progress cost — the quantified safety/efficiency tradeoff.

Connections

The planner closes the prediction→planning loop and is the algorithmic core deployed in the Week 10 runtime under a hard time budget. Its feasibility constraints come from Week 2’s geometry; its inputs from Week 4’s predictions. Course 5’s graph algorithms and Morin’s dynamics are the foundations applied here.