解析百度Apollo之决策规划模块

本文是Apollo项目系列文章中的一篇，会解析自动驾驶系统中最核心的模块 - 决策规划模块。

前言

Apollo系统中的Planning模块实际上是整合了决策和规划两个功能，该模块是自动驾驶系统中最核心的模块之一（另外三个核心模块是：定位，感知和控制）。

关于决策规划的理论值得我们研究好久。所以接下来会通过几篇文章来专门讲解Planning模块。

这些文章会以百度Apollo的Planning模块实现为基础。当然，我也希望我们能够不单单局限于Apollo项目实现。如果可以，尽可能的一起了解一下目前这些技术的其他研究成果。

本文是讲解Planning模块的第一篇文章，会对Planning模块的整体架构做一些解析。

其他关于Apollo相关的文章如下：

• 解析百度Apollo自动驾驶平台
• 解析百度Apollo之Routing模块

本文以Apollo项目2019年初的版本为基础进行讲解。

• 版本：3.5
• 获取代码时间：2019年1月24日

Apollo系统与Planning模块

下图是Apollo系统的整体架构图。从这幅图中我们可以看出，整个系统可以分为5层。从下至上依次是：

• 车辆认证平台：经过Apollo认证的电子线控车辆。
• 硬件开发平台：包含了计算单元以及各种传感器，例如：GPS，摄像头，雷达，激光雷达等等。
• 开放软件平台：这就是Apollo开源代码的主体，也是自动驾驶最核心的部分。
• 云服务平台：包含了各种云端服务，实现自动驾驶的车辆一定不是孤立的，而是跑在基于互联网的云端上的。
• 量产交付方案：专门为各种场景量产的解决方案。

我们可以再将最核心的开放软件平台这一层放大近看一下。

下面这幅图描述了这其中的模块和它们之间的交互关系。

这其中的黄线代表了数据流，黑线代表了控制流。

Planning模块负责整个车辆的驾驶决策，而驾驶决策需要根据当前所处的地理位置，周边道路和交通情况来决定。Planning不直接控制车辆硬件，而是借助于控制模块来完成。

从这幅图中可以看出，对于Planning模块来说：

• 它的上游模块是：定位，地图，导航，感知和预测模块。
• 它的下游模块是控制模块。

模块概述

决策规划模块的主要责任是：根据导航信息以及车辆的当前状态，在有限的时间范围内，计算出一条合适的轨迹供车辆行驶。

这里有好几个地方值得注意：

1. 车辆的行驶路线通常由Routing模块提供，Routing模块会根据目的地以及地图搜索出一条代价尽可能小的路线。关于Routing模块我们已经在另外一篇文章中详细讲解过（《解析百度Apollo之Routing模块》），这里不再赘述。
2. 车辆的当前状态包含了很多因素，例如：车辆自身的状态（包括姿态，速度，角速度等等），当前所处的位置，周边物理世界的静态环境以及交通状态等等。
3. Planning模块的响应速度必须是稳定可靠的（当然，其他模块也是一样）。正常人类的反应速度是300ms，而自动驾驶车辆想要做到安全可靠，其反应时间必须短于100ms。所以，Planning模块通常以10Hz的频率运行着。如果其中某一个算法的时间耗费了太长时间，就可能造成其他模块的处理延迟，最终可能造成严重的后果。例如：没有即时刹车，或者转弯。
4. ”合适的轨迹“有多个层次的含义。首先，”轨迹“不同于“路径”，“轨迹”不仅仅包含了行驶路线，还要包含每个时刻的车辆的速度，加速度，方向转向等信息。其次，这条轨迹必须是底层控制可以执行的。因为车辆在运动过程中，具有一定的惯性，车辆的转弯角度也是有限的。在计算行驶轨迹时，这些因素都要考虑。最后，从人类的体验上来说，猛加速，急刹车或者急转弯都会造成非常不好的乘坐体验，因此这些也需要考虑。这就是为什么决定规划模块需要花很多的精力来优化轨迹，Apollo系统中的实现自然也不例外。

模块架构

Apollo的之前版本，包括3.0都是用了相同的配置和参数规划不同的场景，这种方法虽然线性且实现简单，但不够灵活或用于特定场景。随着Apollo的成熟并承担不同的道路条件和驾驶用例，Apollo项目组采用了更加模块化、适用于特定场景和整体的方法来规划其轨迹。

在这个方法中，每个驾驶用例都被当作不同的驾驶场景。这样做非常有用，因为与先前的版本相比，现在在特定场景中报告的问题可以在不影响其他场景的工作的情况下得到修复，其中问题修复影响其他驾驶用例，因为它们都被当作单个驾驶场景处理。

Apollo 3.5中Planning模块的架构如下图所示：

