在使用LIO-SAM进行Lidar+IMU建图之前，需要做两个标定工作，IMU内参标定和Lidar-IMU外参标定。IMU内参标定，可以解决其固有的测量误差问题。Lidar和IMU的外参标定(以下简称“外参标定”)，目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，对应最终输出结果中的平移向量与旋转矩阵。

标定环境如下：

• OS：Ubuntu20.04
• ROS：1 Noetic（选ROS1的原因是大多数标定工具都没有ROS2版本）
• IMU：H30
• Lidar：Hesai Pandar40P

一、IMU内参标定

IMU的内参标定主要分为确定性参数标定和不确定性参数标定：

• 确定性参数标定：包括尺度、坐标系非正交（安装误差）、传感器偏置, 代表性工具imu_tk。
• 不确定性参数标定：包括量化噪声（Q）、角度随机游走（N）、零偏稳定性（B）、角速率随机游走（K）、速率斜坡（R），代表性工具imu_utils。

IMU自身的重置校准已经通过厂商工具完成，这里主要还需要采用imu_utils工具对IMU进行内参标定。imu_utils的核心旨在通过Allan方差分析来评估IMU（加速度计和陀螺仪）的随机误差特性。它不仅提供了C++实现的Allan方差工具，还支持从静态测试中收集的数据进行细致的噪声分析。用户能够通过这个工具包对不同型号的IMU（如ADIS16448、3DM-GX4等）在静置状态下的数据进行长达两小时的采集，进而分析“白噪声”、“偏置不稳定性”等关键参数，这些对于导航系统和自动驾驶应用至关重要。通过使用imu_utils，研究者和工程师能够更有效地校准和选择适合他们应用的IMU，确保传感器数据的可靠性和精确度。无论是无人机、自动驾驶车辆还是机器人领域，imu_utils都是一个不可或缺的工具箱。

imu方差收敛示意图：

• 左列：分别是加速度计和陀螺的allen方差（可以粗略的判断出我们需要静置imu的时间，即初始化时间）
• 右列：根据标定获得的imu内参，仿真产生的allan方差图

1. 编译imu_utils工具

# 1. ceres-solversudo apt-get install libdw-dev libceres-dev# 2. download codemkdir -p ~/catkin_calib/srccd ~/catkin_calib/srcgit clone git@github.com:yanjingang/code_utils.gitcd ..# 3. edit for build bug fix vim src/code_utils/src/sumpixel_test.cpp    row2: #include "backward.hpp" 改为 #include "code_utils/backward.hpp"     row 84/107: CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE    row 94/117: CV_MINMAX 改为 CV_MMXvim src/code_utils/src/mat_io_test.cpp    row 33: CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED# 4. buildcatkin_make -DCATKIN_WHITELIST_PACKAGES=code_utilscatkin_make -DCATKIN_WHITELIST_PACKAGES=imu_tuils

2. 采集IMU数据

打开IMU后，等待10分钟再开始录制（上电10分钟之内误差会较大），录制时应保持IMU完全静止不动，时长建议至少两小时左右，否则标定出来的参数不够准确。

# 启动IMUroslaunch yesense_imu yesense_ahrs.launch# 录制bagrosbag record -O h30.bag /sensor/imu/data# 查看包信息rosbag info h30.bag     path: h30.bag    version: 2.0    duration: 9hr 41:47s (34907s)    start: Nov 04 2024 23:48:21.41 (1730735301.41)    end: Nov 05 2024 09:30:08.77 (1730770208.77)    size: 2.5 GB    messages: 6981520    compression: none [3249/3249 chunks]    types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]    topics: /sensor/imu/data 6981520 msgs : sensor_msgs/Imu

3. 内参标定

3.1 配置IMU标定参数

# confvim src/imu_utils/launch/yesense_h30.launch    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">        <param name="imu_topic" type="string" value= "/sensor/imu/data"/>        <param name="imu_name" type="string" value= "yesense_h30"/>        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/yesense_h30/"/>        <param name="max_time_min" type="int" value= "200"/>        <param name="max_cluster" type="int" value= "100"/>    </node># buildcatkin_make -DCATKIN_WHITELIST_PACKAGES=imu_utils

