课程链接：Apollo开发者社区_Apollo星火计划之PnC专项 (baidu.com)

一、决策技术解析及实现

（一）Apollo决策技术详解

注意：Apollo的决策和规划是一起做的，一起称之为Planning模块

Planning模块的运行机制如图，基于双层状态机，一层是Stage，一层是Task

根据场景，确定Stage，每个Stage都包含了路径的决策，规划

然后将任务分为两种，一种是有车道线的规划，一种是没有车道线的规划

下面介绍决策模块

 为了提高准确性，输入了所有的信息，包括Routing，道路结构交通信息以及感知的障碍物，以及预测信息

决策模块主要实现了以下五个功能

 下面分别来介绍

首先是参考路径 ，也叫参考线，很重要

后面的各种规划和决策都是基于参考线运行

此外参考线也可以表达环道需求，如果需要换道，生成一个目标参考线，如图所示，目标参考线的优先级大于当前参考路径，表示环道需求。

参考线实现方法主要是根据Routing的输出来找到道路中心线，也就是图中蓝色的线

当有了这个参考线后，进行交规决策 

结合高精地图和信号灯状态输出，遍历参考线上的交通灯以及交通灯的状态

下面来判断路口是否生成停止墙（红线）

路径决策

路径判断是否需要换道：

如果需要换道，再判断能不能换道，如果可以换道，就会产生一个换道的路径边界，否则产生道内边界

如果不需要换道，会判断是否要做一个避让，

避让有两个条件，一个是道路宽度是否足够去换道，另一个是障碍物是不是静态的

注意：路径决策的时候考虑静态障碍物，动态障碍物由速度规划来考虑

下面来看路径边界

一个是车道内的边界，如图1所示，绿色的线，如果遇到一个障碍物，就会触发绿色的边界

第二个借道，其实同理

第三个，换道场景，边界由优先级比较高的道路决定

速度决策 

为了行驶安全，要对速度进行限制

比如道路限速，人行避让等等

得到平滑的路径后，进行速度决策，对当前路径上产生一个或者多个速度边界，然后集成得到最终速度边界，然后再使用ST图得到时间上的位置限制边界，通过两个边界来得到平滑的速度规划。

完成速度规划就可以生成轨迹了

下面是Planning模块的运行流程 （很重要，值得反复学习）

全部流程如下：

（1）参考线平滑

（2）交规生成

（3）换道决策

（4）

下面来看场景的概念，Apollo3.5后提出

可以是一个静态的十字路口，也可以是无人车完成的动作，比如借道避让

针对不同的自动驾驶场景，比如物流车，taxi也是不一样的

只要合理即可，没有明确界定

Apollo场景分类主要是两类：

一种是Lane Follow的场景，就是主车沿着一条车道线行驶，而且该车道前方一定距离内，是没有其他的车道与其交汇和穿越的

另一种是Lane Breaking场景，又分很多场景，分类为路口的，包含红绿灯等信息

场景划分优点：

场景之间互不干扰，适合并行开发，独立调参，方便管理。

功能之间可以相互解耦，有利于开发者开发自己的场景。

（二）交规决策刹场景实现机制

Scenario Manager主要定义了9种交规的类型

for循环遍历配置文件

 比如车辆靠近交叉路口的时候，Scenario Manager判断进入哪个场景，根据信息来判断进入哪个场景。

由于察觉到了红绿灯，所以判断为traffic light的有保护的场景，又分为两个Stage

第一个Stage是车辆靠近阶段，第二个是进入场景阶段

每个Stage下又有很多的Task，每个Stage都会进行之前讲的决策规划流程

场景的进入阶段

进入场景之前需要判断，一个是判断它是不是交叉路口，另外一个是判断有没有保护的场景

进入场景后开始定义的Stage 

两个场景，定义了不同的优先级

先执行apporach，然后再执行

配置文件里可以看到

进入Approach阶段 

