科学网 › 标签 › 运动规划

标签: 运动规划

相关帖子	版块	作者	回复/查看	最后发表

没有相关内容

相关日志

读书笔记——基于效用引导的位姿空间采样方法: wycpp 2014-2-18 21:34; 本文作为论文Toward Optimal Configuration Space Sampling的阅读笔记。这篇文章最大的看点在于把经济学中的“效用（utility）”概念引入运动规划，通过不断选取效用最高的采样点加入roadmap，使得roadmap能够较好地捕获自由空间的连通性，同时保持较小的规模。说白了，引入“效用”概念还是为了“把好钢用在刀刃上”，效用高的采样点往往位于困难区域，效用低的点往往位于空旷区域，每次采样都选择效用值高的点，使得困难区域的采样点密度得以增强，同时避免在空旷区域分布过多的采样点。先来介绍一下“效用”的概念。维基百科上给出了一个极为通俗的解释：Utility is usefulness, the ability of something to satisfy needs or wants，即效用就是有用性，是某事物满足需求的能力。在经济学中，维基百科说“In economics, utility is a representation of preferences over some set of goods or services”，即效用是对某些商品或服务偏好的表现。我们越偏好一个东西，它的效用就越高。基于效用的概念，1738年，来自著名的“伯努利家族”的Daniel Bernoulli，首次提出了形式化的“期望效用理论（expected utility theory）”，用于分析人们在具有不同结果（outcome）的风险项目（risky project）之间做出的选择。据维基百科说，期望效用理论的第一次重要应用是John von Neumann和Oskar Morgenstern把期望效用最大化的假设用于他们的博弈论（game theory）中。本论文中计算一个采样点的效用时，采用的就是其“期望效用（expected utility）”，因此，这里还要对期望效用理论做一个简单介绍。期望效用理论中有一个非常重要的概念，叫做“抽彩（lottery）”，中文直译为“乐透”！顾名思义，这是一个和运气有关的行为，对于一个风险项目而言，每个抽彩都对应着一组不同的可能结果，事实上，抽彩就是这些结果的概率分布。设L是一个简单抽彩（simple lottery），X是L所有可能结果（outcome）的集合，L定义了X上的概率分布：，其中是在给定抽彩L后，结果出现的概率。如果抽彩L的可能结果中依然存在抽彩，则L称为复合抽彩（compound lottery）。下图分别表示了具有三个可能结果的简单抽彩和具有两个可能结果的复合抽彩：给定一个抽彩集合，由于每个抽彩的结果都是不同的。对于一个个体（agent）而言，它需要一个偏好函数（preference function），用于计算对某个抽彩的偏好程度，从而可以将不同的抽彩按照偏好程度排序。这个函数就是效用函数（utility function）。效用函数计算出的值就是效用，即个体对抽彩的偏好程度。效用函数的作用就是将这个偏好程度数字化了。设存在两个抽彩和，如果某个体偏好的程度比偏好的程度高，则的效用大于的效用。现在的问题是，每个抽彩的效用应该怎样计算呢？由于每个抽彩都包含了若干结果，我们自然能够想到利用这些结果去表示抽彩。这就引出了“期望效用（expected utility）”的概念。令为抽彩所有可能结果的集合，为某个结果x的效用，则的期望效用定义为：其中，是结果x出现的概率。一旦计算出每个抽彩的期望效用，就可以对这些抽彩排序了。一个抽彩的效用越高，则我们越偏好它。对应到采样过程中，一个采样点的效用越高，则说明它对建立roadmap的贡献越大。期望效用理论就介绍这么多。我们更关心的是它在运动规划中的应用。为了规划出完整有效的路径，规划器（planner）一定希望它能够完全掌握位姿空间的结构信息——显然，这是不可能的，对于依靠采样来探索位姿空间的规划器尤其不可能。既然如此，我们就退而求其次，力求用尽量少的采样，即探索（exploration），来获取尽量多的关于位姿空间结构的信息。俗话说“兵不贵多而贵精”，只要每一个采样点足够“精”，恰好落在最有价值的地方，即它的效用足够高，用少数采样点就能获得极有价值的位姿空间结构信息。现在的问题是，这可能吗？答案是肯定的。我在上一篇博客中描述了论文Sampling-Based Motion Planning Using Predictive Models提出的利用局部加权回归，结合已有采样点信息建立位姿空间的近似模型，并以此模型预测采样点的有效性，同时结合主动学习指导新采样点生成的方法。在这篇论文中，能够最大限度减小近似模型方差的采样点被认为是最有价值的。不过，用局部加权回归来预测采样点有效性的计算复杂度似乎有点大，我们可以用一种更简洁的方法预测采样点有效性，那就是K近邻。据该论文说，如果一个采样点的周围都是碰撞点，那么该采样点很可能是碰撞点；反之，如果它的周围都是自由点，那么它极可能是自由点。所谓“近朱者赤，近墨者黑”嘛！也就是说，我们的近似模型不用局部加权回归来建立，而是用K近邻的方法来建立。现在，我们需要一种衡量采样点“价值”的标准，用于取代上述论文给出的“方差度量”——毫无疑问，该论文使用的正是期望效用。设M是根据当前roadmap中的采样点信息建立的位姿空间近似模型，R为当前roadmap，对于一个即将加入R的采样点q而言，其期望效用的计算公式如下：现在，我们将每次采样都看作一次抽彩。每次抽彩的可能结果都有两个：碰撞和自由。公式中的就是抽彩结果的概率分布。注意，上面的期望效用是我们用某种方法预测出、用于指导采样点生成的，采样点q到底是碰撞还是自由也是预测的结果。这里所用的预测方法就是刚才提到的K近邻。论文作者声称K近邻的效果和局部加权回归的效果差不多，咱们就姑且相信吧！由于位姿空间中的点只有自由和碰撞两种状态，因此定义函数，用以反映状态已知的采样点q是自由还是碰撞。若q的状态未知，则利用K近邻预测q有效性的具体方法是：设是q的k个最近邻，预测结果可以按照如下公式计算：我们把认为是q为自由点的概率。设M是当前状态已知的采样点集合，则既然已经可以预测采样点为自由的概率了，即这一项已经搞定，下一个要着力解决的问题就是如何确定公式中的另一项。在建立roadmap的过程中，一个采样点效用的大小，取决于它能够在多大程度上增加roadmap对位姿空间连通性的反映程度。文章中给出了一个比较夸张的示意图：显然，图中的四个采样点的效用非常高，因为这四个点几乎可以完全反映位姿空间的连通性。这些极有价值的点是如何选取的呢？该论文的另一个亮点出现了，那就是利用熵（entropy）！熵的概念起源于热力学，但是香农将此概念引入了信息论，并取得巨大成功。在信息论中，熵是随机变量不确定性的度量，换句话说，熵是对信息内容的不可预测性的度量，熵越高，信息内容越不可预测。设是集合D上的一个离散概率分布，则该分布的熵为：给定系统S，新知识K，S在获取K之前的熵为，获取K之后的熵为，则K给S带来的信息增益（information gain）为：我们追求采样点期望效益的最大化，其实就是要使每个即将加入roadmap的采样点都能给当前系统R，也就是当前的roadmap，带来最大的信息增益。因此，论文就把信息增益当作了即将加入roadmap的采样点q的效用值：为了使上式具有合理性，我们必须定义一个概率分布，使得roadmap完全连通时具有的熵最小。事实上，roadmap是由若干连通分支组成的。每个连通分支都有自己的可见区域（visibility region），该区域包含了所有能够用一条无碰撞的直线路径连接到该连通分支的自由点。方便起见，我们希望每个连通分支的可见区域都互不相交。做到这一点也不难，为了方便描述，我定义（不是论文作者定义的，确实是我为了省事而定义的），如果点q属于连通分支C的可见区域，则C是q的可见连通分支。若点q具有多于一个的可见连通分支，我们就把q划入距离它最近的连通分支的可见区域，这里，我们设函数能够返回距离q最近的可见连通分支。定义了可见区域之后，再定义一个概率分布，即随机均匀选择的点落在某个连通分支的可见区域中的概率。显然，当roadmap完全连通，即只有一个连通分支时，R的熵最小，为0；当roadmap中有较多的连通分支时，R的熵较大。至此，我们已经可以计算一个即将加入R的采样点q所具有的期望效用了：注意，上面的推导过程中，我们假设碰撞点的效用为0。为何这样假设？并不是说碰撞点就一点用都没有，毕竟是位姿空间的组成部分嘛——关键在于它的作用不是特别大，更关键的是，论文作者似乎没想明白怎样判断一个碰撞点的效用，于是，这些点的效用就统统置零了。下面我们定量化分析R的熵和一个即将加入R的点q为R带来的信息增益。对于每个连通分支，我们定义体积，使得对于，有。设自由位姿空间的体积为，则随机均匀选择的点落在连通分支的可见区域中的概率为。于是，R的熵可以按照下式计算：若向R中新加一个点q，得到新的roadmap，记为R'。由于加入点q而为系统带来的信息增益为：由于难以计算，因此想要得到精确的信息增益值几乎是不可能的。不过，注意到是常量，由上式可得：在上式中，第一项是常量，因此只要最大化第二项，就能得到最大的信息增益。对于一个即将加入roadmap的点q，它可能成为一个独立的连通分支，也可能连接到已经存在的连通分支上。如果q成为一个新的独立的连通分支，则，也就是说，上式第二项求和的项数会增加，导致第二项变小（注意，是小于0的）。因此，我们应该尽量保证q可以连接到已有的连通分支上。然而，已有的连通分支有大有小，q应该连到较大的连通分支还是较小的呢？按照论文作者Brendan Burns的博士论文中所说，应该连接到较大的连通分支上，这么做可以使较大的连通分支变得更大，从而使第二项中的更接近0，而且，连接到较大的连通分支比连接到较小的连通分支可以更多地降低R的熵。综上所述，把即将加入roadmap的点q与已有的最大连通分支连接能够最大化信息增益。根据上面给出的原则可以推断，如果把采样点q放在两个较大的连通分支之间，即它们的边界区域，这样可以通过q将两个较大的连通分支连成一个更大的连通分支，同时也能减少R中连通分支数目，R的信息增益将被最大化。然而，怎样计算两个连通分支之间的边界区域是个难题。Brendan Burns在他的博士论文中指出，可以从两个连通分支中随机各选一个点，然后根据这两点算出它们的中点，再从中点周围随机选择一个点作为我们想要求得的点q。根据以上思路，作者给出了他的算法： OK，至此，我就把效用引导的采样方法介绍完了。不得不说，有很多老外的思路都很开阔，能把经济学和信息论的知识应用于运动规划，真的服了！; 个人分类: 运动规划|5818 次阅读|0 个评论

