SLAM：同时定位与建图

概述

SLAM（Simultaneous Localization and Mapping）是机器人在未知环境中同时估计自身位姿和构建环境地图的问题。它是移动机器人自主导航的核心技术。

SLAM 问题的概率表述：

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

其中：

符号	含义
\(x_{1:t}\)	从时刻 1 到 \(t\) 的机器人位姿序列
\(m\)	环境地图
\(z_{1:t}\)	传感器观测序列
\(u_{1:t}\)	控制输入（里程计）序列

graph LR
    A["传感器数据<br/>相机/LiDAR/IMU"] --> B["前端<br/>数据关联/特征提取"]
    B --> C["状态估计<br/>滤波/优化"]
    C --> D["后端<br/>回环检测/全局优化"]
    D --> E["输出<br/>位姿 + 地图"]
    D -->|"回环约束"| C
    style B fill:#e8f4fd,stroke:#2196F3
    style C fill:#fff3e0,stroke:#FF9800
    style D fill:#f3e5f5,stroke:#9C27B0

1 SLAM 的数学基础

1.1 运动模型

\[ x_t = f(x_{t-1}, u_t) + w_t, \quad w_t \sim \mathcal{N}(0, R_t) \]

1.2 观测模型

\[ z_t = h(x_t, m) + v_t, \quad v_t \sim \mathcal{N}(0, Q_t) \]

1.3 SLAM 的两种形式

形式	估计量	方法
Online SLAM	\(p(x_t, m \mid z_{1:t}, u_{1:t})\)	EKF-SLAM, 粒子滤波
Full SLAM	\(p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\)	图优化

2 基于滤波的 SLAM

2.1 EKF-SLAM

EKF-SLAM 是最经典的 SLAM 方法，维护一个包含机器人位姿和所有路标位置的联合高斯分布。

状态向量：

\[ \hat{\mu}_t = \begin{bmatrix} x_t \\ m_1 \\ m_2 \\ \vdots \\ m_N \end{bmatrix}, \quad \Sigma_t = \begin{bmatrix} \Sigma_{xx} & \Sigma_{xm_1} & \cdots & \Sigma_{xm_N} \\ \Sigma_{m_1 x} & \Sigma_{m_1 m_1} & \cdots & \Sigma_{m_1 m_N} \\ \vdots & & \ddots & \vdots \\ \Sigma_{m_N x} & & \cdots & \Sigma_{m_N m_N} \end{bmatrix} \]

预测步（运动更新）：

\[ \bar{\mu}_t = f(\hat{\mu}_{t-1}, u_t) \]

\[ \bar{\Sigma}_t = F_t \Sigma_{t-1} F_t^T + R_t \]

更新步（观测更新）：

\[ K_t = \bar{\Sigma}_t H_t^T (H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1} \]

\[ \hat{\mu}_t = \bar{\mu}_t + K_t (z_t - h(\bar{\mu}_t)) \]

\[ \Sigma_t = (I - K_t H_t) \bar{\Sigma}_t \]

其中 \(F_t = \frac{\partial f}{\partial x}\big|_{\hat{\mu}_{t-1}}\)，\(H_t = \frac{\partial h}{\partial x}\big|_{\bar{\mu}_t}\) 为雅可比矩阵。

EKF-SLAM 的局限性：

计算复杂度 \(O(N^2)\)（\(N\) 为路标数），不适合大规模环境
线性化误差导致不一致
数据关联错误难以恢复

2.2 粒子滤波 SLAM（FastSLAM）

FastSLAM（Montemerlo et al., 2002）利用 Rao-Blackwellized 粒子滤波，将 SLAM 问题分解为：

\[ p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) = p(x_{1:t} \mid z_{1:t}, u_{1:t}) \prod_{j=1}^{N} p(m_j \mid x_{1:t}, z_{1:t}) \]

用粒子表示路径后验 \(p(x_{1:t} \mid \cdot)\)
每个粒子维护独立的地图（EKF 更新每个路标）

优势：

计算复杂度 \(O(M \log N)\)（\(M\) 粒子数，\(N\) 路标数）
天然处理多模态分布
数据关联可以每个粒子独立处理

3 视觉 SLAM

3.1 概述

视觉 SLAM 使用相机作为主传感器，分为：

特征法：提取特征点，基于几何约束估计位姿
直接法：直接利用像素灰度值优化位姿
半直接法：结合两者优点（如 SVO）

3.2 ORB-SLAM3 系统

ORB-SLAM3（Campos et al., 2021）是目前最完整的开源视觉 SLAM 系统，支持单目、双目、RGB-D 和视觉惯性模式。

系统流程：

