第3章传感器数据采集

3. 传感器数据采集：GPS/RTK定位原理、IMU姿态解算、超声波避障传感器

各位同学，大家好。今天我们来聊聊植保机器人身上最关键的几个“感官”——传感器。说白了，没有它们，你的机器人就是个“瞎子”和“聋子”。

我做了这么多年嵌入式，发现很多同学容易犯一个毛病：把传感器数据直接拿来用，不做任何处理。结果呢？定位飘到天上去，姿态乱跳，避障撞墙。嗯，今天我们就来把这些坑填上。

3.1 GPS/RTK定位原理

先说说GPS。普通GPS的精度大概在2-5米，这在农田里根本不够用。你想想看，行距才50厘米，误差2米的话，机器直接压到作物上去了。

所以我们要用RTK——实时动态差分定位。它的原理其实不复杂：

基准站：放在已知坐标点，接收卫星信号，计算误差
流动站：装在机器人上，接收卫星信号的同时，也接收基准站的误差修正
差分修正：流动站用基准站的误差数据，修正自己的定位结果

我在项目中遇到过最头疼的问题，就是RTK信号丢失。特别是靠近树林或者高压线塔的时候，固定解突然变成浮点解，定位精度从2厘米直接掉到20厘米。怎么办？

重要经验：RTK数据必须做质量检查。只有状态为“固定解”（Fix）时才能使用，浮点解（Float）和单点解（Single）都要丢弃或降权使用。

实际代码中，我一般这样处理RTK数据：

// RTK状态检查示例
typedef enum {
    RTK_NONE = 0,
    RTK_SINGLE,   // 单点解，精度差
    RTK_FLOAT,    // 浮点解，精度中等
    RTK_FIX       // 固定解，精度高
} rtk_status_t;

int process_rtk_data(rtk_data_t *data) {
    if (data->status != RTK_FIX) {
        // 不是固定解，标记为不可用
        data->valid = 0;
        return -1;
    }
    // 固定解，可以用于路径规划
    data->valid = 1;
    return 0;
}

警告：千万不要在RTK信号不好时还继续使用旧数据。我见过有人用卡尔曼滤波外推位置，结果机器人直接冲进沟里。宁可停下来等信号恢复，也不要“猜”位置。

3.2 IMU姿态解算

IMU（惯性测量单元）包含加速度计和陀螺仪。加速度计测重力方向，陀螺仪测角速度。但单独用哪个都不行——加速度计容易受振动干扰，陀螺仪有零漂。

所以我们要做数据融合。最经典的方法是互补滤波，说白了就是：

短期看陀螺仪（响应快，但会漂移）
长期看加速度计（稳定，但响应慢）
两者互补，得到准确的姿态

我建议初学者先从互补滤波入手，别一上来就搞卡尔曼滤波。为什么？因为互补滤波参数少，调起来直观。我曾经在一个项目里用卡尔曼滤波，调了三天参数都没调好，换成互补滤波半天就搞定了。

这里给出一段互补滤波的核心代码：

// 互补滤波姿态解算
void complementary_filter(imu_data_t *imu, attitude_t *att, float dt) {
    float alpha = 0.98;  // 滤波系数，越大越信任陀螺仪
    
    // 从加速度计计算俯仰角和横滚角
    float acc_pitch = atan2(-imu->acc_x, sqrt(imu->acc_y*imu->acc_y + imu->acc_z*imu->acc_z));
    float acc_roll  = atan2(imu->acc_y, imu->acc_z);
    
    // 互补融合
    att->pitch = alpha * (att->pitch + imu->gyro_x * dt) + (1-alpha) * acc_pitch;
    att->roll  = alpha * (att->roll  + imu->gyro_y * dt) + (1-alpha) * acc_roll;
}

小技巧：alpha值一般取0.95-0.99之间。如果机器人振动大，可以适当降低alpha，多信任加速度计。我一般从0.98开始调，看实际效果再微调。

3.3 超声波避障传感器

超声波传感器，原理很简单：发射超声波，接收回波，根据时间差算距离。但实际用起来坑不少。

我踩过最大的坑是什么？超声波在农田里的多径反射。作物叶子、地面、甚至水洼都会反射超声波，导致测距值忽远忽近。有一次我的机器人明明离树还有2米，超声波却报0.5米，直接急刹车。

怎么解决？三个方法：

多次采样取中值：连续采5-7次，去掉最大最小值，取中间值
设置合理阈值：超过3米的数据直接丢弃（超声波本身精度有限）
与其它传感器融合：比如结合红外或毫米波雷达做双重确认

代码实现中值滤波：

// 超声波数据中值滤波
#define SAMPLE_COUNT 7

float ultrasonic_median_filter(void) {
    uint16_t samples[SAMPLE_COUNT];
    
    // 连续采样
    for (int i = 0; i < SAMPLE_COUNT; i++) {
        samples[i] = read_ultrasonic();
        delay(10);  // 间隔10ms
    }
    
    // 冒泡排序
    for (int i = 0; i < SAMPLE_COUNT - 1; i++) {
        for (int j = 0; j < SAMPLE_COUNT - i - 1; j++) {
            if (samples[j] > samples[j+1]) {
                uint16_t temp = samples[j];
                samples[j] = samples[j+1];
                samples[j+1] = temp;
            }
        }
    }
    
    // 返回中值（第4个）
    return samples[3] * 0.034 / 2;  // 转换为厘米
}

核心要点：超声波传感器的最佳探测角度是15-30度。安装时要注意，不要让它对着地面或者天空。我一般把超声波装在机器人前方30厘米高度，向下倾斜10度，这样既能探测到障碍物，又不会误报地面。

3.4 传感器数据融合策略

单个传感器都有缺陷，所以我们要做多传感器融合。在植保机器人上，我常用的策略是：

传感器	用途	采样频率	数据优先级
RTK-GPS	全局定位	10Hz	高（固定解时）
IMU	姿态估计	100Hz	中（用于短时推算）
超声波	避障	20Hz	高（近距离时）

实际项目中，我一般这样分配任务：

路径跟踪时，主要依赖RTK定位，IMU做姿态补偿
RTK信号丢失时，用IMU航位推算撑10-15秒
超声波作为安全兜底，任何时候检测到障碍物都优先响应

注意：传感器数据的时间戳一定要对齐。我见过有人把10Hz的GPS数据和100Hz的IMU数据直接混用，结果位置和姿态差了半秒，路径规划全乱套。建议用同一个定时器给所有传感器打时间戳。

好了，这一章的内容就到这里。传感器数据采集是植保机器人最基础也最容易出问题的一环。记住一句话：不要相信任何一个传感器的单次读数。多采样、多滤波、多融合，才能让你的机器人“看得清、走得稳”。

下一章我们讲路径规划算法，到时候会用到今天讲的这些传感器数据。大家先把基础打牢。

📚 课程目录

3. 传感器数据采集：GPS/RTK定位原理、IMU姿态解算、超声波避障传感器

3.1 GPS/RTK定位原理

3.2 IMU姿态解算

3.3 超声波避障传感器

3.4 传感器数据融合策略