全局规划总结

参考
有哪些应用于移动机器人路径规划的算法
无人驾驶（三）——汽车路径规划概述
运动规划 | 简介篇

路径规划的方法, 两大类：

1. 基于Grid, 完备的（complete)
2. 基于采样的（sampling-based）又称为概率完备的

0. 较为粗糙的算法

Walk To算法

最简单的“算法”应当数Walk To算法了，顾名思义，就是让机器人直接从当前位置朝目标点走，直到到达目标点为止。

Bug算法

Bug算法就是为了应对一些简单的障碍物而提出的，其说明如下：

• 沿着起始点与终点的连线M运动；
• 遇到障碍物，则绕着障碍物顺（或逆）时针运动，直到回到连线M。
  
  这个算法在二维空间内是完备的，但其路径不能保证最优。

1. 完备的规划算法

所谓完备就是要达到一个systematic的标准，即：如果在起始点和目标点间有路径解存在那么一定可以得到解，如果得不到解那么一定说明没有解存在。

1.1 Dijkstra

1.2 A*算法

Dijkstra算法是典型的广度优先搜索算法。它是一个按路径长度递增的次序产生的最短路径的方法，是求解最短路径的经典算法之一。Dijkstra算法是一种贪心算法，它在每一步都选择局部最优解，以产生一个最优解。这也会导致该算法的时间复杂度较高，在图规模较大时，该算法的计算速度慢，很难满足路径规划实时性的要求。

A* 算法是经典的启发式搜索算法，它是由Dijkstra算法改进而来的。其最显著的特点就是在搜索过程中增加了启发函数，通过给定启发函数来减少搜索节点，从而提高路径搜索效率。研究表明，A*算法搜索得到的路径能够同时满足实时性和最优性要求。

这一大类算法在移动机器人领域通常直接在occupancy grid网格地图上进行规划（可以简单理解成二值地图的像素矩阵）以深度优先寻路算法、广度优先寻路算法、Dijkstra(迪杰斯特拉)算法为始祖，以A算法(Dijstra算法上以减少计算量为目的加上了一个启发式代价)最为常用，近期的Theta算法是在A*算法的基础上增加了line-of-sight优化使得规划出来的路径不完全依赖于单步的栅格形状（答主以为这个算法意义不大，不就是规划了一条路径再简单平滑了一下么）。

• 优势: 对于解的捕获能力是完全的，
• 缺点:　但是由此产生的缺点就是算法复杂度较大。这种缺点在二维小尺度栅格地图上并不明显，但是在大尺度，尤其是多维度规划问题上，比如机械臂、蛇形机器人的规划问题将带来巨大的计算代价。

2 基于图方法

2.1 Visibility Graph可视图法

• 可视图法用封闭多面体描述障碍物；并将连接所有多面体的所有顶点，去除与障碍物相交的连线；之后将起始点与目标点与所有顶点连接，并去除与障碍物相交的连线。由此可得到一个图。
• 可以证明，该方法完备且最优。

2.2 基于广义Voronoi图

3. 基于采样的规划算法

这种算法一般是不直接在grid地图进行最小栅格分辨率的规划，它们采用在地图上随机撒一定密度的粒子来抽象实际地图辅助规划。

如PRM算法及其变种就是在原始地图上进行撒点，抽取roadmap在这样一个拓扑地图上进行规划；
RRT以及其优秀的变种RRT-connect则是在地图上每步随机撒一个点，迭代生长树的方式，连接起止点为目的，最后在连接的图上进行规划。

• 优势:　这些基于采样的算法速度较快．
• 缺点:　但是生成的路径代价（可理解为长度）较完备的算法高，而且会产生“有解求不出”的情况（PRM的逢Narrow space卒的情况）。
• 应用场景： 这样的算法一般在高维度的规划问题中广泛运用。

3.1 概率图算法（Probabilistic Road Map， PRM）

概率图算法是在规划空间内随机选取N个节点，之后连接各节点，并去除于障碍物接触的连线，由此得到一个可行路径。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但可以增加采样点使该算法达到完备，所以PRM是概率完备但不是最优的。

3.2 快速随机扩展树算法（Rapidly-exploring Random Tree，RRT）。

快速随机扩展树最初主要用于解决含有运动学约束的路径规划问题。由于RRT在状态空间中采用随机采样确定扩展节点，不需要预处理，搜索速度快。因此这种算法作为一种快速搜索算法在路径规划问题中获得广泛应用RRT算法

