航姿解算从器件到算法

航姿解算的核心是「传感器数据→误差补偿→姿态融合算法→输出可靠姿态角」，所有知识都围绕这个链路展开：

1
2
3
4
5
6
7
传感器原始数据                    误差补偿              姿态融合算法           可靠的姿态角
     │                             │                      │                  │
 陀螺仪 ──→ 角速度             零偏、尺度因子、        互补滤波、             Roll (横滚)
 加速度计 ──→ 比力(非引力外力)   温度、对准误差          Mahony /             Pitch (俯仰)
 磁力计 ──→ 地磁场             硬铁/软铁干扰           Madgwick /           Yaw (航向)
 气压计 ──→ 大气压             气流/温度扰动           EKF
 GNSS  ──→ 位置/速度/基线      多径/大气延迟

1. 传感器部分

航姿解算板的核心任务是实时获取载体的姿态（横滚 Roll、俯仰 Pitch、航向 Yaw）和运动信息。所有姿态解算都始于传感器对物理量的原始测量。

传感器总览

传感器 测量物理量 输出形式 在姿态解算中的作用
三轴陀螺仪 角速度 $\boldsymbol{\omega} = [\omega_x, \omega_y, \omega_z]^T$ 数字量 (dps 或 rad/s) 短时高动态姿态变化的主心骨
三轴加速度计 比力 $\boldsymbol{f} = [f_x, f_y, f_z]^T = \boldsymbol{a} - \boldsymbol{g}$ 数字量 (g) 静态/准静态时提供水平基准（Roll/Pitch）
三轴磁力计 地磁场强度 $\boldsymbol{m} = [m_x, m_y, m_z]^T$ 数字量 (μT) 提供绝对航向参考（Yaw）
气压计 大气压 $P$ 数字量 (Pa) 换算高度，辅助垂直定位
GNSS（双天线） 全局位置、速度、基线矢量、精确时间 数字量（NMEA-0183 / 经纬高、速度、航向角、PPS 秒脉冲） 全局绝对位置与速度基准；不依赖地磁的绝对航向；多传感器时间同步的基准时钟

传感器常用单位速查：

物理量 常用单位 换算关系 直觉参考
角速度 dps (°/s) 1 rad/s ≈ 57.3 °/s 人手缓慢转动约 30°/s；无人机快速翻转可达 500°/s
rad/s 1 °/s ≈ 0.0175 rad/s 工程公式中常用 rad/s，Datasheet 常用 °/s
比力/加速度 g 1 g ≈ 9.8 m/s² 静止时加速度计 Z 轴读 1g（支持力）
m/s² 1 g ≈ 9.8 m/s² SI 国际单位，公式推导中常用
mg 1 mg = 0.001 g ≈ 0.0098 m/s² 描述加计零偏：20 mg ≈ 0.2 m/s²
磁场强度 μT (微特斯拉) 1 μT = 10⁻⁶ T 地磁场约 25~65 μT（赤道弱、极地强）
Gauss (G) 1 G = 100 μT 部分旧文献用 Gauss
气压 Pa (帕斯卡) 1 hPa = 100 Pa 海平面标准大气压 ≈ 101325 Pa ≈ 1013 hPa
hPa / mbar 1 hPa = 1 mbar = 100 Pa 气象常用单位
噪声密度 mdps/√Hz — 陀螺噪声密度：ICM-42688-P = 2.8 mdps/√Hz（即 0.0028 °/s/√Hz）
μg/√Hz — 加计噪声密度：ICM-42688-P ≈ 70 μg/√Hz
零偏稳定性 °/hr — 陀螺零偏漂移速率：消费级 >50°/hr，战术级 1~50°/hr
μg — 加计零偏稳定性：消费级 >1 mg，战术级 0.05~1 mg

关键换算口诀：

  • ARW（角随机游走）°/√hr = 60 × 噪声密度（°/s/√Hz）——例 (ICM42688P)：2.8 mdps/√Hz → 0.168 °/√hr
  • 1 g ≈ 9.8 m/s²，mg = 千分之一 g
  • 气压→高度：近地面每升高 1 m 气压约降 12 Pa（§1.3 详述）

1.1 最小基础：三轴陀螺仪 + 三轴加速度计（6 轴 IMU）

要计算载体在三维空间中的水平姿态（横滚角和俯仰角），理论上只需要两个信息源。若要获得完整的三维姿态（含航向），则还需引入磁力计或 GNSS。

陀螺仪：测量绕三个轴的角速度（$\omega_x, \omega_y, \omega_z$）。通过对角速度积分，可以得到角度变化。陀螺仪动态响应快、不受加速度和磁场干扰，但存在积分漂移——随时间推移，误差不断累积，导致姿态角越来越不准。

加速度计：测量作用在载体上的比力（$\boldsymbol{f} = \boldsymbol{a} - \boldsymbol{g}$，即运动加速度减去重力加速度的矢量差）。比力是传感器感受到的非引力外力——当载体静止在桌面上时（绝对加速度 $\boldsymbol{a} = \boldsymbol{0}$），在 NED 坐标系下重力矢量 $\boldsymbol{g}^n = [0, 0, +g]^T$（Z 轴向下），加速度计测到的比力 $\boldsymbol{f}^n = \boldsymbol{0} - \boldsymbol{g}^n = [0, 0, -g]^T$（Z 轴读数为负，方向向上，即桌面的支持力）。因此当载体静止或匀速运动（$\boldsymbol{a} \approx \boldsymbol{0}$）时，加速度计测量的就是重力矢量的反号，从而可以计算出水平姿态角（横滚 $\phi$、俯仰 $\theta$）。加速度计不产生漂移，但动态下会被运动加速度干扰。

二者互补：陀螺仪提供短时高动态精度，加速度计提供长期绝对水平基准。没有陀螺仪，无法测量快速旋转；没有加速度计，陀螺仪的漂移无法被修正。因此，一个 6 轴 IMU（三轴陀螺仪 + 三轴加速度计）是航姿解算的最小硬件基础。

注意：加速度计不能直接解算位置。它是通过比力测量，在已知姿态后对运动加速度（$\boldsymbol{a} = \boldsymbol{f} + \boldsymbol{g}$）双重积分才能推算相对位置，强依赖姿态信息且误差发散极快。因此，加速度计的主要作用是提供重力矢量参考，其次才是辅助解算相对位移。

多 IMU 异构冗余设计

虽然三轴陀螺仪 + 三轴加速度计（6 轴 IMU）已经能组成姿态解算的最小单元，但在高性能航姿解算板中，仅依赖单颗 IMU 存在以下风险：

  • 单点故障：一颗 IMU 损坏或数据异常，整个姿态解算崩溃
  • 单一场景局限：不同 IMU 的性能优势不同，单颗无法兼顾所有工况
  • 振动场景：某些 IMU 对高频机械振动抑制能力强，但功耗较高；低功耗 IMU 振动抑制能力相对弱

因此，本设计采用三颗 IMU 异构冗余方案，各司其职：

型号 定位 典型电流 (6 轴 LN) 陀螺噪声密度 陀螺零偏稳定性 核心优势
ICM-42688-P 主 IMU 0.88 mA 2.8 mdps/√Hz 2°/hr 超低噪声、低功耗，飞控成熟方案
IIM-42652 工业基准 0.67 mA ~4 mdps/√Hz ~4°/hr 工业级宽温（-40~105°C），全温度范围稳定
BMI088 抗震 IMU 5.15 mA 14 mdps/√Hz <2°/hr（估） 分体式设计，振动抑制极强，零偏稳定性优

三异构全融合 + 三选二容错策略：

  • 正常工况（三异构全融合）：三颗 IMU 全部参与加权融合——ICM-42688-P 权重最高（噪声最低、功耗低），IIM-42652 次之（工业级稳定性），BMI088 在平稳环境下权重较低（噪声密度偏高）
  • 高振动工况：BMI088 权重自动提升（分体式架构对振动抑制最强），ICM/IIM 权重降低
  • 故障隔离（三选二降级）：当某颗 IMU 输出与另外两颗的偏差超过阈值，或 IMU 内部自检报错时，故障检测机制自动隔离该传感器，由剩余两颗继续融合
  • 单点容错：三选二架构能容忍任意单颗 IMU 故障，保证姿态解算不中断

三异构 vs 三选二：正常时三颗都用，按噪声特性和工况加权融合——这是”三异构全融合”的优势，充分利用异构互补；故障时自动降级为两颗——这是”三选二容错”的兜底。两者不是对立关系，而是正常→异常的自适应降级。

没有选完全一样的三颗 IMU，因为同构冗余只能防单点故障，无法解决”某种工况下单颗 IMU 性能不足”的问题。异构冗余兼顾了容错性和多工况适应性。

6 轴 IMU 的局限性

由于陀螺仪存在积分漂移、加速度计噪声大且无法感知航向变化，一般认为：

  • 陀螺仪 + 加速度计（6 轴）仅能获取较为稳定的横滚和俯仰信息
  • 航向角完全依赖陀螺积分，典型漂移 1~10°/min，不可接受

要获得完整的、无漂移的三维姿态，至少需要陀螺仪 + 加速度计 + 磁力计（9 轴）。

1.2 增加磁力计：9 轴 AHRS

磁力计测量地磁场强度在载体三轴上的分量（$m_x, m_y, m_z$）。地磁场（近似为地球的偶极场）在水平面上指向磁北。通过磁力计数据，可以计算出绝对航向角（$\psi$），也称磁航向。

加上磁力计后的效果

无磁力计（6 轴） 有磁力计（9 轴）
Roll / Pitch ✅ 可靠（加计修正陀螺漂移） ✅ 可靠
Yaw ❌ 纯陀螺积分，漂移 1~10°/min ✅ 地磁场修正，长期无漂移
适用场景 仅需水平姿态的简单应用 无人机、机器人、船舶等需绝对方向的应用

9 轴 AHRS 是绝大多数无人机、机器人、船舶的标准配置。

磁力计的脆弱性

磁力计是所有传感器中最容易受干扰的：

  • 硬铁干扰（Hard Iron）：载体上的铁磁材料（螺钉、电机外壳）产生固定偏移，使磁力计输出中心偏移
  • 软铁干扰（Soft Iron）：载体上的导磁材料（钢板、铝框靠近铁磁体时）使地磁场发生畸变，表现为椭球变形
  • 时变干扰：大电流线路（电源线、电机驱动线）产生的磁场随电流变化，无法静态校准
  • 高纬度失效：在地磁极附近，地磁场水平分量趋近于零，磁航向精度急剧下降

应对措施：

  1. 椭球校准：将磁力计原始数据从偏移椭球映射回标准球体，消除硬铁和软铁影响
  2. 磁场异常检测：当测量磁场模长偏离本地地磁场值（25~65 μT 范围）时，暂时禁用磁修正
  3. 物理隔离：磁力计尽量远离电机、大电流线路，必要时使用外置磁力计模块（通过 I2C 延长线远距安装）

磁航向 vs 真航向（磁偏角问题）

磁力计解算出的是磁北航向——指向地磁北极的方向。而 GNSS 双天线解算出的是真北航向——指向地理北极的方向。两者之间存在的偏差称为磁偏角（Magnetic Declination），且随地理位置变化：

  • 在中国，磁偏角范围约 -11°（新疆西端）~ +6°（东北东端）
  • 在北美，磁偏角范围可达 -20°~+20°
  • 磁偏角随时间缓慢变化，需定期更新

在多传感器融合中必须统一参考方向：要么将磁力计的磁航向通过磁偏角查表（WMM2020 世界磁场模型）补偿为真航向，要么将 GNSS 的真航向转换为磁航向。本设计中，UM982 双天线直接输出真北航向，因此需要将磁力计航向补偿为真航向后再送入融合算法。

双磁力计冗余设计

本设计选用两颗磁力计（RM3100 + IST8310），其冗余逻辑不同于多 IMU 的”异构分工”，而是主辅校验：

型号 角色 技术 接口 典型功耗 核心优势
RM3100 主磁力计 AMR SPI5 70~260 μA（Cycle Count 50~200） 高精度、低噪声、无零偏漂移
IST8310 辅助磁力计 AMR I2C3 ~1 mA (连续) 体积小、集成度高、使用简单

冗余价值：

  • 当检测到磁场模长异常时，对比两颗磁力计输出：若两颗同时异常 → 真实磁场变化或共同干扰；若仅一颗异常 → 该传感器可能故障或受局部干扰，可切换到另一颗
  • RM3100 精度更高作为主传感器；IST8310 作为辅助校验和故障备份
  • 提高磁修正的鲁棒性，避免单颗磁力计异常导致航向角突变

为什么不像 IMU 那样做”三取二”？ 磁力计的误差主要来自外部干扰而非传感器本身故障，多颗磁力计在同一位置受相同干扰，”三取二”投票无法区分干扰和真实磁场。因此磁力计冗余的核心是异常检测+切换，而非投票融合。

1.3 增加气压计

气压计测量大气压 $P$，根据大气压随高度递减的规律，可以计算出绝对海拔高度。

压高公式

其中 $R$ 为气体常数，$T$ 为绝对温度，$M$ 为空气摩尔质量，$g$ 为重力加速度。

注意：上式为等温大气近似（假设温度 $T$ 恒定），实际大气温度随高度变化。工程应用中通常使用标准大气模型或查表修正。气压高度的相对精度约 0.1~1 m，绝对精度受天气影响约 ±10 m。

加上气压计后的效果

无气压计 有气压计
高度 ❌ 加速度计二次积分，几秒内发散 ✅ 稳定绝对高度，精度 0.1~1 m
垂直速度 ❌ 不可用 ✅ 气压变化率 → 垂直速度
典型应用 — 无人机定高、楼层检测、户外高度辅助

气压计的干扰与防护

气压计虽不像磁力计那样容易被电磁干扰，但有其独特的干扰源：

  • 旋翼下洗气流：无人机旋翼产生的气流使气压计读数剧烈波动
  • 阵风：户外突风导致气压瞬变
  • 温度梯度：传感器自身发热或环境温度变化影响测量

应对措施：

  1. 硬件防护：气压计上方覆盖防风海绵（Gore-Tex 材质），允许气体缓慢扩散但隔绝快速气流
  2. 算法滤波：与加速度计 Z 轴数据进行互补滤波或卡尔曼滤波，气压提供低频基准、加计提供高频动态

双气压计设计

型号 角色 相对精度 典型功耗 尺寸 状态
BMP581 主气压计 ±6 Pa（±0.5 m） 1.3 μA (1Hz) 2.0x2.0x0.75mm ✅ 量产，当前飞控首选
ICP-20100 辅助气压计 ±1 Pa （±0.083 m） 2.65 μA (1Hz) 2.0x2.0x0.8mm ⚠️ EOL 停产
DPS310 备选 ±6 Pa （±0.5 m） 1.7 μA (1Hz) 2.0x2.5x1.0mm ✅ 量产
MS5611 备选 ± 150Pa （±12.5 m） 1 μA 5.0x3.0x1.0mm ✅ 量产

⚠️ ICP-20100 已于 2025-12-11 发布停产通知（Product Discontinuation Notification），新设计不建议选用。如需双气压计冗余，可考虑 DPS310（Infineon）或 MS5611（TE）作为替代。
为了方便对比，我只列举了气压计的相对精度，并未对高精度模式，绝对精度等参数进行对比。
一般来说，BMP581 单颗足以满足需求，具体情况具体分析吧。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
title: 补充知识点：气压单位换算关系
icon: fire
**基础换算：**

$$1\ \mathrm{hPa}=1\ \mathrm{mbar}=100\ \mathrm{Pa}$$

$$1\ \mathrm{kPa}=10\ \mathrm{hPa}=1000\ \mathrm{Pa}$$

气压→高度经验公式（低空近地面，国际标准大气ISA）
> 近地近似：**每升高1m，气压约下降 12 Pa**（常温海平面标准梯度，气压高度解算）

$$
\Delta H(\mathrm{m}) \approx \frac{\Delta P(\mathrm{Pa})}{12}
\quad\Rightarrow\quad
\Delta P \approx 12\cdot \Delta H
$$

对于相对精度±6 Pa 的气压计，其换算成米的方式为： $$6\div12=\boldsymbol{\pm0.5\ \mathrm{m}}$$

1.4 增加双天线 RTK-GNSS 模块（UM982）

UM982 是一款全系统全频点高精度定位定向模块，基于和芯星通 NebulasIV SoC 芯片设计。它利用双天线接收 GNSS 信号（GPS、北斗、GLONASS、Galileo、QZSS），可以实现：

  • RTK 厘米级定位：实时动态差分，定位精度达 1 cm + 1 ppm
  • 双天线定向：通过两个天线接收载波相位差，直接解算载体绝对航向角（不依赖地磁场）

加上 UM982 后的层级提升

无 GNSS 单天线 GNSS 双天线 GNSS
位置 ❌ 无 ✅ 米级~厘米级（RTK） ✅ 厘米级（RTK）
速度 ❌ 无 ✅ 多普勒测速 ✅ 多普勒测速
航向 Yaw 磁力计（受干扰） 运动方向角 COG（低速不可靠） ✅ 双天线定向，不受磁干扰
对磁力计依赖 必须 仍依赖（低速/静止时） 不依赖

双天线定向的核心价值：航向角完全独立于地磁场，不受铁磁干扰、高纬度失效影响。这是目前最高可靠性的航向解决方案。

UM982 电气特性

参数 数值
VCC（模块主电） 3.3V（3.0~3.6V），典型 180 mA，最大 300 mA
LNA_PWR（天线馈电） 5V，每路天线最大 ~100 mA
工作温度 -40°C ~ +85°C
接口 3×UART (LVTTL), SPI 从, I2C, PPS, EVENT
模块尺寸 16 mm × 21 mm

PPS 时间同步——多传感器融合的”节拍器”

UM982 输出的 PPS（Pulse Per Second）秒脉冲是整个航姿系统的关键时间基准：

  • 问题：IMU 以 1~8kHz 高频输出，GNSS 以 1~20Hz 低频输出，气压计以 10~50Hz 输出。如果没有统一的时间戳，EKF 融合时无法确定”这条 GNSS 观测量对应哪个时刻的 IMU 积分结果”，时间错位直接导致融合发散。
  • 解决方案：UM982 的 PPS 脉冲上升沿与 UTC 秒对齐（精度达 ns 级）。MCU 用 PPS 中断同步本地时钟，再给每个 IMU/气压计采样打上精确时间戳，实现多传感器时间对齐。
  • EVENT 引脚：支持外部事件打标（如相机曝光时刻），可用于事后对齐图像与姿态数据。

VCC 与天线馈电必须分开供电。VCC 是 3.3V 供模块内部基带和射频，LNA_PWR 是 5V 供外部有源天线。若天线短路导致馈电电源拉垮，VCC 不受影响，模块仍能正常工作。

天线馈电防短路设计

有源 GNSS 天线内部有低噪声放大器（LNA），需要通过同轴电缆的芯线馈电。天线安装在户外，存在进水、电缆破损导致短路的风险。

防短路方案：使用限流开关 MT9700-N（西安航天民芯），当天线电流超过设定阈值（推荐 >100 mA）时，2μs 内切断输出，保护上游电源，同时 FLAG 引脚通知 MCU 天线故障。

1
2
3
4
5
5V电源 → [MT9700-N 限流开关] → 馈电电感(68nH) → 天线 LNA
                │                        │
         >100mA触发保护            [ESD: PESD5V0F1BL, 0.4pF]
                │                        │
           FLAG → MCU中断          [TVS: 浪涌防护]

关键器件说明：

器件 功能 关键参数
MT9700-N 限流保护开关，天线短路时自动断开 SOT-23-5，80mΩ，RSET 可调限流阈值
PESD5V0F1BL 射频口 ESD 防护，电容仅 0.4pF 对射频透明 DFN1006-2，10kV，AEC-Q101
68nH 馈电电感 DC 馈电通路，1.5GHz 下阻抗 ~640Ω 隔离射频 0603 射频电感，SRF > 2GHz
100pF 隔直电容 射频信号通路，隔断 DC 馈电 RF 通路阻抗 ~1Ω @1.5GHz

偏置馈电（Bias Tee）原理：同轴电缆上射频信号和 DC 电源共存——电感”通直流、阻交流”（DC 馈电通过，射频被阻断），电容”通交流、阻直流”（射频信号通过，DC 被隔断），两者在分叉点自然分离，互不干扰。

UM982 VCC 上电时序约束

根据 UM982 官方手册，模块上电有严格要求：

约束 要求
VCC 起始电平 < 0.4V（上电前必须充分放电）
上电单调性 必须单调上升，下冲和振铃 < 5% VCC
爬升时间（10%~90%） 100 μs ~ 1 ms
VCC 滤波电容 总容值 > 30 μF（大小容值搭配）
底部散热焊盘 35 个 GND 焊盘必须接大面积地平面

LDO 选型注意：按领导要求，实际电流 ≤ LDO 额定电流的 1/3。UM982 Imax = 300 mA，需要 LDO ≥ 900 mA。推荐 LT1963A-3.3（1.5A，裕量 5×）或 TPS7A7002（3A 可调设 3.3V，裕量 10×）。

