对于纵向轨迹的采样，我们需要考虑巡航、跟车或超车、停车这三种状态。
作者 | 许珂诚
编辑 | Natalie
大家好，我是来自百度智能驾驶事业群的许珂诚。今天很高兴能给大家分享 Apollo 3.0 新发布的 Lattice 规划算法。
本次分享将会从以下五个方面展开：
一、什么是 Lattice Planner
二、Lattice 规划算法的工作流程
三、Lattice Planner 的采样过程
四、如何采样横向和纵向轨迹
五、轨迹 COST 的实现方法
一、什么是 Lattice Planner
Lattice 算法隶属于规划模块。规划模块以预测模块、Routing 模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。我们可以看到，规划模块在 Apollo 中是一个承上启下的重要模块。
这是 Apollo 中规划模块的工作流程。首先依据 Routing 和定位，通过平滑算法，生成一条平滑的参考线（平滑的道路中心线）。再通过规划算法，生成一条符合交规、安全舒适的规划轨迹。Lattice 算法就是 Apollo 开源平台中的一种规划算法。
一个合格的规划算法，必须满足几个条件。首先，必须能够使自动驾驶汽车到达目的地；其次，必须符合交规；第三，能够避免碰撞；最后，也需要能保证一定的舒适性。在 Apollo 中，规划算法的输出是一系列轨迹点连成的轨迹。每一个轨迹点包含位置、速度、加速度等信息。
二、Lattice 规划算法的工作流程
下面我来介绍一下 Lattice 规划算法的工作流程。我们以右图中的场景为例。其中红车是我们的自动驾驶汽车，蓝车是其他障碍车，前面蓝色带尖头的曲线是蓝车的预测轨迹。那么这是一个前方即将有车辆并入的场景。
面对这样的场景，有些司机会按照右图中浅红色的轨迹，选择绕开蓝色的障碍车。另外有一些司机开车相对保守，会沿着右图中深红色较短的轨迹做一个减速，给蓝色障碍车让路。
既然对于同一个场景，人类司机会有多种处理方法，那么 Lattice 规划算法的第一步就是采样足够多的轨迹，提供尽可能多的选择。
Lattice 规划算法的第二步是计算每一条轨迹计算的 cost。这个 cost 考虑了轨迹的可行性、安全性等因素。我会在后面为大家详细介绍。
有了轨迹的 cost 以后，第三步就是一个循环检测的过程。在这个过程中，我们每次会先挑选出 cost 最低的轨迹，对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条 cost 最低的轨迹。
以右图为例，假设我们首先挑选出 cost 最低的是深红色较短的轨迹。但我们发现即便猛踩刹车也无法执行这条轨迹。也就是说，这条轨迹超出了汽车的减速度上限。那么它就无法通过物理限制检测，我们会将其筛除。
假设我们下一条选出来 cost 最低的轨迹是右图中深红色较长的轨迹。我们会发现若沿着这条轨迹前进，红车会和蓝色障碍车发生碰撞。也就是说，这条轨迹轨迹无法通过碰撞检测。于是只能放弃这条轨迹，考虑下一条 cost 最低的。
这样的过程循环继续下去，假设我们现在挑选出右图中靠左边的深红色轨迹，它既符合汽车的物理性状，也不会有碰撞风险。
我们最终就将这条轨迹作为规划轨迹输出。那么下面我们对每一个步骤进行详细介绍。
三、Lattice Planner 的采样过程
首先是采样过程。在正式介绍采样过程之前，作为铺垫，我先来介绍一下 Frenet 坐标系。在二维平面中，我们通常采用 X-Y 坐标系来描述问题。但在自动驾驶规划问题中，我们的工作是基于道路的。这种情况下，X-Y 坐标系并不是最方便的。所以我们这里需要使用基于车道线横向和纵向的 Frenet 坐标系。
那么如何用 Frenet 坐标系来表示一辆汽车的状态呢？首先我们有一条光滑的参考线（右图中红线），我们可以按右图所示将汽车的坐标点投影到参考线上，得到一个参考线上的投影点（图中蓝色点）。从参考线起点到投影点的路径长度就是汽车在 Frenet 坐标系下的纵向偏移量，用 S 表示。而投影点到汽车位置的距离则是汽车在 Frenet 坐标系下的横向偏移量，用 L 表示。因为参考线是足够光滑的，我们也可通过汽车的朝向、速度、加速度来计算出 Frenet 坐标系下，横向和纵向偏移量的一阶导和二阶导。
这里需要注意的是，我们将横向偏移量 L 设计成纵向偏移量 S 的函数。这是因为对于大多数的汽车而言，横向运动是由纵向运动诱发的。
有了 Frenet 坐标系的概念，我们下面来介绍一下如何生成一条轨迹。首先我们可以通过计算得到自动驾驶汽车在 Frenet 坐标系下零时刻的起始状态，也就是汽车的当前状态。为了生成一条轨迹，第一步就是在 Frenet 坐标系下采样一个在 T1 时刻的末状态。
第二步就是将末状态和起始状态做多项式拟合，分别形成横向和纵向的多项式轨迹。
有了横向轨迹和纵向轨迹之后，第三步就是二维合成。给定一个时刻 T*，我们可以计算出在 T* 时刻的纵向偏移量和横向偏移量，再通过参考线，即可还原成一个二维平面中的轨迹点。通过一系列的时间点 T0，T1，...，Tn，可以获得一系列的轨迹点 P0，P1，…，Pn，最终形成一条完整的轨迹。了解了如何生成一条轨迹之后，下面我来介绍一下如何采样一系列轨迹。
四、如何采样横向和纵向轨迹
首先介绍如何采样横向轨迹。横向轨迹的采样需要涵盖多种横向运动状态。现在 Apollo 的代码中设计了三个末状态横向偏移量，-0.5，0.0 和 0.5，以及四个到达这些横向偏移量的纵向位移，分别为 10，20，40，80。用两层循环遍历各种组合，再通过多项式拟合，即可获得一系列的横向轨迹。
对于纵向轨迹的采样，我们需要考虑巡航、跟车或超车、停车这三种状态。
对于巡航状态，我们通过两层循环来完成采样。外层循环将速度从零到上限值按等间隔均匀遍历。内层循环遍历到达末状态速度的时间，我们从 1 秒到 8 秒按 1 秒的间隔均匀遍历。由于巡航状态不需要指明到达末状态的 S 值，所以这里只需要用四次多项式拟合即可。
在停车状态中，给定停车点，末状态的速度和加速度都是零，所以末状态是确定的。那么我们只需用一层循环来采样到达停车点的时间即可。
在介绍跟车 / 超车的采样逻辑之前，我们需要介绍一下 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 是碰撞 cost。左图中的两条轨迹，反映在右图 S-T 图中，我们可以发现红色的轨迹和蓝色障碍车在 S-T 图中的阴影区域有重叠，说明有碰撞风险，那么它的碰撞 cost 就会相对较高。而绿色的轨迹在 S-T 图中反映出来的碰撞风险较小，那么它的碰撞 cost 就相对较低。
第四个 cost 是纵向加加速度的 cost。加加速度（jerk）是加速度对时间的导数，表示加速度的变化率。我们用加加速度的最大值值来表示这个 cost。
第五个 cost 是横向加速度的 cost。设计这个 cost 是为了平稳地换道。像左图猛打方向盘的轨迹，它的横向加速度 cost 就会相对较大。
最后一个 cost 是向心加速度 cost。设计这个 cost 是为了在转弯或调头的时候能够减速慢行。在弯道处，车速慢的轨迹，其向心加速度 cost 就会相对较低，就会更容易被率先挑选出来。
这六个 cost 的加权求和就是轨迹的总 cost。开发者可以根据产品的需要，调试这六个权重。
这里介绍一下限制检测和碰撞检测。限制检测考察的内容有轨迹的加速度、加加速度和曲率。碰撞检测则是把自动驾驶汽车的轨迹和其他障碍物的预测轨迹进行比对，观察是否有轨迹重叠。
对于换道场景，Lattice 算法仅仅需要对目标车道对应的参考线做一次采样选择的流程。本车道和目标车道均能产生一条最优轨迹。给换道轨迹的 cost 上增加额外的车道优先级的 cost，再将两条轨迹比较，选择 cost 较小的那条即可。
以上就是 Lattice Planner 规划算法的介绍和分享。非常感谢大家的参加！也欢迎大家提出问题，进行交流。更多 Apollo 相关的技术干货也可以继续关注后续的社群分享。相关学习资料和自动驾驶相关技术内容，大家可以关注【Apollo 开发者社区】的微信公众号来获取，也可以在 Apollo GitHub 上提出技术问题与我们互动，欢迎大家沟通交流！
Q & A
Q1：无论是 EM Planner 还是 Lattice Planner，构造 sl 坐标系的 s 是怎样得来的？正常来说参考线平滑后应该重构 s 的，但是没找到 s 计算方式的由来。
A1: EM Planner 和 Lattice Planner 中的 S 值都是从参考线的起点开始算的。具体的相关代码在 apollo/modules/planning，frame.cc 和 reference_line_info.cc 这两个文件中。
Q2：Lattice Planner 规划的目标点如何确定？在纵向 S 规划多远与时距有关吗？
A2：规划的目标点通过采样多种情况，然后由 cost 函数来选择确定。在纵向 S 规划多远，主要取决于自动驾驶场景和本身的车速。
Q3：按照 Lattice 的算法，我们规划出来的曲线是时间 - 速度曲线，要怎么得到位置 - 速度曲线呢？
A3：Lattice 规划出来的曲线是“时间 - 多项式”曲线。多项式的值（零阶导）对应位置，多项式的一阶导对应速度，二阶导对应加速度。
Q4：今天的规划算法中包括速度规划算法吗，能否简单介绍下常见场景如过弯道 / 变道 / 超车的速度规划逻辑？
A4：Lattice 算法对速度的规划已经融合在了横向和纵向的多项式曲线中。他们各自的一阶导组合起来就是速度的规划。在弯道中，我们通过向心加速度 cost 来实现过弯减速；变道过程，我们基于目标车道的参考线来采样轨迹，实现换道；对某一个障碍物超车还是跟车，我们会把这两种策略都进行采样，然后由 cost 进行选择。
Q5：请问下这个规划依赖于导航吗？
A5： 该规划算法依赖于高精定位和高精地图。
Q6：Lattice Planner 将规划统一成代价函数，寻找代价最小的。在规划的上层是否还需要决策层？
A6：在规划上层的决策仅仅包含了来自交规的停车指令（比如红绿灯），其余的策略均由下层采样 +cost 来完成。
Q7：这个算法适合多弯道复杂的场景吗？或者复杂的停车场等？
A7：该算法可以处理多弯道的场景。对于停车场暂不使用，因为这个算法首先需要参考线，而复杂的停车场很难做出一条参考线。
Q8：cost 里面已经考虑了碰撞，为什么还要做碰撞检测？
A8：cost 里面的碰撞仅仅是把有碰撞风险的轨迹的 cost 值设置得比较高，为了把这样的轨迹优先级排到比较后，从而使得我们能够优先考察其他更安全的轨迹，但它并没有起到删选轨迹的作用，是一个比较 soft 的限制。而后面的碰撞检测是出于安全的考虑，把这条轨迹筛除，这是一个 hard 的限制。
Q9：Lattice Planner 和 EM Planner 的区别是什么？或者说分别应用在什么场景下？
A9：Lattice Planner 主要基于采样 + 选择，而 EM Planner 的思路是逐层优化迭代。从规划层面来说，两者均可用于各种场景。从决策层来看，Lattice 的决策相对简单，适用于相对简单的场景，如低速园区、高速公路。EM 算法对交规的决策做的相对更完善，可以处理相对复杂的普通城市道路。
Q10：横向轨迹和纵向轨迹两两组合怎么理解? 是横向的一条轨迹和纵向的所有轨迹组合吗？
A10：两两组合指的是每一条横向轨迹和每一条纵向轨迹的组合。
Q11：这样做，计算量是不是有点大？普通 CPU 可以吗？
A11：以目前的经验来看，普通 CPU 是可以处理的。当然，这个算法可以随着计算机性能的提升，采样更多的轨迹，使得我们对解空间的涵盖更加完备。
匿名用户
