博文
读书笔记——桥测试，一种解决PRM中窄道问题的好方法
|||
本文作为论文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，没有照顾到空旷区域$Fsetminus P$。但是，我们知道，均匀分布$\pi_{U}$虽然对窄道区域无可奈何，却能够很好地照顾到空旷区域。于是，我们自然而然地考虑将二者结合，就有了混合采样分布（a hybrid sampling distribution）：
$begin{equation} pi = left (1-w right )pi_{B}+wpi_{U}tag{3}end{equation}$
其中，$0\leq w \leq 1$是权值，它的选取取决于在窄道中采样的难度和覆盖F所需里程碑的数目。由于论文作者假设F中包含了一些窄道，他们将$\pi_{B}$的值调得较大。
将自由空间F分成P和$F\setminus P$ 两部分来看，我们通过调整每一部分的采样策略来获得在整个采样空间的最好效果，即我们必须知道两种采样分布如何互补才能在不丢失自身优势的情况下形成完美组合。抛开权值的选择，有一点还是可以取巧的：那些被RBB“抛弃”的点，均匀分布可以拿来复用。
实验部分和总结部分略过。
https://blog.sciencenet.cn/blog-795645-628510.html
上一篇：读书笔记——PRM的高斯采样策略
下一篇：读书笔记——MAPRM，在自由空间的中轴上采样
窄道（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，没有照顾到空旷区域$Fsetminus P$。但是，我们知道，均匀分布$\pi_{U}$虽然对窄道区域无可奈何，却能够很好地照顾到空旷区域。于是，我们自然而然地考虑将二者结合，就有了混合采样分布（a hybrid sampling distribution）：
$begin{equation} pi = left (1-w right )pi_{B}+wpi_{U}tag{3}end{equation}$
其中，$0\leq w \leq 1$是权值，它的选取取决于在窄道中采样的难度和覆盖F所需里程碑的数目。由于论文作者假设F中包含了一些窄道，他们将$\pi_{B}$的值调得较大。
将自由空间F分成P和$F\setminus P$ 两部分来看，我们通过调整每一部分的采样策略来获得在整个采样空间的最好效果，即我们必须知道两种采样分布如何互补才能在不丢失自身优势的情况下形成完美组合。抛开权值的选择，有一点还是可以取巧的：那些被RBB“抛弃”的点，均匀分布可以拿来复用。
实验部分和总结部分略过。
https://blog.sciencenet.cn/blog-795645-628510.html
上一篇：读书笔记——PRM的高斯采样策略
下一篇：读书笔记——MAPRM，在自由空间的中轴上采样