1.5 传感器组合总结

传感器组合 可获得的角度 可获得的位置/高度 航向漂移 对外部条件依赖
6 轴 IMU Roll, Pitch 无 航向漂移严重（1~10°/min） 无
+磁力计（9 轴 AHRS） Roll, Pitch, Yaw 无 无漂移（但受磁干扰） 地磁场可用、无强磁干扰
+气压计 同上 绝对高度 同上 大气稳定
+双天线 GNSS 无磁依赖的 Yaw 厘米级经纬度 + 高度 无 GNSS 信号可用

本设计完整配置：

1
2
3
4
5
6
7
8
9
10
11
12
13
ICM-42688-P（主IMU）     ─┐
IIM-42652（工业基准）      ├── 异构冗余 → 高可靠角速度 + 比力
BMI088（抗震IMU）        ─┘
                        │
	                    ─┐
RM3100（主磁力计）         ├── 主辅校验冗余 → 鲁棒地磁航向
IST8310（辅助磁力计）     ─┘
                        │
                        ─┐
BMP581（主气压计）         ├── 绝对高度 + 垂直速度
DPS310 （备选）          ─┘
                        │
UM982（双天线RTK-GNSS）   ── 厘米级定位 + 不依赖地磁的绝对航向

→ 获得无漂移的绝对姿态、厘米级定位、不受磁干扰的航向、稳定的高度。适用于高性能无人机、自动驾驶、机器人导航。

最低可用配置：若预算或功耗受限，至少保留 6 轴 IMU + 磁力计（9 轴 AHRS），气压计和双天线 GNSS 可根据应用需求选配。

2. 误差补偿

2.1 为什么需要误差补偿？

理想传感器只存在于教科书。真实的传感器输出永远不是物理量的精确复现：

确定性误差（Deterministic Errors）：每次上电都存在、有一定规律可循的偏差。就像一把刻度偏了 0.5mm 的尺子——每次量都偏，但偏差是固定的，可以通过标定消除。

随机误差（Stochastic Errors）：无法预测的随机波动。就像尺子刻度边缘有毛刺——每次读数都有微小偏差，不可标定消除，只能通过统计建模和滤波抑制。

不补偿的后果链：

1
2
3
4
5
6
7
8
陀螺零偏 0.5°/s（未补偿）
  → 1秒后姿态角偏 0.5°
  → 1分钟后偏 30°
  → 飞控以为水平实际已严重倾斜 → 坠机

加速度计零偏 20mg（未补偿）
  → 静态水平角计算偏 ~1.1°
  → 飞控持续补偿这个"不存在的倾斜" → 姿态振荡

因此，误差补偿是准确姿态解算的基石——没有补偿，再好的融合算法也只能在错误数据上做文章。

2.2 IMU 误差全谱（陀螺仪 + 加速度计）

若将 IMU 的误差按照确定与随机的差异分类则可以表示为：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
IMU 误差
├─ 一、确定性误差（可标定补偿）
│   ├─ 1. 零偏类误差
│   │   ├─ 静态/常值零偏（Static Bias）
│   │   ├─ 零偏不对称性（Bias Asymmetry）
│   │   └─ 上电重复性（Run-to-run Repeatability）
│   ├─ 2. 刻度因子类误差
│   │   ├─ 线性刻度误差（Scale Factor Error）
│   │   ├─ 非线性误差（Non-linearity）
│   │   └─ 刻度因子不对称性（Scale Factor Asymmetry）
│   ├─ 3. 几何与安装失准类误差
│   │   ├─ 轴内非正交误差（Cross-axis Coupling）
│   │   └─ 轴间失准角（Inter-triad Misalignment）
│   └─ 4. 温度敏感类误差
│       ├─ 温度零偏漂移（Thermal Bias Drift）
│       └─ 温度刻度因子漂移（Thermal Scale Factor Drift）
│
└─ 二、随机性误差（只能统计建模、滤波抑制）
    ├─ 1. 量化噪声（Quantization Noise）
    ├─ 2. 角/速度随机游走（ARW / VRW）
    ├─ 3. 零偏不稳定性（Bias Instability）
    ├─ 4. 速率/加速度随机游走（RRW / AccRW）
    └─ 5. 速率/速度斜坡（Rate/Velocity Ramp）

陀螺仪和加速度计的误差结构完全对应，只是单位不同（陀螺: °/s, °/hr; 加速度计: g, μg）。下文以陀螺仪为主讲解，加速度计对应项在括号中标注。

2.2.1 确定性误差

① 零偏类误差（Bias Errors）

静态/常值零偏（Static Bias）

  • 是什么：传感器在零输入（完全静止、无角速度/无比力）时，输出不为零。记为 $b_0$。
  • 物理原因：MEMS 传感器的机械结构在制造时存在微小不对称（如差分电容的极板间距不完全相等），导致零输入时仍有差分输出。
  • 数学表达：$\omega_{\text{meas}} = \omega_{\text{true}} + b_0$
  • 不补偿的后果：陀螺零偏积分后直接导致姿态角线性漂移。$b_0 = 0.5°/s$ → 1 分钟漂移 30°。
  • 补偿方法：上电后静止采集 N 个样本取平均，从后续输出中减去。或六面法标定。

零偏不对称性（Bias Asymmetry）

  • 是什么：正方向输入和负方向输入时，零偏不同。
  • 物理原因：MEMS 悬臂梁结构的推拉不对称（驱动方向和回复方向的弹性系数有微小差异）。
  • 数学表达：$b_0^+ \neq b_0^-$（正转和反转时零偏不同）
  • 不补偿的后果：在正反交替运动时，积分漂移会呈现非对称累积。
  • 补偿方法：正反两个方向分别标定零偏，根据运动方向选用对应零偏值。一般消费级 IMU 此项较小，可忽略。

上电重复性（Run-to-run Repeatability）

  • 是什么：每次上电，零偏值不完全相同，在一个范围内波动。
  • 物理原因：热应力释放、封装应力变化导致每次上电时 MEMS 结构的初始状态略有不同。
  • 数学表达：$b_0^{(k)} = b_0 + \Delta b_k$，其中 $\Delta b_k$ 每次上电不同
  • 不补偿的后果：上次标定的零偏值这次不一定适用，残留零偏仍会导致漂移。
  • 补偿方法：每次上电后重新做零偏标定（静止初始化）；或在融合算法中将零偏作为状态量在线估计。

本设计 IMU 的零偏参数：

参数 ICM-42688-P IIM-42652 BMI088
陀螺初始零偏 ±0.5 °/s ±0.5 °/s ±1 °/s
加计初始零偏 — — ±20 mg
陀螺零偏温度系数 ±0.005 °/s/°C ±0.02 °/s/°C ±0.015 °/s/K
加计零偏温度系数 — — ±0.2 mg/K
② 刻度因子类误差（Scale Factor Errors）

线性刻度误差（Scale Factor Error）

  • 是什么：传感器的实际灵敏度与标称值不同。标称 16.4 LSB/(°/s)，实际可能是 16.48 LSB/(°/s)。
  • 物理原因：MEMS 结构的弹性系数、电容间隙等加工偏差，导致实际灵敏度偏离设计值。
  • 数学表达：$\omega_{\text{meas}} = (1 + s) \cdot \omega_{\text{true}}$，其中 $s$ 为刻度因子误差（典型 0.1%~1%）
  • 不补偿的后果：运动角速度被”放大”或”缩小”。$s = 0.5\%$ 时，200°/s 的旋转被测为 201°/s，积分后姿态角误差随运动量线性增长。
  • 补偿方法：六面法（加速度计）或转台标定（陀螺仪），测量实际灵敏度后修正。

非线性误差（Non-linearity）

  • 是什么：刻度因子在不同输入幅度下不是常数，大角速度时灵敏度发生变化。
  • 物理原因：MEMS 悬臂梁在大位移时弹簧特性偏离胡克定律；电容检测在大间隙时非线性。
  • 数学表达：$\omega_{\text{meas}} = (1 + s_0) \cdot \omega_{\text{true}} + s_1 \cdot \omega_{\text{true}}^2 + \cdots$
  • 不补偿的后果：在剧烈机动（大角速度/大加速度）时引入额外误差。对于飞控，快速翻转时姿态精度下降。
  • 补偿方法：多点标定（不同输入幅度），拟合非线性曲线修正。多数 MEMS IMU 非线性 <0.1%，一般应用可忽略。

刻度因子不对称性（Scale Factor Asymmetry）

  • 是什么：正方向和负方向输入的刻度因子不同。
  • 物理原因：与零偏不对称性类似，MEMS 结构推拉不对称。
  • 数学表达：$(1+s^+) \neq (1+s^-)$
  • 补偿方法：正反方向分别标定刻度因子。消费级通常可忽略。

本设计 IMU 的刻度因子参数：

参数 ICM-42688-P IIM-42652 BMI088
刻度因子初始误差 ±0.5% ±0.5% —
刻度因子温度系数 ±0.005%/°C ±0.005%/°C —
非线性 ±0.1% ±0.1% —
③ 几何与安装失准类误差（Alignment Errors）

轴内非正交误差 / 交叉耦合（Cross-axis Coupling）

  • 是什么：理想情况下三轴传感器两两正交（90°），实际加工中三轴不完全正交，导致一个轴的输入会耦合到另一个轴的输出上。
  • 物理原因：MEMS 芯片上三个传感元件的安装方向有微小偏差（加工精度限制）。
  • 数学表达：

其中 $m_{ij}$ 为交叉耦合系数（非对角元素），典型值 0.1%~2%。

  • 不补偿的后果：三轴数据互相”串扰”。比如 X 轴旋转时 Z 轴也有输出，姿态解算的各轴不再独立。
  • 补偿方法：六面法标定可同时求出 9 个矩阵元素（含刻度因子和对角项），构建修正矩阵。

轴间失准角（Inter-triad Misalignment）

  • 是什么：陀螺仪三轴坐标系和加速度计三轴坐标系之间不完全对齐。
  • 物理原因：MEMS IMU 中陀螺和加计是独立传感单元，虽然封装在同一芯片内，但存在微小的安装角度偏差；更显著的是 PCB 焊接时的旋转偏差。
  • 数学表达：$\omega_{\text{body}} = R_{\text{align}} \cdot \omega_{\text{gyro}}$，其中 $R_{\text{align}}$ 是一个小角度旋转矩阵
  • 不补偿的后果：陀螺积分出的姿态旋转矩阵与加计测量的重力矢量不在同一坐标系下，融合算法出现系统性偏差。
  • 补偿方法：多位置标定，通过比对陀螺和加计的坐标系关系求解 $R_{\text{align}}$。

本设计 IMU 的交叉耦合参数：

参数 ICM-42688-P IIM-42652 BMI088
交叉耦合 ±1.25% ±1.25% —
④ 温度敏感类误差（Temperature-dependent Errors）

温度零偏漂移（Thermal Bias Drift）

  • 是什么：零偏随温度变化而漂移。温度每变化 1°C，零偏变化一定量。
  • 物理原因：MEMS 材料的热膨胀导致机械结构变形，弹性系数随温度变化，电容间隙改变。
  • 数学表达：$b(T) = b_0 + \text{TCO} \cdot (T - T_0)$，其中 TCO 为温度系数
  • 不补偿的后果：设备从室温 25°C 拿到户外 0°C，温度变化 25°C → 零偏变化 TCO × 25。例如 BMI088 陀螺 TCO = 0.015 °/s/K → 25°C 温变导致 0.375°/s 的额外零偏 → 1 分钟漂移 22.5°！
  • 补偿方法：
    1. 温度补偿表：在温箱中不同温度点标定零偏，绘制温度-零偏曲线，运行时查表补偿
    2. IIM-42652 特殊优势：工业级宽温（-40~105°C），且内置温度传感器，支持片上温度补偿算法

温度刻度因子漂移（Thermal Scale Factor Drift）

  • 是什么：刻度因子也随温度变化。
  • 物理原因：与温度零偏漂移类似，热膨胀改变了传感器的灵敏度。
  • 数学表达：$s(T) = s_0 + \text{TCs} \cdot (T - T_0)$
  • 补偿方法：同温度零偏漂移，温度补偿表或片上补偿。

温度补偿的重要性：在无人机应用中，电机和电子元件发热导致板上温度可在几分钟内变化 20~40°C。如果不做温度补偿，即使上电时零偏标定得再准，飞行中零偏也会漂移。这是多 IMU 冗余的核心价值之一——IIM-42652 在宽温范围内的 TCO 虽然数值上更大（0.02 °/s/°C），但它的 -40~105°C 全温区都经过工业级校验；而 ICM-42688-P 的 0~70°C 温区外性能不保证。

2.2.2 随机性误差

确定性误差可以通过标定”减掉”，但随机误差不可预测、无法逐次消除。它们决定了传感器长期精度的天花板。

随机误差的分析工具是 Allan 方差——将传感器静止采集的长时间数据按不同时间窗口分组求方差，在 log-log 图上不同斜率对应不同噪声类型：

Allan 图斜率 噪声类型 陀螺 加速度计
-1 量化噪声 量化噪声 量化噪声
-1/2 白噪声积分 角随机游走 ARW 速度随机游走 VRW
0 闪烁噪声 零偏不稳定性 BI 零偏不稳定性 BI
+1/2 随机游走 速率随机游走 RRW 加速度随机游走 AccRW
+1 趋势发散 速率斜坡 速度斜坡
① 量化噪声（Quantization Noise, QN）
  • 是什么：ADC 将模拟信号转换为数字量时的舍入误差。
  • 物理原因：ADC 位数有限。16 位 ADC 在 ±2000°/s 量程下，LSB = 4000/65536 ≈ 0.061°/s，低于此值的角速度变化无法分辨。
  • 数学表达：$Q = \frac{\text{LSB}}{\sqrt{12}}$（均匀分布量化噪声的 RMS）
  • Allan 图：斜率 -1
  • 影响：对 MEMS IMU 通常较小（16~20 位 ADC 已足够），不是主要误差源
  • 补偿：无法补偿，但可通过提高 ADC 位数或减小量程来降低
② 角随机游走 / 速度随机游走（ARW / VRW）
  • 是什么：陀螺白噪声积分后导致的角度随机发散。这是决定短中期精度的最关键参数。
  • 物理原因：MEMS 传感器内部的热噪声（Brownian 运动）、电子学噪声等，表现为输出信号中的高频随机抖动。
  • 数学表达：陀螺白噪声 $\sigma_g$（°/s/√Hz）积分时间 $t$ 后，角度不确定度：

ARW 的单位为 °/√hr，与噪声密度的关系：

  • Allan 图：斜率 -1/2，读 τ=1s 处的值
  • 不补偿的后果：无法补偿！ 这是白噪声，不可标定、不可预测。ARW = 0.17°/√hr 意味着纯惯性导航 1 小时后，仅白噪声就导致 ±0.17° 的角度不确定度（1σ）。
  • 应对：选择低噪声密度的传感器；在融合算法中用加速度计/磁力计的绝对基准持续修正。

类比：ARW 就像你蒙眼走路——每一步有微小的随机偏差，虽然每步很小，但走久了你就不知道自己在哪了。你没法预测下一步偏哪，但可以时不时睁眼看一下路标（加速度计/磁力计修正）来纠正。

③ 零偏不稳定性 / 偏置不稳定性（Bias Instability, BI）
  • 是什么：零偏在运行过程中缓慢、随机地波动。这是评价 IMU 等级的核心指标。
  • 物理原因：1/f 闪烁噪声（Flicker Noise）——低频段噪声功率谱密度与频率成反比，导致零偏在分钟~小时时间尺度上缓慢漂移。
  • 数学表达：BI 单位为 °/hr（陀螺）或 μg（加计），表示零偏在某个时间尺度上的波动范围
  • Allan 图：斜率 0（曲线最低点的纵坐标即为 BI 值）
  • 不补偿的后果：零偏不稳定性是无法通过单次标定完全消除的。即使上电时标定了零偏，运行几分钟后零偏就已经漂走了。BI = 2°/hr 意味着零偏可能在 ±2°/hr 范围内缓慢变化。
  • 补偿方法：在融合算法（如 EKF）中将陀螺零偏作为状态量在线估计，用加速度计和磁力计的绝对基准持续修正。

为什么 BI 是 IMU 等级的”分水岭”？

等级 定位误差 (纯惯性 1h) 陀螺零偏 加速度计零偏 典型器件 主要应用
战略级 < 30 m/hr < 0.001 °/h ~1 µg RLG / FOG + 石英加计 核潜艇、洲际导弹
导航级 0.5 – 2 nmi/hr ~0.015 °/h
约是地球自转的千分之一		 50 – 100 µg RLG / FOG 系统
（HG9900、Safran）		 航空、航海、高精度测绘
战术级 10 – 20 nmi/hr
(纯惯性短时可用)		 0.1 – 10 °/h 100 – 1000 µg 高端 MEMS / 小型 FOG
（ADIS16470约为 8°/h）		 战术导弹、组合导航、短时 GNSS 拒止
工业级 > 20 nmi/hr
(纯惯性积分迅速发散)		 10 – 50 °/h 0.5 – 5 mg 工业 MEMS
（BMI088、IIM-42652、ICM-42688-P）		 机器人、AGV/AMR、车载、
巡检无人机
消费级 > 100 nmi/hr
（纯惯性无意义）		 >50 °/h
(全温漂移可达数百度)		 5 – 20 mg 低端 MEMS
（MPU6050、BNO055）		 手机、VR/AR、玩具无人机

本设计三颗 IMU 均在 高端消费级/工业级 范围内。

④ 速率随机游走 / 加速度随机游走（RRW / AccRW）
  • 是什么：角速度（或加速度）本身在随机游走——零偏的变化率是白噪声驱动的随机游走过程。
  • 物理原因：环境因素（温度缓变、机械应力松弛）导致传感器参数缓慢变化，这种变化的”变化率”是随机的。
  • 数学表达：$\dot{b}(t) = w_r(t)$，其中 $w_r$ 为白噪声，RRW 是 $w_r$ 的功率谱密度
  • Allan 图：斜率 +1/2
  • 影响：长时间尺度（小时级）误差的主导因素。对于短时间（分钟级）飞行的无人机，影响较小。
  • 补偿方法：在 EKF 中对零偏建立一阶马尔可夫过程模型：$\dot{b} = -\frac{1}{\tau} b + w_r$，同时估计零偏及其变化率。
⑤ 速率斜坡 / 速度斜坡（Rate Ramp / Velocity Ramp）
  • 是什么：传感器输出存在确定性趋势性漂移——即使没有输入，输出也在缓慢单调递增或递减。
  • 物理原因：传感器老化、持续的温度梯度、或 PCB 应力缓慢释放。
  • 数学表达：$\omega_{\text{meas}}(t) = \omega_{\text{true}} + R \cdot t$，其中 $R$ 为斜坡率
  • Allan 图：斜率 +1
  • 影响：长时间运行后误差持续增长。对于航姿板（通常连续工作数小时），不可忽略。
  • 补偿方法：如果斜坡率恒定，可通过长时间静态数据拟合 $R$ 后减去；如果缓慢变化，EKF 在线估计。

2.2.3 频率混叠与物理减震——误差补偿的”第零道防线”

在讨论了传感器内部的各类随机误差后，还有一个来自外部环境但会”伪装”成低频误差的重要问题：

频率混叠（Aliasing）

无人机电机的振动频率往往极高（多旋翼 200~500Hz 的基频，加上 2~5 次谐波可达 1~3kHz）。当振动频率超过 IMU ADC 的奈奎斯特频率（采样率的一半）时，高频噪声会折叠（混叠）到低频带，表现为低频的”零偏漂移”——而算法上的数字低通滤波对此完全无能为力，因为混叠后的信号已经”伪装”成了低频有效信号。

例如：电机振动 2kHz，IMU 采样 1kHz → 混叠频率 $|2000 - 2 \times 1000| = 0$ Hz → 表现为常值零偏偏移！

物理减震——第零道防线

防线层级 方法 作用
第零道 物理减震（减震海绵/减震柱） 在机械振动到达传感器之前就将其衰减
第一道 片上抗混叠滤波器 ADC 前的模拟低通，衰减奈奎斯特频率以上的信号
第二道 数字低通滤波 片上 DSP 滤除带外噪声
第三道 算法融合 EKF 估计残余零偏

物理减震的具体措施：

  1. 减震海绵：在航姿板与机架之间粘贴 3~5mm 厚的减震海绵（如 3M VHB 胶带、Poron 泡棉），对 100Hz 以上的振动衰减 10~20dB
  2. 减震柱/减震球：用硅胶减震柱将板卡悬浮安装，低频减震效果优于海绵
  3. BMI088 的分体式优势：BMI088 将陀螺和加计封装为独立芯片，物理上分离意味着陀螺的驱动共振不会通过封装内耦合串扰到加计，这是它”抗震”的核心原因之一

