1.Apollo Planning 模块总结

关注

原创 2025-08-13 00:13:20 838 阅读 AI 写同款· GEO 优化 ›

Apollo开发-apollo介绍之planning模块(四) 规划(planning)模块的作用是根据感知预测的结果，当前的车辆信息和路况规划出一条车辆能够行驶的轨迹，这个轨迹会交给控制(control)模块，控制模块通过油门，刹车和方向盘使得车辆按照规划的轨迹运行。规划模块的轨迹是短期轨迹，即车辆短期内行驶的轨迹，长期的轨迹是routing模块规划出的导航轨迹，即起点到目的地的轨迹，规划模块会先生成导航轨迹，然后根据导航轨迹和路况的情况，沿着短期轨迹行驶，直到目的地。阅读详情

1.整体架构

1.1 Planning 的上下游

在这里插入图片描述
Planning 模块的输入为：
HMI : 人机交互界面，也就是司机开启了自动驾驶
External Comand：转发的导航命令
Localization : 位置信息
Planning 模块的输出为：
control：输出方向盘和踏板

1.2 Planning 具体运行机制

在这里插入图片描述
Planning 模块由 Planning 基础库和 Planning 插件组成，基础库中包含了一些父类方法，比如 scenario.cc 是一种通用类，插件就是该类的子类
Planning component ：是 Planning 模块的外部接口，Init 函数用于初始化组件
TrafficRule ：主要运行 ApplyRule 函数对交通规则进行处理
PublicRoadPlanner : 对开放道路进行规划
Plan()：使用场景机制进行规划
ScenarioManager：Update() 场景选择，切换
Scenario：具体轨迹规划

在这里插入图片描述

planning_interface_base 是 planning 插件的父类接口，也保存了 scenario tasks traffic_rules 的父类文件
palnner 文件夹中包含几种规划器
pnc_map 是用来生成参考线的插件
scenario 是场景插件
task 是任务插件
traffic_rule 是交通规则插件

1.3 工程架构

架构图

1.3.1 PlanningComponent::Init()

在这里插入图片描述
(1) 定义 Planning 的两种模式

if (FLAGS_use_navigation_mode) {
  planning_base_ = std::make_unique<NaviPlanning>(injector_);
} else {
  planning_base_ = std::make_unique<OnLanePlanning>(injector_);
}

一共有两种规划模式：
NaviPlanning 比较简单用于规划高速场景
OnLanePlanning 用于规划城市道路场，泊车
(2) 订阅模块的初始化
在 planing_component.cc 中对规划的输入和输出进行初始化，也就是调用他们的 Reader 和 Writer 方法
在这里插入图片描述

1.3.2 PlanningComponent::Proc()

在这里插入图片描述

1.4 Senario Stage Task 机制

在不同的 Planner 下有不同的 Scenario，在不同的 Scenario 下又会有不同的动作也就是 Stage ，在不同的 Stage 下又有不同的 task，他们之间相互调用
每个 Senario 使用双层状态机机制，双层状态机 Top Layer 是 Scenario 状态机，BottomLayer 是 Stage 状态机
最外层状态机：
Pull Over 靠边
Parking 泊车
Junction
在这里插入图片描述

2.Planning Base

在这里插入图片描述
这两个 Planner 主要保存在：

navi_planning 和 on_lane_planning 都继承 PlanningBase，这里以 on_lane_planning 为例

2.1 Init 函数

S1:启动参考线

// 参考线提供器，每 50ms 提供一次参考线
reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
    injector_->vehicle_state(), reference_line_config);
reference_line_provider_->Start();

S2:创建 Planner
OnLanePlanning 继承于 public PlanningBase 在该类中有一个 std::shared_ptr<Planner> planner_; 的成员变量
Planner 是具体执行规划的类，在 planner 文件夹下 planner 一共有四种

在这里插入图片描述
每个 Planner 的作用如下：

rtk- 根据录制的轨迹来规划行车路线，
public_road- 开放道路的轨迹规划器，
lattice- 基于网格算法的轨迹规划器，
navi- 基于实时相对地图的规划器。
这里根据不同 config 和 injector_ 创建不同的具体 Planner

首先创建具体 Planner

LoadPlanner();

在该函数中根据 config 创建具体的 Planner ，默认是 PublicRoadPlanner

void PlanningBase::LoadPlanner() {
  // Use PublicRoadPlanner as default Planner
  std::string planner_name = "apollo::planning::PublicRoadPlanner";
  if ("" != config_.planner()) {
    planner_name = config_.planner();
    planner_name = ConfigUtil::GetFullPlanningClassName(planner_name);
  }
  planner_ =
      cyber::plugin_manager::PluginManager::Instance()->CreateInstance<Planner>(
          planner_name);
}

Planner 的 Init 主要是对 Planner 的 injector 进行赋值

return planner_->Init(injector_, FLAGS_planner_config_path);

virtual apollo::common::Status Init(
    const std::shared_ptr<DependencyInjector>& injector,
    const std::string& config_path = "") {
  injector_ = injector;
  return common::Status::OK();
}

2.2 RunOnce 函数

S1:初始化 Frame

