VLP-16这套雷达我已经用了大半年说句实话外参标定这件事在过去很长时间里都是我最不愿意碰的环节。雷达和IMU之间的外参没标好后面建图、定位全都会出问题点云重影、地图翘边、跑一圈回来轨迹漂移查来查去最后发现都是外参的锅。最近把lidar_imu_calib这套开源的“激光雷达-IMU标定工具”完整跑通了一遍用的正是VLP-16加一个普通九轴IMU收获很大。这篇文章就把整个流程、原理和踩过的坑完整记录下来从环境搭建到采集数据再到分析结果给正要入坑或者已经卡住的朋友一份能直接照着做的保姆级教程。1. 标定前先搞清楚的几件事外参为什么要较真lidar_imu_calib到底适合谁1.1 从一次让我抓狂的建图失败说起先说个真实经历。有次我做室内走廊的SLAM建图雷达是VLP-16IMU是某国产工业级模块当时图省事外参直接用尺子量了个大概填进系统。结果跑出来的地图整个走廊的地面是歪的转角处点云重影严重回环检测倒是能检测到但位姿图优化之后地图依然有肉眼可见的错位。后来我把激光雷达的topic和IMU的topic录下来用EVO工具对比轨迹发现Z方向漂移特别快差不多每10秒就有十几厘米的累计误差。当时第一反应是IMU本身零偏太大但换了一个IMU之后问题依旧这才意识到是外参的初始值差太远导致前端里程计里IMU预积分和点云配准融合时互相打架系统每帧都在用错误的外参做坐标变换误差自然越攒越多。1.2 lidar_imu_calib的定位不是唯一选择但足够快、足够省心市面上做雷达和IMU外参标定的工具其实不少比如lidar_align、lidar_to_imu_calib还有后来一些基于LIO框架的在线标定方案。我选lidar_imu_calib主要看中三点它把外参旋转、平移和时间延时放在一个位姿图里联合优化不需要标定板也不用人工选点操作门槛低。工具原本在Livox系列雷达上验证过但对标准机械式雷达也兼容只需要处理一下点云数据格式。有比较完善的RVIZ可视化标定过程能直观看到点云有没有对齐很方便排查问题。当然它也有自己的脾气极其依赖输入点云的质量和IMU的频率采集数据时必须按规范来后面会详细说。适合的人群是已经在跑LIO或LOAM类SLAM、需要把雷达和IMU外参精确标定出来的工程师或研究者。如果只想拿外参用在纯视觉方案里那这套流程就有点绕远了。2. 环境准备与编译Ubuntu 18.04 ROS Melodic 下的完整依赖清单2.1 依赖安装顺序与版本建议我在Ubuntu 18.04、ROS Melodic上编译通过后来也在Ubuntu 20.04配合ROS Noetic上跑通过主要差异在PCL版本和OpenCV版本代码本身兼容性不错。安装依赖时建议按下面的顺序来# 基础工具链 sudo apt update sudo apt install -y cmake g git # ROS 基础包 sudo apt install -y ros-melodic-pcl-ros ros-melodic-rviz ros-melodic-tf2 ros-melodic-tf2-ros ros-melodic-velodyne-msgs # Eigen3 和 CERES sudo apt install -y libeigen3-dev libceres-dev # PCLROS自带版本即可 sudo apt install -y libpcl-devCERES是重头戏lidar_imu_calib的全局优化部分完全依赖CERES求解。这里强调一点装CERES的时候尽量不要用源码去编译太浪费时间直接用系统仓库里的版本就行。如果你的项目对CERES版本有特殊要求必须要源码编译那么记得先装好所有依赖否则编译CERES时会报一堆关于gflags和glog的错误。2.2 编译代码时最容易翻车的两个位置代码克隆和编译本身并不复杂cd ~/catkin_ws/src git clone https://github.com/APRIL-ZJU/lidar_IMU_calib.git cd .. catkin_make source devel/setup.bash但几乎所有人都会遇到下面两个问题第一个是缺少livox_ros_driver的自定义消息头文件。lidar_imu_calib的源码里直接include了livox_ros_driver/CustomMsg.h不管你的数据源是Livox还是Velodyne这个头文件都得有。最简单的做法是把livox_ros_driver2编译到同一个工作空间里或者只编译它的消息定义部分。我当时的做法是cd ~/catkin_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd .. catkin_make如果不想装整套驱动也可以只把livox_ros_driver2建为其他项目依赖的消息包但直接用整套驱动最省心。第二个是CERES版本导致Solver::Options里部分成员访问报错。新版本CERES2.1以上对某些接口有收紧报错现场一般是找不到ceres::Solver::Options::num_threads或者ceres::RobilsonLoss这类类型。我建议检查一下当前CERES版本如果版本过新并且报这类错误要么降级到老版本CERES要么给代码打补丁把相关调用改成新版接口。对这个工具来说Ubuntu 18.04系统仓库自带的CERES版本是1.14表现最稳定。编译阶段还有一个容易被忽略的坑工作空间里如果同时编译了其他依赖Eigen3源码包里版本冲突的功能包会出现“Eigen3 on the system is not compatible with the one included with the package”这种提示。解决办法是把工作空间里无关的功能包先移出去保持最小化编译。3. 摸清lidar_imu_calib的计算逻辑时间戳对齐、IMU预积分和全局位姿图的配合3.1 工具的输入输出全景先理清这套工具在干什么忽略原理直接跑参数会一头雾水。lidar_imu_calib的输入数据只有两类雷达点云流和IMU数据流。它会对输入的bag文件进行离线处理不会实时输出标定结果最终输出的是激光雷达坐标系到IMU坐标系的旋转矩阵和平移向量。IMU与雷达之间的时间延时。用于验证的拼接点云地图和轨迹文件。整个算法可以拆成两大部分前端里程计和后端优化。前端里程计主要做的是把连续的激光雷达点云帧配准在一起同时用IMU的角速度和加速度做预积分为帧间匹配提供一个初始位姿猜测。后端则是把所有帧间约束和IMU约束放到一个全局位姿图里用CERES统一优化在优化过程中把外参当作需要估计的状态量之一不断迭代修正。3.2 前端里程计里为什么IMU预积分这么关键激光雷达帧间配准本质上是一个非线性优化问题每一帧点云都要去上一帧或局部地图里找对应关系。如果没有一个靠谱的初始值迭代求解很容易掉进局部最小值。VLP-16这类低线束雷达点云密度稀疏帧与帧之间的重合区域少对初始值的要求就更高。IMU预积分的作用正好是提供一个高频率、短时可靠的位姿变化估计。它不需要知道全局位置只需要积分出两帧雷达之间相对旋转和相对平移的变化量。IMU的短时积分精度很高角速度积分得到的旋转经过重力和零偏补偿之后已经足够给点云配准提供一个很好的初始猜测。这就是为什么数据采集时要求IMU和雷达必须同时运动还要保持足够的激励如果一直静止或者匀速直线运动IMU预积分无法准确估计姿态变化整个前端里程计的精度会显著下降。前端里程计还有一个容易被忽略但很重要的细节点云被送入配准之前通常要做一次体素滤波下采样这样可以降低VLP-16约3万点每帧的规模提高计算效率。在下采样之前工具还会根据局部点云粗糙度提取角点和平面点吗实际上对lidar_imu_calib来说关键不是点云特征点筛选而是点云几何结构是否足够丰富。如果环境里全是玻璃墙面或空旷场地点云配准的约束不足前端里程计也会跟着漂。3.3 后端位姿图优化外参和延时是怎么被“逼”出来的后端优化的目标函数大致可以理解为通过调整装在外参、时间延时以及每一帧的位姿让所有雷达点云在全局坐标系下投影到同一物理点时的残差最小。也就是说工具会重复“用当前外参把点云从雷达系投影到IMU系再从IMU系投影到世界系”这个过程然后统计投影后的重合程度重合程度越高残差越小外参就越准。时间延时在这里也很有意思。VLP-16和IMU的时间戳往往不是同一时刻VLP-16的点云帧头时间戳通常对应的是扫描开始时刻而IMU的每个采样值时间戳是精确的采集时刻两个传感器之间会有一个固定的时间偏移。lidar_imu_calib会把时间偏移作为优化变量放到后端里估计出一个补偿值。实际标定后我发现延时估计值大多在几毫秒到几十毫秒之间数值取决于采集时的坐标变换和驱动配置。如果没有这个联合优化光靠手动对齐时间戳非常痛苦。理解了这套逻辑之后下面所有操作其实都是为了让这两部分能稳定工作。4. VLP-16接入点云格式改造是绕不开的关卡4.1 为什么原始代码默认面向Livox点云lidar_imu_calib源码大量使用了livox_ros_driver中自定义的CustomMsg消息类型。这种消息相比sensor_msgs/PointCloud2多保存了每个点相对于帧起始时刻的偏移量以及激光线号、距离精度等信息非常适合后端做时间延时估计。而VLP-16默认发布的是sensor_msgs/PointCloud2里面只保留了XYZ、强度还有driver算好的每个点相对扫描起始时刻的time字段。这就导致源代码里的订阅回调函数根本接不到VLP-16的点云。解决思路有两个。第一个是改源码把CustomMsg全部换成PointCloud2牵涉到消息回调函数、偏移量字段赋值、线号解析工作量不小而且容易在后续更新代码时出冲突。第二个是写一个小的格式转换节点把/velodyne_points转成CustomMsg然后重新发到工具订阅的/laser_cloud话题上这样原算法代码几乎不用动。我强烈推荐第二种理由很简单出问题时排查范围小而且转换节点可以复用到其他类似的低线束雷达上。4.2 一个能直接复用的转换节点编写思路转换节点本质上要解决的问题是把PointCloud2里每个点的相对时间填充到CustomMsg的offsets数组里并把雷达线号填到lines字段。VLP-16点云的每个点在PointCloud2里的时间域由Velodyne驱动计算好单位是秒表示该点相对整个点云扫描开始时刻的时间偏移。我用Python写过一版转换节点这里给出核心思路和关键代码具体字段名以你安装的livox_ros_driver2消息定义为准#!/usr/bin/env python3 import rospy import struct import numpy as np from sensor_msgs.msg import PointCloud2 from livox_ros_driver2.msg import CustomMsg, CustomPoint class PointCloud2ToLivox: def __init__(self): self.sub rospy.Subscriber(/velodyne_points, PointCloud2, self.pointcloud_callback, queue_size10) self.pub rospy.Publisher(/laser_cloud, CustomMsg, queue_size10) self.custom_msg CustomMsg() rospy.loginfo(PointCloud2ToLivox started) def pointcloud_callback(self, msg): custom CustomMsg() custom.header msg.header custom.points [] # 这里需要根据你的PointCloud2字段布局解析出x,y,z,intensity和time for idx in range(msg.width): # 简化示意实际需要用msg.fields定位每个字段的偏移 x, y, z, intensity, time self.read_point(msg, idx) cp CustomPoint() cp.x x cp.y y cp.z z cp.reflectivity intensity cp.offset_time int(time * 1e9) # 秒转纳秒 custom.points.append(cp) custom.timebase time custom.stamp msg.header.stamp custom.lines 16 self.pub.publish(custom)注意这个转换节点里读取PointCloud2字段时不能直接用偏移量硬编码因为不同版本的Velodyne驱动PointCloud2的字段顺序可能不同。稳妥的方式是启动节点时动态打印msg.fields确认time字段是否存在然后根据字段的offset读取。4.3 话题重映射与配置文件的匹配当转换节点把点云重新发布到/laser_cloud之后接下来要做的就是修改lidar_imu_calib的配置文件使命中话题和IMU话题对应上。在lidar_imu_calib的配置目录下找到calib.yaml我通常重点修改这几个字段laser_topic: /laser_cloud # 转换后的点云话题 imu_topic: /imu/data # IMU话题 bag_file: /home/xxx/calib_data.bag # bag路径如果IMU话题名字不是/imu/data可以直接在yaml里改。工具读取bag之后会按yaml里配置的话题名订阅不会自动去找。这里最容易踩的坑是bag里点云和IMU的时间戳系统不一致。我在采集时VLP-16的驱动通过PTP同步了主机的网络时间IMU则直接使用主机本地时间这样才能保证两个传感器的时间基准一致否则后端估计时间延时永远有歧义。具体采集注意事项下一节展开。5. 数据采集实战bag录得好标定差不了5.1 采集环境要求几何特征、空间尺度、激励手段数据采集是整个标定流程里最花心思的部分。lidar_imu_calib本质上是一个优化问题输入的数据要能提供足够约束否则外参不可观求解器会给出一个看似合理但实际错误的解。我总结了几条硬性要求环境必须有丰富的几何特征但不能是绝对对称的。室内房间比较好有墙、柱子、桌子这些结构尽量避免在空旷场地或者一个完全长直走廊里采集。运动过程中要让雷达和IMU各个轴向都有充分的角速度激励。我的操作习惯是先把设备放置稳定静止5秒左右然后手持或者放在小车平台上先做慢速旋转绕X轴转、绕Y轴转、绕Z轴转每个轴至少转到接近180度再做一些大幅度的俯仰和横滚动作。动作幅度要足够大但速度不能过快。VLP-16一帧点云的扫描时间大约是0.1秒如果动作过快点云会严重畸变帧间配准难度陡增。理想角速度大约在每秒30到60度之间既能让IMU预积分显著起作用也不至于让点云畸变到不可用。采集时间建议控制在40到90秒之间。太短则约束不足太长则动作难免重复或过渡到退化场景。我一般采集60秒左右其中最后再留5秒静止。5.2 录制bag的完整指令录制bag时我使用rosbag record同时录制点云和IMU原始话题rosbag record /velodyne_points /imu/data -O calib_data.bag如果你的IMU驱动同时发布多个话题比如/imu/data_raw是原始加速度和角速度/imu/data是经过处理的姿态消息那么这里建议录原始数据/imu/data_raw。lidar_imu_calib需要的是加速度计和陀螺仪的线性加速度与角速度不需要磁力计和姿态四元数。录制时还可以用rosbag info确认bag内话题和时间范围rosbag info calib_data.bag重点检查三件事两个话题是否有持续数据输出bag时间长度是否足够点云消息数量是否和IMU消息数量符合预期。VLP-16以10Hz运行时60秒的bag大约有600帧点云IMU如果200Hz则有12000条消息左右。如果数量级差很多先排查驱动问题再继续标定。6. 标定运行与参数调整从启动到拿到外参6.1 启动流程和观察入口确认配置文件和bag都准备好之后启动工具就非常简单了roslaunch lidar_imu_calib lidar_imu_calib.launch工具的launch文件会启动核心节点同时打开RVIZ。RVIZ中会实时显示前端里程计输出的局部地图、后端优化后的全局地图以及IMU轨迹。标定过程中可以直观看到“点云在当前外参下是否对齐到了同一物理点”。如果外参初始值和真实值差得非常远RVIZ里会看到一个明显的左右分层或前后错开的情况这时候不用慌等后端优化多跑几轮通常会慢慢收敛。6.2 关键参数调整的一个可行参考具体参数名会因为仓库版本不同略有差异但基本都会涉及这几个方向。我提供一个我在VLP-16上实测比较稳的配置思路参数类别作用我的建议下采样体素大小控制降采样后的点云密度VLP-16用0.2m到0.3m太大损失细节太小计算量激增迭代次数控制后端优化轮数至少50轮不收敛再逐步增加初始外参的旋转和平移作为优化的起点尽量用尺量和初步估算值不要全部填0时间偏移初值时间延时估计的起点一般填0工具会自动估计与IMU噪声分布相关的权重影响预积分项和配准残差的相对权重按IMU手册填写没有手册就先用默认值我需要强调一点初始外参不要用全零或者完全离谱的值。后端优化虽然在理论上能处理较大初值误差但实际操作中如果初始旋转差了30度以上迭代很容易陷入局部最优收敛到的外参看起来合理实际却错得离谱。我的习惯是先用直尺量出大概平移量再用激光雷达和IMU各自坐标系的方向关系估一个大致旋转填进去作为初值后面交给优化去精修。6.3 标定结果怎么读运行结束后工具会在配置指定的路径下输出标定结果文件里面包含四元数或旋转矩阵形式的外参。我这里给一个我当时的输出示例方便大家参考Estimated extrinsics (lidar-imu): Rotation matrix: 0.999736 -0.004527 0.022443 -0.004375 0.999987 -0.001911 -0.022512 0.001810 0.999745 Translation: 0.003752 -0.005214 0.031258 Time offset: 0.018302这个结果的含义后面会细说。拿到数值之后不要高兴得太早必须做验证。7. 结果验证不能只看一个数值要用RVIZ和实际SLAM说话7.1 点云重叠验证我最常做的第一个验证方法是把标定外参填入一个在线LIO或LOAM系统然后跑一段之前录过的数据观察把多帧点云投影到世界系后是否有重影。在RVIZ里有一个更轻量的方法。工具本身会输出一个拼接地图这个地图是把每一帧点云按照优化后的位姿和外参投影到同一个坐标系后的叠加结果。如果外参正确墙壁、柱子和地面这些平面特征会清晰锐利边缘没有拖影。如果外参略微偏差地面会变厚墙体会出现双层远处点云会和近处点云错开。我当时用一张走廊拐角的地图来判断走廊墙体和地面交界线在标定后是一条干净的单线这就是比较理想的状态。7.2 时间延时的合理性时间延时的结果同样值得验证。对于VLP-16一个点云帧从第一线扫描到最后一线的总时间大约在0.1秒左右所以延时估计值如果超出0.1秒就要小心是不是两个传感器的时间基准没有对齐。如果估计出来的时间延迟大于0.2秒通常不是真延时而是时间戳系统不同步。验证时间延时是否合理的另一个办法是观察标志物在不同帧中的位置在RVIZ中如果一帧点云内部已经发生前后错位说明工具在用延时补偿点云内部畸变这可能只是一种折中方案不一定是物理现实的准确反映。最稳妥的做法是在采集前就通过硬件或驱动配置把两个传感器的时间基准同步好让延时尽量在几毫秒到十几毫秒的合理范围内。7.3 标定失败的模式与常见原因我遇到过的失败模式主要有三种后端优化迭代不收敛残差一直不平。这种情况最常见原因是采集数据时运动激励不足或者环境太退化。标定结果外参数值每次跑都不一样。这说明观测约束不够数据里存在多解性需要重新采集增加不同轴的旋转激励。全局拼接地图看起来差不多了但局部细节有轻微模糊。这可能是点云下采样体素过大或者IMU噪声权重给得太高可以稍微调小点体素尺寸再试。遇到失败不要反复用同一组数据调整参数那是浪费时间。最好重新采集一次数据针对性地增加激励和场景特征再用默认参数跑往往一次就能收敛。8. 踩坑记录我摔过的跟头和一些能救命的细节8.1 运动速度过快导致点云畸变我第一次采集的时候手持设备挥动得相当随性觉得动作越大越能激励IMU结果跑出来的前端里程计疯狂漂移。原因很简单VLP-16是机械旋转式雷达一帧点云不是瞬间同时获得的而是激光头旋转一整圈的过程中逐步扫描获得。如果这个过程中雷达本身在大幅转动点云每一帧内部就已经发生了畸变。lidar_imu_calib在帧间配准时对这种畸变有一定容忍度但容忍度有限。经过几轮对比动作控制在比较“缓慢但幅度大”的状态是最好的。我的具体做法是旋转角速度大概控制在每秒30度左右绕每个轴旋转时保持动作连续均匀不要忽快忽慢。匀速缓慢运动能同时保证点云畸变小和IMU激励信号质量高。8.2 IMU消息的协方差和频率问题IMU驱动发布的消息里协方差矩阵经常被很多人忽略。lidar_imu_calib在后端优化时会读取IMU消息里的协方差来加权数据如果协方差全部填0代码可能会在求逆时报错或者默认使用一个不合理的权重导致优化结果偏差。我踩过的一个坑是某个IMU驱动组件把加速度计和陀螺仪的协方差统一设成了单位矩阵而实际上陀螺仪噪声远低于加速度计噪声这导致预积分约束过于信任加速度积分出来的位置变化最终外参平移项估计偏差达到了好几厘米。解决方法是根据IMU手册修改配置或者至少保证消息中协方差不为全零且数量级合理。IMU频率也很关键。VLP-16的帧率是10HzIMU至少应该达到100Hz以上。如果IMU只有50Hz两帧雷达之间只有5个IMU采样点预积分的精度会明显下降。我当时把IMU频率从100Hz调到了200Hz标定残差有了肉眼可见的下降。8.3 时间戳系统的Sync坑软件同步最容易埋雷这个问题我在前面已经提过但值得单独说明。VLP-16的velodyne驱动默认支持两种时间戳模式一种是使用当前系统时间另一种是使用雷达内置GPS模块的时间。如果IMU使用的是系统本地时间而雷达使用的是GPS时间两者之间可能相差十几秒甚至更多这会让后端的时间延时估计彻底混乱。我的解决方式是在录制bag之前通过PTP或者外部授时把主机时间同步好并在Velodyne驱动里设置让点云时间戳直接取自主机系统时钟保证和IMU同一时间基准。这样在标定过程中时间延时估计的初值从0开始很快就能收敛到正确值。如果不做同步即便标定算法再强大也会在时间维度上产生一个很大的补偿量严重影响整体优化精度。8.4 在RVIZ里看到的点云颜色看着很乱怎么办RVIZ中可能出现点云颜色不一致、看起来花里胡哨的现象。颜色通常表示的是点云类别、强度或者所属线束。此时关注的重点应该是对齐趋势而不是颜色。如果点云在优化过程中颜色逐渐“聚拢”说明残差在下降。如果颜色越分越开大概率是参数配置不对比如初值偏差大导致优化发散。另外如果RVIZ刷新很卡可以把点云显示的最大点数调低一些只保留关键特征。VLP-16一帧点云在下采样之后通常只有几千个点RVIZ渲染压力并不大卡顿多半是电脑性能或显示插件问题。关于这套工具和VLP-16的组合我最终得到的经验是标定这件事真的不能靠运气前期把数据采集规范做到位后面基本一路顺畅。工具本身虽然对Livox系列更友好但只要把点云转换节点搞定VLP-16同样能标得很准。如果你也卡在最开始的格式改造或者采集数据阶段希望这份教程能帮你少走几趟弯路直接跑出一个能用的外参。