Motion Planning

Motion planning converts goals and scene understanding into a physically executable trajectory. It must respect obstacles, lanes, traffic rules, comfort, vehicle dynamics, actuator limits, uncertainty, and the future motion of other agents. In an autonomous-driving stack, planning is where perception errors and prediction uncertainty become concrete choices: brake, continue, yield, change lanes, creep, pull over, or reroute.

Figure: A real autonomous vehicle grounds the driving stack in a physical platform. Image: Wikimedia Commons, Grendelkhan, CC BY-SA 4.0.

This page covers search-based planning, sampling-based planning, optimization-based planning, model predictive control, iLQR, trajectory optimization, and lattice planners. It links prediction, behavior planning, and control, because planning sits between high-level intent and low-level actuation.

Definitions

Motion planning finds a time-parameterized path or trajectory from the ego vehicle's current state to a desired state while satisfying constraints. A path describes geometry without timing, such as $x(s), y(s)$. A trajectory includes time, such as $x(t), y(t), v(t), a(t)$.

Search-based planners discretize the problem into graph nodes and edges. A* searches a graph using a cost-to-come plus heuristic. Hybrid A* extends A* with continuous vehicle heading and motion primitives, making it useful for car-like vehicles.

Sampling-based planners explore continuous spaces through sampled states or controls. RRT rapidly expands a tree toward random samples. RRT* improves asymptotic optimality by rewiring. PRM builds a roadmap, then queries it.

Optimization-based planners formulate trajectory generation as minimizing a cost subject to constraints. They can encode smoothness, comfort, obstacle clearance, lane preference, progress, and dynamic feasibility.

MPC, or model predictive control, repeatedly solves a finite-horizon constrained optimization problem, executes the first control action, observes the new state, and solves again. In AV stacks, MPC may appear as a planner, a controller, or both.

iLQR, iterative linear quadratic regulator, solves nonlinear trajectory optimization by repeatedly linearizing dynamics and quadratizing costs around a nominal trajectory.

Lattice planning precomputes or generates motion primitives in a structured grid, such as Frenet coordinates along a lane centerline. It is common in road driving because lane-following geometry gives strong structure.

Key results

A* minimizes path cost on a graph when edge costs are nonnegative and the heuristic is admissible, meaning it never overestimates the remaining cost. The priority of a node is:

$$f(n) = g(n) + h(n),$$

where $g(n)$ is the known cost from start to node and $h(n)$ is the heuristic estimate to the goal. For grid planning, Euclidean or Manhattan distance is often used depending on allowed moves.

Car-like vehicle constraints are nonholonomic. The vehicle cannot move sideways instantaneously, so a path that looks collision-free for a point robot may be impossible for a car. The kinematic bicycle model gives:

$$\begin{aligned} \dot{x} &= v\cos\psi, \\ \dot{y} &= v\sin\psi, \\ \dot{\psi} &= \frac{v}{L}\tan\delta, \\ \dot{v} &= a. \end{aligned}$$

The steering angle $\delta$ and acceleration $a$ are controls, and $L$ is wheelbase. Planning with this model prevents trajectories that require impossible curvature or acceleration.

A typical trajectory optimization problem is:

$$\begin{aligned} \min_{x_{0:T},u_{0:T-1}}\quad & \sum_{t=0}^{T-1} \left( \|x_t-x_t^{\mathrm{ref}}\|_Q^2 + \|u_t\|_R^2 + c_{\mathrm{obs}}(x_t) + c_{\mathrm{comfort}}(u_t) \right) \\ \mathrm{s.t.}\quad & x_{t+1}=f(x_t,u_t), \\ & x_t \in \mathcal{X}_{\mathrm{safe}}, \\ & u_t \in \mathcal{U}. \end{aligned}$$

The hard part is not writing this equation. The hard part is making the cost and constraints produce legal, comfortable, robust behavior under uncertainty and real-time compute limits.

Frenet planning represents motion relative to a reference path with longitudinal coordinate $s$ and lateral offset $d$. This simplifies lane-following and lane-change candidates, but it can become awkward in intersections, parking lots, complex merges, and map errors.

