自动驾驶决策控制及运动规划史上最详细最接地气总览

看前须知

本人方向为自动驾驶决策控制与运动规划（Decision Making And Motion Planning, DMAP），先前在某SCI期刊发了一篇review（抱老板大腿），SCI文献看了几百篇（最后用上的也就100多，可以说走了很多曲里拐弯的路了），想着当初入门的时候那种困难且没有方向的感觉，而且网上实在缺少对这方面知识简单易懂且全面的梳理，我觉得可以把我所学分享出来，供大家参考借鉴。需要注意的是：这些东西你完全可以从网上或者文献中获得，从这篇blog中了解信息的优势仅仅是帮你省去大把大把大把自己找文献梳理脉络的时间（对症下药的那种，而不是漫无目的的随意的文献），梳理出决策规划方向到底都有什么。并且我会讲许多比较常见的算法比如AD之类的。我会尽量秉持着大白话的风格，喜欢直接看科研风格的可以直接看文献**
因为这篇blog是依托于我很久之前给老板汇报工作时的学习成果，所以在讲述DMAP时，你可能会看到一些网上的图。我自己文章内的东西我也会抛出来一些，参考文献需要的我会都给你们，文末有惊喜！文末有惊喜！文末有惊喜！文末有惊喜！文末有惊喜！文末有惊喜！文末有惊喜！ 但是文章名字我就不抛出来了，怕人肉。

1.介绍

图1：当前自动驾驶汽车存在的三大类感知决策控制方法
图1所示的决策控制方法由上至下依次为：sequential planning、behavior-aware planning、以及end-to-end planning。sequential planning属于最传统的方法，感知、决策与控制三个部分层次较为清晰；behavior-aware planning这种方法相比第一种，亮点在于引入人机共驾、车路协同以及车辆对外部动态环境的风险预估；end-to-end planning这种方法基于DL、DRL技术，借助大量的数据做训练，获得从图像等感知信息到方向盘转角等车辆控制输入的关系，属于时下最热门的方法之一。
本文将对上述三种方法的内容与遇到的问题以及已取得的成果做简要介绍，第二节将介绍sequential planning，这一节里会按照整个决策控制顺序讲述自动驾驶汽车的感知控制过程。第三节介绍端到端的自动驾驶，第四届介绍behavior-aware planning决策控制方法，第五节介绍安全性验证的方法，最后会简要总结一下前文所提到的待解决的问题。

2.路径规划

图2：sequential planning 的过程
如图2所示为sequential planning 的过程，简要概括为路径规划->决策过程->车辆控制，本小结将要讲述的路径规划属于其中的第一步与第三步。
在无人车的运动轨迹生成问题上，有直接轨迹生成法与路径-速度分解法两种，相比第一种，路径-速度难度更小，因此更加常用。路径规划可分为四大类：以PRM、RRT为代表的基于采样的算法、以为A*、D*代表的基于搜索的算法，以β样条曲线为代表的基于插值拟合的轨迹生成算法，以及以MPC为代表的用于局部路径规划的最优控制算法。本小节将按照上述顺序逐一讲解：


图3：路径规划算法的效果图
表2：路径规划算法优缺点

2.1基于采样的算法

1. 基本算法PRM与RRT的构建
   
   图4：PRM效果图
   (1) PRM的构建[1]
   1) 预处理阶段：对状态空间内的安全区域均匀随机采样ｎ个点，每个采样点分别与一定距离内的邻近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图
   2) 查询阶段：对于给定的一对初始和目标状态，分别将其连接到已经构建的图中，再使用搜索算法寻找满足要求的轨迹
   容易看出，一旦构建一个PRM之后，可以用于解决不同初始、目标状态的运动规划问题，但是这个特性对于无人车运动规划而言是不必要的．另外PRM要求对状态之间作精确连接，这对于存在复杂微分约束的运动规划问题是十分困难的
   (2) RRT的构建[2]
   1） 树的初始化：初始化树的结点集和边集，结点集只包含初始状态，边集为空
   2） 树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展（或连接）．若生成的轨迹不与障碍物发生碰撞，则将该轨迹加入树的边集，该轨迹的终点加入到树的结点集
   3） 重复步骤2），直至扩展到目标状态集中，相比PRM的无向图而言，RRT构建的是初始状态作为根结点、目标状态作为叶结点的树结构，对于不同的初始和目标状态，需要构建不同的树。另外，RRT不要求状态之间的精确连接，更适合解决像无人车运动规划这样的运动动力学问题
2. 基于采样的算法所面临的问题与解决办法
   几乎所有算法都有着几个相同的问题——求解效率与是否最优 解。PRM与RRT拥有概率完备性的原因在于其几乎会遍历构型空间 中所有位置，如图2中e所示。
   在提升求解效率方面，优化RRT的核心思想在于引导树向空旷 区域，即尽量远离障碍物，避免对于障碍物处的节点的重复检查，以 此提升效率，具体方法如下：
   （1）均匀采样
   标准RRT算法对状态空间均匀随机采样，当前树中结点获得扩展的概率与其Voronoi区域面积成正比，所以树会向着状态空间的空旷区域生长，均匀充满状态空间的自由区域。
   文献[3]提出RRT-connect算法，同时构建两棵分别起始于初始状态和目标状态的树，当两棵树生长到一起时则找到可行解。Go-biaing[4]是指在随机采样序列中以一定比例插入目标状态，引导树向目标状态扩展，加快求解速度，提高求解质量。
   文献[5]提出启发式RRT算法（heuristic RRT），使用启发式函数增加扩展代价低的结点被采样的概率，这个函数要求计算树中每个结点的代价，但是在复杂环境中，代价函数的定义往往是很困难的，为解决这一问题，文献[6]提出f-biased采样方法，先将状态空间离散化为网格，再使用Dijkstra算法计算每个网格上的代价，这个网格所在区域的点的代价值都等于该值，以此构建启发式函数
   （2）优化距离度量
   距离用来度量构形空间（状态