七、动作规划

7.1、动作规划的任务以及基本概念

规划控制的基本任务就是在感知到周围环境的基础上，结合无人车的目的地和当前位置对于车辆做出最合理的决策、规划和控制。整个决策规划控制软件系统按照解决问题的不同层面，从上至下可以分为全局路径规划、行为决策、动作规划、动作控制。

在动作控制模块，我们需要在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地。

动作规划的本质，是在车辆给定状态给定环境下，寻找到一条最优路径，使车辆移动到目标点。所谓的最优，不仅需要考虑到行驶距离，还要考虑道路上其他车辆、舒适性以及安全性。

在介绍常见的规划器之前，首先介绍规划器的一些属性，如图所示

我们一般从以下几个维度来判断规划器的优劣程度

（1）完备性，即是不是在任何情况下都能找到一个从起点到终点的路径，它又分为概率完备性和决策完备性；

完备性：完备性指如果在起点和终点之间有路径存在，规划器一定能求出一个解，如果求不出解，则说明起点和终点之间不存在路径；

概率完备性：概率完备性指只要起点与终点之间有路径存在，只要规划或者搜索的时间足够长，就一定有路径解；

决策完备性：决策完备性指只要有最优路径存在，通过一定的时间，就一定能找到最优解；

（2）效率，即完成规划计算的快慢程度；

一般使用大O表示法来反映算法的效率；

（3）最优性，即规划出来的路径是不是最优的；

（4）复杂度，即这个方法能不能求解一个复杂的问题；

（5）最后是规划器在不同的约束下的表现，其中包括路径的约束和运动的约束；

7.2、基于采样的路径规划

7.2.1 RRT算法

RRT 算法全称为Rapid-exploration Random Tree，即快速搜索随机树法，是一种基于采样的路径规划方法，在多维空间中搜索效率不低。算法在初始时随机树只包含路径起点，以其作为随机树的根节点，通过随机采样不断产生新的节点，各节点相互连接形成一棵向外扩展的随机树，当目标点也被划入随机树中则搜索路径成功。

RRT算法的流程图如下所示：

根据流程图可知 RRT 算法具体步骤如下：

（1）当算法初始化时，随机树内只有根节点$Q_{init}$，也就是路径规划的起点，算法开始对随机树进行扩展，得到第一个随机采样点$Q_{rand}$；

（2）如下图所示，通过计算随机树中每个节点到$Q_{rand}$的欧氏距离，从随机树中选择距离$Q_{rand}$最近的节点$Q_{nearest}$；

（3）从最近点$Q_{nearest}$向随机点$Q_{rand}$方向生长一段距离，得到新节点$Q_{new}$。如果$Q_{new}$的生长过程与状态空间中的障碍物无任何碰撞，则将其加入随机树中，否则放弃此次生长；

（4）重复进行随机采样，随机树不断加入新的节点，直到某一次得到的新节点是目标点Qgoa或与其距离小于提前设定的阈值，则说明随机树已生长到目标点位置，并将其也加入到随机树中;

（5）从目标点$Q_{goal}$反推回起点$Q_{init}$，无需搜索便可得到无障碍路径。

RRT 算法优势在于：

（1）算法结构简单，搜索域适应性较广，随机采样将搜索导向空白区域，避免了对空间的建模过程，且搜索路径具有概率完备性；

（2）在考虑微分约束而容易产生多维度问题的情况下，不会出现基于几何构造等传统常规算法易出现的维度灾难问题，可直接应用于非完整约束和多自由度机器人的运动规划中；

（3）得到的由多个分段曲线组成的路径，完全符合移动机器人等系统受到的非完整微分约束，可被其直接跟踪。

而RRT算法缺陷如下：

（1）路径规划结果具有随机性，得到的路径不一定是最优路径，且往往棱角分明，不够平滑，若应用于无人驾驶汽车则会造成车辆航向角突变角度较大，不满足车辆运动学约束；

（2）算法缺乏导向性，当状态空间中狭窄通道较多时，较难找到出路，导致局部节点数量增加，路径搜索效率降低。

7.2.2 RRTstar算法

RRTstar算法针对RRT算法路径非最优这一问题，在其基础上进行改进，不断调整随机树的结构，使得当前结构为最优，随着算法不断的迭代，最后得到的路径也为最优路径，这是一个渐进最优的过程。

RRTstar算法之所以能够得到最优路径，是因为该算法在不断获得新节点的同时会为其更新代价更小的节点作为父节点，比RRT算法多了一个重新连接构造随机树（rewire）的过程。算法主要思路为：得到的节点$Q_{nearest}$和$Q_{new}$并不会直接连接，而是在局部进行优化处理，当有更优路径时，$Q_{new}$会重新选择父节点，随机树的结构也会发生改变。具体步骤如下：

（1）初始步骤和RRT 算法一样，当得到一个节点$Q_{new}$且通过碰撞检测后，以$Q_{new}$为圆心，在半径为R的范围内找到若干个$Q_{near}$；

（2）分别计算$Q_{near}$到起始点$Q_{init}$的距离以及$Q_{near}$到$Q_{new}$的距离，并相加，此距离即为$Q_{new}$经过重新找到的$Q_{near}$后到达起点的代价$C$，代价最小值为$C_{min}$，该$Q_{near}$记为$Q_{min}$；

（3）若$C_{min}$小于$Q_{new}$经过原本父节点$Q_{nearest}$后到达$Q_{init}$的代价时，则更换$Q_{new}$的父节点为$Q_{min}$，连接$Q_{new}$和$Q_{min}$；

（4）在$Q_{new}$重新选择父节点后，对范围内其他$Q_{near}$包括之前的父节点$Q_{nearest}$进行重新检查，假设$Q_{new}$为它们的父节点，计算该节点经过$Q_{n}ew$后到达起点的代价$C_{new}$，并与原本代价$C_{old}$进行比较，若$C_{new}<C_{old}$，则将$Q_{new}$作为它们的父节点，并重新构造随机树，反之不变。

RRTstar算法的改进方向主要包含选择代价更小的父节点和重新构造随机树两部分，两者互相配合，在使新节点$Q_{new}$构成的路径代价尽可能小的同时，减少随机树的冗余通路，所以最后得到的路径是最优路径，这既是 RRTstar算法的优势，也造成了其不可避免的问题，即并不是每一次重构的节点都与路径相关，过多无用迭代导致算法搜索时间变长，效率大幅度降低。

7.2.3 RRT碰撞检测

RRT算法中最复杂的一步就是碰撞检测，关于RRT的碰撞检测，可以参考博文RRT与碰撞检测Atarasin的博客-CSDN博客rrt碰撞检测

7.3、Lattice planner

看着篇就挺好

Lattice Planner规划算法原理 - 知乎 (zhihu.com)

7.4 结语

随后两节课的内容是决策以及全局路径规划的部分，决策部分只是讲了框架，理论的部分很少，而全局路径规划部分介绍了A_star以及Dijkstra两种全局寻径算法，以前也学过了，所以这两节就不记录。

这一段的学习就此结束，下一阶段主要还是把仿真环境搭建起来，学习一些开源框架以及验证算法。