第三章 传感器数据采集与预处理：I2C/SPI/UART接口驱动、数据滤波与坐标变换

各位同学，欢迎来到第三章。这一章的内容，说白了就是无人机避障系统的「感官神经」怎么工作。传感器把物理世界的距离、角度变成电信号，但信号是脏的、乱的，你得把它洗干净、翻译好，才能喂给飞控大脑。

我个人习惯把这一章分成三个部分来讲：接口驱动（怎么把数据读进来）、数据滤波（怎么把噪声去掉）、坐标变换（怎么把传感器数据对齐到无人机坐标系）。这三步走完，传感器数据才算真正能用。

3.1 传感器接口驱动：I2C、SPI、UART

先说说接口。无人机上常见的测距传感器，比如超声波模块、激光雷达、红外传感器，用的基本都是这三种协议。你想想看，选哪个接口，其实取决于三个因素：速度、引脚数、距离。

接口 速度 引脚数 典型距离 常见传感器
I2C 100kHz~400kHz 2 (SDA, SCL) 板级（<30cm） VL53L0X激光测距
SPI 1MHz~20MHz 4 (MOSI, MISO, SCK, CS) 板级（<20cm） TFmini激光雷达
UART 9600~115200bps 2 (TX, RX) 可达数米 HC-SR04超声波模块

嗯，这里要注意：I2C虽然引脚少，但速度慢，而且总线上的设备多了容易出时序问题。我在项目中遇到过，挂三个VL53L0X在一条I2C总线上，结果其中一个老是丢数据。后来我改成每个传感器单独用一条I2C总线，问题就解决了。说白了，不要贪图省引脚而牺牲可靠性。

3.1.1 I2C驱动要点

I2C的驱动其实不复杂，但坑不少。我建议你直接用MCU的硬件I2C外设，别自己用GPIO模拟。为什么？因为硬件I2C有超时检测、仲裁机制，模拟的很容易卡死在某个状态。

下面是我常用的I2C读取距离传感器的代码模板：

// I2C读取VL53L0X距离值
uint16_t vl53l0x_read_distance(void) {
    uint8_t reg = 0x00;  // 距离寄存器地址
    uint8_t buf[2] = {0};
    
    // 写寄存器地址
    i2c_master_write(VL53L0X_ADDR, &reg, 1);
    delay_ms(10);  // 等待传感器转换完成
    
    // 读2字节距离数据
    i2c_master_read(VL53L0X_ADDR, buf, 2);
    
    return (buf[0] << 8) | buf[1];
}

这里有个小技巧：读数据之前一定要加延时。VL53L0X的测量周期大约20ms，你读得太快，拿到的就是上次的数据。我曾经因为这个坑，让无人机在悬停时忽高忽低，查了两天才发现是数据没更新。

3.1.2 SPI驱动要点

SPI比I2C快得多，适合激光雷达这种高数据率的传感器。但SPI有个麻烦事——片选信号（CS）的时序必须严格。我见过不少新手，CS拉低之后马上开始读数据，结果第一个字节全是0xFF。

正确的做法是：CS拉低后，至少等一个时钟周期再开始传输。另外，SPI的时钟极性（CPOL）和时钟相位（CPHA）一定要和传感器手册对上。我习惯在初始化时打印出配置参数，方便调试。

// SPI读取TFmini距离值（示例）
uint16_t tfmini_read_distance(void) {
    uint8_t tx_buf[2] = {0x00, 0x00};
    uint8_t rx_buf[2] = {0};
    
    CS_LOW();
    delay_us(1);  // 等待片选稳定
    spi_transfer(tx_buf, rx_buf, 2);
    CS_HIGH();
    
    return (rx_buf[0] << 8) | rx_buf[1];
}

3.1.3 UART驱动要点

UART最简单，但要注意波特率匹配。超声波模块通常用9600或115200，激光雷达可能用更高的。我建议统一用115200，因为9600在传输大量数据时太慢了。

还有一个容易忽略的点：UART的接收缓冲区要开大一点。比如TFmini每秒输出100帧数据，每帧9字节，你的缓冲区至少得256字节，否则数据会丢。我刚开始做的时候只开了64字节，结果飞着飞着距离数据突然变成0，吓出一身冷汗。

3.2 数据滤波：中值滤波与卡尔曼滤波

数据读进来了，但你看过原始数据吗？超声波模块在室内测距，数据跳变能有±5cm。激光雷达遇到玻璃，直接给你返回一个错误值。所以，滤波是必须的。

3.2.1 中值滤波

中值滤波的原理很简单：连续采N个数据，排序后取中间值。它能有效去除「野点」——就是那种突然跳变很大的错误数据。

我一般取5个数据做中值滤波。为什么是5？因为3个太少，滤不掉连续的错误；7个以上延迟太大。你想想看，无人机每秒要处理几十次避障决策，延迟高了就撞墙了。

