apollo PlanOnReferenceLine函数

betterthanYSD

已于 2023-11-17 10:37:49 修改

阅读量453

点赞数 14

CC 4.0 BY-SA版权

文章标签：自动驾驶 c++

于 2023-11-17 10:32:17 首次发布

本文链接：https://blog.youkuaiyun.com/betterthanYSD/article/details/134456428

本文详细解读了Apollo8.0中的PlanOnReferenceLine函数，涉及判断参考线类型、路径速度融合、终点判断和轨迹有效性检查等步骤，展示了自动驾驶规划中的关键决策过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言

一、PlanOnReferenceLine是什么？

前言

提示：这里可以添加本文要记录的大概内容：

随着自动驾驶技术热，百度的开源工程apollo也更新到8.0版本，很多爱好者们也争相学习apollo工程。本文就从apollo8.0planning模块下的public_road_planner下边的lane_follow_stage.cc浅介绍PlanOnReferenceLine函数

提示：以下是本篇文章正文内容，下面案例可供参考

一、PlanOnReferenceLine是什么？

示例：pandas 是基于NumPy 的一种工具，该工具是为了解决数据分析任务而创建的。

PlanOnReferenceLine是一个决策规划函数，该函数在已知参考线的情况下，在参考线附近生成一条局部轨迹。

二、本层函数内容

1.判断参考线是否是变道参考线

代码如下（示例）：

  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }

该参考线不是变道参考线的情况下，为该参考线增加直行线代价

2.循环执行任务

代码如下（示例）：

  auto ret = Status::OK();
  for (auto* task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();

    ret = task->Execute(frame, reference_line_info);

    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    ADEBUG << "after task[" << task->Name()
           << "]:" << reference_line_info->PathSpeedDebugString();
    ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
    RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);

    if (!ret.ok()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }

    // TODO(SHU): disable reference line order changes for now
    // updated reference_line_info, because it is changed in
    // lane_change_decider by PrioritizeChangeLane().
    // reference_line_info = &frame->mutable_reference_line_info()->front();
    // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
    //        << reference_line_info->IsChangeLanePath();
  }

任务的输入为frame，和reference_line_info。frame是一帧规划中所需要的信息集合，reference_line_info是该参考线的相关信息，内容包含决策类信息，和轨迹信息的存放。这两个变量由apollo规划模块重要的两个类定义而成，这篇文章中暂时不做展开介绍，

3.路径速度融合

代码如下（示例）：

  DiscretizedTrajectory trajectory;
  if (!reference_line_info->CombinePathAndSpeedProfile(
          planning_start_point.relative_time(),
          planning_start_point.path_point().s(), &trajectory)) {
    const std::string msg = "Fail to aggregate planning trajectory.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

路径速度融合的输入是规划起点的相对时间，规划起点的s弧长。输出是离散轨迹。

4.终点判断

代码如下（示例）：

  double dest_stop_s = -1.0;
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->LongitudinalDecision().has_stop() &&
        obstacle->LongitudinalDecision().stop().reason_code() ==
            STOP_REASON_DESTINATION) {
      SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                  reference_line_info->reference_line());
      dest_stop_s = dest_sl.s();
    }
  }

  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    if (!obstacle->IsStatic()) {
      continue;
    }
    if (obstacle->LongitudinalDecision().has_stop()) {
      bool add_stop_obstacle_cost = false;
      if (dest_stop_s < 0.0) {
        add_stop_obstacle_cost = true;
      } else {
        SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                    reference_line_info->reference_line());
        if (stop_sl.s() < dest_stop_s) {
          add_stop_obstacle_cost = true;
        }
      }
      if (add_stop_obstacle_cost) {
        static constexpr double kReferenceLineStaticObsCost = 1e3;
        reference_line_info->AddCost(kReferenceLineStaticObsCost);
      }
    }
  }

判断参考线上是否包含目的地。若有静止的障碍物，在对应参考线的代价上加上静止障碍物代价

5.轨迹有效性检查

代码如下（示例）：

  if (FLAGS_enable_trajectory_check) {
    if (ConstraintChecker::ValidTrajectory(trajectory) !=
        ConstraintChecker::Result::VALID) {
      const std::string msg = "Current planning trajectory is not valid.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }

不解释

6.赋值轨迹

代码如下（示例）：

  reference_line_info->SetTrajectory(trajectory);
  reference_line_info->SetDrivable(true);
  return Status::OK();

将规划的路径赋值到参考线信息内部，并标记该参考线可行驶

总结

以上就是这篇介绍的内容，本文简单介绍了planonreferenceline函数的作用。从代码中可以看出，plaoonreferenceline函数执行过后，本次规划的轨迹已经存放在了reference_line_info当中。随程序流，轨迹将被reference_line_info带出本函数。