本文作为论文Multi-Level Free-Space Dilation for Sampling Narrow Passages in PRM Planning的阅读笔记。
      与SSRP类似，这篇文章提出的方法也属于收回策略，即通过收缩障碍物和机器人的几何模型来实现窄道增宽，在得到的膨胀自由空间$F^{'}$ 中规划出一条完整路径，之后通过将路径形变，使之成为一条在F中真正可用的路径。这篇文章与提出SSRP的那篇文章Finding Narrow Passages with Probabilistic Roadmaps: The Small-Step Retraction Method不同点是：前者的创新点在于提出了一种收缩障碍物和机器人几何模型的新方法，而后者直接使用了另一篇论文中提出的瘦化方法，创新点在于乐观算法和悲观算法的优势互补。这篇文章的关键就在于找到一个高效的膨胀方法，而且要把握好膨胀的尺度：如果膨胀太少，窄道的宽度增加不多，就可能无法找到路径；如果膨胀太多，自由空间F形变严重，则将路径从$F^{'}$中形变到F中会变得困难。
      要想说明自己方法强，先看看其他方法弱在哪里。论文作者提到了专门提高自由空间边界附近采样密度的方法，这种方法的典型代表就是高斯采样，相信你已经对它的不足有所了解（如果不了解，去看我的相关博客）；还提到利用工作空间（workspace）信息去定位弱可视性（poor visibility）区域的方法，这种方法的依据就是工作空间中的窄道往往对应了位姿空间中的窄道，但是，定位自由空间F中的弱可视性区域是一个难题，因为我们没有关于F的精确描述；有些方法通过膨胀F来提高它的可视性，而膨胀F又有很多方法，例如，其中一种就是通过允许机器人轻微刺入（penetration）障碍物来实现F的膨胀，但是，由于难以有效计算刺入距离，使得该方法在复杂几何环境中难以应用；SSRP需要计算中轴，这是一项艰巨任务，而且，每当几何形状有轻微变化，中轴都会产生巨大变化，如下图所示：
其中，虚线表示几何形状的中轴。不仅如此，SSRP中的膨胀程度是人工设定的，这就使SSRP的灵活性大打折扣。下面，作者提出的收缩算法隆重登场！
      收缩算法将多面体模型M作为输入，首先计算M的四面体化（tetrahedralization）模型T，即将M内部剖分为四面体集合。虽然有些多面体不能被四面体化，例如Schonhardt多面体（Schonhardt polyhedron），不过，如果我们为M增加一些顶点，四面体化总是可行的。我们可以大致了解一下Schonhardt多面体。根据维基百科上的解释，它是最简单的、在不增加新顶点的情况下无法被四面体剖分（triangulated）的非凸多面体（non-convex polyhedron），如下图所示：
