自动驾驶路径规划-Lattice Planner算法

本文整合了网络上关于百度 算法的详细介绍和相关Q&A

大家好，今天很高兴能给大家分享 3.0新发布的规划算法。

算法隶属于规划模块。规划模块以预测模块、模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。我们可以看到，规划模块在中是一个承上启下的重要模块。

Lattice算法隶属于规划模块。规划模块以预测模块、routing模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。我们可以看到，规划模块在Apollo中是一个承上启下的重要模块。

一个合格规划算法，必须满足几个条件。首先，必须能够使自动驾驶汽车到达目的地；其次，必须符合交规；第三，能够避免碰撞；最后，也需要能保证一定的舒适性。

在Apollo中，规划算法的输出是一系列轨迹点连成的轨迹。每一个轨迹点包含位置，速度，加速的等信息。

下面我来介绍一下规划算法的工作流程。我们以右图中的场景为例。其中红车是我们的自动驾驶汽车，蓝车是其他障碍车，前面蓝色带尖头的曲线是蓝车的预测轨迹。那么这是一个前方即将有车辆并入的场景。

面对这样的场景，有些司机会按照右图中浅红色的轨迹，选择绕开蓝色的障碍车。另外有一些司机开车相对保守，会沿着右图中深红色较短的轨迹做一个减速，给蓝色障碍车让路。

既然对于同一个场景，人类司机会有多种处理方法，那么规划算法的第一步就是采样足够多的轨迹，提供尽可能多的选择

Lattice规划算法的第二步是计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。我会在后面为大家详细介绍。

那么有了轨迹的cost以后，第三步就是一个循环检测的过程。在这个过程中，我们每次会先挑选出cost最低的轨迹，对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条cost最低的轨迹。

以右图为例，假设我们首先挑选出cost最低的是深红色较短的轨迹。但我们发现即便猛踩刹车也无法执行这条轨迹。也就是说，这条轨迹超出了汽车的减速度上限。那么它就无法通过物理限制检测，我们会将其筛除。

假设我们下一条选出来cost最低的轨迹是右图中深红色较长的轨迹。我们会发现若沿着这条轨迹前进，红车会和蓝色障碍车发生碰撞。也就是说，这条轨迹轨迹无法通过碰撞检测。于是只能放弃这条轨迹，考虑下一条cost最低的。

这样的过程循环继续下去，假设我们现在挑选出右图中靠左边的深红色轨迹，它既符合汽车的物理性状，也不会有碰撞风险。

我们最终就将这条轨迹作为规划轨迹输出。

那么下面我们对每一个步骤，进行详细的说介绍。

首先是采样过程。在正式介绍采样过程之前，作为铺垫，我先来介绍一下Frenet坐标系。在二维平面中，我们通常采用X-Y坐标系来描述问题。但在自动驾驶规划问题中，我们的工作是基于道路的。这种情况下，X-Y坐标系并不是最方便的。所以我们这里需要使用基于车道线横向和纵向的Frenet坐标系。

那么如何用坐标系来表示一辆汽车的状态呢？首先我们有一条光滑的参考线（右图中红线），我们可以按右图所示将汽车的坐标点投影到参考线上，得到一个参考线上的投影点（图中蓝色点）。从参考线起点到投影点的路径长度就是汽车在坐标系下的纵向偏移量，用S表示。而投影点到汽车位置的距离则是汽车在坐标系下的横向偏移量，用L表示。因为参考线是足够光滑的，我们也可通过汽车的朝向、速度、加速度来计算出坐标系下，横向和纵向偏移量的一阶导和二阶导。

这里需要注意的是，我们将横向偏移量L设计成纵向偏移量S的函数。这是因为对于大多数的汽车而言，横向运动是由纵向运动诱发的。

有了坐标系的概念，我们下面来介绍一下如何生成一条轨迹。首先我们可以通过计算得到自动驾驶汽车在坐标系下的在零时刻的起始状态，也就是汽车的当前状态。为了生成一条轨迹，第一步就是在坐标系下采样一个在T1时刻的末状态。

第二步就是将末状态和起始状态做多项式拟合。分别形成横向和纵向的多项式轨迹。

有了横向轨迹和纵向轨迹之后，第三步就是二维合成。给定一个时刻T*，我们可以计算出在T*时刻的纵向偏移量和横向偏移量，再通过参考线，即可还原成一个二维平面中的轨迹点。通过一系列的时间点T0，T1，...，Tn，可以获得一系列的轨迹点P0，P1，…，Pn，最终形成一条完整的轨迹。了解了如何生成一条轨迹之后，下面我来介绍一下如何采样一系列轨迹。

