第八篇连载聊LVI-SAM复现。题目里的那个“6轴IMU”背后有一段故事从数据集跑通到实机部署前后折腾了整整一周最后发现失败原因根本不是性能不够而是消息格式、协方差、坐标系和外参四处细节。这篇文章我在Ubuntu20.04 ROS Noetic环境下完整走了一遍从源码编译到官方数据集复现再到替换成自己机器上的6轴IMU每一步都记录在这里。想从零开始跑LVI-SAM、或者和我一样要在实机上适配非9轴IMU的朋友可以直接按这篇的顺序来。1. 先搞清楚LVI-SAM的组成四个节点、两条融合链路1.1 LVI-SAM在SLAM框架里处于什么位置LVI-SAM全称是Lidar-Visual-Inertial Simultaneous Localization and Mapping本质上是Tixiao Shan团队在LIO-SAM基础上扩展出来的紧耦合方案。LIO-SAM只做激光雷达和IMU融合视觉部分完全是盲区遇到走廊、玻璃墙、细长杆这类特征稀疏场景容易飘。LVI-SAM的思路是在LIO-SAM的因子图里再塞进一路视觉里程计用单目相机提供的特征约束去弥补激光雷达退化场景下的定位空洞。从架构上说LVI-SAM可以理解为LIO-SAM和VINS-Mono的合体激光雷达负责几何约束相机负责纹理约束IMU预积分把两者串起来。四个节点分工明确imageProjection做点云预处理和去畸变visual_odometry做视觉特征跟踪和视觉惯性初始化imuPreintegration做IMU预积分mapOptmization做全局位姿图优化和回环检测。1.2 视觉惯性子系统和激光惯性子系统如何配合整个系统内部有两套相互依赖的小系统。视觉惯性系统VIS负责从图像和IMU数据中恢复视觉六自由度位姿激光惯性系统LIS负责用雷达点云做帧到局部地图的配准。VIS计算出的位姿会作为雷达SLAM的初值LIS优化后的位姿又会把特征深度信息反哺给视觉系统。这个“你喂我初值我喂你深度”的循环关系是LVI-SAM能同时保持视觉和激光优势的关键。运行起来后用rqt_graph可以看到四个节点之间的topic已经连成一张网。imageProjection从雷达topic接收点云从IMU topic接收惯性数据输出去除畸变的点云特征visual_odometry订阅图像、IMU和一个经过校正的IMU话题输出视觉里程计因子imuPreintegration同样订阅IMU为位姿图提供预积分约束mapOptmization收集所有因子做优化发布全局地图和位姿路径。四条链路之间没有断点任何一个topic断流整个系统都会在几秒内出现漂移。1.3 6轴IMU可行性的来源磁力计其实没参与计算很多人在实机部署前都会卡在一个问题上LVI-SAM官方数据集里用的是9轴IMU自己手头只有6轴IMU能不能跑我的结论是能而且从代码层面看6轴和9轴的差别几乎可以忽略。LVI-SAM的IMU预积分因子只使用加速度计和陀螺仪数据也就是6轴IMU的全部输出。磁力计不存在于图片的因子图约束中也没有任何节点订阅磁场话题。系统的初始化阶段是利用连续多帧加速度均值求重力方向再结合视觉运动恢复结构估算尺度整个过程完全不依赖磁力计。也就是说9轴IMU在LVI-SAM里多出来的三个轴大概率是摆设。既然原理上可行那为什么实机替换后还是有人跑飞问题一般出在话题格式、数据单位、协方差和坐标方向上这些我放在第四章细讲。2. 环境准备与依赖编译从干净Ubuntu20.04到catkin_make通过2.1 安装ROS Noetic的取舍LVI-SAM是基于ROS Noetic维护的建议不要在Ubuntu22.04上强行用ROS2跑改动量太大。Ubuntu20.04安装ROS Noetic有两种路径手动添加源安装或者用社区流传的一键安装脚本。这个系列前几篇已经讲过手动安装这里说结论如果你只是想把LVI-SAM跑通而不是研究ROS本身直接用一键安装脚本最省事。手动安装的坑集中在rosdep和源的选择上。rosdep init和rosdep update经常因为网络原因失败需要耐心重试。一键安装脚本会把这些步骤全部封装好选择“ros1”和“noetic”即可。装完之后记得执行echo source /opt/ros/noetic/setup.bash ~/.bashrc再新开一个终端验证一下roscore能不能正常启动。2.2 三个依赖库的版本匹配问题LVI-SAM是老代码对依赖库版本要求比较“挑剔”不是越新越好。我整理了一张版本对照表按这个组合编译可以省掉大量折腾时间依赖库推荐版本原因gtsam4.0.24.1之后部分接口变更老代码会报参数类型错误Ceres Solver1.14.02.x版本FindCeres兼容性和Option字段名有变化OpenCV系统自带4.2官方代码包含老式CV_宏需要手动适配Eigen系统自带3.3无需额外处理gtsam的编译比较耗时建议放在最前面操作。源码clone到本地后需要checkout到4.0.2标签再编译安装。Ceres也建议从源码编译依赖的glog、gflags、suitesparse直接用apt安装。OpenCV之所以推荐系统自带是因为我们自己编译OpenCV很容易踩到CUDA和contrib模块的坑而LVI-SAM只用到了基础的图像读取和特征提取功能4.2版本完全够用。2.3 编译LVI-SAM并处理兼容性报错依赖库装好之后把LVI-SAM源码放到~/catkin_ws/src目录下然后编译cd ~/catkin_ws catkin_make -j4第一次编译过程中大概率会碰上下面这些报错。error: CV_LOAD_IMAGE_UNCHANGED was not declared in this scope是OpenCV版本宏变化导致的把源码里出现的CV_LOAD_IMAGE_UNCHANGED批量替换成cv::IMREAD_UNCHANGEDCV_GRAY2BGR替换成cv::COLOR_GRAY2BGR。gtsam相关的报错如果出现在编译阶段多半是版本太高回到4.0.2基本能解决。还有一个小细节catkin_make建议加-j4而不是默认的满核编译。LVI-SAM编译时内存占用比较高尤其gtsam链接阶段用-j8以上很容易把内存吃满导致编译进程被系统杀掉。编译完成后执行. ~/catkin_ws/devel/setup.bash再roscd lvi_sam能跳转到包目录说明编译链路通了。3. 官方数据集复现先把链路跑通再说实机3.1 数据集的topic结构与参数核对官方仓库的Release里有一个handheld.bag这是作者拿着手持设备采集的数据包含三路核心topic/imu_correct是经过校正的IMU数据/lili/livox是Livox雷达点云/camera/image_color是单目图像。下载下来后用rosbag info查看topic列表和频率再对照源码里的订阅名称确保每个topic名都一致。LVI-SAM的代码里有一个容易忽略的问题几个核心节点的topic名不完全是参数化配置的有些是硬编码在源码里的。比如visual_odometry.cpp里直接写了订阅/camera/image_colorimageProjection.cpp里订阅了雷达topicimuPreintegration.cpp里订阅了/imu_correct。拉下来源码后先在src目录里搜一遍这些字符串实机部署时如果不一致可以用remap在launch里映射不需要改代码。3.2 运行和观察的关键指标启动官方数据集的最简命令如下roslaunch lvi_sam run.launch rosbag play handheld.bag在另一个终端打开rviz加载lvi_sam包里的config/lvi_sam.rviz能看到当前扫描、全局地图、位姿路径和视觉特征四个Display。官方数据集的跑通标准有三个rviz里地图连续无重影路径在回环处闭合终端出现“loop found!”字样。如果看到视觉特征大量漂浮或地图出现双层先暂停bag检查参数文件中的话题名是不是被改乱了。3.3 数据集跑通后替换成自己设备前要做的事数据集能跑通只代表代码链路没问题换到自己的机器人还要做三件事。第一确认相机内参和畸变模型。官方数据集用的相机内参在params.yaml里如果你的相机型号不同必须替换自己的内参否则视觉初始化会失败。第二确认IMU噪声参数不是照抄的官方数据集里的IMU是特定型号噪声密度和随机游走参数跟你的6轴IMU完全不同。第三确认雷达点云的字段结构满足代码要求机械雷达需要ring字段非重复扫描雷达要先将点云转换成带相对时间或按规则分配行号的PointCloud2。4. 适配6轴IMU消息格式、协方差、坐标系三层坑一次说清4.1 为什么官方9轴IMU实机6轴也能跑回到标题里的核心问题。官方的IMU消息是/imu_correct名字叫“correct”很多人就误以为它对磁力计有依赖。实际上这个topic只是一个经过了坐标旋转和单位换算的IMU数据流核心内容是加速度和角速度。LVI-SAM的IMU预积分公式里只有角速度和加速度两个输入视觉惯性初始化恢复重力方向使用的也是静止状态下加速度计的矢量均值与磁力计无关。6轴IMU适配的正确思路不是给系统“补”一个磁力计而是把驱动输出的数据打磨成LVI-SAM期望的标准形态。我把实机踩坑过程中遇到的所有问题归成三层消息格式层、协方差层、坐标系层。下面逐一拆解。4.2 第一层消息格式和单位LVI-SAM接收的IMU消息必须是标准的sensor_msgs/Imu。不少6轴IMU模块的ROS驱动会发布自定义消息比如只发原始寄存器值或者用std_msgs/Float64MultiArray打包数据轴分量这些都要在转发节点里整理成标准Imu。单位方面有两个高频错误点加速度计输出的是g而不是m/s²角速度输出的是deg/s而不是rad/s。ROS标准中linear_acceleration单位是m/s²angular_velocity单位是rad/s。如果你的IMU模块串口打印的是0.98, -0.02, 9.81这类数值基本可以断定是g值发布前要乘以9.81。角速度如果是度每秒要乘以π/180。这个错误很隐蔽因为g值看起来像是重力加速度的合理量级不乘以9.81的话系统初始化时会把重力方向估计成9.81/9.81≈1g的伪重力初始化必然失败。4.3 第二层协方差和orientation字段的坑这是6轴IMU适配过程中最容易踩的暗坑。很多IMU驱动的实现里由于没有磁力计融合姿态所以orientation字段全为零orientation_covariance也是全零。LVI-SAM的视觉惯性初始化借鉴了VINS-Mono的逻辑在处理IMU测量时会检查orientation_covariance[0]是否大于0全零协方差会让系统认为没有有效的姿态信息从而跳过初始姿态设置。解决办法是发布时给一个单位四元数作为初值并给协方差填一个很小的非零值比如对角线全是1e-6。注意这不是让你伪造真实姿态只是满足代码检查逻辑。系统的IMU预积分并不依赖orientation字段它用的是角速度和加速度所以这个单位四元数不会影响后续计算精度但能让初始化流程走通。4.4 第三层坐标系转换坐标系问题最容易导致“代码跑通了但机器人的轨迹在乱转”。ROS遵循REP-103标准IMU坐标系约定为x轴向前、y轴向左、z轴向上也就是常说的ENU系。很多6轴IMU模块出厂坐标系是x向右、y向前、z向上或者x向前、y向右、z向下这些都需要在驱动侧完成旋转。一个通用的做法是写一个转发节点读取原始IMU测量乘以旋转矩阵变换到ENU系再发布给LVI-SAM。下面是Python版本的关键代码假设原始坐标系是x右、y前、z上#!/usr/bin/env python3 import rospy import numpy as np from sensor_msgs.msg import Imu R np.array([ [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0] ]) G 9.81 DEG2RAD np.pi / 180.0 def imu_callback(msg): out Imu() out.header msg.header out.header.frame_id imu_link out.orientation.x 0.0 out.orientation.y 0.0 out.orientation.z 0.0 out.orientation.w 1.0 out.orientation_covariance [1e-6, 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 1e-6] acc np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) * G acc_enu R.dot(acc) out.linear_acceleration.x acc_enu[0] out.linear_acceleration.y acc_enu[1] out.linear_acceleration.z acc_enu[2] out.linear_acceleration_covariance msg.linear_acceleration_covariance gyr np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]) * DEG2RAD gyr_enu R.dot(gyr) out.angular_velocity.x gyr_enu[0] out.angular_velocity.y gyr_enu[1] out.angular_velocity.z gyr_enu[2] out.angular_velocity_covariance msg.angular_velocity_covariance pub.publish(out) rospy.init_node(imu_relay_node) pub rospy.Publisher(/imu/data_enu, Imu, queue_size100) rospy.Subscriber(/imu/data_raw, Imu, imu_callback) rospy.spin()如果你的模块输出单位已经是m/s²和rad/s就去掉对应乘法。如果已经是ENU系旋转矩阵改为单位阵。发布后静止放置IMU用rostopic echo检查加速度z轴应该稳定在9.81附近x和y接近0角速度三轴都接近0。4.5 在params.yaml里需要改哪些参数LVI-SAM的params.yaml里有几个关键参数必须和实机对应起来。imu_topic改成转发节点发布的话题imu_rate填IMU实际发布频率camera_rate填相机实际帧率scan_period填雷达扫描周期。举个例子如果IMU是200Hz、相机30Hz、雷达10Hz这组数要如实填写错一个都会影响系统时间轴。噪声参数也不能照抄官方配置。imuAccNoise、imuGyrNoise、imuAccBiasN、imuGyrBiasN这组参数官方数据集里的数值针对的是特定IMU型号换到自己的6轴IMU后最好参考器件手册上的噪声密度和随机游走指标填写。如果买的是比较便宜的模块参数会在合理范围内偏差较大可以在跑通后通过静止实验微调。5. 实机部署时间同步、外参标定与启动顺序5.1 时间同步为什么实机和数据集最大的区别在这里官方bag里的所有topic时间戳来自同一个采集过程天然对齐。实机部署时三个传感器各自的时间戳可能来自不同的驱动节点如果驱动里用的是接收时刻那IMU、相机、雷达之间的时间误差可以从几毫秒到几十毫秒。这个误差对LVI-SAM来说是致命的尤其是IMU和相机之间的延迟会让视觉惯性初始化里的预积分和视觉匹配对不上号。我的建议是先检查驱动能否打硬件时间戳。很多工业相机驱动支持roscamera的TimeReference机制激光雷达的时间戳来自设备内置时钟或者PTP同步。如果硬件同步做不了至少要保证三个驱动节点在同一台机器上运行且系统时间通过NTP校准。IMU驱动务必用ros::Time::now()打时间戳不要自己手动读chrono再转否则会出现时间基不一致的问题。5.2 外参标定与手动微调方法相机和IMU之间的外参推荐用Kalibr标定棋盘格充分激励的IMU运动标出来的旋转和平移精度比较可靠。Kalibr在Ubuntu20.04上安装不算轻松需要一堆Python依赖建议用Docker镜像跑可以少走很多弯路。激光雷达和IMU之间的外参我用过一个非常实用但不够严谨的手动微调方法适合没有高精度标定条件的场合。操作步骤如下把棋盘格贴在平整墙面上启动相机和雷达在rviz里同时加载图像和点云。用Publish Point工具选取点云中棋盘格的角点记录三维坐标。再根据当前params.yaml里的extrinsicTranslation和extrinsicRotation把角点投影到像素坐标对比是否落在图像中对应位置。不断微调外参直到投影误差缩小到几个像素以内。这个方法精度有限但能快速把系统从“完全跑不起来”带到“能稳定跟一段距离”的状态之后再用正式工具精标。5.3 多传感器启动顺序与launch配置实机启动不要一股脑全都打开。我习惯按“驱动层→检查层→SLAM层”的顺序来。先启动雷达驱动用rostopic hz确认点云频率再启动相机驱动用rqt_image_view确认图像画面最后启动IMU转发节点用rostopic echo确认坐标系和单位正确。三层都确认没问题了再启动roslaunch lvi_sam run.launch。如果LVI-SAM代码里写死的topic名和你的驱动话题不一致不用改代码用launch里的remap统一映射。比如你的IMU话题是/imu/data_enu就在启动lvi_sam的launch里加这几行remap from/imu_correct to/imu/data_enu/ remap from/camera/image_color to/camera/image_raw/ remap from/lili/livox to/velodyne_points/还要确认TF树完整。LVI-SAM需要知道map、base_link、imu_link之间的关系如果IMU驱动没有发布imu_link到base_link的静态变换需要在launch里加一个static_transform_publisher否则rviz里的点云和图像会错位。6. 运行中的调参与常见问题处理6.1 系统工作正常的判断方法LVI-SAM实机跑起来后不要只看rviz里有没有地图就以为正常。我总结了三个快速检查点。第一用rostopic hz /lvi_sam/odometry确认里程计话题输出频率稳定一般视觉里程计能达到10Hz以上就算健康。第二看rviz里的位姿路径正常情况是一条平滑的轨迹如果出现连续小周期跳变多半是时间同步或外参有问题。第三把机器人推着走一段再回到起点观察地图闭合程度闭合误差在几十厘米内说明系统整体状态可以接受。6.2 常见异常与修复视觉初始化失败是实机部署最高频的异常。触发原因通常是启动时机器人不够晃运动激励不足导致视觉SfM无法恢复出稳定结构。解决办法是启动后将机器人拿起来做几个缓慢的推拉和旋转动作让图像和IMU都有充分的运动。如果还是初始化失败检查IMU频率是否达到100Hz以上图像是否模糊特征点是否太少。地图出现重影或双层大概率是雷达到IMU的外参不对或者时间同步误差太大。先用rqt_bag检查三个传感器的消息时间戳确认没有几十毫秒以上的错位。外参方面可以先把旋转矩阵的对角元素设成近似的单位阵调整平移量再逐步微调旋转不要一上来就调一个看似合理的复杂旋转矩阵。6.3 IMU零偏简单标定LVI-SAM对陀螺仪零偏比较敏感。零偏太大会导致预积分结果持续偏转系统即使初始化成功几十秒后也会慢慢飘掉。如果暂时没有条件做完整Allan方差标定可以先做一个静态均值补偿把机器人水平静止放置录制3到5分钟的IMO话题数据分别计算角速度和加速度的均值。角速度均值就是陀螺仪的零偏加速度均值理论上应该是(0, 0, 9.81)如果z轴偏差超过0.05基本都是加速度单位没换算对。把测出来的陀螺仪零偏在驱动转发节点里减掉再重新跑一次你会发现LVI-SAM的稳定性上升一个台阶。这里多说一句很多6轴IMU模块的静止零偏会随温度漂移如果机器人工作环境温度变化大建议在驱动里集成简易温度补偿或者至少开机后让IMU预热一两分钟再启动系统。实机部署LVI-SAM最耗时间的地方其实不在算法本身而在数据质量。数据集为什么一跑就对实机为什么各种飘差别就在IMU消息是否标准、外参标定是否准确、时间戳是否对齐这三件事上。这篇的6轴IMU适配路径我后来在另一台机器人上复用过把单位、协方差、坐标系三层检查做扎实整个系统从启动到稳定输出地图的速度会快很多希望对你也有用。