UNIVERSIDAD DE OVIEDO E.P.S.

DE INGENIERA DE GIJN MASTER EN SISTEMA MECATRNICOS Y MICROMECATRNICOS (EU4M)

OBSTACLES AVOIDANCE STRATEGIES FOR VISUAL SERVOING OF A MOBILE ROBOT

Estudiante: Daniel Casagrande Viviane Cadenat - LAAS - Toulouse (Francia) Tutores: Adrien Durand Petiteville - LAAS - Toulouse (Francia) Nadine Piat - ENSMM - Besanon (Francia) Miguel ngel Jos Prieto - UNIOVI - Gijn (Espaa) Lugar de trabajo: Laboratoire d'Analyse et d'Architecture des Systmes - CNRS Toulouse, Francia.

1. Resumen
Realizar complejas tareas de navegacin en entornos desconocidos es un problema muy rico. Diferentes enfoques pueden ser utilizados para resolver el problema. Uno de ellos es la utilizacin de la informacin proveniente de una cmara de video, pudiendo ser sta embarcada o no. Esta tcnica, llamada control visual, es limitada en entornos con obstculos. Para realizar la navegacin en estos casos es recomendable emplear sensores proximtricos, como un telmetro laser, adems de la informacin visual. Durante esta prctica trabajamos con un robot no holonmico, implementando un control visual 2D y dos tcnicas para evitar obstculos: path following y artificial rotating potential field. Tambin exploramos la posibilidad de utilizar el detector de puntos de inters SURF para realizar navegaciones empleando referencias naturales.

2. Objetivos
Se definieron dos objetivos para este proyecto final de mster: 1. explorar la posibilidad de realizar navegaciones utilizando puntos de referencia naturales. 2. implementar las rutinas seleccionadas para evitar obstculos.

3. Introduccin
Lugar de trabajo

El LAAS es una unidad de investigacin del Centre Nationale de la Recherche Scientifique. Est ubicado en Toulouse, y contando con ms de 600 empleados, es uno de los mayores laboratorios de Francia y uno de los ms reconocidos entre las organizaciones europeas de investigacin en el campo de la robtica. Adems, es un instituto Carnot, nombre dado por el Ministerio Francs de Investigacin, indicando la excelencia de sus relaciones con la industria. El LAAS lleva a cabo actividades de investigacin sobre distintos tipos de sistemas: micro y nano sistemas, sistemas embebidos, sistemas integrados, sistemas de gran escala, sistemas biolgicos, sistemas mviles, sistemas autnomos y sistemas de informacin crtica; y en un variado abanico de aplicaciones como: la aeronutica, el espacio, el transporte, la energa, los servicios, la salud, las telecomunicaciones, el medio ambiente, la produccin y la defensa. El LAAS cuenta con 21 equipos trabajando en ocho temas de investigacin: Procesamiento de Informacin Crtica Redes y Comunicaciones Robtica Decisin y Optimizacin MicroNanosistemas RF y pticos Nano-Ingeniera e Integracin Gestin de la Energa Tecnologas MicroNanoBio El presente trabajo encuentra su lugar dentro del grupo de investigacin llamado Robtica, Accin y Percepcin (RAP). Las labores de este equipo se centran principalmente en la capa funcional integrada en los sistemas robticos: ya sea en robots mviles para interior

o exterior equipados generalmente con varios sensores (visin, sonido, escneres lser, lectores RFID) o robots humanoides con sensores visuales y de audio.

Antecedentes del proyecto