读书笔记——基于预测模型的运动规划: 热度 1 wycpp 2014-1-17 14:14; 本文作为论文Sampling-Based Motion Planning Using Predictive Models的阅读笔记。个人感觉，将机器学习应用到运动规划是个大趋势，可是，这方面的论文数量却是少之又少，实在令人费解。偶然间读到了University of Massachusetts-Amherst的两个牛人Brendan Burns和Oliver Brock写的论文，才发现人家早在2004年就已经有利用机器学习解决运动规划问题的研究成果了。若抛开运动规划不谈，我们通常用机器学习解决分类问题，其实也就是一个预测问题。我们希望找到一个函数，给定一个输入x，就能判断，或者说预测出x的类别，输出y就是分类结果。对于监督学习而言，我们不仅要给定一个训练样本集，还要给出各个样本所属的类别，这一特点非常适合于解决位姿空间中的运动规划问题。在位姿空间中，每一个状态已知的采样点都被当做一个类别已知的训练样本，碰撞点的类别标记为-1，自由点的类别标记为1，分类结果的输出值在之间，最终得到的类别是将y向两极离散（“两极离散”是我自己造的名词，说白了，就是看看y值更偏向哪边：，则y偏向-1；，则y偏向1；，则偏向不确定）后得到的结果。这篇论文的整体思路就是先在位姿空间中生成少量的采样点，不论是碰撞点还是自由点，全部保留，把这些点当做训练样本集。然后利用局部加权回归（locally weighted regression，LWR）建立回归方程，这个方程就是位姿空间的近似模型（approximate model），将采样点输入方程，输出就是近似模型预测的采样点的状态。但是，回归总是有误差的，因此，为了使误差最小，论文采用主动学习（active learning）的方法选取采样点。回归误差比较大的区域是近似模型没有学习好的区域，这些区域往往是困难区域。主动学习选取采样点的标准恰好是能否减小回归误差，因此，利用主动学习指导采样，能够重点增强困难区域的采样密度。不仅如此，通过结合局部加权回归与主动学习两种方法，论文还提出了预测边有效性的算法，使得该论文提出的方法可以结合延迟碰撞检测的思想并用于单查询规划。其实，依我看，这篇论文最突出的贡献应该是应用创新。它把主动学习和局部加权回归用于指导位姿空间中的采样过程。有兴趣的读者可以查阅论文Active Learning with Statistical Models，这篇论文完整地给出了用主动学习的方法降低局部加权回归误差的一套理论，恰好也是Brendan Burns和Oliver Brock的最主要参考文献。局部加权回归（下文简写为LWR），顾名思义，是一种回归方法，它的特殊之处就在于四个字：局部加权。给定一个状态未知的采样点，LWR利用该点附近的采样点适配出一个曲面，以此预测该采样点的状态。为了强调“局部”，LWR给出了一个距离加权函数，即：该函数是一个高斯函数，其中，x是待预测点，是训练样本点。可以看出，距离x越近的样本点，其权重越大，从而对x的影响越大。k是平滑参数，它决定了高斯函数的“宽度”。关于k的选择，在论文Active Learning with Statistical Models中有比较详细的谈论，这里就不展开了。既然给定了训练样本集和待预测点，就可以估计出高斯函数的均值，如下：其中，x是待预测点，是训练样本点的坐标和状态。估计出训练样本集的方差如下：协方差如下：条件方差如下：利用上面的这些参数，我们可以计算出输出的期望值：上面的方程就是局部加权回归方程，也就是我们需要的预测模型，这个模型的方差如下：至此，我们已经利用训练样本集和LWR建立了预测模型，输入一个待预测的采样点x，该模型输出它的分类结果y。这意味着后续加入模型的采样点，只需经过常数时间的计算就能获取预测的分类结果。剩下的问题就是，如何向模型中添加后续的采样点。向模型中添加采样点的过程可以看作是对位姿空间的探索过程，采样点的状态总是可以通过碰撞检测得到，加入模型的每个采样点都会增加模型对位姿空间的认知程度。一个好的采样策略所选择的采样点能够最大程度上获取关于位姿空间的信息，于是，论文采用了主动学习的方法，以选择“最有用”的采样点加入模型。什么是主动学习？这里做一点简单的科普吧！咱们先来看个例子：在学校里，好学生的学习积极性总是很高，他们不用老师督促，自己就会找很多书来读，找很多题目来做，可以主动地获取知识，这些学生的成绩越来越好；一般的学生在学习上就会稍显被动，他们被动地接收着来自老师传授，老师讲一点，他们就学一点，这些学生的成绩自然难以赶上学习积极性较高的那些学生。把这个例子放在机器学习领域，就出现了主动学习和被动学习的概念。其实，在机器学习中，大部分的学习机制都是被动的（passive），这些学习器被动地接收数据，然后做处理，可是，学习器最强大的能力恰恰在于主动收集有用数据，并“影响它们正在试图理解的世界“！（这句话太霸气，Active Learning with Statistical Models原文中就这么说的。）主动学习正是要研究如何挖掘学习器的这一强大能力，即学习器应当自主选择有用的数据来处理。在主动学习中，学习器自己负责收集训练数据集。我们假设学习器能够迭代地选择输入，获得相应的输出，然后将加入训练集（当然，可能是从某个特定集合中选取的）。主动学习关心的问题是，下一个应该如何选取？这个问题很有实际意义，如果选择得当，即进行了一次恰当的查询或者动作，一些问题中对数据的需求可能大幅下降，一些NP完全的学习问题可能变成多项式时间可解的问题。在实际应用中，主动学习在一些数据难以获取或者获取代价较大的环境中发挥了巨大威力。例如，在工业环境中，每获取一个训练数据可能要用几天的时间，花费大量的资金；如果有一种方法，能够不断选择最有用的数据，无疑可以节省大量的时间和金钱。主动学习就是这样一种方法。说完了主动学习，我们终于可以来看看它在这篇论文中的应用了。对于由LWR建立的位姿空间预测模型来说，最大的改进就是使模型的方差最小。为何要使方差最小？因为方差越大，模型的预测就越不可靠，相反，方差越小，说明模型预测越可靠。事实上，已经被模型理解地很好的区域方差较低，这些区域往往是非困难区域；未被模型较好地理解的区域方差较大，这些区域往往是困难区域。主动学习的目的正是为了找到使期望方差（expected variance）最小的采样点，以此提高困难区域的采样点密度，降低非困难区域的采样点密度。假设我们下一步要向模型中添加采样点，则加入该点后，模型具有新的均值，如下：其中，x是待预测点。在位姿空间中，它的坐标是已知的，状态是未知的。训练样本集新的方差和期望方差如下：期望协方差和平方协方差如下：期望条件协方差如下：利用以上参数，可以计算出预测模型新的期望方差，如下：由于x坐标已知，上式就成为关于的函数，主动学习要求出使上式取最小值的。在实现该算法时，可以随机选择一些状态已知的采样点作为有限样本集，然后从样本集中选择使上式取极小值的点即可。当然，我们也可以对上式求导，得到关于的导数，然后再计算出使上式取最小值的点，这个点就是下一步要生成的采样点。可见，论文中提出的采样策略将LWR与主动学习结合在一起，因此得名“主动采样（active sampling）”，算法如下：预测模型不仅能够指导采样，而且能够预测roadmap边的有效性。先说说为什么要预测边的有效性吧！首先，边的碰撞检测是很耗时的，拿普通的PRM来说，建立roadmap的时间有四分之三是用在了边的碰撞检测上。其次，roadmap中的有些边是冗余的，本来就没有必要检测这些边的有效性，因为它们根本不会最终规划出的路径上，对于单查询规划尤其如此。对于第一个问题，在建立roadmap的过程中，我们考虑用时间复杂度较低的预测机制代替时间复杂度较高的碰撞检测；对于第二个问题，由于在建立roadmap时使用了边有效性的预测机制，只有被预测为有效的边才会加入roadmap，当然，被预测为有效的边未必真的有效，在路径查询的时候，还要进行真正的碰撞检测，但是，预测机制就像一轮筛选，那些被预测为无效的边根本没有机会加入roadmap，因此可以直接减少大量冗余的边。既然明白了边的有效性预测的重要性，我们就来看看如何进行有效性预测。边的有效性预测其实很简单，形式几乎与上文所述的点的有效性预测一模一样，也是用LWR实现的，即利用边附近的采样点推测边的有效性。LWR的距离加权函数如下：其中，表示边e上距离最近的那个点。边有效性的预测方程如下：其中，是预测结果。如果预测结果大于无效阈值，就判断为无效；如果预测结果大于有效阈值，就判断为有效；否则，将边等分成两部分，分别检测这两部分的有效性，如果两部分都判断为有效，则判断为有效，否则就判断为无效。边有效性预测算法如下：利用边有效性预测机制建立的roadmap称为“predictive roadmap”。现在，我们终于可以把上文中的主动采样算法与边有效性预测算法合二为一，组成基于预测模型的路径规划算法了：其中，EXTRACTPATH过程就是真正的路径查询过程，在这个过程中需要调用真正的碰撞检测对之前被预测为有效的边进行最终检验。至此，基于预测模型的运动规划算法就介绍完了。这篇论文让我收获很大，至少了解了LWR和主动学习的概念，知道了将机器学习应用到运动规划中的办法。或许，我也可以按照这个思路写一篇论文呢。; 个人分类: 运动规划|5279 次阅读|2 个评论

读书笔记——一种基于连通性的PRM增强方法: wycpp 2014-1-3 14:01; 本文作为论文A Connectivity-Based Method for Enhancing Sampling in Probabilistic Roadmap Planners的阅读笔记。个人觉得这篇文章并不算经典，无论是算法还是行文，都没有让我产生“醍醐灌顶”的感觉。之所以还是想把这篇笔记写出来，是因为我觉得此论文给出的方法还是能够代表一类思路的，即在建立roadmap之前，先把位姿空间分成若干区域，然后在建立roadmap的过程中对每个区域一边采样一边分析，一旦发现某个区域内的自由点已经足够多，就将该区域标记为“已解决（solved）”，后续的采样就不在该区域内进行了。显然，同高斯采样和桥测试一样，这是一种非均匀采样策略，困难区域会获得更多的采样机会。这篇文章的一个亮点是在建立roadmap的过程中考察其连通性，利用连通性信息决定是否保留采样点。这是一种很直观的策略，我们在位姿空间采样，建图，无非就是为了比较准去得获得自由空间的连通性信息，一旦某个区域的具有了较好的连通性，在这个区域采样更多的点意义就不大了。下面我们来看看这篇论文给出的算法到底是怎样的。按照论文中所说，在建立roadmap之前，需要先把位姿空间划分成若干大小接近的区域。如何划分呢？文中说可以利用Hammersley序列（Hammersley sequence）生成这些区域。我只知道Hammersley序列一种低差异采样方法，具体过程不了解。另外，什么叫做低差异采样呢？貌似在很多运动规划的论文中都看到过这个概念。我在网上查这个概念，发现有一本书把它讲得很生动很清晰，这篇博客的最后我会专门解释这个概念。不管用什么方法，我们暂且认为位姿空间已经被划分成若干区域了，然后计算每个区域的质心（centroid）以及这个区域的质心到它的最邻近区域质心的距离，并将此距离当做这个区域的半径。这样，每个区域都可以看做一个由质心和半径定义的超球面（hypersphere）。显然，这些区域是相互重叠的。完成区域划分之后，采样正式开始。采样过程是一个循环过程，循环变量就是这些区域，即对于位姿空间中的每个区域，我们尝试在其中生成一个采样点，如果该采样点是自由的，而且该区域满足一定的条件，我们就把这个采样点加入到roadmap中。这里所说的“一定条件”，主要与该区域内部的roadmap的连通性有关，在后面给出的算法中会有详细体现。经过对所有区域的多轮遍历，许多区域中自由点已经达到了一定的数量，而一旦某个区域中的自由点数量足够多，我们就检查这个区域中的所有自由点是否在同一个连通分支上，如果是，说明该区域的连通性已经被准确捕获了，于是将该区域标记为“已解决”，后续的采样就不会在该区域内进行了。下面我们来看一下用于建立roadmap的完整算法：算法写得比较清楚，结合上文中我的解读，还是不难理解的。不过，还是有几处需要特别注意的地方。首先是第2行的循环的终止条件，算法中并未提及。但是，很明显，该算法描述的是一个采样和建立roadmap的过程，因此循环次数可以根据roadmap的规模来确定。其次，第4行用于在当前区域R中生成一个采样点，具体算法如下：再次，第8行的if语句还是值得我们思考一下的。如果自由点q的邻居数量少于2个，或者q的邻居并未全部位于同一连通分支上，那就说明q所在区域的连通性还没有被很好地捕获，因此需要将q加入roadmap，用来增强该区域的连通性；否则，q的邻居数量多于2个，并且全部位于同一连通分支上，说明q附近的连通性已经被很好地捕获了，再加入q的实际意义不大，因此选择不加入q。另外。第12行，算法中的句子翻译出来是“如果剩下的区域数量多于1个”，但是仔细想想，这个描述是不准确的，例如，如果现在只剩下1个区域尚未删除，那么根据算法的描述，它将永远无法被删除。所以，这句话应该理解为“如果剩下的区域数量大于或等于1”。第13行的freelimit是一个很重要的参数，因为它决定了一个区域被判定为“已解决”的标准。注意，根据算法开头第一行所说，每个区域都有一个属性，因此可以很方便地获取当前区域内自由点的数量并与freelimit作比较。显然，简单区域中的自由点数量可以很快达到freelimit并被标记为“已解决”，而困难区域中的自由点数量则迟迟无法达到该限制，算法会在这些区域生成更多的采样点。再看第14行，如果区域R中所有的自由点都在同一连通分支上，则区域R的连通性已经被很好地捕获了，因此可以将区域R标记为“已解决”并把它删除，后面就不在该区域中采样了。我们来看一个示意图，其中，所有黑色点位于同一区域，圆圈表示该区域中采样比较集中的部分，白色点属于其他区域。图a中，位姿空间中只有一个障碍物，困难区域就在障碍物的边界部分，而该区域的所有采样点都位于同一连通分支上，因此该区域被标记为“已解决”。图b中，位姿空间中有两个障碍物，它们之间形成一条窄道（narrow passage），该区域中的采样点并不在同一个连通分支上，说明该区域的连通性还未被很好地捕获，因此无法被标记为“已解决”。图c中，位姿空间中同样有两个位子障碍物和一条窄道，与图b不同的是，图c中，区域内的所有采样点都位于同一个连通分支上。虽然窄道中并没有分布采样点，但是由于该区域内部的连通性已经被很好地捕获了，因此也就不需要在窄道中采样了。我们的最终目的是找到路径，而不是找到一条穿过困难区域的路径，由此可见，考察区域内部的连通性能够帮助我们决定是否需要再该区域中继续采样。下图体现的是，随着采样过程的进行，roadmap中采样点数量和区域数量的变化趋势。可以看出，在采样刚刚开始进行时，区域数量几乎不变，采样点数量迅速增加。这是因为大部分区域中的采样点数量还没有达到freelimit，因此不会被删除。随着采样点数量的增加，区域数量逐渐减少，采样点数量增加速度变缓。这是因为很多区域的连通性已经被捕获，导致该区域被删除，对于连通性很好的一些区域，算法将不再增加这些区域中的采样。至此，论文的主要内容就介绍完了。下面我简单解释一下什么叫做“低差异采样（low discrepancy sampling）”。先介绍一本书，叫做《Physically Based Rendering: From Theory to Implementation》，是计算机图形学领域赫赫有名的巨作。我在网上找到了这本书第二版的英文电子版，有需要的可以email我（snailcoder@163.com）。这本书的第7章第4节专门讲述低差异采样，我没有全部看完，而是本着拿来主义的精神，把前面一小部分看了看，大概知道了什么是低差异采样。数学家提出“差异（discrepancy）”的概念，用于评价一种采样模式的好坏。良好分布的采样模式具有低差异值，因此，生成良好采样的问题就被转化为寻找一种合适的低差异点模式（pattern of points）的问题。那么，到底什么是差异呢？在n维空间中，为了评价一个采样点集的“质量（quality）”，我们可以考察该空间中的每个区域，并且比较该区域的体积（volume）大小与其中包含的采样点数量。通常，区域体积应该和它包含的采样点数量大体上成正比。也就是说，给定一个区域的采样点数量，我们应该可以估算出该区域的体积。当然，这种估算一般都是有误差的，当然，此误差越小越好。这时，我们需要有一个概念来描述区域的实际体积和利用采样点数量估算的体积之间的误差，即差异（discrepancy）。请看下图：上图所示的是一个二维空间中的采样模式，即在空间中采样。图中共有4个采样点，左下角的正方形区域中包含一个采样点，因此我们可以估计这个区域的面积为0.25。事实上，它的面积为0.3*0.3=0.09。因此，对于这个区域来说，差异值为0.25-0.09=0.16。然而，我们更关心的不是某个特定的区域具有的差异值，而是所有区域中最大的那个差异值。这样就引出了“差异（discrepancy）”的真正定义。在n维空间中，我们定义一个形状（shape）集合B，为了方便，我们假设B中的所有元素都有一个顶点在原点，即其中，。给定一个采样点序列，P关于B的差异定义如下：其中，是b中的采样点数量，是b的实际体积。显然，是根据采样点序列P对b的体积进行估计得到的值。所以，差异就是利用每个中采样点数量估计b实际体积的最大误差。这里的差异比较特殊，因为B中的每个形状都有一个顶点在原点，这样计算出的差异叫做“星差异（star discrepancy）”，表示为。再举一个简单的例子。在一维空间中有一个点集：，则该点集的星差异为。比如说，取区间，则，而，利用这个区间的采样点数量估计出的区域体积（长度）与区域实际体积（长度）之间具有最大差异值。关于低差异采样的内容就介绍这么多，有兴趣的同学可以找到那本书：《Physically Based Rendering: From Theory to Implementation》，仔细品味其中真谛吧！; 个人分类: 运动规划|2600 次阅读|0 个评论

