[1996 TRO] Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

Abstract

这篇文章就是第一篇提出PRM的文章。

这篇文章提出了一种新的应用于静态工作空间的运动规划方法。这种方法分为两个阶段进行：1.learning phase 2.query phase。

1. 在learning phase中，建立roadmap。roadmap就是graph，其中的node就是collision-free configurations，edge就是这些configuration之间的feasible path（ feasible path怎样理解），这些path是通过一个简单快速的local planner计算的。

2. 在query phase中，任何给定的initial configuration和goal configuration都可以近似成roadmap中的两个node，然后在roadmap中搜索来找到可以连接这两个node的path。

这种方法可以被应用于任何类型的完整机器人。

在这篇文章中，这种方法被应用于有很多自由度的平面铰接机器人。

Intro

这篇文章提出的一种新的规划方法，可以计算出无碰撞路径，适用于在静态空间中运动的任何类型的机器人。但是，这种方法更适用于有很多自由度的机器人（五个或者更多）。这种方法进行时分为两个阶段：1.learning phase 2.query phase。

在learning phase，建立了一个roadmap，建立方式是重复产生机器人的 random free configuration，然后连接这些configuration。因此，roadmap是建立在free configuration space中的，并且roadmap以无向图的方式存储。在learning phase的最后，会对roadmap做一些后处理来提高它的连通性。

接下来就是query phase（A query asks for a path between two free configurations of the robot.）。来了一个query之后，首先做的肯定是找到给定的initial configuration和goal configuration对应的图中的node。然后就是做图搜索来找到a sequence of edges，然后再转化为a feasible path。

注意，learning phase和query phase不一定要被连续的执行。 相反，他们可以被交织执行，以调整roadmap的大小来适应query phase中遇到的困难，因此提高了learning的效果。举个例子：可以首先建立一个小的roadmap，然后这个roadmap可以被加强（或者减小），通过使用在query被处理时得到的中间数据。这篇文章不会讨论这种思路。

这种方法有一些参数需要被选择，例如：在learning phase要花的时间。虽然这些参数取决于场景（机器人和workspace），但是根据作者的经验，当值跨越相当大的区间时就会得到良好的结果。因此，通过一些预先实验来选择一些好的参数是不难的。而且，可以通过裁剪一些这种方法的组件来提高效率，特别是local planner。

Related Works

在known and static workspace中的路径规划已经被研究很久了。最近，人们对heuristic的路径规划重新产生了兴趣。

在高维构型空间中的完备的路径规划方法的复杂性已经让研究者们开始寻找那些有更弱完备性（例如概率完备性）的heuristic method。

在近些年，最令人印象深刻的结果中的一些是基于人工势场的方法。人工势场法的缺点是local-minima和难以应用于多自由度。

Roadmap也不是这篇文章提出的概念，它是一个很general的概念，意思就是 a network of paths in free C-space，表达了整个configuration space的连通性。
在这之前的roadmap方法包括visibility graph [39]，Voronoi diagram [41]，silhouette [8]。所有这三种方法都在a single shot中计算出a roadmap。前两者限制到了低维空间。最后一种倒是可以应用到任意自由度，但是它的复杂性让它几乎不实用。

在一些其它方法中，roadmap也被递增的建立和使用。

General method

learning phase

learning phase包括连续的两步，一步是construction，一步是expansion。
前者的目的是建立一个连接合理的graph，其中有足够的vertices来相当均匀的覆盖free C-space，并且确保这个空间中大多数“difficult”区域都有一些node。
后者的目的是在进一步提高这个图的连通性，它根据heuristic function在在roadmap的difficult region中选择一些node，并通过在这些被选择的node的周围产生一些额外的node来扩展这张图。
所以，the covering of $C_f$ by the final roadmap is not uniform, but depends on the local intricacy of the C-space./font>

the construction step

初始化时，graph $R=(N,E)$是空的，然后，重复的，生成一个randon free configuration，然后把它添加进$N$。对于每个这样的新的node $c$，我们从当前的$N$中选择一些node (1)，然后使用local planner将它们和$c$连接在一起。如果成功的在$c$和一个选择的node $n$，这条edge $(c,n)$ 就会被加入到 $E$ 中（The actual local path is not memorized.）

(1) 首先，a set $N_c$ of candidate neighbors is chosen from $N$. 这个集合是nodes within a certain distance of $c$, for some metric $D$.