• imu_topic：录制的数据包中的imu话题，可使用 rosbag info ***.bag 命令查看对应的imu话题，并注意以 / 开头，虽然info命令不显示斜杠，但也必须加上斜杠。
• imu_name：你自己的imu名字，可随便起名。
  data_save_path：你期望保存的标定结果的具体路径，由于imu_utils/data目录下默认有很多文件，后续标定以后不好判断结果文件的生成，更改路径以方便后续判断标定结果是否生成；另外注意以 / 结尾，代表在此目录下生成。
• max_time_min：录制的数据包的时长，单位分钟，120代表2h，可使用 rosbag info ***.bag 命令查看数据包时长(duration)，并注意在这里设置的分钟数不能高于数据包的实际时间（比如我的数据包duration是1h59min59s，那在launch文件中就只能设置119。

3.2 IMU内参标定

cd ~/catkin_calibsource devel/setup.bash# 启动标定程序roslaunch imu_utils yesense_h30.launch# 200倍速播包（直接启动imu的输出等2-6小时，现采现标也可以，不过建议还是录下来bag，方便重复标）rosbag play -r 200 h30.bag

2小时的包需要等待一分钟左右，数据包即播放完毕，一般来说播放完毕瞬间会显示计算过程：

 程序在数秒内会计算完成，并显示finish并自动退出。

查看设定的data_save_path目录下会生成yesense_h30_imu_param.yaml文件，我们只要其中的4个值，分别是 gyr_n（平均轴旋转高斯白噪声）、gyr_w（平均轴旋转 bias 随机游走）、acc_n（平均轴加速度高斯白噪声）、acc_w（平均轴加速度 bias 随机游走）。

将yesense_h30_imu_param.yaml中的这四个内参值放到LIO-SAM/config/params.yaml对应位置：

$ cat ~/catkin_calib/src/imu_utils/data/yesense_h30/yesense_h30_imu_param.yaml   %YAML:1.0   ---   type: IMU   name: yesense_h30   Gyr:      unit: " rad/s"      avg-axis:         gyr_n: 1.2050330151032497e-03         gyr_w: 1.4177677047615486e-05   Acc:      unit: " m/s^2"      avg-axis:         acc_n: 7.5656614363139437e-03         acc_w: 2.9325075138204079e-04

3.3 配置LIO-SAM的IMU内参

对应LIO-SAM的IMU内参配置：

vim ~/ros2_ws/src/LIO-SAM/config/params.yaml    # IMU Settings (IMU内参)    imuAccNoise: 7.5656614363139437e-03        # acc_n    imuGyrNoise: 1.2050330151032497e-03        # gyr_n    imuAccBiasN: 2.9325075138204079e-04        # acc_w    imuGyrBiasN: 1.4177677047615486e-05        $ gyr_w

 

二、IMU与Lidar外参标定

外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。

这里采用浙江大学开发的 lidar_imu_calib 工具（简称li_calib）进行外参标定，它是一个用于校准 6DoF 刚性变换和 3D LiDAR 与 IMU 之间的时间偏移的工具包，基于连续时间批量优化，建图的实践证明此方法具有较好的鲁棒性。这个工具只支持VLP-16与IMU进行标定，我们使用的Pandar40P需要进行适配才能使用。

1. 源码编译

最好科学上网执行以下操作，能节约很多时间：

# dependsudo apt install -y python3-wstool ros-noetic-pcl-ros ros-noetic-velodyne-msgs ros-noetic-velodyne-pointcloud# downloadcd ~/catkin_calib/src# slam codegit clone git@github.com:yanjingang/ndt_omp.git    # 这个是使用c++14编译的版本git clone git@github.com:yanjingang/li_calib.git# sensor codegit clone https://github.com/HesaiTechnology/HesaiLidar_General_SDK.gitgit clone --recursive git@github.com:yanjingang/HesaiLidar_General_ROS.gitgit clone git@github.com:yanjingang/yesense_ros1.git# depend pangolincd li_calib./build_submodules.sh# buildcd ~/catkin_calibcatkin_make -DCATKIN_WHITELIST_PACKAGES=ndt_ompcatkin_make -DCATKIN_WHITELIST_PACKAGES=li_calibcatkin_make -DCATKIN_WHITELIST_PACKAGES=   # 清除包指定，全编译source ./devel/setup.bash