graph TB
    A["图像输入"] --> B["ORB 特征提取"]
    B --> C["特征匹配"]
    C --> D["跟踪线程<br/>（帧间位姿估计）"]
    D --> E["关键帧决策"]
    E -->|"是关键帧"| F["局部建图线程<br/>（局部 BA）"]
    F --> G["回环检测线程<br/>（DBoW2）"]
    G -->|"检测到回环"| H["位姿图优化"]
    H --> I["全局 BA"]
    D --> J["位姿输出"]
    F --> K["地图点云"]
    style D fill:#e8f4fd,stroke:#2196F3
    style F fill:#fff3e0,stroke:#FF9800
    style G fill:#f3e5f5,stroke:#9C27B0

核心模块：

1. 特征提取

使用 ORB（Oriented FAST and Rotated BRIEF）特征：

FAST 角点检测 + 方向计算
rBRIEF 描述子（256-bit 二进制）
图像金字塔实现尺度不变性
提取速度快，匹配使用汉明距离

2. 跟踪（Tracking）

恒速模型预测初始位姿
特征匹配 + PnP 求解
局部地图跟踪：与局部地图点匹配，优化当前位姿
关键帧选择策略

3. 局部建图（Local Mapping）

新关键帧插入
地图点三角化
局部 Bundle Adjustment

\[ \min_{\{T_i\}, \{p_j\}} \sum_{i,j} \rho\left(\|z_{ij} - \pi(T_i, p_j)\|^2_{\Sigma_{ij}}\right) \]

其中 \(\pi\) 为投影函数，\(\rho\) 为鲁棒核函数（如 Huber）。

4. 回环检测（Loop Closing）

基于词袋模型（DBoW2）的图像相似度检索
几何验证（RANSAC + Sim(3)/SE(3)）
位姿图优化（修正累积漂移）
可选的全局 BA

3.3 视觉惯性 SLAM（VIO）

融合 IMU 预积分：

\[ \Delta R_{ij} = \prod_{k=i}^{j-1} \text{Exp}((\omega_k - b_g) \Delta t) \]

\[ \Delta v_{ij} = \sum_{k=i}^{j-1} \Delta R_{ik} (a_k - b_a) \Delta t \]

\[ \Delta p_{ij} = \sum_{k=i}^{j-1} \Delta v_{ik} \Delta t + \frac{1}{2} \Delta R_{ik} (a_k - b_a) \Delta t^2 \]

VIO 系统能在纹理贫乏和快速运动场景下保持鲁棒性。

4 LiDAR SLAM

4.1 概述

LiDAR SLAM 使用激光雷达获取精确的 3D 点云（参见传感器），适合大规模室外环境。

4.2 LOAM（Lidar Odometry and Mapping）

LOAM（Zhang & Singh, 2014）是影响力最大的 LiDAR SLAM 算法之一。

核心思想：将问题分为高频里程计和低频建图两个模块。

特征提取：

边缘点（Edge points）：曲率大的点
平面点（Planar points）：曲率小的点

点到边缘/平面距离最小化：

边缘点残差：

\[ d_e = \frac{|(p_i - p_a) \times (p_i - p_b)|}{|p_a - p_b|} \]

平面点残差：

\[ d_p = \frac{(p_i - p_a) \cdot ((p_a - p_b) \times (p_a - p_c))}{|(p_a - p_b) \times (p_a - p_c)|} \]

4.3 Cartographer

Google Cartographer（Hess et al., 2016）是一个广泛使用的 2D/3D SLAM 系统。

前端：

Submap 构建：使用 scan matching 将新扫描插入子图
局部优化：Ceres Solver 求解扫描匹配

后端：

回环检测：分支定界搜索
位姿图优化：Sparse Pose Adjustment (SPA)

4.4 其他 LiDAR SLAM 系统

系统	特点
LeGO-LOAM	地面分割 + 轻量级 LOAM
LIO-SAM	紧耦合 LiDAR-IMU，因子图优化
FAST-LIO2	紧耦合，iKD-tree 加速，高效
CT-ICP	连续时间 ICP，处理运动畸变

5 图优化 SLAM

5.1 位姿图优化

将 SLAM 建模为图优化问题：

节点：机器人位姿 \(x_i\)
边：位姿间的相对约束 \(z_{ij}\)（里程计、回环检测）

优化目标：

\[ x^* = \arg\min_x \sum_{(i,j) \in \mathcal{E}} \|e_{ij}(x_i, x_j)\|^2_{\Omega_{ij}} \]

其中误差函数：

\[ e_{ij} = z_{ij}^{-1} \oplus (x_i^{-1} \oplus x_j) \]

\(\Omega_{ij}\) 为信息矩阵（协方差的逆）。

