一文读懂自动驾驶轨迹规划的核心解法-绵羊汽车生活记录

自动驾驶轨迹规划的核心就是要解决车辆该怎么走的问题。一辆自动驾驶车辆处在周围有行人、骑自行车的人以及前方有卡车的环境下，现在它需要左转，该怎么做？这就是轨迹规划该解决的问题。

轨迹规划的输入包括拓扑地图、障碍物及障碍物的预测轨迹、交通信号灯的状态，还有定位、导航（因为知道目的地才能规划路径）、车辆状态等其他信息。而轨迹规划的输出则是一个轨迹，轨迹是一个时间到位置的函数，也就是特定的时刻要求车辆在特定的位置上。上图中轨迹函数是t→(x,y,z)，把z标灰是因为目前为止我们的车还不会飞，所以z是(x, y)的函数。

首先看轨迹规划的约束，第一个约束就是车辆要遵守交规，这是强制性的约束；第二个约束是要避免碰撞；第三个约束是要使规划的轨迹在车辆控制上可实现，例如不能出现规划出一条车根本拐不过来的急弯的轨迹。

而优化的目标是使得自动驾驶跟人类司机驾驶相似，具体表现就是乘客乘坐时感到舒适。但“开得像人”也有不同的优化目标，比如我们可以驾驶得稳一点也可以开得急一点，像老司机一般。

这样一个优化问题在数学上的性质是什么呢？数学上，解决一个优化问题，首先看这个优化问题是不是凸的，因为凸的问题比较好解一些。

什么是凸优化问题？可以简单描述为，一个问题如果有两个可行解，则要满足这两个可行解的线性组合也是可行的，且不比这两个可行解都差。

那么轨迹规划是不是一个凸优化问题呢？可以拆解来看，我们求解轨迹规划的t→(x,y)问题时，发现其复杂度较高且计算量较大。所以常见的解法是把轨迹规划分成横向规划和纵向规划。可以把这两个子问题看成t→(x,y)在空间上的投影，只要二者之一不是凸的，则轨迹规划问题就不会是凸的。

横向规划，即s→(x,y)，决定了轨迹的形状。纵向规划是t→s，是指在此形状上运动的速度状态，也就是时间与位移的关系。横向规划和纵向规划联合起来就是t→(x,y)。

那么横向规划是凸优化问题吗？如上图中的左侧图，没有显示时间信息，这就是横向规划。两条黑线代表不同的变道轨迹都是可行解，然后我们看其线性组合也就是中间的红线，这条红线的变道轨迹明显是不能用的——它变道太慢，骑线行驶距离过长。所以，根据数学定义，横向规划是非凸优化问题。

再来看看纵向规划。纵向规划常以上图右侧的t – s图表示，t表示时间，s表示我们走过的路程。当一个行人横穿马路时，我们可以用白色矩形在t – s图上表示此过程，左边界表示行人进入规划路径（考虑车宽）的时刻，右边界表示离开规划路径的时刻。横轴上的上下边界差可以理解为行人占用的规划路径的宽度。

面对这样场景，纵向规划将有两种选择，一种是车要让人，对应图中白色矩形下方的绿色路线；一种是车辆加速超过，即上方的绿色路线。但它的线性组合可能是图中的红线，而这条路线中行人与车已经撞上，显然是不可行的路线（解）。所以纵向规划也不是一个凸的问题。

什么是自动驾驶的决策模块呢？从数学上来讲，决策就是为了限定非凸问题（轨迹规划）的解空间，将问题转化为凸的。

我们可以看上图的例子：两个行人正在横穿马路，自动驾驶大体上可以作出四种不同的决策，即让两个人；超两个人；让一超一；超一让一。

而一旦决策确定，那么路径规划就可转换为凸的问题，求解就会相对来说比较容易，也会使解更加稳定。因为一个单纯的数值优化问题的求解，很难保证每一帧的解都是相对稳定的，甚至可能出现第一帧要让，而第二帧却要冲的问题，决策可以提前规避这种不稳定。

我们需要注意，决策是一个NP-hard（非确定性多项式困难）问题。自动驾驶需要决策的场景涉及许多，包括抢行还是让行，是否要冲黄灯，在哪两辆车之间变道、并线，是否要主动变道，是从左还是右绕行前方障碍物。只有做了决策才能使轨迹规划问题变为凸优化问题。

处理凸问题可以利用的快速算法有许多，线性规划，二次规划，序列二次规划……这些都是数值求解优化问题的方式。例如在二次规划中，如果Q正定，二次规划就是凸的问题，求解它会更加迅速和容易。

首先，将t – s图进行离散化，离散化之后，我们可以认为每一步的决策只与前边的两步有关系。两步是因为我们可以近似求出加速度，并能写出一个状态转移方程。

