GMapping的粒子滤波框架

粒子滤波器是一种通用的滤波框架，在上一篇文章中，我们从总体上介绍了粒子滤波器的基本思想，以及常用的SIR粒子滤波的过程。并没有交代如何将之用于SLAM问题中。所谓的SLAM问题，可以拆分为两个子问题：定位机器人、构建地图。这两个子问题的解是相互依赖的：若要精确的定位机器人，就需要一个精确的地图；若要构建一个精确的地图，就需要明确机器人的具体位置。两者之间的关系有点先有鸡还是先有蛋的意思。

对于SLAM问题，我们除了需要估计机器人的位姿或者运动轨迹之外，还需要估计地图的一些状态。我们完全可以将机器人的状态和地图的状态一起写到一个状态向量里，写出系统的状态方程和观测方程，然后直接套用粒子滤波器的框架。但是，对于复杂的环境，或者地图尺寸很大的场景，这个状态向量的维度就会变得很高，以至于几乎不可能通过计算机实现。

自从Rao-Blackwellized粒子滤波器的提出，极大的简化了SLAM问题中的粒子滤波过程，降低了空间和时间的复杂度。GMapping是一种RBPF的实现，它在构建粒子滤波器的建议分布时，考虑了传感器的数据，达到了降低粒子数量，提高准确度的目的。此外，还提出了一个指标用于自适应的进行重采样，在一定程度上缓解了粒子匮乏的问题。

本文中，我们将详细介绍GMapping的算法思想，及其滤波框架。

1. Rao-Blackwellized粒子滤波器

Murphy等人[?]，提出了一种Rao-Blackwellized粒子滤波器(RBPF)。他们认为在已知机器人运动轨迹的情况下，地图的特征之间就不存在依赖关系，即它们是条件独立的。因此，我们可以将估计的后验概率密度函数分解为:

$$ \begin{equation}\label{f1} p(x_{0:t}, m | z_{1:t}, u_{1:t}) = p(m | x_{0:t}, z_{1:t})p(x_{0:t} | z_{1:t}, u_{1:t}) \end{equation} $$

其中，$x_{0:t}$表示从初始时刻到$t$时刻的机器人状态序列，机器人的初始状态记为$x_0$，$m$是环境的地图，$z_{1:t}$则是机器人的观测序列， $u_{1:t}$则是机器人历次的控制序列。上式($\ref{f1}$)使得我们可以先只对机器人的状态进行估计，然后在估计的机器人状态基础上估计地图状态。这个过程就是所谓的Rao-Blackwellization。

在已知$x_{0:t}$和$z_{1:t}$的情况下，可以很容易的计算$p(m | x_{0:t}, z_{1:t})$，根据《Probabilistic Robotics》的介绍，这个过程可以通过EKF等方法解决。在RBPF中，其粒子滤波体现在对概率密度$p(x_{0:t} | z_{1:t}, u_{1:t})$的估计上。每个粒子都是一个对机器人位姿轨迹的估计，相应的对于每个粒子都会有一个地图。整个SLAM建图迭代过程可以总结为如下的四个步骤：

采样(Sampling): 根据系统的状态方程$x_t = g_t(u_t, x_{t-1}) + \varepsilon_t$，在$t-1$时刻的粒子集合$\{x_{t-1}^{(i)}\}$的基础上，对$t$时刻的粒子$\{x_t^{(i)}\}$进行预测，这个过程也可以说是对建议分布$\pi$的采样。比较常用的做法就是对以里程计数据作为系统的控制量。
加权(Weighting): 然后根据粒子滤波器的套路，为每个粒子计算一个重要性权重$w_t^{(i)}$，描述目标分布与建议分布之间的比值。 $$ \begin{equation}\label{f2} w_t^{(i)} = \frac{p(x_{0:t}^{(i)} | z_{1:t}, u_{1:t})}{\pi(x_{0:t}^{(i)} | z_{1:t}, u_{1:t})} \end{equation} $$
重采样(Resamplling): 对加权的粒子进行重采样，以增加粒子集合中权重较大的粒子数量，降低权重较小的粒子数量。重采样之后，粒子集合中的每个粒子的权值都将相等。
估计地图(Map Estimation): 对于每个粒子，我们都有一个运动轨迹$x_{0:t}^{(i)}$的估计，此外所有的粒子的观测量$z_{1:t}$都是一样的。进而为每个粒子估计一幅地图，完成对$p(m^{(i)} | x_{1:t}^{(i)}, z_{1:t})$的估计。

上述框架留下了两个开放的问题：如何计算建议分布，何时进行重采样。GMapping的工作就是在RBPF的建图框架下，增加扫描匹配的操作，将激光传感器的扫描值考虑到建议分布中，以提高准确度降低粒子数量。同时根据一个定量的指标来自适应的进行重采样，降低了系统的计算量，还在一定程度上缓解了粒子匮乏问题。

