IMU刚性外参静态标定：从重力矢量到SVD解算的完整实操指南

干多传感器融合的同行应该都有过这种经历：拿着 lidar_imu_calib 或 Kalibr 跑动态标定，数据采了十几分钟，迭代半天不收敛，最后定位到原因——初始旋转外参给歪了。IMU 的刚性外参标定，第一步通常不应该是动态优化，而是静态标定。先把旋转外参的粗值定准，后面的动态精标才有得玩，这个顺序我踩过好几次坑才真正体会到。

这篇文章把 IMU 刚性外参静态标定这件事完整拆开：为什么静态标定能标旋转、却基本标不了平移；重力矢量怎么变成最可靠的标定基准；多姿态数据怎么用 SVD 一步解算；以及实测里那些会导致结果偏掉的坑和完整排查链路。无论你是做自动驾驶、机器人、无人机，还是智能小车的 IMU 纠偏调试，这套方法都能直接落地用。

1. 静态标定能解决什么：先认清六自由度外参的观测边界

1.1 旋转三自由度里，静态条件下真正被约束的是哪几个

IMU 外参是 IMU 坐标系到目标传感器坐标系（LiDAR、相机、毫米波雷达等）的刚体变换，包含旋转 R 和位置 t。很多人一上来就想把六个自由度全部要出来，我早期就是这么干的，结果发现纯静态条件下，观测方程根本没提供那么多信息。

IMU 静止时只输出两个物理量：角速度和比力。静止时角速度理论上为零，信息量为零；比力等于重力反方向，归一化之后是一个三维单位向量，实际只携带两个自由度——也就是两个轴的角度信息。展开说：以世界系 z 轴朝上、重力为 [0, 0, -9.81] 的约定为例，静止 IMU 的加速度计读数是 [0, 0, +9.81] 在 IMU 系下的投影。这个向量能直接约束 IMU 相对于重力垂直方向的两个旋转角，对应到姿态上就是横滚角和俯仰角。而绕重力方向的偏航角，在"只有重力这一个参考"的条件下完全不可观。

所以纯 IMU 自身信息，静态下只能解出 roll 和 pitch。要想连 yaw 一起解出来，必须借助其他传感器提供不只垂直方向的空间参考。

1.2 另一个传感器的信息怎么参与进来：从平面法向量到完整旋转

以 LiDAR-IMU 为例，最常见的配置是设备放在一个水平地面上，LiDAR 点云里能分割出地面平面和墙面平面。地面平面法向量在 LiDAR 坐标系下的表达，就是一个"垂直方向"的观测；墙面法向量则是"水平方向"的观测。如果场景里有地平面加一面墙，两个不共线的方向就能完整确定一个坐标系朝向，配合 IMU 的重力方向，旋转外参的 3 个自由度就全被约束住了。

这一点的意义在于：静态标定的本质不是"碰运气让算法收敛"，而是把环境中的几何约束（平面法向量）与惯性约束（重力方向）对齐。要做得稳，关键是保证参考方向足够多、足够不共线。用一句话概括就是：**静态标定能不能解出完整旋转，取决于你能不能从场景里提取出至少两个不共线的方向参考。**这句话我在实际项目里反复验证过，比任何理论分析都直观。

1.3 平移自由度为什么在静态下"消失"了

平移 t 在静态标定里几乎是不可观的。原因是 IMU 本身不测量位置，静止时它只给方向信息（重力方向），而平面法向量、墙角线这些环境几何特征也都是方向信息，不包含"传感器中心到底在哪"的位置信息。换句话说，你把 LiDAR 装在 IMU 前方 5 厘米还是 10 厘米，对静态观测方程没有任何区别。

这是很多初学者对着标定结果一脸懵的原因：程序跑通了，旋转看起来合理，但 t 输出乱跳或者被算法忽略。这不是 bug，是观测性决定的。你换一个初值重新跑，t 就会变成另一个数，这是典型的不可观表现。平移外参要落地，只能靠后期测量、外部跟踪设备，或者动态激励手段来解决。后面第 4 节我会专门讲工程上的绕行做法。

2. 核心原理拆解：从加速度计读数到 SVD 旋转解算

2.1 静止时 IMU 到底在测什么：比力与重力反方向

