Tutorial Módulo Acelerómetro y giroscopio
Arduino > shelds
Es obra de Transductor de http://robologs.net/
Tutorial de Arduino y MPU-6050
Saludos, humano. Al habla Transductor. ¿Alguna vez has intentado configurar una IMU con Arduino y obtener lecturas? Si tu respuesta es afirmativa, habrás notado que no es nada fácil. Muchas IMUs utilizan protocolos como I2C o SPI, dan valores extraños, requieren trigonometría… oh, ¿he mencionado ya que las IMUs dan siempre lecturas erróneas?
Si has acabado en esta página es que necesitas un empujón. Sabes que una IMU es un dispositivo electrónico. Y que no se come. Pero ahí se acabó. Probablemente no tienes ni idea de cómo conectarla a Arduino y ya no digamos obtener una lectura decente.
Así pues, has venido al lugar indicado. Transductor va a explicarte cómo conseguir unas lecturas precisas con una IMU MPU-6050 y Arduino. Depuraremos los valores, calcularemos el ángulo de inclinación y filtraremos el ruido con un Filtro Complementario (Complementary Filter).
Pero hay que empezar por el principio. Y el principio es…
Da Theory
Antes de empezar, ¿qué es una IMU? Se trata de un dispositivo capaz de mesurar la fuerza (acceleración) y la velocidad. Genéricamente consta de un Acelerómetro y unGiroscopio. Por lo tanto: una IMU no mesura ángulos. Por lo menos no directamente, requiere algunos cálculos.
El MPU-6050 es una IMU de 6DOF (se lee “6 Degrees Of Freedom“). Esto significa que lleva un acelerómetro y un giroscopio, ambos de 3 ejes (3+3 = 6DOF). Hay IMUs de 9DOF, en este caso también llevan un magnetómetro. Otras pueden tener 5DOF, en cuyo caso el giroscopio sólo mide dos ejes, etc.
MPU-6050
El MPU-6050 opera con 3.3 voltios, aunque algunas versiones llevan un regulador que permite conectarla a 5V.
El MPU-6050 utiliza el protocolo de comunicación I2C. En este tutorial no voy a entrar en detalles de cómo funciona este protocolo, pero puedes encontrar más información aquí.
Ya tienes claro lo que es una IMU. Veamos cada una de sus partes.
El acelerómetro
El acelerómetro mide la aceleración. Quién iba a decirlo. La aceleración puede expresarse en 3 ejes: X, Y y Z, las tres dimensiones del espacio. Por ejemplo, si mueves la IMU hacia arriba, el eje Z marcará un cierto valor. Si es hacia delante, marcará el eje X, etc.
Si haces memoria de tus clases de la ESO (o equivalentes), recordarás que la gravedad de la Tierra tiene una aceleración de aprox. 9.8 m/s², perpendicular al suelo como es lógico. Así pues, la IMU también detecta la aceleración de la gravedad terrestre.
¿Esto es un problema? ¡Al contrario, lector! Gracias a la gravedad terrestre puedes usar las lecturas del acelerómetro para saber cuál es el ángulo de inclinación respecto al eje X o eje Y.
Supongamos que la IMU esté perfectamente alineada con el suelo. Entonces, como puedes ver en la imagen, el eje Z marcará 9.8, y los otros dos ejes marcarán 0. Ahora supongamos que giramos la IMU 90 grados. Ahora es el eje X el que está perpendicular al suelo, por lo tanto marcará la aceleración de la gravedad.
Si sabemos que la gravedad es 9.8 m/s², y sabemos que mesura dan los tres ejes del acelerometro, por trigonometría es posible calcular el ángulo de inclinación de la IMU. Una buena fórmula para calcular el ángulo es:
El eje Z se suele ignorar.
Hay otras fórmulas equivalentes, pero yo prefiero la tangente.
El Giroscopio
En un principio, los giroscopios eléctricos eran unos voluminosos artefactos que valían la mayor parte del presupuesto militar de un estado. Más tarde, durante la segunda guerra mundial se emplearon para dirigir cohetes y torpedos. Por suerte, gracias la revolución digital y la miniaturización de circuitos, hoy en día cualquier aficionado a la electrónica puede permitirse uno. Aunque no para construir misiles.
Los giroscopios eléctricos se utilizaron en los cohetes V-2
El giroscopio mide la velocidad angular. Si no tienes muy frescas tus lecciones de física del instituto voy a recordarte que la velocidad angular es el número de grados que se gira en un segundo.
Sólo que en vez de mesurarse en grados por segundo, suele mesurarse en otra unidad que son radianes por segundo (1rad/s = 180/PI grados/s)
Si sabemos el ángulo inicial de la IMU, podemos sumarle el valor que marca el giroscopio para saber el nuevo ángulo a cada momento. Supongamos que iniciamos la IMU a 0º. Si el giroscopio realiza una medida cada segundo, y marca 3 en el eje X, tendremos el ángulo con esta sencilla fórmula:
Dónde Δt es el tiempo que transcurre cada vez que se calcula esta fórmula.
Y lo mismo pasa con los ejes X, Z. Sólo que al igual que con el acelerómetro se suele ignorar el eje Z.
Ya tenemos las lecturas. ¿Simple, verdad? Pues en realidad no tanto.
Error en las medidas
En un mundo ideal donde hadas y elfos cantan alegres y dulces canciones alrededor de hogueras, donde los dragones surcan los cielos, los árboles hablan con las setas y C++ no provoca errores en tiempo de ejecución, las IMUs son unos artefactos mágicos que con un poco de trigonometría puden dar un ángulo con total exactitud.
Pero estás en el mundo real, hijo. Y hay dos problemas muy importantes: el ruido y los errores.
El ruido son todas aquellas interferencias que afectan a los dispositivos electrónicos. El acelerómetro es capaz de medir cualquier ángulo, sin embargo sus lecturas son ruidosas y tienen un cierto margen de error.
Si alguna vez quieres dibujar un gráfico de las medidas de un acelerómetro en función del tiempo, verás algo de este estilo:
El ángulo real (ideal) está marcado en azul, y las medidas reales están en rojo. Puedo afirmar que no cumple a rajatabla la definición de “preciso”.
Por si esto fuera poco, el acelerómetro también detecta cualquier aceleración que no sea la de la gravedad. Por tanto, si mueves la IMU sin girarla, al aplicar una aceleración en otro eje, la IMU lo detectará como un cambio de rotación.
Por otra parte tenemos el giroscopio. A diferencia del acelerómetro, da las medidas conmucha precisión. Pero al realizar los cálculos del ángulo es inevitable que se produzca un pequeño error, que con el tiempo va acomulándose hasta que cualquier similitud con la realidad es pura coincidencia. Esto en inglés se llama drift.
¿Significa esto que debes darte por vencido? Para nada. Hay una (varias) forma(s) de combinar los datos del acelerómetro y el giroscopio para así obtener medidas acuradas. Y aquí es dónde entra en juego:
Tu amigo el filtro
La idea es muy simple: hay que conseguir eliminar el ruido, el drift y conseguir que el acelerómetro no cambie de ángulo al detectar otra fuerza que no sea la gravedad.
Hay distintos algoritmos, llamados filtros, que hacen esta tarea. Uno de los mejores es el famoso Filtro de Kálman, del que puede que hayas oído a hablar. Se utiliza en losaviones, cohetes y satélites geoestacionarios.
El filtro de Kálman es sorprendente. Es considerado uno de los mayores hallazgos del siglo pasado, y con razón. Capaz de calcular el error de cada medida a partir de las medidas anteriores, eliminarlo y dar el valor real del ángulo. En cierto modo es un algoritmo que aprende en cada iteración. No suena nada mal, ¿verdad?
Sin embargo tiene dos problemas:
- Tiene un coste de procesamiento algo elevado
- Es muy complicado de entender. Y pongo énfasis en el “muy”. Échale un vistazo.
Así pues, tenemos otros filtros a nuestra disposición. El que vamos a utilizar es conocido como Filtro Complementario o Complementary Filter en inglés. Es ideal para implementar con Arduino: fácil de utilizar, bajo coste de procesamiento y con una precisión muy buena.
¿En qué consiste exactamente? El Filtro Complementario es en realidad una unión de dos filtros diferentes: un High-pass Filter para el giroscopio y un Low-pass Filter para al acelerómetro. El primero deja pasar únicamente los valores por encima de un cierto límite, al contrario que el Low-pass filter, que sólo permite a los que están por debajo.
La fórmula resultante de combinar (complementar, de aquí el nombre) los dos filtros es:
Dónde AnguloGyro es el ángulo del Giroscopio que hemos calculado previamente, yAnguloAccel con el Acelerómetro. Esta fórmula es la misma para el eje X, Y.
Y así se termina Da Theory. Ahora hay que utilizar las fórmulas en un código para Arduino.
In Praxis
Las fórmulas y los números viven tranquilas en el abstracto mundo de las matemáticas. A pesar de que este lugar está custodiado por unos seres divinos llamados matemáticos, cuya función es asegurarse de que sólo los licenciados puedan utilizar las fórmulas en el mundo real, vamos a implementar estos algoritmos en una placa Arduino.
Las conexiones son muy simples. Con los 5V del USB tendrás energía de sobra.
- MPU Vcc -> Arduino 3.3v (o 3v3 en algunos modelos)
- MPU Gnd -> Arduino Gnd
- MPU SCL -> Arduino A5
- MPU SDA -> Arduino A4
Así pues, tienes las fórmulas. Tienes la IMU. Tienes una placa Arduino. Tienes el circuito. ¿Qué falta? Saber cómo interaccionar con la IMU para obtener lecturas. El MPU-6050 dará unos raw values (“valores en bruto”) que después habrá que refinar (dividir por una constante) a fin de conseguir valores utilizables.
La web oficial de Arduino dispone de un ejemplo para leer datos de la MPU-6050. Partiré de ella, aunque habrá que modificarla considerablemente ya que sólo da los valores sin refinar del Giroscopio y el Acelerómetro.
Empezamos por las declaraciones.
#include <Wire.h>
#define MPU 0x68 //Direccion I2C de la IMU
#define A_R 16384.0 //Ratios de conversion
#define G_R 131.0
#define RAD_A_DEG = 57.295779 //Conversion de radianes a grados 180/PI
//MPU-6050 da los valores en enteros de 16 bits
//Valores sin refinar
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];
Voy a comentarlo. La primera línea incluye la librería Wire.h, necesaria para la interacción vía protocolo I2C.
#define MPU 0x68 es la dirección I2C de la IMU que se especifica en la documentación oficial.
Los ratios de conversión son los especificados en la documentación. Deberemos dividir los valores que nos dé el Giroscopio y el Acelerómetro entre estas constantes para obtener un valor coherente. RAD_A_DEG es la conversión de radianes a grados.
La IMU da los valores en enteros de 16 bits. Como Arduino los guarda en menos bits, hay que declarar las variables que almacenarán los enteros provinientes de la IMU como un tipo de enteros especiales. int16_t AcX, AcY, AcZ, GyX, GyY son, pues, los raw_values de la IMU.
Finalmente tenemos tres arrays (Acc[], Gy[], Angle[]) que guardan el ángulo X, Y del Acelerómetro, el Giroscopio y el resultado del Filtro respectivamente. [0] se corresponde a X. [1] a Y.
La función setup es la siguiente:
void setup()
{
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}
Se inicia la comunicación por I2C con el dispositivo MPU, y se “activa” enviando el comando 0. También se inicia el puerto de série para ver los resultados.
El void loop es un poco más complejo. Se leen y guardan los datos de la IMU, se calcula el ángulo y se aplica el filtro complementario.
void loop()
{
//Leer los valores del Acelerometro de la IMU
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//Se calculan los angulos Y, X respectivamente.
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
//Leer los valores del Giroscopio
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true); //A diferencia del Acelerometro, solo se piden 4 registros
GyX=Wire.read()<<8|Wire.read();
GyY=Wire.read()<<8|Wire.read();
//Calculo del angulo del Giroscopio
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
//Aplicar el Filtro Complementario
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
//Mostrar los valores por consola
Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("\n");
Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("\n------------\n");
delay(10); //Nuestra dt sera, pues, 0.010, que es el intervalo de tiempo en cada medida
}
Escribe estas tres funciones en un sólo código, compila, carga. Si ahora abres el Monitor de Série, deberías ver como van oscilando los números muy rápidamente. Puedes probar a girar la IMU y detener el Desplazamiento Automático para ver como cambian los ángulos.
El filtro tarda un tiempo a adaptarse a los cambios, lo cuál es normal al filtrar las frecuencias.
Y esto es todo. Espero que esta guía te sirva de referencia para futuros proyectos. Los filtros de este tipo suelen utilizarse conjuntamente con un Controlador PID (PID controller) para controlar y corregir el movimiento de motores en función de las lecturas de la IMU. Si estás interesado en realizar un proyecto como un drone o un robot balanceador, puede ser de utilidad comprender cómo funciona.
Finalmente debo decir que existen librerías para Arduino que incluyen Filtros de Kálman y Filtros Complementarios. ¿Por qué no las he mencionado? Mi consejo cuándo construyas tu proyecto es que, si puedes, evites las librerías de terceros. Aprender a implementar este tipo de algoritmos por tu cuenta mejorará tus conocimientos y posibilidades mucho más que si te limitas a las funciones prefabricadas.
Y al fin y al cabo, ¿qué es la robótica sin pelearse con las fórmulas, problemas y código?
Final de línea.