一、引言

传感器是嵌入式系统感知物理世界的"眼睛"和"耳朵"。温度、湿度、加速度、陀螺仪、气压、光照、距离……不管是消费电子、工业控制还是物联网设备，传感器始终处于数据链的最前端。

然而，把一个传感器"调通"和把它"用好"之间有很大的距离：

• 你能读到一个温度值，但它是准确的吗？

• 加速度计输出抖动得厉害，怎么滤除噪声？

• 陀螺仪积分后漂移严重，如何与加速度计融合？

• 多个传感器的数据时钟不同步，怎么处理？

本文从驱动层到算法层，系统讲解嵌入式传感器开发的完整链路：

• I2C/SPI 传感器的驱动框架

• 传感器的校准方法

• 数字滤波算法（滑动平均、中值滤波、卡尔曼滤波）

• 传感器数据融合（互补滤波、Mahony、Madgwick）

• 工程实践：从原始数据到可靠姿态

平台：STM32F103 + MPU6050（6 轴 IMU）+ SHT30（温湿度）+ BMP280（气压） 重点：不仅"能用"，更要"可靠"

二、传感器驱动框架

2.1 通用的传感器驱动结构

// sensor.h —— 传感器抽象接口 typedef struct { int (*init)(void); // 初始化 int (*read)(void *data); // 读取数据 int (*calibrate)(void); // 校准 int (*set_config)(void *config); // 配置参数 int (*self_test)(void); // 自检 } sensor_driver_t; ​ // 具体传感器实现：MPU6050 static sensor_driver_t mpu6050_driver = { .init = mpu6050_init, .read = mpu6050_read, .calibrate = mpu6050_calibrate, .set_config = mpu6050_set_config, .self_test = mpu6050_self_test, };

2.2 I2C 传感器的通用读写

// i2c_sensor.c —— 适用于所有 I2C 传感器 typedef struct { I2C_TypeDef *i2c; uint8_t dev_addr; // 7 位设备地址（左移 1 位后写入 DR） uint32_t timeout_ms; } i2c_sensor_t; ​ // 读寄存器（最常见操作） int sensor_read_reg(i2c_sensor_t *s, uint8_t reg, uint8_t *buf, uint8_t len) { // 1. 发送寄存器地址 I2C_GenerateSTART(s->i2c, ENABLE); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Transmitter); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); I2C_SendData(s->i2c, reg); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); // 2. 重复起始 + 读 I2C_GenerateSTART(s->i2c, ENABLE); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Receiver); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); for (int i = 0; i < len; i++) { if (i == len - 1) { I2C_AcknowledgeConfig(s->i2c, DISABLE); // 最后一个字节发 NACK } while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_RECEIVED)); buf[i] = I2C_ReceiveData(s->i2c); } I2C_GenerateSTOP(s->i2c, ENABLE); return 0; } ​ // 写寄存器 int sensor_write_reg(i2c_sensor_t *s, uint8_t reg, uint8_t val) { I2C_GenerateSTART(s->i2c, ENABLE); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(s->i2c, s->dev_addr, I2C_Direction_Transmitter); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); I2C_SendData(s->i2c, reg); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_SendData(s->i2c, val); while (!I2C_CheckEvent(s->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_GenerateSTOP(s->i2c, ENABLE); return 0; }

三、MPU6050 驱动实战

3.1 芯片初始化

