IMU MPU6050 + Arduino

La Unidad de Medición Inercial o IMU , su abreviación en inglés, en este caso el MPU6050 consta de dos unidades integradas:

un Giroscopio
un Acelerómetro

Relacionando las medidas de ambos podemos determinar los grados de movimiento que sufre el sensor y a lo que esté sujeto el mismo.

El siguiente artículo pretende mostrar las diferentes partes del código para arduino, su utilidad y función dentro de los cálculos a realizar.

El siguiente código toma los datos obtenidos del giroscopio y acelerómetro y a través de diversos cálculos matemáticos y físicos que son los datos en crudo , poder pasarlos a grados. Se pasan a grados ya que como objetivo a futuro este código será parte de un estabilizador controlado por servomotores.

Más allá de la futura utilización entendiendo sus partes seguramente podrán utilizarlo para otros objetivos cambiando, sacando y/o agregando código para poder tomar los datos que ustedes necesiten.

A tener en cuenta.

Debido a la abundancia de información que recolectan los sensores y la forma de determinar variaciones en los ejes tanto por rotación como por giro se generan datos que podríamos llamar falsos o de error que para este tipo de actividad se conoce como ruido. Mientras mayor sea el flujo de datos tomados y mayor sea el tiempo, mayor será también el margen de error o ruido.

Hay diferentes maneras matemáticas de filtrado en esta ocasión se utilizará uno bastante común para no complicar las cosas.

Aunque aquellos que entiendan bien las matemáticas y coordenadas cartesianas pueden buscar el mejor filtrado utilizado por ejemplo para naves espaciales, misiles etc. que es el filtro de Kalman.

Básicamente con el filtro lo que hacemos es muestrear una cierta cantidad de datos y promediarlos y a ese resultado sumarlo o restarlo al resultado total que arroja cada eje.

Cada IMU es diferente por lo que se requiere chequear cada MPU6050 para obtener el margen de error para un IMU en particular. El chequeo está incluido en el código dado, se debe colocar el MPU en posición correcta y estable sobre una superficie plana.

En la imagen pueden ver de qué manera están los ejes. Tanto el giroscopio como el acelerómetro comparten la misma distribución.

En cuanto al giro o rotación sobre los ejes se desprenden los siguientes términos:

YAW: Es el giro sobre el eje Z
PITCH: Es el giro sobre el eje Y
ROLL: Es el giro sobre el eje X

Conexión MPU6050 y Arduino

El Código.

#include <Wire.h>

const int MPU = 0x68; // MPU6050 I2C direccion

float AceX, AceY, AceZ;

float GiroX, GiroY, GiroZ;

float aceAnguloX, aceAnguloY, giroAnguloX, giroAnguloY, giroAnguloZ;

float roll, pitch, yaw;

float AceErrorX, AceErrorY, GiroErrorX, GiroErrorY, GiroErrorZ;

float tiempoTranscurrido, tiempoActual, tiempoAnterior;

int c = 0;

void setup() {

Serial.begin(19200);

Wire.begin();

Wire.beginTransmission(MPU); // Iniciar comunicacion MPU6050 // MPU=0x68

Wire.write(0x6B); // registro 6B

Wire.write(0x00); // colocamos 0 en el registro 6B

Wire.endTransmission(true); // finalizamos la transmision

// Usamos la siguiente funcion para obtener los errores del IMU de nuestro modulo

calcular_IMU_error();

delay(20);

}