// 初始化Frame   
status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);

S2:判断是否符合交通规则

// 判断是否符合交通规则
 for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
   auto traffic_status =
       traffic_decider_.Execute(frame_.get(), &ref_line_info);
   if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
     ref_line_info.SetDrivable(false);
     AWARN << "Reference line " << ref_line_info.Lanes().Id()
           << " traffic decider failed";
   }
 }

S3:Plan
1.调用 OnLanePlanning 的 Plan 方法

// Plan
status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);

2.在具体的 Plan 函数中，planner 调用自己的 Plan 函数

// 调用具体的 planner 执行
auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                             ptr_trajectory_pb);

3.PublicRoadPlanner

OnLanePlanning 类使用的 planner_ 为 PublicRoadPlanner 这个 Planner 也是默认的 Planner

3.1 配置文件

所有 planner 的配置文件都在目标文件夹下的 conf 子文件夹下，定义了每个 Planner 的 scenario
在这里插入图片描述

2.2 PublicRoadPlanner::Init

会对 Senario Manager 进行初始化，也就是创建多个 scenario 放在一个 list 中

scenario_manager_.Init(injector, config_);

这个 planner_ 支持的场景如下：
在这里插入图片描述

2.2 Plan

在 Plan 中会对 Senario 进行更新，然后执行

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  // 更新场景
  scenario_manager_.Update(planning_start_point, frame);
  // 得到场景
  scenario_ = scenario_manager_.mutable_scenario();
  if (!scenario_) {
    return Status(apollo::common::ErrorCode::PLANNING_ERROR,
                  "Unknown Scenario");
  }
  // 执行场景
  auto result = scenario_->Process(planning_start_point, frame);

  if (FLAGS_enable_record_debug) {
    auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                              ->mutable_planning_data()
                              ->mutable_scenario();
    scenario_debug->set_scenario_plugin_type(scenario_->Name());
    scenario_debug->set_stage_plugin_type(scenario_->GetStage());
    scenario_debug->set_msg(scenario_->GetMsg());
  }
  // 更新执行的状态
  if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) {
    // only updates scenario manager when previous scenario's status is
    // STATUS_DONE
    scenario_manager_.Update(planning_start_point, frame);
  } else if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) {
    return Status(common::PLANNING_ERROR,
                  result.GetTaskStatus().error_message());
  }
  return Status(common::OK, result.GetTaskStatus().error_message());
}

3.Scenario

场景介绍
有如下的 Scenario 场景，所有的 Senario 都用 SenarioManager 进行管理，所有的 Scenario 都会继承 Scenario 类重写里面的方法
在这里插入图片描述

在这里插入图片描述
在 SenarioManager 中有一个 list 用于存放需要运行的 senario

std::vector<std::shared_ptr<Scenario>> scenario_list_;

3.1 SenarioManager Init

将场景加载到 list 中
在这里插入图片描述

3.2 SenarioManager Update

在 scenario.cc 文件中 Process 函数会对 Stage 进行调用，首先先对当前 Stage 进行处理，然后更新 Stage 的状态，然后在创建下一个 Stage
在这里插入图片描述

在这个函数中主要是调用，判断是否可以进行该 scenario 的场景奇切换

scenario->IsTransferable(current_scenario_.get(), *frame)

3.3 Scenario IsTransferable

这里拿最简单的 Lane_follow 的场景来说，下面是判断是否可以 Lane follow 的条件：

bool LaneFollowScenario::IsTransferable(const Scenario* other_scenario,
                                        const Frame& frame) {
  if (!frame.local_view().planning_command->has_lane_follow_command()) {
    return false;
  }
  if (frame.reference_line_info().empty()) {
    return false;
  }
  if (other_scenario == nullptr) {
    return true;
  }
  return true;
}

lane follow 比较简单，下面是判断是否可以靠边停车的代码：
判断是否可以靠边停车
代码中主要判断 Reference 的点是否与红绿灯，停止等标志有 Overlap

auto first_encountered_overlaps =
 frame.reference_line_info().front().FirstEncounteredOverlaps();
// 如果处于红绿灯路口、具有停止标志、避让标志，则不允许靠边停车
if (pull_over_scenario) {
  static constexpr double kDistanceToAvoidJunction = 8.0;  // meter
  for (const auto& overlap : first_encountered_overlaps) {
     if (overlap.first == ReferenceLineInfo::PNC_JUNCTION ||
        overlap.first == ReferenceLineInfo::SIGNAL ||
        overlap.first == ReferenceLineInfo::STOP_SIGN ||
        overlap.first == ReferenceLineInfo::YIELD_SIGN) {
        const double distance_to = overlap.second.start_s - dest_sl.s();
        const double distance_passed = dest_sl.s() - overlap.second.end_s;
       if ((distance_to > 0.0 && distance_to < kDistanceToAvoidJunction) ||
         (distance_passed > 0.0 &&
          distance_passed < kDistanceToAvoidJunction)) {
          pull_over_scenario = false;
          break;
       }
    }
  }
}

3.4 Scenario::Process

会运行具体的 Stage

auto ret = current_stage_->Process(planning_init_point, frame);

