本文作为论文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\left\(x_{i}}\right\)=p_{i}$ ，其中 $p_{i}$ 是在给定抽彩L后，结果 $x_{i}$ 出现的概率。如果抽彩L的可能结果中依然存在抽彩，则L称为复合抽彩（compound lottery）。下图分别表示了具有三个可能结果的简单抽彩 $L_{1}$ 和具有两个可能结果的复合抽彩 $L_{2}$ ：

   给定一个抽彩集合，由于每个抽彩的结果都是不同的。对于一个个体（agent）而言，它需要一个偏好函数（preference function），用于计算对某个抽彩的偏好程度，从而可以将不同的抽彩按照偏好程度排序。这个函数就是效用函数（utility function）。效用函数计算出的值就是效用，即个体对抽彩的偏好程度。效用函数的作用就是将这个偏好程度数字化了。设存在两个抽彩 $L_{i}$ 和 $L_{j}$ ，如果某个体偏好 $L_{i}$ 的程度比偏好 $L_{j}$ 的程度高，则 $L_{i}$ 的效用大于 $L_{j}$ 的效用。现在的问题是，每个抽彩的效用应该怎样计算呢？

   由于每个抽彩都包含了若干结果，我们自然能够想到利用这些结果去表示抽彩。这就引出了“期望效用（expected utility）”的概念。令 $X\left\(L_{i}\right\)$ 为抽彩 $L_{i}$ 所有可能结果的集合， $u\left\(x\right\)$ 为某个结果x的效用，则 $L_{i}$ 的期望效用定义为：

$U\left\(L_{i}\right)=\sum_{x\in X\left\(L_{i}\right)} P\left\(x\right)u\left\(x\right)$

其中， $P\left\(x\right)$ 是结果x出现的概率。一旦计算出每个抽彩的期望效用，就可以对这些抽彩排序了。一个抽彩的效用越高，则我们越偏好它。对应到采样过程中，一个采样点的效用越高，则说明它对建立roadmap的贡献越大。

   期望效用理论就介绍这么多。我们更关心的是它在运动规划中的应用。为了规划出完整有效的路径，规划器（planner）一定希望它能够完全掌握位姿空间的结构信息——显然，这是不可能的，对于依靠采样来探索位姿空间的规划器尤其不可能。既然如此，我们就退而求其次，力求用尽量少的采样，即探索（exploration），来获取尽量多的关于位姿空间结构的信息。俗话说“兵不贵多而贵精”，只要每一个采样点足够“精”，恰好落在最有价值的地方，即它的效用足够高，用少数采样点就能获得极有价值的位姿空间结构信息。现在的问题是，这可能吗？

   答案是肯定的。我在上一篇博客中描述了论文Sampling-Based Motion Planning Using Predictive Models提出的利用局部加权回归，结合已有采样点信息建立位姿空间的近似模型，并以此模型预测采样点的有效性，同时结合主动学习指导新采样点生成的方法。在这篇论文中，能够最大限度减小近似模型方差的采样点被认为是最有价值的。不过，用局部加权回归来预测采样点有效性的计算复杂度似乎有点大，我们可以用一种更简洁的方法预测采样点有效性，那就是K近邻。据该论文说，如果一个采样点的周围都是碰撞点，那么该采样点很可能是碰撞点；反之，如果它的周围都是自由点，那么它极可能是自由点。所谓“近朱者赤，近墨者黑”嘛！也就是说，我们的近似模型不用局部加权回归来建立，而是用K近邻的方法来建立。现在，我们需要一种衡量采样点“价值”的标准，用于取代上述论文给出的“方差度量”——毫无疑问，该论文使用的正是期望效用。设M是根据当前roadmap中的采样点信息建立的位姿空间近似模型，R为当前roadmap，对于一个即将加入R的采样点q而言，其期望效用的计算公式如下：

