4. 避障数据预处理：传感器数据滤波、坐标系对齐与异常值剔除

各位同学，欢迎来到避障算法落地的关键一环——数据预处理。

说实话，很多新手一上来就急着调避障逻辑，结果发现无人机要么乱晃，要么撞墙。为什么？因为传感器喂进来的数据是“脏”的。我见过太多团队在仿真里跑得飞起，一上真机就崩，十有八九是预处理没做好。

今天我们就来聊聊，怎么把这堆原始数据“洗”干净，让避障算法吃得舒服。

4.1 传感器数据滤波：中值滤波与卡尔曼滤波

传感器数据，尤其是超声波和红外这类低成本方案，噪声大得离谱。你想想看，一个超声波测距，可能因为多径反射突然跳变几十厘米。这种毛刺数据直接喂给避障算法，飞控会以为前方突然出现一堵墙，然后猛拉杆——后果嘛，你懂的。

4.1.1 中值滤波：简单粗暴，但有效

中值滤波的原理很简单：取连续N个采样值，排序后取中间那个。它能很好地干掉孤立的尖峰噪声。

我个人习惯在超声波和红外传感器上用窗口大小为5的中值滤波。为什么是5？太小了滤不干净，太大了延迟太高。避障讲究实时性，延迟超过100ms，飞机可能已经撞上了。

// 中值滤波示例（ArduPilot风格）
float median_filter(float new_sample, float buffer[], uint8_t size) {
    float temp[size];
    memcpy(temp, buffer, size * sizeof(float));
    // 将新样本插入缓冲区尾部，移除最旧样本
    for (uint8_t i = 0; i < size - 1; i++) {
        buffer[i] = buffer[i + 1];
    }
    buffer[size - 1] = new_sample;
    // 排序取中值
    for (uint8_t i = 0; i < size - 1; i++) {
        for (uint8_t j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                float t = temp[i];
                temp[i] = temp[j];
                temp[j] = t;
            }
        }
    }
    return temp[size / 2];
}

4.1.2 卡尔曼滤波：更平滑，但更“重”

如果你需要更平滑的估计，比如激光雷达或视觉里程计的数据，卡尔曼滤波是更好的选择。它本质上是一个“预测+修正”的过程：根据上一时刻的状态预测当前值，再用当前测量值去修正这个预测。

我记得第一次在Pixhawk上跑卡尔曼滤波时，算力差点爆了。后来发现，对于一维距离数据，完全可以用简化版——标量卡尔曼滤波，计算量小得多。

// 一维卡尔曼滤波（简化版）
typedef struct {
    float x;  // 状态估计
    float P;  // 估计误差协方差
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
} Kalman1D;

float kalman_update(Kalman1D *kf, float z) {
    // 预测
    float x_pred = kf->x;
    float P_pred = kf->P + kf->Q;
    // 更新
    float K = P_pred / (P_pred + kf->R);
    kf->x = x_pred + K * (z - x_pred);
    kf->P = (1 - K) * P_pred;
    return kf->x;
}

4.2 坐标系对齐：从机体坐标系到NED

传感器装在飞机上，测出来的数据是相对于飞机本身的——这叫机体坐标系（Body Frame）。但避障算法需要知道障碍物在地球坐标系下的位置，也就是NED坐标系（北东地）。

说白了，你得把传感器看到的“前方1米”转换成“北方向0.5米，东方向0.8米，下方0.1米”。

4.2.1 旋转矩阵

这个转换靠的是旋转矩阵。飞机的姿态（横滚、俯仰、偏航）决定了机体坐标系和NED坐标系之间的旋转关系。

// 机体坐标系到NED坐标系的转换
// 输入：机体坐标系下的向量 (x_b, y_b, z_b)
// 输出：NED坐标系下的向量 (x_n, y_n, z_n)
// 姿态角：roll (φ), pitch (θ), yaw (ψ)

void body_to_ned(float x_b, float y_b, float z_b,
                 float roll, float pitch, float yaw,
                 float *x_n, float *y_n, float *z_n) {
    float cφ = cos(roll), sφ = sin(roll);
    float cθ = cos(pitch), sθ = sin(pitch);
    float cψ = cos(yaw), sψ = sin(yaw);

    // 旋转矩阵 R_b^n
    *x_n = (cθ*cψ)*x_b + (sφ*sθ*cψ - cφ*sψ)*y_b + (cφ*sθ*cψ + sφ*sψ)*z_b;
    *y_n = (cθ*sψ)*x_b + (sφ*sθ*sψ + cφ*cψ)*y_b + (cφ*sθ*sψ - sφ*cψ)*z_b;
    *z_n = (-sθ)*x_b + (sφ*cθ)*y_b + (cφ*cθ)*z_b;
}

4.2.2 实战中的坑

嗯，这里要注意：ArduPilot内部已经帮你做了大部分坐标系转换工作。你从AP_Proximity库读到的距离数据，默认已经是NED坐标系下的了。但如果你自己写传感器驱动，或者用第三方库，一定要确认坐标系。

我习惯在代码里加一个坐标系检查函数，打印出原始数据和转换后的数据，悬停时对比一下——如果飞机水平静止，机体坐标系和NED坐标系应该重合，转换前后数据应该一致。

4.3 异常值剔除：别让一个坏数据毁了整个避障

滤波只能平滑噪声，但挡不住传感器彻底“抽风”。比如激光雷达打到透明玻璃上，直接返回一个无效值；或者超声波被螺旋桨气流干扰，测出个负数。

这些异常值必须剔除，否则避障算法会做出荒谬的决策。

4.3.1 阈值法

最简单的方法：设定一个合理范围。比如超声波的有效测距范围是20cm到500cm，那小于20cm或大于500cm的数据直接扔掉。

// 阈值剔除
bool is_valid_range(float distance, float min_range, float max_range) {
    return (distance >= min_range) && (distance <= max_range);
}

4.3.2 一致性检查

更高级一点：结合多个传感器做一致性检查。比如，如果激光雷达说前方2米有障碍，但超声波说前方0.5米，那肯定有一个是错的。我一般会取两个传感器的加权平均，权重根据传感器的置信度来定。

传感器类型	典型有效范围	常见异常	剔除策略
超声波	20cm - 500cm	多径反射、螺旋桨干扰	阈值 + 中值滤波
红外	10cm - 150cm	阳光干扰、表面反射	阈值 + 一致性检查
激光雷达	5cm - 40m	玻璃、镜面、雨雾	阈值 + 卡尔曼滤波
视觉	30cm - 20m	光照变化、纹理缺失	置信度阈值 + 时间一致性

4.3.3 时间连续性检查

还有一种情况：数据突然跳变，但又不是完全离谱。比如上一帧测距是2米，下一帧突然变成0.5米，然后又跳回2米。这种大概率是干扰。

我常用的方法是：如果当前值和上一帧的差值超过一个阈值（比如0.3米），就暂时不信任这个数据，而是用上一帧的值代替，直到连续3帧都稳定在新值上。

// 时间连续性检查
float temporal_consistency(float current, float previous, float max_delta) {
    if (fabs(current - previous) > max_delta) {
        // 跳变过大，暂时不信任
        return previous;
    }
    return current;
}

4.4 本章知识体系

下面这张图总结了避障数据预处理的完整流程。你可以把它当作一个检查清单，每次集成新传感器时，按这个流程走一遍，基本不会出大问题。

好了，这一章的内容就到这里。数据预处理是避障算法的基石，地基没打好，上面盖的房子再漂亮也得塌。下一章我们会把这些处理好的数据喂给避障算法，看看它们是怎么做出“向左躲”还是“向右躲”的决策的。

---