Preliminaries

基于采样的方法与基于搜索的算法不同，基于搜索的规划算法主要适用于低维空间的路径规划，也就是前两章布置的hw所处的一个环境之中，但是在高维空间往往会出现维数灾难。因此，基于随机采样的路径规划方法出现，以解决这样的问题。基于采样的路径规划方法不去苛求完备的c-space构造，更多的关注点在是否碰撞上，采样的方法不具备完备性，但是可以通过概率完备性趋近或替代完备性。

介绍几个概念：

• 概率完备性：随着采样样本不断增加，如果存在解，则一定能找到可行解。
• 渐进最优： 随着采样个数趋近于无穷，一定会无限接近找到最优解。
• Anytime：即时，能很快找到一个可行解，然后不断增量优化。

大概的发展：

分类：

Probabilistic Road Map(PRM)

简介

PRM方法，顾名思义，是利用随机采样的方式在环境中建立网络图，将整个连续空间转换为离散空间的问题后，利用之前A*算法等基于搜索的算法在构筑好的离散空间路线图上寻找路径，这样可以提高路径规划的效率。整个算法也分为两个阶段：Learning Phase 和 Query Phase。

Learning Phase 学习阶段

• 这个阶段即在C-space中先随机采样N个点，当然，要删除在障碍物内的点。
• 随后，尝试连接各个点，如果连线超过一定距离或者出现碰撞，舍弃该连线。
• 经过这样的操作后，得到了一个无碰撞的路线图。
  

Query Phase 查询阶段

如上所述，有了路线图后，我们即可带入之前介绍的A*或Dijkstra算法等寻找始终点之间的最优路线。

• 优点：概率完备性，相比于图搜索的比较高效（毕竟搜索的点少了很多）
• 缺点：两点边界值问题，在状态空间上构建图，但不特别关心所生成路径，而且效率不高。

Trick

因为，极端复杂或高维空间中的碰撞检测十分耗时，为尽量避免碰撞检测，可以采用lazy collision-checking策略。

lazy collision-checking

在撒点和构建图的时候，不管碰撞问题。在搜索到路径之后，再删去发生碰撞的部分，重新搜索其他路径。

Rapidly-exploring Random Tree 快速扩展随机数算法

简介

PRM是构建图结构，而RRT是构建树结构。从起始点开始在解空间中向目标点生长一棵树，来寻找可行解。

算法流程

算法的具体过程如下：

1. 将起点和终点加入到树结构；
2. 在构型空间中随机采样一个点$x_{rand}$；
3. 在已有的树结构中找到距离这个随机点最近的点$x_{near}$；
4. 从$x_{near}$连向$x_{new}$，并取一定步长，得到一个新的点$x_{new}$，如果从$x_{near}$连接到$x_{new}$如果没有碰撞，则连接起来，将$x_{new}$加入到树结构；
5. 如果$x_{new}$和目标点$x_{goal}$小于一定的距离，那么就尝试连接下$x_{new}$和$x_{goal}$，如果连接起来是无碰撞的，则找到目标点。否则则回到步骤2。

优点：

• 比PRM相比：更倾向于去寻找从起点到终点的路径，而不是构建整个空间结构的图。

缺点：

• 解不是优化的。
• 在整个空间采样。
• 不够高效，有很大提升空间。

缺点还是比较明显，尤其是解不是最优的，相当于是以搜索速度换取了最优性。随机选点选新点连线的时候，RRT单纯考虑了范围内最近点与新点的连接，这样的连接还是比较草率。

RRT*

简介

RRT算法中是将范围内最近的点$x_{near}$连向$x_{new}$，RRT*搜索$x_{new}$附近的范围内的节点，挑选$x_{new}$经这些节点到初始点cost最小的情况，选取该点作为$x_{new}$的父节点，并且后续会有剪枝的操作帮助寻找最优路径。

改进的连线策略

如下图，原本依照RRT算法$x_{new}$是要连接到$x_{near}$的，但是RRT*这里在$x_{new}$周围一定半径的范围中，搜索了所有的节点，并对$x_{new}$经这些节点到最初点cost进行计算，选取最小的作为$x_{new}$连接的父节点。

比如下图中$x_{near}$恰好是cost最小的节点，即两点连接，

range的确定

rewire function 重连接操作

新加入了节点，相当于加入了解空间的新的信息，那么这个新的信息能否影响到已经建立的树结构呢？这就涉及到rewire function，重连接的操作了。
如下图，引入添加过$x_{new}$后，还是看刚才一定范围内的点，这里以$x_2$为例。
$x_2$本来有一条经过$x_1$回溯起点的路径，但是添加过$x_{new}$后，$x_2$本来有一条经过$x_{new}$回溯起点的路径明显更短，那么该操作就要将$x_2$的父节点换为$x_{new}$。


相比于RRT算法找到终点就停止，RRT*算法找到路径之后还要继续进行优化，这也是为什么该算法可以一定程度上解决RRT解不是最优的问题。

一些改进的trick

• Bias Sampling ：设定一些概率，使得采样是向着目标点的。
• Sample Rejection：在已有一个解的代价$c^{\ast }$后，如果采样到一个点发现g+h已经大于$c^{\ast }$，则舍弃这个采样点。
• Branch-and-bound（Tree Pruning）：同样，已有$c^{\ast }$的情况下，如果tree的某一个支路到某一个节点时，它的g+h已经远远大于$c^{\ast }$了，那就删除这个支路（不一定是好的）
• Graph Sparsify：一个格子只留一个采样点，稀疏化（连续下不是最优的）。
• Neightbor Query：k最近邻或范围查询，K-D tree和range tree等搜索方法。
• Delay Collision Check：以某种顺序排序neighbours，提高效率（如果中途就找到想要的点的话）
• Bi-directional：起点终点两棵树两个方向一起找，相互连接终止。
• Condition Rewire：相当于先用RRT找到第一个解之后，再去做rewire。
• Data Re-use：针对对称节点，碰撞信息的复用。

Kinodynamic-RRT*

思路

为了更贴合机器人运动学规律，将RRT*点和点之间的直线连接变换为运动学路径。

Anytime-RRT*

思路

机器人在走的时候，实时进行路径规划，更新路径。

Advanced Sampling-based Methods

Informed RRT*

用RRT*找到路径后，限制在路径周围的椭圆中采点（椭圆多次迭代不断收缩）

Cross-entropy motion planning

当一个轨迹生成之后，就在轨迹周围的区域进行采样，这几个点就构成了多高斯模型，采点后就生成了多条轨迹，然后对多个轨迹进行均值和方差的计算，作为新的多高斯分布，

其他变体

Implementation

Open Motion Planning Library

Open Motion Planning Library
Moveit with ROS
Tutorials
以上在机械臂场景中用的比较多。

以上是移动机器人相关的实现。