4. 信号处理：车身姿态传感器（加速度计）信号采集与滤波、卡尔曼滤波算法实现、信号有效性检查
好，咱们进入第四章节。说实话，信号处理这部分，是前大灯水平调节系统里最容易被低估的环节。很多人觉得，不就是读个加速度计数据嘛，能有多难？嗯，我当年也是这么想的，直到在实车上被颠簸路面狠狠教育了一回。
4.1 加速度计信号采集——别小看这一步
车身姿态传感器，说白了就是一颗MEMS加速度计。它输出的是三个轴（X、Y、Z）上的加速度值。但你要知道，这玩意儿输出的原始数据，噪声大得吓人。
我习惯用SPI接口来读数据，速度快，抗干扰也好。I2C也行，但线长了容易出问题。采集流程大概是这样的：
// 伪代码示例：加速度计数据采集
void Accel_ReadRaw(int16_t *x, int16_t *y, int16_t *z) {
uint8_t buf[6];
SPI_ReadReg(ACCEL_OUT_X_L, buf, 6); // 连续读取6字节
*x = (int16_t)(buf[1] << 8 | buf[0]);
*y = (int16_t)(buf[3] << 8 | buf[2]);
*z = (int16_t)(buf[5] << 8 | buf[4]);
}
这里有个坑——字节序。不同厂家的加速度计，高低字节顺序可能不一样。我遇到过某款国产芯片，手册上写的是小端，实际读出来是大端。嗯，调试了两天才发现。
4.2 滤波——把噪声压下去
原始数据长什么样？你想想看，车停在平地上，理论上Z轴应该是1g（约9.8m/s²），X和Y轴接近0。但实际上呢？读数一直在跳，±0.1g的波动很正常。
我最早用的是滑动平均滤波。简单，有效，但有个问题——延迟大。你想想，车身已经倾斜了，滤波后的数据还在慢慢爬，大灯反应就慢了。
后来我换成了一阶低通滤波。公式很简单：
// 一阶低通滤波
float Accel_LowPassFilter(float raw, float prev, float alpha) {
return alpha * raw + (1.0f - alpha) * prev;
}
alpha值怎么选？我一般取0.1到0.3之间。alpha越大，响应越快，但噪声也大。alpha越小，越平滑，但延迟也大。这个需要根据实际路况调。
4.3 卡尔曼滤波——这才是重头戏
一阶低通滤波够用吗？说实话，在大多数场景下够了。但如果你想让系统更聪明一点，那就得上卡尔曼滤波。
卡尔曼滤波的核心思想是什么？说白了，就是用模型预测 + 测量修正。你想想看，车身姿态的变化是有规律的，不是随机乱跳的。卡尔曼滤波就是利用这个规律，把预测值和测量值融合起来，得到一个更准的结果。
对于车身姿态估计，我用的状态量是：
- 角度（θ）—— 我们要的最终结果
- 角速度偏差（bias）—— 陀螺仪的零偏
观测值是加速度计算出来的角度。系统模型是这样的：
// 卡尔曼滤波 - 预测步骤
void Kalman_Predict(Kalman_t *kf, float gyro_rate, float dt) {
// 状态预测
kf->angle += dt * (gyro_rate - kf->bias);
// 协方差预测
kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
kf->P[0][1] -= dt * kf->P[1][1];
kf->P[1][0] -= dt * kf->P[1][1];
kf->P[1][1] += kf->Q_gyro * dt;
}
// 卡尔曼滤波 - 更新步骤
void Kalman_Update(Kalman_t *kf, float accel_angle) {
float y = accel_angle - kf->angle; // 残差
float S = kf->P[0][0] + kf->R_measure; // 残差协方差
float K[2]; // 卡尔曼增益
K[0] = kf->P[0][0] / S;
K[1] = kf->P[1][0] / S;
// 状态更新
kf->angle += K[0] * y;
kf->bias += K[1] * y;
// 协方差更新
float P00_temp = kf->P[0][0];
float P01_temp = kf->P[0][1];
kf->P[0][0] -= K[0] * P00_temp;
kf->P[0][1] -= K[0] * P01_temp;
kf->P[1][0] -= K[1] * P00_temp;
kf->P[1][1] -= K[1] * P01_temp;
}
这里有几个参数需要调：
| 参数 | 含义 | 我的建议值 |
|---|---|---|
| Q_angle | 角度噪声协方差 | 0.001 |
| Q_gyro | 陀螺仪噪声协方差 | 0.003 |
| R_measure | 测量噪声协方差 | 0.03 |
4.4 信号有效性检查——别让坏数据毁了系统
我曾经遇到过一件事。某次路试，大灯突然乱跳。查了半天，发现是加速度计的一根线松了，数据全是0xFFFF。卡尔曼滤波看到这个数据，以为车身翻了180度，于是大灯就照到天上去了。
从那以后，我加了三道检查：
- 范围检查：加速度值必须在±16g以内（根据传感器量程定）。超出范围，直接丢弃。
- 变化率检查：相邻两次采样的差值不能太大。比如，100Hz采样下，差值超过0.5g就不正常。因为车身不可能在10ms内改变那么多。
- 一致性检查：X、Y、Z三个轴的平方和应该接近1g²（静止时）。如果偏差超过20%，说明传感器可能坏了。
// 信号有效性检查
bool Accel_CheckValid(int16_t x, int16_t y, int16_t z) {
// 1. 范围检查
if (abs(x) > 16000 || abs(y) > 16000 || abs(z) > 16000) return false;
// 2. 变化率检查（假设上次值为x_prev等）
if (abs(x - x_prev) > 500) return false;
// 3. 一致性检查
float mag_sq = x*x + y*y + z*z;
if (mag_sq < 0.8f * 16384*16384 || mag_sq > 1.2f * 16384*16384) return false;
return true;
}
4.5 总结一下
信号处理这块，说白了就是三个步骤：采进来、滤干净、验真伪。每一步都有坑，但踩过了也就记住了。
我个人习惯是：先用一阶低通滤波快速验证功能，等系统稳定了再上卡尔曼滤波。别一上来就搞复杂的算法，万一调不通，你连问题出在哪都不知道。
嗯，下一章咱们聊聊怎么把这些滤波后的角度值，转换成大灯的实际控制信号。到时候你会看到，前面这些信号处理工作，到底有多重要。