对Probabilistic Road Map(PRM)概率路图路径规划方法的理解

PRM的总体思路

基于采样的路径规划方法在原理上与基于搜索的路径方法有较大区别。基于搜索的路径方法，如A*和Dijkstra，常常用于grid地图。它们需要搜索目标点到终点间的所有栅格。基于采样的路径规划方法则不同，它在空间中随机撒点，然后将点用线连接起来。这样就形成了一个网络，最后将起点和终点接入到这个网络。这样就抽象出一个路图来简化整个解空间。这对于复杂的空间尤为有效。这里要注意的是当随机生成的点与障碍物重合，或者连线与障碍物重合，这些点和连线会从网络中剔除掉。

当网络或者说路图构建好了，则可以使用A*和Dijkstra等等算法来搜寻最佳路径。

PRM的两个阶段

Learning phase
在空间中随机采样N个点。这里的采样一般使用均匀采样，但也可以使用启发式的采样。这样生成的采样点更高效。

对采样点进行collision free检测，去掉与障碍物重叠的点

将采样点连接起来。连线时有两个准则：

• 只连接自己邻近的点，太远的点不进行连接。
• 连线不能与障碍物重叠。

下面是连接节点的一种实现方式。

void PRM::connectNodes()
{
	const int n = m_nodes.size();
	const int numberOfEdges = (n * (n + 1)) / 2.0 - n;
	const int maxTests = 50;
	const int successfulTests = 15;

	ais_util::ProgressBar progress("prm update", numberOfEdges);

//#	pragma omp parallel
//#	pragma omp single nowait
	{
		for (auto it = m_nodes.begin(); it != m_nodes.end(); ++it)
		{
//#			pragma omp task
			{
				KDL::JntArray randomState, ikSolution;
				KDL::Frame kdlPose;

				PRMNode* n1 = (*it).second;
				Eigen::Vector3d position1 = n1->getPosition();

				auto it2 = it;
				++it2;

				for (; it2 != m_nodes.end(); ++it2)
				{
					PRMNode* n2 = (*it2).second;
					Eigen::Vector3d position2 = n2->getPosition();

//					if (isVisible(position1, position2))
					if ((position1 - position2).norm() < c_config.visibilityDistance)
					{
						PRMEdge* edge = new PRMEdge(n1, n2, m_nextIdEdge);

//#						pragma omp critical
//						{
						m_edges[edge->getId()] = edge;
						n1->addEdge(edge);
						n2->addEdge(edge);
//						}
					}

					progress.increment();
				}
			}
		}
	}

	progress.finish();

	LOG_INFO("Found " << m_edges.size() << " edges");
}

另外，我们也可以考虑机器人的运动模型来连线。



Query phase
相对于栅格地图来说，生成的路图对空间简化了不少。这时再使用A*等算法来搜索路径，效率会更高。

PRM的优缺点

优势

• 它是概率完备的。构建的图中如果解存在，则一定能找到解。
• 它更为高效。路图对解空间做了简化。搜索的复杂度大为降低。

劣势

• 两点的连线没有考虑机器人的运动模型。
• 有些人认为应该在构建图的同时就对路径进行搜索。当图构建好后路径也就找到了。而分为两个阶段显得不是那么高效。

针对collision-checking的一点改进（Lazy collision-checking）

collision-checking是一个非常耗时间的过程。特别在于复杂的环境或者多维度的空间里。所以有人提出在Learning phase阶段不进行collision-checking而在Query phase中再进行collision-checking。具体的做法是，Learning phase就只生成采样点，并将点进行连接。

在Query phase中会寻找可行路径。当得到路径后就进行collision checking，检测路径是否与障碍物重叠。如果有冲突的地方就把冲突的连接和相关的采样点删掉。然后再重新搜寻路径，直到找到一条collision free的路径。

参考网址：
上面的图片基本来自下面的网址。
V-rep学习笔记：机器人路径规划1

PRM的ROS包