读书笔记——RESAMPL，一种区域敏感的自适应规划方法: 热度 1 wycpp 2014-1-1 00:46; 本文作为论文RESAMPL: A Region-Sensitive Adaptive Motion Planner的阅读笔记。这篇论文发表在08年的Algorithm Foundation of Robotics VII上，可见此论文含金量较足。和高斯采样、桥测试一样，它要解决的是位姿空间中存在窄道等困难区域时的路径规划问题。方法的新颖之处如同题目中提到的，这个方法是“区域敏感”的。那么，RESAMPLE到底是个什么方法？ RESAMPL的大体思路是：首先在位姿空间中随机生成一些采样点，不管是自由还是碰撞，统统留下；然后根据这些采样点建立若干区域（region），下一步就是研究各个区域的性质，看看它们到底是“困难”还是“非困难”。直观上，困难区域碰撞点比较多，非困难区域的自由点比较多。将区域分类的直接用途就是指导后续的采样过程，使困难区域多采样，非困难区域少采样。研究完区域自身的信息，我们再来挖掘区域之间的关系。这里用到的数据结构是图，把每个区域看作图中的一个点，如果两个区域有重叠，则在这两个区域对应的两点之间建立一条边，如此可得到一个区域图（region graph）。区域图有什么用呢？显然，它比普通roadmap的“视野”更开阔，因而可以从宏观上指导全局路径规划。RESAMPL的一个特点就是它既适用于多查询（multi-query）问题，又适用于单查询（single-query）问题，区域图在其中扮演了一个重要角色。下图展示了区域模型的建立、分类和指导后续采样的效果： OK，是时候详细了解RESAMPL了！首先，我们要了解所谓的“区域”是如何建立的。上文中提到过，我们必须先在位姿空间中生成一些采样点。从这些初始点集中随机选取一个采样点，作为区域的中心，称为该区域的“代表采样点（representative sample）”，再从初始点集中找到距离该中心最近的k个点，称为该区域的“邻近采样点（neighboring sample）”。一个代表采样点和k个邻近采样点就确定了一个区域。需要注意的是，每次从初始点集中选择代表采样点时，该点必须尚未被任何区域包含，而邻近采样点则无此限制。这里的“区域”是一个比较特殊的概念，没有一个明确的半径，完全是由若干点确定的。但是，通过以上的方法确定的所有区域半径都会比较接近。下面给出建立区域的具体算法：区域已经建好了，下一步就要为区域“分类”，即分析区域的困难程度。论文中提出了“熵（entropy）”的概念，用于衡量区域中自由点和碰撞点的比例。其实我觉得这个概念纯粹是为了博人眼球的，因为“熵”这个词要比“比例”听上去高端得多！这里稍稍科普一下，熵最初用于衡量一个热力学系统的无序程度，1948年，香农把熵的概念引入信息论领域，用于测量不确定性。“但是在信息世界，熵越高，则能传输越多的信息，熵越低，则意味着传输的信息越少”，维基百科如是说。这篇论文并没有明确给出熵的定义，只是给出一个笼统的判断熵高低的规则：如果一个区域的所有采样点都是自由的或者都是碰撞的，我们就认为这个区域具有低熵（low entropy）；如果一个区域中自由点又有碰撞点，则这个区域具有高熵（high entropy）。根据熵的高低，我们把区域分成4类：自由（free），表面（surface），狭窄（narrow）和阻塞（blocked）。其中，自由区域和阻塞区域都是低熵的，表面区域和狭窄区域是高熵的。下图分别表示了区域的建立以及4种不同的区域：现在我们就来看看RESAMPL是怎样根据熵来对区域进行分类的。对于一个区域，RESAMPL采用多次迭代来计评估它的熵，然后尝试对其进行分类。如果暂时无法分类，就在这个区域中增加一些采样点，并在下一次迭代中继续尝试分类。自由区域是最容易区分的，我们计算出该区域中碰撞点所占的比例，或者称之为熵。如果熵足够低，那么这个区域就被认为是自由的。经验表明，其他类型的区域通常不会被这种方法误判为自由区域。如果初始的粗略的采样可以使一个区域中绝大多数的点都是自由的，那么后续的更好的采样依然可以使这个区域中的绝大多数点是自由的。因此，在每次迭代中，我们首先判断该区域是否为自由区域。阻塞区域也可以用一种类似的方法判断，即如果该区域中自由点所占的比例，即熵，足够低，则该区域为阻塞区域。然而，仅凭一次迭代往往不足以说明一个区域是阻塞的，这是因为随着采样点的增加，当前的低熵区域可能会是一个潜在的高熵区域。所以，只有多次尝试对该区域进行分类并增加采样点后，才能判断一个区域是否为阻塞区域。如果一个区域的子区域具有低熵，则这个区域是表面区域。子区域可以这样定义：设为父区域中所有自由点的质心，为父区域中所有碰撞点的质心，将和作为两个子区域的中心，对于父区域中的每一个点，考察它到两个子区域中心的距离，并将它分配到较近的中心所确定的子区域中。两个子区域确定之后，如果它们都是低熵的，该父区域就被分类为表面区域。如果一个区域是高熵的，且无法被划分为两个低熵的子区域，则这个区域是狭窄区域。同阻塞区域一样，狭窄区域容易被误判，因此我们多次尝试对该区域进行分类并增加采样点后，才能判断该区域是否为狭窄区域。如果一个区域无法被上述方法判别，就认为它是表面区域。经验表明，对于无法判别的区域而言，表面区域是最好的归类选择。对一个区域进行分类的算法如下所示：区域也建立了，分类也成功了，下面就要进行后续的采样和规划了。我们先来看一个概念，即上文中提到的区域图。令每个区域对应一个点，重叠的区域之间有一条边，就构成了区域图。根据连接的区域类型，为每条边赋予权值。利用区域图，我们可以获取区域路径（region paths）来辅助解决单查询规划问题，也可以通过改进区域图，辅助解决多查询规划问题。那么，区域图应该怎样改进呢？一种办法是把相邻的两个类型相同的区域合并，或者把分类情况不明晰的区域拆分。如果两个类型相同的区域合并之后得到的父区域与两个子区域的类型相同，则这两个区域是可合并的。区域合并（region merging）可以减少区域数量，并得到更大的与子区域具有相同类型的区域，这对基于区域类型进行采样点增加或删除是很重要的。对于多查询规划，我们希望利用一个能够充分反映位姿空间连通性的roadmap解决多种查询。上述根据区域图进行区域合并的办法可以有效地提高后续采样的效率，在困难区域生成更多的采样点，得到更能反映自由空间连通性的roadmap。同时，在后续采样时，我们可以对不同类型区域内的采样点进行“筛选”：以一个较低的概率保留自由区域中的点，以一个较高的概率保留表面区域中的点，同样以一个较高的概率保留狭窄区域中的点。对于阻塞区域中的采样点，我们不予保留。这样的策略能够使roadmap变得“小而精”。同时，通过区域分类，我们知道了哪些区域是困难区域，就可以利用一些特定的方法来增强这些困难区域，例如利用RRT快速扩展的特性，使之在困难区域迅速生长，从而达到增强困难区域采样密度的目的。对于单查询规划，我们不必完全掌握位姿空间的拓扑结构，而是本着“够用就好”的态度，希望尽量少探索与本次规划路径无关的区域，以提高规划效率。这时，区域图就派上用场了。我们可以先在区域图中获取一条连接起始区域和目标区域的区域路径，用以宏观指导实际路径的规划。现在的问题是，怎样获取区域路径呢？首先，我们要确定起始区域和目标区域：起始位姿和目标位姿能够连接到的最近非阻塞（自由，表面，狭窄）区域分别作为起始区域和目标区域。然后，根据区域图，在起始区域和目标区域之间找到一条最短区域路径。由于区域图的边具有不同权值，使最短区域路径能够尽可能地避开阻塞区域，而最终出现在路径中的阻塞区域也可以采用重新分类的办法将它们变成连续的非阻塞区域。既然已经有了区域路径，现在考虑如何从区域路径中提取实际路径（actual path）。在区域图中，如果一条边连接的是两个自由区域，那么实际路径是容易提取的。然而，如果一条边连接的是两个困难（表面，狭窄，阻塞）区域，提取实际路径就有难度了。论文中给出了一种方法，用于提高从区域路径中提取实际路径的能力：设区域路径上两个连续的区域为和，如果它们都是自由区域，则不进行特殊处理；否则，在提取实际路径的时候，把区域路径上与和相邻的非阻塞区域也包含进来，这样就增加了自由点的数量，使得提取实际路径的成功率更高。那么，实际路径到底是怎样提取的呢？很简单，对两个或更多区域中的每个采样点，找到距它的最近的k个点并用局部规划器连接即可。可见，区域路径起到了一个宏观指导规划的作用。 OK，就这样，一个人深夜在实验室，完成了2014年的第一篇博客！加油，光明就在不远处！; 个人分类: 运动规划|4388 次阅读|3 个评论

读书笔记——OBPRM，一种基于障碍物的PRM: wycpp 2012-12-31 16:27; 本文作为论文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 $ ）。机器人首先从起始位姿$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 )$。这篇文章的实验部分讲了很多内容，讨论了如何为上述的几个算法选取参数和规划器。对于这种经验性的内容，我就不展开介绍了，有兴趣可以自己看。一旦将基本算法掌握，相信阅读实验部分并不困难。本文就此歇笔。; 个人分类: 运动规划|6175 次阅读|0 个评论

