Apollo6.0 PathReuseDecider流程与代码解析

Apollo6.0 PathReuseDecider流程与代码解析

0、引言

对于轨迹规划而言，主要包含横向路径以及纵向位置、速度、加速度规划。当前主流算法，主要都是轨迹规划问题拆解为横向和纵向两个部分进行独立规划，以达到复杂问题的降维解耦，减小对运行平台资源的算力需求。

每一次横向路径规划，都会涉及约束边界、坐标转化等计算工作，这些都会带来时间复杂度上的开销;但在某些场景下，对于横向轨迹的生成，有时并不需要高频的定周期或触发式的重新计算生成。因此有必要设计判断是否可以对前周期的轨迹重复利用。比如在换道场景下，周边没有障碍物工况，如果基于实时的最优化生成轨迹，因某一时刻换道跟踪上产生轨迹轨迹偏差，相同引导线、车速等约束下，则每一次换道轨迹都很难做到相对一致，也很难较好的通过参数实现换道总时长相对一致的把控;甚至出现因优化代价权重代价些许差异，导致换道轨迹很难在相对固定时间内快速收敛至目标车道，最终体现在换道时常不一致、每次换道轨迹都不尽相同。

文中如有错误之处，恳请各位大佬指正！。

见下图示意：|

• 黑色轨迹为采用path_reuse，假设横纵向跟踪偏差不大，不涉及重规划，整个换道过程，轨迹完全一致;
• 红色、黄色轨迹为实时规划示意图，采用trajectory_stitcher 拼接(实际上，从上帝视角看，整个换道轨迹每周期都由一段段轨迹拼接而成，前后段轨迹位置、一阶、二阶连续)，此处为了示意清晰，并未绘制很多段曲线拼接;

本文基于开源Apollo6.0代码，对PathReuseDecider模块进行流程与代码解析。该模块简单易懂，但其对于某些场景下横向路径时间空间一致性具有较大作用。

1、算法流程

Apollo6.0中横向路径规划，需要涉及多个模块的协同工作。以LaneFollow场景为例，PathReuseDecider与PathLaneBorrowDecider、PathBoundDecider、PathOptimizer、PathAssessmentDecider以及PathDecider模块都有这紧密连续。一旦path_resuable置位，则以上这些模块都不需要执行内部代码，这样能有效的在某些场景下减轻控制器的运算负载。

模块主函数为Process(),主要就是枚举那些能启用、停止path_reuse功能

1. 输入数据有效性检查
2. path_reuse_decider_config配置文件读取
3. 非LaneFollow_Scenario场景，置位false
4. 未处于IN_CHANGE_LANE状态，置位false;
5. 参考引导线有效，则置位false;
6. 存在可变车道，且为完成换道轨迹生成，则置位false;
7. • 前一时刻采用path_reuse, 若轨迹重规划、轨迹与静态障碍物发生碰撞、轨迹长度过短以及纵向求解失败，则置位false;
   • 否则，只有前方静止障碍物走开(或大于阈值)、纵向求解成功、未与静态障碍物发生碰撞且轨迹长度大于阈值，才可置位true;

