Tengo una placa Arduino con un sensor de 9 grados de libertad, a partir del cual debo determinar el cabeceo, la guiñada y el balanceo de la placa.

Aquí es un ejemplo de un conjunto de datos desde el sensor 9-DOF:

Acelerómetro (m / s)

• $\text{Acc}_{X}$ = -5,85
• $\text{Acc}_{Y}$ = 1,46
• $\text{Acc}_{Z}$ = 17,98

Giroscopio (RPM)

• $\text{Gyr}_{X}$ = 35,14
• $\text{Gyr}_{Y}$ = -40,22
• $\text{Gyr}_{Z}$ = -9,86

Magnetómetro (Gauss)

• $\text{Mag}_{X}$ = 0,18
• $\text{Mag}_{Y}$ = -0,04
• $\text{Mag}_{Z}$ = -0,15

¿Cómo puedo calcular cabeceo, guiñada y rodar a partir de estos datos?

Paso, balanceo y guiñada se definen como la rotación alrededor de los ejes X, Y y Z. A continuación como una imagen para ilustrar la definición.

En un proyecto anterior utilicé un acelerómetro ADXL345 de Analog Devices para calcular el balanceo y el cabeceo. A continuación se muestran las ecuaciones utilizadas para calcular el balanceo y la inclinación. He puesto algunos de los códigos fuente disponibles para uso público.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

El código fuente completo se puede encontrar aquí .

Base en las definiciones anteriores

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Nota: M_PI = 3.14159265358979323846 se define constantemente en matemáticas.

A continuación se incluyen algunas referencias, incluido el código fuente base de Arduino, que podrían ayudarlo.

Referencias

• Dinámica de vuelo
• Código fuente Beaglebone negro ADXL345
• Adxl345 Beaglebone black Embedded QT Demo
• Arduino IMU: Pitch & Roll desde un acelerómetro

Entonces, mi respuesta más larga a continuación asume que el tablero experimentará aceleración y durante este tiempo aún debe poder medir su cabeceo, balanceo y guiñada en un corto período de tiempo. Si el tablero estará estacionario para todas las mediciones, la respuesta de Mahendra Gunawardena funcionará perfectamente para usted. Si esto entra en un dispositivo como un segway o modelo de avión o multirotor o cualquier cosa que se mueva, es posible que desee seguir leyendo. Esta publicación trata sobre cómo usar los tres sensores a través de un método llamado fusión de sensores. La fusión de sensores le permite obtener las fortalezas de cada sensor y minimizar los efectos de las debilidades de cada sensor.

Características del sensor y antecedentes

Primero entienda que un acelerómetro mide todas las fuerzas que se le aplican, no solo la fuerza de la gravedad. Entonces, en un mundo perfecto con el acelerómetro en una posición estacionaria sin ninguna vibración, podría determinar perfectamente qué camino está haciendo usando alguna trigonometría básica, como se muestra en la respuesta de Mahendra Gunawardena. Sin embargo, dado que un acelerómetro recogerá todas las fuerzas, cualquier vibración dará como resultado ruido. También debe tenerse en cuenta que si el tablero está acelerando, no solo puede usar trigonometría simple, ya que la fuerza que informa el acelerómetro no es solo la fuerza de gravedad de la Tierra, sino también la fuerza que está causando que acelere.

Un magnetómetro es más sencillo que un acelerómetro. El movimiento no causará problemas con él, pero cosas como el hierro y otros imanes terminarán afectando su producción. Si las fuentes que causan esta interferencia son constantes, no será difícil de tratar, pero si estas fuentes no son constantes, creará toneladas de ruido que es problemático eliminar.

De los tres sensores, el giroscopio es discutible el más confiable y normalmente son muy buenos para medir la velocidad de rotación. No se ve afectado por cosas como las fuentes de hierro y las aceleraciones básicamente no tienen impacto en su capacidad para medir la velocidad de rotación. Hacen un muy buen trabajo al informar la velocidad a la que gira el dispositivo, sin embargo, dado que está buscando un ángulo absoluto, debe integrar la velocidad para obtener la posición. Hacer esto agregará el error de la última medición al error de las nuevas mediciones ya que la integración es básicamente una suma de valores en un rango, incluso si el error para una medición es solo 0.01 grados por segundo apagado, en 100 mediciones, su posición puede estar apagado en 1 grado, en 1000 mediciones, puede estar apagado en 10 grados. Si está tomando cientos de mediciones por segundo, Puedes ver que esto causa problemas. Esto se llama comúnmente deriva giroscópica.

Fusión de sensores

Ahora, la belleza de tener todos estos sensores trabajando juntos es que puede usar la información del acelerómetro y el magnetómetro para cancelar la deriva del giroscopio. Esto termina permitiéndole darle la precisión y la velocidad del giroscopio sin el defecto fatal de la deriva del giroscopio.

Combinando los datos de estos tres sensores se puede hacer de más de una manera, hablaré sobre el uso de un filtro complementario porque es mucho más simple que un filtro kalman y los filtros kalman consumirán muchos más recursos en los sistemas integrados. Muchas veces un filtro complementario es lo suficientemente bueno, más sencillo de implementar (suponiendo que no esté utilizando una biblioteca preconstruida) y le permite procesar los datos más rápido.

Ahora en el proceso. Los primeros pasos que debe hacer es integrar la salida del giroscopio para convertir la velocidad angular en posición angular. También es probable que tenga que aplicar un filtro de paso bajo en el acelerómetro y el magnetómetro para lidiar con el ruido en la salida. Aquí funciona un filtro FIR simple como el que se muestra a continuación. Con algo de trigonometría puedes encontrar el cabeceo y balanceo con el acelerómetro y el guiñada con el magnetómetro.

filteredData = (1-weight)*filteredData + weight*newData

El peso es solo una constante que se puede ajustar según la cantidad de ruido que tenga que manejar, cuanto mayor sea el ruido, menor será el valor del peso. Ahora se puede combinar la información de los sensores con la siguiente línea de código.

fusedData = (1-weight)*gyroData + weight*accelMagData

Cabe señalar que los datos son un vector de cabeceo, balanceo y guiñada. Puede usar tres variables para hacer esto también en lugar de matrices si lo desea. Para este cálculo, el giroscopio proporciona una posición en grados en cabeceo, balanceo y guiñada, el magnetómetro proporciona un ángulo para guiñada mientras que el acelerómetro proporciona sus propios números para cabeceo y balanceo.

Si aún desea más información, puede buscar en Google "fusión de sensores con filtro complementario", hay muchos artículos sobre esto.

A partir de los datos del sensor del acelerador, solo puede calcular el cabeceo y balanceo. El siguiente documento de Freescale explica con mucha información lo que necesita:

AN3461: detección de inclinación mediante un acelerómetro de tres ejes

Según los dichos del documento,

lo que equivale a:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Por supuesto, el resultado es esto solo cuando las rotaciones ocurren en un orden específico (Rxyz):

1. Rodar alrededor del eje x por ángulo$\phi$
2. Paso alrededor del eje y por ángulo$\theta$
3. Guiñada alrededor del eje z por ángulo$\psi$

Dependiendo del orden de las rotaciones, obtienes diferentes ecuaciones. Para el orden de rotación , no puede encontrar el ángulo para la guiñada alrededor del eje z.$R_{xyz}$$\psi$