本文作为论文MAPRM: A Probabilistic Roadmap Planner with Sampling on the Medial Axis of the Free Space的阅读笔记。

      首先，做好思想准备：这是一篇需要一点拓扑学知识的文章。如果你对拓扑学有较多的了解，那么这篇文章根本没难度；否则，请先跟随我给自己充一些电，了解一下拓扑学中的几个常见概念。

      先来了解一个听起来超带感的概念——同伦。在《拓扑学奇趣》这本书中给出了浅显易懂的解释和生动的示例：设h是图形X内的一条道路，从起点$x_{0}$引向终点$x_{1}$。换句话说，$h: \left [0,1 \right ]\rightarrow X$是连续映射，满足条件$h\left (0 \right )=x_{0},h\left (1 \right )=x_{1}$，我们让这条道路在图形X内作保留端点$x_{0}$和$x_{1}$不变的连续形变。如下图所示：

      被形变的道路位置用一些细线画出。我们总是只讨论道路的这样的形变，它保持端点不变。图形X内两条有共同端点的道路$h_{1},h_{2}$说成在这个图形内同伦，是说利用（在图形X内变动的）形变可以把$h_{1}$变成$h_{2}$；道路的同伦记做$h_{1}\sim h_{2}$。

      了解同伦以后，再来看一个拓扑学概念——收缩。我参考了维基百科，用以说明何为“收缩”。在拓扑学中，收缩（retraction），顾名思义就是将整个空间收缩到一个子空间：设X是一个拓扑空间，A是X的一个子空间，那么连续映射$r:X\rightarrow A$是一个收缩，如果r在A上的限制是A上的恒等映射，即对$\forall a\in A$，有$r\left (a \right )=a$。看一下映射r的定义域和值域便知，r将一个较“大”的空间X中的元素映射到一个较“小”的空间A中，且A中的元素统统被映射到A中。由此看来，连续映射r确实起到了“收缩”的作用，即把X收缩到A，而A仿佛很稳定，在映射过程中保持着“不变性”。在拓扑学上，这个子空间A就叫做收缩核。

      现在让我们认识一下形变收缩（deformation retraction）。我们称连续映射$d:X\times \left [0,1 \right ]\rightarrow X$是一个形变收缩，如果对$\forall x \in X$以及$\forall a \in A$，有$d\left (x,0 \right )=x,d\left (x,1 \right )\in A,d\left (a,1 \right )=a$。第一次看到这里，你是什么感觉？至于你晕不晕菜，反正我是晕菜了。好吧，再深入思考一下！既然d叫做“形变收缩”，那么，它一定要和“收缩”挂上钩。你能从$d\left (x,0 \right )=x,d\left (x,1 \right )\in A,d\left (a,1 \right )=a$中找到“收缩”的影子吗？好吧，看来还不够直观，让我们把上面的条件重写一遍：对$\forall x \in X$以及$\forall a \in A$，有$d_{0}\left (x \right )=x,d_{1}\left (x \right )\in A,d_{1}\left (a \right )=a$。现在，你应该认识到，$d_{0}$和$d_{1}$是两个不同的连续映射！现在再来找“收缩”的影子：$d_{1}\left (x \right )\in A,d_{1}\left (a \right )=a$，这不就是一个以A为收缩核的收缩吗？它将空间X中的元素映射到子空间A中，并且满足在A上的限制为恒等映射。好了，那么$d_{0}\left (x \right )=x$又是什么呢？显然，它是X上的恒等映射。现在，我们终于认识到，形变收缩是收缩与X上恒等映射的同伦！别告诉我你还没有理解同伦——如果是这样，你最好把我在本文前面介绍的同伦概念再看看，或者，你干脆去买一本《拓扑学奇趣》吧，经典科普名著，你值得拥有！哦，别忘了，子空间A称为X的形变收缩核。

      强形变收缩（strong deformation retraction, SDR）又是怎么回事呢？无非是在形变收缩的定义中再加上如下条件：对$\forall t \in \left [0,1 \right ],d \left (a,t \right )=a$。这显然是增强版的形变收缩，意味着在同伦中始终保持子空间A中的点不动，此时，A就叫做X的强形变收缩核。

      行文至此，我终于可以长出一口气，然后幽幽地来一句：现在，我们开始讲论文。。。同学，你是在坑爹吗？上面讲了一堆拓扑学概念，原来还没进正文啊！没办法，这篇论文就是不好懂，如果没有上面的铺垫，咱们可以就此洗洗睡了。这篇文章的作者之一，在运动规划领域大名鼎鼎的、来自tamu的Nancy M. Amato教授，写出来的牛叉文章一贯如此难以理解，之前她关于OBPRM的文章，我硬是看了两天且啥也没看懂。

      这篇文章的整体结构就是，先罗列一堆关于中轴（medial axis，MA）的事实，然后说明C-Spcace的中轴是其强形变收缩核（SDR）。怎样在C-Space和其SDR之间建立联系呢？答案是经典收缩映射（canonical retraction map）。经典收缩映射能够将自由空间F中的点映射到F的中轴上。以经典收缩映射为基础，作者提出了扩展收缩映射（extended retraction map），该映射可以将C-Space、而不仅仅是F中的点映射到F中轴上。这意味着，就算采样点落在了障碍物上，扩展收缩映射也能将它“拉回”到F的中轴上，这种“变废为宝”的行为显然起到了增强采样点之功效，而且得到增强的部分主要就是狭窄通道。下面听我细细道来。

      我们先抛开运动规划不谈，只考虑平面上某个区域的中轴的性质。说到中轴，我们先来看看它究竟为何物。貌似第一篇提出中轴概念的论文是Harry Blum的A Transformation for Extracting New Descriptors of Shape。这篇论文发表于1967年，年代太久远了，文风相当自由，和现在的论文都不大一样，以至于我刚读两段话就晕头转向了。于是我又找到Evan C. Sherbrooke等人发表于1995年的论文Computation of the Medial Axis Transform of 3-D Polyhedra。这篇文章给出的中轴定义比较易于理解。他先给出了最大闭圆盘（或闭球）的概念：令D是2-D或3-D欧式空间的一个子集。称一个闭圆盘（3-D情况下为闭球）在D中是最大的，如果它包含于D，且不是其他任何一个包含于D的圆盘（球）的真子集。在此基础上，我们定义中轴的概念：集合D的中轴（medial axis，MA），或骨架（skeleton）M(D)，是D中所有最大闭圆盘（或闭球）的中心的轨迹（locus）（包括轨迹的极点（limit points））。D中轴的半径函数（radius function）是定义在M(D)上的连续实函数，中轴上每一点的半径函数值等于相应的最大圆盘或球的半径。D的中轴变换（medial axis transform，MAT）定义为中轴及相应的半径函数。

      事实上，论文中也给出了关于中轴的定义。但是定义中轴之前，论文先定义了一些相关概念。P是有限个闭合多边形（包括它们的内部，可能有洞）的不相交并集。对$\forall x \in P$ ，定义P的子集$B_{P}\left (x \right )$为中心位于x的尽可能大的闭圆盘。注意，这里的“尽可能大”的闭圆盘与上面定义的最大闭圆盘不是一个概念：“尽可能大”的闭圆盘可能包含于最大闭圆盘中。为了便于理解，请看下图：