读书笔记——MLDP，在PRM中解决窄道采样问题: wycpp 2012-11-23 21:20; 本文作为论文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 $和每个顶点可以移动的最大距离$\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{p\tau_{p}}{\left \| p\tau_{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的方法我感觉还是不错的。; 个人分类: 运动规划|5471 次阅读|0 个评论

读书笔记——SSRP，一种解决PRM中窄道问题的方法: wycpp 2012-11-19 17:18; 本文作为论文Finding Narrow Passages with Probabilistic Roadmaps: The Small-Step Retraction Method的阅读笔记。我从这篇文章中得到的第一个有趣信息是关于解决窄道问题方法的分类。作者把这些方法分成两大类：过滤（filtering）和收回（retraction），并给出了解释：过滤策略在C-Space中过采样，然后通过估计每一个无碰撞位姿位于窄道中的可能性来确定该位姿成为里程碑（milestone）的概率。典型例子就是高斯采样策略和桥测试策略。收回策略将一些碰撞位姿移出碰撞区域，利用它们引导生成里程碑。典型例子有不少，本文就算其中一个。本文提出的新方法叫小步收回（small-step retraction）。为什么叫“小步”呢？因为它只将那些几乎没有碰撞的位姿收回到自由空间中。什么又是“几乎”没有碰撞呢？“收回”又是怎样的一种操作呢？莫急，咱们先看看小步收回的主要步骤：预先计算机器人或障碍物的“瘦化（thinned）”几何模型，然后利用这些瘦化模型建立一张地图，最后通过将碰撞空间中的一部分位姿收回，从而使它们脱离碰撞。瘦化，顾名思义，就是将原来的几何模型收缩，使当前的几何模型包含于原来的几何模型。为什么要瘦化呢？注意，本文的题目已经说明了它的主要内容是针对窄道问题的，试想，如果将障碍物和机器人都进行瘦化，窄道当然会变宽。这就是本文的中心思想！收回，是一种修复操作，用于将不在路径上不在自由空间内的部分拉回自由空间。为什么要收回呢？理由很简单，因为先前构建地图时使用的是“减肥”之后的障碍物和机器人，不是它们的本来面目，因而在这种地图上找到的路径往往无法直接使用，这时，收回路径上的某些部分就起到了修复路径的作用。设原始的自由空间为F，瘦化障碍物和机器人之后得到的自由空间为$F^{*}$ ，称作膨胀的（fattened）自由空间，$F^{*}$ 是F的超集。F中的窄道在$F^{*}$中变宽，从而使得在$F^{*}$中连通一幅地图更容易。但是，有些通道（passage）原本在F中并不存在，由于自由空间的膨胀，这些通道出现在$F^{*}$中，我们称之为假通道（false passage）。与之对应，如果有些通道原本就在F中，$F^{*}$中这些通道应该变得更宽，我们称之为真通道（true passage）。假通道是无法修复的，试想，如果一个通道原本并不存在，那么，不管怎样修复，在F中通道总是不存在的，所有的修复都将是徒劳；而真通道是可以修复的，因为真通道在F中原本就是存在的，瘦化机器人和障碍物后，真通道会变宽，规划路径会更方便，然后修复在$F^{*}$ 得到的路径，使之能够适用于F。现在你理解什么叫做“几乎”没有碰撞了吗？是的，那些只穿过真通道的路径就是几乎没有碰撞的路径，因为就算这些路径没有完全位于F中，它也不会差到哪里去，毕竟机器人和障碍物的瘦化程度非常小，路径如果有碰撞，也只是刺入障碍物一点点而已。如果你了解高斯采样和桥测试，那么你一定已经知道了何为过滤策略。但是，收缩策略好像稍微晦涩一些。没关系，咱们先看几个实验现象。第一个实验场景如下图所示：其中，机器人R是一个半径为r的小圆盘，s和g分别是起始点和目标点，锯齿形窄道宽度为w。上图表示的是工作空间，而它的位姿空间和工作空间形状非常相似，不同之处在于位姿空间中的窄道宽度应为w-2r，因为位姿空间中的机器人只是一个点，障碍物的宽度则应该加上圆盘机器人的半径r。为了考察规划时间与窄道宽度之间的关系，作者定义了一个收缩因子（shrinking factor）：$\alpha= 2r/ \left (1-\varepsilon \right )w$，其中，$\varepsilon=0.005$，w为常数。显然，收缩因子与r的大小成正比。如果将r逐渐减小，C-Space中窄道的宽度w-2r应该不断增加，窄道变宽了，规划时间自然应该减小。于是我们分析，规划时间随着$\alpha$的减小不断减小。实验结果印证了我们分析的正确性：其中，左图给出了不同收缩因子对应的规划时间。作者使用SBL作为规划器，对于每一个r，SBL用不同的种子运行100次，每次直至找到路径后才停止，然后求出平均规划时间。当$\alpha=1$时，对应的$r= \left (1- \varepsilon \right )w/2$为最大半径，这时的窄道宽度$0.005 \times w$ 为最小。当$\alpha=0.3$时，窄道宽度$0.7 \times w$为最大。右图是左图的函数图。这个实验结果正常人都能想得到，而且分析起来也相当简单，无非就是增加窄道宽度能够使落入其中的采样点数目增加。而这恰恰是本文SSRP方法的核心思想！第二个实验的场景如下图所示：其中，机器人R依然是半径为r的圆盘，两条宽度分别为$w_{1}$ 和$w_{2}$ 的窄道分别位于工作空间的上方和下方，$s_{i},g_{i},i=1,2,3$分别是三组不同的起始点和目标点。$s_{3},g_{3}$距离上下两个窄道的距离相同，$s_{1},g_{1}$则是距离上方窄道最近的一对点。现在，将SBL作为规划器，运行100次，分别记录穿过上方窄道和下方窄道的路径数，得到如下结果：可以看出，如果在$s_{1},g_{1}$之间找到100条路径的话，有88条穿过上方的窄道，有12条穿过下方的窄道——相差悬殊；如果在$s_{3},g_{3}$之间找到100条路径，有52条穿过上方的窄道，48条穿过下方的窄道——数量相当！隐隐约约能够感觉到SBL好像更倾向于穿过离起始点和目标点比较近的通道。现在使用相同的场景再来做一个实验，但是这次只考虑$s_{3},g_{3}$这么一对点。设$w_{2}=\beta \times w_{1}$，其中$\beta \geq 1$。尝试一系列递增的$\beta$，对每一个$\beta$都运行100次SBL，考察随着$\beta$的变化，穿过上方窄道和下方窄道的路径数目的变化情况，得到如下结果：随着$\beta$的不断增大，$w_{2}$不断增大，对于距离上方和下方两条窄道距离相同的$s_{3},g_{3}$来说，更加宽阔的下方窄道无疑更受欢迎。从上图可以看出，当$\beta=1.5$时，所有的路径都从下方窄道中穿过，上方窄道被完全“冷落”。第二个实验场景的两个实验说明，如果空间中存在多条窄道，SBL倾向于穿过最容易的一条。现在想想，这些窄道有真有假，那么，SBL找到的路径更倾向于穿过真通道还是假通道呢？答案是真通道。在$F^{*}$中，真通道的宽度往往大于假通道。原因很简单，一个原本就存在于F中的通道，一个是原本不存在于F中的通道，当机器人和障碍物经过相同的瘦化后，原本就有的通道应该更宽一些——因为真通道有基础，“起点”比假通道高！因此，$F^{*}$中的真通道比假通道更容易穿过。我们暂时不管障碍物和机器人是怎样被瘦化的，权且认为它们已经“减肥”成功。现在来看作者提出的SSRP算法。这个算法又包含了两个算法：乐观算法（Optimist）和悲观算法（Pessimist）。它们都以SBL为基础，在执行过程中都会调用SBL算法。乐观算法执行速度快，被SSRP首先调用，但是不可靠；悲观算法执行速度较慢，但是更可靠，被放在乐观算法之后调用。令F表示自由空间，$F^{*}$表示膨胀的自由空间，$F \subset F^{*}$ ，$\partial^{*} F = F^{*} \setminus F$称为F的膨胀边界（fattened boundary）。我们先来看看乐观算法：其中，M表示原始机器人和障碍物的几何模型，$M^{*}$ 表示瘦化之后的模型。乐观算法调用SBL，在$F^{*}$中找一条路径$\tau^{*}$。显然，乐观算法认为$\tau^{*}$穿过的都是真通道，并尝试修复$\tau^{*}$。这也是乐观算法名字的来历：认为SBL找到的路径都穿过真通道，等SBL找到一条完整路径后再尝试修复这条路径。修复$\tau^{*}$就是将路径$\tau^{*}$上位于F的膨胀边界$\partial^{*}F$中的部分移回F中。首先修复点，然后修复边。怎样修复点？看下面的算法： Repair-Conf用于修复路径$\tau^{*}$上位于$\partial^{*}F$ 内的每个里程碑c。该函数在中心为c、半径为$\rho$ 的球$B \left (c, \rho \right )$ 中采样，试图为c寻找一个合适的替代点$c^{'}$ 。$\eta 1$ ，半径$\rho$起初较小，然后逐渐增大。如果Repair-Conf在迭代K次之后返回失败，我们就认为路径$\tau^{*}$是无法修复的，即，任何一个点修复失败，乐观算法都会失败。如果乐观算法成功，就用返回的$c^{'}$ 代替里程碑c。路径$\tau^{*}$上的所有里程碑都被修复后，就该修复路径上的每一条与$\partial^{*}F$ 相交的边e了。修复边无非就是修复该边上的若干个点，所以，修复边依然要调用Repair-Conf，但是要将Repair-Conf(c, M)中的c赋值为边e的中点。同样，如果任何一条边修复失败，乐观算法都会失败。由于$F^{*}$中可修复的路径都靠近F的边界，那么，半径$\rho$不用太大应该就能找到合适的$c^{'}$ ，所以我们将K值设得较小，论文中取K=100。乐观算法认为路径总是可以被修复的，但是，现实往往与之相反，即$SBL \left (s, g, M^{*} \right )$找到的路径穿过了假通道。下图示意了在2维C-Space中，路径穿过假通道究竟是怎样的一幅情景：白色区域是自由空间F，浅灰色和深灰色区域是碰撞空间（collision space），其中浅灰色区域为F的膨胀边界$\partial^{*}F$。s是起始点，g是目标点，s和g的直线相距很近，却被中间的障碍物隔开。SBL要在s和g之间寻找一条路径。根据前面提到的实验现象，SBL倾向于找到一条最容易的路径。从图上可以看出，在$F^{*}$中，从s到g有两条路可走：一条是从s出发上行，穿过上方的窄道，再下行至g；另一条是从s出发，直接右行，穿过$\partial^{*}F$到达g。如果按照之前提到的实验结果进行推测，SBL显然更倾向于走第二条路。然而，第二条路径要穿过的窄道就是之前提到的假通道，而假通道是无法修复的。因此，如果只依靠乐观算法，SBL可能无法找到F中的可用路径。这时，作者提出了悲观算法。乐观算法是在SBL找到一条完整路径后再对此路径进行修复，而悲观算法则表现得相当谨慎：一旦发现采样点位于$\partial^{*}F$，立刻将其修复。这只需稍稍修改SBL算法即可做到，即每当采样一个新的位姿c，都按以下步骤执行： SBL的一大特点就是延迟碰撞检测，即直到找到一条完整路径，SBL才对路径上的每一条边进行碰撞检测。悲观算法不改变SBL的这一特点，它不会在立即修复点之后紧接着立即修复边。事实上，它根本就不修复边，也不修复任何一条穿过假通道的路径。对于每个在F以外的采样点c而言，悲观算法都对其进行了两次碰撞检测：一次出现在悲观算法的步骤1，一次出现在悲观算法的步骤2，更精确的位置是Repair-Conf的步骤2.2。如果发现c位于$\partial^{*}F$中，则将其修复。然而，就算c被修复，它也不一定出现在最终的路径上，因此，悲观算法用于每个采样点的平均时间比普通SBL和乐观算法都多。不过，这恰恰保证了悲观算法的可靠性：它避免了乐观算法中产生的致命错误，使每一个采样点被及时收回到自由空间F中；同时，由于产生了较好的分布，使得它构建的地图规模比普通SBL小。作者力图实现乐观算法和悲观算法的优势互补，于是设计出small-step retraction planner即SSRP算法，如下所示：乐观算法速度快，但是生成的路径可靠性低；悲观算法略慢，但是可靠性高。注意，悲观算法速度虽不及乐观算法，但是依旧比普通的SBL快很多。如果乐观算法执行了N次都无法找到可用路径，则调用悲观算法。经过实验，发现如果乐观算法连续失败几次，那么它很可能会一直失败下去，所以可以将N的值设小一些，论文中取N=5。由于乐观算法速度比悲观算法快很多，所以就算乐观算法N次全部失败，对SSRP总的规划时间也没有太大影响。至此，SSRP算法就介绍完了。现在回过头来看看论文如何瘦化障碍物和机器人模型。为了读懂这部分，我花了较多的时间，令人沮丧的是，依然没有完全搞懂。论文中提到这个瘦化方法来源于论文Near Parallel Computation of MAT of 3D Polyhedra，但是我google了很久都没找到这篇文章，因而无法深入了解。这篇博客旨在让大家了解SSRP的乐观算法、悲观算法和两者的优势互补，因而不打算把瘦化模型这种与图形学和计算几何联系密切的理论讲得十分透彻。下面来分享一点我本人对瘦化过程的理解。相信大家已经知道什么是中轴（medial axis，MA）了。论文中采用的瘦化方法就利用了这一概念。首先，每一个物体都被建模为多面体，则任一个物体X的中轴MA可由它的子中轴（sub-MA）组合而成。每个子中轴都由一个控制边界元素（governing boundary element）（包括面，边和顶点）提供。每个子中轴都是由图形硬件独立计算得出，与其他子中轴无关。论文中的瘦化方法只用到了由面提供的子中轴，所以下面描述由物体X的各个面控制的子中轴的建立和组合。先看一幅建立子中轴的示意图：图a给出了一个长方体X，并且以该长方体底面e为控制元素。现在考虑X的其他5个面，它们部分或全部位于e的支撑面（supporting plane）的内侧（inward side）。这里我要声明，我并不知道支撑面的确切定义，也没找到能够解释它的资料，不过，如果将X想象为放在一张桌子上，那么e的支撑面应该就是桌子平面。X的其他5个面都位于桌面以上，于是就说成是位于支撑面的内侧。设其他5个面的任意一个为$e^{'}$，则每个$e^{'}$和e之间都有一个平分面，该面上的所有点到$e^{'}$和e的距离相等。例如，在图a中，设$e^{'}$为顶面，则$e^{'}$和e的平分面就是底面和顶面之间的中位面，该面上的所有点到底面e和顶面$e^{'}$之间的距离相等。再比如，设$e^{'}$为底面右侧的直立侧面，则$e^{'}$和e的平分面就是底面和侧面之间的角平分面。在图形缓存中用不同的颜色渲染这些平分面即可得到可见部分（visible portions），如图b所示。现在沿着e的法线观察e的所有平分面，则可见部分就构成了由e控制的子中轴，如图c所示，这时，缓存中的内容就变成了子中轴。一旦将X的所有面控制的子中轴全部建好，就要将它们组合在一起形成X的中轴MA。图c所示的棱台是由X底面e控制的子中轴，根据对称性，X的顶面也有类似形状的子中轴，只不过是一个倒过来的棱台。这两个棱台有一个公共面，即图c所示的红色面。更一般地，考虑两个子中轴，它们分别由e和$e^{'}$控制，则这两个子中轴必有一个公共面，这个面就是e和$e^{'}$的平分面。现在，将X中每一对e和$e^{'}$都按照它们的公共面对齐，就得到X的MA。有了MA，现在来考虑怎样瘦化模型。先来介绍一个名词：MA球（MA ball）。你应该已经知道（如果不知道，去看我的另一篇介绍MAPRM的博客），3D物体X的MA其实是一些包含于X内部的局部最大球（locally maximal ball）的中心的轨迹。MA上的点连同该点对应的局部最大球的半径定义了中轴转换（medial-axis transform，MAT）。而这些局部最大球就叫做MA球。为了瘦化物体X，我们需要减小MA球的半径，然后建立由这些缩小后的球重建物体模型。采用不同的减小半径方法会得到不同的结果。例如，我们可以将MA球半径乘以一个常数$\beta 1$，也可以将所有MA球的半径减去一个常数$\delta$，并删除所有半径小于$\delta$的MA球。下图的a和b分别示意了这两种方法：在这篇论文中，作者采用了第二种方法，因为它得出的结果更容易计算。设$\delta = \alpha \times r_{max}$，其中$r_{max}$是最大MA球的半径，常数$\alpha 1$称为瘦化因子（thinning factor），在该论文中，$\alpha \approx 0.2$。对于由面e控制的子中轴，令P表示将e的支撑面向内侧平移$\delta$得到的平面。我们将子中轴位于P内侧的部分投影到P上。X的瘦化模型的边界就由它的所有面控制的所有子中轴的投影面构成。下面看一个2D的示意图，其中X是一个用虚线画出的矩形，e是它的底边。e控制的子中轴包括三条边，它们被全部或部分地投影到直线P上：注意，上图中由e控制的子中轴投影到P后好像产生了两个间隙，即子中轴投影后变成了三条共面线段，如果推广到3D情况下，则是三个共面面。这是由投影技术导致的。因此，大部分的物体在瘦化以后都会比原模型多许多多边形面。不过，这对于使用包围体分层方法（bounding-volume hierarchy approach）的碰撞检测器的执行时间影响不大。来看一下瘦化之后的物体模型：最后一点需要注意的是，这篇论文将物体的瘦化过程看作预计算（pre-computation）过程。这意味着瘦化过程的时间不计入规划的总时间。作者给出了几个理由：首先，不论物体的位置和方向如何，对于每个物体而言，瘦化过程只需执行一次，不需要为任何一个新的规划问题重复执行；不仅如此，物体模型早在被用于运动规划之前很长时间就已经可用，这为预计算留下了很多时间；最后，生成一个物体的几何模型要比计算它的MA多花很多时间，因此将瘦化作为预计算不会带来很大影响。事实上，很多计算都被作为预计算。例如，很多规划器都假设几何模型使用多边形网格（polygonal meshes）来描述物体表面。如果几何模型不是以这种形式出现的，就需要从给出的描述形式中提取多边形网格，这一过程就常常作为预计算。与此类似，如果一个规划器使用了BVH碰撞检测器，那么计算包围体层次就被当作预计算，因为对于每个物体它只需进行一次。不过，增加预计算也有弊端。在某些机器人应用中，物体模型事先是不可用的，而且感应-建模-规划-执行（sensing-modeling-planning-execution）序列必须尽快执行，增加任何计算都会降低机器人的反应能力。即使时间不是关键因素，我们依然必须存储预计算结果并在随后几何形状改变时更新它们。幸运的是，在大部分情况下，论文使用的规划器能够在充分瘦化机器人的同时保持障碍物不变，这可以省去相当多的更新几何模型带来的开销。实验和总结部分略过。其实我也不想略过，因为它们都是很重要的部分，而且给出了很多算法的实现细节和实验对比结果。不过，我相信当你看懂我写的博客时，再看实验和总结部分应该不会遇到太多困难。这篇“长文”就此完成！; 个人分类: 运动规划|4771 次阅读|0 个评论

读书笔记——MAPRM，在自由空间的中轴上采样: wycpp 2012-11-10 00:01; 本文作为论文MAPRM: A Probabilistic Roadmap Planner with Sampling on the Medial Axis of the Free Space的阅读笔记。首先，做好思想准备：这是一篇需要一点拓扑学知识的文章。如果你对拓扑学有较多的了解，那么这篇文章根本没难度；否则，请先跟随我给自己充一些电，了解一下拓扑学中的几个常见概念。先来了解一个听起来超带感的概念——同伦。在《拓扑学奇趣》这本书中给出了浅显易懂的解释和生动的示例：设h是图形X内的一条道路，从起点$x_{0}$引向终点$x_{1}$。换句话说，$h: \left \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 \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 ,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）。下面就是一连串的结论了：设$x \in \partial P$，则$x \in MA\left (P \right )$当且仅当x是P的一个凸顶点。 P的任意一个多重点都在$MA\left (P \right )$上。若$x \in MA\left (P \right )$，则x位于P的内部当且仅当x是P的一个多重点。对$\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为垂足的边界法线上。映射$P \setminus MA \left (P \right ) \rightarrow \partial P$将每个点映射到它的最近边界点。该映射是连续的。 $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 $ ，使得对$\forall y \in T,\forall t \in \left $ ，有$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而言，它还是使用了太多的复杂的几何计算，更难实现。实验和总结略过。这篇博客终于到此结束了！; 个人分类: 运动规划|5026 次阅读|0 个评论

读书笔记——桥测试，一种解决PRM中窄道问题的好方法: wycpp 2012-11-2 10:54; 本文作为论文The Bridge Test for Sampling Narrow Passages with Probabilistic Roadmap Planners的阅读笔记。窄道（narrow passage）问题在运动规划领域算是臭名昭著了。由于窄道的体积（volume）总是很小，导致一些基于体积的采样分布，例如经典PRM中的均匀分布，难以在窄道中采样。而且，在处理多自由度机器人时，我们无法精确描述C-Space，更无法通过处理C-Space的几何信息来找到窄道。于是，新加坡国立大学的David Hsu等人就提出了桥测试算法，在仅对局部几何形状进行简单测试的情况下，增强窄道内的采样点密度。David Hsu在斯坦福大学拿到了Ph.D.，导师是运动规划领域大名鼎鼎的Jean-Claude Latombe。要说桥测试（bridge test）好，必先给出其他方法“差”在哪里。对于V.Boor、M.H. Overmars和F. van der Stappen在论文The Gaussian Sampling Strategy for Probabilistic Roadmap Planners中给出的高斯采样策略，Hsu认为，虽然窄道内的位姿（configuraions）会靠近障碍物，但是很多靠近障碍物的位姿并不在窄道内——即位姿靠近障碍物是位姿在窄道内的必要不充分条件。由于高斯采样得到的位姿几乎都在障碍物附近，必然会浪费很多采样点，因为它们根本不在窄道内。对于作者本人在论文On Finding Narrow Passages with Probabilistic Roadmap Planners中提出的膨胀自由空间策略和S.A. Wilmarth等人在论文A Probabilistic Roadmap Planner with Sampling on The Medial Axis Of The Free Space中提出的收缩至C-Space中轴的策略，Hsu认为，它们需要复杂的几何操作，而在高维C-Space中，这是很困难的。作者的目标是，通过采样少量的、位置较好的里程碑（milestones）来建立一幅好地图。为了在窄道内产生里程碑，花费在每一个里程碑上的时间比像均匀采样这样的简单采样策略要多一些，但是由此带来的地图规模的减小可以省去很多对里程碑之间的直线路径进行碰撞检测的时间。作者的实验表明，这种权衡是值得的。不过，作者在确定采样分布时，并没有单一使用桥测试产生的分布$\pi_{B}$。因为桥测试把大部分的采样点都分布在了窄道内，导致大片空旷区域没有采样点，这显然有些“矫枉过正”了。要想获得尽可能好的F的连通性信息，单靠$\pi_{B}$显然是不行的。于是，作者考虑将桥测试分布$\pi_{B}$ 和均匀分布$\pi_{U}$ 相结合，使二者的“强项”自然结合，把它们的加权混合作为最终采样分布策略。桥测试基于一个如下的现象：一个n维C-Space内的窄道，至少存在一个方向v，使得机器人在此方向上的活动是极其受限的。机器人沿方向v进行一个小小的扰动都可能使其与障碍物发生碰撞。若机器人不想发生碰撞，它只能沿着与受限方向v垂直的方向运动。因此，对于一个位于窄道内的无碰撞点p，很容易找一条穿过p的线段s，使得s的两个端点都位于C障碍物上。s被很形象地称作“桥（bridge）”，它的两个端点就是桥墩（piers）。如果能够建立这样一座桥，我们就说p通过了桥测试。显然，在窄道内建桥要比在空旷区域（wide-open free space）建桥容易得多，这导致窄道内的采样点陡然增多，下图比较了窄道和开阔区域内的建桥难度：下面给出随机建桥算法（Randomized Bridge Builder, RBB）： RBB只使用了CLEARANCE一种几何操作，用于进行碰撞检测，与那些需要对C-Space进行大量几何计算的方法相比无疑快了很多。$\lambda_{x}$是中心位于x、标准差$\sigma$ 较小的高斯分布，并且，C-Space在每一维上标准差都等于$\sigma$ 。现在一定有人想要看看由RBB产生的概率密度$\pi_{B}$到底是个什么样子。为了计算$\pi_{B}$，设$X,X^{'}$是两个随机变量，分别表示桥的两个端点。根据RBB算法，第一个端点$X$在C障碍物空间$B=C \setminus F$中服从均匀分布，因此，$f_{X} \left (x \right ) \neq 0$当且仅当x位于B中。不失一般性，假设B的体积为1，则当$x \in B$时，$f_{X} \left (x \right ) =1$；否则，$f_{X} \left (x \right ) =0$。给定$X=x$，我们根据概率密度$\lambda_{x}$选择另一个端点$X^{'}$。根据RBB算法，$X^{'}$必须位于B中才会被接受。定义函数$I$，对于任意点$p \in C$，若$p\in B$ ，则$I \left (p \right )=1$；否则，$I \left (p \right )=0$。于是，给定$X$，得到$X^{'}$的条件概率密度为： $f_{X^{'} \mid X}\left (x^{'}\mid x \right )=\lambda_{x}\left (x^{'} \right )I\left (x^{'} \right )/Z_{x}$ 其中，$Z_{x}=\int_{C}f_{X^{'}\mid X}\left (x^{'}\mid x \right )f_{X}\left (x \right )\mathrm {d}x$是正规化常数。我们以X作为条件，计算点$p \in F$的概率密度$\pi_{B}$： $\begin{equation} \pi_{B} \left (p \right )=\int_{C}f_{X^{'}\mid X}\left (x^{'}\mid x \right )f_{X}\left (x \right )\mathrm {d}x \tag{1}\end{equation}$ 注意到p是线段$\overline{xx^{'}}$的中点，则$x^{'}=2p-x$，将其代入上式： $\begin{equation}\pi_{B}\left (p \right )=\int_{B}\lambda_{x}\left (2p-x \right )I\left (2p-x \right )/Z_{x}\mathrm{d}x \tag{2}\end{equation}$ $\lambda_{x}$是中心位于x且标准差较小的高斯分布。如果$x^{'}=2p-x$靠近x，则$\lambda_{x}$较大。仅当$I\left (2p-x \right )=1$ ，即$x^{'}\in B$ 时，式(1)中的被积函数不为0。对于一个位于窄道中的点p而言，上述两个条件都易满足，这就导致了点p处的$\pi_{B}$较大。 RBB与高斯采样一样，都只用了CLEARANCE一种几何操作；它们的不同点在于RBB增加了窄道内的采样密度，而高斯采样则增加了障碍物边界处的采样密度。RBB比高斯采样更费时：每次采样，前者都比后者多调用一次CLEARANCE。然而，对于避免在不感兴趣的、且无法改进地图连通性的障碍物边界区域采样，RBB显然做得更好。下图给出了这两种采样策略的效果，左侧为高斯采样，右侧为RBB：上图中也暴露了RBB的一个缺陷，即RBB会在F的一些尖角（sharp cormers）区域进行较多的采样。这很容易理解，因为在尖角区域建桥是一件比较容易的事。不过，作者用实验说明了RBB“功大于过”，这点缺陷实在不算什么。下面看看RBB采样和均匀采样的结合。设F中窄道所占的区域为P，概率密度$\pi_{B}$导致采样偏向于P，没有照顾到空旷区域$F\setminus P$。但是，我们知道，均匀分布$\pi_{U}$虽然对窄道区域无可奈何，却能够很好地照顾到空旷区域。于是，我们自然而然地考虑将二者结合，就有了混合采样分布（a hybrid sampling distribution）： $\begin{equation} \pi = \left (1-w \right )\pi_{B}+w\pi_{U}\tag{3}\end{equation}$ 其中，$0\leq w \leq 1$是权值，它的选取取决于在窄道中采样的难度和覆盖F所需里程碑的数目。由于论文作者假设F中包含了一些窄道，他们将$\pi_{B}$的值调得较大。将自由空间F分成P和$F\setminus P$ 两部分来看，我们通过调整每一部分的采样策略来获得在整个采样空间的最好效果，即我们必须知道两种采样分布如何互补才能在不丢失自身优势的情况下形成完美组合。抛开权值的选择，有一点还是可以取巧的：那些被RBB“抛弃”的点，均匀分布可以拿来复用。实验部分和总结部分略过。; 个人分类: 运动规划|4341 次阅读|0 个评论

读书笔记——PRM的高斯采样策略: wycpp 2012-10-31 17:42; 本文作为论文The Gaussian Sampling Strategy for Probabilistic Roadmap Planners的阅读笔记。这篇论文出自荷兰乌徳勒支大学的研究人员之手，发表在1999年的ICRA上。其中一位作者算是运动规划领域响当当的人物——Mark H. Overmars。这人名字很有意思哈，“火星人”！他主要研究的是计算机图形学、计算几何，由于这些东西与运动规划紧密相关，使得他在运动规划领域也相当游刃有余。这种模式我很欣赏，因为我对图形学也非常感兴趣。闲话少说。这篇论文的主要贡献在于给出了一种能够在困难区域产生较多采样点的策略，而这个策略的灵感竟然来自数字图像处理中的高斯平滑，于是该策略被称为高斯采样（Gaussian sampling）。相对于经典PRM而言，这种采样策略必然造成采样时间的增加——经典PRM是随机采样，这几乎是最简单的采样策略了，而高斯采样已经不是“原生态”的了，因为它经过了有目的的人工干预。然而，下面的分析将得出结论：尽管采样时间增加了，但是采样点更加“精炼”了，使得构建地图的时间不升反降！先来看看经典PRM的地图构建算法：两个最耗时的步骤是第3步和第8步，因为他们需要几何操作。令$T_{s}$表示产生一个无碰撞采样点所需时间（第3步），$T_{a}$表示将其加入地图所需时间（第4-9步），那么上述算法的总时间为$T=n \left (T_{s}+T_{a} \right )$，其中n是采样点总数。在PRM的标准实现中，$T_{s}$远小于$T_{a}$，这提示我们考虑通过更“认真”地采样来提升算法的时间效率，即通过增加$T_{s}$来减少n。有些采样算法是这样工作的：生成一个采样点，检测它是否位于自由空间F，如果是，就将它添加到地图中。注意，这里没有对直线路径进行碰撞检测，因此，一个很好的例子就是Lazy PRM。这种算法的总时间可以表示为$T=n_{t}T_{t}+n_{a}T_{a}$，其中$n_{t}$是尝试过的采样点总数，$n_{a}$是成功加入地图的采样点数目，$T_{t}$是对一个采样点进行碰撞检测的时间，$T_{a}$如前所述，是将一个采样点加入地图所需时间。同样地，$T_{t}$远小于$T_{a}$。道理很简单：对一个点进行碰撞检测远远简单于对很多直线路径进行碰撞检测。这提示我们可以尝试更多的采样点（增加$n_{t}$ ），但是在保留采样点时必须“挑剔”（减少$n_{a}$ ）。这样，虽然在采样的时候花费了更多的时间，但是由于“精选”出的点较少，使得最终的规划时间减少。图像处理中有一个著名的“高斯平滑”，这里就借用了它的思想。首先，在d维C-Space中定义高斯分布：$\phi \left (c;\sigma \right )= \frac {1}{\sqrt{2\pi\sigma^{2}}^{d}}e^{-\frac {c^{2}}{2\sigma^{2}}}$。定义函数$Obs \left (c \right )$，当c有碰撞时，$Obs \left (c \right )=1$；否则，$Obs \left (c \right )=0$。函数$f \left (c,\sigma \right )= \int Obs \left (y \right )\phi \left (c-y;\sigma \right )\mathrm{d}x$使用高斯分布平滑C障碍物。为了避免采样点与障碍物碰撞，定义$g \left (c;\sigma \right )= \mathrm{max} \left (0,f \left (c;\sigma \right )-Obs \left (c \right ) \right )$。这就是我们想要的概率分布。在障碍物内部，$g\left (c;\sigma \right )=0$；否则，$g\left (c;\sigma \right )=f \left (c;\sigma \right )$。 $\sigma$是一个重要参数，它指出了我们想让采样点离障碍物有多近。下面的三幅图示意了利用上述分布在2维C-Space中的采样结果。在每一幅图中，颜色越浅，表示采样点出现于此的概率越高。由图可见，当$\sigma$ 选取适当时，障碍物周围被采样的概率很高。三幅图从左到右，$\sigma$依次减小。是时候给出传说中的高斯采样算法了！称该算法为高斯采样（Gaussian sampler）的原因在于它产生的采样点遵从$g\left (c;\sigma \right )$ 分布。该算法避免了在C-Space中的一些计算，而只使用了一种操作，即采样点的碰撞检测，这样可以提高算法效率。该算法对采样点是“挑剔”的：只有当一个采样点本身无碰撞且周围有障碍物时，该采样点才会被添加到地图中。这恰好反映了先前讨论的策略，即每一个采样点在加入地图之前都应该精挑细选。高斯采样比随机采样慢的原因在于前者测试两个点却最多只能向地图中加入一个点。乍一看，算法的最后一步要舍去两个刚刚辛辛苦苦测试完的采样点，好像有悖常理，但是这一步却是至关重要的，它避免了在大片的空旷区域添加采样点。如前所述，标准差$\sigma$的选择大有讲究。如果我们选择了一个小$\sigma$，则大部分的采样点会很靠近障碍物；如果我们选择了一个大$\sigma$，采样点就几乎均匀分布在自由空间F中了。在本论文中，机器人既能平移（translate）又能旋转（rotate），如果能够选择一个适当的$\sigma$，使得大部分位姿到障碍物的距离最多不超过机器人本身的长度，这样既保能证位姿靠近障碍物，又能允许机器人绕轴旋转。当然，旋转自由度（rotational degrees of freedom）和平移自由度（translational degrees of freedom）本应以不同的方式处理。该算法只是使第二个采样点$c_{2}$的位置（position）遵从高斯分布，而它的方向（orientation）则为任意。不过，实验结果依然不错。实验部分和总结部分在此略过。; 个人分类: 运动规划|6149 次阅读|0 个评论

读书笔记——一种在障碍物周围产生采样点的随机地图方法: wycpp 2012-10-30 23:40; 本文作为论文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的雏形。; 个人分类: 运动规划|6792 次阅读|0 个评论

读书笔记——Lazy PRM: wycpp 2012-10-29 23:26; 本文作为论文Path Planning Using Lazy PRM的阅读笔记。这篇论文的意义是重大的，因为它明确提出了利用延迟碰撞检测的办法来减少不必要的碰撞检测次数从而提高规划效率的Lasy思想。这个思想被后人使用了n多次，而这篇，正是Lazy始祖。首先，这篇文章的算法是为单查询任务设计的（也能用于多查询任务，后面会解释）。单查询规划器有个基本要求：快！为何？试想，多查询规划器，比如说经典PRM，建一幅地图可以反复利用，那么，即使它构建地图速度稍慢，也还是可以容忍的。而单查询规划器，本来就是“一次性”的，地图用完一次即作废，如果速度再那么慢，可谓“两头都不占”，必然遭人鄙视。论文的中心思想就是尽量减少调用局部规划器（local planner）的次数，从而获得较快的规划速度。显然，减少调用局部规划器的次数，说白了就是减少不必要的碰撞检测。首先看一下算法流程图：先看Build initial roadmap。给定两个查询位姿$q_{init},q_{goal}$ 和一个采样点个数$N_{init}$，然后随机均匀采样。采样的同时，会涉及到邻域选取的问题，因为每当一个采样点被加到地图上，它都要立刻和它邻域中的点相连，这样，当$N_{init}$个采样点都被加入地图的时候，一幅真正的、包含点和边的地图也就形成了。为什么当前采样点只和它邻域中的采样点相连呢？第一，如果把当前采样点和已经加到地图中的每个采样点相连，会产生大量的边从而耗费很大内存；第二，如果两个采样点离得较远，它们之间很可能有障碍物，尝试用直线路径相连很可能会失败。上述两点可以认为是“出力不讨好”。明白了邻域的重要性，我们来看怎样选择邻域。论文选择了一种尺度$\rho_{coll}:C \times C \rightarrow [0,+ \infty)$ 来衡量距离。这个距离应该反映出用无碰撞的直线路径连接两个采样点的难度，即距离越大，存在直线路径的概率越小。由于该论文主要解决机械臂问题，而机械臂底部移动一个单位比它的末端移动一个单位更容易产生碰撞。因此，作者采用一种加权欧式距离：$\rho_{coll} \left (x,y \right )= \left (\sum\limits_{i=1}^{d} w_{i}^{2}\left (x_{i}-y_{i} \right )^{2} \right )^{1/2}= \left (\left (\textbf{x-y} \right )^{\textbf{T}}W \left (\textbf{x-y} \right ) \right )^{1/2}$ 。其中d是C-Space的维数，$W=diag \left (w_{1}^{2}, \cdots, w_{d}^{2} \right )$ ，$\textbf{x}^\textbf{T}$ 是$\textbf{x}$ 的转置矩阵。这些权值正比于当机器人在C-Space中沿着相应的坐标轴移动一个单位时，机器人上的点在工作空间（workspace）中移动的最大可能欧式距离。乍一看，Build initial roadmap貌似和经典PRM的地图构建很相似；再仔细一想——不对啊，经典PRM在连接当前采样点和它邻域中的采样点时调用了碰撞检测函数啊，这里怎么没有？That's the point! Let's go on! 其实，这恰恰是Lazy PRM的特色所在——延迟碰撞检测。不仅延迟边的碰撞检测，而且延迟点的碰撞检测——难道你没有发现，在Build initial roadmap中根本就没有提到自由空间F吗？在经典PRM的地图构建步骤中，每采样一个点，必先检查其是否位于自由空间F中，之后又要检查它与邻域中的点是否可用直线路径相连。然而，在Lazy PRM中，这些碰撞检测被延迟了。不是不检测，时候未到而已。这恰好体现了单查询的特点，即此时构建的地图以及基于此地图查找的路径是无法复用的，因为这个地图本身就可能不“合法”，于是这条路径也可能是有碰撞的。然后看Search for a shortest path。这里设计两个问题：1.怎样定义距离长短；2.怎样找到最短路径。对于第一个问题，作者采用了一种尺度$\rho_{path}:C \times C \rightarrow [0,+\infty)$。$\rho_{path}$的计算方法和前面的$\rho_{coll}$相似，也是一种加权距离；不同点在于权值取为$\frac{1}{v_{i}},i=1, \cdots ,d$，$v_{i}$是关节i的最大角速度。之所以这么定义，是因为此论文主要针对机械臂问题，C-Space定义为$I_{1} \times \cdots \times I_{d}$，其中$I_{i}$表示第i个关节的活动范围。权值为角速度的倒数，说明时间越大，权值越大。因此，它的距离长短就被表示为时间长短——走完某条路径花费时间越短，$\rho_{path}$就越短。对于第二个问题，作者采用了$A^{*}$算法。众所周知，该算法找到的路径就是最短路径。如果$A^{*}$ 找到路径了，就对其进行碰撞检测；如果没找到，或者转到Node enhancement，或者返回failure，这取决于规定的时间有没有用完。接着看Check path for collision。终于到了碰撞检测的时候。先检测点，后检测边，因为一旦点有碰撞，就会将点删除，同时也会删除与该点相关连的边，这样使算法执行更快。检测顺序是从路径的两头向中间检测，点和边都是如此（具体原因我还没有搞清楚）。检测边时采用将其离散化的方法，先离散化到一个粗一些的分辨率，若检测无碰撞则再进行细化。这样的好处是可以避免很多不必要的碰撞检测。如果边有碰撞，则删除之，然后重新查找最短路径。为了提高规划效率，算法会记录某点是否已被检测过、边的分辨率已经达到多少，这些信息都是可以复用的。然而，分辨率应该如何确定呢？首先，采样点$q,q^{'}$ 之间边的分辨率肯定和$\rho_{coll} \left (q,q^{'} \right )$正相关（试想，边越长，越有可能发生碰撞，于是一个稍大的分辨率就有可能达到检测目的）；其次，分辨率应该和C-Space的规模反相关（试想，对于同一条边而言，它在规模较大的C-Space中会显得较短，障碍物也会显得更小，因此需要把边划分地比较精细才能准确地检测碰撞，这就是分辨率的相对性）。于是，作者给出分辨率公式：$\delta= \frac {\rho_{coll} \left (q,q^{'} \right )}{M_{coll}}$。其中，$M_{coll}$是C-Space中一条可能的最长直线路径需要进行的碰撞检测数，显然，如果把C-Space想象成一个d维“矩形”，它的可能最长直线路径就是对角线。如果$q,q^{'}$分别位于对角线的两端，那么分辨率为1；否则，分辨率小于1。再看Node enhancement。这涉及到一个问题，即应该把这些增强点放到哪里。论文作者采用了和经典PRM类似的办法，先从图上选择一些待增强点，称作种子（seeds），然后把增强点放在这些种子附近。但是，并不是所有的增强点都被放到了种子附近，而是“对半分”，即如果决定产生$N_{enh}$个增强点，则只把其中的一半放到种子周围，另一半则在C-Space中均匀分布。这一方面是因为我们想要获取更多的关于C-Space的信息，另一方面是因为不想太依赖于种子点的选取。下面就看看怎样选择种子点。论文作者的想法是“变废为宝”，即把一些因为有碰撞而被删除的边的中点作为种子，这些边要满足一个条件：至少有一个点位于F中。这样选取种子，可以保证其相对靠近F边界，而F的边界通常是困难区域，需要增强。现在考虑一种情况：某个待增强点被选做种子，然后会有增强点出现在其周围，与之相伴而来的还有一些连接增强点和其邻域点的边，这些边有的无碰撞，有的有碰撞并且被删除。下次选择待增强点时，又将这些被删除的边的中点利用起来，它们的周围又会出现增强点。照此执行下去，无疑会造成点的群聚（cluster）。为了避免这种情况，算法在选择种子点时，只考虑那些最初随机均匀产生的边，后面因为增强而增加又删除的边不予考虑。种子点选好后，就要讨论一下增强点应该怎样安排了。这里简单给出论文中所使用的方法，不做深入分析。论文使用了多元正态分布的方法。引入两个参数$\alpha \in \left (0,1 \right )$和$\lambda 0$，论文作者展示了他们可以选择一种分布，使得事件$\rho_{coll}\left (q,\eta \right )\leq \lambda R_{neighb}$以概率$1-\alpha$发生。为了达到这个目标，他们定义了一个协方差矩阵：$\Sigma = \frac {\lambda^{2}R_{neighb}^{2}}{\chi_{d}^{2} \left (\alpha \right )}W^{-1}$ 。$\left (\mathbf{q-\eta} \right )^{\mathbf{T}}\Sigma^{-1}\left (\mathbf{q- \eta} \right )$ 是d维$\chi^{2}$ 分布，则事件$\left (\mathbf{q-\eta} \right )^{\mathbf{T}}\Sigma^{-1}\left (\mathbf{q- \eta} \right )\leq \chi_{d}^{2} \left (\alpha \right )$的概率为$1-\alpha$ 。如下图所示：其中，$\eta$是种子，q是增强点，C-Space为2维。如果q的分布遵从$N_{d}\left (\eta,\Sigma \right )$，则q位于置信椭圆（实线）中的概率为$1-\alpha$。虚线椭圆是分布函数的边界。至此，Lazy PRM算法介绍完毕。实验部分和总结部分略过。; 个人分类: 运动规划|6006 次阅读|0 个评论

读书笔记——易扩张C-Space中的路径规划: wycpp 2012-10-27 23:20; 本文作为论文Path Planning in Expansive Configuration Spaces的阅读笔记。首先介绍一下该论文的整体思路：作者其实就是提出了一种单查询算法，而且为其做了理论证明，我甚至合理怀疑单查询算法就是发韧于这篇论文。具体来说，作者为了能够给出一个单查询路径规划方法，先为自由空间（free space）F定义了“易扩张性”概念；然后给出“连接序列”的概念，利用“易扩张性”证明“连接序列”最终能够以极大的概率在任意两个查询位姿之间找到路径；最后根据这个理论提出了一种单查询路径规划算法。所谓完备规划器是指如果存在路径，则其必能找到，若不存在，则其必能指出。由于完备规划器搜寻路径的时间复杂度为指数级，因而人们开始逐渐关注随机规划器。RPP（Randomized Path Planner）是一种随机规划器，它沿着C-Space人工势场的负梯度方向搜索路径，并利用随机行走（random walks）脱离局部最小。但是，如果在脱离局部最小时必须穿过一个狭窄通道，RPP的表现就相当糟糕了，因为随机行走能够穿过狭窄通道的概率相当小。 PRM类规划器同样是随机的。不过它们当中有一些能够达到“概率完备”的水平，即如果存在路径，它几乎必能找到。不过，如果不存在，它很可能就没法终止了。该论文给出了易扩张C-Space的概念，并且证明了如果在易扩张空间中用均匀采样的办法建立地图（roadmap），那么该地图的连通性与自由空间的连通性能够以很高的概率保持一致。也就是说，该论文的方法能够达到概率完备的水平。在此论文提出以前，PRM都是多查询的，在整个C-Space采样，然后建立一张能够反映整个C-Space情况的地图。当然，这个地图是可以重复利用的，如果只用一次，显然太浪费了。而事实上，实际情况中有太多的“一次性”问题，我们叫它们单查询问题。你一定觉得在整个C-Space采样实在是没必要，因为那样会建立一张很大的地图，里面可能包含了很多连通分支，而对于两个查询位姿，即$q_{init}$和$q_{goal}$而言，只可能有一个连通分支将它们都包含在内，其他的连通分支对于此次单查询任务来说，统统没用。于是，论文作者想要设计一种策略，使得它能够只在包含$q_{init}$ 和$q_{goal}$的连通分支上采样：首先在$q_{init}$或$q_{goal}$的临域内采样，然后在已经与$q_{init}$ 或$q_{goal}$建立连接的那些点的临域内采样，如此迭代，直到在$q_{init}$ 和$q_{goal}$找到路径。下面给出几个定义：对于任意$S\subseteq F$，$\mu \left (S \right )$表示S的体积，方便起见，假设$\mu \left (F \right )$=1。如果两个位姿能够用位于F中的直线连接，则称它们相互可见（visible）。$\nu \left (p \right )$表示点p在F中的可见区域，其中$p \in F$ 。定义1：设$\alpha , \beta , \epsilon$是开区间(0, 1)之间的常数，则F是$\left (\alpha, \beta, \epsilon \right )$-易扩张的，如果F的任意一个连通分支$F^{'} \subseteq F$满足如下条件： 1.对于任意一个点$p \in F^{'}$，有$\mu \left (\nu \left (p \right ) \right ) \geq \epsilon$； 2.对于任意一个连通子集$S \subseteq F^{'}$，点集$LOOKOUT \left (S \right )=\left \{ q \in S \mid \mu \left (\nu \left (q \right ) \setminus S \right ) \geq \beta \times \mu \left (F^{'} \setminus S\right )\right \}$的体积满足$\mu \left (LOOKOUT \left (S \right ) \right ) \geq \alpha \times \mu \left (S \right )$。解释一下这个定义。首先你要清楚，作者提出这个概念是为了后面利用它证明单查询方法的可行性。第一个条件很容易理解，即F中的每个点可以看见至少$\epsilon$部分的F。为了解释第二个条件，先看下面一张图：可以发现，LOOKOUT是S的一个子集，这个子集满足一个条件，即其中的点所能看到S以外的部分的体积，占S关于其所在连通分支$F^{'}$的补集的比例不小于$\beta$。那么，第二个条件就是说，这个LOOKOUT的体积占整个S的比例不小于$\alpha$。根据上述解释，如果一个自由空间F有较大的$\alpha$和$\beta$，则通过增加新的采样来扩张可见区域就是一件比较容易的事，因为在F的任一个连通分支$F^{'}$ 的子集S中，有相当一部分（$\beta$ ）的点具有辽阔（$\alpha$ ）的视野。我们将S看作是点集M所产生的可见区域，如果可以很容易地在S中找到一些点并添加到M中能够使M的可见区域，即S本身获得较大扩张，那么S最终会覆盖整个F，从而我们可以获取足够多的关于C-Space的信息来解决路径规划问题。下面的两个图用于说明引入$\alpha , \beta , \epsilon$的初衷。上面两幅图反映了运动规划领域“臭名昭著”的窄道（narrow passage）问题。在窄道问题中，随机采样很难完整反映C-Space的连通性，原因在于找到一些能够连接窄道内的里程碑（milestones）并成功穿过窄道的路径实在太难了。如果对上面两幅图进行随机采样，得到的结果很可能是它们的两个半球被割开了，两个半球位于两个不同的连通分支上。用$\alpha , \beta , \epsilon$对C-Space进行参数化是为了更好地捕捉C-Space的复杂结构，并且利用这些参数表示规划时间，进而通过改变这些参数来分析规划时间的变化。上述两幅图的任一幅都不可能同时具有较大的$\alpha$和较大$\beta$ 。以右边的图为例，设它就是F中的某个连通分支$F^{'}$ ，它的左半球为S。如果想要S中的点在看右半球时有比较开阔的视野，即想要有较大的$\beta$ ，则这些点必须靠近两个半球相连的“开口”处，显然，这样的点仅仅是S中的少数，即$\alpha$较小。反之，如果想要$\alpha$较大，则$\beta$必然较小。不过，上述两幅图也有不同的地方。左图，如果想要有较大的$\beta$，则$\alpha$较小，并且如果$\beta$足够大，$\alpha$一定等于0。从图上看，假设$\beta = \frac{1}{2}$，左半球中显然没有视野这么开阔的点，于是$\alpha = 0$。然而，在右图中，无论$\beta$取多大（当然，不超过1），$\alpha \not\equiv 0$。这是因为在两球的“开口”处，点的视野是足够开阔的，它的$\beta$ 能够无线趋近1。下面给出一个概念，两个引理以及一个定理，用来从数学上证明作者用单查询方法构建的地图（其实只是经典PRM地图中很小的一块）的连通性能够以很高的概率与F的连通性保持一致。定义2：点$p \in F$的连接序列（linking sequence）由一个点序列$p_{0} = p, p_{1}, p_{2}, \cdots $和一个点集序列$V_{0}=\nu \left (p_{0} \right ), V_{1}, V_{2}, \cdots \subseteq F$组成，且对于任意$i \geq 1$，有$p_{i} \in LOOKOUT \left (V_{i-1} \right )$且$V_{i}=V_{i-1} \cup \nu \left (p_{i} \right )$。连接序列示意图如下：引理1：设M是从自由空间F中独立、均匀、随机地选出的具有n个里程碑的点集。令$s=1/ \alpha \epsilon$。给定任意点$p \in M$，M中存在一个长度为t的p的连接序列的概率至少为$1-se^{- \left (n-t-1 \right )/s}$。引理2：设$v_{t}=\mu \left (V_{t} \right )$表示由点$p \in F^{'}$ 的连接序列$p_{0}, p_{1}, p_{2},\cdots $ 确定的第t个点集$V_{t}$的体积，其中$F^{'}$是F的一个连通分支，则对于$t \geq \beta^{-1} \ln 4 \approx 1.39/ \beta$，有$v_{t} \geq 3 \mu \left (F^{'} \right )/4$。设S是一个从F中采样得到的里程碑集合，R是一个地图，它由S以及在S中所有相互可见的两个里程碑之间加入边构成，$M \subseteq S$ 。对于F的任意连通分支$F^{'}$ ，令$M_{j} \subseteq M$ 是$F_{j}$ 中里程碑的集合，$R_{j}$ 是包含了$M_{j}$ 中顶点的R的子图。定理3：设$\gamma \in \left (0,1 \right )$为常数，点集S中的2n个点是从自由空间F中独立、均匀、随机地选出，其中$n=\left \lceil 8\ln \left (8/\epsilon \alpha \gamma \right )/\epsilon \alpha +3/\beta +2\right \rceil$。那么，每个$R_{j}$是连通图的概率至少为$1-\gamma$。这些引理和定理的证明在此略过。简单说一下这些定理的意义。引理1说明，对于任意一个通过在自由空间F中随机采样得到的里程碑集合，它包含一个给定长度t的、位于该里程碑集合中的任一点p的连接序列的概率还是相当大的。引理2说明，这个给定长度为t的连接序列的体积能够扩张很大。对于任意的两个里程碑p和q，它们各自的连接序列不断膨胀的结果就是最后一定会相交。这时，p和q之间就能用一条路径连接起来了。定理3估计了$R_{j}$ 是连通图的概率，而且可以看出n越大，$\gamma$ 就越小，$R_{j}$ 是连通图的概率就越大。其实上面的定义、定理很容易把人搞糊涂，因为一会是F，一会又是F的连通分支$F^{'}$ ，乱七八糟，难以看清定理的真正用途。但是，请别忘了，该论文就是想要提出一种单查询方法，这种方法最终只建立一个连通分支。所以，当你看到“对于每一个F的连通分支$F^{'}$”这样的话时，直接认为$F^{'}$就是F即可！下面终于要将前面的理论用于算法设计的实践中了！如果$q_{init}$ 连接序列的可见区域和$q_{goal}$ 连接序列的可见区域相交，那么一条路径就被找到了。于是，算法的整体思路就有了：给定两个查询位姿$q_{init}$ 和$q_{goal}$ ，在C-Space中采样，但是只保留那些与$q_{init}$ 和$q_{goal}$有路径相通的位姿点。这样，分别以$q_{init}$ 和$q_{goal}$ 为根建立两棵树，两棵树不断生长，直到它们的可见区域相交，这时便能找到一条连接$q_{init}$ 和$q_{goal}$ 的路径。算法分为两个步骤，即expansion和connection。由于以$q_{init}$ 为根，或以$q_{goal}$ 生长过程是一样的，不妨设任一棵树$T= \left (V,E \right )$ 。expansion具体步骤如下：其中，$w \left (x \right )$ 表示权重。由步骤1可见，$w \left (x \right )$ 和选择x的概率成反比。这里，$w \left (x \right )$ 定义为T上位于$N_{d} \left (x \right )$ 内的采样点数目。可见，邻域点稀少的点能够以更高的概率获得在其周围采样的机会，这个算法有“损有余而补不足”之功效，即自适应，这样的好处是树上的点在最终形成的包含$q_{init}$ 和$q_{goal}$ 连通分支上均匀分布。$clearance:C \rightarrow R$ 将一个位姿q映射为该位姿下机器人与障碍物的距离。$link \left (x, y \right )$ 可以确定x和y之间是否存在一条直线路径。定义邻域的距离d的选取十分重要：若d选得太大，则很多采样点会落入与当前查询无关的连通分支中（试想，d选那么大，很可能在q和x之间有障碍物，那么在connection调用link函数时会发现它们无法用直线路径连接，于是q和x只好在不同的连通分支上了），这就犯了和经典PRM一样的毛病；若d选得太小，则大部分采样点分别集中在$q_{init}$ 和$q_{goal}$ 周围，两个查询位姿连接序列的可见区域不易相交，从而不易找到路径。关于K的选取，论文中指出它对算法影响不大，选10个就ok。注意，clearance的实现有两种极端情况，一个是精确计算出机器人和障碍物之间的欧式距离，一个是退化为一个碰撞检测器，只返回YES或NO。该论文选择了前者，原因就在于link函数。link用于检测两个位姿p和q之间是否存在直线路径，它会不停地调用clearance。现假设clearance可以精确计算距离，且p和q的clearance分别为$\zeta$ 和$\eta$ 。定义p和q是相邻的（adjacent），如果$dist_{w} \left (p,q \right )max \left (\zeta,\eta \right )$。如果p和q是相邻的，那么它们之间就存在一条直线路径。给定p和q，link迭代地将p和q之间的直线分割成很多小段，若这些小段的端点都是逐个相邻的，那么p和q之间就存在直线路径，否则就不存在。如果clearance不计算距离而只是充当碰撞检测器，那么link必须不停地分割p和q之间的直线，直到达到给定的分辨率为止，但是这样做往往会调用更多次的clearance（试想，若clearance可以计算距离，那么一旦检测到此时所有小段的端点逐个相邻，分割线段的工作就停止了；如果clearance不能计算距离，只好继续苦逼地分割下去），而且会十分依赖这个给定的分辨率。 connection具体步骤如下： connection算法很容易理解，但是有一点需要注意，即此处的$dist_{w}$和expansion中的$dist_{c}$。前者应该反映出两位姿点越近，相互可见的可能性就越大，于是定义为当机器人沿着p和q之间的直线路径运动时，机器人身上所有点走过的最远距离。后者简单定义为两位姿点p和q在笛卡尔坐标系中的距离。如果两个查询位姿之间不存在路径，那么规划器就会不停地运行下去。为了防止这一点，必须给出终止条件。可以限定expansion和connection的最大执行次数，也可以当两棵树上所有点的最小权值超过一定值后停止，因为这意味着我们的采样已经相当充分了。实验部分和总结部分略过。; 个人分类: 运动规划|4500 次阅读|0 个评论

读书笔记——SBL，一种单查询、双向、延迟碰撞检测的PRM: wycpp 2012-10-25 17:10; 本文作为论文A Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking的阅读笔记。 PRM是一种有效的路径规划方法。但是，它在碰撞检测（collision check）上花费了太多时间。降低碰撞检测成本的可能方法主要有三种：1.设计更加快速的碰撞检测器；2.设计更好的采样策略，从而产生规模更小的地图（roadmap）；3.延迟碰撞检测，直到它们确实被需要。而本文的作者认为第三种策略前景广阔。事实上，在Robert Bolin和Lydia E. Kavraki发表于2000年ICRA的论文Path Planning Using Lazy PRM中就已经提出了延迟碰撞检测的概念，但是本文认为延迟碰撞检测的潜力还远远没有被充分挖掘，因为该论文给出的方法需要事先确定需要一个多大的网络（network）。如果网络取得太粗糙，则很可能没有包含一条可用路径；如果取得太细密，显然会非常耗时。本文中，作者提出的SBL算法旨在更好地挖掘延迟碰撞检测的潜力，并结合单查询、双向以及自适应采样技术。SBL受到一些实验现象的启发：为了研究碰撞检测对运行时间的影响，作者删除了规划器中用于检测里程碑（milestone）之间连接是否碰撞的代码，结果发现规划速度提高了2到3个数量级，并且，令人吃惊的是，这种方法产生的相当一部分路径竟然是无碰撞的！最终，根据实验现象他们得出了四个结论： 1.在概率地图中，大部分局部路径并没有出现在最终路径上。实验结果表明，先前的里程碑出现在最终路径上的比例大约在 0.001到0.1之间。 2.在无碰撞的连接上进行碰撞检测是最耗时的。事实上，碰撞检测一旦遇到了真正的碰撞就会立即停止，而如果原本救没有碰撞，那么检测会一直进行，直到达到指定的分辨率。 3.两个里程碑之间的短连接无碰撞的概率很高。因此，过早地对连接进行碰撞检测是无用且耗时的。 4.如果两个里程碑之间的连接有碰撞，那么该连接的中点也很可能有碰撞。因此，下一步应该检查中点。下面介绍几个概念。所谓查询，是由两个查询位姿定义$q_{init}$和$q_{goal}$的，如果这两个位姿位于无障碍空间F的同一个连通分支上，则规划器应返回一条无障碍路径；否则，规划器应该指出无障碍路径不存在。PRM有单查询和多查询之分。多查询规划器需要预先计算出一个地图，之后可用此地图进行多次查询；单查询规划器则需为每一个查询任务建立一张新地图。规划器以$q_{init}$或$q_{goal}$其中之一为根生长一棵树，直到和另一个查询位姿建立连接，这叫单向采样。规划器分别以$q_{init}$和$q_{goal}$为根同时生长两棵树，直到两棵树之间建立连接，这叫双向采样。有了以上的实验现象和相关概念作为基础，下面介绍SBL算法。其中，s表示允许产生里程碑的最大数目，$\rho $ 表示距离阈值。对于C-Space中的任意点q，q的半径为r的临域定义为$B\left ( q,r \right )=\left \{ q^{'}\in C |d\left ( q,q^{'} \right ) r \right \}$ 。首先是算法的整体框架：其中，EXPAND-TREE向两棵树之一添加一个里程碑，CONNECT-TREES连接两棵树。如果算法循环s次依然没有找到可用路径则返回failure。返回failure可能是因为$q_{init}$和$q_{goal}$之间确实没有无碰撞路径，也可能是因为规划器没有找到。下面是EXPAND-TREE算法：算法首先选择要生长的树T，然后以概率$\pi \left ( m \right )$ 从该树上选择一点m。$\pi \left ( m \right )$与T上m周围点密度反相关。最终，与m距离小于$\rho$的一个无碰撞点q被选中并成为一个新里程碑。注意$\pi \left( m \right )$的作用在于使里程碑均匀分布，避免在F的某些区域内过采样。这是一个自适应算法：在开阔区域，从m到q的跨度较大；在狭窄区域，该跨度较小。注意，此处并没有对m和q之间的连接进行碰撞检测，因此，这个连接不叫局部路径，而称作段（segment）。下面是CONNECT-TREES算法：其中，m是EXPAND-TREE刚刚加上的里程碑，$m^{'}$ 是另一棵树上距离m最近的里程碑。连接两棵树的段w叫做桥（bridge）。注意，当桥建立后，连接$q_{init}$和$q_{goal}$的路径的碰撞检测才开始进行。这就是延迟碰撞检测的含义。下面是TEST-SEGMENT算法：其中，u表示需要进行碰撞检测的段，$\kappa \left ( u \right )$指出当前分辨率，它被初始化为0。若$\kappa \left ( u \right )$=0，说明只有u的两个端点被检测为无碰撞；若$\kappa \left ( u \right )$=1，说明u的两个端点以及中点被检测为无碰撞。更一般地，对于任意$\kappa \left ( u \right )$，有$2^{\kappa \left (u \right )}+1$个u的等分点被检测为无碰撞。$\lambda \left (u \right )$表示u的长度。当$2^{-\kappa \left (u \right )}\lambda\left (u \right )\varepsilon $，u即被标记为安全（safe）。$\sigma \left (u,j \right )$表示u上已经被检测为无碰撞的点集，其中$j= \kappa \left (u \right )$。显然，在两种情况下，u不能被标记为安全：一是在当前分辨率下就已经检测出碰撞，返回collision，一是当前分辨率下没有检测出碰撞，且当前分辨率尚未达到$\varepsilon$，此时应将$\kappa \left (u \right )$加一。所有没有被标记为安全的u，$2^{-\kappa \left (u \right )}\lambda \left (u \right )$的值会被存储到用于表示u的数据结构中。令$u_{1}, u_{2}, ..., u_{p}$表示路径$\tau$上所有未被标记为安全的段。$TEST-PATH \left (\tau \right )$维护一个按照$2^{-\kappa \left (u \right )} \lambda \left (u \right )$降序排列的优先队列U。下面是TEST-PATH算法：步骤1.1将提取U中的第一个元素u，并将其从U中删除。如果步骤1.2中的TEST-SEGMENT(u)既没有检测到碰撞，又没有将u标记为安全，则u会被重新插入到U中。注意，当u重新回到U中时，它可能已经不在队列中原来的位置了，因为之前的$2^{-\kappa \left (u \right )} \lambda \left (u \right )$已经在TEST-SEGMENT中被2除了一次。显然，该算法优先挑选当前分辨率较高的段执行碰撞检测，因为它们看上去更像会发生碰撞。最终，如果检测到碰撞，则将发生碰撞的段从当前地图中删除；否则，直到U中的所有段都被标记为安全，返回路径$\tau$。删除段u会导致地图重新被分割为两棵树。这里有两种情况：若删除的段u为桥，则两棵树均回到连接前的状态；否则，删除u会使一些里程碑从一棵树转移到另一棵树。下图示意了删除段非桥的情况，其中w表示桥，$m_{1}, m_{2}, m_{3}$以及它们的一些孩子原本位于$T_{goal}$，删除u后，它们转移到了$T_{init}$上。以上即为SBL算法的提出背景和具体描述。原文的实验及总结部分在本文中略过。; 个人分类: 运动规划|4635 次阅读|0 个评论

更多...

帐号		自动登录	找回密码
密码			注册

关闭 安全验证

标签: 运动规划

相关帖子

相关日志

关闭安全验证