概要：本文主要学习基于Lattice Planner的动作规划器。
文章目录
前言
本文将对基于Lattice Planner的动作规划进行了解研究。
一、Lattice Planner
1.1 概念
- 基于采样的局部运动规划器；
- 在Frenet coordinate坐标系中进行规划。
规划器的输入：
- Localization（高精地图定位信息）
- Perception（感知周围环境信息和障碍物）
- Reference（高精地图的参考线信息）
规划器输出：
由于该规划器直接发给底层控制器的，所以该规划器输出的轨迹是一条光滑的、安全的、没有碰撞风险的轨迹，满足车辆的运动学和速度约束。
1.2 基本流程
Lattice轨迹采样点加入运动学模型约束，具有连续性。
基本原理：车辆坐标系转换到Frenet坐标系，对横轴和纵轴进行规划，形成该坐标系下规划的轨迹。再还原到世界坐标系下的轨迹。
流程如下：
- 参考线离散化；
- 将找到的参考线信息转换到Frenet坐标系，获得车辆在Frenet坐标系下的初始状态；
- 基于障碍物信息、速度约束等信息，决定车辆的行驶目标点；
- 通过多项式进行连接，生成横向和纵向一维的轨迹树，并且确认轨迹是否符合运动学的轨迹和约束，把好的轨迹进行合并就得到需要行驶的轨迹；
- 将得到的轨迹点从Frenet坐标系转换到Cartesian坐标系就得到满足运动学约束、光滑的世界坐标系下的轨迹；
- 在世界坐标系下会进行碰撞检测以及最优轨迹的打分，得到最优的、无碰撞的、光滑的轨迹输入到控制器中进行跟踪。
二、Frenet coordinate
2.1 概念
Frenet坐标系是参考线上的坐标系，是一个动态的坐标系（T轴、N轴）。
2.2 Frenet和Cartesian坐标系联系
2.2.1 Frenet坐标系
Frenet坐标系能够更加直观表示道路位置；常用s、l变量表示；
- 概念
（1）s：纵向的坐标，沿着道路前后的位移；
（2）s‘：纵向坐标对时间的倒数，也就是纵向的速度；
（3）s’‘：纵向坐标对时间的二阶倒数，纵向加速度；
（4）l：横向的坐标，沿着道路左右的位移；
（5）l`:横向坐标对时间的导数，横向速度；
（6）l``横向坐标对时间的二阶导数，横向加速度；
（7）l’：横向坐标对纵向坐标倒数；
（8）l’'：横向坐标对纵向坐标二阶倒数；
2.2.1 Cartesian坐标系
（1）x向量，笛卡尔坐标系下横轴的坐标；
（2）vx，笛卡尔坐标系下横轴的坐标速度标准量；
（3）ax，笛卡尔坐标系下横轴的加速度；
（4）θx，笛卡尔坐标系下横轴的航向角（朝向）；
（5）kx，笛卡尔坐标系下的曲率，ds为当前轨迹的弧长，
2.3 Frenet坐标系和Cartesian坐标系转换
2.3.1 Cartesian to Frenet（x,y->s,l）
推导公式：
2.3.2 Frenet to Cartesian（s,l->x,y）
三、Lattice planner sampling
lattice适合于道路、公路等结构性、有规律的采样，采样方式分为：
（1）Lateral sampling：横向采样，根据车的宽度设置采样区间，形成横向的采样偏移量。
（2）Longitudinal sampling：纵向采样，通过前瞻点的位移长度s作为基准采样长度，对轨迹的速度ds进行采样。
（3）Time sampling：轨迹时间周期采样
（4）state space：根据环境进行采样
（5）Control space：根据车辆运动模式进行采样
四、Lattice Planner：speed planning
轨迹是由多项式曲线生成，就是根据首末点状态，进行多项式曲线的系数求解，这里基于前面阐述的采样，生成横向的l(s)和纵向的s(t)的规划函数并通过4次或者5次的车辆多项式进行拟合保证轨迹的平滑性。
（1）对于纵向轨迹，在停车、跟车、超车时，jerk的要求会更高，可以使用5次多项式来保证路径的平滑性，首末状态都是（s，v，a）；
（2）对于纵向轨迹，在巡航过程中，不需要知道s的状态，可以使用4次多项式进行拟合（首：s，v，a，末：v，a）；
（3）对于横向轨迹，可以使用5次多项式进行拟合。
4.1 跟停车场景
4.1.1 纵向多项式拟合求解（5阶）
4.1.2 约束函数
在轨迹规划过程中，约束条件一般分为：初始状态和结束状态（采样状态）；
（1）初始状态：
初始状态纵向位移、速度、加速度。
（2）采样状态（t1为轨迹运行时间周期）：
采样状态纵向位移、纵向速度、纵向加速度。
（3）总的约束
利用约束方程就可以求出约束系数。
4.2 巡航场景
巡航模式其实是在匀速、恒速运动下对速度的控制，对纵向位移的变化不受约束。
4.2.1 纵向多项式拟合求解（4阶）
4.2.2 约束条件
（1）初始状态
初始纵向位移、纵向速度、纵向加速度
（2）采样状态
纵向速度、纵向加速度
（3）总的约束
四、Lattice Planner：Lateral trajectory planning
（1）5次多项式拟合
横向的拟合函数以纵向的位移为自变量l(s)（5次多项式）
（2）约束条件
初始横向位移：d0;
初始横向速度：sd0；
初始横向加速度：ssd0；
采样横向位移：d1；
采样横向速度：sd1；
采样横向加速度：ssd1；
五、Lattice Planner processing
5.1 Evaluation
在生成横向、纵向一维轨迹树之后，就需要进行可行性的评估。
对于任何轨迹点如果超过位移、速度、加速度的范围的话就会被筛选掉，大大减轻了后续的工作。
5.2 cost function
根据横向和纵向分别cost的计算来进行排序，评估横向和纵向的轨迹对，选择接近参考轨迹、舒适性好、远离障碍物的路径。
5.3 trajectory cost function
5.3.1 Longitudinal Objective achievement cost
寻找一条最靠近目标点的可行路径。
一般分为两种场景：
（1）停车指令场景中（红灯），因为车速要迅速下降，则对应的trajectory cost会很大；
（2）没有停车指令场景，低速轨迹的cost就会更大
实现效果：
1）Jspeed：实际速度要和参考速度去靠近，优化；
2）Jdist：dist越小，cost越大；dist越大，cost越小；
3）Jspeed和Jdist的组合，w为权重比.
5.3.2 Comfort Objective
寻找一条加速度变化小的的舒适路径，一般大的jerk会有一种摇晃的感觉。
5.3.3 Centripetal Objective
寻找一条向心加速度变化小的的可行路径，在大弯或者掉头的时候可以减速慢行。
5.3.4 Collsion Objective
去选择远离一条障碍物的路径。
左图两条轨迹反应到ST图中，红色轨迹和蓝色障碍车在阴影区域有重叠，说明有碰撞的风险，碰撞的风险大则碰撞的cost就会相对较高，碰撞风险低则碰撞的cost就会相对较低。
5.3.5 Lateral offset Objective
选择一条可行使的路径使其沿着道路中心行驶。这样会有更多的余量与道路边界和车辆进行交互。
如图所示，一边行驶或者横向晃动它的cost会比较大，而道路中心行驶cost比较小。
5.3.6 Lateral Acceleration Objective
选择一条可行使的路径使其有一个平滑的换道过程，提升换道的平稳性。
如图所示，左边的图，猛打方向盘，曲率有一个巨大的变化，横向加速度的cost就会比较高，而右图平顺的换道，它的cost就会低。
总结
本文主要是对于自动驾驶规划控制中动作规划中的Lattice Planner网格规划器进行学习，主要介绍了其规划器的概念、cartesian坐标系和Frenet坐标系的转换关系，以及横纵向轨迹规划拟合，最后概括了如何利用规划去选择最优路径，这篇文章希望可以对想要学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!