SLAM后端：滤波与滑窗优化的理论分析

专栏系列：2D/3D/视觉SLAM理论详解（共10篇） |难度：高级 |预计阅读：28分钟
前置知识：贝叶斯滤波（第1章）、非线性优化（第2章）、图优化理论（第5章）
本文定位：第6篇，对比三种SLAM后端方法——从2D激光的RBPF到视觉的滑窗优化

摘要

本章从理论层面比较三种SLAM后端的两大阵营——滤波方法和优化方法——在2D激光、3D激光和视觉中的不同应用。重点揭示：(1) 为什么RBPF在2D激光SLAM中有效但在视觉中不可行，(2) EKF不一致性的数学根源，(3) 现代3D激光SLAM中的IEKF（FAST-LIO2）如何通过迭代更新缓解不一致性，(4) 滑窗优化和边缘化在三种SLAM中的不同应用策略。

目录

• 1. 滤波与优化：两种后端的哲学差异
• 2. 2D激光后端：RBPF与GMapping
• 3. EKF-SLAM与3D激光中的IEKF
• 4. Visual SLAM的后端演化
• 5. 滑窗优化的理论基础
• 6. 三种后端策略的统一对比

1. 滤波与优化：两种后端的哲学差异

1.1 处理历史信息的方式

滤波方法（Filtering）：

• 维护当前时刻的状态后验（通过边缘化丢弃所有历史变量）
• 每个时刻执行 predict → update → marginalize 的三步循环
• 历史线性化误差无法修正（一旦边缘化，历史信息被"压缩"且不可逆）
• 优点：状态规模恒定，适合严格的在线实时估计

优化方法（Smoothing/Optimization）：

• 维护一段窗口内的完整状态（所有位姿+路标）
• 在新信息到来时重新优化整个窗口
• 可以回头修正过去的估计（回环修正的本质）
• 优点：精度远高于滤波；代价：状态规模随时间增长

1.2 三种SLAM中的后端选择

关键问题：为什么滤波在2D/3D激光SLAM中仍然占有一席之地，但在视觉SLAM中几乎被优化完全取代？

2. 2D激光后端：RBPF与GMapping

2.1 Rao-Blackwellization分解

GMapping的核心是RBPF（Rao-Blackwellized Particle Filter）。它利用了2D激光SLAM的一个关键条件独立性：

给定完整的机器人轨迹x 1 : t \mathbf{x}_{1:t}x1:t​，栅格地图的各栅格的后验估计是条件独立的。

数学表述为：

p ( x 1 : t , m ∣ z 1 : t , u 1 : t ) = p ( x 1 : t ∣ z 1 : t , u 1 : t ) ⏟ 粒子滤波估计轨迹 ⋅ ∏ c ∈ cells p ( m c ∣ x 1 : t , z 1 : t ) ⏟ 独立估计每个栅格 p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) = \underbrace{p(\mathbf{x}_{1:t} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})}_{\text{粒子滤波估计轨迹}} \cdot \prod_{c \in \text{cells}} \underbrace{p(\mathbf{m}_c \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t})}_{\text{独立估计每个栅格}}p(x1:t​,m∣z1:t​,u1:t​)=粒子滤波估计轨迹p(x1:t​∣z1:t​,u1:t​)​​⋅c∈cells∏​独立估计每个栅格p(mc​∣x1:t​,z1:t​)​​

FastSLAM对应的是路标的条件独立性（给定轨迹后各路标独立），GMapping对应的是栅格的条件独立性。

2.2 为什么RBPF在2D激光中有效

RBPF在2D激光SLAM中运作良好的三个条件：

(1) 低维状态空间：轨迹是SE(2)（3维）。每个粒子仅需维护3维的轨迹变量——粒子滤波在低维空间中有效，在高维空间中（如视觉SLAM的6维+数百个路标）会遭遇维度灾难。

(2) 高精度观测：激光测距精度~1-3cm，信噪比极高。高精度观测意味着似然函数p ( z t ∣ x t , m ) p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m})p(zt​∣xt​,m)高度集中——绝大多数粒子的权重接近零，仅少数粒子匹配当前观测。这使得粒子滤波能高效地收敛。

(3) 提议分布改进：GMapping的关键贡献是改进的提议分布。标准RBPF从里程计运动模型采样（提议分布宽），导致大多数样本在似然函数下权重极低。GMapping先用扫描匹配（ICP）得到一个高斯近似的最优建议分布x t ∼ N ( x t ICP , Σ ICP ) \mathbf{x}_t \sim \mathcal{N}(\mathbf{x}_t^{\text{ICP}}, \mathbf{\Sigma}^{\text{ICP}})xt​∼N(xtICP​,ΣICP)，从中采样。这使得有效样本数大幅提升——同等粒子数下覆盖的模式更多。

