PX4 EKF2 融合调度 control.cpp 详解

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 775 阅读时长 ≈ 3 分钟
深入解析 PX4 EKF2 control.cpp 中的融合调度中心 controlFusionModes()，以 GPS 融合为例说明各传感器融合的启用条件、新息检验、降级策略，以及高度源仲裁逻辑。

一、controlFusionModes() 职责

controlFusionModes() 是 EKF 中所有传感器融合的调度中心，在每步 IMU 预测后调用，负责：

  1. 检查各传感器 RingBuffer 是否有延迟时刻的新数据
  2. 判断是否满足融合前提条件（传感器健康、EKF 初始化完成等）
  3. 执行新息检验，决定接受或拒绝测量
  4. 调用具体融合函数（aid_sources/*）执行卡尔曼更新
1
2
3
4
5
6
7
8
9
10
11
12
13
void Ekf::controlFusionModes(const imuSample &imu_sample) {
    controlGpsFusion(imu_sample);
    controlGpsYawFusion(imu_sample);
    controlMagFusion();
    controlHeightFusion(imu_sample);
    controlBaroFusion();
    controlRangeFinderFusion();
    controlEvFusion();
    controlOpticalFlowFusion(imu_sample);
    controlAuxVelFusion();
    controlAirspeedFusion();
    controlDragFusion();
}

二、GPS 融合详解 controlGpsFusion()

以 GPS 为例，说明融合流程：

2.1 GPS 使能条件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
bool EKF::controlGpsFusion() {
    // 1. 参数使能检查
    if (!_params.gnss_vel_use && !_params.gnss_pos_use) return;

    // 2. 数据有效性（RingBuffer 查询）
    gnssSample gps_sample;
    if (!_gps_buffer.pop_first_older_than(_time_delayed_us, &gps_sample)) return;

    // 3. GPS 预检（位置精度、卫星数、速度精度等）
    if (!gpsIsHealthy(gps_sample)) {
        // 降级：停止 GPS 融合，依赖气压计+陀螺积分
        stopGpsFusion();
        return;
    }

    // 4. 执行位置融合
    if (_params.gnss_pos_use) {
        fuseGnssPos(gps_sample, _aid_src_gnss_pos);
    }
    // 5. 执行速度融合
    if (_params.gnss_vel_use) {
        fuseGnssVel(gps_sample, _aid_src_gnss_vel);
    }
}

2.2 新息检验（Innovation Gate）

在 fuseGnssPos() 内部执行新息一致性检验：

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
// 计算新息
Vector2f innov = gps_pos_local - _state.pos.xy();  // 预测位置 vs GPS 位置

// 计算新息方差 S = H*P*H' + R
// 对角位置状态的 H 矩阵是单位矩阵，所以 S = P_pos + R_gps
Vector2f innov_var = {
    P(6,6) + sq(gps_sample.hacc),  // 北向
    P(7,7) + sq(gps_sample.hacc),  // 东向
};

// 归一化新息平方（NIS）检验
float test_ratio = (innov(0)*innov(0)/innov_var(0) + innov(1)*innov(1)/innov_var(1)) / 2.0f;

if (test_ratio > sq(_params.gps_pos_innov_gate)) {
    // 拒绝：新息太大，说明 GPS 数据与 EKF 预测差距超过 gate 倍标准差
    _aid_src_gnss_pos.innovation_rejected = true;
    // 连续拒绝超过阈值 → 触发 GPS 重置（直接跳变状态到 GPS 位置）
    if (consecutive_rejections > GPS_RESET_THRESHOLD) {
        resetPositionToGnss();  // 状态重置
    }
    return;
}

// 接受：调用卡尔曼更新
measurementUpdate(_aid_src_gnss_pos, H_pos, innov, innov_var);

Gate 参数意义：EKF2_GPS_P_GATE = 5.0（默认）表示：若 GPS 新息超过 5σ，则拒绝该测量。调大可接受更差的 GPS，调小则对 GPS 质量要求更严。

三、高度融合仲裁 controlHeightFusion()

高度是最复杂的融合场景，需要在多个高度源之间进行仲裁和降级切换：

1
2
3
4
5
6
7
8
9
10
11
12
flowchart TD
    Start["controlHeightFusion"] --> CheckAcc["checkVerticalAccelHealth<br/>检查垂直加速度健康"]
    CheckAcc --> GndEff["controlGroundEffect<br/>地面效应补偿"]
    GndEff --> BaroCtrl["controlBaroHeightFusion<br/>气压计高度"]
    BaroCtrl --> GnssCtrl["controlGnssHeightFusion<br/>GPS高度"]
    GnssCtrl --> RngCtrl["controlRangeHeightFusion<br/>测距仪高度"]
    RngCtrl --> EvCtrl["controlEvHeightFusion<br/>外部视觉高度"]
    EvCtrl --> Fallback["checkHeightSensorRefFallback<br/>高度参考自动切换"]
    Fallback --> CheckRef{"主参考源<br/>是否有效?"}
    CheckRef -->|有效| Done("[继续使用主参考]")
    CheckRef -->|失效| Switch["按优先级切换:<br/>1.EV → 2.测距仪 → 3.GPS → 4.气压计"]
    Switch --> Done

3.1 高度参考优先级

默认优先级（EKF2_HGT_REF 参数控制）：

优先级 传感器 典型场景
1 外部视觉（EV） 室内精确定位
2 测距仪 低空悬停/降落（<10 m）
3 GPS 高度 室外绝对高度
4（默认） 气压计 通用场景回退

3.2 地面效应补偿

近地面时旋翼下洗流导致气压计读数偏低（压力增大 → 高度显示偏低）：

1
2
3
4
5
6
7
void Ekf::controlGroundEffect() {
    // 测距仪读数 < 1 m 时认为在地面效应区域
    if (_rng_sample.rng < 1.0f && _in_air) {
        // 增加气压计观测噪声方差（降低气压计置信度）
        _baro_noise_var_compensated = sq(_params.baro_noise * _params.gnd_effect_baro_scaler);
    }
}

四、磁力计融合 controlMagFusion()

磁力计融合的工作模式由 EKF2_MAG_TYPE 参数控制：

模式 值 说明
自动 0 室外用 3D 磁场融合，室内/干扰大时切换为仅航向
仅航向 1 仅约束 yaw，不估计地球磁场矢量
3D 磁场 2 完整估计地球磁场（NED）+ 机体磁偏差
无磁力计 5 依赖双天线 GPS 或外部视觉提供航向

室外飞行时，3D 磁场融合同时约束：

  • 航向 yaw（主要贡献）
  • 地球磁场方向估计（提高后续磁力计一致性）
  • 机体磁偏差（校准磁铁干扰）

五、外部视觉（EV）融合

controlEvFusion() 支持 4 种信息的独立融合：

子函数 话题字段 融合内容
controlEvPosFusion() pos 本地 NED 位置（MoCap/VIO）
controlEvVelFusion() vel 机体或 NED 速度
controlEvHeightFusion() pos.z 高度
controlEvYawFusion() yaw（从 q 提取） 航向角

EV 数据（vehicle_visual_odometry）延迟通常较大（EKF2_EV_DELAY 默认 175 ms），因此 RingBuffer 需要设置较大深度。

六、小结

control.cpp 是 EKF 的决策层，其核心逻辑：

  1. 多传感器异步调度：各传感器按各自频率独立触发，互不阻塞
  2. 新息门检验：统一的 NIS 检验机制，异常测量自动拒绝
  3. 状态重置：连续拒绝超过阈值时执行状态跳变，防止长期漂移
  4. 高度源仲裁：优先级驱动的自动降级，保证高度估计在任意传感器失效时的鲁棒性

这一层的参数调优（各 *_GATE 参数、EKF2_HGT_REF、EKF2_MAG_TYPE）直接影响飞行稳定性和故障恢复能力。

上一篇：协方差矩阵预测
下一篇：高度融合与多源仲裁
系列总目录：PX4 v1.16 源码解读总目录