Yo apenas había publicado, así que aún más conseguirá un uso. El código se puede encontrar aquí:
https://github.com/TKJElectronics/example-Sketch-for-IMU-including-Kalman-Filter
Hola todo el mundo
Hace poco compre esta placa IMU 6DOF analógico (seis grados de libertad) (http://www.sparkfun.com/products/10010) de watterott.com. Utiliza tres giroscopios y acelerómetros de tres para calcular ángulos en tres dimensiones.
Miré un rato algo de código en línea y cómo conectarse con ellos. Después de muchas horas de investigación, logré hacer una medición precisa af de ángulos en dos direcciones. Decidí escribir a una pequeña guía para los entusiastas compañeros de electrónicos.
El objetivo principal de esta guía es enseñar a otros cómo obtener algunos datos útiles de la IMU o un giroscopio o acelerómetro. El código de Arduino puede encontrarse en github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. Debería ser bastante fácil de implementar mi código a su propio sensor. No voy a describir todos los detalles sobre la teoría detrás de, en lugar de otro puede buscar en las fuentes para obtener más información.
Antes de comenzar tienes que conectar el IMU como sigue:
Acc_Gyro Arduino
<> – 3, 3V 3, 3V
TIERRA <>: GND
Gx4 X <> — AN0
Gx4 Y <> — AN1
Gx4 Z <> — AN2
AN3 ACC X <> —
ACC Y <> — AN4
AN5 ACC Z <> —
También conectar 3, 3V al pin AREF en el Arduino para más exactitud.
Es muy importante que no conecte el sensor a 5V, esto destruirá el sensor.
Ahora está listo para leer algunos datos del sensor.
Para comunicarse con el sensor es sencillo:
El giróscopo mide grados por segundo mientras que el acelerometro mide aceleración (g) en tres dimensiones. Ambas salidas las medidas como una señal analógica.
Para obtener estos traducido a grados que tienes que hacer algunos codificación:
El giroscopio
Primero tienes que traducir quids (un número de 0-1023) en algo útil (esto es para un ADC con una resolución de 10 bits, por ejemplo debe ser 4095 (2 ^ 12-1 = 4095) de 12 bits ADC). Para ello sólo utilizo esta simple ecuación:
gyroRate = (gyroAdc-gyroZero) / sensibilidad - donde gyroAdc el valor leido de nuestro sensor, gyroZero es el valor cuando es inmóvil (esto se hace en el código - mira en la sección "Configuración") mientras que la sensibilidad es la sensibilidad en la hoja de datos, pero traducido a quids.
Si te fijas en las hojas de datos de dos giroscopios (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf y http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf), verá que la sensibilidad es 3.33mV/deg/s para el 4xOUT. Traducir en quids es bastante fácil: sensibilidad / 3.3 * 1023.
Así que en este ejemplo me sale:
0.00333/3.3*1023=1.0323.
Nota: para traducir mV v simple sólo se dividen por 1 mil.
La ecuación final tendrá el siguiente aspecto:
gyroRate = (gyroAdc-gryoZero) / 1.0323
El resultado va a salir como grados por segundo. Para traducir esto en grados tienes que saber la hora exacta desde el último bucle. Afortunadamente, el Arduino tiene un simple comando para hacerlo: millis(). , Uno puede calcular la diferencia de tiempo (tiempo de delta) y así calcular el ángulo del giroscopio. La ecuación final tendrá el siguiente aspecto:
gyroAngle += gyroRate * tiempo/1000
Por desgracia, el giro se desplaza en el tiempo. Eso significa que no se puede confiar para un intervalo de tiempo más largo, pero es muy preciso para un a corto plazo. Esto es cuando el acelerómetro es útil. No tiene ninguna deriva, pero es demasiado inestable para el intervalo de tiempo más corto. Voy a describir cómo combinar estas medidas en un tiempo, pero primero voy a describir cómo traducir las lecturas del acelerómetro en algo útil.
El acelerómetro
El acelerómetro mide la aceleración (g) en tres dimensiones. Para traducir el análogo lecturas en grados que basta leer el eje y para restar el offset cero así:
accVal = accAdc-accZero
Donde accAdc es la lectura analógica y accZero es el valor cuando lee g 0 - se calcula en el comienzo del código, busque en la sección de "Configuración". El valor cero se encuentran también en la hoja de datos: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. Verás que la tensión a 0g es aproximadamente 1, 5V, para traducir esto a quids, otra vez tienes que usar esta ecuación: zeroVoltage / 3.3 * 1023.
Así que en este ejemplo me sale:
1.5/3.3*1023=465.
Entonces puede calcular el pitch y roll utilizando las siguientes ecuaciones:
Pitch = atan2 (accYval, accZval) + PI
rodillo = atan2 (accXval, accZval) + PI
Atan2 tiene un rango de salida de - π a π (ver http://en.wikipedia.org/wiki/Atan2), simplemente añadir π, por lo que la gama se convierte en 0 a 2π.
Para convertir de radianes a grados simplemente multiplicamos el resultado por 57.295779513082320876798154814105 - esto está predefinido en el IDE de Arduino como RAD_TO_DEG.
Filtro de Kalman
Como expliqué anteriormente el giro es muy preciso, pero tienden a la deriva. El acelerómetro es un poco inestable, pero no a la deriva. Puede calcular el ángulo exacto utilizando algo que se llama un filtro de Kalman. Una guía detallada sobre cómo se implementa se puede encontrar en mi blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.
Si quieres usar algo un poco más simple, puede usar lo que denomina un filtro complementario. Es bastante fácil de entender y la matemática es mucho más sencilla, ya que sólo funciona en un solo paso.
Por ejemplo la ecuación podría parecer esto:
ángulo = 0,98 *(angle+gyro*dt) + 0,02 * acc - usted puede afinar los números para lo que quieras. Recuerde que la suma debe ser 1.
Para mí el resultado del filtro complementario fue muy cercano (o casi lo mismo) que el calculado por el filtro de Kalman.
Ahora han aprendido (espero) Cómo obtener datos analógicos de IMU y traducirla a algo útil. He adjuntado mi propio código para mi IMU 6DOF (http://www.sparkfun.com/products/10010), pero con algunas modificaciones un poco, estoy seguro de que es posible utilizarlo con cualquier giroscopio/acelerómetro analógico.
Si usted tiene cualquier pregunta, cayó libremente para enviar un comentario a continuación.
Fuentes:
http://www.Arduino.CC/cgi-bin/yabb2/YaBB.pl?NUM=1284738418
http://www.x-firm.com/?page_id=148
http://Web.MIT.edu/First/Segway/
Actualización
Acabo de terminar un código de procesamiento que imprime datos de Arduino en un gráfico agradable. Como se puede ver en el siguiente vídeo el filtrado es dejar de fumar efectivo. La línea azul claro es el acelerómetro, la línea morada es el giroscopio, la línea negra es el ángulo calculado por el filtro complementario y la línea roja es el ángulo calculado por el filtro de Kalman. Como se puede ver el filtro de Kalman es un poco más preciso (sé que es difícil de ver en el vídeo) que el filtro complementario, sobre todo cuando sacudirla.
He adjuntado mi código, el código actualizado para el Arduino y el código de procesamiento. También es posible ver los datos del eje y. Sólo Quite el comentario de drawAxisY(); en el código.
Firmware más reciente
Decidí poner todo el código fuente en github, ya que es mucho más fácil de mantener.
El código más reciente puede encontrarse ahora en github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
Mi robot de balanceo
A continuación hay un video de mi robot equilibrado. Utiliza el mismo IMU y algoritmo como se describe en el post anterior.
Pedal de arranque
Yo he liberado a mi robot equilibrio en Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit
Por favor considere apoyar el proyecto.
