D435i IMU标定全流程：从飘移到精准的VIO实战指南-育师

1. 从一次"飘移"事故说起：D435i的IMU到底标不标

去年帮一个做机械臂视觉抓取的朋友调系统，他用的就是Intel RealSense D435i，跑VINS-Fusion做视觉惯性里程计。现象很典型：机械臂慢速移动时轨迹还算正常，一旦快速转动或者做大幅度平移，建出来的地图就开始"飘"，回环检测也救不回来。他一开始怀疑是特征点太少、光照变化太大，折腾了快两周，换场景、调参数、加灯光，全都没用。

后来我让他把IMU的原始数据打出来看，问题一下就暴露了——加速度计和陀螺仪的噪声密度、随机游走参数全是默认值，而且相机和IMU之间的外参压根没标定过。VINS-Fusion这类紧耦合的VIO算法，对IMU的噪声模型和外参非常敏感，你给它喂一组"没校准过"的数据，它算出来的预积分和实际运动对不上，飘移是必然的。

这件事让我意识到，很多人上手D435i做SLAM，注意力全放在特征提取、回环、地图优化上，却忽略了最底层的一环：IMU标定。D435i出厂时IMU是经过一定校准的，但那个校准是在特定温度、特定批次条件下做的，而且相机和IMU之间的外参（也就是两者坐标系之间的旋转和平移关系）并没有给你一个高精度的值。你要做高精度的VIO，这一步必须自己来。

这篇内容就是把我这几年在D435i上做IMU标定的完整流程、踩过的坑、以及为什么这么做的逻辑，一次性讲清楚。适合正在用D435i跑VINS-Fusion、ORB-SLAM3、或者任何需要IMU和视觉融合的SLAM系统的朋友。不管你是刚入门还是已经跑通但效果不理想，这里面的细节应该都能帮到你。

2. 先搞明白：IMU标定到底在标什么

2.1 加速度计和陀螺仪的"三宗罪"

IMU标定不是玄学，它本质上是在修正传感器输出的系统性误差。对于D435i内置的BMI055（不同批次可能不同，但原理一致），主要要处理三类问题：

第一类是确定性误差，包括零偏（bias）、尺度因子（scale）、轴间非正交（misalignment）。零偏就是你不动的时候，加速度计输出不是理想的(0,0,9.8)，陀螺仪输出不是(0,0,0)，而是有一个固定的偏移。尺度因子是传感器输出和真实值之间的比例关系不是1:1。轴间非正交是三个轴不完全垂直，导致一个轴的运动会在另一个轴上产生分量。

第二类是随机误差，主要是噪声密度（noise density）和随机游走（random walk）。这两个参数直接决定了VIO算法里IMU预积分的协方差矩阵，你给的值不对，优化时的权重就错了，轨迹自然飘。

第三类是外参，也就是IMU坐标系到相机坐标系的变换矩阵。D435i的IMU和相机是刚性连接的，但它们在物理上不重合，存在一个平移和旋转。VINS-Fusion需要这个外参来把IMU的预积分和视觉观测对齐。

2.2 为什么出厂校准不够用

D435i出厂时确实做了IMU校准，Intel在工厂里用转台标定了零偏和尺度因子，这些值烧在固件里。但问题在于：

出厂校准的温度是常温，你在不同温度下使用，零偏会漂
出厂校准没有给你噪声密度和随机游走参数，这些需要你自己用Allan方差分析
最重要的是，相机和IMU之间的外参没有高精度标定，出厂值只是一个粗略的估计

我实测过几台D435i，用Kalibr标出来的外参和出厂默认值对比，旋转矩阵的差异能到1-2度，平移差异在几毫米到一厘米。对于做近距离抓取或者高精度建图的场景，这个误差足以让你的系统飘移。

2.3 标定工具选型：为什么是Kalibr

标定IMU和相机外参的工具不少，我试过Kalibr、VINS的标定工具、以及一些开源的小工具。最后稳定用Kalibr，原因有几个：

Kalibr支持相机-IMU联合标定，能同时优化相机内参、IMU噪声参数和外参
它用的是连续时间批优化，精度比滤波类方法高
社区成熟，遇到问题容易找到答案
支持多种相机模型，D435i的针孔+畸变模型能直接套

当然Kalibr也有坑，比如对ROS版本有要求、编译容易出问题、标定板制作要精确。这些后面会细说。

