3.1 SLAM 问题表述：状态估计与概率图模型

文档摘要

3.1 SLAM 问题表述：状态估计与概率图模型 SLAM 之所以难，是因为它是一个「鸡生蛋-蛋生鸡」问题：不知道位置就没法建图，没有地图也没法定位。突破口是把这两个问题联合建模成一个概率状态估计问题。 3.1.1 SLAM 问题形式化机器人装备里程计（编码器/IMU/视觉里程计）和外部传感器（相机/激光雷达），在未知环境中运动。设： $x{1:T}$ = 机器人在时刻 $1, 2, ...

3.1 SLAM 问题表述：状态估计与概率图模型

SLAM 之所以难，是因为它是一个「鸡生蛋-蛋生鸡」问题：不知道位置就没法建图，没有地图也没法定位。突破口是把这两个问题联合建模成一个概率状态估计问题。

3.1.1 SLAM 问题形式化

机器人装备里程计（编码器/IMU/视觉里程计）和外部传感器（相机/激光雷达），在未知环境中运动。设：

x_{1:T} = 机器人在时刻 1, 2, ..., T 的位姿序列（待估计）
m = 环境地图（路标点 / 体素 / 网格，待估计）
u_{1:T} = 控制输入（里程计读数，已知）
z_{1:T} = 观测（相机/激光观测，已知）

SLAM 的目标是联合估计后验概率：

P(x_{1:T}, m \mid z_{1:T}, u_{1:T})

即「给定所有观测和控制，机器人轨迹和地图最可能是什么」。这个后验概率是 SLAM 所有方法的统一目标。

3.1.2 两个假设：马尔可夫与条件独立

为让上面的后验可解，SLAM 引入两个标准假设：

马尔可夫假设（一阶）

当前状态只依赖上一时刻状态和当前控制，与更早历史无关：

P(x_t \mid x_{t-1}, u_t)

这是运动模型（Motion Model），描述「知道上一时刻位姿和当前控制，当前位姿怎么变」。例如差速底盘的运动模型：


