TecNoticias, tu portal de información

Trabajar con IMU puede enloquecer por una variedad de razones, pero lo que más asusta a la gente es generalmente la matemática.

Los datos de IMU son inútiles a menos que sepas cómo interpretarlos.

Por ejemplo, el BNO055 es un sensor 9DOF que puede proporcionar datos de aceleración, giroscopios y magnetómetro, así como datos de cuaterniones fusionados que indican orientación absoluta desde una posición inicial. Todos esos datos son completamente inútiles a menos que pueda encontrar una manera de relacionar el marco de referencia de la IMU con un marco de referencia externo y fijo.

Los datos de los sensores no calibrados son inútiles.

Y una vez que descubras cómo estabilizar los datos en un marco de referencia inercial, los ejes mal alineados pueden causar estragos en todo tu trabajo duro. Si no tiene cuidado, terminará haciendo cálculos con datos que se mezclan entre varios ejes de movimiento.

Un sensor calibrado aún debe alinearse con una referencia inercial para que los datos sean realmente útiles.

Por lo general, debe alinear su sensor con el marco de referencia inercial para poder interpretar los datos.

Una vez que su sensor esté alineado en el marco inercial, puede alinear sus datos.

La forma de hacerlo es con matrices de rotación cuyos elementos individuales se rellenan con funciones trigonométricas o cuaterniones. Se prefieren los cuaterniones porque requieren menos cálculo por parte de una MCU.

En este artículo, describiré una matriz de rotación y presentaré algunas de las matemáticas necesarias para configurar la IMU de BNO055 de Bosch para los propósitos de la estimación de cuentas. Estoy usando esta IMU porque tengo una de un artículo anterior sobre cómo capturar datos con el BNO055.

Los lectores interesados ​​pueden querer saber que Bosch reemplazó esta IMU con el BMX055. Sin embargo, las matemáticas que se presentan aquí deberían funcionar con cualquier sistema de sensores. El enfoque del artículo está en las matemáticas, no en el sensor.

Cualquier sensor que uses, debe calibrar el dispositivo de acuerdo con la hoja de datos del fabricante antes de cada uso. A veces esa calibración requiere una EEPROM externa.

Matrices de Rotación

Mover un vector en un espacio tridimensional puede ser un asunto complicado. Una técnica común utiliza rotaciones secuenciales alrededor de ejes fijos para rotar vectores.

La forma más fácil de reorientar sus vectores en una sola rotación es con una matriz de rotación. Una matriz de 3 × 3 contiene toda la información necesaria para mover un vector en una sola rotación sin utilizar trigonometría.

Los vectores de gravedad anteriores (naranja) se giran alrededor de un solo eje (verde) para situarse en el eje z negativo (azul)

Puede aprender más sobre esa técnica buscando "ángulos de Euler" o leyendo mi artículo anterior sobre los cuaterniones.

En los gifs a continuación, puede ver rotaciones secuenciales de giro, inclinación y balanceo, cada una de las cuales requiere una orientación específica de los ejes para su correcto funcionamiento. Cada vez que se puede describir un solo movimiento mediante múltiples rotaciones, es posible el bloqueo de cardán.

Rotaciones de guiñada, cabeceo y balanceo.

Las rotaciones de balanceo, inclinación y giro son populares, pero en última instancia, matemáticamente defectuosas.

El problema con el uso de balanceo, inclinación y desvío es que es engorroso, utiliza trigonometría y está sujeto a bloqueo de cardán. Las funciones trigonométricas requieren cientos de ciclos de reloj en un microprocesador que pueden afectar negativamente el rendimiento de su código. Siempre que sea posible, el código debe usar funciones matemáticas más simples.

Un vector tridimensional que se encuentra en la esfera unitaria se puede girar a cualquier nueva posición con una sola rotación alrededor de un eje fijo. Es mucho más rápido y solo requiere los cálculos básicos de sumar, restar, dividir y multiplicar.

Sin embargo, requiere que hagas algunos cálculos matemáticos antes de comenzar a programar.

Ejemplo de configuración de orientación: Bosch BNO055

Así que aquí está el escenario en el que estaremos trabajando. El BNO055 está dentro de un balón de fútbol, ​​está encendido, los datos se escriben desde la memoria EEPROM en los registros de calibración y el dispositivo está sentado en algún lugar de una habitación. Realmente no sabemos cuál es la orientación, por lo que cualquier información que saquemos del dispositivo podría estar apuntando en cualquier dirección.

Crearemos una matriz de rotación que alinee los datos del sensor de manera que la fuerza de la gravedad apunte en la dirección del eje z negativo. Más tarde, podemos alinear el norte magnético para que quede a lo largo del eje x.