来判断主车和停止线的距离，距离小于某一个值（5m，配置文件可查看）的时候，进入场景

根据状态判断行为，比如红灯就停止

进入交叉路口后进入Cruise阶段

 主要是做一个判断，通过交叉路口后，就会恢复lane follow状态

对于执行过程种，需要对交通灯检测 

当检测到黄灯红灯的时候，生成停止强墙 

如果决策生成停止墙，这个距离也是配置文件定义好，可以看到是1m

总结：车辆行驶到红绿灯后，进入Approach阶段，然后根据红绿灯状态判断时候停止墙，然后Cruise，最后变成Lane Follow

（三）交通灯场景仿真实现

云实验部分

 首先登陆，然后创建场景

二、参考线平滑算法解析与实现

 如何能让车辆U型路口通过尽量平滑

（一）Apollo参考线介绍

首先，参考线是贯穿了整个导航决策规划的模块

 Planning模块主要包含：交通规则的处理，障碍物的投影，路径的优化决策以及速度优化和决策

每个处理的过程，都需要先生成参考线，然后在基础上进行投影和处理

参考线是整个规划算法的基础

导航模块数据有三层

第一层是Routing的搜索结果，是根据地图topo结构，从起点到终点进行搜索，整个路径搜索长度可能几公里，几百公里，因此Routing的结果是宏观数据，是静态的比较固定的，如果没有障碍物，道路堵塞的情况下，车辆会一直沿着它行驶，只有道路堵塞的时候，才会重规划生成新的Routing

第二层是参考线的计算，参考线计算以Routing为基础，会考虑车辆周围的动态信息，比如障碍物和交规信息，参考线包括车辆周围范围内，通常是几百米的范围，相当于是Routing内的局部的数据。

第三层是轨迹，轨迹是决策规划模块最终输出结果，其依据就是第二层的结果，同一时刻可能有多个参考线，比如变道的时候，自身车道和目标车道都有参考线，但是在决策的时候，Planning模块会决定车辆最终要走那一条路线。这个唯一的结果就是输出的轨迹。所以这个轨迹更加具体以及更加局部，不仅包含了车辆的路线信息，还包含了行驶的详细状态，比如说车辆的方向，速度加速度等动态信息。

为什么要重新生成参考线呢？

 因为我们的参考线是高精地图来的，而这个高精地图一般是人为采集的，一般都是不平滑的离散的点，如果直接拿来当参考线，可能导致规划轨迹不平滑，导致车辆具体行驶过程中频繁晃动

此外，导航的路径可能比较长，对自车和障碍物进行投影的时候，由于路径很长，所以可能生成多个投影点，所以要根据Routing导航路径生成局部的平滑的参考线。这样可以节省不必要的算力。

参考线的数据结构如下

 存储了参考线的信息

第一个是priority，表示优先级，多个参考线可以根据它来判断

第二个是限速，表明了参考线的速度限制的起点终点和限制，此外使用一个数组进行分段限速，一条参考线上多段速度限制

第三个是参考点，具体的离散点

最后一个是Map_path，是地图中的参考线，使用的时候需要将其映射到planning的参考线，他们俩是一一对应的

由Map_path继承而来，父类就只有点的坐标，Map_path增加了点的朝向，以及地图上的点对应道路上的点（这里是一个数组，因为一个点可能对应被多个车道占据）

然后RefrencePoint继承，增加了曲率和曲率的导数，这直接和控制相关

下面来看如何处理参考线

 两个线程，一个是Provider，一个是规划线程，对参考线进行规划

Provider根据Pnc地图生成参考线。一部分是参考线生成，一部分是参考线平滑

平滑主要是三种方式，离散点平滑，样条曲线平滑以及螺旋线平滑

Apollo主要采用离散点平滑

下面来看如何生成参考线