Planners usually mix hard constraints and soft costs. A hard constraint forbids collision or actuator-limit violation. A soft cost discourages discomfort, lane-center deviation, or unnecessary braking. Moving a requirement from hard to soft changes system behavior: a soft obstacle cost can be violated if progress reward is high enough, while a hard obstacle constraint can make the optimization infeasible. Safety-critical constraints should be chosen deliberately and paired with fallback behavior when no feasible plan exists.

Uncertainty must change the plan, not merely be logged. A predicted vehicle with high uncertainty may require a larger clearance, lower speed, or a contingency plan. A pedestrian hidden behind a parked van may create an occlusion zone that is treated as potentially occupied. In dense urban driving, planning against the mean prediction is often less safe than planning against a set of plausible futures.

Real-time planning is also a scheduling problem. A theoretically better plan that arrives after the control deadline may be worse than a simpler plan delivered on time. Production planners often keep a previously safe trajectory, use warm starts, and maintain emergency fallback trajectories so that solver hiccups do not immediately become unsafe behavior.

Visual

This diagram expands planning into a search/candidate layer and a constrained optimization layer. A*/Hybrid A* makes graph expansion, heuristic priority, dynamics checks, and collision checks explicit, while MPC shows the finite-horizon variables, constraints, solver deadline, first-action execution, and dotted receding-horizon feedback.

Worked example 1: A* priorities on a small grid

Problem: A grid planner starts at $S=(0,0)$ and wants goal $G=(3,0)$. Moves cost 1. There is an obstacle at $(1,0)$, so the planner considers node $A=(0,1)$ and node $B=(1,1)$. Use Manhattan heuristic $h(x,y)=\vert 3-x\vert +\vert 0-y\vert$. Compute $f=g+h$ for both.

1. Node $A=(0,1)$ is one move from start:

$$g(A)=1.$$

2. Its heuristic is:

$$h(A)=|3-0|+|0-1|=3+1=4.$$

3. Its priority is:

$$f(A)=1+4=5.$$

4. Node $B=(1,1)$ is reached by $S \to A \to B$, so:

$$g(B)=2.$$

5. Its heuristic is:

$$h(B)=|3-1|+|0-1|=2+1=3.$$

6. Its priority is:

$$f(B)=2+3=5.$$

Answer: both nodes have priority 5. A* may choose either depending on tie-breaking, but both lie on shortest detours around the obstacle.

Check: The obstacle blocks the direct path, so the shortest feasible route must step up, go around, and step down.

Worked example 2: Checking curvature from steering

Problem: A vehicle with wheelbase $L=2.8$ m has maximum steering angle $30^\circ$. What is the minimum turning radius under the kinematic bicycle model? Can it follow a planned curve with radius 4 m?

1. The curvature relation is:

$$\kappa = \frac{\tan\delta}{L}.$$

2. Turning radius is $R = 1/\kappa = L/\tan\delta$.

3. Substitute $\delta=30^\circ$:

$$R_{\min} = \frac{2.8}{\tan(30^\circ)} = \frac{2.8}{0.577} \approx 4.85\ \mathrm{m}.$$

4. Compare planned radius:

$$4.0\ \mathrm{m} < 4.85\ \mathrm{m}.$$

Answer: the vehicle cannot follow a 4 m radius curve under this steering limit. The planner must relax the path, use a multi-point maneuver, or choose another route.

Check: Smaller radius means higher curvature. Since required curvature exceeds the vehicle's maximum curvature, the path is dynamically infeasible.

Code

import heapq
import math

def astar_grid(start, goal, blocked, width, height):
    def heuristic(p):
        return abs(goal[0] - p[0]) + abs(goal[1] - p[1])

    open_set = [(heuristic(start), 0, start)]
    parent = {start: None}
    best_g = {start: 0}

    while open_set:
        _, g, node = heapq.heappop(open_set)
        if node == goal:
            path = []
            while node is not None:
                path.append(node)
                node = parent[node]
            return path[::-1]

        x, y = node
        for nxt in [(x+1, y), (x-1, y), (x, y+1), (x, y-1)]:
            if not (0 <= nxt[0] < width and 0 <= nxt[1] < height):
                continue
            if nxt in blocked:
                continue
            new_g = g + 1
            if new_g < best_g.get(nxt, math.inf):
                best_g[nxt] = new_g
                parent[nxt] = node
                heapq.heappush(open_set, (new_g + heuristic(nxt), new_g, nxt))
    return None