El BNO055 produce un vector de gravedad. Como se muestra en el gráfico a continuación, el vector de gravedad siempre apunta hacia abajo. Encontraremos la matriz necesaria para rotar matemáticamente el sensor de modo que el vector de gravedad se encuentre a lo largo del eje z negativo.

Una representación de un vector de gravedad. Los círculos animados representan la dirección de rotación positiva que corresponde a la regla de la mano derecha

Como nota, si el sensor no generara un vector de gravedad, podríamos simplemente leer los datos de los tres ejes del acelerómetro y realizar pasos similares. Sin embargo, tendríamos que ser un poco más cuidadosos con la asignación de variables.

Código de Gravedad Vector

El siguiente código lee el vector de gravedad del BNO055:

                    #incluir 
#incluir 
#incluir 
#incluir 

/ *
Este programa es una versión abreviada de Adafruit BNO055 rawdata.ino disponible después de instalar la biblioteca Adafruit BNO055. Programa modificado por Mark Hughes para TecNoticias.com
Archivo → Ejemplos → Adafruit BNO055 → Datos sin procesar
Conexiones en Arduino Uno
================================================== =======================
SCL a analógico 5 | SDA a analógico 4 | VDD a 3.3V DC | GND a terreno común
* /

#define BNO055_SAMPLERATE_DELAY_MS (100) // Demora entre solicitudes de datos
Adafruit_BNO055 bno = Adafruit_BNO055 (); // Crear un objeto sensor bno basado en la biblioteca Adafruit_BNO055

configuración de vacío (vacío) {
Serial.begin (115200); // Comenzar la comunicación del puerto serie
if (! bno.begin ()) // Inicializar comunicación del sensor
{
 Serial.print ("¡Vaya, no se detectó BNO055 ... Verifique su cableado o la DIRECCIÓN I2C!");
}
retraso (1000);
bno.setExtCrystalUse (true); // Usa el cristal en la placa de desarrollo.
}

bucle vacío (vacío)
{
imu :: Vector gravity = bno.getVector (Adafruit_BNO055 :: VECTOR_GRAVITY);
Serial.print ("gX:"); Serial.print (gravity.x ()); Serial.print (" t");
Serial.print ("gY:"); Serial.print (gravity.y ()); Serial.print (" t");
Serial.print ("gZ:"); Serial.print (gravity.z ()); Serial.print (" t");
Serial.println ("");
}

------ Salida -------

gX: 0,59 gY: 9,15 gZ: -3,47
                  

La aceleración debida a la gravedad cerca de la superficie de la Tierra es de aproximadamente 9.801 m / s². El hecho de que el sensor lea 9.15 en la dirección Y indica que el sensor está casi inclinado (como se muestra a continuación). Nuestro objetivo es hacer que los componentes X e Y de la gravedad 0 y el componente Z de la gravedad sean de casi 9.8.

A continuación puede ver una representación de la orientación del sensor, representada por el puntero azul oscuro. A la derecha está la orientación al inicio. A la izquierda está la orientación que está más cerca de lo ideal, con el eje z negativo paralelo al vector de gravedad.

Izquierda: Orientación del sensor en el arranque. Derecha: Más cerca de la orientación ideal del sensor.

La determinación de la matriz de rotación necesaria para rotar el sensor en la posición adecuada requiere algunas matemáticas. Primero, el vector que sale del sensor y nuestro vector objetivo deben normalizarse. Esto es necesario para poder resolver las ecuaciones. Sin la normalización, los términos se vuelven bastante difíciles de manejar, involucrando términos y conjugados imaginarios. En mi máquina, salió a aproximadamente 4 páginas de tamaño 14 de fuente.

La normalización da un vector que tiene solo dirección; la longitud de un vector normalizado es uno. Para normalizar, divida cada componente por la raíz cuadrada de la suma de los cuadrados de cada componente. A continuación se muestra un ejemplo, hecho de datos reales del sensor.

$$ hat {A} = left { frac {A_x} { sqrt {A_x ^ 2 + A_y ^ 2 + A_z ^ 2}} ; hat {x}, frac {A_y} { sqrt {A_x ^ 2 + A_y ^ 2 + A_z ^ 2}} ; hat {y}, frac {A_z} { sqrt {A_x ^ 2 + A_y ^ 2 + A_z ^ 2}} ; hat {z} right }% 0 $$

$$ hat {A} = left { frac {0.59} { sqrt {0.59 ^ 2 + 9.15 ^ 2 + (- 3.47) ^ 2}} ; hat {x}, frac {9.15} { sqrt {0.59 ^ 2 + 9.15 ^ 2 + (- 3.47) ^ 2}} ; hat {y}, frac {-3.47} { sqrt {0.59 ^ 2 + 9.15 ^ 2 + (- 3.47) ^ 2}} ; hat {z} right } $$

