两路信号,两种滤波:角速度走低通滤波,加速度走卡尔曼滤波

经典三问

Q: 为什么同一个芯片的六个数要用两种滤波?
A: 因为这两路信号的毛病不一样:角速度马上要被积分,怕的是高频抖动;加速度要长期当“哪边是下”的基准,怕的是抖和慢慢偏。一种滤波压不住两边。

Q: 一阶低通和一维卡尔曼差在哪?
A: 低通的系数是写死的;卡尔曼的系数由“该信测量多少”算出来。在这组参数下它跑起来有多慢,是能直接量出来的。

Q: 做完这一步,手里有什么?
A: 两路干净的数据:角速度可以拿去积分,加速度可以拿去定方向。把它们合成姿态,是姿态解算那一步的事。


模块平放在桌上,串口把六个原始数一行行吐出来。角速度和加速度都在动,但动法不一样。

角速度的抖是“毛”:上一拍 gz=19,下一拍 23,再下一拍 17,围着零上下乱跳,看不出方向。

加速度的抖是“沉”:平放时 az 稳稳待在一个大数附近,动一下才整体偏过去,而且偏过去之后待在新位置上不下来——倾斜多少,它就偏多少。它测的不是噪声,是重力的分量。

这两句话就解释了为什么两路要用两种滤波:角速度怕的是加在真值上的毛刺,加速度怕的是“被运动骗到另一个位置”。

角速度那一路:一阶低通

角速度后面要做什么?积分——每一拍乘上时间,一路加起来变成角度。积分这个东西对高频特别敏感:一拍的抖动会被永久地加进角度里,而且再也拿不出来。所以角速度在积分之前必须先压掉高频。

压高频最省事的办法就是一阶低通:

out = alpha * input + (1.0f - alpha) * out;

读法还是那句:新值和当前值差多少,就只补上这个差的一部分。系数取参考起点 alpha = 0.15。

alpha 定多少,等于在回答“允许它慢几拍”。这张表是算出来的(10 毫秒一拍,输出从 0 追到真值的 90%):

alpha 追到 90% 用几拍 换算成时间
0.5 4 拍 0.04 秒
0.25 9 拍 0.09 秒
0.15 15 拍 0.15 秒
0.1 22 拍 0.22 秒
0.05 45 拍 0.45 秒

写成一个只干这件事的小结构,三轴各一个:

typedef struct
{
    float out;          /* 上一次的输出 */
    float alpha;        /* 低通系数:越小越平、越慢 */
    uint8_t started;    /* 第一拍直接赋值,避免从 0 往上爬 */
} Lpf1st;

void Lpf1st_Init(Lpf1st *lpf, float alpha)
{
    lpf->out = 0.0f;
    lpf->alpha = alpha;
    lpf->started = 0U;
}

float Lpf1st_Update(Lpf1st *lpf, float input)
{
    if (lpf->started == 0U)
    {
        lpf->out = input;               /* 第一拍没有历史,直接把输入当成输出 */
        lpf->started = 1U;
        return lpf->out;
    }

    lpf->out += lpf->alpha * (input - lpf->out);
    return lpf->out;
}

开头那个 started 不是可有可无的。低通的状态量默认是 0,如果第一拍就按公式算,输出的起点也是 0——屏幕上会先拉出一条从 0 爬到真值的曲线,看上去像传感器要“热身”。给第一拍一个直接赋值,这条假斜线就没了。

三个轴各一个实例:

static Lpf1st g_gyro_lpf[3];        /* 0=X,1=Y,2=Z */

void Gyro_LpfInit(void)
{
    uint8_t i;

    for (i = 0U; i < 3U; i++)
    {
        Lpf1st_Init(&g_gyro_lpf[i], 0.15f);
    }
}

为什么角速度这里不用卡尔曼

卡尔曼当然能用,只是我觉得没必要。它的力气花在“把'该信测量多少'交给两个参数算出来”上,而角速度这一路的要求很直白:快一点、平一点,别把高频带进积分里。一个系数写死的低通就够,代码短、开销小、行为一眼能算出来。

加速度那一侧的情况不一样,所以那边要上更重的东西。

加速度那一路:三个一维卡尔曼

