多传感器融合SLAM这件事我在实验室和实际项目里前后折腾了差不多两年。最开始用单目相机跑VINS-Mono室内短距离还行一旦场景纹理稀疏或者快速旋转就飘得没法看后来加了IMU情况好转不少但长时间运行还是会累积漂移直到把GPS松组合进来才真正让轨迹在室外大场景下稳下来。VINS-Fusion就是在这个需求下进入视野的——它是香港科技大学沈劭劼团队开源的一套优化框架支持单目/双目/双目IMU等多种组合还能把GPS的全局观测以松耦合方式融进优化。这篇内容我打算把从零搭环境、配IMU、标定相机-IMU外参、接入GPS、调参排错的完整链路讲清楚适合已经跑过VINS-Mono、想进一步做多传感器融合的开发者也适合刚接触SLAM但有一定ROS基础的同学。下面所有步骤都是我在Ubuntu 20.04 ROS Noetic环境下实测跑通的参数和踩坑点会尽量写细。1. 先搞清楚VINS-Fusion到底在优化什么很多人一上来就clone代码、改config结果跑不通也不知道问题出在哪。我觉得有必要先把这套系统的核心逻辑讲明白后面调参时你才知道每个参数在动什么。1.1 滑动窗口优化与因子图的基本盘VINS-Fusion的本质是一个基于滑动窗口的紧耦合优化器。它维护一个固定大小的窗口默认10帧左右窗口内包含若干关键帧的位姿、速度、IMU零偏、以及特征点的逆深度。每次新来一帧图像系统会做IMU预积分把两帧之间的IMU测量压缩成一个相对约束然后和视觉重投影误差一起丢进ceres求解。这里的关键词是“紧耦合”。和松耦合先分别算视觉位姿和IMU位姿再融合不同紧耦合是把视觉残差和IMU残差放在同一个优化问题里联合求解IMU的零偏会被视觉观测反过来修正视觉的尺度也会被IMU的加速度观测约束住。这就是为什么VINS-Fusion在快速运动和纹理缺失时比纯视觉稳——IMU在短时间内的积分能兜住视觉跟丢的那几帧。滑动窗口的边际化marginalization是另一个核心。窗口满了要移出老帧但不能直接丢掉否则会损失历史约束。VINS-Fusion用Schur补把老帧的信息浓缩成先验加到优化问题里。这个操作在代码里对应marginalization_factor如果你看到轨迹在窗口滑动时突然跳变八成是边际化先验出了问题。1.2 视觉、IMU、GPS三路观测各自的角色理解这三路观测的分工是调参的前提。视觉提供相对位姿和尺度双目直接给尺度单目靠IMU或GPS给。它的强项是局部精度高、能闭环弱项是受光照和纹理影响大。IMU提供高频的角速度和加速度负责短时间内的运动预测和姿态约束。它的强项是高频、不受环境纹理影响弱项是零偏随时间漂移长时间积分会发散。GPS提供全局的绝对位置观测负责消除累积漂移、把轨迹拉到全局坐标系。它的强项是绝对无漂移弱项是频率低通常1-10Hz、精度受多路径影响、而且没有姿态信息。VINS-Fusion里GPS是以“松耦合”方式接入的也就是说它不参与视觉-惯性的紧耦合优化而是在优化完成后用GPS观测对全局位姿做一个校正。代码里对应global_fusion节点它订阅VINS输出的局部里程计和GPS的经纬高通过一个EKF或者优化来估计局部坐标系到全局坐标系的变换。注意GPS松耦合意味着GPS的观测不会反过来修正IMU零偏和视觉尺度它只做全局对齐。如果你需要GPS参与紧耦合得自己改代码那是另一个工作量级的事情。1.3 为什么选VINS-Fusion而不是ORB-SLAM3或LIO-SAM这个问题我被问过很多次。简单说选型取决于你的传感器配置和场景需求。方案传感器强项弱项VINS-Fusion相机IMUGPS多传感器融合完善GPS接入成熟代码结构清晰纯视觉精度不如ORB-SLAM3无回环检测需外接ORB-SLAM3相机IMU视觉精度高回环和重定位强GPS接入需要自己改多传感器支持不如VINSLIO-SAM激光雷达IMUGPS大场景建图精度高对光照不敏感依赖激光雷达成本高视觉信息未利用如果你的场景是室外大范围、有GPS、传感器是相机IMUVINS-Fusion基本是最省事的选择。它的GPS接入代码是现成的配置文件改改就能跑。如果你做室内、没有GPS、追求视觉精度ORB-SLAM3可能更合适。如果预算充足上了激光雷达LIO-SAM在建图上更稳。2. 环境搭建从ROS到Ceres的依赖链环境搭建是第一个大坑。VINS-Fusion依赖ROS、Ceres Solver、OpenCV、Eigen版本不匹配会报各种奇怪的编译错误。我下面给的版本组合是实测最稳的。2.1 系统与ROS版本的选择逻辑我用的是Ubuntu 20.04 ROS Noetic。为什么不用18.04 Melodic因为Noetic是最后一个支持Python3的ROS1版本而Ceres 2.x和OpenCV 4.x在20.04上编译更顺。如果你还在18.04Ceres建议用1.14OpenCV用3.4但会遇到一些C标准的问题。安装ROS Noetic的桌面完整版sudo sh -c echo deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main /etc/apt/sources.list.d/ros-latest.list sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt update sudo apt install ros-noetic-desktop-full装完后记得source环境并且把source命令加到.bashrc里否则每次开终端都要手动source。2.2 Ceres Solver的编译与版本陷阱Ceres是VINS-Fusion优化的核心版本不对会直接编译失败。我推荐Ceres 2.0.0这个版本和VINS-Fusion的代码兼容性最好。编译Ceres之前先装依赖sudo apt install libgoogle-glog-dev libgflags-dev libatlas-base-dev libsuitesparse-dev然后下载Ceres 2.0.0源码编译git clone https://ceres-solver.googlesource.com/ceres-solver cd ceres-solver git checkout 2.0.0 mkdir build cd build cmake .. make -j4 sudo make install这里有个坑如果你之前装过Ceres 1.xmake install会覆盖但有些头文件可能残留。建议先sudo rm -rf /usr/local/lib/cmake/Ceres和/usr/local/include/ceres再装。提示编译Ceres时如果报Eigen相关的错误检查Eigen版本。VINS-Fusion需要Eigen 3.3以上Ubuntu 20.04自带的Eigen 3.3.7够用。如果自己装过Eigen 3.4注意Ceres 2.0.0对Eigen 3.4的支持有bug建议降回3.3.7。2.3 OpenCV与cv_bridge的版本对齐VINS-Fusion用OpenCV做特征提取和光流跟踪。Ubuntu 20.04自带OpenCV 4.2ROS Noetic的cv_bridge也是基于OpenCV 4.2编译的所以直接用系统自带的就行不要自己编译OpenCV否则cv_bridge会链接冲突。验证一下pkg-config --modversion opencv4应该输出4.2.0。如果输出的是3.x说明你系统里有多个OpenCV需要调整CMAKE_PREFIX_PATH。2.4 编译VINS-Fusion时的常见报错与修复clone代码cd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd .. catkin_make常见的报错和修复报错ceres/ceres.h: No such fileCeres没装好或者路径不对。检查/usr/local/include/ceres/ceres.h是否存在。报错undefined reference to cv::...OpenCV版本冲突。检查cv_bridge链接的OpenCV版本是否和VINS-Fusion一致。报错Eigen::aligned_allocator相关Eigen版本问题降回3.3.7。报错C11相关在CMakeLists.txt里加set(CMAKE_CXX_STANDARD 11)。编译成功后devel/lib/vins/下应该有vins_node、global_fusion_node等可执行文件。3. IMU选型、标定与数据接入IMU是VINS-Fusion里最容易出问题的环节。很多人跑不通不是代码问题是IMU数据质量太差或者标定不准。3.1 IMU选型消费级和工业级的差距在哪我用过三种IMUMPU6050消费级、BMI088消费级偏工业、以及一款工业级IMU。差距主要体现在零偏稳定性和噪声密度上。IMU型号零偏稳定性噪声密度适用场景MPU6050差几十度/小时高短时间demo不建议长时间跑BMI088中等中等室内短距离配合视觉够用工业级IMU好几度/小时低室外长距离GPS融合必备如果你要做室外GPS融合强烈建议用工业级IMU。消费级IMU的零偏漂移太快VINS-Fusion的零偏估计跟不上轨迹会周期性跳变。3.2 IMU内参标定为什么不能跳过这一步IMU内参包括零偏、尺度因子、轴间非正交误差。消费级IMU的尺度因子误差可能到1%-2%轴间误差到0.5度这些不标定会直接影响预积分精度。标定工具我推荐imu_utils和kalibr。imu_utils基于Allan方差标定零偏和噪声密度kalibr可以同时标定相机-IMU外参和时间偏移。用imu_utils的流程让IMU静止放置2小时以上录制rosbag。用imu_utils的launch文件处理bag得到Allan方差曲线。从曲线读出零偏不稳定性和噪声密度填入VINS-Fusion的config。注意录制静止数据时IMU要放在稳固的平面上不能有振动。我试过放在桌上但旁边有人走动标定出来的噪声密度偏大后来换到地下室才准。3.3 相机-IMU外参标定kalibr的实操细节相机和IMU之间的外参旋转和平移以及时间偏移是VINS-Fusion初始化的关键。外参不准初始化会失败或者轨迹直接飘。用kalibr标定的步骤制作标定板AprilTag或棋盘格打印后贴在硬板上。录制bag相机和IMU同时录制手持标定板在相机前做充分的激励运动三个轴都要转要有平移。运行kalibr_calibrate_imu_camera输入相机内参、IMU内参、bag文件。kalibr_calibrate_imu_camera --bag imu_cam.bag --cam camchain.yaml --imu imu.yaml --target april_6x6.yaml标定结果会输出camchain-imucam.yaml里面的T_cam_imu就是外参。实操心得激励运动要“慢而全”不要快速甩动。我一开始甩得太快图像模糊导致角点检测失败标定结果很差。后来放慢速度每个轴转两圈平移20cm左右标定出来的重投影误差在0.2像素以内。3.4 IMU数据接入ROS的两种方式如果你的IMU是独立设备需要通过串口或USB接入ROS。两种方式方式一厂商提供ROS驱动。直接rosrun或roslaunch启动发布sensor_msgs/Imu话题。方式二自己写节点。用serial库读串口解析数据后发布Imu消息。注意时间戳要用ros::Time::now()并且和相机时间戳对齐。时间同步是重点。如果IMU和相机的时间戳不同步VINS-Fusion的初始化会失败。硬件同步最好用同一触发信号软件同步可以用message_filters做近似时间同步但精度差一些。4. GPS接入从经纬高到局部坐标的转换GPS接入是VINS-Fusion区别于VINS-Mono的核心功能。但GPS数据不能直接用需要做坐标转换和松耦合融合。4.1 GPS数据格式与ROS话题VINS-Fusion的global_fusion节点订阅两个话题/vins_estimator/odometryVINS输出的局部里程计。/gpsGPS数据类型是sensor_msgs/NavSatFix。GPS驱动通常发布NavSatFix包含经纬度和海拔。如果你用的是RTK GPS精度能到厘米级普通GPS精度在米级。4.2 经纬高转局部ENU坐标的原理VINS-Fusion的局部坐标系是ENU东-北-天。GPS给的是WGS84经纬高需要转换。转换分两步经纬高转ECEF地心地固坐标系用WGS84椭球参数做转换。ECEF转ENU以第一个GPS点为原点做旋转平移。代码里对应global_fusion/src/globalOpt.cpp里的GPStoXYZ函数。它用第一个GPS点作为原点后续点都转成相对于原点的ENU坐标。这里有个坑如果第一个GPS点精度差比如刚上电还没收敛整个局部坐标系的原点就偏了。我建议等GPS收敛HDOP小于1.5后再启动VINS或者用前10个点的平均值作为原点。4.3 松耦合融合的EKF实现细节global_fusion节点内部用一个EKF来估计局部坐标系到全局坐标系的变换。状态量包括位置、速度、姿态以及局部坐标系的原点偏移。观测方程是GPS的ENU位置 局部里程计位置 原点偏移 噪声。EKF的预测步用VINS的里程计做输入更新步用GPS做观测。由于GPS频率低1-10HzEKF在两次GPS之间靠里程计递推。提示如果GPS跳变严重多路径效应EKF会被带偏。可以在global_fusion的config里调大GPS的观测噪声让EKF更信任里程计。我实测把GPS噪声从1.0调到5.0轨迹平滑很多但全局精度会略降。4.4 GPS失锁时的处理策略室外场景GPS失锁很常见隧道、高楼间。VINS-Fusion在GPS失锁时global_fusion会退化成纯里程计递推轨迹会漂但不会崩。等GPS恢复后EKF会重新收敛。如果你的场景GPS失锁频繁建议用RTK GPS失锁恢复更快。在config里设置GPS超时阈值超时后暂停EKF更新避免用错误的GPS观测。考虑加轮速计或激光雷达做辅助但那是另一个工作量级。5. 配置文件逐参数拆解与调参逻辑VINS-Fusion的config文件是调参的核心。我下面按参数类别拆解每个参数讲清楚“调它会发生什么”。5.1 相机内参与畸变参数model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: -0.05 k2: 0.01 p1: 0.0 p2: 0.0 projection_parameters: fx: 460.0 fy: 460.0 cx: 320.0 cy: 240.0这些参数必须和你的相机标定结果一致。fx和fy是焦距像素单位cx和cy是主点。畸变参数k1,k2,p1,p2是径向和切向畸变。如果内参不准特征点的重投影误差会偏大优化会收敛到错误解。我建议用kalibr或ROS camera_calibration重新标定不要用厂商给的默认值。5.2 IMU噪声参数与零偏随机游走acc_n: 0.08 gyr_n: 0.004 acc_w: 0.00004 gyr_w: 0.000002这四个参数是IMU的噪声密度和零偏随机游走必须从Allan方差标定得到。acc_n加速度计噪声密度单位m/s^2/√Hz。gyr_n陀螺仪噪声密度单位rad/s/√Hz。acc_w加速度计零偏随机游走。gyr_w陀螺仪零偏随机游走。如果这些值填得比实际大优化会过度信任IMU视觉约束被弱化填得比实际小优化会过度信任视觉IMU的预测作用减弱。我见过有人直接抄别人的参数结果轨迹飘得没法看。一定要自己标定。5.3 外参矩阵与时间偏移body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]body_T_cam0是IMU到相机的变换矩阵。如果你用kalibr标定直接填T_cam_imu的逆。时间偏移td也在标定结果里填到config的td参数。外参错了初始化会失败或者轨迹直接飘。我建议标定完后用kalibr的重投影误差检查误差小于0.5像素才算合格。5.4 GPS融合相关参数gps_topic: /gps gps_meas_noise: 1.0 gps_chi2_thres: 1.0gps_meas_noiseGPS观测噪声单位米。普通GPS填1.0-5.0RTK填0.1-0.5。gps_chi2_thres卡方检验阈值用于剔除GPS异常值。如果GPS跳变严重调小这个值。调参逻辑GPS噪声填大轨迹平滑但全局精度降填小全局精度高但容易被异常值带偏。我一般先用1.0跑一遍看轨迹和GPS的偏差再微调。6. 跑通之后的排错链路与实战经验跑通只是开始实际场景里会遇到各种问题。我下面按“现象-排查-修复”的链路讲。6.1 初始化失败从IMU激励到外参检查现象启动后一直卡在“initializing”或者初始化后轨迹直接飞。排查链路检查IMU数据rostopic echo /imu看角速度和加速度是否有输出数值是否合理静止时加速度约9.8角速度约0。检查时间同步rostopic hz /imu和rostopic hz /camera/image_raw看频率是否稳定。如果IMU和相机时间戳差太多初始化会失败。检查外参用kalibr重新标定确认重投影误差小于0.5像素。检查激励运动初始化需要足够的运动激励三个轴都要有旋转和平移。如果启动时静止不动初始化会失败。我一般启动后手持设备走“8”字。6.2 轨迹漂移区分视觉丢失和IMU零偏现象轨迹跑着跑着就飘了或者周期性跳变。排查链路看视觉特征点数量rostopic echo /vins_estimator/feature_tracker如果特征点少于50个说明视觉跟丢了。检查场景纹理和光照。看IMU零偏估计rostopic echo /vins_estimator/imu_bias如果零偏一直在增大说明IMU零偏估计发散。检查IMU标定参数。看GPS观测如果有GPSrostopic echo /gps看GPS是否跳变。如果GPS跳变调大gps_meas_noise。我遇到过一次轨迹周期性跳变排查后发现是IMU的gyr_w填小了零偏随机游走被低估优化过度信任IMU。调大gyr_w后轨迹平滑了。6.3 GPS融合后轨迹反而变差的原因现象不加GPS时轨迹还行加了GPS后轨迹反而跳变。原因通常是GPS坐标系原点选错第一个GPS点精度差导致整个ENU坐标系偏移。GPS噪声参数填太小EKF过度信任GPS被多路径效应带偏。GPS和VINS时间戳不同步GPS的时间戳和VINS里程计的时间戳差太多EKF融合时对不上。修复等GPS收敛后再启动调大gps_meas_noise检查时间戳同步。6.4 长时间运行的累积误差与回环补救VINS-Fusion本身没有回环检测长时间运行会累积漂移。补救方案加回环检测用pose_graph节点VINS-Fusion自带基于词袋做回环。但配置复杂需要额外的词典文件。加GPSGPS提供全局约束能消除累积漂移。定期重定位如果场景有已知标志物可以做重定位。我实测下来室外场景加GPS后1公里轨迹的累积误差能控制在5米以内普通GPSRTK能到分米级。7. 一些不那么显然的实操细节最后分享几个我在实际项目里踩过的坑这些在官方文档里不会写。IMU安装方向IMU的坐标系要和相机坐标系对齐否则外参标定会出错。我见过有人把IMU贴反了标定出来的旋转矩阵是180度初始化一直失败。安装时用rostopic echo /imu看静止时的重力方向确认Z轴朝上。相机曝光时间室外场景光线变化大如果相机曝光时间固定进隧道时会过曝或欠曝特征点跟丢。建议开自动曝光但自动曝光会引入时间偏移需要在标定时考虑。rosbag录制录制bag时用rosbag record --buffsize256避免丢帧。我试过默认buffsize高速运动时丢帧严重导致VINS跟丢。CPU占用VINS-Fusion的特征跟踪和优化很吃CPU。如果CPU占用超过80%会丢帧。建议用多核CPU并且把特征点数量从150降到100。GPS天线位置GPS天线和IMU之间的杆臂lever arm如果没补偿会引入米级误差。VINS-Fusion的global_fusion没有杆臂补偿需要自己在GPS数据里减掉杆臂。这些细节看起来小但每一个都能让轨迹从“能用”变成“不能用”。我在实际项目里花了大量时间在这些地方希望你能少走弯路。