5.4 多传感器融合 SLAM

5.4 多传感器融合 SLAM

本节摘要：单一传感器各有先天缺陷——单目缺尺度、视觉怕黑暗、IMU 漂移、激光怕走廊。融合按耦合深度分松紧两档，紧耦合的视觉惯性系统靠 IMU 预积分解决"高频信号如何进低频优化"的难题。本节讲清两种耦合的结构差异与预积分的直觉。

前两节的兵种各有软肋：视觉在黑暗、高速旋转下失跟；激光在长走廊里方向失明；而 IMU（惯性测量单元）高频、不依赖环境，却在数十秒内漂出几十米。没有全能传感器，只有互补的传感器组——融合不是可选项，是工程上的必然。

松耦合与紧耦合

松耦合：视觉模块与惯性模块各自独立输出位姿，再用一个滤波器（通常是扩展卡尔曼）融合两个结果。结构简单、模块可替换，但信息在"各自先压缩再交换"中损失——视觉丢掉的每一帧，惯性都无从知晓。

紧耦合：把 IMU 预积分量与视觉重投影误差放进同一个优化目标，联合估计状态。信息无损、精度上限高，是现代主流（视觉惯性系统的标配）。代价是系统复杂度、以及两个新工程问题：外参标定（相机与 IMU 的相对位姿）与初始化（尺度、重力方向、速度的联合求解）。

紧耦合"抗模块失效"是亮点：视觉短暂失跟的零点几秒里，IMU 预积分独自撑住状态估计，恢复后无缝接管——松耦合架构里这段就是纯漂移。

IMU 预积分：为"不同频率"搭桥

视觉前端每帧几十毫秒算一次，IMU 每秒几百次采样。若把两帧之间的全部 IMU 积分拆成上百个状态变量进优化，规模爆炸。预积分的洞见：把这段积分相对"上一时刻姿态"完成，得到一个与初始状态无关的观测量——优化器迭代改了初值，预积分不必重算，只需一阶修正。它把上百个高频样本压成一个携带协方差的复合观测，塞进低频的图优化里。

直觉版本：预积分像把两帧之间的 IMU 读数"打包成一段带保差的位移票据"，后端随时可以兑换，不必逐笔重算原始账。

实战演练：IMU 漂移有多快

背景：为什么 IMU 必须被融合而不能单干？用典型消费级 MEMS 规格算一笔漂移账。数值可复算。

# 消费级 MEMS IMU 的漂移账（规格书典型值）
import numpy as np

gyro_bias = 0.5 / 180 * np.pi / 3600     # 陀螺零偏 0.5 度/小时 -> rad/s
accel_bias = 0.02                        # 加计零偏 约 2 毫g 量级
for t in [1, 10, 60, 600]:
    angle_err = np.degrees(gyro_bias * t)      # 角度漂移
    # 位置误差 = 0.5 * a * t^2
    pos_err = 0.5 * accel_bias * t**2
    print(f'{t:4d} 秒: 姿态漂 {angle_err:.4f} 度, 位置漂 {pos_err:6.1f} 米')
# 输出:
#    1 秒: 姿态漂 0.0001 度, 位置漂   0.0 米
#   10 秒: 姿态漂 0.0014 度, 位置漂   1.0 米
#   60 秒: 姿态漂 0.0083 度, 位置漂  36.0 米
#  600 秒: 姿态漂 0.0833 度, 位置漂 3600.0 米
​

结果解读：一分钟纯惯性推算位置漂出三十六米，十分钟漂三点六公里——位置误差随时间平方增长，这就是"IMU 高频但短命"的含义。反过来，视觉每帧毫米级但只有几十赫兹：融合让 IMU 拿视觉当"零偏校准器"，视觉拿 IMU 当"高速补帧器"，各取所长。

工程实践要点

标定先行：相机与 IMU 的外参（旋转加平移）必须在系统运行前标好，在线自标定只做微调。外参旋转差零点五度，远处路标的重投影误差立即多出数个像素，紧耦合优化会"努力地解释"这个系统偏差，结果污染所有状态。重力对齐：初始化阶段通常先静置估计重力方向（加计在静置时读到的就是重力反方向），再做起始运动估计尺度——视觉惯性系统的启动流程（静置数秒、缓慢加速、避免纯旋转）不是玄学，是可观测性的要求。

⚠️ 常见坑：时间戳不同步。相机与 IMU 的时间戳差哪怕十毫秒，高速运动时等于把观测"挪"了几厘米的位置，紧耦合系统会持续性背锅。硬件同步触发或软件时间对齐，是融合系统的第一块地基。

💡 关键直觉：松耦合像"两个侦察兵各画各的图再拼"，紧耦合像"两人共用一张图边看边对"——后者吵了架（联合优化）反而画得更准。

传感器组合的常见配方

落地的融合系统很少超过三种传感器，常见配方有三张。视觉加 IMU：消费级设备的主流，成本最低，双目加惯性可在无 GPS 环境做到厘米级，弱纹理与高速旋转仍是短板。激光加 IMU 加轮速：工业机器人标配，激光供几何约束、IMU 供高频姿态、轮速计供走廊与隧道方向的尺度锚定，三者的软肋恰好互锁。视觉加激光加 IMU 加卫星定位：车载量产方案，卫星给绝对位置、几何传感器给局部精度，卫星隧道失效时靠惯性短撑。给自己项目选配方时，先列各传感器的失效清单，再看组合后是否仍存在"同时失效"的场景——有，就补最后一块拼图；没有，配方就定了。融合系统调参顺序也有讲究：先时间同步，再外参标定，最后才动各模块的噪声参数，顺序颠倒会让你在错误的参数空间里打转数周。最后记住 IMU 的选购常识：消费级 MEMS 与战术级的价差是两个数量级，但入门实验消费级完全够用——差异体现在零偏稳定性（每小时漂多少度）与出厂标定的温度补偿上；廉价模组开机预热三十秒再采数据、避开电机与电源的电磁干扰，效果立竿见影，这两个零成本动作常被新手忽略。

本节要点回顾

• 融合是必然：每个传感器的软肋恰好被另一个的强项覆盖；
• 紧耦合信息无损、能桥接模块短暂失效，是现代主流；
• 预积分把高频 IMU 打包成低频复合观测，是频率之桥；
• 标定与时间同步是融合系统两大地基，先于一切算法调参；
• IMU 位置误差随时间平方漂移，独木难支。

轨迹仍在累积漂移，远征即将回到出发点——下一节的回环校核是收官的账房先生。