Frenet坐标系下横纵向轨迹决策规划（SL投影及ST投影）及Apollo决策算法解析

参考：

（1）《攻城狮说 | 应对复杂路况，自动驾驶如何规划它的下一步？ “老司机”炼成记！》微信公众号文章Pony.ai小马智行

（2）《【Apollo】apollo3.5决策分享 --by 百度美研 Yifei Jiang老师》知乎

（3）《Baidu Apollo代码解析之s-t图的创建与采样（path_time_graph)》 CSDN

（4）《技术文档 | 二次规划（QP）样条路径》微信公众号文章Apollo开发者社区

（5）《开发者说｜Apollo5.5 规划算法解析》 知乎

（6）《自动驾驶之轨迹规划6——Apollo EM Motion Planner》CSDN

一、何谓轨迹规划

Apollo的 Planning 分为参考线平滑、决策、路径规划、速度规划等部分。

从整体上来说，规划模块的架构分为两个部分：一部分负责对数据的监听、获取和预处理；另一部分负责管理各个优化模块。数据进入后，对其综合处理为规划模块的内部数据结构，由任务管理器调度合适的优化器进行各个优化任务。综合优化的结果，经过最终的验证后，输出给控制模块。

1.1 轨迹规划的概念

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

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

1.2 非凸优化问题

轨迹规划本质上来说是一个优化问题。谈到优化问题，我们往往需要知道优化的约束和优化的目标。

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

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

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

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

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

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

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

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

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

二、基于场景选择的决策模块

决 策 模 块相当于无人驾驶系统的大脑，保障无人车的行车安全，同时也要理解和遵守交通规 则。 为了实现这样的功能，决策模块为无人车提供了各种的限制信息包括：1) 路径的长度以及左右边界限制；2) 路径上的速度限制；3) 时间上的位置限制。

此外，决策模块几乎利用了所有和无人驾驶相关的环境信息，包括: 1) Routing 信息；2) 道路结构，比如当前车道，相邻车道，汇入车道，路口等信息； 3) 交通信号和标示，比如红绿灯，人行横道，Stop Sign，Keep Clear 等； 4) 障碍物状态信息，比如障碍物类型，大小，和速度；5) 障碍物预测信息，比如障碍物未来可能的运动轨迹。

正是因为决策模块处理了所有上层业务逻辑信息，并输出了抽象的限制信息，我们保证了下 游路径和速度优化的抽象性，并完全和上层的地图，感知，预测的解耦。

2.1 为什么需要决策模块？

答案： 决策使轨迹规划化繁为简。既然轨迹规划是非凸优化问题，我们需要利用决策模块来解决这个问题。

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

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

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

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

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

2.2 纵向规划决策求解：动态规划

在纵向规划决策上，我想介绍一种以动态规划算法作为决策的方法。

看看下图的案例，假设有两个人正在横穿马路，我们来研究一下如何决策才是最优的选择。

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

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

注意，这种方法并不能保证最后的速度是否舒适，它给出的是一个大概的决策方法，即到底让或者不让。

2.3 决策面临的挑战

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

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

以上图左侧的场景为例，一辆正在行驶的自动驾驶车辆打算绕行前面白车，但此时前车突然起步了，我们该如何继续行驶？是变道？还是跟在后边行驶，又或是继续绕行？处理这种情况确实依靠人类驾驶的经验。再看上图右侧的例子：自动驾驶车准备在前方左转，但是前车停了很久也没有启动（可能前车驾驶员没反应过来），我们这时该不该变道呢？对这种情况，人类司机有时也很难判断。由此可见，场景的多变而复杂使得决策面临很多挑战。

2.3 Apollo3.5中分场景决策规划