这其中主要的组件包括：

• Apollo FSM：一个有限状态机，与高清地图确定车辆状态给定其位置和路线。
• Planning Dispatcher：根据车辆的状态和其他相关信息，调用合适的Planner。
• Planner：获取所需的上下文数据和其他信息，确定相应的车辆意图，执行该意图所需的规划任务并生成规划轨迹。它还将更新未来作业的上下文。
• Deciders和Optimizers：一组实现决策任务和各种优化的无状态库。优化器特别优化车辆的轨迹和速度。决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行（慢速行进）或爬行何时完成。
• 黄色框：这些框被包含在未来的场景和/或开发人员中，以便基于现实世界的驱动用例贡献他们自己的场景。

整体Pipeline

有相关专业知识的人都会知道，决策规划模块的主体实现通常都是较为复杂的。

Apollo系统中的实现自然也不例外，这里先通过一幅图说明其整体的Pipeline。然后在下文中我们再逐步熟悉每个细节。

这里有三个主要部分需要说明：

• PncMap：全称是Planning and Control Map。这个部分的实现并不在Planning内部，而是位于/modules/map/pnc_map/目录下。但是由于该实现与Planning模块紧密相关，因此这里放在一起讨论。该模块的主要作用是：根据Routing提供的数据，生成Planning模块需要的路径信息。
• Frame：Frame中包含了Planning一次计算循环中需要的所有数据。例如：地图，车辆状态，参考线，障碍物信息等等。ReferenceLine是车辆行驶的参考线，TrafficDecider与交通规则相关，这两个都是Planning中比较重要的子模块，因此会在下文中专门讲解。
• EM Planner：下文中我们会看到，Apollo系统中内置了好几个Planner，但目前默认使用的是EM Planner，这也是专门为开放道路设计的。该模块的实现可以说是整个Planning模块的灵魂所在。因此其算法值得专门用另外一篇文章来讲解。读者也可以阅读其官方论文来了解：Baidu Apollo EM Motion Planner。

基础数据结构

Planning模块是一个比较大的模块，因此这其中有很多的数据结构需要在内部实现中流转。

这些数据结构集中定义在两个地方：

• proto目录：该目录下都是通过Protocol Buffers格式定义的结构。这些结构会在编译时生成C++需要的文件。这些结构没有业务逻辑，就是专门用来存储数据的。（实际上不只是Planning，几乎每个大的模块都会有自己的proto文件夹。）
• common目录：这里是C++定义的数据结构。很显然，通过C++定义数据结构的好处是这些类的实现中可以包含一定的处理逻辑。

proto

proto目录下的文件如下所示：

apollo/modules/planning/proto/
├── auto_tuning_model_input.proto
├── auto_tuning_raw_feature.proto
├── decider_config.proto
├── decision.proto
├── dp_poly_path_config.proto
├── dp_st_speed_config.proto
├── lattice_sampling_config.proto
├── lattice_structure.proto
├── navi_obstacle_decider_config.proto
├── navi_path_decider_config.proto
├── navi_speed_decider_config.proto
├── pad_msg.proto
├── planner_open_space_config.proto
├── planning.proto
├── planning_config.proto
├── planning_internal.proto
├── planning_stats.proto
├── planning_status.proto
├── poly_st_speed_config.proto
├── poly_vt_speed_config.proto
├── proceed_with_caution_speed_config.proto
├── qp_piecewise_jerk_path_config.proto
├── qp_problem.proto
├── qp_spline_path_config.proto
├── qp_st_speed_config.proto
├── reference_line_smoother_config.proto
├── side_pass_path_decider_config.proto
├── sl_boundary.proto
├── spiral_curve_config.proto
├── st_boundary_config.proto
├── traffic_rule_config.proto
└── waypoint_sampler_config.proto

我们在Routing模块讲解的文章中已经提到，通过proto格式定义数据结构好处有两个：

• 自动生成C++需要的数据结构。
• 可以方便的从文本文件导入和导出。下文将看到，Planning模块中有很多配置文件就是和这里的proto结构相对应的。

common

common目录下的头文件如下：

apollo/modules/planning/common/
├── change_lane_decider.h
├── decision_data.h
├── distance_estimator.h
├── ego_info.h
├── frame.h
├── frame_manager.h
├── indexed_list.h
├── indexed_queue.h
├── lag_prediction.h
├── local_view.h
├── obstacle.h
├── obstacle_blocking_analyzer.h
├── path
│   ├── discretized_path.h
│   ├── frenet_frame_path.h
│   └── path_data.h
├── path_decision.h
├── planning_context.h
├── planning_gflags.h
├── reference_line_info.h
├── speed
│   ├── speed_data.h
│   ├── st_boundary.h
│   └── st_point.h
├── speed_limit.h
├── speed_profile_generator.h
├── threshold.h
├── trajectory
│   ├── discretized_trajectory.h
│   ├── publishable_trajectory.h
│   └── trajectory_stitcher.h
└── trajectory_info.h