加速度计有它自己的麻烦。静止时它是可靠的——重力指着哪边,它就说哪边。可一旦动起来,它测的就是“重力 + 运动加速度”的合成:车一加速,它以为世界歪了;压过一个坎,它给你一个尖峰。再加上安装角和器件本身的偏差,它还带着一个慢慢变的“偏”。

也就是说,加速度这一路既有抖,又有偏,而且它的变化不是纯噪声,是有结构的。卡尔曼滤波正好对应这类问题:一边用“我预测它应该是多少”(模型),一边用“现在测到多少”(测量),按两者的可信度加权。

一维版本只有四行:

typedef struct
{
    float p_last;       /* 上一拍的估计误差 */
    float p_now;        /* 这一拍的估计误差(先预测再修正) */
    float out;          /* 滤波输出,也就是当前的估计值 */
    float gain;         /* 卡尔曼增益:这一拍该信测量多少 */
    float q;            /* 过程噪声:模型有多不可信 */
    float r;            /* 测量噪声:测量有多不可信 */
} Kalman1D;

void Kalman1D_Init(Kalman1D *kf, float q, float r, float p_start)
{
    kf->p_last = p_start;
    kf->p_now = p_start;
    kf->out = 0.0f;
    kf->gain = 0.0f;
    kf->q = q;
    kf->r = r;
}

float Kalman1D_Update(Kalman1D *kf, float input)
{
    kf->p_now = kf->p_last + kf->q;                        /* 1. 预测:误差涨了一点 */
    kf->gain = kf->p_now / (kf->p_now + kf->r);            /* 2. 算增益:该信测量多少 */
    kf->out = kf->out + (kf->gain * (input - kf->out));    /* 3. 用增益修正估计 */
    kf->p_last = (1.0f - kf->gain) * kf->p_now;            /* 4. 修正后的误差,留给下一拍 */

    return kf->out;
}

四行逐句说:

  • 第一行:每次先用模型往前推一步,但因为模型不完美,误差要涨一点——涨多少由 q 说了算。
  • 第二行:增益 = 估计误差 ÷(估计误差 + 测量噪声)。误差大、测量噪声小,增益就大,也就是说“这次我更信测量”。
  • 第三行:拿测量和估计之间的差,按增益补一部分进来。这一行和低通那条式子是同一个形状,区别只在系数是算出来的。
  • 第四行:修正之后误差减小了,记下来给下一拍用。

加速度有三个轴,所以要三个实例,每个轴一套状态:

static Kalman1D g_accel_kf[3];      /* 0=X,1=Y,2=Z */

void Accel_KfInit(void)
{
    uint8_t i;

    for (i = 0U; i < 3U; i++)
    {
        Kalman1D_Init(&g_accel_kf[i], 0.001f, 0.543f, 0.02f);
    }
}

q = 0.001、r = 0.543、起始误差 0.02 是一组能用的起点。三个轴各用一套状态,不能共用一个实例:三个轴的噪声水平和动态都不一样,共用会让它们互相污染——X 轴的抖动会把 Y 轴的估计一起带偏。

这组参数下它有多慢

q 和 r 这两个数说的是两件事:q 是“我有多不信自己的模型”,r 是“我有多不信这一次测量”。它们的比值决定了这个滤波器跟得紧还是稳得住:把 r 调大,它更怀疑测量、更信历史,于是更稳也更慢;把 r 调小,它更信新数据,跟得快、抖动也跟着回来。这个换法不是等比例的——想让它稳一倍,r 要加大得比“一倍”多得多。

那句话不能只停在定性上,慢多少是能直接量出来的。让输入保持一个常数,看输出多久走到这个常数的 90%:

这一路 追到 90% 用几拍 换算成时间
角速度(一阶低通,alpha = 0.15) 15 拍 0.15 秒
加速度(一维卡尔曼,q = 0.001、r = 0.543) 56 拍 0.6 秒

两路差三倍多。 这不是小数点后的差别,它决定了后面姿态解算里谁负责快、谁负责慢:角速度那一侧可以当成“跟手的那一路”,加速度那一侧只能当成“慢慢把方向拉回来的那一路”。

