La IMU de la placa Arduino Nano 33 BLE Sense está formada por el chip LSM9DS1 que posee 9 grados de libertad (9DOF), es decir, este chip es capaz de medir 9 magnitudes independientes correspondientes a:
-
- Un acelerómetro de 3 ejes (3DOF).
- Un giroscopio de 3 ejes (3DOF).
- Un magnetómetro de 3 ejes (3DOF).
En nuestro caso para controlar el robot nos interesa obtener la rotación del eje X (Roll) y la rotación del eje Y (Pitch) prescindiendo del eje Z (Yaw).
¿Pitch, Roll, Yaw? pero esto ¿qué es?… existe mucha literatura al respecto pero os recomiendo que visiteis el blog de Luis Llamas donde tiene una entrada donde explica claramente los conceptos básicos de este tipo de dispositivos y así os quedará todo mucho más claro:
https://www.luisllamas.es/medir-la-inclinacion-imu-arduino-filtro-complementario/
Como bien se explica en el post anterior los acelerómetros no son fiables a corto plazo y los giroscopios tienen deriva (drift) a medio plazo con lo que la mejor manera de obtener los ángulos que nos interesan es combinar los mejor de ambos sensores y eso se realiza mediante la utilización del denominado “filtro complementario” el cual se comporta como un filtro de paso alto para la señal del giroscopio y un filtro de paso bajo para la señal del acelerómetro para hacer de esta manera que la señal del giroscopio mande a corto plazo y la señal de del acelerómetro a medio y largo plazo.
El filtro complementario obedece a la siguiente fórmula:
El problema es que una vez implementado el sketch con el filtro complementario he visto que la variación de los ángulos no se produce de forma inmediata y si por ejemplo se pasa rápidamente de una inclinación de 0º a 90º este incremento se hace de manera progresiva (tarda del orden de segundos) y por consecuencia no resulta útil en nuestro caso para controlar el robot puesto que queremos una respuesta más inmediata.
Por tanto una vez visto que la utilización del filtro complementario no era válido para el propósito de este proyecto he optado por utilizar solo la señal del acelerómetro para determinar la inclinación de la placa que a pesar que éstos se ven influenciados por los movimientos del sensor y el ruido para nuestro propósito será más que suficiente.
Por tanto las fórmulas que finalmente usaremos para obtener el Pitch y el Roll serán las siguientes:
NOTA: Estos ángulo irán varían de -90º a + 90º.
A nivel de código utilizaremos las siguientes instrucciones para representar las fórmulas anteriores:
pitch = atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ)); roll = atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ));
Utilizaremos la librería de Arduino LSM9DS3 para obtener la información del acelerómetro, giroscopio y magnetómetro.
https://www.arduino.cc/en/Reference/ArduinoLSM9DS1
Los rangos que se pueden ajustar son:
-
- Acelerómetro: ±2/±4/±8/±16 g.
- Magnetómetro:: ±4/±8/±12/±16 gauss.
- Giroscopio: ±245/±500/±2000 °/sec.
La librería de Arduino LSM9DS3 nos viene con estas inicializaciones por defecto:
-
- Acelerómetro: ±4 g.
- Magnetómetro:: ±4 gauss.
- Giroscopio: ±2000 °/sec.
Pero tenemos el problema que la librería nos proporciona los valores en el sistema internacional, es decir, los valores del acelerómetros los expresa en en G’s, el giroscopio en °/sec y el magnetómetro en uT y a nosotros nos interesan los valores en bruto (RAW) para poder utilizarlo en las fórmulas que hemos visto anteriormente. Por ello tendremos que modificar ligeramente la librería para que nos devuelva los valores en bruto.
Para ello tendremos que comentar el trozo de código que utiliza la librería para expresar los valores de los sensores al sistema internacional y dejar el código para que exprese los valores en bruto. Realizaremos los siguientes cambios en fichero LSM9DS1.cpp de la librería:
nt LSM9DS1Class::readAcceleration(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_XL, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
/* Esta parte es la que hay que comentar *//*
x = data[0] * 4.0 / 32768.0;
y = data[1] * 4.0 / 32768.0;
z = data[2] * 4.0 / 32768.0;
*/
/* Esta parte es la que hay que añadir */
x = data[0];
y = data[1];
z = data[2];
return 1;
}
int LSM9DS1Class::readGyroscope(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_G, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
/* Esta parte es la que hay que comentar *//*
x = data[0] * 2000.0 / 32768.0;
y = data[1] * 2000.0 / 32768.0;
z = data[2] * 2000.0 / 32768.0;
*/
/* Esta parte es la que hay que añadir */
x = data[0];
y = data[1];
z = data[2];
return 1;
}
int LSM9DS1Class::readMagneticField(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS_M, LSM9DS1_OUT_X_L_M, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
/* Esta parte es la que hay que comentar *//*
x = data[0] * 4.0 * 100.0 / 32768.0;
y = data[1] * 4.0 * 100.0 / 32768.0;
z = data[2] * 4.0 * 100.0 / 32768.0;
*/
/* Esta parte es la que hay que añadir */
x = data[0];
y = data[1];
z = data[2];
return 1;
}
NOTA: Aunque solo vamos a utilizar los valores del acelerómetro también hemos dejado los valores en bruto del giroscopio y el el magnetómetro para mantener la coherencia.