当前位置：首页 > news >正文

从理论到实践：深入解析hku-mars Lidar_IMU_Init的标定流程与激励评估

news 2026/8/19 2:41:00

1. 激光雷达与IMU标定的核心挑战

在机器人定位和自动驾驶系统中，激光雷达（Lidar）和惯性测量单元（IMU）是最常用的传感器组合。激光雷达提供高精度的环境三维点云数据，而IMU则能输出高频的运动状态信息。但要让这两个传感器真正发挥协同效应，首先需要解决的就是传感器标定问题——也就是精确确定两个传感器之间的相对位置、姿态关系，以及它们数据采集时的时间同步偏差。

我曾在多个机器人项目中使用过hku-mars团队的Lidar_IMU_Init方案，这个开源工具最大的特点就是不需要任何标定板或特定环境，仅依靠传感器在自然运动中的数据就能完成标定。但在实际应用中，我发现很多工程师容易忽略一个关键点：运动激励的充分性。有一次我们在室内环境下进行标定，虽然采集了5分钟数据，但最终外参误差达到5度以上。后来分析发现，机器人只是做了缓慢的直线移动，缺乏旋转运动。

激光雷达和IMU的标定之所以复杂，主要面临三个核心挑战：

时间同步问题：IMU的采样频率通常在100-1000Hz，而激光雷达只有10-20Hz。不同传感器的时间戳可能存在毫秒级的偏差，这会直接影响运动估计的准确性。
坐标系转换：需要确定IMU坐标系到激光雷达坐标系的旋转矩阵和平移向量，也就是我们常说的外参（Extrinsics）。
传感器噪声：IMU的测量存在零偏（Bias）和噪声，激光雷达的点云也存在畸变和测量误差，这些都需要在标定过程中建模和补偿。

2. 数据预处理与时间对齐

2.1 激光雷达运动补偿与去畸变

激光雷达在扫描过程中本身就在运动，这会导致点云产生运动畸变。hku-mars的方案采用了Fast-lio2算法进行运动补偿。具体操作时，我们会把每帧激光点云分割成多个子帧（sub-frame），相当于提高了激光里程计的频率。例如，对于10Hz的激光雷达，如果把每帧分成5个子帧，就相当于获得了50Hz的姿态估计。

在实际操作中，我建议使用以下参数配置：

# Fast-lio2配置示例 point_filter_num: 1 # 降采样率 max_iteration: 4 # 迭代次数 filter_size_surf: 0.5 # 平面特征滤波尺寸

2.2 IMU数据滤波处理

原始IMU数据包含大量高频噪声，论文中采用了非因果零相位低通滤波器进行处理。这种滤波器的特点是不会引入相位延迟，特别适合离线标定场景。滤波后的IMU角速度和加速度可以表示为：

$$ \begin{aligned} \omega_{I_i} &= \omega_i^{gt} + b_\omega \ a_{I_i} &= a_i^{gt} + b_a \end{aligned} $$

在Python中可以使用scipy实现这种滤波：

from scipy import signal def zero_phase_filter(data, cutoff=10, fs=100): b, a = signal.butter(4, cutoff/(fs/2), 'low') return signal.filtfilt(b, a, data)

2.3 数据同步与插值

由于IMU和激光雷达频率不同，需要进行数据对齐。具体步骤是：

对IMU数据进行三次样条插值
按照激光雷达的时间戳降采样
计算每个时刻的角加速度和线加速度

这里有个实用技巧：插值前务必检查时间戳的单调性。我们曾遇到过一个案例，由于ROS bag录制时出现时间戳跳变，导致插值结果完全错误。建议添加以下检查：

assert np.all(np.diff(imu_timestamps) > 0), "IMU时间戳非单调递增"

3. 时间偏移与旋转外参的联合标定

3.1 基于互相关的时间偏移估计

激光雷达和IMU之间可能存在几十毫秒的时间偏差。hku-mars采用最大互相关方法进行粗估计。基本原理是将两个传感器的角速度信号进行滑动窗口匹配，找到相关性最高的时间偏移量。

实际操作中，我建议：

先以子帧间隔Δt为单位搜索整数倍偏移d*
再用更精细的步长优化小数部分δt
最终时间偏移为：$^It_L = d^*\Delta t + \delta t$

3.2 旋转外参的优化求解

获得时间偏移后，可以建立IMU和激光雷达角速度之间的关系：

$$ \omega_{I_k'} + \delta t \Omega_{I_k'} = ^IR_L \omega_{L_k} + b_\omega $$

这个方程需要同时求解旋转外参$^IR_L$、陀螺零偏$b_\omega$和时间偏移残差$\delta t$。论文将其转化为最小二乘问题：

$$ \underset{^IR_L,b_\omega,\delta t}{\arg \min} \sum | ^IR_L \omega_{L_k} + b_\omega - \omega_{I_k'} - \delta t \Omega_{I_k'} |^2 $$

在实现时，我推荐使用Ceres Solver进行优化：

ceres::Problem problem; problem.AddParameterBlock(rotation, 4, new ceres::QuaternionParameterization()); problem.AddParameterBlock(gyro_bias, 3); problem.AddParameterBlock(&time_offset, 1); ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<RotationCost, 3, 4, 3, 1>( new RotationCost(omega_L, omega_I, omega_dot_I)); problem.AddResidualBlock(cost_function, NULL, rotation, gyro_bias, &time_offset);

4. 平移外参与重力矢量的初始化

4.1 加速度测量模型

在获得旋转外参后，接下来求解平移外参$^Lp_I$、加速度计零偏$b_a$和重力矢量$^Gg$。基于IMU运动学模型，可以建立如下关系：

$$ ^IR_L^T (\bar{a}{I_k} - b_a) = ^LR_G (^Ga{L_k} - ^Gg) + (\omega_{L_k}^{\wedge2} + \Omega_{L_k}^{\wedge}) ^Lp_I $$

这个方程看起来复杂，但其实可以分解理解：

左边是IMU测量值转换到激光雷达坐标系
右边第一项是激光雷达测量值减去重力影响
右边第二项是离心力和欧拉力产生的加速度

4.2 优化问题构建

将上述模型转化为优化问题：

$$ \underset{^Lp_I,b_a,^Gg}{\arg \min} \sum | ^IR_L^T (\bar{a}{I_k} - b_a) - ^LR_G (^Ga{L_k} - ^Gg) - (\omega_{L_k}^{\wedge2} + \Omega_{L_k}^{\wedge}) ^Lp_I |^2 $$

实现时需要注意：

重力矢量的模长应约束为9.81 m/s²
加速度计零偏通常很小，可以添加正则化项
激光雷达加速度$^Ga_{L_k}$需要通过差分计算，建议使用中心差分法

5. 运动激励的量化评估

5.1 可观测性分析

标定的精度很大程度上取决于运动是否充分。hku-mars提出通过评估优化问题的Jacobian矩阵来量化激励充分性。具体来说：

对于旋转标定，检查矩阵$J_r^T J_r = \sum (\omega_{L_k}^{\wedge})^T \omega_{L_k}^{\wedge}$的奇异值
对于平移标定，检查矩阵$J_t^T J_t = \sum (\omega_{L_k}^{\wedge2} + \Omega_{L_k}^{\wedge})^T (\omega_{L_k}^{\wedge2} + \Omega_{L_k}^{\wedge})$的奇异值