这里有如下一些结构值得我们注意：

名称	说明
EgoInfo类	包含了自车信息，例如：当前位置点，车辆状态，外围Box等。
Frame类	包含了一次Planning计算循环中的所有信息。
FrameManager类	Frame的管理器，每个Frame会有一个整数型id。
LocalView类	Planning计算需要的输入，下文将看到其定义。
Obstacle类	描述一个特定的障碍物。障碍物会有一个唯一的id来区分。
PlanningContext类	Planning全局相关的信息，例如：是否正在变道。这是一个单例。
ReferenceLineInfo类	车辆行驶的参考线，下文会专门讲解。
path文件夹	描述车辆路线信息。包含：PathData，DiscretizedPath，FrenetFramePath三个类。
speed文件夹	描述车辆速度信息。包含SpeedData，STPoint，StBoundary三个类。
trajectory文件夹	描述车辆轨迹信息。包含DiscretizedTrajectory，PublishableTrajectory，TrajectoryStitcher三个类。
planning_gflags.h	定义了模块需要的许多常量，例如各个配置文件的路径。

你暂时不用记住所有这些类，在后面的文章中，我们会逐渐知道它们的作用。

模块配置

在下文中大家会发现，Planning模块中有很多处的逻辑是通过配置文件控制的。通过将这部分内容从代码中剥离，可以方便的直接对配置文件进行调整，而不用编译源代码。这对于系统调试和测试来说，是非常方便的。

Apollo系统中，很多模块都是类似的设计。因此每个模块都会将配置文件集中放在一起，也就是每个模块下的conf目录。

Planning模块的配置文件如下所示：

apollo/modules/planning/conf/
├── adapter.conf
├── cosTheta_smoother_config.pb.txt
├── navi_traffic_rule_config.pb.txt
├── planner_open_space_config.pb.txt
├── planning.conf
├── planning_config.pb.txt
├── planning_config_navi.pb.txt
├── planning_navi.conf
├── qp_spline_smoother_config.pb.txt
├── scenario
│   ├── lane_follow_config.pb.txt
│   ├── side_pass_config.pb.txt
│   ├── stop_sign_unprotected_config.pb.txt
│   ├── traffic_light_protected_config.pb.txt
│   └── traffic_light_unprotected_right_turn_config.pb.txt
├── spiral_smoother_config.pb.txt
└── traffic_rule_config.pb.txt

这里的绝大部分文件都是.pb.txt后缀的。因为这些文件是和上面提到的proto结构相对应的。因此可以直接被proto文件生成的数据结构读取。对于不熟悉的读者可以阅读Protocal Buffer的文档：google.protobuf.text_format。

读者暂时不用太在意这些文件的内容。随着对于Planning模块实现的熟悉，再回过来看这些配置文件，就很容易理解每个配置文件的作用了。下文中，对一些关键内容我们会专门提及。

Planner

Planning与Planner

Apollo 3.5废弃了原先的ROS，引入了新的运行环境：Cyber RT。

Cyber RT以组件的方式来管理各个模块，组件的实现会基于该框架提供的基类：apollo::cyber::Component。

Planning模块自然也不例外。其实现类是下面这个：

class PlanningComponent final
    : public cyber::Component<prediction::PredictionObstacles, canbus::Chassis, localization::LocalizationEstimate> 

在PlanningComponent的实现中，会根据具体的配置选择Planning的入口。Planning的入口通过PlanningBase类来描述的。

PlanningBase只是一个抽象类，该类有三个子类：

• OpenSpacePlanning
• NaviPlanning
• StdPlanning

PlanningComponent::Init()方法中会根据配置选择具体的Planning入口：

bool PlanningComponent::Init() { if (FLAGS_open_space_planner_switchable) { planning_base_ = std::make_unique<OpenSpacePlanning>(); } else { if (FLAGS_use_navigation_mode) { planning_base_ = std::make_unique<NaviPlanning>(); } else { planning_base_ = std::make_unique<StdPlanning>(); } } 

目前，FLAGS_open_space_planner_switchable和FLAGS_use_navigation_mode的配置都是false，因此最终的Planning入口类是：StdPlanning。

下面这幅图描述了上面说到的这些逻辑：

所以接下来，我们只要关注StdPlanning的实现即可。在这个类中，下面这个方法是及其重要的：

/**
* @brief main logic of the planning module,
* runs periodically triggered by timer.
*/
void RunOnce(<