// MPU6050 初始化完整流程 #define MPU6050_ADDR 0xD0 // AD0=0 时的地址（含读写位） #define MPU6050_WHO_AM_I 0x75 #define MPU6050_PWR_MGMT 0x6B #define MPU6050_ACCEL_X 0x3B // 加速度寄存器起始（连续 6 字节） #define MPU6050_GYRO_X 0x43 // 陀螺仪寄存器起始（连续 6 字节） ​ typedef struct { int16_t accel_x, accel_y, accel_z; int16_t temp; int16_t gyro_x, gyro_y, gyro_z; } mpu6050_raw_t; ​ int mpu6050_init(void) { uint8_t whoami; i2c_sensor_t s = { .i2c = I2C1, .dev_addr = MPU6050_ADDR }; // 1. 检查 WHO_AM_I 寄存器（应为 0x68） sensor_read_reg(&s, MPU6050_WHO_AM_I, &whoami, 1); if (whoami != 0x68) { printf("MPU6050 not found! WHO_AM_I=0x%02X\r\n", whoami); return -1; } // 2. 唤醒 MPU6050（退出睡眠模式） sensor_write_reg(&s, MPU6050_PWR_MGMT, 0x00); HAL_Delay(100); // 等待起振稳定 // 3. 配置陀螺仪量程：±2000°/s // 寄存器 0x1B, FS_SEL[4:3] = 11 = ±2000°/s sensor_write_reg(&s, 0x1B, 0x18); // 0x18 = 0b00011000 // 4. 配置加速度计量程：±16g // 寄存器 0x1C, AFS_SEL[4:3] = 11 = ±16g sensor_write_reg(&s, 0x1C, 0x18); // 5. 配置数字低通滤波器 (DLPF)：~43Hz 带宽 // 寄存器 0x1A, DLPF_CFG[2:0] = 011 = 43Hz sensor_write_reg(&s, 0x1A, 0x03); // 6. 配置采样率分频：1kHz / (1+9) = 100Hz sensor_write_reg(&s, 0x19, 9); return 0; } ​ // 读取原始数据 int mpu6050_read_raw(mpu6050_raw_t *raw) { uint8_t buf[14]; i2c_sensor_t s = { .i2c = I2C1, .dev_addr = MPU6050_ADDR }; sensor_read_reg(&s, MPU6050_ACCEL_X, buf, 14); raw->accel_x = (int16_t)((buf[0] << 8) | buf[1]); raw->accel_y = (int16_t)((buf[2] << 8) | buf[3]); raw->accel_z = (int16_t)((buf[4] << 8) | buf[5]); raw->temp = (int16_t)((buf[6] << 8) | buf[7]); raw->gyro_x = (int16_t)((buf[8] << 8) | buf[9]); raw->gyro_y = (int16_t)((buf[10] << 8) | buf[11]); raw->gyro_z = (int16_t)((buf[12] << 8) | buf[13]); return 0; } ​ // 转换为物理量 float accel_convert(int16_t raw, float scale) { // scale: ±2g=16384, ±4g=8192, ±8g=4096, ±16g=2048 return (float)raw / scale; // 单位：g } ​ float gyro_convert(int16_t raw, float scale) { // scale: ±250=131, ±500=65.5, ±1000=32.8, ±2000=16.4 return (float)raw / scale; // 单位：°/s } ​ float temp_convert(int16_t raw) { return (float)raw / 340.0f + 36.53f; // 单位：°C }

四、传感器校准

4.1 加速度计六面校准法

// 加速度计校准原理： // 理想情况下，重力加速度只沿一个轴作用 // 但存在零偏 (bias) 和比例误差 (scale) ​ // 真实值 = (原始值 - 零偏) × 比例系数 ​ // 六面法：分别测量 +X, -X, +Y, -Y, +Z, -Z 六个面朝上时的输出 typedef struct { float offset[3]; // 零偏 float scale[3]; // 比例系数 } accel_calib_t; ​ void accel_calibrate_six_face(float raw[6][3], accel_calib_t *calib) { // raw[0] = +X 面朝上, raw[1] = -X 面朝上, ... for (int axis = 0; axis < 3; axis++) { // 零偏 = (正 + 负) / 2 calib->offset[axis] = (raw[axis*2][axis] + raw[axis*2+1][axis]) / 2.0f; // 比例系数 // 理想差值为 2g（+g 减去 -g = 2g） float diff = raw[axis*2][axis] - raw[axis*2+1][axis]; calib->scale[axis] = 2.0f / diff; } }

4.2 陀螺仪静止校准

// 陀螺仪在静止时输出应该为 0，但实际有零偏 void gyro_calibrate_static(float offset[3], int samples) { float sum[3] = {0}; for (int i = 0; i < samples; i++) { mpu6050_raw_t raw; mpu6050_read_raw(&raw); sum[0] += raw.gyro_x; sum[1] += raw.gyro_y; sum[2] += raw.gyro_z; HAL_Delay(10); } offset[0] = sum[0] / samples; offset[1] = sum[1] / samples; offset[2] = sum[2] / samples; printf("Gyro offsets: %.2f, %.2f, %.2f\r\n", offset[0], offset[1], offset[2]); }

