3. 角度数据滤波处理

任务目标

AS5048磁编码器在静止或低速转动时，原始角度读数常伴有±0.5°左右的随机噪声。本案例实现一个滑动平均滤波器，对连续N次角度采样取平均，以降低噪声影响，同时保证滤波后的角度值在0°～360°范围内连续无跳变。

核心原理与实现

角度数据与普通数值不同，存在360°回绕问题。例如350°与10°的算术平均为180°，但实际期望值应为0°（或360°）。因此不能直接对原始角度做平均，需先将角度转换为单位圆上的矢量分量（sin/cos），对分量分别累加平均后，再用atan2还原为角度值。本案例采用窗口大小为5的滑动平均，每次新采样到来时更新窗口并输出滤波结果。

关键完整代码

#include <stdio.h>
#include <math.h>
#include <stdint.h>

#define PI          3.14159265358979323846
#define WINDOW_SIZE 5

// 模拟AS5048原始角度读取（单位：度）
float read_raw_angle_deg(void) {
    // 实际使用时替换为SPI/I2C读取AS5048寄存器值并转换为角度
    // 此处仅演示滤波算法，返回带噪声的模拟值
    static float true_angle = 0.0f;
    static int   tick = 0;
    true_angle += 0.3f;                    // 模拟缓慢转动
    if (true_angle >= 360.0f) true_angle -= 360.0f;
    float noise = ((float)(tick * 7919 % 100) - 50.0f) * 0.01f; // 伪随机噪声 ±0.5°
    tick++;
    float raw = true_angle + noise;
    if (raw >= 360.0f) raw -= 360.0f;
    if (raw < 0.0f)    raw += 360.0f;
    return raw;
}

// 滑动平均滤波（角度专用，处理360°回绕）
typedef struct {
    float sin_sum;
    float cos_sum;
    float window_sin[WINDOW_SIZE];
    float window_cos[WINDOW_SIZE];
    int   index;
    int   count;
} AngleFilter;

void filter_init(AngleFilter *f) {
    f->sin_sum = 0.0f;
    f->cos_sum = 0.0f;
    f->index   = 0;
    f->count   = 0;
    for (int i = 0; i < WINDOW_SIZE; i++) {
        f->window_sin[i] = 0.0f;
        f->window_cos[i] = 0.0f;
    }
}

float filter_update(AngleFilter *f, float angle_deg) {
    float rad = angle_deg * PI / 180.0f;
    float s = sinf(rad);
    float c = cosf(rad);

    // 若窗口已满，先减去最旧值
    if (f->count == WINDOW_SIZE) {
        f->sin_sum -= f->window_sin[f->index];
        f->cos_sum -= f->window_cos[f->index];
    } else {
        f->count++;
    }

    // 存入新值
    f->window_sin[f->index] = s;
    f->window_cos[f->index] = c;
    f->sin_sum += s;
    f->cos_sum += c;
    f->index = (f->index + 1) % WINDOW_SIZE;

    // 计算平均角度
    float avg_sin = f->sin_sum / f->count;
    float avg_cos = f->cos_sum / f->count;
    float filtered_rad = atan2f(avg_sin, avg_cos);
    float filtered_deg = filtered_rad * 180.0f / PI;
    if (filtered_deg < 0.0f) filtered_deg += 360.0f;
    return filtered_deg;
}

// 最小验证入口
int main(void) {
    AngleFilter filter;
    filter_init(&filter);

    printf("采样序号  原始角度(°)  滤波角度(°)\n");
    for (int i = 0; i < 20; i++) {
        float raw = read_raw_angle_deg();
        float filtered = filter_update(&filter, raw);
        printf("%6d  %12.3f  %12.3f\n", i + 1, raw, filtered);
    }
    return 0;
}

编译与运行

gcc -o angle_filter angle_filter.c -lm
./angle_filter

验证方法

运行程序后观察输出：滤波角度曲线应比原始角度平滑，且不会出现跨0°/360°边界时的异常跳变（如原始值从358°变为2°时，滤波输出应平滑过渡到0°附近而非跳到180°）。可修改read_raw_angle_deg中的噪声幅度（当前±0.5°）或转动速度，验证滤波器在不同噪声水平下的平滑效果。若需在真实硬件上验证，将read_raw_angle_deg替换为实际的AS5048 SPI读取函数即可，滤波逻辑无需改动。

常见问题

• 窗口大小如何选择？窗口越大平滑效果越强，但滞后越大。FOC电流环控制建议窗口不超过5，速度环可适当增大。
• 为什么不用算术平均？角度在0°/360°附近时算术平均会产生严重错误（如350°和10°平均为180°），矢量平均法从数学上避免了该问题。
• 滤波后角度用于FOC有何注意？滤波会引入相位滞后，若用于高速电机的位置环，需评估滞后对控制稳定性的影响，必要时改用低通滤波或预测算法。