做无人机定位的人早晚会碰到一个尴尬场景GPS信号一断飞控里的EKF就开始“放飞自我”水平位置在几秒内飘出好几米。室内巡检、桥底检测、地下车库搜救都是这类GPS缺失的环境。VINS-Fusion就是专门用来解决这个问题的视觉惯性导航系统它把摄像头和IMU的数据放到非线性优化框架里紧耦合实时输出无人机的6自由度位姿并用回环检测消除长时间漂移。这篇文章不聊论文公式只讲怎么把这套系统在ROS环境里真正跑起来从环境搭建、传感器标定、官方数据集验证一直到把位姿结果喂给PX4飞控完整走一遍配置流程。VINS-Fusion不是那种装完就能用的现成软件包它更像一套半成品算法框架需要你根据自己飞机的传感器布局去改配置、改话题、调参数。很多新手一上来就在catkin_make阶段被Ceres、OpenCV版本问题卡住或者在真机上跑起来后轨迹飞一会儿就飘了。我踩过的坑不少这篇文章就是把这些坑提前给你标出来让你少走弯路。1. 先搞清楚VINS-Fusion到底在解决什么问题1.1 为什么无人机会在室内和桥下“失明”无人机在开阔室外主要靠GNSS接收机拿绝对位置GPS在空旷环境里确实好用但到了室内、桥底、隧道、高密度城市峡谷卫星信号要么被挡住要么反射严重导致定位跳动。飞控里的惯性导航单元虽然能短时间维持姿态和速度但它本质是个积分过程加速度计噪声会被不断积分放大几秒钟内水平位置就会漂移出去好几米。这时候就需要一套不依赖卫星信号的定位手段。视觉惯性导航系统的基本思路是让无人机自己“看路”摄像头捕捉环境特征IMU感受自身加速度和角速度两个传感器互补。摄像头在纹理丰富的地方定位非常准但怕快速旋转导致运动模糊IMU不怕快速运动但长期会有零偏漂移。把它们的结果融合在一起就能在GPS失效的环境里继续输出稳定的位姿。这个场景听着很窄实际应用非常广。室内无人机巡检、管廊检测、桥墩病害排查、地下车库自主泊车甚至AGV机器人在货架间穿行都需要类似的定位能力。VINS-Fusion作为一套开源实现最大的价值是你不用从零写优化器直接基于它做二次开发就能得到一个可用的视觉惯性里程计。1.2 VINS-Fusion的核心机制紧耦合优化VINS-Fusion是HKUST-Aerial-Robotics团队开源的视觉惯性定位项目相比早期的VINS-Mono它增加了双目支持、GPS融合、4自由度全局位姿图优化代码结构也清晰了不少。它的核心处理流程可以拆成四段前端特征跟踪、IMU预积分、滑动窗口优化、回环检测与全局优化。前端做的事情是提取图像特征点并做光流跟踪相当于在连续图像帧里找到同一个环境点然后输出它的像素坐标变化。后端再把这些像素观测和IMU预积分结果放到一个滑动窗口里做非线性优化同时估计位姿、特征点深度、IMU零偏等状态量。滑动窗口你可以理解成“只看最近15到20帧的数据”避免所有历史数据都参与计算这样实时性和精度能达到一个平衡。这套方案叫“紧耦合”意思是视觉观测和IMU测量不是分开算出结果再加权平均而是在同一个优化目标里互相约束。IMU预积分负责给出两帧之间的相对运动估计视觉观测负责修正这个估计的漂移。回环检测则是当你重新回到之前去过的地方时识别出“这里来过”然后把累计的漂移一次性压掉这对大场景巡检特别重要。理解了这套机制你就知道为什么配置里那些参数不能乱填。比如IMU噪声参数填大了优化器会不信任IMU轨迹会偏向视觉特征特征点少的时候就会飘填小了系统会过于信任IMU轻微的零偏漂移都会被当成真实运动位置误差同样会累积。2. 环境准备Ubuntu、ROS和依赖项2.1 ROS版本怎么选才省心VINS-Fusion是基于ROS1开发的虽然社区里有一些ROS2移植版本但官方源码的维护重点始终在ROS1上。这个定位决定了你选系统的首要原则是能用ROS1就别折腾ROS2。如果你一定要上Ubuntu 22.04和ROS2 Humble就得额外安装ROS1 Noetic的桥接环境再通过ros1_bridge把话题转发过去链路长了之后时间同步和调试难度都会上升。我的推荐组合很明确Ubuntu 20.04搭配ROS Noetic这是目前资料最多、兼容性稳的组合。如果你是老用户用Ubuntu 18.04加ROS Melodic也没问题只是系统本身偏老一些新飞控SDK的支持不及时。Ubuntu 22.04用户也不是不能跑但最好在虚拟机或容器里单独开一个Ubuntu 20.04的环境别浪费大量时间在依赖兼容上。系统组合优点主要风险Ubuntu 18.04 ROS Melodic OpenCV 3.2官方默认版本编译最省事系统旧飞控SDK兼容性一般Ubuntu 20.04 ROS Noetic OpenCV 4.2系统新PX4/MAVROS生态好cv_bridge和OpenCV版本冲突需要处理Ubuntu 22.04 ROS2 Humble符合未来趋势VINS-Fusion需ROS1桥接链路复杂不推荐2.2 从零安装ROS Noetic安装ROS不是重点但很多人卡在这里。下面是一套最小化配置流程适用于Ubuntu 20.04。先把软件源和密钥配好然后安装桌面完整版这样rviz、cv_bridge、image_transport这些常用组件都会带上。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 install curl curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - sudo apt update sudo apt install ros-noetic-desktop-full安装完成后记得初始化rosdep并配置环境变量。sudo apt install python3-rosdep python3-rosinstall python3-catkin-tools sudo rosdep init rosdep update echo source /opt/ros/noetic/setup.bash ~/.bashrc source ~/.bashrc如果你还在熟悉Linux操作、不想手工敲命令市面上也有鱼香ROS一键安装脚本可以用它会自动处理软件源和环境变量配置对新手很友好。但我个人还是建议至少亲手走一遍上面的流程因为后面调试起来你需要知道ROS环境变量是怎么加载的Python脚本依赖装在哪这些都是排查问题的基本功。2.3 编译VINS-Fusion与Ceres SolverVINS-Fusion的另一个硬依赖是Ceres Solver它是做非线性最小二乘优化用的库。Ubuntu自带的软件源里没有合适的版本所以需要从源码编译。推荐用1.14.0稳定版本兼容性经过最多人验证。sudo apt-get install -y libgoogle-glog-dev libgflags-dev libatlas-base-dev libsuitesparse-dev wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz tar zxf ceres-solver-1.14.0.tar.gz cd ceres-solver-1.14.0 mkdir build cd build cmake .. -DCMAKE_BUILD_TYPERelease make -j4 sudo make install编译完Ceres后创建自己的工作空间并克隆VINS-Fusion源码。mkdir -p ~/vins_ws/src cd ~/vins_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd ~/vins_ws catkin_make source devel/setup.bash编译过程中如果报找不到OpenCV先确认系统里是否装了libopencv-dev。如果是Noetic环境默认OpenCV 4.2一般没问题但要注意cv_bridge的版本必须和系统OpenCV一致否则运行时会报符号找不到的错误。这个坑我在后面问题排查部分会细说。3. 标定与配置把传感器的“眼神”调准3.1 相机内参标定很多人觉得VINS-Fusion跑数据集能出轨迹就直接拿到真机上用结果一飞就飘然后怀疑算法不行。实际上数据集里的相机内参、IMU参数都是出厂标定好的真机上每一颗镜头、每一块IMU都不一样标定工作省不了。相机内参标定最简单的方式是使用ROS自带的camera_calibration功能包。你可以把标定棋盘格打印出来拿在手上在镜头前缓慢移动覆盖画面的四角和中间区域尽量让格子出现在不同距离和角度上。系统会自动检测棋盘格角点采集到足够样本后计算出焦距、主点坐标和畸变系数。标定结果类似下面这样%YAML:1.0 --- K: [458.35, 0.0, 326.78, 0.0, 461.72, 243.68, 0.0, 0.0, 1.0] D: [-0.2834, 0.0723, 0.0008, 0.0005, 0.0]这些参数要填进VINS-Fusion的配置文件。注意VINS的配置里还分distortion_model常见的是pinhole和kannala-brandt普通工业相机用pinhole配合plumb_bob畸变模型就够。如果你的镜头是鱼眼必须用MEI模型否则特征点外极约束会出错初始化成功率大幅下降。3.2 IMU噪声密度与随机游走参数IMU参数是VINS-Fusion配置里最容易被忽略但也最关键的一项。你会在配置文件里看到四个小数imu: frequency: 200 gyro_noise_density: 0.00016 gyro_random_walk: 0.0000228 accel_noise_density: 0.0028 accel_random_walk: 0.0004gyro_noise_density是陀螺仪白噪声密度单位是rad/s/√Hz代表测量值里的高频噪声水平。gyro_random_walk是零偏随机游走单位是rad/s²/√Hz描述零偏随时间慢慢漂移的速度。加速度计对应的两个参数同理只是单位和物理量不同。这些参数怎么来最严谨的办法是用imu_utils工具录制一段IMU静止数据再离线分析。具体操作是先让无人机静止放在桌面上录1小时以上IMU数据然后运行工具包里的imu_an.bag分析节点它会输出噪声密度和随机游走估计值。问题是录1小时确实有点磨人很多项目赶时间就直接查IMU芯片的数据手册比如BMI088陀螺仪噪声密度约0.00016这个值用起来差异不大。但有一点必须注意frequency参数必须和你实际发布的IMU话题频率一致。PX4飞控默认IMU频率通常是250Hz或200Hz你如果配置里写400预积分计算的时间戳就会对不上轨迹很容易出现周期性的跳动。3.3 相机到IMU的外参和时延外参解决的是“相机在机体哪个位置、朝哪个方向看”的问题。VINS-Fusion配置文件里对应的是extrinsicRotation和extrinsicTranslation一个是3x3旋转矩阵一个是3x1平移向量表示相机坐标系到IMU坐标系的变换关系。如果你有结构设计图纸可以直接量出大致平移再计算旋转。但更推荐用Kalibr工具做联合标定它能同时估计外参和相机与IMU之间的时间延迟。这个过程需要录制一个包含棋盘格或AprilGrid标定板的bag让无人机或手持设备在标定板前做各种姿态运动。整套操作有点门槛但一遍跑通之后你得到的是一组可以直接写进VINS配置的旋转平移数据比手量靠谱得多。外参误差的影响有多大我遇到过外参旋转角差2度的情况VINS初始化能过但飞行一段时间后轨迹会沿着某个方向持续弯曲误差累积速度肉眼可见。还有时候无人机悬停原地却看到点云在缓慢旋转基本都是外参旋转没对齐。不要为了省事把外参写成单位矩阵除非你的相机和IMU真的做到了光轴完全平行且位置重合但那在实际机械结构里几乎不可能。4. 实战运行先跑官方数据集再上真机4.1 用EuRoC数据集快速跑通流程拿到一个新的机器我习惯先用EuRoC数据集验证VINS-Fusion的编译结果是否正常。EuRoC是苏黎世联邦理工发布的室内无人机数据集里面包含双目相机图像、IMU数据和地面真值轨迹话题名都是固定的VINS-Fusion官方配置文件已经匹配好直接能跑。启动三个终端分别执行roslaunch vins_estimator euroc.launch roslaunch vins_estimator vins_rviz.launch rosbag play /path/to/MH_01_easy.bag第一次跑的时候你会注意到rosbag播放前几秒rviz里没有轨迹这是正常的。VINS需要积累足够的视差和IMU激励才能完成初始化所以一开始会有一段等待。等画面里的特征点被持续跟踪轨迹会自动出现在rviz中。通过这个流程你还能直观感受VINS的初始化条件。EuRoC数据集的起始段包含缓慢平移和旋转就是专门为了让视觉和IMU充分激励设计的。在真机上如果初始姿态太平稳比如放在桌面上静止不动就启动算法初始化就很难成功必须手动拿着无人机转几圈。4.2 修改launch文件适配自己的相机与飞控跑通数据集后就该配置自己的传感器了。VINS-Fusion的launch文件里有几个关键参数config_file指向你自定义的yaml文件.launch文件里还会设定图像话题名和IMU话题名。官方给的示例是launch node namefeature_tracker pkgfeature_tracker typefeature_tracker outputscreen param nameconfig_file typestring value$(find feature_tracker)/../config/my_uav/my_uav.yaml / /node node namevins_estimator pkgvins_estimator typevins_estimator outputscreen param nameconfig_file typestring value$(find vins_estimator)/../config/my_uav/my_uav.yaml / /node /launch如果你的相机话题是/camera/infra1/image_rect_rawIMU话题是/mavros/imu/data需要在launch里加上remap把VINS默认监听的/cam0/image_raw、/cam0/image_raw、/imu0重映射过去。这里最容易出的问题是图像话题用了compressed压缩格式VINS的frontend没有直接接压缩图像需要先用image_transport节点把压缩话题转成原始话题或者直接订阅原始图像。yaml文件里的配置项也要按实际改包括图像分辨率、相机内参、畸变参数、IMU参数、最大特征点数等。如果用的是双目相机左目和右目的外参基线长度必须准确基线误差过大会直接导致深度估计错误单目则不需要填cam1的配置VINS会退化为单目IMU的里程计模式。4.3 把VINS位姿接入PX4飞控真机使用的典型架构是机载计算机运行VINS-Fusion飞控运行PX4两者通过MAVROS通信。VINS估计出的位姿会发布在/vins_estimator/odometry话题上但这个话题格式是nav_msgs/OdometryPX4不能直接使用需要把它转发成/mavros/vision_pose/pose。一个简单做法是写一个几行的Python节点订阅/vins_estimator/odometry然后提取位姿发布到/mavros/vision_pose/pose。PVROS收到后会把视觉位姿送到PX4的EKF2里进行融合。同时需要在PX4参数里设置EKF2_AID_MASK把视觉位置融合的开关打开不同固件版本参数含义略有差异一般对应bit位里的“vision position fusion”。这个环节最容易翻车的是坐标系没有对齐。VINS跑出来的世界系默认是ENUPX4的视觉惯性里程计约定也期望ENU但如果你把相机装歪了或者外参标定得到的是机体坐标系到相机坐标系的变换方向没有顺手转换飞控收到的视觉位置就可能和实际位置差很远的固定偏置。真机测试时如果出现“飞机明明悬停但EKF位置一直往一个方向跑”的现象先回头检查这个坐标变换而不是急着调PID。5. 常见问题与排查技巧速查5.1 编译阶段OpenCV和Ceres版本冲突我见过最多的问题出现在编译阶段典型报错是找不到某个OpenCV组件或者Ceres头文件和库版本不匹配。如果你在Ubuntu 20.04上用Noetic编译VINS-Fusion时一旦报OpenCVConfig.cmake not found先确认是否安装了libopencv-dev。如果装了还是找不到很可能是你自己从源码编译过OpenCVCMake在搜索路径里优先找到了旧版本需要在CMakeLists.txt里手动指定OpenCV路径。Ceres的问题则更隐蔽。很多人用sudo apt install libceres-dev装系统包版本通常是1.14一般没问题。但如果你的系统源里ceres版本很新或者之前为了跑其他项目编译了Ceres 2.xVINS-Fusion在链接阶段就会报一些模糊的API错误比如找不到Ceres::SUMMARY之类。解决办法是卸载旧版本从源码重新安装1.14.0。如果编译过程报内存不足或者make -j8崩溃先降成make -j4试试。Ceres和VINS都是计算密集型项目虚拟机里分2核编译大项目本来就容易把内存耗尽把并行数调低就能解决。5.2 初始化失败别急着怀疑代码VINS初始化失败的典型表现是终端一直刷waiting for image and imu或者rviz里始终没有轨迹出现。这时候先别急着怀疑算法有没有bug按顺序排查三件事。先看话题是否真的通了。用rostopic hz /imu0和rostopic hz /cam0/image_raw确认频率正常图像话题有没有画面IMU话题数据有没有更新。很多人是launch文件里remap写错VINS根本没拿到数据自然会一直等待。再看IMU数据有没有激发。VINS的初始化阶段需要足够的旋转和平移激励来估计重力方向、尺度以及IMU零偏。如果你把无人机放在桌上纹丝不动VINS会一直处于等待初始化状态。正确做法是启动后手持无人机绕各个轴缓慢转一圈再前后左右小幅平移让视觉视差和IMU角速度信号都充分出现。最后检查图像质量。曝光太强的逆光场景、纯白墙面、重复纹理的地面都会让特征提取失败。VINS前端对特征点数量有个最低阈值如果画面干净到提不出20个有效角点初始化就不可能成功。5.3 运行中漂移与跳变先查外参再查硬件如果系统能初始化但飞行过程中轨迹明显漂移最简单的排查顺序是先看是不是外参不准。你可以把无人机放在地面缓慢旋转观察rviz里估计出的机体坐标系是否跟着旋转一致。如果旋转角度对不上或者平移方向反了那几乎可以断定外参矩阵有问题。另一种情况是轨迹整体还好但每隔几十秒突然跳一下。这个现象十有八九和IMU时间戳有关。VINS的td参数可以补偿一部分固定时延但如果你用的相机是滚动快门高速运动时每一行曝光时间都不同VINS默认把整张图像当作同一时刻的观测误差会被放大。这也是为什么做真机时我更推荐用全局快门相机至少要把曝光时间固定住不要用自动曝光。还有一类比较难查的漂移来自IMU本身。机身振动大、飞控减震没做好IMU输出的噪声会明显增大VINS优化器估计出的零偏会频繁跳变。遇到这种情况先检查机臂电机是否平衡飞控和机载电脑的减震海绵是否安装到位再回头怀疑算法参数。我见过不少团队调了一个月参数没改善最后发现是减震没做好的例子。6. 真机落地前必须处理好的几个细节6.1 IMU频率、时间戳和机载算力真机设计和数据集测试最大的区别在于你需要自己保证数据质量。VINS对IMU频率并不挑剔但频率太低会明显削弱IMU预积分的约束能力一般至少要100Hz以上飞控输出200Hz是比较好用的状态。图像频率不用太高10到30Hz都行关键是每帧图像的时间戳要稳定。如果使用机载电脑算力是另一个硬约束。VINS在Jetson Nano级别设备上跑640分辨率图像大约能维持15到20Hz的处理速度勉强够用但留给其他模块的CPU就很紧张了。如果还需要同时跑目标检测或避障网络建议上Jetson Orin NX一类算力更强的设备或者降低图像分辨率到480p。时间戳问题容易被忽视。VINS输出的里程计时间戳依赖输入图像和IMU的ROS时间戳如果相机驱动和飞控驱动的时基没有统一VINS估计出的轨迹会出现周期性抖动。最稳妥的做法是让机载电脑用ptpd或chrony和飞控做网络时间同步或者直接用硬件同步线把相机触发信号和飞控的PPS信号对接起来。6.2 减震、曝光和镜头选择硬件层面的选择直接决定算法效果的上限。首先说镜头强烈建议用全局快门相机。滚动快门在无人机快速转向时会出现果冻效应画面里的竖直杆子会变歪特征点坐标也会带上时间相关的位移误差算法很难补偿。再说曝光固定曝光时间是必须的。自动曝光在室内窗户边飞的时候画面亮度会剧烈变化特征点跟踪会大量丢失。我的做法是先用相机调试工具固定曝光值同时打开增益限制保证室内外切换时画面不会瞬间过曝。IMU减震也不要轻视。很多人直接把机载电脑或者传感器板用铜柱硬装在机架上电机振动直接传到IMU上。正确做法是给IMU模块加一层减震泡棉让它和机架在物理上绝缘。实测下来减震做没做好VINS轨迹噪声能差一个数量级。6.3 先用bag回放调试再让飞机离开地面真机试飞前最该做的事是先用rosbag把相机和IMU的原始数据录下来回放到VINS里调参。这样你不需要让飞机离地就能把外参、噪声参数、特征点数量这些核心参数调到初见雏形然后再进入真机测试。录bag的时候要注意话题名和频率都要和实际使用时的配置一致最好把/mavros/imu/data原始IMU和图像话题一起录上。回放时用rosbag play --clock加use_sim_time true的launch方式这样可以保证时间戳在回放时是单调递增的。真机测试的切入点也很重要我第一次带新机上真机时先在空旷户外让飞机悬停同时启动VINS观察静止状态下的位姿漂移。如果悬停状态下位姿都在缓慢移动说明标定数据还有问题不要贸然切换飞控的视觉融合模式。等悬停漂移控制在厘米级以后再做手动缓慢平移和旋转测试确认轨迹不跟手再逐步放开速度和距离。每一步都验证过之后再解开姿态模式限制把视觉定位真正接入到航线飞行里。这个流程虽然慢但比一次直接飞出去炸机划算得多。