自动驾驶规划-概览

在规划中，我们通过结合高精度地图,定位和预测来构建车辆轨迹。规划的目标是避免碰撞和舒适的可执行轨迹。该轨迹由一系列点构成，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。

路径规划的目标是找到从地图A前往B的最佳路径。路径规划使用了三个输入，第一个输入为地图。apollo提供的地图数据包括公路网和实时交通信息。第二个输入为我们当前在地图上的位置。第三个输入为我们的目的地。

apollo也通过搜索来查找路线，在搜索前，他将地图数据重新格式化为图的数据结构，该图由节点和边缘组成。节点代表路段，边缘代表这些路段之间的连接。

A*算法

A*是经典的路径查找处理算法。考虑两件事，计算开始节点到候选节点的成本g（简单），然后估计从候选节点前往目的地的成本（启发式成本） h（困难）

使用g和h表示每个成本

对于每个候选节点，通过添加g和h来计算总和，即f值。最佳候选节点是f值最小的节点。每当我们抵达新节点，我们通过重复此过程来选择下一个候选点，而且总是选择我们尚未访问过且具有最小f值的节点，这就是A star 算法，他建立了一条稳定前往目的地的路径

轨迹生成

轨迹生成的目标是生成一系列路径点所定义的轨迹，我们为每个路径点分配了一个时间戳和速度。我们让一条曲线与这些路径点拟合，生成轨迹的几何表征 。

我们可以将时间戳和预测模块的输出相结合，以确保我们计划通过时，轨迹上的每个路径点均未被占用，这些时间戳创建了一个三维轨迹，每个路径点由空间中的两个维度以及时间上的第三个维度来定义。

3D Trajectory ：2D Position + Time

现实中的规划由多种约束，首先轨迹应能免于碰撞，意味着必须没有障碍物，其次要让乘客感到舒适，速度的任何变化都必须平滑，第三路径变化要切实可行，最后轨迹应该遵守交通规则

如何选择最佳轨迹，答案是使用“成本函数”。轨迹成本将所有这些缺陷聚合为单个数字。这使我们能对不同轨迹进行排名。不同的环境可能使用不同的成本函数

Frenet坐标

笛卡尔坐标系对车辆来说不是最佳选择，即使知道车辆行驶了多远，也难以确定他是否偏离了车道中心。解决方案是Frenet coordinates。描述了汽车相对于道路的位置，s代表沿道路的距离，也称为纵坐标，d表示与纵向线的位移，也称为横坐标

路径-速度解耦规划

路径规划

生成候选曲线，是车辆可行使的路径，使用成本函数对每条路径进行评估

速度规划

我们需要选择的是与路径点相关的一系列速度，而不是单个速度，我们将该序列称作速度曲线，可以使用优化功能为路径选择受到各种限制的良好速度曲线。

通过将路径和速度结合可构建车辆行驶轨迹

路径生成和选择

为了在路径-速度解耦规划中生成候选路径，我们首先将路段分割成单元格，然后对这些单元格中的点进行随机采样，通过从每个单元格中取一个点并将点链接，创建了候选路径。重复此过程可以构建多个候选路径。使用成本函数对这些路径进行评估。并选择成本最低的路径

ST图

st图可以帮助我们设计和选择速度曲线。s代表纵向位移，t代表时间。说明了车辆在不同时间的位置，我们可以通过查看曲线的斜率从st图上推断速度

速度规划

为构建最佳速度曲线，我们需要将st图离散为多个单元格，单元格之间的速度有所变化，但在每个单元格内，速度保持不变。可简化速度曲线的构建。st图中可以将障碍物绘制为特定时间内阻挡道路的某些部分的矩形，

速度曲线不得与此矩阵相交。

既然有了一张各种单元格被阻挡的ST图，我们便可以使用优化引擎为该图选择最佳的速度曲线，优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案

路径-速度解耦规划很大程度上取决于离散化，路径选择涉及将道路划分为单元格，速度曲线构建涉及将ST图划分为单元格。尽管离散化使问题更易于解决，但该方案生成的轨迹并不光滑。

给了使其平滑，可以是使用二次规划技术quadratic programming，二次规划将平滑的非线性曲线与这些分段式线性拟合。一旦路径和曲线就绪，我们便可以用其构建三维轨迹

Lattice规划

通过使用Frenet坐标，可以将环境投射到纵轴和横轴上，我们的目标是生成三维轨迹-纵向维度-横向维度和时间维度。

我们可以将三维问题分解为两个单独的二维问题，通过分离轨迹的纵向和横向分量来解决

Lattice规划分为两步：

1.分别建立ST和SL轨迹

2.然后将他们合并

ST轨迹的终止状态

我们可以将状态分为3组，巡航，跟随与停止

巡航：意味着车辆将在完成规划步骤后定速行驶

SL轨迹的终止状态

无论车辆进入怎样的终止状态，车辆都应该稳定的与车道中心线对齐，意味着我们只需要在一个小区域内对横向终止位置进行采样。汽车驶向的终止状态应该与车道中心一致

我们想要的候选轨迹应该以车辆与车道对其并直线行驶而结束。

车的朝向和位置的一阶和二阶导数都应该为0.意味着车辆既不是横向移动，（一阶导数）也不是横向加速（二阶导数）

一旦我们有了ST和SL轨迹，我们就需要将他们重新转换为笛卡尔坐标系。然后可以将他们相结合。构建由二维路径点和一维时间戳组成的三维轨迹。由于两个轨迹都有纵坐标s，通过将其S值进行匹配来合并轨迹