3.5 Enter

场景的进⼊函数继承于基类的Enter()函数，在⾸次进⼊场景前调⽤做⼀些预处理的⼯作，重置场景内变量。⽐如，在停⽌标记场景的Enter()函数，⾸先寻找参考线的停⽌标记id保存到上下⽂变量中，然后重置停⽌标记的全局变量。

3.6 Exit

场景的退出函数继承于基类的Exit()函数，在场景切出时会被调⽤，可以⽤来清除⼀些全局变量，⽐如停⽌标记场景的切出函数

bool PullOverScenario::Exit(Frame* frame) {
  injector_->planning_context()->mutable_planning_status()->clear_pull_over();
  return true;
}

3.7 Scenario::Init()

场景的初始化需要继承Scenario的Init()函数，场景基类的Init函数主要是从场景插件中加载场景的流⽔线，将加载的 Stage 实例保存到stage_pipeline_map_中
下面以 pull_over_scenario.cc 为例

if (!Scenario::Init(injector, name)) {
    AERROR << "failed to init scenario" << Name();
    return false;
}

如果场景⾃身还有配置⽂件，则可以调⽤Scenario::LoadConfig()函数加载场景⾃身的配置⽂件，保存到场景实例上下⽂变量中context_。

if (!Scenario::LoadConfig<ScenarioPullOverConfig>(
          &context_.scenario_config)) {
    AERROR << "fail to get config of scenario" << Name();
    return false;
}

4.Stage

每个场景⼜分为⼀个或者多个阶段(stage)，每个阶段⼜由不同的任务(task)组成，
每个 Stage 都包含了：路径决策，路径优化，速度决策，速度优化

4.1 配置文件

当场景中存在先后顺序的业务逻辑时，可以将其划分成多个 Stage。⽐如，在红绿灯⽆保护左转场景中可以划分为三个阶段，第⼀个阶段是接近⾏驶到红绿灯停⽌线前的过程，第⼆个是红绿灯为绿灯时慢速观望的过程，第三个是对向直⾏⻋道通畅快速通过的过程

可以根据 Scenario 的 conf 文件中定义不同的 task 文件，
下面以 lane follow stage 为例：
对于 lane follow 来讲他会有 lane follow 的 stage，lane change ，lane borrow 等等的状态
在这里插入图片描述
下面是 stop sign unprotected Stage ，在开边停车的 Scenario 中就可以看出

stage: { 
  name: "STOP_SIGN_UNPROTECTED_PRE_STOP" // 车辆到达停止牌之前
  type: "StopSignUnprotectedStagePreStop"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  ....
}
stage: {
  name: "STOP_SIGN_UNPROTECTED_STOP" // 到达车辆停止牌处如何处理
  type: "StopSignUnprotectedStageStop"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  ......
}
stage: {
  name: "STOP_SIGN_UNPROTECTED_CREEP" // 在停牌处停好车，起步，缓慢前行
  type: "StopSignUnprotectedStageCreep"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  .......
}
stage: {
  name: "STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE" // 通过了停牌路口，进入路口巡航阶段
  type: "StopSignUnprotectedStageIntersectionCruise"
  enabled: true
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  ......
}

4.2 Stage 的状态机

RUNNING：表示当前状态正在运⾏，Scenario：将继续维持当前阶段运⾏；
FINISHED：表示当前阶段完成，Scenario将会切⼊下⼀个阶段运⾏；
ERROR：表示当前规划存在严重故障。Scenario 会将其上报，主⻋将会刹停。

4.3 LaneFollowStage::Process

这里会对每一条参考线进行处理，这里会对每条参考线调用 task 方法

result =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);

4.4 LaneFollowStage::PlanOnReferenceLine

在这个函数中会逐个调用 task 执行

for (auto task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();
    const auto start_planning_perf_timestamp =
        std::chrono::duration<double>(
            std::chrono::system_clock::now().time_since_epoch())
            .count();

    ret.SetTaskStatus(task->Execute(frame, reference_line_info));

5.Task

task 可以理解为具体步骤，如 path 求解器的定义，boundery 函数的定义，以及速度求解器的定义等等
在这里插入图片描述

5.1 配置文件

在这里插入图片描述

5.2 Init()

5.3 Excute()

所有的 task 都会继承 Task 父类，每个子类实现 Execute 函数
在这里插入图片描述

6.Traffic rule

交通规则插件 TrafﬁcRule 主要是在规划模块执行 Scenario 前对交通规则进⾏处理
在这里插入图片描述

6.1 配置文件

配置文件在每个文件夹下的 conf 子文件夹下

6.2 Init()

这里以 Crosswalk 类为例，Init 函数主要是将 conf 文件加载进来

// Load the config this task.
return TrafficRule::LoadConfig<CrosswalkConfig>(&config_);

6.3 Crosswalk::ApplyRule

traffic rule 的运行函数为 applyRule 函数，该函数会将判断结果保存在 reference line 中

...
  if (!FindCrosswalks(reference_line_info)) {
    injector_->planning_context()->mutable_planning_status()->clear_crosswalk();
    return Status::OK();
  }
...
}