Apollo决策规划planning模块简要分析

决策规划planning模块简要分析

入口

初始化

Planning模块的初始化见Planning::Init()（apollo/modules/planning/planning.cc）。

业务逻辑

Planning模块的业务逻辑见Planning::RunOnce()（apollo/modules/planning/planning.cc），该函数每隔一段时间执行一次（100ms?），由定时器Planning::OnTimer定时调用。

定时任务Planning::RunOnce()大体步骤如下：

• 获取定位数据与底盘数据
  • AdapterManager::GetLocalization()->GetLatestObserved()
  • AdapterManager::GetChassis()->GetLatestObserved()
• 计算拼接轨迹TrajectoryStitcher::ComputeStitchingTrajectory
• 根据拼接轨迹进行决策Planning::Plan()

决策Planning::Plan()大体步骤：

• 调用planner子模块Planner::Plan()进行实际决策
• 输出决策结果ReferenceLineInfo::ExportDecision()

子模块分析

Planner

apollo::planning::Planner
  apollo::planning::EMPlanner
  apollo::planning::LatticePlanner
  apollo::planning::RTKReplayPlanner

子模块planner的基类是Planner类，其包括3个具体实现类（如上）。Planner会加载若干Task，如以EMPlanner为例，主要加载Decider、Optimizer两类：

• 决策器Decider
  • TrafficDecider
  • PathDecider
  • SpeedDecider
• 优化器Optimizer
  • DpPolyPathOptimizer
  • DpStSpeedOptimizer
  • QpSplineStSpeedOptimizer
  • PolyStSpeedOptimizer

EMPlanner::Plan()方法中会调用EMPlanner::PlanOnReferenceLine()，然后这些注册了的Tasks均会被执行，各Task负责各自业务。比如DpPolyPathOptimizer通过动态规划来进行路径规划。

Task

apollo::planning::Task
  apollo::planning::PathDecider
  apollo::planning::PathOptimizer
    apollo::planning::DpPolyPathOptimizer
    apollo::planning::QpSplinePathOptimizer
  apollo::planning::SpeedDecider
  apollo::planning::SpeedOptimizer
    apollo::planning::DpStSpeedOptimizer
    apollo::planning::PolyStSpeedOptimizer
    apollo::planning::QpSplineStSpeedOptimizer
  apollo::planning::TrafficDecider

以DpPolyPathOptimizer为例，DpPolyPathOptimizer类继承自 PathOptimizer类，PathOptimizer::Execute()方法中调用PathOptimizer::Process()虚方法，DpPolyPathOptimizer::Process()方法具体实现如下：

Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
                                    const ReferenceLine &,
                                    const common::TrajectoryPoint &init_point,
                                    PathData *const path_data) {
  if (!is_init_) {
    AERROR << "Please call Init() before Process().";
    return Status(ErrorCode::PLANNING_ERROR, "Not inited.");
  }
  CHECK_NOTNULL(path_data);
  DPRoadGraph dp_road_graph(config_, *reference_line_info_, speed_data);
  dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug());

  if (!dp_road_graph.FindPathTunnel(
          init_point,
          reference_line_info_->path_decision()->path_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();
}

bool DPRoadGraph::FindPathTunnel(
    const common::TrajectoryPoint &init_point,
    const std::vector &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;
  }

  if (!CalculateFrenetPoint(init_point_, &init_frenet_frame_point_)) {
    AERROR << "Fail to create init_frenet_frame_point_ from : "
           << init_point_.DebugString();
    return false;
  }

  std::vector min_cost_path;
  if (!GenerateMinCostPath(obstacles, &min_cost_path)) {
    AERROR << "Fail to generate graph!";
    return false;
  }
  std::vector frenet_path;
  double accumulated_s = init_sl_point_.s();
  const double path_resolution = config_.path_resolution();

  for (std::size_t i = 1; i < min_cost_path.size(); ++i) {
    const auto &prev_node = min_cost_path[i - 1];
    const auto &cur_node = min_cost_path[i];

    const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
    double current_s = 0.0;
    const auto &curve = cur_node.min_cost_curve;
    while (current_s + path_resolution / 2.0 < path_length) {
      const double l = curve.Evaluate(0, current_s);
      const double dl = curve.Evaluate(1, current_s);
      const double ddl = curve.Evaluate(2, current_s);
      common::FrenetFramePoint frenet_frame_point;
      frenet_frame_point.set_s(accumulated_s + current_s);
      frenet_frame_point.set_l(l);
      frenet_frame_point.set_dl(dl);
      frenet_frame_point.set_ddl(ddl);
      frenet_path.push_back(std::move(frenet_frame_point));
      current_s += path_resolution;
    }
    if (i == min_cost_path.size() - 1) {
      accumulated_s += current_s;
    } else {
      accumulated_s += path_length;
    }
  }
  FrenetFramePath tunnel(frenet_path);
  path_data->SetReferenceLine(&reference_line_);
  path_data->SetFrenetPath(tunnel);
  return true;
}

更多信息可参考：http://www.fzb.me/apollo/module_planning.html