六、规划控制——动作规划（1）

概要：本文主要学习动作规划的任务及基本概念。

---

前言

本文将对动作规划的基本概念及术语等进行了解研究。

---

一、动作规划概念

规划的本质就是搜索，从数学的意义上就是给出一个函数找到其最优解，比如谷歌、百度的搜索器，给出一个词条找出你感兴趣的东西做排序。
动作规划的本质也是搜索，在给定的环境状态下，找到无人车移动的最优解，也就是说车辆的某一个状态和其动作有关联，车辆寻找每一个动作去映射到每一个状态上，通过这个动作完成动作到状态的映射。
现代的动作规划方法：

1. 通过大数据的方法找到动作与状态的映射关系。

传统的动作规划方法：

1. 基于搜索的全局路径规划（global path planning）

2. 基于优化的fuction optimization
   

3. 基于采样的方法
   
   
   

4. 其它方法（属性类型）

二、动作规划性质（规划器）

1. Completeness（完备性）
   即能不能从起始点到终点找到一个路径，包括概率完备型、决策完整型。
2. Alogrithm Efficiency（算法效率）
3. Optimality（优化型）
4. Complexity（复杂性问题的解决）
5. Constrain（约束下的表现情况）
   （1）路径约束
   （2）运动约束

2.1 Completeness

1. 完备性：指如果在起点和目标点之间有路径存在，那就一定可以得到一个最优解，如果得不到则就不存在这个解，如Narrow Gap Problem Examples 窄路不能通过。
2. 完备规划器（complete planner）：在一定时间内，规划器总是能够找到一条从起始点到终点的路径，如A*算法。
3. 概率完备性（Probabilistic Completeness）:起始点和目标点之间有路径存在，如果规划和搜索的时间足够长，可以通过采样的方法，就一定能够确保有解找到这个路径如RRT、BIT*算法。
4. 决策完备性（Resolution Completeness）：原理和概率完备性很相似，不过它的采样是比较确定的，一般是在网格上进行采样。

2.2 Alogrithmic efficiency

通常，一个算法最重要的评价指标就是执行时间，尽管算法分析还需要考虑到运行所需要的内存空间，但是随着科技的发展空间已经不是考虑的因素，所以首先需要时间复杂度：

1. 时间复杂度表示（线性）：
   
   通常用O表示。
2. 算法的执行时间和输入的数据量是成正比。

2.3 Optimality

1. 规划得到的路径在某一个评判指标上是最优的；
2. 如果没有最优的，考虑渐进最优性，在有限次的规划迭代中得到的接近最优的次优路径，他会逐渐收敛，每次迭代接近最优。

2.4 Complexity

1. Space dimensionality（维度空间）
   对于刚体来说它是3维的空间，对于非刚体（大的拖车、单车模型）用三维的模型是不准确的。
2. Geometric complexity
   目标物和目标物如何相交的；
   多边形和多边形的轨迹探索。

2.5 路径约束

1. Local Constraints（局部约束）
   避免与障碍物的碰撞；
2. Differntial contrains
   对于车辆的路径曲率和方向盘转角的限制；
3. Global constarins
   寻找最短的路径。

2.6 Holonomic vs non-holonomic constraint

完整性约束：可控自由度等于全部自由度；
非完整性约束：可控自由度小于全部自由度；

三、路径规划

运动规划的实质：搜索出一条好的路径。

3.1 where to search

那如何搜索一条好的路径呢？开始研究时，人类往往以质点模型出发去研究，但是会变成两个点，在数学上，点和点之间是没有交集的，没有办法碰撞，实际上车辆与车辆是可能相撞的，所以实际需要抽象一个车辆的模型去解决车辆与障碍物碰撞的问题。

configuration space

为了解决上述问题，提出了构造空间的概念，就是其处理的工具。

1. 机器人学：考虑实际的物理限制、刚体动力学的问题；
2. 控制理论：关注稳定性和反馈，对于未来趋势的考量（未来速度空间可能在未来存在碰撞）；
3. 人工智能：考虑逻辑和执行动作，从初始的状态变化到期望的状态，Belief Space（信念空间）、马尔可夫的决策过程等。

3.2 How to search

组合方法 （Combinatorial methods）

它是一组精准完整的解决方案，如可见图（visibility graph）

它是一种把起点、终点等所有节点连起来构建的路线图。
节点：包括初始的起点、终点、所有障碍物的顶点，将不与障碍物相交的所有点连起来，包括障碍物的边缘，建立好可视化图表之后，可以利用搜索的算法从开始到终点最短的路径。
**缺点：**搜索路径往往是贴着障碍物走，没有留下一些空间，会增加碰撞的风险。

抽样方法（Sampling-based methods）

1. 确定的采样点（决策完备性）
   包括网格、道路结构图。
2. 随机性采样（概率完备性）
   包括随机抽样方法，需要平滑和后处理提升路线构建的质量。

