Tutorial de acelerómetro y giroscopio: 3 pasos

2024 Autor: John Day | [email protected]. Última modificación: 2024-01-30 08:45

Introducción

Esta guía está dirigida a todas las personas interesadas en utilizar acelerómetros y giroscopios, así como dispositivos IMU combinados (unidad de medida inercial) en sus proyectos de electrónica.

Cubriremos:

• ¿Qué mide un acelerómetro?
• ¿Qué mide un giroscopio (también conocido como giroscopio)?
• Cómo convertir las lecturas analógicas a digitales (ADC) que obtiene de estos sensores en unidades físicas (esas serían g para acelerómetro, grados / s para giroscopio)
• Cómo combinar las lecturas del acelerómetro y el giroscopio para obtener información precisa sobre la inclinación de su dispositivo en relación con el plano del suelo

A lo largo del artículo intentaré mantener las matemáticas al mínimo. Si sabe qué son Sine / Cosine / Tangent, entonces debería poder comprender y usar estas ideas en su proyecto sin importar qué plataforma esté utilizando: Arduino, Propeller, Basic Stamp, chips Atmel, Microchip PIC, etc.

Hay personas que creen que se necesitan matemáticas complejas para hacer uso de una unidad IMU (filtros FIR o IIR complejos como filtros Kalman, filtros Parks-McClellan, etc.). Puede investigar todos esos y lograr resultados maravillosos pero complejos. Mi forma de explicar las cosas requiere solo matemáticas básicas. Soy un gran creyente en la sencillez. Creo que un sistema que es simple es más fácil de controlar y monitorear, además de que muchos dispositivos embebidos no tienen el poder y los recursos para implementar algoritmos complejos que requieren cálculos matriciales.

Usaré como ejemplo una nueva unidad IMU, el Acc_Gyro Accelerometer + Gyro IMU. Usaremos los parámetros de este dispositivo en nuestros ejemplos a continuación. Esta unidad es un buen dispositivo para empezar porque consta de 2 dispositivos:

- LIS331AL (hoja de datos) - un acelerómetro triaxial 2G - LPR550AL (hoja de datos) - un giroscopio de inclinación y balanceo de dos ejes, 500 grados / seg

Juntos representan una unidad de medida inercial de 5 grados de libertad. ¡Ese es un nombre elegante! Sin embargo, detrás del nombre elegante hay un dispositivo de combinación muy útil que cubriremos y explicaremos en detalle en esta guía.

Paso 1: el acelerómetro

Para entender esta unidad, comenzaremos con el acelerómetro. Al pensar en acelerómetros, a menudo es útil imaginar una caja en forma de cubo con una bola en su interior. Puedes imaginar algo más como una galleta o una dona, pero yo me imagino una pelota:

Si tomamos esta caja en un lugar sin campos de gravitación o sin otros campos que puedan afectar la posición de la bola, la bola simplemente flotará en el medio de la caja. Puedes imaginar que la caja está en el espacio exterior, muy lejos de cualquier cuerpo cósmico, o si un lugar así es difícil de encontrar, imagina al menos una nave espacial orbitando alrededor del planeta donde todo está en estado ingrávido. En la imagen de arriba puede ver que asignamos a cada eje un par de paredes (eliminamos la pared Y + para que podamos mirar dentro de la caja). Imagina que cada pared es sensible a la presión. Si de repente movemos la caja hacia la izquierda (la aceleramos con aceleración 1g = 9.8m / s ^ 2), la pelota chocará contra la pared X-. Luego medimos la fuerza de presión que la bola aplica a la pared y sacamos un valor de -1g en el eje X.

Tenga en cuenta que el acelerómetro en realidad detectará una fuerza que se dirija en la dirección opuesta al vector de aceleración. Esta fuerza a menudo se llama fuerza inercial o fuerza ficticia. Una cosa que debe aprender de esto es que un acelerómetro mide la aceleración indirectamente a través de una fuerza que se aplica a una de sus paredes (según nuestro modelo, podría ser un resorte u otra cosa en los acelerómetros de la vida real). Esta fuerza puede ser causada por la aceleración, pero como veremos en el siguiente ejemplo, no siempre es causada por la aceleración.

Si tomamos nuestro modelo y lo ponemos en la Tierra, la bola caerá sobre la pared Z y aplicará una fuerza de 1g en la pared inferior, como se muestra en la siguiente imagen:

