论文研读笔记--"Baidu Apollo EM Motion Planner"

Abstract

三大特点:
(1) The top layer of the system is a multilane strategy that handles lane-change scenarios by comparing lane-level trajectories computed in parallel.
(2) Inside the lane-level trajectory generator, it iteratively solves path and speed optimization based on a Frenet frame.
(3) For path and speed optimization, a combination of dynamic programming and spline-based quadratic programming is proposed to construct a scalable and easy-to-tune framework to handle traffic rules, obstacle decisions and smoothness simultaneously.

I. Introduction

输入模块: HD map module,Perception and localization modules,prediction module
目标: generate a safe and smooth trajectory

• safety
  traffic regulations, range coverage(at least an eight second or two hundred meter motion planning trajectory.), cycle time efficiency and emergency safety(react within 100 ms)
  safety module within the motion planner
• passengers’ ride experience
  scenario coverage, traffic regulation and comfort

A. Multilane Strategy

1. a search algorithm with a cost functional
   缺点:
   algorithm to be computationally expensive

not easy to apply traffic regulations
trajectory stability that avoids sudden changes between cycles

2. nonpassive and passive lane-change scenarios: global and dynamic -> parallel framework

• all obstacles and environment information are projected on lane-based Frenet frames
• traffic regulations are bound with the given lanelevel strategy ->lane-level optimizer.
• crosslane trajectory decider

B. Path-Speed Iterative Algorithm

Frenet frames with time (SLT) to reduce the planning dimension with the help of a reference line.

• direct 3D optimization methods:
  • 缺点: trajectory sampling or lattice search -> search complexity -> spatial and temporal search resolutions increase -> suboptimal.
• the path-speed decoupled method
  • 缺点: not optimal with the appearance of dynamic obstacles
  • 优点: achieves more flexibility in both path and speed optimization
  • EM planner 的策略:
    • low speed obstacle: path 与 speed 相互反馈优化: speed profile from the last cycle is used to estimate interactions with oncoming and low-speed dynamic obstacles in the path optimizer. Then, the generated path is sent to the speed optimizer to evaluate an optimal speed profile.
    • high spedd: lane chage.

C. Decisions and Traffic Regulations

1. Traffic regulations
   non-negotiable hard constraint

2. Decisions

• 特点: negotiable based on different scenarios
• 作用: make decisions prior to providing a smooth trajectory -> make on-road intentions clear and reduce the search space for finding the optimal trajectory
• 方法:
  • hand-tuning decisions: tunability <-> scalability
  • model-based decisions: generally discretize the ego car status into finite driving statuses and use data-driven methods to tune the model =>unified framework to handle decisions and obstacle prediction simultaneously.
• 都不好->EM Planner decision 全部依赖于轨迹
  • ego car moving intention is described by a rough and feasible trajectory
  • interactions between obstacles are measured with this trajectory
  • planner will also generate a convex feasible space for smoothing spline parameters based on the trajectory.
  • 传递给 path planner: A quadratic-programming-based smoothing spline solver

II. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

流程

data center module ->
reference line generator with traffic regulations and obstacles(routing module) ->
lane-level motion planning(Frenet frame) ->
lane-level optimizer(SL projection(E-step) ->
path planning(M-step) ->
ST projection(E-step) ->
speed planning(M-step)) ->
reference line trajectory decider(current car status, regulations and the cost of each trajectory)

III. EM PLANNER AT LANE LEVEL

0. EM Iteration

• E-step
  • intentions of dynamic obstacles are described with an obstacle moving trajectory
  • overlap of dynamic obstacles and the ego car at each time point will be mapped in the Frenet frame.
  • the appearance of dynamic obstacles during path optimization will eventually lead to nudging
  • SL projection of dynamic obstacles will only consider low-speed traffic and oncoming obstacles -> high speed: lane change
• M-step 分为 2 步: DP Path/Speed Decision + QP Path/Speed Planning, 其余同上
  • dynamic programming to first obtain a rough solution <- optimal path and speed solution still lies in a non-convex space -> decisions such as nudge yield and overtake
  • a convex hull for the quadratic-programming-based spline optimizer

A. SL and ST Mapping (E-step)

