Hei Blogapollo决策规划--理论部分
规划
首先将车辆状态、环境信息转换到frenet坐标系下，进行规划，然后再把规划结果转换到笛卡尔坐标系下，进行控制
轨迹选取
成本函数包括：
- 偏离车道中心
- 与障碍物的距离（安全性）
- 速度和曲率的变化（平滑性）
- 对车辆的压力
速度规划
ST图，纵坐标为汽车的纵向位移，横坐标为时间
在选择好路径之后，需要规划出在该路径上的速度，同样在ST图中进行规划：
首先把ST图分解为一个一个小格子（栅格地图），速度规划就是把格子的顶点进行连接（每条直线的速度不变）
然后把障碍物放到ST图中，表示为黄色矩形，该矩形表示在
时间区间内，会在
路段出现障碍物（预测模块给出），那么在速度规划的时候就绝对不能碰到黄色矩形部分
把速度规划问题转换成了搜索问题
lattice规划
使用lattice方法进行轨迹规划：
首先把三维的路径（纵向、横向、时间）解耦成 纵向规划 和 横向规划，纵向规划使用ST图，横向规划使用SL图
然后再将ST和SL结合起来
lattice规划是根据起始状态和终止状态进行规划的方法，所以首先我们应该明确终止状态是什么：
ST图终止状态
巡航
跟随
停止
SL图的终止状态
保持车在车道线中心，终止状态的朝向和位置的一阶和二阶导数都为0（即纵向和横向的a、v都为0）
然后使用二次规划方法得到ST和SL图，再将二者结合成三维轨迹
二次规划
第一步和第二步的路径和速度都是不平滑的，所以要用二次规划对其进行优化
最后将路径和速度进行结合，得到规划结果
