01 概述与预备知识

1.1 SLAM 是什么

1.1.1 基本概念

（1）SLAM 是 Simultaneous Localization and Mapping 的缩写，即同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型。

（2）两大基本问题：

• 定位：我在什么地方？

• 建图：周围环境是什么样的？

（3）准确的定位需要精确的地图，精确的地图来自准确的定位。

（4）传感器

• 内质的（放置在机器人身上，用于感受机器人本体信息）：IMU（惯性测量单元）、激光、相机

• 外质的（安装于环境中，监测机器人的运动状态）：二维码、GPS导轨

（5）相机

以一定速率采集图像，形成视频。相较于激光传感器更便宜，信息更丰富。

相机以二维投影形式记录三维世界的信息，丢掉了距离信息。

分类：

• 单目相机：最常见的普通相机

• 双目相机（立体相机）：类似于人眼，通过视差计算出距离信息

• 深度相机（RGB-D）：发射端、接收端，获得RGB图和深度图

• 其他：全景相机、事件相机

1.1.2 视觉 SLAM 框架

• 前端：Visual Odometry（VO），视觉里程计的任务是估算邻近时刻的相机运动。主要方法：特征点法、直接法

• 后端：Optimization，从带有噪声的数据中估计最优轨迹与地图。常用方法：最大后验估计、滤波器、图优化。

• 回环检测：Loop Closing，主要解决位置估计随时间漂移的问题。判断机器人是否到达过先前的位置，如果检测到回环，将信息提供给后端进行处理。

• 建图：Mapping，根据估计的轨迹，建立地图。主要分为度量地图和拓扑地图两类。度量地图又分为稀疏地图和稠密地图，稀疏地图比较抽象，仅包含一部分有意义的信息——地标。进行导航时，就需要稠密地图（避免碰撞某些障碍物）。

1.1.3 SLAM 问题的数学表述

（1）数学表述

两个方程：运动方程和观测方程

$$\{\begin{array}{c}{\mathbf{x}}_{\mathbf{k}}=f({\mathbf{x}}_{\mathbf{k}-1},{\mathbf{u}}_{\mathbf{k}},{\mathbf{w}}_{\mathbf{k}})\\ {\mathbf{z}}_{\mathbf{k},\mathbf{j}}=h({\mathbf{y}}_{\mathbf{j}},{\mathbf{x}}_{\mathbf{k}},{\mathbf{v}}_{\mathbf{k},\mathbf{j}})\end{array}\text{\hspace{1em}(1-1)}$$

这里，${\mathbf{x}}_{\mathbf{k}}$ 为 $k$ 时刻的位置，${\mathbf{u}}_{\mathbf{k}}$ 是运动传感器的读数或输入，比如相对上一位置前进1 m 或左转 10°；${\mathbf{w}}_{\mathbf{k}}$ 为噪声。${\mathbf{z}}_{\mathbf{k},\mathbf{j}}$ 为机器人在 ${\mathbf{x}}_{\mathbf{k}}$ 位置时对路标 ${\mathbf{y}}_{\mathbf{j}}$ 的观测数据，${\mathbf{v}}_{\mathbf{k},\mathbf{j}}$ 为此次观测的噪声。

对于简单的平面运动，机器人的位姿由横纵坐标和转角组成 ${\mathbf{x}}_{\mathbf{k}}=[x_1,x_2,\theta {]}^T$，变化量 ${\mathbf{u}}_{\mathbf{k}}=[\Delta x_1,\Delta x_2,\Delta \theta {]}^T$，则运动方程可简化为

$${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_k={\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_{k-1}+{\left[\begin{array}{c}\Delta x_1\\ \Delta x_2\\ \Delta \theta \end{array}\right]}_k+{\mathbf{w}}_{\mathbf{k}}\text{\hspace{1em}(1-2)}$$

对于观测方程，以激光传感器为例，可以测量出此位置和路标之间的距离$r$ 和夹角 $\varphi$，路标点位置记为 ${\mathbf{y}}_{\mathbf{j}}=[y_1,y_2{]}^{T_j}$，位姿为 ${\mathbf{x}}_{\mathbf{k}}=[x_1,x_2{]}^{T_j}$ ，观测数据为 ${\mathbf{z}}_{\mathbf{k},\mathbf{j}}=[r_{k,j},{\varphi }_{k,j}{]}^T$，在笛卡尔坐标系中，可以写为

$$\left[\begin{array}{c}{r}_{k,j}\\ {\varphi }_{k,j}\end{array}\right]=\left[\begin{array}{c}\sqrt{(y_{1,j}-x_{1,k}{)}^2+(y_{2,j}-x_{2,k}{)}^2}\\ arctan\frac{(y_{2,j}-x_{2,k})}{(y_{1,j}-x_{1,k})}\end{array}\right]+\mathbf{v}\text{\hspace{1em}(1-3)}$$

当然，对于三维空间中的运动或其他形式传感器，上式有不同的参数化形式。

（2）SLAM 是一个状态估计问题

按运动和观测方程是否为线性，噪声是否符合高斯分布，分为线性/非线性和高斯/非高斯系统。

• 线性高斯系统： 卡尔曼滤波

• 非线性非高斯系统： 扩展卡尔曼滤波、非线性优化

1.2 实践：编程基基础

• Ubuntu 系统
• C++
• 编辑器：Clion、VS Code
• 工具：cmake
• 库：Eigen、G2O、Ceres、OpenCV等

1.3 课后习题