GAZEBO - CONTROL POR CÁMARA

En esta práctica el objetivo que tendrá que superar nuestro robot será navegar por un escenario con la única ayuda de una cámara colocada en la parte frontal.

Los requisitos que deberá cumplir para considerar una navegación buena serán la velocidad a la que circula el robot y la capacidad de evitar obstáculos (estáticos y dinámicos). El escenario por el cual circula el robot es la simulación del departamental II de la Universidad.

Para explicar como hemos desarrollado la práctica vamos a dividir esta entrada en dos partes:

1. Introducción al sensor cámara y su procesamiento

2. Desarrollo del algoritmo de navegación basado en la cámara

1. Introducción al sensor cámara y su procesamiento

En primer lugar, vamos a explicar cuales son las características del entorno simulado del departamental II.

El entorno consta de pasillos(suelo y paredes), puertas y papeleras. El suelo es de color amarillo, las paredes grises, las puertas granate y las papeleras de color azul.

La primera imagen es la que se obtiene directamente a través del sensor cámara.

Esta imagen está formada por 320 columnas y 240 filas (ancho-alto). Es almacenada en memoria como un array unidimensional que contiene información de cada uno de los píxeles. La información que contiene es el valor de RGB de cada pixel. Por lo tanto, el formato de este array es el siguiente:

Tenemos que tener muy en cuenta el formato de este array para sacar la información que deseamos. Las fórmulas que hemos seguido para indexar las posiciones del array es la siguiente:

index=((fila*SIFNTSC_COLUMNS)+columna)*3;

b=(unsigned char)imagenRGB[ index ];

g=(unsigned char)imagenRGB[index+1];

r=(unsigned char)imagenRGB[index+2];

Para llevar a cabo un control por cámara se pueden utilizar varias técnicas:

*Filtros de bordes (Sobel)

*Filtros de colores

*Filtros en el dominio de la frecuencia

* ...

En nuestro caso, con un filtro por colores es suficiente para conseguir nuestro objetivo, ya que filtrando aquellas partes de la imagen que sean de color "pasillo" podemos saber donde estamos posicionados con respecto al escenario.

2. Desarrollo del algoritmo de navegación basado en la cámara

El actuador que permite el avance hacia delante tiene un velocidad continua, de este modo conseguimos una mayor velocidad (v=1000).

Para el actuador que permite el giro de nuestras ruedas hemos decidimos utilizar un controlador P (propocional ).

Controlador P:

La fórmula en la que nos hemos basado es la que hemos visto en las transparencias de clase:

w= -Kp*error

-Kp= constante de proporcionalidad obtenida de forma experimental.

El valor es (-1/2)

error=numPixelPasilloIzq - numPixelPasilloDch

Nuestro objetivo ha sido mantenernos en la posición central de lo que el robot considera pasillo, para ello intentamos que el numero de pixeles "pasillo" a la derecha e izquierda del robot sea el mismo. De esta forma conseguimos que si hay más pixeles a un lado que al otro, el robot corrija su posición de forma proporcional a esta diferencia, hasta que encuentre la posición óptima.

Dado que es un escenario en el que hay pocos obstáculos y es bastante uniforme, no se ha detectado que el robot realice movimientos muy bruscos siendo controlado, simplemente, con un controlador P.

En caso de que el escenario fuese menos uniforme o tuviera mayor cantidad de obstáculos o incluso obstáculos dinámicos sería necesario implementar un controlador PD que suavizaría los movimientos de corrección del robot.

3. Otras consideraciones interesantes

Hemos decidido procesar simplemente una linea de la imagen, en concreto, la linea que procesamos es la número 180. De esta forma conseguimos no cargar en exceso la aplicación (no procesa lineas que no aportan información ya que están recogiendo zonas alejadas).

La linea escogida ha sido la 180 de forma experimental. ¿Por qué no la última, la 240? Si se escoge dicha linea el robot es incapaz de reaccionar a tiempo cuando tiene un obstáculo delante, ya que tiene que estar prácticamente encima para poder detectarlo, y cuando intenta corregir su posición es demasiado tarde.

A continuación se puede visualizar un video con el funcionamiento del programa (nota: el video es más lento de lo que lo es la ejecución)

Si no puedes verlo pincha aqui

Vff en Cheste

Hemos seguido trabajando en el código de navegación vff para mejorarlo aún más.

• El calculo del ángulo de la fuerza total (gracias al cual decidimos cuanto debemos girar) lo hemos hecho con la función atan2 de C. Esta función nos devuelve el ángulo formado entre el robot y la fuerza total y nos lo da en el rango (-pi, pi). Antes lo teníamos con la funcion cos, pero esta función devuelve el resultado entre (0,pi) por lo cual pude dar problemas para ciertos ángulos ya que hay un mismo ángulo puede ser dado por distintos vectores, esto con la función atan2 no ocurre.
  
