概率路图法（PRM）路径规划算法简述

　　   一、概率路图法（PRM）简介 　　   概率路图法（Probabilistic Road Map）由LE Kavraki,、P Svestka等人于1996年在论文《Probabilistic roadmaps for path planning in high-dimensional configuration spaces》中提出，论文链接如下： 　　   https://ieeexplore.ieee.org/abstract/document/508439/ 　　 　　   概率路图法很好的解决了在高维空间中构造出有效路径图的困难。该算法通过在构形空间中进行采样、对采样点进行碰撞检测、测试相邻采样点是否能够连接来表示路径图的连通性。此方法的一个巨大优点是，其复杂度主要依赖于寻找路径的难度，跟整个规划场景的大小和构形空间的维数关系不大。 然而当规划的路径需要通过密集的障碍物或者需要经过狭窄的通道时，PRM方法的效率变的低下。 　　   路线图算法（probabilistic roadmap method，PRM）主要分为两个阶段，离线学习阶段中随机采样大量的机器人位姿点，为每个节点搜索邻居节点并建立连接，构建出路标地图；在线查询阶段中根据起始点，目标点，路标地图信息，采用启发式搜索算法从路标图搜索出一条可行路径。路线图算法可以有效避免对位姿空间中障碍物进行精确建模，能够有效解决复杂的运动动力学约束下的路径规划问题。 　　   二、概率路图法（PRM）流程介绍 　　   概率路图法的主要包括概率路图的构建和可行路径查询两大部分 　　   第一阶段中按照某种策略在环境中撒点一些采样点，比如说采用最简单的均匀撒点的方式，如下面的左图所示，然后去除在障碍物内部的采样点，如下面的右图所示。 　　 　　   在第二阶段中，将采样点按照设定的准则使用线段连接起来，比如距离较远的采样点之间不连接，如下面左图中的红色线段所示，穿过障碍物的点不连接，如下面左图中的绿色线段所示，去除后的连线如下面右图所示。当然不同的需求，对应的连接准则也不同，再比如，对于非完整约束的机器人，往往需要考虑运动学约束，这种情况下，不满足运动学约束的点之间不能连接，或者在规划时不考虑运动学约束，通过后端的轨迹优化处理也是可行的。 　　 　　   在第三阶段中，在上面由采样点和连接构建出的图中，借用Dijkstras算法或者A * 算法查询从起点到目标点的路径，与直接在二维或者高维空间中使用Dijkstras算法或者A * 搜索路径相比，PRM算法通过第一和第二阶段构建出的图上查询路径，很大程度上简化的规划空间 　　   从编程实现上以上内容可归结为以下步骤： 　　   ① 初始化：初始化一个空的无向图G（V，E），其中V代表顶点集合 ，E代表无碰撞的边（顶点间连线）的集合，V和E初始状态均为空。 　　   ② 构型采样：从环境空间中采样一个不在障碍物内部的点 vi， 并加入到顶点集合V中。 　　   ③领域点计算：定义距离p ，对于已经存在于顶点集 V中的点，如果它与vi的距离小于p ，则将其称作点vi点的邻域点。 　　   ④ 边线连接：将点 vi 与其领域点相连，生成连线集合e 　　   ⑤ 碰撞检测：检测连线集合e中的连线是否与障碍物发生碰撞，并将无碰撞的连线加入到连线集E中。 　　   ⑥ 重复执行步骤②~⑤，直至顶点集合V中点的数量达到设定的值，结束采样阶段 　　   ⑦ 路径查询：从构建好的无向图G（V，E）中采用Dijkstras算法或者A * 算法等算法查询可行路径 　　   三、Lazy collision-checking策略 　　   上述的第一阶段中需要判断每个采样点是否位于障碍物内部，耗时较多，Lazy collision-checking策略在第一阶段中不再判断每个采样点是否位于障碍物内部，直接进行第二阶段、第三阶段，如下图所示。 　　 　　   此时，若找到的路径包含位于障碍物内部的线段，则在概率路图G（V，E）中将规划的路线中穿越障碍物的边和顶点删除，如下面的左图所示，并在新得到的概率路图G（V，E）中重新查询路径，若得到的路径依然包含穿越障碍物的边和顶点删除，则重复执行以上过程，直至得到的路径不包含穿越障碍物的边，如下面右图所示，结束规划。 　　 　　   四、动态演示 　　   概率路图构建阶段动态演示如下： 　　 　　   查询可行路径阶段动态演示如下： 　　 　　   参考资料 　　   Motion Planningfor Mobile Robots