En este caso, la caja no se mueve, pero aún obtenemos una lectura de -1g en el eje Z. La presión que la pelota ha aplicado en la pared fue causada por una fuerza de gravitación. En teoría, podría ser un tipo diferente de fuerza; por ejemplo, si imagina que nuestra bola es metálica, colocar un imán al lado de la caja podría mover la bola para que golpee otra pared. Esto se dijo solo para demostrar que, en esencia, el acelerómetro mide la fuerza, no la aceleración. Simplemente sucede que la aceleración causa una fuerza inercial que es capturada por el mecanismo de detección de fuerza del acelerómetro.

Si bien este modelo no es exactamente la forma en que se construye un sensor MEMS, a menudo es útil para resolver problemas relacionados con el acelerómetro. En realidad, hay sensores similares que tienen bolas metálicas en su interior, se llaman interruptores de inclinación, sin embargo, son más primitivos y generalmente solo pueden decir si el dispositivo está inclinado dentro de algún rango o no, no el grado de inclinación.

Hasta ahora hemos analizado la salida del acelerómetro en un solo eje y esto es todo lo que obtendrá con los acelerómetros de un solo eje. El valor real de los acelerómetros triaxiales proviene del hecho de que pueden detectar fuerzas inerciales en los tres ejes. Volvamos a nuestro modelo de caja y giremos la caja 45 grados a la derecha. La pelota tocará 2 paredes ahora: Z- y X- como se muestra en la siguiente imagen:

Los valores de 0,71 no son arbitrarios, en realidad son una aproximación de SQRT (1/2). Esto se hará más claro a medida que presentemos nuestro próximo modelo de acelerómetro.

En el modelo anterior hemos fijado la fuerza de gravitación y hemos rotado nuestra caja imaginaria. En los 2 últimos ejemplos hemos analizado la salida en 2 posiciones de caja diferentes, mientras que el vector de fuerza se mantuvo constante. Si bien esto fue útil para comprender cómo interactúa el acelerómetro con las fuerzas externas, es más práctico realizar cálculos si fijamos el sistema de coordenadas a los ejes del acelerómetro e imaginamos que el vector de fuerza gira a nuestro alrededor.

Por favor, eche un vistazo al modelo de arriba, conservé los colores de los ejes para que pueda hacer una transición mental del modelo anterior al nuevo. Imagínese que cada eje del nuevo modelo es perpendicular a las caras respectivas de la caja del modelo anterior. El vector R es el vector de fuerza que mide el acelerómetro (podría ser la fuerza de gravitación o la fuerza de inercia de los ejemplos anteriores o una combinación de ambas). Rx, Ry, Rz son la proyección del vector R en los ejes X, Y, Z. Tenga en cuenta la siguiente relación:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (Ecuación 1)

que es básicamente el equivalente del teorema de Pitágoras en 3D.

Recuerda que un poco antes te dije que los valores de SQRT (1/2) ~ 0.71 no son aleatorios. Si los inserta en la fórmula anterior, después de recordar que nuestra fuerza de gravitación era de 1 g, podemos verificar que:

1 ^ 2 = (-SQRT (1/2)) ^ 2 + 0 ^ 2 + (-SQRT (1/2)) ^ 2

simplemente sustituyendo R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) en la ecuación 1

Después de un largo preámbulo de la teoría, nos estamos acercando a los acelerómetros de la vida real. Los valores Rx, Ry, Rz en realidad están relacionados linealmente con los valores que generará su acelerómetro de la vida real y que puede usar para realizar varios cálculos.

Antes de llegar, hablemos un poco sobre la forma en que los acelerómetros nos entregarán esta información. La mayoría de los acelerómetros se clasificarán en dos categorías: digitales y analógicos. Los acelerómetros digitales le brindarán información utilizando un protocolo en serie como I2C, SPI o USART, mientras que los acelerómetros analógicos generarán un nivel de voltaje dentro de un rango predefinido que debe convertir a un valor digital utilizando un módulo ADC (convertidor analógico a digital). No entraré en muchos detalles sobre cómo funciona ADC, en parte porque es un tema tan extenso y en parte porque es diferente de una plataforma a otra. Algunos microcontroladores tendrán módulos ADC incorporados, algunos de ellos necesitarán componentes externos para realizar las conversiones ADC. No importa qué tipo de módulo ADC use, terminará con un valor en un cierto rango. Por ejemplo, un módulo ADC de 10 bits generará un valor en el rango de 0..1023, tenga en cuenta que 1023 = 2 ^ 10 -1. Un módulo ADC de 12 bits generará un valor en el rango de 0..4095, tenga en cuenta que 4095 = 2 ^ 12-1.