• 坐标转换
  Cartesian space:location and heading $(x, y, θ)$ of ego car and obstacles + curvature and the derivative of curvature $(κ, dκ)$ for the ego car
  ==>Frenet frame coordinates $(s, l, dl, ddl, dddl)$, which represent station, lateral, and lateral derivatives
• 动态障碍物处理
  Frenet 坐标系下基于前帧自车预测与 trajectory estimated from the prediction module 的 bounding box overlapping 判定碰撞风险与 decision making.

B. M-Step DP Path

• 原理: finding an optimal function of lateral coordinate $l = f(s)$ w.r.t. station coordinate in nonconvex SL space
• 第一步: dynamic-programming-based path decision -> output: a rough path profile with feasible tunnels(区域) and obstacle nudge decisions
  • 1. lattice sampler
    • based on Frenet frame
    • Points between different rows are smoothly connected by quintic polynomial edges
    • The interval distance between rows of points depends on the speed, road structure, lane change and so forth
    • allows customizing the sampling strategy based on application scenarios i.e.: a lane change might need a longer sampling interval than current lane driving
    • the lattice total station distance will cover at least 8 seconds or 200 meters for safety considerations
  • 2. Pointwise cost function:a linear combination of smoothness, obstacle avoidance and lane cost functional
    • smoothness:常数积分和: heading difference between the lane and ego car, curvature of the path, derivative of the curvature
    • obstacle: a sequence of fixed station coordinates ${s_0, s_1, …, s_n}$ with all obstacles, individual cost: 阶梯型.
    • lane cost
      • 1. guidance line cost: generally extracted as the centerline of the path.
      • 2. on-road cost: road boundary.
  • 3. dynamic programmming search
• 第二步: spline-based path planning
  

  C. M-Step Spline QP Path

• objective function of the QP path is a linear combination of smoothness costs and guidance line(DP result) cost
• constraints:
  • 1. boundary constraints(feasible ranges at all station points)
       自车形状: add two half circles on the front and rear ends of the ego car: $l_{left_front_corner} = f(s) + sin(θ)l_f + w/2$ -> 泰勒展开线性化
  • 2. dynamic feasibility

D. M-Step DP Speed Optimizer

• 目的: 建图找粗profile
• 流程:
  • 1. 建图 ST Graph 分格 discretized grid
    • piecewise linear speed profile
    • a feasible tunnel
    • obstacle speed decisions
  • 2. 目标方程
    • v-vref a jerk的积分和 + all 障碍物的cost
    • vref: road speed limits, curvature and other traffic regulations.
  • 3. 约束
    • acceleration, jerk limits and a monotonicity constraint(no backwards)
  • 4. 求解优化项
    • 无

E. M-Step QP Speed Optimizer

• 优化DP结果satisfy dynamic requirements
• objective function is a balance between following the guidance line and smoothness.
• 优化方程: $S-S_{ref}$ $S’ S’’$的积分和.
• constraints:单向连续性,初始速度加速度,jerk,限速

F. Notes on Solving Quadratic Programming Problems

• 约束项(600) >> 求解点(100) -> an active set QP solver + last cycle as a hot start

G. Notes on Non-convex Optimization With DP and QP

• DP效率+QP最优

IV. 计算效率

computational complexity of this algorithm is O(n(M+N)): n obstacles with M candidate path profiles and N candidate speed profiles

V. 感触

1. 按场景来分析,lane following 拓展到 lane change, low speed 拓展到 high speed, static obstacle拓展到dynamic obstacle 基本上还是结构化高速场景.
2. frenet坐标系下path speed分开计算,按照从粗到细,先DP(接近于搜索)再二次优化.
3. 决策部分基本上分散在每个模块中.
4. 高度依赖reference line,论文没有介绍reference line不合格,不可用,多条可用时的对策(HD Map不需要考虑?),尤其是8s/200m的reference line要求.
5. 交通规则/舒适性通过采样/搜索的约束加到算法中.
6. 论文中没有探讨障碍物bounding box投射失真的问题(自车的有考虑).
7. 没有讲lane change的部分如何避障(因为换车道也就意味着换rederence line).
8. 文章没有什么细节基本上都是流程,想理解每一步的做法需要看进一步的分析以及源代码.