前言

这是我阅读 The GraphSLAM algorithm with applications to large-scale mapping of urban structures的第三篇笔记。关于第一篇可以看 The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 1。在第二篇中已经总结了 GraphSLAM 的基本思路和具体算法，这一篇主要是进行 GraphSLAM 算法原理的具体推导。

GraphSLAM 的数学推导分为以下几步：

• 推导出一个关于 SLAM 全后验概率的递归公式
• 对每一个后验概率进行单独分析其对 SLAM 问题的影响
• 推导回复机器人位姿和地图特征点必要的公式

SLAM 后验概率

首先，我们用 $y$ 来表示 SLAM 中的状态量，包括一个或多个机器人位姿 $x$ 和地图 $m$，定义如下：

$$
\begin{aligned}
y_{0:t} &= [x_0, x_1, ..., x_t, m]^T \\
y_t &= [x_t, m]^T \\
\end{aligned}
$$

根据以上定义， SLAM 问题的后验概率是:

$$
p(y_{0:t} | z_{1:t}, u_{1:t}, c_{1:t})
$$

其中：

• $z_{1:t}$ 是观测值
• $c_{1:t}$ 是观测值观测到特征点的序号
• $u_{1:t}$ 是控制输入量

通过贝叶斯法则，后验概率可以写成以下形式：

$$
p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \eta p(z_t | y_{0:t}, z_{1:t-1}, u_{1:t}, c_{1:t}) p(y_{0:t}| z_{1:t-1}, u_{1:t}, c_{1:t})
$$

其中， $\eta$ 是和 $p(z_t)$ 有关的常系数。由于观测值只跟当前时刻状态和特征点系数有关，上面等式右边第一项可以简化为：

$$
p(z_t | y_{0:t}, z_{1:t-1}, u_{1:t}, c_{1:t}) = p(z_t | y_t, c_t)
$$

同样，我们可以将 $y_{0:t}$ 分成 $x_t$ 和 $y_{0:t-1}$ 两部分，所以等式右侧第二项可以化为：

$$
\begin{aligned}
p(y_{0:t}| z_{1:t-1}, u_{1:t}, c_{1:t}) &= p(x_t | y_{0:t-1}, z_{1:t-1}, u_{1:t}, c_{1:t}) p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t})\\
&= p(x_t|x_{t-1}, u_t)p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t})
\end{aligned}
$$

所以 SLAM 的后验概率公式可以转化为：

$$
p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \eta p(z_t | y_t, c_t) p(x_t|x_{t-1}, u_t)p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t})
$$

以上是后验概率关于 t 的迭代公式，将其展开至 $t = 0$可以写成：

$$
\begin{aligned}
p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) &= \eta p(y_0)\prod_tp(x_t|x_{t-1}, u_t)p(z_t| y_t, c_t) \\
&=  \eta p(y_0)\prod_t \Big[p(x_t|x_{t-1}, u_t)\prod_ip(z_t^i| y_t, c_t^i)\Big]
\end{aligned}
$$

上式中，$z_t^i$ 是 $t$ 时刻第 $i$ 个观测值。这里注意一下，先验概率 $p_(y_0)$ 可以分成 $p(x_0)$ 和 $p(m)$ 两部分。而在 SLAM 中，我们通常假定没有对地图的先验知识，所以在后面计算中会将 $p(y_0)$ 用 $p(x_0)$ 代替，而把 $p(m)$ 吸收进系数 $\eta$ 中。

负对数后验概率

上式中的概率可以用对数来简化表示和便于计算，所以对数-SLAM的后验概率可以写成以下形式：

$$
\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} + \log{p(x_0)} + \sum_t\Big[\log{p(x_t|x_{t-1}, u_t)} + \sum_i\log{p(z_t^i| y_t, c_t^i)} \Big]
$$

根据之前的假设，我们假定运动误差和观测误差都是服从高斯分布，所以：