Sigamos considerando un ejemplo simple, supongamos que nuestro módulo ADC de 10 bits nos dio los siguientes valores para los tres canales (ejes) del acelerómetro:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Cada módulo ADC tendrá un voltaje de referencia, supongamos en nuestro ejemplo que es 3.3V. Para convertir un valor adc de 10 bits en voltaje, usamos la siguiente fórmula:

VoltsRx = AdcRx * Vref / 1023

Una nota rápida aquí: que para ADC de 8 bits, el último divisor sería 255 = 2 ^ 8 -1, y para ADC de 12 bits, el último divisor sería 4095 = 2 ^ 12 -1.

Aplicando esta fórmula a los 3 canales obtenemos:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (redondeamos todos los resultados a 2 puntos decimales) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Cada acelerómetro tiene un nivel de voltaje de cero g, lo puede encontrar en las especificaciones, este es el voltaje que corresponde a 0g. Para obtener un valor de voltaje con signo, necesitamos calcular el cambio desde este nivel. Digamos que nuestro nivel de voltaje 0g es VzeroG = 1.65V. Calculamos los cambios de voltaje desde el voltaje de cero g de la siguiente manera:

DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V DeltaVoltsRz = 1.81V - 1.65V = 0.16V

Ahora tenemos las lecturas de nuestro acelerómetro en Voltios, todavía no está en g (9.8 m / s ^ 2), para hacer la conversión final aplicamos la sensibilidad del acelerómetro, generalmente expresada en mV / g. Digamos que nuestra sensibilidad = 478,5 mV / g = 0,4785 V / g. Los valores de sensibilidad se pueden encontrar en las especificaciones del acelerómetro. Para obtener los valores de fuerza final expresados en g usamos la siguiente fórmula:

Rx = DeltaVoltsRx / Sensibilidad

Rx = 0.24V / 0.4785V / g = ~ 0.5g Ry = 0.38V / 0.4785V / g = ~ 0.79g Rz = 0.16V / 0.4785V / g = ~ 0.33g

