📚 课程目录第4章单机器人SLAM回顾

4. 单机器人SLAM回顾

多机器人地图共享与协同定位并不是凭空出现的新技术，它的“地基”仍然是单机器人SLAM。只有先理解单机器人如何在未知环境中同时估计自身位姿与地图，才能进一步理解多机器人之间为什么要共享地图、如何融合各自的估计、以及一致性为何如此困难。本节回顾三类经典单机器人SLAM方法：扩展卡尔曼滤波SLAM、粒子滤波SLAM和图优化SLAM，重点比较它们的概率建模方式、状态表示、更新流程与适用边界。

一、SLAM的统一概率视角

SLAM的本质是估计后验概率 p(x_1:t, m | z_1:t, u_1:t)，其中 x 表示机器人位姿轨迹，m 表示地图，z 是观测，u 是控制输入。不同SLAM方法的区别，主要在于如何表示这个后验、如何增量更新、以及如何控制计算复杂度。

核心矛盾：位姿与地图相互耦合——定位需要地图，建图需要定位。SLAM方法本质上都在解决“先有鸡还是先有蛋”的循环依赖问题。

二、扩展卡尔曼滤波SLAM（EKF-SLAM）

EKF-SLAM把机器人位姿和所有路标点放入同一个状态向量：x = [x_r, y_r, θ_r, m_1x, m_1y, …, m_nx, m_ny]。它假设状态服从高斯分布，用均值向量和协方差矩阵描述不确定性。由于观测模型和运动模型通常非线性，EKF通过一阶泰勒展开进行线性化。

其流程可概括为：

预测：根据运动模型 x_t = g(x_t-1, u_t) 传播状态均值，并用雅可比矩阵 G 传播协方差：Σ' = GΣG^T + R。
观测更新：对已观测路标计算预测观测 ẑ = h(x)，用雅可比矩阵 H 计算卡尔曼增益 K = Σ'H^T(HΣ'H^T+Q)^-1，更新状态与协方差。
新路标初始化：将新观测到的路标加入状态向量，并扩展协方差矩阵。

EKF-SLAM的优点是理论清晰、在线递推、适合小规模环境。但它的协方差矩阵规模随路标数量平方增长，计算复杂度约为 O(n²)；同时线性化误差在强非线性场景下容易导致不一致，甚至发散。

三、粒子滤波SLAM（FastSLAM）

粒子滤波SLAM采用另一种思路：用一组带权粒子表示机器人轨迹的后验分布，每个粒子携带一幅独立的地图。FastSLAM利用 Rao-Blackwellization 分解：p(x_1:t, m | z, u) = p(x_1:t | z, u) · Π p(m_i | x_1:t, z)。也就是说，轨迹用粒子采样，路标在地图条件独立假设下用EKF逐个维护。

其典型流程为：

采样：从运动模型和建议分布中生成新粒子。
观测更新：根据观测为每个粒子计算权重，权重反映该轨迹解释观测的程度。
重采样：淘汰低权重粒子，复制高权重粒子，缓解粒子退化。
地图更新：每个粒子用EKF更新其携带的路标估计。

FastSLAM能表示多峰分布，对非线性运动模型更鲁棒，且每个路标的更新复杂度为 O(M log n)，其中 M 为粒子数。但粒子数需求随状态维度增长，计算与内存开销大；重采样会带来粒子耗尽问题，长期运行需要谨慎设计建议分布。

四、图优化SLAM

图优化SLAM把SLAM建模为因子图：节点表示机器人位姿和路标，边表示运动约束或观测约束。每条边对应一个误差项，例如里程计约束 e_ij = z_ij ⊖ (x_i ⊕ x_j)，观测约束 e_ik = z_ik ⊖ h(x_i, m_k)。目标是求解所有节点使总误差最小：

min<sub>X</sub>  Σ<sub>e∈E</sub> e<sup>T</sup> Ω e
其中 Ω 为信息矩阵，X 为所有位姿与路标节点

图优化SLAM通常分为前端与后端。前端负责构建图：通过扫描匹配、特征匹配或ICP生成位姿间约束，并检测回环。后端负责优化图：利用稀疏性进行非线性最小二乘求解，常用方法包括高斯-牛顿、Levenberg-Marquardt，以及增量式求解器 iSAM、g2o、Ceres、GTSAM 等。

图优化SLAM的优势在于：能够全局优化、有效利用回环消除累积误差、对大规模环境扩展性好。它的代价是：需要存储和维护图结构，回环检测错误会引入错误约束，后端优化需要良好的初值和鲁棒核函数。

五、三类方法对比

方法	状态表示	核心更新	复杂度	优势	局限
EKF-SLAM	高斯均值+协方差	预测-观测更新	O(n²)	递推清晰、在线性好	线性化误差、规模受限
FastSLAM	粒子轨迹+每粒子地图	采样-加权-重采样	O(M log n)	多峰、非线性鲁棒	粒子退化、内存大
图优化SLAM	因子图节点+边	前端建图+后端优化	稀疏求解	全局一致、回环强	依赖回环质量、后端复杂

六、案例场景：室内巡检机器人

假设一台巡检机器人在办公楼走廊运行。若走廊短、路标少，EKF-SLAM可以快速给出位姿与路标估计；若环境动态强、观测噪声大，FastSLAM的多假设特性更稳健；若机器人需要绕楼层多圈并回到起点，图优化SLAM通过回环约束能显著修正漂移，得到全局一致的地图。实际系统常采用“前端图优化+后端回环”的架构，而EKF与粒子滤波则更多用于局部状态估计或作为前端的一部分。

在多机器人系统中，单机器人SLAM的输出通常包括：位姿轨迹、局部地图、协方差或信息矩阵、关键帧与约束。这些量正是后续地图共享与协同定位的输入。

七、常见误区与复盘

误区一：认为EKF-SLAM只是“卡尔曼滤波加地图”。实际上，状态维度和协方差传播方式决定了它与普通KF有本质区别。
误区二：认为粒子滤波一定比EKF好。粒子滤波在高维状态中会遭遇维数灾难，粒子数不足时反而更差。
误区三：认为图优化SLAM不需要概率建模。因子图中的信息矩阵正是概率模型的具体体现，优化目标等价于最大后验估计。
误区四：忽略一致性。无论哪种方法，若协方差被低估，多机器人融合时都会产生过度自信的错误结果。

回顾这三类方法，可以看到一条清晰的技术演进线：从EKF的“单峰高斯递推”，到FastSLAM的“多假设粒子采样”，再到图优化的“全局因子图求解”。它们分别代表了在线递推、蒙特卡洛采样和批量优化三种思想。多机器人协同定位并不是替代这些方法，而是在它们之上增加“机器人间约束”和“地图融合”两个维度。理解单机器人SLAM的状态表示、更新流程与不确定性传播方式，是后续学习多机器人地图共享、相对观测融合与分布式优化的必要前提。