算法集锦（24） | 自动驾驶 |高速公路行驶路径规划算法

如何安全有效的规划行驶路线，是自动驾驶汽车需解决的最大的难题之一。事实上，路径规划技术，现阶段是一个非常活跃的研究领域。路径规划之所以如此复杂，是因为其涵盖了自动驾驶的所有技术领域，从最基础的制动器，到感知周围环境的传感器，再到定位及预测模型等等。准确的路径规划，要求汽车要理解我们所处的位置以及周边的物体（其他车辆、行人、动物等）会在接下来的几秒钟内采取什么样的行为。另一项关键技术是轨迹生成器（trajectory generator），其产生输入路径规划算法的参考轨迹。

本期算法分享，我们重点介绍一种基于C++开发的高速公路路径规划算法。该算法利用jerk minimisation技术，可在模拟器中生成安全且高效的行驶路径。

• 本算法的一些假设如下：
• 任何情况下，不会与其他车辆发生事故
• 最大行驶速度为80KMH
• 最大加速度为10m/s2
• 最大jerk为10m/s3
• 车辆在不同车道之间不超过3s
• 车辆不能超出高速的3条车道
• 车辆不能逆向行驶

本算法的开发难度非常之大，下图显示了早期版本所遇到的问题。

自动驾驶汽车的功能层

首先，我们来详细的研究下自动驾驶汽车的功能层（Functional Layers）。

路径规划需要自动驾驶汽车不同功能层之间的合作、协调。上图给出了一个自动驾驶系统的功能层的配置形式：

• 动作控制层：负责控制汽车，使其尽可能的安按照“设定的”轨迹形式。该层需要最快的反映速度；
• 传感器融合层：负责合并个传感器的输出（如雷达和激光雷达）
• 定位层：负责尽可能准确的在地图上定位车辆的位置，并计算其他物体相对于车辆的位置
• 预测层：负责识别传感器检测到的物体的性质(又名感知)，并根据汽车当前的轨迹、其他车辆的轨迹和场景中的各种元素(如交通灯)预测场景中近未来的变化。这个层的一个重要任务是预测冲突。
• 行为层：该层的主要作用是协调。根据底层的输入信息来决定如何调整行车轨迹
• 轨迹层：负责计算既定条件（速度、距离、车道、jerk等等）下的行车轨迹

生成行车轨迹的方法很多，这里我们采用了Frenet Coordiante System方法。

传感器融合层的意义

我们在模拟器中为车辆设置了一系列传感器，它们的输出融合在一起以产生更精确的测量结果。大多数在Level4上的自动驾驶汽车公司在他们的传感器套件中使用雷达、激光雷达和照相机。拥有多种不同类型的传感器至关重要，因为每种传感器都有各自的优缺点。此外，对于同种传感器进行冗余设计，可以减轻传感器故障带来的影响。

在本算法中，模拟器可以提供以下传感器融合功能：

• 车辆的位置、速度和方向
• 其他车辆的位置和速度
• 上次提交的车辆行驶轨迹

通过以上信息，我们可以计算车辆与其他车辆的准确距离，并通过行车轨迹来预测与其他车辆的碰撞可能性。

下面我们详细介绍轨迹生成器（Trajectory Generation）。

Frenet坐标系

通常，我们习惯使用笛卡尔坐标系来定义空间点的位置。但在现实中，道路往往不是“笔直”的，因此对于人类非常简单的操作（如判断车辆在哪条车道），在电脑的笛卡尔坐标系中，往往是难以准确定义的。下图展示了我们使用笛卡尔坐标系时所面临的问题：

笛卡尔坐标系中的曲线车道

设想一下，如果我们采用的坐标系可以反映道路的曲率，那么在新的坐标系下车辆向前行驶并保持在车道内的轨迹就会变成一条直线，这会大大简化路径规划的难度。而Frenet坐标系正可以实现我们的设想。

不同坐标系下的行车轨迹：

Frenet（左）vs 笛卡尔坐标系（右）

在Frenet坐标系中，可以平面上的点的位置可以由纵轴和横轴定位,分别记为S和D 。其背后的数学原理非常复杂，在此我们不进行累述。你可以认为穿过道路中心的曲线决定了S轴，并指出了车辆在路上走了多远；D轴则映射为车辆的侧向位移。下图显示了弯曲道路在Frenet坐标系上的样子：

行驶轨迹平滑处理

我们假设车道已经被预先映射，并且提供了沿着中黄线的路径点，这条中黄线分隔了公路的两边。这有助于我们确定我们在最近的路径点上的位置。

由于我们所设置的路径点非常稀疏，当我们试图将Frenet转换回真实世界坐标时，会产生带折角的轨迹。这反过来又会导致车辆突然的加速和颠簸。由于函数toRealWorld(s, d) -> (x, y)使用两个路径点之间的线性插值，来确定x和y的最佳逼近值，我们总是冒着产生非平滑轨迹的风险。

如何进行改善呢?在之前分享的算法中，我们发现由多项式拟合的曲线往往会产生非常平滑的轨迹。因此，我们用多项式拟合来替代线性插值。利用在Frenet坐标系中取位置s来创建样条，得到真实世界的坐标（x, y）和偏移量（dx和dy），然后代入这个公式，得到最接近真实世界的坐标点。

x = spline_s_x(s) + d * spline_s_dx(s)

y = spline_s_y(s) + d * spline_s_dy(s)

可以看到，经过优化的行车轨迹非常平滑。

状态控制机制

路径规划的另一个问题是，人类可以根据驾驶方式、环境信息以及目的地，将汽车转换到不同的状态。事实证明，我们可以将机车辆的状态编入字典，并指导它们根据当前状态，将自动驾驶系统的其他层转换到哪些状态。

在算法中，我们的状态控制机制非常简单，如下所示：

最终的状态控制机制

最常见的状态是保持车道，但每当我们想要改变车道，汽车将首先过渡到准备换车道的左/右状态，并在确保转换车道是安全的。在换车道前进入的中间状态，类似于车辆在换车道前打开左/右信号。

关于状态机的实现，我们在Frenet坐标上获得了灵感。我们将一个给定的状态分解为它的纵向和横向分量。纵向状态代表保持当前车道，横向状态代表我们可能会进入的下一个状态，而损失函数（Cost Function）则倾向于选择纵向状态。

损失函数

鉴于我们通常会返回多个候选状态以及轨迹，我们必须找到一种选择“最佳”行动路线的方法。我们用损失函数来解决该问题。损失函数可以教会汽车我们想要的合理行为，以及通过不同的权重来惩罚哪些行为。

我们所有的成本损失函数都遵循在cost_functions文件中定义的接口：

typedef function<double (const Vehicle&, const vector<Vehicle>&,  const Trajectory&, const State&, const double&)> CostFunction;

最终结果

可以看到，当前我们设计的路径规划器运行得相当好，可以确保车辆在车道上安全有效的行驶。后续，我们计划通过调整权重、改进损失函数以及在预测层中加入机器学习算法来进一步优化。