1. 视觉惯性SLAM方案选型与整体架构拆解1.1 为什么在众多SLAM方案里挑中VINS-Fusion做无人机室内外自主飞行定位方案的选择基本决定了后面所有工作的上限。我最早用过纯激光雷达方案建图精度确实好但激光雷达的重量和功耗对小型无人机来说太奢侈后来试过纯视觉的ORB-SLAM光照一变、纹理一少就飘得厉害快速飞行时更是直接跟丢。折腾了几轮之后我把目光锁定在视觉惯性融合这条路上而VINS-Fusion是我实测下来在无人机场景里综合表现最均衡的一套开源方案。VINS-Fusion的核心思路是把相机和IMU的数据做紧耦合优化。相机提供环境的结构信息IMU提供高频的运动先验两者互补视觉在纹理丰富、光照稳定的地方修正IMU的漂移IMU在快速运动、纹理缺失、短暂遮挡时撑住位姿估计不崩。这种互补特性对无人机太重要了因为无人机飞行时既有悬停这种低速场景也有急加速、急转向这种高动态场景单一传感器根本扛不住。选VINS-Fusion还有几个很实际的理由。第一它同时支持单目IMU、双目IMU、纯双目等多种配置你手头有什么硬件就能跑什么配置不用为了算法去凑设备。第二它输出的位姿可以直接对接PX4的EKF2做外部视觉定位省去了自己写融合节点的麻烦。第三社区活跃遇到问题基本能搜到答案这对新手来说能省下大量时间。提示VINS-Fusion不是那种装完就能飞的傻瓜方案它对标定质量、时间同步、参数配置都很敏感。如果你期待的是插上就能用那可能会失望但如果你愿意花时间把标定和配置做扎实它的表现会非常稳。1.2 单目、双目、RGB-D到底怎么选这是新手问得最多的问题我直接给结论再解释原因。配置类型优点缺点适用场景单目IMU硬件成本最低一个相机就够尺度依赖IMU初始化慢纯旋转时退化预算有限、室内慢速飞行双目IMU有绝对尺度初始化快鲁棒性最好硬件成本高标定复杂算力要求高室外飞行、快速机动、专业应用RGB-DIMU直接有深度近距离精度高室外红外失效量程有限重量大室内近距离、避障研究我个人的建议是如果预算允许直接上双目IMU。单目虽然便宜但尺度问题在实际飞行中会带来很多麻烦比如你规划飞5米实际可能飞了4米或者6米这种不确定性在需要精确悬停或者穿窗的场景里是致命的。双目通过基线直接算出深度尺度是确定的初始化也快得多。RGB-D看起来很美但D435这类相机在室外基本废掉红外结构光在阳光下完全被淹没。而且RGB-D的有效量程通常只有几米无人机稍微飞高一点深度就不可靠了。所以除非你只做室内近距离研究否则不建议把RGB-D作为主力方案。1.3 整体系统架构长什么样一套完整的无人机视觉SLAM系统从硬件到软件可以分成四层我用文字把数据流串一遍你脑子里要有这张图。最底层是传感器层相机负责采图IMU负责采角速度和加速度。这两个传感器的时间戳必须对齐否则融合就是灾难。中间是计算层机载电脑比如Jetson系列或者Intel NUC跑VINS-Fusion输出位姿估计。再往上是飞控层PX4接收外部视觉位姿通过EKF2和内部IMU做融合输出最终的控制指令。最上面是应用层你的规划算法、避障逻辑、任务脚本都跑在这一层消费的是飞控融合后的位姿。数据流是这样的相机和IMU的数据先进入VINS-Fusion它输出一个频率通常在10-30Hz的位姿位置姿态。这个位姿通过MAVROS或者直接通过串口发给PX4PX4的EKF2把它当作外部视觉观测和飞控自身的IMU、气压计、磁力计做融合最终输出一个高频通常250Hz以上的稳定位姿给控制器。控制器再根据你的目标位置算出电机指令。这里有个关键点VINS-Fusion的输出频率不高不能直接拿来控电机。它必须经过PX4的EKF2做一次融合和升频才能用于实际控制。很多人第一次做的时候想跳过PX4直接用VINS的位姿控飞机结果就是飞机抖得像筛糠因为10Hz的位姿根本跟不上控制环的需求。2. 硬件选型、环境搭建与标定实操2.1 硬件清单与选型避坑先把硬件列清楚我按重要程度排序。机载电脑是整个系统的算力核心。VINS-Fusion在双目IMU配置下CPU占用大概在1.5到2个核心内存占用500MB左右。Jetson Xavier NX或者Orin NX是很合适的选择算力够、功耗低、重量轻。如果用Intel NUC性能更强但功耗和重量都上去了只适合大一点的机型。我实测Jetson Orin NX跑双目VINS-FusionCPU占用率在40%左右还有余量跑规划算法。相机方面双目我推荐用全局快门相机比如OV9281这类。卷帘快门在快速运动时会有果冻效应对VINS的特征跟踪影响很大。分辨率不用太高640x480或者752x480就够太高反而增加算力负担。帧率建议30Hz以上和IMU的200Hz配合比较舒服。如果是单目普通USB相机也能凑合但同样建议全局快门。IMU的选择很关键。VINS-Fusion对IMU的要求是频率至少200Hz噪声密度要低最好有硬件时间同步。很多飞控自带的IMU比如Pixhawk上的ICM20602其实可以用但需要通过MAVROS把数据转发给VINS延迟会大一些。如果追求性能可以单独挂一个高精度IMU比如BMI088或者ICM42688通过SPI或者串口直接读。飞控用PX4生态的就行Pixhawk 4、Pixhawk 6C、CUAV V5都可以。固件版本建议用1.13或者1.14这两个版本对外部视觉定位的支持比较成熟。注意相机和IMU之间的硬件时间同步是很多人忽略的坑。如果两者时间戳不同步VINS-Fusion的初始化会很难收敛飞起来也会飘。最简单的做法是用飞控给相机发触发信号或者用支持硬件同步的相机模块。如果实在做不到硬件同步至少要在软件层面做时间戳对齐把IMU数据插值到相机时间戳上。2.2 软件环境搭建从系统到依赖软件环境我推荐Ubuntu 20.04 ROS Noetic这个组合这是目前VINS-Fusion和PX4生态兼容性最好的搭配。Ubuntu 22.04 ROS Humble也能跑但有些包的依赖需要手动处理新手容易卡住。系统装好之后先装ROS。这里可以用鱼香ROS的一键安装脚本省去配源的麻烦wget http://fishros.com/install -O fishros . fishros脚本会引导你选择ROS版本选Noetic就行。装完之后验证一下roscore能正常启动就说明ROS没问题。接下来装VINS-Fusion的依赖。主要是Ceres Solver和OpenCV。Ceres建议装2.0以上版本OpenCV用ROS自带的就行。Ceres的安装步骤如下sudo apt-get install libgoogle-glog-dev libgflags-dev sudo apt-get install libatlas-base-dev sudo apt-get install libsuitesparse-dev git clone https://ceres-solver.googlesource.com/ceres-solver cd ceres-solver mkdir build cd build cmake .. make -j4 sudo make install然后编译VINS-Fusioncd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd .. catkin_make source devel/setup.bash编译过程中如果报Eigen或者OpenCV的错大概率是版本不匹配检查一下/usr/include/eigen3是否存在以及OpenCV的版本是否和ROS自带的一致。2.3 相机与IMU标定这一步决定成败标定是整套系统里最枯燥但最重要的环节。标定不准后面所有优化都是在错误的基础上打转。我见过太多人标定随便跑一下就去飞结果飞机飘得怀疑人生。相机标定用ROS的camera_calibration包就行。打印一张棋盘格我习惯用8x6的格子方格边长用尺子量准比如25mm。标定时把棋盘格在相机视野里各个角度晃一遍注意要覆盖画面的四个角和中心倾斜角度也要有变化。采集大概30到50张有效图像然后点CALIBRATE等进度条走完。标定结果里重点看两个数重投影误差和畸变系数。重投影误差最好在0.2像素以内超过0.5就说明标定质量不行重新标。畸变系数如果特别大说明镜头畸变严重VINS-Fusion虽然会做去畸变但畸变太大还是会影响特征跟踪。双目的话还要做立体标定用stereo_calibration同时采集左右目的图像。标定完会得到一个外参矩阵包含基线长度和两个相机之间的旋转平移关系。基线长度要和你实际测量的对一下差太多说明标定有问题。IMU标定分两步内参标定和相机-IMU外参标定。内参标定主要是噪声密度和随机游走可以用imu_utils这个工具让IMU静止放置两小时以上采集数据然后跑标定。噪声密度一般在0.01到0.05之间随机游走在0.0001到0.001之间具体看IMU型号。相机-IMU外参标定用Kalibr这是目前最成熟的工具。需要先录一个包含相机和IMU数据的rosbag录制时把相机和IMU一起拿着做充分的激励运动——三个轴的旋转和平移都要有速度不要太慢也不要太快。录完之后跑Kalibr它会输出相机到IMU的旋转和平移矩阵。实操心得Kalibr标定对数据质量极其敏感。我踩过的坑是运动太温柔结果标定出来的外参和实际差了好几度。后来改成手持设备做明显的八字形运动每个轴都转到位标定结果就稳了。另外标定时环境纹理要丰富光照要均匀否则特征跟踪会丢。标定结果要填到VINS-Fusion的配置文件里主要是cam0_calib.yaml、cam1_calib.yaml和imu_calib.yaml这几个文件。外参矩阵的格式是4x4的变换矩阵旋转部分用四元数或者旋转矩阵都行VINS-Fusion两种格式都支持。3. VINS-Fusion配置与PX4对接全流程3.1 配置文件逐项拆解VINS-Fusion的配置文件在config目录下不同配置对应不同的yaml文件。我以双目IMU为例把关键参数一项一项说清楚。topic设置是第一步必须和你的实际话题名对上imu_topic: /mavros/imu/data_raw image0_topic: /camera/infra1/image_rect_raw image1_topic: /camera/infra2/image_rect_raw output_path: /home/yourname/output如果用D435话题名通常是/camera/infra1/image_rect_raw这种。如果用USB双目话题名可能是/left/image_raw和/right/image_raw。用rostopic list确认一下。相机内参从标定文件里抄过来model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: -0.01 k2: 0.01 p1: 0.0 p2: 0.0 projection_parameters: fx: 300.0 fy: 300.0 cx: 320.0 cy: 240.0fx、fy、cx、cy就是标定得到的焦距和主点畸变系数填k1、k2、p1、p2。IMU参数包括噪声密度和随机游走acc_n: 0.02 gyr_n: 0.002 acc_w: 0.0002 gyr_w: 0.00002这些值从IMU标定结果里来。如果没做标定可以先用经验值但飞起来可能会飘。外参是相机到IMU的变换body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [1, 0, 0, 0.05, 0, 1, 0, 0.0, 0, 0, 1, 0.0, 0, 0, 0, 1]这个矩阵表示IMU坐标系到相机坐标系的变换。平移部分就是相机相对IMU的位置旋转部分用旋转矩阵表示。标定得到的值直接填进去。其他关键参数estimate_extrinsic如果外参标定得准设为0如果不确定设为1让VINS自己优化但会增加计算量。estimate_td时间偏移估计如果时间同步做得好设为0否则设为1。max_solver_time优化求解时间上限机载电脑上建议0.05到0.08秒。max_num_iterations迭代次数一般8到10次。keyframe_parallax关键帧选择阈值双目建议10到20像素。3.2 启动VINS-Fusion并验证输出配置好之后启动流程分三步。第一步启动相机和IMU的驱动节点。如果用D435roslaunch realsense2_camera rs_camera.launch \ enable_infra1:true \ enable_infra2:true \ enable_imu:true \ unite_imu_method:linear_interpolation第二步启动VINS-Fusionrosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml第三步启动可视化roslaunch vins vins_rviz.launch启动后rviz里应该能看到特征点跟踪、相机位姿、轨迹这些信息。如果特征点很少或者位姿乱跳说明标定或者配置有问题。验证输出是否正常看几个话题rostopic hz /vins_estimator/odometry rostopic echo /vins_estimator/odometryodometry的频率应该在10到30Hz之间位姿数据要平滑不能有大的跳变。如果频率很低或者数据抖动厉害检查一下CPU占用和标定质量。注意VINS-Fusion初始化需要一定的激励运动。如果飞机静止不动它可能一直初始化不了。启动后先拿着飞机做几个明显的平移和旋转等rviz里的轨迹稳定了再起飞。3.3 对接PX4让飞控信任视觉位姿VINS-Fusion输出的位姿要发给PX4走MAVROS是最方便的路子。PX4这边需要配置几个参数让它接受外部视觉定位。首先设置EKF2的辅助源param set EKF2_AID_MASK 24这个值表示启用视觉位置和视觉姿态融合。具体位bit3是视觉位置bit4是视觉姿态24就是两者都开。然后设置视觉位姿的话题param set EKF2_HGT_MODE 33表示用视觉作为高度源。如果气压计够准也可以用气压计做高度视觉只做水平位置。MAVROS这边需要把VINS的odometry转发成PX4能识别的vision_pose话题。可以写一个简单的转发节点或者用mavros的vision_pose插件。我习惯自己写一个Python节点做坐标变换和转发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 # 坐标变换VINS的坐标系到PX4的NED坐标系 pose.pose.position.x msg.pose.pose.position.z pose.pose.position.y msg.pose.pose.position.x pose.pose.position.z -msg.pose.pose.position.y 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()坐标变换是重点。VINS用的是右手系前左上PX4的EKF2期望的是NED前右下。所以x、y、z要重新映射符号也要注意。这个变换搞错了飞机就会往反方向飞。配置好之后在QGC里检查EKF2的状态。如果视觉位姿被正确接收EKF2的vision_position标志会亮起来。这时候可以切到Position模式慢慢推杆看飞机是否稳定悬停。3.4 实飞调试与参数微调第一次实飞一定要在开阔、无风、有纹理的环境里做。我建议先在GPS模式下飞起来然后切到Position模式观察飞机是否稳定。如果飞机开始漂移或者抖动立刻切回GPS或者手动模式。调试时重点关注几个参数EKF2_EV_DELAY视觉位姿的延迟补偿。如果飞机响应滞后把这个值调大单位是毫秒。EKF2_EV_POS_X/Y/Z视觉传感器相对机体重心的位置。如果相机不在重心上要填这个偏移。EKF2_EV_NOISE_MD视觉噪声模型默认是0如果视觉跳变厉害可以设为1。VINS这边如果轨迹有漂移可以调keyframe_parallax和max_solver_time。漂移大就减小关键帧阈值让更多帧参与优化计算跟不上就增大求解时间上限。4. 常见问题排查与实战避坑指南4.1 初始化失败与位姿漂移初始化失败是最常见的问题表现是rviz里一直显示initializing或者轨迹乱跳。原因通常有三个激励不够、标定不准、时间不同步。激励不够最好解决拿着飞机做几个明显的平移和旋转每个轴都动一动幅度大一点。如果还是不行检查IMU数据是否正常rostopic echo /mavros/imu/data_raw看角速度和加速度是否有合理的变化。如果IMU数据一直是零或者噪声特别大说明驱动或者硬件有问题。标定不准的话重新标。重点检查相机-IMU外参这个对外参特别敏感。我遇到过一次初始化死活不成功后来发现是外参的旋转矩阵填反了改过来立刻就好了。时间不同步的表现是初始化能成功但飞起来飘。检查相机和IMU的时间戳rostopic echo /camera/infra1/image_rect_raw/header/stamp rostopic echo /mavros/imu/data_raw/header/stamp两个时间戳的差值应该稳定在一个小范围内。如果差值忽大忽小说明时间同步有问题。软件层面可以用message_filters做时间对齐硬件层面最好用触发信号。4.2 特征跟踪丢失与光照问题VINS-Fusion依赖特征点跟踪纹理少、光照变化大的环境很容易丢。表现是rviz里特征点数量骤降位姿开始飘。改善方法有几个。第一调整相机曝光让图像亮度适中不要过曝也不要太暗。D435可以在驱动里设auto_exposure和exposure参数。第二增加特征点数量在配置文件里调max_cnt默认是150可以加到200甚至300但会增加计算量。第三如果环境纹理实在太少可以考虑加一个下视相机或者用激光雷达辅助。实操心得我在一个白色墙壁的走廊里飞过VINS直接跟丢了。后来在墙上贴了几张带纹理的海报问题就解决了。所以如果你的飞行环境纹理少提前布置一些视觉标记是最省事的办法。4.3 计算资源不足与实时性保障机载电脑算力不够时VINS-Fusion的处理频率会下降导致位姿输出延迟飞控融合后控制效果变差。表现是飞机响应迟钝或者rviz里odometry频率低于10Hz。优化方法降低图像分辨率640x480够用就别用1280x720减少特征点数量关闭不必要的可视化把VINS-Fusion的优化线程绑定到独立核心。Jetson平台可以用jetson_clocks锁定最高频率避免降频。如果实在跑不动可以考虑用单目IMU配置算力需求会低不少。或者换更轻量的SLAM方案但精度和鲁棒性会打折扣。4.4 常见问题速查表现象可能原因排查方法解决方案初始化一直不成功激励不够看IMU数据是否有变化做明显的平移旋转运动初始化成功但飞起来飘标定不准检查重投影误差和外参重新标定相机和IMU位姿跳变时间不同步对比相机和IMU时间戳做时间对齐或硬件同步特征点少纹理少或光照差看rviz特征点数量调整曝光或布置纹理odometry频率低算力不足看CPU占用率降分辨率或减特征点飞机往反方向飞坐标变换错误检查转发节点的变换修正坐标系映射EKF2不融合视觉参数没设对看QGC的EKF2状态检查AID_MASK和HGT_MODE4.5 几个容易被忽略的细节第一个是相机的安装位置。相机尽量靠近机体重心减少旋转时的视差。如果相机装得太靠前飞机旋转时视觉位姿会有额外的平移分量EKF2融合时会引入误差。第二个是IMU的减震。无人机振动很大IMU如果直接硬连接在机架上振动会污染加速度数据导致VINS的预积分出错。用减震棉或者减震板把IMU隔离一下效果立竿见影。第三个是rosbag录制。调试阶段一定要录bag出问题了可以回放分析。录的时候把相机、IMU、VINS输出、MAVROS的vision_pose都录上方便对比。rosbag record /camera/infra1/image_rect_raw \ /camera/infra2/image_rect_raw \ /mavros/imu/data_raw \ /vins_estimator/odometry \ /mavros/vision_pose/pose第四个是固件版本匹配。PX4的版本和MAVROS的版本要对应不然消息格式可能不兼容。我用的组合是PX4 1.14 MAVROS 1.14一直很稳。这套系统我从零搭到能稳定飞前后花了大概三周其中标定占了一半时间。现在回头看最难的不是写代码或者配参数而是把标定做扎实、把时间同步做好。这两件事做到位后面的调试会顺很多。如果你刚开始搞建议先把单目IMU跑通熟悉整个流程之后再上双目循序渐进比一上来就啃硬骨头要高效得多。