2. 传感器数据采集与预处理

传感器把物理世界的信号变成了电信号，这第一步算是完成了。但接下来呢？你得把这些信号「读」进来，还得读得准、读得快。我见过不少项目，传感器选得挺好，结果栽在数据采集上——不是接口时序没调对，就是噪声把信号淹没了。

这一节，咱们就聊聊数据采集的那些事儿。从硬件接口到软件滤波，再到时间同步，每一步都有坑，也有技巧。

2.1 数据采集接口：I2C、SPI、UART

选哪个接口，说白了取决于你的传感器和主控能怎么连。三种接口各有脾气，我列个表给你看：

接口 通信方式 速度 引脚数 典型场景
I2C 同步、半双工 100kHz~3.4MHz 2（SDA+SCL） 温湿度、气压、IMU
SPI 同步、全双工 10MHz~80MHz 4（MOSI+MISO+SCLK+CS） 高速ADC、陀螺仪、激光雷达
UART 异步、全双工 9600bps~10Mbps 2（TX+RX） GPS、串口屏、老式传感器
我的小建议： 如果传感器数量多、布线紧张，优先用 I2C。如果对实时性要求高，比如读取高速旋转的编码器，那就上 SPI。UART 嘛，适合距离稍远或者主控没有多余 I2C/SPI 接口的情况。

2.1.1 I2C 采集实战

I2C 的麻烦在于地址冲突和时钟延展。我记得有一次调试一个九轴 IMU，死活读不到数据，折腾了半天才发现是上拉电阻焊错了——4.7kΩ 换成了 10kΩ，信号上升沿太慢，主控不认。

下面是一个标准的 I2C 读取代码片段，以 MPU6050 为例：

#include <Wire.h>

#define MPU6050_ADDR 0x68

void setup() {
  Wire.begin();
  Serial.begin(115200);
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);  // 电源管理寄存器
  Wire.write(0x00);  // 唤醒传感器
  Wire.endTransmission();
}

void loop() {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);  // 加速度计数据起始地址
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 6, true);
  
  int16_t ax = Wire.read() << 8 | Wire.read();
  int16_t ay = Wire.read() << 8 | Wire.read();
  int16_t az = Wire.read() << 8 | Wire.read();
  
  Serial.print("Accel: "); Serial.print(ax); Serial.print(", ");
  Serial.print(ay); Serial.print(", "); Serial.println(az);
  delay(100);
}
注意： I2C 总线上每个设备地址必须唯一。如果两个传感器用了同一个地址，要么换芯片，要么用多路复用器（比如 TCA9548A）。我吃过这个亏，一次接了四个同样的气压计，结果全挤在 0x76 上，谁也读不了。

2.1.2 SPI 采集实战

SPI 比 I2C 快，但引脚多。它的核心是片选（CS）——每个从设备独占一根 CS 线。我习惯在读取前后把 CS 拉低再拉高，确保时序干净。

void readSPI(uint8_t csPin, uint8_t reg, uint8_t *data, uint8_t len) {
  digitalWrite(csPin, LOW);
  SPI.transfer(reg);          // 发送寄存器地址
  for (uint8_t i = 0; i < len; i++) {
    data[i] = SPI.transfer(0x00);  // 读取数据
  }
  digitalWrite(csPin, HIGH);
}

嗯，这里要注意：SPI 的时钟极性和相位（CPOL、CPHA）必须和传感器手册一致。我曾经因为 CPHA 设反了，读出来的数据全是 0xFF，还以为是传感器坏了。

2.1.3 UART 采集实战

UART 最简单，但波特率要对。GPS 模块常用 9600 或 115200，别搞混了。我一般用中断方式接收，避免阻塞主循环。

void setup() {
  Serial2.begin(115200);  // 使用 UART2
}

void loop() {
  if (Serial2.available()) {
    String line = Serial2.readStringUntil('\n');
    if (line.startsWith("$GPGGA")) {
      // 解析 GPS 数据
      parseGPGGA(line);
    }
  }
}

2.2 滤波算法：让数据「干净」起来

传感器原始数据通常很「脏」。噪声来自电源、机械振动、电磁干扰……你想想看，一个加速度计装在机械臂上，读数抖得像筛子，不滤波根本没法用。

三种常用滤波算法，我按场景给你拆开讲。

2.2.1 均值滤波

最简单，也最直观。连续采 N 个值，取平均。适合噪声是白噪声的场景，比如温度、气压。

#define FILTER_SIZE 10
float buffer[FILTER_SIZE];
int index = 0;

float meanFilter(float newValue) {
  buffer[index] = newValue;
  index = (index + 1) % FILTER_SIZE;
  
  float sum = 0;
  for (int i = 0; i < FILTER_SIZE; i++) {
    sum += buffer[i];
  }
  return sum / FILTER_SIZE;
}
缺点： 对突发噪声敏感。如果某个采样值突然跳变（比如被静电干扰了一下），均值会被拉偏。另外，N 越大，延迟越大。

2.2.2 中值滤波

把 N 个值排序，取中间那个。对付椒盐噪声（比如激光雷达的飞点）特别好用。