3. 标定前的硬件和环境准备

3.1 标定板：别在这省事

Kalibr做相机-IMU联合标定，需要一个标定板。我推荐用AprilTag或者棋盘格，但AprilTag更稳，因为它在部分遮挡和光照变化下检测率更高。

标定板的制作有几个硬性要求：

打印精度要高，最好用激光打印机，普通喷墨打印的尺寸误差能到1-2毫米
贴在硬质平板上，我用的是铝板，亚克力也行，但别用纸板，会变形
尺寸要量准，打印出来后用卡尺量实际边长，这个值要填到Kalibr的配置里

我一般用6x6的AprilTag网格，每个tag边长3厘米，整个板子大概20x20厘米。太小了远处检测不到，太大了手持不方便。

注意：标定板的平整度直接影响标定精度。我见过有人用打印纸贴在书皮上，标出来的外参误差大得离谱。花几十块钱做个铝板，能省你几天调试时间。

3.2 数据采集环境：光照和背景很关键

采集标定数据时，环境要满足几个条件：

光照均匀，避免强光直射和阴影。D435i的RGB相机是卷帘快门，光照突变会导致图像模糊
背景尽量干净，不要有和标定板相似的纹理，否则检测器会误检
标定板要占满相机视野的60%以上，但不要超出边界

我通常在实验室的白墙上做，用两个柔光灯从两侧打光。如果条件有限，白天靠窗也行，但要避开阳光直射。

3.3 软件环境：ROS和Kalibr的版本匹配

Kalibr对ROS版本有要求，我目前用的是ROS Noetic + Ubuntu 20.04，Kalibr编译最顺。如果你用ROS Melodic或者更老的版本，可能会遇到依赖问题。

安装Kalibr的步骤大致是：

# 安装依赖 sudo apt-get install -y python3-dev python3-pip python3-scipy python3-matplotlib sudo apt-get install -y libopencv-dev libeigen3-dev libboost-all-dev # 创建工作空间 mkdir -p ~/kalibr_ws/src cd ~/kalibr_ws/src git clone https://github.com/ethz-asl/kalibr.git # 编译 cd ~/kalibr_ws catkin_make -DCMAKE_BUILD_TYPE=Release -j4

编译过程中最容易出问题的是Python版本和Boost版本。如果报错，先检查python3 --version和rosversion -d是否匹配。

提示：Kalibr编译一次大概要20-30分钟，建议用-j4或者更高并行度，但别超过CPU核心数，否则内存不够会卡死。

4. 分步实操：从录包到出结果

4.1 第一步：录制标定数据包

标定数据采集是整个流程里最需要耐心的环节。你需要同时录制相机图像和IMU数据，并且让标定板在相机视野里做充分的运动。

具体操作：

# 启动D435i roslaunch realsense2_camera rs_camera.launch \ enable_gyro:=true \ enable_accel:=true \ unite_imu_method:=linear_interpolation # 录制数据 rosbag record /camera/color/image_raw /camera/imu -O imu_calib.bag

这里有几个关键点：

IMU数据要开启同步。D435i的陀螺仪和加速度计默认是分开的topic，用unite_imu_method:=linear_interpolation可以把它们合并成一个sensor_msgs/Imu消息，时间戳对齐。如果你不做这一步，Kalibr处理时会报时间戳不一致。

运动要充分激励。标定板要在相机前做三轴平移和三轴旋转，每个轴都要有足够的加速度变化。我一般录2-3分钟，包含：

慢速平移：前后、左右、上下各几次
快速平移：给IMU足够的加速度激励
旋转：绕三个轴各转几圈，角速度要有变化
静止：开头和结尾各停5秒，用于估计零偏

避免运动模糊。D435i的RGB相机在快速运动时容易糊，标定板上的tag会检测不到。所以运动要"快但不糊"，我的经验是角速度控制在30-60度/秒，线速度控制在0.2-0.5米/秒。

注意：录包时不要开自动曝光，固定曝光时间。D435i的自动曝光在光照变化时会调整，导致图像亮度不一致，影响标定板检测。

4.2 第二步：相机内参标定

虽然D435i出厂有内参，但为了联合标定的精度，我建议重新标一次相机内参。用Kalibr的kalibr_calibrate_cameras：

