[Paper Notes] Baidu Apollo EM Motion Planner

Milpitas, January 30, 2022
  • Hierarchical architecture:
    - Multilane strategy: compute lane-level trajectories in parallel and select the best one in the end.
    - For each lane, separately solve path and speed optimization on a Frenet frame.
    - Each 2D optimization includes an E-step (projection) and an M-step (planning).
    - Both M-steps include a DP step (for non-convex optimization) and a spline-based QP step (for convex optimization).
  • Safety requirements:
    - Traffic regulation obedience.
    - Range coverage (8 seconds or 200 meters).
    - Execution time (100 ms vs 300 ms reaction for a normal human).
    - Safety emergency module.
  • Ride experience requirements:
    - Scenario coverage.
    - Traffic regulation obedience.
    - Comfort (smoothness of the trajectory).
  • Why not do search across multiple lanes?
    - It’s computationally expensive due to the search space.
    - It might not yield stable trajectories or consistent driving behavior.
  • The multilane strategy covers both non-passive lane changes (triggered by routing) and passive lane changes (triggered by blockage).
EM framework, cited from the original paper¹.
  • Trajectory generation can be reduced to a 3D constraint optimization problem in a Frenet frame (on the reference line) with time.
  • Decouple the path and speed optimization to achieve efficiency and flexibility in both of them, as opposed to direct 3D optimization via trajectory sampling or lattice search.
  • Ego car status representation:
    - In Cartesian frame: location (x and y), heading, curvature and the derivative of curvature.
    - In Frenet frame: station (s), lateral (l) and lateral derivatives (dl, ddl, dddl).
  • E-step SL projection:
    - For safety considerations, only consider static, low-speed and oncoming obstacles.
    - Use the speed profile of the last cycle to estimate the station of the ego car over time and thereby project interactions with dynamic obstacles.
  • E-step ST projection:
    - Interactions with both static and dynamic obstacles are projected on the path.
  • DP steps generate convex domains for QP to search for optimal solutions, and meanwhile determine obstacle decisions, such as nudge, yield and overtake.
  • Heavy-decision-based planners are easier to understood and explained, but light-decision-based planners are more scalable when dealing with complicated scenarios.
  • M-step DP path:
    - The DP path step generates a rough path with a feasible tunnel and obstacle nudge decisions.
    - The edge cost functional is a linear combination of smoothness, obstacle avoidance and lane cost functionals.
    - The smoothness functional penalizes the 1st-, 2nd- and 3rd-order derivatives of the heading difference between the lane and ego car.
    - The obstacle cost functional is based on the bounding box distance between the obstacle and ego car.
    - The lane cost penalizes deviation from the guidance line and violation of lane boundaries.
  • M-step spline QP path:
    - The objective function describes the balance between nudging obstacles and smoothness.
    - The linearized constraints include boundary constraints and dynamic feasibility.
    - The feasible range at each station point is approximated under the assumption that the heading difference between the ego car and road station direction is small.
Spline QP speed optimizer, cited from the original paper¹.
  • M-step DP speed optimizer:
    - The DP speed optimizer generates a piecewise-linear speed profile with a feasible tunnel and obstacle speed decisions.
    - The speed profile is represented as station values among discretized grids over evenly sampled time steps, and derivatives are approximated by the finite difference method.
    - The cost penalizes deviation from the reference speed (for traffic regulations), acceleration, jerk, and distance of the ego car to all obstacles.
  • M-step QP speed optimizer:
    - The objective function is a balance between following the guidance line and smoothness.
    - The linearized constraints include monotonicity of station, traffic regulations and vehicle dynamic constraints.
  • On quadratic programming:
    - ~100 locations or time points, 600+ constraints.
    - An active set QP solver is a good fit.
    - Use the result calculated in the last cycle as a hot start.



Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store
Devin Z

Devin Z

认识世界,改造世界 (Seek truth and solve problems)