还有一个前提要说清:q、r 固定的时候,卡尔曼算出来的那个系数会收敛成一个定值——也就是说,跑到稳定以后,它和一条一阶低通的差别只在系数是从哪来的(一个是拍脑袋定的,一个是由“信模型多少、信测量多少”算出来的)。真正体现“按该信谁来加权”的场合,是参数随时间变、或者要估的量不止一个的时候,那是另一件事。

两路怎么接起来

两路滤波都做在换算之后的物理量上(度每秒、g),而不是原始刻度上。理由是量程:换一个量程,原始刻度的含义就变了,滤波代码得跟着改;在物理量上做,量程只是一开始那个换算常数的事。代价是浮点——F103 没有硬件浮点单元,但每拍只多出十来次乘除,可以忽略。

顺序是这样:

读 14 个字节
   ↓
换算成 度/秒 和 g
   ↓
减掉零偏(讲零偏那一回校出来的那个数)
   ↓
角速度 → 一阶低通(三轴)
加速度 → 一维卡尔曼(三轴)
   ↓
存进一副结构体,后面的解算从这里取数

零偏在低通之前减。因为零偏是一个常数,低通对常数不敏感——先减后减结果几乎一样,但先减省事:不用给每个轴再存一份偏置,也不会让常数在那个“从 0 爬上来”的过程里多绕一圈。

typedef struct
{
    float gyro_x_dps;
    float gyro_y_dps;
    float gyro_z_dps;
    float accel_x_g;
    float accel_y_g;
    float accel_z_g;
} Sensor_Data;

static Sensor_Data g_sensor;

/* 10 毫秒一拍:读、换算、去偏、两路滤波 */
void Sensor_Update(void)
{
    MPU6050_ReadRaw(&g_raw);
    MPU6050_Convert(&g_raw, &g_data);

    g_sensor.gyro_x_dps = Lpf1st_Update(&g_gyro_lpf[0], g_data.gyro_x_dps - gyro_bias.x);
    g_sensor.gyro_y_dps = Lpf1st_Update(&g_gyro_lpf[1], g_data.gyro_y_dps - gyro_bias.y);
    g_sensor.gyro_z_dps = Lpf1st_Update(&g_gyro_lpf[2], g_data.gyro_z_dps - gyro_bias.z);

    g_sensor.accel_x_g = Kalman1D_Update(&g_accel_kf[0], g_data.accel_x_g);
    g_sensor.accel_y_g = Kalman1D_Update(&g_accel_kf[1], g_data.accel_y_g);
    g_sensor.accel_z_g = Kalman1D_Update(&g_accel_kf[2], g_data.accel_z_g);
}

MPU6050_ReadRaw 和 MPU6050_Convert 是讲从寄存器到角度那一回写好的两个函数:前者一次连读十四个字节并拼成七个有符号数,后者按量程把刻度换算成 度/秒 和 g。gyro_bias 是讲零偏那一回校准出来的三轴偏置。

四个现场

一、静止看抖幅。 模块放稳,屏幕上同时看四个数:原始 gz、低通后的 gz、原始 az、卡尔曼后的 az。低通后的角速度抖动幅度明显小于原始;卡尔曼后的加速度更平,但代价是更慢——慢多少,就是上一节那张表。

二、上电看它爬。 把卡尔曼的 out 从 0 开始(Kalman1D_Init 里就是这么写的),上电后屏幕上那个数会从 0 慢慢爬到真值。数十拍左右,大约半秒才到位。低通那一路看不到这个过程,因为第一拍直接赋值了。这一条能亲手验证上一节那个“56 拍”。

三、快速转一下看滞后。 用手把模块绕 Z 轴快转半圈再停住,看低通后的角速度比原始值晚多少拍收住:alpha = 0.15 时大约是十几拍,也就是 0.15 秒。同样的动作看加速度那一侧——它跟得更慢。两路的“跟手程度”差三倍多,这是选参数时最直接的一条线索。

四、敲一下桌子,看两路的反应完全不同。 角速度那一路会跳一下,幅度被低通削掉一部分,仍看得见;加速度那一路几乎不动——增益太小,一根几十毫秒的尖峰进不了它的状态。这里有个代价必须认:如果这一路后面要做碰撞判断,那么撞击在滤波后的加速度上也被一起吃掉了。 这类“要看瞬时冲击”的需求,应该直接取原始值或者单开一路不滤波的数据,而不是把滤波参数往回调。