看不懂上图也没关系，你可以将我的这些解释认为是画蛇添足。为了收缩M，需要将M表面的每个顶点移向M内部，保证收缩后的模型$M^{'}$被包含于M内部，即$M^{'} subseteq M$。收缩的数量由参数$varepsilon$控制，该参数确定了每个顶点可移动的最大距离。这说明对于M上的每一点，$M^{'}$ 上都存在一点，使得它们之间的距离不超过$\varepsilon$。正式地说，M和$M^{'}$表面之间的豪斯多夫距离（Hausdorff distance）至多为$\varepsilon$。关于豪斯多夫距离，我不多解释（因为我也不懂），你可以去维基百科上看看。下面给出收缩算法：
      为了移动M表面的顶点，我们必须计算出一个体积，使得p在其中移动时能够保证$M^{'} \subseteq M$。这块满足要求的体积叫做p的星形区域的核（kernel of the star of p），简称p的核（kernel of p）。与之相对，p的随意移动很可能无法满足这个要求，如下图所示：
      在M的四面体化模型T中，顶点p的星形区域star(p)是p所属的所有四面体的集合。收缩算法的步骤2通过收集T中p所属的四面体来计算star(p)。这些四面体的并形成了被star(p)占据的体积$v_{p}$。现在我们要在$v_{p}$ 的核中计算一点$\tau_{p}$，使得p可以自由地在线段$\overline{p\tau_{p}}$上移动。$v_{p}$ 的核定义为
$\begin{equation} kr \left (v_{p} \right ) = \left \{x \in v_{p} \mid \overline{xr} \subseteq v_{p}, \forall r \in v_{p} \right \}\tag{1} \end{equation}$
具有非空核的多面体叫做星凸（star convex）。直观地，如果我们将$v_{p}$看成一个房间，核就是这样一些点的集合：从这些点我们能够看到整个房间。计算核的一种方法是先找到$v_{p}$边界上的所有三角形，对于每个三角形，都存在一个包含它的平面，该平面将整个空间分成了两个半空间（half-space）。取包含$v_{p}$的半空间。$v_{p}$的核就是所有这样的半空间的交集。根据定义，如果p在它的核内移动，则M中原本与p相互可视的点依旧与p相互可视，这就保证了产生的新多面体总是被包含于原多面体中。如果你没理解，看看上图，它是一个反例。作者给出了正式结论：如果M的每一个表面顶点都在它的星形区域的核中移动，则收缩模型$M^{'}$包含于M。
      在某些情况下，核可能是一个单元集（singleton），即$kr \left (v_{p} \right ) = \left \{p \right \}$。如果你想象不到这样的情景，请看下图：
左图是一个四面体化模型，阴影三角形表示表面三角形，可以看出，表面顶点p的核不是一个单元集。但是，在左图中增加两个四面体后，p的核就变成了单元集。因为p的核肯定位于新加入的两个四面体形成的楔子（wedge）的交集中，而它们的交集仅包含一点p。这时，p就无法做出任何移动。在这种情况下，可以保持p不做任何移动，也可以将p的一个足够小的邻域从模型上剪除（为什么这样做，我没搞清楚，猜测是因为“轻轻地”剪除该顶点就相当于对这个棱角做了平滑处理，让这个顶点不再是一个顶点）。
      现在我们可以在$kr \left (v_{p} \right )$中任选一点$\tau_{p}$，并且在线段$\overline{p\tau_{p}}$上移动点p。特别地，我们选取的移动方向为沿着表面法线$N_{p}$的方向，其垂足位于p，其中，$N_{p}$可由角度平均加权法（mean weighted by angle）估算得出。这个方法我也不清楚是怎么一回事，如果你感兴趣，去看这篇论文：A Comparison of Algorithms for Vertex Normal Computation。事实上，还有一件事令我困惑：既然p是一个顶点，它的法线又该怎样定义呢？算了，暂且不管那么多。令直线l穿过p且与$N_{p}$方向相同，计算l与$kr \left (v_{p} \right )$的交集，由于$kr \left (v_{p} \right )$是凸的（为什么是凸的？还是搞不懂），它们的交集应该是一条线段$\overline{p \tau_{p}}$。不过，也可能交集只包含一个点，即点p。在这种情况下，我们只是简单地将l投射到$kr \left (v_{p} \right )$的边界上（我还是很抓狂地没看懂啊）。
      上述这种寻找$\tau_{p}$的方法从概念上看比较简单（我想说，简单吗），但是效率更高的方法是采用线性编程，将$\tau_{p}$看作位于$-N_{p}$ 方向上的一个满足所有半空间约束（half-space constraints）的极点（extreme point）来计算（我毫无悬念地又没有理解）。
      最后，给定一个收缩因子（shrinking factor）$s \in [0,1]$和每个顶点可以移动的最大距离$\varepsilon$，将p朝$\tau_{p}$移动，如下图所示：
其中，p的新位置$p^{'}$ 按照如下公式计算：
$\begin{equation}p^{'} = p+ \left (s \cdot min \left \{\varepsilon, \left \| p\tau_{p} \right \| \right \} \right )\frac{p\tau_{p}}{\left \| p\tau_{p}\right \|} \tag{2}\end{equation}$
      上述公式中，$\frac{p\tau_{p}}{\left \| p\tau_{p}\right \|}$显然是$\overline{p\tau_{p}}$的单位向量。当s=0时，$p^{'}=p$ 。当s=1时，若$\left \| p\tau_{p}\right \| \leq \varepsilon$ ，则$p^{'}=tau_{p}$，否则，$p^{'}=p+ varepsilon frac{ptau_{p}}{left | ptau_{p}right |}$，此时，p移动的距离已经达到上限。由此可知，收缩因子s越大，$p^{'}$离p越远，模型收缩的程度也就越大。
      在SSRP中，控制模型收缩程度的瘦化因子（thinning factor）是人工设定的，这使得SSRP灵活性不足。本文提出的MLDP，即multi-level dilation planner，通过使用二分查找的办法来自适应地确定s。首先来看MLDP算法：
      算法结构很清晰，整体上是一个二分查找的过程。每次迭代开始时，收缩因子s的值均为0.5。之后调用Shrink算法，得到M的收缩模型$M^{'}$，同时，自由空间F自然而然地被膨胀。然后利用SBL在膨胀自由空间（dilated free space）$F^{'}$ 中寻找一条路径$gamma^{'}$：如果没有找到路径，说明自由空间膨胀程度还不够，即模型的收缩程度还不够，因此需要增大收缩因子s；如果找到了路径，这时就要尝试修复该路径了——因为这条路径位于膨胀自由空间$F^{'}$ 中，现在需要使它产生一定的形变，从而得到原始F中的路径，如果路径修复失败，说明收缩因子s的值过大，导致模型收缩前后自由空间的变化太大，从而无法将$F^{'}$ 中的路径形变回F中，因此需要减小收缩因子s，如果路径修复成功，则返回修复后的路径$\gamma$。
      现在的问题只剩下怎样修复路径了。这里的方法与SSRP中采用的方法类似，即要修复里程碑q，就在q的邻域中采样，如果这个新的采样点被接受，那么点q修复成功，我们就可以着手修复路径$\gamma^{'}$上的下一个里程碑了；否则，我们就增大邻域范围，重新采样。如此反复，直到新的采样点被接受，或者达到最大迭代次数。要修复两个里程碑q和$q^{'}$之间的边，首先对它的中点$q_{m}$进行碰撞检测：如果$q_{m}$有碰撞，则尝试修复它，如果修复成功，并且得到的新点为$q_{m}^{'}$，则原来的边被分成两条新边，一条连接q和$q_{m}^{'}$，一条连接$q_{m}^{'}$和$q^{'}$，之后再对这两条新边进行修复，如此迭代，直到达到了指定份额分辨率（resolution）为止。在此期间，只要有任何一次修复不成功，就只能返回一条空路径NIL。如果一开始$q_{m}$就没有碰撞呢？那得到的两条新边就分别连接q，$q_{m}$和$q_{m}$、$q^{'}$了，剩下的步骤如上所述。
      实验部分和总结部分略过。
      总体上说，这篇文章最主要的部分一个是讲收缩算法，一个是讲如何确定收缩因子。前者我没有完全看懂，几乎就是将它的主要过程翻译了一遍。后者是看懂了，而且这种自适应确定s的方法我感觉还是不错的。


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

上一篇：读书笔记——SSRP，一种解决PRM中窄道问题的方法
下一篇：读书笔记——OBPRM，一种基于障碍物的PRM