欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-07-12 13:16:01
...

决策规划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