先把模型写清楚。加速度计输出的不是重力，而是比力（specific force）f，物理含义是"单位质量承受的非引力外力"。静止在地面上时，支撑力抵消重力，比力恰好等于重力反方向。用公式表达：

• 世界系 w 为 z 轴朝上的当地水平系，重力加速度 g_w = [0, 0, -9.81] m/s²；
• IMU 静止时比力 f = -R_wb · g_w = R_wb · [0, 0, 9.81]，也就是"当地垂直向上"方向在 IMU 系下的表达。

所以拿到一段静止数据，只需要低通滤波后求均值，再归一化，就得到 IMU 系下的垂直方向单位向量 u。这是整个静态标定最核心的量。假设加速度计零偏为 b，则实测均值是 f + b，归一化后的方向跟真实垂直方向会差一个角度，量级大约是零偏沿垂直方向分量除以 g 后取 asin。消费级 MEMS 的零偏通常在几 mg 量级，也就是会引入约 0.1°～0.5° 的方向误差，这一点在精度要求高的项目里必须处理，后面第 5 节的坑里细讲。

2.2 多姿态观测与 Wahba 问题：一次 SVD 解出旋转

单组向量只能解出 2 个自由度，想解出完整 3 自由度旋转，办法是让设备在多个不同姿态下静止，采集多组"IMU 垂直方向 u_k"和"参考传感器垂直方向 v_k"的对应关系。两组不共线的向量对，就能唯一确定旋转矩阵 R，求解框架是经典的 Wahba 问题：

minimize Σ_k ||w_k - R · v_k||²

其中 w_k 为 IMU 系下的单位向量，v_k 为参考传感器系下的单位向量（注意方向约定要统一），求解用 SVD 一步到位：

H = Σ_k w_k · v_kᵀ = U · Σ · Vᵀ，R = U · Vᵀ，如果 det(R) < 0 就把 U 的最后一列取负再乘。

我给一个能直接用的 Python 实现：

import numpy as np def solve_rotation(v_vectors, w_vectors): """ 输入： v_vectors: (n, 3)，参考传感器系下的单位向量 w_vectors: (n, 3)，IMU系下的单位向量 返回： R: (3, 3)，满足 w ≈ R @ v 的旋转矩阵 """ assert len(v_vectors) == len(w_vectors) and len(v_vectors) >= 2 v = np.asarray(v_vectors, dtype=np.float64) w = np.asarray(w_vectors, dtype=np.float64) # 归一化以防手滑 v = v / np.linalg.norm(v, axis=1, keepdims=True) w = w / np.linalg.norm(w, axis=1, keepdims=True) H = w.T @ v U, _, Vt = np.linalg.svd(H) R = U @ Vt if np.linalg.det(R) < 0: U[:, -1] *= -1 R = U @ Vt return R

这个函数我在实际项目里用了很久，配上好的数据，解算本身几乎不会出问题。问题通常出在输入向量质量上，而不是算法上。

2.3 偏航角为什么是硬骨头：什么时候 SVD 救不了你

一个常被忽略的前提：Wahba 问题的向量对必须包含至少两组不共线的方向。如果整个采集过程中设备只是绕垂直轴转来转去，那么每一组 u_k 和 v_k 都指向同一个垂直方向，向量对全是共线的，SVD 解出来的 R 在 yaw 方向上就是病态的——算法会给一个数值结果，但那个 yaw 基本是噪声决定的。

所以要拿全旋转外参，姿态序列必须"翻来覆去"，让设备在不同姿态下把重力方向在 IMU 系里摆成好几个不共线的方向。更讲究的做法是，在场景里放一面垂直墙面，让墙法向量和地面法向量一起参与优化，这样才能把 yaw 牢牢约束住。很多人在这一步偷懒，结果就是 roll 和 pitch 看起来对，yaw 偏了三五度，融合算法跑起来照样飘。

2.4 一个直观的物理类比

把静态标定想成"用手电筒照墙找正"：重力是你的手电筒，它永远只指一个方向；你拿着设备翻来翻去，其实是在让这束光在 IMU 的坐标系里留下不同方向的"影子"。影子的方向摆得越开，你越能反推出设备坐标系和外部坐标系之间的旋转关系。如果从头到尾手电筒都只照同一个方向，那你怎么也推不出设备绕光轴转了多少度。这个类比记住之后，很多关于静态标定能标什么、不能标什么的疑问都会迎刃而解。

