第四节：传感器冗余——GPS/气压计的多重保障

传感器冗余这块，说实话是飞控系统里最让我头疼，但也最有成就感的部分。我早期做的一个项目，就因为GPS单点故障，无人机在悬停时突然飘走，差点撞上大楼。从那以后，我对传感器冗余的重视程度直接拉满。

今天咱们聊聊三个核心话题：多GPS怎么融合、气压计怎么跟IMU互补、以及传感器投票机制怎么落地。说白了，就是让飞控在传感器出问题时，还能稳稳当当干活。

4.1 多GPS融合：不是简单取平均

很多人以为多GPS融合就是两个GPS数据取个平均值。嗯，没那么简单。我见过不少团队这么干，结果精度反而更差。

为什么？因为每个GPS都有不同的误差特性。有的受多径效应影响大，有的卫星几何分布差。简单平均会把坏数据也带进来。

我个人习惯用加权融合。核心思路是：根据每个GPS的定位精度因子（PDOP）和信噪比（SNR）动态分配权重。

// 多GPS加权融合伪代码
float gps_fusion(float lat1, float lon1, float pdop1, float snr1,
                 float lat2, float lon2, float pdop2, float snr2) {
    // 权重计算：PDOP越小权重越大，SNR越大权重越大
    float w1 = 1.0 / (pdop1 * pdop1) * (snr1 / 50.0);
    float w2 = 1.0 / (pdop2 * pdop2) * (snr2 / 50.0);
    
    // 归一化
    float total = w1 + w2;
    w1 /= total;
    w2 /= total;
    
    // 加权融合
    float lat_fused = lat1 * w1 + lat2 * w2;
    float lon_fused = lon1 * w1 + lon2 * w2;
    
    return lat_fused, lon_fused;
}

4.2 气压计与IMU互补：高度估计的黄金搭档

气压计有个毛病——它受温度影响很大。你想想看，无人机从地面飞到100米高空，温度变化个几度，气压计读数可能就漂了1-2米。但IMU的垂直加速度积分，短期精度很高，长期却会漂移。

所以，这两者天生就是互补的。我常用的方法是互补滤波，说白了就是：

• 短期高度变化：信任IMU的加速度积分
• 长期高度基准：信任气压计的绝对高度
• 融合方式：用气压计数据去修正IMU的漂移

// 气压计与IMU互补滤波
float baro_altitude;  // 气压计高度
float imu_velocity;   // IMU垂直速度积分
float imu_altitude;   // IMU高度积分
float tau = 0.5;      // 滤波时间常数

void complementary_filter(float dt) {
    // 预测：用IMU速度更新高度
    imu_altitude += imu_velocity * dt;
    
    // 修正：用气压计校正
    float error = baro_altitude - imu_altitude;
    imu_altitude += error * (dt / tau);
    
    // 同时修正速度漂移
    imu_velocity += error * (dt / tau) * 0.1;
}

我曾经在一个项目中，气压计被排气管的热气流干扰，高度数据跳了3米。幸好有IMU撑着，飞控没乱来。等气压计稳定后，互补滤波慢慢把高度拉回来。整个过程飞行器高度波动不到0.5米。

4.3 传感器投票机制：少数服从多数

投票机制，说白了就是让多个传感器互相监督。我一般用三冗余设计：三个同类型传感器，取中间值或者多数一致的那个。

举个例子，三个气压计读数分别是：100.2米、100.5米、102.1米。明显第三个有问题。投票机制会剔除它，用前两个的平均值。

投票机制的实现，我一般分三步：

1. 数据有效性检查：每个传感器先自检，比如GPS的卫星数、气压计的温度补偿状态
2. 两两差值计算：计算每对传感器之间的差值
3. 决策输出：如果某传感器与其他两个差值都过大，就剔除它

// 三冗余传感器投票
float sensor_vote(float s1, float s2, float s3, float threshold) {
    float diff12 = fabs(s1 - s2);
    float diff13 = fabs(s1 - s3);
    float diff23 = fabs(s2 - s3);
    
    int valid_count = 0;
    float sum = 0;
    
    // 检查每个传感器是否与其他两个一致
    if (diff12 < threshold && diff13 < threshold) {
        valid_count++; sum += s1;
    }
    if (diff12 < threshold && diff23 < threshold) {
        valid_count++; sum += s2;
    }
    if (diff13 < threshold && diff23 < threshold) {
        valid_count++; sum += s3;
    }
    
    if (valid_count >= 2) {
        return sum / valid_count;
    } else {
        // 多数不一致，触发安全模式
        return NAN;
    }
}

4.4 整体架构：一张图说清楚

下面这张图，是我自己项目里用的传感器冗余架构。你一看就明白各模块怎么配合。

从图上你能看到，GPS和气压计先各自做内部投票，然后跟IMU一起进入互补滤波。这样即使某个传感器挂了，系统还能用其他数据撑着。

嗯，传感器冗余这块，说白了就是"别把所有鸡蛋放一个篮子里"。但怎么放、怎么拿，需要你根据实际项目去调。我见过太多人照搬开源代码，结果炸机了还不知道问题在哪。

记住：冗余不是万能的，但没有冗余是万万不能的。

---