2.3 RBPF vs 位姿图优化的权衡

Cartographer最终取代GMapping成为2D激光SLAM的主流，正是因为位姿图优化在处理大规模地图和回环修正上的压倒性优势。

3. EKF-SLAM与3D激光中的IEKF

3.1 EKF-SLAM的不一致性根源

EKF-SLAM最严重的问题是不一致性（Inconsistency）——总是低估真实的不确定性（过于自信）。

三个理论根源（Huang & Dissanayake, 2006）：

(1) 线性化点漂移（Linearization Point Drift）

EKF中均值每帧更新后变化，但协方差矩阵中的块是在旧的线性化点处计算得到的。当均值移动后，这些旧线性化点不再准确——基于它们的协方差无法正确反映新均值处的不确定性。

(2) 观测模型的非线性

视觉SLAM的透视投影和3D激光的点到线/面距离都是非线性函数。EKF的一阶泰勒展开在非线性强的区域产生系统性的线性化偏差，这个偏差在每次更新中累积。

(3) 为什么优化方法不存在这个问题

图优化方法在每次迭代中在当前的线性化点重新评估所有残差和雅可比。关键是：可以回到同一个变量和不同的线性化点重新做线性化——EKF一旦边缘化历史位姿，就永久性地失去了重新线性化的能力。

3.2 FAST-LIO2的IEKF：迭代缓解不一致性

FAST-LIO2使用**迭代扩展卡尔曼滤波（IEKF）**来缓解EKF的不一致性：

标准EKF：在当前均值处做一次线性化和一次更新。

IEKF：在更新步中做多次迭代。每次迭代中：

1. 在当前（已更新）的均值处重新计算观测模型的雅可比H k \mathbf{H}_kHk​
2. 用新的H k \mathbf{H}_kHk​重新计算卡尔曼增益和更新量
3. 重复直到收敛

这相当于在后验均值处做线性化（而非先验均值处），减少了线性化误差。

IEKF与优化的等价性：IEKF的迭代更新等价于在当前状态处做一个高斯-牛顿迭代——它用后验均值作为新的线性化点。这在一定程度上弥合了滤波和优化之间的理论鸿沟。但IEKF仍然不如全优化，因为它继承了滤波的"边缘化不可逆"问题——一旦某帧被边缘化，它的线性化点就被永久锁定。

3.3 为什么IEKF在3D激光中保持竞争力

FAST-LIO2的IEKF在局部里程计（无回环）中提供与全优化相当甚至更好的精度和速度。但当需要全局一致性和回环修正时，因子图优化（LIO-SAM）的架构更优。

4. Visual SLAM的后端演化

4.1 从EKF到BA的范式转换

视觉SLAM的后端经历了从滤波到优化的范式转换：

• 2000-2008：EKF-SLAM（Davison, 2003）。单目EKF-SLAM，状态向量包含相机位姿+所有路标。随着路标从数十增长到数千，O ( N 2 ) O(N^2)O(N2)的协方差更新变得不可行（3000个路标→9000维协方差→每步更新~8 × 10 7 8 \times 10^78×107次运算）。
• 2007-2014：PTAM的分叉架构 + BA。PTAM证明了BA可以在普通PC上实时运行。Keyframe-based BA（仅优化关键帧）大幅减少了优化频率和规模。
• 2015-至今：ORB-SLAM3的全BA + 位姿图优化。局部BA（共视图内）+ 全局位姿图（回环后）+ 全BA（回环精化）的三层优化策略。

4.2 为什么视觉SLAM必须使用优化

视觉SLAM不使用滤波的根本原因不是精度——而是路标的数量级：

• 2D激光RBPF：粒子维护的是栅格地图（固定的分辨率数组），O ( M ⋅ cells ) O(M \cdot \text{cells})O(M⋅cells)，不随路标数增长
• 3D激光IEKF：状态向量仅含位姿+偏置+少数活跃特征，O ( 20 2 ) = 400 O(20^2) = 400O(202)=400维
• 视觉EKF-SLAM：状态向量含所有路标，O ( n 2 ) O(n^2)O(n2)复杂度，n ≈ 5000 n \approx 5000n≈5000→ 完全不实时

视觉BA的O ( m 3 + n ) O(m^3 + n)O(m3+n)复杂度（经过Schur消元）使得在5000+路标下仍然可实时求解。

5. 滑窗优化的理论基础

5.1 滑窗优化 = 精度与效率的折中

