4. IMU数学建模

IMU数学建模是标定、补偿与状态估计之间的桥梁。若只把陀螺仪和加速度计的输出当作“真值”直接积分，姿态会在几十秒内发散；若模型阶次、噪声参数与采样方式不匹配，滤波器也会出现“理论收敛、实际漂移”的假象。本节围绕连续时间模型、离散时间模型、确定性误差、随机误差、状态空间表示与噪声参数定义六个要点展开，目标是建立一套可标定、可离散化、可递推的IMU误差模型。

一、核心原理：从物理量到可估计状态

IMU的原始输出可抽象为“真值 + 确定性误差 + 随机误差”。以三轴陀螺仪为例，连续时间下角速度测量模型可写为：

ω_m(t) = (I + S_g) · ω_true(t) + b_g(t) + n_g(t)
ḃ_g(t) = n_bg(t)

其中 S_g 为标度因数与安装误差矩阵，b_g 为陀螺零偏，n_g 为角度随机游走驱动的白噪声，n_bg 为偏置随机游走噪声。加速度计同理：

a_m(t) = (I + S_a) · a_true(t) + b_a(t) + n_a(t)
ḃ_a(t) = n_ba(t)

确定性误差包括零偏、标度因数误差、非正交/安装误差、非线性与温度漂移中的可重复部分；随机误差包括角度随机游走、速度随机游走、偏置不稳定性、量化噪声等。确定性误差通过标定补偿，随机误差通过状态估计与噪声参数描述。

二、连续时间与离散时间模型

连续时间模型适合描述物理机理，但工程实现必须离散化。设采样周期为 T_s，对陀螺零偏随机游走做一阶保持离散化，可得：

b_g[k+1] = b_g[k] + w_bg[k],  w_bg ~ N(0, Q_bg)
Q_bg = σ_bg² · T_s

角度增量由角速度积分得到：

Δθ[k] = (ω_m[k] - b_g[k]) · T_s
θ[k+1] = θ[k] + Δθ[k]

若采用四元数姿态递推，则需将角增量转换为旋转向量并做指数映射。离散化时要注意两点：一是噪声协方差随 T_s 缩放，白噪声方差为 σ²/T_s，随机游走方差为 σ²·T_s；二是零偏状态在离散递推中保持“随机游走”形式，不能误写成白噪声。

三、状态空间表示与噪声参数定义

将姿态、速度、位置、陀螺零偏、加速度计零偏纳入状态向量，可构建15维误差状态模型：

x = [δθ, δv, δp, b_g, b_a]ᵀ
ẋ = F·x + G·w
z = H·x + v

其中 F 为系统矩阵，包含姿态误差与零偏的耦合项；G 为噪声驱动矩阵；w 为过程噪声；v 为观测噪声。噪声参数定义直接决定滤波器性能，常用参数如下表：

四、案例：静态标定与Allan方差估计

以某MEMS IMU为例，先进行六面静态标定，估计零偏与标度因数；再采集2小时静态数据，计算Allan方差曲线，从斜率-1/2段读取角度随机游走，从斜率+1/2段读取零偏不稳定性。典型处理流程如下：

1. 六面法标定：估计 b_g, b_a, S_g, S_a
2. 静态采集：2 h，采样率 100 Hz
3. 去均值、去趋势
4. Allan方差计算：τ = 1, 2, 4, ..., 2^n
5. 拟合斜率：-1/2 → ARW；+1/2 → BI
6. 写入滤波器 Q 矩阵

若忽略确定性误差，直接估计随机噪声，标定残差会混入零偏与标度因数误差，导致Allan方差曲线在长相关时间被“抬高”，零偏不稳定性被高估。正确顺序是先补偿确定性误差，再估计随机噪声参数。

五、常见误区与复盘

• 误区一：连续模型直接用于离散滤波。 未做离散化，Q矩阵量纲错误，滤波器增益异常。
• 误区二：把零偏当白噪声。 零偏是随机游走状态，若建模为白噪声，姿态误差会快速发散。
• 误区三：Q与R同时放大。 两者同比放大不改变稳态增益，但会改变瞬态响应，需分别标定。
• 误区四：忽略安装误差。 三轴非正交会引入交叉耦合，在动态条件下表现为航向漂移。

综上，IMU数学建模的关键在于：连续时间模型给出物理机理，离散时间模型给出递推形式，确定性误差通过标定消除，随机误差通过噪声参数进入状态空间。只有把“标定—建模—离散化—滤波”串成闭环，姿态与位置估计才能在长时间运行中保持稳定。