设计建议：即使选了 BMI088，仍建议在板卡与机架之间加装减震。物理减震是”第零道防线”——不解决振动源头，后续所有算法补偿都是打补丁。这也是”从器件到算法”的完整闭环：器件选型（BMI088）→ 物理减震（海绵/减震柱）→ 片上滤波 → 算法补偿。

2.2.4 本设计 IMU 关键误差参数对照

基于 Datasheet 数据，汇总本设计三颗 IMU 的核心误差参数：

参数 ICM-42688-P IIM-42652 BMI088 单位 备注
陀螺噪声密度 2.8 ~4 14 mdps/√Hz ICM 最优
加计噪声密度 70 ~70 175~230 μg/√Hz ICM 最优
陀螺初始零偏 ±0.5 ±0.5 ±1 °/s BMI 最大
加计初始零偏 — — ±20 mg —
陀螺零偏温度系数 ±5 ±20 ±15 mdps/°C ICM 最优
加计零偏温度系数 — — ±0.2 mg/K —
刻度因子初始误差 ±0.5 ±0.5 — % —
刻度因子温度系数 ±0.005 ±0.005 — %/°C —
非线性 ±0.1 ±0.1 — % —
交叉耦合 ±1.25 ±1.25 — % —
零偏不稳定性 ~2 ~4 <2（估） °/hr BMI 最优（手册未直接给出）
推算 ARW ~0.17 ~0.24 ~0.84 °/√hr ICM 最优
工作温度 0~70 -40~105 -40~85 °C IIM 宽温
分体式设计 ❌ ❌ ✅ — BMI 抗振

ARW 推算方法：$\text{ARW} = 60 \times \sigma_g$，其中 $\sigma_g$ 为噪声密度（°/s/√Hz）。例如 ICM-42688-P：$\text{ARW} = 60 \times 0.0028 = 0.168$ °/√hr。

三颗 IMU 的误差互补关系：

  • ICM-42688-P：噪声密度最低 → 短期精度最优，适合做主 IMU
  • BMI088：零偏不稳定性最优 → 长期漂移最小，分体式设计抗振动 → 高振动场景首选
  • IIM-42652：工业级全温校验 → 极端温度下的稳定基准

2.2.5 IMU 完整误差模型

综合以上分析，IMU 的输出可建模为：

其中：

  • $\tilde{\omega}$：传感器实际输出（3×1 向量）
  • $M_s$：刻度因子误差矩阵（3×3 对角阵，对角元素为刻度因子误差 $s_i$）
  • $M_m$：非正交/交叉耦合矩阵（3×3，对角为 0，非对角为 $m_{ij}$）
  • $\omega_{\text{true}}$：真实角速度
  • $b_0$：常值零偏
  • $b_T(T)$：温度相关零偏漂移
  • $b_{\text{inst}}(t)$：零偏不稳定性（随机过程）
  • $n_g$：白噪声（ARW 的来源）

简化模型（工程常用）：忽略高阶项，确定性误差保留零偏 + 刻度因子 + 交叉耦合，随机误差保留 ARW + BI：

其中 $S$ 包含刻度因子和非正交误差，$b$ 包含常值零偏和缓变漂移。EKF 中通常将 $b$ 作为状态量在线估计。

2.3 磁力计误差模型

磁力计的误差机理与 IMU 有本质区别——IMU 的误差主要来自传感器内部，而磁力计的误差主要来自外部干扰。

2.3.1 硬铁干扰（Hard Iron Distortion）

  • 是什么：载体上永久磁铁或铁磁材料（螺钉、电机外壳）产生的固定磁场偏移。
  • 数学表达：$m_{\text{meas}} = m_{\text{true}} + b_h$，其中 $b_h$ 为硬铁偏移向量
  • 效果：磁力计输出”球心”偏移——理想情况下静止旋转一周，输出轨迹是以原点为圆心的球面；有硬铁干扰时球心偏移。
  • 补偿方法：椭球校准——将偏移的球心拟合出来并减去。这是最基本也是最有效的磁力计校准。

2.3.2 软铁干扰（Soft Iron Distortion）

  • 是什么：载体上导磁材料（钢板、铝框靠近铁磁体时）使地磁场发生畸变——不同方向上的磁场被”拉伸”或”压缩”。
  • 数学表达：$m_{\text{meas}} = S_s \cdot m_{\text{true}} + b_h$，其中 $S_s$ 为软铁矩阵（3×3 对称矩阵）
  • 效果：输出轨迹从球面变成椭球面——不同方向上的灵敏度不同。
  • 补偿方法：椭球拟合——将椭球参数（6 个独立参数：3 个轴缩放 + 3 个偏移）拟合出来，构建逆变换矩阵将数据映射回球面。

2.3.3 时变干扰

  • 是什么：大电流线路（电源线、电机驱动线）产生的磁场随电流变化。
  • 数学表达：$b_h(t) = k \cdot I(t)$，与瞬时电流成正比
  • 效果：硬铁偏移不再是常数，无法通过静态校准消除。
  • 补偿方法：
    1. 物理隔离：磁力计远离大电流线路（>10cm）
    2. 磁场异常检测：当 $|m_{\text{meas}}|$ 偏离本地地磁场值（25~65 μT）时，暂时禁用磁修正
    3. 外置磁力计：通过 I2C 延长线将磁力计安装在远离干扰源的位置

2.3.4 椭球校准原理

理想情况下，磁力计在空间中旋转一周，输出点集应落在以原点为球心、半径为本地地磁场强度的球面上。实际因硬铁+软铁干扰，点集落在偏移的椭球面上。

校准分两步：

  1. 数据采集：将板卡在空间中缓慢旋转（”画 8 字”），采集 N 个磁力计采样点
  2. 椭球拟合：用最小二乘法拟合椭球参数，求出偏移向量 $b_h$ 和变换矩阵 $S_s^{-1}$

本设计双磁力计的校准策略：RM3100 和 IST8310 分别做椭球校准（各自有不同的硬铁/软铁参数，因为安装位置和方向不同），校准后对比两者的磁场模长——若一致，说明校准有效；若持续不一致，说明存在时变干扰。

2.4 气压计误差

气压计的误差相对简单，主要来源：

误差类型 原因 影响 补偿方法
绝对精度误差 传感器出厂偏差 海拔绝对值偏差 ±几米 出厂标定 / 使用已知海拔点校准
温度漂移 传感器自身温度变化 零点漂移 片上温度补偿（BMP581 内置）
气流干扰 旋翼下洗、阵风 短时气压剧烈波动 防风海绵 + 低通滤波
地效干扰 贴近地面时旋翼气流被地面反弹 测量高度突然”跳水”（假性海拔骤降） 检测离地高度 <1m 时降低气压计权重，依赖超声波/激光雷达
天气变化 气压系统移动 缓慢漂移（几小时内数 hPa） 与 GNSS 高度融合修正

地效干扰详解：无人机在起飞和降落贴近地面时（通常 <1m），旋翼产生的下洗气流被地面反弹后向上冲击气压计，导致气压瞬间升高 → 算法误判为”海拔突然下降”。这种干扰具有明显的工况特征（仅在低高度出现），可通过检测离地高度来切换气压计权重。在起飞/降落阶段，优先信任超声波或激光雷达的高度测量。

BMP581 相对精度达 0.06 Pa（~5cm），绝对精度约 ±50 Pa（~±4m），对于定高和垂直速度测量已足够。

2.5 GNSS 误差

GNSS 的误差来源与 IMU/磁力计完全不同——不是传感器内部噪声，而是信号传播路径上的干扰：

误差类型 原因 量级 RTK 能否消除
电离层延迟 信号穿过电离层时折射 ~5~20 m ✅ 差分消除
对流层延迟 信号穿过对流层时折射 ~2~20 m ✅ 差分基本消除
多径效应 信号被建筑物/地面反射 0.5~10 m ⚠️ 部分消除
卫星钟差 卫星原子钟微小偏差 ~2 m ✅ 差分消除
星历误差 卫星轨道位置不准 ~2~5 m ✅ 差分消除
接收机噪声 天线+前端热噪声 ~0.1~1 m 不可消除

RTK 的核心原理：基准站（已知精确位置）和流动站（UM982）同时接收同一组卫星信号。两者共享的误差（电离层、对流层、钟差、星历）在差分过程中被消除，只残留接收机噪声和多径残余，从而实现厘米级定位。

双天线定向的精度：取决于两天线间的基线长度。基线越长，航向角精度越高：

其中 $L$ 为基线长度，$\lambda$ 为载波波长（GPS L1 约 19cm）。1 米基线典型精度约 0.2~0.5°。

2.6 补偿方法总览

将各类误差的补偿策略汇总：

误差类别 补偿方法 在哪里做 何时做
确定性误差
常值零偏 静态初始化取平均 上电校准 每次上电
刻度因子 六面法/转台标定 出厂标定 一次标定，写入固件
交叉耦合 六面法标定修正矩阵 出厂标定 一次标定
轴间失准 多位置标定 出厂标定 一次标定
温度漂移 温度补偿表 / 片上补偿 运行时 持续
外部环境误差
振动混叠 物理减震（减震海绵/减震柱） 硬件设计 始终
磁偏角 WMM2020 查表补偿 运行时 每次换位置
随机性误差
ARW/VRW 不可补偿，融合算法修正 在线融合 持续
零偏不稳定性 EKF 在线估计零偏状态量 在线融合 持续
RRW 一阶马尔可夫模型 在线融合 持续
磁力计误差
硬铁干扰 椭球校准 使用前校准 定期/每次部署
软铁干扰 椭球校准 使用前校准 定期/每次部署
时变干扰 磁场异常检测 + 物理隔离 运行时 持续
气压计误差
气流干扰 防风海绵 + 低通滤波 硬件+算法 持续
地效干扰 检测低高度 → 降低气压计权重 算法 起飞/降落
天气漂移 GNSS 高度融合 在线融合 持续

核心原则：

  1. 确定性误差靠标定——上电校准或出厂标定，”减掉”就行
  2. 随机误差靠融合——无法消除，只能用绝对基准传感器（加计/磁力计/GNSS）持续修正
  3. 磁力计误差靠校准+隔离——椭球校准消除静态干扰，物理隔离应对动态干扰
  4. 外部环境误差靠物理防护——减震海绵/减震柱防振动混叠，防风海绵防气流干扰，这些是算法无法替代的”第零道防线”
  5. 时间同步是融合的前提——PPS 脉冲为多传感器提供统一时间基准，没有时间对齐就没有有效融合
  6. 所有在线补偿最终都由第 3 章的融合算法实现——EKF 的状态量中包含零偏估计、磁场偏差等，实现闭环自校正

3. 姿态解算与数据融合

前两章回答了”我们有什么传感器”和”传感器有什么误差”，这一章回答最核心的问题：怎么从带误差的传感器数据中算出可靠的姿态？

本章的叙事逻辑：

1
2
3
4
5
6
7
8
9
10
11
12
3.1 姿态怎么表示？     → 数学工具（欧拉角 / DCM / 四元数 / 等效旋转矢量）
3.2 物理基础           → 解算本质（传感器输出什么、坐标系怎么定、初始对准）
3.3 数值实现           → 怎么用数值方法一步步算出来（姿态/速度/位置更新）
3.4 为什么需要融合？   → 单传感器各自有缺陷
3.5 怎么融合？         → 五种算法，从简到繁：
     ① 一阶互补滤波 → 直觉入门
     ② DCM 滤波     → 旋转矩阵 + 叉积误差 + PI，消除万向锁
     ③ Mahony 滤波  → DCM 的四元数版，省掉正交化，飞控主流
     ④ Madgwick 滤波 → 梯度下降优化，单算法统一处理
     ⑤ EKF          → 最严谨，多传感器紧耦合，本设计最终方案
3.9 选哪个？           → 对比与选型
3.10 本设计的融合架构  → 工程落地

3.1 姿态的数学表示

姿态（Attitude / Orientation）描述的是一个刚体在三维空间中的旋转状态。具体地说，是载体坐标系（Body Frame, $b$ 系） 相对于参考坐标系（Reference Frame, $n$ 系，即导航系） 的旋转关系。

在航姿解算中，我们需要的输出是三个姿态角——横滚（Roll $\phi$）、俯仰（Pitch $\theta$）、航向（Yaw $\psi$）。但”三个角”并不是唯一的表示方式，甚至不是最好的表示方式。我们需要理解四种主要的数学表示，以及为什么四元数是工程首选、等效旋转矢量是 ESKF 误差状态的理想表示。

3.1.1 欧拉角（Euler Angles）

定义：欧拉角用三次连续旋转的角度来描述姿态。在航空航天领域，最常用的是 ZYX 内旋序列（也叫泰特-布莱恩角，Tait-Bryan angles）：

  1. 先绕 $Z$ 轴旋转 $\psi$（航向 / Yaw）
  2. 再绕新的 $Y’$ 轴旋转 $\theta$（俯仰 / Pitch）
  3. 最后绕新的 $X’’$ 轴旋转 $\phi$（横滚 / Roll）

ZYX 内旋产生两个互为转置的旋转矩阵（推导详见 3.1.6 节）：

矩阵 变换方向 公式（坐标系变换阵 / 被动旋转） 核心用途
$C_n^b$ 导航系 → 机体系 $C_n^b = R_x(\phi) \cdot R_y(\theta) \cdot R_z(\psi)$ 参考方向转到机体系
$C_b^n$ 机体系 → 导航系 $C_b^n = (C_n^b)^T$ 传感器数据转到导航系

⚠️ 定义说明：本节 $R_x, R_y, R_z$ 采用坐标系变换阵（被动旋转，定义 A），即”向量不动、坐标系旋转”。这与线性代数教材中的向量旋转阵（主动旋转，定义 B）互为转置——两种定义下乘法顺序”相反”，但展开后的数值矩阵完全一致。详见 3.1.6 节的完整推导和两种定义对照表。

展开 $C_n^b$：

其中 $c_\psi = \cos\psi$，$s_\psi = \sin\psi$，其余类推。

优点：直观，三个角对应三个物理方向，人一看就懂。

致命缺陷：万向锁（Gimbal Lock）

当俯仰角 $\theta = \pm 90°$ 时，$c_\theta = 0$，旋转矩阵退化为：

此时 $\psi$ 和 $\phi$ 只以 $(\phi - \psi)$ 的组合出现——系统丢失了一个自由度。无论你怎么单独改变航向角或横滚角，效果都是绕同一个轴旋转，无法区分。

物理直觉：想象一个万向节支架——当中间轴旋转 90° 时，内环和外环共面，失去了绕某一轴独立旋转的能力。这正是阿波罗登月时宇航员遇到的”Gimbal Lock”警告的来源。

另一个问题：旋转的不可交换性

三维空间中，旋转的顺序影响最终结果。先绕 X 转 90° 再绕 Y 转 90°，和反过来做，最终姿态完全不同。欧拉角的三个角是按固定顺序定义的，这意味着不能简单地对三个角独立做运算。

3.1.2 方向余弦矩阵（DCM）

定义：DCM（Direction Cosine Matrix）是一个 $3 \times 3$ 的正交矩阵，直接描述两个坐标系之间的旋转变换：

其中 $C_n^b$ 是从导航系到机体系的旋转矩阵（和上面欧拉角展开的矩阵是同一个东西），$C_b^n = (C_n^b)^T$ 是其转置。

性质：

  • 正交性：$C^T C = I$，即 $C^{-1} = C^T$
  • 行列式为 1：$\det(C) = 1$
  • 只有 3 个独立参数（9 个元素受 6 个正交约束）

优点：

  • 无奇异性——任何姿态都能表示，没有万向锁
  • 向量变换直接——左乘 DCM 即可
  • 旋转复合简单——矩阵连乘

缺点：

  • 9 个参数表示 3 个自由度，冗余（6 个约束方程要始终满足）
  • 积分后不再正交——需要周期性重新正交化（如 Gram-Schmidt 或 SVD）
  • 参数多，计算量大

3.1.3 四元数（Quaternion）

定义：四元数是爱尔兰数学家哈密顿（Hamilton）在 1843 年发明的四维超复数：

通常写成向量形式 $\mathbf q = [q_0, q_1, q_2, q_3]^T$，其中 $q_0$ 为标量部分（实部），$[q_1, q_2, q_3]$ 为向量部分（虚部）。

任意非零四元数 $\mathbf q$ 都可以改写成如下形式：

引入一个沿 $\mathbf q_\mathrm{v}$ 方向上的单位向量 $\mathbf u$：

则可以得到：

单位四元数与旋转：

根据四元数模的定义 $|\mathbf q| = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}$ ，引入变量：

当 $|\mathbf q| = q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1$ 时，四元数可以表示三维空间中的旋转。绕单位轴 $\mathbf{u} = [u_x, u_y, u_z]^T$ 旋转 $\alpha$ 角对应的四元数为：

391

双覆盖问题：$\mathbf q$ 和 $\mathbf {-q}$ 表示同一个旋转（因为旋转 $\alpha$ 绕 $\mathbf{u}$ 和旋转 $360° - \alpha$ 绕 $-\mathbf{u}$ 是同一个变换）。这不影响计算，但需要注意在插值时保证 $\mathbf q$ 的连续性——如果 $\mathbf q$ 突然变号，插值会走”远路”。

四元数乘法（Hamilton 乘积）：

用矩阵表示四元数乘法：

其中左乘矩阵和右乘矩阵分别为：

四元数与旋转矩阵的转换：

⚠️ 约定一致性校验：$q_b^n$ 与 $C_b^n$ 下标方向一致——都是”从 $b$ 系到 $n$ 系”。验证 $C_b^n$ 的 (3,1) 元素 $= 2(q_1 q_3 - q_0 q_2)$，对应欧拉角展开 $C_b^n$ 的 (3,1) 元素 $= -s_\phi c_\theta s_\psi + c_\phi s_\theta c_\psi + s_\phi c_\psi$ … 实际上更直接的验证方式：当 $\phi = 0, \theta = 0$ 时，$C_b^n = R_z(\psi)$，(1,2) 元素应为 $-s_\psi$；本公式 (1,2) = $2(q_1q_2 - q_0q_3)$，当 $\mathbf q = [\cos(\psi/2), 0, 0, \sin(\psi/2)]$ 时 = $-2\cos(\psi/2)\sin(\psi/2) = -\sin\psi$ ✓。关键：本设计统一采用 Hamilton 约定 + $\mathbf q_b^n$（从 $b$ 系到 $n$ 系）+ 标量在前，四元数与 DCM 下标方向一致，代码中四元数→DCM 无需转置。

四元数与欧拉角的转换（ZYX 内旋，Hamilton 约定，$\mathbf q_b^n$）：

⚠️ 符号陷阱提醒：如果你阅读的文献采用了 JPL 约定（标量在后，即 $\mathbf q = [q_1, q_2, q_3, q_0]^T$），或者定义 $\mathbf q_n^b$（从 $n$ 系到 $b$ 系，与本文相反），上述公式中的正负号和下标组合会不同。最常见的问题是 $\theta$ 公式中 $q_0 q_2 - q_3 q_1$ 的正负号——在不同约定下可能变为 $q_0 q_2 + q_3 q_1$。写代码前务必确认你的四元数约定与公式来源一致。

3.1.4 等效旋转矢量（Rotation Vector / Axis-Angle）

定义：任何三维旋转都可以唯一地表示为”绕某个单位轴旋转某个角度”，这由欧拉旋转定理（Euler’s Rotation Theorem）保证。等效旋转矢量（Equivalent Rotation Vector）定义为：

其中：

  • $\mathbf{u} = [u_x, u_y, u_z]^T$ 是单位旋转轴（$|\mathbf{u}| = 1$）
  • $\phi$ 是旋转角度（标量，单位：弧度）
  • 合成 3 个参数，故又称轴角表示（Axis-Angle）

物理直觉：就像用螺丝刀——你只需要知道两件事：往哪个方向拧（轴），拧多少度（角）。这就是旋转的全部信息。

353

与四元数的关系

四元数和等效旋转矢量本质上是同一种旋转表示的不同参数化形式。给定旋转矢量 $\mathbf{\Phi} = \phi\mathbf{u}$，对应的单位四元数为：

两者对比：

特性 四元数 等效旋转矢量
参数个数 4 3
约束 $\ q\ = 1$ 无
与旋转的对应 双覆盖（$q$ 和 $-q$ 同旋） 唯一
旋转复合 四元数乘法 不可直接叠加

等效旋转矢量是四元数的”去冗余版”——去掉了单位约束，3 个参数恰好等于 3 个自由度，没有约束方程需要维护。

Rodrigues 旋转公式

给定旋转矢量 $\mathbf{\Phi} = \phi\mathbf{u}$，将向量 $\mathbf{v}$ 绕轴 $\mathbf{u}$ 旋转 $\phi$ 角后的结果为：

