视觉SLAM笔记（4） SLAM的数学表述

---

1. 数学表述

通过前面部分的介绍，应该对 SLAM 中各个模块的组成和主要功能有了直观上的理解
但仅仅靠直观印象并不能写出可以运行的程序

要把它上升到理性层次——也就是用数学语言来描述 SLAM 过程
假设“小萝卜”正携带着某种传感器在未知环境里运动，怎么用数学语言描述这件事呢？

首先，由于相机通常是在某些时刻采集数据的，所以也只关心这些时刻的位置和地图
这就把一段连续时间的运动变成了离散时刻 t = 1,…,K 当中发生的事情
在这些时刻，用 x 表示“小萝卜”自身的位置
于是各时刻的位置就记为 $x$₁,…, $x$_K，它们构成了“小萝卜”的轨迹
地图方面，设地图是由许多个 路标（Landmark）组成的
而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据

---

2. 运动方程

不妨设路标点一共有 N 个，用 $y$₁,…, $y$_N 表示它们

在这样设定中，“小萝卜”携带着传感器在环境中运动，由如下两件事情描述：

1. 什么是运动？要考虑从 k − 1 时刻到 k 时刻，“小萝卜”的位置 x 是如何变化的
2. 什么是观测？假设“小萝卜”在 k 时刻，于 x_k 处探测到了某一个路标 y_j

通常，机器人会携带一个测量自身运动的传感器，比如说码盘或惯性传感器
这个传感器可以测量有关运动的读数，但不一定直接是位置之差，还可能是加速度、角速度等信息

然而，无论是什么传感器，都能使用一个通用的、抽象的数学模型： $x$_k = $f$ ( $x$_k−1, $u$_k, $w$_k )
这里 u_k 是运动传感器的读数（有时也叫输入）， $w$_k 为噪声

注意到，用一个一般函数 $f$ 来描述这个过程，而不具体指明 $f$ 的作用方式
这使得整个函数可以指代任意的运动传感器，成为一个通用的方程，而不必限定于某个特殊的传感器上
我们把它称为运动方程

---

3. 观测方程

与运动方程相对应，还有一个观测方程
观测方程描述的是，当“小萝卜”在 $x$_k 位置上看到某个路标点 $y$_j，产生了一个观测数据 $z$_k,j
同样，用一个抽象的函数 $h$ 来描述这个关系： $z$_k,j = $h$ ( $y$_j, $x$_k, $v$_k,j )
这里 $v$_k,j 是这次观测里的噪声

由于观测所用的传感器形式更多，这里的观测数据 $z$ 以及观测方程 $h$ 也许多不同的形式

---

4. 参数化

或许觉得用的函数 $f$ 和 $h$，似乎并没有具体地说明运动和观测是怎么回事？
同时，这里的 $x$, $y$, $z$ 又是什么东西呢？

事实上，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化方式（Parameterization）
举例来说，假设“小萝卜”在平面中运动
那么，它的 位姿（位置 & 姿态）由两个位置和一个转角来描述，即 $x$_k = [ $x$， $y$， $θ$]_k^T
同时，运动传感器能够测量到“小萝卜”在每两个时间间隔位置和转角的变化量 $u$_k = [∆ $x$，∆ $y$，∆ $θ$]_k^T
那么，此时运动方程就可以具体化为：
这是简单的线性关系

不过，并不是所有的传感器都直接能测量出位移和角度变化
所以也存在着其他形式更加复杂的运动方程，那时可能需要进行动力学分析
关于观测方程：
比方说“小萝卜”携带着一个二维激光传感器
当知道激光传感器观测一个 2D 路标点时，能够测到两个量：

• 路标点与小萝卜本体之间的距离 $r$
• 路标点与小萝卜本体之间的夹角 $ϕ$

记路标点为 $y$ = [ $p$_x， $p$_y ]^T（为保持简洁，省略了下标）
观测数据为 $z$ = [ $r$， $ϕ$]^T

那么观测方程就具体化为：

考虑视觉 SLAM 时，传感器是相机
那么观测方程就是“对路标点拍摄后，得到了图像中的像素”的过程
可见，针对不同的传感器，这两个方程有不同的参数化形式
如果保持通用性，把它们取成通用的抽象形式，那么 SLAM 过程可总结为两个基本方程：

这两个方程描述了最基本的 SLAM 问题：
当知道运动测量的读数 $u$，以及传感器的读数 $z$ 时
如何求解定位问题（估计 $x$）和建图问题（估计 $z$）？

---

5. 状态估计

这时，把 SLAM问题建模成了一个状态估计问题：
如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关
按照运动和观测方程是否为 线性，噪声是否服从 高斯分布 进行分类
分为 线性/非线性 和 高斯/非高斯 系统

其中线性高斯系统（Linear Gaussian, LG 系统）是最简单的
它的无偏的最优估计可以由卡尔曼滤波器（Kalman Filter, KF）给出

而在复杂的非线性非高斯系统（Non-Linear Non-Gaussian， NLNG 系统）中
会使用以扩展卡尔曼滤波器（Extended Kalman Filter, EKF）和非线性优化两大类方法去求解它
直至 21 世纪早期，以 EKF 为主的滤波器方法占据了 SLAM 中的主导地位

我们会在工作点处把系统线性化，并以预测——更新两大步骤进行求解
最早的实时视觉 SLAM 系统即是基于 EKF开发的
随后，为了克服 EKF 的缺点（例如线性化误差和噪声高斯分布假设）
人们开始使用粒子滤波器（Particle Filter）等其他滤波器，乃至使用非线性优化的方法

时至今日，主流视觉 SLAM 使用以 图优化（Graph Optimization）为代表的优化技术进行状态估计

认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（3） 视觉SLAM框架
视觉SLAM笔记（2） 相机
视觉SLAM笔记（1） 初识SLAM

---

谢谢！

转载：https://blog.csdn.net/qq_32618327/article/details/100978756