3. LiDAR-IMU 静态标定实操流程：从采集到验证

3.1 数据采集的环境与设备要求

静态标定对场景的要求不高，但有几条硬指标：

• 地面尽量平整，最好用水平仪确认，常见标定间的水平度通常能做到 0.1° 以内；
• 地面材质别选玻璃、深色吸光表面，LiDAR 点云会在上面产生大量噪点，建议铺一块平整的亚光板；
• 如果有墙面参与 yaw 约束，墙面和地面夹角越接近 90° 越好，两平面法向量越不共线，约束越强；
• 采集期间严禁人员走动、开关门、风道直吹，这些都会让设备产生微小振动。

设备安装上，IMU 和 LiDAR 需要刚性固连，连接结构不能有软性垫片或松动螺丝。我之前遇到过一版用泡沫双面胶固定的测试件，静态标定结果每次都不一致，最后发现是胶垫在采集过程中发生了蠕变。刚性两个字是外参标定的前提，不是一句口号。

3.2 多姿态序列怎么设计：最少几个、什么角度

姿态序列设计的核心原则是让重力方向在 IMU 系里"铺开"。我的习惯配置是 6 个姿态：

每个姿态保持静止至少 30 秒，IMU 采样率 100Hz 就是 3000 组数据；如果想压制零偏影响，建议 60 秒以上。LiDAR 点云每个姿态录满一帧或几帧即可，用于平面拟合。注意每个姿态之间切换动作要快但轻柔，静止判断以 IMU 角速度方差和加速度方差双条件为准，别靠肉眼感觉。

3.3 从点云和 IMU 数据里提取方向向量

IMU 侧的处理很简单：取每个姿态对应的静止段，对加速度计三轴分别求均值，再除以模长得到单位向量 u_k。如果 IMU 有已知的零偏标定结果，先减掉零偏再做归一化，这一步能显著提升结果质量。

LiDAR 侧我通常这样做：将点云体素降采样到 5cm，然后对目标区域做 RANSAC 平面拟合，得到平面方程 ax + by + cz + d = 0，法向量 n = (a, b, c) 归一化。关键是法向量朝向的一致性——统一约定法向量指向设备上方或场景内侧，否则解算时符号翻车。我的做法是让法向量与 LiDAR 坐标系的 z 轴正方向点乘结果为正，负了就整体取反。

如果场景里有墙面，墙面法向量单独提取，用来参与完整 3 自由度解算，步骤一样。

3.4 解算、残差评估和结果落地

把所有姿态的 (u_k, n_k) 对收集齐，调用上面的 solve_rotation 函数，得到旋转矩阵 R。这里有一个重要的约定问题：求出的 R 究竟是 R_imu_lidar 还是 R_lidar_imu，取决于输入向量顺序。我的习惯是统一约束 w = R · v，w 为 IMU 系向量，v 为 LiDAR 系向量，那么 R = R_imu_lidar，意思是"把 LiDAR 系下的向量旋转到 IMU 系"。建议在代码注释和配置文件中把这个含义写死，避免下次调用时搞反。

验证分三层：

• 逐姿态残差：每个姿态计算 angle(R · v_k, u_k)，画成柱状图，正常应该在 0.3° 以内，超标的姿态直接剔除重新解算；
• 交叉验证：随机抽掉一个姿态重新解算，看 R 变化量，变化超过 0.5° 说明某个姿态数据质量差或姿态数不够；
• 端到端验证：把标定后的 R 应用到实际点云配准或图像投影上，观察是否还有系统性旋转偏差。

这三层验证做完，旋转外参基本就稳了。剩余的偏差来源，多半是第 5 节里的那些坑。

4. 平移外参的工程做法：静态标定覆盖不到的位置量怎么处理

4.1 为什么别指望静态标定程序给你 t

前面说过，静态条件下平移不可观。更直白地说，无论你拿什么工具，只要设备全程静止，任何算法都不可能从数据里把 t 分离出来——优化器要么不收敛，要么给一个数值上说得过去但实际上没意义的 t。如果你在标定软件里看到 t 输出非零，先别高兴，换个初值重新跑一遍，t 变了，那就说明它只是被初值和正则项凑出来的数，不是真值。