• 机器人运动模型的概率服从 $\mathcal{N}(g(u_t, x_{t-1}), R_t)$，其中 $g$ 是运动模型函数，是确定性的；$R_t$ 则是运动误差的协方差矩阵
• 观测模型服从 $\mathcal{N}(h(x_t, c_t^i, i), W_t)$，其中 $h$ 是观测模型函数，是确定性的；$Q_t$ 则是观测误差的协方差矩阵

所以对上述后验概率的各部分我们分别可以写成：

$$
\begin{aligned}
p(x_t|x_{t-1}, u_t) &= \eta \exp\Big\{-\frac{1}{2}(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t01}))\Big\} \\
p(z_t^i|y_t, c_t^i) &= \eta \exp\Big\{-\frac{1}{2}(z_t^i - h(y_t, c_t^i, i))^TQ_t^{-1}(z_t^i - h(y_t, c_t^i, i))\Big\}
\end{aligned}
$$

对于先验概率，我们同样可以将其表示成高斯分布的函数（但是是确定性的，通过将协方差矩阵设为无穷小），将其均值和信息矩阵设为：

$$
p(x_0) = \eta \exp \Big\{ -\frac{1}{2}x_0^T\Omega_0x_0\Big\}
$$

其中，

$$
\begin{aligned}
x_0 &= \begin{bmatrix}
    0 & 0 & 0
\end{bmatrix}^T \\ \\
\Omega_0 &= \begin{bmatrix}
    \infty & 0 & 0 \\
    0 & \infty & 0 \\
    0 & 0 & \infty
\end{bmatrix}
\end{aligned}
$$

在实际实现的时候，对于 $\infty$ 我们只需要用一个很大的正数代替即可。后验概率的负对数形式就可以表示为：

$$
\begin{aligned}
-\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} + \frac{1}{2} \Big[ x_0^T\Omega_0x_0 &+ \sum_t(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t01})) \\
+& \sum_t\sum_i(z_t^i - h(y_t, c_t^i, i))^TQ_t^{-1}(z_t^i - h(y_t, c_t^i, i))\Big]
\end{aligned}
$$

这个和最初在介绍 GraphSLAM 图构建的时候提出的 $J_{GraphSLAM}$ 是等效的，除了一些常数项的忽略。从形式来看，上面的等式将 SLAM 问题中要求的后验概率用三部分二次项表示了出来，分别是：先验、运动模型、观测。

泰勒展开

上面的后验概率公式是建立在函数 $g$ 和 $h$ 的二次型，而不是我们想要估计的量（位姿和地图）。GraphSLAM 将其进行线性化，得到：

$$
\begin{aligned}
g(u_t, x_{t - 1}) &\approx g(u_t, \mu_{t-1}) + \underbrace{g'(u_t, \mu_{t-1})}_{=: G_t}(x_{t -1 - \mu_{t-1}})\\
h(y_t, c_t^i, i) &\approx h(\mu_t, c_t^i, i) + \underbrace{h'(\mu_t)}_{=:H_t^i}(y_t - \mu_t)
\end{aligned}
$$

其中， $\mu_t$ 是对状态向量 $y_t$ 的估计。通过这样的线性化，我们将对数似然概率转化为关于 $y_{0:t}$ 的二次项，如下：

$$
\begin{aligned}
\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} - \frac{1}{2} \Big[ x_0^T\Omega_0x_0 &+ \sum_t(x_t - g(u_t, \mu_{t-1}) - G_t(x_{t-1-\mu_{t-1}}))^TR_t^{-1}(x_t - g(u_t, \mu_{t-1}) - G_t(x_{t-1-\mu_{t-1}})) \\
+& \sum_i(z_t^i - h(\mu_t, c_t^i, i) - H_t^i(y_t - \mu_t))^TQ_t^{-1}(z_t^i - h(\mu_t, c_t^i, i) - H_t^i(y_t - \mu_t))\Big]
\end{aligned}
$$

通过展开和重新排列可以得到：

$$
\begin{aligned}
\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} &= \text{const.} - \frac{1}{2}  \underbrace{x_0^T\Omega_0x_0}_{\text{quadratic in }x_0} \\
&- \frac{1}{2} \sum_t \underbrace{x_{t-1:t}^T\begin{bmatrix}
    1 \\ -G_t
\end{bmatrix} R_t^{-1}\begin{bmatrix}
    1 & -G_t
\end{bmatrix}x_{t-1:t}}_{\text{quadratic in } x_{t-1:t}} \\
&+ \underbrace{x_{t-1:t}^T\begin{bmatrix}
    1 \\ -G_t
\end{bmatrix}R_t^{-1}\begin{bmatrix}
    g(u_t, \mu_{t-1}) + G_t & \mu_{t-1}
\end{bmatrix}}_{\text{linear in } x_{t-1:t}} \\
&- \frac{1}{2} \sum_i \underbrace{y_t^TH_t^{iT}Q_t^{-1}H_t^iy_t}_{\text{quadratic in } y_t}\\
&+ \underbrace{y_t^TH_t^{iT}Q_t^{-1}\Big[z_t^i - h(\mu_t, c_t^i, i) - H_t^i\mu_t\Big]}_{\text{linear in } y_t}
\end{aligned}
$$