En este trabajo se aborda el problema de la navegacin basada en la visin en un ambiente con obstculos. El equipo de RAP propone diferentes soluciones a este problema basadas en control visual. Esta tcnica que permite el control de un robot utilizando la informacin proveniente de una o ms cmaras, que pueden ser integradas o no. El objetivo es minimizar el error entre un conjunto de mediciones sobre las imgenes y un conjunto de valores de referencia que caracterizan la posicin que debe alcanzar el robot. Cuando el error se hace menor que un umbral preestablecido, quiere decir que el controlador ha finalizado su tarea exitosamente. La implementacin de dicho controlador puede ser realizada de diferentes formas. Los distintos mtodos son generalmente clasificados en tres categoras: control visual 3D (o PBVS), control visual 2D (o IBVS) y los mtodos hbridos como el control visual 2D 1/2. El control visual 3D utiliza las mediciones visuales para reconstruir la posicin del robot en el espacio 3D para luego calcular un error cinemtico en el espacio cartesiano y reenvar los valores de control a los actuadores. Este mtodo es altamente sensible a los errores de medicin y de modelizacin, lo que reduce la precisin del esquema de control. Por otro lado, en el control visual 2D el error se calcula directamente a partir de las mediciones visuales. La ventaja principal de esta estrategia es que es menos sensible a ruidos, a la calibracin de la cmara, y a las incertidumbres en el modelo del robot; lo que resulta en un control ms robusto. Otra gran ventaja es que al trabajar directamente con las mediciones de los datos visuales en vez del clculo de posicin 3D del PBVS utiliza la menor cantidad de datos posibles para realizar la tarea. Esto reduce el tiempo de procesamiento, a menudo un parmetro crtico debido a la cantidad de informacin obtenida en cada adquisicin de imagen. Este mtodo tiene la desventaja de no ser fiable si los desplazamientos lineales o rotacionales a lo largo de un eje son demasiado grandes, pudiendo incluso no finalizar correctamente la navegacin. El control visual 2D fue elegido para este proyecto debido a la robustez en cuanto a ruido de la cmara y a las incertidumbres de modelado; y de ahora en adelante, cuando el concepto control visual sea utilizado ser en referencia a este mtodo. Para realizar la

navegacin se pueden utilizar referencias artificiales o naturales. En este proyecto intentaremos realizar navegaciones basadas en referencias naturales. Las estrategias de control visual aisladas no son suficientes cuando el objetivo es llevar a cabo una tarea de navegacin interior en un entorno real donde pueden encontrarse obstculos, ya sean dinmicos o estticos. En este caso, se debe tener en cuenta: el riesgo de oclusiones y el riesgo de colisiones. En otras palabras, el robot tiene que ser capaz de tratar con oclusiones del objeto de referencia y de navegar entre obstculos. Una oclusin se define como la prdida de las caractersticas visuales de la escena. Muchas situaciones pueden causar una oclusin y muchas soluciones para tratar con ellos han sido propuestos y estn disponibles. Sin embargo, este proyecto se centra en la aplicacin de estrategias para evitar obstculos, sobre todo en dos estrategias diferentes y ya existentes: path following y artificial rotating potential field. Los algoritmos desarrollados sern implementados en Rackham, uno de los robots disponibles en el LAAS. Dicho robot cuenta con una arquitectura de software modular, basada en un generador de mdulos llamado Genom. Genom fue desarrollado en el LAAS y es una herramienta muy til para el control de sistemas complejos de tiempo real, en los que cada mdulo realiza una tarea especfica (adquirir las imgenes, controlar el robot, adquirir los datos del lser, etc.) y se comunica con los otros mdulos con el fin de realizar una tarea global y de mayor complejidad.

Figura 1. Arquitectura del robot.

La figura 1 presenta la arquitectura de software simplificada de Rackham, donde surfvs y visuserv son los mdulos ms relevantes para este proyecto. El primero fue desarrollado en su totalidad durante este proyecto final de mster, y el segundo se actualiz en gran parte. Para comprender mejor el funcionamientos de Genom se aconseja leer la bibliografa recomendada.

4. Fundamentos Tericos
Esta seccin describe las bases tericas necesarias para nuestro proyecto. Primero se detalla Rackham, el robot utilizado, y su modelo. Luego se presenta el modelo de la cmara. Y por ltimo la ley de control para realizar una navegacin visual en un entorno libre.
Descripcin del robot

Rackham es una plataforma B21R modificada fabricado por iRobot, una de las empresas lderes a nivel mundial en el campo de la robtica. Rackham, mostrado en la figura 2, es un robot mvil no holonmico, de forma cilndrica y con las siguientes medidas: 53 cm de dimetro, 180 cm de altura y un peso de 140 kg. Cuenta con cuatro ruedas motrices accionadas por 4 servomotores de alto par de 24V CC. El lmite de velocidad para desplazamientos lineales es de 1 m/s y para los rotacionales de 160 /s.

Figura 2. Rackham.

Los dos elementos ms importantes para nuestro proyecto con los que Rackham est equipado son:

una cmara MicroPix IEEE-1394 CCD Camera Progressive Scan, que entrega imgenes color de 24 bits RGB y adquiere hasta 30 cuadros por segundo con una resolucin de hasta 1024x768 pixels;

un telmetro laser SICK LMS-200 2D.

La cmara est montada sobre una unidad PTU (Pan Tilt Unit), empleada para aadir otro grado de libertad (el correspondiente a la inclinacin no se utiliza en este proyecto) para girar la cmara independientemente de la base del robot. Esto tiene como objetivo controlar el movimiento de la cmara en lugar del movimiento del propio robot. Este enfoque permite controlar Rackham como un robot holonmico, eliminando los obstculos que representan la no holonomicidad.
Modelo del robot