2. 扫描匹配

在RBPF的框架中，首先需要对一个建议分布进行采样。直觉上，这个建议分布越接近目标分布，滤波器的效果就越好。极端的，如果建议分布与目标分布之间不存在差异，我们就可以直接对目标分布进行采样。那么所有样本的重要性权值将都是一样的，也就不需要重采样操作了，更不会有粒子退化、粒子匮乏的问题了。

通常人们倾向于使用里程计的数据作为机器人的控制量进行SLAM。这样做法的优势就是其简洁的状态方程，能够方便的使用计算机计算。此时的权重公式可以写成如下的形式： $$ \begin{equation}\label{f3} w_t^{(i)} \propto w_{t-1}^{(i)} · p(z_t | m_{t-1}^{(i)}, x_t^{(i)}) \end{equation} $$ 上式中的$p(z_t | m_{t-1}^{(i)}, x_t^{(i)})$可以由观测方程中的误差模型来描述。这个方程的形式看起来很简洁，但是存在一个反直觉的缺陷，激光传感器的精度相比于通过里程计估计的控制量精度过高时，定位精度就会变得很差。 Grisetii等人认为在马尔可夫假设下，最优的建议分布如下式($\ref{f4}$)所示，由观测误差模型和运动误差模型两个部分构成。 $$ \begin{equation}\label{f4} p(x_t | m_{t-1}^{(i)}, x_{t-1}^{(i)}, z_t, u_t) = \frac{p(z_t | m_{t-1}^{(i)}, x_t)p(x_t | x_{t-1}^{(i)}, u_t)} {\int p(z_t | m_{t-1}^{(i)}, x')p(x' | x_{t-1}^{(i)}, u_t)dx'} \end{equation} $$

由于传感器的精度远高于控制器的精度，所以图中实线所示的观测误差模型具有一个狭窄的尖峰，而虚线所示的运动误差模型的概率密度函数相对平缓很多。以参数$\varepsilon > 0$为阈值，我们可以用区间$L^{(i)} = \left\{ x | p(z_t | m_{t-1}^{(i)}, x) > \varepsilon \right\}$来描述完全由观测误差模型主导的概率密度。在该区间中，我们可以近似的认为运动误差模型的概率密度是一个常数$k$，那么式($\ref{f4}$)可以近似写成如下的形式：

$$ \begin{equation}\label{f5} p(x_t | m_{t-1}^{(i)}, x_{t-1}^{(i)}, z_t, u_t) \simeq \frac{p(z_t | m_{t-1}^{(i)}, x_t)}{\int_{x'\in L^{(i)}} p(z_t | m_{t-1}^{(i)}, x')dx'} \end{equation} $$

图 1. 最优建议分布示意图。由于传感器的精度远高于控制器的精度，所以其中观测误差模型的不确定度更小，如图中的实线所示，具有一个狭窄的尖峰。控制器的不确定度更大，概率密度函数相对平缓很多，如图中虚线所示。图中区间$L^(i)$的概率密度由观测误差模型主导。

更进一步的，在区间$L^{(i)}$的局部最大值处，我们可以用一个高斯分布对式($\ref{f5}$)做出近似：

$$ \begin{equation}\label{f6} p(x_t | m_{t-1}^{(i)}, x_{t-1}^{(i)}, z_t, u_t) \simeq \mathcal{N}\left(\mu_t^{(i)}, \Sigma_t^{(i)}\right) \end{equation} $$

那么对于每个粒子，我们通过一个扫描匹配操作来求取区间$L^{(i)}$的局部最大值所在的位置，并对该位置的一个很小的邻域计算统计量$\mu_t^{(i)}$和$\Sigma_t^{(i)}$来对式($\ref{f6}$)进行采样。

$$ \begin{equation}\label{f7} \mu_t^{(i)} = \frac{1}{\eta} \sum_{j=1}^K x_j p\left(z_t | m_{t-1}^{(i)}, x_j\right) \end{equation} $$ $$ \begin{equation}\label{f8} \Sigma_t^{(i)} = \frac{1}{\eta} \sum_{j=1}^K p\left(z_t | m_{t-1}^{(i)}, x_j\right)\left(x_j - \mu_t^{(i)}\right)\left(x_j - \mu_t^{(i)}\right)^T \end{equation} $$

式中$\eta = \sum_{j=1}^K p\left(z_t | m_{t-1}^{(i)}, x_j \right)$是一个归一化因子。如此调整之后的粒子权重可以近似写为:

$$ \begin{equation}\label{f9} w_t^{(i)} \simeq w_{t-1}^{(i)} k \eta \end{equation} $$

在分析GMapping的扫描匹配器的过程中，我们可以看到GMapping首先通过爬山算法求取使得概率密度获得局部最大值的位置，并根据所对应的似然概率，判定扫描匹配是否通过。若通过，就以类似式($\ref{f7}, \ref{f8}, \ref{f9}$)的形式修正粒子的状态。

3. 自适应重采样

重采样技术也是影响粒子滤波器效果的一个关键因素。一方面，我们只能使用有限的样本来近似描述目标分布，需要合理的选择和组合样本，在合适的时机对样本进行重采样是必要的。另一方面，重采样的过程是用权重大的粒子替代权重小的粒子，有可能删除一些性质很好的样本，最终造成粒子匮乏的问题。因此，人们就开始研究进行重采样的时机。

GMapping中通过计算$N_{eff}$来评价粒子集合的质量，当$N_{eff}$小于某个阈值的时候，就进行重采样。

$$ \begin{equation}\label{f10} N_{eff} = \frac{1}{\sum_{i = 1}^N \left(\tilde{w}^{(i)} \right)^2} \end{equation} $$

其中，$\tilde{w}^{(i)}$是第$i$个样本的归一化后的权值。上式中分母的意义在于衡量样本权值的差异。其背后的思想就是，如果一组样本是对目标分布的良好采样，那么根据重要性权值的定义，各个样本的权重应当基本一致。所以权重的差异越大，意味着样本的分布与目标分布之间存在着较大的差异，需要进行重采样来修正。在上式中最后取一个倒数，是为了调整评价指标的单调性，相当于求取粒子权重的相似度，小于给定阈值时就会触发重采样操作。

4. GMapping的粒子滤波框架

我们将扫描匹配操作和自适应重采样操作整合到RBPF的框架中，就得到了GMapping的粒子滤波框架。在每个迭代过程中，我们都需要以当前最新的控制量和测量值$\langle u_t, z_t \rangle$作为输入。并对样本集合中的每个粒子，依次完成如下的操作：

采样(Sampling): 根据系统的状态方程$x_t = g_t(u_t, x_{t-1}) + \varepsilon_t$，在$t-1$时刻的粒子$x_{t-1}^{(i)}$的基础上，对$t$时刻的粒子进行预测得到$x_t^{\prime(i)}$。与传统RBPF不同的是，这一预测过程并不是对建议分布$\pi$的采样。
扫描匹配(Scan-Matching): 以某种寻优的方法来求取式($\ref{f6}$)所对应的区间$L^{(i)}$中的局部最大值，即求取$\hat{x}_t^{(i)}$，同时计算得到一个匹配度，如果匹配度小于一个指定的阈值，认为匹配失败。 $$ \begin{equation}\label{f11} \hat{x}_t^{(i)} = \mathrm{argmax}_x p\left(x | m_{t-1}^{(i)}, z_t, x_t^{\prime(i)}\right) \end{equation} $$
匹配成功的粒子更新: 如果扫描匹配成功，说明传感器数据可信。我们将根据式($\ref{f7},\ref{f8}$)在$\hat{x}_t^{(i)}$的邻域内计算对$z_t$最佳匹配下的建议分布的均值$\mu_t^{(i)}$和方差$\Sigma_t^{(i)}$，并采样得到修正后的样本$x_t{(i)} \sim \mathcal{N}\left(\mu_t^{(i)}, \Sigma_t^{(i)}\right)$。根据式($\ref{f9}$)计算样本的权重$w_t^{(i)}$。
匹配失败的粒子更新: 如果扫描匹配失败，说明传感器数据精度较低。我们仍然采用传统RBPF的方式更新粒子的机器人状态和权重： $$ \begin{align}\label{f12} x_t^{(i)} & \sim p(x_t | x_{t-1}^{(i)}, u_t) \\ w_t^{(i)} & = w_{t-1}^{(i)} · p(z_t | m_{t-1}^{(i)}, x_t^{(i)}) \end{align} $$
估计地图(Map Estimation): 对于每个粒子，根据更新的机器人状态$x_t^{(i)}$以及观测值$z_t$更新地图信息$m^{(i)}$。

完成对每个粒子的更新之后，就可以根据式($\ref{f10}$)计算粒子集合评价参数$N_{eff}$，当该值小于某个给定的阈值时，对粒子集合进行重采样。

5. 完

本文中，我们介绍了GMapping的粒子滤波框架。它是一种RBPF的粒子滤波算法，先估计机器人的位姿，再根据该位姿下估计地图信息。GMapping在此基础上，优化了对建议分布的采样方式，在采样过程中考虑了传感器的测量值，提高了准确度。此外，还通过一个量化指标来计算粒子权重的相似度，从而自适应的进行重采样，在一定程度上缓解了粒子匮乏问题。