《视觉SLAM十四讲》-- 后端 1（上）

08 后端 1

前端视觉里程计可以给出一个短时间内的轨迹和地图，但由于不可避免的误差积累，地图在长时间内是不准确的，因此，我们希望构建一个大规模、长时间的最优轨迹和地图。

8.1 概述

8.1.1 状态估计的概率解释

（1）两种处理方式

• 批量式：使用过去和未来的信息来更新自己的状态的处理方式；

• 渐进式：仅适用过去的甚至仅是前一个时刻的信息来更新自己的状态的处理方式。

（2）运动方程和观测方程：

$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}k=1,\dots ,N,j=1,\dots ,M\text{\hspace{1em}(8-1)}$$

以下图为例，只有运动方程时，由于误差的不断积累，位置的不确定性会不断增大；当加入正确的观测数据，不确定性就会减小，直至保持稳定。

（3）将批量状态估计问题转化为最大似然估计问题，并使用最小二乘法求解。

定义 ${\mathbf{x}}_k$ 为 $k$ 时刻的所有变量，即包含此时刻的相机位姿和 $m$ 个路标，

x k =  def  { x k , y 1 , … , y m } \boldsymbol{x}_{k} \stackrel{\text { def }}{=}\left\{\boldsymbol{x}_{k}, \boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{m}\right\} xk​= def {xk​,y1​,…,ym​}

同时，把 $k$ 时刻所有的观测记做 ${\mathbf{z}}_k$，于是式（8-1）可写为（注意区别）

$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(8-2)}$$

我们希望用过去 0 到第 $k$ 时刻所有的数据来估计现在的状态分布：

$$P({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})$$

下标 $1:k$ 表示从 1 到 $k$ 时刻所有的数据。

根据贝叶斯法则

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\text{\hspace{1em}(8-3)}$$

第一项称为 似然，第二项为 先验，将第二项以 ${\mathbf{x}}_{k-1}$ 时刻为条件概率展开

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=\int P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\text{\hspace{1em}(8-4)}$$

（可以这样理解，第一项表示 $k$ 时刻状态和 $k-1$ 时刻有关，第二项表示 $k-1$ 时刻状态又与过去所有状态有关，这是一个递进的关系。）

在后续处理上，又有两种方式：一种是假设一阶马尔科夫性，即 $k$ 时刻的状态只与 $k-1$ 时刻状态相关，这样就会得到以 扩展卡尔曼滤波 为代表的滤波器法；另一种是与之前所有的状态均相关，将得到 非线性优化 为主体的优化框架。目前，视觉 SLAM 的主流为非线性优化方法。

8.1.2 线性系统和卡尔曼滤波（KF）

（1）假设马尔可夫性，即当前时刻状态只与前一时刻的状态有关。则式（8-4）第一项可写为

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{u}}_k)\text{\hspace{1em}(8-5)}$$

对于第二项，由于 $k$ 时刻的输入量 ${\mathbf{u}}_k$ 与 $k-1$ 时刻状态无关，则可将其化简为

$$P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{z}}_{1:k-1}\right)\text{\hspace{1em}(8-6)}$$

（2）首先推导 线性高斯系统 的卡尔曼滤波器（也就是说，运动方程和观测方程可以由线性方程来描述）：

$$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(8-8)}$$

假设噪声符合零均值高斯分布，即

$${\mathbf{w}}_k\sim N(0,\mathbf{R}).{\mathbf{v}}_k\sim N(0,\mathbf{Q})\text{\hspace{1em}(8-9)}$$

假设已知 $k-1$ 时刻的后验状态估计 ${\hat{\mathbf{x}}}_{k-1}$ 及其协方差 ${\hat{\mathbf{P}}}_{k-1}$，现在根据 $k$ 时刻的输入和观测数据，确定 ${\mathbf{x}}_k$ 的后验分布。我们约定以上帽子 ${\hat{\mathbf{x}}}_{k-1}$ 表示后验，下帽子 ${\check{\mathbf{x}}}_k$ 表示先验分布。

根据 高斯分布线性组合性质，先通过运动方程确定 ${\mathbf{x}}_k$ 的先验分布

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=N({\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R})\text{\hspace{1em}(8-10)}$$

这一步称为 预测 ，它显示了如何从上一时刻的状态，根据输入信息推断当前时刻的状态分布。这个分布就是 先验，记

$$\check{{\mathbf{x}}_k}={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R}\text{\hspace{1em}(8-11)}$$

由观测方程，我们可以计算在某个状态下应该产生怎样的观测数据，

$$P({\mathbf{z}}_k|{\mathbf{x}}_k)=N({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}))\text{\hspace{1em}(8-12)}$$

为了得到 ${\mathbf{x}}_k$ 的后验概率，我们需要计算它们的乘积，也就是式（8-3）。设最终的结果为 ${\mathbf{x}}_k\sim N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$ ，则

