路径搜索算法整理(二)随机采样篇

本文主要在于整理基于采样的路径搜索算法，关于图搜索的可以参考图搜索篇
优点：当环境中的障碍物较为复杂时，基于随机采样技术可以有效解决高维空间和复杂约束的路径规划问题。

一、PRM

参考文献：
L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
参考博客

PRM是主要分为两步：学习阶段和查询阶段。它将连续空间转换为离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。
绝大多数问题，相对少的样本足以覆盖大部分可行的空间，随着采样点的数量增加，找到可行路径的概率趋向于1。当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采样点的增加，也可以达到完备，故PRM算法是概率完备但不是最优的。

算法详解：

PRM学习阶段

1. 在给定地图的free space里随机撒点(自定义点数)，构建一个路径网络图。
2. 构造一个无向图的路径网络$R=(N,E)$，其中N代表随机点集，E代表所有可能的两点之间的路径集。
3. 将每个点与其K个最近邻点相连，去除穿过障碍的连线，得到PRM图(这里我觉得可以把起点和终点也当作一个采样点放入PRM，或者在附近再采样几个点，不然会出现起点或终点和PRM没有连线的情况)
   
   PRM查询阶段
   将起点和终点与PRM相连，利用A*搜索。
   

二、RRT

参考文献：
S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” Iowa State University, Tech. Rep. TR 98-11, 1998.
参考博客
实现方法，以2D空间为例：

1. 确定整个搜索空间的大小，起点和终点以及平面中的障碍物位置
2. 再搜索平面的范围内随机生成一个点$(x,y)$，记为$x_{rand}$。
3. 计算当前哪个已经存在的节点距离当前的随机点最近，对应图三的$x_{near}$。
4. 当找到$x_{near}$后，$x_{near}$朝着$x_{near}$与$x_{rand}$的连线方向走一个步长(自设参数)的距离得到$x_{new}$。
5. 判断能不能直接从$x_{near}$这一点走到$x_{new}$这一点，要考虑这两点的连线是否满足约束条件，约束条件很多，比如这两点的直线有没有障碍物，或者一些特殊模型的机器人两点状态能否满足动力学约束。
6. 如果满足约束，则将这个新的点加入到集合里，如果不满足，则回到第2个步骤，重新产生新点。循环直到和目标点的距离小于一个步长或者达到人为设定的迭代次数，算法停止。
   
   概率完备但非最优，只是可行路径。

2.1 基于概率的RRT算法

为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率p，根据概率p的值来选择树的生长方向是随机生长($x_{rand}$)还是朝向目标位置($x_{goal}$)生长，向目标生长的机制可以加速路径搜索的收敛速度。

2.2 RRT Connect算法

CMU参考课程
从起点和终点同时扩展随机树从而实现对整个空间的快速搜索。

2.3 RRT*

参考博客
目标在于解决RRT算法难以求解的最优的可行路径的问题，它在路径查找的过程中持续的优化路径，随着迭代次数和采样点的增加，得到的路径越来越优化。迭代的时间越久，就越可以得到相对满意的规划路径。
Rewire
RRT*在找到距离$x_{rand}$最近的节点$x_{nearest}$并通过障碍检测之后，并不立即将$Edge(x_{nearest},x_{rand})$加入扩展树。而是以$x_{rand}$为中心，r为半径，找到所有潜在的父节点集合，并与$x_{nearest}$父节点的Cost对比，看是否存在更优Cost的父节点。

随机树重布线
参考博客
在为$x_{new}$重新选择父节点之后，重布线使得生成新节点后的随机树减少冗余通路，减小路径代价。

总结

基于采样的路径搜索可以灵活地与系统状态输入相结合，这不是基于采样的方法的优势。通过这种方式，来自环境和动力学约束的复杂性将会被克服。现在路径搜索常常是随机采样和A*这类图搜索法结合，这样不仅可以加速找到路径而且也能保证所找的路径趋近于最优。