4.2 手动测量、CAD 与经验值的操作细节

工程上最常用的办法是直接量。对于结构固定的设备，用卡尺量 IMU 外壳几何中心到 LiDAR 中心的相对位置，再根据 CAD 模型把坐标系原点偏移换算进去。这个方法的精度取决于安装一致性，通常能做到 3～10mm，前提是 IMU 内部敏感元件中心与外壳几何中心的位置误差已知。很多 MEMS IMU 模组在手册里会标敏感中心，买的时候注意看，没有就按外壳中心近似，但心里要有数。

实际操作时有一个细节容易被忽略：LiDAR 的坐标系原点在扫描旋转轴的中心，不在外壳的几何中心；IMU 的坐标原点在加速度计敏感元件的安装位置。这两个"中心"都要从 datasheet 或者 CAD 里确认，不能直接拿外壳尺寸相减。我见过有人把两对外壳尺寸相减得到了 30mm 的误差，最后动态标定结果怎么都对不上，查到原因后哭笑不得。

4.3 外部基准与混合标定：让平移有据可依

如果 mm 级精度还不够，就得动用外部基准：

• 运动捕捉系统：在设备上贴 marker，已知 marker 到 IMU 系和 LiDAR 系的偏移，通过动捕给出精确位姿序列，一次性解出 R 和 t；
• 高精度转台：把设备固定在转台上，旋转已知角度，利用转台中心作为位置基准；
• 外部激光跟踪仪或全站仪：直接测量设备上特征点在外部坐标系的位置。

这些方法成本高、操作繁琐，但适合对精度有硬指标的车规或科研项目。更常见的组合玩法是：静态标定出旋转 R，手动量出 t 的粗值，然后拿这个 (R, t) 当动态标定的初值，让动态优化器在激励充分的数据上去精修 t。这套流程我在多套设备上验证过，效率和最终精度都比直接上动态标定高一个台阶。

5. 实测中的坑与完整排查链路：标完为什么还是不对

5.1 加速度计零偏：几毫 g 的偏差就能吃掉零点几度

这是静态标定里最隐蔽的误差源。假设加速度计 z 轴零偏 5mg，在水平姿态下，垂直方向估计会偏向 z 轴约 0.29°；在倾斜姿态下影响方向更复杂。0.3° 的旋转误差对图像投影来说，在 10 米远处大约带来 5cm 的偏差，对要求高的融合系统已经是明显问题了。

解决思路有三条路径：一是用 imu_tk 或 Allan 方差方法先做 IMU 内参标定，把零偏减去再用；二是在姿态设计里加入"翻转"姿态，让重力方向相对 IMU 轴反转，用差分方式抵消零偏的常值部分；三是对每个姿态采集更长时间（60 秒以上）并做均值处理。三条组合起来，静态标定的角度精度通常能压到 0.1° 以内。

5.2 LiDAR 平面拟合的隐性干扰：地面不平、噪点和动态遮拦

LiDAR 地面法向量的精度直接影响最终结果，而它最容易翻车。三个常见情况：

• 地面本身不水平：标定间的地面看起来平，实际可能有 0.5° 的坡度，地面法向量直接带进结果，静态标定的精度天花板就被锁死了。解决方法是用水平仪确认，或者干脆用校准过的水平板架在三角架上作为参考面；
• RANSAC 参数不合适：距离阈值设太大，会把地面附近的细小突出物都算进内点，法向量被带偏。阈值建议按传感器噪声模型设，机械式 LiDAR 在 5 米范围内地面点噪声通常 2cm 左右，距离阈值给 4cm 足够；
• 非目标物混入：采集时有人走过、有推车经过，点云里出现动态遮挡，平面拟合就可能混入墙面或物体表面的点。这也是我坚持每个姿态录点云时看一帧可视化确认的原因。

5.3 坐标系约定和符号正负：标定结果"差 180°"的经典翻车

我见过最多的标定事故，不是算法不收敛，而是 R 的方向约定搞反。IMU 系的 x 轴朝前还是朝后、y 轴朝左还是朝右、z 轴朝上还是朝下，直接决定了 R 的多解。最常见的现象是解算出的 R 在某个轴上差了 180°，或者 roll 和 pitch 的符号整体反了。