3.3 随机采样方法（Stochastic Sampling Methods）

传统的路径规划算法：人工势场法、遗传算法、神经网络、蚁群优化算法等都需要在确定的空间内对障碍物进行建模而且计算的复杂度和机器人的自由度成指数关系，不适合解决多自由度环境的复杂规划问题。

RRT

基于RRT的规划算法是通过状态空间的采样点进行碰撞的检测，避免对空间的建模，能够有效解决高维规划空间和复杂约束规划问题。
RRT特点：
（1）快速有效搜索高维空间，通过状态空间的随机采样点，来将搜索导向空白的区域，从而寻找到一条从起始点到目标点的规划的路径。
RRT应用：
（1）适合解决多自由度复杂环境中动态规划问题。
RRT伪代码：

RRT优点：
（1）容易实现，比较直观；
（2）找到从起始点到目标点的一条路径；
（3）针对于空间中未探索的部分。
RRT缺点：
（1）不是最优解
（2）不是特别高效
（3）整个空间的采样

RRT*

一般来说，RRT算法是一个相对高效率的算法，同时可以很好处理带有非完整约束的轨迹规划问题，在很多方面都有很大的优势，但是RRT算法并不能保证其得到的可行性路径是相对优化的，因此需要对RRT算法进行改进，来解决优化问题，RRT*算法就是优化算法之一。

RRT特点：
（1）可以快速找出初始路径，随着采样点的不断增加，不断进行优化，直到达到目标点或者达到最大循环的次数；
（2）渐近优化，随着迭代次数的增加，得到的路径越来越优化；
（3）RRT生成路径的代价比RRT小了许多，减少了时间成本。

RRT*与RRT的区别：
（1）重新选择父节点（xnear）：路径代价相对小；
（2）重新布线：使新节点之后的随机数减少冗余。

3.4 Obstacle avoidance （障碍物避障）

为了简化问题，路径规划的障碍物会选择比较规范的几何形状：圆、多边形、长方形等等。

circle object （圆形目标物）

圆形的边界属于非线性的问题，通常将圆形进行线性化处理：
只需判断Xnew是否在圆形外接的正方形之内，如果是则意味着碰撞。

polygon：rectangle（长方形障碍物）

Xnear与Xnew的连线的边不能与长方形障碍物任何一边相交。

1. Xnear和Xnew在矩形一边，则不与矩阵相交。
2. Xnear和Xnew在矩形一边的不同侧：
   （1）Xnew在矩形内部，则一定相交；
   （2）Xnear和Xnew都在矩形外部（斜率方法求解）：
   
   如果满足下式，则会发生碰撞。
   

3.5 其它改进方法

插值拟合

在实际应用中，利用RRT或者RRT*得到的路径往往存在不连续性，车辆的跟踪难度和舒适度都会受到影响。
所以，根据已有的Path的点集和曲率，利用插值拟合的方式拟合出一条可以行驶的路径，具有连续性和高可导性，对于车辆的跟踪和舒适度都会有所提高。
插值拟合方法：
3. Dubins
满足曲率约束和规定的始端末端切线方向条件下，连接两个二维平面（x，y）的最短路径，只适用于车辆前进的场景。
4. Reeds-Shepp
适用于车辆倒退、前进的场景。
Dubins和Reeds-Shepp由于几何的固定性，可以很准确的计算车辆和路径的距离。
5. Spline,clothoid,Bezier可以提高插值拟合的性能。

求解效率

核心思想：靠近目标，引导树向更空旷的区域，尽量远离障碍物，避免在障碍物处的重复检查，以提高求解效率。

1. 偏置采样（Bias Sampling）
   使采样点尽量偏向目标点，在随机采样的过程中，以一定的比例插入目标状态，以引导树向目标状态扩展，加快求解速度，提高求解质量。
   标准的RRT算法对空间的采样是均匀的，如果在均匀的采样点中加入目标的点，可以引导树向着目标靠近的更快一些。
2. 拒绝样本（Sample Rejection）、树剪枝（Tree Pruning）、图表稀疏（Graph Sparsify）
   拒绝不好的样本，提高样本成功的几率。
3. Delay Collision Check
   对每一个输入对应的轨迹节点和碰撞障碍物加一个惩罚，惩罚越高，节点扩展的概率越小，可以限制采样区域在当前树的局部区域防止靠近目标节点反复的扩展失败来提高算法的效率。
4. Anytime RRT
   快速构建RRT，并获得可行解的记录，记录其代价，之后再进行采样，这样算法只对有利于降低可行解损失的地方插入更多的树，从而获得更优的可行解。

---

总结

本文主要是对于自动驾驶规划控制中的动作规划进行学习，主要介绍了动作规划的概念、性质以及随机采样的方法，并对RRT和RRT*做了比较，简要讲诉了其提升的方法，这篇文章希望可以对想要学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!