4. EM Planner实践：动态规划与二次规划结合、路径与速度解耦优化、代码实现与调试

EM Planner，说白了就是百度Apollo里最核心的规划算法。我当年刚接触它的时候，第一反应是——这名字起得挺玄乎。后来啃完代码才发现，它本质上就是一套「先粗后细、先路径后速度」的解耦优化框架。

今天咱们就把它拆开揉碎了讲。我会带着你从原理到代码，一步步走通这个流程。

4.1 动态规划（DP）与二次规划（QP）的结合

为什么要结合？因为单一算法搞不定。

动态规划擅长做全局搜索，能找到一条无碰撞的粗路径。但它有个毛病——生成的轨迹不够平滑，车开起来会一顿一顿的。我在项目中遇到过，DP出来的路径，方向盘打起来像在画折线，乘客直接晕车。

二次规划正好相反。它擅长在局部做平滑优化，但如果没有一个好的初始解，它很容易陷入局部最优，甚至直接无解。

所以Apollo的做法是：DP先探路，QP再精修。

你想想看，这就像盖房子。DP是搭脚手架，先把框架立起来；QP是抹水泥，把墙面修平整。缺了哪个都不行。

4.2 路径与速度解耦优化

这是EM Planner最巧妙的设计之一。它把规划问题拆成了两个子问题：

• 路径规划：在SL坐标系下，只关心「往哪走」
• 速度规划：在ST坐标系下，只关心「走多快」

为什么要解耦？因为联合优化太复杂了。路径和速度互相耦合，求解一个高维非线性问题，实时性根本扛不住。

我记得有一次调试，发现车辆在弯道里急刹车。查了半天，原来是路径规划没考虑速度约束，导致速度规划在弯道处强行减速。解耦之后，路径规划会提前预留曲率信息，速度规划就能提前平滑减速。

4.3 代码实现与调试

咱们直接看核心代码。以下是我从Apollo里提炼出来的简化版EM Planner流程：

// 伪代码：EM Planner主流程
bool EMPlanner::Plan(Frame* frame) {
  // 1. DP路径搜索
  DpPath dp_path;
  if (!dp_path.Search(reference_line, obstacles, &dp_result)) {
    AERROR << "DP路径搜索失败";
    return false;
  }

  // 2. QP路径优化
  QpPath qp_path;
  if (!qp_path.Optimize(dp_result, &qp_result)) {
    AWARN << "QP优化失败，回退到DP结果";
    qp_result = dp_result;
  }

  // 3. DP速度规划
  DpStSpeed dp_speed;
  if (!dp_speed.Search(qp_result, obstacles, &st_result)) {
    AERROR << "DP速度规划失败";
    return false;
  }

  // 4. QP速度优化
  QpStSpeed qp_speed;
  if (!qp_speed.Optimize(st_result, &final_trajectory)) {
    AWARN << "QP速度优化失败，回退到DP速度";
    final_trajectory = st_result;
  }

  return true;
}

这段代码看着简单，但坑全在细节里。我挑几个重点说说：

4.3.1 DP路径搜索的代价函数

DP的代价函数设计很关键。我见过很多新手直接把障碍物距离、平滑度、车道偏离三个项加起来，结果路径要么贴着障碍物走，要么在车道里来回晃。

我的做法是：给平滑度加一个非线性权重。当曲率超过某个阈值时，代价指数级增长。这样DP就不会生成急转弯的路径。

// 代价函数示例
double DpPathCost::CalcSmoothCost(double curvature) {
  const double kCurvatureThreshold = 0.1;  // 曲率阈值
  if (std::fabs(curvature) < kCurvatureThreshold) {
    return curvature * curvature;  // 小曲率用二次代价
  } else {
    return 100.0 * curvature * curvature;  // 大曲率用高权重
  }
}

4.3.2 QP优化的约束处理

QP优化最怕约束冲突。比如你同时要求「路径必须平滑」和「路径必须经过某个点」，如果这两个约束矛盾，QP就会无解。

我曾经遇到过一个bug：车辆在狭窄路段突然停住，日志显示QP无解。查了半天，发现是DP生成的路径点太靠近车道边界，QP在优化平滑度时，把路径点推到了车道外，触发了硬约束。

解决方案是：给QP的约束加松弛变量。允许路径稍微偏离DP结果，但付出代价。

// QP约束松弛示例
// 原约束：路径点必须在车道内
// 松弛后：路径点可以在车道外，但代价很高
minimize: 0.5 * x' * H * x + f' * x + w * slack^2
subject to: 
  A * x <= b + slack
  slack >= 0

4.3.3 速度规划的ST图处理

速度规划的核心是ST图。横轴是时间，纵轴是路径长度。障碍物在ST图上表现为一块块「禁区」。DP速度规划就是在ST图上找一条避开所有禁区的路径。

这里有个坑：障碍物的速度预测不准。如果预测障碍物会加速，但实际它减速了，ST图上的禁区位置就会偏移，导致速度规划出来的轨迹要么太激进，要么太保守。

4.4 调试工具与技巧

调试EM Planner，光看日志是不够的。我强烈建议你把中间结果可视化出来。

调试阶段	可视化内容	常见问题
DP路径	离散采样点、代价热力图	采样点太稀疏，漏掉可行路径
QP路径	优化前后的路径对比、曲率曲线	曲率突变、路径抖动
DP速度	ST图、速度曲线	速度突变、频繁加减速
QP速度	加速度曲线、加加速度曲线	加速度超限、舒适度差

我个人习惯用Python脚本把数据dump出来，然后用Matplotlib画图。看一眼曲率曲线，就知道QP有没有优化到位。

4.5 知识体系总览

下面这张图是我自己整理的EM Planner知识体系。你可以把它当作调试时的检查清单。

嗯，这张图基本把EM Planner的骨架画出来了。你调试的时候，可以对照着看：是路径规划出了问题，还是速度规划出了问题？是DP阶段没搜到好路径，还是QP阶段优化过头了？

最后说一句：不要迷信默认参数。每个场景的代价权重、约束阈值都不一样。我见过最夸张的一次，只是把DP的采样间距从0.5米改成0.3米，路径质量就提升了一大截。多试、多看、多调，这才是调试的真谛。

可可精品资料库