轨迹规划的概念：

轨迹规划的核心就是要解决车辆该怎么走的问题。比如我们知道了附近有行人、骑自行车的人以及前方的卡车，如果我们现在将要左转，该怎么办？这就是轨迹规划该解决的问题。

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

轨迹规划本质上来说是一个优化问题。谈到优化问题，我们需要知道优化的约束和优化的目标。首先看约束，第一个约束就是车辆要遵守交规，这是强制性的约束；第二个约束是要避免碰撞；第三个约束是要使规划的轨迹在控制上可实现，不能出现规划了一个急弯的轨迹，车根本拐不过来的状况。而优化的目标总的来说就是要跟人开的像，具体表现就是让人乘坐起来感到舒适。但像人也有不同的优化目标，比如我们可以开的稳一点也可以开的急一点，像老司机一点。

这样一个优化问题在数学上的性质是什么呢？我们关心一个优化问题首先看这个优化问题是不是凸的，因为凸的问题比较好解一些。什么是凸的？我们可以简单描述对于一个问题它的两个可行解，其线性组合也应该是可行的，而且不比这两个解都要差。那么轨迹规划是不是一个凸优化问题呢，并不是,它是非凸的。为什么呢？我们在对t ->(x,y)问题进行求解时，复杂度比较高计算量比较大。所以通常的做法是把他分成横向规划和纵向规划。横向规划，是s ->（x,y）,就是决定轨迹的形状是什么。纵向规划是t ->s,也就是指在此形状上运动的速度状态，也就是时间与位移的关系。横向规划和纵向规划联合起来就是t ->(x,y)。那么横向规划是凸的吗？如下图，图上没有时间信息，这就是横向规划。两条黑线代表不同的变道轨迹都是可行解，然后我们看其线性组合也就是中间的红线，但明显红线的轨迹是不能用的，是很差的，他变道太慢，骑线行驶的距离太长了，所以横向规划是非凸的。

我们再来看纵向规划，我们经常用t - s图表示。t表示时间，s表示我们走过的路程。当一个人横穿马路时我们可以用白色矩形来在t - s图上表示这个过程，左边界表示人进入我们规划路径的时刻，右边界表示离开规划路径的时刻。横向上，上下边界差可以理解为人在占用我们规划路径的宽度。对于这样一个纵向规划，有两种选择，一种是我们要让人，既下边的绿色路线，一种是车辆加速超过，既上边的绿色路线。但它的线性组合可能是这条红线，明显这已经撞上了，所以这并不是一个凸的问题。

决策：

正是因为轨迹规划是非凸的，所以我们需要用决策的模块来解决。什么是决策呢？从数学上来讲，决策就是限定非凸问题的解空间，将问题转化为凸的。我们可以看下图的例子，如果两个人在穿马路的话，我们大体上可以有四种不同的决策，既让两个人，超两个人，让一超一和超一让一。而一旦决策确定，那么其实就可转换为凸的问题，求解就会相对来说比较容易。决策也会使解更加稳定，因为一个单纯的数值优化问题的求解，会很难保证每一帧的解是相对稳定的，不会出现第一帧要让，而第二帧却要冲的问题。但需要注意决策的问题是一个NP-hard 问题。有一些需要决策的场景，比如抢行还是让行，是否要冲黄灯，在哪两辆车之间变道、并线，是否要主动变道，是从左还是右绕行前方障碍物，只有做了决策才能使问题变为凸优化问题，便于求解。对于凸问题，有很多比较快速的算法进行计算了。线性规划，二次规划，序列二次规划，都是数值求解优化问题的方式。我们可以看一下二次规划中，如果Q正定，二次规划就是凸的问题，有很快的方式求解它。

下边就是看如何把自动驾驶中的问题转化为优化问题。

首先是纵向规划的决策，决策的方法我们已经谈到了一些，比如动态规划。这是一个NP-hard问题所以没有一个简单的算法进行精确求解。看下图，如果有两个人穿马路，我们来研究一下如何走才是最优的，我们可以将T-S进行离散化，离散化之后，我们就可作出一个状态转移方程，而这个状态转移方程我们可以看作每一步的决策只与前边的两步有关系，所以有转移方程。虽然这个方程在实际过程中比较难写但确实是一种做法。虽然离散化t和s降低了精度，但降低精度也能降低了运行时间。注意，这种方法并不能保证最后的速度比较舒适，它只是大概的一个决策方法，既到底让或者不让。

我们再来看一下决策方面的挑战。第一个挑战上面已经讲过了，决策问题是一个NP-hard问题，不易直接求解，有多种多样的近似算法。第二个挑战是很难用规则去拟合人的经验，包括上述的状态转移方程中的cost也很难去表示。而一些解决的办法是我们可以根据各种不同的情况去建立一个数学模型。以及用机器学习的方法去做一些决策。比如说下图左边的例子，我们已经打算绕行，但前车突然起步了，我们该怎么办？我们是该变道还是跟在后边走还是继续绕行？这种情况确实很靠人的经验。再看右边的图，我们要左转，但是前边这辆车停了很久也没有走我们要不要变道呢？作为人类司机也很难判断。场景的多变而复杂使得决策面临着很多挑战。

横向规划：

刚刚我们已经说过了，轨迹规划可以拆成横向和纵向的规划。横向规划是一个方向的规划，它可以看成一个如何打方向盘的规划，它只决定了轨迹的形状。这个问题通常的解法分两种，一种是无车道的，比如说freespace中的规划，或者说是停车之类的问题，它是没有车道线信息或者说先验信息的，大多都用OpenSpace路径生成的方式去做，一般也是低速的。对于有车道的情况，虽然有车道线但是输出s ->(x,y)这个函数难度也是比较大的，常见的做法是是离线生成参考线，之后我们就可以将s ->（x，y）这个问题变为一个求解s ->L的问题，L是指车辆在这个参考线上的横向偏移量。可以看到下图右边这张图，本来是沿车道往前走，但由于有车辆的遮挡，我们就必须绕行它。即横向偏移就可以了。

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

