1. 陀螺仪数据读取与预处理

任务说明

本案例实现运动相机IMU陀螺仪数据的读取与预处理闭环：从传感器读取原始三轴角速度，经过去噪（滑动平均滤波）与校准（零偏扣除），输出可直接用于后续EIS算法的角速度序列。代码基于通用I2C接口的IMU（以MPU6050为例），通过模拟数据接口保证可独立运行验证。

核心实现

import numpy as np
import time

# ---------- 模拟IMU驱动（真实场景替换为寄存器读写） ----------
class MockIMU:
    """模拟MPU6050陀螺仪，输出含噪声与零偏的原始数据"""
    def __init__(self, fs=1000.0, gyro_bias=(0.5, -0.3, 0.8), noise_std=0.15):
        self.fs = fs
        self.dt = 1.0 / fs
        self.bias = np.array(gyro_bias, dtype=np.float64)
        self.noise_std = noise_std
        self.t = 0.0
        # 真实角速度：低频摆动 + 高频抖动
        self.true_rate = lambda t: np.array([
            2.0 * np.sin(2 * np.pi * 1.2 * t),
            1.5 * np.cos(2 * np.pi * 0.8 * t),
            0.8 * np.sin(2 * np.pi * 2.0 * t)
        ])

    def read_gyro(self):
        """返回原始三轴角速度(deg/s)，含零偏与高斯噪声"""
        raw = self.true_rate(self.t) + self.bias
        raw += np.random.normal(0, self.noise_std, 3)
        self.t += self.dt
        return raw.copy()

# ---------- 预处理核心 ----------
class GyroPreprocessor:
    def __init__(self, window_size=5):
        self.window_size = window_size
        self.buffer = []          # 滑动窗口缓存
        self.bias_estimate = None # 静态零偏估计

    def calibrate_bias(self, imu, samples=200):
        """静止状态下采集N个样本，取均值作为零偏"""
        if self.bias_estimate is not None:
            return self.bias_estimate
        samples_list = []
        for _ in range(samples):
            samples_list.append(imu.read_gyro())
        self.bias_estimate = np.mean(samples_list, axis=0)
        return self.bias_estimate

    def denoise(self, sample):
        """滑动平均去噪，返回滤波后的单点值"""
        self.buffer.append(sample.copy())
        if len(self.buffer) > self.window_size:
            self.buffer.pop(0)
        return np.mean(self.buffer, axis=0)

    def process_sample(self, raw_sample):
        """完整预处理：去噪 -> 去零偏"""
        filtered = self.denoise(raw_sample)
        if self.bias_estimate is None:
            raise RuntimeError("请先调用calibrate_bias()完成零偏校准")
        return filtered - self.bias_estimate

# ---------- 主流程 ----------
def main():
    # 初始化IMU与预处理器
    imu = MockIMU(fs=500.0)
    pre = GyroPreprocessor(window_size=5)

    # 1. 静止校准（前200个样本）
    bias = pre.calibrate_bias(imu, samples=200)
    print(f"校准零偏(deg/s): X={bias[0]:.3f}, Y={bias[1]:.3f}, Z={bias[2]:.3f}")

    # 2. 采集并预处理100个样本
    processed_series = []
    raw_series = []
    for _ in range(100):
        raw = imu.read_gyro()
        raw_series.append(raw.copy())
        clean = pre.process_sample(raw)
        processed_series.append(clean.copy())

    # 3. 输出统计结果
    raw_arr = np.array(raw_series)
    clean_arr = np.array(processed_series)
    print(f"\n原始数据均值(含零偏): {np.mean(raw_arr, axis=0)}")
    print(f"预处理后均值(应接近真实值): {np.mean(clean_arr, axis=0)}")
    print(f"预处理后标准差(噪声抑制效果): {np.std(clean_arr, axis=0)}")

if __name__ == "__main__":
    main()

落地与验证

运行方式：保存为gyro_preprocess.py，执行python gyro_preprocess.py。依赖仅需NumPy。

验证方法：观察输出中“预处理后均值”应接近模拟真实角速度的统计均值（由于模拟信号为周期函数，100个样本内均值应接近0附近），且“预处理后标准差”应明显小于原始含噪数据的标准差（原始噪声σ=0.15，滤波后理论σ≈0.15/√5≈0.067）。若校准零偏输出与设定值(0.5,-0.3,0.8)偏差小于0.05，且预处理后均值较原始均值更接近0，则判定通过。

判定标准

1. 校准零偏输出与设定值(0.5, -0.3, 0.8)的误差小于0.05 deg/s；
2. 预处理后角速度序列的均值值小于0.1 deg/s（原始含零偏数据均值约为0.5~0.8）；
3. 预处理后标准差小于0.1 deg/s，明显低于原始噪声水平0.15 deg/s。
满足以上三点即验证通过。真实硬件场景下，将MockIMU替换为I2C寄存器读取驱动，并保持预处理逻辑不变即可直接复用。