从零开始的OMPL库算法学习（3）PRM算法
简介
PRM(Probabilistic Roadmaps) 是一种基于图搜索的方法，一共分为两个步骤：学习阶段， 查询阶段。它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。这种方法能用相对少的随机采样点来找到一个解，对多数问题而言，相对少的样本足以覆盖大部分可行的空间，并且找到路径的概率为1（随着采样数增加，P（找到一条路径）指数的趋向于1）。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。
其步骤：
1.学习阶段：
在给定图的自由空间里随机撒点（自定义个数），构建一个路径网络图。
a）构造步骤
b）扩张步骤
2.查询阶段：
查询从一个起点到一个终点的路径。
a）局部路径规划
b）距离计算
c）碰撞检查
伪代码
第一阶段：学习阶段
1-2 创建一个空的无向图集G=（V，E）
3 总共撒n个 点
4-7 随意撒点 ，若点没有碰撞到障碍，则加入V中。
第二阶段：查询阶段
9-10 对于所有的q点，寻求它的临近点组成集合Nq
11-13 对于集合Nq中的点，如果边（q,q’）不属于集合 E，而且q和q‘不是一个点，则把这条边加入E中。
这样就得到了所有的可行路径，再通过其他搜索算法检索即可。