拿到RealSense D455之后如果只是想用自带的深度图像那确实插上USB就能用。但如果你要跑VINS-FUSION这种视觉惯性里程计算法直接默认参数启动十有八九会给你表演一个原地漂移重则连初始化都过不去。原因也不复杂VINS-FUSION是紧耦合算法它对IMU噪声模型、相机和IMU之间的外参都非常敏感而这些参数在设备出厂后并不会跟着买家的机器一起被精细优化过。尤其是D455和上一代D435i在IMU型号、双目基线、彩色相机的相对位置上都不同直接套网上的D435i配置基本是给自己挖坑。这篇文章就用最朴素的方式把整条链路捋一遍从相机内参、IMU噪声、相机IMU联合外参到最终把参数填进VINS-FUSION并跑起来。适合刚接触VIO的学生、实验室里被分配去搭SLAM环境的新人也适合那些手里有D455但一直被标定折磨的老哥。整个过程我会按自己实际跑通的流程写尽量把每一步为什么这么做、有哪些坑都讲清楚。1. 环境准备与整体思路先搞清楚链路要过哪几关1.1 标定到底在标什么VINS-FUSION同时使用左右目图像和IMU数据做状态估计。要让两种传感器在同一个框架下工作就需要几个关键参数相机内参焦距、主点、畸变系数描述三维空间点如何投影到像素平面。左右目之间的相对位姿平移和旋转让两幅图能构成双目几何约束。IMU噪声参数加速度计和陀螺仪的噪声密度、随机游走。VINS-FUSION里卡尔曼滤波的协方差递推全靠它。IMU到相机的外参IMU坐标系和相机坐标系之间的旋转和平移这是初始化阶段能不能正确估计重力方向、尺度、速度的关键。很多人只关注外参觉得IMU噪声参数无所谓。实际上IMU噪声给得过于乐观算法会过于信任IMU稍微有点剧烈运动就发散给得过于保守系统又过度依赖视觉纹理弱的时候轨迹就开始飘。所以这四个环节都必须过一遍。另外先说明一个概念边界这里说的传感器标定和工业机器人领域常说的“九点标定”“手眼标定”不是一回事。九点标定是解决像素坐标到机器人平面坐标的映射手眼标定是解决机械臂基座、相机、末端执行器之间的空间变换关系CANape之类工具里的“标定”则偏向汽车总线数据解析参数。这些都不涉及IMU噪声模型别混在一起。1.2 软硬件环境版本我这边用的是非常经典的Ubuntu 18.04 ROS Melodic组合这套环境下VINS-FUSION和Kalibr的资料最全遇到问题最容易搜到答案。硬件就是一台RealSense D455搭配USB 3.0线材最好再准备一个三脚架或稳固支架标定时会用到。依赖项整理如下版本不一定要完全一致但尽量避免大版本差太多组件推荐版本备注Ubuntu / ROS18.04 / Melodic最稳妥的组合librealsense22.50以上建议源码编译realsense-ros与librealsense2匹配需要改launch参数Eigen3.3.xVINS编译依赖OpenCV3.x或4.x3.x兼容性更好Ceres Solver1.14.xVINS优化求解依赖imu_utils1.0IMU Alllan方差分析Kalibr1.0相机IMU联合标定我自己的习惯是把VINS-FUSION、imu_utils、Kalibr全部放进同一个catkin工作空间编译一次过后续改配置也方便。需要说明的是Ubuntu 18.04下Kalibr默认用的还是Python2那一套而imu_utils编译依赖的code_utils也要放在同一空间里顺序要先编译code_utils再编译imu_utils这个网上有太多人踩过后面也会提到。1.3 realsense-ros的安装与话题检查Realsense SDK装好之后启动相机的方式是roslaunch realsense2_camera rs_camera.launch启动之后先花一分钟确认话题列表重点看这几个rostopic list至少应该有/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw、/camera/imu。如果发现/camera/imu不存在大概率是launch参数里IMU没有启用或者IMU话题没有合并。realsense-ros有时会把加速度计和陀螺仪拆成/camera/accel/sample和/camera/gyro/sample两个话题VINS-FUSION需要的是合并后的/camera/imu。解决办法是在rs_camera.launch里加入或调整参数arg nameenable_imu valuetrue/ arg nameunite_imu_method valuelinear_interpolation/unite_imu_method表示把加速度计和陀螺仪按时间戳线性插值合并成同一个sensor_msgs/Imu话题这是正规做法。D455的IMU型号是Bosch BMI088和D435i的BMI055不同输出频率默认可以设到200Hz甚至400Hz。建议在launch里用imu_output_rate参数明确一下比如设成200Hz并把这个数字记下来后面VINS配置要对应上。2. 相机内参标定出厂参数只供参考2.1 准备棋盘格与标定环境D455虽然出厂时带内参但每次买的机器都会有一点装配公差而且内参数据在后期的标定恢复操作后也可能变化。尤其是要跑双目VINS左右目各自的内参和双目之间的外参必须可靠。所以我习惯按“新机器也重新标一遍”来对待。准备一个棋盘格标定板推荐7x9的内角点、格子边长30mm或更大。尺寸会在标定时填进工具不能用错。标定环境要求光线均匀背景不要太杂乱不要在强反光的桌面上标。把棋盘格贴在一块硬板上手持移动时不要弯折。2.2 用camera_calibration包标定左右目在ROS里最直接的标定工具是camera_calibration包。先启动D455然后分别标定左右红外相机rosrun camera_calibration cameracalibrator.py \ --size 7x9 \ --square 0.03 \ image:/camera/infra1/image_rect_raw \ camera:/camera/infra1窗口打开后慢慢移动棋盘格让角点出现在画面的各个角落。需要保证标定板在不同位置、不同倾斜角度下都有数据不光是在画面中央晃两下。采集过程中x、y、size、skew几个进度条都会慢慢变绿全部变绿后CALIBRATE按钮才会可点。标定结束后会显示重投影误差这个值越小越好一般低于0.2像素就算优秀。如果大于0.5像素就要检查是不是标定板有弯曲或者图像有运动模糊。点击保存会把结果写到/tmp下的tar.gz里面包含calibraion.yaml、camchain文件等。用同样的流程把/camera/infra2也标一遍。这里还有个细节image_rect_raw这个名字虽然带rect但在realsense-ros中它输出的不一定完全消畸变后续还是要依赖标定参数。所以从第一步就严格按标准流程走别省事。2.3 验证内参是否可靠标完先别急着进行下一步。把相机固定好对着一个纹理丰富的静态场景录制一段图像然后用标定得到的内参把角点重投影回去或者直接回放录制画面看边缘是否有明显的弯曲。更直接的方法是看VINS跑起来后静止状态下的IMU积分轨迹是不是稳定。如果你发现左右目的主点和焦距和出厂数据差得非常多不要慌多标两次取中间值。D455出厂数据本身是比较准的自标定结果一般只会小幅修正。2.4 D455的双目与彩色图偏移问题很多教程会用/camera/color/image_raw当主图像输入VINS-FUSION但D455的彩色相机和左右红外相机之间是有明显物理偏移的彩色图的光心和红外图的光心不重合。如果你只想跑单目VINS倒也可以用彩色图但外参需要把彩色相机和IMU单独标一遍。更推荐的方式是直接用左右红外图做双目输入双目匹配能给初始化提供更大的深度信息范围而且D455的红外相机是全局快门运动场景下不会出现卷帘快门带来的果冻效应跑起来更稳。3. IMU噪声参数标定Allan方差没有你想的那么玄3.1 为什么必须静止采集两小时IMU的噪声参数反映的是器件本身的随机误差特性包括角速度噪声密度、角速度随机游走、加速度噪声密度、加速度随机游走。这些值虽然可以从传感器手册里找到大概范围但具体到每台设备的差异仍然不可忽略而且VINS-FUSION对这几个数值的敏感度远高于你的想象。标定IMU噪声最常用的方法是Allan方差分析。它的原理很简单静止放置一段足够长的时间记录IMU输出然后统计分析不同时间尺度下数据的随机游走特性。通常要采集1到2小时的静态数据时间短了低频段的长相关特性看不出来。我一般直接用两小时数据一次标定到位。3.2 数据采集姿势与注意事项把D455放在一个绝对稳定的地方最好是地面或厚重的实验台而不是那种一碰就晃的电脑桌上。IMU最怕低频振动如果桌面刚好有风扇或者楼下在施工噪声数据会很难看。采集期间不要碰相机也不要在线程里开太多东西导致USB带宽不足。录制数据rosbag record /camera/imu -O imu_static.bag录完两小时用rosbag info imu_static.bag确认一下消息数量和时间长度。IMU消息频率要和相机话题的时钟对得上如果频率忽高忽低说明USB供电不足或realsense-ros配置有问题建议先解决这一点再标定。3.3 用imu_utils生成噪声参数IMU静态数据标定我用的是imu_utils配合code_utils。编译顺序是先在catkin工作空间里放code_utils再放imu_utils。如果顺序反了编译时会报找不到code_utils头文件。先写一个launch文件或者直接用命令播放bagrosbag play imu_static.bag然后运行IMU标定节点rosrun imu_utils imu_an_node节点默认订阅/imu/data等话题如果是/camera/imu需要去imu_an_node.cpp里把订阅topic改一下再重新编译。标定完成后会生成的结果文件一般在src/imu_utils/data目录下格式大概是gyr_n: 0.003731362066057757 gyr_w: 6.236344598225582e-05 acc_n: 0.08072696520027577 acc_w: 0.0010157289252614234这四个值就是VINS-FUSION配置里要用到的IMU噪声参数。注意imu_utils的输出文件名可能带日期和后缀复制出来时不要搞混。3.4 参数出来了怎么看把标定得到的噪声密度和随机游走写进VINS配置后做个简单验证把D455放在桌面上不动启动VINS-FUSION观察Rviz里IMU积分的速度和位置是否基本稳定。如果静止状态下速度很快漂到零点几m/s以上多半是噪声参数偏小给算法的“信任度”过高了。反之如果初始化特别迟钝静止时也不收敛可以适当调大噪声参数让算法更依赖视觉。4. 相机与IMU联合标定把两个传感器装进同一个坐标系4.1 生成Aprilgrid靶标联合标定的目的是得到IMU坐标系和相机坐标系之间的旋转和平移。这里Kalibr是很成熟的方案但它需要的靶标不是普通棋盘格而是Aprilgrid。使用Kalibr自带的生成工具kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.088 --tspace 0.3参数含义是生成6x6的Aprilgrid每个格子边长8.8cm格子间距与边长之比为0.3。把PDF打印到A4或A3纸上贴在硬纸板上。打印后一定要用尺子量一下实际格子边长因为打印比例可能不是100%误差会影响标定结果。4.2 采集标定数据的关键要领联合标定的数据采集是一门手艺活。先把相机挂在支架上开启realsense然后同时录制图像和IMU话题rosbag record /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/imu -O calib_data.bag接下来手持设备最好连支架一起做以下几个动作先左右旋转再上下俯仰再画“8”字每个动作做几轮。整个过程中要保证Aprilgrid始终在左右目视野内并且尽量占据画面较大面积。动作幅度可以大一点但速度不要太快否则图像模糊会导致Kalibr检测不到角点。录60到90秒就够太长反而容易引入运动模糊和光照变化。4.3 运行Kalibr联合标定运行前需要准备三份文件Aprilgrid的yaml配置、相机内参的camchain文件、IMU的yaml文件。相机内参文件可以直接用前面标定得到的结果转换需要注意Kalibr里相机模型一般是pinhole-equi或pinhole-radtan而VINS-FUSION里一般用PINHOLE模型。如果畸变较大可以在Kalibr阶段用pinhole-equi到VINS端再转成PINHOLE只要畸变系数对应清楚就行。然后执行联合标定kalibr_calibrate_imu_camera \ --target aprilgrid.yaml \ --cam camchain.yaml \ --imu imu.yaml \ --bag calib_data.bag \ --timeoffset-padding 0.1这个过程会跑一段时间终端里会刷很多迭代日志。如果顺利结束会生成一个带有T_ci的结果文件。这个矩阵就是IMU坐标系到相机坐标系的4x4位姿变换也就是我们要的外参。4.4 把Kalibr结果写成VINS-FUSION需要的外参VINS-FUSION的配置里一般需要body_T_cam0和body_T_cam1这里的body指IMUcam0和cam1分别指左右相机。含义是“从IMU坐标系变换到相机坐标系”的矩阵。Kalibr输出的T_ci从命名上讲也是IMU到相机所以方向通常是一致的但仍然建议打开结果文件仔细核对一下旋转矩阵的物理意义别盲填。一个4x4矩阵在VINS-FUSION的yaml里长这样body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ 0.0151, -0.9997, -0.0002, 0.0103, 0.0012, 0.0003, -0.9999, -0.0015, 0.9998, 0.0151, 0.0011, 0.0157, 0, 0, 0, 1 ]如果Kalibr给出的矩阵方向反了只需要求逆就可以。判断方法很简单在VINS-FUSION里把estimate_extrinsic设为0跑一小段如果初始化正常且轨迹不发散说明外参方向填对了如果一开始就崩得厉害第一件事就检查外参矩阵是不是填反了。5. 运行VINS-FUSION真正动手改配置5.1 编译VINS-FUSION下载源码后放到catkin工作空间的src目录git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git然后在工作空间根目录执行catkin_make如果编译过程报Ceres或OpenCV相关错误多半是版本不匹配。Cerel版本和OpenCV版本宁可保守一点也不要追新。编译通过后检查一下能否找到vins_node和vins_rviz这两个可执行文件。5.2 修改realsense-ros的launchVINS-FUSION需要稳定的图像频率和IMU频率如果realsense-ros输出的话题分辨率不是VINS配置里写好的分辨率算法不会直接报错但表现会很奇怪。建议重新确认一下rs_camera.launch中的几个参数arg nameenable_color valuefalse/ arg nameenable_depth valuefalse/ arg nameenable_infra1 valuetrue/ arg nameenable_infra2 valuetrue/ arg nameenable_imu valuetrue/ arg namergb_camera_profile value640x480x30/ arg namedepth_width value640/ arg namedepth_height value480/跑双目VINS时不需要采集彩色图和深度图关掉它们可以降低USB带宽消耗减少帧丢失。双目图像保持640x48030HzIMU保持200Hz或400Hz这和后续VINS配置要一致。5.3 写realsense_stereo_imu_config.yaml建议在VINS-FUSION的config目录下新建一个自己的配置文件不要大改原来自带的realsense示例。初始化部分这样写imu_topic: /camera/imu image0_topic: /camera/infra1/image_rect_raw image1_topic: /camera/infra2/image_rect_raw output_path: ~ cam0_parameters: model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 projection_parameters: fx: 386.0 fy: 386.0 cx: 320.0 cy: 240.0这个示例里的fx、fy、cx、cy只是示意一定要替换成你自己标定后的真实值。畸变系数同样要替换成实际值不能留0。IMU参数部分acc_n: 0.08072696520027577 acc_w: 0.0010157289252614234 gyr_n: 0.003731362066057757 gyr_w: 6.236344598225582e-05 estimate_extrinsic: 0 estimate_td: 1estimate_extrinsic设为0表示直接使用配置里的外参不再在线估计。estimate_td设为1表示在线估计相机和IMU之间的时间偏移这个可以补偿realsense-ros和VINS-FUSION之间毫秒级的时间戳误差。因为D455的IMU消息和图像消息是在不同线程里触发的完全对齐很难在线估计时间偏移能省去很多手动对时的痛苦。5.4 启动VINS-FUSION建议先启动相机再启动VINS节点roslaunch realsense2_camera rs_camera.launch然后另开终端rosrun vins_estimator vins_node ~/catkin_ws/src/VINS-FUSION/config/realsense/realsense_stereo_imu_config.yaml roslaunch vins_estimator vins_rviz.launch如果RViz里能出现图像、IMU状态、相机轨迹并且初始化之后在缓慢运动下轨迹不飘那就基本成功了。首次启动会有一段初始化过程大概几秒钟期间需要缓慢平移或旋转相机让算法完成重力对齐和尺度估计。6. 常见问题排查与避坑实录6.1 时间戳乱掉了怎么办最常见的问题是播放bag文件时没有设置时钟。如果回放rosbag play calib_data.bag时另外开着VINS节点需要在播放终端加上rosbag play calib_data.bag --clock否则bag里的时间戳是录制时的墙钟时间而VINS-FUSION用的是ROS系统时间两者一错乱IMU和图像的对齐就会崩。另外如果用realsense-ros实际跑也可能出现图像时间戳和IMU时间戳差几十毫秒的情况建议打开estimate_td让VINS自己去估时间偏移。6.2 IMU参数填进配置文件还是飘如果标定数据看起来没问题但VINS-FUSION跑起来仍然在静止时发生明显漂移先检查是不是realsense-ros输出的IMU频率没有匹配上。VINS配置文件里通常没有直接的“imu频率”字段它是通过时间戳差来计算周期的。如果IMU消息缺帧或频率忽高忽低算法内部的时间间隔就会异常表现为漂移和抖动。这时可以用rostopic hz /camera/imu看实时频率再决定去解决USB带宽还是realsense参数。6.3 分辨率不一致导致图像异常有的同学在标定时用1280x720跑VINS时却用640x480结果没改配置文件里的image_width和image_height导致图像信息残缺、初始化很难完成。VINS-FUSION不会帮你自动缩放图像一切以配置文件的尺寸为准。你有两个选择要么固定一个分辨率标定和运行时完全一致要么在launch里固定输出分辨率别让realsense-ros自动切换。6.4 D455和网上的D435i教程不能直接套网上大量教程用的是D435iD435i的IMU型号是BMI055外参参数和D455完全不同。D455的双目基线更宽、IMU噪声特性不一样而且彩色相机和红外相机之间的相对位姿也和D435i不同。所以任何网上分享的realsense_stereo_imu_config.yaml都只能当格式参考不能直接复制。哪怕同样是D455不同批次之间也存在细微误差严格按照本文流程自己标一遍才是正道。6.5 标定流程与常见“标定”术语的区别再提醒一次相机内参标定、手眼标定、九点标定、CANape/CANoe里的数据标定各自解决的问题完全不同。本文涉及的标定只服务于VINS-FUSION这类视觉惯性SLAM算法别拿着机器人手眼标定的结果去填VINS外参也别把九点标定的映射矩阵当相机内参用。搜索资料时注意甄别关键词能少走很多弯路。6.6 一些问题速查表现象可能原因处理办法/camera/imu话题不存在IMU未启用或未合并打开enable_imu并设置unite_imu_method初始化始终不成功外参方向反或IMU噪声参数离谱检查body_T_cam矩阵确认噪声数量级跑起来轨迹持续旋转相机IMU外参或时间偏移错误打开estimate_td重跑联合标定图像和IMU时间严重不同步bag播放时钟没设置使用rosbag play --clock左右目拼接异常双目内参不一致或极线校正失效重新标定双目内参检查棋盘格尺寸我在实际标定中最深的体会是标定不是一个一次性的仪式而是一个反馈闭环。不要把所有参数标完之后才去跑VINS那是拿一整串未知数一次性撞运气。先把相机内参标好用单目看静态场景再把IMU噪声填进去看静止漂移最后才动视觉惯性联合标定。每做一步就验证一步出了问题也能快速定位到具体环节。这样整套流程跑下来D455配VINS-FUSION的稳定程度会让你觉得前面折腾的时间都值了。