五、数字滤波算法

5.1 滑动平均滤波（Moving Average）

// 最简单、最快速的平滑滤波 #define MA_WINDOW 16 ​ typedef struct { float buffer[MA_WINDOW]; int index; float sum; int count; } moving_average_t; ​ void ma_init(moving_average_t *f) { memset(f, 0, sizeof(*f)); } ​ float ma_update(moving_average_t *f, float new_val) { // 减去最旧值，加入新值 f->sum -= f->buffer[f->index]; f->buffer[f->index] = new_val; f->sum += new_val; f->index = (f->index + 1) % MA_WINDOW; if (f->count < MA_WINDOW) f->count++; return f->sum / f->count; } ​ // 优点：极快（只需 2 次加减法 + 1 次除法） // 缺点：对脉冲噪声抑制能力弱

5.2 中值滤波（Median Filter）

// 最适合抑制脉冲噪声（尖刺） // 但排序开销较大，适合低频数据 ​ float median_filter(float *window, int size) { // 复制并排序（或使用选择算法） float sorted[size]; memcpy(sorted, window, size * sizeof(float)); // 冒泡排序（数据量小时可用） for (int i = 0; i < size - 1; i++) { for (int j = 0; j < size - 1 - i; j++) { if (sorted[j] > sorted[j + 1]) { float tmp = sorted[j]; sorted[j] = sorted[j + 1]; sorted[j + 1] = tmp; } } } return sorted[size / 2]; // 中值 }

5.3 一阶低通/互补滤波器

// ★ 嵌入式最常用的滤波器 // 公式：output = α × input + (1-α) × prev_output ​ // α 取值范围：0.0 ~ 1.0 // α ≈ 1.0 → 几乎无滤波（跟随输入） // α ≈ 0.0 → 强滤波（响应慢） ​ typedef struct { float alpha; float prev_output; } lowpass_filter_t; ​ void lpf_init(lowpass_filter_t *f, float alpha) { f->alpha = alpha; f->prev_output = 0; } ​ float lpf_update(lowpass_filter_t *f, float input) { f->prev_output = f->alpha * input + (1.0f - f->alpha) * f->prev_output; return f->prev_output; } ​ // 时间常数与截止频率的换算： // α = dt / (dt + RC) —— 时间常数形式 // α = 2π × fc × dt —— 截止频率形式（当 α << 1 时近似成立） ​ // 例：采样周期 dt=10ms，希望截止频率 fc=5Hz // α = 2π × 5 × 0.01 = 0.314 // 在代码中：lpf_init(&f, 0.314f);

5.4 卡尔曼滤波器（单变量）

// 一维卡尔曼滤波器——适合单传感器滤波 // 公式清晰，计算量可控 typedef struct { float x; // 状态估计值 float p; // 估计误差协方差 float q; // 过程噪声协方差（越大→越信任测量值） float r; // 测量噪声协方差（越大→越不信任测量值） float k; // 卡尔曼增益（内部计算） } kalman1d_t; void kalman1d_init(kalman1d_t *kf, float init_val, float q, float r) { kf->x = init_val; kf->p = 1.0f; kf->q = q; kf->r = r; } float kalman1d_update(kalman1d_t *kf, float measurement) { // 预测（Prediction） // 假设状态不变（最简单模型） kf->p = kf->p + kf->q; // 更新（Update） kf->k = kf->p / (kf->p + kf->r); kf->x = kf->x + kf->k * (measurement - kf->x); kf->p = (1.0f - kf->k) * kf->p; return kf->x; } // 调参经验： // q 大 → 滤波器更"信任"测量值，响应更快 // r 大 → 滤波器更"不信任"测量值，输出更平滑 // 典型初始值：q=0.01, r=1.0

六、姿态解算——传感器融合

6.1 为什么需要融合？

加速度计：静态精度好，可以测重力方向 但动态时受运动加速度干扰——无法区分重力和运动加速度 陀螺仪：动态响应快，不受运动加速度影响 但积分累积漂移——静止 1 分钟后可能漂移好几度 → 融合思路： 陀螺仪提供短期姿态变化（高频响应好） 加速度计提供长期姿态基准（低频稳定好） = 互补滤波器（取各自的长处）

