路径规划：解决不动的地图

输出：

从a到b的最佳路径

输入：

地图
真实情况是由上层提供的地图数据，包括路网和实时交通信息（如果做毕业设计的话，我们可以自定义的建模）
location
车辆当前位置，由上层传感器定位获取
aim place
乘客或计划

具体过程：

将世界地图图化为“节点”和“边缘” 分别代表‘路段’、‘路口’

世界图化
采用图论中的算法，处理上述问题

经典算法

A-star算法
面对路径搜索的爆炸问题，加入cost函数计算两个成本：前往候选节点，从候选节点到目的地的成本
成本估算可以根据实际情况自定义

轨迹生成：解决移动的不属于地图的物体

目标

生成一系列由路径点所规定的轨迹，为每个路径点分配时间戳和对应速度（加入速度的目的：确保能够按时到达对应的路径点），这部分需要和预测模块的输出相结合，确保生成的路径点与其他车辆的预测轨迹无冲突
生成路径点的3D trajectory：2D Position + Time
如果是飞机的话，生成的应该是4D航迹

轨迹生成

评价指标

避障：不能与其他车辆，行人等有冲突
舒适：路径点之间的过度，速度的变化都必须是平滑的
可执行: 无物理冲突，例如不能要求立即转弯180度
合法：遵循相关交通规定

成本函数

在规划出来的满足上述要求的轨迹可能有很多条，使用成本函数，为每条轨迹分配一个成本，通过成本函数将轨迹排序，选出最优轨迹

Frenet坐标

笛卡尔坐标在当前环境无法直观表现车辆特征，引入Frenet坐标系，纵坐标为车道S中心线，横坐标是处处与之垂直的线

Frenet坐标

路径速度解耦规划

解耦将轨迹生成分成了两步：

路径规划
- 生成候选曲线
  1）首先在路径中生成单元格
  2）然后对单元格中的点进行随机取样
  3）每个单元格取点，将之连线，生成一条候选曲线
  4）重复3）步，构建多条候选曲线
  
  多条候选曲线
- 成本函数对路径进行评估（平滑度，安全性，车道中心的偏离等）
- 成本函数排名，选择成本最低的路径
速度规划
- 确定沿这条路线行进的速度（一系列的速度值，构成速度曲线）
  1）使用工具ST图设计和选择速度曲线：s表示纵向位移，t表示时间，从而表征车辆在不同时间的位置。其中斜率表征当前速度值，曲线越陡，说明更短的时间t有更大的移动距离s
  
  ST图
  
  2）为构建最佳速度曲线，将st图离散化为多个网格，每个网格的速度相同
  3）结合预测模块的输出，将其在ST图中做对应表示，具体如下图所示：
  
  黄色区域是禁止区
  4）限制范围内的最优化选取曲线，限制可能包括：法律，例如限速；车距；汽车本身物理限制，例如加速度限制；
  使用最优引擎！
  选取最优速度曲线