SLAM

Simultaneous Localization and Mapping 同步定位与地图构建

在不知道环境地图的情况下，同时估计自身的位置和构建地图 Map 本质上是一个 递归状态估计问题，可以抽象成概率图模型

在未知环境中，让机器人或自动驾驶汽车等智能体通过传感器数据实时地构建环境地图并同时定位自身位置的技术。

SLAM的基本知识

SLAM 两大任务：定位，建图

1. 运动估计（前端）：根据传感器信息估计机器人当前位姿。
2. 地图优化（后端）：构建地图，纠正漂移，保证全局一致性。

• 前端 (Frontend): 也称为“视觉里程计 (Visual Odometry)”或“激光里程计”。它负责处理实时的传感器数据，进行特征提取、数据关联，并估算短时间内的、局部的机器人运动。
• 后端 (Backend): 负责接收前端的信息，并进行全局的状态估计和优化。后端是实现高精度和全局一致性的关键，如图优化就是在后端完成的。

关键技术组件

模块	主要算法
前端里程计	ICP、scan matching、光流、特征点匹配
后端优化	非线性最小二乘法、图优化（g2o、Ceres Solver）
闭环检测	Bag-of-Words（DBoW2）、光流匹配、扫描匹配
地图表示	占据栅格地图、稀疏特征图、点云地图
传感器融合	卡尔曼滤波、因子图、滑窗优化

数学基础

1. 基本任务

假设机器人的状态为 $x_t$ ，控制输入为 $u_t$ ，观测为 $z_t$ ，地图为 $m$ 。 SLAM 的目标是求解状态估计问题：

$$p(x_{1:t},m\mid z_{1:t},u_{1:t})$$

即：在已知的控制输入和观测下，估计机器人运动路径 $x_{1:t}$ 和环境地图 $m$ 的后验概率。其中：

• $x_{1:t}$ ：机器人从时刻 1 到 t 的位姿轨迹
• $m$ ：环境地图
• $z_{1:t}$ ：从时刻 1 到 t 的传感器观测（激光、图像等）
• $u_{1:t}$ ：机器人控制输入（里程计、IMU 等）

经典贝叶斯递归公式

$$p(x_t,m\mid z_{1:t},u_{1:t})=\eta p(z_t\mid x_t,m)\int p(x_t\mid u_t,x_{t-1})p(x_{t-1},m\mid z_{1:t-1},u_{1:t-1})d{x}_{t-1}$$

• $p(z_t\mid x_t,m)$ ：观测模型
• $p(x_t\mid u_t,x_{t-1})$ ：运动模型
• $\eta$ ：归一化因子

2. 概率图基础

机器人运动遵循运动模型：

$$p(x_t|x_{t-1},u_t)$$

传感器观测遵循观测模型：

$$p(z_t|x_t,m)$$

整个 SLAM 问题的贝叶斯公式递归表示：

$$p(x_{1:t},m|z_{1:t},u_{1:t})\propto p(z_t|x_t,m)\cdot p(x_t|x_{t-1},u_t)\cdot p(x_{1:t-1},m|z_{1:t-1},u_{1:t-1})$$

SLAM 的信息流可以用图结构描述，被转化为一个图优化问题。

• 节点（Node） ：每个时刻的机器人位姿，或环境特征点。
• 边（Edge） ：节点之间的运动约束（里程计、IMU）或观测约束（激光扫描、视觉特征）。

3. 误差建模与优化目标

机器人控制输入存在误差，运动模型用高斯误差建模：

$$x_t=f(x_{t-1},u_t)+w_t,w_t\sim \mathcal{N}(0,Q_t)$$

传感器测量存在误差，观测模型用高斯误差建模：

$$z_t=h(x_t,m)+v_t,v_t\sim \mathcal{N}(0,R_t)$$

整个 SLAM 转化为最小化所有约束误差的非线性最小二乘问题：

$$\underset{\{x_i\},m}{\min }\sum _{(i,j)}\Vert z_{ij}-h(x_i,x_j){\Vert }_{{\Omega }_{ij}}^2$$

其中：

• ${\Omega }_{ij}$ ：信息矩阵，表示观测精度
• $z_{ij}$ ：观测数据
• $h(x_i,x_j)$ ：运动模型预测观测值