参考线生成时在CreateRef中生成的，首先获取车辆状态，然后获取Routing结果，根据Routing结果生成RouteSegments，最后结合车辆状态来对参考线进行平滑以及分割（Shrink）

如果不是新的Routing，就可以使用上个周期的参考线进行扩展，沿用旧的参考线，节省计算量

（二）参考线平滑算法

三种平滑算法

 离散点平滑分为两种，一种CosTheta，一种是FemPos

下面是对平滑算法的配置函数

平滑函数 

在raw上面设置中间点，然后再进行平滑，在smotther中进行

如何设置中间点 

根据参考线在纵向的投影进行0.25m的采样，获取AnchorPoint，AnchorPoint包括路径点。裕度（周围可以平移的空间）。以及是不是强约束，如果是强约束需要满足裕度限制，首尾两个点都要满足，中间点不用。

Smooth函数

 可以手动配置两种离散点平滑算法

输入参数为，Anchor Point的横纵向坐标以及裕度

 首先对裕度进行收缩，然后调用Solve进行求解

 三种求解方式

考虑曲率约束为非线性的，使用Ipopt求解，不考虑曲率约束则为线性的，使用OSQP求解

默认是不考虑曲率约束

基于离散点的平滑

问题为二次优化问题，使用OSQP求解

下面来看约束和代价函数

优化函数三部分组成，一部分是平滑度，一部分是长度，一部分是相对原始点便宜量

长度很容易理解，就是长度。cost误差也就是差值

cost smooth有两种计算方式

 FemPos采用，和向量模的平方来表示，假设有三个点P0，P1以及P2，求出P1P0和P1P2的和向量P1PS，然后计算P1PS的模，如果模比较小说明比较平滑

 CosTheta则采用相邻两个向量之间的夹角值，CosTheta值越大，夹角越小，也就是越接近直线

所以平滑度可以采用CosTheta的负值来表示

两种约束条件

上面的约束是软约束，还需要添加硬约束，也就是横纵向裕度，添加box对参考点进行限制，限制方法为在xy上面添加位置约束

 此外还需要考虑曲率约束，Apollo通过近似假设计算，找到相邻的三个点，求出外接圆，

假设1：外接圆上的点两两之间均匀采样

假设2：相邻线段之间的夹角很小，相当于两个向量

假设2：前两个条件可以推出弧长与弦长接近

然后计算出最大曲率cur_cstr，带入可以计算出约束方程

（三）U型路口仿真调试

路径规划算法

（一）路径规划算法总体介绍

首先进行路径规划，然后进行速度规划

 路径规划主要对于静态障碍物，速度规划主要针对动态障碍物

配置文件中，DECIDER都是决策器，OPTIMIZER都是优化器

下面具体讲路径规划task做什么

首先是四种决策

（1）Task: LANE_CHANGE DECIDER

 如果有一条参考线就不换道，如果多条就判断需不需要换道

所有条件满足会产生换道决策

（2）

路径规划中，每一帧都规划，如果感知识别出来的障碍物bounding box不稳定，每一帧也会随着bounding box的改变而上下跳动，带来的问题就是行驶中的不稳定，路径根据障碍物上下跳动

所以Apollo引入路径重用决策，如果上一帧路径没和障碍物发生碰撞，这一帧路径直接复用上一帧路径，不会再进行接下来的路径规划，直接跳到速度规划的部分。

如果上一帧的路径和障碍物存在碰撞，就继续进行后面的路径规划

（3）

主要处理自车道前方有没有一个阻挡自身的障碍物

 决策也是一些判断条件：

如果只有一条车道，就没法借道，

如果其他车道有障碍物，

如果障碍物距离路口比较近，也不借道。

障碍物是够长期存在。旁边车道是实线还是虚线。

条件满足后会产生向左或者向右的决策。

（4）

 SL坐标系内，确定路径可以走的边界范围，使用前几个决策器产生候选边界：

如果只是自车道行驶，就按照自车道边界

