Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1

1. 规划技术功能概述

1.1 自动驾驶系统

    在自动驾驶系统中，与人类驾驶员对比，感知模块相当于人类眼和耳朵;
    地图搜索路径和车道级别的路径规划，相当于人脑对道路的记忆和搜索；
    最后决策规划功能，相当于人类对道路环境和客观世界的反应和判断。
    规划技术就是包括地图搜索路径，以及道路决策和规划这部分重要的功能。

1.2 规划的作用

    规划：在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地。
    从专业术语上来说，路径规划是在遵循道路交通规则的前提下，计算出具体运动指令，将车辆从当前位置导航到目的地。它在自动驾驶技术中起到承上启下的重要作用。
    在开放道路上，自动驾驶要处理的信息非常复杂，需要根据已知周围环境信息，如行人，红绿灯等交通信号，计算出安全、舒适的轨迹，完成自动驾驶任务。

1.3 规划主要功能

    从功能上说，规划主要实现了三个功能：路由寻径，行为决策和轨迹规划。
    其中路由寻径在宏观路网中形成衔接起点、终点的行车路线，这个路线是车道级别，反映道路前后的衔接关系；
    行为决策在微观层面确定车辆当前需采取的行驶方式，主要包括轻微绕行、换道绕行、减速让行、加速抢行、停车以及跟车；
    轨迹规划在微观层面生成符合行为决策结果的时空连续轨迹。与行为决策相比，轨迹规划环节的输出信息更加具体，直接对应着车辆具体运动过程。
    下面对这三个功能进行一进步的解释。

1.3.1 路由寻径(Routing)

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

路由寻径算法： A Star算法

    首先讲路由寻径，路由寻径的目的是为了在地图上搜索出最优的全局路径，这点与手机导航软件类似。 它的输入是地图的拓扑结构信息，以及起点和终点的位置。输出是连接起点到终点的路由线路。
    Routing输出的全局路径是一种静态路径，与时间无关。
    常用的搜索算法包括Dijkstra和A Star算法，以及在这两种算法基础上的多种改进算法。
    为了在地图上搜索起终点之间的最短路径，可以把道路的长度当做边，路口当做节点，把道路抽象为一个如左下角图中所示的有向拓扑地图，然后通过搜索算法，查找两点之间的最短路径。
Apollo的routing模块使用A Star算法搜索最优路径。在Apollo中，routing模块从base map地图获取道路的拓扑信息，并根据RoutingRequest中包含的起点和终点位置，使用A Star算法搜索出最终的路由线路，放在RoutingResponse中。

图中是一个RoutingResponse的例子，其中红色表示路由线路中前后连通的车道线，通过它们最终将起点和终点连接起来。地图中实线表示不可换道，虚线表示可以换道，在这个搜索结果中，除了当前红线标出的通道外，其左右可以换道的车道线也可以作为从起点到终点的路线，供后续行为决策时根据实际的路况进行选择。

1.3.2 行为决策

目的：
保障无人车的行车安全并遵守交通规则
为路径和速度的平滑优化提供限制信息
输入：
Routing信息
道路结构信息
交通信号和标识
障碍物信息
输出：
路径信息
速度限制和边界
时间上的位置限制边界
    行为决策是规划技术的第二个功能，它的目的是为了保障无人车的行车安全并遵守交通规则，同时为路径和速度的平滑优化提供限制信息。
    它的输入除了Routing输出路由结果信息外，主要是周围路况的实时信息，第一个路况信息是道路结构信息，如当前车道，相邻车道，汇入车道，路口等；第二个是交通信号和标识，如红绿灯、人行横道、停车标志等；第三个是障碍物信息，包括障碍物类型、位置、大小、速度、未来的可能运动轨迹。
    它的输出有，路径信息，包括路径长度以及左右限制边界；第二个是路径上的速度限制边界，第三个是时间上的位置限制边界.

行为决策模型分为四类：
    行为决策功能有四种模型，有限状态机模型，决策树模型，基于知识的推理决策模型和基于价值的决策模型。其中有限状态机模型简单易行，应用也比较广泛，Apollo中的场景决策就是采用这种模型。

    行为决策主要做的的决策类型有：循线行驶，超车行驶，左转行驶，区域行驶，堵塞处理，交通流汇入处理等，最后再将这些决策进行融合，得到最终的决策结果。并作为轨迹规划的目标参考和限制。

