PX4 EKF2 RingBuffer 环形缓冲区详解

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 540 阅读时长 ≈ 2 分钟
深入解析 PX4 EKF2 中 RingBuffer 的实现结构，重点说明延迟时间融合架构中 RingBuffer 如何保存历史 IMU 数据，以及 pop_first_older_than() 查询机制。

一、延迟融合的核心问题

EKF2 的延迟时间融合架构要求：当 GPS 数据（延迟 ~110 ms）到达时，需要找到 110 ms 前对应时刻的 IMU 预测状态进行融合，而不是与当前时刻的 IMU 状态对齐。

解决方案是 RingBuffer<T>——一个固定大小的环形缓冲区，持续保存一段历史时间窗口内的传感器数据。

二、RingBuffer 结构

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
template <typename data_type>
class RingBuffer {
public:
    // 初始化，分配 size 个槽位
    bool allocate(int size);
    void unallocate();

    // 写入最新数据（覆盖最旧）
    void push(const data_type &sample);

    // 读取最旧数据（不移除）
    data_type &get_oldest();
    int get_oldest_index() const;

    // 按时间戳查询：找到第一个时间戳 <= time_us 的数据，移除并返回
    bool pop_first_older_than(uint64_t time_us, data_type *sample);

    // 状态
    int get_length() const;
    int get_total_size() const;
    bool empty() const;

private:
    data_type *_buffer{nullptr};  // 动态分配的环形数组
    int _head{0};   // 最新数据的索引
    int _tail{0};   // 最旧数据的索引
    int _size{0};   // 缓冲区槽位数
    bool _first_write{true};
};

三、关键操作详解

3.1 push(sample) — 写入

1
2
3
写入前: [oldest] → ... → [head]
写入后: [oldest+1] → ... → [head] → [new sample]
若满则覆盖最旧

实现逻辑（伪代码）：

1
2
3
4
5
_head = (_head + 1) % _size;
if (_head == _tail) {  // 满了，移动 tail
    _tail = (_tail + 1) % _size;
}
_buffer[_head] = sample;

每次 IMU 降采样（~250 Hz）后的 imuSample 都会被 push 进 IMU RingBuffer，缓冲区深度通常为 maxDelay / dtImu，保证足够覆盖最大传感器延迟（通常设 0.5 s）。

3.2 pop_first_older_than(time_us, sample) — 延迟对齐查询

这是延迟补偿的核心操作：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool pop_first_older_than(uint64_t time_us, data_type *sample) {
    // 从 tail（最旧）往 head（最新）遍历
    for (uint8_t i = 0; i < _size; i++) {
        int index = (_tail + i) % _size;
        if (_buffer[index].time_us <= time_us) {
            // 找到第一个时间戳 <= time_us 的条目
            *sample = _buffer[index];
            // 推进 tail（丢弃已使用的旧数据）
            _tail = (index + 1) % _size;
            return true;
        }
    }
    return false;  // 缓冲区中没有足够旧的数据
}

调用时机：control.cpp 中，当 GPS 数据到达时：

1
2
3
4
5
6
gnssSample gps_sample;
if (_gps_buffer.pop_first_older_than(
        _time_delayed_us,   // 当前处理的延迟时刻
        &gps_sample)) {
    controlGpsFusion(gps_sample);  // 用延迟时刻的GPS数据融合
}

3.3 get_oldest() — 获取历史最旧 IMU

ekf.cpp 主循环中：

1
2
3
4
5
6
7
8
9
// 从 IMU RingBuffer 取出最旧（即最大延迟时刻）的 IMU 数据
_imu_sample_delayed = _imu_buffer.get_oldest();

// 更新内部时钟（延迟时刻推进一步）
_time_delayed_us = _imu_sample_delayed.time_us;

// 用该延迟 IMU 驱动一步预测
predictState(_imu_sample_delayed);
predictCovariance(_imu_sample_delayed);

四、各传感器缓冲区配置

缓冲区 类型 深度（典型） 最大延迟
IMU imuSample ~20 由最大传感器延迟决定
GNSS gnssSample 10 EKF2_GPS_DELAY = 110 ms
磁力计 magSample 5 EKF2_MAG_DELAY = 0 ms
气压计 baroSample 10 EKF2_BARO_DELAY = 0 ms
光流 flowSample 15 EKF2_OF_DELAY = 5 ms
测距仪 rangeSample 10 EKF2_RNG_DELAY = 5 ms
外部视觉 extVisionSample 15 EKF2_EV_DELAY = 175 ms

延迟参数通过 EKF2_*_DELAY PX4 参数设定，需与实际传感器延迟匹配，否则会造成状态跳变。

五、与 IMU 降采样器的配合

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
sequenceDiagram
    participant IMU_Driver as IMU驱动 (1kHz)
    participant Downsampler as ImuDownsampler (降采样)
    participant IMU_Buffer as IMU RingBuffer
    participant GPS_Buffer as GPS RingBuffer
    participant EKF as ekf.cpp update()

    IMU_Driver->>Downsampler: 原始角速度+加速度 @ 1kHz
    Downsampler->>Downsampler: 累积增量 (4次 = 1步@250Hz)
    Downsampler->>IMU_Buffer: push("imuSample") @ ~250Hz
    Note over EKF: 每次IMU_Buffer有新数据
    EKF->>IMU_Buffer: get_oldest() 取最大延迟时刻IMU
    EKF->>EKF: predictState() + predictCovariance()
    GPS_Buffer->>EKF: pop_first_older_than("time_delayed_us, &gps")
    EKF->>EKF: controlGpsFusion("gps")
    EKF->>EKF: measurementUpdate() 卡尔曼更新状态+协方差

六、关键约束与注意事项

  1. 缓冲区大小必须足够覆盖最大传感器延迟：size >= delay_ms / dt_imu_ms。estimator_interface.cpp 在初始化时根据参数动态计算并分配。
  2. RingBuffer 不是线程安全的。EKF2 整体运行在单个 WorkQueue 回调中，无并发问题。
  3. 若 pop_first_older_than 返回 false（缓冲区中没有足够旧的数据），表示当前延迟时刻还没有对应传感器数据，本周期跳过该传感器融合。
  4. 缓冲区满时 push 会自动覆盖最旧条目，因此实时性较差的传感器（EV 延迟 175 ms）需要更大的缓冲区。

上一篇：common.h 数据结构
下一篇：State.h 状态向量
系列总目录：PX4 v1.16 源码解读总目录