公式的三项直觉理解：

  1. $\mathbf{v}\cos\phi$：将 $\mathbf{v}$ 沿自身方向缩放 $\cos\phi$（这部分不旋转）
  2. $(\mathbf{u} \times \mathbf{v})\sin\phi$：垂直于 $\mathbf{u}$ 的分量绕轴旋转
  3. $\mathbf{u}(\mathbf{u} \cdot \mathbf{v})(1-\cos\phi)$：平行于 $\mathbf{u}$ 的分量保持不变（乘以 $(1-\cos\phi)$ 后加回去）

与 DCM 的关系

Rodrigues 公式的矩阵形式为：

其中 $[\mathbf{u}]_\times$ 是 $\mathbf{u}$ 的反对称矩阵（叉乘矩阵）：

⚠️ 符号陷阱：不同文献中 Rodrigues 公式的符号可能不同——取决于你定义的是 $C_n^b$（从 n 到 b）还是 $C_b^n$（从 b 到 n），以及旋转方向是绕 $\mathbf{u}$ 正向还是负向。本设计统一使用 $C_b^n = I\cos\phi + (1-\cos\phi)\mathbf{u}\mathbf{u}^T + \sin\phi[\mathbf{u}]_\times$。

在 ESKF 中的核心地位

ESKF（Error-State Kalman Filter）的 15 维误差状态中，姿态误差用 3 维旋转矢量 $\delta\boldsymbol{\theta} = [\delta\theta_x, \delta\theta_y, \delta\theta_z]^T$ 表示，而非 4 维 $\delta q$。这是因为：

  1. 匹配自由度：姿态只有 3 个自由度，用 3 参数恰好表示
  2. 避免奇异：$\delta q$ 所在的单位四元数流形在 $|q|=1$ 约束下，误差协方差矩阵会在某些方向退化；$\delta\boldsymbol{\theta}$ 没有约束，$P$ 矩阵始终满秩
  3. 线性化友好：小角度下 $\delta\boldsymbol{\theta} \approx \boldsymbol{\omega}\Delta t$，物理意义清晰

小角度近似的物理含义：在短时间 $\Delta t$ 内，角速度 $\boldsymbol{\omega}$ 近似恒定，则旋转的角度大小 $|\delta\boldsymbol{\theta}| \approx |\boldsymbol{\omega}| \Delta t$，旋转轴方向约为 $\boldsymbol{\omega} / |\boldsymbol{\omega}|$。

优缺点总结

维度 优点 缺点
参数化 ✅ 最小参数化（3 参数 = 3 自由度），无约束 ❌ 不直观（轴 + 角不如欧拉角好理解）
旋转复合 — ❌ 大角度下旋转不可叠加（需用四元数复合）
工程应用 ✅ ESKF 误差状态的理想表示 ❌ 不能直接用于连续旋转更新
物理意义 ✅ 小角度下 $\delta\boldsymbol{\theta} \approx \boldsymbol{\omega}\Delta t$ 清晰 —

旋转矢量在圆锥运动下的不可交换性补偿

这是高精度惯导中的经典问题。当载体做高频小幅振动（如振动台上的 IMU）时，连续旋转的不可交换性（non-commutativity）会导致简单积分 $\Delta\boldsymbol{\theta} = \boldsymbol{\omega}\Delta t$ 产生系统性漂移——即使陀螺仪完美，积分结果也会偏离真实姿态。

问题本质：三维旋转不满足交换律，$\mathbf{R}(\boldsymbol{\omega}_1)\mathbf{R}(\boldsymbol{\omega}_2) \neq \mathbf{R}(\boldsymbol{\omega}_2)\mathbf{R}(\boldsymbol{\omega}_1)$。在一个 $\Delta t$ 内采样多个角速度点时，简单求和会丢失旋转的不可交换性部分。

Miller 算法（多子样补偿）：在 $\Delta t$ 内采样 $N$ 个角速度点，计算加权旋转矢量增量：

补偿项包含角速度叉乘的积分，典型形式为：

MEMS IMU 的实际影响：在消费级/工业级 MEMS IMU（如 ICM-42688）中，陀螺仪的角随机游走（ARW）噪声远大于圆锥误差。对于 1 kHz 的采样率，圆锥补偿项的量级约为 $10^{-6}$ rad，远低于陀螺噪声水平。因此，在航姿解算板的设计中，圆锥补偿可以忽略，不影响姿态精度。但若用于高精度惯性导航系统（如战略导弹、水下航行器），则必须使用多子样算法。

3.1.5 四种表示的转换关系

三种姿态表示（欧拉角、DCM、四元数）加上等效旋转矢量，一共四种。它们之间可以两两转换，构成一个完整的转换关系图：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
            ┌─────────────────────────────────────────────┐
            │           姿态表示转换关系图                  │
            └─────────────────────────────────────────────┘

                 等效旋转矢量 (3D)
                      ↑
               3参数 ↔ | ↔ 4参数
                       ↓
      ┌─────────────────┴─────────────────┐
      │                                   │
四元数 (4D) ────────────→─────────────── DCM (9→3D)
      ↑                                   ↑
      │                                   │
欧拉角 (3D) ────────────→──────────────────┘
      (3参数，最直观)

四种姿态表示方法总结：

特性 欧拉角 DCM 四元数 等效旋转矢量
参数个数 3 9（3 独立） 4（3 独立） 3
万向锁 ❌ 有（Pitch=±90°） ✅ 无 ✅ 无 ✅ 无
旋转复合 困难 矩阵乘法 四元数乘法 ❌ 不可直接叠加
插值 不可用 不方便 SLERP 球面插值 不方便
约束 无 6 个正交约束 1 个范数约束 无
计算量 高（三角函数） 高（9×9 矩阵运算） 低（仅加减乘） 最低
直观性 ✅ 最直观 一般 ❌ 不直观 一般（轴+角）
适用场景 人机交互/输出 向量变换 姿态解算内部运算 ESKF 误差状态

一般地：姿态解算的内部运算用四元数（无奇异性、计算高效），仅在最终输出时转换为欧拉角供用户理解。

1. 欧拉角 ↔ DCM

欧拉角 → DCM（ZYX 内旋）：已在 3.1.1 节给出完整展开：

DCM → 欧拉角：从矩阵元素反解（假设 $|\theta| < 90°$）：

⚠️ atan2 的选择：这里用 $\text{atan2}(y, x)$ 而非 $\arcsin$ / $arccos$，是因为 atan2 在矩阵元素接近边界时数值更稳定，能正确处理象限信息。

2. 欧拉角 ↔ 四元数

欧拉角 → 四元数：三次旋转对应的四元数复合：

其中单轴旋转四元数为：

展开后（Hamilton 约定，标量在前）：

四元数 → 欧拉角：已在 3.1.3 节给出：

3. DCM ↔ 四元数

四元数 → DCM：已在 3.1.3 节给出（Hamilton 约定）：

DCM → 四元数（Shepperd 方法）：从 DCM 元素提取四元数，避免数值不稳定的方法是选最大元素作为计算基础：

如果 $C_{11}$ 最大，则用另一组公式（避免除以接近零的 $w$）：

其余情况类似。最后归一化 $q \leftarrow q / |q|$。

4. 四元数 ↔ 等效旋转矢量

四元数 → 等效旋转矢量：

⚠️ 当 $\phi = 0$ 时：$\sin(\phi/2) = 0$，无法直接计算 $\mathbf{u}$。此时旋转角度为零，旋转轴无定义，但四元数退化为 $[1, 0, 0, 0]^T$，约定 $\mathbf{u} = [1, 0, 0]^T$（任意方向均可）。

等效旋转矢量 → 四元数：

小角度近似（ESKF 中常用）：

当 $\phi$ 很小时，$\sin(\phi/2) \approx \phi/2$，$\cos(\phi/2) \approx 1$，因此：

这就是 ESKF 中”四元数误差到旋转矢量误差”的近似转换——避免了三角函数计算，直接用线性关系。

5. DCM ↔ 等效旋转矢量

等效旋转矢量 → DCM：Rodrigues 公式的矩阵形式：

DCM → 等效旋转矢量：

从矩阵的迹（trace）和反对称部分提取：

⚠️ 当 $\phi = 0$ 时：$\sin\phi = 0$，无法直接提取 $\mathbf{u}$。此时 $C \approx I$，旋转角度为零，$\mathbf{u}$ 无定义。

转换关系总结对照表

表示 参数数 约束条件 奇异性 旋转复合 推荐适用场景
欧拉角 3 无 Pitch = ±90°（万向锁） 困难（需转 DCM 或四元数） 人机交互/最终输出显示
DCM 9（3 独立） 正交性 $C^T C = I$ 无 矩阵乘法 向量坐标变换/飞控底层计算
四元数 4（3 独立） $\ q\ = 1$ 无 Hamilton 乘积 $\otimes$ 姿态解算内部运算（推荐）
等效旋转矢量 3 无 无 不可直接叠加（需转四元数） ESKF 误差状态表示

工程实践中的选择：

  • 姿态更新（积分）：用四元数——无奇异性，计算高效，自动保持约束
  • 向量变换（如重力投影）：用 DCM 或四元数转换的 DCM——左乘即得结果
  • ESKF 误差状态：用旋转矢量 $\delta\boldsymbol{\theta}$ ——3 参数无约束，协方差矩阵满秩
  • 输出显示：用欧拉角——人类最容易理解

3.1.6 $C_n^b$ 与 $C_b^n$：ZYX 内旋的矩阵乘法顺序

这一节专门解决一个最开始让我十分困惑的问题：$C_n^b$ 和 $C_b^n$ 到底谁是谁？矩阵乘法的顺序怎么写才对？为什么网上博客的公式互相矛盾？

答案先说：所有矛盾都源于一个根源——“旋转矩阵”有两种定义，很多资料混着用却不说清楚。

第一步：区分两种旋转矩阵

同一个物理旋转，可以用两种视角的矩阵描述：

定义 A：坐标系变换阵（被动旋转） 定义 B：向量旋转阵（主动旋转）
物理含义 向量不动，坐标系旋转；描述同一向量在新坐标系下的坐标 坐标系不动，向量在空间中被旋转
数学关系 $\mathbf{v}^{新系} = R^{pas} \cdot \mathbf{v}^{原系}$ $\mathbf{v}_{旋转后} = R^{act} \cdot \mathbf{v}_{旋转前}$
典型出处 秦永元《惯性导航》、PX4 飞控 线性代数/机器人教材

核心关系：两者互为转置（正交矩阵的逆 = 转置）：

三轴完整定义对照：

快速识别法：看 $R_z(\psi)$ 的 (1,2) 元素。如果是 $+s_\psi$，就是被动旋转（定义 A）；如果是 $-s_\psi$，就是主动旋转（定义 B）。以后看到任何博客的旋转矩阵，先看这一格。

第二步：为什么需要区分 $C_n^b$ 和 $C_b^n$？

传感器测量的是机体系（$b$ 系）下的物理量——加速度计输出 $\mathbf{a}^b$，陀螺仪测量绕机体系的角速度 $\boldsymbol{\omega}^b$。但我们最终需要的是导航系（$n$ 系）下的量——导航算法要用北东地坐标表示速度、姿态、位置。

因此，我们需要一个矩阵，把机体系向量”变”到导航系：

$C_b^n$ 是航姿解算的核心目标矩阵——它描述了机体系相对于导航系的旋转关系。

而 $C_n^b$ 恰好是它的逆（因为 DCM 是正交矩阵）：

第三步：用坐标系变换阵（定义 A）推导 $C_n^b$ ——顺理成章，无任何转置凭空出现

我们定义 $R_x^{pas}, R_y^{pas}, R_z^{pas}$ 为坐标系变换阵（定义 A）。ZYX 内旋的物理过程是：

1
n系 → 绕Z转ψ → 1系 → 绕Y'转θ → 2系 → 绕X''转φ → b系

每一步都是在做坐标系变换——同一个向量，从旧坐标系变到新坐标系下的坐标：

  1. 从 $n$ 系变到 1 系（绕 $Z$ 转了 $\psi$）：$\mathbf{v}^1 = R_z^{pas}(\psi) \cdot \mathbf{v}^n$
  2. 从 1 系变到 2 系（绕 $Y’$ 转了 $\theta$）：$\mathbf{v}^2 = R_y^{pas}(\theta) \cdot \mathbf{v}^1 = R_y^{pas}(\theta) \cdot R_z^{pas}(\psi) \cdot \mathbf{v}^n$
  3. 从 2 系变到 $b$ 系（绕 $X’’$ 转了 $\phi$）：$\mathbf{v}^b = R_x^{pas}(\phi) \cdot \mathbf{v}^2 = R_x^{pas}(\phi) \cdot R_y^{pas}(\theta) \cdot R_z^{pas}(\psi) \cdot \mathbf{v}^n$

因此，从导航系到机体系的整体变换矩阵顺理成章地诞生了：

注意：矩阵乘法顺序 $R_x \cdot R_y \cdot R_z$ 与物理旋转顺序 Z→Y→X 相反。这不是错误——因为每一步是把向量变到新坐标系，最后转的 Roll（$R_x$）反而最先作用于原始向量 $\mathbf{v}^n$，所以写在最左边。

而 $C_b^n$ 就是它的转置：

第四步：反推

①用被动旋转（坐标系变换，定义 A）推导 $C_b^n$ ——“剥洋葱”直觉

也可以从坐标系变换的角度理解：已知 $b$ 系是由 $n$ 系经过 ZYX 内旋得到的。要把 $b$ 系下的向量坐标还原到 $n$ 系，需要将坐标系反向旋转回去：

  1. 先绕 $X$ 轴旋转 $-\phi$（撤销最后一次 Roll），
  2. 再绕 $Y$ 轴旋转 $-\theta$（撤销 Pitch），
  3. 最后绕 $Z$ 轴旋转 $-\psi$（撤销第一次 Yaw）。

用被动旋转矩阵写出该过程：

因此：

利用关系 $R^{pas}(-\alpha) = R^{pas}(\alpha)^T = R^{act}(\alpha)$，可得：

“剥洋葱”口诀：要还原坐标系，后转的先还原，而且反方向转。Roll 是最后转的，所以先绕 $X$ 转 $-\phi$（写在最右边）；Yaw 是最先转的，所以最后绕 $Z$ 转 $-\psi$（写在最左边）。写成主动旋转后，负号消失，顺序不变。

②用主动旋转（向量旋转，定义 B）推导 $C_b^n$ ——“剥洋葱”直觉

也可以从另一个角度理解：已知一个在 $b$ 系的向量 $\mathbf{v}^b$，怎么把它还原到 $n$ 系？

回忆 ZYX 内旋物理过程：n 系先绕 Z 转 ψ，再绕 Y’ 转 θ，最后绕 X’’ 转 φ，变成了 b 系。要还原回去，就得反着来——先撤销最后一步，再撤销倒数第二步，最后撤销第一步。

  1. 先剥最后一步：$b$ 系是 2 系绕 $X’’$ 转 $\phi$ 得到的，左乘 $R_x^{pas}(\phi)$ 的逆（即 $R_x^{act}(\phi)$）：

  2. 再剥第二步：2 系是 1 系绕 $Y’$ 转 $\theta$ 得到的，左乘 $R_y^{act}(\theta)$：

  3. 最后剥第一步：1 系是 $n$ 系绕 $Z$ 转 $\psi$ 得到的，左乘 $R_z^{act}(\psi)$：

连起来：

由于 $R^{act}(\theta) = R^{pas}(\theta)^T$，上式等价于：

因此：

“剥洋葱”口诀：想象一个洋葱，最后包上的先剥掉。Roll 是最后转的，所以 $R_x$ 写在最右边（最先剥）；Yaw 是最先转的，所以 $R_z$ 写在最左边（最后剥）。

第五步：统一——两种推导给出同一个结果
验证 定义 A（被动旋转，坐标变换） 定义 B（主动旋转，向量旋转）
$C_n^b$ $R_x^{\text{pas}}(\phi) R_y^{\text{pas}}(\theta) R_z^{\text{pas}}(\psi) \= R_x^{\text{act}}(-\phi) R_y^{\text{act}}(-\theta) R_z^{\text{act}}(-\psi)$ $\big[R_z^{\text{act}}(\psi) R_y^{\text{act}}(\theta) R_x^{\text{act}}(\phi)\big]^T \= R_x^{\text{act}}(-\phi) R_y^{\text{act}}(-\theta) R_z^{\text{act}}(-\psi)$
$C_b^n$ $\big(C_n^b\big)^T = R_z^{\text{pas}}(\psi)^T R_y^{\text{pas}}(\theta)^T R_x^{\text{pas}}(\phi)^T \= R_z^{\text{act}}(\psi) R_y^{\text{act}}(\theta) R_x^{\text{act}}(\phi)$ $R_z^{\text{act}}(\psi) R_y^{\text{act}}(\theta) R_x^{\text{act}}(\phi)$

无论用哪种定义，展开后的数值矩阵完全相同。 关键是：定义和顺序必须配套，不能混着用。

想求得 $C_b^n$，主动一步到位，被动需转置。（但要注意二者的旋转矩阵是不一样的！）

为什么网上博客互相矛盾？

典型错误写法：

❌ “因为是 ZYX 旋转，所以 $C_b^n = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi)$” （需要结合给出的单位旋转矩阵）

这句话在哪种定义下是对的？

  • 如果 $R_z, R_y, R_x$ 是主动旋转（定义 B），则 $C_b^n = R_z^{act} \cdot R_y^{act} \cdot R_x^{act}$ ✅
  • 如果 $R_z, R_y, R_x$ 是被动旋转（定义 A），则 $C_n^b = R_x^{pas} \cdot R_y^{pas} \cdot R_z^{pas}$，而 $C_b^n = R_z^{pas} \cdot R_y^{pas} \cdot R_x^{pas}$ ✅

所以公式本身没问题——但必须说清楚用的是哪种定义。博客的毛病在于：用了定义 B 的矩阵符号，却没声明；或者把定义 A 的矩阵和定义 B 的乘法顺序混在一起，导致展开结果正负号全反。

⚠️ 识别博客定义的快速方法：先看坐标系与旋转顺序的动议，若与本文一致，就看 $R_z(\psi)$ 的 (1,2) 元素。如果是 $-s_\psi$（主动旋转），那 $C_b^n = R_z \cdot R_y \cdot R_x$；如果是 $+s_\psi$（被动旋转），那 $C_n^b = R_x \cdot R_y \cdot R_z$。

展开的 $C_n^b$ 和 $C_b^n$ 矩阵

验证方法

方法①：查看矩阵的单一元素

验证项 检查位置 预期值 物理含义
$C_n^b$ 的 (1,3) 元素 第 1 行第 3 列 $-s_\theta$ Pitch 向上时，北方向在机体系 Z 轴分量为负
$C_b^n$ 的 (3,1) 元素 第 3 行第 1 列 $-s_\theta$ 同理，物理意义对称

⚠️ 常见错误：如果你的代码里 $C_n^b$ 的 (1,3) 元素是 $+s_\theta$，说明你把两个矩阵写反了，或者混用了两种定义的矩阵。

方法②：将具体姿态代入

姿态 预期 $C_b^n$ 机头在导航系方向
$\psi=0,\theta=0,\phi=0$ $I_{3×3}$ 北
$\psi=90^\circ,\theta=0,\phi=0$ $\begin{bmatrix}0&-1&0\1&0&0\0&0&1\end{bmatrix}$ 东
$\psi=0,\theta=90^\circ,\phi=0$ $\begin{bmatrix}0&0&1\0&1&0\-1&0&0\end{bmatrix}$ 地（朝下）

读者可以用这三组数值秒查自己写的矩阵对不对。

外旋 XYZ 的关系

如果每次都绕地面固定轴（导航系坐标轴）旋转：

  1. 先绕固定 $X$ 轴转 $\phi$
  2. 再绕固定 $Y$ 轴转 $\theta$
  3. 最后绕固定 $Z$ 轴转 $\psi$

用坐标系变换阵（定义 A），总变换为：

这与内旋 ZYX 的结果完全相同。内旋 ZYX 等价于外旋 XYZ——两种描述方式给出同一个旋转，只是视角不同。

为什么飞控用内旋？ 因为人只能感知绕自身轴（机体轴）的转动——你不会说”我绕地面 north 轴转了多少度”，而是说”我机头偏了多少度”。所以内旋 ZYX 更符合人类直觉。

复习口诀

你在想什么 应该怎么写
物理世界 飞机先 Yaw（绕 Z），再 Pitch（绕 Y），最后 Roll（绕 X）
用定义 A（被动旋转） $C_n^b = R_x^{pas} \cdot R_y^{pas} \cdot R_z^{pas}$（乘法顺序与旋转顺序相反，最后转的写最左）
用定义 B（主动旋转） $C_b^n = R_z^{act} \cdot R_y^{act} \cdot R_x^{act}$（剥洋葱：后转的先剥，$R_x$ 写右边）
两者关系 $C_b^n = (C_n^b)^T$，展开后完全一致
快速验证 $C_n^b$ 的 (1,3) = $-s_\theta$，$C_b^n$ 的 (3,1) = $-s_\theta$

