1. 从零搭建无人机视觉SLAM系统整体方案与选型思路搞无人机视觉SLAM这件事我前前后后折腾了快两年从最早的ORB-SLAM跑通单目跑得歪歪扭扭到后来用VINS-Fusion把双目IMU的组合调稳中间踩的坑能写满一个笔记本。这篇文章不讲虚的就把我从零搭建一套能用的无人机视觉SLAM系统的完整过程拆开来讲包括为什么选VINS-Fusion、传感器怎么配、标定怎么做、和PX4怎么对接以及那些只有真正上手才会遇到的坑。先说清楚这套系统能干什么它让无人机在没有GPS的环境下仅靠摄像头和IMU就能知道自己在哪、往哪飞、姿态如何。适合谁看如果你已经会基本的ROS操作手上有一架能飞的无人机或者正在搭一台想上视觉SLAM但不知道从哪下手那这篇就是写给你的。如果你连ROS都没装过建议先把ROS基础过一遍再回来不然中间很多操作会卡住。1.1 为什么是VINS-Fusion而不是ORB-SLAM3很多人入门视觉SLAM第一个接触的是ORB-SLAM系列这没错ORB-SLAM2/3在纯视觉里程计上确实成熟。但放到无人机场景情况就不一样了。无人机飞行时面临几个现实问题快速运动导致图像模糊、光照变化剧烈、纯视觉尺度不确定、单目初始化慢。ORB-SLAM3虽然支持IMU但它的IMU融合是松耦合的在无人机这种高动态场景下容易跟丢。VINS-Fusion的核心优势在于它是紧耦合的视觉惯性里程计把IMU预积分和视觉观测放在同一个优化框架里求解。通俗点说ORB-SLAM是“视觉为主、IMU辅助”VINS-Fusion是“视觉和IMU平起平坐一起算”。这个区别在无人机快速机动时体现得非常明显——我实测同一段飞行数据ORB-SLAM3在急转弯时位姿跳变明显VINS-Fusion就平滑得多。另一个关键点是VINS-Fusion原生支持双目IMU双目直接给出尺度信息不需要像单目那样靠IMU激励来恢复尺度。对于无人机来说尺度准确意味着你算出来的位置直接就是米不用再额外换算和PX4对接时省了很多事。1.2 硬件选型不是越贵越好传感器这块我走过弯路一开始觉得相机越贵越好买了个工业级全局快门相机结果发现和IMU的时间同步搞不定反而耽误了两周。后来换成Intel RealSense D435i虽然参数不是顶级但它出厂就做了相机和IMU的时间同步省了大事。具体配置我列一下组件型号关键参数选它的理由相机Intel RealSense D435i双目红外RGB内置BMI055 IMU出厂同步ROS驱动成熟飞控Pixhawk 4 / Pixhawk 6C内置ICM20689BMI055PX4原生支持社区资料多机载计算机Jetson Xavier NX6核ARM384核GPU算力够跑VINS-Fusion功耗低飞控固件PX4 v1.14.3稳定版和ROS2桥接成熟这里要特别说一句D435i内置的IMU是BMI055这个IMU精度一般零偏不稳定如果你对精度要求高建议外接一个更好的IMU比如ICM42688或者ADIS16470。但外接IMU就涉及时间同步问题需要用硬件触发或者PPS信号复杂度上一个台阶。我建议新手先用D435i内置IMU跑通全流程再考虑升级。机载计算机选Jetson Xavier NX是因为VINS-Fusion虽然不算特别吃算力但双目特征提取后端优化在ARM上跑还是需要一定性能的。我试过用树莓派4B跑帧率只能到10Hz左右而且发热严重降频。Xavier NX能稳定跑30Hz留有余量。1.3 软件架构ROS1还是ROS2这个问题我被问过无数次。2024年了ROS2确实是大势所趋PX4 v1.14之后官方主推ROS2桥接。但VINS-Fusion的官方代码是基于ROS1的虽然社区有ROS2移植版但成熟度不如ROS1版本。我的建议是如果你只是做算法验证和室内飞行用ROS1 Noetic就够了Ubuntu 20.04 Noetic是经过大量验证的组合资料最全。如果你要做产品化或者需要和最新PX4深度集成那就上ROS2 Humble但要做好踩坑的准备。我自己的配置是Ubuntu 20.04 ROS Noetic PX4 v1.14.3这个组合最稳。安装ROS我用的是鱼香ROS一键安装脚本确实省事但要注意脚本版本老版本可能源地址失效建议用最新版。整个软件数据流是这样的D435i通过ROS驱动发布双目图像和IMU数据VINS-Fusion订阅这些话题做视觉惯性里程计输出位姿估计然后通过MAVROS或者microRTPS桥接发给PX4PX4用这个位姿做位置控制。听起来简单但中间每个环节都有坑。2. 环境搭建与传感器标定最容易被低估的环节环境搭建这块我不想按部就班地列命令网上教程太多了。我想讲的是那些教程里不会告诉你、但实际一定会遇到的问题。标定更是重灾区很多人SLAM跑不起来90%是标定没做好。2.1 ROS与PX4开发环境的避坑安装ROS Noetic安装本身不难鱼香ROS一键安装确实方便但有几个点要注意。第一安装脚本会修改你的源列表如果你之前手动配过源可能会冲突建议先备份/etc/apt/sources.list。第二安装完成后一定要执行rosdep init和rosdep update这两个步骤在国内网络环境下经常失败多试几次或者换时间段。PX4开发环境搭建更麻烦一些。官方推荐用Ubuntu 20.04因为PX4 v1.14.3的工具链在这个版本上验证最充分。安装步骤大致是克隆PX4源码、运行ubuntu.sh脚本安装依赖、然后make px4_fmu-v5_default编译。编译过程大概需要15-30分钟取决于你的机器性能。这里有个大坑PX4的ubuntu.sh脚本会安装特定版本的gcc-arm-none-eabi工具链如果你系统里已经有其他版本可能会冲突。我遇到过编译报错“arm-none-eabi-gcc: not found”就是因为PATH里工具链版本不对。解决办法是手动下载官方推荐的版本解压后把bin目录加到PATH最前面。还有一个常见问题是Python依赖。PX4的编译脚本依赖Python3但有些包需要特定版本。我建议用virtualenv创建一个独立环境避免污染系统Python。具体操作python3 -m venv ~/px4-venv source ~/px4-venv/bin/activate pip install --upgrade pip pip install empy3.3.4 pyros-genmsg jsonschema注意empy的版本新版empy 4.x和PX4编译不兼容必须用3.3.4。2.2 相机与IMU标定精度决定成败标定是VINS-Fusion能否跑稳的关键。我见过太多人SLAM跑飞了最后发现是标定参数错了。标定分两部分相机内参标定和相机-IMU外参标定。相机内参标定用ROS的camera_calibration包就行准备一个棋盘格标定板注意棋盘格要平整打印精度要高。标定的时候要覆盖图像各个区域不要只在一个位置晃。我一般采集40-50张不同角度的图像保证重投影误差在0.2像素以内。双目相机还要做立体标定得到两个相机之间的外参。D435i的话其实出厂已经标定好了但如果你要自己重新标用camera_calibration的stereo模式。重头戏是相机-IMU外参标定。这里我推荐用Kalibr工具虽然安装麻烦但精度最高。Kalibr需要ROS环境安装过程大概是mkdir -p ~/kalibr_ws/src cd ~/kalibr_ws/src git clone https://github.com/ethz-asl/kalibr.git cd .. catkin build -DCMAKE_BUILD_TYPERelease编译Kalibr可能会遇到依赖问题主要是Python版本和某些库的兼容性。如果报错检查你的Python是不是3.8Kalibr对Python版本比较敏感。标定数据采集有讲究要把标定板在相机视野内充分激励所有轴特别是绕X、Y、Z轴的旋转每个轴至少激励两次。IMU数据要同步采集Kalibr会自动做时间对齐。采集时间建议60-90秒太短激励不充分太长数据量大处理慢。标定完成后Kalibr会输出一个yaml文件里面包含相机内参、畸变系数、相机-IMU外参和时间偏移。这个文件就是VINS-Fusion的配置文件基础。注意标定时的环境光线要均匀避免强光直射和阴影。标定板移动要平滑不要快速晃动否则运动模糊会导致角点检测不准。2.3 IMU噪声参数标定被忽视的重要步骤很多人标定完相机-IMU外参就以为完事了其实IMU的噪声参数同样重要。VINS-Fusion的配置文件里需要填IMU的噪声密度和随机游走这些参数如果随便填优化结果会差很多。IMU噪声标定我推荐用imu_utils工具它基于Allan方差分析。操作步骤是把IMU静止放置2小时以上采集数据然后用imu_utils处理。注意采集时IMU要放在稳固的平面上避免任何振动。Allan方差分析会给出几个关键参数陀螺仪噪声密度、陀螺仪随机游走、加速度计噪声密度、加速度计随机游走。这些值直接填到VINS-Fusion的配置文件里。我实测D435i内置BMI055的陀螺仪噪声密度大概在0.0002 rad/s/√Hz左右加速度计噪声密度在0.002 m/s²/√Hz左右。但每颗IMU个体差异大一定要自己标。这里有个经验如果你发现VINS-Fusion跑起来位姿漂移严重先检查IMU噪声参数。我遇到过标定参数填错一个数量级结果轨迹直接发散的情况。3. VINS-Fusion配置与运行从配置文件到实飞标定做完接下来就是配置VINS-Fusion。这部分我会把配置文件的关键参数逐个解释然后讲怎么和PX4对接。3.1 配置文件逐参数解析VINS-Fusion的配置文件是yaml格式主要分几块相机参数、IMU参数、外参、输出设置。我拿我的D435i配置举例逐条说明。相机参数部分model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: -0.05 k2: 0.01 p1: 0.0001 p2: 0.00001 projection_parameters: fx: 380.0 fy: 380.0 cx: 320.0 cy: 240.0这些参数来自你的相机标定结果。注意D435i的红外双目图像是灰度图分辨率我设的640x480帧率30Hz。如果你用RGB图像参数会不同。IMU参数imu_topic: /camera/imu acc_n: 0.002 gyr_n: 0.0002 acc_w: 0.00002 gyr_w: 0.000002acc_n和gyr_n是噪声密度acc_w和gyr_w是随机游走来自IMU标定。外参部分body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ 1, 0, 0, 0.05, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ]这是相机到IMU的变换矩阵来自Kalibr标定。注意VINS-Fusion用的是body坐标系IMU坐标系到相机坐标系的变换方向不要搞反。输出设置output_path: /home/user/vins_output/ pose_graph_save_path: /home/user/vins_output/pose_graph/ save_image: 0output_path是轨迹和地图保存路径save_image设0节省磁盘。3.2 单目、双目、双目IMU模式的选择VINS-Fusion支持多种模式配置文件里通过参数控制。单目模式只需要一个相机话题但需要IMU做尺度恢复。双目模式不需要IMU也能跑但精度和鲁棒性不如双目IMU。我强烈建议用双目IMU模式这是最稳的配置。双目提供尺度IMU提供高频运动估计和重力方向两者互补。配置时注意imu: 1 num_of_cam: 2imu设1启用IMUnum_of_cam设2用双目。如果你只有单目相机那imu必须设1否则尺度不确定。单目IMU的初始化需要足够的运动激励无人机起飞时要有一段明显的平移和旋转否则初始化失败。3.3 与PX4的对接MAVROS还是microRTPSVINS-Fusion输出的是位姿估计要发给PX4做控制需要桥接。ROS1环境下用MAVROSROS2环境下用microRTPS。我用的是MAVROS配置相对简单。MAVROS安装sudo apt install ros-noetic-mavros ros-noetic-mavros-extras然后要安装GeographicLib数据集sudo /opt/ros/noetic/lib/mavros/install_geographiclib_datasets.sh这个脚本会下载地理数据国内网络可能慢耐心等。启动MAVROS连接飞控roslaunch mavros px4.launch fcu_url:/dev/ttyUSB0:921600fcu_url根据你的连接方式改USB直连一般是/dev/ttyUSB0波特率921600。VINS-Fusion的输出要转成MAVROS能识别的格式。VINS-Fusion发布的是/vins_estimator/odometry话题类型是nav_msgs/Odometry。MAVROS需要的是mavros/vision_pose/pose类型是geometry_msgs/PoseStamped。写一个简单的转换节点就行#!/usr/bin/env python3 import rospy from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseStamped def odom_callback(msg): pose PoseStamped() pose.header msg.header pose.pose msg.pose.pose pub.publish(pose) rospy.init_node(vins_to_mavros) pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size10) rospy.Subscriber(/vins_estimator/odometry, Odometry, odom_callback) rospy.spin()PX4那边要设置参数启用视觉位姿融合param set EKF2_AID_MASK 24 param set EKF2_HGT_MODE 3 param set EKF2_EV_DELAY 0.01EKF2_AID_MASK设24表示启用视觉位置和视觉偏航融合。EKF2_HGT_MODE设3表示用视觉做高度估计。EKF2_EV_DELAY是视觉延迟根据你的系统调整一般10-20ms。注意PX4的参数设置要在QGC或者MAVROS里做设置完要重启飞控生效。第一次测试一定要在室内悬停模式下确认视觉位姿稳定后再切定点模式。4. 常见问题与排查技巧实录这部分是我踩坑最多的每个问题都是真金白银换来的经验。4.1 VINS-Fusion初始化失败初始化失败是最常见的问题表现是启动后一直不输出位姿或者输出“initialization failed”。原因通常有几个第一IMU数据没进来。检查rostopic hz /camera/imu看有没有数据。D435i的IMU话题默认是/camera/imu但有些驱动版本是/camera/imu/data配置要对应。第二运动激励不够。VINS-Fusion初始化需要足够的平移和旋转如果你把设备静止放着它永远初始化不了。正确做法是手持设备做“8”字运动或者无人机起飞时先缓慢平移一段。第三时间戳不同步。相机和IMU的时间戳必须对齐如果差太多初始化会失败。D435i出厂同步过但如果你用外接IMU要确保时间同步。第四标定参数错误。检查外参矩阵方向对不对IMU噪声参数数量级对不对。排查步骤我整理成表现象可能原因排查方法一直不初始化IMU无数据rostopic hz检查初始化后漂移外参错误检查body_T_cam方向初始化后跳变时间戳不同步对比相机和IMU时间戳初始化后尺度不对双目外参错误重新立体标定4.2 轨迹漂移与跟丢跑起来之后轨迹漂移是另一个大问题。VINS-Fusion虽然比纯视觉稳但在纹理少、光照变化大的场景还是会漂。我遇到过一次在长走廊飞行两侧白墙纹理极少VINS-Fusion直接跟丢。解决办法是降低特征点提取阈值让它在弱纹理下也能提取到点。配置文件里max_cnt参数控制每帧最大特征点数默认150可以调到200-250。但注意调太高会增加计算量Jetson Xavier NX上可能跑不到30Hz。另一个漂移原因是IMU零偏估计不准。VINS-Fusion在线估计IMU零偏但如果零偏太大估计收敛慢。解决办法是起飞前让无人机静止30秒让IMU零偏收敛。还有回环检测。VINS-Fusion支持回环但默认可能没开。配置文件里loop_closure设1启用。回环能显著减少累积漂移但会增加计算量。4.3 与PX4对接后的控制问题VINS-Fusion和PX4对接后常见问题是无人机切定点模式后乱飞或者高度不稳。第一个检查点坐标系。VINS-Fusion输出的位姿是ENU坐标系东-北-天PX4内部是NED北-东-地。MAVROS会自动转换但如果你自己写转换节点要注意坐标系。第二个检查点高度源。PX4默认用气压计做高度如果你要用视觉高度要设EKF2_HGT_MODE为3。但视觉高度在纹理少的地面可能不准我建议室内飞行用视觉高度室外还是用气压计或者GPS。第三个检查点延迟。视觉位姿有延迟如果EKF2_EV_DELAY设得不准会导致控制振荡。测量延迟的方法是让无人机快速上下移动对比视觉高度和气压计高度的相位差估算延迟。第四个检查点频率。VINS-Fusion输出频率一般20-30HzPX4的EKF2需要至少10Hz的视觉位姿。如果频率太低EKF2会拒绝融合。4.4 性能优化让Jetson跑得更流畅Jetson Xavier NX跑VINS-Fusion默认配置可能只有15-20Hz。优化方法有几个第一降低图像分辨率。640x480够用不用上1280x720。第二减少特征点数量。max_cnt从150降到100精度损失不大速度提升明显。第三关闭不必要的输出。save_image设0pose_graph设0。第四用GPU加速。VINS-Fusion的特征提取可以用GPU但需要编译时开启CUDA支持。这个稍微麻烦但效果显著能提升30%以上。第五调整Jetson功耗模式。sudo nvpmodel -m 0是最大性能模式但发热大注意散热。我实测优化后能稳定跑30HzCPU占用60%左右留有余量给其他节点。5. 实飞测试与调参经验理论讲完了最后说说实飞。第一次实飞一定要在室内有保护措施比如系绳或者在大空间。5.1 首次实飞检查清单起飞前逐项检查VINS-Fusion是否初始化成功位姿输出是否稳定MAVROS是否连接rostopic echo /mavros/state看connected是否为true视觉位姿是否发布到/mavros/vision_pose/posePX4参数EKF2_AID_MASK和EKF2_HGT_MODE是否正确遥控器是否设置好切换开关能随时切回手动模式电池电量是否充足首次飞行建议先手动起飞悬停观察VINS-Fusion位姿是否和实际运动一致。然后切定点模式看无人机是否能稳定悬停。如果出现漂移立即切回手动。5.2 调参心得从能飞到飞得好VINS-Fusion的调参主要围绕几个参数max_solver_time后端优化最大时间默认0.04秒。如果Jetson性能不够可以调到0.05-0.06但精度会下降。keyframe_parallax关键帧选择阈值默认10像素。调大减少关键帧数量速度快但精度低调小增加关键帧精度高但计算量大。estimate_td是否在线估计时间偏移默认1。如果相机和IMU时间同步好可以设0减少计算量。rolling_shutter卷帘快门补偿D435i是卷帘快门设1。如果是全局快门相机设0。我调参的经验是先保证能稳定飞再逐步优化精度。不要一上来就追求完美SLAM是个系统工程每个环节都有误差关键是整体稳定。5.3 后续扩展方向这套系统跑通后可以往几个方向扩展第一加激光雷达做多传感器融合。VINS-Fusion本身不支持激光但可以外接一个激光SLAM做位姿融合比如LIO-SAM。激光视觉IMU的组合在复杂环境下鲁棒性更好。第二加避障。VINS-Fusion只做定位避障需要额外的感知模块。可以用D435i的深度图做局部避障或者加超声波/红外传感器。第三做自主导航。定位有了加一个路径规划模块就能做自主飞行。ROS里的move_base或者ego-planner都可以用。第四优化计算平台。Jetson Xavier NX够用但如果要做更复杂的任务可以考虑Orin NX或者X86平台。我个人在实际操作中的体会是视觉SLAM这件事理论看再多不如动手跑一遍。标定、配置、调试每个环节都会遇到意想不到的问题但解决一个就进步一点。VINS-Fusion是目前无人机视觉SLAM里比较成熟好用的方案值得花时间深入。最后分享一个小技巧每次修改配置文件前先备份SLAM调试过程中你会改无数遍参数有个备份能省很多事。