激光雷达与IMU联合标定实战：LIO-SAM外参标定流程与避坑指南

1. 为什么激光雷达和IMU的联合标定是SLAM落地的第一道坎搞过激光雷达SLAM的人都有一个共识建图飘不飘一半看标定。很多人拿到一套LIO-SAM的配置跑公开数据集效果惊艳换到自己设备上就各种重影、漂移、Z轴跳变折腾几天也找不到原因。问题往往不在算法本身而是激光雷达和IMU之间的外参没有标定好或者标定参数写错了。LIO-SAM的全称是Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping它把激光雷达的scan-to-map匹配和IMU的预积分紧耦合在一起通过因子图优化来估计位姿。这个架构对传感器外参非常敏感——激光雷达和IMU之间的旋转矩阵哪怕差0.5度在几十米的建图范围内就会产生肉眼可见的错层。所以联合标定不是可选项是必选项。这篇文章面向的是已经跑通LIO-SAM基本流程、但被标定问题卡住的开发者也适合刚接触多传感器融合、想搞清楚外参到底怎么来的同学。我会从标定的基本原理讲起然后手把手走一遍完整流程最后把我踩过的坑和排查方法整理出来。你不需要有很深的数学背景但需要基本的ROS操作能力和对刚体变换的理解。2. 联合标定到底在标什么核心概念拆解2.1 外参和内参的区别别搞混了很多人一上来就说我要标定IMU其实要分清楚标的是什么。IMU内参包括加速度计的零偏、刻度因数、非正交误差以及陀螺仪的零偏和刻度因数这些是IMU自身器件特性决定的。激光雷达内参包括每个激光束的垂直角度、水平角度偏移、测距偏差等。而联合标定标的是外参也就是激光雷达坐标系到IMU坐标系的刚体变换矩阵包含三个旋转量和三个平移量。LIO-SAM的配置文件params.yaml里extrinsicRot和extrinsicRPY这两个参数就是外参的旋转部分extrinsicTrans是平移部分。extrinsicRot负责把IMU坐标系下的角速度和加速度旋转到激光雷达坐标系extrinsicRPY负责处理激光雷达和IMU之间因为安装方向不同带来的差异。这两个参数搞错IMU预积分的积分方向就是错的优化出来的轨迹必然发散。2.2 为什么LIO-SAM对标定精度这么敏感LIO-SAM的因子图里有四类因子IMU预积分因子、激光雷达里程计因子、GPS因子和回环因子。IMU预积分因子约束的是相邻关键帧之间的运动它把高频的IMU数据积分成相对位姿。如果外参有偏差积分出来的相对位姿和激光雷达匹配出来的相对位姿就会打架优化器只能在两个错误约束之间找折中结果就是轨迹扭曲。举个直观的例子假设IMU和激光雷达之间实际有5度的安装夹角但你在配置里写的是0度。当设备转弯时IMU感知到的角速度方向就和激光雷达实际运动方向差了5度积分出来的位移方向也偏了5度。在10米的运动距离上这个偏差就是0.87米足够让点云匹配彻底失败。2.3 标定方法的选型为什么不用手眼标定传统的手眼标定AXXB需要精确控制机械臂运动或者用标定板做多姿态采集流程繁琐且对设备要求高。对于激光雷达和IMU的联合标定目前主流做法有两类一类是基于运动激励的标定让设备做充分的旋转和平移运动通过对比激光雷达里程计和IMU积分的结果来优化外参另一类是基于连续时间轨迹的标定把外参作为优化变量放进因子图里一起估计。LIO-SAM本身不提供在线标定功能它假设外参是已知的。所以我们需要借助外部工具来获取外参。常用的方案有LI-Init、lidar_align、以及一些基于ROS的标定包。我实测下来LI-Init对LIO-SAM的适配性最好因为它专门针对激光雷达惯性系统设计而且能同时估计时间偏移和外参。3. 标定前的准备工作硬件、软件和环境3.1 硬件连接与时间同步检查在开始标定之前必须确保硬件连接正确。激光雷达和IMU通常通过不同的接口连接到主机激光雷达走网口或USBIMU走串口或USB。如果你的IMU是内置在激光雷达里的比如某些固态激光雷达集成了IMU那外参就是出厂固定的但仍然需要验证。时间同步是容易被忽略的关键点。LIO-SAM要求激光雷达和IMU的时间戳在同一个时间基准上。如果IMU自带硬件时间同步功能最好让激光雷达的PPS信号触发IMU的时间戳。如果没有硬件同步至少要在驱动层面做软件对齐。我遇到过因为时间戳差了20毫秒导致建图重影的案例排查了一整天才发现是驱动的时间戳没有对齐。检查方法很简单录一个包含旋转和平移的bag用rosbag info看两个话题的时间戳范围是否一致再用rqt_plot把角速度和激光雷达的帧时间画出来看是否有系统性偏移。3.2 软件环境搭建与依赖安装标定工具链一般需要ROS环境。如果你用的是ROS1推荐Ubuntu 18.04或20.04加ROS Melodic或Noetic。如果是ROS2LIO-SAM也有对应的分支但标定工具的选择会少一些。我建议标定阶段还是在ROS1下做工具成熟度高资料也多。需要安装的依赖包括Ceres Solver、GTSAM、PCL、Eigen3。LI-Init还需要OpenCV和Sophus。安装GTSAM的时候注意版本LIO-SAM对GTSAM 4.0.2以上版本兼容性较好太新的版本可能有API变动导致编译失败。# 以Ubuntu 20.04 ROS Noetic为例 sudo apt install ros-noetic-pcl-ros ros-noetic-tf2-geometry-msgs sudo apt install libceres-dev libeigen3-dev libopencv-dev # GTSAM建议从源码编译 git clone https://github.com/borglab/gtsam.git cd gtsam git checkout 4.0.2 mkdir build cd build cmake -DGTSAM_BUILD_WITH_MARCH_NATIVEOFF .. make -j4 sudo make install注意GTSAM编译时如果开启MARCH_NATIVE在某些CPU上会出现非法指令错误建议关掉。3.3 数据采集的场地和运动要求标定数据采集不是随便拿着设备走一圈就行。LI-Init这类工具需要充分的运动激励具体要求包括三个轴向上都要有足够的旋转尤其是绕Z轴的旋转要充分因为激光雷达在水平方向的观测约束最强平移运动要包含各个方向避免只在一个平面内运动运动速度不要太快避免运动模糊和点云畸变太严重。我通常选一个空旷的室外场地周围有建筑物或树木提供特征但不要有太多动态物体。采集时间控制在60到90秒太短激励不充分太长会累积漂移影响标定精度。采集时先静止10秒用于初始化然后做几组8字形运动穿插几次原地旋转和前后左右平移。4. 手把手实操从数据采集到外参输出4.1 录制标定bag的完整命令假设你的激光雷达话题是/velodyne_pointsIMU话题是/imu/data录制命令如下rosbag record -O calibration.bag /velodyne_points /imu/data录制完成后先检查bag的基本信息rosbag info calibration.bag重点看两个话题的消息数量和频率是否正常。激光雷达一般10HzIMU一般100Hz以上。如果IMU频率只有几十Hz标定精度会受影响。4.2 用LI-Init跑标定并解读输出LI-Init的安装和运行流程如下# 安装 cd ~/catkin_ws/src git clone https://github.com/hku-mars/LiDAR_IMU_Init.git cd .. catkin_make source devel/setup.bash # 运行标定 roslaunch lidar_imu_init velodyne.launch # 另开终端播放bag rosbag play calibration.bag标定完成后终端会输出外参矩阵和时间偏移。外参矩阵是一个4x4的变换矩阵左上角3x3是旋转右上角3x1是平移。你需要把这个矩阵转换成LIO-SAM需要的格式。LIO-SAM的params.yaml中extrinsicRot和extrinsicRPY都是3x3矩阵按行展开写。extrinsicTrans是3x1向量。注意LI-Init输出的旋转矩阵可能是从IMU到激光雷达的变换而LIO-SAM需要的是从激光雷达到IMU的变换需要求逆。4.3 把标定结果写入LIO-SAM配置假设LI-Init输出的旋转矩阵是R_ilIMU到激光雷达平移是t_il。LIO-SAM需要的是R_li R_il的转置t_li -R_li * t_il。然后按照LIO-SAM的格式写入extrinsicRot: [-1, 0, 0, 0, 1, 0, 0, 0, -1] extrinsicRPY: [1, 0, 0, 0, 1, 0, 0, 0, 1] extrinsicTrans: [0.0, 0.0, 0.0]这里的数值只是示例实际要填入你标定出来的结果。extrinsicRot和extrinsicRPY的区别在于前者处理IMU数据的旋转后者处理激光雷达数据的旋转具体填法取决于你的传感器安装方向。4.4 验证标定效果看什么指标标定完不能直接跑建图就完事要先验证。最简单的验证方法是用标定后的参数跑LIO-SAM看建图结果是否有明显重影。更定量一点的方法是对比激光雷达里程计和IMU积分的轨迹如果外参正确两条轨迹应该高度重合。我常用的验证流程是录一段包含回环的bag用标定参数跑LIO-SAM然后看回环处的点云是否对齐。如果回环处错层明显说明外参还有问题。另一个指标是看Z轴漂移如果设备在水平运动时Z轴持续单向漂移通常是IMU的俯仰角或横滚角外参有偏差。5. 避坑指南标定中最容易翻车的六个问题5.1 时间戳不同步导致的隐性误差这是最隐蔽的问题。表面上看标定流程都跑通了外参也输出了但建图就是飘。原因可能是激光雷达和IMU的时间戳有几十毫秒的固定偏移。LI-Init虽然能估计时间偏移但如果偏移太大超过100毫秒标定结果就不可信了。排查方法用rosbag play --clock播放bag然后在rviz里同时显示激光雷达点云和IMU的orientation看旋转时两者是否同步。如果IMU已经转完了激光雷达才动说明IMU时间戳超前了。5.2 外参矩阵方向搞反这是新手最常犯的错误。LI-Init输出的是T_ilLIO-SAM要的是T_li两者互为逆矩阵。旋转部分转置一下就行平移部分要记得乘负号再旋转。我见过有人只转置了旋转矩阵平移直接取负结果平移量完全错了。记住口诀旋转转置平移先旋转再取负。5.3 IMU安装方向与配置不符LIO-SAM的配置里有一个imu_enable参数还有一组extrinsicRot和extrinsicRPY。如果你的IMU是倒装的或者旋转了90度安装这些参数都要相应调整。最稳妥的方法是把IMU的坐标系和激光雷达坐标系在rviz里画出来直观确认方向关系。5.4 运动激励不充分导致标定发散有些人采集数据时只是拿着设备慢慢走了一圈旋转不够结果LI-Init优化不收敛输出的外参和初始值差不多。标定数据一定要有充分的旋转激励尤其是绕三个轴的旋转都要有。我通常要求至少包含3组完整的360度旋转每组绕不同轴。5.5 点云畸变未去除影响标定精度激光雷达在运动过程中采集的点云会有畸变如果不做去畸变处理标定精度会大打折扣。LIO-SAM内部有去畸变模块但LI-Init标定时也需要考虑这个问题。建议在采集标定数据时控制运动速度避免剧烈加减速。5.6 标定结果过拟合有些人为了追求标定残差最小反复调整参数结果过拟合了。标定残差小不代表实际建图效果好。我建议用两组独立的数据做交叉验证一组用于标定另一组用于验证。如果验证集上效果也好说明标定参数是可靠的。6. 标定后的建图验证与调优6.1 用回环检测验证外参质量标定完成后跑一段包含回环的建图数据是最直接的验证。如果外参正确回到起点时点云应该完美重合。如果出现错层错层的方向和大小能反推出外参的偏差方向。比如回环处点云在垂直方向错开说明俯仰角或Z轴平移有偏差水平方向错开说明偏航角或X/Y平移有偏差。6.2 Z轴漂移的排查思路Z轴漂移是LIO-SAM建图中最常见的问题之一。如果标定后仍然有Z轴漂移先检查IMU的加速度计零偏是否标定过。加速度计零偏会导致重力方向估计错误进而导致Z轴漂移。可以用IMU标定工具先标定内参再做联合标定。另一个可能的原因是激光雷达的安装角没有精确测量。如果激光雷达相对于水平面有倾斜而外参里没有体现这个倾斜建图时Z轴就会持续漂移。6.3 参数微调的经验值标定得到的外参通常已经比较准了但实际建图时可能还需要微调。我的经验是旋转参数一般不需要大调除非明显看到重影平移参数中Z轴平移对标定精度最敏感因为激光雷达在垂直方向的观测约束较弱。如果建图时地面出现弯曲可以尝试微调Z轴平移每次调整0.5厘米左右。7. 常见问题速查表问题现象可能原因排查方法解决措施建图整体重影外参旋转错误检查旋转矩阵是否转置重新计算外参确认方向Z轴持续漂移加速度计零偏或俯仰角外参错误静止时看IMU重力方向先标定IMU内参再联合标定回环处错层外参平移有偏差测量错层方向和大小微调对应轴的平移量标定不收敛运动激励不足检查bag中旋转是否充分重新采集增加旋转运动时间戳不同步驱动未做时间对齐对比两话题时间戳硬件同步或软件对齐点云扭曲运动过快或去畸变未开检查点云形状降低运动速度开启去畸变8. 我在实际标定中积累的几个心得第一个心得是标定不是一劳永逸的。每次拆装设备后外参都可能变化尤其是平移量。我养成的习惯是每次重要实验前都快速验证一下外参用一段短bag跑一下看回环是否对齐。第二个心得是不要迷信标定工具的默认参数。LI-Init有一些可调参数比如最大迭代次数、点云降采样分辨率等这些参数对结果有影响。我通常会把降采样分辨率调小一些用更多的点参与优化虽然慢一点但结果更稳。第三个心得是标定数据的质量比数量重要。与其录一个5分钟的大bag不如录三个1分钟的高质量bag每个都包含充分的旋转和平移。数据太长反而会引入漂移影响标定精度。第四个心得是如果条件允许用全站仪或激光跟踪仪测量外参作为初值能大幅提高标定收敛速度和精度。没有这些设备的话用卷尺粗略测量平移量用角度尺测量旋转量也比完全不给初值要好。第五个心得是LIO-SAM的配置文件里extrinsicRot和extrinsicRPY的区分容易搞混。简单记法是extrinsicRot管IMU数据的旋转extrinsicRPY管激光雷达数据的旋转。如果你的IMU和激光雷达坐标系定义一致extrinsicRPY通常是单位矩阵。最后分享一个快速验证外参是否合理的小技巧在rviz里同时显示激光雷达点云和IMU的orientation箭头然后旋转设备看点云和箭头的旋转方向是否一致。如果方向相反说明外参旋转矩阵有180度的错误。这个方法虽然粗糙但能快速发现大方向上的错误避免在错误参数上浪费时间。