lattice planner和《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》解读

Lattice planner

参考资料来源：Apollo开发者社区推文、csdn和知乎博客、《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》一文
推文链接：https://mp.weixin.qq.com/s/YDIoVf20kybu8JEUY3GZWg

注：本文的文章顺序为先解读《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》、在介绍lattice planner的过程。大家在学习过程中请思考二者的联系和区别。

I.论文解读

Frenet坐标系中的无人车轨迹规划方法是宝马工程师 Moritz Werling 在2010年的论文《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》里首次提出的。其思想被广泛采用，其中百度Apollo比较出名的lattice planner算法的一些idea也来自这一篇经典文章，其重要性不言而喻。

1 基础知识介绍

1.1 自然坐标系

Frenet坐标系是为了描述车辆与道路之间的相对关系而引入，具体内容就不细说了，大家可以参考之前写的EM PLANNER一文章或者其他博客学习。

1.2 五次多项式

引言：人对加速度的变化比较敏感，因此衡量舒适度的指标常用位置的三阶导表示，基于此背景引入下面问题
Q:使下面指标取得最小的p(t)形式是什么？

A:五次多项式

1.3 cost函数

注：cost函数是后面规划的重要组成部分，所以放入基础知识模块中，后续的cost函数基于此设计。

cost函数由三项构成，前面的系数k表示权重，均大于0；g和h为任意函数；T表示时间间隔，Jt表示1.2中叙述的性能指标

2 横向轨迹生成

横向轨迹生成，论文将其分为高速和低速两种场景。高速和低速类似，不同的区别在于是否d与s可以解耦。

2.1 High Speed Trajectories

对于高速场景来说，设计成本函数如上。第一项为舒适度指标（一段时间内jerk的平方求和）、第二项为收敛性指标（T越小意味着收敛越快）、第三项为中心线指标（越靠近中心线越好）
注：初状态由上一个周期的状态确定（保证连续性），末状态一阶和二阶导取0（希望平行参考线）

结合上图来阐述一下轨迹生成的流程
1.采样阶段（给定终末状态以及经历的时间）

2.选出代价最小的有效轨迹

注：灰色代表无效轨迹（论文中指发生碰撞）、黑色代表有效轨迹、绿色代表最优轨迹（每个周期规划出来的轨迹会沿着上一个周期规划出来的最优轨迹继续规划）

2.2 Low Speed Trajectories

对于低速场景，解耦的方式忽略了车辆的非完整性约束，导致大部分轨迹不有效。设计成本函数如上。第一项为舒适度指标、第二项为收敛性指标（S越小意味着收敛越快）、第三项为中心线指标（越靠近中心线越好）

轨迹生成的流程
1.采样阶段（给定终末状态以及经历的时间）

2.选出代价最小的有效轨迹

3 纵向轨迹生成

针对纵向轨迹生成，论文将其按照场景划分为3.1和3.2两类（个人感觉是s-t规划和s导-t两类），与横向轨迹相比，除了要关注cost函数，还需要关注S_target的计算

3.1 Following, Merging, and Stopping

对于跟车、并道以及停车等场景，需要有一个目标状态$s_{target}(t)$作为引导，再在每一次的周期中，更新末端约束(会有不同的$\Delta s_i$以及$T_j$)
采样阶段也即：

cost函数
cost函数主要在于第三项的变化，s1代表末状态，sd代表希望的状态（二者差值的平方越小越好）

3.1.1 Following

$s_{target}(t)$计算：$s_{lv}(t)$表示前车的位置，$D_0$和 tau 为常数

注：论文中给出来$s_{lv}(t)$和${\dot{s}}_{lv}(t)$的计算过程以及$s_{target}(t)$的推导

假设目标前车的纵向位置加速度为常数

泰勒展开可求出$s_{lv}(t)$和${\dot{s}}_{lv}(t)$

可以验证目标点加速度和前车加速度保持一致

3.1.2 Merging

并行就很容易理解了，要求位于a和b之间

3.1.3 Stopping

停车也很容易理解，像有红灯场景时，$s_{target}(t)$是预先知道的，同时要求其速度和加速度为0

3.1.4 结果解读

与横向轨迹规划类似，也是采样，选出cost最小的一条轨迹（类似于EM planner的速度规划）

注：灰色代表无效轨迹（论文中指发生碰撞）、黑色代表有效轨迹、绿色代表最优轨迹、蓝色代表$s_{target}(t)$轨迹

3.2 Velocity Keeping

对于前方无车等场景（需要保持一定的速度），此时不需要有一个目标状态$s_{target}(t)$作为引导，再在每一次的周期中，更新末端约束(会有不同的$\Delta {\dot{s}}_i$以及$T_j$)
采样阶段也即：

cost函数
与3.1类似，不同的是第三项变成了速度

结果解读

注：与之前类似，就不详细说了，注意纵坐标是${\dot{s}}_i$

4 轨迹的合成

将横纵向轨迹的Cost进行加权求和，然后选择Cost最小的轨迹。（转换到笛卡尔坐标系，方便下游控制模块执行）

II Lattice planner

Lattice planner是百度Apollo在提出EM planner后出现的，与EM planner相比，Lattice planner简单易操作，适合简单场景下的运动规划。本文先讲述Lattice planner的过程，随后介绍其与EM planner的区别以及与上一篇论文的异同点

1 横向采样

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

2 纵向采样

纵向采样和Moritz Werling法类似，需要考虑巡航、跟车或超车、停车这几种状态。

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

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

针对跟车和超车场景，可以在障碍物上下进行采样（上方代表超车，下方代表跟车）

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

3 cost函数设计

3.1 轨迹合并

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

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

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

3.2 cost函数设计准则

针对到达目的、符合交规、避免碰撞、平稳舒适四点要求。Apollo设计了六个cost，cost越高就表示越不满足要求。下面一一介绍这六个cost的设计思路。

• Objective achievement cost : 如图所示，会给一个参考速度，越靠近参考速度越好

• Lateral offset cost ： 如图所示，越靠近道路中心线越好

• Collision cost ：如图所示，碰撞到障碍物会给一个很大的惩罚项

• Longitudinal jerk cost：如图所示，考虑jerk作为舒适度的指标

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

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

4 与EM planner区别

大家可以看一看二者的区别，顺便可以思考一下这个问题

Q: Lattice Planner 将规划统一成代价函数，寻找代价最小的。在规划的上层是否还需要决策层？EM planner的决策方法是什么？

A:在规划上层的决策仅仅包含了来自交规的停车指令（比如红绿灯），其余的策略均由下层采样+cost来完成。而EM planner的决策方法是基于DP思想的一种轻决策算法。

5 lattice planner与Moritz Werling法的异同点

• 在横向轨迹采样时，lattice planner考虑l和s耦合的情况（也即Moritz Werling法对应的Low Speed Trajectories）
• 在纵向轨迹采样时，lattice planner和Moritz Werling法均都考虑了巡航、跟车或超车、停车这几种状态。
• 在cost设计方面，lattice planner考虑的更加充分，设计了6种cost;而Moritz Werling法从舒适度、收敛速度、靠近中心线、靠近设定目标等方面考虑