PX4 EKF2 高度融合与多源仲裁

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 814 阅读时长 ≈ 3 分钟
深入解析 PX4 EKF2 高度融合的实现：从 controlHeightFusion 的调度逻辑，到气压计/GNSS/测距仪/外部视觉各高度源的融合机制，以及高度参考切换的自动降级策略。

一、高度融合的核心挑战

高度估计需要解决以下问题：

  1. 多源矛盾：气压计（长期稳定但受环境影响）、GPS 高度（绝对但精度低）、测距仪（高精度但距离有限）、外部视觉（精确但依赖基础设施）同时工作时，如何仲裁
  2. 坐标系差异：PX4 内部高度（NED 坐标 D 轴，向下为正），外部 API 高度（altitude，向上为正），需要符号统一
  3. 地面效应：近地面时气压计受旋翼下洗流影响，导致高度读数偏低
  4. 参考切换：主高度源失效时，无缝切换至备用源，避免高度跳变

二、高度传感器选择参数

EKF2_HGT_REF 参数控制首选高度源：

参数值 传感器 适用场景
0 气压计（默认） 通用室外场景
1 GPS 高度 需要绝对高度参考
2 测距仪 低空精确悬停
3 外部视觉 室内精确定位

实际工作中，多个高度源可以同时工作，只是只有一个是”参考”（reference），其他作为辅助约束。

三、controlHeightFusion() 实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
flowchart TD
    A["controlHeightFusion"] --> B["checkVerticalAccelerationHealth"]
    B --> C{"垂直加速度<br/>是否异常?"}
    C --是--> D["设置 bad_vert_accel 标志<br/>抑制垂直状态更新"]
    C --否--> E["controlGroundEffect<br/>地面效应补偿"]
    E --> F["controlBaroHeightFusion<br/>气压计高度"]
    F --> G["controlGnssHeightFusion<br/>GNSS高度"]
    G --> H["controlRangeHeightFusion<br/>测距仪高度"]
    H --> I["controlEvHeightFusion<br/>外部视觉高度"]
    I --> J["checkHeightSensorRefFallback<br/>主参考健康检查"]
    J --> K{"主参考是否持续输出?"}
    K --是--> L("[继续当前参考]")
    K --否超时--> M["按优先级选择下一个可用源"]
    M --> N["resetHeightToBaro/GPS/Range/EV<br/>执行高度重置对齐"]
    N --> L

3.1 垂直加速度健康检查

1
2
3
4
5
6
7
8
9
10
11
12
void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_sample) {
    // 检测加速度计纵轴削波（delta_vel_clipping[2]）
    if (imu_sample.delta_vel_clipping[2]) {
        _clip_counter++;
        if (_clip_counter > CLIP_COUNT_THRESHOLD) {
            _fault_status.flags.bad_acc_vertical = true;
        }
    } else {
        _clip_counter = math::max(0, _clip_counter - 1);
    }
    // 恢复条件：连续若干步无削波
}

当 bad_acc_vertical 置位时，EKF 停止使用加速度计进行垂直状态预测（防止削波导致高度突变）。

3.2 气压计高度融合

气压计高度单位转换：气压计提供的是气压高度（相对起飞点 MSL 高度差）：

1
2
3
4
// 新息计算（NED 坐标，D 轴向下为正）
// EKF 内部高度 = -_state.pos(2)（向上为正）
// 气压计高度 = baro_sample.hgt（向上为正，相对参考）
float innov = -_state.pos(2) - (_baro_hgt_offset + baro_sample.hgt);

_baro_hgt_offset 是气压计高度与 EKF 原点的偏差，在初始化或高度重置时更新。

3.3 GNSS 高度融合

GPS 高度使用 WGS84 椭球高，与气压计 MSL 高度存在大地水准面起伏差（geoid undulation），需要通过 _gpos.altitude() 接口统一处理。

1
2
3
4
// GNSS 高度新息
float innov = _gpos.altitude() - gnss_sample.alt;
// 注意符号：altitude() 返回向上的高度，gnss_sample.alt 也是向上
// EKF 内部通过坐标变换对齐

3.4 测距仪高度融合

测距仪测量地面以上高度（HAGL），而非 NED 原点高度：

1
2
3
4
// 测距仪高度新息（HAGL）
// _terrain_vpos 是估计的地面高度（NED D轴值）
float innov = -(gpos_height - _terrain_vpos) - rng_sample.rng;
// 等价于：EKF估计的HAGL - 测距仪读数

四、高度参考自动切换