• Para el movimiento hemos considerado diferentes rangos y el signo del ángulo devuelto por la función atan2. Para evitar que el robot oscile continuamente cuando tiene que ir de frente hemos decidido que en el rango [-5,5] el robot no gire. Además entre [155,180] y [-180,-155] hemos decidido que el robot solo gire en un sentido, de esta forma cuando encontramos un obstaculo que hace que la fuerza total apunte hacia el opuesto al frontal, el robot no oscilará continuamente y girará hacia un lado esquivando el obstaculo.
• Hemos aumentado la velocidad lineal maxima hasta 800.

A continuación mostramos una serie de imágenes y videos que ilustran el funcionamiento de nuestro robot.

Recorrido del circuito sin poner obstáculos:
Tarda en dar la vuelta completa más o menos 3'15''

Si no puedes ver el video pincha puedes pinchar aqui
Recorrido del circuito con obstaculos:

Se puede observar en la imagen y en el video que nuestro robot apura bastante para esquivar el robot pero finalmente lo consigue y no se para. Los círculos verdes marcan donde ha sido colocado el obstáculo para ser esquivado.

Si no puedes ver el video pincha aqui

VFF: Mejoras y Cheste

Hemos realizado mejoras en el codigo de nuestro vff . Las mejores han sido las siguientes:

• Hemos hecho el calculo de las fuerzas repulsivas teniendo en cuenta lo que se ha explicado en el foro. Es decir, hacer los cálculos en el sistema de referencia que es más amigable con el robot. A continuación pondremos la fórmula utilizada:

fRepulsion.x=(cos(auxiliar2)*K)/(dist_Obstaculo);
fRepulsion.y=(sin(auxiliar2)*K)/(dist_Obstaculo);

donde auxiliar2 no es directamente el indice del laser que nos ha dado un posible obstaculo, sino que aplicamos un pequeña modificación para obtener directamente las fuerzas repulsivas en el mismo sistema de referencias que el robot. Esta pequeña modificación del ángulo es: (indice-90+180)*DEGTORAD;

• Otra modificación que hemos hecho es no considerar todos las zonas del robot iguales a la hora de calcular la fuerza repulsiva. Cuando detectamos un obstáculo en el frontal de robot hacemos que se tenga muy en cuenta, para evitar que nos estrellemos contra él. A estos valores de fuerza de repulsión le damos 5 veces más valor. También consideramos de forma especial a los obstaculos que encontremos a los lados del robot, hacemos algo parecido que con el frontal, pero aumentando la fuerza de repulsión 3 veces
  

• Otra cosa que hemos tenido en cuenta es si nos encontramos muy cerca de un obstaculo disminuiremos la velocidad lineal, de esta forma, evitamos que por ir muy rápido no nos de tiempo a esquivar un obstáculo. Lo que hacemos es multiplicar la velocidad lineal por 0.7
  

• El algoritmo de movimiento lo seguimos calculando en función del ángulo. Si el frontal del robot está centrado (entre 85º y 95º) nos moveremos recto, sin giros, en otro caso, giraremos más o menos en función del ángulo.
  

A continuación pondremos dos videos, uno del vff en el circuito simple y otro en el de Cheste.
El tiempo que tardamos en dar la vuelta completa al circuito de Cheste es 3'35''

CIRCUITO SIMPLE:

CIRCUITO CHESTE:

PRACTICA 2: NAVEGACION VFF

En una navegacion vff, el robot está sometido a un campo potencial, donde el punto de destino del robot ejerce una fuerza de atraccion (Fa) sobre el robot y los obstáculos generan una fuerza de repulsion (Fr) sobre el robot, por lo que se moverá en la dirección de la fuerza resultante(Ft) que se obtendrá al sumar la Fa y la Fr:
A continuacion explicaremos como hemos calculado cada una de estas fuerzas:


Fuerza de Atraccion (Fa)

Fa_X=Katraccion * (Xo - Xf)
Fa_Y=Katraccion * (Yf - Yo)

Si Xo,Yo=0

Fa_X=Katraccion * (Xf)
Fa_Y=Katraccion * (Yf)





Fuerza de Repulsion (Fr)
Detectamos una fuerza de repulsión cuando el láser detecta una barrera a una distancia de 1000 o menos, si el obstáculo está más lejos, éste no generará ninguna fuerza de repulsión.
Además si nos encontramos muy muy cerca del objetivo la fuerza de repulsión será nula para evitar que si el objetivo está muy cerca de una barrera el robot se mueva alrededor del objetivo sin llegar a él