void loop() {

// Lectura de los datos del acelerometro

Wire.beginTransmission(MPU);

Wire.write(0x3B); // Iniciamos con el registro 0x3B (ACCEL_XOUT_H)

Wire.endTransmission(false);

Wire.requestFrom(MPU, 6, true); // Se leen 6 registros en total, cada valor de eje es almadcenado en 2 registros

// Para el rango de +-2g, necesitamos dividir los valores en bruto por 16384 de acuerdo con el datasheet

AceX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Valor Eje-X

AceY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Valor Eje-Y

AceZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Valor Eje-Z

// Calculating Roll and Pitch from the accelerometer data

// Calculando los datos de Roll y Pitch del acelerometro

aceAnguloX = (atan(AceY / sqrt(pow(AceX, 2) + pow(AceZ, 2))) * 180 / PI) - 0.58;

aceAnguloY = (atan(-1 * AceX / sqrt(pow(AceY, 2) + pow(AceZ, 2))) * 180 / PI) + 1.58;

// Lectura de los datos del giroscopio

tiempoAnterior = tiempoActual; // Guardamos el tiempo anterior antes que el tiempo actual sea leido

tiempoActual = millis(); // Tomamos y almacenamos el tiempo actual

tiempoTranscurrido = (tiempoActual - tiempoAnterior) / 1000; // Dividido por 1000 para obtener los segundos

Wire.beginTransmission(MPU);

Wire.write(0x43); // Dato de Gyro primer registro en la direccion 0x43

Wire.endTransmission(false);

Wire.requestFrom(MPU, 6, true); // Leemos 4 registros en total, cada valor de eje es almacenado en 2 registros

GiroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Para el rango 250deg/s tenemos que dividir primero los datos en bruto por 131.0 de acuerdo con el datasheet

GiroY = (Wire.read() << 8 | Wire.read()) / 131.0; // Para el rango 250deg/s tenemos que dividir primero los datos en bruto por 131.0 de acuerdo con el datasheet

GiroZ = (Wire.read() << 8 | Wire.read()) / 131.0; // Para el rango 250deg/s tenemos que dividir primero los datos en bruto por 131.0 de acuerdo con el datasheet

// Realizamos la correcion en las salidas de acuerdo con los valores de error calculados

GiroX = GiroX + 0.56; // GiroErrorX ~(-0.56)

GiroY = GiroY - 2; // GiroErrorY ~(2)

GiroZ = GiroZ + 0.79; // GiroErrorZ ~ (-0.8)

// Actualmente los valores en bruto son valores en grados por segundo, deg/s, necesitamos multiplicarlo por segundos para obtener los angulos en grados

giroAnguloX = giroAnguloX + GiroX * tiempoTranscurrido; // deg/s * s = deg

giroAnguloY = giroAnguloY + GiroY * tiempoTranscurrido;

yaw = yaw + GiroZ * tiempoTranscurrido;

// En este punto hay que realizar un filtrado ya que los datos y la forma en que los tratamos acumulan errores o datos falsos

// de manera exponencial. Hay muchos tipos de filtros, este es el más sencillo.

// La forma de filtrado más compleja y esacta que por ejemplo se utiliza en la NASA para naves espaciales o en misiles balisticos son los filtros de Kalman.

// Filtrado complemetario que combina los datos del acelerometro y giroscopio

roll = 0.96 * giroAnguloX + 0.04 * aceAnguloX;

pitch = 0.96 * giroAnguloY + 0.04 * aceAnguloY;

// Imprimimos los valores en el monitor serie

Serial.print(roll);

Serial.print("/");

Serial.print(pitch);

Serial.print("/");

Serial.println(yaw);

}

void calcular_IMU_error() {

// Llamamos a esta funcion al inicio en el setup para calcular los datos erroneos del acelerometro y el giroscopio. Con esto podemos obtener los valores de error

// y utilizarlos en la ecuciones mas abajo e imprimirlas en el monitor.

// Para una correcta lectura de los valores tenemos que pocisionar el IMU en una superficie plana asi obtenemos lecturas correctas.

// Leemos 200 valores del acelerometro

while (c < 200) {

Wire.beginTransmission(MPU);

Wire.write(0x3B);

Wire.endTransmission(false);

Wire.requestFrom(MPU, 6, true);

AceX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

AceY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

AceZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

// Sumamos todas las lecturas

AceErrorX = AceErrorX + ((atan((AceY) / sqrt(pow((AceX), 2) + pow((AceZ), 2))) * 180 / PI));

AceErrorY = AceErrorY + ((atan(-1 * (AceX) / sqrt(pow((AceY), 2) + pow((AceZ), 2))) * 180 / PI));

c++;

}

// Dividimos la suma por 200 para sacar un promedio y obtener el valor de error

AceErrorX = AceErrorX / 200;

AceErrorY = AceErrorY / 200;

c = 0;

// Lo mismo pero con el giroscopio

// Leemos 200 valores del giroscopio

while (c < 200) {

Wire.beginTransmission(MPU);

Wire.write(0x43);

Wire.endTransmission(false);

Wire.requestFrom(MPU, 6, true);

GiroX = Wire.read() << 8 | Wire.read();

GiroY = Wire.read() << 8 | Wire.read();

GiroZ = Wire.read() << 8 | Wire.read();

// Sumamos todas las lecturas

GiroErrorX = GiroErrorX + (GiroX / 131.0);

GiroErrorY = GiroErrorY + (GiroY / 131.0);

GiroErrorZ = GiroErrorZ + (GiroZ / 131.0);

c++;

}

// Dividimos la suma por 200 para sacar un promedio y obtener el valor de error

GiroErrorX = GiroErrorX / 200;

GiroErrorY = GiroErrorY / 200;

GiroErrorZ = GiroErrorZ / 200;

// Imprimimos en el monitor serie

Serial.print("AceErrorX: ");

Serial.println(AceErrorX);

Serial.print("AceErrorY: ");

Serial.println(AceErrorY);

Serial.print("GiroErrorX: ");

Serial.println(GiroErrorX);

Serial.print("GiroErrorY: ");

Serial.println(GiroErrorY);

Serial.print("GiroErrorZ: ");

Serial.println(GiroErrorZ);

}

Como resultado veremos lo siguiente:

Las variables de los ejes X, Y y Z que contienen el resultado final son: roll, yaw y pitch en el código.

#arduino #electrónica #albertoalbertos #giroscopio #acelerómetro #MPU6050

Page updated

Google Sites

Report abuse