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
更多推荐
所有评论(0)