Por supuesto, podríamos combinar todos los pasos en una fórmula, pero pasé por todos los pasos para dejar en claro cómo se pasa de las lecturas de ADC a un componente de vector de fuerza expresado en g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Sensibilidad (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Sensibilidad Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensibilidad

Ahora tenemos los 3 componentes que definen nuestro vector de fuerza de inercia, si el dispositivo no está sujeto a otras fuerzas distintas de la gravitación, podemos asumir que esta es la dirección de nuestro vector de fuerza de gravitación. Si desea calcular la inclinación del dispositivo en relación con el suelo, puede calcular el ángulo entre este vector y el eje Z. Si también está interesado en la dirección de inclinación por eje, puede dividir este resultado en 2 componentes: inclinación en los ejes X e Y que se puede calcular como el ángulo entre el vector de gravitación y los ejes X / Y. Calcular estos ángulos es más simple de lo que piensas, ahora que hemos calculado los valores de Rx, Ry y Rz. Volvamos a nuestro último modelo de acelerómetro y hagamos algunas notaciones adicionales:

Los ángulos que nos interesan son los ángulos entre los ejes X, Y, Z y el vector de fuerza R. Definiremos estos ángulos como Axr, Ayr, Azr. Puede observar en el triángulo de ángulo recto formado por R y Rx que:

cos (Axr) = Rx / R, y de manera similar: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Podemos deducir de la ecuación 1 que R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Ahora podemos encontrar nuestros ángulos usando la función arccos () (la función inversa cos ()):

Axr = arccos (Rx / R) Ayr = arccos (Ry / R) Azr = arccos (Rz / R)

Hemos recorrido un largo camino para explicar el modelo del acelerómetro, solo para llegar a estas fórmulas. Dependiendo de sus aplicaciones, es posible que desee utilizar cualquier fórmula intermedia que hayamos derivado. También presentaremos el modelo de giroscopio pronto, y veremos cómo se pueden combinar los datos del acelerómetro y del giroscopio para proporcionar estimaciones de inclinación aún más precisas.

Pero antes de hacer eso, hagamos algunas notaciones más útiles:

cosX = cos (Axr) = Rx / R cosy = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Este triplete a menudo se llama coseno de dirección, y básicamente representa el vector unitario (vector con longitud 1) que tiene la misma dirección que nuestro vector R. Puede verificar fácilmente que:

SQRT (cosX ^ 2 + acogedor ^ 2 + cosZ ^ 2) = 1

Esta es una buena propiedad ya que nos absuelve de monitorear el módulo (longitud) del vector R. Muchas veces, si solo estamos interesados en la dirección de nuestro vector inercial, tiene sentido normalizar su módulo para simplificar otros cálculos.

Paso 2: giroscopio

No vamos a introducir ningún modelo de caja equivalente para el giroscopio como hicimos para el acelerómetro, sino que vamos a saltar directamente al segundo modelo de acelerómetro y mostraremos qué mide el giroscopio de acuerdo con este modelo.

Cada canal del giroscopio mide la rotación alrededor de uno de los ejes. Por ejemplo, un giroscopio de 2 ejes medirá la rotación alrededor (o algunos pueden decir "sobre") los ejes X e Y. Para expresar esta rotación en números, hagamos algunas notaciones. Primero definamos:

Rxz - es la proyección del vector de fuerza inercial R en el plano XZ Ryz - es la proyección del vector de fuerza inercial R en el plano YZ

Del triángulo rectángulo formado por Rxz y Rz, usando el teorema de Pitágoras obtenemos:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2, y de manera similar: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

también tenga en cuenta que:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, esto se puede derivar de la Ec. 1 y ecuaciones anteriores, o se puede derivar de un triángulo en ángulo recto formado por R y Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

No vamos a utilizar estas fórmulas en este artículo, pero es útil notar la relación entre todos los valores de nuestro modelo.

En su lugar, vamos a definir el ángulo entre el eje Z y los vectores Rxz, Ryz de la siguiente manera:

Axz - es el ángulo entre Rxz (proyección de R en el plano XZ) y el eje Z Ayz - es el ángulo entre Ryz (proyección de R en el plano YZ) y el eje Z

Ahora nos estamos acercando a lo que mide el giroscopio. El giroscopio mide la tasa de cambios de los ángulos definidos anteriormente. En otras palabras, generará un valor que está relacionado linealmente con la tasa de cambio de estos ángulos. Para explicar esto, supongamos que hemos medido el ángulo de rotación alrededor del eje Y (que sería el ángulo Axz) en el tiempo t0, y lo definimos como Axz0, luego medimos este ángulo en un tiempo posterior t1 y fue Axz1. La tasa de cambio se calculará de la siguiente manera:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

Si expresamos Axz en grados y el tiempo en segundos, este valor se expresará en grados / s. Esto es lo que mide un giroscopio.

En la práctica, un giroscopio (a menos que sea un giroscopio digital especial) rara vez le dará un valor expresado en grados / s. Al igual que para el acelerómetro, obtendrá un valor de ADC que deberá convertir a grados / s usando una fórmula similar a la ecuación. 2 que hemos definido para acelerómetro. Introduzcamos la fórmula de conversión de ADC a grados / s para giroscopio (asumimos que estamos usando un módulo ADC de 10 bits, para ADC de 8 bits reemplace 1023 por 255, para ADC de 12 bits reemplace 1023 por 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensibilidad Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensibilidad

AdcGyroXZ, AdcGyroYZ: se obtienen de nuestro módulo adc y representan los canales que miden la rotación de proyección del vector R en XZ respectivamente en los planos YZ, lo que equivale a decir que la rotación se realizó alrededor de los ejes Y y X respectivamente.

Vref - es el voltaje de referencia ADC que usaremos 3.3V en el ejemplo a continuación VzeroRate - es el voltaje de tasa cero, en otras palabras, el voltaje que emite el giroscopio cuando no está sujeto a ninguna rotación, para la placa Acc_Gyro es por ejemplo, 1.23V (puede encontrar estos valores en las especificaciones) Sensibilidad: es la sensibilidad de su giroscopio, se expresa en mV / (grados / s) a menudo escrito como mV / grados / s, básicamente le dice cuántos mV la salida del giroscopio aumenta, si aumenta la velocidad de rotación en un grado / s. La sensibilidad de la placa Acc_Gyro es, por ejemplo, 2mV / deg / so 0.002V / deg / s

Tomemos un ejemplo, supongamos que nuestro módulo ADC devolvió los siguientes valores:

AdcGyroXZ = 571 AdcGyroXZ = 323

Usando la fórmula anterior y usando los parámetros de especificaciones de la placa Acc_Gyro obtendremos:

RateAxz = (571 * 3.3V / 1023 - 1.23V) / (0.002V / deg / s) = ~ 306 grados / s RateAyz = (323 * 3.3V / 1023 - 1.23V) / (0.002V / deg / s) = ~ -94 grados / s

En otras palabras, el dispositivo gira alrededor del eje Y (o podemos decir que gira en el plano XZ) con una velocidad de 306 grados / sy alrededor del eje X (o podemos decir que gira en el plano YZ) con una velocidad de - 94 grados / s. Tenga en cuenta que el signo negativo significa que el dispositivo gira en la dirección opuesta a la dirección positiva convencional. Por convención, una dirección de rotación es positiva. Una buena hoja de especificaciones del giroscopio le mostrará qué dirección es positiva; de lo contrario, tendrá que encontrarla experimentando con el dispositivo y observando qué dirección de rotación da como resultado un aumento de voltaje en el pin de salida. Esto se hace mejor usando un osciloscopio, ya que tan pronto como detenga la rotación, el voltaje volverá al nivel de tasa cero. Si está utilizando un multímetro, tendrá que mantener una tasa de rotación constante durante al menos unos segundos y anotar el voltaje durante esta rotación, luego compararlo con el voltaje de tasa cero. Si es mayor que el voltaje de tasa cero, significa que la dirección de rotación es positiva.

Paso 3: Combinando el Acelerómetro y el Giroscopio

Poniéndolo todo junto: combinando datos de acelerómetro y giroscopio

Si está leyendo este artículo, probablemente haya adquirido o esté planeando adquirir un dispositivo IMU, o probablemente esté planeando construir uno a partir de dispositivos de acelerómetro y giroscopio separados.

El primer paso para usar un dispositivo IMU combinado que combina un acelerómetro y un giroscopio es alinear sus sistemas de coordenadas. La forma más sencilla de hacerlo es elegir el sistema de coordenadas del acelerómetro como su sistema de coordenadas de referencia. La mayoría de las hojas de datos de acelerómetros mostrarán la dirección de los ejes X, Y, Z en relación con la imagen del chip o dispositivo físico. Por ejemplo, aquí están las direcciones de los ejes X, Y, Z como se muestra en las especificaciones de la placa Acc_Gyro:

Los siguientes pasos son:

Identifique las salidas del giroscopio que corresponden a los valores RateAxz, RateAyz discutidos anteriormente Determine si estas salidas deben invertirse debido a la posición física del giroscopio en relación con el acelerómetro

No asuma que si un giroscopio tiene una salida marcada como X o Y, corresponderá a cualquier eje en el sistema de coordenadas del acelerómetro, incluso si esta salida es parte de una unidad IMU. La mejor forma es probarlo. Suponiendo que haya fijado la posición del giroscopio en relación con el acelerómetro. Se supone que los bordes del giróscopo y del acelerómetro son paralelos entre sí, es decir, está colocando el giróscopo en un ángulo múltiplo de 90 grados con respecto al chip del acelerómetro. Si adquirió una placa IMU, es probable que ya estén alineados de esta manera. No vamos a discutir en este artículo modelos en los que el giroscopio se coloca en un ángulo irregular con respecto al acelerómetro (digamos 45 o 30 grados), aunque esto podría ser útil en algunas aplicaciones.

Aquí hay una secuencia de muestra para determinar qué salida del giroscopio corresponde al valor RateAxz discutido anteriormente.

- empezar colocando el dispositivo en posición horizontal. Ambas salidas X e Y del acelerómetro generarían el voltaje de cero g (por ejemplo, para la placa Acc_Gyro, esto es 1,65 V)

- A continuación, comience a girar el dispositivo alrededor del eje Y, otra forma de decirlo es que gire el dispositivo en el plano XZ, de modo que las salidas del acelerómetro X y Z cambien y la salida Y permanezca constante. - mientras gira el dispositivo a una velocidad constante, observe qué salida del giroscopio cambia, las otras salidas del giroscopio deben permanecer constantes - la salida del giroscopio que cambió durante la rotación alrededor del eje Y (rotación en el plano XZ) proporcionará el valor de entrada para AdcGyroXZ, a partir del cual calculamos RateAxz - el paso final es asegurar que la dirección de rotación corresponda a nuestro modelo; en algunos casos, es posible que tenga que invertir el valor de RateAxz debido a la posición física del giroscopio en relación con el acelerómetro - realice nuevamente la prueba anterior, girando el dispositivo alrededor el eje Y, esta vez monitorea la salida X del acelerómetro (AdcRx en nuestro modelo). Si AdcRx crece (los primeros 90 grados de rotación desde la posición horizontal), AdcGyroXZ también debería crecer. De lo contrario, necesita invertir RateAxz, puede lograrlo introduciendo un factor de signo en la ecuación 3, de la siguiente manera:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity, donde InvertAxz es 1 o -1

Se hace el mismo bastón de prueba para RateAyz, girando el dispositivo alrededor del eje X, y puede identificar qué salida del giroscopio corresponde a RateAyz y si es necesario invertirla. Una vez que tenga el valor de InvertAyz, debe usar la siguiente fórmula para calcular RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensibilidad

Si hiciera estas pruebas en el tablero Acc_Gyro, obtendría los siguientes resultados:

- el pin de salida para RateAxz es GX4 e InvertAxz = -1. - el pin de salida para RateAyz es GY4 e InvertAyz = -1

A partir de este momento, consideraremos que ha configurado su IMU de tal manera que puede calcular los valores correctos para Axr, Ayr, Azr (como se define en la Parte 1. Acelerómetro) y RateAxz, RateAyz (como se define en la Parte 2. Giroscopio). A continuación analizaremos las relaciones entre estos valores que resultan útiles para obtener una estimación más precisa de la inclinación del dispositivo con respecto al plano del suelo.

Es posible que se esté preguntando en este punto, si el modelo de acelerómetro ya nos dio ángulos de inclinación de Axr, Ayr, Azr, ¿por qué querríamos molestarnos con los datos del giroscopio? La respuesta es simple: no siempre se puede confiar al 100% en los datos del acelerómetro. Hay varias razones, recuerde que el acelerómetro mide la fuerza de inercia, tal fuerza puede ser causada por la gravitación (e idealmente solo por la gravitación), pero también puede ser causada por la aceleración (movimiento) del dispositivo. Como resultado, incluso si el acelerómetro está en un estado relativamente estable, sigue siendo muy sensible a las vibraciones y al ruido mecánico en general. Esta es la razón principal por la que la mayoría de los sistemas IMU utilizan un giroscopio para suavizar los errores del acelerómetro. Pero, ¿cómo se hace esto? ¿Y el giroscopio está libre de ruido?

El giroscopio no está libre de ruido, sin embargo, debido a que mide la rotación, es menos sensible a los movimientos mecánicos lineales, el tipo de ruido que sufre el acelerómetro, sin embargo, los giroscopios tienen otros tipos de problemas como, por ejemplo, la deriva (no volver al valor de tasa cero). cuando la rotación se detiene). Sin embargo, al promediar los datos que provienen del acelerómetro y el giroscopio, podemos obtener una estimación relativamente mejor de la inclinación actual del dispositivo que la que obtendríamos usando solo los datos del acelerómetro.

En los próximos pasos, presentaré un algoritmo que se inspiró en algunas ideas utilizadas en el filtro de Kalman, sin embargo, es mucho más simple y más fácil de implementar en dispositivos integrados. Antes de eso, veamos primero qué queremos que calcule nuestro algoritmo. Bueno, es la dirección del vector de fuerza de gravitación R = [Rx, Ry, Rz] del cual podemos derivar otros valores como Axr, Ayr, Azr o cosX, cosy, cosZ lo que nos dará una idea de la inclinación de nuestro dispositivo. en relación con el plano de tierra, discutimos la relación entre estos valores en la Parte 1. Se podría decir: ¿no tenemos ya estos valores Rx, Ry, Rz de la Ec. 2 en la Parte 1? Bueno, sí, pero recuerde que estos valores se derivan únicamente de los datos del acelerómetro, por lo que si los usara directamente en su aplicación, es posible que obtenga más ruido del que su aplicación puede tolerar. Para evitar una mayor confusión, redefinamos las medidas del acelerómetro de la siguiente manera:

Racc: es el vector de fuerza inercial medido por el acelerómetro, que consta de los siguientes componentes (proyecciones en los ejes X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Sensibilidad RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Sensibilidad RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Sensibilidad

Hasta ahora tenemos un conjunto de valores medidos que podemos obtener puramente de los valores ADC del acelerómetro. Llamaremos a este conjunto de datos un "vector" y usaremos la siguiente notación.

Racc = [RxAcc, RyAcc, RzAcc]

Debido a que estos componentes de Racc se pueden obtener de los datos del acelerómetro, podemos considerarlos como una entrada a nuestro algoritmo.

Tenga en cuenta que debido a que Racc mide la fuerza de gravitación, estará en lo correcto si asume que la longitud de este vector definido de la siguiente manera es igual o cercana a 1g.

| Racc | = SQRT (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Sin embargo, para estar seguro de que tiene sentido actualizar este vector de la siguiente manera:

Racc (normalizado) = [RxAcc / | Racc |, RyAcc / | Racc |, RzAcc / | Racc |].

Esto asegurará que la longitud de su vector Racc normalizado sea siempre 1.

A continuación, presentaremos un nuevo vector y lo llamaremos

Descanso = [RxEst, RyEst, RzEst]

Esta será la salida de nuestro algoritmo, estos son valores corregidos basados en datos de giroscopio y basados en datos estimados anteriores.

Esto es lo que hará nuestro algoritmo: - el acelerómetro nos dice: "Ahora estás en la posición Racc" - decimos "Gracias, pero déjame comprobar", - luego corrige esta información con los datos del giroscopio, así como con los datos de reposo anteriores y generamos un nuevo Descanso vectorial estimado. - Consideramos que Rest es nuestra "mejor apuesta" en cuanto a la posición actual del dispositivo.

Veamos cómo podemos hacer que funcione.

Comenzaremos nuestra secuencia confiando en nuestro acelerómetro y asignando:

Descanso (0) = Racc (0)

Por cierto, recuerde que Rest y Racc son vectores, por lo que la ecuación anterior es solo una forma simple de escribir 3 conjuntos de ecuaciones y evitar la repetición:

RxEst (0) = RxAcc (0) RyEst (0) = RyAcc (0) RzEst (0) = RzAcc (0)

A continuación, haremos mediciones regulares a intervalos de tiempo iguales de T segundos, y obtendremos nuevas mediciones que definiremos como Racc (1), Racc (2), Racc (3) y así sucesivamente. También emitiremos nuevas estimaciones en cada intervalo de tiempo Descanso (1), Descanso (2), Descanso (3) y así sucesivamente.

Supongamos que estamos en el paso n. Tenemos dos conjuntos de valores conocidos que nos gustaría usar:

Descanso (n-1): nuestra estimación anterior, con Descanso (0) = Racc (0) Racc (n): nuestra medición actual del acelerómetro

Antes de que podamos calcular Rest (n), introduzcamos un nuevo valor medido, que podemos obtener de nuestro giroscopio y una estimación previa.

Lo llamaremos Rgyro, y también es un vector que consta de 3 componentes:

Rgyro = [RxGyro, RyGyro, RzGyro]

Calcularemos este vector componente a componente. Empezaremos con RxGyro.

Comencemos observando la siguiente relación en nuestro modelo de giroscopio, del triángulo de ángulo recto formado por Rz y Rxz podemos derivar que:

tan (Axz) = Rx / Rz => Axz = atan2 (Rx, Rz)

Atan2 podría ser una función que nunca usó antes, es similar a atan, excepto que devuelve valores en el rango de (-PI, PI) en lugar de (-PI / 2, PI / 2) como lo devuelve atan, y toma 2 argumentos en lugar de uno. Nos permite convertir los dos valores de Rx, Rz a ángulos en el rango completo de 360 grados (-PI a PI). Puede leer más sobre atan2 aquí.

Entonces, conociendo RxEst (n-1) y RzEst (n-1) podemos encontrar:

Axz (n-1) = atan2 (RxEst (n-1), RzEst (n-1)).

Recuerde que el giroscopio mide la tasa de cambio del ángulo de Axz. Entonces podemos estimar el nuevo ángulo Axz (n) de la siguiente manera:

Axz (n) = Axz (n-1) + TasaAxz (n) * T

Recuerde que RateAxz se puede obtener de nuestras lecturas ADC de giroscopio. Una fórmula más precisa puede usar una tasa de rotación promedio calculada de la siguiente manera:

RateAxzAvg = (RateAxz (n) + RateAxz (n-1)) / 2 Axz (n) = Axz (n-1) + RateAxzAvg * T

De la misma forma podemos encontrar:

Ayz (n) = Ayz (n-1) + RateAyz (n) * T

Ok, ahora tenemos Axz (n) y Ayz (n). ¿A dónde vamos desde aquí para deducir RxGyro / RyGyro? De la ecuación. 1 podemos escribir la longitud del vector Rgyro de la siguiente manera:

| Rgyro | = SQRT (RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2)

Además, debido a que normalizamos nuestro vector Racc, podemos suponer que su longitud es 1 y no ha cambiado después de la rotación, por lo que es relativamente seguro escribir:

| Rgyro | = 1

Adoptemos una notación temporal más corta para los cálculos a continuación:

x = RxGyro, y = RyGyro, z = RzGyro

Usando las relaciones anteriores podemos escribir:

x = x / 1 = x / SQRT (x ^ 2 + y ^ 2 + z ^ 2)

Dividamos el numerador y el denominador de la fracción por SQRT (x ^ 2 + z ^ 2)

x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))

Tenga en cuenta que x / SQRT (x ^ 2 + z ^ 2) = sin (Axz), entonces:

x = sin (Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))

