Robot esquiva obstáculos con Arduino

De Jose Castillo Aliaga
Ir a la navegación Ir a la búsqueda

En este artículo vamos a construir un robot muy específico con Arduino, un robot que esquiva obstáculos y consigue salir de un laberinto.

En Internet encontrarás muchos tutoriales, la mayoría utiliza diferentes drivers para los motores. Muchos tienen un sólo sensor ultrasónico que se mueve con una torreta hecha con un servo. Elijas el que elijas, fíjate en que el código es distinto. El robot que describe este artículo tiene el Arduino Motor Shield Rev3 y 3 sensores ultrasónicos. Para moverse, utilitza dos ruedas motrices y una pivotante que, en nuestro caso, está situada en la parte delantera.

Construcción del Robot

Materiales

El chasis que verás en las fotos es específico, creado a medida por la empresa What's Next? para el proyecto [Robots Boost Skills]. El resto de componentes son genéricos y se pueden comprar Arduinos oficiales, What's Next Yellow o cualquier clon compatible.

Esta es la lista de materiales:

  • Chasis que permita 2 ruedas con motor DC analógicos y una rueda delantera.
  • 2 Motores DC analógicos con reducción y ruedas.
  • Arduino Uno o equivalente.
  • 3 Sensores ultrasónicos HR-SC04 o SRF05
  • Arduino Motor Shield o alguno que tenga el mismo Chip L298
  • Baterías, entre 9V y 12V

Fotos

Construcción del Chasis

En el caso del robot del ejemplo, el chasis tiene todos los elementos necesarios. Si lo tienes que construir, aquí tienes algunos consejos:

  • Se recomienda cuidar el centro de gravedad de robot para que no plante rueda y tenga la adherencia necesaria. Por ejemplo, las baterías deberían estar entre las ruedas motrices y la rueda delantera.
  • Los sensores deberían estar situados de forma que se cubra el mismo ángulo por los dos lados. En nuestro caso, al haber 3 sensores, se han puesto uno en el centro hacia adelante y los otros en un ángulo de 45º.
  • Hay que dejar espacio para los cables y para poder modificar las conexiones sin necesidad de desmontar todo el robot.

Cableado

Hay que tenen en cuenta que el Arduino Motor Shield utiliza algunos pines para su funcionamiento:

Function 	A 	 B
Direction 	D12 	D13
PWM 	        D3 	D11
Brake 	        D9 	D8
Curr. Sensor 	A0 	A1

Por tanto, no podemos utilizar esos pines. Cada sensor necesita 2 pines digitales para el echo i el trig. Así que necesitamos ocupar 6 pines digitales, más los 6 para los motores. Para los sensores utilitzaremos los pines 2,4,5,6,7,10.

Este es el esquema de cables del robot:

Robot bb.png

Alimentaremos tanto al Arduino como al Shield por la entrada del shield. Este alimenta al arduino por el pin VIN. Como el voltaje va a ser moderado, no es necesario aislar el Arduino del shield. Si los motores necesitaran más de 9V hay que cortar el pin VIN y alimentar por separado el Arduino.

Programación del Robot

Como este artículo pretende ser didàctico, no vamos a poner la solución final diréctamente (tampoco hay una solución final perfecta). Iremos poniendo versiones distintas en las que se explican las sucesivas mejoras.

Leyendo de los sensores

El siguiente ejemplo lee la distancia de los sensores y la imprime por el puerto serie:

//Declaración de las variables de los pines:
int trigiz = 7;
int echoiz = 10;
int trigcen = 5;
int echocen = 6;
int trigder = 2;
int echoder = 4;
int distiz, distcen, distder; //Las variables de las distancias

void setup()
{
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
}
int ping(int trig, int echo){
  digitalWrite(trig,LOW);  
  delayMicroseconds(2);    // Deja en LOW 2 microsegundos para "resetar" el sensor
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);   // Envía un pulso durante 10 microsegundos
  digitalWrite(trig,LOW);
  int duration = pulseIn(echo,HIGH); // Capta el tiempo que pasa hasta que retorna el pulso.
  int distance = duration * 0.034 / 2; // Cálculo de la distancia en cm
  return(distance);
}

