Leer el ángulo de inclinación con el MPU6050

Introducción

El MPU6050 es la respuesta barata y honesta a «¿dónde está arriba?». Contiene dos sensores independientes en un chip —un acelerómetro de tres ejes y un giroscopio de tres ejes— y un robot autoequilibrado necesita ambos, porque cada uno es inútil por sí solo de una forma que el otro compensa exactamente.

Este tutorial te lleva de un módulo pelado a un ángulo de inclinación en grados en el que puedes confiar mientras el robot está quieto. Hacer que ese ángulo sobreviva a que el robot se mueva es el filtro complementario, que es el siguiente tutorial y depende de todo lo de aquí.

Lo que mide realmente cada sensor

Un acelerómetro no mide la inclinación. Mide aceleración, incluida la aceleración debida a la gravedad, que es por lo que funciona como sensor de inclinación: quieto, la única aceleración es 1 g hacia abajo, y cómo se reparte ese vector entre los ejes indica tu orientación.

Ese «quieto» es la trampa. El sensor no puede distinguir la gravedad de cualquier otra aceleración. En cuanto las ruedas de tu robot se mueven, el acelerómetro informa la suma vectorial de la gravedad y el propio movimiento del robot, y lee una inclinación que no existe. En un robot equilibrista —que por definición nunca deja de acelerar— este error es continuo.

Un giroscopio mide velocidad angular, en grados por segundo. Integra la velocidad en el tiempo y obtienes un ángulo. Los giróscopos son inmunes a la aceleración lineal, así que siguen siendo honestos mientras el robot se mueve.

Su debilidad es que la integración acumula error. Cualquier pequeño sesgo constante en la lectura de velocidad se integra en un ángulo que crece sin límite. Un giróscopo que lee 0,5 °/s estando perfectamente quieto afirmará que has girado 30° al cabo de un minuto. Esto es la deriva, y no se arregla soldando mejor.

Así que: el acelerómetro tiene razón a largo plazo pero se equivoca en cada instante; el giróscopo tiene razón en cada instante pero se equivoca a largo plazo. Esa simetría es toda la razón por la que existe la fusión de sensores.

Cableado

El MPU6050 habla I²C, así que necesita cuatro cables —y si el escáner no lo encuentra en 0x68, esa página explica por qué su hoja de datos dice 0xD0.

En un robot equilibrista, monta el módulo de forma que uno de sus ejes quede realmente horizontal y lo más cerca posible del eje de las ruedas. Montarlo alto multiplica la aceleración lineal que ve durante una corrección, lo que dificulta la fusión sin ningún beneficio.

Leer los valores en bruto

Los registros que necesitas son consecutivos, así que una sola lectura en ráfaga lo obtiene todo:

#include <Wire.h>

const uint8_t MPU = 0x68;        // 0x69 if AD0 is tied high

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);              // PWR_MGMT_1
  Wire.write(0);                 // clear sleep bit — it boots asleep
  Wire.endTransmission(true);
}

void readRaw(int16_t& ax, int16_t& ay, int16_t& az,
             int16_t& gx, int16_t& gy, int16_t& gz) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);              // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();      // temperature, discarded
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

El módulo arranca en modo reposo. Olvidar de limpiar el bit de reposo de PWR_MGMT_1 devuelve un flujo constante de ceros, que se lee exactamente igual que un fallo de cableado y manda a la gente de vuelta al soldador sin ningún motivo.

Convertir cuentas a unidades

Los valores brutos son cuentas de 16 bits con signo sobre el rango configurado del sensor. Con los ajustes por defecto:

• Acelerómetro: ±2 g sobre ±32768 cuentas → 16384 cuentas por g
• Giróscopo: ±250 °/s sobre ±32768 cuentas → 131 cuentas por °/s

float accelX = ax / 16384.0;     // g
float rateY  = gy / 131.0;       // grados por segundo

Inclinación desde el acelerómetro

Con el robot vertical y la placa montada en vertical, el ángulo de inclinación en el plano en que cae el robot sale de dos de los tres ejes:

float pitch = atan2(accelX, accelZ) * 180.0 / PI;

Usa atan2, no asin de un solo eje. atan2 toma ambas componentes, así que sigue siendo correcto en los cuatro cuadrantes y no explota cuando un eje se acerca a cero. Una versión con asin de un solo eje parece correcta en el banco con ángulos pequeños y luego se comporta de forma extraña la primera vez que el robot se inclina más de 45°.

Calibrar los offsets

Cada MPU6050 tiene un offset de fabricación. Tu giróscopo no leerá cero estando quieto, y tu acelerómetro no leerá exactamente 1 g en un eje y cero en los demás. Mide el error una vez al arrancar, con el robot sostenido quieto:

float gyroBiasY = 0;

void calibrate() {
  const int samples = 1000;
  long total = 0;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRaw(ax, ay, az, gx, gy, gz);
    total += gy;
    delay(2);
  }
  gyroBiasY = (total / (float)samples) / 131.0;
}

Resta gyroBiasY de cada lectura de velocidad posterior. Este único paso es la diferencia entre un giróscopo que deriva un grado por minuto y uno que deriva un grado por segundo.

Dos cosas que debes saber. El sensor debe estar realmente quieto durante la calibración —un robot que calibra mientras lo sostienes grabará el temblor de tu mano en el sesgo. Y el sesgo cambia con la temperatura, así que un robot calibrado en frío derivará un poco a medida que la electrónica se calienta. Para un robot equilibrista el término integral del bucle de control absorbe ese residuo, que es una de las razones por las que los controladores de equilibrio casi siempre necesitan un término I.

El ajuste de montaje no es lo mismo que el sesgo del sensor

Hay un segundo offset que la calibración no puede eliminar: tu IMU no está montada de forma perfecta. Si la placa queda 1,5° fuera de la vertical real, un sensor perfectamente calibrado seguirá informando 1,5° cuando el robot esté realmente vertical.

Esto importa enormemente en un robot equilibrista, y es invisible en el banco. El bucle de control mantendrá fielmente el ángulo que le digas que es vertical —así que un error de 1,5° significa que el robot mantiene una inclinación permanente de 1,5°, lo que significa aceleración constante, lo que significa que cruza la sala hasta chocar con algo.

Mídelo poniendo el robot de pie a mano, realmente equilibrado, y anotando lo que lee el ángulo. Resta ese número como constante. Puedes ver este fallo exacto en el simulador de equilibrio —el deslizador de ajuste inyecta un error de montaje y el robot se aleja insistiendo en que está nivelado.

Verifica antes de construir sobre ello

Imprime el ángulo y comprueba tres cosas a mano:

1. En vertical lee cerca de cero. Si no, tienes ajuste de montaje que restar.
2. Inclinar el robot hacia adelante aumenta el ángulo, y hacia atrás lo disminuye. Si el signo está invertido, cámbialo aquí en lugar de compensarlo en el bucle de control, donde te confundirá después.
3. La lectura es estable en reposo, e inmediata al moverlo. Ruidosa en reposo significa revisar cableado y alimentación; con retardo significa que estás promediando con demasiada agresividad en algún punto.

Una vez que el ángulo es fiable en reposo, estás listo para hacerlo fiable en movimiento con el filtro complementario, y luego para cerrar el bucle en control de equilibrio PID en cascada.

El módulo arranca dormido. Si olvidas borrar el bit de sueño en PWR_MGMT_1 obtendrás un flujo constante de ceros, que se lee exactamente como un fallo de cableado y te devuelve al soldador sin motivo.