这个优化问题用以下方法求解求解：

• 滤波器（EKF、粒子滤波）
• 图优化（高斯-牛顿、Levenberg-Marquardt、g2o、Ceres）

一、按状态估计方法分类

1. 基于滤波 Filtering-based SLAM

早期的主流方法，将 SLAM 视为一个隐马尔可夫模型

用递归滤波（如扩展卡尔曼滤波 EKF 或粒子滤波）维护机器人状态和地图状态的联合概率分布，递推地估计机器人当前时刻的状态 代表算法：EKF-SLAM、FastSLAM、Particle Filter SLAM

扩展卡尔曼滤波 SLAM（EKF-SLAM）

• 假设状态和观测误差服从高斯分布，利用 EKF 递归估计状态和地图。
• 地图通常用地标（Landmarks）表示。
• 工作方式: 只维护当前时刻的状态估计，当新的数据到来时，用它来更新当前状态。它不保留所有历史信息。
• 特点: 计算效率高，内存占用小。但由于线性化近似和信息丢弃，其精度和长期一致性会受到影响。
• 优点： 理论成熟，实时性好。
• 缺点： 计算复杂度高（随地图规模增加而恶化），对非线性系统适用性有限。

基于粒子滤波的 SLAM：FastSLAM

• 将路径和地图分开估计，使用粒子滤波估计路径，条件独立地使用 EKF 估计地图。
• 优点： 适合大规模环境，复杂度较低。
• 缺点： 粒子数需要足够多以保证精度，粒子退化问题。

2. 基于图优化 Graph-Based SLAM

用图模型表示机器人的运动约束与观测约束，转化为非线性最小二乘优化问题。 代表算法：Graph-SLAM、G2O、Cartographer、slam_toolbox 常用优化器：g2o，Ceres Solver。

将机器人轨迹和地图中的约束建模成图结构，通过非线性优化求解最优解。

• 支持大规模地图，后端优化更准确。
• 可以进行全局一致性优化（闭环检测）。
• 后端优化计算较复杂（不过可以后台异步优化）。
• 工作方式: 将 SLAM 问题构建为一个图。图中的节点 (Nodes) 代表机器人经过的每一个历史位姿和环境中的路标点。图中的边 (Edges) 代表由运动模型或观测模型产生的约束（即误差）。SLAM 的目标就变成了调整图中所有节点的位置，使得整个图的总误差最小。
• 特点: 能够利用所有历史信息进行全局优化，因此精度更高、更鲁棒。但计算量和存储需求也更大。当检测到回环 (Loop Closure)（即机器人回到了一个曾经到过的地方）时，图优化方法可以非常有效地消除累积误差。

二、按传感器类型分类

1. 激光 LiDAR SLAM

代表算法：Gmapping、Hector SLAM、Cartographer、LIO-SAM 特点：

• 高精度、抗光照变化
• 对激光雷达质量敏感

2. 视觉 V-SLAM

代表算法：ORB-SLAM、DSO、LSD-SLAM、RTAB-Map

ORB-SLAM 系列（特征点法）

• 使用 ORB 特征进行前端匹配，后端图优化。
• 支持单目、双目

LSD-SLAM（半稠密直接法） 不依赖特征点，直接利用图像像素灰度进行优化。

RTAB-Map RGB-D SLAM

3. 多传感器融合 SLAM

代表算法：VINS-Mono、LIO-SAM、Cartographer 3D 特点：

• 融合 IMU、激光、视觉、GPS，提高鲁棒性和精度
• 适用于动态、复杂环境

三、按构建地图类型分类

1. 稠密建图（Dense Mapping）：完整还原环境，适用于三维重建
2. 稀疏建图（Sparse Mapping）：只记录特征点与轨迹，适合导航定位
3. 拓扑建图（Topological Mapping）：使用图结构表示节点和连边，不保存具体几何信息

四、发展趋势与实际使用

1. 发展趋势

多传感器融合（激光、视觉、IMU、GPS） 高精度 3D 稠密建图（自动驾驶、AR/VR） 实时闭环检测与自适应图优化 基于深度学习的 SLAM（特征提取、数据关联） 端到端学习型 SLAM（研究阶段）

2. 实际使用

ROS2 slam_toolbox

开源库