本设计的约定：文档统一使用坐标系变换阵（定义 A，被动旋转）。$C_n^b$ 表示从导航系到机体系的旋转矩阵，即 $R_n^b$。上下标规则：第一个字母是源坐标系，第二个字母是目标坐标系。

3.1.7 本设计的坐标约定

在航姿解算中，坐标约定必须始终一致，否则公式中的正负号会出问题。这里参考的是牛小骥老师的 NED 坐标系（严龚敏老师使用的是 ENU 坐标系）：

约定项 本设计选择 说明
导航系（$n$ 系） NED（北-东-地） $X$ 北、$Y$ 东、$Z$ 向下。航空航天标准
机体系（$b$ 系） 前右下（FRD） $X$ 前、$Y$ 右、$Z$ 下。与 NED 对应
旋转序列 ZYX 内旋 Yaw → Pitch → Roll，航空航天标准
四元数定义 $q_b^n$：从 $b$ 系到 $n$ 系的旋转 右乘更新：$q_{k+1} = q_k \otimes \Delta q$
角速度方向 右手定则 绕某轴逆时针为正
重力方向 $\boldsymbol{g}^n = [0, 0, g]^T$（NED 系中 $Z$ 向下） $\ \boldsymbol{g}\ \approx 9.8$ m/s²

注意：不同文献的四元数定义可能不同（有的用 $[q_1, q_2, q_3, q_0]$ 即标量在后，有的旋转方向是 $b$ 系到 $n$ 系），阅读参考文献时务必核对约定。

3.2 惯导解算的物理基础

惯导解算不只是”算姿态”，而是一条完整的链路：传感器原始数据 → 物理单位转换 → 误差补偿 → 坐标变换 → 姿态更新。本节建立这条链路的物理基础——传感器输出什么物理量、坐标系怎么定义、初始姿态怎么确定。

3.2.1 传感器原始数据到物理单位：灵敏度与 LSB

IMU 传感器输出的原始数据是 ADC（模数转换器）的计数值——一个整数。以加速度计为例，寄存器中存储的并非物理加速度值，而是代表 ADC 转换结果的原始 LSB 值。

LSB 定义：ADC 的最小分辨率，即寄存器中数值变化 1 所对应的最小物理量变化。LSB 是”Least Significant Bit”的缩写。

灵敏度（Sensitivity）：厂家标定的常数，表示每 LSB 对应的物理量。这是将原始数字量转换为物理量的关键参数。

参数 含义 单位
sen_accel 加速度计灵敏度 LSB/(m/s²) 或 LSB/g
sen_gyro 陀螺仪灵敏度 LSB/(rad/s) 或 LSB/(°/s)

转换公式：

例如：(参考 ICM45686 的代码)某时刻加速度计原始读数为 raw = 16000 LSB，灵敏度 sen_accel = 8192/9.8 ≈ 835.918 LSB/(m/s²)，则：

⚠️ 双 g 陷阱：工程中常见的混淆
这两个”9.8”不是同一个东西：

  • 分母里的 9.8：厂家手册中的单位换算常数，用于将 LSB 转为 m/s²（如 sen_accel = 8192/9.8）。这是固定的刻度因子，不随地点变化。
  • 消除重力时的 gravity：本地真实重力加速度（如北京 9.7914 m/s²，三亚 9.7880 m/s²）。这是物理量，在重力补偿时使用。

 

1
2
3
4
5
6
7
8
// 正确做法
float sen_accel = 8192.0f / 9.8f;      // 常数，用于ADC转换
float physical_acc = raw / sen_accel; // 得到 m/s²

// 重力补偿用本地真实g（NED坐标系）
float gravity = 9.7914f;  // 北京地区
float a_line = physical_acc + [0, 0, gravity];  // NED: 补偿重力（g^n=[0;0;+g]）
// ENU坐标系下应改为 - [0,0,gravity]

 

寄存器拼接：IMU 传感器通常采用 16 位 ADC，但高分辨率传感器会将数据分成两个 8 位寄存器传输（MSB 和 LSB）。读取时需要拼接：

 

1
2
// 16位有符号整数拼接
int16_t raw = (int16_t)((uint16_t)high_byte << 8 | low_byte);

3.2.2 加速度计输出的本质：比力方程

这是惯导理论的核心概念，比力（Specific Force） 是惯导系统中的基本测量量。

比力定义（惯导标准）：

其中 $\boldsymbol{a}$ 是绝对运动加速度，$\boldsymbol{g}$ 是重力加速度（矢量），$\boldsymbol{f}$ 是比力。

⚠️ 物理直觉：加速度计无法感知万有引力，只能感知非引力加速度：

  • 自由落体：$\boldsymbol{a}=\boldsymbol{g} \Rightarrow \boldsymbol{f}=\boldsymbol{0}$（加速度计显示失重）
  • 水平静止：$\boldsymbol{a}=\boldsymbol{0} \Rightarrow \boldsymbol{f}=-\boldsymbol{g}$（比力向上，大小为 $g$）
  • 猛烈上提：$a>g \Rightarrow f>0$（加速度计显示超重）
  • 匀速飞行：$\boldsymbol{a}=\boldsymbol{0} \Rightarrow \boldsymbol{f}=-\boldsymbol{g}$（与静止相同）

从比力求真实加速度：

这意味着：测得的比力 + 重力 = 真实运动加速度（导航系下）。

⚠️ 比力不是运动加速度——这是初学者最常犯的错误！

比力必须投影到导航系，加上重力，才是运动加速度。

3.2.3 坐标系定义与重力矢量符号（极易混淆）

本文档采用 NED 坐标系（North-East-Down，北-东-地），这是航空航天领域的标准惯导坐标系，除此之外还有 ENU 也是导航领域的常见坐标系。

ENU vs NED 重力矢量对照表：

属性 ENU（东-北-天） NED（北-东-地）
Z 轴方向 向上 ↑ 向下 ↓
重力矢量 $\boldsymbol{g}^n$ $[0; 0; -g]$ $[0; 0; +g]$
水平静止加计 Z 轴读数 $+9.8$ m/s² $-9.8$ m/s²
重力补偿代码（速度更新） a_line = R*a - [0; 0; gravity] a_line = R*a + [0; 0; gravity]

快速判断代码坐标系：看重力扣除那行是减号还是加号。

  • a_line = R*a - [0,0, g] → ENU
  • a_line = R*a + [0,0, g] → NED

验证方法：将 IMU 水平静止放置，打印加速度计 Z 轴均值：

  • $+9.8$ → ENU 坐标系
  • $-9.8$ → NED 坐标系

⚠️ pitch 公式符号与坐标系：ENU 下 pitch = -atan2 (ax, sqrt (ay²+az²))，而 NED 下符号可能不同。必须严格对齐坐标系定义。

本设计采用 NED 坐标系（与航空航天惯导标准一致）。

3.2.4 初始对准：从静止段确定初始姿态

初始对准是惯导系统的第一步：利用静止时传感器输出，确定初始姿态四元数 $q_b^n (t_0)$。

原理：静止时，加速度计感受重力，陀螺仪输出≈0（忽略噪声）。

陀螺仪零偏估计：

其中 $N$ 是静止段采样数。在对准期间，陀螺仪测量的角速度即为零偏。

加速度计求 Roll/Pitch：

由 3.2.2 节，静止时 $\boldsymbol{f}^b \approx -\boldsymbol{g}^n$（NED 系）。由旋转矩阵反解：

注意：用 $\text{arctan}$ 而非 $\text{arcsin}$，数值更稳定。

初始 Yaw 设 0：初始时刻航向角设为 0（真北方向）。这是因为：

  1. 加速度计无法感知绕竖直轴的旋转
  2. 需要磁力计才能确定航向

欧拉角 → 四元数（ZYX 内旋顺序，Hamilton 约定，$q_b^n$，标量在前）：

注意：以下公式假设旋转顺序为 $Z (\psi) \to Y (\theta) \to X (\phi)$，即先 Yaw 再 Pitch 最后 Roll，与航空航天 NED 坐标系一致。

磁力计求航向角（如需要）：

磁力计测量地磁场在机体系下的投影 $m^b = [m_x, m_y, m_z]^T$。利用已知的 Roll 和 Pitch，将其投影到水平面：

加上本地磁偏角得到真航向。

对准时间：取决于 IMU 噪声和所需精度，一般 5~15 秒。对准期间载体必须保持静止。

3.3 惯导解算的数值实现

3.2 节建立了惯导解算的物理基础——传感器输出什么、坐标系怎么定、初始对准怎么做。本节回答第二个问题：怎么用数值方法把物理量一步步算出来？

姿态解算的核心任务：已知 $t$ 时刻的姿态 $q_b^n (t)$ 和陀螺仪测得的角速度 $\omega^b$ ，求 $t + \Delta t$ 时刻的姿态 $q_b^n (t+\Delta t)$ 。

这是一个初值问题 + 微分方程——给定初始姿态，通过对角速度积分递推姿态。

3.3.1 陀螺积分——姿态更新

陀螺仪测量的是机体系下的角速度 $\boldsymbol{\omega}^b = [\omega_x, \omega_y, \omega_z]^T$。

根据刚体转动动力学，在 Hamilton 约定下，表示机体系（$b$ 系）相对于导航系（$n$ 系）姿态变化的四元数 $\mathbf{q}_b^n$ 的微分方程（四元数的时间导数与角速度的关系）为：

其中 $\bar{\boldsymbol{\omega}}_{nb}^b = [0, \omega_x, \omega_y, \omega_z]^T$ 是角速度的纯四元数表示（标量部分为零）。

⚠️ 四元数约定说明：本设计定义四元数为 $\mathbf{q}_b^n$（从 $b$ 系到 $n$ 系的旋转），与旋转矩阵 $\mathbf{C}_b^n$ 下标方向一致——两者都表示”机体系→导航系”的变换。机体系角速度 $\boldsymbol{\omega}^b$ 右乘（$\mathbf{q}$ 右边乘 $\bar{\boldsymbol{\omega}}^b$），物理直觉：机体角速度是在当前姿态之后”追加”的小旋转，四元数乘法中”后追加=右乘”。如果角速度是导航系下的 $\boldsymbol{\omega}^n$，则应当左乘：$\dot{\mathbf{q}}_b^n = \frac{1}{2} \bar{\boldsymbol{\omega}}^n \otimes \mathbf{q}_b^n$。此约定与 PX4 ECL、OpenVINS、Sola ESKF、牛小骥团队讲义一致。

💡 为什么不用 $\mathbf{q}_n^b$？ 国内部分教材（如秦永元《惯性导航》）习惯将四元数标为 $\mathbf{q}_n^b$，但实际微分方程 $\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \circ \bar{\boldsymbol{\omega}}^b$ 是 $\mathbf{q}_b^n$ 的方程。使用 $\mathbf{q}_n^b$ 标签会导致四元数与 DCM”下标方向相反”（$\mathbf{q}_n^b$ 对应 $C_n^b$ 而非 $C_b^n$），在代码中需要额外转置，且容易引入符号混淆。统一为 $\mathbf{q}_b^n$ 后，四元数→DCM 直接给出 $C_b^n$，速度更新中 $\boldsymbol{f}^n = C_b^n \boldsymbol{f}^b$ 直译代码，无需任何中间转换。

展开为矩阵形式：

数值积分方法：

① 一阶欧拉法（最简单，最常用）：

优点：计算量最小，适合高频 IMU（1~8 kHz）。
缺点：一阶精度，长时间积分误差累积。

② 中点法 / 二阶龙格-库塔：

③ 旋转矢量法（最优，推荐用于低频场景）：

将 $\Delta t$ 内的旋转等效为绕某一轴旋转 $\Delta\theta$ 角：

其中 $\Delta\theta = \omega \cdot \Delta t$。然后：

本设计的选择：IMU 输出 1~8 kHz，$\Delta t$ 极小（0.125~1 ms），一阶欧拉法的截断误差 $\sim O (\Delta t^2)$ 已经挺小的了。为节省算力，主循环用一阶欧拉法，每步后归一化。

积分后必须归一化：

快速逆平方根：归一化需要计算 $1/\sqrt{x}$，经典方法使用”魔数”近似（Quake III 源码中的 0x5f3759df），一次牛顿迭代即可达到浮点精度。

高精度姿态更新方程：

完整姿态微分方程：

其中相对角速度：

各项含义：

  • $\omega_{ie}^n$：地球自转角速度，大小为 $15.041^\circ/\text{h}$（≈ 0.004178°/s 或 72.9 μrad/s）
  • $\omega_{en}^n$：运载角速度（Transport Rate），因地球曲率产生的角速度

其中 $L$ 是纬度，$\omega_e = 7.2921150 \times 10^{-5}$ rad/s。

⚠️ 消费级 MEMS 忽略地球自转和运载角速度的原因：地球自转信号（72.9 μrad/s）远小于消费级陀螺仪的噪声密度（通常 1~10 mrad/s/√Hz）。地球自转在陀螺仪噪声中完全被淹没，强行补偿只会增加计算复杂度而不会提升精度。

圆锥误差与补偿：

当机体做高频角振动时（如无人机螺旋桨振动），旋转不可交换误差（圆锥误差）会累积。旋转矢量法天然处理了这个问题。对于更高精度的应用，可采用双子样圆锥补偿：

3.3.2 速度更新——从比力到速度

速度更新的核心是将机体系测量的比力 $\boldsymbol{f}^b$，通过姿态旋转矩阵 $C_b^n$ 投影到导航系，加上重力，得到速度变化率。

基础模型（MEMS 级）：

其中 $C_b^n = (C_n^b)^T$ 是机体系到导航系的旋转矩阵（NED 系），$\dot{\boldsymbol{v}}^n$ 是速度的时间导数（即线运动加速度）。

展开为代码形式（NED 系）：

1
2
3
4
5
6
7
8
// 将比力从机体系投影到导航系
f_n = C_b_n * f_b;

// 补偿重力，得到线运动加速度（a = f + g，NED 下 g^n=[0; 0;+g]）
a_line = f_n + [0; 0; gravity];  // NED: 重力项为正（"补偿"而非"扣除"）

// 速度更新（梯形积分）
v += (v_prev + v) * 0.5 * dt;

工业级完整模型：

