Práctica 3 - Obstacle Avoidance

 

Introducción

En esta práctica programaremos y ajustaremos un algoritmo de VFF (Virtual Force Field) para hacer que el coche esquive obstáculos de manera automática mediante el uso del láser de 180 grados como sensor principal, guiándonos así, por las distancias a los objetos más cercanos de su entorno frontal.

Implementación

Primera aproximación: definición de fuerzas repulsiva y atractiva.

Definiremos la fuerza repulsiva como la suma vectorial de subfuerzas repulsivas (una por cada haz del láser, exceptuando los haces del láser cuya distancia indicada sea mayor o igual que la máxima, en cuyo caso, esa subfuerza no se tendrá en cuenta).

Definiremos una función que pase cada medición del láser (distancia) a una subfuerza repulsiva (vector), que tendrá siguiente forma:

 

Donde el eje de abscisas representa la distancia medida por el láser y el eje de ordenadas la subfuerza repulsiva resultante (módulo de nuestro vector).

Para conseguir nuestro vector solo necesitamos el ángulo (coordenadas polares) que forma con la horizontal, por lo que sumaremos 180 grados al ángulo del haz del láser para obtener el mismo ángulo pero en el sentido contrario (repulsiva).

En este caso hemos decidido que los objetos que están muy cerca aporten mucho más que los que están lejos, como vemos en la función, ya que queremos que cuando estén cerca nos repelan mucho y no chocarnos, pero que apenas afecten cuando estén muy lejos (a partir del punto B de la función la fuerza resultante será 0).

También definiremos la fuerza atractiva en función de la distancia al objetivo, por lo que tendremos una función lineal constante de la siguiente forma:

Donde se sigue la función de color verde desde el punto B hasta el punto A, donde se pasa a seguir la función de color azul, para no pasarse con la fuerza cuando el objetivo esté muy lejos, pero poder ir decrementándola a medida que nos acercamos.

 

Segunda aproximación: de fuerza total a movimiento del coche.

Definiremos la fuerza total como la suma vectorial ponderada de la fuerza total repulsiva y la fuerza atractiva, siguiendo la siguiente fórmula:

Ftot = αFatr + βFrep

Ahora traduciremos esta fuerza total resultante a velocidades, que al fin y al cabo es lo que entienden los motores del coche (o por lo menos la capa de abstracción del hardware o HAL, que nos ayuda a controlar esto). Lo haremos de la siguiente manera:

Tendremos un controlador de la velocidad lineal inversamente proporcional a la componente x del vector, y recordemos que el eje x positivo apunta hacia el frente del coche, por lo que cuanto mayor sea esta componente, mayor será la velocidad lineal, puesto que la fuerza nos atrae hacia el objetivo. En caso de ser negativa, mantendremos una velocidad lineal constante positiva muy baja para poder girar sin chocarnos con el obstáculo, ya que no queremos una velocidad negativa que nos haga dar marcha atrás, puesto que iríamos ciegamente al no tener ningún sensor a nuestras espaldas.

Mantendremos un controlador proporcional a la componente y del vector para la velocidad angular W, cuanto mayor sea dicha componente, mayor será la velocidad angular comandada al coche, y viceversa, cuando menor sea (negativa), menor será la velocidad angular (negativa también, indicando el sentido contrario).

Teniendo un buen resultado de fuerza total, podremos ver un comportamiento correcto en casos como los siguientes:

   

Haremos algunos ajustes y arreglos:

• Corregimos la componente y (horizontal respecto al coche) de la fuerza repulsiva, que estaba erróneamente invertida.
• Corregimos la componente y de la fuerza atractiva, que estaba erróneamente ponderada y como consecuencia era siempre cercana a 0, por lo que no apuntaba correctamente hacia el objetivo. Para las componentes de esta fuerza pusimos un máximo (independiente en cada componente) para no obtener fuerzas muy grandes cuando el objetivo estuviese muy lejos, como ya comentamos anteriormente.
  
• Con estos dos últimos arreglos y soluciones de bugs nos toca volver a calibrar los valores de las constantes, para obtener un nuevo y mejor funcionamiento.
• Quitamos las trazas una vez finalizamos para obtener una visualización más limpia.

Este es el nuevo resultado, funcionalmente similar al anterior:

 Vemos como ahora no se choca con la pared derecha en la curva larga, gracias los máximos en la fuerza atractiva.