A tener en cuenta en el control del robot

• Tanto la velocidad lineal como angular del robot se deben controlar suavemente, esto es, no se debe asignar una velocidad (ya sea angular o lineal) muy diferente a la que lleva el robot en un momento determinado, ya que esa gran diferencia de velocidad puede provocar comportamientos raros en el robot o incluso peligrosos llegando a volcar.

VFF (Virtual Force Field)

Esta práctica consiste en emplear la técnica de navegación local VFF (Virtual Force Field)  para que el robot navegue por el mundo con el objetivo de alcanzar un destino evitando chocar con obtáculos.

Para ello la única información de la que se dispone es el punto al que debe llegar y las medidas ofrecidas por el láser que lleva incorporado en la parte frontal.

Para resolver el problema empleando VFF podemos distinguir dos fases: una primera fase en la que se obtendrá la fuerza de atracción hacia el punto al que se desea llegar, la fuerza repulsiva a partir de las medidas del láser y la combinación ponderada de ambas fuerzas, y una segunda fase en la que se comandará el robot en base a la fuerza resultante obtenida.

Fase I: composición de fuerzas

Fuerza repulsiva

La fuerza repulsiva se calcula en base a las medidas del láser. Para ello debemos traducir estas medidas en fuerzas respulsivas y representarlas como vectores relativos en el sistema de coordenadas del propio robot.

En la siguiente imagen podemos ver la relación entre las distintas componentes del láser (enumeradas del 0 al 179 en el sentido contrario a las agujas del reloj) y el sistema de coordenadas del robot (el eje X corta longitudinalmente al robot con sentido de atrás hacia adelante, y el eje Y corta transversalmente al robot con sentido de derecha a izquierda).

Atendiendo a esta relación, las distintas componentes del láser se podrían representar en el sistema de coordenadas del robot como vectores con origen en centro del robot (x=0, y=0) y extremo en x=sen(componente_laser)*medida(componente_laser), y=-cos(componente_laser)*medida(componente_laser)

Esta relación es importante para obtener los vectores que representarán las distintas fuerzas repulsivas a partir de las medidas del láser, y que tendrán el sentido contrario a la representación de las medidas del láser en el sistema de coordenas.

Una vez tenemos esto claro, únicamente nos faltaría asignar un módulo correcto a cada fuerza atendiendo a la idea básica que será inversamente proporcional a la medida del láser en la componente correspondiente (cuanto más cerca estemos de un obstáculo, más fuerza de repulsión habrá, y viceversa).

En concreto se ha empleado una función compuesta que da más importancia a unas componentes u otras dependiendo en qué rango se encuentre la medida correspondiente:

adjustDst = laser->distanceData[i];
if (adjustDst <= SECURE_DST)
     adjustDst = 1;
else if (adjustDst <= OBS_NEAR)
     adjustDst = adjustDst;
else if (adjustDst <= OBS_FAR)
     adjustDst = 2*adjustDst;
else
     adjustDst = 4*adjustDst;

force = 1.0 / adjustDst;

La fuerza de repulsión resultante será la suma vectorial de todas las fuerzas calculadas según lo explicado anteriormente.

Fuerza atractiva

Por su parte, la fuerza atractiva estará representada por un vector que apunte al destino donde el robot debe llegar y tendrá un módulo constante hasta que se encuentre lo suficientemente cerca del objetivo, cuyo módulo será proporcional a la distancia a dicho punto.

Fuerza resultante

La fuerza resultante será la suma vectorial de la fuerza repulsiva y atractiva ponderadas adecuadamente para que el robot no tenga demasiada prudencia y le impida avanzar hacia el objetivo ni sea demasiado "valiente" y se choque con cualquier obstáculo.

Fase II: comandar robot

El robot se comandará en base al ángulo que forme la fuerza resultante en el sistema de coordenadas del robot y a la distancia que se encuentre éste del objetivo.

A partir el ángulo que forma el vector asociado a la fuerza resultante se calcula un error que varía entre 0 y 1 según apunte en el mismo sentido que el robot o en sentido contrario, respectivamente, además de calcular en qué sentido tiene que girar el robot para orientarse en el sentido que marca dicho vector.

Dicho esto, en función del error obtenido tendremos dos casos:

• ALIGN: en el que prima que el robot se alinee con el vector resultante. Para ello se proporcoina un controlador PID que asigna velocidades angulares altas si hay mucho error reduciéndolas según se acerque a lo deseado, y velocidades lineales bajas.
• RECT: si el robot está prácticamente alineado con el vector resultante se aumentará la velocidad lineal y se mantendrán velocidades angulares para mantenerse alineado con el vector resultante en la medida de lo posible.

