Robot controlado por acelerómetro

Introducción

En este proyecto vamos a utilizar la IMU (Unidad de Medición Inercial) presente en la placa Arduino Nano 33 BLE Sense para controlar un robot. La idea es que en función de la inclinación que le demos a la placa generemos los movimientos correspondientes para controlar el robot.

El robot estará formado por una placa Arduino MKR WiFi 1010 acoplada a la placa Arduino MKR Motor Carrier. 

La comunicación entre el Arduino Nano 33 BLE Sense y el robot se realizará mediante Bluetooth Low Energy.

La placa Arduino Nano 33 BLE Sense actuará como nuestro mando a distancia y por tanto representará el dispositivo central o máster en la comunicación BLE (Bluetooth Low Energy).

Por otra parte el Arduino MKR WiFi 1010 acoplado al la placa Arduino MKR Motor Carrier actuará como receptor y representará el dispositivo periférico o esclavo en la comunicación BLE.

El funcionamiento será el siguiente:

    • El dispositivo central se conectará al dispositivo periférico buscando la característica del servicio especificado. 
    • Una vez establecida la conexión, el dispositivo central enviará (escribirá) los ángulos de inclinación detectados por la IMU del Arduino Nano 33 BLE Sense  (será una cadena de texto tipo: “ánguloRotaciónY,ánguloRotaciónX”) en la característica del servicio del dispositivo periférico que a su vez traducirá estos ángulos en movimientos que harán mover al robot.

Listado de componentes

Motores y ruedas

    • 2 x Motor con Reductora DC 3V – 6V 1:48.
    • Pareja de ruedas compatibles con los motores anteriores.
    • 1 rueda “loca” metálica.

Electrónica

    • Arduino Nano 33 BLE Sense.
    • Arduino MKR WiFi 1010.
    • Arduino MKR Motor Carrier. 
    • 4 cables de tipo Dupont macho – macho de 20 cm de longitud.
    • Mini protoboard de 170 puntos de color blanco.

Alimentación

    • 2 pilas Panasonic NCR18650B de Li-Ion recargables ( 3400 mAh, 3.7V).
    • Portapilas para dos pilas 18650 con cable alimentación.
    • Conector Hembra JST PH 2.54 mm.
    • Portapilas 4 x AAA 6v con cable alimentación, tapa e interruptor.

Chasis

    • Chasis de aluminio estilo mBot.
    • 4 separadores hexagonales metálicos hembra – hembra de 20 mm.
    • 8 tornillos de M3 (métrica 3) de 5/8 mm de longitud.

Montaje

El control remoto lo crearemos acoplando la placa Arduino Nano 33 BLE Sense a una mini protoboard de 170 puntos que a su vez engancharemos al portapilas gracias a la cinta adhesiva que tiene en su base. En la terminación de los cables de alimentación del portapilas hemos puestos dos conectores macho de tipo Dupont para poder alimentar de una manera sencilla la placa a través de los pines GND y VIN.


Los movimientos quedarán definidos en función de la inclinación que le demos al control remoto.

En la siguiente tabla se muestra la relación de los ángulos de los ejes X e Y (inclinación del control remoto) con los movimientos que se generan en el robot:

Movimiento Inclinación Y Inclinación X
avanzar (y >= 30º) (x > -20º),  ( x < 20º)
retroceder (y <= -30º) (x > -20º) , ( x  < 20º)
izquierda (y > -30º) ,  (y < 30º) (x >= 20º)
derecha (y > -30º) ,  (y < 30º) (x <= -20º)
avanzar + izquierda (y >= 30º)  (x >= 20º) 
avanzar + derecha (y >= 30º)  (x <= -20º) 
retroceder + izquierda (y <= -30º)  (x >= 20º) 
retroceder + derecha (y <= -30º)  (x <= -20º) 

La placa Arduino Nano 33 BLE Sense tiene un LED RGB incorporado y lo utilizaremos para indicar distintos los siguientes estados:

    • Color rojo: No se ha establecido todavía la conexión con el robot. 
    • Color azul: Se ha establecido la conexión con el robot pero no se están enviando todavía datos.
    • Color verde: Se ha establecido la conexión con el robot y se están enviado los datos de orientación pitch y roll.

Opcionalmente mediante dos gomas elásticas podemos acoplar el control remoto en la mano tal y como se muestra en la siguiente imagen y de esta manera controlar el robot:

Robot

El robot estará formado por la placa Arduino MKR WiFi 1010 acoplada al la placa Arduino MKR Motor Carrier que estará fijada a un chasis de aluminio (chasis que es un clon del utilizado en el robot mBot de Makeblock) . Todo el conjunto será movido por dos motores con reductora DC 3V – 6V 1:48  a los cuales soldaremos 2 cables de tipo Dupont macho – macho de 20 cm de longitud.

Tendremos que hacer 4 agujeros de 3 mm de diámetro en el chasis de aluminio del robot para poder poner 4 separadores hexagonales metálicos hembra – hembra de 20 mm de M3 (métrica 3) que servirán para fijar la placa Arduino MKR Motor Carrier al chasis del robot.

Todo el conjunto lo alimentaremos con 2 pilas Panasonic NCR18650B de Li-Ion recargables ( 3400 mAh, 3.7V) que estarán albergadas en un portapilas con cable de alimentación al que hemos añadido un conector hembra JST PH 2.54 mm. El portapilas quedará sujeto entre los 4 separadores hexagonales.

NOTA: Alternativamente también se puede alimentar la placa Arduino MKR Motor Carrier mediante las regletas azules de conexión y de esta manera no sería necesario disponer del conector hembra JST PH 2.54 mm en el portapilas pero en mi caso he optado por alimentarlo a través del conector de batería que tiene la propia placa porque resulta más fácil a la hora de quitar/poner la batería.

En la siguientes imágenes podemos ver todo el proceso comentado anteriormente:

Determinación de la orientación

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.

Programación

Skecth a cargar en el dispositivo central Arduino Nano 33 BLE Sense:    

https://github.com/avilmaru/robotRCporAcelerometro/blob/master/central/central.ino

Skecth a cargar en el dispositivo periférico Arduino MKR WiFi 1010:

https://github.com/avilmaru/robotRCporAcelerometro/blob/master/peripheral/peripheral.ino