经典三问
Q: 为什么同一个芯片的六个数要用两种滤波?
A: 因为这两路信号的毛病不一样:角速度马上要被积分,怕的是高频抖动;加速度要长期当"哪边是下"的基准,怕的是抖和慢慢偏。一种滤波压不住两边。
Q: 一阶低通和一维卡尔曼差在哪?
A: 低通的系数是写死的;卡尔曼的系数由"该信测量多少"算出来。在这组参数下它跑起来有多慢,是能直接量出来的。
Q: 做完这一步,手里有什么?
A: 两路干净的数据:角速度可以拿去积分,加速度可以拿去定方向。把它们合成姿态,是姿态解算那一步的事。
模块平放在桌上,串口把六个原始数一行行吐出来。角速度和加速度都在动,但动法不一样。
角速度的抖是"毛":上一拍 gz=19,下一拍 23,再下一拍 17,围着零上下乱跳,看不出方向。
加速度的抖是"沉":平放时 az 稳稳待在一个大数附近,动一下才整体偏过去,而且偏过去之后待在新位置上不下来------倾斜多少,它就偏多少。它测的不是噪声,是重力的分量。
这两句话就解释了为什么两路要用两种滤波:角速度怕的是加在真值上的毛刺,加速度怕的是"被运动骗到另一个位置"。
角速度那一路:一阶低通
角速度后面要做什么?积分------每一拍乘上时间,一路加起来变成角度。积分这个东西对高频特别敏感:一拍的抖动会被永久地加进角度里,而且再也拿不出来。所以角速度在积分之前必须先压掉高频。
压高频最省事的办法就是一阶低通:
c
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 秒 |
写成一个只干这件事的小结构,三轴各一个:
c
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 爬到真值的曲线,看上去像传感器要"热身"。给第一拍一个直接赋值,这条假斜线就没了。
三个轴各一个实例:
c
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);
}
}
为什么角速度这里不用卡尔曼
卡尔曼当然能用,只是我觉得没必要。它的力气花在"把'该信测量多少'交给两个参数算出来"上,而角速度这一路的要求很直白:快一点、平一点,别把高频带进积分里。一个系数写死的低通就够,代码短、开销小、行为一眼能算出来。
加速度那一侧的情况不一样,所以那边要上更重的东西。
加速度那一路:三个一维卡尔曼
加速度计有它自己的麻烦。静止时它是可靠的------重力指着哪边,它就说哪边。可一旦动起来,它测的就是"重力 + 运动加速度"的合成:车一加速,它以为世界歪了;压过一个坎,它给你一个尖峰。再加上安装角和器件本身的偏差,它还带着一个慢慢变的"偏"。
也就是说,加速度这一路既有抖 ,又有偏 ,而且它的变化不是纯噪声,是有结构的。卡尔曼滤波正好对应这类问题:一边用"我预测它应该是多少"(模型),一边用"现在测到多少"(测量),按两者的可信度加权。
一维版本只有四行:
c
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说了算。 - 第二行:增益 = 估计误差 ÷(估计误差 + 测量噪声)。误差大、测量噪声小,增益就大,也就是说"这次我更信测量"。
- 第三行 :拿测量和估计之间的差,按增益补一部分进来。这一行和低通那条式子是同一个形状,区别只在系数是算出来的。
- 第四行:修正之后误差减小了,记下来给下一拍用。
加速度有三个轴,所以要三个实例,每个轴一套状态:
c
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 没有硬件浮点单元,但每拍只多出十来次乘除,可以忽略。
顺序是这样:
text
读 14 个字节
↓
换算成 度/秒 和 g
↓
减掉零偏(讲零偏那一回校出来的那个数)
↓
角速度 → 一阶低通(三轴)
加速度 → 一维卡尔曼(三轴)
↓
存进一副结构体,后面的解算从这里取数
零偏在低通之前减。因为零偏是一个常数,低通对常数不敏感------先减后减结果几乎一样,但先减省事:不用给每个轴再存一份偏置,也不会让常数在那个"从 0 爬上来"的过程里多绕一圈。
c
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 频率或者限制最大占空比)、或者干脆不采这一项,用别的量推出来。
这一条在按键、串口这些地方看不见,只有对着一个会随开关动作变化的模拟量,才会冒出来。
一份完整的程序
c
#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 已经在把字符接到串口那一回接好了,这里发出去的就是几列逗号隔开的整数,把小数点往后挪两位、三位就是真实值。
到这里,两路数据都准备好了:角速度又快又平,加速度稳得住偏。但角度还没有出来------角速度得积分才有角度,积分又要面对零偏;加速度能定出"哪边是下",可它自己分不清那是重力还是车在加速。把它们合起来、再决定姿态用什么表示,是姿态解算要做的事。