本文及其所属的专栏是笔者阅读学习高翔博士视觉 SLAM 十四讲的产物，看书跑代码总是匆匆而过，写点笔记加深印象。本来的打算是按书的节奏依次为每章做总结，但最后觉得这样效率太低且不够连贯，还是凭自己的思路写下去。观前提示，这一部分偏向理论，不可避免地文字会比较多

0x00 To begin with

这篇文章将会包含以下内容：

  • 什么是 SLAM 问题
  • SLAM 问题的数学模型

For reference：

0x01 何为 SLAM？ What is SLAM

认识任何一个事物或者技术总是可以从两个角度出发：它是什么以及它能做什么？

SLAM，全称为 Simultaneous Localization and Mapping，翻译过来就是同步定位与建图。它要求携带传感器的主体（例如运动机器人），在环境信息未知的情况下，通过自身在环境中的运动同步估计运动过程并建立环境的模型。这样来看，SLAM 的核心问题有二：

  • 定位（Localization）：携带传感器的主体需要通过传感器感知环境信息判断自身的状态（位姿）；
  • 建图（Mapping）：主体基于自身连续轨迹上的观测数据，逐步建立环境地图；

此外，这两个问题间还有一个实时性的要求，即 Simultaneous（同步），同步地完成定位与建图，而非先进行信息采集再将数据传送回去做全局的估计和优化（那样耗时和局限就比较大，也就没意义了）。

在 SLAM 问题中，传感器作为运动主体感知环境的唯一途径，承担了从环境中获取观测数据的作用。传感器的不同意味着观测数据不同，也将 SLAM 问题分为不同类别，常见的如以相机为主要传感器的视觉 SLAM（Visual SLAM）和以激光雷达为主要传感器的激光 SLAM（LiDAR SLAM）等。在视觉 SLAM 的背景下，SLAM 问题就变成了从一系列连续图像中估计相机位姿，并基于图像特征等信息构建环境地图的过程。

对人类来说，这个过程是很自然的：置身于陌生环境中时，我们可以很容易地四处观察，理解环境的构成，然后在脑中建立环境的概念模型。但对计算机来说则不然，单是提取图像中蕴含的丰富语义信息这一任务就极具挑战，尤其考虑到图像在计算机中仅仅是一系列数值矩阵。幸运的是，随着数十年来的研究进展，SLAM 技术已渐趋成熟：一方面，依靠人工智能的机器学习、深度学习中数值驱动的方法，计算机能够从图像中识别特征、分辨物体；另一方面，硬件的进步带来了更为先进的传感器，例如能够直接估计场景深度的 RGB-D 相机，和高精度的 3D 激光雷达。这些先进的传感器提供了更为丰富、精确的环境信息，为在复杂环境中的定位和建图提高了准确性和鲁棒性。

SLAM 技术最早可以追溯到上世纪 90 年代，为机器人学的一个概念，随着 30 多年的发展，演变为机器人学和计算机视觉中的热门研究领域。如今的 SLAM 技术早已迈出大学的实验室，走向了工业界的实际应用：通过 SLAM 技术构建的环境地图，可以支撑很多的下游任务，例如三维重建、路径规划、自动驾驶、场景标注与理解、VR & AR 等等。下至家中的扫地机器人，上至太空中的天体探测车，背后都有 SLAM 的身影。

0x02 问题建立 Problem Setup

高翔博士的 SLAM 十四讲是以视觉 SLAM 为前提的，因此这里的问题模型也是基于视觉 SLAM

要让机器人能够解决上述提及的两个核心问题，它需要首先知道一些信息：

  • 自身的初始状态与随时间的运动过程
  • 一些时刻一些位置的某些观测信息

前者可以通过设定的初始值和运动传感器每个时刻的读数估计，后者可以通过相机传感器的观测数据获得。现在假设机器人在一个未知环境中运动，相机在离散时刻 $t=1,\dots,K\in \mathcal{T}$ 采集环境数据，用 $\boldsymbol{x}$ 表示机器人的位姿，那么这些时刻的位姿为 $\boldsymbol{x}_{1},\dots,\boldsymbol{x}_{K}$。运动传感器在这些时刻会产生输入数据 $\boldsymbol{u}_{1},\dots \boldsymbol{u}_{K}$，相机传感器在这些时刻也可能观测到某些路标（视觉 SLAM 中可以理解为图像的特征点，假设一共有 $N$ 个路标 $\boldsymbol{y}_{1},\dots,\boldsymbol{y}_{N}$）。为了更好地描述这两个过程，我们可以将它们抽象地建模为两个方程：

$$ \begin{cases} \boldsymbol{x}_{k}=f(\boldsymbol{x}_{k-1},\boldsymbol{u}_{k})+\boldsymbol{w}_{k}& k=1,\dots,K \\ \boldsymbol{z}_{k,j}=h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})+\boldsymbol{v}_{k,j}& (k,j)\in \mathcal{O} \end{cases} \tag{2.1} $$

