Baidu Apollo EM Motion Planner

• Baidu Apollo EM Motion Planner is a real-time motion planning system that employs a hierarchical architecture using dynamic programming and quadratic programming for both path and speed optimization.
• It leverages a Frenet-frame representation to decompose the planning problem into iterative subproblems, ensuring smooth and safe trajectories in multilane highway and urban environments.
• Extensive real-world and simulated tests demonstrate the planner’s reliability and scalability, meeting strict computation deadlines (≤100 ms) while producing kinematically feasible paths.

The Baidu Apollo EM Motion Planner is a real-time motion planning system for Level-4 autonomous driving scenarios, implemented within the open-source Apollo platform and deployed on commercial autonomous vehicles since at least Apollo v1.5. It addresses the fully automated planning task for multilane and single-lane environments, integrating safety, comfort, and scalability within highway and urban driving. The EM planner decomposes the overall spatiotemporal planning problem in Cartesian space into iterative path and speed planning subproblems using a Frenet-frame representation. It employs a hierarchical architecture combining a multilane strategy and decoupled lane-level optimizers, each realized via dynamic programming (DP) coupled with quadratic programming (QP) refinement. This design has demonstrated robust real-world and simulated performance, achieving high planning cycle reliability and resolution-complete, kinematically feasible trajectories (Fan et al., 2018, Zhang et al., 2021).

1. Problem Formulation and Planning Objectives

The core objective is the synthesis of real-time, dynamically feasible, and smooth trajectories (path plus speed) over a planning horizon of at least 8 seconds or 200 meters, with a worst-case computation cycle below 100 ms. The planner operates given high-definition map data, a steady stream of static and dynamic obstacle detections, up-to-date ego-vehicle state, and an assigned routing line.

The principal planning goals are:

• Safety: Hard constraints ensure adherence to traffic rules, explicit collision avoidance, timely emergency response (≤100 ms), and a safety shell enabling failsafe vehicle stops.
• Comfort: Trajectory optimization explicitly minimizes jerk and curvature rates, producing smooth polynomials ($f(s)$ for path, $S(t)$ for speed).
• Scalability: The hierarchical approach and decoupled lane-level planners support arbitrary numbers of obstacles and operate across high-speed highways and dense urban cut-in scenarios, bounding decisions to "nudges," "overtakes," or "yields" (Fan et al., 2018).

2. Hierarchical Architecture and Multilane Strategy

The planner is structured in two primary layers:

• Multilane Strategy Layer (Top Layer):

1. Reference-Line Generator extracts all candidate drivable reference lines (lane center-lines) from the HD map based on routing, incorporating lane-specific traffic rules such as right-of-way or speed limits. 2. Parallel Lane-Level Trajectory Generators each instantiate a full EM motion-planning cycle on their candidate lanes using identical Frenet-frame optimizers. 3. Reference-Line Trajectory Decider applies a hard constraint filter and cost-based selection to prioritize feasible, comfort-optimal trajectories among candidates, enforcing safety and compliance (Fan et al., 2018).

• Lane-Level Trajectory Generator (Frenet EM Planner):
  • The reference line defines a local Frenet frame $(s,l)$ (station and lateral offset).
  • Planning alternates between:
  • E-step Path: Obstacle projection into $SL$ (station-lateral) diagrams,
  • M-step Path: Path optimization via DP lattice search and QP spline refinement,
  • E-step Speed: Obstacle projection into $ST$ (station-time) diagrams,
  • M-step Speed: Speed optimization via DP and QP spline refinement.

This structure ensures the system’s scalability and enables independent consideration of each lane and obstacle (Fan et al., 2018, Zhang et al., 2021).

3. Frenet Frame Formulation and Optimization Methods

3.1 Path Planning: Piecewise-Jerk QP in Frenet SL Space

The planner operates on a guide line extracted and then smoothed from the map's center line. It discretizes the longitudinal station $s \in [0,s_{\max}]$ into $n$ points with step $\Delta s$ and introduces, at each point, optimized variables:

• $l_i$ (lateral offset), $l_i'$ (heading), $l_i''$ (curvature); enforced to yield globally $C^2$-continuous curves by matching derivatives across intervals.

The objective penalizes:

• deviation from lane center,
• lateral derivatives (up to jerk),
• “clearance” from obstacles.

The full continuous cost is: $$J[l(\cdot)] = w_\ell\!\int l(s)^2\,ds + w_{\ell'}\!\int \bigl(l'(s)\bigr)^2\,ds + w_{\ell''}\!\int \bigl(l''(s)\bigr)^2\,ds + w_{\ell'''}\!\int \bigl(l'''(s)\bigr)^2\,ds + w_{obs}\!\int \bigl(l(s)-\tfrac{\ell_{\min}(s)+\ell_{\max}(s)}2\bigr)^2 ds$$ where $[\ell_{\min}(s),\,\ell_{\max}(s)]$ defines the safe collision-free corridor (Zhang et al., 2021).

Discrete QP is structured with a decision vector $x$ collecting $l_i, l_i', l_i''$, with the cost in standard quadratic form, and constraints encoding:

• box corridor boundaries on $l_i$,
• linearized kinematic feasibility (curvature upper bounds),
• and all necessary continuity equalities.

3.2 Speed Planning: DP-Lattice and Spline-QP in ST Space

Speed profiling leverages DP search over a discretized $ST$ (station-time) lattice, incorporating dynamic constraints (acceleration, jerk, monotonicity). The refined QP yields a spline $S(t)$ within a convex “speed tunnel” defined by the DP guide, minimizing deviation from desired velocity and penalizing acceleration/jerk violations.