$$N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)=\eta N({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}))\cdot N({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k)\text{\hspace{1em}(8-13)}$$

我们知道高维高斯分布的概率密度函数为

$$p(x)=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$

因此，将指数部分展开

$$({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k{)}^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k)={\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)+{\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)\text{\hspace{1em}(8-14)}$$

为了求左侧的 ${\hat{\mathbf{x}}}_k$ 和 ${\hat{\mathbf{P}}}_k$ ，将两侧展开，并比较一次和二次项系数。对于二次系数

$${\hat{\mathbf{P}}}_k^{-1}={\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{C}}_k+{\check{\mathbf{P}}}_k^{-1}\text{\hspace{1em}(8-15)}$$

该式给出了协方差的计算过程。这里定义一个中间变量

$$\mathbf{K}={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}\text{\hspace{1em}(8-16)}$$

将式（8-15）两侧时左乘乘 ${\hat{\mathbf{P}}}_k$，得

$$\mathbf{I}={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{C}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}=\mathbf{K}{\mathbf{C}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}\text{\hspace{1em}(8-17)}$$

于是，得

$${\hat{\mathbf{P}}}_k=(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k){\check{\mathbf{P}}}_k\text{\hspace{1em}(8-18)}$$

然后比较一次项的系数

$$-2{\hat{\mathbf{x}}}_k^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k=-2{\mathbf{z}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{C}}_k{\mathbf{x}}_k-2{\check{\mathbf{x}}}_k^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k\text{\hspace{1em}(8-19)}$$

整理，得

$${\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k={\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{z}}_k+{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\text{\hspace{1em}(8-20)}$$

两侧同时左乘 ${\hat{\mathbf{P}}}_k$，并代入式（8-16）

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{z}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\\ & =\mathbf{K}{\mathbf{z}}_k+\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)\end{array}\text{\hspace{1em}(8-21)}$$

至此，我们得到了后验协方差 ${\hat{\mathbf{P}}}_k$（式（8-15））和均值 ${\hat{\mathbf{x}}}_k$ （式8-21）的表达式。

（3）线性卡尔曼滤波可归纳为 预测 和 更新 两个步骤：

——————————————————————————————————————————————————————————
① 预测：

$$\check{{\mathbf{x}}_k}={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R}$$

② 更新：先计算卡尔曼增益 $\mathbf{K}$（这与前面 定义的 $\mathbf{K}$ 形式有所差别，但实际上是等价的 ）

$$\mathbf{K}={\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{Q}}_k\right)}^{-1}$$

再计算后验概率分布

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\check{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)\\ {\hat{\mathbf{P}}}_k& =(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k){\check{\mathbf{P}}}_k\end{array}$$
——————————————————————————————————————————————————————————

这就是经典卡尔曼滤波中的五个公式。事实上，卡尔曼滤波还有其他的表达形式。

8.1.3 非线性系统和扩展卡尔曼滤波（EKF）

实际上，SLAM 中的运动方程和观测方程都是非线性的，而高斯分布经过非线性变换，其结果往往也不再是高斯分布。因此，在非线性系统中，必须取一定的近似，将非高斯分布近似为高斯分布。

（1）我们希望将上面的线性卡尔曼滤波器扩展到非线性系统中。即在某点附近，将运动方程和观测方程一阶泰勒展开，只保留一阶项（线性部分），然后按照线性系统进行推导。

假设 $k-1$ 时刻的均值和协方差矩阵为 ${\hat{\mathbf{x}}}_{k-1}$ 和 ${\hat{\mathbf{P}}}_{k-1}$，在 $k$ 时刻，把运动方程和观测方程在 ${\hat{\mathbf{x}}}_{k-1}$ 、 ${\hat{\mathbf{P}}}_{k-1}$ 处一阶展开，有

$${\mathbf{x}}_k\approx f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)+{\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k\text{\hspace{1em}(8-22)}$$

记

$$\mathbf{F}={\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}\text{\hspace{1em}(8-23)}$$

同样，对观测方程，在 ${\mathbf{x}}_k$ 处展开

$${\mathbf{z}}_k\approx h\left(\check{\mathbf{x}}\right)+{\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k\text{\hspace{1em}(8-24)}$$

记

$$\mathbf{H}={\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}\text{\hspace{1em}(8-25)}$$

类似式（8-10），根据运动方程得到 ${\mathbf{x}}_k$ 的先验分布

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{0:k-1}\right)=N(f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k)\text{\hspace{1em}(8-26)}$$

记先验的均值和协方差为

$${\check{\mathbf{x}}}_k=f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k),{\check{\mathbf{P}}}_k=\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k\text{\hspace{1em}(8-27)}$$

在观测中，有

$$P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)=N(h\left(\check{\mathbf{x}}\right)+\mathbf{H}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{Q}}_k)\text{\hspace{1em}(8-28)}$$

类似线性卡尔曼滤波，定义增益 ${\mathbf{K}}_k$

$${\mathbf{K}}_k=\check{{\mathbf{P}}_k}{\mathbf{H}}^{\mathrm{T}}(\mathbf{H}\check{{\mathbf{P}}_k}{\mathbf{H}}^{\mathrm{T}}+{\mathbf{Q}}_k){)}^{-1}\text{\hspace{1em}(8-29)}$$

那么，${\mathbf{x}}_k$ 的后验概率分布为

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k(z_k-h({\check{\mathbf{x}}}_k)),{\hat{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}){\check{\mathbf{P}}}_k\text{\hspace{1em}(8-29)}$$

8.1.4 小结

EKF 的优点：

• 推导简单，适用于各种形式传感器；

• 易做多传感器融合。

EKF 的缺点

• 一阶马尔科夫性过于简单；

• 可能会发散；

• 线性化误差；

• 从程序实现上来说，需要储存所有状态量的均值和方差，不适用于大型场景。