尽管这个方程在实际过程中比较难写，但确实是一种做法。虽然离散化t和s降低了精度，但降低精度也帮助降低了运行时间。

第一个挑战上面已经提到，由于决策问题是一个NP-hard问题，不易直接求解，存在多种多样的近似算法。

第二个挑战是很难用规则去拟合人的经验，包括上述的状态转移方程中的cost也很难去表示。目前解决这个挑战可用的部分办法是根据各种不同的情况建立数学模型，以及采用机器学习的方法进行决策。

以上图左侧的场景为例，一辆正在行驶的自动驾驶车辆打算绕行前面白车，但此时前车突然起步了，我们该如何继续行驶？是变道？还是跟在后边行驶，又或是继续绕行？处理这种情况确实依靠人类驾驶的经验。

再看上图右侧的例子：自动驾驶车准备在前方左转，但是前车停了很久也没有启动（可能前车驾驶员没反应过来），我们这时该不该变道呢？对这种情况，人类司机有时也很难判断。由此可见，场景的多变而复杂使得决策面临很多挑战。

前面提到轨迹规划可以拆成横向和纵向的规划，现在我来具体介绍横向规划的解法。横向规划就是行车方向上的规划，可以看成是如何打方向盘的规划，它决定了轨迹的形状。

这个问题通常的解法分两种，一种是无车道的场景，比如在freespace（自由空间）中规划或者泊车之类的场景，车辆一般处在低速行驶状态，缺乏车道线等先验信息。业界大多都用搜索等路径生成的方式去处理无车道场景。

另一种是有车道的情况。虽然可以参考车道线信息，但是规划上想输出s→(x,y)函数，难度并不小。常见的做法是离线生成参考线，随后就可以将求解s→(x,y)的问题变为求解s→l的问题，l是指车辆在这个参考线上的横向偏移量。

以上图右侧场景为例，原本车是沿车道向前走，但由于有前方车辆的遮挡，我们就必须绕行它，即向右横向偏移。

而参考线的生成，其实类似于开卡丁车时别人教你的过弯的最优路线，它也是一个优化问题，当然也要保证安全性和舒适性。方便的是，有了高精地图辅助后，参考线可以通过离线去进行，所以可以用一些开销比较大的算法做到最优。参考线的约束在于其需要在车道线内，并且在控制上可实现。优化目标则是参考线需接近车道中心，曲率不能太大，曲率变化率也不大。

确定参考线后，通过把参考线离散化，采一些点出来，那么横向规划问题就转化为求解一个离参考线偏移距离的一个问题，即转化成s→l的问题。其约束是车辆行驶不跨越边界，避免碰撞，而优化的目标是要离参考线近，要离障碍物远，曲率不大，曲率变化率不大等等。

借助上图右侧的例子，你会发现横向规划可看成一个二次规划（QP）的问题。其中“0”，“-1”，“-2”是自动驾驶车行驶过的路径，0号点是车当前的位置，现在我们需要解的就是1，2，3，4，5，6这些点相对于参考线的横向偏移x。换句线]等。

该函数约束是车辆行驶不能超过左右边界包括马路牙，实线，障碍物等。优化目标则是车辆要离参考线近，方向盘不能打太多太快，保证乘坐的舒适。上图中的公式其实是一个关于x的二次的形式，所以可以使用二次规划的方法来解决。

虽然大部分时候车都行驶在有车道线的马路上，但也会面临特殊的挑战。例如下图里左侧显示的路口，我们的车行驶的方向上有三条直行车道，但过路口后变成四条直行车道，并且对得很不齐。现实中，右侧的行驶的车（白车）往往不依据车道线行驶，可能会横跨车道线挤占自动驾驶车辆所在车道的空间。

而下图右上角则展示了没有车道线的主辅路场景。在这种主辅路之间切换，就像解决一种没有参考线的freespace的规划，挑战也不小。总的来说，要想解决没有车道线或者说普通车辆不按车道线行驶的路径规划问题，难度都不小。

另外的挑战就是环境的问题，因为车外行驶环境瞬息万变，要对周围进行环境预测也很困难。以下图为例，我们的自动驾驶车准备往左变道，而左下角橙色块代表的摩托车正飞速地向前行驶，于是我们的车辆迅速给出取消变道的决策，生成平滑的曲线回到原来行驶的道路上。因此，面对这类的情况，轨迹规划模块需要保证规划的路径光滑且在控制上可实现。

求解这类优化问题，第一个约束是遵守交规（信号灯、限速、停车让行等），第二个约束是避免碰撞。而纵向规划的优化的目标是乘坐舒适，也意味着车辆的速度变化率不大，加速度变化率不大，行驶速度也要尽量快一点（限速内）等。

前边我提到了行人横穿马路的场景，在t – s图中，行人的运动过程可以转化一个矩形，最终给出了两种车辆的对应决策——加速超过行人或减速让行。那么决策之后该怎么做呢？