Ahora multiplica el numerador y el denominador de la fracción dentro de SQRT por z ^ 2

x = sin (Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))

Tenga en cuenta que z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) y y / z = tan (Ayz), así que finalmente:

x = sin (Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)

Volviendo a nuestra notación obtenemos:

RxGyro = sin (Axz (n)) / SQRT (1 + cos (Axz (n)) ^ 2 * tan (Ayz (n)) ^ 2)

de la misma manera que encontramos que

RyGyro = sin (Ayz (n)) / SQRT (1 + cos (Ayz (n)) ^ 2 * tan (Axz (n)) ^ 2)

Ahora, finalmente podemos encontrar:

RzGyro = Signo (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).

Donde Sign (RzGyro) = 1 cuando RzGyro> = 0, y Sign (RzGyro) = -1 cuando RzGyro <0.

Una forma sencilla de estimar esto es tomar:

Signo (RzGyro) = Signo (RzEst (n-1))

En la práctica, tenga cuidado cuando RzEst (n-1) está cerca de 0. En este caso, puede omitir la fase giroscópica y asignar: Rgyro = Rest (n-1). Rz se usa como referencia para calcular los ángulos Axz y Ayz y cuando está cerca de 0, los valores pueden desbordar y desencadenar malos resultados. Estará en el dominio de grandes números de punto flotante donde las implementaciones de funciones tan () / atan () pueden carecer de precisión.