El cálculo de la fuerza de repulsión lo hemos realizado a traves de la siguiente formula:
Fr=sum(Fr(i))K/dist_obstaculo
La constante K ha sido elegida a través de las prueba realizadas con el entorno, el valor de K es 35000


Fuerza Total (Ft)
Ft=Katraccion*Fa + Krepulsion*Fr
( Katraccion= 1-Krepulsion rango de la K es [0-1] )


Algoritmo del movimiento
El algoritmo de movimiento lo hemos calculado basándonos en la diferencia entre el ángulo formado por la fuerza total y el que formal a posición del robot.
Si la resta de esta ángulo está entre [0 - 180) (en valor absoluto) comprobamos si la resta es positiva o negativa,para girar en un sentido u otro. Además comprobamos si estamos más cerca o lejos del ángulo objetivo, para girar más rápido o más lento.

Si la resta de esta ángulo está entre [180 - 360] (en valor absoluto) comprobamos si la resta es positiva o negativa,para girar en un sentido u otro. Además comprobamos si estamos más cerca o lejos del ángulo objetivo, para girar más rápido o más lento.

Se puede observar el comportamiento del robot en el siguiente video
También podeis ver el siguiente video:

PRACTICA1 : CHOCA-GIRA (versión 2)

Hemos corregido las cosas que se nos pidió en la entrega. Los cambios realizados han sido los siguientes:

• Hemos programado el robot como un autómata de estados (tal y como se explicaba en la entrada anterior), ya que en la primera versión no se consiguió este comportamiento en la programación.
• Hemos hecho que nuestro robot vaya a la máxima velocidad (v=1000) y que funcione bien. También hemos detectado que ha mayor velocidad más probable es que el robot detecte el obstáculo cuando está más cerca de él.
• Hemos conseguido que el tiempo que el robot gira hacia la derecha o la izquierda sea aleatorio.

Para ver el funcionamiento del robot podeis pinchar en el siguiente enlace o bien puedes verlo en el video que aparece a continuación:

PRACTICA 1: CHOCA - GIRA

El objetivo de esta práctica es programar el robot de tal forma que sea capaz de avanzar y cuando detecta un obstáculo justo delante de él parará y girará un tiempo determinado para esquivar el obstáculo y volverá a seguir avanzando.
Para implementar este tipo comportamiento vamos a utilizar un autómata de estado finito que se compone de los siguientes estados: Avanza, Retrocede, Gira.
A continuación exponemos un diagrama que indica como se procederá para pasar de un estado a otro:



A continuación os pongo algunos datos que me parecen interesantes:
*La distancia máxima que vamos a permitir al robot acercarse a un objeto que tenemos delante será de 350. De esta forma nos aseguramos que el robot no se chocará con ningún obstáculo, y en el caso de quererlo llevar al mundo real, el hardware del robot no sufriría daños físicos.

*Hacemos distinción del lado del cual el robot recibe información de que hay un obtáculo.
Si el obstáculo está en el lado izquierdo giramos al lado derecho y si el obstáculo está en el lado derecho giraremos a la izquierda. Así evitamos que nos quedemos encerrados en rincones y optimizamos el movimiento del robot.

*Hemos hecho un programa modularizado de forma que el código pueda ser reutilizable y sea más sencillo de entender.

A continuación mostramos un video con el funcionamiento de nuestro choca-gira:




Si no puedes ver el video pincha en este enlace

Practica 1: Entorno de Trabajo

*** Clase del día 15/02/2010 ***

En esta primera clase nos hemos familiarizado con el entorno de trabajo de Robótica.

A modo de nota recordatoria vamos a poner los comandos necesarios para ejecutar tanto el entorno de gazebo como el entorno de stage.

ENTORNO GAZEBO

~/Gazebo/gazebo_siguelinea/wxgazebo

~/Gazebo/gazebo_siguelinea/wxgazebo pioneer2dx.world

~/jdeRobot/jderobot pioner_gazebo

ENTORNO STAGE

~/stage/stage_simple/player

~/stage/stage_simple/player simple2.cfg

~/introrob/jderobot pioner-stage

¿COMO SE PROGRAMA NUESTRO PRIMER PROGRAMA?

1. Modificamos el código que hay en ~/introrob/navegacion.c

2. Tenemos que modificar la función "your_code"

3. Una vez modificado el código lo compilamos:

~/introrob/make

4. Copiamos el .so que se nos ha generado a la carpeta de plugings:

cp ~/introrob/introrob.so ~/jdeRobot/

5. Por último ejecutamos nuestro codigo

~/introrob/jderobot pioner-stage