其中，以x为中心的圆盘是尽可能大的，但是以$r_{P}\left (x \right )$为中心的圆盘是最大的。论文为了说明$B_{P}\left (x \right )$的概念，还给了很形式化的定义：$B_{P}\left (x \right )=\overline{B}\left (x,\rho_{P}\left (x \right ) \right )$，其中$\overline{B}\left (x,r \right )$表示中心为x、半径$r\geq 0$的闭圆盘，$\rho_{P}\left (x \right )=dist\left(x,\mathbb{R}^{2}\setminus P \right )$是P中的点到边界的距离（若点位于P外，则其值为0），$dist\left (x,S \right )=inf_{y\in S}d\left (x,y \right )$。现在来给出中轴的另一个定义：P的中轴MA(P)定义为P中这样一些点x的集合，这些点对应的$B_{P}\left (x \right )$是最大的，即$MA\left (P \right )= \left \{x\in P\mid \nexists y\in P with B_{P}\left (x \right )\subsetneq B_{P}\left (y \right ) \right \}$。下面来看看传说中的中轴到底长什么样：

      显然，多边形内部的实线就是中轴。设$\partial P$为P的边界，$x \in P$，若x在$\partial P$上有唯一一个最近点$x_{0}$（此时，$d \left (x,x_{0} \right )=\rho_{P}\left (x \right )$ ），则称$x_{0}$为一个简单点（simple point）；否则，称$x_{0}$为一个多重点（multiple point）。

      事实上，P的凸顶点（convex vertex）全部位于P的中轴上。因此，在P的所有顶点中，凸点处理起来还是比较方便的。为了方便表达，定义$P^{'}$ 为P减去它的非凸顶点（non-convex vertex）。下面就是一连串的结论了：

1. 设$x \in \partial P$，则$x \in MA\left (P \right )$当且仅当x是P的一个凸顶点。
2. P的任意一个多重点都在$MA\left (P \right )$上。若$x \in MA\left (P \right )$，则x位于P的内部当且仅当x是P的一个多重点。
3. 对$\forall x\in P^{'}$，$B_{P}\left (x \right )$包含于唯一一个最大圆盘$B_{P}\left (y \right )$，其中$y \in MA \left (P \right )$。并且，如果$x \in P^{\circ}\setminus MA \left (P \right )$，则y在射线$\overrightarrow{x_{0}x}$上，其中，$x_{0} \in \partial P$是x的唯一的最近边界点（unique nearest boundary point）。如果x在P边界上，但不是P的顶点，则y在以x为垂足的边界法线上。
4. 映射$P \setminus MA \left (P \right ) \rightarrow \partial P$将每个点映射到它的最近边界点。该映射是连续的。
5. $MA\left (P \right )$是一个闭集合。

      以上结论都是有证明的，但是在本论文中没有给出。说实话，就算给了，我也不想看，况且，我也很可能看不懂。现在要发扬“拿来主义”，对于自己没必要知道的，让它当黑盒去吧！
      说了那么多，终于轮到强形变收缩核（SDR）出场了！设Y是$X \subseteq \mathbb{R}^{2}$的SDR，那么Y就保留了X的拓扑结构。记住，这点相当重要！因为这意味着，如果X中某两点之间存在路径，那么，它们在Y中的像之间也存在路径。如果我们将X看作规划问题中的自由空间F，那么，我们就可以把求X中的路径转化为求Y中的路径。谢天谢地，我们在本文的刚开始的时候就知道了什么是SDR，什么是中轴，现在，我们十分希望多边形P的中轴就是其SDR，因为这样看上去才达到了化简问题的效果。不过，刚才说过，不包含非凸顶点的$P^{'}$更容易处理。让我们来看看MA(P)是不是$P^{'}$的SDR。首先，定义一个映射$r_{P}:P^{'} \rightarrow MA \left (P \right )$ ，它将每个不在MA(P)上的点x映射为唯一一个点$y \in MA \left (P \right )$，使得$B_{P}\left (x \right ) \subseteq B_{P}\left (y \right )$。我们称$r_{P}$ 为经典收缩映射（canonical retraction map），如下图所示：
      对于强形变收缩的定义，论文里是这么写的：Y是$X \subseteq \mathbb{R}^{2}$ 的子集，如果X可以连续地形变为Y而不需Y的任意一点，则Y是X的强形变收缩核（SDR），即一定存在这样一族函数$h_{t}:X \rightarrow X$ ，其中$t \in \left [0,1 \right ]$ ，使得对$\forall y \in T,\forall t \in \left [0,1 \right ]$ ，有$h_{t}\left (y \right )=y$ ；对$\forall x \in X$ ，有$h_{0}\left (x \right )=x,h_{1}\left (x \right ) \in Y$ ；$h_{t}\left (x \right )$ 是t和x的连续函数。有了本文开始处对SDR的讲解，相信你已经能够理解该论文对SDR的定义了。现在，令$h_{t}\left (x \right )=\left (1-t \right )x + tr_{P}\left (x \right )$，显然，连接x和$r_{P}\left (x \right )$的线段位于P内，而$h_{t}$的几何意义无非是该线段上一点。这个函数到底能否用作SDR定义中的那一族函数呢？显然可以，但是这里就不分析了。论文作者以结论的形式给出了我们想要的结果：MA(P)是$P^{'}$在映射$h_{t}$下的SDR。
      现在，我们要将上面的一堆理论、结论用于真正的运动规划问题了。先来考虑C-Space为二维的情况。如果我们假设自由空间F是一个多边形（有洞），那么我们可以用前面的理论将F（确切地说，是$F^{'}$ ）映射到它的中轴MA(F)，不过，这样做的好处大概也仅限于增大了采样点与障碍物之间的间隙（clearance）而已。而我们想要的是，增加在窄道（narrow passage）中采样的机会。于是，作者基于经典收缩映射，提出了扩展收缩映射（extended retraction map）的概念。
      记位姿空间为C，它是$\mathbb{R}^{2}$的一个封闭矩形区域；C障碍物空间为$B \subseteq C$，它是有限个多边形的不相交并集；自由空间为$F=\overline{C \setminus B}$，它由有限个连通分支组成，每一个连通分支都是一个多边形；接触空间（contact space）为$\partial F$，简单起见，我们假设$\partial B$和$\partial C$不相交，所以，$\partial B \subseteq \partial F$。
      根据前面的理论，我们已经可以利用经典收缩映射$r_{F}$ 将$F^{'}$映射到它的中轴MA(F)。现在，我们考虑怎样对这种映射做一个扩展，使之可以将$C \setminus MA \left (B \right )$映射到MA(F)。有一个显而易见的想法：$F^{'}$中的点是沿着一些以F边界上的非顶点点（non-vertex points）为垂足的法线收缩到MA(F)的。这些线可以被连续地延伸至B中，直到碰到B的中轴，这样，$B\setminus MA \left (B \right )$中的任一点都可以沿着这些线移动到F中。也就是说，将B中的每一个点朝着它的位于$\partial B$上的最近边界点移动，然后穿过$\partial B$，进入F，直到碰到MA(F)。如下图所示：
      于是，论文作者又给出了一个肯定的结论：C，B和F的定义如上所述，经典收缩映射$r_{F}:F^{'} \rightarrow MA\left (F \right )$可以被连续地扩展为映射$C \setminus MA \left (B \right ) \rightarrow MA\left (F \right )$。这个结论的证明也在这篇论文中给出：根据前面给出的结论4，即“映射$P \setminus MA \left (P \right ) \rightarrow \partial P$将每个点映射到它的最近边界点。该映射是连续的”，我们可以将$B \setminus MA\left (B \right )$连续地映射到边界$\partial B \subseteq \partial F$。F的所有非凸顶点都是B的凸顶点，而B的凸顶点都位于MA(B)上，且不可能是任何一个$B \setminus MA\left (B \right )$内的点的最近边界点，因此，我们实际上把$B \setminus MA\left (B \right )$映射到了$F^{'}$中，然后再应用经典收缩映射$r_{F}$，就得到映射$C \setminus MA \left (B \right ) \rightarrow MA\left (F \right )$。这个映射，就叫做扩展收缩映射。使用先前定义过的$h_{t}$，可以证明MA(F)是$C \setminus MA \left (B \right )$的SDR。
      利用中轴的概念，可以很容易地定义通道（corridor）。令$r_{F}:F^{'} \rightarrow MA \left (F \right )$位经典收缩映射，F中的一条通道是$F^{'}$的连通子集，满足$r_{F}\left (S \right ) \subseteq S,r_{F}^{-1}\left (S \cap MA\left (F \right ) \right ) \subseteq S$。所有的通道显然都满足上面两个条件，如果你感觉对通道的这个定义还是没有把握，不妨以B代S，然后看看是否满足上述的两个条件。
      现在，我们来看看扩展收缩映射到底有没有在狭窄通道（narrow corridors）中增加采样点。首先，通过扩展收缩映射，原本就位于通道S以内的点仍在S中；除此以外，B中的所有最近边界点位于S中的点也被映射到S中；如果$x \in \partial S \cap B^{'}$，则x是所有位于（开）线段$\overline{xr_{B}\left (x \right )}$上的点的最近边界点。这些线段垂直于$B^{'}$的分段线性边界。于是，论文作者又给出了一个至关重要的结论：设$S \subset F^{'}$是一条通道，$r_{B}:B^{'} \rightarrow MA \left (B \right )$是B上的经典收缩映射，$q:C\setminus MA \left (B \right ) \rightarrow MA \left (F \right )$是扩展收缩映射，$x \in C$， 则C中由q映射到S的点的体积（我感觉这里用“面积”更合适）不小于：
$\begin{equation}Vol\left (S \right ) +\int_{\partial S \cap B^{'}}d\left (x,r_{B}\left (x \right ) \right )\mathrm{d}l\left (x \right )\tag{1}\end{equation}$
其中，$\mathrm{d}l$是边界上的单位弧长。
      上式和增强狭窄通道采样点又有何关系呢？咱们来分析一下。首先，式子的第一项不用看，因为它代表的就是S自身的体积。于是，关键的地方就在于式子的第二项。第二项其实是一个线积分，$d \left (x,r_{B} \left (x \right ) \right )$为“长”，$\mathrm{d} l \left (x \right )$为“宽”，这个积分表示$r_{B}$为q所贡献的那一部分体积（面积）。从直观的几何意义上讲，$d \left (x,r_{B} \left (x \right ) \right )$可以体现出x附近障碍物的“厚度”——试想，如果$d \left (x,r_{B} \left (x \right ) \right )$的值较小，则说明障碍物的中轴距离障碍物表面较近。而构成狭窄通道的障碍物，相对于狭窄通道来说，往往较“厚”，于是，上面的式子无疑告诉我们，通过扩展收缩映射q，狭窄通道中的采样点会增加不少；反之，如果通道并不狭窄而是比较宽阔，那么构成这些通道的障碍物必然相对不“厚”，于是上面式子的第二项所做的贡献就相对较少了。这说明，扩展收缩映射确实起到了在狭窄通道内增强采样点的作用！
      是该展现传说中的MAPRM算法的时候了！
      事实上，MAPRM就是将扩展收缩映射应用到PRM的采样过程中而已，结果就是采样点几乎都在自由空间F的中轴上。值得注意的是，算法并没有计算中轴，而是在第9步采用二分法将采样点拖放到中轴上。下面再看两张效果图：
其中，图a是均匀采样的结果，图b是MAPRM采样的结果。可以明显看出，图b中的采样点几乎都在F的中轴上，且图a中，两个障碍物之间的狭窄通道中的采样点较少，而图b中则较多。再看两张图：
其中，图a是均匀采样的结果，图b是MAPRM采样的结果。这两幅图的区别好像仅仅在于图b的采样点几乎都在F的中轴上，而图a和图b在狭窄通道中的采样点似乎都不太多。这是因为图b中构成横向狭窄通道的障碍物不够“厚”，根据式1，自然导致狭窄通道中的采样点不够多。
      文章写到这里，我已经实在不想继续写下去了，我感觉我这个博客的长度都要赶上一篇论文了，而且我对tamu的这位牛女的这种用到各种几何和拓扑信息的论文已经相当有意见了。但是，上面介绍的MAPRM应用场景是二维的C-Space。如果是三维的呢？我还是再接再厉写完整吧，好在二维和三维的原理相同，应该不用费太多时间精力了。于是，下面的场景切换为：在$\mathbb{R}^{3}$ 中，一个刚性多面体在多面体障碍物之间运动。论文作者选用了自由飞行刚体（free-flying rigid body）U作为例子。
      论文作者怕大家不懂C-Space，于是花了一些篇幅介绍怎样为U建立C-Space。我觉得自己也有必要熟悉一下这种基础性的东西。在U上建立一个可以移动的坐标系，称为物体坐标系（body frame），同时，还要建立一个固定的坐标系，称为世界坐标系（world frame），则U的位姿可以由物体坐标系相对于世界坐标系的位置和方向确定。$SO \left (3 \right )$中的一个旋转矩阵（rotation matrix）给出了物体坐标系相对于世界坐标系的方向，$\mathbb{R}^{3}$中的一个向量则确定来物体坐标系的原点相对于世界坐标系的位置。注意，$SO \left (3 \right )= \left \{R \in \mathbb{R}^{3 \times 3} \mid RR^{T}=I ,det \left (R \right )=1\right \}$。令$SE \left (3 \right ) = SO \left (3 \right ) \times \mathbb{R}^{3}$，$\left (R,p \right ) \in SE \left (3 \right )$表示对物体坐标系中某点的坐标$q_{a}$进行$Rq_{a}+p$操作，从而得到其对应的世界坐标系中的坐标。令$c= \left (R,p \right )$，则$c \cdot U$或$\left (R,p \right ) \cdot U$表示当U位于位姿c时，U上所有点在世界坐标系中的坐标。
      现在，在工作空间（workspace）$\mathbb{R}^{3}$ 中给定障碍物V，那么，一些U的位姿就会与V产生碰撞，即$\left \{c \in SE \left (3 \right )\mid \left (c \cdot U \right ) \cap V \neq \varnothing \right \}$，这个集合里的位姿就叫做V的C障碍物（C-obstacle of V）。为了方便采样，我们令位姿空间C是$SE \left (3 \right )$的一个“矩形”子集，即$C=SE \left (3 \right ) \times W$，其中，W是$\mathbb{R}^{3}$的一个封闭矩形区域；令U和$V_{j},j=0, \cdots ,n$是$\mathbb{R}^{3}$中的封闭多面体（包括内部，可能有洞），其中$V_{j}$之间不相交，$V= \bigcup V_{j}$；自由空间F定义为C减去$V_{j}$的C障碍物；$\partial F$是接触空间（contact space），它使U与V或C的边界接触。
      到这里，我们已经可以看到曙光了。我们只需将二维情况下的平面（plane）和多边形障碍物换成$SE \left (3 \right )$和C障碍物即可。作者给出了如下几个结论：

• 设c是一个自由位姿（free configuration），如果$c \cdot U$有两个不同的最近障碍物点（nearest obstacle point），即存在两个不同的点$y_{1},y_{2} \in V$，使得$dist \left (V,c \cdot U \right )=dist \left (y_{1},c \cdot U \right )=dist \left (y_{2},c \cdot U \right )$，则c在F的中轴上。
• 将一个无碰撞位姿收缩到F的中轴上，意味着移动U，使之远离它在V上唯一的最近点，直到出现至少两个不同的最近障碍物点为止，即如果c是一个没有在中轴上的自由位姿（free configuration），且$x \in c \cdot U$和$y \in V$是$c \cdot U$和V之间最近的一对点，那么，收缩映射沿$\underset{xy}{\leftrightarrow}$移动U，使之远离y，直到U不再有唯一的最近障碍物点。
• 对于碰撞位姿c，（扩展）收缩映射生成使U脱离碰撞的最短平移路径（因此，U将与V接触），并且一直沿着这个方向平移，直到U不再有唯一的最近障碍物点。
• 我们仅收缩这样的碰撞位姿：它们的最近接触位姿使U与V仅有一点接触。那些使U与V有一个以上接触点的接触位姿已经在中轴上，这种情况就像二维情况下F的凸顶点一样。

      好了，现在，我们终于可以把算法摆上来了！
      算法没什么好讲的了，前面的理论懂了，算法就懂了。值得注意的地方是，算法并没有计算中轴或者障碍物边界。这是起到了很好的省时效果。不过，整体上来看，MAPRM不是一盏省油的灯，相对于一般的PRM而言，它还是使用了太多的复杂的几何计算，更难实现。
      实验和总结略过。这篇博客终于到此结束了！


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

上一篇：读书笔记——桥测试，一种解决PRM中窄道问题的好方法
下一篇：读书笔记——SSRP，一种解决PRM中窄道问题的方法