Para lograr un control de movimiento preciso es necesario contar un modelo adecuado. El robot sufri modificaciones en su estructura por lo que fue necesario recalcular el modelo.

Figura 3. Modelo del robot.

La figura 3 presenta el modelo actualizado de Rackham; se pueden apreciar 4 sistemas de referencia: R0 (0,x0,y0,z0): sistema de referencia de la escena. RM (M,xM,yM,zM): sistema de referencia de la base del robot.

RP (P,xP,yP,zP): sistema de referencia de la unidad PTU. RC (C,xC,yC,zC): sistema de referencia de la cmara.

La posicin de la base del robot con respecto al sistema R0 est representada por XM y YM, y su rotacin, tambin con respecto a R0, se define por . La posicin de la PTU relativa a RM se define por Dx y su rotacin por . La posicin de la cmara con respecto a RP se define solamente por Cx y Cy debido a que la orientacin de la cmara est fija a la orientacin de la PTU. Las distancias Dx, Cx y Cy son fijas y estn definidas por la geometra de Rackham, mientras que XM, YM, y varan con el movimiento del robot. Adems de estos elementos se definen las matrices de transformacin entre sistemas de referencia y el vector de control del robot. Y luego se calcula el torsor cinemtico de la cmara con respecto a la escena expresado en el sistema de referencia R C. Dicho torsor puede ser representado como la multiplicacin de una matriz J robot por el vector de control. Jrobot es llamada la matriz jacobiana del robot.
Modelo de la cmara

La cmara est configurada para adquirir imgenes RGB en la mxima resolucin, luego por software se reduce la resolucin a 320x240 pixeles por razones de compatibilidad con los mdulos preexistentes de Genom.

Figura 4. Modelo de la cmara.

La figura 4 presenta el modelo empleado, se trata del modelo "cmara de agujero de alfiler" (pinhole camera model). Se asume que toda la luz entra por slo un punto, llamado centro ptico. Este modelo nos permite conocer la relacin entre un punto del espacio 3D y su proyeccin en el plano 2D de la imagen obtenida por la cmara.
Ley de control

Implementamos una ley de control basada en el formalismo de la funcin de tareas (task function formalism); y cuyo objetivo es posicionar la cmara frente a una referencia esttica. La ley de control para realizar navegaciones en entornos libres est definida por la siguiente ecuacin: qvs = -( C L Jrobot )-1 vs C ( s - s* )

5. Extraccin de las referencias visuales

Para realizar navegaciones en entornos reales, ste debe ser caracterizado a travs de las referencias visuales independientemente de la cmara, el zoom, la resolucin, del ngulo de visin, etc. Las referencias visuales empleadas en este trabajo son puntos, que sern extrados por el detector de puntos de inters SURF [Lowe, 1999; Evans, 2009] cuando se trabaje con referencias naturales y mediante un algoritmo desarrollado en la Universidad Paul Sabatier basado en colores para trabajar con referencias artificiales. La principal ventaja de utilizar referencias naturales es que el entorno no requiere modificaciones, y que, en trminos generales, la referencia no est limitada en tamao, forma o color. Primero presentaremos el algoritmo utilizado para trabajar con referencias artificiales y luego la implementacin del detector SURF para realizar una tarea de navegacin empleando referencias naturales. En la ltima seccin se pueden ver los resultados de las pruebas.
Referencias artificiales

La referencia artificial empleada es un conjunto de cuatro crculos rojos. Un algoritmo capaz de encontrar el centro de cada uno de ellos ya estaba disponible. La imagen proporcionada por Rackham puede verse en la figura 4; esta imagen es filtrada por el algoritmo creando una imagen binaria cuyos unos son los pxeles con un valor determinado del canal rojo. Luego, los unos de la imagen binaria son agrupados en conjuntos. Dichos conjuntos se filtran considerando el tamao y luego se encuentra el centro de los mismos.

Figura 5. Imagen adquirida por Rackham.

Referencias naturales

Para el empleo de referencias naturales se utiliza una implementacin de cdigo abierto del detector de puntos de inters SURF llamada openSURF. En primer lugar se deben adquirir dos imgenes: la que caracteriza la posicin final deseada del robot y la que caracteriza la posicin inicial del mismo.

Figura 6. Posicin deseada.

Figura 7. Posicin inicial.