第一个方程中，$\boldsymbol{x}_{k}$ 为 $k$ 时刻的位姿，$\boldsymbol{u}_{k}$ 为 $k$ 时刻的运动传感器输入，$\boldsymbol{w}_{k}$ 为 $k$ 时刻的噪声（传感器不可能完美地反馈真实的运动情况），$f$ 为描述从 $\boldsymbol{x}_{k-1},\boldsymbol{u}_{k},\boldsymbol{w}_{k}$ 中得到 $\boldsymbol{x}_{k}$ 的一般函数；第二个方程中，$\boldsymbol{z}_{k,j}$ 表示视觉传感器在 $k$ 时刻观测到了路标点 $\boldsymbol{y}_{j}$ 产生的观测数据，$\mathcal{O}$ 是一个集合，描述在哪些时刻观测到了哪些数据，$\boldsymbol{v}_{k,j},h$ 同理为该过程中的噪声和函数。至于为何不解析地表达这些函数，是因为不同的传感器产生的数据形式不同，从而有不同的参数化方式，这里使用一个抽象的表达，可以保持通用性~~（多态？）~~。

有了这两个方程，我们可以将 SLAM 问题描述为已知运动传感器读数 $\boldsymbol{u}$ 和观测数据 $\boldsymbol{z}$ 时，估计 $\boldsymbol{x}$ 和 $\boldsymbol{y}$ 的条件分布——这是一个状态估计问题：

$$ P(\boldsymbol{x},\boldsymbol{y}|\boldsymbol{z},\boldsymbol{u}) \tag{2.2} $$

它是一个后验概率，描述在当前所有可用信息下，对轨迹和地图的最佳一致性估计。直接求解后验概率是分布的，状态空间的维度太高，但利用贝叶斯法则，可以将其转化为一个正比于似然和先验概率的乘积的分布：

$$ P(\boldsymbol{x},\boldsymbol{y}|\boldsymbol{z},\boldsymbol{u})=\frac{P(\boldsymbol{z},\boldsymbol{u}|\boldsymbol{x},\boldsymbol{y})P(\boldsymbol{x},\boldsymbol{y})}{P(\boldsymbol{z},\boldsymbol{u})}\propto \underbrace{P(\boldsymbol{z},\boldsymbol{u}|\boldsymbol{x},\boldsymbol{y})}_{\text{Likehood}}\,\underbrace{P(\boldsymbol{x},\boldsymbol{y})}_{\text{Prior}}\tag{2.3} $$

其中似然描述了在给定的位姿下可能产生怎样的观测数据，先验概率描述了观测前对机器人位姿和场景的了解。分母 $P(\boldsymbol{z},\boldsymbol{u})$ 是归一化因子，用于保证概率分布和为 1，这里和估计变量无关，所以可以忽略。通过贝叶斯法则，我们不直接求后验分布，而是退而求其次，找一个最可能的 $\boldsymbol{x},\boldsymbol{y}$ 状态代表解，也就是做最大后验估计（Maximum A Posteriori Estimation，MAP）：

$$ (\boldsymbol{x},\boldsymbol{y})^{\ast}_{\text{MAP}}=\arg \max P(\boldsymbol{x},\boldsymbol{y}|\boldsymbol{z},\boldsymbol{u})=\underbrace{P(\boldsymbol{z},\boldsymbol{u}|\boldsymbol{x},\boldsymbol{y})}_{\text{Likehood}}\,\underbrace{P(\boldsymbol{x},\boldsymbol{y})}_{\text{Prior}} \tag{2.4} $$

在未知环境中，通常也没有 $\boldsymbol{x},\boldsymbol{y}$ 的全局先验信息，这样最后求解状态估计问题就可以转化为做最大似然估计（Maximum Likelihood Estimation，MLE）：

$$ (\boldsymbol{x},\boldsymbol{y})^{\ast}_{\text{MLE}}=\arg \max P(\boldsymbol{z},\boldsymbol{u}|\boldsymbol{x},\boldsymbol{y}) \tag{2.5} $$

也就是 $\boldsymbol{x},\boldsymbol{y}$ 在怎样的状态下最容易产生现有的观测数据。

对于这个联合分布似然，假设给定轨迹 $\boldsymbol{x}$，运动输入 $\boldsymbol{u}$ 独立于 $\boldsymbol{z},\boldsymbol{y}$，我们可以将其拆分为观测模型和运动模型：

$$ P(\boldsymbol{z},\boldsymbol{u}|\boldsymbol{x},\boldsymbol{y})=P(\boldsymbol{z}|\boldsymbol{x},\boldsymbol{y})P(\boldsymbol{u}|\boldsymbol{z},\boldsymbol{x},\boldsymbol{y})=\underbrace{P(\boldsymbol{z}|\boldsymbol{x},\boldsymbol{y})}_{\text{Observation}}\,\underbrace{P(\boldsymbol{u}|\boldsymbol{x})}_{\text{Action}}\tag{2.6} $$