2. 数据采集

录制数据时，需要将Lidar-IMU设备固定到一起，并基于充分的运动激励。

• 需要在平面多的房间里录制，特征点过少或者平面过少可能会导致失败，经测试一般办公室环境就可以。
• XYZ三轴方向都需要充分移动、充分转动，但不宜加速度过大的猛烈撞击式运动，可各轴方向正常转动4-5次， 具体方法可以参考这里 。
• 整体时间1分钟左右即可。

开始录制：

cd ~/catkin_calibsource devel/setup.bash# 启动Lidar (Frame: Pandar40P, Topic: /hesai/pandar)roslaunch hesai_lidar hesai_lidar.launch# 启动IMU (Frame: imu_link, Topic: /sensor/imu/data)roslaunch yesense_imu yesense_ahrs.launch# 开启Lidar PTP时钟同步sudo ptp4l -m -4 -i eno2 -S# 检查帧率rostopic hz /sensor/imu/data /hesai/pandar    topic rate min_delta max_delta std_dev window    /sensor/imu/data 200.0 0.003751 0.006297 0.001005 201     /hesai/pandar 10.01 0.09922 0.1012 0.0006164 201 # 检查时间戳是否正常rostopic echo /hesai/pandar -prostopic echo /sensor/imu/data -p# 启动录包rosbag record -O pandar40p-h30.bag  /hesai/pandar /sensor/imu/data# 查看包信息rosbag info pandar40p-h30.bag     path: pandar40p-h30.bag    version: 2.0    duration: 3:19s (199s)    start: Nov 06 2024 23:48:12.51 (1730908092.51)    end: Nov 06 2024 23:51:32.15 (1730908292.15)    size: 5.9 GB    messages: 41925    compression: none [1997/1997 chunks]    types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]           sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]    topics: /hesai/pandar 1996 msgs : sensor_msgs/PointCloud2            /sensor/imu/data 39929 msgs : sensor_msgs/Imu

3. 外参标定

3.1 修改配置

根据录制数据修改Lidar类型、Topic、Bag位置、录制时长等参数：

vim ~/catkin_calib/src/li_calib/launch/licalib_gui.launch     <arg name="lidar_model"         default="HESAI_40P" />                                  <!-- Lidar类型: VLP_16 / HESAI_40P / RS_16 -->    <arg name="topic_imu"           default="/sensor/imu/data" />                           <!-- Lidar topic -->    <arg name="topic_lidar"         default="/hesai/pandar" />                              <!-- IMU topic -->    <arg name="path_bag"            default="/home/work/catkin_calib/pandar40p-h30.bag" />  <!-- 录制的数据包的具体路径 -->    <arg name="bag_durr"            default="120" />                                         <!-- 录制的数据包的时长，单位秒 -->    <arg name="show_ui"             default="true" />                                       <!-- 打开可视化操作窗口 -->    

*注：如果是velodyne的雷达，需要录制/velodyne_packets 而不是 /velodyne_points才可以。我这里用的Pardar40P，是在li_calib里单独适配了下格式，适配的时候注意sensor_msgs::PointCloud2和velodyne_msgs::VelodyneScan格式是2个不同的unpack_scan()，根据你的雷达只适配其中一个方法即可。

3.2 启动标定程序

cd ~/catkin_calibsource devel/setup.bashroslaunch li_calib licalib_gui.launch

3.3 Initialization初始化

在弹出界面上，点击Initialization按钮进行初始化，控制台输出如下：

Load dataset lidar_type=2 bag=/home/work/catkin_calib/pandar40p-h30.bag/hesai/pandar: 1980/sensor/imu/data: 39601Ceres Solver Report: Iterations: 29, Initial cost: 6.809535e+06, Final cost: 5.975024e+02, Termination: CONVERGENCE[Initialization] Done. Euler_ItoL initial degree: 178.901 178.979 -86.1102

3.4 Data Association数据关联

点击Data Association按钮进行数据关联，控制台输出如下：

[Association] start ....Plane type :32 12 19; Plane number: 63Surfel point number: 46353[Association] 201426 ms