Luego, el detector encuentra los puntos de inters entre ambas imgenes y se muestran los diez puntos con el mejor puntaje de correspondencia (el 0 posee el mejor puntaje). Dichas imgenes pueden observarse en las figuras 6 y 7. El usuario debe elegir 4 puntos entre estos 10, considerando que los puntos seleccionados deben tener una mnima distancia entre ellos. En la figura 8 se muestra la imagen en tiempo real de un ensayo en el que el usuario ha seleccionado los puntos 0, 1, 2 y 3. Se implement un filtro para poder descartar las correspondencias errneas, ya que el detector no siempre encuentra el punto correcto entre la imagen de referencia (imagen de la posicin final) y la imagen actual.

El filtrado se realiza teniendo en cuenta las distancias entre los puntos y conociendo que la posicin de un punto de una imagen a la siguiente no puede variar abruptamente. La figura 9 muestra las distancias consideradas. Con estas distancias se calculan nueve ndices (para cada punto) que servirn para confirmar si cada correspondencia es vlida. En caso de detectar que alguno de los puntos no fue correctamente encontrado, se considerar el punto anterior para realizar el control.

Figura 8. Adquisicin en tiempo real.

Figura 9. Distancias entre puntos.

Resultados de los ensayos en entornos libres

Se realizaron ensayos en diferentes condiciones de iluminacin, y con distintas configuraciones iniciales y finales del robot. Primero mostraremos los resultados obtenidos empleando la referencia artificial, y luego los resultados al emplear openSURF. Los ensayos empleando la referencia artificial fueron exitosos; se presenta un ejemplo en el cual la velocidad lineal est limitada a 10 cm/s y las velocidades angulares a 0,2 rad/s. La posicin inicial de la PTU es de -10.

Figura 10. Posicin final

Figura 11. Posicin inicial.

El ensayo fue realizado de noche, con iluminacin artificial. Las figuras 10 y 11 muestran las posiciones final e inicial del robot. En la figuras 12 se muestra la evolucin de

las velocidades de la base del robot, mientras que en la figura 13 se puede observar la evolucin de la velocidad angular de la PTU, y la evolucin del error en la imagen.

Figura 12. Velocidad linear y angular de la base del robot.

Figura 13. Evolucin de la velocidad angular de la PTU y del error en la imagen.

Los ensayos empleando openSURF revelaron que el detector no encuentra los puntos adecuados si la diferencia de orientacin entre la imagen final y la imagen inicial es mayor a 15. Adems, las velocidades debieron ser drsticamente reducidas, por lo que se decidi continuar empleando slo la referencia artificial para la implementacin de las rutinas para evitar obstculos.

6. Navegacin en entornos con obstculos

Las estrategias de navegacin pueden ser clasificadas en dos grandes grupos: Globales: cuando se posee un mapa del entorno, la ubicacin de los

obstculos (en caso de haber) es conocida de antemano y se calcula una trayectoria antes de comenzar la navegacin. Existen mtodos para realizar pequeas desviaciones a la trayectoria calculada en caso de encontrar obstculos imprevistos. Locales: el entorno es desconocido para el robot por lo que no se

calcula una trayectoria de antemano sino que se emplea la informacin proveniente de los sensores (cmaras, telmetros laser, sensores rf y de ultrasonido, etc.) en tiempo real para controlar el robot y dirigirlo a la posicin deseada. En este proyecto se emplea un enfoque local para realizar la navegacin en entorno libre, y se agrega otro mtodo local para detectar y evitar obstculos. Los mtodos implementados para evitar obstculos ya haban sido desarrollados y son: path following y artificial rotating potential field. Ambos mtodos definen una zona de seguridad y un contorno que el robot debe seguir para eliminar el riesgo de colisin y evitar el obstculo de forma segura. Estos mtodos slo proporcionan los valores de velocidad de la base del robot; por lo que se controla la PTU para que los puntos de referencia permanezcan en el centro de la imagen durante la fase de contorneo del obstculo. Se implementa el nuevo controlador, y luego una ley global de control en la cual se realiza una combinacin convexa de ambos controladores para establecer qu controlador (o una combinacin de ambos) debe estar activo. Esta decisin se realiza considerando la distancia al obstculo. Si la distancia es muy grande slo el controlador para entornos libres es empleado, si la distancia es muy pequea slo el controlador para evitar el obstculo y una mezcla de ambos controladores para distancias medias.
Determinacin y deteccin del obstculo

