IMU和视觉融合这个话题我前前后后啃了大半年从纯视觉SLAM转过来的时候一开始真没把这颗“小陀螺”当回事。直到在暗光、快速旋转、纯纹理缺失的走廊里被现实狠狠教育了几轮才明白IMU和视觉融合不是简单的“把数据拼在一起”而是一整套从标定到初始化的系统工程。这篇笔记不是教科书式的理论整理更多是我在实际跑数据集、调参、排查问题过程中沉淀下来的东西。内容会覆盖IMU和视觉融合的动机、IMU内参标定、相机与IMU联合标定、IMU与雷达外参标定、重力对齐、位姿解算中yaw慢漂的根源以及我踩过的各种坑。适合正在入门多传感器融合、或者已经在跑VINS、ORB-SLAM3这类开源方案但知其然不知其所以然的朋友。1. 为什么非要把IMU和视觉放在一起单目相机最怕两件事一是快速运动时帧间特征点脱靶二是纯旋转场景下没有视差三角化直接失效。IMU恰好相反它以几百赫兹的频率输出角速度和加速度不依赖外部环境短时间内的位姿积分非常平滑。但IMU也有它的硬伤——加速度计测不到重力方向以外的绝对位置陀螺仪的零偏会随温度和时间缓慢变化纯积分出来的轨迹几秒之后就开始飘。视觉和IMU的互补性就在这里体现出来。视觉提供低频但绝对的地图约束IMU提供高频但相对的运动预测。两者融合之后不仅能把运动估计频率从30帧提升到200赫兹以上还能在相机短暂遮挡、快速旋转的场景里维持稳定的姿态输出。另一个容易被忽略的好处是IMU的加速度计能够观测重力方向这为单目视觉提供了真实的尺度信息。没有IMU的单目SLAM轨迹和地图只有相对尺度乘以任意常数都不违反重投影约束而一旦引入重力方向尺度就被唯一确定了。我个人的体会是融合的关键不是“把两个估计结果平均一下”而是搞清楚在什么条件下该信谁。低纹理场景信视觉会崩高速运动信纯积分会飞融合的目的是让两种信息源的置信度在优化框架内自动权衡。1.1 传感器各自的“脾气”要先摸清IMU的核心参数有两个输出频率和噪声特性。消费级IMU常见的输出频率在200到1000赫兹工业级或专用组合导航上可以到2000赫兹以上。频率越高短时间积分的精度越好但计算压力和数据处理复杂度也越高。视觉SLAM里常用的IMU输出频率大概在200到400赫兹之间既能满足前端跟踪又不会让后端优化压力太大。噪声特性我习惯分三部分看零偏稳定性、角度随机游走、速度随机游走。零偏稳定性描述的是IMU静止时输出围绕真值的波动程度角度随机游走对应陀螺噪声积分后的角度不确定度速度随机游走则对应加速度计噪声积分后的速度不确定度。这些值通常通过Allan方差分析得到也就是后面要讲的IMU内参标定的核心输出。相机这边常见的是全局快门和卷帘快门两种。全局快门所有像素同一时刻曝光运动畸变很小适合和IMU做紧耦合卷帘快门则是逐行曝光快速运动时图像会产生明显的扭曲。做视觉惯性融合前最好先确认相机的快门类型卷帘快门在高动态场景下会引入额外的测量误差差的CMOS相机在剧烈运动时甚至会出现“果冻效应”整个图像像被揉过一样。1.2 融合能解决哪些具体痛点视觉惯性融合最典型的三类应用场景是无人机室内定位、AR设备姿态跟踪、自动驾驶城市复杂工况。无人机在GPS拒止环境里靠纯视觉飞在光线不足或地面纹理重复的室内会迅速丢失。加上IMU后即使视觉特征短暂失效系统也能靠IMU短时间积分维持位姿一般能撑过几百毫秒到一两秒足够无人机调整姿态或找回视觉信息。AR设备上用户快速转头时相机可能只拍到白墙或者运动模糊严重纯视觉系统会掉帧。IMU能在图像与图像之间提供精确的姿态增量让虚拟物体在真实世界里稳定悬停不会有明显的拖影或错位。自动驾驶场景里地面车道线有时候会消失高楼遮挡下的GNSS信号也会跳变IMU提供的高频运动信息可以补上这些缺口让车辆在桥梁下、隧道里依然有平滑的车身姿态估计。这些场景的共通逻辑是视觉负责“认路”IMU负责“稳住”两者配合才能覆盖比单一传感器更宽的工况边界。2. 标定是融合的第一步绕不开我没见过哪个视觉惯性系统能跳过标定直接跑出好效果。IMU内参标定、相机内参标定、IMU-相机外参标定、IMU-雷达外参标定这四件事如果做不好后面的融合全是空中楼阁。标定的本质是建立传感器输出与真实物理量之间的映射关系。IMU内参标定解决的是陀螺仪和加速度计自身的偏差、尺度误差和轴间对准问题相机内参标定解决的是镜头畸变和焦距、主点IMU-相机外参标定解决的是两个传感器在空间上的旋转和平移关系IMU-雷达外参标定解决的是IMU与激光雷达坐标系的对齐。2.1 IMU内参标定把bias和噪声密度搞清楚IMU内参标定主要做两件事估计确定性误差bias、尺度因子、安装误差和随机误差噪声密度、随机游走。确定性误差可以通过六面静止法或多位置法标定更常用的方式是匀速旋转和静止交替采集然后用优化方法拟合。随机误差则通过Allan方差分析来求。Allan方差分析的基本思路是对长时间静止的IMU数据做不同时间间隔的方差统计得到的方差-时间对数曲线里曲线斜率能对应不同类型的噪声源。斜率是-1的部分对应量化噪声斜率是-1/2的部分对应角度/速度随机游走斜率是为0的底部对应零偏不稳定性斜率是1/2的部分对应速率随机游走斜率是1的部分对应速率斜坡。实操中用imu_utils这个开源工具比较靠谱。采集2小时以上静止的IMU数据用rosbag录制然后通过imu_utils生成Allan方差曲线最终输出陀螺仪和加速度计的噪声密度、随机游走系数和零偏不稳定性。这里有几个注意事项IMU需要被稳定固定不能有振动源最好放在大理石桌面或光学平台上bag录制过程中不要触碰传感器也不要改变室内温度温度漂移会让结果失真。2.2 IMU和相机联合标定kalibr的完整流程Kalibr是目前应用最广的相机-IMU联合标定工具其核心思路是通过观测标定板在图像中的运动结合IMU的角速度和加速度测量联合优化相机内外参、IMU内参和它们之间的外参关系。实际标定时我强烈建议至少准备一个7x6或6x6的Apriltag棋盘格标定板尺寸在A2或A3以上打印之后贴在平整的硬板上。标定过程分三步先用棋盘格标定相机内参包括焦距、主点、径向畸变和切向畸变。Kalibr自带的camchain工具可以对单目、双目、鱼眼相机分别处理。然后相机和IMU固定在一起手持或放在云台上做充分的六自由度运动。运动激励要足够丰富包括沿着各个轴的平移、旋转、往复运动同时不能太快否则图像运动模糊会毁掉标定点提取。把录制的bag交给kalibr_calibrate_imu_camera设置好相机模型、相机内参文件、IMU参数文件、目标标定板参数跑完后会得到IMU-相机的外参旋转四元数和平移向量以及优化后的IMU内参修正量。我这里踩过的一个大坑是标定板歪斜。一开始图省事把标定板贴在墙上结果墙面不垂直标定出的平移量偏差很大。后来换成三脚架架标定板保证标定板表面与地面基本垂直IMU和相机在运动时保持相对静止结果好了很多。还有一个常见的坑是标定过程中IMU的角速度激励不足。很多人手持设备来回平移但旋转幅度很小。这样IMU的陀螺仪在优化里贡献非常有限外参的旋转部分会很不稳定。正确的做法是让设备尽量多做大角度往复旋转每轴至少旋转正负45度以上。2.3 IMU和雷达外参标定顺带补充一下标题虽然聚焦在视觉融合但最近圈子里的热词也常把lidar和imu标定放在一起说。摄像头和雷达都是感知世界的传感器但一个是光的感知一个是飞行时间的感知两者坐标系之间的关系完全依赖外参标定。雷达-IMU外参标定的常用方法有两类。一类是基于运动约束的标定让设备做充分的旋转和平移运动利用雷达成像中获得的手眼标定关系来求解外参另一类是基于特征的标定利用环境中自然存在的平面特征或角点特征将里程计轨迹与雷达点云中的平面进行匹配从而估计外参。我个人的体会是如果没有自建系统的充分时间直接用开源的lidar_camera_calib这类工具会很省事。它通过给定的初始外参在点云和图像之间做边缘对齐和发散度最小化能获得亚像素级的外参优化结果。先粗对齐再精优化这是通用的流程。2.4 重力对齐初始化阶段绕不开的关键步骤重力对齐很多人第一次听到这个概念时不太明白到底在做什么。简单来说视觉惯性系统的世界坐标系通常被定义为与重力方向对齐。IMU加速度计在静止时测得的输出就是重力矢量通过它可以确定世界坐标系的Z轴方向从而完成系统与世界物理坐标系的对齐。在VINS-Mono这类方案里初始化阶段会执行“视觉惯性对齐”其中核心就是对重力向量g进行估计。系统先用纯视觉SFM估算出初始的运动和地图然后通过IMU预积分和视觉观测之间的残差联合求解尺度、重力向量、速度等参数。在实际工程中重力对齐的质量直接决定系统启动初期的高度稳定性。如果重力估计有偏差即使视觉和IMU的融合精度再高系统在垂直方向上的轨迹也会产生系统性偏差地面上跑着的机器人会像在爬山一样缓慢爬升。3. 核心算法链路拆解融合方案里IMU和视觉通常以“前端跟踪后端优化”的结构配合。前端跟踪负责高频的状态预测和视觉特征匹配后端优化负责在滑窗或全局范围内对状态做最优估计。整个系统里IMU承担的任务可以概括为在每帧图像之间对系统状态进行预测同时为后端的因子图提供惯性约束。这个流程听起来简单但其中的“检测逻辑”和“预积分”细节非常丰富。3.1 IMU检测逻辑与数据预处理IMU检测逻辑指的是系统在使用IMU测量值之前做的数据有效性检查。常见的检查项包括数据时间戳是否连续、是否有重复帧、加速度计是否饱和、陀螺仪测量是否超出量程、零偏是否发散。我遇到过一种很隐蔽的问题在某些低成本IMU里当温度快速变化时陀螺仪的零偏会缓慢漂移这个漂移在几秒内看起来像是正常的运动信号系统很难察觉。处理办法是在IMU驱动层维护一个滑动窗口持续监控窗口内的均值变化如果检测到明显的变化趋势就主动触发零偏重置或给后端一个提示信号。数据预处理的另一个重点是时间同步。视觉和IMU的时间戳不可避免存在偏差哪怕只有几毫秒在高速旋转场景里也会产生明显的角度误差。常用的处理方式是硬件同步或者通过软件在线估计时间偏移Kalibr里就有时间偏移估计的功能标定完成后的时间偏移量要记录好融合代码里要用到。3.2 IMU预积分从“每次都重积分”到“只算一次”预积分的核心解决的是后端的计算效率问题。如果IMU姿态、位置在每次优化迭代后都需要重新积分计算量会爆炸因为后端的非线性优化往往要迭代几十次。预积分的思想是把两次图像帧之间的所有IMU测量值预先积分成一个相对增量这个增量独立于每一帧的绝对位姿只是在线性化点附近提供约束。也就是说预积分量可以在优化之前一次算好然后在每次迭代时只做线性更新从而大幅减少重复计算。VINS和ORB-SLAM3的IMU因子都采用了预积分模型区别在于VINS对加速度偏置和陀螺仪偏置进行了在线估计并在每次滑窗优化时更新预积分量的一阶近似。ORB-SLAM3则在IMU因子和视觉因子之间做了更严密的不确定性传递。理解预积分最关键的是知道它把IMU测量转换为“相对测量”并且给出了这个相对测量的噪声协方差后续的优化才真正可信。3.3 位姿解算为什么yaw会慢漂这是我在实践中最关注的问题也是网络上讨论热度非常高的问题基于IMU的位姿解算中yaw仍然会缓慢漂移。这个问题的根源在于IMU本身缺乏对yaw的绝对观测。重力方向为系统约束了roll和pitch两个自由度但是yaw旋转是绕着重力方向旋转的加速度计无法区分“绕重力轴的旋转”和“水平运动”导致的加速度变化。因此系统对yaw的估计完全依赖角速度积分而角速度积分会把陀螺仪的零偏随时间积累成角度误差。这种漂移是缓慢但持续存在的即使视觉-IMU融合系统已经工作的很好只要运行时间足够长yaw还是会慢慢飘走。视觉环路检测可以修正一部分漂移但前提是系统能够识别到曾经去过的位置。在没有环路检测的场景下长时间运行后yaw的累积漂移会成为一个无法忽视的瓶颈。降低yaw漂移的常用手段有几类。一是提高IMU内参标定精度让零偏尽量小二是使用更高精度的MEMS陀螺仪比如光纤陀螺或环形激光陀螺但成本较高三是在线估计并补偿陀螺零偏让系统在运行过程中不断修正四是融合磁力计作为航向参考但磁力计很容易被环境磁场干扰五是利用其他绝对航向观测源比如卫星导航的航向信息或视觉特征中的主方向。我个人的建议是在工程项目里两条腿走路一方面做好IMU内参标定保证系统本身的低漂移特性另一方面设计好应用场景的约束避免长时间无校正的自由运动。对于长时间运行的机器人最好周期性触发闭环修正或外部航向参考更新。4. 实操一套可落地的联合标定流程前面讲了这么多理论这里分享一套我实测下来比较稳定的标定流程直接对着做基本可以复现。4.1 环境准备与工具链我使用的是Ubuntu 20.04 ROS Noetic的环境。工具链包括imu_utilsIMU内参标定与Allan方差分析code_utilsimu_utils的依赖kalibr相机内参标定、IMU-相机联合标定livox_camera_calib或lidar_camera_calib如果涉及雷达可做雷达-相机外参标定这里有个小提醒kalibr的编译对依赖要求比较苛刻通常需要提前装好Eigen3、OpenCV、Ceres、Sophus等库。要是编译不顺利先检查环境里的OpenCV版本是否与kalibr兼容建议使用3.x的OpenCV能少很多麻烦。4.2 数据采集要点采集设备前先确认三件事情IMU是否已经预热标定板是否平整且固定好相机镜头是否干净。IMU预热的意思是先把设备通电运行一段时间让内部的温度稳定下来这样标定出来的bias更贴近实际运行状态。一般我用静态采集2小时的数据做Allan方差同样保证环境温度稳定。相机和IMU联合标定的数据采集运动设计很关键。我总结了一个“三二一”原则三个慢速大范围平移沿X轴、Y轴、Z轴各往复一次让加速度计充分激励。两个中速旋转序列绕相机光轴和水平轴分别做往返旋转让陀螺仪有足够的激励。一个综合运动序列慢速绕8字、快速甩动组合让系统观测到足够的运动变化。整个采集过程建议控制在3到5分钟太短会激励不足太久又容易让用户手动操作变得疲劳导致动作走形。4.3 标定结果怎么看Kalibr跑完之后会输出一个类似“camchain-imucam.yaml”的文件里面包含相机参数、IMU参数、外参旋转四元数和平移向量。我重点看几个指标重投影误差一般应该在0.2到0.5个像素以内如果大于0.8像素说明标定板的数据质量有问题可能需要重新采集。IMU激励是否充分Kalibr的输出会显示IMU加速度与角速度的激励范围如果某个轴的激励非常小外参在这个轴上的不确定性会很大官方文档里的提示是“warning: high covariance”这时候要补数据。外参的不确定性Kalibr会输出协方差矩阵如果某些方向的协方差特别大说明标定结果在这一维上不可信。我自己跑完一次标定后还会做一个快速验证手持设备做30秒的旋转运动把标定后的外参代入系统观察视觉重投影结果是否稳定。如果重投影稳定、没有明显的横滚或俯仰漂移说明外参比较可信。5. 常见问题与排查实录这里把我在实践中遇到的几个典型问题整理成一张速查表后面详细展开说明。现象可能原因排查方向初始化时尺度错误重力估计不准、激励不足检查IMU数据采集质量补做旋转激励运动过程中高度漂移重力对齐不好、IMU零偏估计不准重新做IMU内参标定和重力对齐融合后yaw持续漂移陀螺零偏在线估计不收敛检查IMU内参、yaw漂移量与时间的关系标定结果有重投影残差标定板不平整、运动过快换标定板、降低运动速度、增加运动幅度时间戳不同步导致误差相机和IMU时间戳偏差大做时间偏移在线估计或硬件同步开机初期系统不稳定初始化阶段重力收敛慢延长启动初始化时长或让设备在启动时静止数秒5.1 标定结果差先怀疑采集动作我刚开始学kalibr时特别追求一次把标定跑完恨不得全程用三脚架固定设备只做缓慢平移。结果每次输出的协方差都很大重投影误差也偏高。后来看了kalibr作者的公开教程才明白标定的核心是“激励”不是“稳定”。设备的运动要覆盖到所有六个自由度。比如平移时要尽量多方向旋转时要绕不同轴不要总是绕同一个轴转。还有一点是运动速度要变化慢速、中速、快速交替这样IMU的加速度和角速度信号才有丰富的频率成分才能把bias和外参估计出来。如果你发现标定结果里外参旋转四元数的模不是1或者平移向量非常大那大概率是数据采集出了问题。回到原始数据看一眼是不是图像太模糊标定点提取失败太多或者IMU数据有大量dropout。5.2 初始化不稳定注意启动静止期很多视觉惯性系统要求设备在启动初期保持静止数秒这不是软性建议而是硬性要求。静止期的目的是给IMU的偏置一个初始估计也给加速度计的重力方向一个清晰的观测。VINS-Mono里如果初始化失败往往和启动初期运动太快有关。我在实际测试时发现启动时的最初2到3秒静止能让初始化的成功率大幅提升特别是在光照变化明显的场景里。静止期IMU数据的均值和方差还会作为后续零偏先验被放进优化框架中。5.3 如何甄别yaw漂移的来源假设你已经发现yaw在慢慢漂这时候最关键的是弄清楚漂移是来自IMU内参误差、外参误差还是算法层面的问题。一个简单有效的实验是将设备固定在桌面上不动记录IMU原始数据和融合后的姿态输出观察yaw角是否随时间线性增长。如果yaw在完全静止条件下仍然线性增长那基本可以断定是陀螺零偏估计或补偿没有收敛需要重新标定。如果静止时yaw稳定但运动一会儿后yaw出现非线性变化那更可能是外参标定误差或者预积分处理中的时间同步问题。我在测试一款工业级IMU时通过静止实验发现yaw在20分钟内漂移了5度换算下来是每秒0.004度看起来很小但对长时间运行的SLAM系统来说这个累积量在1小时后会达到15度足以让地图和真实世界对不上。后来重新做了IMU内参标定并启用在线零偏估计漂移量降低到了每小时0.5度左右。6. 学习路径上的几点建议最后聊一些我个人的学习体会。如果你也是从零开始学IMU和视觉融合我的建议是先搞清楚一条链为什么需要标定标定到底在标什么标定结果如何影响系统表现然后再去跑代码。我就是一开始心太急直接上手跑VINS-Mono结果在初始化失败、yaw漂移和标定残差里反复打转。后来耐下心把IMU内参标定、相机-IMU联合标定、预积分原理逐块啃明白后再回头看那些报错和异常现象思路就清晰多了。一个很值得投入时间的实验是在同一个数据集上分别用默认IMU参数、手动调参和标定得到的IMU参数跑三遍系统对比轨迹误差。你会直观感受到标定带来的差异有时候这个差异比换一版更强的优化算法还要明显。实操中我还养成了一个习惯每次标定完成后都顺手把IMU的零偏、噪声密度、外参旋转四元数记录到一个小本子上并随手标注采集时的环境温度、设备型号。不同温度下的零偏差异有时会超过标称值这些记录在后来的系统调优里帮了大忙。最后再分享一个小技巧采集标定数据时别忘了在rosbag里同时录下相机的曝光时间信息。相机在曝光时间内相当于一个移动快门会产生运动模糊。如果曝光时间太长即使标定板静止图像里的标定点也会因为设备运动而变得模糊。控制曝光时间在5毫秒以内标定点提取会更稳定标定结果的重复性也会好很多。