一、引言传感器是嵌入式系统感知物理世界的眼睛和耳朵。温度、湿度、加速度、陀螺仪、气压、光照、距离……不管是消费电子、工业控制还是物联网设备传感器始终处于数据链的最前端。然而把一个传感器调通和把它用好之间有很大的距离你能读到一个温度值但它是准确的吗加速度计输出抖动得厉害怎么滤除噪声陀螺仪积分后漂移严重如何与加速度计融合多个传感器的数据时钟不同步怎么处理本文从驱动层到算法层系统讲解嵌入式传感器开发的完整链路I2C/SPI 传感器的驱动框架传感器的校准方法数字滤波算法滑动平均、中值滤波、卡尔曼滤波传感器数据融合互补滤波、Mahony、Madgwick工程实践从原始数据到可靠姿态平台STM32F103 MPU60506 轴 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 // AD00 时的地址含读写位 #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_I0x%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 / (19) 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: ±2g16384, ±4g8192, ±8g4096, ±16g2048 return (float)raw / scale; // 单位g } float gyro_convert(int16_t raw, float scale) { // scale: ±250131, ±50065.5, ±100032.8, ±200016.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*21][axis]) / 2.0f; // 比例系数 // 理想差值为 2gg 减去 -g 2g float diff raw[axis*2][axis] - raw[axis*21][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 时近似成立 // 例采样周期 dt10ms希望截止频率 fc5Hz // α 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 大 → 滤波器更不信任测量值输出更平滑 // 典型初始值q0.01, r1.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; }八、传感器选型速查传感器类型常用型号接口精度/特点6 轴 IMUMPU6050 / ICM-42688I2C/SPI消费级~$19 轴 IMUICM-20948 / BNO055I2C/SPI磁力计BNO055 自带融合温湿度SHT30 / AHT20I2C±0.3°C / ±2%RH气压BMP280 / MS5611I2C/SPI±1m 高度分辨率环境光BH1750 / VEML7700I2C1~65535 luxToF 测距VL53L0X / VL53L1XI2C2m/4m 量程磁力计QMC5883L / MMC5983MAI2C电子罗盘用九、总结层级核心工作关键点驱动层I2C/SPI 读写WHO_AM_I 验证 错误处理配置层量程/采样率/滤波器权衡精度和带宽校准层零偏/比例/温度补偿校准比算法更重要滤波层低通/中值/卡尔曼越简单越稳定融合层互补/卡尔曼/梯度下降Mahony 在 MCU 上最实用核心理念传感器开发的第一步不是上高级算法而是做好校准和滤波。一个正确校准的简单滤波器效果远好于一个未经校准的高级融合算法。
一、引言传感器是嵌入式系统感知物理世界的眼睛和耳朵。温度、湿度、加速度、陀螺仪、气压、光照、距离……不管是消费电子、工业控制还是物联网设备传感器始终处于数据链的最前端。然而把一个传感器调通和把它用好之间有很大的距离你能读到一个温度值但它是准确的吗加速度计输出抖动得厉害怎么滤除噪声陀螺仪积分后漂移严重如何与加速度计融合多个传感器的数据时钟不同步怎么处理本文从驱动层到算法层系统讲解嵌入式传感器开发的完整链路I2C/SPI 传感器的驱动框架传感器的校准方法数字滤波算法滑动平均、中值滤波、卡尔曼滤波传感器数据融合互补滤波、Mahony、Madgwick工程实践从原始数据到可靠姿态平台STM32F103 MPU60506 轴 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 // AD00 时的地址含读写位 #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_I0x%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 / (19) 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: ±2g16384, ±4g8192, ±8g4096, ±16g2048 return (float)raw / scale; // 单位g } float gyro_convert(int16_t raw, float scale) { // scale: ±250131, ±50065.5, ±100032.8, ±200016.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*21][axis]) / 2.0f; // 比例系数 // 理想差值为 2gg 减去 -g 2g float diff raw[axis*2][axis] - raw[axis*21][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 时近似成立 // 例采样周期 dt10ms希望截止频率 fc5Hz // α 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 大 → 滤波器更不信任测量值输出更平滑 // 典型初始值q0.01, r1.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; }八、传感器选型速查传感器类型常用型号接口精度/特点6 轴 IMUMPU6050 / ICM-42688I2C/SPI消费级~$19 轴 IMUICM-20948 / BNO055I2C/SPI磁力计BNO055 自带融合温湿度SHT30 / AHT20I2C±0.3°C / ±2%RH气压BMP280 / MS5611I2C/SPI±1m 高度分辨率环境光BH1750 / VEML7700I2C1~65535 luxToF 测距VL53L0X / VL53L1XI2C2m/4m 量程磁力计QMC5883L / MMC5983MAI2C电子罗盘用九、总结层级核心工作关键点驱动层I2C/SPI 读写WHO_AM_I 验证 错误处理配置层量程/采样率/滤波器权衡精度和带宽校准层零偏/比例/温度补偿校准比算法更重要滤波层低通/中值/卡尔曼越简单越稳定融合层互补/卡尔曼/梯度下降Mahony 在 MCU 上最实用核心理念传感器开发的第一步不是上高级算法而是做好校准和滤波。一个正确校准的简单滤波器效果远好于一个未经校准的高级融合算法。
![[C++]内存管理：串顺序存储的内存回收](http://pic.xiahunao.cn/yaotu/[C++]内存管理：串顺序存储的内存回收)