Status PathReuseDecider::Process(Frame* const frame,
                                 ReferenceLineInfo* const reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  if (!Decider::config_.path_reuse_decider_config().reuse_path()) {
    ADEBUG << "skipping reusing path: conf";
    reference_line_info->set_path_reusable(false);
    return Status::OK();
  }

  // skip path reuse if not in LANE_FOLLOW_SCENARIO
  const auto scenario_type = injector_->planning_context()
                                 ->planning_status()
                                 .scenario()
                                 .scenario_type();
  if (scenario_type != ScenarioConfig::LANE_FOLLOW) {
    ADEBUG << "skipping reusing path: not in LANE_FOLLOW scenario";
    reference_line_info->set_path_reusable(false);
    return Status::OK();
  }

  // active path reuse during change_lane only
  auto* lane_change_status = injector_->planning_context()
                                 ->mutable_planning_status()
                                 ->mutable_change_lane();
  ADEBUG << "lane change status: " << lane_change_status->ShortDebugString();

  // skip path reuse if not in_change_lane
  if (lane_change_status->status() != ChangeLaneStatus::IN_CHANGE_LANE &&
      !FLAGS_enable_reuse_path_in_lane_follow) {
    ADEBUG << "skipping reusing path: not in lane_change";
    reference_line_info->set_path_reusable(false);
    return Status::OK();
  }

  // for hybrid model: skip reuse path for valid path reference
  const bool valid_model_output =
      reference_line_info->path_data().is_valid_path_reference();
  if (valid_model_output) {
    ADEBUG << "skipping reusing path: path reference is valid";
    reference_line_info->set_path_reusable(false);
    return Status::OK();
  }

  /*count total_path_ when in_change_lane && reuse_path*/
  ++total_path_counter_;

  /*reuse path when in non_change_lane reference line or
    optimization succeeded in change_lane reference line
  */
  bool is_change_lane_path = reference_line_info->IsChangeLanePath();
  if (is_change_lane_path && !lane_change_status->is_current_opt_succeed()) {
    reference_line_info->set_path_reusable(false);
    ADEBUG << "reusable_path_counter[" << reusable_path_counter_
           << "] total_path_counter[" << total_path_counter_ << "]";
    ADEBUG << "Stop reusing path when optimization failed on change lane path";
    return Status::OK();
  }

  // stop reusing current path:
  // 1. replan path
  // 2. collision
  // 3. failed to trim previous path
  // 4. speed optimization failed on previous path
  bool speed_optimization_successful = false;
  const auto& history_frame = injector_->frame_history()->Latest();
  if (history_frame) {
    const auto history_trajectory_type =
        history_frame->reference_line_info().front().trajectory_type();
    speed_optimization_successful =
        (history_trajectory_type != ADCTrajectory::SPEED_FALLBACK);
  }

  // const auto history_trajectory_type = injector_->FrameHistory()s
  //                                          ->Latest()
  //                                          ->reference_line_info()
  //                                          .front()
  //                                          .trajectory_type();
  if (path_reusable_) {
    if (!frame->current_frame_planned_trajectory().is_replan() &&
        speed_optimization_successful && IsCollisionFree(reference_line_info) &&
        TrimHistoryPath(frame, reference_line_info)) {
      ADEBUG << "reuse path";
      ++reusable_path_counter_;  // count reusable path
    } else {
      // stop reuse path
      ADEBUG << "stop reuse path";
      path_reusable_ = false;
    }
  } else {
    // F -> T
    auto* mutable_path_decider_status = injector_->planning_context()
                                            ->mutable_planning_status()
                                            ->mutable_path_decider();
    static constexpr int kWaitCycle = -2;  // wait 2 cycle

    const int front_static_obstacle_cycle_counter =
        mutable_path_decider_status->front_static_obstacle_cycle_counter();
    const bool ignore_blocking_obstacle =
        IsIgnoredBlockingObstacle(reference_line_info);
    ADEBUG << "counter[" << front_static_obstacle_cycle_counter
           << "] IsIgnoredBlockingObstacle[" << ignore_blocking_obstacle << "]";
    // reusing current path:
    // 1. blocking obstacle disappeared or moving far away
    // 2. trimming successful
    // 3. no statical obstacle collision.
    if ((front_static_obstacle_cycle_counter <= kWaitCycle ||
         ignore_blocking_obstacle) &&
        speed_optimization_successful && IsCollisionFree(reference_line_info) &&
        TrimHistoryPath(frame, reference_line_info)) {
      // enable reuse path
      ADEBUG << "reuse path: front_blocking_obstacle ignorable";
      path_reusable_ = true;
      ++reusable_path_counter_;
    }
  }

  reference_line_info->set_path_reusable(path_reusable_);
  ADEBUG << "reusable_path_counter[" << reusable_path_counter_
         << "] total_path_counter[" << total_path_counter_ << "]";
  return Status::OK();
}

重点函数解析