1.3.3 轨迹规划

目的：
在合理的时间到达规划的目标或终点
避免与障碍物碰撞
更好的乘坐体验，加速和减速过程尽量平滑
输入：
决策输出信息
道路结构信息
交通信号和标识
障碍物信息
输出：
稳定平滑的轨迹点
    轨迹规划是规划技术的第三个功能。规划的目标主要是为了规划安全舒适的轨迹供无人驾驶车辆完成行驶任务，对其要求有三：一是能够在合理的时间到达规划的目标或终点；二是避免与障碍物碰撞；三是有更好的乘坐体验，加速和减速过程尽量平滑。
    规划的输入与决策类似，除了决策的输出信息外，同样也有周围路况的实时信息，即道路结构信息，交通信号和标识和障碍物信息。
    它的输出是条稳定平滑的轨迹点，其中包含了每个点的位姿和时间信息。

2. Apollo规划模块概述

2.1 模块输入输出

    Apollo规划模块根据感知预测的结果，当前车辆定位信息，以及Routing的结果，规划出一条安全行驶的轨迹，这个轨迹会发送给Control模块，Control模块通过油门，刹车和方向盘，控制车辆按照规划的轨迹运行。Guardian模块监督控制信号的合理性（安全/合法），并输出给车俩，使车辆运行。

2.2 代码结构

    Apollo的Planning模块在modules/planning目录下，Planning组件的入口在planning_component.cc中。输入数据通过Reader读取，Reader在Init函数中初始化。当Proc参数中任意一个类型有收到新的message时，Proc函数被调用。Proc函数中通过调用planning_writer_->Write(adc_trajectory_pb)输出规划结果。

2.3 工程框架

    规划模块一共支持三种规划模式：OnLanePlanning，NaviPlanning和OpenSpacePlanning。
    其中NaviPlanning主要用于高速公路的导航规划，OnLanePlanning用于城区及高速公路各种复杂车道；OpenSpacePlanning用于没有车道线的场景，主要在自主泊车和狭窄路段的掉头时使用.
    Apollo规划模块的框架以场景调度为基本决策框架，场景调度本质上是一个双层状态机，顶层是基于场景的管理器，底层是一系列小模块的任务组合，由此来构建出Apollo planning的整体框架。

2.4 场景配置

    目前apollo一共支持了11多种场景和场景的定义，场景首先通过配置文件进行配置, 如在/planning/proto/planning_config.proto中定义一个场景的结构。首先定义了一个ScenarioType, 包含了Planning支持的场景类型列表，如Lane Follow，跟随当前的参考线行驶；PULL_OVER靠边停车;VALET_PARKING是泊车场景，等.
    场景通过配置文件进行配置：

2.5 注册场景

场景初始化：
    ScenarioManager负责实际的场景注册，Init函数负责对场景的初始化，首先调用::RegisterScenario这个函数对各个场景配置文件进行读取和注册；在开始创建LANE_FOLLOW作为默认运行场景。

2.6 场景转换

    ScenarioManager除了负责场景的配置与注册外也负责对场景进行转换。场景转换在ScenarioManager::Update中实现，这个函数有两个输入，车辆当前的状态信息ego_point，和planning循环计算所需要的其他相关信息，frame。
    Update函数首先调用Observe函数，获取当前要处理的overlaps。
    在apollo里 Overlap 是指地图上任意重合的东西，比如PNC_JUNCTION里是道路之间有相互重合，SIGNAL是信号灯与道路有重合，STOP_SIGN是停止标志与道路有重合，YIELD_SIGN是合标志与道路有重合。
    当遇到这些overlaps时，在函数ScenarioDispatch中对其进行具体的处理或者场景切换。

2.7 Scenario：场景

场景结构：
    在执行的每一个场景中，又分为一个或多个阶段，称为stage；而每一个stage又包含一个或多个任务，称为task。执行一个场景，最终就是顺序执行不同阶段的不同任务。
    Scenario的配置在“planning/conf/scenario”文件夹中。以pull over场景为例，在对应的scenario type中配置包含的stage列表，在stage中配置其包含的task列表。