Both subproblems use warm-started, active-set or operator-splitting QP solvers. The path and speed planners are decoupled, simplifying each subproblem and supporting efficient updates (Fan et al., 2018, Zhang et al., 2021).

3.3 E-M Style Iterative Process

In each planning cycle (at ~10 Hz), the E-step alternately projects obstacles into SL or ST space and carves feasible tunnels, while the M-step refines feasible smooth splines via QP, guided by DP results. Previous cycle solutions warm-start the QP, enhancing real-time performance.

4. Algorithmic Workflow and Pseudocode

Each planning cycle proceeds as follows (Fan et al., 2018, Zhang et al., 2021):

1. Guide-line Smoothing and Lane Selection:
   • Extract candidate lines from routing.
   • Smooth with QP (penalize curvature deviation).
2. Obstacle Projection in Frenet Frame:
   • Map obstacles to SL (static/low-speed) or ST (dynamic) diagrams for each candidate.
3. Path Planning (SL):
   • Dynamic Programming: Lattice search to select coarse, obstacle-avoiding paths.
   • QP Refinement: Spline or piecewise-jerk QP imposes smoothness, comfort, and corridor constraints.
4. Speed Planning (ST):
   • Dynamic Programming: Lattice search ensures feasible progress with respect to obstacle temporal placement.
   • QP Refinement: Enforces dynamical smoothness (velocity, acceleration, jerk).
5. Trajectory Selection and Output:
   • Decider selects overall best lane-trajectory meeting safety and comfort.
   • Controller executes resulting path and speed.

Representative Pseudocode ((Fan et al., 2018), adapted):

for each planning cycle: smoothGuide = solveQP(guideLineRaw, laneBounds) obstaclesFrenet = mapToFrenet(obstacles, smoothGuide) lanesUsed = decideLanes(egoState, obstaclesFrenet, mapRules) [l_min[], l_max[]] = computeCorridor(lanesUsed, obstaclesFrenet, Delta_s) x = solveQP(P, q, A_eq, b_eq, A_ineq, b_ineq) pathFrenet = reconstructPath(x, Delta_s) pathMap = frenetToMap(pathFrenet, smoothGuide) output pathMap to speed-planner & controller

5. Implementation, Performance, and Scalability

The Apollo EM Planner is designed for standard production hardware (Nuvo-6108GC, i7 CPU, 16 GB RAM). Core timings are as follows (Fan et al., 2018, Zhang et al., 2021):

Stage	Compute Time	Variables/Constraints
Guide-line QP	~20 ms	~300 variables
Path (SL) DP + QP	~13 ms (10+3 ms)	~900 variables/~1500 cons
Speed (ST) DP + QP	~13 ms (10+3 ms)	similar scale
Overhead (I/O, logic)	~70 ms	—
Total Per Cycle	<100 ms	—

The planner supports $O(n(M+N))$ complexity with respect to number of obstacles ($n$), and candidate paths ($M$) or speeds ($N$), as each lane-level optimizer is independent. Cost weights and planning horizons are human-in-the-loop tuned, and the same parameters are used for both highway and urban deployments (decider weights differ) (Fan et al., 2018).

Traffic signals, stop signs, and similar rules are encoded as hard constraints in the ST space. Fast dynamic obstacles affecting lane changes are handled at the top level, not the local path DP.

6. Experimental Evaluation and Practical Deployment

The EM Motion Planner has undergone extensive real-world and simulated evaluation:

• Real-World Closed-Loop Testing: 3,380 hours and ~68,000 km on public roads across Beijing, Sunnyvale, and San Francisco; >99.7% of cycles met the 100 ms planning deadline. Mixed environments included dense stop-and-go and 100 km/h highways (Fan et al., 2018).
• Simulation: >100,000 hours and >1 million km, enabling edge-case discovery and further validation.
• Quantitative Path Quality: In challenging (e.g., U-turn) scenarios, the planner remains within physical curvature bounds due to kinematic constraints.
• Resolution Completeness: The use of explicit box constraints on lateral motion provides formal completeness up to spatial resolution $\Delta s$ (Zhang et al., 2021).

Qualitative assessments note the "light-decision" approach to cut-in traffic, reducing brittleness in rule-dominated scenarios and naturally yielding “slow, nudge, accelerate” maneuvers. If any QP fails, the DP subroutine provides a backup safe trajectory; thus, safety is preserved even in failure modes.

7. Limitations and Directions for Further Research

Identified areas for enhancement include:

• Dynamic Obstacles: Current path-planning phase considers only static obstacles; dynamic obstacles are processed in the downstream speed planner, potentially requiring reactive re-planning in fast-changing environments (Zhang et al., 2021).
• Kinematic Model Approximation: The small-angle approximation underlying curvature constraints can fail during very tight or complex maneuvers.
• Joint Path-and-Speed Optimization: The decoupled planning approach could be extended to unified spatiotemporal trajectory optimization for potentially improved comfort, though this remains an open research problem.
• Empirical Robustness: The planner demonstrates high reliability in a wide range of simulated and real-world conditions, though performance is subject to perception, prediction, and map quality (Fan et al., 2018, Zhang et al., 2021).

Further research is focusing on tightening dynamic feasibility, incorporating richer modeling of high-speed moving obstacles in the path layer, and exploring full nonlinear curvature constraints for greater handling of complex geometry.

---

References:

• "Baidu Apollo EM Motion Planner" (Fan et al., 2018)
• "Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform" (Zhang et al., 2021)