void loop()
{
  distiz = ping(trigiz,echoiz);   // Llama a la función ping para el sensor izquierdo.
  Serial.print("Izquierda: "); Serial.print(distiz);
  delay(50);
  distcen = ping(trigcen,echocen);
  Serial.print(" Centro: "); Serial.print(distcen);
  delay(50);
  distder = ping(trigder,echoder);
  Serial.print(" Derecha: "); Serial.println(distder);
  delay(50);
}
El sensor tiene aproximadamente 55º y es más efectivo en la zona sombreada.

Como se ve, necesita enviar un trig de 10 microsegundos y escuchar hasta que vuelve. Después calcula la distancia teniendo en cuenta la velocidad del sonido.

El problema del ejemplo anterior es que al echo le pueden afectar muchos factores físicos del mundo real y puede que la medición no sea ajustada. Por eso, es mejor hacer varias mediciones, descartar valores extremos y calcular la media. Todo esto tiene un coste computacional que puede que nos afecte al rendimiento, por lo que puede que calcular la media o no hacer nada sea suficiente. Si queremos saber más, podemos buscar por filter o smoothing de los sensores en un buscador.
Multiplicar por 0.034 y dividir entre 2 puede ser costoso para algunos microcontroladores. Se puede meter duration / 58 que es una aproximación suficiente y más fácil de hacer. De hecho, no es necesario transformar a cm, se puede trabajar directamente con el valor del tiempo.

La lectura de los sensores no es muy precisa por las propiedades del sonido. Por un lado, los objectos excesivamente blandos, pueden absorber el sonido y los objectos lisos y reflectantes del sonido pueden hacer que rebote en otra dirección y no sea detectado por el sensor. Estos son los falsos negativos.

También pueden ocurrir falsos positivos si se tarda demasiado poco en hacer otra medición y detecta el sonido rebotado de la anterior o de otro sensor. En nuestro caso hay tres sensores y puede que los triggers de unos afecten a otros. En este sensor se recomienda no hacer una medición hasta al menos 50ms después.

Para mejorar y entender los sensores hay que pensar en el ángulo efectivo de los mismos. Este es de unos 55º, aunque es más preciso por el centro a medida que el obstáculo está más lejos. De esta manera, es capaz de captar obstáculos a una distancia de 1.5m en una amplitud de aproximádamente 1m. Se pueden producir falsos negativos a poca distáncia por el ángulo ciego del sensor y a mucha distáncia se pueden producir falsos positivos al detectar objetos que no están en el camino recto del robot.

Mover el Robot

En nuestro proyecto, los motores se mueven con el Arduino Motor Shield rev3. Aquí hay un ejemplo de cómo mover los motores adelante y atrás:

void setup() {
  //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin
  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel A pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel A pin
}

void loop(){
  //Motor A forward @ full speed
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  //Motor B backward @ half speed
  digitalWrite(13, LOW);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);    //Spins the motor on Channel B at half speed

  delay(3000);

  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B

  delay(1000);
 
  //Motor A backward @ full speed
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  //Motor B forward @ full speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);   //Spins the motor on Channel B at full speed
 
  delay(3000);
 
  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B
 
  delay(1000);
}

Funciones

La programación de este robot puede ser muy larga y complicada. Hay algunas cosas que se hacen siempre igual y que no es necesario volver a copiar y pegar el código. Por ejemplo, la lectura de los sensores es igual o el movimiento del robot.

En el ejemplo de los sensores hay una función para leer de los sensores:

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  int duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

También se puede hacer una función para moverse:

void front(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);   //Spins the motor on Channel B at full speed
}
void left(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, HIGH);   //Disengage the Brake for Channel B
  analogWrite(11, 0);      // <-- Stop the B motor
}
void right(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, HIGH);   //Disengage the Brake for Channel A
  analogWrite(3, 0);    // <-- Stops the A motor
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);     
}

Incluso se podría hacer que la función recibiera el tiempo que deseamos que esté moviendose y la velocidad:

void front(int time, int speed){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, speed);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, speed);   //Spins the motor on Channel B at full speed
  delay(time);
}

Cambiar de dirección en función de los sensores

Una vez ya sabemos cómo mover el robot y leer de los sensores, debemos saber utilitzar la información de los sensores para decidir la dirección del robot.

