初始无人驾驶—规划

课程简介

本次课程将学习不同的规划方式，课程节是较多，我也会将知识点总结的更加精确，减少废话的字数，也希望大家坚持学习下去！

一、规划简介

上一节课我们学习了如何预测车辆的行动，这次我们将学习如何规划路径。

那么规划的第一步是什么呢？当然是路线导航，这就需要高精度地图的支持了，当车辆需要从A点运动到B点时，进行路径规划时，先将地图数据作为输入，并输出可行驶路径。我们一般通过手机进行导航，Apollo平台利用更精细的地图导航，以避开前方障碍物，并未乘客提供平稳的乘车体验。这就需要研究平台的规划模块了，下面我就一起来研究这些神奇的知识点吧！

二、Sebastian介绍规划

“通过之前的学习，相信大家都知道了车是如何去感知然后预测的，现在我们就来学习如何规划，这是很难的，我们将在两种不同的背景下讨论“规划”。一个是你要去的地方，无论地方如何远或者复杂。另一个是运动轨迹水平，你用来决定在执行特点操作时，汽车应该采取哪种转向。他们是一种时间曲线，因为你很可能会选择速度非常快或慢。

三、路由

当汽车向要从A点开到B点，我们需要输入三个数据：第一个是输入地图，Apollo提供的地图数据包括公路网和实时交通信息。第二是输入我们当前的位置，第三则是输入我们的目的地（取决于乘客）。有了这三个输入，路线规划就为寻找前往目的地的路径做好了准备。

四、世界地图

同样的道理，从A->B点是，车通常会沿着道路追踪路径，以查看是否存在通往目的地的任何路径，这就是搜索。Apollo也使用了搜索，只是更智能一些。

Apollo在搜索之前，把地图数据重新格式化为“图形”的数据结构，该图形有“节点”和“边缘”组成，节点代表路段，边缘代表这些路段之间的连接。

例如车辆在十字路口时，可以从节点1到2、3、4三个任意节点：

我们可以对一个节点到另一个节点移动所需的成本进行建模。例如拐弯比自行要费劲，所以从节点1到节点4比节点1到3消耗成本更高。这样转换一种思考会让计算机在图形中找到路径时更好更加，一旦找到路径就可以返回到地图的真实路况了。

五、网格世界_a

A*算法是经典的路径查找处理算法，下面会有演示一个点如何找到另一个点（每个网格代表节点）：

我们能够从任何一个节点移动到其任意的相邻节点，这些网格包含一些阻挡潜在路径的墙壁。

六、网格世界_b

对于人类来说，通过图形查看路径很容易就能得到，计算机就很难了，大家也都能相信的到。首先计算机必须检查是否存在通往目的地的任何路径，它会尽可能的尝试各种路径找出最短时间，这就有些费时了。对于上一小节的地图，对于红色节点我们可采用不断往外扩张的方式可以找到绿色节点，但这很费时也浪费计算机空间。

七、网格世界_c

首先从初节点开始我们需要确定8个相邻节点，对于每个节点，我们需要考虑两件事。首先，我们计算从开始节点到候选节点的成本，然后我们估计候选节点到目标节点的成本（候选节点就是我们拓展出去的点）。计算第一个成本很简单，只需要计算我们走出去多少步就好了，但是第二个成本计算就有点困难了，但是我们可以估计成本，这样就能解决这个问题。

可以看到，我们通过添加g值和f值来计算成本。g值就是开始到候选的成本，h值就是候选到目标的成本，f值为两者总和。在图形中我们很容易就能看到最小的成本候选带你是最下方的节点，当然我们总是选择我们尚未访问过且具有最小f值得节点。

八、现实世界地图

假设我们达到了一个交叉路口，我们可以左右转和直行。首先设置三个候选节点得图形。接下来，我们进行节点得评估。因为拐弯比较费劲，所以我们为left节点分配更高得g值。而直行的话我们发现要走很远的路才会到达目标节点，所以我们为其赋予给大的h值。最后我们计算f值（g和h之和），找出最小的那个值。

九、轨迹生成

高级地图只是规划过程中的一部分，我们仍需要构建沿这条路线前进的低等级轨迹。这就需要处理一些不属于地图的物体，例如其他车辆、自行车或行人。

