1、Apllo算法框架原理
Apollo开源自动驾驶平台中，高清地图模块提供了每个在线模块都可以访问的高清地图。感知和定位模块提供了必要的动态环境信息，可以在预测模块中进一步用于预测未来的环境状态。运动规划模块考虑所有信息，以生成安全平滑的轨迹，并将其输入车辆控制模块。
目前Apollo官方最新版本Apollo 7.0在感知和预测模块集成了3个全新的深度学习模型。该版本引入了Apollo Studio，并与数据管道相结合，以提供一站式在线开发平台。Apollo 7.0还发布了基于先前模拟服务的PnC强化学习模型培训和模拟评估服务。在我们的专栏里将会重点介绍PnC模块。
2、Apollo规划模块概述
Apollo 规划模块功能的实现是基于多个场景（scenario-based）实现的，不同的场景分成多个阶段（stage），每一阶段执行多个任务（task）来完成轨迹规划。其功能是规划出实时的轨迹，并保证轨迹的安全、避障、遵守交通规则、舒适度等。
规划模块代码架构如下，输入包括Routing信息、感知预测信息（障碍物、交通灯）以及车辆地盘信息、高精地图和定位信息。在Apollo的CyberRT框架中，输入输出是由Reder和Writer组成，在planning_component.h里。其为消息触发。
bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&
localization_estimate) override;
输出 ：ADCTrajectory即轨迹的相关信息，时间、路径长度、轨迹点。代码在planning.proto中。
planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
Planning类实现两个线程：ReferenceLineProvider启动了一个单独的线程，每隔50ms执行一次，和Planning主流程并行执行。
3、规划模块代码框架
1、重要数据结构
LocalView: 包含规划模块所有上游输入数据。
（预测障碍物、底盘信息、定位信息、交通灯信息、routing信息、相对地图）
ReferenceLine: 规划的参考线。（地图原始中线，参考线优化前的初始轨迹点）
**ReferenceLineInfo:**对ReferenceLine进行封装还包括参考线算法中其他信息。
Frame: 包括规划模块一次循环中用到的所有信息。（地图、规划初始点、车辆状态、参考线、障碍物、交通灯等信息）
三种规划器：
OnLanePlanning（车道规划，可用于城区及高速公路各种复杂道路）
NaviPlanning（导航规划，主要用于高速公路）
OpenSpacePlanning （自主泊车和狭窄路段的掉头）
四种规划算法：
PublicRoadPlanner（默认规划器）
LatticePlanner、NaviPlanner（主要用于高速公路场景）
RTKPlanner（循迹算法，一般不用）
2、运行机制
（1） 入口函数PlanningComponent::Proc()，在cyber中注册模块。
bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&
localization_estimate) override;
过程：模块注册（planning_component.h）-> 模块初始化（OnLanePlanning::Plan(和proc）-> 模块运行（OnLanePlanning::RunOnce() ）
（2） 选择Planning模式：
OnLanePlanning（车道规划，用于城区及高速公路各种复杂道路）
NaviPlanning（导航规划，用于高速公路）
OpenSpacePlanning （自主泊车和狭窄路段掉头）
注： Planning类实现两个线程：ReferenceLineProvider启动了一个单独的线程，每隔50ms执行一次，和Planning主流程并行执行。
（3） 根据不同的Dispatcher，选择Planner。
PublicRoadPlanner（默认规划器）
LatticePlanner、NaviPlanner（用于高速公路场景）
RTKPlanner（循迹算法，一般不用）
// 配置文件
standard_planning_config {
planner_type: PUBLIC_ROAD
planner_type: OPEN_SPACE
...
}
// OnLanePlannerDispatcher具体实现
std::unique_ptr<Planner> OnLanePlannerDispatcher::DispatchPlanner() {
PlanningConfig planning_config;
apollo::cyber::common::GetProtoFromFile(FLAGS_planning_config_file,
&planning_config);
if (FLAGS_open_space_planner_switchable) {
return planner_factory_.CreateObject(
// OPEN_SPACE规划器
planning_config.standard_planning_config().planner_type(1));
}
return planner_factory_.CreateObject(
// PUBLIC_ROAD规划器
planning_config.standard_planning_config().planner_type(0));
}
（4） OnLanePlanning的主要逻辑在"RunOnce()"中，调用具体的Planer实现轨迹规划算法(scenario->stage->task），实现在Init和Plan中
void OnLanePlanning::RunOnce(const LocalView& local_view,
ADCTrajectory* const ptr_trajectory_pb) {
// 初始化Frame
status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
...
// 判断是否符合交通规则
for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
TrafficDecider traffic_decider;
traffic_decider.Init(traffic_rule_configs_);
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";
continue;
}
}
// 执行计划
status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
...
}
Status OnLanePlanning::Plan(
const double current_time_stamp,
const std::vector<TrajectoryPoint>& stitching_trajectory,
ADCTrajectory* const ptr_trajectory_pb) {
...
// 调用具体的(PUBLIC_ROAD)Planner执行
auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
ptr_trajectory_pb);
...
}
(5) 具体的轨迹规划过程PublicRoadPlanner（类似Em planer算法）：scenario->stage->task
场景注册：
Status PublicRoadPlanner::Init(const PlanningConfig& config) {
// 读取public Road配置
const auto& public_road_config =
config_.standard_planning_config().planner_public_road_config();
// 根据配置注册不同的场景
for (int i = 0; i < public_road_config.scenario_type_size(); ++i) {
const ScenarioConfig::ScenarioType scenario =
public_road_config.scenario_type(i);
supported_scenarios.insert(scenario);
}
scenario_manager_.Init(supported_scenarios);
}
场景运行：
Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame,
ADCTrajectory* ptr_computed_trajectory) {
DCHECK_NOTNULL(frame);
// 更新场景，决策当前应该执行什么场景
scenario_manager_.Update(planning_start_point, *frame);
// 获取当前场景
scenario_ = scenario_manager_.mutable_scenario();
// 执行当前场景的任务
auto result = scenario_->Process(planning_start_point, frame);
// 当前场景完成
if (result == scenario::Scenario::STATUS_DONE) {
// only updates scenario manager when previous scenario's status is
// STATUS_DONE
scenario_manager_.Update(planning_start_point, *frame);
} else if (result == scenario::Scenario::STATUS_UNKNOWN) {
// 当前场景失败
return Status(common::PLANNING_ERROR, "scenario returned unknown");
}
return Status::OK();
}
场景更新
bool ScenarioManager::Init(
const std::set<ScenarioConfig::ScenarioType>& supported_scenarios) {
// 注册场景
RegisterScenarios();
default_scenario_type_ = ScenarioConfig::LANE_FOLLOW;
supported_scenarios_ = supported_scenarios;
// 创建场景，默认为lane_follow
current_scenario_ = CreateScenario(default_scenario_type_);
return true;
}
// 更新场景
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
const Frame& frame) {
CHECK(!frame.reference_line_info().empty());
// 保留当前帧
Observe(frame);
// 场景分发
ScenarioDispatch(ego_point, frame);
}
// 通过一个有限状态机，决定当前的场景
void ScenarioManager::ScenarioDispatch(const common::TrajectoryPoint& ego_point,
const Frame& frame) {
...
}
运行场景：
/ Scenario对应的Stage
scenario_type: SIDE_PASS
stage_type: SIDE_PASS_APPROACH_OBSTACLE
stage_type: SIDE_PASS_GENERATE_PATH
stage_type: SIDE_PASS_STOP_ON_WAITPOINT
stage_type: SIDE_PASS_DETECT_SAFETY
stage_type: SIDE_PASS_PASS_OBSTACLE
stage_type: SIDE_PASS_BACKUP
// Stage对应的Task
stage_type: SIDE_PASS_APPROACH_OBSTACLE
enabled: true
task_type: DP_POLY_PATH_OPTIMIZER
task_type: PATH_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: QP_SPLINE_ST_SPEED_OPTIMIZER
执行阶段:
Stage::StageStatus LaneFollowStage::Process(
const TrajectoryPoint& planning_start_point, Frame* frame) {
...
// 根据参考线规划
auto cur_status =
PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
...
}
// LANE_FOLLOW中的PlanOnReferenceLine
Status LaneFollowStage::PlanOnReferenceLine(
const TrajectoryPoint& planning_start_point, Frame* frame,
ReferenceLineInfo* reference_line_info) {
// 顺序执行stage中的任务
for (auto* optimizer : task_list_) {
const double start_timestamp = Clock::NowInSeconds();
// 任务
ret = optimizer->Execute(frame, reference_line_info);
}
// 增加障碍物的代价
for (const auto* obstacle :
reference_line_info->path_decision()->obstacles().Items()) {
if (obstacle->IsVirtual()) {
continue;
}
if (!obstacle->IsStatic()) {
continue;
}
if (obstacle->LongitudinalDecision().has_stop()) {
...
}
}
// 返回参考线
reference_line_info->SetTrajectory(trajectory);
reference_line_info->SetDrivable(true);
return Status::OK();
}
接下来就是执行各个task，执行具体的轨迹规划过程（DP+QP），在之后的博文中，我们继续学习。
深入学习上述每一步具体地过程可看相关函数：
（1） 模块注册（planning_component.h）-> 模块初始化（OnLanePlanning::Plan(和proc）-> 模块运行（OnLanePlanning::RunOnce() ）
（2） 默认OnLanePlanning，其输入：Planning上下文信息、Frame结构体。-> 初始化：分配具体planer、参考线生成（Init）-> RunOnce消息触发
（3） Planner注册 -> PublicRoadPlanner默认规划器(Init和Plan中实现) -> init(注册场景) - >plan（运行场景scenario.cc）-> 状态机实现场景转换（主要Scenario_Manager.cc中）
（4） 执行场景（planning/conf/scenario中Process函数））-> 执行stage（planning/conf/stage中Process函数）->任务 task (决策器decider和优化器optimizer：动态规划DP和二次规划QP）。
整理不易，欢迎点赞订阅，一起学习交流！
参考链接：
Apollo官网源码
Apollo规划模块代码详解
apollo介绍之planning模块（四）
图片来源于网络和参考链接，侵权联系删