6.2 Mahony 互补滤波器

// Mahony AHRS (Attitude and Heading Reference System) 互补滤波器 // 用四元数表示姿态，计算量小，适合 MCU typedef struct { float q0, q1, q2, q3; // 四元数 float kp; // 比例增益（典型 2.0） float ki; // 积分增益（典型 0.0~0.1） float integral_fb[3]; // 积分项（补偿陀螺仪漂移） float dt; // 采样周期 } mahony_t; void mahony_init(mahony_t *m, float kp, float ki, float dt) { m->q0 = 1.0f; // 初始姿态：水平朝北 m->q1 = 0.0f; m->q2 = 0.0f; m->q3 = 0.0f; m->kp = kp; m->ki = ki; m->dt = dt; m->integral_fb[0] = m->integral_fb[1] = m->integral_fb[2] = 0.0f; } // 核心更新函数 void mahony_update(mahony_t *m, float gx, float gy, float gz, // 陀螺仪 rad/s float ax, float ay, float az) // 加速度 g { float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3; float recip_norm; float vx, vy, vz; // 重力的估计方向 float ex, ey, ez; // 误差（加速度测量 - 重力估计） // 1. 归一化加速度计读数 recip_norm = 1.0f / sqrtf(ax*ax + ay*ay + az*az); ax *= recip_norm; ay *= recip_norm; az *= recip_norm; // 2. 从四元数估计重力方向（在机体坐标系下） vx = 2.0f * (q1*q3 - q0*q2); vy = 2.0f * (q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 3. 计算误差 = 测量值 - 估计值 的叉积 ex = ay*vz - az*vy; ey = az*vx - ax*vz; ez = ax*vy - ay*vx; // 4. PI 控制器修正陀螺仪 m->integral_fb[0] += m->ki * ex * m->dt; m->integral_fb[1] += m->ki * ey * m->dt; m->integral_fb[2] += m->ki * ez * m->dt; gx += m->kp * ex + m->integral_fb[0]; gy += m->kp * ey + m->integral_fb[1]; gz += m->kp * ez + m->integral_fb[2]; // 5. 用修正后的角速度更新四元数（一阶龙格-库塔） q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * m->dt; q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * m->dt; q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * m->dt; q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * m->dt; // 6. 归一化四元数 recip_norm = 1.0f / sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3); m->q0 = q0 * recip_norm; m->q1 = q1 * recip_norm; m->q2 = q2 * recip_norm; m->q3 = q3 * recip_norm; } // 四元数→欧拉角（Roll/Pitch/Yaw） void mahony_get_euler(mahony_t *m, float *roll, float *pitch, float *yaw) { float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3; *roll = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2)); *pitch = asinf(2.0f*(q0*q2 - q3*q1)); *yaw = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3)); // 转换为度 *roll *= 57.29578f; *pitch *= 57.29578f; *yaw *= 57.29578f; }

七、多传感器数据处理模式

7.1 时间同步策略

多个传感器各自独立采样，时间标签不同： 传感器 A (100Hz) ──●──●──●──●──●──●──●──●── 传感器 B (50Hz) ────●──────●──────●──────●── 传感器 C (10Hz) ──────────●──────────────●── 融合处理时的时间对齐方法： 1. 最近邻插值——取时间最接近的数据点 2. 线性插值——根据两个采样点插值 3. 统一采样时钟——用一个定时器同时触发所有传感器（最佳）

7.2 IMU 温度补偿

// MPU6050 的零偏随温度变化 // 开机后芯片温度上升 → 零偏漂移 → 需要补偿 typedef struct { float temp_offset[3]; // 温度-零偏关系的截距 float temp_slope[3]; // 温度-零偏关系的斜率 } temp_compensation_t; // 在恒温箱中对不同温度下的零偏进行标定 // 简单模型：零偏 = offset + slope × temperature float gyro_compensate(float raw, float temp, temp_compensation_t *comp, int axis) { float bias = comp->temp_offset[axis] + comp->temp_slope[axis] * temp; return raw - bias; }

八、传感器选型速查

九、总结

核心理念：传感器开发的第一步不是上高级算法，而是做好校准和滤波。一个正确校准的简单滤波器，效果远好于一个未经校准的高级融合算法。