引言
SLAM 的核心矛盾在于：要定位就得有地图，要建图就得知道自己的位置。这个循环依赖通过「前端估计 + 后端优化 + 回环修正」的结构破解。前端用相邻帧的匹配给出局部一致的轨迹，后端把长时间跨度的约束统一优化，回环检测则提供「我回到了老地方」这一关键信息来消除累积漂移。
工程上的难点首先是漂移的不可预测性。里程计误差按距离累积，激光雷达 2D SLAM 的典型漂移是 1%~2% 行走距离，视觉 SLAM 在纹理不足时可能瞬间失跟。其次是动态环境：行人、移动车辆、开关的门会让「同一地点」的观测不一致，回环检测被误导。第三是计算量的实时性约束：后端优化是全局问题，随位姿数增长，必须用增量式方法（iSAM2）或滑动窗口控制规模。
第四个难点常被低估：地图表示与下游需求的匹配。规划器需要 ESDF（欧氏符号距离场）来做梯度下降避障，导航需要 2D 占据栅格做膨胀，可视化需要点云。同一套 SLAM 要输出三种表示，且要保持一致。
本文按「问题定义 → 前端 → 激光路线 → 视觉路线 → 后端 → 回环 → 地图表示 → 定位模式 → 工程选型」的顺序展开，给出算法结构与参数取值。示例覆盖 2D 激光、3D 激光惯性、以及视觉惯性三类系统。
目录
- SLAM 问题定义与两大流派
- 前端：扫描匹配与里程计
- 激光 SLAM：2D 栅格与 3D 点云
- 视觉 SLAM：特征法与直接法
- 后端：位姿图优化与因子图
- 回环检测与地图一致性
- 地图表示：占据栅格、点云与 ESDF
- 定位模式：重定位与纯定位
- 工程实践：框架选型与调参
1. SLAM 问题定义与两大流派
SLAM 的数学形式是估计后验概率，历史上分化为滤波与优化两大流派。
全 SLAM 后验：
p(x_{1:T}, m | z_{1:T}, u_{1:T})
x：机器人轨迹，m：地图，z：观测，u：控制输入
滤波流派（在线、递归）：
EKF-SLAM：把地图特征加入状态向量，协方差 O(n²)，特征多时不可行
FastSLAM：用粒子滤波分解为「轨迹粒子 + 每粒子的独立地图」
优点：在线、增量、内存固定
缺点：线性化误差累积、回环处理弱、地图规模受限
优化流派（批处理 / 滑动窗口，现代主流）：
把问题写成因子图，用非线性最小二乘求解
优点：精度高、回环处理自然、可用稀疏性加速
缺点：需要维护图结构，全局优化有延迟
现代 SLAM 系统几乎都是优化流派，滤波主要用于传感器融合（见传感器融合与状态估计一篇）。因子图的表述是理解现代 SLAM 的关键：
因子图 = 变量节点（位姿、路标、外参）+ 因子（约束）
先验因子：固定第一个位姿，消除规范自由度
里程计因子：相邻位姿间的相对变换
观测因子：位姿与路标的观测关系
回环因子：非相邻位姿间的相对变换（关键！）
外参因子：IMU 与雷达之间的标定关系
求解：min_X Σ ||r_i(X)||²_{Σ_i}
用高斯-牛顿或 Levenberg-Marquardt 迭代
稀疏性来自「每个因子只涉及少数变量」，用稀疏 Cholesky 或 iSAM2 增量求解
2. 前端：扫描匹配与里程计
前端负责给出局部一致的相对运动，是后端的输入。
扫描匹配（Scan Matching）的三类方法：
1. ICP（迭代最近点）
最近邻找对应点 → 求使距离最小的变换 → 迭代
缺点：依赖初值，点到点误差度量收敛慢
改进：点到面（point-to-plane）收敛快 3~5 倍
GICP（广义 ICP）在概率框架下统一点到点与点到面
2. NDT（正态分布变换）
把参考帧的体素建成正态分布，优化新帧落在分布中的似然
优点：不依赖最近邻、对初值容忍度高
缺点：体素大小需调，太大失真太小慢
Cartographer 用 NDT 的变体（相关扫描匹配 + 分支定界）
3. 特征匹配
提取平面、边缘特征，用特征对应求变换
LOAM / LIO-SAM 的做法：边缘点与平面点分开匹配
优点：速度快、对稀疏点云友好
// 点到面 ICP 的残差与雅可比（用于理解收敛快的来源）
// 残差：r = n^T · (R·p_src + t - q_tgt)
// 其中 n 是目标点 q_tgt 处的法向量
// 相比点到点 r = ||R·p_src + t - q_tgt||，点到面在切向不惩罚
// 因此每次迭代的有效步长更大，收敛更快
Eigen::Vector3d residual(const Eigen::Vector3d & p_src,
const Eigen::Vector3d & q_tgt,
const Eigen::Vector3d & n_tgt,
const Eigen::Matrix3d & R,
const Eigen::Vector3d & t) {
return n_tgt * (n_tgt.transpose() * (R * p_src + t - q_tgt));
}
3. 激光 SLAM：2D 栅格与 3D 点云
2D 激光 SLAM 在室内移动机器人上仍然是最成熟可靠的方案，因为单线雷达便宜、算力要求低、退化场景少。
2D SLAM 的典型流程（以 Cartographer 为例）：
1. 局部 SLAM（Local Trajectory Builder）
用扫描匹配把每帧雷达配到当前子图（submap）上
子图是概率栅格，大小通常 5m × 5m，分辨率 5 cm
2. 子图完成（插入约 90 帧后）后加入全局图
3. 全局 SLAM（后端）
用分支定界做快速扫描匹配，检测回环
位姿图优化，把回环约束加入
4. 输出：优化后的轨迹 + 拼接地图
关键参数：
num_range_data: 90 # 每个子图的帧数
submaps.num_range_data: 90
real_time_correlative_scan_matcher.linear_search_window: 0.1
ceres_scan_matcher.occupied_space_weight: 20.0
pose_graph.constraint_builder.min_score: 0.55 # 回环接受阈值
3D 激光惯性 SLAM 是当前室外与大场景的主流，代表是 LIO-SAM 与 FAST-LIO2，二者都用「紧耦合 IMU + 激光」。
LIO-SAM 的结构：
1. IMU 预积分提供高频运动先验（见传感器融合一篇）
2. 点云去畸变（用 IMU 积分补偿运动畸变）
3. 提取边缘与平面特征，构建局部地图
4. 因子图：IMU 因子 + 激光里程计因子 + GPS 因子 + 回环因子
5. GTSAM 做增量优化（iSAM2）
FAST-LIO2 的结构：
1. 直接用原始点云（不提取特征），配到增量式 ikd-Tree 上
2. 迭代扩展卡尔曼滤波（IEKF）做状态更新
3. 计算量 O(点云数)，单线程可跑 100 Hz
优势：无特征提取误差、对非结构化环境鲁棒
代价：地图内存随行驶距离线性增长（ikd-Tree 增量维护）
| 系统 | 类型 | 特点 | 适用 |
|---|---|---|---|
| Cartographer | 2D/3D 激光 | 子图 + 分支定界回环 | 室内 2D、静态环境 |
| LIO-SAM | 3D 激光惯性 | 因子图、多传感器融合 | 室外、有 GPS |
| FAST-LIO2 | 3D 激光惯性 | IEKF、无特征、极快 | 高动态、非结构化 |
| LOAM/Lego-LOAM | 3D 激光 | 特征法经典 | 教学、轻量部署 |
4. 视觉 SLAM：特征法与直接法
视觉 SLAM 的优势是传感器便宜、能提供语义，劣势是对光照与纹理敏感。
特征法（ORB-SLAM3 为代表）：
提取 ORB 特征 → 匹配 → 三角化 → PnP 求位姿 → BA 优化
优点：对光照变化有一定鲁棒、可用描述子做回环
缺点：纹理稀疏区域特征不足、弱纹理场景失败
直接法（DSO、LSD-SLAM）：
直接最小化光度误差，不提取特征
优点：弱纹理可用、能建半稠密地图
缺点：对光照变化极敏感、需要曝光补偿
视觉惯性（VINS-Mono、ORB-SLAM3 VI、OpenVINS）：
紧耦合 IMU + 相机，尺度可观测
优点：单目也能得到尺度、快速运动鲁棒
缺点：标定要求高（相机-IMU 外参与时间偏移）
视觉 SLAM 的工程门槛主要在标定。相机-IMU 的时间偏移（通常几毫秒到几十毫秒）不标定会导致高速运动时轨迹弯曲。标定工具用 Kalibr，输出外参与时间偏移，标定质量看重投影误差是否小于 0.5 像素。
5. 后端：位姿图优化与因子图
后端把前端给出的相对约束统一优化，是消除漂移的关键。位姿图优化是最常用的形式。
位姿图：节点是位姿 x_i ∈ SE(3)，边是相对约束 x_ij
残差：r_ij = Log( T_ij^-1 · (T_i^-1 · T_j) ) （SE(3) 上的对数映射）
目标：min_X Σ r_ij^T · Σ_ij^-1 · r_ij
求解（高斯-牛顿）：
J：雅可比，稀疏（每个残差只涉及两个节点）
H = J^T Σ^-1 J：稀疏矩阵，用 Cholesky 分解
Δx = -H^-1 · J^T Σ^-1 r
x ← x ⊞ Δx （流形上的更新）
// 用 GTSAM 构建位姿图并优化
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
gtsam::NonlinearFactorGraph graph;
auto prior_noise = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) << 1e-6,1e-6,1e-6, 1e-6,1e-6,1e-6).finished());
graph.addPrior(gtsam::Symbol('x', 0), gtsam::Pose3::identity(), prior_noise);
auto odom_noise = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) << 0.05,0.05,0.05, 0.02,0.02,0.02).finished());
for (size_t i = 0; i + 1 < poses.size(); ++i) {
graph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
gtsam::Symbol('x', i), gtsam::Symbol('x', i+1),
relative[i], odom_noise);
}
// 回环因子：噪声比里程计小，权重更大
auto loop_noise = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) << 0.02,0.02,0.02, 0.01,0.01,0.01).finished());
graph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
gtsam::Symbol('x', 10), gtsam::Symbol('x', 250), loop_tf, loop_noise);
gtsam::Values initial;
gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial);
gtsam::Values result = optimizer.optimize();
增量式求解（iSAM2）只重算受影响的部分，是长时运行的必要条件。全量重优化在 1000 个位姿时约需几百毫秒，iSAM2 只需几毫秒。
6. 回环检测与地图一致性
回环检测提供「回到老地方」的约束，是消除长距离漂移的唯一手段。误检（假回环）比漏检危害大得多，因为它会把正确的地图强行扭曲。
三种回环检测手段：
1. 几何搜索（Scan Context、M2DP）
把点云压缩成旋转不变的描述子，检索相似帧
优点：不依赖历史位姿估计、对视角变化鲁棒
2. 词袋模型（DBoW2/DBoW3，视觉用）
把描述子量化成视觉词，用倒排索引快速检索
需要预训练词典
3. 位置先验（GPS、里程计累积）
只在「估计位置接近历史位置」时才检测回环
优点：计算量小；缺点：漂移大时可能错过
误检防护（必做）：
几何验证：候选帧对之间做 ICP/NDT 匹配，残差超阈值则拒绝
一致性检查：多个候选互相印证（如连续 3 帧都指向同一区域）
回环后验证：加入约束优化后，若整体残差暴增则回滚该约束
回环的接受阈值（如 Cartographer 的 min_score）是精度与鲁棒性的权衡旋钮。阈值高（0.7）几乎不会误检但可能漏检；阈值低（0.4）回环多但误检风险上升。室内结构化环境可以设 0.55~0.65，室外非结构化环境建议更高或加几何验证。
7. 地图表示：占据栅格、点云与 ESDF
地图的表示形式由下游用途决定，一套 SLAM 通常要输出多种。
占据栅格（Occupancy Grid）：
每个格子存占据概率，用对数几率更新
log_odds += log(p_hit/(1-p_hit)) 或 log(p_miss/(1-p_miss))
优点：导航友好、更新简单、内存可控
缺点：分辨率固定、三维场景内存爆炸
典型：2D 分辨率 5 cm，用于 Nav2
点云地图：
直接存点，或体素下采样后存
优点：精度高、保留全部信息
缺点：体积大（1 km² 的 3D 地图可达 GB 级）
典型：体素大小 0.1~0.3 m，配合八叉树压缩
ESDF（欧氏符号距离场）：
每个体素存到最近障碍的距离
优点：可直接求梯度做避障，规划器友好
缺点：计算与更新成本高
典型：Voxblox、FIESTA，分辨率 0.1~0.2 m
占据栅格的对数几率更新（工程取值）：
p_hit = 0.7 → log_odds += +0.85
p_miss = 0.4 → log_odds += -0.40
上限 clamp 到 ±2.0（约 p = 0.88 / 0.12），防止长期观测导致不可逆
这样一次观测不足以判定占据，需要多次一致观测才确认
8. 定位模式：重定位与纯定位
建图之后进入运行阶段，机器人需要在已知地图里定位，且要能从上电或失跟中恢复。
三种运行模式：
1. 纯里程计 + 地图匹配（无先验）
上电时不知道在哪，需要全局定位
2. 重定位（Global Localization）
在地图中搜索最匹配的位置
方法：AMCL（自适应蒙特卡洛，粒子滤波）
分支定界扫描匹配（Cartographer 的纯定位模式）
粒子数典型 500~2000，收敛后自适应降到 200~500
3. 跟踪定位（Tracking）
已知大致位置，只需局部精化
用 EKF/UKF 或 ICP 跟踪，延迟低、鲁棒性依赖初始位姿
AMCL 的关键参数与失效模式：
amcl:
ros__parameters:
min_particles: 500
max_particles: 2000
laser_model_type: "likelihood_field" # 比 beam 模型快且稳
laser_max_range: 12.0 # 超过此距离的读数丢弃
update_min_d: 0.25 # 移动 0.25m 才更新，省算力
update_min_a: 0.2 # 转 0.2rad 才更新
resample_interval: 1
recovery_alpha_slow: 0.001 # 慢速恢复：长期失跟
recovery_alpha_fast: 0.1 # 快速恢复：突发失跟
AMCL 最常见的失效是「被抬起来搬到别处」后位置跳变——这属于绑架问题（kidnapped robot）。恢复靠 recovery_alpha_* 注入随机粒子，但恢复时间可能几秒到几十秒。要求快速恢复的场景应该用分支定界全局搜索或加视觉重定位。
9. 工程实践：框架选型与调参
选型的判断顺序是：传感器配置 → 环境特性 → 算力预算 → 精度要求。
决策树：
只有 2D 单线雷达 + 室内 → Cartographer 2D 或 slam_toolbox
3D 雷达 + 室内外、要求精度 → LIO-SAM（有 IMU 与 GPS）
3D 雷达 + 高动态、要速度 → FAST-LIO2
相机 + IMU、要低成本 → ORB-SLAM3 VI 或 VINS-Fusion
需要语义地图 → 视觉 SLAM + 语义分割（2D 分割的做法见[图像分割](/cv-image-segmentation/)，与定位的协同见[机器人视觉与抓取](/robotics-perception-grasping/)）
调参的优先级与验证方法：
# 1. 先验证前端：录制一段数据，只跑里程计，看轨迹是否平滑
ros2 bag record -s mcap -a -o slam_test
# 2. 检查 IMU 与雷达外参标定，外参错会导致轨迹系统性弯曲
ros2 run tf2_ros tf2_echo base_link lidar_link
# 3. 检查时间同步，用 ros2 topic delay 看时间戳偏差
ros2 topic delay /points
# 4. 打开回环，观察地图闭合质量
# 5. 用 evo 工具做定量评估（ATE/RPE）
evo_ape tum groundtruth.txt estimated.txt --align --plot
evo 的 ATE（绝对轨迹误差）是评估 SLAM 精度的标准指标。室内 2D SLAM 的 ATE 典型在 310 cm，室外 3D 激光惯性在 1050 cm。若 ATE 明显超标，先查时间同步与外参，再查回环阈值。
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 滤波 vs 优化 | 滤波：在线、内存固定、小规模 | 优化：精度高、回环自然、主流 |
| 2D vs 3D | 2D：便宜、算力低、室内 | 3D：室外、三维障碍、精度高 |
| 激光 vs 视觉 | 激光：精度高、光照无关 | 视觉：便宜、有语义 |
| 特征 vs 直接 | 特征：光照鲁棒、可回环 | 直接：弱纹理、半稠密 |
| 地图表示 | 占据栅格：导航友好 | ESDF：规划梯度友好 |
| 回环阈值 | 高：不误检、可能漏检 | 低：回环多、误检风险 |
核心原则：回环宁少勿错。一个假回环会把整张地图扭曲，而漏检只是保留了漂移。工程上应优先保证回环的正确性，再考虑覆盖率。
常见坑清单
- IMU 与雷达外参未标定，轨迹系统性弯曲——先用
tf2_echo验证外参，用标定工具求精确值。 - 相机-IMU 时间偏移未标定，高速运动时轨迹弯曲——用 Kalibr 标定时间偏移。
- 回环阈值过低导致假回环，地图被扭曲——提高阈值并加几何验证与一致性检查。
- 点云未做运动畸变补偿，快速旋转时建图模糊——用 IMU 积分去畸变。
- 动态物体（行人、车辆）被写入地图，形成幽灵障碍——加动态点滤除（如距离图像聚类）。
- 后端全量重优化，位姿上千后延迟到秒级——改用 iSAM2 增量优化。
- 占据栅格的对数几率不设上下限，长期观测导致不可逆——clamp 到 ±2.0。
- ESDF 每帧全量重算，CPU 占满——用增量式更新（FIESTA/Voxblox）。
- 只用里程计不做重定位，上电后位置随机——必须实现全局重定位或给出初始位姿。
- 评估只用肉眼观察地图，无定量指标——用
evo算 ATE/RPE，建立回归基线。
小结
现代 SLAM 的骨架是「前端给相对约束、后端统一优化、回环消除漂移」。理解因子图就能理解主流系统（LIO-SAM、ORB-SLAM3、Cartographer 的全局部分）的共同结构，差异只在前端如何产生约束与地图如何表示。
下一步建议阅读传感器融合与状态估计 ，那是 SLAM 前端的数学基础，尤其是 IMU 预积分与 ESKF；移动平台的导航落地见运动规划与轨迹优化 的 Nav2 部分；如果要做带语义的抓取，机器人视觉与抓取会讲清感知如何与定位协同。
最后一句经验：SLAM 的问题九成出在标定与时间同步，而不是算法本身。动手调算法参数之前，先把外参与时间偏移验证一遍。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。