首先介绍如何采样横向轨迹。横向轨迹的采样需要涵盖多种横向运动状态。现在Apollo的代码中设计了三个末状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80。用两层循环遍历各种组合，再通过多项式拟合，即可获得一系列的横向轨迹。

对于纵向轨迹的采样，我们需要考虑巡航、跟车或超车、停车这三种状态。

在停车状态中，给定停车点，末状态的速度和加速度都是零，所以末状态是确定的。
那么我们只需用一层循环来采样到达停车点的时间即可。

在介绍跟车/超车的采样逻辑之前，我们需要介绍一下S-T图的概念。以左图中的场景为例，蓝色障碍车从车道右侧切入，在T_in时刻开始进入当前车道。那么这个场景对应的S-T图就如右图所示。从T_in时刻开始出现一块斜向上的阴影区域。这块阴影区域的高度就是蓝色障碍车的车身长，上边界表示车头，下边界表示车尾，斜率表示车速。

如果上述场景变成这样，障碍车从T_in时刻进入车道，然后在T_out时刻离开车道。那么这个场景对应的S-T图就会缩短。

有了S-T图的概念，我们观察左图中的两条规划轨迹。红色的是一条跟车轨迹，绿色的是超车轨迹。这两条轨迹反映在S-T图中，就如右图所示。红色的跟车轨迹在蓝色阴影区域下方，绿色的超车轨迹在蓝色阴影区域上方。

我们采样末状态时，就可以分别在S-T图中障碍物对应的阴影区域的上方和下方分别采样。上方的末状态对应超车，下方的末状态对应跟车。

如果有多个障碍物，我们就对这些障碍物分别采样超车和跟车所对应的末状态。

那么总结下来就是遍历所有和车道有关联的障碍物，对他们分别采样超车和跟车的末状态，然后用多项式拟合即可获得一系列纵向轨迹。

我们将三组纵向轨迹组合起来，就可以获得所有纵向轨迹。再将所有纵向轨迹和所有横向轨迹两两配对二维合成，就可以完成轨迹采样的工作。

现在我们来介绍一下轨迹的cost的实现方法。我们前面提到，轨迹规划所需要满足的四点要求，分别是到达目的、符合交规，避免碰撞、平稳舒适。针对这四点要求，我们设计了六个cost，cost越高就表示越不满足要求。下面我们一一介绍这六个cost的设计思路。

首先是到达目的的cost。这里分成两种情况，一个是存在停车指令（比如红灯）的情况，另一个是没有停车指令的。如果存在停车指令，相对大的车速，其对应的轨迹cost就越大；如果没有停车指令，那么低速轨迹的cost就会越大。

怎么实现这样的效果呢？我们针对这两种情况分别设计了参考速度。左图蓝线表示没有停车指令时的参考速度。

我们可以看到这种情况下，绿色的加速轨迹会获得一个较小的cost，而红色的减速轨迹会获得一个相对较大的cost。那么如果存在停车指令，参考速度就会想右图中的蓝色曲线一样呈下降趋势。那么这种情况下，同样的两条轨迹，他们的cost大小关系就会正好相反。

第二个cost是横向偏移cost。设计这个cost是为了让自动驾驶汽车能尽量沿着道路中心行驶。那么像左图汽车靠道路一边行驶，和中图画龙的行驶轨迹，他们的cost都相对较高。

第三个cost是碰撞cost。左图中的两条轨迹，反映在右图S-T图中，我们可以发现红色的轨迹和蓝色障碍车在S-T图中的阴影区域有重叠，说明有碰撞风险，那么它的碰撞cost就会相对较高。而绿色的轨迹在S-T图中反映出来的碰撞风险较小，那么它的碰撞cost就相对较低。

第四个cost是纵向加加速度的cost。加加速度（jerk）是加速度对时间的导数，表示加速度的变化率。我们用加加速度的最大值值来表示这个cost。

第五个cost是横向加速度的cost。设计这个cost是为了平稳地换道。那么像左图猛打方向盘的轨迹，它的横向加速度cost就会相对较大。

最后一个cost是向心加速度cost。设计这个cost是为了在转弯或调头的时候能够减速慢行。在弯道处，车速慢的轨迹，其向心加速度cost就会相对较低，那么就会更容易被率先挑选出来。

这六个cost的加权求和就是轨迹的总cost。开发者可以根据产品的需要，调试这六个权重。