$$ hat {A} = left {0.061 ; hat {x}, 0.933 ; hat {y}, -0.354 ; hat {z} right } $$

Ya que elegimos el vector objetivo como el eje z negativo, no tenemos que pasar por el problema de normalizarlo.

$$ hat {G} = left {0 ; hat {x}, 0 ; hat {y}, -1 ; hat {z} right }% 0 $$

Matemáticas vectoriales

Producto de punto

Para determinar la parte del vector A que se proyecta sobre el vector G, use el producto puntual.

$$ hat {A} cdot hat {G} = left | A right | left | G right | cos ( theta) $$

Y para encontrar el ángulo entre ellos, reorganiza las ecuaciones para resolver para θ.

$$ cos ( theta) = frac { hat {A} cdot hat {G}} { left | A right | left | G derecho |} $$

Nuestra normalización de vectores ya está dando sus frutos. Desde | A | y | G | Ambos están ya configurados en uno, el denominador desaparece. Además, como los componentes X e Y de G son cero, la ecuación se reduce rápidamente.

$$ cos ( theta) = A_x cdot 0 + A_y cdot 0 ; + ; – 1 cdot A_z $$

Entonces, el ángulo de rotación depende solo de la componente Z de la aceleración.

$$ theta = arccos (-1 cdot A_z) $$

Producto cruzado

Determinar el vector para rotar este ángulo no es para un corazón débil. No es que sea particularmente difícil, pero es particularmente tedioso. Hay varios pasos, y es fácil cometer un pequeño error. Aquí es donde brillan los programas como Mathematica: no cometen errores a menos que sus programadores humanos se lo indiquen. A continuación se muestra la matriz de rotación antes de decirle al programa que estaba usando vectores unitarios.

Una vez que te das cuenta de que hay varias simplificaciones que se pueden hacer en la matriz, se reduce rápidamente a algo mucho más manejable.

Y después de la sustitución de nuestros valores originales, tenemos una matriz de rotación que rotará nuestro vector de gravedad en el eje z negativo. Y hay ciertas simetrías que pueden aprovecharse para reducir aún más la complejidad computacional.

Esta matriz, basada en el vector de gravedad, se determina una vez en el inicio para obtener una orientación con respecto a la gravedad. Si se aplica a futuras lecturas de aceleración, esta matriz rotará los valores para que, sin importar cómo esté montado el sensor, el eje z negativo apunte hacia abajo. Esta matriz de rotación, cuando se multiplica por cualquier vector de aceleración (normalizada o no), la rotará. Veamos un ejemplo y utilicemos el vector de gravedad original.

Para aquellos de ustedes que requieren un breve repaso de la multiplicación de matrices, los elementos de cada fila de la matriz se multiplican por cada elemento en la columna. Los resultados se totalizan después de reorganizar los componentes.

Cuando todo está dicho y hecho, el resultado es el siguiente:

No exactamente cero en la dirección X y en la dirección Y, pero lo suficientemente cerca. Recuerde que utilizamos solo dos lugares decimales de precisión al iniciar este viaje, y tenemos una lectura de cero en dos lugares decimales en X e Y, así como el esperado -9.801 m / s². Llamaré a eso una victoria.

Afortunadamente, la rotación de la lectura de la brújula para que el eje xo el eje magnético se extienda a lo largo del norte magnético es mucho más simple, ya que los valores de medición ahora se pueden girar alrededor del eje z. Y hay mucho espacio para la simplificación del código: las variables temporales se pueden usar para almacenar términos comunes para acelerar la ejecución del código. Además, la simetría se puede utilizar para reducir el cálculo redundante.

Sustituir el valor de la tangente de θ = Y / X en la matriz proporciona la rotación. Puede alinearse con el eje x positivo, el eje y positivo, el eje y negativo o el eje x negativo manipulando el lado derecho de esta ecuación (por ejemplo, tan[θ] = -X / Y), o puede alinearse con otro ángulo (por ejemplo, el norte geográfico) manipulando el lado izquierdo de la ecuación (tan[θ+14°]= Y / X).

Resumen

Las IMU deben calibrarse en el entorno en el que funcionarán. Parte de esa calibración generalmente requiere que el usuario defina cómo se orienta el sensor en el espacio y en referencia al entorno en el que se encuentra el sensor. Este artículo demostró cómo crear una matriz de rotación arbitraria alineada con los datos del acelerómetro así como una rotación bidimensional. Matriz basada en datos del magnetómetro.

La gravedad combinada y la transformada de la matriz de rotación del magnetómetro se muestran arriba.

Las matrices iniciales (gravedad y magnetómetro) se pueden multiplicar para crear una matriz de rotación única que se aplica a todas las mediciones futuras, lo que garantiza que los datos que recopile se puedan analizar con éxito más adelante.