Apollo无人驾驶入门课程笔记之规划 （六）

时间：2020.11.08
内容：无人驾驶之预测

一、规划简介

当我们驾驶从某个地点到另一个地点的时候，我们通常会在心中想好一条可行的路线，无人车的规划也是如此。

其第一步为路线规划，假设无人车要从A点到B点，那么其可行的路径有很多，就如我们平时使用手机地图进行导航时一样，当你输入目的地之后，软件会为你规划出几条可行的路线。这些路线都能满足你到目的地的需求，但可能这个过程中你可能会遇到堵车或者路不好的情况，而这些都是大家希望避免的情况。无人车也是如此，所以路线规划的目的就是为了寻找到一条从A到B的最佳路径。

而当无人车确定这条路线之后，就需要进行轨迹规划了，即在这条路线上定义一系列的点，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。轨迹规划的目的就是为了在行驶规程中有效避开障碍物，提高驾驶体验。

路线规划一般有三个输入：地图、出发位置、目的地

二、路线的规划

2.1 地图的处理

在规划A到B点的路径时，通常都是沿着道路进行进行追踪，看是否能到达目的地，这种方法叫做搜索。无人车在进行路径规划时，也是通过搜索算法进行处理，但在搜索之前我们需要先将地图数据格式化为图形的数据结构。

该图形由节点和边缘组成，节点代表路段，边缘代表这些节点的连接。例如：在交叉路口，汽车可从节点1移动到节点2、节点3或节点4，反之亦然。

在搜索时，我们就可以对从一个节点到另一个节点的成本进行建模，成本越小，就代表路径越好。

2.2 路径搜索算法 A*

A*算法时经典的路径搜索算法，其通过网格进行搜索，具体的算法原理，将在后面学习的时候进一步研究。

2.3 轨迹生成

高等级路径规划只是规划的一部分，我们需要构建沿着这条线路前进的低等级轨迹。这意味着要处理一些不属于地图的物体：如其他车辆、自行车或行人。例如，我们可能需要与试图在我们前面掉头的汽车互动，或者我们可能希望超过一辆在公路上行驶的慢车。这些场景需要更低级别、更高精确度的规划。我们将这一级别的规划称为轨迹生成。
轨迹生成的目标是一系列路径点所定义的轨迹，每个路径点都分配了一个时间戳和速度，但事实上在真正行驶的时候，由于障碍物的移动，路径点的时间戳需要与预测模块的输出相结合，时刻更新来确保我们顺利通过这些路径点。同时每个路径点都指定了一个速度，来确保车辆按时到达每一个路径点。

在现实的规划中，我们必须满足以下多种约束：1、轨迹应避免碰撞；2、尽量让乘客感到舒适；3、确保每个路径点都可执行（高速上是不能规划掉头的轨迹的）；4、轨迹应合法，遵守交通法律。

那么在A点与B点之间选择最佳轨迹的标准是什么呢？那就是成本函数，成本函数为每一个轨迹都分配了一个“成本”，成本越低，路径越好。轨迹成本将不同轨迹上的缺陷聚合为单个数字，进行排名。

2.4、Fernet坐标系

通常情况下我们使用笛卡尔坐标系描述物体位置，但车辆的行驶路径时弯曲多变的，所以笛卡尔坐标系很难描述车辆行驶了多远，也很难知道车辆是否偏离道路中心。所以解决方案时Fernet坐标系。

Frenet坐标系描述了汽车相对于道路的位置。在Frenet框架中，s代表沿道路的距离，也被称为纵坐标。d表示与纵向线的位移，也被称为横坐标。在道路的每个点上，横轴和纵轴都是垂直的。纵坐标表示道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

2.5 路径-速度解耦规划

路径-速度解耦规划分为两步：路径规划、速度规划。具体的实现方法为：首先先在路径规划中生成候选曲线（多条可行驶路线），然后使用成本函数对路径进行评估，然后按照成本对路径进行排名选择成本最低的路径。

选好路径之后就可以确定这条线路行进的速度，即为每一个路径点分配一个速度，这些速度形成的序列称为速度曲线，此外为了获得更好的速度曲线需要对其进行优化。优化后的速度序列与路径点结合就构成了车辆的行驶轨迹。