$U_{exp}\left\(q\mid M\right\)=\sum_{i \in \left\{obs,free\right\}}P\left\(q=i \mid M\right\)U\left\(q=i,R\right\)$

   现在，我们将每次采样都看作一次抽彩。每次抽彩的可能结果都有两个：碰撞和自由。公式中的 $P\left\(q=i \mid M\right\)$ 就是抽彩结果的概率分布。

   注意，上面的期望效用是我们用某种方法预测出、用于指导采样点生成的，采样点q到底是碰撞还是自由也是预测的结果。这里所用的预测方法就是刚才提到的K近邻。论文作者声称K近邻的效果和局部加权回归的效果差不多，咱们就姑且相信吧！由于位姿空间中的点只有自由和碰撞两种状态，因此定义函数 $C\left\(q\right\)=0,1$ ，用以反映状态已知的采样点q是自由还是碰撞。若q的状态未知，则利用K近邻预测q有效性的具体方法是：设 $N\left\(q,k\right\)$ 是q的k个最近邻，预测结果 $\tilde{C}$ 可以按照如下公式计算：

$\tilde{C}\left\(q\right\)=\sum_{i}^{N\left\(q,k\right\)}C\left\(q_{i}\right\)/k$

我们把 $\tilde{C}\left\(q\right\)$ 认为是q为自由点的概率。设M是当前状态已知的采样点集合，则

$P\left\(q=free \mid M\right\)=\tilde{C}\left\(q\right\)$

$P\left\(q=obs \mid M\right\)=1-\tilde{C}\left\(q\right\)$

   既然已经可以预测采样点为自由的概率了，即 $P\left\(q=i \mid M\right\)$ 这一项已经搞定，下一个要着力解决的问题就是如何确定公式中的另一项 $U\left\(q,R\right\)$ 。在建立roadmap的过程中，一个采样点效用的大小，取决于它能够在多大程度上增加roadmap对位姿空间连通性的反映程度。文章中给出了一个比较夸张的示意图：

显然，图中的四个采样点的效用非常高，因为这四个点几乎可以完全反映位姿空间的连通性。这些极有价值的点是如何选取的呢？该论文的另一个亮点出现了，那就是利用熵（entropy）！

   熵的概念起源于热力学，但是香农将此概念引入了信息论，并取得巨大成功。在信息论中，熵是随机变量不确定性的度量，换句话说，熵是对信息内容的不可预测性的度量，熵越高，信息内容越不可预测。设 $P\left\(d\right\)$ 是集合D上的一个离散概率分布，则该分布的熵为：

$H\left\(D\right\)=-\sum_{d \in D}P\left\(d\right\)\log P\left\(d\right\)$

   给定系统S，新知识K，S在获取K之前的熵为 $H\left\(S\right\)$ ，获取K之后的熵为 $H\left\(S|K\right\)$ ，则K给S带来的信息增益（information gain）为：

$IG\left\(S|K\right\)=H\left\(S\right\)-H\left\(S|K\right\)$

   我们追求采样点期望效益的最大化，其实就是要使每个即将加入roadmap的采样点都能给当前系统R，也就是当前的roadmap，带来最大的信息增益。因此，论文就把信息增益当作了即将加入roadmap的采样点q的效用值：

$U\left\(q,R\right\)=IG\left\(q,R\right\)$

   为了使上式具有合理性，我们必须定义一个概率分布，使得roadmap完全连通时具有的熵最小。事实上，roadmap是由若干连通分支组成的。每个连通分支都有自己的可见区域（visibility region），该区域包含了所有能够用一条无碰撞的直线路径连接到该连通分支的自由点。方便起见，我们希望每个连通分支的可见区域都互不相交。做到这一点也不难，为了方便描述，我定义（不是论文作者定义的，确实是我为了省事而定义的），如果点q属于连通分支C的可见区域，则C是q的可见连通分支。若点q具有多于一个的可见连通分支，我们就把q划入距离它最近的连通分支的可见区域，这里，我们设函数 $f\left\(q,R\right\)$ 能够返回距离q最近的可见连通分支。定义了可见区域之后，再定义一个概率分布，即随机均匀选择的点落在某个连通分支的可见区域中的概率。显然，当roadmap完全连通，即只有一个连通分支时，R的熵最小，为0；当roadmap中有较多的连通分支时，R的熵较大。

   至此，我们已经可以计算一个即将加入R的采样点q所具有的期望效用了：

