1.Apollo Planning 模块总结

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 Plannerstd::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_DONEscenario_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;  // meterfor (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)组成，

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: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}....
}
stage: {name: "STOP_SIGN_UNPROTECTED_STOP" // 到达车辆停止牌处如何处理type: "StopSignUnprotectedStageStop"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}......
}
stage: {name: "STOP_SIGN_UNPROTECTED_CREEP" // 在停牌处停好车，起步，缓慢前行type: "StopSignUnprotectedStageCreep"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}.......
}
stage: {name: "STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE" // 通过了停牌路口，进入路口巡航阶段type: "StopSignUnprotectedStageIntersectionCruise"enabled: truetask {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

交通规则插件 TrafficRule 主要是在规划模块执行 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();}
...
}

7.参数设置

7.1 全局参数设置

全局参数定义在 plannning.conf 文件夹下

在 plannig_gflags.cc 中进行初始化，使用 FLAGS_XXX 的方式进行调用