关联成功后ui界面上会显示出来采集数据的地图：

3.5 Batch Optimization迭代优化初始化

点击Batch Optimization按钮进行迭代优化初始化，控制台输出如下：

================ Iteration 0 ==================Ceres Solver Report: Iterations: 31, Initial cost: 7.883758e+06, Final cost: 1.245670e+05, Termination: NO_CONVERGENCE============== After optimization ================[Gyro]  Error size, average: 39579; 0.00828549 0.00818175 0.00496492[Accel] Error size, average: 39579;   0.028903 0.0306898 0.0593846[LiDAR] Error size, average: 46353; 0.0838659P_LinI      :  0.0329959 -0.0504669  0.0173797euler_LtoI  : 0.765177 0.508529 -93.3331P_IinL      : -0.0482363 -0.0356993 -0.0183443euler_ItoL  :  0.552158 -0.734316   93.3333time offset : 0gravity     : -0.670243  -0.65516   9.74503acce bias   : -0.0602972   0.137668 -0.0115294gyro bias   : -0.00037345 0.000105357 0.000272736[BatchOptimization] 59086.6 ms

3.6 Refinement迭代优化

点击Refinement按钮进行迭代优化，与上一部的初始化输出类似，但是需要多次点击。终端每出现 [Refinement] 24056.2 ms 信息以后就点击一次，直到每次终端显示的字段都完全一样不再变化（一般5-20轮左右），即视为标定成功，然后手动退出程序。标定结果会保存在bag包相同路径的同名文件夹下calib_result.csv 文件内。

tail -n1 ~/catkin_calib/pandar40p-h30/calib_result.csv    p_IinL.x p_IinL.y p_IinL.z q_ItoL.x q_ItoL.y q_ItoL q_ItoL.w    0.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.692938

这个参数可以直接配置到tf中：

# static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms<node pkg="tf" type="static_transform_publisher" name="imu_to_lidar"	args="0.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.692938 imu_link lidar_link 20"></node>

3.7 配置LIO-SAM的Lidar -> IMU外参

标定外参要提供给LIO-SAM使用，因为涉及到四元数转化为旋转矩阵以及对应关系等，解释起来有点繁琐，我这里直接写了个自动生成脚本来根据Lidar-IMU标定外参一键生成LIO-SAM的配置参数：

• P_IinL.x、P_IinL.y、P_IinL.z：以激光雷达为参考坐标系时，IMU的三轴坐标，组成了雷达到IMU的平移向量（对应LIO-SAM的extrinsicTrans参数）；
• q_ItoL.x、q_ItoL.y、q_ItoL.z、q_ItoL.w：以激光雷达为参考坐标系时，IMU的三轴旋转四元数，可后续转化为旋转矩阵（对应LIO-SAM的extrinsicRot/extrinsicRPY参数）。
  

$ python3 tools/gen_liosam_extrinsics.py ~/catkin_calib/pandar40p-h30/calib_result.csvp_IinL.x p_IinL.y p_IinL.z q_ItoL.x q_ItoL.y q_ItoL q_ItoL.w0.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.6929380.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.692938Avg:  2  /  210.0169827 -0.0285108 0.097318 -0.00186273 -0.00394953 0.720984 0.692938IMU_to_LiDAR: [[-0.03966705 -0.99917971 -0.00815956] [ 0.99920914 -0.0396428  -0.00311358] [ 0.00278756 -0.00827661  0.99996186]]Extrinsics (lidar -> IMU):    extrinsicTrans:  [0.0169827, -0.0285108, 0.097318]    extrinsicRot:  [-0.03966705, -0.99917971, -0.00815956,                    0.99920914, -0.0396428,  -0.00311358,                    0.00278756, -0.00827661,  0.99996186]    extrinsicRPY:  [-0.03966705, -0.99917971, -0.00815956,                    0.99920914, -0.0396428,  -0.00311358,                    0.00278756, -0.00827661,  0.99996186]

 

三、总结

至此，在趟了无数个坑之后，我们的Lidar-IMU外参标定工作就做好了，下一次我带大家一起用这个外参SLAM建图看下标定效果是否精准。