SLAM 全景

本文最后更新于：2026年7月9日上午

SLAM（Simultaneous Localization And Mapping，同时定位与建图）要求移动平台在未知环境、无先验地图的前提下，一边估计自身位姿、一边构建环境地图。这篇梳理 SLAM 的整体框架、数学本质与关键脉络，把前端跟踪、后端优化、回环检测、建图四个模块拧成一个整体来看。

定位与建图

SLAM 要同时完成两件事：估计自身位姿（Localization），构建环境地图（Mapping）。这里有个看似死锁的矛盾——

定位需要地图：要知道自己在哪，先得有地图当参照；
建图需要位姿：要把多帧观测拼成地图，先得知道每帧在哪。

解法是联合估计：把"位姿轨迹"和"地图"当作一个联合状态，用所有传感观测同时约束两者，迭代收敛到一致的解。不是"先有鸡还是先有蛋"，而是两者一起长出来。

几个容易混淆的概念要分清：已知地图的定位叫 Tracking（如 ARKit 在已知空间里跟踪）；已知位姿的建图叫 Reconstruction（如离线 SfM 重建）；SLAM 是两者都未知，而且要求实时、在线、增量。

数学本质

在贝叶斯框架下，SLAM 是个后验概率估计：

$$ p(\mathbf{x}_{1:t},\,\mathbf{m}\mid\mathbf{z}_{1:t},\,\mathbf{u}_{1:t}) $$

其中 $\mathbf{x}_{1:t}$ 是各时刻位姿序列、$\mathbf{m}$ 是地图（路标点 / 栅格 / 面片）、$\mathbf{z}_{1:t}$ 是观测（图像特征 / 激光点）、$\mathbf{u}_{1:t}$ 是控制 / 里程计 / IMU 输入。由贝叶斯和马尔可夫假设，这个后验可以分解，工程上有两条路。

滤波式（filter-based）：只维护"当前位姿 + 当前地图"的后验，每来一帧做一次预测-更新（EKF-SLAM、UKF-SLAM）。优点是增量、省内存；缺点是线性化误差累积、对大场景不鲁棒，且全状态协方差矩阵随路标数 $O(n^2)$ 膨胀。卡尔曼滤波的细节另文记录。

优化式（graph / bundle-adjustment-based，现代主流）：把 SLAM 看成因子图 / 图优化问题，节点 = 位姿 + 路标，边 = 观测约束，维护一段历史（滑窗或全部），定期全局优化：

$$ \min_{\mathbf{x},\mathbf{m}}\sum_i\big\|\mathbf{r}_i(\mathbf{x},\mathbf{m})\big\|_{\Sigma_i}^{2} $$

其中 $\mathbf{r}_i$ 是第 i 个约束的残差（重投影误差 / 里程计误差 / 回环误差），用 LM 或 Gauss-Newton 求解。优点是精度高、可全局一致（回环后整体调整）；代价是计算量大，需要工程优化（滑窗、边缘化、稀疏求解）。

现代 SLAM（ORB-SLAM2/3、VINS、LIO-SAM）几乎都是"滤波式前端 + 优化式后端"的混合。

图优化与滤波

这是理解现代 SLAM 演化的一个关键点。EKF-SLAM 衰退有两个结构性原因：一是它在线性化点处做一阶近似，误差随时间累积，大场景下发散；二是协方差矩阵 $O(n^2)$ 增长，路标一多就扛不住。图优化胜出的本质，是 SLAM 的约束图稀疏（每个路标只被少数相机看到、每个相机只看少数路标），稀疏矩阵的 LM 求解加 Schur 补消元，让全局优化在工程上可行；而且它保留全部历史，回环时能把误差沿图重新分配，得到全局一致解——这是纯滤波给不了的。

传感器与前端分类

