3. 矿用防爆IMU的静态标定与零偏补偿

为什么矿用IMU必须先做静态标定

矿用防爆IMU在井下长期承受振动、温升与防爆壳体装配应力，出厂零偏会随安装姿态和温度漂移。直接拿原始角速度/比力积分，静止30秒就可能累积数度的姿态误差。静态多位置标定的核心，是在已知重力矢量与已知静止角速度（为零）的条件下，把IMU的确定性误差分离出来：

• 零偏 b_a、b_g：静止时输出不为零的常值分量；
• 标度因数 k_a、k_g：输出与真实输入的斜率偏差；
• 安装误差/非正交 M：三轴不完全正交及与壳体的对准误差。

标定模型采用一阶线性形式（以加速度计为例，陀螺同理）：

a_meas = M_a · k_a · a_true + b_a + n_a
g_meas = M_g · k_g · ω_true + b_g + n_g

静止时 ω_true = 0，因此陀螺输出直接反映零偏；加速度计则利用“静止时 |a_true| = g”这一约束，通过多组不同姿态下的重力投影方程求解 9 个未知量（3 零偏 + 3 标度 + 3 非正交）。

六位置静态标定的几何布置

把IMU固定在标定台上，依次让三个敏感轴分别朝上、朝下，共 6 个静态位置。每个位置保持完全静止 ≥ 60 s，采样 200 Hz，取中段 30 s 均值作为该位置的观测值。这样每个轴都能获得 +1g 与 −1g 两个激励点，从而分离零偏与标度因数。

标定参数求解与实时补偿实现

下面给出可直接运行的 Python 实现：先用六位置均值求解加速度计零偏与标度因数，再对一段静止数据做补偿，并对比补偿前后的静态漂移（用姿态积分角度漂移量衡量）。数据以教学示例形式给出，参数为假设值，用于演示流程与判读方法。

import numpy as np

G = 9.80665  # 重力加速度 m/s^2

# 教学示例：六位置加速度计均值观测 (m/s^2)，每行一个位置
# 顺序: X+, X-, Y+, Y-, Z+, Z-
a_meas = np.array([
    [ 9.92,  0.18, -0.12],
    [-9.71, -0.15,  0.10],
    [ 0.16,  9.88, -0.09],
    [-0.14, -9.75,  0.11],
    [ 0.13, -0.10,  9.90],
    [-0.11,  0.12, -9.73],
])

# 每轴 +g/-g 两点求零偏与标度
b_a = np.zeros(3)
k_a = np.zeros(3)
for i in range(3):
    plus  = a_meas[2*i,   i]
    minus = a_meas[2*i+1, i]
    b_a[i] = (plus + minus) / 2.0
    k_a[i] = (plus - minus) / (2.0 * G)

print("加速度计零偏 b_a (m/s^2):", np.round(b_a, 4))
print("加速度计标度 k_a       :", np.round(k_a, 4))

# 陀螺零偏：六位置静止均值（示例值，rad/s）
g_meas_static = np.array([0.0042, -0.0031, 0.0027])
b_g = g_meas_static.copy()
print("陀螺零偏 b_g (rad/s)   :", np.round(b_g, 5))

# 实时补偿函数
def compensate(a_raw, g_raw, b_a, k_a, b_g):
    a_c = (a_raw - b_a) / k_a
    g_c = g_raw - b_g
    return a_c, g_c

# 用一段 30 s 静止数据验证（示例：含零偏与噪声）
np.random.seed(0)
dt = 0.005
N = int(30 / dt)
a_raw = np.tile(b_a + k_a * np.array([0, 0, G]), (N, 1)) \
        + np.random.normal(0, 0.02, (N, 3))
g_raw = np.tile(b_g, (N, 1)) + np.random.normal(0, 0.0008, (N, 3))

a_c, g_c = compensate(a_raw, g_raw, b_a, k_a, b_g)

# 姿态积分：仅用陀螺积分，比较补偿前后横滚角漂移
def roll_drift(gyro):
    roll = 0.0
    for w in gyro:
        roll += w[0] * dt
    return abs(np.degrees(roll))

print("补偿前 30s 横滚角漂移: %.3f deg" % roll_drift(g_raw))
print("补偿后 30s 横滚角漂移: %.3f deg" % roll_drift(g_c))
print("补偿后静止比力模长均值: %.4f m/s^2 (应接近 %.4f)" %
      (np.linalg.norm(a_c, axis=1).mean(), G))

标定结果记录与验收