进一步假设各时刻的运动输入间、各个观测间是独立的，那么可以将两个模型分解为各个观测和输入分布的乘积：

$$ P(\boldsymbol{z}|\boldsymbol{x},\boldsymbol{y})P(\boldsymbol{u}|\boldsymbol{x})=\prod_{k,j}P(\boldsymbol{z}_{k,j}|\boldsymbol{x}_{k},\boldsymbol{y}_{j})\prod_{k}P(\boldsymbol{u}_{k}|\boldsymbol{x}_{k-1},\boldsymbol{x}_{k})\tag{2.7} $$

回顾式 $(2.5)$，最大化似然就相当于最大化上式的乘积，实践中通常假设各个分布为高斯分布，然后使用最小化负对数似然（Minimize Negative Log-Likelihood）来求最大似然。

对于某个观测数据的观测方程 $\boldsymbol{z}_{k,j}=h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})+\boldsymbol{v}_{k,j}$，假设噪声项服从一个高斯分布 $\boldsymbol{v}_{k,j}\sim \mathcal{N}(0,\boldsymbol{Q}_{k,j})$，那么观测数据条件概率为：

$$ P(\boldsymbol{z}_{k,j}|\boldsymbol{x}_{k},\boldsymbol{y}_{j})=N(h(\boldsymbol{y}_{j},\boldsymbol{x}_{k}),\boldsymbol{Q}_{k,j})\tag{2.8} $$

给定 $\boldsymbol{x}_{k},\boldsymbol{y}_{j}$，理想观测数据为 $h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})$，但由于噪声存在，真实观测的条件概率也为一个以 $h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})$ 为均值，$\boldsymbol{Q}_{k,j}$ 为协方差的高斯分布。同理假设运动数据的噪声项 $\boldsymbol{w}_{k}\sim \mathcal{N}(0,\boldsymbol{R}_{k})$，那么运动数据也服从高斯分布，其条件概率为：

$$ P(\boldsymbol{u}_{k}|\boldsymbol{x}_{k-1},\boldsymbol{x}_{k})=N(g(\boldsymbol{x}_{k-1},\boldsymbol{x}_{k}),\boldsymbol{R}_{k})\tag{2.9} $$

高斯分布的概率密度函数为：

$$ P(x)=\frac{1}{\sqrt{ (2\pi)^N \det(\Sigma)}}\exp\left( -\frac{1}{2}(x-\mu)^\top \Sigma^{-1}(x-\mu) \right)\tag{2.10} $$

取负对数：

$$ -\ln(P(x))=\frac{1}{2}\ln \left((2\pi)^N\det(\Sigma)\right)+\frac{1}{2}(x-\mu)^\top \Sigma^{-1}(x-\mu)\tag{2.11} $$

我们想要最小化上式，而第一项（其中 $N$ 为 $x$ 的维度）与变量无关，可以忽略，所以最终只要最小化右侧的二次型：

$$ x^{\ast}_{\text{MLE}}=\arg \max P(x)=\arg \min (x-\mu)^\top \Sigma^{-1}(x-\mu)\tag{2.11} $$

最后代入 SLAM 的观测模型和运动模型，定义每个估计的观测数据和运动数据与真实结果（Ground Truth）的残差项：

$$ \begin{aligned} \boldsymbol{e}_{\boldsymbol{z},j,k}&=\boldsymbol{z}_{k,j}-h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})\\ \boldsymbol{e}_{\boldsymbol{u},k}&=\boldsymbol{x}_{k}-f(\boldsymbol{x}_{k-1},\boldsymbol{u}_{k}) \end{aligned}\tag{2.12} $$

代入式 $(2.11)$，得到一个数据的最优估计，结合式 $(2.7)$，由于对数的性质将乘积转为求和：

$$ \min J(\boldsymbol{x},\boldsymbol{y})=\sum_{k}\sum_{j}\boldsymbol{e}_{\boldsymbol{z},j,k}^\top\boldsymbol{Q}_{k,j}^{-1}\boldsymbol{e}_{\boldsymbol{z},j,k}+\sum_{k}\boldsymbol{e}_{\boldsymbol{u},k}^\top\boldsymbol{R}_{k}^{-1}\boldsymbol{e}_{\boldsymbol{u},k}\tag{2.13} $$

可以看到，最终得到了一个最小二乘问题。该问题的目标函数是由多个误差项的加权求和组成，虽然整体状态空间的维度很高，但对于每一个残差项来说是简单的，仅与少量有限个状态变量有关，这使得求解具备了稀疏性，可以使用稀疏优化方法高效求解。

0x03 Afterwords

本篇文章中笔者仅将 SLAM 问题使用一个抽象的数学模型描述，而没有讨论具体的一些实现框架和算法细节，本篇也旨在让读者对 SLAM 有大致的了解，至少明白它是什么、能解决什么问题、有怎样的应用领域。在之后的文章中，会逐步介绍 SLAM 主流求解框架以及它的各个模块的主要算法。

Enjoy it!