Para la deteccin de los obstculos se emplea un telmetro laser SICK Range Finder. Este sensor posee un ngulo de visin de 180 y caracteriza su entorno a travs de 360 puntos, es decir un punto cada 0,5. Entre las distintas mediciones y clculos que debimos realizar para definir totalmente el obstculo y para obtener todos los datos necesarios para las estrategias seleccionadas se pueden mencionar:

determinacin de los puntos extremos del obstculo, es decir del primer y ltimo punto perteneciente al obstculo,

determinacin del punto ms prximo del obstculo al robot, determinacin del radio de curvatura del obstculo en el entorno del punto ms cercano.

estimacin del centro de masa del obstculo, estimacin de la posicin del obstculo cuando el robot ya se encuentra siguiendo el contorno de seguridad y el obstculo sale del campo de visin del telmetro lser.

Resultados de los ensayos en entornos libres

Se realizaron ensayos en diferentes condiciones de iluminacin, y con distintas configuraciones iniciales y finales del robot. Se muestran a continuacin los resultados obtenidos empleando la referencia artificial. Los ensayos empleando la referencia artificial fueron exitosos, obteniendo similares resultados con ambos mtodos. Se presenta un ejemplo en el cual se emple el mtodo path following y en el cual la velocidad lineal fue limitada a 10 cm/s y las velocidades angulares a 0,2 rad/s. La posicin inicial de la PTU es de 0.

Figura 14. Posicin final

Figura 15. Posicin inicial.

El ensayo fue realizado de noche, con iluminacin artificial. Las figuras 14 y 15 muestran las posiciones final e inicial del robot. En la figuras 16 se muestra la evolucin de

las velocidades de la base del robot, mientras que en la figura 17 se puede observar la evolucin de la velocidad angular de la PTU, y la evolucin del error en la imagen.

Figura 16. Velocidad linear y angular de la base del robot.

Figura 17. Evolucin de la velocidad angular de la PTU y del error en la imagen.

Se puede observar la suave evolucin de la velocidad angular de la base del robot durante al evitar el obstculo en el intervalo t=1 seg hasta t=18 seg.

7. Conclusiones
Durante este proyecto hemos podido implementar las tcnicas de control necesarias para realizar navegaciones en ambientes libres y con obstculos empleando nuestro robot mvil. Un algoritmo fue desarrollado para detectar obstculos a travs de la informacin

suministrada por el sensor lser, y para calcular todas las medidas necesarias para las dos estrategias para evitar el obstculo. Tambin hemos propuesto una solucin para el problema de los obstculos que abandonan el campo de visin del lser mientras el robot est contorneando el obstculo. Y realizamos un estudio para determinar si openSURF podra ser utilizado para detectar las caractersticas visuales de referencias naturales en las imgenes adquiridas por la cmara. Los resultados de los ensayos de navegacin utilizando referencias artificiales fueron satisfactorios. Rackham pudo llegar a su destino incluso si se encontraban obstculos en su camino. Por otro lado, la estimacin de la posicin del obstculo demostr funcionar correctamente, reduciendo drsticamente las maniobras del robot al rodear el obstculo. Sin embargo, creemos que las trayectorias pueden ser mejoradas, y para lograr esto se debera tratar de anticipar el obstculo con el fin de obtener una trayectoria ms fluida y natural. Entendiendo como natural una trayectoria que es corta (en comparacin con las otras posibilidades) y con suaves cambios de direccin. Respecto a la utilizacin de referencias naturales, nuestros ensayos indican que openSURF no es capaz de encontrar las mismas caractersticas visuales a lo largo de toda la trayectoria si la diferencia de orientacin entre la imagen final y la actual es mayor que aproximadamente 15. Adems, las velocidades del robot, especialmente las angulares, deben ser severamente limitadas en comparacin con los valores usados al utilizar referencias artificiales. Por lo tanto sugerimos dos lneas de trabajo para continuar con este estudio: desarrollar un algoritmo que encuentre varias puntos de referencia (por ejemplo diez) situados en el mismo plano, seleccionar cuatro de ellos para llevar a cabo el control y en caso de perder uno de los puntos de referencia, sustituirlo con alguno de los otros, y sin que se produzca una discontinuidad en la ley de control. Aunque el detector SIFT necesita ms potencia de procesamiento y tiempo para encontrar coincidencias entre las imgenes, es ms robusto en cuanto a la orientacin que el detector SURF, por lo que se propone implementar SIFT y comparar estos resultados con los que obtuvimos usando openSURF. Por ltimo, pero muy importante, es que obtuve una valiosa experiencia no slo en el modelado de robots y en tcnicas de control visual, sino tambin en programacin modular para sistemas complejos de tiempo real mediante el uso de la plataforma Genom.