★ FEATURED ARTICLE 先说一个我的观察很多做视觉SLAM的朋友实验室里明明有D435i真机但真要调算法的时候要么相机被师兄占着要么在室内场景跑两圈就特征丢失要么IMU和图像的时间戳对不上一个简单的问题能折腾一下午。这种时候一套能完全复现真实相机内外参、可以随时重置、甚至能灌入噪声的仿真环境就是帮你把算法调顺的利器。这篇文章我就手把手演示怎么在Gazebo里搭一个带着Realsense D435i模型的机器人平台把VINS-MONO跑起来实现完整的VIO SLAM仿真。我默认你有一定的ROS基础知道launch文件大概长什么样也对VINS-MONO的代码结构有基本概念。如果你还没编译过VINS-MONO那正好我们从环境准备、模型配置到算法跑通一步不落走一遍。整个过程在Ubuntu 20.04 ROS Noetic Gazebo 11下验证过其他版本要稍作调整后面会提到。1. 整体方案设计与核心思路拆解为什么选择Gazebo加VINS-MONO这套组合而不是直接用公开的数据集答案很简单数据集的真值和轨迹是固定的你想测测算法对噪声的鲁棒性或者想验证一个新的传感器安装位置对VIO精度的影响数据集根本改不了。仿真环境最大的价值就是可控性——我可以随意改相机外参、换环境纹理、加大IMU噪声甚至模拟某个传感器完全失效这在真机上几乎没法做。1.1 系统组成与数据流我们先理清整个系统的数据流。Gazebo作为物理引擎和渲染引擎负责维护一个虚拟世界里面有一个带相机和IMU的机器人模型。当机器人移动时Gazebo根据物理引擎计算出IMU的角速度和加速度根据渲染引擎生成图像然后通过ROS话题发出来。VINS-MONO这边接收三个关键输入左目或单目图像、IMU数据、以及相机内参。图像进前端进行特征提取和光流跟踪IMU数据进预积分模块两者在后端紧耦合优化最终输出相机在世界坐标系下的位姿也就是我们说的VIO轨迹。这套架构在仿真里的好处是所有传感器数据都带有精确的时间戳而且Gazebo本身就是一个同步环境天然避免了真机上“图像30帧、IMU 200Hz时间戳对不齐”的典型问题。但要注意/camera/color/image_raw和/imu/data必须挂在同一台机器上的同一个/clock下否则VINS会报大量“IMU and Image gap”警告。1.2 为什么不是ROS 2或新版本VINS-Fusion我特意用ROS 1 Noetic加VINS-MONO不是为了厚古薄今而是这套组合最省心。VINS-MONO的原始实现基于ROS 1很多依赖比如cv_bridge在ROS 2下需要重新适配对新手不太友好。你想用ros2_gazebo_slam那套得花不少时间在桥接和数据格式转换上调试门槛明显高。VINS-Fusion虽然也是港科大团队的作品支持双目和GPS融合但单体式的VINS-MONO在代码可读性上更清晰非常适合学习和原理验证。等你在仿真里完全跑通了再迁移到VINS-Fusion或者自己的改进算法逻辑都是一样的。Gazebo版本方面我用的是Gazebo 11。它可以跟ROS Noetic无缝集成软件源直接安装不需要编译这很重要我在Ubuntu 22.04上试过自己编译Gazebo Harmonic花了整整一个周末。而且Gazebo 11的SDF格式和URDF插件兼容性已经非常成熟网上大量现成模型都可以直接用。1.3 最小化方案只保留D435i IMU市面上也有一些现成的仿真机器人模型比如Pioneer、Jackal都可以直接在Gazebo里加载。但我的建议是自己搭一个最小化模型一个底盘加一个D435i传感器模型就够了。为什么因为VIO的核心在于视觉惯性紧耦合激光雷达、轮式里程计这些传感器在这个场景下都是多余的。模型越复杂URDF里要维护的关节和惯性参数就越多出问题的概率越大。我自己第一次就是在别人的多传感器平台上改半天最后发现bowl模型里一个角度不对整个机器人翻车。所以下面的方案里我只用一个差速驱动底盘可以手动发速度指令控制它运动底盘上固定一个D435i模型D435i内部包含一个彩色相机发布/camera/color/image_raw和/camera/color/camera_info一个深度相机用于后续扩展VINS本身不需要深度图一个IMU传感器发布/imu/data这个结构非常清晰后续你想升级也很容易无非就是往URDF里再塞几个传感器。2. 环境准备从零开始搭建VINS-MONO编译环境很多人跑VINS-MONO失败8成倒在了编译阶段剩下的2成死在运行时话题对不上。这一节我把环境搭建和编译踩坑一次说清楚。2.1 系统与ROS安装Ubuntu 20.04 ROS Noetic是我最推荐组合。安装ROS时只装桌面完整版ros-noetic-desktop-full因为它自带Gazebo、RVIZ、cv_bridge、tf等一堆依赖省下很多工夫。这里有个容易忽略的细节ros-noetic-desktop-full依赖的OpenCV是4.2而VINS-MONO原版代码里有些接口是OpenCV 3的写法编译时会报错。解决方案有两种一是用我后面给的补丁代码二是安装ros-noetic-cv-bridge时让它依赖OpenCV 4然后改VINS源码里的头文件。我建议走第一种少改点东西。安装好之后记得初始化sudo apt install ros-noetic-desktop-full source /opt/ros/noetic/setup.bash echo source /opt/ros/noetic/setup.bash ~/.bashrc2.2 Ceres Solver一个必须手动编译的依赖VINS-MONO的后端优化依赖CeresUbuntu 20.04的apt源里最新版是1.14VINS的CMake可以编译过但运行时偶尔会有LAPACK相关的报错。我的经验是直接用1.13.0或者1.14.0搭配Eigen 3.3.7最稳。Ceres依赖的库比较多我一条命令装齐sudo apt install libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev然后下载Ceres 1.14.0编译安装git clone https://github.com/ceres-solver/ceres-solver.git cd ceres-solver git checkout 1.14.0 mkdir build cd build cmake .. make -j8 sudo make install注意不要用最新版Ceres 2.x它要求Eigen 3.4会跟ROS Noetic自带的Eigen版本冲突到时候报一堆模板错误排查起来非常痛苦。2.3 编译VINS-MONO时最常见的两个坑把VINS-MONO clone下来之后放到你的catkin工作空间里mkdir -p ~/vins_ws/src cd ~/vins_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git cd .. catkin_make这一步大概率会卡在两个地方。第一个是OpenCV版本接口冲突。在vins_estimator和camera_model两个包里搜索CV_LOAD_IMAGE_GRAYSCALE这类宏定义改成cv::IMREAD_GRAYSCALE。还有CV_FM_LMEDS改成cv::FM_LMEDSCV_RANSAC改成cv::RANSAC。一共大概十几处编译报错看哪一行就改哪一行都是机械操作。第二个坑是cv_bridge的链接冲突。如果你之前装过其他版本的OpenCVCMake可能会错误选中系统级的cv_bridge而不是ROS自带的。解决方案是在vins_estimator的CMakeLists里把find_package(cv_bridge REQUIRED)放到find_package(OpenCV REQUIRED)之前并显式指定set(OpenCV_INCLUDE_DIRS /usr/include/opencv4)其实还有一个坑是Eigen对齐问题报错长这样/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:144: ...。这个坑的根源是Eigen 3.4里的aligned_allocator变化了VINS-MONO的老代码没跟上。解决办法就是老老实实用apt源里的Eigen 3.3.7不要手贱去升级。编译通过之后务必验证一下能不能跑自带的euroc数据集。如果连数据集都跑不通那肯定不是你后面配置的问题而是编译出的二进制本身有问题。这一步别跳过我就是图省事跳过验证结果调试Gazebo模型调了半天最后发现是编译时少了一个宏定义。3. Gazebo仿真场景搭建与Realsense D435i模型配置接下来就是核心环节了。我们分两块先搭一个最简单的室内仿真环境再把D435i模型塞进机器人的URDF里。3.1 仿真世界与机器人模型选择为了不让特征点不足的问题干扰调试我建议在起步阶段用一个会议室场景。Gazebo官网有一个gazebo_ros_demo但太简陋我直接用Building Editor画了一个8m×6m的房间里面放了几把椅子模型和桌子模型。更简单的方式是直接加载pioneer3dx的世界文件它自带丰富的纹理还带两个摄像头虽然我们不用它的摄像头但环境纹理对视觉特征提取很重要。如果你的显卡驱动没问题建议开启GPU加速。默认的Gazebo用OGRE软件渲染CPU占用高而且帧率低。在~/.gazebo/gui.ini里加上[geometry] x1024 y768 [render] enable_gputrue然后启动时用gzserver --verbose看看输出如果GPU加速生效fps会明显改善。我实测在同一台电脑上GPU加速后图像话题的发布频率从18Hz提到了满帧30Hz这对VINS的影响非常大。3.2 D435i的URDF模型与Gazebo插件配置D435i在Gazebo里的难点不是外观网上有很多现成stl而是传感器的配置。我这里直接给出核心的gazebo插件配置块已经经过我验证可以直接用。首先定义传感器链接link namecamera_link visual geometry mesh filenamepackage://realsense_description/meshes/d435i.dae scale0.001 0.001 0.001/ /geometry /visual inertial mass value0.05/ origin xyz0 0 0/ inertia ixx1e-5 iyy1e-5 izz1e-5 ixy0 ixz0 iyz0/ /inertial /link然后给这个link挂上camera插件。这里有个非常重要的细节VINS-MONO通常需要单目图像但D435i的真机模型里有三个摄像头——左目、右目、RGB。在仿真的VIO场景里我们只需要RGB这一个就够了。不需要为了追求真实把一个相机插件配成双目这样只会增加多余的话题和计算量。最关键的是相机参数要和真机一致。我用的配置如下gazebo referencecamera_link sensor typecamera namecamera_color update_rate30/update_rate camera horizontal_fov1.3962634/horizontal_fov image width640/width height480/height formatR8G8B8/format /image clip near0.1/near far10/far /clip noise typegaussian/type mean0.0/mean stddev0.007/stddev /noise /camera plugin namecamera_driver filenamelibgazebo_ros_camera.so ros namespace/camera/color/namespace remappingimage_raw:image_raw/remapping /ros camera_namecolor/camera_name frame_namecamera_link/frame_name image_topic_nameimage_raw/image_topic_name camera_info_topic_namecamera_info/camera_info_topic_name hack_baseline0.07/hack_baseline /plugin /sensor /gazebo这里的关键参数说明一下horizontal_fov1.3962634 弧度正好是80度和D435i RGB相机的HFOV一致。width640 height480VINS-MONO的默认配置文件里分辨率就是这个匹配能少改很多参数。stddev0.007这是图像高斯噪声的标准差实验下来这个值既模拟了真实传感器的噪声又不会让特征点提取崩溃。然后是IMU。D435i内置BMI055在Gazebo里用libgazebo_ros_imu.so插件就行gazebo referencecamera_link sensor typeimu nameimu_sensor update_rate200/update_rate always_ontrue/always_on imu angular_velocity xnoise typegaussian mean0.0 stddev0.003//x ynoise typegaussian mean0.0 stddev0.003//y znoise typegaussian mean0.0 stddev0.003//z /angular_velocity linear_acceleration xnoise typegaussian mean0.0 stddev0.05//x ynoise typegaussian mean0.0 stddev0.05//y znoise typegaussian mean0.0 stddev0.05//z /linear_acceleration /imu plugin nameimu_driver filenamelibgazebo_ros_imu.so ros namespace/imu/namespace /ros frame_namecamera_link/frame_name topic_namedata/topic_name /plugin /sensor /gazebo注意IMU的频率是200HzVINS-MONO配置文件里imu_topic的频率参数如果写的不是200后面会疯狂刷WARNING: IMU frequency is too low。我们后面配置VINS时会对应改。3.3 idle机器人如何驱动起来Gazebo里的机器人必须有控制器才能运动。差速驱动底盘最简单的方式就是用libgazebo_ros_diff_drive.so插件监听cmd_vel话题。这样我们可以在终端里发指令让它走起来。不过在真正跑VIO之前我强烈建议你用键盘遥控或者手动发速度指令来一段平滑的轨迹不要直接原地旋转。VINS初始化时必须有足够的视差如果机器人只原地自转特征点都在同一个深度平面上初始化会失败或者轨迹严重漂移。后面实操环节我会给一段简单的cmd_vel发布脚本让机器人自动走一个长方形路径这样每次跑出来的轨迹相对固定方便对照真值。4. VINS-MONO配置与Launch文件全解到了这一步Gazebo能出图像和IMU数据了VINS也能编译了接下来就是把两边接上。这里最核心的就是config文件和launch文件很多人在这一步反复折腾。4.1 config_yaml文件逐项解释VINS-MONO会从yaml文件里读取相机和IMU的配置。我从官方euroc示例改了一份针对我们仿真的D435i做了调整核心修改如下imu: 1 num_of_cameras: 1 image_width: 640 image_height: 480 camera_model: pinhole distortion_model: radial-tangential distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 k3: 0.0 projection_parameters: fx: 385.3 fy: 385.3 cx: 319.5 cy: 239.5这里fx和fy来自你Gazebo相机的FOV和分辨率换算。公式很简单fx width / (2 * tan(HFOV/2))我们宽度640水平视场角1.3962634算出来就是385.3。cy和cx取图像中心。需要留意的是Gazebo的相机数值和真实D435i不完全一样真机的fx通常是617左右VGA分辨率下这里是因为仿真焦距不同不要照搬真机参数。我之前在仿真里直接填真机内参结果VINS初始化后轨迹扭曲得很厉害就是因为这个。IMU外参部分也要改。假设IMU和相机中心完全重合那就是单位矩阵body_T_cam0: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]如果你的URDF里IMU是挂在相机link底下的但两者的坐标系之间有偏移这个矩阵就要改。推导方法是先看URDF的joint origin把平移和旋转转成齐次变换矩阵然后取逆就是body_T_cam0。还有两个关键参数max_solver_time: 0.04 max_num_iterations: 8如果跑起来CPU占用高、实时性跟不上就把solver_time调大一点到0.1、迭代次数减到6精度会略降但不容易掉帧。4.2 Launch文件怎么写VINS-MONO的launch文件负责两件事加载config、启动三个节点feature_tracker、vins_estimator、pose_graph。官方自带一个euroc.launch但它的input topic是/cam0/image_raw和/imu0我们的Gazebo话题名字不一样要改。我起的launch文件核心部分如下launch node namefeature_tracker pkgfeature_tracker typefeature_tracker outputscreen param nameconfig_file typestring value$(find vins_estimator)/../config/vi_sim/d435i_sim.yaml / param namevins_version typeint value0 / /node node namevins_estimator pkgvins_estimator typevins_estimator outputscreen param nameconfig_file typestring value$(find vins_estimator)/../config/vi_sim/d435i_sim.yaml / /node node namerviz pkgrviz typerviz args-d $(find vins_estimator)/../config/vi_sim/vins_rviz_config.rviz / /launch但等一下VINS-MONO源码里默认订阅的是/cam0/image_raw所以要么改launch里的remap要么改VINS源码里的订阅话题名。我一般用remapremap from/cam0/image_raw to/camera/color/image_raw/ remap from/imu0 to/imu/data/这里还有一个极易踩的坑相机话题的camera_info必须同时发布否则feature_tracker会等/cam0/camera_info等到超时。VINS源码在getImage函数里有个逻辑如果收到的图像不配对的camera_info会一直缓存图像直到camera_info到来——如果你发现VINS启动后图像话题迟迟不处理优先检查camera_info是否有数据。4.3 启动顺序和真值对比启动顺序有个讲究。我的经验是先启动Gazebo等图像话题稳定输出大概3秒再启动VINS。如果同时启动VINS会收到空图像或时间戳跳跃的图像容易初始化失败。另外为了验证VINS轨迹正确性最好加一个真值对比。Gazebo自带gazebo_ros_p3d插件输出机器人位置到/ground_truth/odom。这样在RVIZ里可以很好的对比plugin nameground_truth_odom filenamelibgazebo_ros_p3d.so ros namespace/ground_truth/namespace remappingodom:odom/remapping /ros body_namebase_link/body_name frame_nameworld/frame_name update_rate100/update_rate /plugin然后可以用plotjuggler或者evo工具对齐真值和VINS轨迹算一下ATE和RMSE。evo的安装是pip install evo用法很简单evo_ape tum ground_truth.txt vins_trajectory.txt -a这篇文章的重点是仿真配置轨迹精度分析我会在后续实操内容里再展开。先跑通比什么都重要。5. 从启动到稳定跑通VIO的完整实操记录理论讲完了下面进入实战。我按自己操作顺序记录整个流程包括每步应该看到什么输出以及出现问题怎么排查。5.1 第一步启动Gazebo仿真环境打开第一个终端source /opt/ros/noetic/setup.bash source ~/vins_ws/devel/setup.bash roslaunch vins_sim_gazebo simulation.launch启动成功后你会在终端看到gzserver输出同时会弹出Gazebo的GUI窗口。这时先确认话题输出正常rostopic hz /camera/color/image_raw rostopic hz /imu/data正常情况下图像话题频率在30Hz左右Gazebo渲染性能差时可能降到20Hz但只要稳定就会影响不大IMU话题在200Hz。第一次跑的时候我遇到过图像话题只有几Hz的情况查了半天发现是publish_tf插件造成了线程阻塞。后来把launch里不必要的TF发布器全部删掉只保留base_link到camera_link的TF问题就解决了。5.2 第二步驱动机器人运动机器人静止时VINS无法初始化。我写了一个简单的Python脚本让机器人按矩形路径行走import rospy from geometry_msgs.msg import Twist rospy.init_node(cmd_publisher) pub rospy.Publisher(/cmd_vel, Twist, queue_size1) rate rospy.Rate(10) twist Twist() motions [ (0.3, 0, 5), # 前进 5秒 (0, 0.5, 2), # 原地左转 (0.3, 0, 5), (0, 0.5, 2), ] while not rospy.is_shutdown(): for linear, angular, duration in motions: twist.linear.x linear twist.angular.z angular end rospy.Time.now() rospy.Duration(duration) while rospy.Time.now() end: pub.publish(twist) rate.sleep() twist.linear.x 0 twist.angular.z 0 pub.publish(twist) rospy.sleep(3)启动这个脚本后你会看到Gazebo里机器人在移动。此时强烈建议打开RVIZ订阅/camera/color/image_raw确认画面里有没有足够的特征点。如果画面里是大片白墙或者地面VINS初始化就会很费劲。5.3 第三步启动VINS-MONO新开一个终端roslaunch vins_estimator d435i_sim.launch如果一切正常你会看到以下特征feature_tracker终端会不断输出feature num: xxx说明前端在提取特征vins_estimator终端开始输出Initialization finished!然后出现VINS init succeeds字样RVIZ中出现相机轨迹和特征点云这里有一个经验初始化通常需要2到5秒取决于纹理丰富程度和运动是否充分。如果一直卡在Waiting for the initial gravity vector说明IMU数据有问题。此时用rostopic echo /imu/data看第一个数据的时间戳和值是否正常再检查URDF里IMU的坐标系方向是不是倒了。5.4 第四步保存轨迹并评估精度跑完一段稳定的轨迹后在RVIZ里你只能得到一个可视化结果想评估精度还得导出轨迹数据。VINS-MONO并没有直接保存TUM格式轨迹的接口但可以在运行过程中用rostopic echo /vins_estimator/odometry把位姿记录下来或者修改源码在pose_callback里转成TUM格式写入文件。我比较推荐直接在pose_graph_node.cpp里找一个回调用C的ofstream把T_w_i保存下来。这样轨迹数据是准实时导出的不用事后再处理ROS bag。到这里Gazebo加VINS-MONO的VIO SLAM仿真就算彻底跑通了。6. 常见问题速查与避坑技巧最后把我在整个过程中踩过的坑和排查经验整理成一张表格希望帮你省下一些摸索时间。症状可能原因解决方案VINS编译时报OpenCV接口错误Noetic自带OpenCV4代码是旧API把CV_LOAD_IMAGE_GRAYSCALE改IMREAD_GRAYSCALECV_RANSAC改cv::RANSAC编译卡在Eigen对齐错误Eigen版本太高3.4卸载新Eigen装apt源的3.3.7图像话题发布频率极低机器显卡配置低或使用了软件渲染gazebo gui.ini开启GPU加速或直接跑headless模式IMU话题没数据IMU插件没加载或命名空间冲突用gzmodel命令行检测传感器检查URDF中gazebo reference是否对应link名称VINS一直初始化失败相机运动过于平缓或图像特征少加大运动速度避免原地旋转提高图像分辨率或降低噪声VINS输出轨迹剧烈漂移相机内参或外参填错用公式重算fx/fy确认body_T_cam0不是转置RVIZ里看不到图像话题名不匹配用rostopic list检查用remap对齐图像和IMU时间戳不匹配多台机器分布式运行保证Gazebo和VINS在同一台机器同一个/clock下除了表格里的还有两个非常值得说的经验。第一个是关于噪声参数的。刚开始我把IMU噪声全部设成0也就是理想IMU跑了半天发现VINS的累计漂移极小反而有点不真实后面想测试算法鲁棒性时又得重新设噪声。建议从一开始就设上合适的噪声真机D435i的IMU噪声标准差在0.003 rad/s和0.05 m/s²这个量级和我在上面给的值差不太多。第二个经验是关于场景设计的。VINS在真机上最怕的是白墙和重复纹理仿真里也一样。我第一次搭建的场景是一间全白房间地面上有均匀的地毯纹理结果特征点全集中在地板上初始化倒是成功了但轨迹稍微一远就飞了。后来在场景里添加几把带颜色的椅子、桌子和墙壁贴图整个系统的稳定性立刻上了一个台阶。这一条对任何做视觉SLAM仿真的人都适用场景的纹理丰富度直接决定你能不能愉快地调试。最后再补充一个Gazebo版本的坑。如果你不幸在Ubuntu 22.04上装了ROS 2和Gazebo HarmonicVINS-MONO的迁移会很痛苦因为我上面用的libgazebo_ros_camera.so这类插件在Harmonic里完全不兼容需要改成像gazebo_ros2_camera这样的新插件API差别很大。我的建议是如果你想专注VIO算法的学习就老老实实用Ubuntu 20.04 Noetic Gazebo 11这套黄金组合。等VINS跑熟了再尝试ROS 2也不迟。在实际操作中我的体会是仿真环境最大的意义不是代替真机而是帮你把传感器的数据质量、参数配置和算法逻辑这三件事解耦开来。在Gazebo里你可以确保IMU和图像时间戳严格对齐相机内参精确已知环境可控——当VINS在这种“理想条件”下还是表现不好那问题多半在算法参数或者运动激励上。反过来说如果仿真都跑不顺上真机只会更难调。希望这篇文章能帮你在VIO仿真这条路上少踩几个坑把更多时间花在真正有趣的算法改进上。
阅读完成 · 觉得有帮助？
