Apollo：Planning决策规划代码
简介
planning的工程实现是三分策略：分场景，分阶段，分任务实现。即将运动规划问题，先划分到不同场景(泊车，左转等)，再分阶段执行(减速，转弯灯)，最后分任务执行。
Apollo开源框架经过近几年的发展，已经开发了多种方案的规划器，此处采用基于车道线规划的LatticePlanner进行示例解读
为了便于后续简化表述，此处做一些不太严谨的名词约束：
- planning包：表示apollo/modules/planning文件夹下的内容
- planning组件：表示apollo/modules/planning/planning_component.h文件中定义的PlanningComponent类、或者对应的对象
- planning模块：表示来自apollo/modules/planning/planning_base.h中的PlanningBase类、对象，以及其子类，包括OnLanePlanning、NaviPlanning类、对象等
代码结构
第一级
- 第一层级主要由cyber、modules两个目录构成
- cyber是操作系统的组成部分，实现了节点管理、资源调度、消息通讯等功能
- modules集合了各模块的实现，包括perception、location、planning等
第二级
- 在modules中与planning比较相关的是planning、map、common等目录
- planning内部是所有规划（planning）模块的实现代码
- map内部是地图接口相关实现，planning规划时需要调用
- common内部是一些通用代码，包括math函数、proto协议（通信协议的设定）等，属于模块间的共享代码
第三级
- 在planning目录内部，都是与planning模块相关的实现
- 其中launch、dag、conf都是与模块启动相关的配置，用于模块在cyber系统中的注册
- planning_component.h实现了planning模块的封装，以作为操作系统cyber的组件使用
- on_lane_planning.h是继承自planning_base.h的基于车道线的planning模块实现框架逻辑
- planner内部实现了继承自planner.h的多种规划器，如lattice_planner、navi_planner等
- planner内部也实现了基于规划期分配器dispatcher，利用工厂模式实现规划器的动态调度
- 每个planner的具体实现都在对应的目录下，比如lattice_palnner在planning/lattice目录下实现
- lattice目录下包括behavior目录负责建图相关代码实现、trajectory_generation则是负责轨迹生成、优化、组合评估的框架实现
- scenario目录下负责所有的场景实现和场景管理，用来落地分场景的设计策略
- math目录下负责一些数学计算的代码实现，包括1维曲线生成的计算curve1d等
- constraint_checker与lattice_planner相关的约束检查、碰撞检查的代码实现
- common是planning模块内部的共享代码、如trajectory生成、trajectory拼接相关的代码，主要是不同planner之间的共享
- proto内部包含了planning内部需要的一些协议定义，包括planning_conf.proto这类的模块配置的定义。
planning模块启动
- 利用cyber系统的luanch命令启动，启动配置取决于启动文件（*.launch)
- 默认启动文件是launch/planning.launch，内部决定了模块间的通讯配置建图文件(*.dag)
- 默认建图文件是dag/planning.dag，内部实现模块节点间的通讯配置，构建DAG(有向无环图)
- dag中再设置了config中的相关配置文件，以实现planning模块更为细致的配置工作
Planning主流程梳理
整体架构
其内部结构及其与其他模块的交互示意图如下。
Deciders&Optimizers：决策任务和各种优化。优化器特别优化车辆的轨迹和速度，决策者是基于规则的分类决策者。
主流程
Apollo6.0 代码库的planning模块数据流大致如下图所示。
- 一个planning模块，作为planning的核心结构，负责规划工作
- 多个输入，输出通道(channel)，负责与其他模块进行数据通信
- 一个数据缓存区(DependencyInjector)，负责全程数据的缓存与交互
- 两个处理流，分别实现初始化(Init)，和消息响应处理(Proc)
其主流程如下：
四大类
cyber
- 此处的Cyber类为泛指，表示cyber系统，不是具体的类
- Cyber类负责模块管理，消息通讯，资源分配等工作
- cyber中的初始化功能(Init)，定时器功能(Timer)决定了整个planning的工作流水线
- Init代指初始化相关的代码实现，负责触发planning模块的初始化逻辑
- Timer代指定时器相关的代码实现，负责触发planning的消息响应逻辑
- Timer的触发逻辑，与planning模块启动时配置的工作频率有关，cyber系统负责定时触发回调函数proc()
PlanningComponent
- planning模块的封装类，构建planning组件，在cyber完成注册
- 继承自 cyber::Component 类
- 内部包含planning_base_对象，用于管理planning模块框架
OnLanePlanning
- 具体的基于车道线的planning框架类，继承自PlanningBase类
- 内部实现了planning模块的整体框架函数
- 内部包含planner_dispatcher_用于规划器的动态分发
- 内部包含planner_用于实现具体的规划工作
LatticePlanner
- 此处以LatticePlanner是一种规划器 。
- LatticePlanner继承自PlannerWithReferenceLine，间接继承自Planner - 横纵分离的规划器，通过ST、SL的规划实现Trajectory的生成工作。
- 注意：代码库中默认的是PublicRoadPlanner，
PublicRoadPlanner
Public Road Planner是目前默认的Planner，它实现了 EM Planner（Expectation Maximization）算法。
EM Planner是基于轻决策的规划算法。与其他基于重决策的算法相比，EM Planner的优势在于能够处理许多复杂的场景（例如多障碍物的场景）。当基于重决策的方法试图预先确定如何处理每个障碍物时，困难是显而易见的：（1）很难理解和预测障碍物如何与主车相互作用，因此它们的跟随运动难以描述，因此很难被任何规则考虑；（2）当多个障碍物阻塞道路时，无法找到满足所有预定决策的轨迹概率大大降低，从而导致规划的失败。
Planning大概流程是这样：先由 Routing模块进行全局规划得到参考线 Reference_line，然后 Planning模块在此基础上进行局部轨迹规划，EM Planner将路径和速度进行分层规划，并在 SL 和 ST坐标系下，使用动态规划（DP）进行路径和速度的决策和粗规划，然后再使用二次规划（QP）进行平滑处理。
Lattice Planner 用 Frenet 坐标系来表示汽车的状态，在Frenet坐标系下同时进行横向和纵向采样，再进行二维合成生成足够多的轨迹，然后计算每条轨迹的代价，再循环检测过程中每次挑选出代价最低的轨迹，对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条cost最低的轨迹。
该算法的主要执行流程如下：
- Planning主流程先是选择对应的Planner
- 在配置文件中定义了Planner支持的场景(Scenario)，把规划分为具体的几个场景来执行，每个场景又分为几个阶段(Stage)，每个阶段会执行多个任务(Task)，任务执行完成后，对应的场景就完成了
- 不同场景间的切换是由一个状态机(ScenarioDispatch)来控制的。规划控制器根据ReferenceLineProvider提供的参考线，在不同的场景下做切换，生成一条车辆可以行驶的轨迹，并且不断重复上述过程直到到达目的地。
所有规划算法共用的流程略去不表，与PublicRoadPlanner规划算法相关的有两处，一处是PublicRoadPlanner::Init，另一处是PublicRoadPlanner::Plan。
初始化
ScenarioManager会实例化一个全局的scenario_manager_对象来进行场景管理，在PublicRoadPlanner初始化时会调用配置文件里的参数来建立这个对象。
Status PublicRoadPlanner::Init(const PlanningConfig& config) {
config_ = config;
scenario_manager_.Init(config);
return Status::OK();
}
"PublicRoadPlanner"支持的场景有哪些？
// 还是在"/conf/planning_config.pb.txt"中
standard_planning_config {
planner_type: PUBLIC_ROAD
planner_public_road_config {
}
}
默认啥也没有。可以自己配置，比如：
// 还是在"/conf/planning_config.pb.txt"中
standard_planning_config {
planner_type: PUBLIC_ROAD
planner_public_road_config {
// 支持的场景
scenario_type: LANE_FOLLOW // 车道线保持
scenario_type: SIDE_PASS // 超车
scenario_type: STOP_SIGN_UNPROTECTED // 停止
scenario_type: TRAFFIC_LIGHT_PROTECTED // 红绿灯
scenario_type: TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN // 红绿灯左转
scenario_type: TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN // 红绿灯右转
scenario_type: VALET_PARKING // 代客泊车
}
否则：
除了通过配置文件配置场景，还可以通过参数服务器获取支持的配置场景：
运行
接着看"Plan"中的实现
Planner的算法实现依赖于3个输入：
- 车辆自身状态planning_start_point：通过TrajectoryPoint描述。该结构中包含了车辆的位置，速度，加速度，方向等信息。
- 当前环境信息frame：通过Frame描述。前面我们已经提到，Frame中包含了一次Planning计算循环中的所有信息。
- ptr_computed_trajectory
Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame,
ADCTrajectory* ptr_computed_trajectory) {
DCHECK_NOTNULL(frame);
// 更新场景，决策当前应该执行什么场景 ：因为Frame中包含了当前状态的所有信息，所以通过它就可以确定目前是处于哪一个场景下。
scenario_manager_.Update(planning_start_point, *frame);
// 获取当前场景
scenario_ = scenario_manager_.mutable_scenario();
// 执行当前场景的任务：通过Scenario进行具体的处理。
auto result = scenario_->Process(planning_start_point, frame);
// 打印debug信息
if (FLAGS_enable_record_debug) {
auto scenario_debug = ptr_computed_trajectory->mutable_debug()
->mutable_planning_data()
->mutable_scenario();
scenario_debug->set_scenario_type(scenario_->scenario_type());
scenario_debug->set_stage_type(scenario_->GetStage());
scenario_debug->set_msg(scenario_->GetMsg());
}
// 当前场景完成
if (result == scenario::Scenario::STATUS_DONE) {
// only updates scenario manager when previous scenario's status is
// STATUS_DONE
// 如果处理成功，则再次通过ScenarioManager更新。
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();
}
可以看到"Planner"模块把具体的规划转化成一系列的场景，每次执行规划之前先判断更新当前的场景，然后针对具体的场景去执行。
下面我们先看下"Scenario"模块，然后把这2个模块串起来讲解。
Scenario
planning/scenarios目录如下：
| 目录 | 说明 |
|---|---|
| bare_intersection | |
| BUILD | |
| common | |
| emergency | |
| lane_follow | 车道线保持 |
| learning_model | |
| narrow_street_u_turn | 狭窄掉头 |
| park | |
| park_and_go | |
| scenario.cc | |
| scenario_manager.cc | |
| stage.cc | |
| stop_sign | 停止 |
| traffic_light | 红绿灯 |
| util | |
| yield_sign |
其中需要知道场景如何转换，以及每种场景如何执行。
代码框架
navi_planning代码框架？
小结
planner模块流程：
- PlanningComponent在cyber中注册
- 选择Planning
- 根据不同的Dispatcher，分发Planner
参考
- https://zhuanlan.zhihu.com/p/568057286
- https://space.bilibili.com/287989852
旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。
更多推荐
5
0- 0
已为社区贡献65条内容
所有评论(0)