Hoy vamos a ver cómo combinar las mediciones de distintos sensores IMU (unidad de medición inercial) para crear un sistema AHRS que nos proporcione la orientación de un sensor, robot, o vehículo en un procesador como Arduino.
Hemos hablado mucho en el blog sobre IMUs, dispositivos que combinan acelerómetros, giroscopios, brújulas magnéticas y/o barómetros para obtener sensores de 3, 6, 9 o 10 grados de libertad.
Así vimos en su día las entradas básicas Cómo usar un acelerómetro con Arduino, Cómo usar un giroscopio con Arduino y Medir la inclinación con IMU.
Lógicamente, el objetivo último y más deseado de este tipo de sensores es crear un sistema AHRS (Attitude Heading Reference Systems), es decir, un sensor que nos diga la orientación absoluta en los tres ejes X, Y, Z del acelerómetro.
Dejando un momento la altitud (que solo es relevante para grandes cambios de altura, es decir, básicamente para drones), obtener la orientación es una necesidad muy frecuente y nos permite realizar un montón de proyectos interesantes.
Algunos de ellos son participar en la sensórica del sistema de guiado de un robot, hacer un estabilizador, por ejemplo de una cámara o una plataforma, sistemas que se autoequilibran, entre un montón de proyectos de los cuales podréis encontrar muchos ejemplos simplemente buscando en youtube.
Para ver que el funcionamiento es correcto, otro ejemplo inicial muy habitual es emplear la visualización 3D, de un cubo o prisma, que se mueva en la misma orientación en la pantalla que la que ponemos girando el sensor.
Precisamente esto es lo que vamos a hacer en esta entrada,montar un sistema AHRS con un sensor IMU 9DOF y visualizar las mediciones en el ordenador en un programa hecho en Processing. Pues ala, vamos a por ese cubito, para lo cual necesitaremos la librería RTIMULib-Arduino.
La librería RTIMULib-Arduino
Cómo vimos en las entradas básicas, obtener la orientación y un AHRS requiere combinar las mediciones de los tres sensores individuales. También vimos que esto es debido a que cada tipo se sensor tiene sus propias ventajas y desventajas, y aporta “algo” (velocidad, estabilidad, etc).
Para hacer un sistema AHRS necesitaremos combinar las medidas aplicando algún tipo de filtro. En general, se usa algún tipo de filtro de Kalman, o versiones simplificadas del mismo. Un ejemplo de librería que implementa este tipo de filtrado es la Adafruit_AHRS https://github.com/adafruit/Adafruit_AHRS/
En general, este filtro es un algoritmo algo complejo y pesado, por lo que, aunque existen varias librerías la mayoría ocupan más espacio de las que dispone un Atmega328P, por lo que no pueden usarse en Arduino Uno, Micro, o Pro, y necesitaremos algo superior tipo Due.
Afortunadamente, una de las mejores librerías (probablemente la mejor) para realizar un AHRS en Arduino es la RTIMULib-Arduino. Que, además de ser una de las que mejores resultados da, cabe incluso en un Atmega328p, consumiendo aproximadamente un 56% del espacio disponible.
La librería RTIMULib-Arduino es compatible con una gran variedad de IMUs, que incluyen los más habituales en nuestros proyectos como la MPU-6050, MPU-9150, MPU-9250, L3GD20H + LSM303D y BNO055.
RTIMULib-Arduino es Open Source y su código original estaba en el repositorio https://github.com/luisllamasbinaburo/RTIMULib-Arduino, que ha sido eliminado, así como el blog donde se contaban las novedades del mismo.
Afortunadamente, se conversan copias de la misma, y aquí tenéis un clon del repositorio https://github.com/luisllamasbinaburo/RTIMULib-Arduino donde podéis descargar (o contribuir) con esta genial librería.
Usar la librería RTIMULib-Arduino
Configurar la librería
Para usar la librería RTIMULib-Arduino en primer lugar debemos configurar el sensor que estemos empleando y, si es necesario, su dirección I2C. Para ello tenemos que entrar en el fichero “RTIMULibDefs.h” de la librería y descomentar el sensor que estemos empleando.
Calibrar IMU
El siguiente paso es calibrar el magnetómetro del IMU. Deberemos repetir este proceso cuando cambiemos de sensor (incluso aunque sea del mismo tipo). Para ello, cargamos el siguiente programa.
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "CalLib.h"
#include <EEPROM.h>
RTIMU *imu; // the IMU object
RTIMUSettings settings; // the settings object
CALLIB_DATA calData; // the calibration data
// SERIAL_PORT_SPEED defines the speed to use for the debug serial port
#define SERIAL_PORT_SPEED 115200
void setup()
{
calLibRead(0, &calData); // pick up existing mag data if there
calData.magValid = false;
for (int i = 0; i < 3; i++) {
calData.magMin[i] = 10000000; // init mag cal data
calData.magMax[i] = -10000000;
}
Serial.begin(SERIAL_PORT_SPEED);
Serial.println("ArduinoMagCal starting");
Serial.println("Enter s to save current data to EEPROM");
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
imu->IMUInit();
imu->setCalibrationMode(true); // make sure we get raw data
Serial.print("ArduinoIMU calibrating device "); Serial.println(imu->IMUName());
}
void loop()
{
boolean changed;
RTVector3 mag;
if (imu->IMURead()) { // get the latest data
changed = false;
mag = imu->getCompass();
for (int i = 0; i < 3; i++) {
if (mag.data(i) < calData.magMin[i]) {
calData.magMin[i] = mag.data(i);
changed = true;
}
if (mag.data(i) > calData.magMax[i]) {
calData.magMax[i] = mag.data(i);
changed = true;
}
}
if (changed) {
Serial.println("-------");
Serial.print("minX: "); Serial.print(calData.magMin[0]);
Serial.print(" maxX: "); Serial.print(calData.magMax[0]); Serial.println();
Serial.print("minY: "); Serial.print(calData.magMin[1]);
Serial.print(" maxY: "); Serial.print(calData.magMax[1]); Serial.println();
Serial.print("minZ: "); Serial.print(calData.magMin[2]);
Serial.print(" maxZ: "); Serial.print(calData.magMax[2]); Serial.println();
}
}
if (Serial.available()) {
if (Serial.read() == 's') { // save the data
calData.magValid = true;
calLibWrite(0, &calData);
Serial.print("Mag cal data saved for device "); Serial.println(imu->IMUName());
}
}
}Con el programa cargado debemos girar el sensor en los 3 ejes, de forma que abarquemos todo el ángulo de movimiento posible para que el programa registre el rango del campo magnético medido.
Cada vez que el sensor amplía el rango lo muestra por puerto serie. De forma que el proceso finaliza cuando, por mucho que movamos el sensor, no aparecen nuevas líneas.
Cuando hayamos finalizado el proceso, pulsamos la tecla ‘Y’ para que los valores registrados se graben en la EEPROM. De esta forma estarán disponibles aunque reiniciemos aunque desconectemos Arduino.
Obtener mediciones
Con el magnetómetro calibrado, ya podemos empezar a obtener mediciones del IMU con la medición de los tres sensores combinadas con el algoritmo de fusión. Para ello, cargamos el siguiente programa.
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>
RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
#define DISPLAY_INTERVAL 100
#define SERIAL_PORT_SPEED 115200
unsigned long lastDisplay;
void setup()
{
Serial.begin(SERIAL_PORT_SPEED);
Wire.begin();
imu = RTIMU::createIMU(&settings);
Serial.print("ArduinoIMU starting using device ");
Serial.println(imu->IMUName());
int errcode;
if ((errcode = imu->IMUInit()) < 0) {
Serial.print("Failed to init IMU: "); Serial.println(errcode);
}
if (imu->getCalibrationValid())
Serial.println("Using compass calibration");
else
Serial.println("No valid compass calibration data");
fusion.setSlerpPower(0.02);
fusion.setGyroEnable(true);
fusion.setAccelEnable(true);
fusion.setCompassEnable(true);
}
void loop()
{
unsigned long now = millis();
unsigned long delta;
int loopCount = 1;
while (imu->IMURead()) {
if (++loopCount >= 10) // this flushes remaining data in case we are falling behind
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
Serial.print(((RTVector3&)fusion.getFusionPose()).y() * RTMATH_RAD_TO_DEGREE);
Serial.print(';');
Serial.print(((RTVector3&)fusion.getFusionPose()).x() * RTMATH_RAD_TO_DEGREE);
Serial.print(';');
Serial.println(((RTVector3&)fusion.getFusionPose()).z() * RTMATH_RAD_TO_DEGREE);
}
}
}Al ejecutarlo, veremos que se muestran los ángulos de Euler de la orientación medida por el IMU.
Mostrar resultados en processing
Y llegamos a la parte más “vistosa”, que como decíamos era comprobar el funcionamiento del IMU y de la librería graficando los ángulos de Euler en un programa. Por simplicidad, lo vamos a hacer en processing, aunque por supuesto podríamos hacerlo en cualquier otro lenguaje.
Aquí tenéis un ejemplo para mostrar un prisma que muestra la orientación registrada por el IMU. Recordad cambiar el puerto “COM4” por el que estéis usando vosotros.
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;
Serial arduinoPort;
float roll, pitch, yaw;
void setup()
{
size (1280, 768, P3D);
arduinoPort = new Serial(this, "COM4", 115200);
arduinoPort.bufferUntil('\n');
}
void draw()
{
drawBackground();
rotateY(radians(-yaw));
rotateX(radians(-pitch));
rotateZ(radians(-roll));
drawCube();
}
void drawBackground() {
translate(width/2, height/2, 0);
background(#374046);
fill(200, 200, 200);
textSize(32);
text("Roll: " + int(roll) + " Pitch: " + int(pitch) + " Yaw: " + int(yaw) , -200, -320);
}
void drawCube() {
strokeWeight(1);
stroke(255, 255, 255);
fill(#607D8B);
box (500, 40, 300);
strokeWeight(4);
stroke(#E81E63);
line(0, 0, 0, 300, 0, 0);
stroke(#2196F2);
line(0, 0, 0, 0, -70, 0);
stroke(#8BC24A);
line(0, 0, 0, 0, 0, 200);
}
void serialEvent (Serial myPort) {
String data = myPort.readStringUntil('\n');
if (data != null) {
data = trim(data);
String items[] = split(data, ';');
if (items.length > 1) {
roll = float(items[0]);
pitch = float(items[1]);
yaw = float(items[2]);
}
}
}Tenéis el código del visualizador en Processing disponible en Github en este enlace. Dependiendo del IMU que estéis usando es posible que tengáis que ajustar los ángulos (cambiar el orden, invertir la salida).
El resultado es lo podéis ver en el siguiente vídeo.
Descarga el código
Todo el código de esta entrada está disponible para su descarga en Github. 