例如，我们可能需要与试图在我们前面掉头的汽车互动，或者我们想要超车。这些都需要更低级别、更高精度的规划。

轨迹生成的目标是生成一系列路径点所定义的轨迹。我们为每个路径点分配了一个时间戳和速度。每个路径点都有时间戳。我们可以将时间戳与预测模块的输出相结合，以确保我们计划通过是，轨迹上的每个路径点均未被占有。

这些时间戳和空间上的两个维度（2D）共同创建了一个三维轨迹（3D)…我们还为每个路径点指定了一个速度，用于确保车辆按时到达每个路径点。

现实世界的规划面临很多约束。首先避免障碍物碰撞，然后需要确保乘客舒适感，再者排除不可能的行为，例如一百八十度托马斯回旋二脚踢。而且我们的驾驶还要受法律约束等等。

在道路任何两点，可能有多个不会发生碰撞、行驶很舒适、可行合法的轨迹。所以我们选择时就需要加入“成本函数”了。成本函数（偏离道路、有可能碰撞、速度限制、轨迹曲率和加速度舒适性）为每个轨迹分配一个“成本”，我们选择成本最低的轨迹。

轨迹成本将所有这些缺陷聚合为单个数字，这使我们能对不同的轨迹按数字大小进行排名。例如，高速公路的成本函数可能与停车场的不同。

十、Frenet坐标系

我们通常使用笛卡尔坐标系来描述物体的位置，但笛卡尔坐标系对车辆并不是最佳选择。即使给出了车辆坐标位置（x，y），如果我们不知道道路在哪儿，就很难知道车辆行驶路多远，也很难知道车辆是否偏离了道路中心。

因为笛卡尔的限制，那我们九选择性能更优越的Frenet坐标系。Frenet坐标系描述了汽车相对于道路的位置。在该坐标系下，s代表沿道路的距离，也被称为纵坐标。d表示与纵向线的位移，也被称为横坐标。

在车辆行驶时中，纵坐标表示道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

这节课的内容就到这里，但是本章的内容仍未结束，我们休息片刻接着继续。

看看风景，咱们接着又来。

十一、路径-速度解耦规划

路径-速度解耦规划将轨迹划分为两步：路径规划和速度规划。

首先在路径规划步骤中生成候选曲线，这是车辆可行驶的路径。接着使用成本函数对每条线进行评估，该函数包含平滑度、安全性、与车道中心线的偏离以及开发者考虑的问题等。然后按照成本对路径进行排名并选择最低的路径。

下一步是确定沿这条路线行进的速度。我们改变该路径上的速度，所以真正需要选择的是与路径相关的一系列速度，而不是单个速度。我们称之为“速度曲线”。我们可以使用优化功能为路径选择受到各种限制的良好速度曲线。通过将路径和速度相结合可构建车辆行驶轨迹。

十二、路径生成与选择

为了在路径-速度解耦规划中生成候选路径，首先将路段分割为单元格。然后对单元格随机采样。通过每个单元格中取出一个点并将其连接，我们创建了候选路径。通过重复该过程，就能实现对路径的构建。其它内容大同小异，就不赘述。

十三、ST图

选择路径后的下一步是选择与该路径相关联的速度曲线，一个被称为“ST图”“的工具可以帮助设计和选择速度曲线。图中，”s“表示车辆纵向位移，”t“表示时间。由于速度是位置裱花的速率，所以可以通过查看曲线的斜率从ST图上推断速度。斜坡越陡则表示在更短的时间段内有更大的位移，对应更快的速度。

十四、速度规划

为构建最佳速度曲线，需要将ST图离散为多个单元格。单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建并保持曲线相似度。

在 ST 图中可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。

例如，假设预测模块预测车辆将在 t0 到 t1 的时间段内驶入的车道。由于该车将在此期间占据位置 s0 到 s1，因此在 ST 图上绘制了一个矩形，它将在时间段 t0 到 t1 期间阻挡位置 s0 到 s1。为避免碰撞，速度曲线不得与此矩形相交。既然有了一张各种单元格被阻挡的 ST 图，便可以使用优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案。这些限制可能包括：法律限制，如速度限制；距离限制，如与障碍物的距离；汽车的物理限制，如加速度限制。

