l———S，S———T
规划模块在apollo中承上启下，Lattice算法隶属于规划模块。规划模块以预测模块、routing模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。
图片.png
1.规划处理流程：根据定位和路由模块，经过平滑生成一条平滑的参考线（道路中心线），再经过规划算法生成一条平稳、舒适、安全的轨迹。
图片.png
规划算法需要，能到达目的地、遵守交规、避障、舒适。生成的轨迹包含位置、速度、加速度信息。
图片.png
2.lattice规划算法流程
图片.png
以上图中的场景为例。其中红车是我们的自动驾驶汽车，蓝车是其他障碍车，前面蓝色带尖头的曲线是蓝车的预测轨迹。那么这是一个前方即将有车辆并入的场景。面对这样的场景，有些司机会按照右图中浅红色的轨迹，选择绕开蓝色的障碍车。另外有一些司机开车相对保守，会沿着右图中深红色较短的轨迹做一个减速，给蓝色障碍车让路。
既然对于同一个场景，人类司机会有多种处理方法，那么Lattice规划算法的第一步就是采样足够多的轨迹，提供尽可能多的选择。
图片.png
Lattice规划算法的第二步是计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。
3.采样过程
图片.png
那么如何用Frenet坐标系来表示一辆汽车的状态呢？首先有一条光滑的参考线（右图中红线），可以按右图所示将汽车的坐标点投影到参考线上，得到一个参考线上的投影点（图中蓝色点）。从参考线起点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用S表示。而投影点到汽车位置的距离则是汽车在Frenet坐标系下的横向偏移量，用L表示。因为参考线是足够光滑的，也可通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。
将横向偏移量L设计成纵向偏移量S的函数。这是因为对于大多数的汽车而言，横向运动是由纵向运动诱发的。
有了Frenet坐标系的概念，下面来介绍一下如何生成一条轨迹。首先我们可以通过计算得到自动驾驶汽车在Frenet坐标系下的在零时刻的起始状态，也就是汽车的当前状态。为了生成一条轨迹，第一步就是在Frenet坐标系下采样一个在T1时刻的末状态。
图片.png
第二步就是将末状态和起始状态做多项式拟合。分别形成横向和纵向的多项式轨迹。
图片.png
有了横向轨迹和纵向轨迹之后，第三步就是二维合成。给定一个时刻T，可以计算出在T时刻的纵向偏移量和横向偏移量，再通过参考线，即可还原成一个二维平面中的轨迹点。通过一系列的时间点T0，T1，...，Tn，可以获得一系列的轨迹点P0，P1，…，Pn，最终形成一条完整的轨迹。了解了如何生成一条轨迹之后，下面介绍一下如何采样一系列轨迹。
图片.png
4.如何采样横向和纵向轨迹
首先介绍如何采样横向轨迹。横向轨迹的采样需要涵盖多种横向运动状态。现在Apollo的代码中设计了三个末状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80。用两层循环遍历各种组合，再通过多项式拟合，即可获得一系列的横向轨迹。
图片.png
对于纵向轨迹的采样，需要考虑巡航、跟车或超车、停车这三种状态
图片.png
对于巡航状态，通过两层循环来完成采样。外层循环将速度从零到上限值按等间隔均匀遍历。内层循环遍历到达末状态速度的时间，从1秒到8秒按1秒的间隔均匀遍历。由于巡航状态不需要指明到达末状态的S值，所以这里只需要用四次多项式拟合即可。
图片.png
在停车状态中，给定停车点，末状态的速度和加速度都是零，所以末状态是确定的。那么我们只需用一层循环来采样到达停车点的时间即可。
在介绍跟车/超车的采样逻辑之前，需要介绍一下S-T图的概念。以左图中的场景为例，蓝色障碍车从车道右侧切入，在T_in时刻开始进入当前车道。那么这个场景对应的S-T图就如右图所示。从T_in时刻开始出现一块斜向上的阴影区域。这块阴影区域的高度就是蓝色障碍车的车身长，上边界表示车头，下边界表示车尾，斜率表示车速。
图片.png
如果上述场景变成这样，障碍车从T_in时刻进入车道，然后在T_out时刻离开车道。那么这个场景对应的S-T图就会缩短。
图片.png
有了S-T图的概念，观察左图中的两条规划轨迹。红色的是一条跟车轨迹，绿色的是超车轨迹。这两条轨迹反映在S-T图中，就如右图所示。红色的跟车轨迹在蓝色阴影区域下方，绿色的超车轨迹在蓝色阴影区域上方。
图片.png
采样末状态时，就可以分别在S-T图中障碍物对应的阴影区域的上方和下方分别采样。上方的末状态对应超车，下方的末状态对应跟车。
图片.png
如果有多个障碍物，就对这些障碍物分别采样超车和跟车所对应的末状态
图片.png
是遍历所有和车道有关联的障碍物，对他们分别采样超车和跟车的末状态，然后用多项式拟合即可获得一系列纵向轨迹。
图片.png
将三组纵向轨迹组合起来，就可以获得所有纵向轨迹。再将所有纵向轨迹和所有横向轨迹两两配对二维合成，就可以完成轨迹采样的工作。
图片.png
5.轨迹cost实现方法
轨迹规划所需要满足的四点要求，分别是到达目的、符合交规，避免碰撞、平稳舒适。针对这四点要求，我们设计了六个cost，cost越高就表示越不满足要求。下面一一介绍这六个cost的设计思路。
图片.png
首先是到达目的的cost。分成两种情况，一个是存在停车指令（比如红灯）的情况，另一个是没有停车指令的。如果存在停车指令，相对大的车速，其对应的轨迹cost就越大；如果没有停车指令，那么低速轨迹的cost就会越大。
图片.png
怎么实现这样的效果呢？针对这两种情况分别设计了参考速度。左图蓝线表示没有停车指令时的参考速度。可以看到这种情况下，绿色的加速轨迹会获得一个较小的cost，而红色的减速轨迹会获得一个相对较大的cost。那么如果存在停车指令，参考速度就会想右图中的蓝色曲线一样呈下降趋势。那么这种情况下，同样的两条轨迹，他们的cost大小关系就会正好相反。
图片.png
第二个cost是横向偏移cost。设计这个cost是为了让自动驾驶汽车能尽量沿着道路中心行驶。那么像左图汽车靠道路一边行驶，和中图画龙的行驶轨迹，他们的cost都相对较高。
图片.png
第三个cost是碰撞cost。左图中的两条轨迹，反映在右图S-T图中，我们可以发现红色的轨迹和蓝色障碍车在S-T图中的阴影区域有重叠，说明有碰撞风险，那么它的碰撞cost就会相对较高。而绿色的轨迹在S-T图中反映出来的碰撞风险较小，那么它的碰撞cost就相对较低。
图片.png
第四个cost是纵向加加速度的cost。加加速度（jerk）是加速度对时间的导数，表示加速度的变化率。我们用加加速度的最大值值来表示这个cost。
图片.png
第五个cost是横向加速度的cost。设计这个cost是为了平稳地换道。那么像左图猛打方向盘的轨迹，它的横向加速度cost就会相对较大。
图片.png
最后一个cost是向心加速度cost。设计这个cost是为了在转弯或调头的时候能够减速慢行。在弯道处，车速慢的轨迹，其向心加速度cost就会相对较低，那么就会更容易被率先挑选出来。
图片.png
这六个cost的加权求和就是轨迹的总cost。开发者可以根据产品的需要，调试这六个权重。
图片.png
介绍一下限制检测和碰撞检测。限制检测考察的内容有轨迹的加速度、加加速度、和曲率。碰撞检测则是把自动驾驶汽车的轨迹和其他障碍物的预测轨迹进行比对，观察是否有轨迹重叠。
图片.png
对于换道场景，Lattice算法仅仅需要对目标车道对应的参考线做一次采样+选择的流程。本车道和目标车道均能产生一条最优轨迹。给换道轨迹的cost上增加额外的车道优先级的cost，再将两条轨迹比较，选择cost较小的那条即可。
图片.png
