路径规划: PRM路径规划算法

路径规划作为机器人完成各种任务的基础，一直是研究的热点。研究人员提出了许多规划方法：如人工势场法、单元分解法、随机路标图（PRM）法、快速搜索树（RRT）法等。传统的人工势场、单元分解法需要对空间中的障碍物进行精确建模，当环境中的障碍物较为复杂时，将导致规划算法计算量较大。基于随机采样技术的PRM法可以有效解决高维空间和复杂约束中的路径规划问题。

　　PRM是一种基于图搜索的方法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。这种方法能用相对少的随机采样点来找到一个解，对多数问题而言，相对少的样本足以覆盖大部分可行的空间，并且找到路径的概率为1（随着采样数增加，P（找到一条路径）指数的趋向于1）。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。

　　The PRM path planner constructs a roadmap in the free space of a given map using randomly sampled nodes in the free space and connecting them with each other. Once the roadmap has been constructed, you can query for a path from a given start location to a given end location on the map. 

　　The probabilistic roadmap planner consists of two phases: a construction and a query phase. 学习阶段：在给定图的自由空间里随机撒点（自定义个数），构建一个路径网络图。查询阶段：查询从一个起点到一个终点的路径。

• Roadmap is a graph G(V, E)  （无向网络图G，其中V代表随机点集，E代表所有可能的两点之间的路径集）

• Robot configuration q→Q_free is a vertex （每个点都要确保机器人与障碍物无碰撞）

• Edge (q1, q2) implies collision-free path between these robot configurations

• A metric is needed for d(q1,q2) (e.g. Euclidean distance)  （Dist function计算Configuration Space中点与点之间的距离，判断是否是同一个点）

• Uses coarse sampling of the nodes, and fine sampling of the edges

• Result: a roadmap in Q_free

Step 1, Learning the Map

• Initially empty Graph G

• A configuration q is randomly chosen

• If q →Q_free then added to G (collision detection needed here)

• Repeat until N vertices chosen

• For each q, select k closest neighbors，

• Local planner connects q to neighbor q’

• If connect successful (i.e. collision free local path), add edge (q, q’)