【自动驾驶规划】Lattice Planner

Lattice Planner简介

LatticePlanner算法属于一种局部轨迹规划器，输出轨迹将直接输入到控制器，由控制器完成对局部轨迹的跟踪控制。因此，Lattice Planner输出的轨迹是一条光滑无碰撞满足车辆运动学约束和速度约束的平稳安全的局部轨迹。Lattice Planner的输入端主要由三部分组成，感知及障碍物信息、参考线信息及定位信息。局部规划模块的输出是带有速度信息的一系列轨迹点组成的轨迹，其保证了车辆控制器在车辆跟踪控制过程中的平稳性和安全性。

局部规划模块的输出是带有速度信息的一系列轨迹点组成的轨迹，其保证了车辆控制器在车辆跟踪控制过程中的平稳性和安全性。

Lattice规划算法实现过程

Lattice规划算法是一种基于采样的运动规划算法，通过将车辆坐标系转换到参考线坐标系，也就是frenet坐标系下，然后在frenet坐标系下分别对frenet的d轴和s轴进行规划，形成frenet坐标系下的规划轨迹，然后将frenet坐标系下的轨迹合成到世界坐标系下还原为世界坐标系下的轨迹。算法实现过程大概可以分为以下几步：

1. 将车辆当前位姿信息转换到frenet坐标系下，获得车辆在frenet坐标系的初始状态；根据当前速度计算前瞻距离，获得前瞻点，获得车辆在前瞻点位置frenet坐标系下的目标状态。
2. 对轨迹状态进行采样，分别是轨迹运行时间t，目标速度v，及到参考线的横向位移d，通过这三个规划参数可以获得采样状态。
3. 构建横向位移和纵向位移的多项式规划函数s(t)，d(s)，获得横向位移和纵向位移的规划函数后，进行时间插值就可以获得参考线frenet坐标系下的轨迹点，最后将轨迹点从frenet坐标系转换到cartesian坐标系，就可以获得物理世界采样轨迹，由于横向和纵向都是通过高次多项式插值获得，以此cartesian坐标系下的轨迹也是光滑的。
4. 采样轨迹的碰撞检测、曲率约束及最优轨迹打分。采样轨迹是一系列满足速度约束的光滑轨迹，但其还需要满足无碰撞和车辆运动学曲率约束的强制约束，及远离障碍物和靠近参考线等组成的代价约束。采样轨迹的打分就是为了获得一条最优的满足约束条件的无碰撞光滑轨迹。该轨迹也是lattice输出到controller用于车辆跟随的轨迹。

Frenet坐标系和Cartesian坐标系的相互转换

Frenet坐标系是参考线上的坐标系，是一个动坐标系。Frenet坐标系的建立，以车辆位置到参考线的最近点R作为frenet坐标系的原点，以参考线切线方向作为T轴，垂直于T轴向外为N轴。如下图所示，是frenet坐标系和cartesian坐标系的相互转换关系，黑色虚线是车辆当前运行的轨迹方向，黑色实线是车辆运行的参考线。

 如上图所示，参考线（Reference line）是一条光滑的车道线，按上图所示将汽车的坐标点P（图中红色点）投影到参考线上，得到一个参考线上的投影点R（图中绿色点）。从参考线起点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用s表示。而投影点到汽车位置的距离 则是汽车在Frenet坐标系下的横向偏移量。因为参考线是足够光滑的，我们也可通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。这里将横向偏移量设计成纵向偏移量s的函数。这是因为对于carlike模型的汽车而言，横向运动是由纵向运动诱发的。而将坐标点转换到frenet坐标系的目的则是为了方便规划曲线的生成和车道线横向和纵向方向上的轨迹采样，从而获得覆盖整个车道的光滑采样轨迹。

frenet坐标系和cartesian坐标系的转换关系可以可以参考如下论文

如下所示是两个坐标系之间的变换公式。

cartesian坐标系到frenet坐标系的变换公式：

frenet坐标系到cartesian坐标系的变换公式：

上式中，各变量的含义如下：

