本文作为论文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算法的提出背景和具体描述。原文的实验及总结部分在本文中略过。


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


下一篇：读书笔记——易扩张C-Space中的路径规划