路由寻径(Routing)
负责在宏观路网中形成街接起点终点的概略行车路线
- 目的
在地图上搜索出最优的全局路径
- 输入
高精地图拓扑信息
起点和终点的位置
- 输出
从起点到终点的路由线路
- 转换成拓扑地图
基于高精地图生成拓扑地图，边缘可以根据行驶难度赋予不同代价，用A*搜索
- 寻路流程
routing的流程图
行为决策
在微观层面确定车辆当前需采取的行驶方式包括直行，轻微绕行、换道绕行、减速让行、加速抢行停车以及跟车
- 目的
保障无人车的行车安全并遵守交通规则
为路径和速度的平滑优化提供限制信息
- 输入
routing信息
道路结构信息（当前车道、相邻车道、汇入车道、路口等）
交通信号和标识（红绿灯，限速信息）
障碍物信息
- 输出
路径信息
速度限制和边界
时间上的位置限制边界
- 方法分类
- 决策内容
轨迹规划
在微观层面生成符合行为决策结果的时空连续轨迹
- 目的
在合理的时间到达规划的目标或终点
避免与障碍物碰撞
更好的乘坐体验
- 输入
决策输出信息
道路结构信息
交通信号和标识
障碍物信息
- 输出
稳定平滑的轨迹点
注：在不同场景评价函数可能不一样
代码部分
双层状态机，主要包括三个关键部分，底层状态机是各种小任务的集合
规划算法
框架
总体框架
路径规划步骤
速度规划框架
lattice planner
算法流程，红车为被控对象，场景为前车交汇，最终选择cost最小并且满足安全性的轨迹
采样过程：
纵向、横向分别采样，然后拟合成一条三维曲线，并可以将其转换为笛卡尔坐标系下
横向采样过程（这是在状态空间进行采样）
纵向采样，分为三种情况
最后对采样轨迹进行评价：
评价指标（速度限制与目的有关，看目的是巡航还是停车）
评价指标（向心加速度——转弯或掉头时需要减速慢行）
EM planner
计算期望值E，M则最大化E，然后交替进行。E步骤相当于把问题变换为图搜索问题，M步骤相当于解决搜索+非凸优化问题
算法流程图（DP为了得到决策结果，QP为了对DP结果进行平滑）
路径规划：
E步骤：红车为被控对象，将预测障碍物轨迹在SL空间进行投影：红色的部分是预测障碍物轨迹，蓝色的部分是自车轨迹，重叠部分（紫色区域）为估计交互区
基于DP的路径决策：首先用lattice采样+DP搜索+代价函数 得到可行通道和粗参考轨迹；基于QP的样条优化：基于可行通道和粗参考轨迹，得到优化轨迹
速度规划：
E步骤：将障碍物投影到ST图中，例如红色表示在2s时离自车40m远的障碍物
M步骤：在E步骤得到的ST图中求解最优路径S(t)，使用DP+样条曲线进行搜索，得到分段线性速度规划、可行通道和障碍物速度决策；然后用QP对分段速度进行优化平滑
算法比较
规划器实现
onlaneplanning task
流程
路径决策是指要不要换道，路径优化是指生成光滑无碰路径；速度决策是指当有前车并入时要不要超车
路径规划
换道决策：根据车道数量决定是否换道，如果要换道会考虑：是否达到换道条件、前后方是否有障碍物
路径重用决策：由于感知的不稳定性，需要判断是否保留上一帧规划的路径，当上一帧路径没有发生碰撞时，就沿用
借道决策
边界确定部分，分情况讨论
开始规划：
根据参考路径进行路径优化
当有多个路径时，进行评价选择
最后给出绕行还是等待的决策
速度规划
1. 在规划好的路径上进行停止决策
2. 产生速度可行使边界，把障碍物投影到ST图中
在2的基础上，使用粗优化的动态规划算法得到离散路径，决定是超车还是让行
4. 决定了超车还是让行后，就可以把原来的非凸优化问题转换为凸优化问题，使用二次规划方法进行路径优化（根据道路限速、减速带等对速度进行限制），得到平滑曲线；使用非线性优化器考虑路径曲率对速度规划的影响
5. 结合ST和SL曲线，得到完整轨迹
openspace task
该场景适用于自主泊车
确定可行驶区域
寻路+平滑，根据行驶方向分为前进段和后退段，分别下发
检测是否会发生碰撞，如果为fallback模式，则规划一条在检测到碰撞的位置停车的轨迹
Hei Blog