排查方法很土但很有效：拿一个已知安装方向（比如 IMU 上印着 x 轴朝前）的设备和一把水平尺，摆一个水平姿态，把 R 乘到 LiDAR 系的已知方向上，看结果是否跟物理方向一致。标定程序运行前，先在代码里写一组自检向量对，模拟一个已知的 R，跑通流程再上真实数据。这套自检逻辑能拦掉 90% 的符号问题。

5.4 一个完整排查实例：标定后点云投影仍有 1.2° 偏差

有一次做 LiDAR-IMU 融合，静态标定之后做端到端验证，发现点云投影到图像上整体偏了 1.2°。第一反应是算法写错了，但检查 SVD 解算逻辑没有问题。排查链路如下：

第一步，回看逐姿态残差，发现第 4 个姿态的残差有 2.1°，其他姿态都在 0.3° 以内，怀疑这个姿态的数据有问题。调出当时的点云录屏，看到那条数据采集时正好有人在设备旁边蹲下站起来，点云地面区域混入了人体点，RANSAC 拟合出的法向量偏了。剔除这个姿态后重新解算，残差降到 0.35°。

第二步，剩余的 0.35° 仍然偏大。用水平仪检查地面，发现标定间地面前后方向带 0.3° 左右的坡度。把设备搬到水平校准板上重新采集，残差降到 0.08°。

第三步，再次做端到端验证，偏差降到了 0.1° 以内。这个案例说明，静态标定的误差通常不是解算环节造成的，而是数据质量造成的。遇到结果不对，先查数据，再查算法，这个顺序不要反。

6. 静态标定和动态标定的分工：怎么选、怎么衔接

6.1 静态标定的适用精度范围和应用场景

基于上面的误差分析，静态标定的旋转精度通常在 0.1°～0.5° 之间，取决于 IMU 等级、采集时长、参考面水平度。这个精度对很多场景来说已经够用了：

• 松耦合的融合架构，比如 GNSS/INS + LiDAR 松耦合、视觉 + IMU 的松耦合导航；
• 对 yaw 精度要求不高的系统，比如巡检机器人沿固定路径作业，0.5° 的 yaw 偏差在短距离内影响有限；
• 智能小车 IMU 纠偏调试特别适合先用静态标定把 IMU 安装角度和滚转俯仰初始偏差校出来，再搭配纠偏参数去调 PID 和转向映射。如果安装角都没校平，后面怎么调都费劲，纠偏参数很容易被当成"万能调节旋钮"，实际是在补偿安装误差。

6.2 什么时候静态标定不够，必须上动态

当系统需要紧耦合高精度融合时，静态标定的精度就不够了。典型场景包括：LiDAR-惯性里程计紧耦合、视觉-惯性紧耦合 VIO、需要高精度 lever arm 补偿的机械臂末端标定。动态标定通过让设备充分运动，把旋转和平移的观测性都激发出来，精度可以做到 0.01°～0.1° 量级，同时把 t 解出来。

动态标定的工具链路比较成熟：相机-IMU 用 Kalibr，LiDAR-IMU 用 APRIL-ZJU 的 lidar_imu_calib 或者港大 MARS 组的 li_calib。这些工具的共同要求是有一段激励充分的运动数据，并且给一个说得过去的初始外参。初值给不好，迭代发散是家常便饭。

6.3 我推荐的标准衔接流程

实践下来最省心的流程是：

1. 静态标定解 R，手动量出 t 粗值；
2. 用第 2 节的 SVD 方案做个快速验证，确认 R 没有符号和方向错误；
3. 把 (R, t) 作为动态标定工具的初值，跑一段充分激励的数据精修；
4. 动态结果出来后，再用静态标定的独立手段交叉验证，两个结果一致才算闭环。

这个流程的好处是，静态标定把最容易发散的初值问题提前解决掉了，动态标定收敛快、结果稳定。我最近三个项目都是这么做的，最顺利的一次，Kalibr 在第一轮迭代就收敛到了和静态结果相差 0.02° 的位置上。

最后再补一句个人的小习惯：静态标定每个姿态的原始数据、IMU 均值、解算出的 R 和 t 一定要归档，带上时间戳和姿态备注。等到动态标定或者后续排查问题时，这些归档数据能帮你快速定位是标定漂了还是安装松了，省下的排查时间远超当时多花的那几分钟记录功夫。