4. LiDAR-IMU外参标定原理
好，咱们今天聊点硬核的。LiDAR和IMU这对搭档，在自动驾驶和机器人领域几乎是标配。但说实话，如果外参没标好，那融合出来的定位结果，用我同事的话说——「就像喝醉了酒走路，东倒西歪」。
我自己刚入行那会儿，就吃过这个亏。有一次在园区测试，激光雷达点云和IMU推算出来的轨迹，明明应该重合，结果差了半米多。排查了两天，最后发现是外参旋转矩阵里一个符号搞反了。嗯，从那以后，我对标定这件事就格外上心。
今天咱们重点讲两种主流方法：手眼标定法（AX=XB）和基于运动约束的标定。说白了，它们都是在解决同一个问题——怎么把LiDAR和IMU这两个传感器的坐标系，精确地「拧」到一起。
为什么需要外参标定？
先想一个问题：LiDAR看到了一棵树，IMU测到了加速度和角速度。这两个数据来自不同的坐标系，如果不做对齐，你拿什么去融合？
外参标定要算的就是两个东西：
- 旋转矩阵 R：LiDAR坐标系到IMU坐标系的旋转关系
- 平移向量 t：两个坐标系原点之间的位移
有了它们，LiDAR点云才能被正确投影到IMU的坐标系下，或者反过来。这是所有多传感器融合的基石。
核心要点：外参标定的本质，就是求解一个刚体变换 T = [R | t]，使得两个传感器在同一时刻观测到的物理量在空间上对齐。
方法一：手眼标定法（AX=XB）
这个方法的名字很有意思。它最早来自机器人领域——机械臂末端装了个相机，要标定相机和机械臂末端之间的变换关系。你想想看，这不就跟LiDAR和IMU的关系一模一样吗？
公式很简单：AX = XB
其中：
- A：IMU在两个不同时刻之间的相对运动（旋转+平移）
- B：LiDAR在两个不同时刻之间的相对运动
- X：我们要求的外参（LiDAR到IMU的变换）
为什么这个等式成立？因为传感器之间的相对位姿是固定的。IMU动了多少，LiDAR也必须跟着动同样的量，只不过表达方式不同而已。
我在项目中遇到过一个问题：IMU的角速度积分出来的旋转，和LiDAR通过点云配准（比如ICP）算出来的旋转，时间戳对不上。结果解出来的X乱七八糟。后来我加了一个时间戳对齐的预处理步骤，才搞定。
我的小技巧：解AX=XB时，建议先解旋转部分，再解平移部分。因为旋转误差会直接传递到平移上。我习惯用四元数或者李代数来参数化旋转，避免万向锁问题。
具体求解步骤：
- 采集一段运动数据，确保有足够的旋转和平移激励
- 分别计算IMU和LiDAR的帧间运动（A和B）
- 构建超定方程组，用最小二乘法求解X
- 验证：用标定后的外参重投影，看残差是否在阈值内
// 伪代码示例：手眼标定核心逻辑
// 输入：IMU运动序列 A_list, LiDAR运动序列 B_list
// 输出：外参 X
Matrix4d solveHandEye(vector<Matrix4d> A_list, vector<Matrix4d> B_list) {
// 1. 先解旋转
Matrix3d R = solveRotation(A_list, B_list);
// 2. 再解平移
Vector3d t = solveTranslation(A_list, B_list, R);
// 3. 组合成变换矩阵
Matrix4d X = Matrix4d::Identity();
X.block<3,3>(0,0) = R;
X.block<3,1>(0,3) = t;
return X;
}方法二：基于运动约束的标定
手眼标定法有个前提——你得能准确算出LiDAR的帧间运动。但如果场景特征少，或者运动太快导致点云畸变，那ICP算出来的运动就不靠谱了。
这时候，基于运动约束的方法就派上用场了。它的思路是：不直接算运动，而是利用运动过程中的物理约束来反推外参。
举个例子：IMU测到的重力方向，和LiDAR观测到的地面法向量，理论上应该对齐。如果不对齐，那就是外参有问题。
具体来说，常用的约束包括：
- 重力约束：IMU的加速度计在静止时只测到重力，LiDAR扫描到的水平面法向量应与重力方向一致
- 旋转约束：IMU角速度积分得到的旋转，应与LiDAR点云配准得到的旋转一致
- 非完整约束：对于轮式机器人，侧向速度应为零，这个约束可以联合优化外参
注意：基于运动约束的方法对数据质量要求很高。我曾经在室内用这个方法标定，结果地面有点坡度，重力方向算偏了，外参也跟着偏。后来我加了一个地面拟合的鲁棒性处理，才解决。
这种方法的优势在于：
- 不需要精确的帧间运动估计
- 对场景退化（比如长走廊、空旷场地）更鲁棒
- 可以联合多个约束一起优化，提高精度
但缺点也很明显：收敛速度慢，而且容易陷入局部最优。所以实际工程中，我通常先用运动约束方法给一个初值，再用手眼标定法精调。
两种方法的对比
| 对比项 | 手眼标定法（AX=XB） | 基于运动约束的标定 |
|---|---|---|
| 核心原理 | 利用帧间运动的一致性 | 利用物理约束（重力、运动模型等） |
| 数据需求 | 需要丰富的旋转和平移激励 | 需要静止段和运动段数据 |
| 对场景依赖 | 依赖LiDAR配准精度 | 对场景退化更鲁棒 |
| 计算复杂度 | 低，可实时求解 | 较高，通常需要迭代优化 |
| 典型应用 | 高精度标定，场景特征丰富 | 初值获取，场景退化情况 |
知识体系结构图
下面这张图，是我自己梳理的LiDAR-IMU外参标定的整体逻辑。你可以把它当作一个思维导图来看。
实际工程中的建议
说了这么多理论，最后分享几点我踩过的坑：
- 运动激励要足够：别只在一个平面上转圈。上下左右、俯仰翻滚，都要有。我见过有人拿着设备原地转圈，结果平移部分完全不可观。
- 时间戳对齐是前提：LiDAR和IMU的时钟不同步，标定结果就是错的。我习惯用硬件同步或者插值对齐。
- 多段数据联合优化：别只靠一段数据。采集3-5段不同运动模式的数据，联合优化，鲁棒性会好很多。
- 验证环节不能省：标定完了，拿一段没参与标定的数据做验证。看点云投影到图像上（如果有相机）或者看轨迹对齐误差。
我的习惯：每次标定完，我都会把外参保存下来，然后跑一遍离线融合，看定位轨迹的平滑度和一致性。如果发现跳变或者漂移，大概率是外参还有问题。
好了，关于LiDAR-IMU外参标定的两种核心方法，就聊到这儿。说白了，手眼标定法适合场景特征丰富的情况，基于运动约束的方法更适合退化场景。实际项目中，我建议两者结合着用，效果最好。