传感器	优点	缺点	代表系统
单目相机	便宜、信息丰富	尺度不确定、纯旋转退化	ORB-SLAM(单目)、Mono-VINS
双目 / RGBD	直接得深度（绝对尺度）	标定敏感、深度有效距离受限	ORB-SLAM2(RGBD)、RTAB-Map
LiDAR(2D/3D)	精度高、不受光照影响	贵、纹理信息少	LOAM、LIO-SAM、FAST-LIO
IMU	高频、尺度已知、对快速运动鲁棒	零偏 / 噪声累积漂移	VINS、OKVIS
轮式里程计	简单可靠	打滑误差	常作为约束融合

多传感器融合（视觉-惯性 VIO、激光-惯性 LIO）是工业主流，互补短板：IMU 提供高频运动先验和尺度，相机 / LiDAR 提供低频但精确的观测修正。

四模块框架

主流视觉 SLAM 基本遵循"前端跟踪 + 后端优化 + 回环检测 + 建图"四模块：

1
2
3

传感器数据 → [前端 Tracking] → [后端 Optimization] → [回环 Loop Closing] → [建图 Mapping]
                ↑                    ↑                     ↑
          逐帧位姿            局部/全局一致性       累积漂移校正

前端 Tracking

前端按已知信息递进，分三个层次：

① 2D-2D：对极几何（第一帧或丢失跟踪时）。两帧特征点对应，用本质矩阵 E / 基础矩阵 F 约束（对极约束 $\mathbf{x}_2^\top F\,\mathbf{x}_1=0$）。用 5/8 点法加 RANSAC 估 F，再从 E 分解 R, t。主要缺陷是纯旋转退化（平移为零时 E 不可观），且只有相对尺度。

② 3D-2D：PnP（已有局部地图时，最常用）。已知 3D 路标点和当前帧 2D 像素，求相机位姿 $\lambda\mathbf{p}=K(RX+\mathbf{t})$。算法有 EPnP / P3P / DLT / 迭代 LM，工程标配 EPnP 加 RANSAC。PnP 是相机定位里核心的姿态求解，其本质（射线交汇、EPnP 控制点推导）另文展开。

③ 3D-3D：ICP（有深度图 / 点云时）。两组 3D 点云求 R, t：$\min_{R,\mathbf{t}}\sum_i\|Rp_i+\mathbf{t}-q_i\|^2$，点到点用 SVD 闭式解、点到面用 Gauss-Newton 迭代。多用于 RGBD / LiDAR SLAM。

后端 Optimization

局部 BA：维护当前帧加共视关键帧加它们的路标，做窗口内光束法平差，保证局部一致性。
全局位姿图优化：回环触发后，把回环约束加入图，全局调整所有关键帧位姿（不优化路标，快）。
全局 BA：关键帧位姿固定后，全量 BA 优化所有位姿加路标（精度最高、也最耗时）。

BA 的本质是最小化重投影误差：

$$ \min_{\mathbf{x},\mathbf{m}}\sum_{i,j}\rho\big(\big\|\pi(\mathbf{x}_i,\mathbf{m}_j)-\mathbf{u}_{ij}\big\|_{\Sigma}^{2}\big) $$

其中 $\pi$ 是投影函数、$\rho$ 是鲁棒核（Huber / Cauchy 抑制外点）。解法是 LM 加 Schur 补：路标数远多于相机数，雅可比稀疏，消掉路标变量后只解相机位姿，大幅降维。工具用 Ceres、g2o、GTSAM。BA 的工程实现另有专文。

IMU 预积分

IMU 高频（200-1000Hz），相机低频（30Hz）。朴素做法是把 IMU 逐次积分到相机帧，但位姿变量一旦改变就要重新积分，开销大。Forster 2015 的预积分把两帧间的 IMU 积分表达成与起点状态解耦的预积分量——它只依赖两帧间的 IMU 数据；优化时位姿改变，预积分项加一阶偏差修正即可，无需重积分。由此得到两帧间的相对运动约束加协方差，加入后端因子图。代表系统是 VINS-Mono/Fusion、OKVIS。