IsCollisionFree()函数其实一种近似计算，只考虑静态障碍物的碰撞处理，采用frenet坐标代替笛卡尔坐标进行碰撞检测。

bool PathReuseDecider::IsCollisionFree(
    ReferenceLineInfo* const reference_line_info) {
  const ReferenceLine& reference_line = reference_line_info->reference_line();
  static constexpr double kMinObstacleArea = 1e-4;
  const double kSBuffer = 0.5;
  static constexpr int kNumExtraTailBoundPoint = 21;
  static constexpr double kPathBoundsDeciderResolution = 0.5;
  // current vehicle sl position
  common::SLPoint adc_position_sl;
  GetADCSLPoint(reference_line, &adc_position_sl);

  // current obstacles
  std::vector<Polygon2d> obstacle_polygons;
  for (auto obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    // filtered all non-static objects and virtual obstacle
    if (!obstacle->IsStatic() || obstacle->IsVirtual()) {
      if (!obstacle->IsStatic()) {
        ADEBUG << "SPOT a dynamic obstacle";
      }
      if (obstacle->IsVirtual()) {
        ADEBUG << "SPOT a virtual obstacle";
      }
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    // Ignore obstacles behind ADC
    if ((obstacle_sl.end_s() < adc_position_sl.s() - kSBuffer) ||
        // Ignore too small obstacles.
        (obstacle_sl.end_s() - obstacle_sl.start_s()) *
                (obstacle_sl.end_l() - obstacle_sl.start_l()) <
            kMinObstacleArea) {
      continue;
    }
    obstacle_polygons.push_back(
        Polygon2d({Vec2d(obstacle_sl.start_s(), obstacle_sl.start_l()),
                   Vec2d(obstacle_sl.start_s(), obstacle_sl.end_l()),
                   Vec2d(obstacle_sl.end_s(), obstacle_sl.end_l()),
                   Vec2d(obstacle_sl.end_s(), obstacle_sl.start_l())}));
  }

  if (obstacle_polygons.empty()) {
    return true;
  }

  const auto& history_frame = injector_->frame_history()->Latest();
  if (!history_frame) {
    return false;
  }
  const DiscretizedPath& history_path =
      history_frame->current_frame_planned_path();
  // path end point
  common::SLPoint path_end_position_sl;
  common::math::Vec2d path_end_position = {history_path.back().x(),
                                           history_path.back().y()};
  reference_line.XYToSL(path_end_position, &path_end_position_sl);
  for (size_t i = 0; i < history_path.size(); ++i) {
    common::SLPoint path_position_sl;
    common::math::Vec2d path_position = {history_path[i].x(),
                                         history_path[i].y()};
    reference_line.XYToSL(path_position, &path_position_sl);
    if (path_end_position_sl.s() - path_position_sl.s() <=
        kNumExtraTailBoundPoint * kPathBoundsDeciderResolution) {
      break;
    }
    if (path_position_sl.s() < adc_position_sl.s() - kSBuffer) {
      continue;
    }
    const auto& vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(
            history_path[i]);
    std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();
    for (const auto& corner_point : ABCDpoints) {
      // For each corner point, project it onto reference_line
      common::SLPoint curr_point_sl;
      if (!reference_line.XYToSL(corner_point, &curr_point_sl)) {
        AERROR << "Failed to get the projection from point onto "
                  "reference_line";
        return false;
      }
      auto curr_point = Vec2d(curr_point_sl.s(), curr_point_sl.l());
      // Check if it's in any polygon of other static obstacles.
      for (const auto& obstacle_polygon : obstacle_polygons) {
        if (obstacle_polygon.IsPointIn(curr_point)) {
          // for debug
          ADEBUG << "s distance to end point:" << path_end_position_sl.s();
          ADEBUG << "s distance to end point:" << path_position_sl.s();
          ADEBUG << "[" << i << "]"
                 << ", history_path[i].x(): " << std::setprecision(9)
                 << history_path[i].x() << ", history_path[i].y()"
                 << std::setprecision(9) << history_path[i].y();
          ADEBUG << "collision:" << curr_point.x() << ", " << curr_point.y();
          Vec2d xy_point;
          reference_line.SLToXY(curr_point_sl, &xy_point);
          ADEBUG << "collision:" << xy_point.x() << ", " << xy_point.y();

          return false;
        }
      }
    }
  }
  return true;
}

• 看完其他路径相关决策器的同学们会发现，一旦path_reusable置位, 则在其他路径规划决策与优化器中都会出现如下代码.

// skip path_lane_borrow_decider if reused path
  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
    // for debug
    AINFO << "skip due to reusing path";
    return Status::OK();
  }