优势与不足

1. 优势

• 快。因为它是随机采样，不需要对空间中所有的点都进行搜索。
• 不需要对障碍物空间进行精确建模。因为在碰撞检测时它只需要知道给定节点是否为障碍物就可以，而不需要知道障碍物的范围、形状、大小和精确的模型表示。
• 在多维空间中更具优势。维数越高，节点状态就越多，对于传统路径规划方法，很容易导致维数灾难，而RRT为随机采样，只需要随机更多的维数，而不需要采样空间中的每一个状态，因此维数的升高对RRT的效率并不会有太大的影响。

2. 不足

• 最显著的一点就是，因为RRT采样的随机性，其得到的路径有很多冗余的节点，增加了路径的长度。当然，现在也有许多的优化方法去得到次优和最优的路径。
• “窄缝就死”

最常用也是最简单的优化方法就是剪枝，裁剪掉不必要的节点。如下图所示，a,b两段就可以用c来替换，由三角不等式可知，替换后的路径更短且中间的冗余节点可以被裁剪。

4 Experience-based（Experience Graph经验图算法）算法

基于经验的规划算法，这是一种存储之前规划路径，建立知识库，依赖之进行规划的方法，

• 比较:　这种方法牺牲了一定的空间代价达到了速度与完备兼得的优势。此外还有基于广义Voronoi图的方法进行的Fast-marching规划，类似dijkstra规划和势场的融合，该方法能够完备地规划出位于道路中央，远离障碍物的路径。

高维度将会带来两个问题：①求解计算量增加；②障碍物无法描述。要看一个规划算法能否用在机械臂，就应该看它能否应对这两个问题。我们一个个看。

• Walk To：这个算法当然在任何维度下都能直接使用，当然，它完全无法应对障碍物等情况，我们甚至不把它称为规划算法。

• Bug：因为Bug算法需要绕障碍物顺（或逆）时针旋转，高维空间不存在这样的方向，所以自然是无法使用的。

• 蚁群算法：依旧是收敛慢，用的人不多。

• 人工势场法：由于势场法需要在C-Space中构建人工势场（与障碍物在C-Space中的分布有关），所以经典的人工势场法无法直接用在高维机械臂上。当然，研究人员想出了解决方法，例如在机械臂上放几个控制点（Control Pointss）；在工作空间建立势场函数后，对每个控制点施加势场力；最后通过力与雅克比的关系（可由虚功原理推导出），将控制点处的力转换到每个关节力矩；关节力矩就是构形空间的势场力。

• 人工势场法在多自由度机械臂中实时性好，可以与控制算法结合在一起。但是，依旧是既不完备也不最优，所以很有可能遇到无法求解的情况。

• 可视图法：与Bug算法类似，可视图法需要确定障碍物在C-Space中的分布情况，因此无法用在高维空间中。

• 空间离散法：理论上讲，空间离散法可以适用于任何维度的空间，但问题是它会引入很大的计算量。例如对于一个六自由度机械臂，我们按照6°分辨率（已经是很低的分辨率了）划分网格，那么将会产生60^6=4.67×1010个网格，单是对每个网格进行碰撞检测（如果碰撞检测速度为0.1ms），就需要1296小时。除了碰撞检测，规划算法的计算量也是随着空间维度指数增长的。所以一般大于3自由度的规划问题，空间离散法就不具有实际应用价值了。

• 概率路图法PRM和快速拓展随机树法RRT：这两种算法的相同点是，他们均不考虑障碍物在C-Space中的分布情况，而是采用碰撞检测函数对几个随机采样点进行碰撞检测。因为被催着要去吃饭了，所以直接说结论，PRM和RRT算法在高维空间中都可以使用，且满足概率完备性（非最优），规划速度相当快。

• 用RRT规划的机械臂
  虽然基于采样的规划算法速度很快，但他们也有致命的缺点，那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，这就使得自动规划的机器人无法进入工业领域（极端追求稳定性）

路径规划需要考虑的问题

1. 至于D* 、势场法、DWA(动态窗口法)、SR-PRM属于在动态环境下为躲避动态障碍物、考虑机器人动力学模型设计的规划算法。
2. 现实环境远比这要复杂，良好的规划必须建立对周边环境的深刻理解，另外还需要建立大量的数学方程，以及需要考虑障碍物、车道线、路径曲率、曲率变化率以及车辆速度、加速度等多种因素的影响。