博文
读书笔记——OBPRM，一种基于障碍物的PRM
|||
本文作为论文OBPRM:An Obstacle-Based PRM for 3D Workspaces的阅读笔记。
其实，提出OBPRM的这篇论文算是我在运动规划这个方向上较早地读过的一篇文章，但是，由于自己对PRM的理解不够透彻，OBPRM到底是什么，我一直没有搞清楚。TAMU的Nancy M. Amato教授写的论文好像一向不易理解，之前读过她提出MAPRM的那篇论文，一读就是一个星期，查阅各种资料，最终才理解何为MAPRM。这篇提出的OBPRM的论文同样难以理解，我读了好几遍，决定现在和大家分享一下自己的理解。
请注意，下文中的内容不只局限于OBPRM:An Obstacle-Based PRM for 3D Workspaces一篇论文。为了全面且详细地介绍OBPRM，我还参考了Yan Wu在TAMU完成的硕士论文An Obstacle-Based Probabilistic Roadmap Method for Path Planning以及发表于1998年ICRA的论文Choosing Good Distance Metrics and Local Planners for Probabilistic Roadmap Methods。
既然是基于障碍物的PRM，则该方法必然跟障碍物信息有千丝万缕的联系。之前写过一篇关于论文A Randomized Roadmap Method for Path and Manipulation Planning的阅读笔记，其实那篇文章中提出的方法就是OBPRM的原型。这篇文章的主要贡献是在OBPRM的原型上做了一些关于改进方法的讨论，比如应该如何选取局部规划器、点生成策略以及连接策略等。因此，要想读懂这篇文章，必须理解OBPRM的原型。OBPRM的大体思路是这样的：在C障碍物（C-obstacle）表面产生一些采样点，将这些点连接，构成基于障碍物的地图（roadmap）。这种地图看上去就像为C障碍物勾勒出了轮廓。机器人沿着这幅地图移动，总是不会碰到障碍物——最多就是移动到位于C障碍物表面而已。C障碍物表面的采样点叫做接触位姿（contact configuration）。下面的一幅图就是OBPRM的示例：
显然，这是一个二维位姿空间。其中，s和g分别是起始点和目标点。可以看到，地图上的所有采样点都是接触位姿。稍后会讲到OBPRM的一种改进，它产生的地图中不仅有数量丰富的接触位姿，而且有相当多的完全位于自由空间的自由位姿。
想要读懂这篇论文，必须先了解OBPRM的原型到底是何模样。这些内容在An Obstacle-Based Probabilistic Roadmap Method for Path Planning中讲述得很清楚，我之前的博文《读书笔记——一种在障碍物周围产生采样点的随机地图方法》也进行了描述和解读，如果想了解OBPRM，不妨一读。OBPRM，从本质上说，也是一种PRM。PRM的框架是怎样的呢？请看下图：
因此，OBPRM也有产生采样点的阶段和连接采样点的阶段，只不过这里的采样点比较特殊——它们都是接触位姿。先来看看产生采样点的阶段：
对于每一个C障碍物X，都执行上图所示算法，即：首先在X上产生一个碰撞位姿，记为$c_{in}$
；从$c_{in}$
射出m个随机方向，构成方向集合D；对于每一个$d \in D$
，在d上找到一个自由位姿$c_{out}$
，然后在$c_{in}$
和$c_{out}$
之间二分查找接触位姿。如果你不清楚怎样二分查找接触位姿，去看我的博文《读书笔记——一种在障碍物周围产生采样点的随机地图方法》。
产生接触位姿之后，自然要尝试连接它们。本论文提出了三种连接算法，OBPRM原型中使用的算法自然是最简单的一种，在本论文中叫做stage 1：
上面的算法描述了这样一个过程：设$V_{i}$是与第i个C障碍物关联的采样点集合——说白了，就是该C障碍物周围的那“一圈”接触位姿。共有m个C障碍物。对于C空间中的每一个接触位姿$v \in V$
，进行以下操作：对于每一个障碍物（不妨设为第i个障碍物），计算$V_{i}$中距离v最近的$K_{1}$个点，记这些点构成的集合为$C_{v,i}$，尝试将v与$C_{v,i}$中的每一个点连接。这个过程是不是不太好懂呢？看下图：
上图表示的C空间中有三个C障碍物。v位于左下方的障碍物上。令$K_{1}=5$
，即尝试将v与每一个障碍物上距离它最近的5个点连接，结果如上图。注意，图中的虚线表示失败的连接，实线表示成功的连接。由此可见，连接过程允许存在失败，且连接失败的概率往往很高。
由于采样点全部位于障碍物表面，最后得到的路径往往是在障碍物表面“跳跃”（skip），这样的地图对于很多实际应用来说是很不方便的。于是，为了提高地图的质量，可以采取一些优化策略，例如，当局部规划器连接了两个位于不同C障碍物上的采样点时，保留每一条局部路径的中点，然后连接过程就在这些中点之间进行。最终效果如下图所示：
连接任务是由局部规划器（local planner）完成的。本论文给出了三种可选的局部规划器，分别是直线规划器（straight-line）、s旋转规划器（rotate-at-s）和类A星规划器（A*-like planners）。在本文中，机器人的位姿由六元组$\left (x,y,z,\alpha, \beta, \gamma \right )$表示，前三个坐标表示位置（position），后三个坐标表示方向（orientation）。局部规划器至少需要两个输入，即起始点和目标点（注意，这里的起始点和目标点都是局部的，而非全局的）。设起始点和目标点分别为$c_{1}=\left (x_{1},y_{1},z_{1},\alpha_{1}, \beta_{1}, \gamma_{1} \right )$
和$c_{2}=\left (x_{2},y_{2},z_{2},\alpha_{2}, \beta_{2}, \gamma_{2} \right )$
。
直线规划器如下所示：
直线规划器的算法非常简单：输入起始点$c_{1}$
和目标点$c_{2}$ 以及给定的每个坐标轴上的分辨率，规划器首先确定需要检测的中间位姿（intermediate configurations）的数目nstep，然后计算出每一个坐标轴上的增量（increment），这6个增量组成增量向量（increment vector）I，再对每个中间位姿进行碰撞检测，若全部无碰撞，则返回YES，否则返回NO。
s旋转规划器如下所示：
相对于直线规划器而言，s旋转规划器的工作就稍微复杂一些（$s \in \left [0,1 \right ]$
）。机器人首先从起始位姿$c_{1}$
平移（translate）到中间位姿$c^{'}$
，再从$c^{'}$旋转（rotate）到另一个中间位姿$c^{''}$
，最后从$c^{''}$
平移到目标位姿$c_{2}$
。据我个人理解，s表示的是$c_{1}$和$c^{'}$之间的平移距离（translational distance）占$c_{1}$和$c_{2}$之间平移距离的比例。$c_{1}$和$c^{'}$之间，$c^{'}$和$c^{''}$之间，以及$c^{''}$和$c_{2}$之间的局部规划都由直线规划器完成。s旋转规划器的算法如下所示：
类A星规划器如下所示：
类A星规划器的算法思路并不复杂，它首先计算$c_{1}$的邻居（neighbors），然后选出最“有前途”的邻居$c^{'}$
，重新放入算法中进行迭代。直到到达$c_{2}$或者无法继续前进，则算法终止。现在有三个问题：1.每次迭代要计算几个邻居。2.在选取最“有前途”的邻居时，评价函数（evaluation function）是什么。3.由于A星算法运行时间可能非常长，所以需要设定一个最大迭代次数，那么迭代次数应该设置为多少。对于第一个问题，OBPRM目前支持三种不同的邻居函数（neighbor function），分别产生3个、9个和15个邻居。对于这些邻居函数而言，产生的前三个邻居都是一样的：一个是只在位置坐标上向目标点方向有增量，一个是只在方向坐标上向目标点方向有增量，一个是所有坐标向目标点方向都有增量。其余的6个或12个邻居，它们中的每一个只有一个坐标向目标点方向（或远离目标点的方向）有增量。对于第二个问题，有两个评价函数可选：一个是距离目标点的最小距离（minimum distance），一个是距离工作空间障碍物的最大间隙（maximum clearance）。这些函数都用物体的质心（center of mass）进行计算。对于第三个问题，最大迭代次数是直线规划器迭代次数的倍数，通常取值为3，6，9，15。注意，这里类A星算法并非真正的A星算法，仅仅是像一些而已。下文中用$A^{*}-eval\left (nbrs,steps \right )$ 表示类A星规划器，例如，$A^{*}-clearance\left (9nbrs,6steps \right )$
，表示此规划器每次迭代要计算9个邻居，最大迭代次数为6。
介绍完该论文中提到的局部规划器，咱们回来看看刚才产生采样点的算法。它足够简单，这是它的优点；可是，如果你读过我之前提到的那篇博文，你会立刻质疑该算法产生的接触位姿在障碍物，尤其是形状复杂的障碍物表面能否有一个好的分布。我们称$c_{in}$ 为种子（seed），因为接触位姿全都依靠它产生。事实上，种子的位置和障碍物的形状对接触位姿的最终分布有重大影响。例如，如果C障碍物是长而窄的，则上述只产生一个种子的采样算法无法产生均匀分布的采样点，相对于离$c_{in}$ 较近的接触位姿而言，离$c_{in}$ 较远的接触位姿与邻居之间的距离比较大。一个优化方法是在两个距离较远的接触位姿之间生成更多的点。如下图所示：
本论文改进了上述简单的采样点生成策略，设计了三种方法，分别用于生成接触位姿、自由位姿（在接触表面附近）和一个被称作外壳（shell）的围绕在C障碍物周围的点集。
为了产生在C障碍物表面均匀分布的接触位姿，使用单个种子是难以达到要求的，因此，作者使用了多个种子。实际上，作者实现的OBPRM为每一个接触位姿的产生都使用了不同的种子，算法如下图所示：
可以看到，与之前产生采样点的方法不同，上图中所示的方法，每调用一次只产生一个接触位姿，且每次调用使用的种子都不同；而之前的算法，对于一个障碍物而言，总共只需调用一次，同一个种子会产生多个接触位姿。使用这种方法产生的位姿仍然依赖于机器人和障碍物的形状，以及$p_{rob}$和$p_{obj}$的选取。注意，这里提到的形状定义在工作空间中，也就是机器人和障碍物的实际物理形状。$p_{rob}$和$p_{obj}$通常取机器人或障碍物的顶点（vertex），不过，论文作者尝试了更多的选取方法，如下表所示：
有时，地图的连通性可以通过在接触表面附近产生自由位姿来提高。这些位姿可以为局部规划器提供方便，使之绕过物体的拐角（corners）。作者将上述产生采样点的算法进行了简单修改，使之能够直接用于产生符合条件的自由位姿。修改过程是这样的：分别在机器人和障碍物上选取两个点$p_{rob}$和$p_{obj}$之后，平移机器人，使$p_{rob}$和$p_{obj}$重合，然后随机旋转机器人，得到一个自由位姿。
OBPRM希望在C障碍物周围产生高密度的采样点，因为在障碍物周围规划路径往往是困难的。由于在接触表面做规划很困难，我们希望在地图中包含这样一些路径：它们从机器人与障碍物之间的间隙中穿过。生成这些路径所需的点并不难。请注意生成接触位姿的最后一步：二分查找！二分查找只返回一个最终结果，即一个接触位姿，但是查找过程中产生的所有二分点都被忽略了。当一个C障碍物的所有接触位姿都生成后，其“中间产物”数量客观，我们考虑可以把这些“中间产物”利用起来。例如，挑选距离障碍物质心最近的s个“中间产物”，从整体上看，这些点就像外壳（shell）一样将障碍物包围在其中。s就是外壳的数量。
好了，关于如何生成采样点的讨论就此告一段落，现在我们来看怎样“连点成图”。上文中已经给出了一种简单的连接算法，但是，这种方法存在一个难以克服的缺点，即它将很多尝试建立连接的努力耗费在同一个C障碍物的接触位姿之间。啥意思？请向上翻页，直到发现我给出的那张连接示意图。图中的v需要尝试与每个障碍物上的5个点建立连接，但是，大部分都失败了，而且v尝试连接与它同处一个障碍物上的位姿点五次，从图上看，貌似只成功了两次——不到50%的成功率。
在同一个C障碍物的接触位姿之间建立连接往往不如在不同的C障碍物之间建立连接有用。于是，论文作者提出了建立连接的三个阶段（stage）。第一阶段是简单连接（simple connection），这个阶段调用最快的局部规划器（通常为几千次），完成一些最简单的连接任务。第二阶段和第三阶段分别是连接连通分支（connecting components）和增长连通分支（growing components）。这两个阶段要完成的连接任务较少，调用的规划器较强。注意，第三阶段可能会向地图中增加一些采样点。一个看似简单的连接过程，为什么要分成三个阶段呢？因为速度较快的规划器连接能力往往较弱，无法胜任一些非常关键的连接任务；连接能力较强的规划器计算代价很大，用这种规划器执行一些简单的连接任务就显得很浪费，而且这些简单的连接实际上占了地图大部分的边。
现在的问题是，这三个阶段分别起什么作用？它们的算法又是怎样的？
第一阶段简单连接的算法，上文中已经给出了。这里，OBPRM使用的唯一的规划器就是最简单、最快速的直线规划器。第1至第8步都容易理解，关键是第9步：analysizeroadmap用于分析地图，计算连通分支和每个连通分支的顶点数，如果发现当前的地图包含了不止一个连通分支，则进入到第二阶段。
第一阶段的目标是在点与点之间建立连接，第二阶段的目标则是在连通分支与连通分支之间建立连接，它的算法如下：
显然，上述算法中的m不再是C障碍物的数目，而是地图中连通分支的数目。首先将所有连通分支按照顶点数目从小到大排序，然后使用两层for循环，尝试将所有连通分支两两相连。现在考虑连接两个连通分支$V_{i}$和$V_{j}$，且$V_{i}$顶点数目比$V_{j}$少。如果$V_{i}$顶点数目不超过$MAX2$（通常为10到30），则尝试将$V_{i}$的每一个点与$V_{j}$的最近的$K2.1$个点连接；如果$V_{i}$顶点数目超过$MAX2$，则尝试连接$V_{i}$与$V_{j}$之间距离最近的$K2.2$个顶点对。接下来的操作让我倍感费解，也可能是我的惯性思维在作怪吧，反正总觉得不该是这样做的——每当成功建立一个连接，上述算法立即终止。这是怎么做到的呢？我估计是在函数$ATTEMPTALL \left (V_{i},V_{j},K2.1 \right )$和$ATTEMPTK \left (V_{i},V_{j},K2.2 \right )$中都包含了return语句，一旦建立一个连接，立即return。为什么要这么做呢？也许子阶段（substages）的引入能够给我门一些启发。论文中提到，第二阶段其实是由几个子阶段组成的，这些子阶段之间的不同之处在于$MAX2,K2.1,K2.2$的值不同，采用的局部规划器也不同。子阶段和第二阶段之间的关系是什么呢？我觉得是每一个子阶段都执行一次上述的第二阶段算法，子阶段一个接一个执行，每次执行完毕，由于地图的连通性可能已经被改变，所以需要重新分析地图的连通性，直到整个地图只包含一个连通分支为止，这就构成了完整的第二阶段。下面是两个重要的子阶段：
第三阶段的目标也是合并不同的连通分支。第二阶段尝试将两个连通分支上已经存在的顶点连接起来，第三阶段不仅要完成第二阶段的任务，而且尝试向个别连通分支上增加顶点。增加顶点的目的很明显，就是为了方便在连通分支之间建立连接。这个过程看上去就像经典PRM中的增强操作。
作者给出了第三阶段的两种不同的算法。第一种叫做扩展失败路径（expanding failed paths）。提出该算法的动机是，即使连通分支之间的连接尝试失败了，这些尝试所得到的一些进展可能是有用的。通常的想法是如果两个位姿$c_{1},c_{2}$之间的连接尝试失败了，则保留尝试过程中产生的最后一个有效点$c_{3}$。算法如下所示：
算法第2步计算了连通分支$V_{i}$的质心。所谓质心，就是$V_{i}$中所有位姿的平均值。在第4步，得到的质心被用于挑选连通分支$V_{j}$，使得$V_{i}$和$V_{j}$之间质心距离最近。然后从$V_{i} \times V_{j}$中挑选$K3.1$个最近的点对$\left (c_{1},c_{2} \right )$，构成点对集合$P_{i,j}$
。对于$P_{i,j}$中的任意一对顶点$\left (c_{1},c_{2} \right )$，如果无法连接$c_{1}$和$c_{2}$，就保留该失败路径上的最优一个有效位姿$c_{3}$，然后尝试连接$c_{3}$与$V_{j}$中$K3.2$个最近点。如果还是连接失败，则尝试将$c_{3}$的所有邻居$Nbrs \left (c_{3} \right )$与$V_{j}$中$K3.2$个最近点相连。如果这也失败了，那么就把$c_{3}$加入到地图中，然后进入下一轮循环。
通常，$K3.1$和$K3.2$应该取较小值。作者取$K3.1=15,K3.2=10$。算法中用到了两种局部规划器：第7步到第9步，使用的规划器是$A^{*}-clearance \left (15nbrs,9steps \right )$；第11步使用的规划器是s旋转规划器。
第三阶段的第二种算法称为扩展小分支（expanding small components）。提出该算法的动机是，位于困难区域（difficult regions）的位姿往往处在较小的连通分支上，而且这些小分支可以充当大分支之间相互连接的桥梁。这是为什么呢？我是这样想的：生成接触位姿、自由位姿和外壳后，它们都是一个个孤立的顶点，这时的地图上一条边也没有，用图论的术语来说，就是“零图”；第一阶段尝试进行简单连接，如果一个顶点位于困难区域，它与其他顶点之间的连接成功率必然很低，这就造成了这个连通分支“发育不良”，最终只能成为小连通分支；第二阶段尝试进行连通分支之间的连接，同样地，如果一个连通分支上的大部分顶点都位于困难区域，就会造成它与其他连通分支之间的连接成功率较低，这个小连通分支意图通过与其他连通分支合并来扩大自身的梦想实现起来依旧比较艰难。这就是能够通过连通分支大小看出某区域困难程度的原因。通常的想法是，通过在小连通分支附近产生更多的采样点，实现对小连通分支的扩展，算法如下所示：
与前面的算法一样，该算法也涉及到参数和局部规划器的选取。作者实现的OBPRM，取$MAX3=10,K3.3=10$，对于类A星规划器，尝试连接的邻居数目取15。算法的第7步使用直线规划器。注意算法第9步使用的方法与第二阶段的算法相同，调用$A^{*}-clearance\left (15nbrs,9steps \right )$。
这篇文章的实验部分讲了很多内容，讨论了如何为上述的几个算法选取参数和规划器。对于这种经验性的内容，我就不展开介绍了，有兴趣可以自己看。一旦将基本算法掌握，相信阅读实验部分并不困难。本文就此歇笔。
https://blog.sciencenet.cn/blog-795645-648224.html
上一篇：读书笔记——MLDP，在PRM中解决窄道采样问题
下一篇：读书笔记——RESAMPL，一种区域敏感的自适应规划方法
其实，提出OBPRM的这篇论文算是我在运动规划这个方向上较早地读过的一篇文章，但是，由于自己对PRM的理解不够透彻，OBPRM到底是什么，我一直没有搞清楚。TAMU的Nancy M. Amato教授写的论文好像一向不易理解，之前读过她提出MAPRM的那篇论文，一读就是一个星期，查阅各种资料，最终才理解何为MAPRM。这篇提出的OBPRM的论文同样难以理解，我读了好几遍，决定现在和大家分享一下自己的理解。
请注意，下文中的内容不只局限于OBPRM:An Obstacle-Based PRM for 3D Workspaces一篇论文。为了全面且详细地介绍OBPRM，我还参考了Yan Wu在TAMU完成的硕士论文An Obstacle-Based Probabilistic Roadmap Method for Path Planning以及发表于1998年ICRA的论文Choosing Good Distance Metrics and Local Planners for Probabilistic Roadmap Methods。
既然是基于障碍物的PRM，则该方法必然跟障碍物信息有千丝万缕的联系。之前写过一篇关于论文A Randomized Roadmap Method for Path and Manipulation Planning的阅读笔记，其实那篇文章中提出的方法就是OBPRM的原型。这篇文章的主要贡献是在OBPRM的原型上做了一些关于改进方法的讨论，比如应该如何选取局部规划器、点生成策略以及连接策略等。因此，要想读懂这篇文章，必须理解OBPRM的原型。OBPRM的大体思路是这样的：在C障碍物（C-obstacle）表面产生一些采样点，将这些点连接，构成基于障碍物的地图（roadmap）。这种地图看上去就像为C障碍物勾勒出了轮廓。机器人沿着这幅地图移动，总是不会碰到障碍物——最多就是移动到位于C障碍物表面而已。C障碍物表面的采样点叫做接触位姿（contact configuration）。下面的一幅图就是OBPRM的示例：
想要读懂这篇论文，必须先了解OBPRM的原型到底是何模样。这些内容在An Obstacle-Based Probabilistic Roadmap Method for Path Planning中讲述得很清楚，我之前的博文《读书笔记——一种在障碍物周围产生采样点的随机地图方法》也进行了描述和解读，如果想了解OBPRM，不妨一读。OBPRM，从本质上说，也是一种PRM。PRM的框架是怎样的呢？请看下图：
产生接触位姿之后，自然要尝试连接它们。本论文提出了三种连接算法，OBPRM原型中使用的算法自然是最简单的一种，在本论文中叫做stage 1：
由于采样点全部位于障碍物表面，最后得到的路径往往是在障碍物表面“跳跃”（skip），这样的地图对于很多实际应用来说是很不方便的。于是，为了提高地图的质量，可以采取一些优化策略，例如，当局部规划器连接了两个位于不同C障碍物上的采样点时，保留每一条局部路径的中点，然后连接过程就在这些中点之间进行。最终效果如下图所示：
直线规划器如下所示：
s旋转规划器如下所示：
类A星规划器如下所示：
介绍完该论文中提到的局部规划器，咱们回来看看刚才产生采样点的算法。它足够简单，这是它的优点；可是，如果你读过我之前提到的那篇博文，你会立刻质疑该算法产生的接触位姿在障碍物，尤其是形状复杂的障碍物表面能否有一个好的分布。我们称$c_{in}$ 为种子（seed），因为接触位姿全都依靠它产生。事实上，种子的位置和障碍物的形状对接触位姿的最终分布有重大影响。例如，如果C障碍物是长而窄的，则上述只产生一个种子的采样算法无法产生均匀分布的采样点，相对于离$c_{in}$ 较近的接触位姿而言，离$c_{in}$ 较远的接触位姿与邻居之间的距离比较大。一个优化方法是在两个距离较远的接触位姿之间生成更多的点。如下图所示：
为了产生在C障碍物表面均匀分布的接触位姿，使用单个种子是难以达到要求的，因此，作者使用了多个种子。实际上，作者实现的OBPRM为每一个接触位姿的产生都使用了不同的种子，算法如下图所示：
OBPRM希望在C障碍物周围产生高密度的采样点，因为在障碍物周围规划路径往往是困难的。由于在接触表面做规划很困难，我们希望在地图中包含这样一些路径：它们从机器人与障碍物之间的间隙中穿过。生成这些路径所需的点并不难。请注意生成接触位姿的最后一步：二分查找！二分查找只返回一个最终结果，即一个接触位姿，但是查找过程中产生的所有二分点都被忽略了。当一个C障碍物的所有接触位姿都生成后，其“中间产物”数量客观，我们考虑可以把这些“中间产物”利用起来。例如，挑选距离障碍物质心最近的s个“中间产物”，从整体上看，这些点就像外壳（shell）一样将障碍物包围在其中。s就是外壳的数量。
好了，关于如何生成采样点的讨论就此告一段落，现在我们来看怎样“连点成图”。上文中已经给出了一种简单的连接算法，但是，这种方法存在一个难以克服的缺点，即它将很多尝试建立连接的努力耗费在同一个C障碍物的接触位姿之间。啥意思？请向上翻页，直到发现我给出的那张连接示意图。图中的v需要尝试与每个障碍物上的5个点建立连接，但是，大部分都失败了，而且v尝试连接与它同处一个障碍物上的位姿点五次，从图上看，貌似只成功了两次——不到50%的成功率。
在同一个C障碍物的接触位姿之间建立连接往往不如在不同的C障碍物之间建立连接有用。于是，论文作者提出了建立连接的三个阶段（stage）。第一阶段是简单连接（simple connection），这个阶段调用最快的局部规划器（通常为几千次），完成一些最简单的连接任务。第二阶段和第三阶段分别是连接连通分支（connecting components）和增长连通分支（growing components）。这两个阶段要完成的连接任务较少，调用的规划器较强。注意，第三阶段可能会向地图中增加一些采样点。一个看似简单的连接过程，为什么要分成三个阶段呢？因为速度较快的规划器连接能力往往较弱，无法胜任一些非常关键的连接任务；连接能力较强的规划器计算代价很大，用这种规划器执行一些简单的连接任务就显得很浪费，而且这些简单的连接实际上占了地图大部分的边。
现在的问题是，这三个阶段分别起什么作用？它们的算法又是怎样的？
第一阶段简单连接的算法，上文中已经给出了。这里，OBPRM使用的唯一的规划器就是最简单、最快速的直线规划器。第1至第8步都容易理解，关键是第9步：analysizeroadmap用于分析地图，计算连通分支和每个连通分支的顶点数，如果发现当前的地图包含了不止一个连通分支，则进入到第二阶段。
第一阶段的目标是在点与点之间建立连接，第二阶段的目标则是在连通分支与连通分支之间建立连接，它的算法如下：
作者给出了第三阶段的两种不同的算法。第一种叫做扩展失败路径（expanding failed paths）。提出该算法的动机是，即使连通分支之间的连接尝试失败了，这些尝试所得到的一些进展可能是有用的。通常的想法是如果两个位姿$c_{1},c_{2}$之间的连接尝试失败了，则保留尝试过程中产生的最后一个有效点$c_{3}$。算法如下所示：
通常，$K3.1$和$K3.2$应该取较小值。作者取$K3.1=15,K3.2=10$。算法中用到了两种局部规划器：第7步到第9步，使用的规划器是$A^{*}-clearance \left (15nbrs,9steps \right )$；第11步使用的规划器是s旋转规划器。
第三阶段的第二种算法称为扩展小分支（expanding small components）。提出该算法的动机是，位于困难区域（difficult regions）的位姿往往处在较小的连通分支上，而且这些小分支可以充当大分支之间相互连接的桥梁。这是为什么呢？我是这样想的：生成接触位姿、自由位姿和外壳后，它们都是一个个孤立的顶点，这时的地图上一条边也没有，用图论的术语来说，就是“零图”；第一阶段尝试进行简单连接，如果一个顶点位于困难区域，它与其他顶点之间的连接成功率必然很低，这就造成了这个连通分支“发育不良”，最终只能成为小连通分支；第二阶段尝试进行连通分支之间的连接，同样地，如果一个连通分支上的大部分顶点都位于困难区域，就会造成它与其他连通分支之间的连接成功率较低，这个小连通分支意图通过与其他连通分支合并来扩大自身的梦想实现起来依旧比较艰难。这就是能够通过连通分支大小看出某区域困难程度的原因。通常的想法是，通过在小连通分支附近产生更多的采样点，实现对小连通分支的扩展，算法如下所示：
这篇文章的实验部分讲了很多内容，讨论了如何为上述的几个算法选取参数和规划器。对于这种经验性的内容，我就不展开介绍了，有兴趣可以自己看。一旦将基本算法掌握，相信阅读实验部分并不困难。本文就此歇笔。
https://blog.sciencenet.cn/blog-795645-648224.html
上一篇：读书笔记——MLDP，在PRM中解决窄道采样问题
下一篇：读书笔记——RESAMPL，一种区域敏感的自适应规划方法
