自动驾驶学习笔记（二十）——Planning算法
#Apollo开发者#
学习课程的传送门如下，当您也准备学习自动驾驶时，可以和我一同前往：
《自动驾驶新人之旅》免费课程—> 传送门
《Apollo 社区开发者圆桌会》免费报名—>传送门
文章目录
前言
参考线平滑
双层状态机
EM Planner
Lattice Planner
算法调试
总结
前言
见《自动驾驶学习笔记（十五）——交通灯识别》
见《自动驾驶学习笔记（十六）——目标跟踪》
见《自动驾驶学习笔记（十七）——视觉感知》
见《自动驾驶学习笔记（十八）——Lidar感知》
见《自动驾驶学习笔记（十九）——Planning模块》
参考线平滑
高精地图通过Routing模块给过来的道路参考线，平滑度不满足决策规划算法的要求，参考线平滑算法类型有如下几种：
离散点平滑算法是在离散的参考点之间插值，使用低价函数求得最优参考线，示例如下：
代价函数：cost = costsmooth + costlength + costdeviation
双层状态机
Planning算法的整体框架是一个双层状态机结构，示例如下：
第一层是各种场景，场景的识别是Planning模块根据高精地图、感知、定位过来的信息综合判断出来的。
第二层是某个场景下进行路径规划的各个Stage和Task。
场景、Stage和Task之间的结构关系，示例如下：
车道跟随场景算法执行的过程，示例如下：
EM Planner
最大期望算法（Expectation Maximum）经过两个步骤交替进行计算。第一步是计算期望（E），利用对隐藏变量的现有估计值，计算其最大似然估计值。第二步是最大化（M），最大化在E步上求得的最大似然值来计算参数的值。M步上找到的参数估计值被用于下一个E步计算中，这个过程不断交替进行。示例如下：
基于EM算法开发的Planner示例如下：
DP 动态编程（dynamic programming）
QP 基于样条的二次规划（quadratic programming）
当可规划空间中有多条车道是时，需要使用EM算法对各车道分别规划，最后决策选用一个车道，示例如下：
Lattice Planner
Lattice Planner算法属于一种局部轨迹规划器，是一种基于采样的运动规划算法。示例如下：
Lattice Planner算法的实现过程大概可以分为以下几步：
1、 根据当前速度计算前瞻距离，获得前瞻点，获得车辆在前瞻点位置frenet坐标系下的目标状态。
2、 对轨迹状态进行采样，分别是轨迹运行时间t，目标速度v，及到参考线的横向位移d，通过这三个规划参数可以获得采样状态。
3、 构建横向位移和纵向位移的多项式规划函数s(t)，d(s)，获得横向位移和纵向位移的规划函数后，进行时间插值就可以获得轨迹点，
4、 采样轨迹是一系列满足速度约束的光滑轨迹，但其还需要满足无碰撞和车辆运动学曲率约束的强制约束，及远离障碍物和靠近参考线等组成的代价约束。采样轨迹的打分就是为了获得一条最优的满足约束条件的无碰撞光滑轨迹。
EM Planner和Lattice Planner的特点对比如下：
| EM Planner |
Lattice Planner |
| 参数较多(DP/QP Path/Speed) |
参数较少且统一化 |
| 横纵向（Path和Speed）分开求解 |
横纵向同时求解 |
| 流程复杂 |
流程简单 |
| 单周期解空间受限 |
简单场景解空间较大 |
| 能适应复杂场景 |
适合简单场景，例如高速公路 |
| 功能更强 |
性能可靠，鲁棒性好 |
算法调试
Planning算法调试的方法有如下两种。
1、实车运行是Control模块把Planning算法输出的轨迹点让车辆底盘去执行，让车辆在实际道路上运行，示例如下：
2、仿真运行是在Dreamview环境下，由SimCongtrol模块把Planning算法输出的轨迹点给到虚拟的车辆模型，让车辆在虚拟的地图上运行，示例如下：
Dreamview仿真环境提供了丰富的调试工具，可以对各个算法模块进行开关操作，不仅能可视化算法模块的输出信息，还能输出仿真车辆硬件的状态。
总结
以上就是本人在学习自动驾驶时，对所学课程的一些梳理和总结。后续还会分享另更多自动驾驶相关知识，欢迎评论区留言、点赞、收藏和关注，这些鼓励和支持都将成文本人持续分享的动力。
另外，如果有同在小伙伴，也正在学习或打算学习自动驾驶时，可以和我一同抱团学习，交流技术。
版权声明，原创文章，转载和引用请注明出处和链接，侵权必究！
文中部分图片来源自网络，若有侵权，联系立删。