Así que recapitulemos lo que tenemos hasta ahora, estamos en el paso n de nuestro algoritmo y hemos calculado los siguientes valores:

Racc - lecturas actuales de nuestro acelerómetro Rgyro - obtenidas de Rest (n-1) y lecturas actuales del giroscopio

¿Qué valores utilizamos para calcular la estimación actualizada Rest (n)? Probablemente adivinó que usaremos ambos. Usaremos un promedio ponderado, de modo que:

Descanso (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Podemos simplificar esta fórmula dividiendo tanto el numerador como el denominador de la fracción por w1.

Descanso (n) = (Racc * w1 / w1 + Rgyro * w2 / w1) / (w1 / w1 + w2 / w1)

y después de sustituir w2 / w1 = wGyro obtenemos:

Descanso (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

En el foro anterior wGyro nos dice cuánto confiamos en nuestro giroscopio en comparación con nuestro acelerómetro. Este valor puede elegirse experimentalmente, normalmente valores entre 5..20 darán buenos resultados.

La principal diferencia de este algoritmo con el filtro de Kalman es que este peso es relativamente fijo, mientras que en el filtro de Kalman los pesos se actualizan permanentemente en función del ruido medido de las lecturas del acelerómetro. El filtro de Kalman se centra en brindarle "los mejores" resultados teóricos, mientras que este algoritmo puede brindarle resultados "suficientemente buenos" para su aplicación práctica. Puede implementar un algoritmo que ajuste wGyro dependiendo de algunos factores de ruido que mida, pero los valores fijos funcionarán bien para la mayoría de las aplicaciones.

Estamos a un paso de obtener nuestros valores estimados actualizados:

RxEst (n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst (n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst (n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

Ahora normalicemos este vector nuevamente:

R = SQRT (RxEst (n) ^ 2 + RyEst (n) ^ 2 + RzEst (n) ^ 2)

RxEst (n) = RxEst (n) / R RyEst (n) = RyEst (n) / R RzEst (n) = RzEst (n) / R

Y estamos listos para repetir nuestro ciclo nuevamente.

Esta guía apareció originalmente en starlino.com, hice algunas ediciones ligeras y la volví a publicar con permiso. ¡Gracias Starlino!