Matriz de rotación para IMU con numpy

Convirtiendo los valores de la IMU

En posts anteriores hemos visto como conectar y leer el sensor BMX055 al ESP32. De aquí obteníamos los valores de acelerómetro y giróscopo que medían los sensores en cada momento. Sin embargo, generalmente toda esta información la usamos para mostrar o calcular otros datos.

En el caso de KartTracker, los datos de la IMU los queremos a modo de experimentación, para poder medir las aceleraciones que sufrimos en los Karts ( y ya de paso hacer una comparación entre como conducimos cada uno). Podemos sacar las aceleraciones que tenemos en el paso por curva… Cómo frenamos…

El problema que tenemos a la hora de medir esto radica en que las IMU no están todas orientadas en el mismo sentido. Cuando estamos sentados en el Kart, la IMU se coloca de una manera diferente en cada uno. Por esto, no se puede hacer una comparativa de los datos en bruto de los sensores. En la imagen podemos ver lo que sería la IMU descolocada en referencia al sistema de coordenadas global. Y poniendo el ejemplo de la gravedad, en el sistema real la gravedad va siempre hacia abajo, mientras que la IMU lee la gravedad con componentes en todos los ejes.

Medidas originales frente a medidas de la IMU

Para poder hacer la comparación de los datos tendríamos que organizar los ejes de aceleración longitudinal y lateral en el mismo sentido. Es decir: El eje Z apuntando hacia arriba (contrario a la gravedad), el eje y debería apuntar hacía delante del kart para marcar la aceleración o desaceleración “en recta”, mientras que en eje X apuntará hacía la derecha del kart. En las siguientes imágenes podemos ver lo que habría en un principio donde, cuando el kart está en reposo la IMU tiene valores en varios ejes. Lo que pretendemos conseguir es la segunda imagen. Con las transformaciones que veremos más adelante, podemos obtener los datos de forma que en reposo, solo la componente Z tenga aceleración (la de la gravedad).

Datos de la IMU antes de la transformación

Datos de la IMU transformados

Para hacer todas estas transformaciones necesitamos de aplicar algo de álgebra y trigonometría.

Nuestras amigas las matemáticas

Como no podía ser de otra forma, nos topamos con nuestras amigas las matemáticas. Hay diferentes formas de llegar a la solución de este problema. Principalmente se usan los ángulo de Euler pero en nuestro caso es más complicado calcular los ángulos que tenemos que rotar nuestro sistema, que la rotación en sí.

Esto que tenemos que hacer se conoce en álgebra como un cambio de base. Y hay millones de páginas con mucha información sobre el tema. Como no me gusta seguir los métodos canónicos vay a hacerlo con el siguiente procedimiento (el cuál ha resultado funcionar bastante bien):

– Con el vector de gravedad podremos calcular el eje Z directamente (será el contrario a la gravedad)
– Hacemos la proyección del vector actual Y, sobre el plano que tiene al nuevo Eje Z (lo llamaremos Z’) como normal.
– Calculamos el Eje X como el vector perpendicular a Z e Y
– Giramos el sistema al rededor del eje Z, hasta que la X y la Y se orienten según la premisa anterior ( Y: Dirección del movimiento, X: aceleración lateral)

El plano

Por suerte en matemáticas, los planos se expresan con las coordenadas de sus vectores normales de la forma Ax + By + Cz = 0, donde A,B y C son las coordenadas de la normal. Teniendo en cuenta que nuestro vector z es es contrario a la gravedad, entonces:

Esta ecuación nos da los puntos del mundo 3D, que pertenecen al plano. Es decir, cualquier punto que esté en el plano tiene que cumplir la ecuación.

Hasta aquí, de momento fácil, no?

Vamos con la proyección.

Aquí la cosa se empieza a complicar. Antes de orientar bien los ejes X e Y tenemos que generar nuestro sistema ortogonal con el eje Z bien colocado, para que no afecte a las medidas. Así que la manera más fácil de obtener el eje Y es haciendo la proyección sobre el plano.

Para obtener el nuevo eje Y (Y’) hay que obtener la componente de Y que proyecta sobre Z’, y restar a Y esa proyección, de forma que nos queda de Y solo la componentes ortogonal a Z’.

– Módulo de la componente de Y que es parte de
– Componente completa de Y que es parte de
– Resta para obtener Y’:

El eje X’

Ahora mismo, ya tenemos Z’ e Y’, por lo tanto con un simple producto vectorial podemos obtener X’

Vale pero… ¿Cómo se implementa?

Para empezar, tenemos que importar todos los paquetes que vamos a usar a lo largo de nuestro programa:

import pandas as pd
import numpy as np

Primero las funciones de algebra

Lo primero que vamos a hacer es implementar las funciones para poder hacer cada uno de los pasos descritos antes, de una manera más sencilla. Empezamos implementando una función que encuentro bastante importante, cómoda y sencilla, es la función para normalizar vectores.

def Normalizar( vec ):
    return np.array(vec)/np.linalg.norm(vec)

Además, necesitamos hacer la proyección de un vector sobre un plano. Para ello podemos hacer una función que acepte la normal del plano y el vector que queremos proyectar. Esta función implementa la parte de la teoría.

def ProjVecToPlane( vec, plane ):
p1 = np.dot(vec, plane)
p2 = p1 * plane
return (vec - p2)

Como el producto vectorial, que es la parte que nos falta para terminar todos los procesos que ya hemos explicado, ya está implementado por numpy podemos ir directamente a la función que nos calcula todo. En este caso vamos a aceptar como parámetro de entrada el vector que marca la dirección de la gravedad y vamos a obtener a partir de ahí la transformación.

def GetRotMatrixFromGravity(vecg):
EjeZ = -Normalizar(vecg)
EjeY = Normalizar(ProjVecToPlane([0,1,0], EjeZ))
EjeX = Normalizar(np.cross(EjeZ, EjeY))
return [EjeX, EjeY, EjeZ]

Y ahora como aplicamos esta matriz?

La manera de usar la matriz que obtenemos de la función anterior es aplicando una multiplicación de matrices usando la función ‘dot’ de numpy.

# Suponemos que tenemos una variable con todas las mediciones de la IMU
imu_data = [ [x1, y1,z1], [x2,y2,z2] , ... ]

# Y que sabemos que en la muestra N la IMU está en lo que se considera "recto"
vecg = imu_data[N]
vecg = Normalizar(vecg)

# Calculamos la matriz de transformación
Mat = GetRotMatrixFromGravity(vecg)

# Aplicamos la matriz a cada dato recibido de la IMU
corrected = []
for i in range(len(imu_data)):
corrected.append(np.dot(Mat, imu_data[i]))

De esta manera en la variable ‘corrected’ tenemos los valores de la IMU ya reorientados.

Espero que os haya servido de ayuda!

Un saludo!
Mr.Cuit