滑窗优化是在全批图优化（精度最高、计算量随窗口增大）和滤波（计算量恒定、精度受限）之间的工程折中。维护最近k kk帧（k kk通常7-15），每新帧到来时：

1. 优化窗口内所有变量（位姿+路标/偏置）
2. 边缘化超出窗口的最老变量（通过Schur补保留先验信息）

5.2 三种SLAM中的滑窗策略

5.3 边缘化 = 信息保留 + Fill-in代价

边缘化通过Schur补保留被移除变量的历史信息。代价是引入填充（Fill-in）：

p ( x in ) ≈ N ( x ^ in , Σ in − Σ in,old Σ old,old − 1 Σ old,in ) p(\mathbf{x}_{\text{in}}) \approx \mathcal{N}(\hat{\mathbf{x}}_{\text{in}}, \mathbf{\Sigma}_{\text{in}} - \mathbf{\Sigma}_{\text{in,old}}\mathbf{\Sigma}_{\text{old,old}}^{-1}\mathbf{\Sigma}_{\text{old,in}})p(xin​)≈N(x^in​,Σin​−Σin,old​Σold,old−1​Σold,in​)

三种SLAM中fill-in的可接受度差异：

• 2D激光：不边缘化→无此问题（每帧仅3维的全历史保留可行）
• 3D激光：每帧15维+50-100帧滑窗→fill-in是主要瓶颈→定期重置窗口丢弃先验
• 视觉：路标的3×3对角块使得边缘化路标fill-in最小；边缘化旧帧fill-in大→优先边缘化路标

5.4 线性化点锁定问题

边缘化同时"锁定"了被移除变量的线性化点。如果这些变量的线性化在当前更准确的点下是有偏的，这部分偏差会被永久保留在先验因子中——无法被后续优化修正。这与EKF不一致性的根源完全相同。

实践中的缓解策略：

• DSO的动态边缘化：根据运动大小选择边缘化对象
• VINS-Mono的边缘化策略：仅边缘化路标（fill-in最小，线性化点锁定影响小）
• 定期重置滑动窗口：丢弃先验中的累积线性化误差

6. 三种后端策略的统一对比

6.1 方法谱系

三种SLAM的后端构成了一个从纯滤波到纯优化的连续谱：

纯滤波 ←——————————————————————→ 纯优化 │ │ │ EKF-SLAM RBPF(GMapping) 全BA(视觉) │ IEKF(FAST-LIO2) │ │ 滑窗BA(DSO/VINS) │ │ 位姿图(Cartographer) │ └─ 状态恒定 ──── 有限增长 ──── 全历史 ─┘

6.2 选择后端的决策因素

6.3 核心论点

滤波和优化的本质差异不在于框架本身（两者都在向信息矩阵中添加因子），而在于：

1. 滤波：每次添加因子后立即边缘化全部历史——状态恒定但线性化误差不可逆
2. 优化：保留全部或部分历史——可以重线性化修正误差，但计算量增长

2D激光能使用滤波（RBPF）是因为：低维状态空间（SE(2), 3维）使得全历史保留（优化）和部分历史保留（RBPF）的精度差异不大——激光的高信噪比导致了"滤波已足够好"的局面。

视觉必须使用优化是因为：高维状态空间+低信噪比→单次线性化的误差不可接受→需要反复重线性化→优化是唯一途径。

本文总结

• 三种SLAM的后端方法由传感器特性和状态维度共同决定：2D激光（低维+高信噪比→RBPF可行）→ 3D激光（中维+高信噪比→IEKF/因子图并行）→ 视觉（高维+低信噪比→必须BA）
• RBPF在2D激光中有效的三个前提：低维状态空间（SE(2), 3维）、高精度观测（激光~1cm）、改进提议分布（GMapping先做ICP缩小采样范围）
• EKF不一致性的根源：线性化点漂移 + 历史线性化误差无法修正——IEKF通过在后验均值处迭代更新缓解了这一问题
• FAST-LIO2的IEKF= 滤波和优化之间的桥梁——迭代重线性化使滤波性能接近优化，但仍无法修正已边缘化的历史
• 视觉必须用BA：n ≈ 5000 n \approx 5000n≈5000个路标 × EKF的O ( n 2 ) O(n^2)O(n2)协方差更新 = 数学上完全不可行；Schur消元+稀疏BA将复杂度降为O ( m 3 + n ) O(m^3 + n)O(m3+n)
• 滑窗 = 精度vs效率的工程折中：边缘化保留信息但引入fill-in——三种SLAM根据状态维度选择不同的边缘化策略

标签

SLAMRBPFEKFIEKF滑窗优化后端