以LaneFollow场景为例的调用流程
    PlanningComponent的Proc函数，最终会调用Scenario的Process函数，对具体的场景进行处理。在Scenario的Process函数中，调用Stage的Process函数，如果当前stage结束，获取并创建下一个stage，然后顺序执行就可以了。     接着看一下Stage中“Process”函数的执行，以“LaneFollowStage”为例。
    Process函数中主要处理在PlanOnReferenceLine函数中，不同的场景中的“PlanOnReferenceLine”实现可能也不一样。这样设计的好处是，当发现一个场景有问题，修改不会影响到其他的场景，同时也可以针对不同场景做优化。
    右图就是LaneFollow场景的PlanOnReferenceLine函数实现。
    接下来我们来了解Task。
    Task在planning/tasks文件夹中，Task分为3类：deciders(决策)，optimizers（规划），learning_model。task.h中包含Task基类的声明，其中重要的是2个Execute()函数。

    这两个函数会在前面stage部分被ExecuteTaskOnReferenceLine调用，以执行Task的具体逻辑。
    右图是Task子类之间的继承关系，Decider中有三个子类比较特殊，SpeedDecider，PathDecider和RssDecider直接继承自Task而不是Decider基类。
    Optimizer中主要包含了PathOptimizer，SpeedOptimizer和TrajectoryOptimizer三种类型的优化器。
    到此apollo规划模块的概述基本介绍完了，下面我们对apollo中使用的规划算法做一个简单介绍。

3. Apollo规划算法介绍

3.1 轨迹规划

    在apollo 1.5中增加了EMPlanner的算法，2.5中增加了LatticePlanner算法，在3.5中对EMPlanner做了较大的改动，EMPlanner改为PublicRoadPlanner，在这里主要是取消了SL和ST的迭代机制。在5.0以后的版本中，取消了SL的动态规划。
    为了对apollo的规划算法有一个比较全面的了解，我们在这对apollo的轨迹规划介绍思路主要如下，首先介绍规划算法的前期处理和基本概念，主要是对Frenet坐标系和参考线平滑的介绍。然后重点介绍两种规划算法，EMPlanner和LatticePlanner。由于后期版本是基于EMPlanner进行的改动，在此我们主要将EMPlanner的思路介绍清楚即可。

3.2 参考线平滑

    首先是参考线平滑。
    参考线为无人车指示一条车道级别的参考道路，参考线通常来源于高精地图。为了体感和安全性的考虑，决策规划模块需要输出连续稳定的、平滑的轨迹点。高精地图里面提供的道路描述的点，通常不够平滑，达不到决策规划对平滑度的要求，所以需要在这些点的基础之上，用平滑算法得到一系列在数学上平滑的点。
    Apollo中提供了多种算法对参考线进行平滑，默认配置为基于离散点的平滑。
    参考线是高精地图中提供的参考道路的离散点，但是这些点往往不够平滑，达不到规划对平滑度的要求。所以在进行规划之前，需要先对这些点进行平滑。
    Apollo中默认的平滑算法是对离散点的平滑，它将参考线平滑构成了一个二次优化的问题，并用osqp求解器进行求解。其中：

• cost_smooth 为平滑度代价
• cost_length 为长度代价
• cost_deviation 为相对原始点偏离代价

    除了对离散点参考线的平滑算法外，apollo中还有对五次多项式以及五次螺旋线的参考线平滑

参考线平滑算法类型

3.3 Frenet坐标系

• 运动轨迹降维
• 更好的场景理解：不受几何形状的影响

    规划算法用到的另外一个概念是Frenet坐标系。
    Frenet坐标系使用道路的中心线作为参考线，使用参考线的切向向量和法向向量建立坐标系。
    相比于笛卡尔坐标系，Frenet坐标系可以明显地简化问题，因为在车辆行驶过程中，总是能够找到道路的参考线(即道路中心线)，那么基于参考线的位置，就可以使用纵向距离和横向距离来描述任意位置，同时纵向和横向的速度，加速度也便于计算。这样就促成了apollo规划对于横纵向解耦的思想。