有了参考线之后我们就可以把参考线离散化，采一些点出来，那么就变成了解一个离参考线偏移距离的一个问题。就变成了s ->L的问题，约束是不跨越边界，避免碰撞，而优化的目标是要离参考线近，要离障碍物远，曲率不大，曲率变化率不大等等。

我们看下这个例子，那么这个问题就可以看成一个二次规划QP的问题。0，-1，-2是我们车过去的路径，0号点是我们车当前的位置，我们需要解的就是1，2，3，4，5，6这种点的相对于参考线的横向偏移x，已知就是x[-2]，x[-1]，x[0]，求解x[1],x[2]等，而约束就是他不能超过左右边界包括马路牙，实线，障碍物等。优化目标就是我们要离中心参考线近，方向盘不能打太多太快。下图中的公式的形式就是一个关于x的二次的形式，所以它们可以去用QP的方法来解。

之后我们再来看看横向规划的挑战。虽然大部分时候车都在一个有车道线的地方开，但比如像下图左下部分的路口，这边是三条直行的车道，而对面是四条条直行的车道，右边的白车就不会按车道线开，会横跨车道线挤你。右上的图，是有辅路的情况，他是没有车道线的，这种进辅路的问题更像是一种没有参考线的freespace的规划，这种都是比较大的挑战，如果说没有车道线或者说没有人按车道线开，那么这样的规划就会比较难做一点。

另外一些挑战就是环境的问题，因为周围的环境是瞬息万变的周围的环境预测也是困难的，我们看下图，本来车辆是要往左变道，而左下角黄色的块表示摩托车，这个摩托车正在飞速的过来，所以车辆又进行了取消变道的决策，也就是生成平滑的曲线回到原来的道路上去，这样的情况我如何能保证路径光滑且在控制上可实现，也是一件很难的事情。

纵向规划：

纵向规划其实是对于我们设定好的路径上的速度上的规划，决定了在这整个轨迹上的运动过程。求解这种优化问题，有的约束第一就是遵守交规（信号灯、限速、停车让行等），第二就是避免碰撞。优化的目标，想都可以想的到就是舒适，舒适就是速度变化率不大，加速度变化率不大，行驶也要尽量快一点，等等。

我们举一些例子，前边提到了行人横穿马路，行人就是这样一个矩形，我们对应了两种决策，决策完成后我们应该怎么办呢？如果决定要抢行，我们就可以将矩形的约束条件扩展到最下部，这样就是凸问题求最优解。如果是避让我们就可从下边过，但是如果是行人斜着穿过马路呢？那么请大家想一下按s ->t图是如何表示的呢？答案就是它是斜向上的平行四边形。

再看一些类似的例子,如下图。比如黄灯，黄灯是一个特殊的例子，如果决策要冲的话，需要尽快通过路口，否则的话很容易被逼停在路中间出不去，这种情况我们可以用一个这样的s - t图表示，左边界是表示黄灯亮的时刻，大家看这样的白色矩形有一个缺角，当黄灯亮起的时候，我们如果要尽快通过路口，t在增大的过程中s也要尽快的增大，增大的速率要超过缺角的斜率。

我们再看一些更高级的想法，我们在跟车的时候，假如前车在匀减速，在s - t，如下图所示，如果能够精确描述前车的运动的状态，那么白色部分会有各种各样的形状，这样就可以在解优化问题是解出一个好的速度曲线。

之后我们再看一下纵向规划的挑战。如下图，比如我们绿灯左转，而有一个电动车，在左侧很快速的横穿马路，正常来说那么人应该怎么开呢？人会和电动车司机对一下眼神，通过眼神比较气势谁猛，然后谁就会主动的让对方，开个玩笑。但在决策上是这个事情并不好处理，这是一个博弈的过程，并不能一开始就决定要无视他还是让他，而要在一开始对他有一点反应，所以在很多时候要在激进和保守之间掌握一个平衡点，很多时候要有不同的参数和不同的模式去处理这个事情。

另一方面，感知和预测带来的困难也会带来纵向规划的挑战，可以看下边这连续两张图，突然就有人从车前冲出来了，这个我们俗称叫做鬼探头，你要检测到有盲区，需要在在检测到有盲区的时候进行减速等等。还有问题就是预测，我们可以看到左下角的图，右车道有一个面包车，而前边有速度慢的一辆自行车，人一看就知道，这个面包车极有可能变道，但这对预测也提出了很大的挑战，如果没有预测那么你的反应就会比较慢。右下角的图，你也无法判断这辆左拐的车要拐到哪条车道上去。

还有一些极端的情况，需要考虑到横纵向协调配合，比如下图一个极限的车辆插入，你在高速行驶，而旁边的车突然就要插进来，如果是人类司机他会打方向避让，如果左边没有车，甚至会变道，左边有车，他也会扭一点方向去避让。这种就需要横纵向的配合去一起解决这种极端的情况，从纵向规划来说，这种情况，你已经不可能保持安全车距了，你更多的只能说保证不撞，然后尽快拉开车距，而不是一脚刹到底。

作者介绍：

梁亚雄，Pony.ai Tech lead，清华大学计算机硕士，ACM-ICPC亚洲区赛金牌得主，此前任职于Hulu，目前在Pony.ai负责自动驾驶路径规划和控制模块的技术研发。

本文来自梁亚雄在 DataFun 社区的演讲，由 DataFun 编辑整理。

创作场景

自动驾驶中轨迹规划的探索和挑战