$U_{exp}\left\(q\mid M\right\)=\sum_{i\in \left\{obs,free\right\}}P\left\(q=i \mid M\right\)U\left\(q=i,R\right\) \\=P\left\(q=obs\mid M\right\)U\left\(q=obs,R\right\)+P\left\(q=free\mid M\right\)U\left\(q=free,R\right\) \\=P\left\(q=free\mid M\right\)U\left\(q=free,R\right\) \\=\tilde{C}\left\(q\right\)IG\left(M\mid q\right\)$

   注意，上面的推导过程中，我们假设碰撞点的效用为0。为何这样假设？并不是说碰撞点就一点用都没有，毕竟是位姿空间的组成部分嘛——关键在于它的作用不是特别大，更关键的是，论文作者似乎没想明白怎样判断一个碰撞点的效用，于是，这些点的效用就统统置零了。

   下面我们定量化分析R的熵和一个即将加入R的点q为R带来的信息增益。

   对于每个连通分支 $R_{i}$ ，我们定义体积 $A_{i}$ ，使得对于 $\forall q \in A_{i}$ ，有 $f\left\(q,R\right\)=R_{i}$ 。设自由位姿空间的体积为 $V_{free}$ ，则随机均匀选择的点落在连通分支 $R_{i}$ 的可见区域中的概率为 $\frac{A_{i}}{V_{free}}$ 。于是，R的熵可以按照下式计算：

$H\left\(R\right\)=-\sum_{R_{i} \in R}P\left\(R_{i}\right\)\log P\left\(R_{i}\right\)=-\sum_{R_{i}\in R}\frac{A_{i}}{V_{free}}\log\frac{A_{i}}{V_{free}}$

   若向R中新加一个点q，得到新的roadmap，记为R'。由于加入点q而为系统带来的信息增益为：

$IG\left\(q,R\right\)=H\left\(R\right\)-H\left\(R'\right\) \\ =-\sum_{R_{i}\in R}\frac{A_{i}}{V_{free}}\log\frac{A_{i}}{V_{free}}+\sum_{R_{i}\in R'}\frac{A_{j}}{V_{free}}\log\frac{A_{j}}{V_{free}} \\ =\frac{1}{V_{free}}\left\(-\sum_{R_{i} \in R}A_{i}\log\frac{A_{i}}{V_{free}}+\sum_{R_{j}\in R'}A_{j}\log\frac{A_{j}}{V_{free}}\right\)$

   由于 $V_{free}$ 难以计算，因此想要得到精确的信息增益值几乎是不可能的。不过，注意到 $\frac{1}{V_{free}}$ 是常量，由上式可得：

$IG\left\(q,R\right\) \propto -\sum_{R_{i}\in R}A_{i}\log\frac{A_{i}}{V_{free}}+\sum_{R_{j}\in R'}A_{j}\log\frac{A_{j}}{V_{free}}$

在上式中，第一项是常量，因此只要最大化第二项，就能得到最大的信息增益。对于一个即将加入roadmap的点q，它可能成为一个独立的连通分支，也可能连接到已经存在的连通分支上。如果q成为一个新的独立的连通分支，则 $\left |R\right |<\left |R'\right|$ ，也就是说，上式第二项求和的项数会增加，导致第二项变小（注意， $\log\frac{A_{j}}{V_{free}}$ 是小于0的）。因此，我们应该尽量保证q可以连接到已有的连通分支上。然而，已有的连通分支有大有小，q应该连到较大的连通分支还是较小的呢？按照论文作者Brendan Burns的博士论文中所说，应该连接到较大的连通分支上，这么做可以使较大的连通分支变得更大，从而使第二项中的 $\log\frac{A_{j}}{V_{free}}$ 更接近0，而且，连接到较大的连通分支比连接到较小的连通分支可以更多地降低R的熵。综上所述，把即将加入roadmap的点q与已有的最大连通分支连接能够最大化信息增益。

   根据上面给出的原则可以推断，如果把采样点q放在两个较大的连通分支之间，即它们的边界区域，这样可以通过q将两个较大的连通分支连成一个更大的连通分支，同时也能减少R中连通分支数目，R的信息增益将被最大化。然而，怎样计算两个连通分支之间的边界区域是个难题。Brendan Burns在他的博士论文中指出，可以从两个连通分支中随机各选一个点，然后根据这两点算出它们的中点，再从中点周围随机选择一个点作为我们想要求得的点q。根据以上思路，作者给出了他的算法：

   OK，至此，我就把效用引导的采样方法介绍完了。不得不说，有很多老外的思路都很开阔，能把经济学和信息论的知识应用于运动规划，真的服了！