Sigue-líneas

Esta práctica consiste en programar un robot para que siga una línea roja en base a la información captada por una de sus cámaras, la cual será la única fuente información que se tendrá del entorno.

Es base a estas especificaciones el programa que controlará el robot está dividido en dos fases diferenciadas: una primera fase de obtención de información a partir de la cámara izquierda del robot y una segunda fase en la que se comandará el robot en base a la información obtenida.

Fase I: obtención de información

En esta fase se pretende sintetizar la información más relevante a partir de los frames que nos ofrece la cámara.

Análisis de la imagen

Para ello lo primero que se hace es analizar la imagen de la cámara para localizar la línea roja. Como la información que proporciona la cámara es bastante densa, analizar toda la imagen sería bastante costoso en tiempo, con lo que se ha decidido analizar únicamente las líneas negras transversales que se observan en la siguiente imagen:

En concreto, en el caso que nos atañe las líneas analizadas son: 239, 230, 220, 200, 180, 160, 140 y 130 (en una imagen de 320x240 píxeles).

Una vez elegidas las líneas a analizar, se aplica sobre cada una de ellas un filtro de color para detectar los píxeles de color rojo, los cuales nos "dirán" dónde se encuentra la línea roja (si es que se ve). Tras varias pruebas, el filtro que se utiliza para detectar si un píxel es rojo o no es el siguiente:

((r > 230) && (g < 25) && (b < 25)) ||

((r > 200) && (abs(g-b)<10) && (g < 150) && (b < 150))

, donde r, g y b son las componentes red, green y blue respectivamente de cada píxel.

Para detectar el punto medio de la línea roja (por cada línea de la imagen analizada), para cada píxel que supera el filtro de color se anota en qué posición horizontal se encuentra. Una vez analizada toda la línea, el punto medio de la línea roja es la suma de todas las posiciones de los píxeles que han superado el filtro dividido entre el número de píxeles que lo han pasado.

De esta forma obtenemos de forma fiable el punto medio de la línea roja, ya que puede haber píxeles fuera de la línea que superen el filtro y píxeles dentro de la línea que no lo superen (puede que haya "impurezas" en la imagen).

Por cada línea de la imagen analizada se indicará si se ha encontrado la línea roja y, si es así, se  hallará la diferencia entre este punto medio (real) y el esperado, determinando el error asociado.

Los puntos esperados están asociados a cómo se desea que la linea sea vista desde la cámara izquierda y que determina que el robot está justo encima de la misma. Esta imagen se ha obtenido empíricamente y sería algo así:

Para expresar matemáticamente esta recta esperada y poder calcular el punto medio esperado para cada línea transversal se ha sacado la siguiente ecuación:

w = 148,615 + 0.215*h , donde w representa la componente horizontal del píxel y h la componente vertical.

Por tanto, a partir de esta ecuación y las líneas que hemos decidido analizar los puntos medios esperados serán los siguientes (wxh): 200x239, 198x230,  196x220, 192x200, 187x180, 183x160, 178x140 y 176x130.

Detección de casos

Una vez analizada la imagen hay que sintetizar los datos extraídos para detectar en qué situación se encuentra el robot. En nuestro caso nos interesa saber si el robot está en alguna de las siguientes situaciones:

• SEARCH_LINE: si en las 3 líneas más cercanas no se detecta la línea roja.
• STRONG_CURVE: si en las 5 líneas más alejadas no se ha encontrado la línea roja.
• CURVE:  si en 4 de las 5 líneas más alejadas no se detecta la línea roja.
• RECT: en cualquier situación no contemplado en los casos anteriores.
• CENTER_IN_LINE: si se encuentra en una recta pero no encima de la línea roja.
• STRONG_CENTER_IN_LINE: si se encuentra en una recta pero no encima de la línea roja y con bastante error.

Combinación de los errores

Por último hay que combinar los errores calculados por cada línea analizada. Esta combinación se hace de forma ponderada dando más peso a las líneas más cercanas, lo que hará que el robot se comporte de una forma más segura previniendo choches en zonas más peligrosas.

Fase II: comandar el robot

A partir del caso en el que se encuentra el robot y del error obtenidos en la fase anterior hay que comandar el robot para que siga la línea adecuadamente. A partir de los casos detectados en la fase anterior, el robot responderá de forma diferente:

En el caso de SEARCH_LINE el robot dará marcha atrás con una velocidad baja y girará hacia el lado donde vió por última vez la línea roja.

Para el resto de casos se ha empleado un controlador PID para asignar la velocidad lineal (V) y otro para la angular (W), ambos específicos en cada caso:.

• Para STRONG_CURVE y CURVE se da prioridad a girar rápido y no avanzar demasiado para tomar las curvas mejor. Aún así, cuando se detecta que la curva no es demasiado cerrada se permite una mayor velocidad lineal que en curvas más cerradas.
• En el caso RECT, el robot tomará la mayor velocidad lineal posible y minimizará los giros bruscos para no salirse de la trazada.
• Para STRONG_CENTER_IN_LINE y CENTER_IN_LINE, el robot pretende situarse encima de la línea para evitar chocarse con posibles obstáculos (ya que es la única fuente de información) y alinearse lo máximo posible para llegar al estado de RECT cuando antes y aumentar la velocidad.

Primera práctica: choca-gira

La primera práctica que debemos realizar consiste en implementar un comportamiento de choca-gira, para habituarnos al entorno de desarrollo y a los distintos programas que necesitamos para arrancar el entorno de simulación.

Este comportamiento de choca-gira es bastante sencillo y consiste en hacer que el robot navegue por el entorno de forma "aleatoria" y si detecta algún obstáculo cambiar el rumbo para evitar chocar con él.

Para llevar a cabo este comportamiento se ha optado por hacer un diseño basado en autómatas de estado finito siguiendo el siguiente modelo:

 

 Este autómata está constituido por tres estados y la transición de uno a otro se produce si se cumplen una serie de condiciones:

• GO_AHEAD: en este estado el robot se moverá hacia delante con una velocidad lineal determinada. En cada iteración se comprobarán las medidas obtenidas con el láser (en concreto las correspondientes a las medidas de los ángulos 45, 90 y 135) y si se observa que hay un obstáculo demasiado cerca el robot se detendrá y se pasará al estado GO_BACK.
• GO_BACK: el robot retrocerá hasta que determine que la distancia al obstáculo u obstáculos es la suficiente para pasar al estado ROTATE.
• ROTATE: en este estado el robot esperará a estar parado antes de iniciar un giro sobre sí mismo en un sentido elegido al azar utilizando una función psudoaleatoria. Esto nos asegurará que el robot no gira a la vez que está retrocediendo debido a la inercia del estado GO_BACK y evitar de este modo un posible choque. Una vez parado, girará hasta determinar que no hay obstáculos en un cierto perímetro (utilizando para ello las medidas de los ángulos 45, 90 y 135 del láser) y pasará al estado GO_AHEAD.

 Parametrización

Todo lo comentado anteriormente está debidamente parametrizado para poder ajustar los parámetros afinando el comportamiento del robot. Los parámetros más importantes y los valores que toman son los siguientes:

• Velocidad lineal en GO_AHEAD: v=200 mm/s.
• Velocidad lineal en GO_BACK: v=100 mm/s.
• Velocidad angular en ROTATE: w=15 deg/s.
• Distancia al obstáculo en la que el robot pasa del estado GO_AHEAD a GO_BACK: <1000 mm (medida en 45, 90 ó 135 grados).
• Distancia al obstáculo en la que el robot pasa del etado GO_BACK a ROTATE: >1000mm (medida en 45, 90 ó 135 grados).
• Distancia al obstáculo en al que el robot pasa del estado ROTATE a GO_AHEAD: >1300 mm (medida en 45, 90 y 135 grados)

A tener en cuenta...

•  Inercia: el robot no responde de forma instantánea cuando comandamos los motores de las ruedas. Por ejemplo si el robot lleva una velocidad lineal de 200 mm/s y le decimos que se detenga (v=0), el robot no se detendrá hasta pasado un tiempo, ya que tiene que iniciar un proceso de frenado en el que irá disminuyendo la velocidad hasta detenerse. Es importante tener en cuenta que el tiempo de frenado depende de muchos factores y no es predecible, con lo que tendremos que parametrizar debidamente los comportamientos para evitar situaciones problemáticas como chocarse.

Presentación

En este blog personal iré comentando a lo largo del curso la evolución de las distintas prácticas de la asignatura de Robótica impartida en el Máster de Sistemas Telemáticos e Informáticos de la Universidad Rey Juan Carlos.

La mayor parte de la documentación que se necesitará así como los foros de resolución de dudas son accesibles desde el moodle:
docencia.etsit.urjc.es/moodle/course/view.php?id=23