目录
1 基于采样的路径规划的优点和一些重要概念
2 概率路图 Probabilistic Road Map
3 快速搜索随机树Rapidly-exploring Random Tree
3.1 RRT
3.2 RRT Connect
4 RRT算法的优化
4.1 RRT*
4.2 Kinodynamic-RRT*
4.3 Anytime-RRT*
5 Advanced Sampling-based Methods
5.1 Informed RRT*
5.2 Cross-entropy motion planning
6 实际应用
1 基于采样的路径规划的优点和一些重要概念
不构建 C 空间及其边界： 与传统的规划方法不同，采样型规划器不需要显式地构建配置空间（C-Space）及其边界。这使得算法更加高效，因为它不需要在内存中存储整个空间的表示。
只需检测单个机器人配置是否碰撞： 算法只关注单个机器人配置是否与障碍物或环境中其他物体发生碰撞。这样的检测方式更加简洁、高效。
利用简单的碰撞测试： 采样规划器利用简单的碰撞测试来确定机器人是否与障碍物相碰。这些测试可能是基于几何形状或其他形式的预定义规则，使得计算开销相对较小。
碰撞检测作为独立模块： 碰撞检测是作为一个独立模块实现的，可以根据具体的应用场景进行定制。这种模块化的设计使得算法更具灵活性和可扩展性。
随着碰撞检测的改进，算法也改善： 如果碰撞检测方法得到改进，采样规划器的性能也会相应提高。更精确、更快速的碰撞检测方法能够带来算法整体性能的提升。
单一查询和多次查询请求的不同处理方式： 采样型规划器针对单一查询和多次查询请求采用不同的处理策略。对于单一查询，它可能会采取特定的优化策略以提高规划速度；而对于多次查询，可能会利用之前的计算结果或其他缓存来加速规划过程。
总的来说，基于采样的规划器通过简化碰撞检测和对空间的直觉理解，提供了一种高效、灵活的路径规划方式，特别适用于处理大规模环境下的路径规划问题。
我们来看几种不同的规划器：
完备规划器（Complete Planner）： 这类规划器总是能在有限时间内正确地回答路径规划查询。无论问题的复杂性如何，它们都能够保证找到一个解（如果解存在的话）。
概率完备规划器（Probabilistic Complete Planner）： 如果存在解，这类规划器最终会找到它，使用随机采样方法进行规划（比如蒙特卡罗采样）。它们不像完备规划器一样绝对确保在有限时间内找到解，但在理论上，长时间运行下，它们能够以高概率找到解。
分辨率完备规划器（Resolution Complete Planner）： 类似于概率完备规划器，但它们基于确定性采样（例如在固定网格上进行采样）。这类规划器也能够在理论上保证找到解（如果存在的话），但采用确定性方法而不是随机方法来达到完备性。
2 概率路图 Probabilistic Road Map
如何在空间里面构建一个图：
学习阶段（Learning phase）：
- 这个阶段可能涉及对环境或空Lazy collision-checking
· Collision-checking process is time-consuming, especially in
complex or higLazy collision-checking
· Collision-checking process is time-consuming, especially in
complex or high-dimensional environments.
· Sample points and generate segments without considering
the collision (Lazy).h-dimensional environments.
· Sample points and generate segments without considering
the collision (Lazy).间的学习和了解，以便后续的路径规划。查询阶段（Query phase）：
- 在这个阶段，图结构被用于规划路径。
- 通过高效地检查采样的配置和它们之间的连接，以确保碰撞检测的准确性。
- 仅需采用相对较少的里程碑和局部路径就能够捕捉自由空间的连通性。
这种方法的关键在于利用图结构的连接性质。在查询阶段，只需要检查采样的配置和它们之间的连接情况，而不需要对整个空间进行详细的碰撞检测。通过选择相对较少的关键点和局部路径，就能够有效地捕捉自由空间的连通性，从而简化了规划过程。
查询阶段：随机、均匀撒点并删去障碍物区域的点
采样过后将这些点连接起来：起始点终止点要被连接到网络，并且还要有一定的距离要求，距离太长的不要（计算效率）（红线），经过障碍物的不要（绿线）。
查询阶段：用A*或Dijkstra算法规划路径。
需登录Nvidia账号才能下载，登一下吧。
优点：
- 具备概率完备性（Probabilistically complete），在一定条件下能够找到解决方案。
缺点：
- 需要解决两点边界值问题（2 point boundary value problem）。（两点机器人是否可行是否满足运动学约束）
- 在状态空间上构建图，但没有特别关注路径的生成。
- 效率不高。
这种方法的概率完备性意味着在一定概率下能够找到解决方案，但它也存在一些限制，例如需要解决特定类型的问题以及对路径生成的关注不足，可能导致效率较低。
- 碰撞检查过程耗时： 特别是在复杂或高维环境中，碰撞检查是一个耗时的过程。
- 采样点和段的生成不考虑碰撞（懒惰式）： 在这种策略下，路径规划算法会先生成可能的路径段或采样点，而不进行实时的碰撞检查。只有在需要时才会对这些路径段或采样点进行碰撞检查，以减少计算开销。
懒惰式碰撞检查（Lazy collision-checking）允许算法在生成路径时暂时忽略碰撞检查（只检查距离准则），从而提高了路径规划的速度。然而，这也可能意味着在后续的检查阶段需要更多的计算，以确保生成的路径是collision-free的。
用A*生成的路径毫无意外有碰撞！当然，这些不可行的路径我们要去删掉。重新进行路径规划！
可行，但如果又不可行呢？？继续迭代！
概率路图：学习阶段、寻路阶段、改进模式Lazy collision-checking。
3 快速搜索随机树Rapidly-exploring Random Tree
3.1 RRT
Build up a tree through generating “next states” in the tree by executing random controls.
伪代码如下：找一条红色点到绿色点的路径。
第一步就是我先得到一个采样点（蓝色的点），找到了从红色点到蓝色点的路线，由大红色的点向小红色的点移动一小段距离derta，如果这段线没有障碍物，则加入树中完成树的扩展。
Sample函数在地图中进行随机采样获得了Xrand
在树中找到一个距离Xrand距离最近的点Xnear。
我们找到Xnew，如果没有碰撞的话我们加入生成树中。
如果有碰撞呢？不会连通到树中呗～
‘ 树不停的扩展，如果树周围的点被添加，路径规划完成。
可以看看RRT算法的demo：
https://www.youtube.com/watch?v=pOFtvZ_qVsA
https://www.youtube.com/watch?v=pOFtvZ_qVsA
那么它的优点和缺点是什么：
优点：先对于PRM更有针对性的找到一个起点到终点的路径。
缺点：并不是最短（优秀）路径、也没有那么高效（狭窄区域）、取样区域不明确。
KD Tree改进算法：
1.用KDTREE寻找near节点：
左图产生了RRT的树，假设我们新到了一个采样点，我如何在这个树上找到一个离这个点最近的点呢？
把所有的点都比较一下？？？效率太低了。KD tree就是个非常好的工具。
那么我们首先把所有的点包括起点终点都用KD tree要求的格式去保存，当新到一个点Xrand时候我们可以用KD tree算法找到距离它最近的点。
我们看看具体原理：
左图就是KD tree对空间的一个划分，具体方式是横向找一个所有节点x的中位数（4.0，4.7）划分为左右两枝，然后在找到纵坐标两边的中位数（1.9，2.2）（6.7，4.3）继续这样的划分....就构建了KD tree。二叉树在计算机里面搜索效率是非常高的。
3.2 RRT Connect
这个是从起始点和终点同时构建两棵树。
我得到了Xrand从左面和右面同时寻找随机邻域同时扩展，也就是撒一个点完成两棵树的构建，那么这个树什么时候生长截止呢？当Xrand同时连接左右两棵树的时候生长截止。
对于狭窄点RRT Connect同时生长两棵树可以解决问题。但他不是一个唯一的解决方案，很多人通过改进采样函数来解决问题。
4 RRT算法的优化
我们回顾RRT算法的缺陷：
1.生成路线并不是最短或者最优的
2.路径通过直线连接，并不是很光滑的适合机器人去执行的路线
4.1 RRT*
我们先看RRT*的伪代码：
看看它的执行过程：
我们首先通过采样在空间中得到点Xrand，在Xrand周围搜索找到它的最近的邻域节点Xnear，通过Xnear向Xrand移动一定距离我们得到新的节点Xnew。在RRT中我们直接将Xnear和Xnew连起来完成树的扩展。
在RRT*中，我们不将Xnear和Xnew连起来，我们直接去搜索Xnew周围的范围找到Xnew附近的一些节点，如下图：
在RRT*中，将这些节点都与Xnew连接起来：
算一下从起点到Xnew哪一个花费比较小（起点到Xnear -- > Xnew）（起点到Xnear -- > x1 --> Xnew）（起点到Xnear -- > x2 --> Xnew），这时候就可以选择Xnew的父节点添加边了。
还没完呢....我们现在进行优化树，现在我们已经把Xnew添加到了树里面。现在我们考虑X1，X1原来有一个到达起始点的路径（起始点 --> Xnear --> x1），现在我们判断（起始点 --> Xnear --> Xnew）新路径的代价，如果新的路径短的话，我们X1就通过Xnew到达初始节点了。
我们对X2进行修改，改变其父节点进行剪枝：
https://www.youtube.com/watch?v=YKiQTJpPFkA
4.2 Kinodynamic-RRT*
无论是RRT还是RRT*，我们最终生成的路径是一条线段，对于机器人来说是很难实现的。
这里改进的是steer fuction，我们得到了一个点Xnew，我们之前用直线链接Xnew和Xnear，我们可以看到右面的路径我们生成了更符合运动学约束的曲线。
直线遇见障碍物在RRT*中会抛弃，而 Kinodynamic-RRT* 会规避。
4.3 Anytime-RRT*
在导航的过程中还会更新树。Keep optimizing the leaf RRT tree when the robot
executes the current trajectory Anytime Fashion。更好的适应环境的变化。
5 Advanced Sampling-based Methods
5.1 Informed RRT*
将采样的范围限制在了椭圆中。
当我们生成路径时，我们将随机采样点限制在椭圆中。
那么如何去构建一个椭圆呢？
以起点、终点作为椭圆的交点。以我们生成的轨迹的长度作为椭圆到达两个焦点之和。
5.2 Cross-entropy motion planning
首先我们生成了一条路径。
我们在轨迹的节点周围进行采样，六个圈。构成多条路径。
生成多个轨迹后，我们对多个轨迹多个节点求均值，可以得到一个新的分布。得到下一轮的采样点。
6 实际应用
Open Motion Planning Library集成了很多路径规划的算法。
The Open Motion Planning Library
MoveIt Motion Planning Framework Incorporating the latest advances in motion planning, manipulation, 3D perception, kinematics, control and navigation.
