Baidu Apollo代码解析之EM Planner中的DP Path Optimizer

大家好，我已经把优快云上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

apollo中的EM Planner采用优化的思路做轨迹规划，并将轨迹分为path和speed 2部分，分别优化，求取5次多项式曲线，最终合并为一条trajectory。优化过程分为动态规划DP和二次规划QP。我打算分多篇文章介绍DP-Path、QP-Path、DP-Speed、QP-Speed 4部分对应的代码。目前只看完了DP-Path的代码，那就从这个最简单的部分开始吧。

有关EM Planner的objective functions和constraints请参考官方论文和我的一篇博文。

DP-Path的主要思路是以自车当前位置为起点，沿着车道横纵向（横向为L或d，纵向为s，这里为了字母更清晰，横向统一以d表示）采样一些点，横向采样的一组点叫做level，点封装成node后，分别计算不同level间的node的cost，就构成了graph。利用DP更新node的最小cost，便找到了代价最小的一条路径。听起来很像传统的图搜索方法找最短路径，区别在于cost包含了平滑、无碰的指标。

DP函数入口在DpPolyPathOptimizer::Process()，该函数构造DpRoadGraph类的一个实例，所有构造graph、计算cost、search的过程都在dp_road_graph.FindPathTunnel()中。

//处理结果：最短路径就是依据若干level之间分段5次多项式的采样点，保存在
//path_data.frenet_path_（SL系）和path_data.discretized_path_（XY系）中
Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
                                    const ReferenceLine &,
                                    const common::TrajectoryPoint &init_point,
                                    PathData *const path_data) {
  CHECK_NOTNULL(path_data);
  const auto &dp_poly_path_config = config_.dp_poly_path_config();
  DpRoadGraph dp_road_graph(dp_poly_path_config, *reference_line_info_,
                            speed_data);
  dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug());
  dp_road_graph.SetWaypointSampler(
      new WaypointSampler(dp_poly_path_config.waypoint_sampler_config()));

  if (!dp_road_graph.FindPathTunnel(
          init_point,
          reference_line_info_->path_decision()->obstacles().Items(),
          path_data)) {
    AERROR << "Failed to find tunnel in road graph";
    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation");
  }

  return Status::OK();
}

FindPathTunnel()主要分为3部分：先设置相关前提条件，然后查找代价最小路径，最后对每段代价最小路径采样以构造FrenetFramePath类的实例，并存入path_data中。

//FindPathTunnel()的结果是依据若干level之间分段5次多项式的采样点，
//保存在path_data.frenet_path_（SL系）和path_data.discretized_path_（XY系）中
bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
                                 const std::vector<const Obstacle *> &obstacles,
                                 PathData *const path_data) {
  CHECK_NOTNULL(path_data);

  init_point_ = init_point;
  if (!reference_line_.XYToSL(
          {init_point_.path_point().x(), init_point_.path_point().y()},
          &init_sl_point_)) {
    AERROR << "Fail to create init_sl_point from : "
           << init_point.DebugString();
    return false;
  }
  //起始点所对应的在参考线上的点
  init_frenet_frame_point_ =
      reference_line_.GetFrenetPoint(init_point_.path_point());

  waypoint_sampler_->Init(&reference_line_info_, init_sl_point_,
                          init_frenet_frame_point_);
  waypoint_sampler_->SetDebugLogger(planning_debug_);

  std::vector<DpRoadGraphNode> min_cost_path;
  if (!GenerateMinCostPath(obstacles, &min_cost_path)) {
    AERROR &l