4. 传感器数据采集与融合架构
飞控系统对“自身状态”的估计质量，直接决定姿态控制、位置控制和任务执行的可靠性。单一传感器无法在所有工况下提供完整、连续、可信的状态量：IMU 高频但会漂移，GNSS 提供绝对位置但易受遮挡与多径影响，气压计给出高度趋势却受气流与温度扰动，磁力计提供航向基准却易被电机与电流干扰，视觉传感器能补充位置与速度约束但依赖纹理与光照。多源融合的本质，是把这些互补、冗余、异构的观测组织成一条可管理的数据流，并在统一的状态空间内做最优估计。
一、传感器特性与数据流组织
从数据流角度看，五类传感器在采样率、时延、观测维度和失效模式上差异显著。IMU 通常以 200 Hz～1 kHz 输出三轴角速度与比力，是姿态与速度递推的核心；GNSS 以 5～20 Hz 输出经纬高与地速，绝对但存在跳变；气压计以 25～50 Hz 输出气压换算的相对高度，短期平滑、长期受天气漂移；磁力计以 50～100 Hz 输出三轴磁场，用于航向观测；视觉传感器（单目/双目/光流）以 20～60 Hz 输出特征跟踪、位姿增量或光流速度，计算量大且时延较高。
合理的数据流组织应遵循“高频递推、低频校正、异步对齐”的原则。IMU 作为预测主干，在每个采样周期执行状态递推；其余传感器作为观测源，在各自到达时刻触发量测更新。为避免时间戳错位，所有传感器数据进入融合前必须统一到同一时间基准，并保留采集时刻与到达时刻两个时间戳，用于时延补偿。
二、融合框架与状态空间
工程上最常用的融合框架是误差状态卡尔曼滤波（ESKF）。其核心思想是：用名义状态做非线性递推，用误差状态做线性化滤波，避免四元数归一化约束带来的协方差奇异问题。名义状态通常包含位置 p、速度 v、姿态四元数 q；误差状态包含位置误差 δp、速度误差 δv、姿态误差 δθ、加速度计零偏 ba、陀螺仪零偏 bg，共 15 维。
预测阶段由 IMU 驱动：角速度积分更新姿态，比力经重力补偿后积分更新速度与位置。更新阶段按观测类型分别构造量测矩阵。例如 GNSS 位置观测 z_gnss = p + n_gnss，量测矩阵 H = [I₃ 0₃ 0₃ 0₃ 0₃]；磁力计航向观测则通过磁场矢量与姿态的几何关系构造非线性观测方程，再对误差状态求雅可比。
三、关键实现细节与参数示例
以下给出一个典型的多源融合配置示例，参数为教学假设值，用于说明各传感器的权重与失效处理策略：
| 传感器 | 观测类型 | 典型噪声（1σ） | 更新频率 | 失效判据 |
|---|---|---|---|---|
| IMU | 角速度/比力 | 陀螺 0.01°/s，加计 0.05 m/s² | 400 Hz | 零偏突变 > 0.5°/s |
| GNSS | 位置/速度 | 水平 1.5 m，垂直 3 m | 10 Hz | HDOP > 2.5 或跳变 > 5 m |
| 气压计 | 相对高度 | 0.3 m | 50 Hz | 与 GNSS 高度差 > 10 m |
| 磁力计 | 航向 | 2° | 100 Hz | 模值偏离 > 30% |
| 视觉 | 位置/速度增量 | 0.1 m（近距） | 30 Hz | 特征数 < 15 或重投影误差 > 2 px |
在实现时，每个观测源进入更新前需经过卡方检验：计算新息 r = z - Hx 及其协方差 S = HPHᵀ + R，若 rᵀS⁻¹r 超过阈值（如 3 自由度对应 7.81），则判定该观测为野值并跳过更新，同时记录故障计数。连续多次超限则将该传感器标记为失效，切换至降级融合模式。
四、降级与容错策略
多源融合的价值不仅在于精度，更在于容错。当 GNSS 失效时，系统应自动切换至“IMU + 气压计 + 视觉”组合，水平位置由视觉或光流维持，高度由气压计维持，此时水平漂移会随时间增长，需触发返航或悬停策略。当磁力计受扰时，航向观测权重应降低或隔离，姿态估计退化为“陀螺积分 + 视觉航向”模式。当视觉失效时，若 GNSS 可用，则位置精度下降但系统仍可稳定飞行。
判断融合是否健康，可观察三个指标：新息序列的均值应接近零、协方差应保持有界、各传感器故障计数应稳定。若新息均值持续偏置，说明存在未建模的零偏或标定误差；若协方差发散，说明观测信息不足或过程噪声设置过小。实际调试中，可先静态采集 5 分钟数据，检查零偏稳定性与噪声水平，再逐步加入动态观测，观察状态估计的收敛过程。
综上，传感器数据采集与融合架构的核心是：以 IMU 为递推主干，以 GNSS、气压计、磁力计、视觉为观测校正源，通过统一时间基准、预处理、误差状态滤波与故障检测，形成连续、鲁棒的状态估计。理解每个环节的输入输出与失效边界，才能在真实飞行中快速定位问题并做出合理降级决策。