十五、优化

路径-速度解耦规划在很大程度上取决于离散化。路径选择涉及将道路划分为单元格，速度曲线构建涉及将ST图划分为单元格。但是这种解决方案并不理想。

于是我们就可以使用”二次规划“技术（将平滑的非线性曲线与这些分段式线性段拟合）。我们只需要简单使用几种不同的优化包中的一种，包括一种Apollo推出的运行方案生成轨迹，一旦路径和速度曲线就绪，便可以用其构建三维轨迹。

十六、路径-速度规划的轨迹生成

假设我们正在路上行驶，车辆感知到有一辆慢速行驶的车离我们越来越近。首先，计算机在这辆车的周围生成多条候选路径，使用成本函数对这些候选路径进行评估并选择。然后使用ST图来进行速度规划。最后，将路径和速度曲线合并构建轨迹。下图的红色表示速度很快，蓝色表示速度慢。

十七、Lattice规划

这是一个很绕脑壳的算法，所以你需要打起十二分的精神来学习。

该方法通过使用Frenet坐标可以将环境投射到纵轴和横轴上，目标是生成三维轨迹：纵向轨迹、横向轨迹、时间维度。

当然，为了更加容易理解，我们可以把维度分解成两个单独的二维问题，这是通过分离轨迹的纵向和横向分量来解决的。其中一个二维轨迹是具有时间戳的纵向轨迹称之为ST轨迹，另一个二维轨迹是相对于纵向轨迹的横向偏移称之为SL轨迹。

Lattice 规划具有两个步骤即先分别建立 ST 和 SL 轨迹，然后将它们合并为生成纵向和横向二维轨迹。

先将初始车辆状态投射到 ST 坐标系和 SL 坐标系中，通过对预选模式中的多个候选最终状态进行采样，来选择最终车辆状态。

对于每个候选最终状态构建了一组轨迹将车辆从其初始状态转换为最终状态，使用成本函数对这些轨迹进行评估并选择成本最低的轨迹。

十八、ST轨迹的终止状态

根据情况可以将状态分成 3 组：巡航 、跟随、停止。

巡航意味着车辆将在完成规划步骤后定速行驶，实际上在对图上的点进行采样，在图中横轴代表时间，纵轴代表速度。对于该图上的点，这意味着汽车将进入巡航状态，在时间 t 以 s 点的速度巡航，对于这种模式，所有最终状态的加速度均为零。

下一个要考虑的模式为跟随车辆，在这种情况下要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，在跟随车辆时，需要与前方的车保持安全距离，这时速度和加速度将取决于要跟随的车辆，这意味着在这种模式下，速度和加速度都会进行修正。

最后一种模式是停止，对于这种模式只需要对汽车何时何地停止进行抽样，这里速度和加速度会 被修正为0。

十九、SL轨迹的终止状态

根据这样一个假设来进行SL 规划，即无论车辆进入怎样的终止状态，车辆都应该稳定地与车道中心线对齐。

这意味着只需要在一个小区域内，对横向终止位置进行采样。

具体来说采样的是道路上相邻车道中心线周围的位置。为了确保稳定性，汽车驶向的终止状态应该与车道中心一致。当用横向位置与纵向位置作图时 ，想要的候选轨迹应该以车辆与车道对齐并直线行驶而结束。为了达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为零。这意味着车辆既不是横向移动的，那是一阶导数；也不是横向加速，那是二阶导数。这意味着车辆正沿着车道直行。

二十、Lattice规划的轨迹生成

一旦同时拥有了 ST 和 SL 轨迹，就需要将它们重新转换为笛卡尔坐标系。

然后可以将它们相结合构建由二维路径点和一维时间戳组成的三维轨迹。

ST 轨迹是随时间变化的纵向位移，SL 轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标 S，所以可以通过将其 S 值进行匹配来合并轨迹。

本次课程总结就到这里了，相信大家都是收获满满，也有很多朋友表示太难了，我不想学习了，大家一定要坚持哦，因为还剩最后一节，你就完成入门学习了，想不想多一门技能就靠你的毅力了。加油吧！奥力给！