rosrun kalibr kalibr_calibrate_cameras \ --bag camera_calib.bag \ --topics /camera/color/image_raw \ --models pinhole-radtan \ --target aprilgrid_6x6.yaml \ --show-extraction

aprilgrid_6x6.yaml是标定板配置文件，内容大概是这样：

target_type: 'aprilgrid' tagCols: 6 tagRows: 6 tagSize: 0.03 tagSpacing: 0.3

tagSize是你实际测量的tag边长，tagSpacing是tag之间的间距比例。这两个值填错，标定结果全废。

标定完成后会生成一个camchain.yaml，里面包含相机内参和畸变系数。检查重投影误差，一般要小于0.3像素，如果大于0.5像素，说明数据质量不行，得重录。

4.3 第三步：IMU噪声参数标定

IMU的噪声密度和随机游走参数，用Allan方差分析来估计。Kalibr自带了一个工具kalibr_allan，但我觉得用Python的allan_variance_ros包更方便。

步骤：

让D435i静止放置2-3小时，录制IMU数据
用Allan方差工具计算噪声密度和随机游走
把结果填到IMU配置文件里

静止录制的包大概是这样：

rosbag record /camera/imu -O imu_static.bag

录完后用allan_variance_ros处理：

rosrun allan_variance_ros allan_variance.py imu_static.bag

生成的imu.yaml大概长这样：

imu0: model: 'calibrated' noise_density: 0.002 random_walk: 0.00003 update_rate: 200

这里的noise_density和random_walk就是Allan方差曲线在1秒和3秒处的值。具体怎么读曲线，后面会细说。

4.4 第四步：相机-IMU联合标定

这是最关键的一步。用kalibr_calibrate_imu_camera：

rosrun kalibr kalibr_calibrate_imu_camera \ --bag imu_calib.bag \ --cam camchain.yaml \ --imu imu.yaml \ --target aprilgrid_6x6.yaml \ --time-calibration

--time-calibration会同时估计相机和IMU之间的时间偏移，这个很重要。D435i的IMU和相机时间戳虽然硬件同步了，但仍有微小延迟，不校准的话VIO会飘。

标定完成后会生成camchain-imucam.yaml，里面包含：

T_cam_imu：IMU到相机的变换矩阵
time_shift：时间偏移
优化后的IMU噪声参数

检查重投影误差和IMU残差，如果IMU残差大于0.1，说明数据质量有问题，得重来。

5. 标定结果怎么看：别只看数字

5.1 重投影误差：小于0.3像素才合格

Kalibr会输出每个图像的重投影误差。这个值反映的是标定板角点检测的精度和相机模型的拟合程度。一般来说：

小于0.2像素：优秀
0.2-0.3像素：合格
大于0.5像素：数据有问题，重录

如果误差大，先检查标定板检测是否准确，再看图像是否有运动模糊。

5.2 IMU残差：判断外参是否合理

IMU残差反映的是标定出来的外参和噪声参数，能否解释实际运动。Kalibr会输出加速度计和陀螺仪的残差曲线。正常情况下，残差应该在零附近波动，没有明显的趋势。

如果残差有趋势，说明外参或者时间偏移没标好。这时候可以尝试：

增加标定数据量，尤其是旋转运动
检查IMU和相机的时间戳是否对齐
重新标定相机内参

5.3 外参的物理意义：验证一下

标出来的T_cam_imu，你可以手动验证一下。D435i的IMU在相机左侧，所以平移向量的x分量应该是负的（假设相机坐标系x向右）。旋转矩阵应该接近单位矩阵，因为IMU和相机的轴向大致对齐。

如果标出来的旋转矩阵和单位矩阵差很多，比如某个轴转了90度，那肯定是标定出错了。

6. 把标定结果喂给VINS-Fusion

6.1 配置文件怎么写

VINS-Fusion的配置文件里，需要填IMU噪声参数和外参。以d435i_config.yaml为例：

imu: 1 imu_topic: "/camera/imu" acc_n: 0.002 gyr_n: 0.00003 acc_w: 0.0001 gyr_w: 0.00001

这里的acc_n和gyr_n就是Allan方差标出来的噪声密度，acc_w和gyr_w是随机游走。外参在extrinsicRotation和extrinsicTranslation里填，从camchain-imucam.yaml里抄。

6.2 标定前后的对比：飘移改善明显

我做过对比测试，同一段数据，用出厂默认参数和标定后的参数跑VINS-Fusion：