其中：

  • $(2\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n$：哥氏加速度项（Coriolis acceleration）
  • $\boldsymbol{g}_l^n$：当地有效重力矢量（包含正常重力+重力异常，与纯万有引力 $g$ 略有差异）

哥氏项的物理含义：物体在旋转坐标系（地球）中的运动，会产生额外的加速度。

⚠️ 消费级 MEMS 忽略哥氏加速度项的原因：哥氏加速度 $2\omega_e \times v$ 最大量级约为 $2 \times 7.3 \times 10^{-5} \times 300 \approx 0.044$ m/s²（以高速飞机 300 m/s 计算）。消费级加速度计零偏（~0.1 m/s²）已经远大于这个值，补偿没有意义。

数值积分方法：

① 矩形法（一阶欧拉）：

② 梯形法（二阶精度，推荐）：

梯形法在同等计算量下精度更高，因为考虑了速度变化的趋势。

划桨误差（Sculling Error）：

当机体同时存在角振动和线振动时（如无人机飞行），会产生划桨误差。高频下积分会引入额外误差。高精度解算需要进行划桨补偿，利用角增量和速度增量的叉乘项。

⚠️ 速度误差增长规律：

误差源 速度误差增长
加速度计零偏 $\delta b_a$ $\delta v \approx \delta b_a \cdot t$（线性增长）
刻度因子误差 $\delta s_a$ 与加速度大小成正比

以消费级 IMU 为例（$\delta b_a \approx 1$ mg = 0.01 m/s²）：

  • 1 秒后速度误差 ≈ 0.01 m/s
  • 60 秒后速度误差 ≈ 0.6 m/s

3.3.3 位置更新——从速度到位置

基础模型：

数值积分（梯形法）：

⚠️ 位置误差增长规律：

误差源 位置误差增长
加速度计零偏 $\delta b_a$ $\delta p \approx \frac{1}{2}\delta b_a \cdot t^2$（二次方增长！）

以消费级 IMU 为例（$\delta b_a \approx 1$ mg）：

  • 5 秒后位置误差 ≈ ½ × 0.01 × 25 ≈ 0.125 m
  • 10 秒后位置误差 ≈ ½ × 0.01 × 100 ≈ 0.5 m
  • 60 秒后位置误差 ≈ ½ × 0.01 × 3600 ≈ 18 m

结论：纯惯导位置解算在消费级 IMU 上完全不可靠，60 秒内误差可达十几米。必须融合 GNSS 等外部传感器。

高精度位置更新（WGS-84 椭球模型）：

高精度惯导使用地球椭球模型（WGS-84），纬度、经度、高度的变化率为：

其中：

  • $L, \lambda, h$：纬度、经度、大地高
  • $R_M$：子午圈曲率半径，$R_M = \frac{a (1-e^2)}{(1-e^2\sin^2 L)^{3/2}}$
  • $R_N$：卯酉圈曲率半径，$R_N = \frac{a}{\sqrt{1-e^2\sin^2 L}}$
  • $a = 6378137.0$ m（WGS-84 长半轴）
  • $e^2 = 0.00669437999014$（椭球扁率）

⚠️ 本设计不需要椭球模型的原因：本设计定位为高端消费级/工业级无人机应用，纯惯导积分时间较短，且最终融合 GNSS 定位。因此使用简单的平面投影模型即可满足精度需求，不需要复杂的椭球计算了。

3.3.4 杆臂效应与尺寸效应

📌 工程分级：本节内容属于 L2+ 级别（战术级/高精度应用），消费级 MEMS 场景下三颗 IMU 间距仅 1~2cm，产生的杆臂加速度在中低速机动下约 $\sim 0.05$ m/s²，可安全忽略。仅在高速旋转或需要极高精度时才需补偿。

物理本质：当 IMU 不是安装在载体的质心（CG）时，载体的旋转运动会通过杠杆臂产生额外的加速度。对于本设计的多 IMU 场景，各 IMU 芯片不在同一空间位置，也需要将各 IMU 数据归一化到同一虚拟中心点。

设 IMU 相对于虚拟中心点的位置矢量为 $\boldsymbol{r}^b = [r_x, r_y, r_z]^T$（在机体系下，可在 PCB EDA 软件中测量），则杠杆臂产生的附加加速度为：

  • 第一项 $\dot{\boldsymbol{\omega}} \times \boldsymbol{r}$：切向加速度，由角加速度产生
  • 第二项 $\boldsymbol{\omega} \times (\boldsymbol{\omega} \times \boldsymbol{r})$：向心加速度，由角速度产生

多 IMU 场景的补偿公式（将各 IMU 数据统一到虚拟中心点）：

其中 $\boldsymbol{r}_i$ 为第 $i$ 颗 IMU 芯片中心相对于虚拟中心点的三维坐标向量，$\boldsymbol{\omega}_{\text{fused}}$ 为当前融合角速度，$\dot{\boldsymbol{\omega}}_{\text{est}}$ 为角加速度估计值。

角加速度 $\dot{\boldsymbol{\omega}}_{\text{est}}$ 的获取：对于本设计，推荐采用简单的一阶差分+低通滤波：

⚠️ 为什么不用分布式 IMU 差分求角加速度？ 理论上，两颗不在同一位置的 IMU 的加速度差除以间距可以得到角加速度，但信噪比极差（加速度计噪声远大于角加速度信号），工程上不可行。

在导航系下：

高精度解法：精确测量各 IMU 相对于质心的 3D 矢量 $r_i^b$，在比力方程中实时补偿：

⚠️ 杆臂效应是否纳入 ESKF 扩展：此设计将杆臂效应作为L2+ 级可选扩展。在标准 15 维 ESKF 中，杆臂通常是固定参数（标定阶段确定），不需要扩展状态维度。如果需要在线估计杆臂参数，可扩展为 18 维误差状态（增加 3 维杆臂误差），但这超出了本设计 L1 级工程范围。

3.3.5 传感器的高级误差：g 敏感性与温度补偿

g 敏感性漂移（g-sensitivity）：

当载体做高 g 机动时，陀螺仪结构会发生微小弹性形变，导致输出额外正比于加速度的零偏：

其中 $K_g$ 是 3×3 g 敏感度矩阵（单位：rad/s/(m/s²)）。

补偿方法：用加速度计输出实时估计并扣除：

⚠️ 消费级 IMU 的处理方式：消费级 MEMS 的 g 敏感性通常在规格书中标注为典型值，不会进行离线标定。其影响在高动态场景下可纳入陀螺仪零偏状态量，在 EKF 中在线估计。

温度补偿：

传感器的零偏和刻度因子随温度非线性漂移：

补偿方法：

  1. 出厂标定：在温箱中多点测量，拟合 3~5 次多项式系数
  2. 在线补偿：代码中通过温度传感器实时查表或计算补偿值
1
2
3
4
// 温度补偿示例
float temperature = read_temperature ();
float compensated_bias = bias_25c + k1 * (temperature - 25.0f) 
                                + k2 * powf (temperature - 25.0f, 2);


 

⚠️ 消费级 IMU 的温度漂移：消费级 IMU 通常不进行出厂温箱标定，而是将温漂参数纳入 EKF 的零偏状态量进行在线估计。这是消费级传感器的标准工程实践。

3.3.6 工程分级与嵌入式应用范式

在实际嵌入式工程中，IMU 的使用方式绝非”一刀切”——传感器精度、算力资源和应用场景的差异，催生了从消费级到导航级的完全不同的算法架构。

三层工程分级表：

层级 姿态更新 速度更新 位置更新 适用场景
L1：消费级 MEMS 一阶欧拉积分 + 四元数归一化；忽略地球自转和运载项 梯形积分；忽略哥氏加速度；重力补偿仅用本地 $g$ 平面投影 + 梯形积分 短时（<60s）纯惯导或 AHRS
L2：工业级（战术） 旋转矢量法 + 双子样圆锥补偿 加入哥氏加速度项 $-2\omega_{ie}^n \times v$（可忽略 $\omega_{en}$）；梯形或更高阶积分 可选 WGS-84 椭球（长距离时） 光纤陀螺、长航时（数小时）
L3：高精度（导航级） 完整姿态方程 $\omega_{nb}^b = \omega_{ib}^b - C_n^b (\omega_{ie}^n + \omega_{en}^n)$ + 多子样圆锥补偿 完整哥氏项 $-(2\omega_{ie}^n + \omega_{en}^n) \times v$ + 划桨补偿 + 杠杆臂补偿 WGS-84 椭球 + 重力异常模型 航空/潜器、测绘、长航时数天

本设计定位：本文档主要针对 L1（消费级 MEMS） 场景，但在各节末尾已注明向 L2/L3 扩展的方法。

嵌入式系统中的两种 IMU 应用范式：

在工程实践中，两种范式有本质区别：

维度 范式一：低成本 MEMS（百元级） 范式二：高精度惯导（万元至十万元级）
姿态层 互补滤波（Mahony/Madgwick）或轻量级 3 轴/6 轴卡尔曼滤波 全通道捷联解算（SINS）
速度/位置层 通常放弃纯惯导积分，速度位置完全依赖外部传感器（GNSS/DVL/激光雷达） 保留完整姿态-速度-位置三通道纯惯导递推
融合机制 互补滤波直接修正姿态角 ESKF 估计误差状态，反馈修正捷联解算回路
IMU 加速度计角色 主要提供重力矢量参考（Roll/Pitch），不参与积分 比力积分是速度/位置通道的核心
无外部信号时 姿态由陀螺短时维持（秒级），速度位置立即失效 纯惯导可维持高精度导航（分钟~小时级）

为什么范式一”砍掉”速度位置积分？ 由 3.3.5 节的误差增长分析可知，消费级 IMU（$\delta b_a \approx 1$ mg）60 秒位置漂移已达 18m，积分毫无意义。姿态通道因陀螺短期精度尚可，可通过互补滤波维持；但速度位置通道的积分误差发散过快，必须依赖外部传感器（GNSS 提供速度位置，气压计提供高度）。

 

为什么范式二必须保留全通道？ 在水下、地下或电磁对抗环境中，GPS 信号不可用。此时必须靠纯惯导的完整三通道递推来维持导航，ESKF 利用偶尔的外部基准（声学定位 USBL、多普勒 DVL）修正漂移。高精度 IMU 的零偏极小，纯惯导可以在无外部信号时维持较长时间的可用精度。

一句话总结：消费级系统是”姿态靠 IMU，位置靠外部”的分工架构；高精度系统是”IMU 全通道解算 + 外部信号纠偏”的闭环架构。选择哪种范式，取决于你的传感器等级和应用场景。

3.3.7 完整的惯导解算链路总结

三个通道的统一总结表：

通道 基础模型（MEMS 级） 工业级扩展 高精度扩展
姿态 $\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \bar{\boldsymbol{\omega}}$，一阶欧拉法 圆锥补偿 + 旋转矢量法 地球自转补偿 + 运载角速度补偿
速度 $\dot{\boldsymbol{v}}^n = C_b^n \cdot \boldsymbol{f}^b + \boldsymbol{g}^n$，梯形积分 哥氏加速度补偿 $(2\boldsymbol{\omega}_{ie}+\boldsymbol{\omega}_{en})\times \boldsymbol{v}$ 划桨补偿 + 杠杆臂补偿
位置 $\dot{\boldsymbol{p}}^n = \boldsymbol{v}^n$，梯形积分 平面投影 WGS-84 椭球模型 + 重力异常模型

误差增长规律总结：

误差源 姿态误差 速度误差 位置误差
陀螺零偏 $\delta b_g$ $\delta\phi \approx \delta b_g \cdot t$（线性） $\delta v \approx g \cdot \delta b_g \cdot t^2$ $\delta p \approx \frac{1}{2}g \cdot \delta b_g \cdot t^3$
加计零偏 $\delta b_a$ — $\delta v \approx \delta b_a \cdot t$（线性） $\delta p \approx \frac{1}{2}\delta b_a \cdot t^2$

⚠️ 消费级 IMU 误差预测（典型值：$\delta b_g \approx 1^\circ/\text{s}$，$\delta b_a \approx 1$ mg）：

时间 姿态误差（陀螺漂移） 速度误差（加计零偏） 位置误差（加计零偏）
1s ~1° ~0.01 m/s ~0.005 m
10s ~10° ~0.1 m/s ~0.5 m
60s ~60°（完全不可用） ~0.6 m/s ~18 m（完全不可用）

惯导解算的本质——误差逐级放大链路：

1
2
3
4
5
姿态误差 (δφ) 
    ↓ 重力投影
速度误差 (δv) ≈ g·δφ·t
    ↓ 积分
位置误差 (δp) ≈ ½g·δφ·t²

一句话总结：惯导解算的三个通道（姿态→速度→位置）是一个误差逐级放大的链路——姿态误差通过重力投影污染速度，速度误差积分放大为位置误差。这就是为什么纯惯导不可行、必须融合外部传感器的原因。

各传感器独立解算的致命局限：

传感器 能解算什么 致命缺陷
陀螺仪（积分） Roll, Pitch, Yaw（全姿态） 积分漂移——60 秒漂移 60°，不可用
加速度计 Roll, Pitch（仅静态） 动态下严重失真，且无法求 Yaw
磁力计 Yaw（需已知水平姿态） 受磁干扰，高纬度失效，依赖准确水平姿态

核心矛盾：

  • 陀螺仪短期准、长期漂 → 需要绝对基准来修正
  • 加速度计/磁力计长期准（静态下）、短期差 → 提供绝对基准但噪声大

3.4 为什么需要数据融合

3.4.1 从频域看互补性

理解数据融合的关键是频域思维：

861

1
2
3
4
5
6
7
8
频率 (Hz)
  │
  │    陀螺仪积分                   加速度计/磁力计
  │    ████████████████            ░░░░░░░░░░░░░░░░
  │    高频段可信                    低频段可信
  │    （短时精度高）                （长期基准准）
  │                                  ───────────────→
  0                                 交越频率 fc       ∞
  • 陀螺仪积分的功率谱：白噪声积分后谱密度按 $1/f^2$ 衰减（低频误差大），但高频响应精确
  • 加速度计/磁力计的功率谱：静态下低频信号准确（重力/地磁场是 DC 量），但高频噪声大

融合的本质：设计一个互补滤波器，让陀螺仪信号通过高通，让加速度计/磁力计信号通过低通，两者在频域上无缝拼接：

其中 $\omega_c$ 是交越频率（crossover frequency），决定了”多大程度上信任陀螺 vs 信任加计”。

  • $\omega_c$ 大 → 更信任加计（低通带宽宽）→ 响应慢但噪声小
  • $\omega_c$ 小 → 更信任陀螺（高通带宽宽）→ 响应快但漂移大

类比：就像两只耳朵——陀螺是”近处听得清”的耳朵，加计是”远处听得清”的耳朵。融合就是两耳朵一起听，近远都清楚。

3.4.2 为什么不能简单加权平均

我刚开始会想：直接 $\hat{\theta} = \alpha \cdot \theta_{\text{gyro}} + (1-\alpha) \cdot \theta_{\text{acc}}$ 不就行了？

不行，因为：

  1. 陀螺仪积分误差是累积的：$\theta_{\text{gyro}} = \theta_{\text{true}} + \int b \, dt$，零偏 $b$ 导致角度线性增长的误差。简单加权平均只能”稀释”误差，无法消除。
  2. 加速度计在高动态下完全不可信：无人机加速时，加速度计读数偏移可能达数 g，此时如果给它 $(1-\alpha)$ 的权重，姿态估计会严重偏差。需要动态调整权重——加速度大时降低加计权重。
  3. 航向角的特殊性：加速度计无法提供航向，磁力计提供航向但有磁干扰。需要独立的融合通道。

因此，融合算法需要做到：

  • 在角速度层面修正陀螺仪（而非角度层面简单加权）
  • 能在线估计陀螺零偏，消除累积漂移
  • 能自适应调整观测权重（动态大时降权，磁异常时降权）

3.5 一阶互补滤波

最简单的融合算法，帮助建立直觉。

3.5.1 基本形式

以单轴为例（多轴独立处理）：

其中：

  • $\hat{\theta}_k + \omega_k \Delta t$ 是陀螺积分预测
  • $\theta_{\text{acc}, k}$ 是加速度计解算角
  • $\alpha$ 是融合系数，通常取 0.98~0.998

等价形式（更直观）：

解读：先让陀螺积分往前推一步，然后用加速度计和陀螺积分之间的偏差来”拉回”。

3.5.2 频域分析

对连续时间形式做拉普拉斯变换：

其中 $\tau = \frac{\alpha \Delta t}{1 - \alpha}$ 是时间常数。

  • 陀螺通道传递函数：$H_{\text{gyro}}(s) = \frac{\tau s}{\tau s + 1}$（高通）
  • 加计通道传递函数：$H_{\text{acc}}(s) = \frac{1}{\tau s + 1}$（低通）
  • $H_{\text{gyro}}(s) + H_{\text{acc}}(s) = 1$（互补性，无信息丢失）

交越频率 $f_c = \frac{1}{2\pi\tau}$。$\alpha = 0.98$、$\Delta t = 0.01$s 时，$\tau = 0.49$s，$f_c \approx 0.33$ Hz——0.33 Hz 以下信任加计，以上信任陀螺。

3.5.3 局限性

  1. 无法估计零偏：只修正角度，不修正角速度，陀螺零偏持续累积
  2. 固定权重：$\alpha$ 恒定，无法根据动态条件自适应
  3. 单轴独立处理：三轴姿态解算独立进行，忽略了轴间耦合
  4. 无法融合磁力计：只处理 Roll/Pitch，航向角没有绝对基准修正

一阶互补滤波是”入门款”——它帮你理解融合的频域本质，但在工程中几乎不用。下面进入正式的飞控级算法。

3.6 DCM 互补滤波（方向余弦矩阵法）

DCM 互补滤波由 William Premerlani 和 Paul Bizard 于 2009 年在论文 “Direction Cosine Matrix IMU: Theory” 中提出，是开源飞控社区第一种工程可用的全姿态融合算法，曾是 ArduPilot 早期版本的核心算法。虽然今天已被 Mahony 和 EKF 取代，但它是从一阶互补到 Mahony 的关键过渡——Mahony 的叉积误差 + PI 控制器，正是从 DCM 算法得来的。

一句话定位：DCM 算法 = 用旋转矩阵替代欧拉角（消除万向锁）+ 向量叉积误差 + PI 控制器（零偏估计）+ 正交化（维持矩阵约束）。下一节的 Mahony = DCM 的误差修正框架 + 四元数替代旋转矩阵（消除正交化）。

3.6.1 从一阶互补到 DCM——解决万向锁

一阶互补滤波用欧拉角表示姿态，三个轴独立处理。致命问题：

  1. 万向锁：Pitch ≈ ±90° 时 Roll 和 Yaw 耦合，姿态解算崩溃
  2. 轴间耦合被忽略：三个轴独立滤波，但实际旋转是耦合的

DCM 算法的核心改进：用 3×3 方向余弦矩阵 $C_b^n$ 替代欧拉角。

  • 3.1.2 节已经介绍过 DCM 作为姿态数学表示——它没有万向锁、天然处理轴间耦合
  • 现在的问题是：如何递推更新 DCM，并用加速度计/磁力计修正漂移？

3.6.2 DCM 的递推更新

连续形式——DCM 的微分方程：

其中 $[\boldsymbol{\omega}_{nb}^b \times]$ 是角速度的反对称矩阵（skew-symmetric matrix）：

直观理解：$C_b^n \cdot [\boldsymbol{\omega} \times]$ 的含义——当前姿态矩阵乘以角速度的反对称矩阵，就是在当前姿态基础上叠加一个小旋转。这与四元数微分方程 $\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \bar{\boldsymbol{\omega}}$ 的思想完全一致，只是矩阵语言 vs 四元数语言。

离散一阶更新（对应一阶欧拉法）：

注意：$\mathbf{I} + [\boldsymbol{\omega} \times] \Delta t$ 只是旋转矩阵的一阶近似，长时间积分会导致 $C_b^n$ 偏离 SO (3) 群（不再正交）。这就是后面要讲的正交化问题的根源。更精确的方法可用罗德里格斯公式（3.1.4 节的等效旋转矢量）：

3.6.3 误差计算——叉乘误差的诞生

这是 DCM 算法最核心的创新，也是后来被 Mahony 直接继承的部分。

问题：陀螺积分的 DCM 会漂移，需要用加速度计和磁力计修正。

方法：

步骤 1：由当前 DCM 计算机体系下的估计重力方向

其中 $\hat{\boldsymbol{g}}^n = [0,0,1]^T$ 是 NED 系下归一化重力方向。$C_n^b$ 的第三列就是”如果 DCM 准确，重力在机体系应该指向的方向”。

步骤 2：归一化加速度计读数

步骤 3：计算叉乘误差

这和 Mahony 完全一样！ 叉乘误差 $\boldsymbol{e}$ 的方向是”需要绕哪个轴旋转”才能让估计重力对齐到测量重力，大小近似等于偏差角。

如果有磁力计，同样计算磁力计误差 $\boldsymbol{e}_m = \hat{\boldsymbol{m}} \times \hat{\boldsymbol{b}}$，总误差 $\boldsymbol{e}_{\text{total}} = \boldsymbol{e}_a + \boldsymbol{e}_m$。

历史注记：叉乘误差 + PI 控制器的框架，最早由 Premerlani 在 DCM 算法中以工程直觉提出。Mahony 2008 年的论文从 SO (3) 流形上的非线性互补滤波理论给出了更严谨的数学推导，但工程框架与 DCM 算法本质相同。我最近接触航姿解算的互补滤波这个框架时也是从 Mahony 开始的，但在查阅 Ardupilot 代码是突然还想 DCM 应该是误差叉乘抑制零飘和矫正陀螺的前身。

3.6.4 PI 控制器修正角速度

与 Mahony 完全相同：

比例项（即时修正）：$\boldsymbol{\omega}_p = K_p \cdot \boldsymbol{e}$

积分项（零偏估计）：$\boldsymbol{b}_{k+1} = \boldsymbol{b}_k + K_i \cdot \boldsymbol{e} \cdot \Delta t$

修正后的角速度：$\boldsymbol{\omega}_{\text{corr}} = \boldsymbol{\omega}_{\text{meas}} - K_p \cdot \boldsymbol{e} - \boldsymbol{b}_{k+1}$

然后用修正后的角速度更新 DCM：

**再次感慨：PI 控制器的结构、参数含义、零偏估计原理，DCM 和 Mahony 完全一致。区别仅在于”PI 修正后的角速度如何更新姿态”——DCM 用矩阵乘法，Mahony 用四元数乘法。

3.6.5 正交化——DCM 的阿喀琉斯之踵

问题：每步数值积分后，$C_b^n$ 不再严格属于 SO (3) 群——行/列向量不再两两正交、不再单位长度。随着时间积累，正交性偏差越来越大，姿态矩阵”变质”。

这是 DCM 算法最大的工程负担，也是 Mahony 用四元数替代 DCM 的根本动机。

Premerlani 的正交化方法：

设 DCM 的三行为 $\boldsymbol{r}_1, \boldsymbol{r}_2, \boldsymbol{r}_3$（分别是机体系 X/Y/Z 轴在导航系下的单位向量），应满足 $\boldsymbol{r}_i \cdot \boldsymbol{r}_j = \delta_{ij}$。

第一步：修正正交性误差

计算正交性偏差：$\epsilon = \boldsymbol{r}_1 \cdot \boldsymbol{r}_2$（理想情况下应为 0）

将偏差均分到两行：

第二步：用叉积确保第三行严格正交

第三步：归一化

计算量：正交化本身约需 ~30 FLOPs，加上 DCM 更新的 ~50 FLOPs，总计 ~80 FLOPs/步。相比之下，Mahony 用四元数更新 + 归一化只需 ~55 FLOPs/步。

为什么四元数不需要正交化？ 四元数只有一个约束 $|\mathbf{q}| = 1$，归一化只需一次除法。而 3×3 矩阵有 6 个正交约束（3 个正交性 + 3 个单位长度），维护这 6 个约束就是正交化的全部工作。四元数把 9 个元素压缩到 4 个，约束从 6 个减到 1 个——这是表示效率的巨大提升。

3.6.6 完整算法流程

输入：陀螺仪 $\boldsymbol{\omega}_{\text{meas}} = [\omega_x, \omega_y, \omega_z]^T$，加速度计 $\boldsymbol{a}_{\text{meas}} = [a_x, a_y, a_z]^T$，磁力计 $\boldsymbol{m}_{\text{meas}} = [m_x, m_y, m_z]^T$（可选）

状态：3×3 DCM 矩阵 $C_b^n$，零偏积分 $\boldsymbol{b} = [b_x, b_y, b_z]^T$

每步更新：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
① 归一化加速度计（磁力计同理）
   若 ‖a_meas‖ ≠ 0: â = a_meas / ‖a_meas‖

② 由 C_b^n 计算估计重力方向
   v̂ = (C_b^n)^T · [0, 0, 1]^T    // 即 C_n^b 的第三列

③ 计算叉乘误差
   e = â × v̂
   （如果有磁力计，同样计算 e_m 并加入 e）

④ PI 修正
   b ← b + Ki · e · Δt             // 积分项更新
   ω_corr = ω_meas - Kp·e - b     // 修正角速度

⑤ DCM 更新（一阶欧拉法）
   C_b^n ← C_b^n · (I + [ω_corr×] · Δt)

⑥ 正交化
   ε = r1 · r2                     // 正交性偏差
   r1 ← r1 - (ε/2)·r2             // 修正 r1
   r2 ← r2 - (ε/2)·r1             // 修正 r2
   r3 ← r1 × r2                   // 叉积保证正交
   r1 ← r1/‖r1‖,  r2 ← r2/‖r2‖,  r3 ← r3/‖r3‖  // 归一化

⑦ 转欧拉角输出（仅在需要时）
   φ = atan2 (C[2][1], C[2][2])
   θ = -asin (C[2][0])
   ψ = atan2 (C[1][0], C[0][0])

⚠️ 步骤⑦的欧拉角提取公式假设 $C_b^n$ 是 ZYX 内旋顺序的被动旋转矩阵。不同旋转顺序或不同定义（主动/被动）的公式不同，务必与 3.1.6 节的约定一致。

3.6.7 DCM → Mahony 的演进逻辑

理解了 DCM，再去看 Mahony 就非常清晰——Mahony 本质上就是把 DCM 的旋转矩阵换成四元数：

演进项 DCM Mahony 改进效果
姿态表示 3×3 矩阵（9 元素） 四元数（4 元素） 元素减半
误差计算 向量叉积 $\hat{\boldsymbol{a}} \times \hat{\boldsymbol{v}}$ 完全相同 —
修正机制 PI 控制器 完全相同 —
零偏估计 $K_i \int \boldsymbol{e}\, dt$ 完全相同 —
约束维护 正交化（6 约束） 归一化（1 约束） 大幅简化
计算量 ~80 FLOPs/步 ~55 FLOPs/步 减少 ~30%

核心结论：Mahony 并非凭空发明，而是 DCM 的”四元数重构版”。DCM 的叉积误差 + PI 控制器是整个互补滤波家族的灵魂，Mahony 只是换了更好的”容器”（四元数 vs 矩阵）。

1
2
3
4
5
6
7
8
 一阶互补          DCM                Mahony            Madgwick
(频域分离)     (叉积+PI+矩阵)      (叉积+PI+四元数)    (梯度下降+四元数)
    │               │                   │                  │
    │  +旋转矩阵     │  +四元数替代矩阵   │  +梯度替代 PI      │
    │  +叉积误差     │  +归一化替代正交化 │  +优化框架        │
    │  +PI 零偏估计   │                   │                  │
    ▼               ▼                   ▼                  ▼
 教学入门      已淘汰/教学参考      消费级飞控主流      IMU 库/开源项目

3.7 Mahony 互补滤波

Mahony 滤波由 Robert Mahony 等人于 2008 年提出 [1] ，是开源飞控领域最具影响力的姿态估计算法之一。它是 Betaflight 的核心算法，也曾在 ArduPilot 和 PX4 的早期版本中广泛使用，但在后两者中目前已被 EKF 系列替代为主力方案。

核心创新：在四元数流形上做 PI 反馈控制，直接修正角速度，而非修正角度。

与 DCM 的关系：如果你已经读了 3.6 节的 DCM 算法，会发现 Mahony 的叉积误差 + PI 控制器与 DCM 完全相同。Mahony 的本质改进只有一点——用四元数替代旋转矩阵，从而把”正交化”（6 约束）简化为”归一化”（1 约束）。理解了这一点，Mahony 就不是”新算法”，而是 DCM 的”四元数重构版”。

3.7.1 核心思想

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
       ┌─────────────────────────────────────────────────┐
       │              Mahony 滤波核心回路                  │
       │                                                 │
ω_meas ──→ [减零偏] ──→ [加 PI 修正] ──→ ω_corrected         │
       │       ↑              ↑                          │
       │    b_gyro        Kp·e + Ki·∫e                   │
       │       ↑              ↑                          │
       │    [积分项]      [叉乘误差 e]                     │
       │       ↑              ↑                          │
       │    Ki·∫e dt    a_meas × ĝ(估计重力)              │
       │                      ↑                          │
       │              ┌──────────────────┐               │
       │              │ 由当前四元数 q     │                │
       │              │ 推算重力方向ĝ     │                │
       │              └──────────────────┘               │
       │                                                 │
ω_corrected ──→ [四元数微分方程积分] ──→ q_{k+1}            │
       │                                                 │
       └─────────────────────────────────────────────────┘

三步循环：

  1. 预测：用陀螺仪角速度更新四元数
  2. 误差计算：用加速度计（和磁力计）计算姿态误差
  3. 修正：用 PI 控制器将误差反馈到角速度，再更新四元数

3.7.2 误差向量推导——叉乘误差的物理意义

关键洞察：如果当前四元数 $q$ 完全准确，那么由 $q$ 推算出的重力方向应该和加速度计测量完全一致。如果不一致，说明 $q$ 有偏差——偏差的方向和大小可以用向量叉乘来衡量。

步骤 1：由当前四元数 $q$ 计算机体系下的估计重力方向

在导航系中重力方向为 $\boldsymbol{g}^n = [0, 0, 1]^T$（NED 系，归一化后），则机体系下：

这就是”如果姿态四元数正确，重力应该在机体系的哪个方向”。

步骤 2：归一化加速度计读数

如果载体静止，$\hat{a}$ 应该和 $\hat{v}$ 同方向。

步骤 3：计算叉乘误差

叉乘误差的物理意义：

  • $\boldsymbol{e}$ 的方向：从 $\hat{\boldsymbol{a}}$ 指向 $\hat{\boldsymbol{v}}$ 的旋转轴——也就是”需要绕哪个轴旋转”才能让估计重力对齐到测量重力
  • $\boldsymbol{e}$ 的大小：$|\boldsymbol{e}| = |\hat{\boldsymbol{a}}||\hat{\boldsymbol{v}}|\sin\alpha \approx \sin\alpha \approx \alpha$（当偏差角 $\alpha$ 很小时）——也就是偏差角的大小

直观理解：想象你闭眼猜一个方向（$\hat{v}$），然后睁开眼看实际方向（$\hat{a}$）。叉乘告诉你”偏了多少、往哪边偏”——这就是误差。PI 控制器根据这个误差调整你的猜测，让你越来越准。

如果还有磁力计，同理可计算磁力计误差：

其中 $\hat{\boldsymbol{m}}$ 是归一化的磁力计测量，$\hat{\boldsymbol{b}}$ 是由 $\mathbf{q}$ 推算的机体系下地磁场方向。总误差：

3.7.3 PI 控制器修正陀螺

得到误差向量 $\boldsymbol{e}$ 后，用 PI 控制器生成角速度修正量：

比例项（即时修正）：

积分项（零偏估计）：

修正后的角速度：

为什么积分项能估计零偏？ 陀螺零偏 $b_{\text{gyro}}$ 导致积分预测的姿态持续偏移，偏移方向由叉乘误差 $e$ 反映。积分项 $b = K_i \int e \, dt$ 持续积累这个偏移信号，最终收敛到 $b \approx b_{\text{gyro}}$，从测量值中减去零偏，漂移被消除。这是 Mahony 最精妙的设计——用积分项实现了零偏在线估计。

3.7.4 完整算法流程

输入：陀螺仪 $\omega_{\text{meas}} = [\omega_x, \omega_y, \omega_z]^T$，加速度计 $a_{\text{meas}} = [a_x, a_y, a_z]^T$，磁力计 $m_{\text{meas}} = [m_x, m_y, m_z]^T$（可选）

状态：四元数 $q = [q_0, q_1, q_2, q_3]^T$，零偏积分 $b = [b_x, b_y, b_z]^T$

每步更新：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
① 归一化加速度计（磁力计同理）
   若 ‖a_meas‖ ≠ 0: â = a_meas / ‖a_meas‖

② 由 q 计算估计重力方向 v̂
   v̂_x = 2 (q1·q3 - q0·q2)
   v̂_y = 2 (q2·q3 + q0·q1)
   v̂_z = q0² - q1² - q2² + q3²

③ 计算叉乘误差（误差 = 测量向量 × 估计向量（叉积））
   e = â × v̂
   （如果有磁力计，同样计算 e_m 并加入 e）$e = a_{meas}​ × a_{est} + m_{meas​} × m_{est}$ ​

④ PI 修正
   b ← b + Ki · e · Δt       // 积分项更新
   ω_corr = ω_meas - Kp·e - b  // 修正角速度

   ⚠️ 符号陷阱：这里 ω_corr = ω_meas - Kp·e - b 用的是减法，
   因为误差定义为 e = â × v̂。如果你去看 Pixhawk（PX4/ArduPilot）
   或 Betaflight 的源码，会发现代码里写的是 gx += Kp * ex + Ki * ix
   （加法），因为开源飞控的误差定义相反：e = v̂ × â。
   叉乘不满足交换律——â × v̂ = -(v̂ × â)，差一个负号。
   本文档的减法与叉乘定义 e = â × v̂ 自洽，但写代码时务必
   与你选定的误差定义方向统一，否则姿态会往反方向修正！

⑤ 四元数积分（一阶欧拉法）
   q̇ = ½ q ⊗ [0, ω_corr]
   q ← q + q̇ · Δt

⑥ 归一化
   q ← q / ‖q‖

⑦ 转欧拉角输出（仅在需要时）
   φ = atan2 (2 (q0q1+q2q3), 1-2 (q1²+q2²))
   θ = arcsin (2 (q0q2-q3q1))
   ψ = atan2 (2 (q0q3+q1q2), 1-2 (q2²+q3²))

3.7.5 参数调优指南

参数 推荐范围 作用 调参原则
$K_p$ 1.0~5.0 比例增益，控制修正速度 $K_p$ 越大，加速度计修正越快，但噪声也越大；一般 $K_p \approx 2 \times$ 预期最大角速度（rad/s）
$K_i$ 0.001~0.1 积分增益，控制零偏估计速度 $K_i \approx 0.01 \times K_p$；$K_i = 0$ 退化为纯互补滤波（不估计零偏）
$\Delta t$ IMU 采样周期 步长 越小越好，1~8 kHz

本设计建议参数（基于 ICM-42688-P 1kHz 输出）：

  • $K_p = 2.0$（对静态误差的响应时间约 0.5s）
  • $K_i = 0.01$（零偏估计收敛时间约几十秒）
  • $\Delta t = 0.001$ s

3.7.6 与一阶互补和 DCM 的本质区别

特性 一阶互补滤波 DCM Mahony
修正方式 角度层面加权 角速度层面 PI 反馈 角速度层面 PI 反馈
零偏估计 ❌ 无 ✅ 积分项在线估计 ✅ 积分项在线估计
姿态表示 欧拉角（有万向锁） 3×3 矩阵（无奇异性） 四元数（无奇异性）
约束维护 不需要 正交化（6 约束） 归一化（1 约束）
磁力计融合 ❌ 难以加入 ✅ 叉乘误差自然扩展 ✅ 叉乘误差自然扩展
自适应能力 ❌ 固定 α ✅ 可动态调 Kp/Ki ✅ 可动态调 Kp/Ki
计算量 极低 中等（~80 FLOPs/步） 低（~55 FLOPs/步）

3.8 Madgwick 梯度下降滤波

Madgwick 滤波由 Sebastian Madgwick 于 2010 年在其技术报告中首次提出 [2] 。与 Mahony 滤波采用的非线性互补滤波框架不同，Madgwick 将姿态估计建模为一个最优化问题，通过梯度下降法最小化传感器测量值与参考方向之间的几何误差来修正姿态。两者虽数学路径不同，但同属轻量级确定性姿态估计器，均避免了卡尔曼滤波的高计算开销。

3.8.1 核心思想：将姿态估计转化为最优化问题

问题：找一个四元数 $q$，使得”由 $q$ 旋转后的参考方向”尽可能接近”传感器的实测方向”。

数学表达：

其中：

  • $\hat{d}$：参考方向在导航系下的表示（如重力 $[0,0,1]$、地磁场水平分量）
  • $\hat{s}$：传感器实测方向在机体系下的归一化向量
  • $q^* \otimes \hat{d} \otimes q$：将 $\hat{d}$ 用 $q$ 旋转到机体系

直观理解：如果你猜的姿态 $q$ 是对的，那么”把导航系的重力方向用 $q$ 旋转到机体系”应该和加速度计读数完全一样。如果不一样，差距 $f$ 越大，说明 $q$ 越不准。梯度下降法就是沿着”让差距变小”的方向调整 $q$。

3.8.2 目标函数与梯度

以加速度计为例，参考方向为重力 $\hat{d} = [0, 0, 0, 1]^T$（纯四元数形式），传感器读数为 $\hat{s} = [0, a_x, a_y, a_z]^T$（归一化后）。

目标函数（3×1 向量，取四元数的虚部）：

梯度：

其中雅可比矩阵 $J$ 为：

3.8.3 梯度下降步

其中 $\mu$ 是步长（学习率），控制收敛速度。

步长选择：Madgwick 建议 $\mu$ 应满足：

确保梯度下降的收敛速度不低于陀螺积分的发散速度。实际上取 $\mu = \alpha \cdot |\dot{q}_{\omega}| \cdot \Delta t$，其中 $\alpha$ 是过冲系数（通常 $> 3$）。

3.8.4 融合步：互补结构

Madgwick 的融合方式与 Mahony 不同——它分别从陀螺和观测得到两个四元数估计，然后加权融合：

来自陀螺的预测（和 Mahony 一样）：

来自梯度下降的修正：

融合：

其中 $\gamma \in [0, 1]$ 控制梯度下降修正的权重。$\gamma$ 越大越信任观测修正。

归一化后输出。

与 Mahony 的对比：Mahony 是在角速度上做修正（PI 控制器输出修正角速度），Madgwick 是在四元数上做修正（梯度下降直接调整四元数）。两者本质等价——都是在四元数流形上的互补滤波，只是修正量的计算方式不同。

3.8.5 完整算法流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
① 归一化传感器
   â = a_meas / ‖a_meas‖
   m̂ = m_meas / ‖m_meas‖

② 陀螺积分预测
   q̇ω = ½ q ⊗ [0, ω_meas]
   qω = q + q̇ω · Δt

③ 计算目标函数 f 和梯度 ∇f
   f = f (q, ĝ, â)        // 重力方向目标函数
   J = 雅可比矩阵
   ∇f = J^T · f

④ 梯度下降步
   q∇ = q - μ · ∇f / ‖∇f‖

⑤ 融合
   q = γ · q∇ + (1-γ) · qω

⑥ 归一化
   q = q / ‖q‖

⑦ 转欧拉角输出

如果同时使用加速度计和磁力计，Madgwick 将两个目标函数合并：

对应 6×4 的雅可比矩阵，一次性求解。

3.8.6 Mahony vs Madgwick 对比

特性 Mahony Madgwick
核心思想 PI 控制器修正角速度 梯度下降优化四元数
误差计算 叉乘 $\hat{a} \times \hat{v}$ 目标函数 $f (q)$ 的梯度
零偏估计 ✅ 积分项 $K_i \int e$ （原生、稳定） ✅ 论文 Section 3.4 提供零偏补偿模型（需主动启用，部分开源库已删减，仅保留纯梯度下降；工程中常需外挂零偏估计器）
收敛性 PI 控制器保证（Kp/Ki 合理时） 需调步长 $\mu$，不当可能震荡
多传感器 自然扩展（加叉乘项） 自然扩展（加目标函数行）
计算量 ~50 FLOPs/步 ~80 FLOPs/步（含雅可比）
参数 Kp, Ki μ, γ
社区 ArduPilot、PX4 使用 开源 IMU 库常用

选型建议：两者均支持零偏估计（Mahony 积分项，Madgwick 论文 Section 3.4 的偏置补偿模型），性能接近，但 Madgwick 原始报告中的零偏补偿是在梯度下降框架内做的，收敛性和稳定性不如 Mahony 的 PI 积分项。Mahony 在飞控社区更主流，Madgwick 的优化框架更统一。

3.9 扩展卡尔曼滤波（EKF）

Mahony 和 Madgwick 是”互补滤波”家族的成员——它们的结构是固定的，参数需要手动调优。而 EKF 是基于统计最优的递推估计器，能够自动调整各传感器的权重，并且可以同时估计姿态、零偏、速度、位置等多个状态量。

本设计的最终融合方案是 EKF——因为我们需要同时融合 IMU、磁力计、气压计和 GNSS 四类传感器，并且需要在线估计零偏，EKF 是最合适的框架。

3.9.1 卡尔曼滤波回顾

标准卡尔曼滤波（KF）解决的是线性高斯系统的最优估计问题：

状态方程：$x_{k+1} = F_k x_k + B_k u_k + w_k$

观测方程：$z_k = H_k x_k + v_k$

其中 $w_k \sim \mathcal{N}(0, Q_k)$ 是过程噪声，$v_k \sim \mathcal{N}(0, R_k)$ 是观测噪声。

两步递推：

预测步：

更新步：

核心洞察：卡尔曼增益 $K_k$ 自动决定”多信任预测 vs 多信任观测”——当观测噪声 $R_k$ 大时 $K_k$ 小（不太信任观测），当预测不确定度 $P_{k|k-1}$ 大时 $K_k$ 大（更信任观测）。

3.9.2 从 KF 到 EKF：线性化

姿态系统是非线性的——四元数微分方程是非线性、观测方程也是非线性。标准 KF 不能直接用。

EKF 的思路：在工作点附近做一阶泰勒展开，将非线性系统线性化，然后用 KF 公式。

非线性状态方程：$x_{k+1} = f (x_k, u_k) + w_k$

非线性观测方程：$z_k = h (x_k) + v_k$

线性化后：

然后套用 KF 的预测-更新公式，只是 $F_k$ 和 $H_k$ 每步都重新计算。

EKF 的代价：雅可比矩阵的推导和编程工作量较大，且一阶线性化有截断误差。但姿态系统中角速度和姿态偏差通常较小，线性化近似足够好。

3.9.3 状态方程设计

EKF 的第一步是选择状态向量——哪些量需要估计？

⚠️ 关键理论修正：从直接 EKF 到误差状态卡尔曼滤波（ESKF）

如果直接将 4 维四元数 $\mathbf{q}$ 放入 EKF 状态向量，名义状态为 16 维（$\mathbf{q}$ 4 维 + $\boldsymbol{b}_g$ 3 维 + $\boldsymbol{b}_a$ 3 维 + $\boldsymbol{v}$ 3 维 + $\boldsymbol{p}$ 3 维 = 16 维），协方差矩阵 $P$ 为 16×16。但由于单位四元数约束 $|\mathbf{q}| = 1$，$P$ 矩阵在姿态误差方向上会退化，出现奇异性，滤波极易发散。

高性能组合导航（如 Pixhawk 的 EKF2/EKF3）普遍采用误差状态卡尔曼滤波（ESKF / MEKF），将状态分为两层：

层次 内容 维度 说明
名义状态（Nominal State） $\boldsymbol{x} = [\mathbf{q}_b^n,\; \boldsymbol{b}_g,\; \boldsymbol{b}_a,\; \boldsymbol{v}^n,\; \boldsymbol{p}^n]^T$ 16 维 用四元数保存姿态，由陀螺/加计积分递推，不经过 KF 更新
误差状态（Error State） $\delta\boldsymbol{x} = [\delta\boldsymbol{\theta},\; \delta\boldsymbol{b}_g,\; \delta\boldsymbol{b}_a,\; \delta\boldsymbol{v},\; \delta\boldsymbol{p}]^T$ 15 维 姿态误差用 3 维旋转矢量误差 $\delta\boldsymbol{\theta} = [\delta\theta_x, \delta\theta_y, \delta\theta_z]^T$，协方差 $P$ 为 15×15

为什么用 3 维 $\delta\boldsymbol{\theta}$ 而不是 4 维 $\delta\mathbf{q}$？ 单位四元数只有 3 个自由度（受 $|\mathbf{q}|=1$ 约束），用 3 维旋转矢量表示误差恰好匹配自由度，$P$ 矩阵满秩，不存在奇异性问题。

本设计的名义状态向量（16 维，松耦合方案）：

状态量 维度 含义 来源
$\mathbf{q} = [q_0, q_1, q_2, q_3]^T$ 4 姿态四元数 陀螺积分
$\boldsymbol{b}_g = [b_{gx}, b_{gy}, b_{gz}]^T$ 3 陀螺零偏 在线估计
$\boldsymbol{b}_a = [b_{ax}, b_{ay}, b_{az}]^T$ 3 加计零偏 在线估计
$\boldsymbol{v} = [v_N, v_E, v_D]^T$ 3 速度（导航系 NED） 加计积分 + GNSS 修正
$\boldsymbol{p} = [p_N, p_E, p_D]^T$ 3 位置（导航系 NED） 速度积分 + GNSS 修正

状态量选择说明：

  • 陀螺零偏 $\boldsymbol{b}_g$：必须估计，是 Mahony 积分项的”正规化”版本
  • 加计零偏 $\boldsymbol{b}_a$：对位置/速度精度影响大，高精度系统需要估计
  • 速度和位置：仅在需要融合 GNSS 时加入。如果只做姿态估计，可去掉 $\boldsymbol{v}$ 和 $\boldsymbol{p}$，名义状态缩减为 10 维，误差状态缩减为 9 维
  • ESKF 的更新流程：KF 只更新 15 维误差状态 $\delta\boldsymbol{x}$，然后将误差注入名义状态 $\boldsymbol{x}$（即 $\mathbf{q} \leftarrow \mathbf{q} \otimes \Delta\mathbf{q}(\delta\boldsymbol{\theta})$，其余直接加），注入后 $\delta\boldsymbol{x}$ 清零。名义状态始终由运动学积分递推，KF 仅修正其误差

连续时间状态方程：

其中 $\boldsymbol{w}_{bg}$、$\boldsymbol{w}_{ba}$ 是过程噪声，功率谱密度由 Allan 方差分析确定（参见第 2 章）。

3.9.4 观测方程设计

EKF 的观测方程描述”给定状态 $\boldsymbol{x}$，传感器应该读什么”。每种传感器提供不同维度的观测：

① 加速度计观测（3 维）：

静止时加速度计应读到 $C_b^n (\mathbf{q})^T \cdot \boldsymbol{g}^n$：

② 磁力计观测（3 维）：

其中 $\boldsymbol{m}^n$ 是本地地磁场在导航系下的参考方向（由 WMM2020 查得），$\boldsymbol{b}_m$ 是磁力计偏移（如果估计的话）。

③ GNSS 观测（6 维：位置 + 速度）：

④ 气压计观测（1 维：高度）：

⚠️提醒：本文约定 $C_b^n$ 为 body→nav 的旋转矩阵。某些 PX4 版本代码库可能使用 nav→body 约定，此处转置关系相反，务必统一。

松耦合 vs 紧耦合：本设计采用松耦合方案——GNSS 模块 UM982 内部已解算出位置/速度/航向，EKF 直接使用这些已解算结果作为观测。紧耦合方案则直接使用 GNSS 的伪距/载波相位作为观测，精度更高但实现复杂度大增（需要处理卫星星历、电离层模型等）。对于航姿板，松耦合已足够。

3.9.5 EKF 完整流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
初始化：
  q = [1, 0, 0, 0]（或由加计+磁力计初始化）
  b_g = b_a = [0, 0, 0]
  v = p = [0, 0, 0]
  P = diag ([σ_q², σ_bg², σ_ba², σ_v², σ_p²])  // 初始不确定度

主循环（IMU 触发，1~8 kHz）：

  ① 预测步
     ω = ω_meas - b_g
     q̇ = ½ q ⊗ [0, ω]
     q = q + q̇ · Δt        // 归一化
     b_g = b_g              // 零偏保持（随机游走模型）
     b_a = b_a
     a_n = C_b^n (q) · (a_meas - b_a) + g^n
     v = v + a_n · Δt
     p = p + v · Δt
     P = F · P · F^T + Q    // F 每步重新计算

  ② 更新步（观测到达时执行）

     如果加速度计数据到达（每 IMU 周期）：
       K = P · H_a^T · (H_a · P · H_a^T + R_a)^{-1}
       δx = K · (a_meas - h_a (x))
       x = x + δx
       P = (I - K · H_a) · P

     如果磁力计数据到达（~100 Hz）：
       K = P · H_m^T · (H_m · P · H_m^T + R_m)^{-1}
       δx = K · (m_meas - h_m (x))
       x = x + δx
       P = (I - K · H_m) · P

     如果 GNSS 数据到达（1~20 Hz）：
       K = P · H_g^T · (H_g · P · H_g^T + R_g)^{-1}
       δx = K · (z_gnss - h_g (x))
       x = x + δx
       P = (I - K · H_g) · P

     如果气压计数据到达（10~50 Hz）：
       K = P · H_b^T · (H_b · P · H_b^T + R_b)^{-1}
       δx = K · (z_baro - h_b (x))
       x = x + δx
       P = (I - K · H_b) · P

  ③ 四元数归一化
     q = q / ‖q‖

  ④ 转欧拉角输出
     φ = atan2 (2 (q0q1+q2q3), 1-2 (q1²+q2²))
     θ = arcsin (2 (q0q2-q3q1))
     ψ = atan2 (2 (q0q3+q1q2), 1-2 (q2²+q3²))

⚠️ 连续→离散：状态转移矩阵与过程噪声的离散化

伪代码中的 $F \cdot P \cdot F^T + Q$ 需要将连续时间雅可比 $F_c$ 和噪声驱动矩阵 $G$ 离散化。这是 STM32 代码实现的直接数学基础：

状态转移矩阵离散化（一阶泰勒近似，IMU 采样间隔 $\Delta t$ 足够小时精确）：

其中连续时间雅可比 $F_c$ 的块结构（15×15，按 $[\delta\theta, \delta b_g, \delta b_a, \delta v, \delta p]$ 排列）：

  • 第 (1,1)块 $-\lfloor\hat{\omega}\times\rfloor$：姿态误差对自身传播（角速度反对称矩阵）
  • 第 (1,2)块 $-I_3$：陀螺零偏直接耦合到姿态误差
  • 第 (4,1)块 $-C_b^n \lfloor\hat{f}^b \times\rfloor$：姿态误差导致比力投影偏移，影响速度
  • 第 (4,3)块 $-C_b^n$：加计零偏直接耦合到速度误差
  • 第 (5,4)块 $I_3$：速度误差积分到位置误差

编程提示：$F_c$ 中大量零块意味着 $F_k \approx I + F_c \Delta t$ 也有大量零块。实际代码中不需要完整 15×15 矩阵乘法，可以只计算非零块的更新。例如姿态误差协方差的预测只需更新 $P_{11}, P_{12}, P_{21}$ 三个子块。

过程噪声协方差离散化：

其中噪声驱动矩阵 $G$（15×6）和连续时间噪声功率谱密度 $Q_c$（6×6 对角阵）：

代入得：

⚠️ 此处省略了陀螺/加计零偏游走噪声。完整 $Q_k$ 还应在对角线 (2,2) 和 (3,3) 块加入 $\sigma_{\text{RRW}}^2 \Delta t \cdot I_3$（陀螺零偏游走）和 $\sigma_{\text{ba_rw}}^2 \Delta t \cdot I_3$（加计零偏游走）。上表简化了展示，实际代码中必须包含这两项，否则零偏状态无法被 KF 驱动更新。
过程噪声的完整形式：$Q_k = \text{diag}\left (\sigma_{\text{ARW}}^2 \Delta t \cdot I_3,\; \sigma_{\text{RRW}}^2 \Delta t \cdot I_3,\; \sigma_{\text{ba_rw}}^2 \Delta t \cdot I_3,\; \sigma_{\text{VRW}}^2 \Delta t \cdot I_3,\; \mathbf{0}_3\right)$

另外，位置的过程噪声通常不直接设为零，而是通过速度噪声传播间接获得。如果你的 EKF 实现中位置没有独立过程噪声注入，长时间 GNSS 中断时位置协方差可能增长不足，导致恢复时响应过慢。

⚠️ 位置过程噪声的处理：严格来说，位置的运动学模型 $\dot{p}=v$ 不含直接过程噪声，其协方差通过速度误差传播间接增长。但在工程中，由于姿态耦合误差、未建模动力学等因素，实际位置不确定度的增长远快于理想模型。建议在 $Q_k$ 的位置对角块注入一个小的经验噪声 $\sigma_{p_rw}^2 \Delta t \cdot I_3$（典型值 0.01~0.1 m/√s）。若此项为零，GNSS 长时间中断后 $P_{pp}$ 增长不足，导致 GNSS 恢复时卡尔曼增益过小、位置修正迟缓。

$Q_k = \text{diag}\left (\sigma_{\text{ARW}}^2 \Delta t \cdot I_3,\; \sigma_{\text{RRW}}^2 \Delta t \cdot I_3,\; \sigma_{\text{ba_rw}}^2 \Delta t \cdot I_3,\; \sigma_{\text{VRW}}^2 \Delta t \cdot I_3,\; \sigma_{p_rw}^2 \Delta t \cdot I_3 \right)$

 

多速率处理：IMU 以 1~8 kHz 高频运行，GNSS/气压计以低频运行。EKF 的预测步以 IMU 频率执行，更新步仅在对应传感器数据到达时执行。这就是为什么 PPS 时间同步如此重要——需要精确知道”这条 GNSS 观测量对应哪个时刻的 IMU 积分状态”。

 

⚠️ 时间延迟补偿：在实际工程中，GNSS 观测往往存在几十毫秒的硬件延迟（从信号接收到 NMEA 输出需经过捕获、跟踪、解算等流程）。如果直接用当前时刻的 EKF 状态去关联迟到的 GNSS 数据，会导致系统震荡甚至发散。常用解决方案：

  • 环形缓冲区（Ring Buffer）：将历史 IMU 预测状态存入环形缓冲区，当 GNSS 数据到达时，根据其时间戳回溯到对应时刻的状态进行更新
  • 状态后退恢复：从当前状态回退到 GNSS 时间戳对应的状态，执行 EKF 更新，再前向重播到当前时刻
  • 延迟状态增广：将延迟的状态量加入 EKF 状态向量，直接估计（计算量增大但更精确）

本设计中 UM982 的 PPS 秒脉冲提供了精确的时间锚点，IMU 数据打上 PPS 同步时间戳后，可以在缓冲区中精确匹配 GNSS 观测对应的 IMU 状态。

3.9.6 噪声参数设置

EKF 的性能高度依赖噪声参数 $Q$ 和 $R$ 的设置。不合理的参数会导致滤波发散或过度平滑。

过程噪声 $Q$（对角矩阵，各状态量独立）：

状态量 噪声来源 建议值 依据
$\mathbf{q}$ 陀螺白噪声 ARW $\sigma_{\text{ARW}}^2 \cdot \Delta t$ ICM-42688-P: $\sigma_{\text{ARW}} \approx 0.17°/\sqrt{\text{hr}}$
$\boldsymbol{b}_g$ 陀螺零偏游走 RRW $\sigma_{\text{RRW}}^2 \cdot \Delta t$ Datasheet 或 Allan 方差分析
$\boldsymbol{b}_a$ 加计零偏游走 $\sigma_{\text{ba_rw}}^2 \cdot \Delta t$ Datasheet 或 Allan 方差分析
$\boldsymbol{v}$ 加计白噪声 VRW $\sigma_{\text{VRW}}^2 \cdot \Delta t$ Datasheet
$\boldsymbol{p}$ 速度积分 $\sigma_v^2 \cdot \Delta t$ 由速度不确定度传播

观测噪声 $R$（对角矩阵，各传感器独立）：

观测源 噪声参数 建议值 说明
加速度计 $R_a$ 各轴方差 $(0.05 \sim 0.5)^2$ m/s²² 静态小、动态大，需要自适应
磁力计 $R_m$ 各轴方差 $(0.5 \sim 5)^2$ μT² 受干扰时急剧增大
GNSS 位置 $R_{gp}$ 各轴方差 RTK: $(0.02)^2$ m²；非 RTK: $(2)^2$ m² UM982 RTK 精度 ~1cm+1ppm
GNSS 速度 $R_{gv}$ 各轴方差 $(0.05)^2$ (m/s)² 多普勒测速精度
气压计 $R_b$ 高度方差 $(0.5)^2$ m² 相对精度 ~5cm，但受气流影响大

自适应观测噪声：工程中常用的技巧——当检测到异常条件时动态增大 $R$：

  • 加速度模长偏离 $g$ 超过阈值 → $R_a \leftarrow 100 \times R_a$（载体在加速，加计不可信）
  • 磁场模长偏离本地地磁场 → $R_m \leftarrow 1000 \times R_m$（磁异常）
  • GNSS 卫星数 < 6 → $R_g \leftarrow 10 \times R_g$（定位精度下降）

3.9.7 本设计的 EKF 特殊处理

① 多 IMU 融合

⚠️ 多 IMU 融合前必须进行时间对齐/同步。不同 IMU 的采样时刻可能有几十 μs 的偏差，在高动态下这会导致融合后的角速度出现虚假脉冲。Ai 建议：以主 IMU（ICM-42688）的采样时刻为基准，对其他 IMU 数据做线性插值或最近邻匹配后再加权。未对齐的多 IMU 融合在振动环境下性能可能反而劣于单 IMU。

三颗 IMU 全部参与融合（非简单平均），在 EKF 框架下按噪声特性加权——这正是”三异构全融合”的核心：

  • 三颗 IMU 各自输出 $\boldsymbol{\omega}_i$ 和 $\boldsymbol{a}_i$（$i = 1,2,3$）
  • 融合后输入 EKF 的角速度和加速度为加权平均：
  • 权重由各 IMU 的噪声协方差和当前工况决定：
    • 正常工况：ICM 权重最高（噪声最低）
    • 高振动工况：BMI 权重提升（抗振最优）
    • 极端温度：IIM 权重提升（宽温稳定）
    • 故障检测：某颗 IMU 输出异常时权重置零

📌 FDI 故障检测与隔离：多 IMU 的完整故障处理架构（物理校正 → 新息卡方检验隔离 → 动态自适应加权）属于系统数据管理层的范畴，将在第 4 章详细展开。本节仅描述 EKF 内部的加权融合逻辑。

② 双天线航向观测与自适应切换

UM982 双天线输出的航向角 $\psi_{\text{GNSS}}$ 不依赖地磁场，作为独立观测输入 EKF：

基线长度与航向噪声的关系：双天线航向精度取决于基线长度 $L$（两天线物理间距）和载波相位测量精度 $\sigma_\phi$：

基线长度 $L$ 载波精度 $\sigma_\phi$ 航向标准差 $\sigma_\psi$ 观测噪声 $R_{\text{yaw}}$
30 cm ~1° ~1° $(1 \cdot \frac{\pi}{180})^2 \approx 3.0 \times 10^{-4}$
60 cm ~1° ~0.5° $(0.5 \cdot \frac{\pi}{180})^2 \approx 7.6 \times 10^{-5}$
100 cm ~1° ~0.3° $(0.3 \cdot \frac{\pi}{180})^2 \approx 2.7 \times 10^{-5}$

自适应切换逻辑：

1
2
3
4
5
6
7
8
9
10
if (um982. heading_status == FIX_STEADY) {
    // 双天线固定解可用：优先使用 GNSS 航向，磁力计大幅降权
    R_yaw = R_yaw_baseline;       // 根据基线长度设置
    R_mag = 1000.0f * R_mag_nominal;  // 磁力计极大程度降权
    Update_EKF_Yaw_GNSS ();        // GNSS 航向更新
} else {
    // GNSS 航向不可用（多径/室内/失锁）：恢复磁力计权重
    R_mag = R_mag_nominal;         // 恢复磁力计正常权重
    Update_EKF_Yaw_Mag ();          // 传统磁力计/陀螺融合
}

 

⚠️ 为什么磁力计降权而不是直接禁用？ 即使在双天线固定解可用时，磁力计仍可作为冗余观测：当双天线航向突然跳变（如遮挡导致短暂失锁又恢复）时，磁力计的连续性可以防止 EKF 航向状态产生大幅跳变。将 $R_m$ 增大 1000 倍相当于将磁力计权重降至原来的 0.1%，磁力计不再主导航向估计，但其信息仍被保留在滤波器中。

③ 气压计与 GNSS 高度的融合

  • 气压计提供高频高度变化（10~50 Hz），GNSS 提供低频绝对高度（1~20 Hz）
  • EKF 自然处理两者的互补：气压计修正短期高度波动，GNSS 修正长期漂移
  • 检测到地效干扰（低高度 + 高垂直速度异常）时，$R_b \leftarrow 100 \times R_b$，降低气压计权重

3.10 五种算法对比与选型

特性 一阶互补 DCM Mahony Madgwick EKF
核心思想 频域互补 PI 控制 + 叉乘误差 + 矩阵 PI 控制 + 叉乘误差 + 四元数 梯度下降优化 贝叶斯最优估计
姿态表示 欧拉角 3×3 旋转矩阵（9 维） 四元数（4 维） 四元数（4 维） 四元数
万向锁 ⚠️ 有 ✅ 无 ✅ 无 ✅ 无 ✅ 无
零偏估计 ❌ ✅ (Ki 积分项) ✅ (Ki 积分项) ✅ (论文 Sec. 3.4 偏置补偿) ✅ (ESKF 误差状态)
约束维护 不需要 正交化（6 约束） 归一化（1 约束） 归一化（1 约束） 归一化（1 约束）
自适应权重 ❌ ❌ (固定 Kp/Ki) ❌ (固定 Kp/Ki) ❌ (固定 μ/γ) ✅ (卡尔曼增益自动调)
多传感器 仅 IMU IMU + 磁力计 IMU + 磁力计 IMU + 磁力计 IMU + 磁力计 + 气压计 + GNSS
磁异常处理 ❌ 手动降权 手动降权 手动调 μ 自动 (增 $R_m$)
动态自适应 ❌ 需外部逻辑 需外部逻辑 需外部逻辑 自动（增 $R_a$）
位置/速度 ❌ ❌ ❌ ❌ ✅ (状态量)
计算量 (FLOPs/步) ~10 ~80 ~55 ~80 ~500~2000
调参难度 低（α） 中（Kp, Ki） 中（Kp, Ki） 中（μ, γ） 高（Q, R 矩阵）
适用场景 教学/简单系统 已淘汰/教学参考 消费级飞控 IMU 库/VR 高性能航姿/导航
代表项目 — ArduPilot 早期 Betaflight, INAV Arduino IMU 库 Pixhawk EKF2/EKF3

3.10 本设计的融合架构

综合以上分析，本设计的姿态融合架构如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
┌──────────────────────────────────────────────────────────────────┐
│                     航姿解算板融合架构                             │
│                                                                  │
│  传感器层                                                         │
│  ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌──────┐ ┌──────┐ ┌────┐    │
│  │ICM-42688│ │IIM-42652│ │ BMI088  │ │RM3100│ │BMP581│ │UM982│   │
│  │ (主 IMU) │ │(工业基准)│ │(抗震 IMU)│ │(主磁)│ │(气压)│ │(GNSS)│  │
│  └────┬────┘ └────┬────┘ └────┬────┘ └───┬──┘ └───┬──┘ └─┬──┘   │
│       │           │           │          │        │       │     │
│  ┌────┴───────────┴───────────┴────┐     │        │       │     │
│  │  多 IMU 加权融合 + 故障检测         │     │        │       │     │
│  │  ω_fused, a_fused               │     │        │       │     │
│  └────────────┬────────────────────┘     │        │       │     │
│               │                          │        │       │     │
│  ┌────────────┴──────────────────────────┴────────┴───────┴──┐  │
│  │                    EKF 融合核心                            │  │
│  │                                                           │  │
│  │  名义状态: x = [q, b_g, b_a, v, p] (16 维)                  │  │
│  │  误差状态: δx = [δθ, δb_g, δb_a, δv, δp] (15 维)            │  │
│  │                                                           │  │
│  │  高频预测 (1~8kHz):                                        │  │
│  │    陀螺积分 → 姿态预测                                      │  │
│  │    加计积分 → 速度/位置预测                                 │  │
│  │                                                           │  │
│  │  低频更新:                                                 │  │
│  │    加计     → Roll/Pitch 修正（每步）                       │  │
│  │    磁力计   → Yaw 修正（~100Hz）                           │  │
│  │    气压计   → 高度修正（10~50Hz）                          │  │
│  │    GNSS     → 位置/速度/Yaw 修正（1~20Hz）                 │  │
│  │                                                           │  │
│  │  自适应:                                                   │  │
│  │    高动态 → 增大 R_a（降低加计权重）                         │  │
│  │    磁异常 → 增大 R_m（降低磁力计权重）                       │  │
│  │    GNSS 可用 → 增大 R_m（磁力计让位给双天线航向）             │  │
│  │    地效干扰 → 增大 R_b（降低气压计权重）                     │  │
│  └───────────────────────────┬───────────────────────────────┘  │
│                              │                                  │
│  ┌───────────────────────────┴───────────────────────────────┐  │
│  │  输出层                                                    │  │
│  │  Roll (φ), Pitch (θ), Yaw (ψ)    → 姿态控制                │  │
│  │  位置 (p), 速度 (v)              → 导航                     │  │
│  │  陀螺零偏估计 (b_g)              → 健康监测                  │  │
│  │  磁偏估计 (隐含在 q 中)          → 磁异常报警                 │  │
│  └─────────────────────────────────────────────────────────────┘  │
└───────────────────────────────────────────────────────────────────┘

架构设计要点：

  1. 分层结构：传感器层 → 数据预处理层 → EKF 核心层 → 输出层，各层职责清晰
  2. 多 IMU 在 EKF 之前融合：在输入 EKF 前完成加权融合和故障检测，EKF 只看到一组融合后的 IMU 数据，降低了状态维度和计算量
  3. 多速率处理：IMU 高频驱动预测步，各观测传感器按各自频率触发更新步，互不干扰
  4. 自适应噪声：通过动态调整 $R$ 矩阵实现工况自适应，无需切换算法或参数
  5. PPS 时间同步：UM982 的 PPS 脉冲为所有传感器数据打上统一时间戳，确保多速率 EKF 的时间一致性

3.12 本章小结

姿态解算的本质：从陀螺仪角速度积分递推姿态，用加速度计/磁力计/GNSS 的绝对基准修正漂移。

数据融合的本质：不同传感器在频域上互补——陀螺高频可信，加计/磁力计低频可信。融合就是让”快的跑在前，准的拽回来”。

算法演进路线：

1
2
3
4
5
6
7
8
一阶互补滤波      DCM              Mahony           Madgwick            EKF
(角度加权)    (叉积+PI+矩阵)    (叉积+PI+四元数)    (梯度下降优化)       (贝叶斯最优)
    │               │                  │                │               │
    │  +旋转矩阵     │  +四元数替代矩阵  │  +梯度替代 PI    │  +自适应权重   │
    │  +叉积误差     │  +归一化替代正交化 │  +优化框架      │  +多传感器    │
    │  +PI 零偏估计   │                  │                │  +位置/速度    │
    ▼               ▼                  ▼                ▼                ▼
  教学入门      已淘汰/教学参考     消费级飞控主流    IMU 库/VR 设备     高性能航姿系统 ← 本设计

本设计的最终选择：EKF——因为我们需要同时融合四类传感器、在线估计零偏、自适应调整权重，并且需要输出位置和速度。EKF 是唯一能满足所有需求的框架，虽然计算量最大，但 STM32H743 的 480MHz 主频 + 硬件浮点完全能胜任。

参考文献：
[1] R. Mahony, T. Hamel, J.-M. Pflimlin, “Nonlinear Complementary Filters on the Special Orthogonal Group,” IEEE Trans. Automatic Control, vol. 53, no. 5, pp. 1203–1218, 2008.
[2] S. O. H. Madgwick, “An efficient orientation filter for inertial and inertial/magnetic sensor arrays,” Internal Report, University of Bristol, 2010.
[3] G. L. G. Choukroun, I. Y. Bar-Itzhack, Y. Oshman, “Novel Quaternion Kalman Filter,” IEEE Trans. Aerospace and Electronic Systems, vol. 42, no. 1, pp. 174–190, 2006.
[4] G. M. Lerner et al., “Attitude Error Representations for Kalman Filtering,” NASA Technical Report, 2002.
[5] G. M. Lerner et al., “Attitude Error Representations for Kalman Filtering,” NASA Technical Report, 2002.