2.5.1 路径生成与选择

上文说到，在路径-速度解耦规划中首先需要生成候选路径，其具体方法为：首先将路段分成单元格，然后对这些单元格的点进行随机采样，将这些从单元格中采样的点连接起来就创建了候选路径。重复上诉过程，我们就可以获得很多这样的路径，然后使用成本函数对这些路径进行评价，就可以筛选出最佳路径了。

2.5.2 ST图

选择路径后的下一步是选择与该路径关联的速度曲线，ST图工具可以帮助设计和选择速度曲线。在ST图中，s代表车辆的纵向位移，t代表时间。ST图上的曲线是对车辆运动的描述。斜率越大则代表速度越大，位移越大。

2.5.3 速度规划

在构建最佳速度曲线的时候，需要将ST图进行离散成为多个单元格，各单元格速度存在变化，但每个单元格内的速度是一样的。此外在ST图中还可以将障碍物绘制为特定时间段内，阻挡道路的某些部分的矩形。如假设预测在t0至t1时间内汽车回阻挡s0到s1的位置，那么速度曲线则不能与矩形相交。ST图可以清晰的表示出车辆在各个时间点的速度以及障碍物的位置，对其进行优化则可以为该图规划出最佳的速度曲线。

2.5.4 优化

根据上文所述路径-速度解耦规划与速度曲线规划ST图都涉及的离散化，但离散化在时问题变得简单的同时也使得轨迹并不平滑。

为了使路径规划以及速度规划曲线转变为平滑轨迹，在这里使用二次规划技术。该方法可以将平滑的非线性曲线与这些分段式线段拟合。

2.5.5 路径-速度规划的轨迹生成

根据上文已经学习了路径的规划、速度曲线规划，通过路径规划的原理以及ST图等工具，在车辆行驶过程中，我们完全可以通过计算与优化规划出躲避障碍物或其他车辆的最佳路线与速度曲线，完成我们在路上的自动驾驶。

2.6 Lattice规划

Lattice规划轨迹生成方法是一种通过使用Frenet坐标将环境投射到纵轴和横轴上，生成三维轨迹（纵向维度、横向维度、时间维度）的规划方法。

可以将三维问题分解成两个单独的二维问题，这是通过分离轨迹的纵向和横向分量来解决。其中一个二维轨迹是具有时间戳的纵向轨迹称为ST轨迹，另一个二维轨迹是相对于纵向轨迹的横向偏移成为SL轨迹。

Lattice规划主要具有两个步骤即先分别建立ST和SL轨迹，然后将它们合并为生成纵向和横向二维轨迹。先将初始车辆状态投射到ST坐标系和SL坐标系中，然后对预选模式中的多个候选最终状态进行采样，来选择最终车辆状态。对于每个候选最终状态构建一组轨迹将车辆从其初始状态转换为最终状态，使用成本函数对这些轨迹进行评估，并选择成本最低的轨迹。

2.6.1 ST轨迹的终止状态

根据情况可以将状态分为3组：巡航、跟随、停止。巡航意味着车辆将在规划完成后定速行驶，实际是对图上的点进行采样，其最终状态的加速度均为零。

下一个状态的模式是跟随车辆，这种状态是对位置和时间状态进行采样，这时速度和加速度将取决于跟随的车辆，为了保持与前车的安全距离，速度和加速度都会进行修正。

最后一个状态是停止，这种模式只对汽车何时何地停止进行抽样，这里速度和加速度都会被修正为0.

2.6.2 SL轨迹的终止状态

SL规划的前提是：无论车辆进入怎样终止状态，车辆都应该稳定地与车道中心对齐。
其详细解释后续学习。

2.6.3 Lattice规划地轨迹生成

一旦同时拥有了 ST 和 SL 轨迹，就需要将它们重新转换为笛卡尔坐标系。然后可以将它们相结合构建由二维路径点和一维时间戳组成的三维轨迹。ST 轨迹是随时间变化的纵向位移，SL 轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标 S，所以可以通过将其 S 值进行匹配来合并轨迹。