print(astar_grid((0, 0), (3, 0), blocked={(1, 0)}, width=4, height=3))

Recent systems and papers

Ma et al. adapt RRT-style sampling to structured on-road driving by adding road-scene priors, exact-goal pressure, and vehicle-model postprocessing [1]. Their "fast RRT" starts with offline rule templates for common maneuvers such as straight driving, left turns, right turns, and U-turns, attaches the collision-free parts of those templates to the search tree, and then uses an aggressive extension step that tries to connect directly to the goal instead of merely sampling the goal more often [1]. If that direct rush collides, the algorithm keeps a useful collision-free prefix as a multi-edge branch; if templates and rushing are insufficient, it falls back to ordinary RRT-style expansion with scene-aware sampling and a Dubins-like metric [1]. The resulting path is smoothed through model-based prediction so that the planner outputs a feasible control sequence for a front-steered bicycle model, which is why the method sits between sampling-based planning and the low-level control concerns on this page [1]. For moving obstacles, the paper lifts the problem into configuration-time space, where a spatially clear path can still be rejected because it intersects a future obstacle volume [1]. The reported evidence is scenario-based simulation with prototype constraints, not a modern public AV benchmark: the method is very fast in simple template-covered scenes, remains below real-time budgets in multi-obstacle examples, and should be read as a structured-road planner rather than a generic replacement for closed-loop urban planning [1].

Fu et al. present an urban planning and decision layer built for the IN2BOT competition vehicle, where the hard problem is not finding a globally optimal curve but keeping path and speed commands stable when lane detection, GPS, and GIS map alignment are imperfect [2]. The system fuses lidar, radar, camera, integrated navigation, wheel encoder, obstacle-map, traffic-light, traffic-sign, pedestrian, moving-vehicle, and GIS-route information into local path planning plus a run/stop and expected-speed decision output [2]. Its local planner scores parallel candidate lines on a $3000\times3000$ grid representing a $60$ m by $60$ m area, using obstacle cost, distance cost, and rule cost; it switches among lane-based, GIS-line-based, and road-edge-centerline modes depending on lane availability and the correlation between GIS and perceived road edges [2]. The mode switching and stop/run decisions use hysteresis, and the selected 301-point path is temporally filtered before control so that noisy perception or a mode change does not immediately produce a lateral jump [2]. This is a useful complement to decision making and behavior planning: it shows how behavior cues such as crossroads, stop signs, traffic signs, selected-line angle, obstacle distance, and path confidence can shape speed before the controller sees the trajectory [2]. Evaluation is competition-style rather than dataset-style, with a 14.5 km route requiring four interventions, a 5 km route completed without intervention, and average speed around 35 km/h, so the paper is best treated as a robust structured-urban integration pattern rather than proof of broad dense-city autonomy [2].

Common pitfalls

• Planning a path without timing. Collision avoidance needs time because other agents move.
• Ignoring vehicle footprint. A centerline can be collision-free while the vehicle body clips a curb or cone.
• Using a point-mass planner for a car-like vehicle. Nonholonomic constraints and steering limits matter.
• Treating predictions as deterministic obstacles. Planners should account for uncertainty and multiple possible futures.
• Over-tuning comfort costs until the vehicle becomes indecisive. Comfort matters, but safety and progress still require assertive behavior in some scenarios.
• Validating on easy log replay only. Planning must be tested in closed-loop simulation because planner actions change future scene evolution.

Connections

• Prediction and motion forecasting
• Decision making and behavior planning
• Control: PID, MPC, pure pursuit, and Stanley
• Simulation and data
• Reinforcement learning
• Engineering math for optimization and ODEs
• Further reading: LaValle's planning text, A*, Hybrid A*, RRT*, lattice planning papers, MPC and iLQR trajectory optimization literature.

References

[1] L. Ma, J. Xue, K. Kawabata, J. Zhu, C. Ma, and N. Zheng, "Efficient Sampling-Based Motion Planning for On-Road Autonomous Driving," IEEE Transactions on Intelligent Transportation Systems, vol. 16, no. 4, pp. 1961-1976, Aug. 2015.

[2] M. Fu, W. Song, Y. Yang, and M. Wang, "Path Planning and Decision Making for Autonomous Vehicle in Urban Environment," in Proc. IEEE 18th International Conference on Intelligent Transportation Systems, 2015, pp. 686-692.