自动驾驶高速路上的路径规划算法

如何安全有效的规划行驶路线，是自动驾驶汽车需解决的最大的难题之一。

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

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

· 本算法的一些假设如下：

· 任何情况下，不会与其他车辆发生事故

· 最大行驶速度为80KMH

· 最大加速度为10m/s2

· 最大jerk为10m/s3

· 车辆在不同车道之间不超过3s

· 车辆不能超出高速的3条车道

*

· 车辆不能逆向行驶

本算法的开发难度非常之大

1.自动驾驶汽车的功能层

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

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

(

· 动作控制层：负责控制汽车，使其尽可能的按照“设定的”轨迹形式。该层需要最快的反应速度；

· 传感器融合层：负责合并个传感器的输出（如雷达和激光雷达）

· 定位层：负责尽可能准确的在地图上定位车辆的位置，并计算其他物体相对于车辆的位置

· 预测层：负责识别传感器检测到的物体的性质(又名感知)，并根据汽车当前的轨迹、其他车辆的轨迹和场景中的各种元素(如交通灯)预测场景中近未来的变化。这个层的一个重要任务是预测冲突。

· 行为层：该层的主要作用是协调。根据底层的输入信息来决定如何调整行车轨迹

· 轨迹层：负责计算既定条件（速度、距离、车道、jerk等等）下的行车轨迹

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

2.传感器融合层的意义

】

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

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

· 车辆的位置、速度和方向

· 其他车辆的位置和速度

· 上次提交的车辆行驶轨迹

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

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

—

坐标系

通常，我们习惯使用笛卡尔坐标系来定义空间点的位置。但在现实中，道路往往不是“笔直”的，因此对于人类非常简单的操作（如判断车辆在哪条车道），在电脑的笛卡尔坐标系中，

往往是难以准确定义的。下图展示了我们使用笛卡尔坐标系时所面临的问题：

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

设想一下，如果我们采用的坐标系可以反映道路的曲率，那么在新的坐标系下车辆向前行驶

并保持在车道内的轨迹就会变成一条直线，这会大大简化路径规划的难度。而Frenet坐标

系正可以实现我们的设想。

不同坐标系下的行车轨迹：Frenet（左）vs 笛卡尔坐标系（右）

在Frenet坐标系中，可以平面上的点的位置可以由纵轴和横轴定位分别记为S和D 。其背

后的数学原理非常复杂，在此我们不进行累述。你可以认为穿过道路中心的曲线决定了S轴，并指出了车辆在路上走了多远；D轴则映射为车辆的侧向位移。下图显示了弯曲道路在

Frenet坐标系上的样子：

—

4.轨迹平滑处理

我们假设车道已经被预先映射，并且提供了沿着中黄线的路径点，这条中黄线分隔了公路的

两边。这有助于我们确定我们在最近的路径点上的位置。

由于我们所设置的路径点非常稀疏，当我们试图将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)

)

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

5.状态控制机制

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

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

…

最终的状态控制机制

最常见的状态是保持车道，但每当我们想要改变车道，汽车将首先过渡到准备换车道的左/

右状态，并在确保转换车道是安全的。在换车道前进入的中间状态，类似于车辆在换车道前

打开左/右信号。

关于状态机的实现，我们在Frenet坐标上获得了灵感。我们将一个给定的状态分解为它的

纵向和横向分量。纵向状态代表保持当前车道，横向状态代表我们可能会进入的下一个状态，而损失函数（Cost Function）则倾向于选择纵向状态。

6.损失函数

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

不同的权重来惩罚哪些行为。

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

7.最终结果