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爲例,主要加載DeciderOptimizer兩類:

  • 決策器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<const PathObstacle *> &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<DPRoadGraphNode> min_cost_path;
  if (!GenerateMinCostPath(obstacles, &min_cost_path)) {
    AERROR << "Fail to generate graph!";
    return false;
  }
  std::vector<common::FrenetFramePoint> 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

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章