关于“场景”的概念是在 Apollo3.5 中首次提出，一个场景既可以是地图中的一个静态路段， 比如十字路口；也可以是无人车想要完成的一个动态目标，比如借道避让(Side Pass)。 依据场景来做决策和规划有以下两个优点：1)场景之间互不干扰，有利于并行开发和独立调参； 2)在一个场景里我们可以实现一系列的有时序或者依赖关系的复杂任务。

这样的场景架构也有利于开发者贡献自己的特有场景，并不影响其他开发者需要的功能。 同时场景架构也带来了一些难点: 首先，场景的通用部分会带来代码的冗余，不便于代码的升级维护；其次，场景的划分，尤其是保证场景之间没有功能上的覆盖也很困难； 最后，有了多个场景之后，需要额外的逻辑来进行场景识别以及处理场景之间的转换。

场景的划分其实没有特别严格的规定，同时这也取决于自动驾驶的应用场景，比如送货小车 和高速卡车在场景的划分上肯定不太一样。上图中，给出了 Apollo 场景分类的一个例子， 来尽量保证每个场景之间相对独立 我们把场景分为两大类，Lane Follow 和 Lane Breaking: Lane Follow 场景下主车沿一条车道驾驶，该车道前方一定距离内没有其他车道与其交汇或 者穿越;并且主车也没有切换到其他车道的意图。非 Lane follow 的其他场景被分类为 Lane Breaking。 在 Lane Breaking 下，我们又细分为三个小类: Intersection 是所有的路口场景; Active Pass 包括了所有临时借道的场景; Lane Switch 包括了所有换道的场景(包括车道合并)。

有了场景的分类之后，我们就可以对场景进行识别和转换。对于选择哪个场景，我们采用了 两层的识别机制。 首先，每一个场景自己会根据当前的环境信息，确定是否属于自己的场景，并返回该信息给 场景管理器，场景管理器再统一进行选择，以保证场景的选择最优。 场景的退出也由每个场景自己决定，比如场景运行完成或者内部出错。一旦进入一个场景， 该场景会有较高优先级来完成。

用借道避让场景的一个实现，来说明场景是如何实现的。 在这个场景中，我们有 6 个 Stage(阶段)，每个 Stage 完成借道避让的一个步骤，类似于有 限状态机中的一个状态。主要步骤/状态有一定的时序依赖关系，如果在一个 Stage(阶段) 中发现环境变化了，或者出现错误，会出现 Stage 之间的跳转或者退出该场景。

在每一个 Stage(阶段)中，都要实现上图中的功能，包括交规决策、路径决策、路径优化、 速度决策、速度优化。 我们把每个功能定义为一个或者几个基本的 Task(任务)，每个 Stage(阶段)或者直接调 用(使用默认参数)，或者修改参数，或者修改输入值。这样的实现可以极大的提高场景之 间的代码复用。

三、横纵向轨迹规划

3.1 横向规划

（1）横向规划的解法

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

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

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

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

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

参考路径(Reference Line) 在 Apollo 决策中起着非常重要和关键的作用。 首先，参考路径是在没有障碍物路况下的默认行车路径。 其次，后续的交规决策，路径决策，和速度决策都是基于参考路径或者参考路径下的 Frenet Frame 完成的。 最后，参考路径也用于表达换道的需求，一般换道时会有两条参考路径，分别有不同的优先级。其中，优先级高的路径表示目标路径，优先级低的路径为当前路径(如图所示)。参考路径的获取可以有多种方式。 在 Apollo 里，参考路径的计算是根据 routing 的线路，找到相应的高精地图中的道路中心线，然后进行平滑。 有了参考路径之后，我们会沿着该路径找到所有在该路径上的交通标示和交通信号灯，并根据一系列的交通规则来决定是否在需要停止在交通标示或者交通信号灯的停止线前。

确定参考线后，通过把参考线离散化，采一些点出来，那么横向规划问题就转化为求解一个离参考线偏移距离的一个问题，即转化成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的二次的形式，所以可以使用二次规划的方法来解决。

（2）横向规划的挑战

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

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

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