作者：Steve
导读
本篇文章想和大家分享一下 Apollo7.0 最新的规划算法。由于 Apollo 的 planning 整体代码都相当庞大，一开始还是理清其框架，再分算法块逐个击破这样效果更好。
这篇文章希望能带领读者理清 planning 的整体框架，梳理数据流，以主要场景为例，一直分析到 task (apollo planning 的主要算法所在处) 逻辑前的准备工作、输入如何构造的，之后再深入看 task 内部的细节也更容易理解。
特别注意，本文中的流程图均为作者花费了大量时间梳理而来，以便读者能够更易理解 planning 的整体框架，抓住重点不被细节带跑偏。
planning 的输入输出
读懂一个模块，首先必然是了解其的上下游，即输入输出是什么。熟悉 Apollo CyberRT 框架的小伙伴都知道在该框架下，输入输出由 Reader 和 Writer 构成，并定义在每个模块的 component 文件中。除此之外，CyberRT 框架定义了两种模式，分别为消息触发和时间触发，而 planning 中采用的为消息触发，因此必须接到特定的上游消息后，才会进入内部主逻辑，而消息触发的上游消息，定义为 component 中 Process () 函数的入参。
因此总结来看，planning 的上下游关系总结为下图：
这里再重复一下，planning 的输入分为 Reader 和 Process () 入参的原因在于，planning 依赖于 Process () 的三个上游输入，只有同时接到这三个输入，才会触发 planning 的主逻辑，即是 planning 正常启动的必要条件。而 Reader 则不是，其中部分上游还依赖于配置参数是否打开，具体可以查看 Apollo 的源码。
planning 的输出就比较简单了，主要是给控制的 ADCTrajectory 数据，包含了一条带时间、速度的轨迹点集，具体的格式定义可以查看对应的 proto 文件。
planning 整体框架
上面两张流程图是我整理的 Apollo 规划算法的框架，整体框架和之前并无太大变化。主框架分为两个线程，子线程 ReferenceLineProvider 以 20HZ 的频率运行，用于计算 planning 中最重要的数据结构 reference_line；主线程上还是基于场景划分的思路，多数场景下还是采用基于 ReferenceLine 的规划算法，对于泊车相关场景，则利用 open space 算法。目前 Apollo 的场景划分为了 16 种，在 proto 文件中可以查看到。在 Apollo 7.0 中，新增了 deadend_turnaround 场景，用于无人车遇到断头路时，采用 openspace 的方法进行调头的轨迹规划，后续我会详细看一下里面的算法实现细节。
planning 中的场景管理
Apollo 中的规划里的一个重要思想就是基于场景划分来调用不同的 task 处理，而其中如何进行场景分配便是实现该思想的核心。从上面的配置参数可以看到目前 Apollo 设定了 16 种场景，而场景下的具体切换逻辑也比较复杂，scenario_manager 的具体流程逻辑如下图所示：
在上图红框中的场景判断中，我只画了第一步的场景判断，红框内的 5 种场景为大类，剩余的场景在这 5 大类中再细分做出判断。另外需要注意的是，红框内的 5 种场景是有优先级顺序的，即如果判断为某种场景后，后续的场景也就不再判断。下面从这 5 种场景出发，介绍一下 Apollo 中的场景判断条件，以及每个大类场景下包含哪些细分的场景小类。
- ParkAndGo
该场景的判断条件为车辆是否静止，并且距离终点 10m 以上，并且当前车辆已经 off_lane 或者不在城市道路上，在该场景下采用的是 open_space 相关的算法。个人感觉该场景在驶离目标车道并正常规划失败导致的停车时会触发，利用 open_space 方法使其重新回到正常道路上，因此也是场景判断中首先需要 check 的。
- Intersection
在该场景下，又可细分四个场景。首先，根据先前计算的地图中第一个遇到的 overlap 来确定大类型，是包含交通标识的交叉口，还是其他交叉口。其次，若是包含交通标识的交叉口，还细分为 stop_sign、traffic_light 以及 yield_sign，具体结构图如下所示。
- PullOver
PullOver 场景即靠边停车，需要满足以下条件才可切换到该场景：
-
不在 change_line 的时候，即 reference_line 只有一条
-
当前位置距离终点在一定范围内并且满足 pullover 可以执行的最短距离
-
地图中能够找到 pullover 的位置
-
终点的位置不在交叉路口附近
-
能查找到最右边车道的 lane_type，并且该车道允许 pullover
-
只有从 lane_follow 场景下才能切换到 pullover
大体逻辑如上述所示，具体的参数设置可以查看代码。
- ValetParking
ValetParking 场景即代客泊车，判断逻辑如下：
-
从 routing 中得到 target_parking_spot_id
-
从地图中搜索是否存在 path 能够抵达该 parking_spot
-
查询当前位置至 parking_spot 的距离，满足条件即可切换至该场景
- DeadEnd
Apollo 7.0 中新增的断头路场景，增加了「三点掉头」 功能，增加了驶入驶出的能力，扩展了城市路网运营边界。「三点掉头」 功能基于 open space planner 框架，包含以下几个部分：断头路场景转换、开放空间 ROI 构建、掉头轨迹规划。下列图片展现了从进入 DeadEnd 到驶离 DeadEnd 的整个过程，后续我也会详细了解该算法实现逻辑。
上面就是 Apollo 中的场景切换及管理逻辑。在每个场景 scenario 下，还分为一个或多个 stage，而每个 stage 下面，又划分了多个 task 来完成相应的规划任务。以用到最多的 lane_follow 场景为例，它就包含了一个 stage——lane_follow_default_stage，而在这个 stage 下包含了多个 task，如下图所示：
仔细查看 lane_follow 场景下的 task，我们可以看出 Apollo 的规划思路也是横纵向解耦，先规划 path，再规划 speed。具体的，对于 path 来说，先做出是否需要 lane_change 或者 lane_borrow 的决策，再根据决策状态来生成凸空间，最终基于 reference_line 及凸空间求解一个二次优化问题，从而得到优化后的 path。对于 speed 来说，是基于 ST 图进行 DP+QP 的优化方法，先利用 DP 来找到一个 cost 值最小的可行解，再利用 QP 对可行解进行平滑，得到最终平滑后的 ST 图点集。最终，基于 s 值对 path 和 speed 进行融合，得到一条平滑的轨迹。
总结
至此，Apollo 7.0 planning 的核心框架及核心算法的输入都已经解释清楚了，总体来看 Apollo 规划的整体思路非常清晰，但是细节部分真的需要花费大量时间来理解，我觉得如果没能将这套算法部署到实车上跑过的话，很多算法可能真的无法很好地理解。
我个人也陆陆续续看这套代码有两年多了，始终觉得很多细节都没有深入理解透，代码光看是肯定不行的，如果没有条件在实车，或者是仿真里实际运行，解决相应场景下遇到的问题的话，是始终不能转变为自己的知识的，希望与大家共勉，也欢迎大家有任何疑问在评论区留言交流。