5.2 因子图

因子图是一种更通用的图模型表示：

变量节点：待估计的状态（位姿、路标、传感器偏置等）
因子节点：约束（先验、里程计、观测、回环）

\[ p(X) \propto \prod_k f_k(X_k) \]

常见因子类型：

因子	连接变量	信息来源
先验因子	\(x_0\)	初始位姿
里程计因子	\(x_{t-1}, x_t\)	轮式/视觉/惯性里程计
路标观测因子	\(x_t, l_j\)	相机/LiDAR 观测
回环因子	\(x_i, x_j\)	回环检测
IMU 预积分因子	\(x_i, x_j, v_i, v_j, b_i\)	IMU 数据
GPS 因子	\(x_t\)	GPS 定位

5.3 求解方法

图优化问题最终转化为非线性最小二乘：

\[ \min_x \sum_k \|r_k(x)\|^2_{\Sigma_k} \]

使用 Gauss-Newton 或 Levenberg-Marquardt 迭代求解：

\[ (J^T \Sigma^{-1} J) \Delta x = -J^T \Sigma^{-1} r \]

稀疏性

SLAM 问题的信息矩阵 \(H = J^T \Sigma^{-1} J\) 是稀疏的（因为只有相邻/共视关系的变量之间有约束）。利用稀疏 Cholesky 分解可以高效求解。

常用优化库：

g2o：通用图优化框架
GTSAM：基于因子图，支持增量优化（iSAM2）
Ceres Solver：通用非线性最小二乘求解器

6 学习增强的 SLAM

6.1 深度估计

MonoDepth2：自监督单目深度估计
DPT：基于 Transformer 的深度预测

6.2 特征提取与匹配

SuperPoint：自监督关键点检测
SuperGlue：基于 Graph Neural Network 的特征匹配
LightGlue：轻量级特征匹配

6.3 端到端学习 SLAM

DROID-SLAM（Teed & Deng, 2021）：

基于 RAFT 光流网络的稠密匹配
可微分的 Bundle Adjustment 层
端到端训练，泛化能力强

\[ \min_{\{T_i\}, \{d_j\}} \sum_{(i,j)} \|p_{ij}^* - \Pi(T_i \cdot T_j^{-1}, d_j)\|^2_{\Sigma_{ij}} \]

其中 \(p_{ij}^*\) 为网络预测的对应点，可微分优化通过展开迭代实现梯度传播。

6.4 隐式表示

iMAP：使用 NeRF 作为地图表示
NICE-SLAM：层次化隐式表示
SplaTAM：基于 3D Gaussian Splatting 的 SLAM

7 SLAM 评估指标

7.1 轨迹精度

绝对轨迹误差（ATE）：

\[ \text{ATE} = \sqrt{\frac{1}{N}\sum_{i=1}^{N} \|p_i^{\text{est}} - p_i^{\text{gt}}\|^2} \]

相对位姿误差（RPE）：

\[ \text{RPE}_i = (T_i^{\text{gt}})^{-1} T_{i+\Delta}^{\text{gt}} \cdot ((T_i^{\text{est}})^{-1} T_{i+\Delta}^{\text{est}})^{-1} \]

7.2 标准数据集

数据集	传感器	场景
KITTI	双目 + LiDAR + GPS	室外驾驶
EuRoC	双目 + IMU	室内无人机
TUM-RGBD	RGB-D	室内手持
Newer College	多 LiDAR + 相机	室外步行

7.3 评估工具

evo：Python 轨迹评估工具包
rpg_trajectory_evaluation：ETH 开发的评估工具

8 系统集成

SLAM 系统通常运行在 ROS/ROS2 框架下（参见 ROS2），与导航栈协同：

SLAM 提供位姿估计和地图
导航栈基于地图进行路径规划
控制器执行运动命令

传感器的选型和标定对 SLAM 性能至关重要（参见传感器）。

参考文献

Thrun, S., Burgard, W. & Fox, D. (2005). Probabilistic Robotics. MIT Press.
Cadena, C. et al. (2016). Past, present, and future of simultaneous localization and mapping. IEEE TRO.
Campos, C. et al. (2021). ORB-SLAM3: An accurate open-source library for visual, visual-inertial and multi-map SLAM. IEEE TRO.
Zhang, J. & Singh, S. (2014). LOAM: Lidar Odometry and Mapping in Real-time. RSS.
Teed, Z. & Deng, J. (2021). DROID-SLAM: Deep Visual SLAM for Monocular, Stereo, and RGB-D Cameras. NeurIPS.
Grisetti, G. et al. (2010). A tutorial on graph-based SLAM. IEEE ITSM.