SLAM

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

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

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

SLAM的基本知识

SLAM 两大任务：定位，建图

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

关键技术组件

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

数学基础

1. 基本任务

假设机器人的状态为 $x_{t}$ ，控制输入为 $u_{t}$ ，观测为 $z_{t}$ ，地图为 $m$ 。
SLAM 的目标是求解状态估计问题：

p (x_{1 : t}, m ∣ 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 ∣ z_{1 : t}, u_{1 : t}) = η p (z_{t} ∣ x_{t}, m) \int p (x_{t} ∣ u_{t}, x_{t - 1}) p (x_{t - 1}, m ∣ z_{1 : t - 1}, u_{1 : t - 1}) d x_{t - 1}

$p (z_{t} ∣ x_{t}, m)$ ：观测模型
$p (x_{t} ∣ u_{t}, x_{t - 1})$ ：运动模型
$η$ ：归一化因子

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）或观测约束（激光扫描、视觉特征）。
SLAM 问题被转化为一个图优化问题。

3. 误差建模与优化目标

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

x_{t} = f (x_{t - 1}, u_{t}) + w_{t}, w_{t} \sim N (0, Q_{t})

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

z_{t} = h (x_{t}, m) + v_{t}, v_{t} \sim N (0, R_{t})

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

min_{{x_{i}}, m} \sum_{(i, j)} ∥ z_{i j} - h (x_{i}, x_{j}) ∥_{Ω_{i j}}^{2}

其中：

$Ω_{i j}$ ：信息矩阵，表示观测精度
$z_{i j}$ ：观测数据
$h (x_{i}, x_{j})$ ：运动模型预测观测值

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

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

一、按状态估计方法分类

1. 基于滤波

用递归滤波（如扩展卡尔曼滤波 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。

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

支持大规模地图，后端优化更准确。
可以进行全局一致性优化（闭环检测）。
后端优化计算较复杂（不过可以后台异步优化）。

二、按传感器类型分类

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，提高鲁棒性和精度
适用于动态、复杂环境

三、按构建地图类型分类

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

四、发展趋势与实际使用

1. 发展趋势

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

2. 实际使用

ROS2 slam_toolbox