MPU6050数据融合入门：用Arduino和简易卡尔曼滤波做个自平衡装置
MPU6050数据融合实战：用Arduino打造会自我平衡的智能装置
当你第一次看到两轮平衡车稳稳立在原地时，是否好奇它如何抵抗重力不倒下？这背后藏着传感器融合的魔法。本文将带你用最常见的Arduino开发板和MPU6050模块，从零构建一个能实时平衡的物理装置，无需复杂数学推导，只需动手实践就能理解卡尔曼滤波的精妙。
1. 硬件准备与环境搭建
手边需要准备的硬件非常简单：一块Arduino开发板（UNO或Nano皆可）、MPU6050六轴传感器模块、SG90舵机或直流电机，以及若干杜邦线。整个项目的硬件成本可以控制在百元以内，非常适合学生和创客入门。
MPU6050作为核心传感器，能同时测量三轴加速度和三轴角速度。但原始数据存在明显缺陷：
- 加速度计数据：易受瞬时振动干扰，但长期稳定
- 陀螺仪数据：短期精确但会产生累积误差
// MPU6050基本连接示例 #include <Wire.h> const int MPU_ADDR = 0x68; // I2C地址 void setup() { Wire.begin(); Wire.beginTransmission(MPU_ADDR); Wire.write(0x6B); // PWR_MGMT_1寄存器 Wire.write(0); // 唤醒MPU6050 Wire.endTransmission(true); }提示：焊接MPU6050模块时，建议保留排针方便调试。若使用3.3V供电的Arduino版本，注意电平匹配问题。
2. 数据采集与预处理
原始传感器数据需要经过校准和初步处理才能使用。先通过串口绘图观察原始波形：
- 校准偏移量：静止状态下记录200组数据取平均值
- 单位转换：
- 加速度计：±2g量程对应16384 LSB/g
- 陀螺仪：±250°/s量程对应131 LSB/°/s
- 数据同步：确保加速度和角速度时间戳对齐
void loop() { Wire.beginTransmission(MPU_ADDR); Wire.write(0x3B); // 从ACCEL_XOUT_H开始读取 Wire.endTransmission(false); Wire.requestFrom(MPU_ADDR, 14, true); // 读取14字节 // 原始数据转换 AccX = (Wire.read()<<8|Wire.read()) / 16384.0; GyroY = (Wire.read()<<8|Wire.read()) / 131.0; // 时间间隔计算 unsigned long currentMillis = millis(); float dt = (currentMillis - prevMillis) / 1000.0; prevMillis = currentMillis; }| 参数 | 典型值 | 单位 | 说明 |
|---|---|---|---|
| AccX | ±2.0 | g | X轴加速度 |
| GyroY | ±250 | °/s | Y轴角速度 |
| 采样率 | 100 | Hz | 推荐设置 |
3. 简易卡尔曼滤波实现
传统互补滤波虽然简单，但在动态场景下表现欠佳。我们采用经过简化的单维卡尔曼滤波，只需理解三个核心参数：
- Q_angle：过程噪声协方差（默认0.001）
- Q_bias：偏差噪声协方差（默认0.003）
- R_measure：测量噪声协方差（默认0.03）
typedef struct { float Q_angle; float Q_bias; float R_measure; float angle; float bias; float P[2][2]; } Kalman_t; float kalmanUpdate(Kalman_t *kalman, float newAngle, float newRate, float dt) { // 预测阶段 kalman->angle += dt * (newRate - kalman->bias); kalman->P[0][0] += dt * (dt*kalman->P[1][1] - kalman->P[0][1] - kalman->P[1][0] + kalman->Q_angle); // 更新阶段 float S = kalman->P[0][0] + kalman->R_measure; float K[2] = {kalman->P[0][0]/S, kalman->P[1][0]/S}; float y = newAngle - kalman->angle; kalman->angle += K[0] * y; kalman->bias += K[1] * y; return kalman->angle; }注意：实际调试时，可用手机水平仪作为参考，先用
Serial.print()输出原始角度和滤波后角度对比效果。
4. 平衡控制与系统集成
将滤波后的俯仰角(Pitch)转换为执行器控制信号，形成闭环系统：
- 角度映射：±30°对应舵机0-180°位置
- PID控制：简单比例控制即可实现基本平衡
- 机械结构：建议先用纸板制作简易平衡平台测试
void controlServo(float angle) { // 角度限幅 angle = constrain(angle, -30, 30); // 转换为舵机角度 int servoAngle = map(angle * 100, -3000, 3000, 0, 180); // 写入舵机 myServo.write(servoAngle); // 调试输出 Serial.print("Raw:"); Serial.print(rawAngle); Serial.print(" Filtered:"); Serial.println(kalmanAngle); }常见问题排查指南：
- 数据漂移：重新校准陀螺仪零偏
- 响应迟钝：减小卡尔曼滤波的R_measure值
- 剧烈振荡：降低PID比例系数
5. 进阶优化方向
当基础功能实现后，可以尝试以下增强功能：
多传感器融合方案对比
| 方法 | 复杂度 | 实时性 | 抗干扰性 |
|---|---|---|---|
| 互补滤波 | 低 | 高 | 中 |
| 卡尔曼滤波 | 中 | 中 | 高 |
| Mahony滤波 | 较高 | 高 | 高 |
性能优化技巧
- 使用
micros()替代millis()提高定时精度 - 启用MPU6050的DLPF(数字低通滤波器)
- 尝试固定点运算提升计算效率
// 启用DLPF配置示例 Wire.beginTransmission(MPU_ADDR); Wire.write(0x1A); // 配置寄存器 Wire.write(0x03); // 带宽5Hz Wire.endTransmission();我在实际测试中发现，用热熔胶固定传感器能有效减少高频振动干扰。对于教学演示，可以故意引入干扰（如轻敲装置），观察滤波算法如何逐步修正偏差，这种直观展示比理论讲解更有说服力。