这段代码意思是：一旦配置文件enable_skip_path_tasks功能开启且path_reusable置位则相关模块无需执行主函数。哪问题来了，横向路径的path_data从哪获取呢?

请看PathReuseDecider::TrimHistoryPath()函数。

bool PathReuseDecider::TrimHistoryPath(
    Frame* frame, ReferenceLineInfo* const reference_line_info) {
  ...

//找到上一帧轨迹的起始点
  const common::TrajectoryPoint history_planning_start_point =
      history_frame->PlanningStartPoint();
  common::PathPoint history_init_path_point =
      history_planning_start_point.path_point();
//当前周期规划的起点
  const common::TrajectoryPoint planning_start_point =
      frame->PlanningStartPoint();
  common::PathPoint init_path_point = planning_start_point.path_point();

  const DiscretizedPath& history_path =
      history_frame->current_frame_planned_path();
  DiscretizedPath trimmed_path;
  common::SLPoint adc_position_sl; 
   // 依据当前自车所处位置，计算其frenet坐标
  GetADCSLPoint(reference_line, &adc_position_sl);

  size_t path_start_index = 0;

  for (size_t i = 0; i < history_path.size(); ++i) {
    // 找到上周期轨迹规划的起点索引
    if (history_path[i].s() > 0) {
      path_start_index = i;
      break;
    }
  }

  // get current s=0
  common::SLPoint init_path_position_sl;
  //计算当前轨迹的起点的在上周期reference_line中的frenet坐标
  reference_line.XYToSL(init_path_point, &init_path_position_sl);
  bool inserted_init_point = false;

//匹配当前规划起点位置，裁剪该点之后的轨迹
  for (size_t i = path_start_index; i < history_path.size(); ++i) {
    common::SLPoint path_position_sl;
    common::math::Vec2d path_position = {history_path[i].x(),
                                         history_path[i].y()};

    reference_line.XYToSL(path_position, &path_position_sl);

    double updated_s = path_position_sl.s() - init_path_position_sl.s();
    // insert init point
    if (updated_s > 0 && !inserted_init_point) {
      trimmed_path.emplace_back(init_path_point);
      trimmed_path.back().set_s(0);
      inserted_init_point = true;
    }

    trimmed_path.emplace_back(history_path[i]);

    trimmed_path.back().set_s(updated_s);
  }

  ADEBUG << "trimmed_path[0]: " << trimmed_path.front().s();
  ADEBUG << "[END] trimmed_path.size(): " << trimmed_path.size();

  if (!NotShortPath(trimmed_path)) {
    ADEBUG << "short path: " << trimmed_path.size();
    return false;
  }

  // 更新规划后的路径信息
  auto path_data = reference_line_info->mutable_path_data();
  ADEBUG << "previous path_data size: " << history_path.size();
  path_data->SetReferenceLine(&reference_line);
  ADEBUG << "previous path_data size: " << path_data->discretized_path().size();
  path_data->SetDiscretizedPath(DiscretizedPath(std::move(trimmed_path)));
  ADEBUG << "not short path: " << trimmed_path.size();
  ADEBUG << "current path size: "
         << reference_line_info->path_data().discretized_path().size();

  return true;
}

2、总结

在换道场景下，满足路径重复使用时，可以有效的保障换道轨迹时间空间一致性；能适量地减轻控制器的算力输出。

3、参考

• apollo

在此，感谢本文中索引的博客大佬们将知识share，感谢Baidu apollo开源算法!。