contruction 这一步的整体算法如下：

上面算法的很多组件其实还没有被指定。

特别是，怎样采样cofigurations (4)，怎样设计local planner（8），怎样定义candidate neighbor（5），怎样选择distance function $D$ （7）

下面将逐一进行说明：

• creating of random configurations

宗旨就是roadmap中的node应该$C_f$的均匀随机采样。
具体采样方式就是将每个自由度的值都分为区间，然后每个自由度的区间的概率分布都是均匀分布，然后每个自由度都是独立采样。
每次采样出一个样本还要进行碰撞检测，如果是无碰撞的configuration，那么就加入到$N$中，否则就丢弃。

碰撞检测的导致步骤就是检测机器人的任何部分是否与障碍物相交以及是否机器人的两个部分是否相交。这部分现在有一些现成的技术

• local planner

对于local planner没有什么严格的要求，下面我们要讨论一下选择local planner的一些权衡。

如果使用nondeterministic planner，local path就要被储存在roadmap中。

关于local planner究竟要多快，这里要权衡 在每个单独的call要画的时间 和 call的数量。

local planner的选择也会影响query phase。learning phase的目的就是想准实时的回答query。快速的将任何给定的start configuration和goal configuration联系到roadmap是非常重要的，如果检测到没有这样的连接的话及时回答也是非常重要的。这就需要roadmap足够dense，所以它至少包括一些（至少一个）node能被initial configuration和goal configuration连接到。因此似乎我们更想使用更快的local planner，即使它不那么powerful，来建立建立一个大的roadmap。
另外，如果local planner是非常快的，我们在query时可以使用同样的planner连接start configuration和goal configuration。

一个很普遍的local planner是直接用直线连接configurations，这种适用于任何holonomic robots。对这种直线连接进行碰撞检测和joint limits。进行碰撞检测的方法可以如下：首先，把这条直线离散化成一些configurations

• the node neighbors

这部分就是讲怎样生成$N_c$。
通过只提交相对距离（根据distance function $D$）在一个阈值内的configuration pair，我们可以避免那些对于local planner可能会返回failure的调用。

• the distance function

distance function的作用就是建立和排序 $N_c$。它应该被定义为对于任意的 configuration pair，它可以反映local planner返回failure的机会。

一种可能性是定义为在没有障碍物的情况下，机器人沿着local planner计算的configuration pair之间的path扫过的workspace region的一种度量。configuration pair的欧式距离也可以。

expansion step

这一步的目的是提高上一步产生的图的连通性。

一种典型的情况是，如果图在$C_f$连通的地方没有连通，这些地方一般对应free的C-space中一些狭窄的区域，这些区域就是 difficult region。

这一步的想法就是从$N$中选择一些很可能位于difficult region的node，然后去expand它们。

具体expand的方式其实非常简单：如果我们想要expand $c$，那么我们就从$c$的neighborhood中选择一个新的free configuration，然后把它加入到$N$中, 然后执行上面contruction那一步的一些operation。所以expansion这一步就是提高在difficult region的roadmap configuration的密度。

这篇文章提出了expansion step如下的probabilistic scheme：

query phase

在查询阶段，可以使用在learning phase构建的roadmap，来找到任意start configuration和goal configuration之间的path。

暂时假设 free C-space 是连通的并且roadmap只有一个连通分量 $R$。

给定一个start configuration $s$ 和 goal configuration $g$ 之后，我们尝试用两条feasible path $P_s$和$P_g$ 分别将$s$和$g$连接到$R$中的两个node $\hat{s}$和$\hat{g}$。

如果上面这个过程失败了，那么query就返回fail。否则，我们就计算出$R$中的一条path $P$来连接 $\hat{s}$和$\hat{g}$。

现在主要问题就是怎样计算$P_s$和$P_g$。
我们想要query是准实时的，所以不想应用expensive algorithm。
我们将s连接到$R$的策略是 以离$s$的距离（根据上面的distance function）升序的顺序 来逐个考虑$R$中的node，具体方式就是尝试用local planner连接它们和$s$，直到一次成功的连接。

一旦$s$被成功的连接进$R$了，那么我们就执行同样的步骤把$g$也连接进$R$。

但是在一般情况，roadmap都会包含几个连通分量$R_i,i=1,2,...,p$。这可能是