Apollo规划模块学习（一）

Apollo规划模块学习

---

---

一、规划的作用

规划：在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地。

1.1 规划的主要功能

1、路由寻径（Routing）：负责在宏观路网中形成衔接起点、终点的概略行车路线。
2、行为决策：在微观层面确定车辆当前需采取的行驶方式，包括直行，轻微绕行、换道绕行、减速让行、加速抢行、停车以及跟车。
3、轨迹规划：在微观层面生成符合行为决策结果的时空连续轨迹。

1.1.1 路由寻径

目的：在地图上搜索出最优的全局路径
输入：高精地图拓扑信息；起点和终点的位置
输出：从起点到终点的路由线路

路由寻径的地图格式：Topo Graph = 节点 + 边缘
节点表示实际的道路路段；边缘表示路段之间的连接线，可赋予不同的代价值

路径搜索算法：A*算法

Apollo的Routing过程：

1.1.2 行为决策

目的：
保障无人车的行车安全并遵守交通规则；
为路径和速度的平滑优化提供限制信息

输入：
Routing信息；
道路结构信息(如：当前车道、相邻车道、汇入车道、路口等)；
交通信号和标识(如：红绿灯、人行横道、停车标志等)；
障碍物信息(障碍物类型、大小、位置、速度、未来的可能运动轨迹)

输出：
路径信息(路径长度、左右限制边界)；
速度限制和边界；
时间上的位置限制边界

行为决策模型：

Apollo中的行为决策采用有限状态机模型

行为决策的主要决策类型：

1.1.3 轨迹规划

目的：
在合理的时间到达规划的目标或终点；
避免与障碍物碰撞；
更好的乘坐体验

输入：
决策输出信息；
道路结构信息；
交通信号和标识；
障碍物信息

输出：
稳定平滑的轨迹点(包含点的位姿、时间信息)

轨迹组成：由一系列位置点和到达点对应的时间戳组成 (x,y,t)

轨迹评估指标：无碰撞、舒适性、可执行

轨迹评估成本函数：轨迹的成本有各种处罚规则组成，如偏离道路中心、有可能发生碰撞、加速度或曲率水平

二、Apollo规划模块解析

Apollo系统整体模块框架

2.1 planning模块代码结构

planning模块在modules/planning目录下，planning组件的入口在planning_component.cc文件中，其中Init()是组件的初始化函数，Proc是运行函数。

模块的输入数据通过Reader获取，Reader在Init()函数中初始化；组件的输出通过Writer发送，Writer在Init()函数中创建和初始化。
Proc函数的输入包括障碍物预测数据、车辆状态和定位数据；planning_base -> RunOnce函数是规划执行的主体函数。

2.2 工程框架

规划模式：
NaviPlanning：主要用于高速公路的导航规划；
OnLanePlanning：用于城市及高速公路等各种复杂车道；
OpenSpacePlanning：用于没有车道线的场景在，主要在自主泊车和狭窄路段的掉头时使用

Apollo规划模块以场景调度为基本决策框架，其本质上是 一个双层状态机，主要有三个重要元素Scenario、Stage、Task。

2.2.1 场景配置

场景配置的目录在planning/conf/scenario中，Apollo提供了10多个场景，如lane_follow(根据参考线行驶)、pull_over(靠边停车)、valet_parking(泊车)等。

2.2.2 场景注册

场景注册在ScenarioManager中进行，注册函数是RegisterScenarios，对之前场景配置文件进行读取和注册，默认运行场景为lane_follow。

2.2.3 场景转换

场景转换在ScenarioManager::Update中实现，函数输入为：车辆当前状态信息(ego_point)、planning循环计算需要得相关信息(frame)。
是否需要场景转化通过Overlap判断，Apollo中Overlap指地图上任意重合的东西，如PNC_JUNCTION指道路之间有相互重合、SIGNAL指信号等与道路有重合、STOP_SIGN指停止标志与道路有重合。
通过ScenarioDispatch函数对场景进行转化和处理。

2.2.4 Scenario 场景

Scenario是planning模块中最上层的元素，一个Scenario包含多个Stage，一个Stage包含多个Task；执行一个Scenario就是按顺序执行不同Stage的不同Task。

Scenario的配置在planning/conf/scenario文件夹中，下图为pull_over场景的配置文件。

场景执行在Scenario::Process函数中进行，lane_follow场景的执行流程如下图左侧所示，图右侧为Scenario::Process函数的具体处理逻辑和流程。

2.2.5 Stage 步骤

步骤执行在Stage::Process函数中进行，下图为LaneFollowStage::Process函数的具体结构。

2.2.5 Task 任务

Task在planning/tasks文件夹中，task包含deciders、optimizers、learning_model三类；
在task.h文件中，Execute函数会被Stage中的ExecuteTaskOnReferenceLine调用，以执行Task的具体逻辑；

下图右侧为Task子类之间的继承关系，其中SpeedDecider、PathDecider、RssDecider三个子类直接继承自Task，优化器包含PathOptimizer、SpeedOptimizer、TrajectoryOptimizer。

三、Apollo规划算法概要

Apollo规划算法的版本迭代变化情况：
在EMPlanner变为PublicRoad Planner的改动中，主要是取消了SL和ST的迭代机制。

