MPU6050 六轴 IMU 详解：Arduino 姿态解算与 3D 可视化

本教程讲解 MPU6050 加速度计与陀螺仪 传感器的原理及 Arduino 读取方法，并实现两个实战例程：

1. 使用 Processing 实现传感器姿态 3D 可视化
2. 基于融合数据的自稳定平台（DIY 云台），三舵机保持平台水平

可观看视频或阅读图文教程。

概览

例程一：Processing 3D 可视化姿态；例程二：自稳定平台/云台，融合加速度计与陀螺仪数据驱动三舵机保持水平。

工作原理

MPU6050 集成 三轴加速度计 与 三轴陀螺仪 于单芯片。

• 陀螺仪：测量 X/Y/Z 轴角速度（°/s），基于 MEMS 科里奥利效应。积分得角度，但长期漂移
• 加速度计：测量三轴加速度（含重力），通过三角函数解算倾斜角。短期噪大，长期准
• 融合算法（互补滤波）：短期信陀螺仪（96%），长期信加速度计（4%），消除漂移与噪声，得高精度 Roll/Pitch。Yaw 无重力参考，需磁力计或 DMP 修正

MPU6050 即 6 轴运动追踪器 / 6 DoF（3 加速度 + 3 陀螺）。

Arduino 与 MPU6050 连接

I2C 通信，仅需 2 根信号线 (SDA/SCL) + 电源。

所需元件

• MPU6050 IMU 模块
• Arduino 开发板
• 面包板与跳线

MPU6050 Arduino 读取代码（含互补滤波）

/* Arduino + MPU6050 加速度计陀螺仪教程 by Dejan, https://howtomechatronics.com */
#include 
const int MPU = 0x68; // I2C 地址

float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;

void setup() {
  Serial.begin(19200);
  Wire.begin();
  // 复位电源管理寄存器 0x6B
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
  /* 可选：配置量程
  // 加速度计 ±8g (0x1C 写 0x10)
  // 陀螺仪 ±1000°/s (0x1B 写 0x10)
  */
  calculate_IMU_error(); // 校准误差
  delay(20);
}

void loop() {
  // === 读取加速度计 ===
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // 读 6 字节
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // ±2g 对应 16384 LSB/g
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  // 加速度计解算 Roll/Pitch
  accAngleX = atan(AccY / sqrt(pow(AccX,2) + pow(AccZ,2))) * 180 / PI - 0.58; // AccErrorX
  accAngleY = atan(-AccX / sqrt(pow(AccY,2) + pow(AccZ,2))) * 180 / PI + 1.58; // AccErrorY

  // === 读取陀螺仪 ===
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000.0; // 秒
  previousTime = currentTime;

  Wire.beginTransmission(MPU);
  Wire.write(0x43); // GYRO_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // ±250°/s 对应 131 LSB/°/s
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

  // 零偏修正
  GyroX += 0.56;  // GyroErrorX
  GyroY -= 2.0;   // GyroErrorY
  GyroZ += 0.79;  // GyroErrorZ

  // 积分得角度
  gyroAngleX += GyroX * elapsedTime;
  gyroAngleY += GyroY * elapsedTime;
  yaw        += GyroZ * elapsedTime;

  // 互补滤波融合
  roll  = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

  // 串口输出
  Serial.print(roll); Serial.print("/");
  Serial.print(pitch); Serial.print("/");
  Serial.println(yaw);
}

void calculate_IMU_error() {
  // 静止平放采集 200 次求平均零偏
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccErrorX += atan(AccY / sqrt(pow(AccX,2) + pow(AccZ,2))) * 180 / PI;
    AccErrorY += atan(-AccX / sqrt(pow(AccY,2) + pow(AccZ,2))) * 180 / PI;
    c++;
  }
  AccErrorX /= 200; AccErrorY /= 200; c = 0;

  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroErrorX += GyroX; GyroErrorY += GyroY; GyroErrorZ += GyroZ;
    c++;
  }
  GyroErrorX /= 200; GyroErrorY /= 200; GyroErrorZ /= 200;

  // 串口打印误差值，填入上方修正代码
  Serial.print("AccErrorX: "); Serial.println(AccErrorX);
  Serial.print("AccErrorY: "); Serial.println(AccErrorY);
  Serial.print("GyroErrorX: "); Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: "); Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: "); Serial.println(GyroErrorZ);
}

代码关键点

1. I2C 读取原始数据，按数据手册灵敏度换算（±2g: 16384 LSB/g；±250°/s: 131 LSB/°/s）
2. 加速度计解算 Roll/Pitch，减去静态零偏
3. 陀螺仪积分得角度，修正零偏
4. 互补滤波：0.96 * 陀螺角度 + 0.04 * 加速度角度
5. calculate_IMU_error() 静止采样 200 次求平均零偏，结果写入修正常量

Processing 3D 姿态可视化

Arduino 串口发送 roll/pitch/yaw，Processing 读取旋转 3D 物体。

/* Arduino + MPU6050 - 3D 可视化 by Dejan, https://howtomechatronics.com */
import processing.serial.*;
Serial myPort; String data=""; float roll, pitch, yaw;

void setup() {
  size(2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200);
  myPort.bufferUntil('\n');
}

void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + " Pitch: " + int(pitch), -100, 265);
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  textSize(30); fill(0, 76, 153);
  box(386, 40, 200);
  textSize(25); fill(255);
  text("www.HowToMechatronics.com", -183, 10, 101);
}

void serialEvent(Serial myPort) {
  data = myPort.readStringUntil('\n');
  if (data != null) {
    data = trim(data);
    String[] items = split(data, '/');
    if (items.length > 2) {
      roll  = float(items[0]);
      pitch = float(items[1]);
      yaw   = float(items[2]);
    }
  }
}

运行效果：3D 盒子实时跟随传感器姿态，响应灵敏。Yaw 会随时间漂移（无加速度计修正）。

进阶：使用 DMP (Digital Motion Processor) 消除 Yaw 漂移

MPU6050 内置 DMP 可输出四元数，无万向节锁、自动融合磁力计修正 Yaw。需额外连接中断引脚 (INT → Arduino D2)，使用 i2cdevlib 库。

库自带 MPU6050_DMP6 示例与 Processing 可视化代码（输出四元数 OUTPUT_TEAPOT）。四元数旋转更稳健、无 Gimbal Lock。

/* DMP 四元数 3D 可视化 */
import processing.serial.*;
Serial myPort; String data=""; float roll, pitch, yaw;

void setup() {
  size(2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200);
  myPort.bufferUntil('\n');
}

void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + " Pitch: " + int(pitch), -100, 265);
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  textSize(30); fill(0, 76, 153);
  box(386, 40, 200);
  textSize(25); fill(255);
  text("www.HowToMechatronics.com", -183, 10, 101);
}

void serialEvent(Serial myPort) {
  data = myPort.readStringUntil('\n');
  if (data != null) {
    data = trim(data);
    String[] items = split(data, '/');
    if (items.length > 2) {
      roll  = float(items[0]);
      pitch = float(items[1]);
      yaw   = float(items[2]);
    }
  }
}

第二个例程（DIY 云台/自稳定平台）已单独成文，详见后续文章。

欢迎评论区提问，别忘了查看我的 Arduino 项目合集！