指标	出厂默认	标定后
轨迹终点误差	0.35米	0.08米
回环成功率	60%	95%
快速旋转时飘移	明显	轻微

这个改善在机械臂抓取场景里非常关键，0.35米的误差足以让抓取失败。

6.3 如果还是飘：检查这几个地方

标定完了还飘，别急着怀疑标定。先检查：

IMU和相机的时间戳是否真的对齐，用rqt_plot看/camera/imu和/camera/color/image_raw的时间差
VINS配置里的td（时间偏移）是否设对了
标定时的运动是否充分，尤其是旋转
环境光照是否稳定，特征点是否足够

我遇到过标定没问题但飘移的情况，最后发现是D435i的USB线接触不良，IMU数据偶尔丢帧。换根线就好了。

7. 几个容易踩的坑和我的经验

7.1 Kalibr编译报错：Python版本是元凶

Kalibr对Python版本很敏感。ROS Noetic默认Python3.8，但Kalibr的一些依赖可能装的是Python2的包。解决办法是手动指定Python3：

catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3

如果还报错，检查cv_bridge的版本，有时候需要从源码编译。

7.2 标定板检测不到：光照和距离

AprilTag检测对光照和距离有要求。如果检测不到，先调整光照，再检查标定板是否在相机焦距范围内。D435i的RGB相机最近对焦距离大概0.3米，太近了会糊。

7.3 时间偏移标不准：运动要包含快速变化

--time-calibration估计时间偏移，需要数据里有快速运动。如果全程慢速平移，时间偏移估计不准。我一般会在数据里加几段快速旋转，角速度到60度/秒以上。

7.4 标定结果不稳定：多标几次取平均

Kalibr的标定结果有一定随机性，因为优化是非凸的。我一般标3-5次，取外参的中位数。如果几次结果差异很大，说明数据质量不行，得重录。

7.5 D435i的IMU频率：200Hz还是400Hz

D435i的IMU默认输出200Hz，但有些固件版本可以到400Hz。频率越高，VIO的预积分越准，但数据量也越大。我一般用200Hz，够用了。如果你要做高速运动，可以试试400Hz，但要注意USB带宽。

8. 标定之后的验证：用实际数据跑一遍

标定完别急着上系统，先用一段实际数据验证一下。我通常这么做：

拿着D435i在房间里走一圈，录一段包
用标定后的参数跑VINS-Fusion
对比轨迹和实际路径，看是否闭合
检查IMU预积分和视觉观测的一致性

如果轨迹闭合好，IMU残差小，说明标定成功。如果还有飘移，回到第6节检查。

提示：验证时尽量在纹理丰富的环境里，白墙和纯色地面特征点太少，VIO容易跟丢，这不一定是标定的问题。

9. 一些进阶话题：温度漂移和在线标定

9.1 温度对IMU零偏的影响

D435i的IMU零偏随温度变化，冷启动和热稳定后的零偏能差0.01-0.02 m/s²。如果你做长时间运行，建议开机后等5-10分钟，让IMU热稳定后再开始标定和使用。

9.2 在线标定：VINS-Fusion自带的优化

VINS-Fusion支持在线估计外参和时间偏移，但前提是你的初始值不能太离谱。如果离线标定做得好，在线优化会更快收敛。我一般离线标定后，在配置里开启在线优化，让系统自己微调。

9.3 多传感器融合：IMU和轮式里程计

如果你做移动机器人，IMU还可以和轮式里程计融合。这时候IMU标定的精度直接影响融合效果。我做过测试，标定后的IMU和轮式里程计融合，定位精度比不标定提升30%以上。

10. 最后分享几个实操小技巧

标定这件事，说难不难，但细节多。我总结几个能帮你省时间的点：

标定板用AprilTag，别用棋盘格，检测率高很多
录包时固定曝光，别开自动曝光
运动要"快但不糊"，角速度30-60度/秒，线速度0.2-0.5米/秒
Allan方差录制至少2小时，太短了低频噪声估计不准
标定结果多跑几次，取中位数
标定后一定要用实际数据验证，别直接上系统

我在机械臂项目里，标定前抓取成功率大概70%，标定后到95%以上。这个提升不是玄学，是实打实的精度改善。如果你也在用D435i做SLAM或者VIO，花半天时间把IMU标定做好，后面能省你几周的调试时间。