读得比采得慢,手里的数就已经换了

前面的程序是主循环每 10 毫秒读一次、每 200 毫秒刷一次屏。这个结构在实验阶段够用,但它有一个隐含前提:每一次读,读到的都是这一拍的新数据。

手册对数据寄存器的说法是:随时可以读,但寄存器只保存最新的那一份。芯片按采样率更新,读得慢了,中间那些样本就被新的覆盖掉了。手册在同一节还给了一句推荐——可以用中断来判断“有新数据了”,INT_ENABLE 的最低位就是 DATA_RDY_EN。

更彻底的做法是 FIFO。这颗芯片有 1024 字节的 FIFO,可以选哪些通道往里写,还带一个计数器告诉你攒了多少字节,支持一口气读出来。攒着不怕被覆盖,取的时候一次取完。

回到程序里看:刷屏一次要二十几毫秒,按 100 Hz 采样算,这段时间里有两三个样本已经被覆盖。于是滤波拿到的序列其实不是等间隔的——alpha 是按“每 10 毫秒一拍”定的,实际拍长一变,它的等效截止频率也跟着变。噪声大一点、刷屏慢一点,数值就会有微妙的不同,这正是调参时最难受的那种“不稳定”。

两条最小的改法:一是把读挪进 1 毫秒中断,中断里只置一个“有新数据”的标志,主循环负责换算和滤波;二是不用中断,直接开 FIFO,攒够字节数再一次性读。两条都不是免费的——中断里做 I2C 会把中断拉长,FIFO 要处理溢出。实验阶段还有个更省事的办法:把刷屏和读数据分到两个不同的节拍,别让它们挤在同一轮里。

要走中断这条路,有三处细节手册写得很具体,不照着来就会踩。

一是 INT_ENABLE 的最低位 DATA_RDY_EN 什么时候置位:手册的说法是“每次对所有传感器寄存器的写操作完成时”产生。它跟的不是“我读完了”,是芯片自己把这一拍写完的那一刻。

二是中断脚的行为。INT_PIN_CFG 里 LATCH_INT_EN 为 0 时,INT 只输出一个 50 微秒的脉冲;为 1 时一直保持到被清除。INT_RD_CLEAR 为 0 时只有读 INT_STATUS 才会清标志,为 1 时任何一次读都会清。INT_OPEN 选推挽还是开漏。这几位的组合决定了中断服务程序要写几行、会不会漏掉一次触发。

三是 INT_STATUS 本身:每一位都是读一次就自动清零。所以进中断第一件事是把它读出来,别在别处顺手读了同一个寄存器。

FIFO 那三个寄存器也要认准:总开关是 USER_CTRL(0x6A)的 bit6 FIFO_EN;往 FIFO 里写哪些通道,由 FIFO_EN(0x23)的八位分别控制——温度、三轴陀螺、加速度、外部从机;攒了多少字节看 FIFO_COUNT(0x72 和 0x73)这个 16 位数,读数据走 FIFO_R_W(0x74)。还有个坑:FIFO_RESET 必须在 FIFO_EN 等于 0 的时候置位,否则不生效;而且 FIFO 里的内容不会跟着寄存器复位一起清掉,除非整颗芯片掉电。

还有个更隐蔽的:采样点挑在哪

上面说的是“读的次数不够”。还有一个同类的问题,藏得更深:采样时刻挑在哪。

ADC 读到的不是“此刻的真实值”,是“此刻采样电容上充到的电压”。信号源有内阻,前面还有 RC,从被选通那一刻到数值稳定,中间有一段建立时间。在建立时间之内采,读到的就是一个没有意义的中间值。

这个时间可以估。采样保持电容大概几个皮法,加上前面源阻抗和 RC,就是一个小 RC 回路。充到 12 位精度要九个时间常数左右:10 千欧配 5 皮法,一个时间常数 50 纳秒,九个就是 0.45 微秒。采样时间设得比这还短,读数必然偏低。

小车上最直接的例子是量电池电压。分压之后通常还并一个电容,就是为了让读数别跟着电机转的节奏跳。电机那一路 PWM 一通,电池电压被拽下去一截;电机关了,又慢慢弹回来。如果在主循环里随手采,采到的是“管子刚打开 20 微秒”还是“关断 5 毫秒之后”,读数能差出几百毫伏。