如下图所示绿色线代表了参考线reference_line，红色和蓝色线代表经过横向偏移位移均匀变化之后形成的路线。

Lattice规划器的轨迹采样，主要分为横向采样、纵向采样以及轨迹时间周期采样。 横向轨迹的采样需要涵盖多种横向运动状态，需要根据车道宽度设置横向采样的采样区间，通过横向采样间隔，形成不同的横向采样偏移量。纵向采样的采样区间可以通过前瞻点的位移长度s，作为基准采样长度，然后通过对轨迹速度ds进行采样。时间周期采样，就是对轨迹的运行周期时间进行采样。而百度Apollo的轨迹采样，只对横向位移和纵向位移进行了采样，并设计了采样状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80来得到采样状态。所以Lattice规划器的轨迹采样主要是对轨迹横纵向状态进行采样，但采样方式可以根据环境情况进行调整。

Lattice Planner速度规划

有了前面的采样状态，现在需要做的是根据采样状态生成横向和纵向和规划函数，两种规划函数都是通过多项式进行拟合求解生成。主要使用了4次和5次多項式拟合，从而满足了车辆运行过程中的一阶导，二阶导联系，也就是速度和加速度连续，保证了轨迹的平滑性要求。

对于纵向轨迹，在停车和跟车状态，都是五次多项式，但对于巡航状态，由于我们不需要确定状态的S值，所以只有五个变量，因此用四次多项式就可以了。对于横向轨迹也使用了五次多项式拟合。

这里规划器的采样方式没有使用Apollo中Lattice的横纵向采样方式，而是采用了上文中提到的采样方式，因此约束变量有：

停车和跟车下的纵向拟合函数的求解

解方程组，即可得到多项式系数 。

巡航模式下的纵向拟合函数的求解

而对于巡航模式，由于采样状态的纵向位移s是变化的、不受约束的，因此少了一个约束条件，因此为了饱和约束，采用了4次多项式拟合。

纵向拟合4次多项式函数为：

横向拟合函数的求解

横向拟合函数是以纵向位移s(t)为自变量，其拟合函数用表示。

约束变量有：

解方程组，即可得到多项式系数。

如下图所示是仿真测试得到的纵向和横向速度规划曲线：

轨迹生成及轨迹评价函数

轨迹的生成成就是将frenet坐标系下的轨迹转换到cartesian坐标系中，前面我们知道了位姿点在frenet坐标系和cartesian坐标系的相互转换关系，因此现在我们需要做的就是对横纵向轨迹函数和  进行轨迹的时间细分形成规划函数的横纵向轨迹规划点 和 ，该规划点是在frenet坐标系中，因此需要进行frenet坐标系到cartesian坐标系的坐标转换，从而形成控制器可用的采样轨迹。

获得采用轨迹之后，接着需要进行目标轨迹的曲率检查和碰撞检测，目的是为了使目标采样轨迹满足车辆的运动学控制要求和无碰撞要求，这样就形成了安全可靠的轨迹簇。这些轨迹簇都可以满足车辆的控制要求，但并不是最优的，因此需要从轨迹簇中选出一组最优的运行轨迹。这时就需要引入轨迹评价函数，用来对候选轨迹进行打分。

轨迹评价函数主要为了使得目标轨迹尽量靠近静态参考线轨迹运行，同时，速度尽量不发生大突变，满足舒适性要求，且尽量远离障碍物。因此最后轨迹评价函数可以通过如下伪代码描述：

traj_cost = k_lat * cost_lat + k_lon * cost_lon + k_obs * obs_cost;

• 上式中， k_lat : 表示纵向误差代价权重
• cost_lat： 表示纵向误差，综合考虑纵向速度误差，时间误差及加加速度的影响。
• k_lon : 表示横向误差代价权重
• cost_lon： 表示横向向误差，综合考虑了横向加速度误差及横向偏移误差的影响。
• k_obs : 表示障碍物代价权重
• obs_cost： 表示障碍物距离损失。

最后选择出代价值最好的一条轨迹输入到控制器，用于控制器的跟踪控制。