// 中值滤波（窗口大小=5）
uint16_t median_filter(uint16_t new_value) {
    static uint16_t buffer[5] = {0};
    static uint8_t index = 0;
    uint16_t sorted[5];
    uint8_t i, j;
    
    // 更新环形缓冲区
    buffer[index] = new_value;
    index = (index + 1) % 5;
    
    // 复制并排序
    for(i = 0; i < 5; i++) sorted[i] = buffer[i];
    for(i = 0; i < 4; i++) {
        for(j = i+1; j < 5; j++) {
            if(sorted[i] > sorted[j]) {
                uint16_t tmp = sorted[i];
                sorted[i] = sorted[j];
                sorted[j] = tmp;
            }
        }
    }
    
    return sorted[2];  // 返回中间值
}
提示：中值滤波的窗口大小要根据传感器更新率来调。超声波一般10Hz，窗口5就够了；激光雷达100Hz，窗口可以开到11。但记住，窗口越大，延迟越大。

3.2.2 卡尔曼滤波

卡尔曼滤波听起来高大上，其实核心就一句话：用上一刻的预测值，结合这一刻的测量值，算出最优估计。它比中值滤波更智能，能根据噪声大小动态调整权重。

对于无人机避障，我一般用一维卡尔曼滤波就够了。为什么？因为距离数据是标量，不需要考虑速度和加速度。下面是我常用的简化版：

// 一维卡尔曼滤波
typedef struct {
    float x;      // 状态估计值
    float p;      // 估计误差协方差
    float q;      // 过程噪声协方差
    float r;      // 测量噪声协方差
} kalman_1d_t;

float kalman_update(kalman_1d_t *kf, float measurement) {
    // 预测
    float p_pred = kf->p + kf->q;
    
    // 更新
    float k = p_pred / (p_pred + kf->r);  // 卡尔曼增益
    kf->x = kf->x + k * (measurement - kf->x);
    kf->p = (1 - k) * p_pred;
    
    return kf->x;
}

这里有两个参数需要调：Q（过程噪声）和R（测量噪声）。Q越大，滤波器响应越快，但噪声也大；R越大，滤波器越平滑，但延迟也大。我一般先设Q=0.1、R=1.0，然后根据实际数据微调。

警告：卡尔曼滤波不是万能的。如果传感器数据连续出错（比如激光雷达被遮挡），卡尔曼滤波也会跟着错。我建议先做中值滤波剔除野点，再用卡尔曼滤波平滑数据。两层保护，更安全。

3.3 坐标变换：从传感器坐标系到机体坐标系

滤波后的数据，是传感器自己坐标系下的距离。但无人机避障需要的是「相对于机体的距离」。比如，左前方的传感器测到50cm，但传感器装在机头右侧30度，那实际障碍物在哪个方向？这就需要坐标变换。

3.3.1 传感器安装位置与角度

我习惯把无人机机体坐标系定义为：X轴朝前，Y轴朝右，Z轴朝上。每个传感器安装时，记录它的位置偏移（dx, dy, dz）和安装角度（俯仰、偏航、横滚）。

举个例子，一个超声波传感器装在机头左侧10cm，朝左前方45度：

  • 位置偏移：dx=0.1m, dy=-0.05m, dz=0m
  • 安装角度：偏航=45度（相对于机体X轴）

3.3.2 坐标变换公式

坐标变换其实就是一个旋转加平移。对于二维情况（只考虑水平方向），公式很简单：

// 传感器数据转换到机体坐标系
// sensor_distance: 传感器测得的距离
// sensor_yaw: 传感器安装偏航角（弧度）
// sensor_dx, sensor_dy: 传感器位置偏移

float body_x = sensor_distance * cos(sensor_yaw) + sensor_dx;
float body_y = sensor_distance * sin(sensor_yaw) + sensor_dy;

三维情况稍微复杂一点，需要用到旋转矩阵。但大多数无人机避障只考虑水平方向，所以二维变换就够了。

重点：坐标变换的精度取决于传感器安装参数的准确性。我建议在装机后用激光测距仪实际测量一下传感器的位置和角度，别只看图纸。图纸上的尺寸和实际安装总有偏差。

3.3.3 多传感器数据融合

当你有多个传感器时，坐标变换之后的数据需要融合。比如，前方有3个传感器，分别测到50cm、55cm、48cm，那最终的前方距离是多少？

我一般用加权平均：距离近的传感器权重高，因为近处的数据更可靠。或者用最小值法：取所有传感器中的最小值，因为避障时最关心最近的障碍物。

// 多传感器数据融合（最小值法）
float get_front_distance(void) {
    float distances[3];
    distances[0] = sensor1_to_body();  // 已转换到机体坐标系
    distances[1] = sensor2_to_body();
    distances[2] = sensor3_to_body();
    
    float min_dist = 999.0;
    for(int i = 0; i < 3; i++) {
        if(distances[i] < min_dist) {
            min_dist = distances[i];
        }
    }
    return min_dist;
}

嗯，这一章的内容就到这里。总结一下：接口驱动是基础，数据滤波是保障，坐标变换是桥梁。三者缺一不可。下一章我们会讲避障算法，到时候这些预处理好的数据就要派上大用场了。

对了，如果你在实际调试中遇到数据跳变的问题，不妨先检查一下滤波参数。我曾经花了一整天调PID，结果发现是卡尔曼滤波的Q值设得太大了。有时候，问题不在算法，而在数据本身。

← 上一章 📖 返回目录 下一章 →