极端一点的情况:20 千赫的 PWM,周期 50 微秒,占空比 95% 的时候只剩 2.5 微秒是关断的;扣掉死区、再扣掉建立时间,真正能采的窗口可能只有零点几微秒。窗口比建立时间还短的时候,已经谈不上准不准——那一下根本没有采到稳定值。

所以采样点和 PWM 的相位是绑在一起的。占空比越高,关断的那段窗口越短;窗口短到比建立时间还短,怎么采都是错的。这时候只有三条路:把采样点挪到窗口中间、把窗口拉长(降 PWM 频率或者限制最大占空比)、或者干脆不采这一项,用别的量推出来。

这一条在按键、串口这些地方看不见,只有对着一个会随开关动作变化的模拟量,才会冒出来。

一份完整的程序

#include "stm32f10x.h"
#include <stdio.h>

static MPU6050_Raw  g_raw;
static MPU6050_Data g_data;

/* 屏幕上四行:角速度原始值、低通后、加速度原始值、卡尔曼后 */
void Sensor_ShowOnOled(void)
{
    OLED_ShowString(0, 0, "gz", OLED_8X16);
    OLED_ShowSignedNum(32, 0, (int32_t)(g_data.gyro_z_dps * 100.0f), 6, OLED_8X16);

    OLED_ShowString(0, 16, "gz lp", OLED_8X16);
    OLED_ShowSignedNum(48, 16, (int32_t)(g_sensor.gyro_z_dps * 100.0f), 6, OLED_8X16);

    OLED_ShowString(0, 32, "az", OLED_8X16);
    OLED_ShowSignedNum(32, 32, (int32_t)(g_data.accel_z_g * 1000.0f), 6, OLED_8X16);

    OLED_ShowString(0, 48, "az kf", OLED_8X16);
    OLED_ShowSignedNum(48, 48, (int32_t)(g_sensor.accel_z_g * 1000.0f), 6, OLED_8X16);

    OLED_Update();
}

/* 串口一行六个数:原始角速度两个滤波后各一路,原始加速度与滤波后各一路 */
void Sensor_PrintCsv(void)
{
    printf("%d,%d,%d,%d,%d,%d\r\n",
           (int32_t)(g_data.gyro_z_dps * 100.0f),
           (int32_t)(g_sensor.gyro_z_dps * 100.0f),
           (int32_t)(g_data.accel_z_g * 1000.0f),
           (int32_t)(g_sensor.accel_z_g * 1000.0f),
           (int32_t)(g_data.gyro_x_dps * 100.0f),
           (int32_t)(g_sensor.gyro_x_dps * 100.0f));
}

int main(void)
{
    uint32_t read_last_ms = 0U;
    uint32_t show_last_ms = 0U;

    OLED_Init();
    MPU6050_Init();                     /* 里面先读一次身份寄存器 */
    Gyro_LpfInit();
    Accel_KfInit();

    while (1)
    {
        if ((uint32_t)(tick_ms - read_last_ms) >= 10U)
        {
            read_last_ms = tick_ms;
            Sensor_Update();
        }

        if ((uint32_t)(tick_ms - show_last_ms) >= 200U)
        {
            show_last_ms = tick_ms;
            Sensor_ShowOnOled();
            Sensor_PrintCsv();
        }
    }
}

几句白话交代:tick_ms 是那条每毫秒加一的时间轴;MPU6050_Init 是初始化并在里面读一次身份寄存器;OLED 那几个显示函数分别是写字符串和写带符号整数,参数里带字体大小;printf 已经在把字符接到串口那一回接好了,这里发出去的就是几列逗号隔开的整数,把小数点往后挪两位、三位就是真实值。

到这里,两路数据都准备好了:角速度又快又平,加速度稳得住偏。但角度还没有出来——角速度得积分才有角度,积分又要面对零偏;加速度能定出“哪边是下”,可它自己分不清那是重力还是车在加速。把它们合起来、再决定姿态用什么表示,是姿态解算要做的事。

posted @ 2026-11-18 23:12  Zw-awa  阅读(5)  评论(0)    收藏  举报