[x', y', θ'] = [x, y, θ] + [v·cos(θ)·dt, v·sin(θ)·dt, ω·dt]

观测条件独立假设

给定机器人位姿和地图，每个观测独立：

P(z_t \mid x_t, m)

这是观测模型（Observation Model），描述「知道位姿和地图，会观测到什么」。例如相机看到第 i 个路标，观测模型是从路标 3D 位置投影到像素坐标。

这两个假设把高维联合概率拆解成可计算的乘积形式，是 SLAM 数学框架的基石。

3.1.3 贝叶斯滤波：在线递推估计

SLAM 最早的形式化是贝叶斯滤波（Bayes Filter），用两步递推：


预测步（用运动模型）:
  P(x_t, m | z_{1:t-1}, u_{1:t}) = ∫ P(x_t | x_{t-1}, u_t) P(x_{t-1}, m | z_{1:t-1}, u_{1:t-1}) dx_{t-1}

更新步（用观测模型）:
  P(x_t, m | z_{1:t}, u_{1:t}) ∝ P(z_t | x_t, m) · P(x_t, m | z_{1:t-1}, u_{1:t})

这是所有 SLAM 算法的母公式。预测步用运动模型把状态往前推，更新步用观测修正预测。循环往复就实现了「边走边定位边建图」。

💡 直觉理解：贝叶斯滤波就像开车导航——你大致知道刚才往哪开了（预测），同时 GPS 给你一个测量（观测），两者融合得到更准确的位置估计。SLAM 比导航难在「地图也是未知的」。

3.1.4 卡尔曼滤波与扩展卡尔曼滤波

贝叶斯滤波是通用框架，但积分通常无法解析求解。卡尔曼滤波（Kalman Filter, KF） 是最经典的特例——假设运动模型和观测模型都是线性的，噪声是高斯的，则后验也是高斯，只需递推均值和协方差。

但机器人运动模型（如差速底盘）和观测模型（如透视投影）都是非线性的。于是有了扩展卡尔曼滤波（Extended Kalman Filter, EKF）：在当前估计点做一阶泰勒展开，把非线性函数线性化，再用 KF 公式。

EKF-SLAM 的状态向量包含机器人位姿 + 所有路标位置：

X = [x_{robot}, y_{robot}, θ_{robot}, l_{x,1}, l_{y,1}, ..., l_{x,N}, l_{y,N}]

协方差矩阵 \Sigma 是 (3+2N) \times (3+2N)。致命问题：路标数量 N 增大时，协方差矩阵平方级膨胀，每步更新成本 O(N^2)。1000 个路标就要算 2000×2000 矩阵，难以扩展。

滤波方法	模型假设	复杂度	代表
卡尔曼滤波 KF	线性 + 高斯	O(N²)	理论基础
扩展 KF (EKF)	非线性一阶展开	O(N²)	EKF-SLAM（最早）
无迹 KF (UKF)	非线性 sigma 点展开	O(N²)	改进 EKF
粒子滤波 (PF)	任意分布	O(M·N)，M 粒子数	FastSLAM
因子图优化	全局批量优化	稀疏高效	现代主流

⚠️ 历史转折：EKF-SLAM 在 2000 年代是主流，但因为 O(N^2) 复杂度和线性化误差累积，2010 年后逐渐被因子图优化取代。今天你在任何现代 SLAM 系统里看到的核心都是因子图。

3.1.5 因子图优化：现代 SLAM 的核心

因子图（Factor Graph） 把 SLAM 建模成一个二分图：

变量节点：机器人各时刻位姿 x_t、路标位置 l_i。
因子节点：约束——运动约束（相邻位姿）、观测约束（位姿观测到路标）、回环约束（不同时刻看到同一处）。

每个因子对应一个误差项，SLAM 变成最小化所有误差的优化问题：

X^* = \arg\min_X \sum_{因子} \| r_i(X) \|^2_{\Sigma_i}

其中 r_i 是第 i 个因子的残差（如观测残差 = 实际观测 - 预测观测），\Sigma_i 是其信息矩阵。

因子图的优势

优势	含义
稀疏性	大多数因子只连接少数变量，稀疏求解器高效
批量优化	不需要在线递推，可以全批量优化整条轨迹
统一框架	运动约束、观测约束、回环约束、GPS 约束都化为因子
非线性优化	用高斯-牛顿 / Levenberg-Marquardt 迭代求解，精度高于一阶展开
增量求解	iSAM / iSAM2 等算法支持增量更新，接近实时

这张图里，变量节点（位姿、路标）和因子节点（运动约束、观测约束）交替连接。SLAM 就是求解所有变量，让所有因子的残差最小。

求解算法

主流的非线性最小二乘求解器：

高斯-牛顿法（Gauss-Newton）：在当前点线性化，解线性方程组 H \Delta X = -b（H 是 Hessian 近似，b 是梯度），更新 X \leftarrow X + \Delta X。
Levenberg-Marquardt（LM）：高斯-牛顿 + 阻尼项，更稳健。
iSAM / iSAM2：增量求解，每来一批新观测不必从头算，接近实时。

g2o、GTSAM、Ceres Solver 是三大开源因子图优化库，几乎所有现代 SLAM 都建立其上。

3.1.6 SLAM 的三段式架构

理解了数学框架，工程实现上 SLAM 系统普遍采用「前端-后端-回环」三段式：

模块	职责	输入	输出
前端	数据关联、特征提取、初始位姿估计	原始传感器流	帧间运动 + 观测约束
后端	因子图优化，全局一致位姿与地图	前端输出的约束	优化后的轨迹与地图
回环检测	识别是否回到旧地，添加回环约束	关键帧描述子	回环约束（强约束，纠正漂移）

⚠️ 为什么回环检测如此关键：纯前端+后端只能产生漂移——误差逐渐累积，绕一圈回到原点时位置差了几米。回环检测就是「诶，这地方我之前来过」，添加一个强约束把整条轨迹拉回正确位置。没有回环检测的 SLAM 永远是局部最优。

3.1.7 漂移与回环：SLAM 的永恒主题

下面用 SVG 展示漂移与回环修正：


<svg viewBox="0 0 600 320" xmlns="http://www.w3.org/2000/svg">
  <!-- 真实轨迹（黑虚线） -->
  <rect x="60" y="60" width="280" height="180" fill="none" stroke="#374151" stroke-width="2" stroke-dasharray="6 4"/>
  <text x="200" y="50" text-anchor="middle" font-size="11" fill="#374151">真实轨迹（方形环路）</text>

  <!-- 漂移估计（红） -->
  <path d="M 60 60 L 360 70 L 380 240 L 80 260 Z" fill="none" stroke="#dc2626" stroke-width="2"/>
  <text x="220" y="290" text-anchor="middle" font-size="11" fill="#dc2626">仅前端+后端：漂移（起点终点不闭合）</text>

  <!-- 起点与漂移后的终点 -->
  <circle cx="60" cy="60" r="5" fill="#16a34a"/>
  <text x="40" y="55" font-size="10" fill="#16a34a">起点</text>
  <circle cx="80" cy="260" r="5" fill="#dc2626"/>
  <text x="90" y="278" font-size="10" fill="#dc2626">漂移终点</text>

  <!-- 回环约束（蓝箭头） -->
  <line x1="80" y1="260" x2="62" y2="62" stroke="#2563eb" stroke-width="2" stroke-dasharray="3 3" marker-end="url(#arr)"/>
  <text x="450" y="160" font-size="11" fill="#2563eb">回环约束</text>
  <text x="450" y="178" font-size="11" fill="#2563eb">把漂移拉回</text>
  <defs>
    <marker id="arr" markerWidth="8" markerHeight="8" refX="6" refY="3" orient="auto">
      <path d="M0,0 L6,3 L0,6 Z" fill="#2563eb"/>
    </marker>
  </defs>
</svg>

💡 工程含义：在工厂、家庭这类会反复经过同一位置的场景，回环检测几乎一定可用。但在野外长程探索（如搜救、长距离巡检），可能整段任务都不回到旧地，回环无法触发，纯漂移无可避免。这是为什么 SLAM 在不同场景表现差异巨大。

3.1.8 SLAM 的三种不确定性来源

最后总结 SLAM 必须处理的三类不确定性：

不确定性	来源	处理方法
传感器噪声	相机抖动、激光散射、IMU 漂移	概率模型 + 协方差加权
数据关联错误	误判「这个路标是不是之前那个」	鲁棒核函数、RANSAC、概率数据关联
运动模型误差	轮子打滑、IMU 偏差	在线标定、紧耦合 IMU

⚠️ 数据关联是 SLAM 最隐蔽的敌人：一旦前端把两个不同的路标当成同一个，后端优化会把这个错误扩散到整张地图，导致整条轨迹扭曲。鲁棒核函数（如 Huber、Cauchy）就是为了在优化中抑制错误观测的影响。

本节小结

SLAM 问题是联合估计机器人轨迹 x_{1:T} 和地图 m 的后验概率 P(x, m | z, u)。
两个关键假设：马尔可夫运动模型 + 观测条件独立，把联合概率拆成可解形式。
贝叶斯滤波是母框架，EKF 是其经典特例，但 O(N^2) 复杂度限制了扩展性。
现代主流是因子图优化：稀疏、批量、统一处理运动/观测/回环约束。
工程上 SLAM 采用前端-后端-回环三段式，回环检测是消除漂移的关键。
三类不确定性（传感器噪声、数据关联、运动模型）是 SLAM 永恒的敌人。

下一节《3.2 视觉 SLAM 与激光 SLAM》将对比工业界主流方法（ORB-SLAM、VINS、LOAM、LIO-SAM），看不同传感器如何落地这套数学框架。