checkHeightSensorRefFallback() 检测主高度参考是否超时无数据：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void Ekf::checkHeightSensorRefFallback() {
    // 超时阈值：2 * 最大传感器延迟
    const uint64_t timeout_us = 2 * _params.hgt_timeout_us;

    if (_time_last_hgt_fuse + timeout_us < _time_delayed_us) {
        // 主高度源超时，触发切换
        switch (_params.hgt_sensor_ref) {
        case HeightSensor::EV:
            if (isBaroHealthy()) resetHeightToBaro();
            else if (isGnssHgtHealthy()) resetHeightToGnss();
            break;
        case HeightSensor::GNSS:
            if (isBaroHealthy()) resetHeightToBaro();
            break;
        // ...
        }
    }
}

切换时会执行 resetHeight*()，将 EKF 高度状态直接跳变到新传感器的读数，同时：

  1. 更新高度参考偏置（_*_hgt_offset）
  2. 清除高度状态的协方差相关性
  3. 重置 OutputPredictor 中的高度积分

五、速度融合通用模式

速度融合（velocity_fusion.cpp）统一处理各传感器速度：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void Ekf::fuseVelocity(AidSourceStatus<3>& status) {
    // 3D 速度融合，每轴独立执行
    for (int i = 0; i < 3; i++) {
        // 新息
        status.innovation[i] = _state.vel[i] - status.observation[i];

        // 新息方差
        status.innovation_variance[i] = P(3+i, 3+i) + status.observation_variance[i];

        // 新息检验
        const float test_ratio = sq(status.innovation[i]) / status.innovation_variance[i];
        if (test_ratio > sq(_params.vel_innov_gate)) {
            status.innovation_rejected = true;
            continue;
        }

        // 卡尔曼更新（单维测量）
        VectorState K = P.col(3+i) / status.innovation_variance[i];
        applyKalmanGainToState(K, status.innovation[i]);
        applyKalmanGainToCovariance(K, 3+i);
    }
}

六、航向融合 fuseYaw()

航向角 ψ 无法由重力观测，必须依赖磁力计或 GPS 双天线或外部视觉：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
void Ekf::fuseYaw(AidSourceStatus<1>& status) {
    // 观测矩阵 H 由 Symforce 自动生成（非线性，ψ 是四元数 q 的函数）
    sym::ComputeYawInnovVarAndH(q, R_yaw, &status.innovation_variance[0], &H);

    // 病态检查：新息方差必须大于观测噪声
    if (status.innovation_variance[0] < status.observation_variance[0]) {
        // P 矩阵崩溃（非正定），触发全局协方差重置
        initialiseCovariance();
        return;
    }

    // 卡尔曼更新
    measurementUpdate(status, H, status.innovation, status.innovation_variance);
}

航向重置 resetQuatStateYaw()：当航向误差过大时，直接重置四元数而非卡尔曼更新：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
void Ekf::resetQuatStateYaw(float yaw, float yaw_var) {
    // 构建仅包含 yaw 变化的旋转四元数
    const Dcmf R_body_to_earth{_state.quat_nominal};
    // 提取当前 roll/pitch，替换 yaw
    const Eulerf euler_angles(R_body_to_earth);
    _state.quat_nominal = Quatf(Eulerf(euler_angles.phi(), euler_angles.theta(), yaw));

    // 通知 OutputPredictor 同步更新（外环控制器感知坐标系旋转）
    _output_predictor.resetQuatState(_state.quat_nominal);

    // 更新协方差（清零 yaw 相关项，设置新的 yaw 方差）
    P(2,2) = yaw_var;
    // 清除 yaw 与其他状态的相关性
    for (int i = 0; i < _k_num_states; i++) {
        if (i != 2) {
            P(2,i) = P(i,2) = 0.0f;
        }
    }
}

七、小结

EKF2 高度融合的关键设计：

  1. 多源并行：气压计、GPS、测距仪、EV 可同时工作，各自提供独立约束
  2. 参考仲裁：单一主参考 + 自动降级，失效时平滑切换
  3. 坐标统一：所有高度源通过偏置对齐到 EKF 内部 NED 坐标原点
  4. 故障隔离：垂直加速度健康检查 + 地面效应补偿，防止特殊场景下的错误融合

航向融合的病态检查体现了 EKF2 在数值稳定性方面的防御性编程思路。

上一篇：融合调度 control.cpp
下一篇：辅助函数与卡尔曼更新 ekf_helper.cpp
系列总目录：PX4 v1.16 源码解读总目录