Cálculo de la posición actual con el Giroscopio (IMU)

El siguiente paso es añadir las lecturas del giroscopio para contrarrestar los errores implícitos de usar únicamente el acelerómetro.

Voy a seguir basándome en este tutorial: A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications.

De esta manera el bucle principal del programa se ve modificado quedando así:

  1. Leemos los valores del acelerómetro
  2. Transformamos esos valores en G’s
  3. Normalizamos los valores obtenidos
  4. Leemos los valores de los giroscopios
  5. Los pasamos a grados/segundo
  6. Calculamos la posición estimada a partir de la última posición y los valores del giroscopio
  7. Mezclamos las dos posiciones estimadas (accelerómetro y giroscopio) para obtener la definitiva
  8. Normalizamos los valores
  9. Calculamos los ángulos Pitch y Roll

El código tiene algunas constantes referentes a voltajes y sensibilidades que he sacado del DataSheet del IMU y que luego he ido retocando a mano tras ir probando.

Tras unas primeras pruebas no tengo muy claro que haya mejorado la cosa. Con el circuito en la mano y moviendolo yo, la sensación es que la posición obtenida cuando está en reposo se ve alterada, como si realmente el aparato se estuviese moviendo.

Si muevo el circuito obtengo unos valores que creo que son buenos pero de vez en cuando obtengo algún valor un poco raro.

Por ejemplo, dejando el circuito quieto encima de una mesa y sacando en el depurador los ángulos Pitch y Roll obtengo cosas de este estilo:

Pitch Roll
0,3 – 0,4
0,05 0,01
5,3 0,5
0,04 – 0,3

Es un ejemplo pero de vez en cuando obtengo algún valor que no es real (por ejemplo el 5,3). Todo esto dándole el mínimo peso recomendado según el enlace a las mediciones del giroscopio, que es 5:1 a favor del giroscopio. Si le doy más peso, aún es peor.

Si inclino el aparato me pasa algo similar…. obtengo medidas razonables y de vez en cuando alguna «se va de madre».

Creo que hasta que no haga pruebas reales no voy a poder corregir (si es que hay que hacerlo) esta parte.

Tras revisar el código he visto que estaba haciendo mal unos cálculos. Estaba mezclando ángulos en grados con ángulos en radianes y claro, la cosa no podía salir bien. Ahora si que obtengo una posición lógica combinando los valores del acelerómetro y del Gyro.

Tras las primeras pruebas he empezado a tener problemas de rendimiento y problemas de quedarme sin memoria. Ha habido varios frentes en los que he atacado para solucionarlo:

  • Reducir el código, eliminando clases o métodos que no use
  • Reducir las variables a lo mínimo e imprescindible para ahorrar memoria. Por ejemplo usar «byte» (1 byte) en vez de «integer» (4 bytes)
  • Reducir precisiones excesivas de las variables para ganar rendimiento. Por ejemplo usar «float» (7 dígitos) en vez de «double» (15-16 dígitos)
  • Evitar  la continua creación y destrucción de variables en las continuas iteracciones de los bucles, declarando las variables como globales
  • Llamar al recolector de basura cada ciertos segundos para forzar la liberación y compactación de basura
  • En el caso del cálculo del Seno de un ángulo, búsqueda de método de cálculo rápido y eficiente ya que es una operación muy costosa.
  • Por otro lado el programa funciona a una cierta velocidad de «recalculo» de la posición y potencia de los motores del aparato. El reducir esta velocidad (Hz) elimina problemas de quedarnos sin memoria. Habrá que ir probando para ver los límites.

En este momento los motores están de camino a mi casa desde Hong Kong así que de momento no puedo hacer pruebas reales. Cuando pueda tendré que hacer pruebas con la relación de peso entre la posición calculada a partir de los acelerómetros y los gyroscopos.