#define MEDIAN_SIZE 5

float medianFilter(float newValue) {
  static float window[MEDIAN_SIZE];
  static int idx = 0;
  
  window[idx] = newValue;
  idx = (idx + 1) % MEDIAN_SIZE;
  
  float sorted[MEDIAN_SIZE];
  memcpy(sorted, window, sizeof(sorted));
  
  // 冒泡排序（N 小，够用）
  for (int i = 0; i < MEDIAN_SIZE - 1; i++) {
    for (int j = 0; j < MEDIAN_SIZE - i - 1; j++) {
      if (sorted[j] > sorted[j+1]) {
        float tmp = sorted[j];
        sorted[j] = sorted[j+1];
        sorted[j+1] = tmp;
      }
    }
  }
  return sorted[MEDIAN_SIZE / 2];
}
我的经验： 中值滤波的窗口大小选 3 或 5 就够了。窗口太大，计算量上去了，而且会抹掉真实的边缘变化。我在做超声波测距时用过 7 点中值，结果移动目标跟丢了。

2.2.3 卡尔曼滤波

这个稍微复杂点，但效果最好。它结合了预测和观测，能给出最优估计。说白了，就是「猜一个值，再拿测量值修正它」。

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

  1. 预测状态： x̂k|k-1 = x̂k-1|k-1
  2. 预测协方差： Pk|k-1 = Pk-1|k-1 + Q
  3. 卡尔曼增益： Kk = Pk|k-1 / (Pk|k-1 + R)
  4. 更新状态： x̂k|k = x̂k|k-1 + Kk(zk - x̂k|k-1)
  5. 更新协方差： Pk|k = (1 - Kk)Pk|k-1

代码实现：

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

void kalmanInit(KalmanFilter *kf, float initValue) {
  kf->Q = 0.01;
  kf->R = 0.1;
  kf->P = 1.0;
  kf->X = initValue;
}

float kalmanUpdate(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;
}
调参提醒： Q 和 R 的比值决定了滤波器的「信任倾向」。Q 大，更相信测量；R 大，更相信预测。我调四轴飞行器的高度数据时，R 设太小，结果滤波器跟着噪声乱跳；R 设太大，又跟不上快速升降。最后 Q=0.01、R=0.5 才稳住。

2.3 数据对齐与时间戳同步

多传感器系统里，每个传感器采集的时刻可能不一样。你想想看，摄像头 30fps，IMU 200Hz，激光雷达 10Hz——如果不做时间同步，融合出来的轨迹就是「鸡同鸭讲」。

我遇到过最典型的问题：视觉 SLAM 里，图像和 IMU 数据差了 20ms，结果轨迹估计直接发散。后来加了硬件时间戳才解决。

2.3.1 软件时间戳

最简单的方法：在读取传感器数据的瞬间，记录当前系统时间（微秒级）。

uint32_t timestamp_us = micros();
float value = readSensor();
saveToBuffer(timestamp_us, value);

但这里有个坑：如果传感器内部有 FIFO 缓存，你读到的是「之前」的数据。比如 MPU6050 的 FIFO 可以存 32 组数据，你读的时候，数据其实是几百微秒前采的。这时候软件时间戳就不准了。

2.3.2 硬件时间戳

更可靠的做法：让传感器在数据就绪时触发一个中断，主控在中断服务函数里打时间戳。

void sensorISR() {
  timestamp_us = micros();  // 硬件触发，时间最准
  sensorReady = true;
}

void loop() {
  if (sensorReady) {
    sensorReady = false;
    float data = readSensor();
    saveWithTimestamp(timestamp_us, data);
  }
}
核心原则： 时间戳越靠近物理事件发生的时刻，精度越高。硬件中断优于轮询，轮询优于软件延迟补偿。

2.3.3 多传感器时间对齐

当你有多个传感器时，需要把数据插值到同一个时间基准上。常用的方法是线性插值：

// 已知 t1 时刻的值 v1，t2 时刻的值 v2
// 求 t 时刻的值（t1 < t < t2）
float interpolate(float t1, float v1, float t2, float v2, float t) {
  float ratio = (t - t1) / (t2 - t1);
  return v1 + ratio * (v2 - v1);
}

举个例子：IMU 在 t=10ms 时输出 1.2m/s²，t=15ms 时输出 1.5m/s²。你想知道 t=12ms 时的值，插值一下就是 1.32m/s²。

2.4 本章知识体系总览

下面这张图把数据采集到预处理的完整链路串起来了。你可以把它当作一个检查清单：

传感器数据采集与预处理流程 传感器数据采集与预处理流程 物理传感器 数据采集接口 I2C SPI UART 滤波算法 均值滤波 中值滤波 卡尔曼滤波 数据对齐与时间戳同步 软件时间戳 硬件时间戳 线性插值对齐 干净、对齐的数据 接口选不对，后面全白费 滤波不匹配，数据没法用 时间不同步，融合必翻车
一句话总结： 接口选对、滤波用对、时间戳打准——这三步走扎实了，后面的轨迹调整才有好数据可用。别急着调算法，先把数据链路搞干净。
← 上一章 📖 返回目录 下一章 →