4、IMU误差模型与标定：IMU工作原理、加速度计与陀螺仪误差模型（零偏、尺度因子、交轴耦合）、六面法标定

好，咱们今天聊聊IMU的误差模型和标定。说实话，IMU这东西看着简单，就是个测加速度和角速度的传感器，但真要把它用好，误差模型这块必须吃透。我刚开始做车载项目那会儿，就吃过亏——IMU数据直接拿来用，结果定位飘得亲妈都不认识。后来才明白，不标定的IMU就是个“半成品”。

4.1 IMU工作原理：它到底怎么测的？

IMU的核心就两个东西：加速度计和陀螺仪。加速度计测的是比力，说白了就是物体受到的惯性力减去重力。陀螺仪测的是角速度，也就是你转得有多快。

加速度计内部有个质量块，外面连着弹簧。你加速的时候，质量块会动，弹簧的形变转换成电信号，就得到了加速度值。陀螺仪稍微复杂点，MEMS陀螺仪用的是科里奥利效应——一个振动的质量块，你一转它，它就会产生一个垂直于振动方向的位移，测这个位移就知道角速度了。

嗯，这里要注意：加速度计测的不是重力，是比力。静止的时候它测的是重力加速度g，但如果你在自由落体，它测出来是0。这个细节很多新手会搞混。

4.2 误差模型：为什么IMU不准？

理想情况下，IMU输出应该和真实值完全一致。但现实嘛，总得有点“瑕疵”。IMU的误差主要分三类：零偏、尺度因子、交轴耦合。我一个个说。

4.2.1 零偏（Bias）

零偏就是IMU静止时，输出不为0。比如加速度计平放着，按理说z轴应该输出g，x和y轴输出0。但实际测出来，x轴可能有个0.05 m/s²，y轴有个-0.03 m/s²。这就是零偏。

陀螺仪也一样，静止时角速度应该是0，但实际可能有个0.01 rad/s的输出。这个零偏会随时间漂移，温度一变它也变。我在项目中遇到过，IMU刚上电和运行半小时后，零偏能差好几倍。所以标定不是一次性的，最好每次上电都做。

零偏的数学模型很简单：

a_measured = a_true + b_a + noise
ω_measured = ω_true + b_g + noise

其中b_a是加速度计零偏，b_g是陀螺仪零偏。单位分别是m/s²和rad/s。

4.2.2 尺度因子（Scale Factor）

尺度因子就是增益误差。你输入1g的加速度，它输出可能是1.02g；输入2g，输出2.05g。这个比例不对，就是尺度因子问题。

说白了，就是IMU的“刻度”不准。陀螺仪也一样，你转10°/s，它可能输出9.8°/s。这个误差通常用百分比表示，比如2%的尺度因子误差，意味着100°/s的输入会输出102°/s。

数学模型加进来：

a_measured = s_a * a_true + b_a + noise
ω_measured = s_g * ω_true + b_g + noise

s_a和s_g就是尺度因子，理想情况下等于1。实际可能在0.98到1.02之间。

4.2.3 交轴耦合（Cross-axis Sensitivity）

这个稍微绕一点。理想情况下，IMU的三个轴应该是完全正交的，x轴只测x方向的加速度，y轴只测y方向的。但实际制造时，三个轴不可能绝对垂直，总会有点偏差。

比如x轴方向有加速度，y轴也能测到一点。这就是交轴耦合。我见过最夸张的一次，一个低端IMU，x轴加速度有10%串到了y轴上。这种数据做融合，姿态能偏到姥姥家。

完整的误差模型长这样：

a_measured = M_a * a_true + b_a + noise
ω_measured = M_g * ω_true + b_g + noise

其中M是一个3x3的矩阵，包含了尺度因子和交轴耦合：

M = [s_xx, m_xy, m_xz;
     m_yx, s_yy, m_yz;
     m_zx, m_zy, s_zz]

对角线是尺度因子，非对角线是交轴耦合系数。理想情况下，M是单位矩阵。

4.3 六面法标定：最经典的加速度计标定方法

六面法，顾名思义，就是把IMU放在六个不同的姿态下采集数据。这六个姿态分别是：x轴朝上、x轴朝下、y轴朝上、y轴朝下、z轴朝上、z轴朝下。

为什么是这六个？因为每个姿态下，重力在某个轴上的投影是已知的。比如z轴朝上时，理论上加速度计输出应该是[0, 0, g]；z轴朝下时是[0, 0, -g]。利用这些已知值，就能反推出误差参数。

具体步骤是这样的：

1. 把IMU固定在一个水平平台上，确保姿态准确
2. 记录每个姿态下至少10秒的静态数据，取平均
3. 六个姿态的数据凑齐后，用最小二乘法求解

我曾经犯过一个低级错误：用三脚架手动翻转IMU，结果每次翻转后平台没放平，标定出来的参数全是错的。后来我学乖了，用精密转台或者至少用水平仪确认一下。

六面法的数学模型：

对于每个姿态i，有：
a_measured_i = M_a * a_true_i + b_a

其中a_true_i是已知的（重力在三个轴上的投影），a_measured_i是实测值。

写成矩阵形式：
[a_measured_x, a_measured_y, a_measured_z] = M_a * [a_true_x, a_true_y, a_true_z] + b_a

六个姿态，12个方程（每个姿态3个轴），求解12个未知数。

代码实现大概这样：

import numpy as np

# 六个姿态的理论值（假设g=9.81）
g = 9.81
true_acc = np.array([
    [g, 0, 0],   # x朝上
    [-g, 0, 0],  # x朝下
    [0, g, 0],   # y朝上
    [0, -g, 0],  # y朝下
    [0, 0, g],   # z朝上
    [0, 0, -g]   # z朝下
])

# 实测值（示例数据）
meas_acc = np.array([
    [9.85, 0.12, -0.08],
    [-9.78, 0.09, 0.05],
    [0.15, 9.82, -0.10],
    [-0.11, -9.79, 0.07],
    [0.08, -0.06, 9.88],
    [-0.05, 0.04, -9.76]
])

# 构建最小二乘问题
# 这里简化处理，实际需要解12个参数
# 可以用线性最小二乘或非线性优化
# 推荐用scipy.optimize.least_squares

4.4 陀螺仪标定：比加速度计麻烦一点

陀螺仪的六面法不太一样。因为陀螺仪测的是角速度，静止时所有轴输出都应该是0。所以六面法只能标定零偏，标不了尺度因子和交轴耦合。

要标定陀螺仪的尺度因子和交轴耦合，需要转台。让IMU绕每个轴以已知角速度旋转，记录输出，然后解方程。如果没有转台，也可以用地球自转来标定——但地球自转只有15°/h，信号太弱，对低精度IMU基本没用。

我个人的做法是：对于消费级IMU，陀螺仪只标零偏，尺度因子和交轴耦合用出厂参数。对于工业级或战术级IMU，必须上转台做全参数标定。

陀螺仪零偏标定很简单：静止采集30秒数据，取平均就是零偏。注意要扣除地球自转的影响（如果你在赤道附近，地球自转在水平轴上的投影最大，大约15°/h）。

嗯，IMU误差模型和标定这块，说白了就是“知己知彼”。你知道它哪里不准，才能想办法补偿。六面法是最基础的方法，但也是最实用的。我到现在做项目，第一件事就是拿六面法把加速度计标一遍，心里才有底。

下一章咱们聊聊多传感器时空同步，那个更刺激——激光雷达和相机的时间戳对不上，融合出来的点云全是重影。到时候我给你们讲讲我踩过的坑。