Apollo规划模块：lattice算法原理解析

一、概述

Apollo规划模块以预测模块、routing模块、高精地图和定位的结果作为输入，输出一条平稳、舒适、安全的轨迹，下游交给控制模块去执行，由控制器完成对局部轨迹的跟踪控制。

Lattice算法属于规划模块，是一种局部轨迹规划器，输出是一条光滑无碰撞满足车辆运动学约束和速度约束的平稳安全的局部轨迹。输入端主要由三部分组成，感知及障碍物信息、参考线信息及定位信息。（预测模块、routing模块、高精地图和定位的结果）输出是带有位置、速度信息的一系列轨迹点组成的平稳、舒适、安全的轨迹。

二、算法思想

Lattice规划算法是一种基于采样的运动规划算法，通过将车辆坐标系转换到参考线坐标系（frenet坐标系），然后在frenet坐标系下分别对frenet的d轴和s轴进行规划，形成frenet坐标系下的规划轨迹，然后将frenet坐标系下的轨迹合成到世界坐标系下还原为世界坐标系下的轨迹。

三、算法流程

1、主要流程

（1）采样足够多的候选轨迹，提供尽可能多的选择。
（2）计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。
（3）循环检测的过程。每次会先挑选出cost最低的轨迹，对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条cost最低的轨迹。
（4）输出最优轨迹或者停车轨迹。

2、原理解析

(1) Frenet坐标系和ST图
采用Frent坐标系表示车辆状态。参考线起点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用s表示；投影点到汽车位置的距离则是汽车在Frenet坐标系下的横向偏移量，用l表示。

由于参考线是足够光滑的，通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。
（将横向偏移量L设计成纵向偏移量S的函数，横向运动是由纵向运动诱发的）

st图中上边界表示车头，下边界表示车尾，斜率表示车速，蓝车在tin时刻进入车道，tout时刻离开车道。红车红线跟车在阴影下方，绿线超车在阴影下方。

（2）生成轨迹过程
**第一步：**计算得到汽车在在零时刻的当前状态，接着在Frenet坐标系下采样一个在T1时刻的末状态。

**第一步：**将末状态和起始状态做多项式拟合。分别形成横向和纵向的多项式轨迹
**第三步：*二维合成。给定一个时刻T，计算出在T时刻的纵向偏移量和横向偏移量，再通过参考线，可还原成一个二维平面中的轨迹点。通过一系列的时间点T0，T1，…，Tn，可以获得一系列的轨迹点P0，P1，…，Pn，最终形成一条完整的轨迹。
（3）采样轨迹过程
横向轨迹的采样： 包括多种横向运动状态。设计了三个末状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80。用两层循环遍历各种组合，再通过多项式拟合，即可获得一系列的横向轨迹。

纵向轨迹的采样：考虑巡航、跟车或超车、停车这三种状态。
停车状态： 给定停车点，末状态的速度和加速度都是零，末状态是确定的。用一层循环来采样到达停车点的时间。

巡航状态：
两层循环来完成采样。外层循环将速度从零到上限值按等间隔均匀遍历。内层循环遍历到达末状态速度的时间，从1秒到8秒按1秒的间隔均匀遍历。由于巡航状态不需要指明到达末状态的S值，只需要用四次多项式拟合即可。

超车状态：
采样末状态时，分别在S-T图中障碍物对应的阴影区域的上方和下方分别采样。上方的末状态对应超车，下方的末状态对应跟车。多个障碍物时，对这些障碍物分别采样超车和跟车所对应的末状态。

遍历所有和车道有关联的障碍物，分别采样超车和跟车的末状态，然后用多项式拟合即可获得一系列纵向轨迹。

所有纵向轨迹进行组合，再将所有纵向轨迹和所有横向轨迹两两配对二维合成，完成采样。

（4）轨迹COST设计：

轨迹规划所需要满足到达目的、符合交规，避免碰撞、平稳舒适。设计了六个cost，cost越高就表示越不满足要求。

到达目的的cost：
存在停车指令，相对大的车速，其对应的轨迹cost就越大；
没有停车指令，那么低速轨迹的cost就会越大。（设计了参考速度实现）
横向偏移cost：
为了让自动驾驶汽车能尽量沿着道路中心行驶。靠道路一边行驶，画龙的行驶轨迹，cost都相对较高。
碰撞cost：
轨迹和在S-T图中的阴影区域有重叠，说明有碰撞风险，碰撞cost就会相对较高。轨迹在S-T图中反映出来的碰撞风险较小，碰撞cost就相对较低。
纵向加加速度的cost：
加加速度（jerk）是加速度对时间的导数，表示加速度的变化率。加加速度的最大值来表示这个cost。
横向加速度的cost：
设计这个cost是为了平稳地换道。猛打方向盘的轨迹，它的横向加速度cost就会相对较大。
向心加速度cost：
设计这个cost是为了在转弯或调头的时候能够减速慢行。在弯道处，车速慢的轨迹，其向心加速度cost就会相对较低。

六个cost加权求和就是轨迹的总cost。根据需要，调试六个权重。

（5）限制检测和碰撞检测
限制检测:轨迹的加速度、加加速度、和曲率。
碰撞检测:自动驾驶汽车的轨迹和其他障碍物的预测轨迹是否有轨迹重叠。

三、应用场景：

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

Lattice采样+选择过程，横纵同时求解。更多用于封闭结构化道路的规划，低速园区和高速公路的场景；城市道路简单路况，对于相对复杂的交规处理还有待完善。

Apolo开发者社区：http://apollo.baidu.com/developer/index_cn.html#/
参考链接：
https://www.cnblogs.com/fuhang/p/9563884.html

内容皆来源于网络和参考链接整理，侵权联系删。