3.4 轨迹规划解耦

    横纵向解耦是指在轨迹规划时，对路径的规划和对速度的规划分开，这种方式降低了规划的复杂度。
    在Frenet坐标系中，S轴为纵向，始终代表参考线的方向；L轴为横向，始终代表参考线的法线方向；Apollo先规划路径，再规划速度。路径规划可以称为SL问题，规划变量是参考点坐标，朝向和曲率；速度规划称为ST问题，规划变量是与时间相关的运动量，有转向角，速度和加速度。

SL-路径规划：
参考点坐标+朝向+转向曲率
$q=(x,y,\theta ,\kappa )$
ST-速度规划：
时间相关的运动变量（速度，加速度，加加速度）
$s=(q,v,a)$

3.5 EM Planner

    基于以上的基础概念，下面我们开始讲apollo中EMPlanner，最大期望算法。
    EMPlanner经过两个步骤交替进行计算，第一步是计算期望（E），利用对隐藏变量的现有估计值，计算其最大似然估计值；第二步是最大化（M），通过最大化在E步上求得的最大似然值，来计算参数的值。
    M步上找到的参数估计值被用于下一个E步计算中。
    E-Step对应当前规划的Trajectory，进行的极大似然估计（Path Planning）。
    M-Step重新生成规划轨迹，使E-Step中的估计值最大化。
    EMPlanner采用优化的思路做轨迹规划，并将轨迹分为path和speed 2部分，分别优化，求取5次多项式曲线，最终合并为一条trajectory。
    对于路径和速度优化，用动态编程（DP：dynamic programming）和基于样条样的二次规划（QP：quadratic programming）的组合，以处理交通规则，障碍物决策和平滑度。
    对于多车道规划，每一个候选车道基于车道级别的优化器都会产生一个最优的轨迹；最终多车道的decider根据cost functional与safety rules决定哪一个车道；

3.6 Lattice Planner

• 适合简单场景和高速轨迹
• 性能可靠

    相对于EMPlanner，LatticePlanner由于其参数少，并且统一化，更适合简单场景和高速轨迹，在简单场景下它的解空间较大，因而也更鲁棒。
    Lattice Planner和EM Planner在设计上最大的区别在于，Lattice是横向纵向综合求解的，而EM Planner是分开求解的。
    和EM planner一样，Lattice Planner也将轨迹规划问题分解成横向和纵向两个1维空间的独立的轨迹规划问题, 降低规划难度。
    Lattice Planner会根据起点和终点的状态，在位置空间和时间上同时进行撒点。撒点的起始状态和终止状态各有6个参数，包括了3个横向参数，即横向位置、横向位置的导数和导数的导数；3个纵向参数，即纵向位置、纵向位置的一阶导数也就是速度、纵向位置的二阶导数（也就是加速度）。
    起点的参数是车辆当时真实的状态，终点状态则是撒点枚举的各个情况。在确定了终点和起点状态以后，再通过五阶或者四阶的多项式连接起始状态和终止状态，从而得到规划的横向和纵向轨迹。
    在生成所有横向和纵向的一维轨迹以后，将其排列组合起来，合成大量的二维轨迹，然后根据损失函数筛选出最好的合成轨迹。
    Lattice Planner直接考虑到车辆的动力学状态，尤其是横向动力学状态，产生更适合高速运动的轨迹，简单可靠。

4. Apollo规划模块使用

4.1 Dreamview

    Apollo规划模块可以在仿真环境中运行和调试。
    Dreamview是Apollo仿真调试时的可视化页面，它是一个web应用程序，提供如下功能：
    可视化显示当前自动驾驶车辆模块的输出信息，例如规划路径、车辆定位、车架信息等。
    为使用者提供人机交互接口以监测车辆硬件状态，
    对模块进行开关操作，启动自动驾驶车辆等。
    提供调试工具，例如PnC监视器可以高效的跟踪模块输出的问题

4.2 SimControl

    SimControl作为Dreamview的模块的重要功能，能辅助测试Planning模块的功能。它能够在没有实车底盘的前提下进行仿真，模拟车辆的运行。
    在实车运行时，Planning模块为车辆规划出来一条运动轨迹（包含坐标、速度、加速度、加加速度等信息），发送给控制模块。
    开启SimControl模块之后，就无需开启Control模块。SimControl就是完美地按照Planning输出的轨迹运行，这样我们可以专注于Planning的调试。