Primera aproximación: Ir hacia el lado con más distancia

La primera aproximación a esto es ir hacia el lado que más distancia lee del robot. Más adelante ya se intentará que sea un poco más inteligente. Para ello, hay que modificar la función loop() para llamar a las funciones de lectura de los sensores:

void loop()
{
  distiz = ping(trigiz,echoiz);
  Serial.print("Izquierda: "); Serial.print(distiz);
   delay(50);
  distcen = ping(trigcen,echocen);
  Serial.print(" Centro: "); Serial.print(distcen);
  delay(50);
  distder = ping(trigder,echoder);
  Serial.print(" Derecha: "); Serial.println(distder);
  delay(50);
  if(distiz > distcen && distiz > distder){
   left(0,130);  // Tiempo y velocidad  
  }
  if(distder > distcen && distder > distiz){
   right(0,130);  
  }
  if(distcen > distder && distcen > distiz){
   front(0,130);  
  }
}

El problema es que cuando el robot tiene un obstáculo, por ejemplo, a su derecha, pero está acercándose en diagonal al mismo.

Arduino sensors 1.png

En este caso, detecta que la distancia de su derecha es menor que la central. De hecho, el sensor del centro está calculando distancias muy grandes porque el sonido rebota y se pierde. Por tanto, decide ir al centro aunque, evidentemente, la distancia a la izquierda es mayor.

Para evitar este fallo, debemos plantear la siguiente estrategia:

Segunda aproximación: Distancia mínima y corregir el rumbo:

En primer lugar, hay que establecer una distancia mínima permisible. Si esta distancia es alcanzada por alguno de los sensores, debe corregirse el rumbo en dirección contraria. En el caso contrario, puede ir hacia delante o hacia donde más distancia detecte.

Algoritmo arduino robot1.png

void loop() {
  // front distance check
  ///distcen = ping(trigcen,echocen);
  distcen = ping(trigcen,echocen);
  if (distcen < DistanciaMaximaCentre) {  // 25 en el centre i 20 als costats
    Serial.println("Massa prop");
    distder = ping(trigder,echoder);
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distiz < distder)
      right();
    else if (distiz > distder) {
      left();
    }
  }
  else {
    Serial.println("OK");
    front();
  }
  // left distance check
  distiz = ping(trigiz,echoiz);
  if (distiz < maxLeftDistance) {
    Serial.println("Left too close");
    delay(20);
    distiz = ping(trigiz,echoiz);
    delay(20);
    distder = ping(trigder,echoder);
    delay(20);
    if (distiz > distder)
      front();
    else if (distiz < distder) {
      right();
    }
  }
  // right distance check
  distder = ping(trigder,echoder);
  if (distder < maxRightDistance) {
    Serial.println("Right too close");
    delay(20);
    distder = ping(trigder,echoder);
    delay(20);
    distiz = ping(trigiz,echoiz);
    delay(20);
    if (distder > distiz)
      front();
    else if (distder < distiz) {
      left();
    }
  }
}

Refinando el algoritmo:

Si hemos ido mirando el monitor del puerto serie, habremos visto que a veces tiene lecturas muy extremas cuando no funciona del todo bien el sensor. Si nos fiamos de una sola lectura, puede cambiar el rumbo hacia el lugar que no nos interesa. Por eso, podemos sacar la media de varias lecturas, de manera que se suavizan los valores extremos. Para hacerlo, podem usar una media móvil, de manera que los valores anteriores influyen en la distáncia media actual.

La media móvil tiene en cuenta unos cuantos valores anteriores.

 Lectura de sensores	  200	200	200	200	7	200	200	7	7	5
 Media móvil N=3	 		200	200	135	135	135	135	71	6

Sin la media móvil, el robot pararía en la lectura 7, pero no para. Luego cuando ya lee 7,7 y 5, baja la media y para.

Para poder hacer la media móvil, la manera más fácil es con un buffer circular para el que hay librerías para arduino.

En cualquier caso, hay lecturas demasiado extremas con valores negativos o demasiado altos. En ese caso, se puede establecer un máximo y un mínimo aceptable como medición y repetir la medición las veces que haga falta hasta que salga un valor aceptable con el que luego sacar la media móvil.

Enlaces