本文作为论文A Randomized Roadmap Method for Path and Manipulation Planning的阅读笔记。
        这篇论文发表于1994年，那时候还没有明确提出PRM的概念，不过当时斯坦福大学的Lydia E. Kavraki和Jean-Claude Latombe已于同年早些时候在论文Randomized Preprocessing of Configuration Space for Fast Path Planning中提出了PRM的雏形。到了1996年，还是Lydia E. Kavraki和Jean-Claude Latombe等人，终于明确给出了PRM算法框架。
        这篇论文的主要贡献在于给出了一种能在障碍物表面产生采样点的方法。这对于后面的OBPRM以及困难区域问题是十分重要的，一幅图可以说明问题：

        很明显，这幅图中存在窄道（narrow passages）。这是困难区域的典型表现形式。上图中，障碍物周围的采样点和连接它们的边就像篱笆一样把障碍物围在里面，并勾勒出障碍物与障碍物之间的狭窄“走廊”（corridors）。这些障碍物表面的采样点和边构成了随机地图。这无疑为解决困难区域问题提供了一个不错的思路。
        现在看看怎样建立这样的地图。
        设机器人自由度为d，$S=\left \{s_{i} \mid 1 \leq i \leq n \right \}$为工作空间（workspace）中的障碍物集合。据此，设C-Space维数为d，$S^{'}=\left \{s_{i}^{'} \mid 1 \leq i \leq n \right \}$为C-Space中与工作空间中相对应的障碍物集合。现在要为每一个障碍物$s_{i}$产生一个点集$V_{i}$，使得任意$p \in V_{i}$都位于$s_{i}^{'}$的表面且不在任何$s^{'} \in S^{'}$的内部。理想情况下，这些点应该均匀分布在障碍物表面。地图中的所有点的集合V就是各个$V_{i}$的并集，即$V=\bigcup V_{i}$。对于任意障碍物$s_{i} \in S$，产生采样点的步骤如下：
        1.产生$m_{i}$个点$p_{1},p_{2},\cdots ,p_{m_{i}}$ ，均匀分布在$s_{i}^{'}$表面，并将它们加入$V_{i}$；
        2.删除$V_{i}$中所有位于任意障碍物$s_{j}^{'} \in S^{'}$内部的点，其中$1 \leq j \leq n$。
        上述两个步骤只是一个简单的框架，下面来看其实现细节。首先，我们考虑怎样在C-Space障碍物（C-obstacle） $s^{'} \in S^{'}$ 表面产生m个采样点。我们既想让采样点均匀分布在障碍物表面，又想避免计算关于障碍物表面的具体信息，于是就有了下面的方法：
        1.在$s^{'}$内部确定一个点o作为原点（the origin）；
        2.从o发射m条射线，射线方向在C-Space中均匀分布；
        3.在步骤2得到的每一条射线上，用二分查找的方法确定一个点，使得该点位于$s^{'}$的边界上。
        先看一幅示意图，其中，C-Space为2维，m=8：

        步骤1要求选择一个位于障碍物内部的点o，为了使将来的采样点均匀分布在障碍物表面，我们希望o尽量接近障碍物的中心位置，比如说，取障碍物各顶点坐标的平均值。步骤2要求m条射线均分整个C-Space，上图中，将$360^{\circ}$分均分成了m=8份。步骤3要求使用二分查找确定位于障碍物边界上的点。我们已经有一个障碍物内部的点o，假设我们还知道一个障碍物外部的点，在上图中为点1。线段o1的中点为点2，而点2位于障碍物以内，所以障碍物的边界点必然位于线段21上，故再取线段21的中点，记为点3。点3位于障碍物外部，所以障碍物的边界点必然位于线段23上，故再取线段23的中点，记为点4。一旦找到了障碍物边界点，或者二分的线段已经达到了最小步长（在这种情况下，就算没找到边界点，应该也离边界点不远了），该过程就停止。怎样找到最初的位于障碍物以外的点1呢？可以找一个已知的位于C-Space边界的点，也可以沿着射线方向、即远离原点o的方向多测试几个点。注意，外部点不一定能找到，因为射线r可能根本就没有穿透障碍物。
        尽管我们希望产生的采样点能够均匀分布在障碍物表面，但是有些时候却未能如愿。很明显，C-Space障碍物的形状以及原点o的位置对采样点分布有很大影响。只有当障碍物近似圆形且原点o位于它的中心时才能得到均匀分布，试想，如果C-Space障碍物是狭长的形状，则上述方法无法使采样点均匀分布。另外，障碍物表面离原点o较远的点与它们的临近点（neighbors）之间的距离往往也会远一些。例如，在上图中，点p离原点的距离比点q离原点的距离稍远，那么p离它的临近点的距离也比q离临近点的距离稍远。这种现象会造成采样点在障碍物表面分布不均。解决办法就是在这些相互离得较远的点之间再产生一些采样点。如下图所示：

        上图中的空心点表示新增加的“调和点”。还有一种用于确定是否应该增加这些调和点的方法就是考察障碍物表面法向量，如果一个采样点的法向量与其临近点的法向量相差较多，说明障碍物表面弯曲较大，这时就应该在它们中间增加一些“调和点”了。
        另一个难题是C障碍物有折叠（folds），即从原点发出的射线与障碍物边界多次相交。例如，在本文第二幅图中，尽管从欧式距离上看，所有点的分布基本均匀，但是p和q之间的表面距离（boundary distance）要比其他相邻点之间的表面距离远。不幸的是，如果不经过对C障碍物的计算，我们很难发现这种折叠现象；更不幸的是，就算我们发现了这种现象，也不清楚该在不经过大量计算的情况下怎么解决。
        现在考虑怎样把产生的采样点$V=\bigcup V_{i}$ 连接起来构成地图。基本想法就是采用简单快速的局部规划器（local planner），同时，为了节省空间，这些路径不会被存储，因为重新生成它们的速度很快。这个思想在后来的经典PRM中也被采用。连接过程整体上也与经典PRM类似，即选择k个最临近点进行连接。有的是尝试连接每一个$v \in V$到它的位于V中k个最邻近点；有的是对于每一个$v \in V_{i}$，尝试将它连接到某个$V_{j} \in V$中的k个最邻近点，其中$1 \leq j \leq n$；也有的是对于每一个$v \in V_{i}$，尝试将它连接到$V_{i}$中的个邻近点，同时尝试连接到$V-V_{i}$中的$k_{2}$个邻近点，通常$k_{1}>k_{2}$。
        地图建好后，我们会有一种奇怪的感觉——至少对于已经熟悉PRM地图的人来说是这样。因为我们所有的采样点都在障碍物表面，看上去并没有直接给出一条好用的路径。这时，你可以尝试在这些“走廊”中间增加一些点，比如说，在连接两点的直线路径的中点处增加一个点，得到下图所示的效果：

        至此，这篇论文的核心部分已经介绍完毕。至于实现的细节，对于熟悉经典PRM的人而言是非常简单的。这篇论文中反复提到了Lydia E. Kavraki和Jean-Claude Latombe的论文Randomized Preprocessing of Configuration Space for Fast Path Planning，甚至干脆说成是其某种意义上的扩展。而Randomized Preprocessing of Configuration Space for Fast Path Planning恰好是PRM的雏形。

转载本文请联系原作者获取授权，同时请注明本文来自王彦科学网博客。
链接地址：https://wap.sciencenet.cn/blog-795645-627764.html

上一篇：读书笔记——Lazy PRM
下一篇：读书笔记——PRM的高斯采样策略