4. 数据采集与预处理：传感器数据同步、滤波算法与异常值处理

各位同学，欢迎来到第四章。这一章咱们要聊的，是嵌入式多传感器融合里最“脏”最“累”的活——数据采集与预处理。

说实话，很多刚入行的工程师，一上来就急着调融合算法，结果数据乱七八糟，算法再牛也白搭。我自己的经验是，预处理做得好，后面至少省一半的调试时间。你想想看，传感器出来的原始数据，就像没洗过的菜，直接下锅能好吃吗？

4.1 传感器数据同步：别让时间戳骗了你

多传感器融合的第一个坑，就是时间不同步。我见过一个项目，IMU和摄像头的数据差了50毫秒，结果融合出来的姿态一直在抖，查了三天才发现是时间戳没对齐。

为什么会这样？因为每个传感器都有自己的时钟源，采样频率也不一样。比如IMU可能跑200Hz，摄像头只有30Hz，GPS更是只有10Hz。你直接拿时间戳去对齐，误差会越来越大。

我常用的同步策略有三种：

• 硬件同步：用同一个外部中断触发所有传感器。精度最高，但硬件上要额外布线。我在做无人机飞控时用过，效果确实好，但成本也上去了。
• 软件时间戳对齐：每个数据包到达时，立即打上系统时间戳。然后通过插值或最近邻匹配，把不同频率的数据对齐到同一个时间轴上。这是最常用的方法。
• 缓冲队列法：为每个传感器维护一个环形缓冲区，按时间顺序排列。融合时从缓冲区里取时间最接近的数据。嗯，这里要注意，缓冲区大小要合理，太大会引入延迟，太小又容易丢数据。

4.2 滤波算法：从均值到卡尔曼，一步步来

滤波，说白了就是让数据变“干净”。传感器原始数据里，噪声是躲不掉的。我刚开始做项目时，看到ADC采出来的电压值跳来跳去，还以为电路坏了，后来才知道是正常的噪声。

4.2.1 均值滤波：简单但有效

均值滤波是最基础的。取N个采样值，求个平均数。代码很简单：

// 均值滤波示例
#define FILTER_SIZE 10
static float buffer[FILTER_SIZE];
static int index = 0;

float mean_filter(float new_value) {
    buffer[index] = new_value;
    index = (index + 1) % FILTER_SIZE;
    
    float sum = 0;
    for (int i = 0; i < FILTER_SIZE; i++) {
        sum += buffer[i];
    }
    return sum / FILTER_SIZE;
}

但要注意，均值滤波对突发噪声很敏感。比如你测温度，突然有个干扰脉冲，均值会被拉偏。我在一个环境监测项目里就吃过这个亏，后来改用了中值滤波。

4.2.2 中值滤波：抗干扰能力强

中值滤波的原理很简单：把N个值排序，取中间那个。它对脉冲噪声的抑制效果特别好。

// 中值滤波示例
#define MEDIAN_SIZE 5

float median_filter(float new_value) {
    static float buffer[MEDIAN_SIZE];
    static int index = 0;
    float temp[MEDIAN_SIZE];
    
    buffer[index] = new_value;
    index = (index + 1) % MEDIAN_SIZE;
    
    // 拷贝并排序
    memcpy(temp, buffer, sizeof(temp));
    // 冒泡排序（实际项目中建议用更高效的排序）
    for (int i = 0; i < MEDIAN_SIZE - 1; i++) {
        for (int j = 0; j < MEDIAN_SIZE - i - 1; j++) {
            if (temp[j] > temp[j + 1]) {
                float t = temp[j];
                temp[j] = temp[j + 1];
                temp[j + 1] = t;
            }
        }
    }
    return temp[MEDIAN_SIZE / 2];
}

4.2.3 卡尔曼滤波入门：别被名字吓到

卡尔曼滤波，听起来很高大上，其实核心思想就一句话：用预测值修正测量值，再用测量值修正预测值。

我当年第一次看卡尔曼滤波的公式，也是一头雾水。后来在做一个平衡车项目时，发现IMU的数据噪声太大，用均值滤波延迟又太高，硬着头皮上了卡尔曼。结果发现，只要理解了那五个公式，其实没那么难。

卡尔曼滤波的五个核心公式：

步骤	公式	说明
预测状态	x̂k|k-1 = A·x̂k-1|k-1 + B·uk	根据上一时刻的状态，预测当前状态
预测协方差	Pk|k-1 = A·Pk-1|k-1·AT + Q	预测的不确定性
卡尔曼增益	Kk = Pk|k-1·HT·(H·Pk|k-1·HT + R)-1	相信预测还是相信测量？
更新状态	x̂k|k = x̂k|k-1 + Kk·(zk - H·x̂k|k-1)	用测量值修正预测值
更新协方差	Pk|k = (I - Kk·H)·Pk|k-1	更新不确定性

对于初学者，我建议先理解一维的卡尔曼滤波。比如只滤波一个温度值：

// 一维卡尔曼滤波示例
typedef struct {
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
    float P;  // 估计误差协方差
    float K;  // 卡尔曼增益
    float X;  // 状态估计值
} KalmanFilter;

void kalman_init(KalmanFilter* kf, float init_value) {
    kf->Q = 0.01;   // 根据实际情况调整
    kf->R = 0.1;    // 根据传感器手册或实测
    kf->P = 1.0;
    kf->X = init_value;
}

float kalman_update(KalmanFilter* kf, float measurement) {
    // 预测
    kf->P = kf->P + kf->Q;
    
    // 更新卡尔曼增益
    kf->K = kf->P / (kf->P + kf->R);
    
    // 更新状态估计
    kf->X = kf->X + kf->K * (measurement - kf->X);
    
    // 更新协方差
    kf->P = (1 - kf->K) * kf->P;
    
    return kf->X;
}

4.3 异常值处理：别让一个坏数据毁了整个系统

传感器偶尔会抽风，这是常态。比如超声波测距突然跳出一个极大的值，或者GPS信号被遮挡后位置乱飘。如果不处理，融合算法会把这些异常值当成真实数据，结果可想而知。

我常用的异常值处理方法：

• 阈值法：设定一个合理的范围，超出范围的数据直接丢弃。比如温度传感器，正常范围是-40°C到85°C，突然来个200°C，肯定是错的。
• 变化率限制：相邻两次采样的差值不能太大。比如加速度计，正常变化率不会超过某个值，如果突然跳变，大概率是干扰。
• 3σ原则：假设数据服从正态分布，超出均值±3倍标准差的数据视为异常。这个方法在静态测量中很好用，但动态场景下要小心。
• 中值替代法：检测到异常值时，用前N个有效值的中值替代。这样既不会丢数据，也不会引入太大误差。

4.4 实战建议：预处理流程怎么搭？

说了这么多，到底怎么落地？我一般按这个流程来：

1. 数据采集：每个传感器独立线程/任务，数据到达后立即打时间戳，放入环形缓冲区。
2. 时间同步：以主传感器（通常是IMU或摄像头）的时间为基准，其他传感器数据通过插值对齐。
3. 异常值检测：先做阈值判断，再做变化率判断。异常数据标记但不删除，方便后续分析。
4. 滤波处理：根据传感器特性和应用场景，选择合适的滤波算法。我一般先用中值滤波去脉冲噪声，再用卡尔曼滤波平滑。
5. 数据输出：预处理后的数据，按固定格式打包，送给融合算法模块。

嗯，这一章的内容就到这里。预处理看起来琐碎，但绝对是决定项目成败的关键。下一章我们会聊传感器标定，那也是个大坑，到时候再细说。

公众号:蓝海资料掘金营，微信deep3321