这里介绍一下限制检测和碰撞检测。限制检测考察的内容有轨迹的加速度、加加速度、和曲率。碰撞检测则是把自动驾驶汽车的轨迹和其他障碍物的预测轨迹进行比对，观察是否有轨迹重叠。

对于换道场景，算法仅仅需要对目标车道对应的参考线做一次采样+选择的流程。本车道和目标车道均能产生一条最优轨迹。给换道轨迹的cost上增加额外的车道优先级的cost，再将两条轨迹比较，选择cost较小的那条即可。

Lattice规划算法已经在新石器无人驾驶微型物流车和长沙智能驾驶研究院重卡中落地。我们也将继续深度合作，提高技术的同时，拓展更多产品的应用。
Apollo问答 | 关于Lattice Planner规划算法的若干问答
1、Q:Lattice Planner将规划统一成代价函数，寻找代价最小的。在规划的上层是否还需要决策层？
A: 在规划上层的决策仅仅包含了来自交规的停车指令（比如红绿灯），其余的策略均有下层采样+cost来完成。
2、Q:Lattice Planner适用于哪些场景？
A: Lattice Planner现在已经在低速园区和高速公路的场景中由产品落地。对于普通城市道路，对于相对复杂的交规处理还有待完善。
3、Q:这个算法适合多弯道复杂的场景下吗？复杂的停车场等？
A: 该算法可以处理多弯道的场景。对于停车场暂不使用，因为这个算法首先需要参考线，而复杂的停车场很难做出一条参考线。
4、Q:Cost里面已经考虑了碰撞，为什么还要做碰撞检测
A: Cost里面的碰撞仅仅是把有碰撞风险的轨迹的cost值设置的比较高，为了把这样的轨迹优先级排到比较后，从而使得我们能够优先考察其他更安全的轨迹，但它并没有起到删选轨迹的作用，是一个比较soft的限制。而后面的碰撞检测是出于安全的考虑，把这条轨迹筛除，这是一个hard的限制。
5、Q:Lattice Planner和EM Planner的区别是？或者说分别应用在什么场景下？
A: Lattice Planner主要基于采样+选择，而EM Planner的思路是逐层优化迭代。从规划层面来说，两者均可用于各种场景。从决策层来看，Lattice的决策相对简单，适用于相对简单的场景，如低速园区，高速公路。EM算法对交规的决策做的相对更完善，可以处理相对复杂的普通城市道路。
6、Q:高横向轨迹和纵向轨迹俩俩组合咋样理解?是横向的一条轨迹和纵向的所有轨迹组合吗？
A: 两两组合指的是每一条横向轨迹和每一条纵向轨迹的组合。
7、Q:计算量是不是有点大？普通CPU可以吗？
A: 以目前的经验来看，普通CPU是可以处理的。当然，这个算法可以随着计算机性能的提升，采样更多的轨迹，使得我们对解空间的涵盖更加完备。
8、Q:关于轨迹的生成，Lattice使用的是多项式拟合，一般使用三项还是五项？另外，老师说是在st图上下阴影处取样，那是取多少个点？那个末状态是指超车结束的状态点吗？
A: 对于横向轨迹的多项式拟合，因为有有初始状态的零阶导，一阶导，二阶导，和末状态的零阶导，一阶导，二阶导，一共六个变量，所以拟合五次多项式。对于纵向轨迹，在停车和跟车状态，也都是五次多项式，但对于巡航状态，由于我们不需要确定末状态的S值，所以只有五个变量，那么用四次多项式就可以了。在ST图上下取样，点的数目可以由开发者自行决定，这个没有限制。末状态指的就是超车结束的状态点。
9、Q:只有起始状态和末状态怎么进行横向和纵向的拟合？
A: 构建多项式，使得多项式在起始时刻和末时刻的各阶导数和起始状态末状态吻合。
10、Q:Lattice Planner是路径规划的算法，但也涵盖了部分行为规划的处理内容？
A: 是的。横向轨迹主要针对路径，纵向轨迹主要针对速度、加速度等行为。
11、Q:多项式拟合，是什么多项式？拟合后如何保证满足无人车的运动学和动力学要求？
A: 多项式指的就是普通的多项式，拟合时已经通过高阶导数考虑了动力学要求。
12、Q:Apollo的代码中出现了S-L坐标系和frenet坐标系的名称，这两个是否是指同一个坐标系？
A: 这两个指的是同一个坐标系。
13、Q:高精度地图的输出格式是怎样的？包括用于导航的矢量图、用于定位的点云图以及全局的路径。对于高精度地图这一块希望能够有详细的介绍。
A:可以参考
https://github.com/ApolloAuto/apollo/blob/master/modules/map/data/README.md 如 README 所示，xml, bin, txt, lb1 都是不同的文件格式，适配不同的读取器，内容是一致的。
14、Q:多项式拟合具体方法是什么？如何避免拟合的曲线超出路面范围？
A: 拟合的具体方法是求解多项式系数的线性方程组。开发者可以通过添加在路检测的检查validity check来避免曲线超出路面范围。
15、Q:换道场景，是要提前获取目标车道和当前行驶车道的参考线吗？
A: 是的，这个信息在Apollo里都是有的。
16、Q:Frenet坐标系下面是一个车道一个中心线？还是一条线一个中心线？这两的s值的0点是从什么地方开始算的？
A: 一个车道一个中心线。S值的0点是从参考线的起点开始。参考线的起点会随着主车的位置做实时动态调整，通常是在主车车身后30米左右的位置。
17、Q:在障碍车辆较多的环境下可能需要频繁的规划路径，由于cost值有多个评价组成，有可能多次出现最佳轨迹的横向方向完全相反的情况，可能造成车辆左右微微摆动，如何解决这种情况？
A: 在这种情况下，建议额外补充一个和上一个计算周期相似性的cost。
18、Q:对于单向双车道场景，规划的时候是只规划行驶车道还是规划双车道?
A: 如果有换道需求的话，会同时规划两条车道。
19、Q:初始状态本车的theta是车头朝向还是指车速朝向， 初始状态曲率怎么计算，用方向盘角度推？
A: 初始状态的theta是车头方向。曲率是基于IMU的信息计算出来的。
20、Q:在运行Apollo demo时 ，在只有软件模拟没有硬件的情况，能不能测试和调试规划算法，能的话怎么做？
A: Apollo有一个开放的仿真平台，azure.apollo.auto，开发者可以在仿真平台中调试规划算法。
21、Q:多项式具体形式如何？拟合拟合后如何保证满足无人车的运动学和动力学要求？
A: 多项式就是普通的幂次多项式。拟合出来以后，可以通过限制检测来筛选出符合无人车动力学模型的轨迹。
22、Q:Lattice算法的输入也是referenceLine吧，这个是通过routing模块给出的route segment生成的，和EM Planner一样，理解正确么？
A: 正确。
23、Q:有碰撞风险的轨迹为什么不在cost环节或者之前直接删掉？
A: Cost环节仅仅是一个soft的排序，并不做删除的工作。
24、Q:如果如果在过弯道的时候，reference line上的end point有噪音，在不停都抖动，如何在这种情况下规划一条稳定的轨迹，谢谢。
A: 这就需要我们不断优化reference line的平滑算法。
25、Q:现在Apollo的代码中设计了三个末状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80。能解释下为什么这样定意思这些常量？
A: 这些常量是根据平时路测的经验得到的。开发者可以根据自己的产品和场景来调整这些常量。
26、Q:只有起始状态和末状态怎么进行横向和纵向的拟合？
A: 通过求解多项式的系数是的起始点和终止点的各阶导数吻合。
27、Q:关于轨迹的生成，Lattice使用的是多项式拟合，一般使用三项还是五项？另外，老师说是在st图上下阴影处取样，那是取多少个点？那个末状态是指超车结束的状态点吗？
A: 一般是5阶多项式，在cruise状态下，用4阶多项式。在st图中的取点个数没有规定，开发者可以根据需要来调试。
28、Q：
 
图中确定了纵向拟合曲线之后，如何确定横向的拟合曲线？
A、横向轨迹在第18页ppt中有介绍。
29、Q:轨迹采样时纵向位移选取10 20 40 80 ，这是经验值吗？是不是需要根据车速进行调整？
A: 这是经验值。我们也正在改进，使得它能根据车速调整。
30、Q:规划轨迹中的速度如何设定，是否根据起始点速度、终点速度、以及轨迹距离，计算出一个平稳加速度，然后平稳地加速过去？但是我看PPT中跟车的场景，到达reference speed的过程加速度似乎并非为一个常量。
A: 轨迹的平稳性可以通过cost来筛选。所以就要求我们在sample的过程中涵盖面尽可能的广泛。
31、Q:横向运动是由纵向运动诱发的，该如何理解？
A: 普通的车子的轮胎不会转到水平位置来做单纯的横向运动。