3.1 规划的基本概念

Frenet坐标系
在车辆的轨迹规划中，笛卡尔坐标系无法直接获取车辆与道路之间的关系，Frenet坐标系中纵轴S表示沿道路行驶的距离，横轴L表示车辆偏离道路中心线的距离，可直接描述车与道路的位置关系。

路径-速度规划解耦
在Frenet坐标系的基础上，对轨迹规划进行解耦，分为路径规划和速度规划两部分。
路径规划中生成候选曲线，即车辆的可行驶曲线，然后使用成本函数评估每条曲线，选择成本最低的路径。
速度规划在路径规划选择的路径上，考虑与障碍物的时空交集，做出跟车、停车、超车等速度决策，并使用优化算法对决策后的速度进行平滑，最终得到一条最优的速度曲线。
通过将路径和速度曲线结合，构建车辆的行驶轨迹。

3.2 轨迹规划的主要流程

路径规划
1、首先对输入的中心线采集点进行平滑，得到一条方向和曲率都连续的参考线；
2、将道路分割成单元格，对单元格中的点随机采样，连接单元格中的点，得到候选路径；
3、计算候选路径的成本，主要考虑偏离参考线、碰撞、速度限制和舒适度等要求；
4、选择成本最低的路径。

速度规划
1、速度规划采用S-T图，横坐标是时间T，纵坐标是距离S，并将S-T图网格化；道路的障碍物绘制为特定时间段内阻挡道路某些部分的矩形；
2、规划出无碰撞的速度曲线；
3、采用优化引擎算法选择成本最低的速度曲线；
4、对速度曲线进行平滑处理；

3.3 Lattice-Planner

Lattice-Planner是一种局部轨迹规划器，输出一条光滑无碰撞，并且满足车辆动力学约束的轨迹。
规划流程：
1、采样足够多的轨迹，以便提供尽量多的选择；
2、计算每一条轨迹的成本，考虑轨迹的可行性、安全性等因素；
3、循环检测：选择成本最低的轨迹，对其速度物理限制和碰撞进行检测，判断是否满足要求，若满足要求则退出循环；
4、选择最优的车辆轨迹

Lattice-Planner采样过程：
1、在Frenet坐标系下纵向采样，即纵向距离 s 关于时间 t 的采样；然后进行横向采样，即横向距离 l 关于纵向距离 s 的采样；
2、用多项式对其分别进行拟合，得到Frenet三维轨迹，表示为 s, l, t 的多项式函数；
3、Frenet三维轨迹结合参考线信息，可得到笛卡尔坐标系下的运动轨迹 (x, y, t)；

在横向采样中，Apollo设计了3个末状态横向偏移量，分别为 -0.5, 0, 0.5；4个纵向位移，分别为10, 20, 40, 80；用两层循环遍历各种组合，然后通过多项式拟合，获得一系列横向轨迹。

纵向采样考虑巡航、跟车/超车、停车三种状态。

巡航状态采用两层循环进行采样，外层循环将速度从0到速度上限值进行等间隔遍历，内层循环遍历到末状态速度的时间，从1s到8s的间隔均匀遍历；采用四次多项式拟合。
在停车状态中，给定停车点，末状态的速度和加速度为0；一层循环即可采样到停车点。
跟车/超车状态中，需对每个障碍物分别采样超车/跟车所对应的末状态，然后采用多项式拟合，获得一系列的纵向轨迹。

采样完成后，计算轨迹的成本，选择最优轨迹。轨迹成本考虑的因素有：速度限制、偏离道路中心线、碰撞、jerk、横向加速度、向心加速度。

3.4 EMPlanner

EMPlanner(Expectation Maximum Planner)为最大期望算法，其分两个步骤交替进行：第一步计算期望值E，利用对隐藏变量的现有估计值，计算其最大似然估计值；第二步是最大化M，为了最大化在E步骤上求得的似然值来计算参数的值；M步上找到的参数估计值被用于下一个E步骤计算中，以此循环迭代。
Apollo的EMPlanner分别对路径和速度进行迭代优化，E步骤将当前障碍物投影，计算车辆轨迹与障碍物之间的关系；M步依次采用动态规划(DP)、二次规划(QP)进行粗略求解、精细求解(平滑)。

3.4.1 路径规划

E-Step：
基于曲率导数连续的光滑参考线，将障碍物在SL空间投影；给定一个时刻，如果自车与预测的障碍物轨迹有重叠区域，此区域被标注为与动态障碍物的估计交互区域。

M-Step：
求解Frenet坐标系下的最优路径规划，即在一个非凸的区间，找到一个最有的L关于S的方程。主要包括基于DP的路径决策和基于样条的路径规划两。
基于DP的路径决策提供一条粗略的路径信息，包含可行通道和绕障决策；根据DP产生的路径，采用基于QP的样条曲线产生光滑的路径。

3.4.2 速度规划

E-Step：
对障碍物进行ST投影

M-Step：
采用DP配合样条曲线计算光滑的速度规划，DP动态搜索的结果包括速度规划、可通行通道和障碍物速度决策，QP基于DP的速度规划曲线进行平滑处理。

3.5 算法对比

---

参考文献

1、Apollo自动驾驶技术详解25讲