上式中， $x_{t-1:t} := [x_t \quad x_{t - 1}]^T$，因此有： $[x_t - G_tx_{t-1}] = x_{t-1:t}^T[1 \quad -G_t]^T$

如果我们将上式中所有的二次项 (quadratic term) 收集进矩阵 $\Omega$，所有的一次项 (linear term) 收集进向量 $\xi$，则上式可以写成：

$$
\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} - \frac{1}{2} y_{0:t}^T\Omega y_{0:t} + y_{0:t}^T\xi
$$

构建信息矩阵

从上式中，我们可以推导出的 GraphSLAM_linearize 中的实现是正确的。关于具体的算法参考我的上一篇笔记： The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 2，初始化分为：

先验 ：通过二次项 $x_0^T\Omega_0 x_0$，进行初始化：

$$
\Omega \leftarrow \Omega_0
$$

控制量：从上式中，我们可以发现控制量 $u_t$ 对整体信息矩阵和信息向量的更新如下：

$$
\begin{aligned}
\Omega &\leftarrow \Omega + \begin{bmatrix}
1 \\ -G_t
\end{bmatrix}R_t^{-1}\begin{bmatrix}
1 & -G_t
\end{bmatrix} \\
\xi &\leftarrow \xi + \begin{bmatrix}
1 \\ -G_t
\end{bmatrix}R_t^{-1}\begin{bmatrix}
g(u_t, \mu_{t-1}) + G_t & \mu_{t-1}
\end{bmatrix}
\end{aligned}
$$

观测：同样，观测量 $z_t^i$ 对整体信息矩阵和信息向量的更新如下：

$$
\begin{aligned}
\Omega &\leftarrow \Omega + H_t^{iT}Q_t^{-1}H_t^i \\
\xi &\leftarrow H_t^{iT}Q_t^{-1}\begin{bmatrix}
z_t^i - h(\mu_t, c_t^i, i) - H_t^i\mu_t
\end{bmatrix}
\end{aligned}
$$

从上面的式子可以发现，对于非对角线元素，我们只更新了和位姿有关系的部分（位姿和位姿或者特征点和位姿），所以对于特征点和特征点（不是同一个）的值始终为 0。

信息矩阵简化

为了将信息矩阵简化，首先需要将后验概率分解成如下形式：

$$
p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = p(x_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) p(m | z_{1:t}, y_{1:t}, c_{1:t})
$$

对于第一部分有：

$$
p(x_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \int p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) dm
$$

实际中这个积分算不了，因为 $m$ 中包含的变量可能会非常多。所幸的是对于高斯变量，这个积分有解析解，主要利用了下表展示的对于高四遍了的边缘化引理：

我们可以将 $\Omega$ 和 $\xi$ 按机器人位姿 $x_{0:t}$ 和地图 $m$ 分成：

$$
\begin{aligned}
\Omega &= \begin{bmatrix}
    \Omega_{x_{0:t}, x_{0:t}} & \Omega_{x_{0:t}, m} \\
    \Omega_{m, x_{0:t}} & \Omega_{m, m}