如果是Nudge障碍物，（Nudge障碍物指的是前方障碍物没有完全阻挡住自身车道的障碍物），我们会采用障碍物的边界以及自车道边界

向左借道超车，边界采用被超车的边界和换道的边界组成

换道决策，采用当前车道和换道的边界组成

（5） 

最后是一个优化算法

规划出一条平滑路径

（6）Task: PATH ASSESSMENT_DECIDER

 路径规划后，产生多条路径，再对你每条路径进行打分

比如向左向右和当前车道的路径，最后进行评价，基于规则，选出最优路径

从上到下按照规则选择

（） 

根据路径对障碍物赋予决策，比如最后是向左绕行的路径，会对绕行障碍物进行决策，就产生一个停止的决策，这个决策主要是对速度规划使用

（二）基于二次规划的路径规划算法

二次规划标准型

 求解二次型的，线性约束的优化问题

P是二次项系数，P是一个正定矩阵，为什么是正定矩阵呢？

答：正定矩阵保证了x^TPx大于等于0，这样才会有一个极小值点

q是一次项系数，是一个n维实数向量

A是约束函数的一次项系数，

l和u是m维向量，上边界和下边界

常用开源的求解器：OSQP，qpOASES

OSQP使用ADMM求解，Apollo主要采取OSQP

二次规划流程

定义优化变量

首先需要把障碍物投影到参考线上

先把障碍物投影到参考线，然后再以delta s进行离散成s0到sn-1

选择纵向的l作为优化的变量

此外还需要选取l的一阶导dl和二阶导ddl作为优化变量

目标函数设计

根据实际需求来设计目标函数：安全性，舒适性，以及接近终点

w为惩罚项

三阶导可以使用相邻两个二阶导求得

然后写成矩阵形式

然后说一下约束设计

（1）主车必须在道路边界内

bound由之前task确定的

（2）运动学限制

对于曲率的公式做一些假设，

得到ddl的约束

此外还需要满足曲率变化率的要求，也就是规划出的路径， 能让方向盘最大打的过来

假设在路径上是匀速的，带入三阶导公式

得到了三阶导边界

约束总结：

此外要满足基本物理原理

也就是三阶导恒定，二阶三阶导连续

转化为约束矩阵

第一部分l约束，然后dl的约束，以及ddl约束，然后是连续性约束

最后是起点硬性约束

然后是带入OSQP进行求解

OSQP是一种求解QP问题的数值优化开源软件

求解分为四步

设置求解器参数，这里都是用的默认参数

然后是计算QP系数矩阵

CalculateKernel是计算二次型系数P矩阵，

AffineCons是A矩阵，Offset是q向量

算完之后调用setuo函数去求解

获取优化值

（三）路径规划算法执行流程

首先获取参考线平滑结果，然后生成一系列任务

每一个决策都是一个Process

输入的为参考线info，frame（这一帧的总的数据结构）

首先是换道决策

结果保存在injector中，是一个单例类，保存的变量会贯穿始终，因为好几帧都要依靠它来

然后是借道决策

判断结果使用truefalse来保存，向左向右保存在injector中

第三步是路径边界产生

最后把它保存在参考线info内

最后进行路径生成，调用优化器

每条路径保存在候选路径内

最后进行路径结果比较

路径规划算法实践

速度规划

（一）速度规划算法总体介绍

速度规划和路径规划的坐标系不同，路径规划采用SL轴，而速度规划采用ST轴

此外速度规划的S和路径规划的S也不是一个东西，速度规划的S是沿着路径方向上的位移，路径规划的S是沿着参考线方向上的位移

下面来看不同场景的ST图

蓝车为障碍车

平行四边形的斜率为速度，红线为主车的速度规划，斜率刚开始大，后面减少到和蓝色相同

障碍车在红车轨迹下方

下面来看速度规划的任务

首先是SPEED_BOUNDS