Мастер-класс по MPU6050: акселерометр и гироскоп для Arduino

Обзор

MPU6050 — 6-осевой IMU (3-осевой акселерометр + 3-осевой гироскоп) на I2C. Два практических примера: 3D-визуализация ориентации в Processing и самстабилизирующаяся платформа (DIY Gimbal) на 3 сервоприводах.

Как это работает

• Гироскоп — измеряет угловую скорость (°/с) по осям X,Y,Z. Интегрируя во времени, получаем угол. Дрифтит со временем.
• Акселерометр — измеряет ускорение (в т.ч. гравитацию). По проекциям g на оси вычисляет наклон (roll, pitch). Шумный, но не дрифтит.
• Сенсорная фузия (Complementary Filter): 96% гироскоп (краткосрочная точность) + 4% акселерометр (долгосрочная коррекция дрифта). Yaw (рыскание) по акселерометру не вычисляется — дрифтит.

Подключение и базовый код (сырые данные + фузия)

I2C: SDA → A4, SCL → A5 (Uno/Nano), VCC → 3.3V/5V, GND → GND. Адрес 0x68 (AD0=GND) или 0x69 (AD0=VCC).

#include <Wire.h>
const int MPU = 0x68;
float AccX, AccY, AccZ, 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();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0x00); // Сброс (PWR_MGMT_1)
  Wire.endTransmission(true);
  // Опционально: настройка диапазонов (0x1C, 0x1B) — закомментировано, используем дефолт ±2g / ±250°/s
  calculate_IMU_error(); // Калибровка ошибок
  delay(20);
}

void loop() {
  // === Акселерометр ===
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  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;

  accAngleX = atan(AccY / sqrt(AccX*AccX + AccZ*AccZ)) * 180/PI - 0.58; // -AccErrorX
  accAngleY = atan(-AccX / sqrt(AccY*AccY + AccZ*AccZ)) * 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;

  // Коррекция ошибок (получены из calculate_IMU_error)
  GyroX += 0.56; GyroY -= 2.0; GyroZ += 0.79;

  gyroAngleX += GyroX * elapsedTime;
  gyroAngleY += GyroY * elapsedTime;
  yaw        += GyroZ * elapsedTime;

  // Complementary Filter
  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(AccX*AccX + AccZ*AccZ)) * 180/PI;
    AccErrorY += atan(-AccX / sqrt(AccY*AccY + AccZ*AccZ)) * 180/PI;
    c++;
  }
  AccErrorX /= 200; AccErrorY /= 200; c = 0;

  // Гироскоп: 200 замеров в покое
  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);
}

3D-визуализация в Processing (Complementary Filter)

Arduino шлёт roll/pitch/yaw через Serial. Processing читает, вращает 3D-коробку.

import processing.serial.*;
Serial myPort; String data=""; float roll, pitch, yaw;

void setup() {
  size(960, 640, P3D);
  myPort = new Serial(this, "COM7", 19200); // Ваш COM-порт
  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));
  fill(0, 76, 153); box(386, 40, 200);
  fill(255); textSize(25); 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]);
    }
  }
}

Yaw дрифтит — нужен магнетометр или DMP.

DMP (Digital Motion Processor) — встроенная фузия без дрифта Yaw

MPU6050 имеет DMP — аппаратный процессор движений. Выдаёт кватернионы (нет Gimbal lock), точный yaw. Требует:

• Дополнительный провод: INT (прерывание) → Arduino D2;
• Библиотека i2cdevlib (Jeff Rowberg) + пример MPU6050_DMP6.

Processing-скетч для DMP (OUTPUT_TEAPOT) — аналогичен, но парсит кватернионы.

Второй пример: DIY Gimbal (самстабилизирующаяся платформа)

Вынесено в отдельную статью. Управление 3 сервами по roll/pitch/yaw для удержания платформы горизонтально.

Заключение

MPU6050 — мощный и дешёвый IMU. Базовый код с комплементарным фильтром даёт хорошие roll/pitch. Для профессиональной ориентации (дроны, VR) используйте DMP (кватернионы) или добавьте магнетометр (MPU9250). Библиотека i2cdevlib упрощает работу с DMP.