\end{bmatrix} \\
\xi &= \begin{bmatrix}
    \xi_{x_{0:t}} \\
    \xi_m
\end{bmatrix}
\end{aligned}
$$

根据边缘化引理，有：

$$
\begin{aligned}
\tilde{\Omega} &= \Omega_{x_{0:t}, x_{0:t}} - \Omega_{x_{0:t}, m}\Omega_{m, m}^{-1}\Omega_{m, x_{0:t}} \\
\tilde{\xi} &= \xi_{x_{0:t}} - \Omega_{x_{0:t}, m}\Omega_{m, m}^{-1}\xi_m
\end{aligned}
$$

因为 $\Omega_{m,m}$ 是块对角矩阵，所以它的逆也很好算：

$$
\Omega_{m, m}^{-1} = \sum_jF_j^T\Omega_{j,j}^{-1}F_j
$$

其中， $\Omega_{j,j} = F_j^T\Omega F_j$ 是地图中关联第 $j$ 个特征点的子矩阵，如下：

$$
F_j = \begin{bmatrix}
0\cdots0 & 1 \quad 0 & 0\cdots 0 \\
0\cdots0 & \underbrace{0 \quad 1}_{j\text{-th feature}} & 0\cdots 0
\end{bmatrix}
$$

通过这样，简化后的信息矩阵和信息向量可以表示为：

$$
\begin{aligned}
\tilde{\Omega} &= \Omega_{x_{0:t}, x_{0:t}} - \sum_j\Omega_{x_{0:t}, j}\Omega_{j, j}^{-1}\Omega_{j, x_{0:t}} \\
\tilde{\xi} &= \xi_{x_{0:t}} - \sum_j\Omega_{x_{0:t}, j}\Omega_{j, j}^{-1}\xi_j
\end{aligned}
$$

上式中， $\Omega_{x_{0:t}, j}$ 只在 $\tau(j)$ 中包含的位姿的位置的值不为 0，$\tau(j)$是观测到特征点的位姿集合。以上就是关于 GraphSLAM_reduce 的推导，这个边缘化的过程也可以理解成变量消除的过程。

求解机器人路径和地图

最后是求解机器人路径和地图。关于机器人位姿均值和方差的部分比较简单，如下：

$$
\begin{aligned}
\tilde{\Sigma} &= \tilde{\Omega}^{-1}\\
\tilde{\mu} &= \tilde{\Sigma}\tilde{\xi}
\end{aligned}
$$

剩下是求 $p(m | z_{1:t}, y_{1:t}, c_{1:t})$，这里要用到另一个引理：

通过这个引理，求解地图特征点均值和方差的公式如下：

$$
\begin{aligned}
\Sigma_m &= \Omega_{m,m}^{-1} \\
\mu_m &= \Sigma_m(\xi_m + \Omega_{m, x_{0:t}}\tilde{\xi})
\end{aligned}
$$

因为我们知道 $\Omega_{m,m}$ 是块对角矩阵，我们可以分解为：

$$
p(m | z_{1:t}, y_{1:t}, c_{1:t}) = \prod_jp(m_j | z_{1:t}, y_{1:t}, c_{1:t})
$$

其中，对每一个 $p(m_j | z_{1:t}, y_{1:t}, c_{1:t})$，

$$
\begin{aligned}
\Sigma_j &= \Omega_{j,j}^{-1} \\
\mu_j &= \Sigma_j(\xi_j + \Omega_{j, x_{0:t}}\tilde{\mu}) = \Sigma_j(\xi_j + \Omega_{j, \tau(j)}\tilde{\mu}_{\tau(j)})
\end{aligned}
$$

值得注意的是，这里算出来是特征点根据机器人位姿的条件概率。所以在算法中只返回了特征点的均值，没有方差（方差只有机器人位姿的部分）。

结语

目前关于 GraphSLAM 的大部分内容已经总结完了，但是还存在一些问题：目前我们假设观测到的特征点都能和地图中的特征点一一对应，但是实际中可能不会这样，那么我们怎么进行处理呢？下一篇将主要讲数据处理还有论文中的实验和最后的总结。