1. 为什么你的SLAM总在“飘”先搞懂D435i的IMU到底怎么回事如果你正在用Intel RealSense D435i跑VINS-Fusion、ORB-SLAM3或者自己搭的VIO管线大概率遇到过这种场景机器人明明走的是直线建出来的地图却像喝醉了酒一样歪歪扭扭机械臂末端定位跑着跑着就偏了几厘米好不容易闭环了轨迹却整体旋转了一个角度。很多人第一反应是调参——把imu_rate调高、把acc_n和gyr_n改小、把外参矩阵反复微调折腾几天下来发现该飘还是飘。问题很可能不在算法而在你从来没认真做过IMU标定。D435i这个相机很有意思它集成了深度模块、RGB模块和一颗BMI055六轴IMU。很多人买它就是因为“开箱即用”插上USB就能出深度图和IMU数据觉得省事。但恰恰是这种“省事”埋了雷出厂时IMU的零偏、尺度因子、轴间非正交误差以及和深度相机之间的外参都是按照消费级标准做的粗略校准。对于做演示、跑个demo这些误差可以忽略但对于SLAM这种对惯性数据极其敏感的task出厂参数远远不够。我见过太多人把D435i直接架上机器人就开始跑VINS-Fusion结果初始化阶段就各种失败好不容易初始化成功走两米就飘得不成样子。然后开始怀疑算法、怀疑代码、怀疑人生唯独没怀疑过IMU的标定数据。实际上IMU标定是VIO/SLAM系统里性价比最高的一步——花半小时认真标一次比调三天参数管用得多。这篇文章面向的是正在用D435i做SLAM、VIO、机器人定位、机械臂视觉伺服的朋友不管你是刚入门的学生还是在做产品原型的工程师只要你的系统里用到了D435i的IMU这篇内容都值得你花时间看完。我会从IMU标定的基本原理讲起然后手把手带你用Kalibr完成D435i的IMU标定包括数据采集、参数配置、结果验证最后分享一些我踩过的坑和实操技巧。整套流程在Ubuntu 20.04 ROS Noetic环境下验证过硬件就是D435i加一个普通的标定板。2. IMU标定到底在标什么拆开揉碎讲清楚2.1 IMU误差模型你的陀螺仪和加速度计没那么“诚实”一颗MEMS IMUD435i用的是Bosch BMI055在理想情况下陀螺仪输出的是角速度加速度计输出的是比力。但现实中由于制造工艺的限制IMU的输出和真实值之间隔着一堆误差项。工程上常用的误差模型是这样的对于陀螺仪ω_measured T_g * K_g * (ω_true b_g n_g)对于加速度计a_measured T_a * K_a * (a_true b_a n_a)这里面每一项都有具体含义T轴间非正交矩阵理想情况下三轴互相垂直但实际MEMS加工做不到完美正交存在微小的轴间耦合。这个矩阵通常接近单位矩阵但非对角项不为零。K尺度因子矩阵理想情况下输入1 rad/s输出也是1 rad/s但实际存在比例误差通常在对角线上量级在千分之几到百分之几。b零偏这是最要命的一项。即使IMU静止不动陀螺仪输出也不是零加速度计输出也不是纯重力。零偏还分静态零偏和动态零偏静态零偏可以标定补偿动态零偏随温度和时间变化只能靠滤波器在线估计。n噪声包括白噪声和随机游走。白噪声影响瞬时精度随机游走影响长时间积分精度。这两个参数直接决定了VIO滤波器里Q矩阵的取值。Kalibr做的事情就是通过采集IMU静止和运动的数据把T、K、b这几个确定性误差项估计出来同时给出噪声参数。标定完成后你用这些参数去补偿原始IMU数据VIO的输入质量会有一个质的提升。2.2 为什么D435i的出厂标定不够用有人会问Intel出厂不是已经标过了吗为什么还要自己标原因有三。第一出厂标定是在恒温条件下做的而且标定设备是产线级别的但标定参数写入固件后就不再更新。你的使用环境温度可能和产线差十几度MEMS IMU的零偏对温度非常敏感温度变化几度零偏就能漂几个mg。第二出厂标定主要针对IMU自身的确定性误差但IMU和深度相机之间的外参也就是IMU坐标系到相机坐标系的旋转和平移在出厂时只是根据结构设计给的标称值实际装配公差会导致外参和标称值有偏差。第三也是最关键的——出厂标定没有给出噪声参数。VIO滤波器需要知道IMU的噪声密度和随机游走系数来设置Q矩阵这些参数出厂文档里根本没有你只能猜。猜错了滤波器要么过于信任IMU导致漂移要么过于信任视觉导致响应迟钝。所以自己标定IMU不仅是为了补偿零偏和尺度因子更是为了拿到一组可信的噪声参数和外参让VIO滤波器工作在正确的状态。2.3 Kalibr为什么是D435i IMU标定的首选工具标定IMU的工具不少有imu_utils、Kalibr、Allan方差工具箱等。我推荐Kalibr的原因很直接一体化Kalibr可以同时标定相机内参、相机-IMU外参、IMU内参和噪声参数一套工具全搞定。支持多种相机模型D435i的深度相机和RGB相机都可以用Kalibr标定不用换工具。社区成熟ROS环境下安装方便文档齐全遇到问题容易找到答案。输出格式友好标定结果直接输出YAML文件VINS-Fusion和Kalibr自己的格式都能用。当然Kalibr也有缺点比如对数据采集要求比较高标定过程比较耗时。但相比它带来的精度提升这些投入完全值得。3. 标定前的准备工作硬件、软件、环境一个都不能少3.1 硬件准备标定板、支架和光线标定IMU需要两类数据静止数据和运动数据。静止数据用来估计零偏和噪声运动数据用来激励各个轴估计尺度因子和非正交矩阵。所以硬件上你需要D435i相机确保固件版本不要太老建议用realsense-viewer检查一下IMU是否正常出数。标定板Kalibr支持AprilTag、Checkerboard和Circular Grid三种靶标。我推荐用AprilTag检测更稳定对光照和部分遮挡更鲁棒。标定IMU时其实不需要标定板但如果你要同时标定相机-IMU外参就需要标定板。建议一步到位相机-IMU外参一起标了。稳固的支架标定IMU运动数据时相机需要做充分的激励运动。手持当然可以但手持的运动会引入额外的振动噪声。有条件的话用三脚架或者机械臂夹持运动更平滑。光线如果同时标定相机光线要均匀避免标定板反光或阴影。注意D435i的IMU和深度相机是刚性连接的标定相机-IMU外参时标定板要同时被相机看到所以标定板的尺寸要足够大保证在相机视野内清晰可见。3.2 软件环境搭建Ubuntu ROS Kalibr我的环境是Ubuntu 20.04 ROS Noetic这是目前最稳的组合。Kalibr的安装有两种方式源码编译和Docker。我强烈推荐用Docker因为Kalibr的依赖比较多源码编译容易遇到各种版本冲突。如果你坚持源码编译大致步骤如下# 安装依赖 sudo apt-get install -y python3-dev python3-pip python3-scipy python3-matplotlib \ ipython python3-wxgtk4.0 python3-tk python3-igraph python3-numpy \ python3-pytest python3-setuptools python3-rosdep python3-rosinstall \ python3-rosinstall-generator python3-wstool build-essential # 创建工作空间 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_TYPERelease -j4编译过程大概需要20到30分钟取决于机器性能。编译完成后记得source一下source ~/kalibr_ws/devel/setup.bash用Docker的话更简单docker pull stereolabs/kalibr:kinetic然后启动容器时把数据目录挂载进去就行。不过Docker里的ROS版本是Kinetic和Noetic有些差异如果你后续要用VINS-Fusion建议还是源码编译。3.3 检查D435i的IMU数据质量在正式标定之前先确认IMU数据是正常的。启动realsense节点roslaunch realsense2_camera rs_camera.launch unite_imu_method:linear_interpolation这里有个关键参数unite_imu_method。D435i的IMU实际上是两个独立的传感器——加速度计和陀螺仪它们的采样频率不同加速度计250Hz陀螺仪400Hz时间戳也不完全对齐。unite_imu_method参数控制如何合并这两路数据none不合并分别发布/camera/accel/sample和/camera/gyro/samplecopy简单复制时间戳精度差linear_interpolation线性插值对齐时间戳推荐用这个然后查看IMU话题rostopic echo /camera/imu把相机静止放在桌面上观察linear_acceleration和angular_velocity的数值。正常情况下加速度计的z轴应该接近9.8 m/s²取决于相机朝向陀螺仪三轴应该接近0。如果陀螺仪静止时输出超过0.01 rad/s说明零偏比较大更需要认真标定。实操心得D435i的IMU在刚上电时零偏不稳定建议上电后预热5到10分钟再开始采集数据。我实测过冷启动时陀螺仪零偏能到0.005 rad/s预热后降到0.001 rad/s以下。4. 手把手实操用Kalibr完成D435i IMU标定4.1 数据采集静止段和运动段怎么录Kalibr标定IMU需要两个bag文件一个静止bag一个运动bag。静止bag用来估计零偏和噪声运动bag用来估计尺度因子和非正交矩阵。静止数据采集把D435i放在稳固的平面上确保不会晃动。然后录制5到10分钟的静止数据rosbag record -O imu_static.bag /camera/imu --duration10m录制期间不要碰相机不要开关附近的电器不要有人在旁边走动。我一般选在晚上安静的时候录避免振动干扰。运动数据采集运动数据需要充分激励IMU的各个轴。具体来说要让相机在三个轴向上都有明显的旋转和加速度变化。我的做法是手持相机先绕x轴横滚缓慢旋转±90度来回5次再绕y轴俯仰缓慢旋转±90度来回5次再绕z轴偏航缓慢旋转±90度来回5次然后做几个“8”字运动让三个轴同时被激励最后做几次快速的平移运动激励加速度计整个过程大概3到5分钟录制命令rosbag record -O imu_motion.bag /camera/imu注意运动数据采集时动作要平滑不要有剧烈的抖动或撞击。Kalibr假设IMU的运动是连续的剧烈抖动会被当作噪声处理影响标定精度。另外运动幅度要足够大如果只是轻微晃动某些轴的尺度因子可能无法被充分激励。4.2 标定文件配置YAML怎么写Kalibr标定IMU需要一个配置文件指定IMU话题名、噪声参数初值等。创建一个imu.yaml# IMU标定配置文件 imu_topic: /camera/imu imu_rate: 200 # 加速度计噪声密度初值单位 m/s^2/sqrt(Hz) accelerometer_noise_density: 0.01 # 陀螺仪噪声密度初值单位 rad/s/sqrt(Hz) gyroscope_noise_density: 0.001 # 加速度计随机游走初值 accelerometer_random_walk: 0.0001 # 陀螺仪随机游走初值 gyroscope_random_walk: 0.00001这些初值不需要很准Kalibr会在标定过程中优化。但初值也不能太离谱否则优化可能不收敛。我一般用BMI055数据手册里的典型值作为初值。如果你要同时标定相机-IMU外参还需要一个相机配置文件。D435i的RGB相机用pinhole-radtan模型# camera.yaml cam0: camera_model: pinhole intrinsics: [fx, fy, cx, cy] # 需要先用camera_calibration标定 distortion_model: radtan distortion_coeffs: [k1, k2, r1, r2] resolution: [640, 480] rostopic: /camera/color/image_raw相机内参可以用ROS自带的camera_calibration工具先标好这里不展开。4.3 执行标定命令和参数详解先标IMU内参和噪声rosrun kalibr kalibr_calibrate_imu_camera \ --bag imu_motion.bag \ --bag-from-to 10 200 \ --imu imu.yaml \ --imu-models scale-misalignment \ --time-calibration等等这里有个细节。kalibr_calibrate_imu_camera是用来标定相机-IMU外参的如果只标IMU内参应该用kalibr_calibrate_imu实际上Kalibr并没有单独的IMU内参标定命令IMU内参是在相机-IMU联合标定过程中一起估计的。所以如果你只想标IMU内参可以只提供IMU数据不提供相机数据Kalibr会退化为纯IMU标定。但更常见的做法是相机-IMU一起标这样外参和内参都能拿到。命令如下rosrun kalibr kalibr_calibrate_imu_camera \ --bag imu_motion.bag \ --cam camera.yaml \ --imu imu.yaml \ --target aprilgrid.yaml \ --imu-models scale-misalignment \ --time-calibration \ --bag-from-to 5 180参数解释--bag运动数据的bag文件--cam相机配置文件--imuIMU配置文件--target标定板配置文件--imu-models scale-misalignment使用完整的IMU误差模型包括尺度因子和轴间非正交--time-calibration估计相机和IMU之间的时间偏移--bag-from-to只使用bag中指定时间段的数据跳过开头和结尾不稳定的部分标定过程大概需要10到30分钟取决于数据量和机器性能。Kalibr会先做相机标定然后做IMU标定最后联合优化。终端会输出优化进度和重投影误差。4.4 标定结果解读哪些参数值得关注标定完成后Kalibr会输出几个文件camchain-imucam-*.yaml包含相机内参、IMU内参、相机-IMU外参imu-*.yamlIMU噪声参数results-imucam-*.txt标定报告打开camchain-imucam-*.yaml重点看这几项# IMU内参 imu: model: scale-misalignment # 陀螺仪尺度因子和轴间非正交矩阵 M_gyro: [1.001, 0.0002, -0.0001, -0.0003, 0.999, 0.0004, 0.0001, -0.0002, 1.002] # 加速度计尺度因子和轴间非正交矩阵 M_acc: [1.002, 0.0003, -0.0002, -0.0001, 0.998, 0.0003, 0.0002, -0.0001, 1.001] # 陀螺仪零偏 bias_gyro: [0.0001, -0.0002, 0.0001] # 加速度计零偏 bias_acc: [0.01, -0.02, 0.03]尺度因子矩阵的对角线元素应该接近1非对角线元素应该接近0。如果对角线元素偏离1超过1%说明IMU的尺度误差比较大标定补偿会很有必要。零偏值的大小取决于你的IMU个体差异但陀螺仪零偏一般在0.001 rad/s以内加速度计零偏在0.05 m/s²以内。再看imu-*.yaml里的噪声参数accelerometer_noise_density: 0.008 accelerometer_random_walk: 0.00008 gyroscope_noise_density: 0.0008 gyroscope_random_walk: 0.000008这些值要和BMI055的数据手册对比。如果标定出来的噪声密度比手册值大一个数量级说明数据采集时引入了额外噪声需要重新采集。5. 标定完了怎么用VINS-Fusion配置实战5.1 把标定结果填入VINS-Fusion配置文件VINS-Fusion的配置文件在config/realsense_d435i/目录下主要有两个文件realsense_d435i_config.yaml和realsense_d435i_imu.yaml。realsense_d435i_imu.yaml里填入标定得到的噪声参数# IMU噪声参数 acc_n: 0.008 # 加速度计噪声密度 gyr_n: 0.0008 # 陀螺仪噪声密度 acc_w: 0.00008 # 加速度计随机游走 gyr_w: 0.000008 # 陀螺仪随机游走realsense_d435i_config.yaml里填入相机-IMU外参# 相机-IMU外参 body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]这里的body_T_cam0是IMU坐标系到相机坐标系的变换矩阵从Kalibr输出的camchain-imucam-*.yaml里直接复制T_cam_imu的逆矩阵。实操心得VINS-Fusion里外参矩阵的方向很容易搞反。Kalibr输出的是T_cam_imu表示从IMU坐标系到相机坐标系的变换。VINS-Fusion需要的是body_T_cam0表示从相机坐标系到IMU坐标系的变换。所以要对Kalibr的结果求逆。我一开始没注意直接复制过去结果VINS-Fusion初始化就失败排查了半天才发现是外参方向反了。5.2 验证标定效果从“飘”到“稳”的对比标定前后各跑一次VINS-Fusion对比轨迹。我的测试场景是手持D435i在办公室里走一圈回到起点看闭环误差。标定前走一圈回到起点轨迹终点和起点偏差大约0.5米而且轨迹有明显的旋转漂移地图整体歪了大概10度。标定后同样的路线终点和起点偏差缩小到0.1米以内轨迹基本闭合地图没有明显旋转。这个提升是非常直观的。标定前VINS-Fusion的初始化经常失败需要反复移动相机才能成功标定后基本一次就能初始化成功而且初始化后的轨迹很稳定。5.3 标定参数对SLAM性能的影响分析IMU标定参数对VIO的影响可以从几个维度来看参数影响标定前典型值标定后典型值陀螺仪零偏直接影响姿态积分漂移0.005 rad/s0.0005 rad/s加速度计零偏影响速度和位置估计0.05 m/s²0.01 m/s²尺度因子影响大角速度/大加速度下的精度1.5%误差0.2%误差噪声密度影响滤波器Q矩阵猜的实测相机-IMU外参影响视觉惯性融合标称值标定值陀螺仪零偏是最关键的。假设零偏是0.005 rad/s积分10秒就是0.05 rad的姿态误差换算成角度大概是3度。对于SLAM来说3度的姿态误差足以导致地图明显变形。标定后零偏降到0.0005 rad/s10秒积分误差只有0.3度完全可以接受。6. 常见问题与排查技巧实录6.1 标定不收敛怎么办Kalibr标定不收敛是最常见的问题表现是终端一直输出优化迭代信息但残差降不下去或者直接报错退出。原因通常有几个数据激励不足。运动数据里某些轴没有被充分激励导致对应的尺度因子无法被观测。解决办法是重新采集运动数据确保三个轴都有足够的旋转和加速度变化。我一般会录两遍选激励更充分的那一遍。标定板检测不稳定。如果同时标定相机标定板检测失败会导致整个优化失败。检查标定板配置文件里的参数是否正确特别是tagRows、tagCols、tagSize、tagSpacing这几个。AprilTag的tagSpacing是标签间距和标签尺寸的比值不是绝对距离很容易填错。时间偏移太大。D435i的IMU和相机时间戳可能存在偏移如果偏移超过0.02秒Kalibr可能无法正确对齐。--time-calibration参数可以估计时间偏移但如果初值太离谱优化会失败。可以先用--time-calibration跑一次看输出的时间偏移是多少如果超过0.05秒检查ROS节点的时间同步配置。6.2 标定结果看起来“不对劲”怎么排查标定完成后如果发现某些参数明显不合理比如尺度因子矩阵对角线偏离1超过5%或者零偏大得离谱说明标定过程有问题。排查思路检查IMU数据本身是否正常。用rostopic echo看原始数据静止时陀螺仪输出是否接近0加速度计模长是否接近9.8。检查bag文件的时间戳是否连续。用rosbag info看消息数量和时间跨度如果时间戳有跳变标定会受影响。检查标定板配置文件。如果同时标定相机标定板参数错误会导致外参标定失败进而影响IMU内参。检查数据采集时的运动是否平滑。如果运动数据里有剧烈抖动Kalibr会把抖动当作噪声导致噪声参数被高估。6.3 D435i IMU标定的独家避坑技巧技巧一预热很重要。D435i的IMU零偏对温度敏感冷启动和预热后的零偏能差好几倍。我一般上电后先跑10分钟让IMU温度稳定下来再开始采集数据。如果条件允许可以用realsense-viewer看IMU温度等温度稳定在40度左右再开始。技巧二静止数据要够长。静止数据用来估计噪声参数数据越长噪声估计越准。我一般录10分钟最少不低于5分钟。录制时确保环境安静不要有振动源。技巧三运动数据要“慢而大”。运动数据的激励要充分但动作要慢。快速运动会产生额外的振动噪声而且可能导致运动模糊如果同时标定相机。我的经验是每个轴向的旋转用2到3秒完成幅度尽量大但角速度不要超过1 rad/s。技巧四外参方向别搞反。前面提过Kalibr输出的是T_cam_imuVINS-Fusion需要的是body_T_cam0两者互为逆矩阵。我建议标定完成后先用Kalibr的可视化工具检查一下外参是否合理再填入VINS-Fusion。技巧五标定不是一劳永逸。IMU的零偏会随温度和时间变化建议每隔几个月重新标定一次或者在温度变化大的环境下使用时重新标定。如果发现SLAM性能突然下降先检查IMU零偏是否漂了。6.4 常见问题速查表问题现象可能原因排查方法解决方案标定不收敛数据激励不足检查运动数据各轴是否有足够变化重新采集运动数据标定不收敛标定板检测失败检查标定板配置文件修正tagSize和tagSpacing标定不收敛时间偏移太大查看时间偏移输出检查ROS时间同步尺度因子偏离大IMU数据异常rostopic echo检查原始数据检查IMU连接和固件零偏大得离谱数据采集时有振动检查采集环境重新采集静止数据VINS-Fusion初始化失败外参方向反了检查body_T_cam0对Kalibr结果求逆VINS-Fusion轨迹漂移噪声参数不对对比标定值和手册值重新标定或调整Q矩阵标定后效果反而变差标定结果有误检查标定报告残差重新标定7. 进阶话题多传感器联合标定与在线标定7.1 相机-IMU-激光雷达联合标定思路如果你的系统里除了D435i还有激光雷达比如做激光雷达-视觉-惯性融合的SLAM那就需要标定三个传感器之间的外参。D435i的IMU和相机是刚性连接的可以看作一个整体所以问题简化为标定这个整体和激光雷达之间的外参。标定方法有两种基于标定板的方法和无目标方法。基于标定板的方法需要一个同时能被相机和激光雷达看到的标定板比如带AprilTag的平面板。无目标方法利用环境中的特征比如墙角、地面通过优化残差来估计外参。我一般用基于标定板的方法精度更高但需要制作专门的标定板。7.2 在线标定让SLAM系统自己修正外参离线标定虽然精度高但标定完成后外参就固定了。如果相机和IMU之间的相对位姿因为振动或温度变化发生了微小变化离线标定的外参会失配。在线标定可以在SLAM运行过程中实时估计和修正外参提高系统的鲁棒性。VINS-Fusion支持在线外参标定在配置文件里设置estimate_extrinsic: 1即可。但在线标定需要良好的初始值如果初始外参偏差太大在线优化可能不收敛。所以我的建议是先离线标定得到较好的初值再开启在线标定做微调。7.3 标定频率与维护建议IMU标定不是做一次就完事了。我的建议是新相机到手立即做一次完整标定建立基准。每3到6个月重新标定一次检查零偏是否漂移。温度变化大的环境使用前重新标定或者至少做一次静止零偏估计。SLAM性能下降时首先怀疑IMU标定重新标定后再排查其他原因。标定数据要保存好包括bag文件、配置文件、标定结果。这样下次标定时可以参考对比快速判断标定是否正常。8. 写在最后一些个人体会我做D435i的IMU标定前前后后折腾了大概两个月踩过的坑比这篇文章里写的多得多。最开始我也觉得出厂标定够用直到有一次跑VINS-Fusion轨迹飘得连起点和终点都分不清才下定决心认真标定。第一次标定用了Kalibr的Docker版本结果因为ROS版本不匹配数据格式对不上折腾了一周才跑通。后来换成源码编译又遇到Eigen版本冲突编译报错几十个一个个查资料解决。但所有这些折腾都是值得的。标定完成后VINS-Fusion的初始化成功率从不到50%提升到95%以上轨迹精度提升了一个数量级。更重要的是我终于理解了IMU标定在VIO系统里的位置——它不是可有可无的预处理而是整个系统精度的基石。如果你正在做SLAM或者VIO我强烈建议你花半天时间认真标定一次IMU。不要觉得麻烦不要觉得出厂标定够用。等你看到标定前后轨迹精度的对比你会回来感谢自己的。最后分享一个小技巧标定完成后把标定参数和对应的bag文件一起保存命名带上日期和温度。下次标定时可以对比如果发现零偏变化很大说明IMU可能老化了需要考虑更换。这个习惯帮我提前发现了一颗快要坏掉的IMU避免了在重要实验中翻车。