回环检测

SLAM 长时间运行会累积漂移。回环检测识别"机器人回到了以前去过的地方"，用这个约束做全局优化消除漂移。核心是词袋模型（DBoW2）做粗筛找候选，再用几何验证加 Sim3 校正确认——这是 SLAM 区别于纯里程计的关键。词袋与回环的完整流程分两篇展开：词袋模型负责检索候选，回环检测：候选之后负责候选之后的几何验证与全局校正。

建图

建图模块按用途产出不同形式的地图：稀疏点云（定位用）、稠密点云 / 网格（导航 / 重建用）、八叉树栅格（避障用）、语义地图（语义 SLAM）。需要注意，3D 占据地图的表示（如 OctoMap 的八叉树加概率占据加 ray-casting）本身是个独立的大话题，和 SLAM 的位姿估计主线相对解耦——它消费 SLAM 输出的位姿去构建可执行的持久地图。

难点

难点	成因	应对手段
尺度不确定性	单目投影丢深度	双目/RGBD/IMU 补尺度；单目用三角化+回环 Sim3
纯旋转退化	对极几何在 t≈0 时 E 不可观	退化为单应 H；引入 IMU 或运动先验
累积漂移	帧间误差长期累积	回环检测 + 全局优化
动态场景	移动物体违反静态世界假设	语义分割剔动态物 / 多模态运动一致性
弱纹理 / 重复纹理	特征匹配歧义	学习特征 / 直接法 / 多传感器融合
快速运动（运动模糊）	特征点模糊丢失	IMU 高频补帧 + 滑窗
大尺度场景	计算与漂移膨胀	子图拼接、分层位姿图、全局优化
实时性	BA/优化昂贵	滑窗、边缘化、稀疏求解、关键帧策略

代表性系统

视觉 SLAM：PTAM（2007，首提 keyframe 加 BA 分前后端）；ORB-SLAM2/3（ORB 特征加三线程；v2 支持单目/双目/RGBD，v3 再纳入视觉惯性与多地图 atlas，常用的教学范本）；VINS-Mono/Fusion（视觉-惯性紧耦合加滑窗加 IMU 预积分，工业上常用的 VIO 方案）；DSO/LSD-SLAM（直接法，不提特征直接用像素灰度）；DROID-SLAM / NICE-SLAM / 3DGS-SLAM（学习端到端 / NeRF / 高斯泼溅建图，前端趋势）。

LiDAR SLAM：LOAM（2014，激光特征加高低频里程计分离，奠基）；LIO-SAM（LiDAR-IMU 紧耦合加因子图）；FAST-LIO2（LiDAR-IMU 加 IESKF 加 ikd-Tree，当前主流）。

工程框架：g2o（图优化）、GTSAM（因子图加 iSAM2 增量平滑）、Ceres（通用非线性最小二乘）。

小结

SLAM 是在未知环境里联合估计位姿与地图的状态估计问题。现代主流是"前端跟踪（对极几何 / PnP / ICP 求逐帧运动）+ 后端优化（BA / 因子图求全局一致）+ 回环检测（DBoW2 消累积漂移）+ 建图"四模块。数学基础是 SE(3) 李群上的非线性最小二乘，靠 LM 加 Schur 补实时求解。其中回环检测加全局优化，是把它从"里程计"提升为"全局一致 SLAM"的关键。

参考资料

高翔，《视觉SLAM十四讲：从理论到实践》
C. Campos et al., “ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM,” IEEE T-RO, 2021.
T. Qin et al., “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE T-RO, 2018.
G. Klein and D. Murray, “Parallel Tracking and Mapping for Small AR Workspaces (PTAM),” ISMAR, 2007.
C. Forster et al., “IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,” RSS, 2015.
J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in Real-time,” RSS, 2014.
相关笔记：词袋模型 BoW 与回环检测、回环检测：候选之后、卡尔曼滤波、Bundle Adjustment、SfM 流程、PnP 算法