左上角到右下角的路径 oj_【机器人路径规划】概率路线图(PRM)方法

阅读本文的基础知识要求：

• 了解机器人构型空间的含义
• 了解图论与基本搜索方法

原理

机器人运动规划的基本任务可以描述为：从开始位置到目标位置的运动。这一任务通常涉及到两项基本问题：

1. 如何躲避构型空间中出现的障碍物（几何路径规划）
2. 如何满足机器人本身在机械、传感方面的速度、加速度等限制（不确定性、反馈、微分约束等）

其中，基于抽样的运动规划算法是用于解决第 1 个问题的重要方法，其核心思想为：先对机器人的单个构型进行碰撞检测，建立无碰撞构型的数据库，再对不同的构型进行采样以生成无碰撞路径。该算法的优点在于具有通用性，只需要针对不同的机器人运动规划问题进行合理的参数调整。该算法的缺点在于完备性较弱，即当参数设置不合理时，即使存在可行的路径，也不一定能够找到。

典型的抽样规划方法有综合查询方法和单一查询方法两类。前者首先构建路线图，先通过采样和碰撞检测建立完整的无向图，以得到构型空间的完整连接属性。再通过图搜索即可得到可行的路径。后者则从特定的初始构型出发局部建立路线图，在构型空间中延伸树型数据结构，最终使它们相连。

本文介绍的概率路线图（Probabilistic Roadmap，PRM）属于综合查询方法，其步骤如下：

预处理：

1. 初始化。设
   为一个无向图，其中顶点集
   代表无碰撞的构型，连线集
   代表无碰撞路径。初始状态为空。
2. 构型采样。从构型空间中采样一个无碰撞的点
   并加入到顶点集
   中。
3. 领域计算。定义距离
   ，对于已经存在于顶点集
   中的点，如果它与
   的距离小于
   ，则将其称作点
   的邻域点。
4. 边线连接。将点
   与其领域点相连，生成连线
   。
5. 碰撞检测。检测连线
   是否与障碍物发生碰撞，如果无碰撞，则将其加入到连线集
   中。
6. 结束条件。当所有采样点（满足采样数量要求）均已完成上述步骤后结束，否则重复2-5。

搜索：

采用图搜索算法对无向图G进行搜索，如果能找到起始点A到终点B的路线，说明存在可行的运动规划方案。

实现

下面通过python编程予以实现，源代码下载地址：

https://github.com/XM522706601/robotics_tutorial_for_zhihu/blob/master/PRM_algorithm.py​github.com

实现思路如下：

1.先绘制一个500*500像素的bmp图像，用白色表示可行的构型空间，用深色表示障碍区域：

注释：这里的二维构型空间的横纵坐标可以有多种理解。对于平面型移动机器人，横纵坐标可以表示其在平面上的位置坐标，障碍区域为平面上的不可达区域坐标；对于二自由度的关节型机器人，横纵坐标可以分别表示其两个关节的转角，障碍区域为不可达区域在关节空间的映射。

图 1 构型空间与障碍区域地图

2.为了便于碰撞检测，我们再把图像变为黑白，使用了opencv中的灰度化函数cvtColor()和二值化函数threshold()。

图 2 二值化后的地图

3.构型采样。在图中随机采样一定数量（如50个）的无碰撞点。需要进行碰撞检测check_point()，抛弃与障碍物发生碰撞的点。（此处绘图仍然采用原地图，但碰撞检测使用的是二值化地图）

注意：左上角和右下角两个点是规划的起点和终点，在顶点集中的索引为0和1

图 3 随机采样50个无碰撞点

4.邻域计算，边线连接与碰撞检测。对每一个点，取其领域内（例如直线距离在200以内）的所有点进行连线，对连线进行碰撞检测，将结果存放在邻接矩阵中。

这里可以看到，由于邻域和碰撞的限制，右上角两个点和其余点不存在合理的连线。

图 4 检测并存储无碰撞的邻域连线

5.A*搜索。采用A*搜索算法对上图进行搜索，找到从左上到右下的最短路径，即为可行的路径规划方案。

图 5 A*搜索得到的最优路径

讨论

1.采样数量的影响

显然，对同一地图，采样点的数量越多，找到合理路径以及更优路径的概率就越大。但同时，采样点数量越多，计算与搜索时间也会更长。

如果只设置10个采样点，邻域200。可以看到，并不能找到可行路径，这也说明了抽样规划算法存在的完备性弱的问题。

10个采样点，邻域200

而若设置50和100个采样点，虽然都能找到可行路径，但耗时却存在差异。

如果采样点设置为50，耗时1.323s；设置为100，耗时4.663s

50个采样点，邻域200，耗时1.323s

100个采样点，邻域200，耗时4.663s

2.邻域设置的影响

邻域的设置影响着连线的建立与检测。当邻域设置过小，由于连线路径太少，可能找不到解；当领域设置太大，会检测太多较远的点之间的连线，而增加耗时。

如果邻域设置为100，找不到解；设置为1000，耗时4.470s，耗时较长

50个采样点，邻域100，找不到解

50个采样点，邻域1000，耗时4.470s

由上述结果可以看到，概率路线图(PRM)方法适用范围很广，对于不同的应用场景，只需要调整相应的参数即可。

但需要注意，抽样方法的完备性很弱，即使空间中存在合理的路径，由于抽样参数的设置问题，也可能无法找到路径；另外，由于抽样过程的随机性，该方法的稳定性也不好，对于同样的问题，前后两次的解也不一样，因此在严格要求稳定性的场合并不适用。

项目地址

https://github.com/XM522706601/robotics_tutorial_for_zhihu/blob/master/PRM_algorithm.py​github.com

参考文献

[1] Siciliano B, Oussama K. Springer Handbook of Robotics[M]. 2007.

[2] PRM路径规划算法 - XXX已失联 - 博客园

[3] A*算法 - t972409256的博客 - CSDN博客