Auto localización de robots móviles y modelado del entorno
Transcripción
Auto localización de robots móviles y modelado del entorno
Proyecto final de carrera: Auto localización de robots móviles y modelado del entorno mediante visión estereoscópica Realizado por: Carlos Ezquerra Cerdán Director de proyecto: Miguel Ángel García García Año 2006 Ingeniería informática Unsiversidad Rovira i Virgili Índice 1. Introducción ……………………………………………. 2 2. Objetivos del proyecto ……………………………………………. 4 3. Especificaciones ……………………………………………. 5 3.1 Hardware: ……………………………………………. 5 3.1.1 Robot Pioneer ……………...……………………. 5 3.1.2 Cámara Digiclops ………………………………...… 7 3.1.3 Conexión blue-tooth ……………...……………………. 8 4. Diseño general …………………………………………… 10 5. Movimiento inteligente del robot explorador …………………… 12 5.1 Introducción. …………………………………………… 12 5.2 Objetivos …………………………………………… 12 5.3 Diseño …………………………………………… 12 5.3.1 Mapa de decisiones ..….……………………………... 13 5.4 Implementación …………………………………………… 15 5.5 Entorno de trabajo …………………………………………… 16 5.6 Pruebas y resultados. …………………………………………… 16 5.7 Problemáticas detectadas y soluciones propuestas ……………. 26 5.8 Conclusiones. …………………………………………… 30 6. Auto localización del robot explorador …………………………………… 31 6.1 Introducción …………………………………………… 31 6.2 Objetivos …………………………………………… 32 6.3 Algoritmo slam3D …………………………………………… 33 6.4 Diseño …………………………………………… 37 6.5 Entorno de pruebas …………………………………………… 38 6.6 Implementación en el robot …………………………………… 38 6.7 Evaluación del algoritmo en el robot explorador 6.8 Conclusiones ……………. 40 ………………………………………….... 71 7. Desarrollo del proyecto …………………………………………... 72 8. Conclusiones …………………………………………... 73 9. Recursos utilizados …………………………………………... 74 10. Manual de uso. …………………………………………... 75 1 1 Introducción Hoy en día el uso de robots está ampliamente extendido, de manera que encontramos desde robots artificieros hasta robots exploradores, pasando por la infinidad de tipos de robots que podemos encontrar, tales como los brazos articulados. En este proyecto nos centraremos en los robots exploradores. Los robots exploradores son aquellos que disponen de algún mecanismo de visión, ya sea por medio de cámaras o sensores, que les permiten explorar un entorno desconocido sin colisionar con los distintos obstáculos que se pueden encontrar en el terreno. Un ejemplo claro de robots exploradores son los robots Oportunity y Spirit de la Mars Exploration Rover (MER) Misión. Estos robots fueron enviados por la NASA en el año 2003 al planeta Marte para realizar un estudio de su superficie geológica. En nuestro caso, para implementar el proyecto, hemos elegido el robot explorador Pioneer P2AT mostrado en la figura 1. Fig. 1 El robot Pioneer P2AT es un robot explorador que cuenta con un anillo de 8 sensores de ultrasonidos para localizar los obstáculos, pero en nuestro caso se ha decidido que la exploración no se hará usando los sensores de ultrasonidos, sino que lo haremos mediante una cámara de visión estéreo. La cámara elegida para el caso es la cámara digiclops, mostrada en la figura 2. Fig. 2 2 La cámara dispone de 3 objetivos, por lo que podremos realizar imágenes en 3 dimensiones en las que podremos identificar la posición exacta de un punto de la imagen respecto donde estamos nosotros. Una vez tenemos definido que es un robot explorador y elegido el robot adecuado para la puesta en práctica de nuestro proyecto, veremos el apartado de la auto localización del robot. Como ya sabemos, la auto localización de un robot es muy importante. Dado que en entornos desconocidos puede que no dispongamos de GPS1, por ejemplo en otro planeta que no sea la tierra, necesitaremos un sistema de auto localización del robot, de manera que podamos saber en cada momento en la posición en la que está. De los elementos de los que disponemos con los que podríamos hacer una auto localización eficiente del robot tendremos los sensores de las ruedas del robot y la cámara. Desechamos el uso de los sensores de las ruedas por el grado de error que tienen y el que podrían tener dependiendo del terreno en el que se moviera el robot y nos quedamos con la cámara. Para la auto localización se utilizará un algoritmo de auto localización mediante la comparación de imágenes de una cámara de visión estéreo. Dicho algoritmo ha sido cedido para su uso por el Dr.Miguel Ángel García García. Por lo tanto, en este proyecto trataremos el algoritmo de movimiento del robot, el algoritmo de auto localización y los errores y problemas con los que nos podemos encontrar en ambos algoritmos. 1 Global Positioning System: sistema de localización por medio de más de 24 satélites que mediante ondas de radio indican al receptor su longitud, latitud y altitud en cualquier lugar de la tierra. Nótese que éste sistema sólo es útil en la tierra dada la posición de los satélites. 3 2. Objetivos del proyecto El objetivo principal del proyecto es la programación, evaluación e integración de los algoritmos de movimiento del robot, así como la integración y evaluación del algoritmo de auto localización del robot. Para conseguir estos objetivos deberemos definir una serie de objetivos a cumplir en cada una de las partes en las que podemos dividir este proyecto. Estos objetivos que debemos marcar son: 1. Estudio y comprensión de las librerías de movimiento del robot. 2. Programación, integración en el robot y evaluación del algoritmo implementado, viendo las limitaciones de éste y buscando soluciones satisfactorias para ellas. 3. Integración del algoritmo de auto localización con el programa de movimientos del robot. 4. Evaluación del algoritmo de auto localización en diferentes entornos. Éstos son los objetivos que deberemos cumplir en éste proyecto. 4 3 Especificaciones Los elementos utilizados en el proyecto han sido: 1. Robot Pioneer P2AT 2. Cámara Digiclops 3. Librerías Aria, Digiclops y Triclops 4. Conexión blue-tooth entre PC y Robot. A continuación comentaremos cada uno de éstos elementos. 3.1 Hardware 3.1.1 Robot Pioneer El robot seleccionado para el proyecto es el Pioneer P2AT. (Figuras 1 y 3). Fig. 3 El robot Pioneer P2AT es un robot todo terreno muy versátil, con software compatible con todos los MobileRobots2, elegido por gran parte de las empresas que requieren un robot de alto rendimiento con un grado alto de personalización. Fácil de usar y con gran poder es un robot excepcional para su uso en el exterior. El Pioneer P2AT ofrece la opción de utilizar un computador incorporado en el mismo robot, con lo que se podrá utilizar y programar con los programas y lenguajes convencionales y ya no se deberá programar directamente sobre el microcontrolador. Además dispone de conexión ethernet y otras funciones autónomas. En nuestro caso además se le han añadido los conectores usb para conectar el emisor/receptor de bluetooth y de una tarjeta firewire para la conexión de la cámara. 2 MobileRobots es la gama de robots móviles de la empresa ActiveMedia Robotics. 5 El Pioneer P2AT dispone de ruedas gigantes, que le permiten superar gran cantidad de obstáculos, además de motores que le permiten alcanzar una velocidad de 0.8 metros por segundo y cargar más de 30 Kg. El robot Pioneer P2AT combinado con la librería Aria es capaz de: Vagar de forma aleatoria. Ser conducido mediante un joystick o teclado. Planear caminos. Mostrar un mapa de su sonar. Localizarse usando el sonar. Comunicar la información de control y los sensores. Simular mediante el simulador Saphira el funcionamiento del robot. El robot Pioneer P2AT está indicado para: Mapeado de terrenos. Navegación. Monitorización. Reconocimiento. Visión. Manipulación. Cooperación y otros comportamientos. La distribución de los ejes de coordenadas del robot la podemos ver en la figura 4. Fig. 4 Se ha podido comprobar que el robot tiene errores tanto en el desplazamiento como en el giro, de manera que se ha medido un desplazamiento de 50 cm. dando como resultado un desplazamiento de sólo 46.5 cm. Por lo tanto los movimientos del robot de cara a la auto localización se usarán sólo de forma orientativa para mirar que se realiza la auto localización de forma correcta. 6 3.1.4 Cámara Digiclops La cámara elegida para su integración con el robot ha sido la cámara Digiclops (figura 5), fabricada por Point Grey Resesarch. La cámara Digiclops de visión estéreo es un dispositivo sensorial que permite la captura de imágenes en 3 dimensiones en tiempo real, para aplicaciones como medida, modelado VRML, robots móviles, etc. Digiclops es una cámara calibrada que proporciona imágenes estéreo al software Triclops, que será el encargado de extraer la información de dichas imágenes. Digiclops es un revolucionario sistema de visión que combina las imágenes estéreo en 3D con la tecnología de las cámaras digitales. La cámara Digiclops realiza imágenes de alta calidad, con excelente calibración de la cámara, transmisión de imágenes digitales y configuración de los múltiples sensores. La combinación de sus características convierte a la cámara Digiclops en la cámara ideal para cualquier aplicación que requiera de un sistema de visión en 3 dimensiones. Fig. 5 Especificaciones: – Tres objetivos Sony ICX084. – Sensor de imagen de alta definición con píxeles cuadrados. – Formato VGA (640 x 480) – Búsqueda progresiva. – Iluminación mínima: 6 Lux – Lentes optimizadas para mejorar la visualización. – Alimentación: 12V 400mA proporcionada por cable firewire. 7 Para tratar las imágenes 3D y obtener la información de ellas lo hacemos como se ve en la figura 6. La posición X, Y, Z de un punto en la escena puede ser determinada por medio de la triangulación de los puntos en la escena. Fig. 6 La triangulación la realizaremos entre los puntos Pl, Pr y Pt obtenidos por la cámara de la izquierda, la de la derecha y la de arriba respectivamente bh y bv son las líneas horizontales y verticales del desplazamiento de la cámara. F es la longitud local de las cámaras. 3.1.3 Conexión blue-tooth Para la conexión entre nuestro PC y el PC interno del robot se utilizará una conexión mediante ondas de las conocidas como blue-tooth. Para la configuración de la conexión blue-tooth en entorno de desarrollo Linux se utilizarán los paquetes bluez-utils y bluez-pan. Una vez instalados los paquetes se deberá configurar la conexión para que cuando se conecte por medio del blue-tooth se configure de forma automática la conexión con la dirección IP seleccionada, que en nuestro caso será 10.0.0.1 para el servidor y 10.0.0.2 para el cliente. Por último deberemos activar el demonio pand seleccionando el tipo adecuado de servicio, ya sea servidor o cliente. Para el servidor deberemos escribir: Pand –listen –role GN Para el cliente el comando es: Pand –search –role PANU –service GN Con esto, por medio de una conexión ssh, nos podremos conectar al PC interno del robot desde nuestro PC. 8 Deberemos tener en cuenta las limitaciones de la conexión blue-tooth, de manera que si se utilizan comandos que requieran la transferencia de gran cantidad de información o ventanas gráficas se colgará la conexión, con lo que se requerirá el reinicio de los equipos que estén conectados. Por lo tanto, no mostraremos en tiempo real de forma gráfica los pasos que sigue el robot, sino que se guardará en un fichero de movimientos que puede ser consultado en cualquier momento. 9 4.Diseño general Para el diseño general de la aplicación crearemos diferentes módulos que serán utilizados desde un programa principal por medio de llamadas a sus funciones. De esta manera dividiremos los módulos en 2 tipos: Módulos de movimiento del robot. Módulos de auto localización del robot. El módulo de movimiento del robot contiene todos aquellos módulos que se utilizan para la conexión del robot, conexión de la cámara y decisiones de movimientos. El módulo de auto localización engloba todos los módulos del algoritmo Slam3D junto con el módulo que realiza la llamada a éste, que será el que se comunique con el programa principal. (diagrama 7). Movimi ento? Programa principal Módulo de conexión con auto localización Módulo de movimientos Módulo de auto localización Fig. 7 El robot generará diferentes archivos para su consulta. Éstos se eliminarán en cada movimiento para evitar la sobresaturación del disco y la degradación de su sistema de ficheros. Estos archivos son: Fichero_movimientos Æ en él se guardarán todos los movimientos hechos por el robot. Este archivo no se borrará y podrá ser consultado en cualquier momento, ya que se abre, se escribe y se cierra para cada uno de los movimientos que se realiza. 10 Puntos_sensor Æ fichero de puntos que muestra el mapa que ha creado el robot para decidir si girar a la izquierda, a la derecha o seguir recto. Este fichero se sobrescribirá en cada movimiento. Out*.ppm Æ foto que se realiza antes de cada movimiento. El archivo se borrará después de cada movimiento y auto localización para impedir que se sature el disco. Out*.pts Æ fichero de puntos extraídos de la fotografía. Igual que el anterior se borrará después de realizar el movimiento y la auto localización. c0, c1, d0, d1, e0, e1 Æ ficheros utilizados para la auto localización. En ellos tenemos los puntos identificados en las imágenes, los emparejamientos entre los puntos y los puntos que quedan después de la filtración de puntos incorrectos. Estos archivos se eliminarán después de hacer cada auto localización. Para evitar que se borren los archivos, ya que puede ser que nos interese consultarlos, se ha diseñado el programa de manera que tengamos 2 posibles modos de ejecución. En el modo guiado el robot preguntará al usuario si desea realizar el siguiente movimiento antes de borrar los ficheros, de manera que antes de realizar el movimiento el usuario podrá recuperar toda la información. En el modo no guiado el robot explorará sin consultar al usuario y borrará o sobrescribirá los ficheros en cada uno de los movimientos. En nuestro caso, y dado que queremos evaluar el correcto funcionamiento de los programas, utilizaremos el modo guiado. El programa principal será el encargado de establecer el modo de funcionamiento del programa, realizará las conexiones con el robot y con la cámara, hará la fotografía y extraerá los puntos de ella por medio de llamadas a las funciones de la librería triclops. Una vez tengamos la fotografía con sus puntos definidos llamará al módulo de movimientos para que decida que movimiento realizar, al módulo de auto localización y finalmente a la librería Aria para realizar el movimiento adecuado con el robot. En cada uno de los apartados del proyecto veremos más a fondo el diseño de estos módulos. 11 5. Movimiento inteligente del robot explorador 5.1 Introducción Un robot explorador es aquel que se programa para la exploración de entornos desconocidos y que cuenta con diferentes sensores o sistemas de visión que le permiten explorar el entorno. En los robots exploradores es básico encontrar un movimiento que, además de no colisionar con los obstáculos, permita explorar todo el terreno en el menor tiempo posible. Basándonos en esto trabajaremos para conseguir explorar todo el terreno, sin que se produzcan colisiones con obstáculos. Para la exploración del terreno se dispone de un robot Pioneer P2AT dotado de un anillo de 8 sensores de ultrasonidos y una cámara de visión estéreo Digiclops. Combinando ambas tecnologías conseguiremos el movimiento propuesto. 5.2 Objetivos Los objetivos que se han marcado en este apartado del proyecto son: Implementar un algoritmo de decisión de movimiento en el robot explorador, mediante el sistema de visión de la cámara, con la finalidad de que se explore de forma correcta un entorno desconocido. Evaluar dicho algoritmo para comprobar su correcto funcionamiento tanto en casos de uso normal como en los casos más extremos. Buscar soluciones a los problemas que se puedan encontrar para todos los casos y así conseguir el algoritmo correcto. Identificar los mínimos locales y solucionarlos. Identificar las limitaciones del algoritmo de movimiento. 5.3 Diseño Como se ha comentado con anterioridad, en este apartado se mostrará es diseño del módulo de movimientos del robot explorador. El módulo de movimientos del robot explorador se compone de los siguientes módulos: Módulo funciones_robot Æ En él se encuentran las funciones de inicialización y movimientos del robot. Módulo cámara Æ en él encontramos las funciones para inicializar y finalizar la cámara. 12 Módulo movimientos Æ en él encontramos las funciones para detectar si puede haber colisión con algún obstáculo y la decisión del movimiento a realizar. Módulo sensores Æ módulo para tratar los sensores de ultrasonidos del robot. Todos estos módulos serán llamados directamente por el programa principal, y como ya hemos visto antes generarán una serie de ficheros para poder comprobar y consultar la correcta realización del movimiento. Además se ha definido la estructura punto, con la que trabajarán algunos módulos, que contendrá toda la información relacionada a un punto de la imagen. 5.3.1 Mapa de decisiones Para decidir el movimiento adecuado a realizar por el robot, a partir de la imagen con sus puntos encontrados, el robot construye un mapa con los puntos encontrados a una cierta altura. Para construir este mapa se buscará entre todos los puntos encontrados en la imagen, si hay alguno que esté a una distancia menor de la distancia que nos vamos a mover. En nuestro caso hemos añadido una distancia de seguridad a esa distancia. Si encontramos algún punto a menor distancia que la determinada como distancia de colisión, haremos un corte horizontal de los obstáculos a la altura del punto encontrado. En el caso en el que no encontremos ningún punto susceptible de colisión se elegirá una fila, que en nuestro caso será la que ocupa la posición 125, cercana al centro de la imagen. Por lo tanto, se guardarán todos los puntos de la imagen que estén a la altura deseada (en píxeles). Una vez guardados los puntos se procederá a trabajar con ellos para encontrar la mejor opción de giro. Dado que éste sistema sólo será correcto cuando trabajemos con obstáculos en forma de polígonos tales como cubos, rectángulos, etc., se ha añadido otro mapa, que se creará plasmando en el suelo todos los obstáculos, de manera que quedaría el mapa como si viéramos los obstáculos desde una vista aérea. Por medio de los 2 mapas conseguiremos encontrar la mejor opción de giro. Un ejemplo de mapa, visto con el programa gnuplot, se puede ver en la figura 8. 13 Fig. 8 Éste es un ejemplo de mapa con el que nos podríamos encontrar. El eje vertical muestra los metros a los que se encuentra el punto en cuestión y el eje horizontal el píxel de la imagen en el que se encuentra dicho punto. Como ya hemos dicho antes la altura se fija a un punto, que éste será el de colisión con el objeto, en el caso en el que haya posible colisión, o el de la mitad de la imagen en el caso en el que no la haya. Los puntos que encontramos a distancia 0 no es que el robot está tocando contra ellos, sino que son puntos que no se han podido detectar con la cámara, por lo que los tomaremos como espacio libre. Se ha decidido que el robot seguirá recto si no hay peligro inminente de colisión y queda espacio por explorar. Con que queda espacio para explorar queremos decir que no se encuentra con que la mayoría de puntos encontrados se encuentran a una distancia menor de 2 metros y el robot, aunque siguiera hacia delante, no podría pasar por ninguno de los espacios libres, por lo que es inútil seguir hacia delante y ya se da la superficie por explorada. En este caso realizaremos un movimiento aleatorio, que en este caso hemos decidido que será girar hacia la izquierda. Si encontramos un obstáculo con el que tenemos una posible colisión giraremos hacia el lado en el que el robot tenga mayor espacio libre y que quepa el robot por él. En el caso en el que no quepa por ninguno de los lados el robot girará a la izquierda por defecto para evitar colisionar con el obstáculo. 14 Por último, para detectar y salir de un mínimo local se han creado las funciones que cuando detectan una secuencia de giros izquierda-derecha-izquierda o derechaizquierda-derecha giran 50 grados en intervalos de 5 grados para no perder la auto localización. Con esto conseguiremos salir del mínimo local de forma satisfactoria. 5.4 Implementación Se han creado las siguientes funciones dentro de los módulos: • Módulo de funciones_robot: o robotInitialize Æ inicializa la conexión con el robot y el propio robot. o robotForward Æ mueve el robot hacia delante tantos centímetros como se indique por parámetro. o robotRight Æ gira el robot hacia la derecha tantos grados como se indique por parámetro. o robotLeft Æ gira el robot hacia la izquierda tantos grados como se indique por parámetro. o robotClose Æ para el robot y la conexión con su microcontrolador • Módulo de cámara: o cameraInitialize Æ inicializa la cámara y su conexión. o cameraClose Æ cierra la conexión con la cámara. • Módulo de movimientos: o busca_direccion Æ busca la dirección hacia la que debe girar el robot para evitar los obstáculos. o Collision Æ devuelve si hay posible colisión del robot con algún obstáculo. • Módulo sensores: o busca_colision Æ devuelve si hay una posible colisión en el próximo movimiento usando los sensores para comprobarlo. o busca_mayor_distancia Æ devuelve el número del sensor de ultrasonidos que está más lejano a un obstáculo. o busca_menor_distancia Æ devuelve el número del sensor de ultrasonidos que está más próximo a un obstáculo. 15 5.5 Entorno de trabajo El entorno de trabajo con el que trabajará en el robot explorador es un sistema operativo linux Red Hat 7.3, en el que programaremos los algoritmos en lenguaje C ayudándonos de las librerías: - Aria Æ funciones de movimiento del robot y uso de los ultrasonidos para mirar la distancia correcta a los obstáculos. - Digiclops Æ funciones para la captación de imágenes por medio de la cámara. - Triclops Æ funciones para la captación de imágenes 3D y de puntos en ellas por medio de la cámara de visión estéreo. En cuanto al entorno de pruebas en el que se encuentra el robot es el laboratorio de robótica, en el cual encontraremos diferentes obstáculos tales como sillas, mesas y armarios para comprobar el correcto funcionamiento del algoritmo de decisión de movimientos. 5.6 Pruebas y resultados Se han realizado diversas pruebas para comprobar el funcionamiento correcto del programa que controla el movimiento del robot. Las pruebas se han realizado con la cámara sólo y con la cámara y los ultrasonidos de forma simultánea. Los resultados que hemos obtenido son los siguientes: - Prueba 1 Dejamos al robot vía libre por delante, de manera que debería avanzar sin problemas al no detectar obstáculos a menos de 2 metros. Trabajará con la siguiente foto: 16 Esta es la foto realizada por la cámara digiclops y para decidir el siguiente movimiento a realizar buscará si hay algún punto menor a la distancia de colisión y los que haya a menos de 2 metros. El mapa que crea el robot a partir de la foto es: Si nos fijamos en la escala inferior, vemos que en el mapa se distingue en el rango [0-50] la silla más cercana por la parte de la izquierda, en el rango [50-100] se identifican las dos sillas más lejanas. Del 100 al 250 se cogería todo como camino libre, ya que todos los puntos encontrados están a distancia mayor de 2 metros y para acabar en el rango [250-300] se detecta el armario. Dado todo esto, el robot ve que delante de él no hay obstáculos y no se puede explorar puntos a distancia mayor de 2 metros, por lo que decide seguir recto. - Prueba2 En esta prueba se ha querido comprobar que el giro a la derecha se realiza de forma correcta cuando encontramos un obstáculo a la izquierda, el robot puede colisionar con él y tiene espacio hacia la derecha para seguir explorando, por lo que deberá girar hacia este lado para seguir explorando. La foto que trataremos es la siguiente: 17 Como podemos observar en la foto la caja que tenemos a la izquierda impediría que el robot siguiera recto, ya que se produciría la colisión contra ella. El mapa que genera el robot a partir de la foto es: En el mapa podemos observar que la cara de la caja que mira hacia el robot no la puede detectar, ya que no hay suficiente iluminación para captar los puntos. Sin embargo si que capta los del lateral de la caja, que es el intervalo [90-100]. El robot decidirá seguir hacia la derecha porque la zona libre hacia la izquierda es mayor, que es el intervalo [100-245]. En el caso en el que hubiera un obstáculo en éste rango el robot podría decidir girar hacia la izquierda lo que podría provocar la colisión contra la caja. 18 En el apartado de problemas y soluciones se ha propuesto y puesto en práctica la solución para casos como éstos. Aunque en este caso en particular comprobamos que el robot lo haría de forma correcta y giraría hacia la derecha. - Prueba 3 En este caso tenemos el obstáculo en el lado derecho, por lo que queremos comprobar que el robot gira de forma correcta hacia la izquierda al detectar el obstáculo y el espacio libre del que se dispone hacia la izquierda. La foto con la que tratamos es: Como podemos observar en este caso el espacio libre queda hacia la izquierda y la iluminación de la caja es mejor, por lo que el robot debería detectar puntos de la caja. Esto lo podremos ver en el siguiente mapa: 19 El robot consigue detectar algunos de los puntos de la caja, aunque en el rango [200300] no encuentre ningún punto. Gracias a los puntos detectados el robot podrá girar de forma correcta hacia la izquierda, aunque nos podríamos encontrar con el mismo problema que en la prueba anterior si en la zona de la izquierda tuviéramos algún obstáculo que sí se detectase. - Prueba 4 En esta prueba se ha querido comprobar que funciona de forma correcta la exploración de zonas en las que no podemos explorar más, es decir, que el robot no cabe físicamente por ninguno de los lados, ya que en ambos hay obstáculos. Si miramos la foto a tratar veremos este hecho: 20 En este caso la caja tapa los posibles huecos por los que podría pasar el robot. Esto se ve en el siguiente mapa: En el mapa podemos observar como el robot detecta las sillas en la zona izquierda y a la derecha detecta el armario, por lo que es físicamente imposible que el robot quepa por los espacios que quedan disponibles, de manera que el robot decidirá hacer un giro estático (sin ir hacia delante) hacia la izquierda, con la finalidad de acabar encontrando un espacio libre por el que seguir explorando. Éste movimiento se realizará todas las veces que sean necesarias hasta encontrar un espacio libre por el que pasar el robot. - Prueba 5 En esta prueba se volverá a evaluar el giro hacia la izquierda al haber peligro de colisión si seguimos recto. La diferencia con el caso anterior en el que evaluábamos el mismo caso es que en este caso hemos probado con una caja con diferentes colores, por lo que se detectarán los puntos de la caja. Esta podría ser una solución a los problemas vistos en las pruebas anteriores, aunque no sería efectiva, ya que necesitaremos obstáculos con diferentes puntos susceptibles de ser captados por la cámara. La foto que tratáremos es: 21 Si vemos el mapa generado por el robot comprobaremos que la cantidad de puntos captados por la cámara es sustancialmente más grande. Como se puede observar en este caso se detectan prácticamente todos los puntos de la caja, por lo que el robot no tiene problema alguno para decidir hacia el lado al que debe girar y éste es hacia la izquierda. - Prueba 6 En esta prueba se ponen los obstáculos a más de 2 metros, distancia que se ha considerado suficiente como para afirmar que nos debemos acercar más a los obstáculos para poder explorarlos de forma correcta. En la foto podemos observar que el obstáculo está en una posición lo suficientemente lejana. 22 El mapa que genera el robot a partir de la foto y sus puntos detectados es: En el podemos observar que los puntos que se captan son lejanos o no se captan, por lo que el robot decidirá seguir recto con la finalidad de acercarse más a los obstáculos y hacer la exploración de forma correcta. - Prueba 7 En este caso se ha querido ver el caso que tendríamos en el caso en el que nos encontrásemos con un obstáculo lejano (con el que no tendremos problemas de colisión, ya que está por encima de los 50 cm. que nos moveremos) y nos acercaremos 50 cm., que como ya hemos dicho con anterioridad realmente serán 46.5 cm. La primera imagen que tenemos es: 23 Con su mapa de puntos: Se observa que no hay problemas de colisión y no detectamos todo el espacio cubierto, de manera que nos acercaremos más al obstáculo y seguidamente decidiremos hacia donde debemos girar. La imagen que se obtiene después de avanzar 46.5 cm. es: 24 Y su mapa de puntos es: Como podemos observar en este caso el obstáculo ya se detecta en una zona de posible colisión (50 cm. más una pequeña distancia de seguridad) por lo que el robot en este caso decidirá girar hacia la izquierda, que es el lado por el que encontramos mayor distancia libre. - Prueba 8 Por último comprobaremos uno de los errores que era previsible que sucediese, que es el caso en el que un obstáculo esté tan cerca que la cámara sea incapaz de captar sus puntos, por lo que el robot previsiblemente decidiría seguir recto con lo que se 25 produciría la colisión segura. Éste caso nos dará paso a la sección de las soluciones propuestas para que no ocurran este tipo de cosas. En la imagen se puede apreciar que el obstáculo está tan cercano al robot que no se pueden distinguir los puntos. La zona azul es toda un obstáculo. El mapa de puntos generado es: Por lo que podemos ver en el mapa no se detecta ningún punto y aquellos que se detectan lejanos (a más de 2 metros) se producen por la reflexión de la luz en la caja. Este pequeño grupo de puntos se puede ver en la parte derecha de la imagen. El robot decide seguir recto y se produce la colisión contra el obstáculo. 26 5.7 Problemáticas detectadas y soluciones propuestas Como se ha visto en las pruebas de movimiento hay casos en el que la cámara no es suficiente para detectar un obstáculo cercano. Una posible solución ya se ha dado antes, utilizando sólo obstáculos de los cuales la cámara sea capaz de identificar puntos, pero como ya se ha dicho, la solución no es buena, ya que en la mayoría de los casos en los que se utilice un robot explorador no se podrá elegir el color y la fisonomía de los obstáculos. Pongamos como ejemplo un robot explorador en la superficie de Marte. Los obstáculos que encontrará en la superficie marciana serán en su mayoría rocas de diferentes colores y formas y, evidentemente, no se podrá elegir las formas ni los colores. Con todo lo visto tenemos un problema de difícil solución, ya que sólo utilizando la cámara será muy difícil, por no decir imposible, captar los puntos de los obstáculos muy cercanos. La solución que se propone y se ha puesto en práctica es utilizar la medición de distancia por ultrasonidos aprovechando que el robot Pioneer dispone de ella. 5.7.1 Medición de distancias mediante ultrasonidos Los ultrasonidos son una onda acústica cuya frecuencia está por encima del límite perceptible por el oído humano (aproximadamente 20 Khz.). Muchos animales como los delfines y los murciélagos lo utilizan de forma parecida al radar en su orientación. Nosotros los utilizaremos de la misma forma que los animales para decidir si hay obstáculo o no lo hay. Basándonos en el tiempo que tarda la señal en ir desde el emisor hasta el objeto obstáculo y volver rebotada desde éste hasta el receptor y conociendo la velocidad del sonido podemos calcular con suficiente precisión y exactitud la distancia entre el objeto y nuestros receptores. Para explicarlo de forma correcta explicaremos las bases físicas que lo explican: • Los ultrasonidos son antes que nada sonido, exactamente igual que los que oímos normalmente, salvo que tienen una frecuencia mayor que la máxima audible por el oído humano. Ésta comienza desde unos 16 Hz y tiene un límite superior de aproximadamente 20 Khz., mientras que nosotros vamos a utilizar 27 sonido con una frecuencia de 40 Khz. A este tipo de sonidos es a lo que llamamos Ultrasonidos. • El sonido se desplaza en el aire a una velocidad de 340 m/s a una temperatura de 20º C, con una variación térmica de 0.6 m/s por cada grado centígrado. Así, por ejemplo, a 30º C de temperatura del aire, la velocidad del sonido es de 340 m/s + (0.6 m/s/º C * 10º C) = 346 m/s (Intervienen otros parámetros pero que vamos a dejar de lado en este trabajo por complicarnos excesivamente sin aportarnos mayor resolución) • La distancia d recorrida por un móvil es directamente proporcional a la velocidad v de éste y al tiempo t empleado en ello. Podemos escribirlo como una fórmula en la sencilla forma: d=v*t dónde d es la distancia recorrida en metros, v la velocidad en metros por segundo y t el tiempo en segundos. Esta misma relación la podemos escribir de otra forma que nos relacione el tiempo con las otras variables, estableciendo que el tiempo t que tarda un móvil en recorrer una distancia d es inversamente proporcional a la velocidad v del mismo e directamente proporcional a la distancia d a recorrer: t=d/v dónde t el tiempo en segundos, v la velocidad en metros por segundo y d es la distancia en metros. • Si deseamos medir una distancia usando los ultrasonidos tenemos que emitir un pulso ultrasónico breve, esperar que llegue hasta el obstáculo a medir, se refleje en éste y vuelva hasta nuestro dispositivo sensor, midiendo entonces el tiempo t que ha tardado en hacer todo el trayecto de ida y vuelta mediante nuestro microcontrolador PIC. Conociendo la velocidad v a que se ha desplazado nuestra onda tal como hemos visto, podemos fácilmente calcular según lo expresado en el punto anterior la distancia d entre el sensor y el objeto. 28 • Por ejemplo: Un objeto situado a 15 cm. de nuestro sensor, con una temperatura del aire de 20º C, debe emitir un eco tras nuestro pulso que debe tardar en llegar t = (0.15 m / 340 m/s) * 2 = 0.000882 segundos (o escrito de otra forma 882 uS, microsegundos) Nota el detalle de que multiplicamos por 2 el tiempo empleado debido a que el sonido debe ir y volver reflejado desde el objeto, por lo que recorre 2 veces la distancia que separa al sensor del objeto. 5.7.2 Puesta en práctica de los ultrasonidos Con lo visto en el apartado anterior podemos estar seguros de medir la distancia al obstáculo de forma correcta, pero no queremos dejar de lado la cámara para realizar esta función, dado que hemos comprobado en las pruebas que excepto para algunos casos aislados la edición de las distancias por medio de la cámara funciona de forma correcta. Por esto se ha decidido utilizar los 2 métodos de forma simultánea. Para esto le daremos prioridad a la cámara, de manera que cuando la cámara decida que no hay obstáculo y por lo tanto podemos seguir recto activaremos los ultrasonidos para comprobar que realmente no hay peligro de colisión. En el caso en el que los ultrasonidos detectaran un peligro inminente de colisión pasaría a tener el control del robot el módulo de los ultrasonidos durante el siguiente movimiento. El módulo de ultrasonidos controla el movimiento del robot de la forma siguiente: El robot dispone de 8 emisores y receptores de ultrasonidos dispuestos en la parte delantera del robot tal como muestra la figura siguiente. 29 Miraremos cual es el emisor/receptor que nos da una distancia más pequeña al obstáculo. esto se hará de forma ponderada teniendo en cuenta la posición de los emisores de ultrasonidos. Debemos anotar que es diferente que el emisor 0 nos de una distancia menor a 20 cm., con lo que no habría problema de colisión, en cambio si el emisor/receptor 3 diera una distancia menor a 20 cm. habría un riesgo de colisión elevado. Por lo tanto, para el emisor/receptor 3 y 4 tendremos una distancia para una posible colisión mayor que de los demás emisores/receptores (60 cm.), para 2, 1, 5, y 6 una distancia de 20 cm. y finalmente para 0 y 7 una distancia de 11 cm., que es el grosor de las ruedas. Una vez hemos detectado posible colisión por parte de uno o más emisores/receptores miraremos el de menor distancia, y por lo tanto, sabremos hacia que lado está el objeto más próximo con el que tenemos riesgo de colisión. Si el de menor distancia es 1, 2 ó 3 giraremos hacia la derecha. En cambio si es 4, 5 ó 6 giraremos hacia la izquierda. Debemos anotar que el giro se realiza de forma estática, es decir, que como hemos visto, nuestro robot tiene 3 grados de libertad en X, Z y YAW (giro en Y), por lo tanto podremos hacer el giro sin cambiar la Z. esto se traduce en que aunque hubiese 2 o más emisores/receptores que detectaran colisión, cada uno que hiciese girar a un lado (uno a izquierda y el otro a la derecha) no colisionaríamos con el obstáculo al elegir uno de los lados, ya que no variaremos nuestra Z. Una vez seleccionado el movimiento por parte de los ultrasonidos ya le podremos devolver el control al módulo de control por cámara. 30 5.8 Conclusiones Se ha realizado, por medio de un algoritmo de captación de puntos, un algoritmo para el movimiento de un robot explorador. Sobre el podremos sacar las siguientes conclusiones: El algoritmo funciona de forma correcta para gran cantidad de los casos, en el que el algoritmo elige de forma correcta si girar hacia el lado derecho o el lado izquierdo. El algoritmo de captación de puntos identificativos por medio de la cámara tiene diversas limitaciones, de manera que en algunos casos provoca que el algoritmo del movimiento del robot no funcione de la forma adecuada. Las limitaciones consisten en que la cámara es incapaz de captar puntos de obstáculos muy próximos a ella así como también es incapaz de captar puntos de superficies lisas y sin cambios de color. Para subsanar estas limitaciones se ha recurrido al uso de los emisores/receptores de ultrasonidos de los que dispone el robot. De esta forma daremos prioridad a lo seleccionado por la cámara, pero en el caso en el que la cámara diga que se puede avanzar recto recurriremos a los ultrasonidos para comprobar que lo que decía el algoritmo de movimiento por medio de la cámara es correcto. En el caso en el que no sea correcto el módulo de ultrasonidos pasará a tener el control del movimiento del robot durante ese movimiento. Se ha tenido en cuenta la posibilidad de obstáculos móviles para evitar que el robot en movimiento colisione contra ellos. Se ha buscado una solución satisfactoria al problema que nos encontramos cuando el robot se encuentra en un mínimo local, de manera que se podrá salir del mínimo local sin que el robot pierda su auto localización. Se ha comprobado y demostrado el funcionamiento correcto del algoritmo de movimiento por medio de diferentes pruebas e imágenes. Con todo lo visto podremos afirmar que el algoritmo de movimiento del robot funciona de forma correcta, tal como se había marcado en los objetivos, que por lo tanto se han cumplido. 31 6. Auto localización del robot explorador 6.1 Introducción Una vez visto el correcto funcionamiento del movimiento del robot explorador pasamos al apartado de la auto localización del robot. Este apartado tiene una gran importancia en el mundo de la robótica móvil, ya que en gran cantidad de casos es vital conocer la posición real en la que se encuentra el robot. Pongamos como ejemplo otra vez un robot explorando la superficie del planeta Marte. En la superficie de dicho planeta a parte de encontrar rocas también encontraremos arena, en la que el robot podría embarrancar provocando, al realizar diferentes movimientos, que los sensores del robot informen de que el robot se ha movido cuando realmente el robot no se ha movido y sus ruedas han derrapado sobre la arena. Además, como ya hemos visto con anterioridad, los movimientos realizados por el robot mediante sus sensores no son exactos (nótese que se movía 46.5 cm. cuando realmente se le había dicho que se moviera 50 cm.) por lo que esa pequeña inexactitud que tienen multiplicado por la gran cantidad de movimientos que realizará el robot se transformará en un desfase bastante grande. Para evitar estas inexactitudes hemos decidido dotar al robot explorador de un algoritmo de auto localización. Dicho algoritmo ha sido realizado íntegramente por el Dr. Miguel Ángel García, profesor de la universidad Rovira i Virgili, y que ha cedido el uso de dicho algoritmo para poder realizar este proyecto. El algoritmo de auto localización utilizado es el slam3D, que utiliza una cámara con visión estero, como la que se dispone en el robot, para realizar su auto localización por medio de la comparación de las fotografías hechas por la cámara. Vista la importancia de la auto localización del robot, integraremos el algoritmo de auto localización en el robot con la finalidad de evaluar si es viable utilizar dicho algoritmo en robots exploradores así como comprobar el funcionamiento de dicho algoritmo en diferentes ambientes y en las circunstancias más extremas. 32 6.2 Objetivos El objetivo principal de éste apartado del proyecto es integrar el algoritmo de auto localización dado en el robot explorador, así como la evaluación de dicho algoritmo en cuanto a su correcto funcionamiento. Por lo tanto, los objetivos marcados para este apartado son: Integración correcta del algoritmo de auto localización con el algoritmo de movimiento del robot realizado en el apartado anterior. Evaluación de dicho algoritmo para comprobar el correcto funcionamiento de éste con un robot explorador. Comprobar e identificar las limitaciones del algoritmo, así como encontrar posibles soluciones a dichas limitaciones. 33 6.3 Algoritmo slam3D El algoritmo de auto localización utilizado en el proyecto es el algoritmo slam3D. Éste algoritmo se basa en la comparación de diversas fotografías conseguidas mediante una cámara de visión estéreo para saber cual es el movimiento que se ha realizado. El funcionamiento del algoritmo es el siguiente: Se realizan 2 fotografías consecutivas habiendo un movimiento entre ellas 2. En el caso en el que no hubiese ningún movimiento entre ellas el algoritmo debería devolver un desplazamiento igual a 0. Se identifican puntos representativos en dichas fotografías. Se identifican los puntos que son iguales pero que tienen un desplazamiento entre ellos. Se filtran los emparejamientos de puntos producidos por el ruido. Se calcula la distancia entre los puntos cogiendo todos los puntos y haciendo una ponderación. Veamos como funciona el algoritmo por medio de un ejemplo: Realizamos las fotografías e identificamos los puntos representativos en ellas: Primera imagen: Segunda imagen: 34 Como podemos ver en las imágenes ya se ha hecho el emparejamiento de puntos. Nótese en la parte superior de la imagen los puntos mal emparejados producidos por el ruido. A continuación filtraremos los emparejamientos incorrectos, de manera que las imágenes que tendremos serán: Primera imagen: Segunda imagen: 35 En estas imágenes se puede identificar como se han eliminado de forma correcta los emparejamientos producidos por el ruido. Por último haremos otra filtración y calcularemos la distancia que se ha movido el robot haciendo la ponderación de los enlaces, con lo que las imágenes que nos quedarán serán: Primera imagen: Segunda imagen: Con lo que ya podremos calcular la distancia. En este caso nos habíamos movido 50 centímetros en dirección hacia delante (eje Z positivo del robot). Debemos tener en cuenta que nos hemos movido 50 cm. según los sensores del robot, que como ya se ha dicho antes, tienen un error bastante grande. El resultado obtenido por medio del algoritmo de auto localización es: Translation -0.006932 -0.005879 0.402671 Pitch(X) 0.312178 Yaw(Y) -0.172794 Roll(Z) -0.040578 Error 59.177221 36 Viendo los campos del resultado podemos ver que tenemos ciertos errores de cálculo, pero que los errores son bastante pequeños. En los campos Translation tenemos: -0.006932 Æ desplazamiento en el eje X del robot. Éste desplazamiento es de 6 milímetros, por lo que lo podremos considerar despreciable. -0.005879 Æ desplazamiento en el eje Y del robot. Igual que en el caso anterior se puede considerar despreciable, ya que es de sólo 5 milímetros. 0.402671 Æ desplazamiento en el eje Z del robot. En él se puede apreciar el movimiento realizado, que es de 40 cm. hacia delante. Mediante una cinta métrica se ha medido el desplazamiento real del robot, que ha sido de 43 cm., por lo que el algoritmo da un valor bastante aproximado. Pasamos a ver los giros que ha habido en los ejes: Pitch 0.312178 Æ giro que ha habido en el eje X. El valor de 0.3 grados que nos devuelve el algoritmo es un pequeño error, aunque parte de éste está producido por la imprecisión del robot, que al producirse un movimiento en uno de los ejes se puede producir pequeños movimientos en los otros. Yaw -0.172794 Æ giro que ha habido en el eje Y. Tenemos un caso similar al caso anterior, en el que se produce un pequeño error por imprecisiones en el robot. Roll -0.040578 Æ giro que ha habido en el eje Z. En éste caso el error es muy pequeño, tan sólo de 0.04 grados, por lo que se considerará despreciable. Con esto hemos visto el funcionamiento del algoritmo a evaluar. 37 6.4 Diseño El diseño de este apartado del proyecto se divide en 2 partes diferenciadas: - Módulo de auto localización, que permite la llamada del programa principal al programa slam3D. - Programa slam3D. Dado que en este proyecto no se ha diseñado ni implementado el programa slam3D no hablaremos del diseño de éste módulo. Sí que hablaremos sin embargo del módulo que nos permite hacer las llamadas al programa slam3D. El programa slam3D tiene un fichero de configuración en el que le tenemos que pasar las 2 imágenes que se tratarán para emparejar los puntos. Dicho fichero, llamado sequence, deberá ser modificado en cada uno de los movimientos que realiza el robot. Estas modificaciones se harán desde el módulo autolocation de nuestra implementación. Una vez realizados los cambios necesarios, deberemos llamar al programa slam3D, que imprimirá en el fichero de movimientos el movimiento que se ha realizado. Como una decisión de diseño también se ha tenido que tomar que si un fichero de puntos contiene 0 puntos, es decir, que el robot no ha sido capaz de identificar ningún punto de la imagen, ese fichero no se deberá pasar al programa de auto localización, por lo que realizaremos el siguiente movimiento y calcularemos la auto localización de 2 movimientos, siendo conscientes de que podemos encontrar un error algo más grande que el que encontraríamos si hiciésemos un solo movimiento. Esto se debe a que no tiene sentido pasarle al algoritmo de auto localización un fichero vacío, ya que no conseguirá emparejar ningún punto. 38 6.5 Entorno de pruebas Las pruebas se han querido realizar en 2 tipos de entornos. El primero elegido es el laboratorio de robótica, en el que encontramos diferentes tipos de obstáculos, de manera que se puede comprobar el funcionamiento correcto para cuando previsiblemente encontraremos gran cantidad de puntos, debido a la gran cantidad de obstáculos diferentes con los que nos encontraremos. El problema con el que nos podremos encontrar aquí es que el robot no distingue puntos de las patas de las sillas, ya que éstas son cromadas y reflejan la luz. Otro problema con el que nos podemos encontrar es que el robot se acerque tanto a una pared o un armario que no pueda distinguir ningún punto, ya que la superficie es homogénea. En este caso el problema de la colisión está solucionado dado que se utilizarán los ultrasonidos, pero se puede perder la auto localización correcta si en pocos movimientos no encontramos una fotografía en la que podamos distinguir y emparejar puntos. El segundo entorno de pruebas elegido ha sido el pasillo exterior del laboratorio. En este entorno se captan mejor los puntos gracias a la luz. En él no tenemos más obstáculos que la barandilla y las paredes, por lo que a priori debería funcionar el algoritmo de forma correcta. Se ha comprobado que el algoritmo funciona de forma correcta en ambos escenarios, tal como mostraremos en el apartado de pruebas. 6.6 Implementación en el robot Dado que el algoritmo slam3D estaba implementado en C para Linux ya no será necesario implementarlo. Lo que si será necesario será integrarlo en el robot junto al programa que controla los movimientos. Dado que para realizar la auto localización necesitaremos 2 imágenes se deberá realizar 2 imágenes antes de realizar la auto localización. Esto puede provocar algunos problemas, ya que deberemos hacer la fotografía, acto seguido haremos la auto localización y después se le preguntará al usuario si quiere realizar el movimiento, pero el movimiento se realizará sobre la fotografía que se ha tomado antes de la auto localización y dado que el proceso de auto localización es lento (en torno a 30 segundos) el entorno podría haber cambiado si disponemos de diversos obstáculos móviles. Dado que en los entornos que se desea explorar con el robot no disponemos de gran cantidad de obstáculos móviles obviaremos este posible problema. Además, como se ha comentado en el apartado anterior, se ha dotado al robot de un sistema de 39 detección de colisión por medio de ultrasonidos mientras está en movimiento, lo que garantiza que no colisionará con los obstáculos. Para integrar el módulo de auto localización con el de movimientos utilizaremos un nuevo módulo que modificará el fichero de configuración del programa de auto localización y llamará a éste. Se ha modificado el programa slam3D para que escriba el resultado directamente en el fichero de movimientos y no lo haga por pantalla, como lo hacía por defecto. 40 7.6 Evaluación del algoritmo en el robot explorador Para la correcta evaluación del algoritmo hemos realizado diferentes pruebas comprobando sus resultados, tanto en giros como en movimientos hacia delante. En cada una de las pruebas realizadas hemos analizado las fotografías tomadas por las cámaras así como los puntos elegidos y sus emparejamientos. Prueba 1 En esta prueba se quiere comprobar que el algoritmo funciona de forma correcta cuando no ha habido movimiento del robot o éste se ha intentado mover pero sus ruedas están atrapadas, de manera que ,aunque el robot devuelva que se ha movido 50 cm. hacia delante, no se ha podido mover. Las fotografías tomadas con el emparejamiento de puntos han sido: Nótese que hay ciertos puntos emparejados que devuelven un pequeño desplazamiento. Éstos puntos están marcados con un círculo rojo en la segunda fotografía. Los puntos incorrectos se deberán eliminar en el proceso de filtrado. Después del primer filtrado de las imágenes obtenemos: 41 Como se puede observar los puntos marcados antes con el círculo rojo han desaparecido, ya que se han desechado por ser incorrectos. Después de la segunda filtración obtenemos: 42 Donde ya podemos apreciar los puntos elegidos para calcular la distancia recorrida por el robot. El resultado que devuelve el algoritmo es: Translation 0.000824 -0.000633 -0.000244 Pitch(X) 0.012788 Yaw(Y) 0.014146 Roll(Z) -0.002612 Error 8.067394 Con lo que podemos apreciar que los errores cometidos por el robot en este caso son prácticamente inapreciables, menores a 1 milímetro en desplazamientos y menor a 0.1 grados en giros. Prueba 2 En esta prueba se ha querido comprobar que se realiza el cálculo de los desplazamientos sobre el eje Z (hacia delante) de forma correcta. Para esto hemos colocado el robot en una posición sin obstáculos delante, por lo que el robot decidirá seguir recto 50 cm. Las fotos obtenidas con sus puntos identificados son: 43 En estas fotos apreciamos bastantes emparejamientos incorrectos (marcados con un circulo rojo) que deberá filtrar el algoritmo. Después del filtrado obtenemos: Y haciendo el segundo filtrado se obtiene: Por lo que el cálculo de la distancia nos devuelve: Translation -0.001138 0.005834 0.464195 Pitch(X) 0.243812 Yaw(Y) -0.053211 Roll(Z) -0.565056 Error 61.749334 44 Con la cinta métrica se ha medido la distancia real que se ha movido el robot en el eje Z, que ha sido de 46.5 cm. Viendo los resultados obtenido por el algoritmo de auto localización vemos que el dato retornado en cuanto al movimiento en el eje Z es correcto. El error es despreciable. En cambio tenemos algunos errores en los cálculos de los giros. Parte de éstos errores se producen por el algoritmo de auto localización, pero es la parte más pequeña, ya que la mayor parte del error se produce por los errores en el movimiento del robot. De todas formas el error máximo está en torno al medio grado, que es un valor prácticamente despreciable. Prueba 3 En esta prueba se ha querido comprobar que se realizan de forma correcta los cálculos de los grados en el caso de los giros hacia la izquierda. Para realizar esta prueba se ha puesto un obstáculo delante del robot dejándole espacio para explorar por el lado izquierdo, por lo que el robot decidirá girar hacia la izquierda. El robot debería girar 5 grados hacia la izquierda pero, como se ha podido comprobar, realmente gira en torno a 2 grados, por lo que el algoritmo de auto localización nos debería devolver en torno a los 2 grados para ser correcto. La foto que trataremos es: Filtramos los puntos que no son correctos y nos quedará: 45 Y haciendo el segundo filtrado nos queda: Dando como resultado el movimiento: Nuevo Movimiento Translation -0.000181 0.008490 0.017059 Pitch(X) -0.075986 Yaw(Y) 1.686430 Roll(Z) -0.349223 Error 45.381230 Como podemos ver, si dejamos de lado los pequeños errores, el movimiento se produce de 1.6 grados en el eje Y, por lo tanto podremos decir que la auto localización funcionaría de forma correcta en este caso pero con un pequeño error, que en parte será debido al giro del robot y otro pequeño error del algoritmo de auto localización. Prueba 4 De la misma forma que antes se ha querido comprobar los giros hacia la derecha. Para esto se pone un obstáculo en el lado izquierdo del robot, por lo que el robot decidirá girar hacia la derecha. Con ello, las imágenes que obtendremos son: 46 Y haciendo la filtración obtendremos: Con lo que los resultados devueltos por el programa de auto localización son: Nuevo Movimiento Translation 0.006127 0.001168 -0.000707 Pitch(X) -0.033169 Yaw(Y) -1.923543 Roll(Z) 0.248585 Error 33.351548 Donde vemos que el movimiento realizado ha sido de 2 grados hacia la derecha, además de los pequeños errores que están en torno a centésimas de milímetro en los desplazamientos en los ejes y en menos de 0.3 grados en los giros sobre los ejes. Una vez se ha visto el correcto funcionamiento del algoritmo de auto localización del robot se probarán diversos movimientos libres, tanto por el laboratorio como por un entorno exterior. Para estos casos ya no pondremos las imágenes con los puntos y los filtrados dada la cantidad de pruebas que tenemos que hacer para verificar el funcionamiento correcto y la extensión de éstas. 47 Prueba 5 Se ha querido realizar diversas pruebas en otras zonas del laboratorio de robótica y ésta es una de ellas. En ella tenemos una secuencia de movimientos del robot en las que se mueve de forma libre por el laboratorio, como podemos ver se realiza de forma correcta tanto el movimiento como la auto localización La secuencia de imágenes que obtenemos es: Fig. 1 Fig.2 El movimiento realizado entre las figuras 1 y 2 es, supuestamente, de 50 cm. Hacia delante. El resultado que nos devuelve la auto localización es: Translation 0.027025 -0.016987 0.436839 Pitch(X) 0.428932 Yaw(Y) -0.176388 Roll(Z) 0.653457 Error 47.604457 En el encontramos pequeños errores de los giros en los ejes, pero el resultado más importante es el de 43.6 cm. Hacia delante, que es lo que realmente se ha movido el robot. Fig. 3 Fig.4 48 Igual que antes el movimiento entre las figuras 2 y 3 es, otra vez supuestamente ya que nos movemos siguiendo los sensores del robot, de 50 cm. Hacia delante, es decir, en el eje Z del robot. Los resultados son: Translation 0.029868 -0.009686 0.523805 Pitch(X) 0.539723 Yaw(Y) -0.471682 Roll(Z) 0.197698 Error 59.713067 Vemos pequeños errores en los giros en torno al medio grado, y que el robot ha avanzado una distancia realmente de 0.52 cm. Otra vez tenemos un movimiento entre las figuras 3 y 4 hacia delante de 50 cm. Los resultados obtenidos son: Translation 0.056848 -0.011693 0.510023 Pitch(X) 0.583067 Yaw(Y) -0.064909 Roll(Z) -0.175913 Error 44.840349 Igual que en los movimientos anteriores tenemos algún pequeño error prácticamente despreciable en los giros sobre los ejes. El movimiento real del robot ha sido de 0.51 cm. en su eje Z. Fig. 5 Fig. 6 Entre las figuras 4 y 5 tenemos un avance de 50 cm. Los resultados obtenidos son: Translation 0.070247 -0.014282 0.519166 Pitch(X) -0.317659 Yaw(Y) 0.247615 Roll(Z) 0.129556 Error 55.394193 49 Como en todos los demás movimientos tenemos pequeños errores prácticamente despreciables. El movimiento real ha sido de 51.9 cm. Entre las figuras 5 y 6 volvemos a tener un avance de 50 cm. Los resultados obtenidos son: Translation 0.061890 -0.016380 0.512949 Pitch(X) 0.630717 Yaw(Y) 0.063784 Roll(Z) 0.060798 Error 85.124385 Como en todos los demás movimientos tenemos pequeños errores prácticamente despreciables. El movimiento real ha sido de 51.9 cm. Fig. 7 Fig. 8 Avance de 50 cm. entre las imágenes 6 y 7. Los resultados obtenidos son: Translation 0.050451 -0.007752 0.515882 Pitch(X) 0.437057 Yaw(Y) -0.455925 Roll(Z) -0.124631 Error 59.558509 Como en todos los demás movimientos tenemos pequeños errores prácticamente despreciables. El movimiento real ha sido de 51.5 cm. Avance de 50 cm. entre las figuras 7 y 8. Resultados obtenidos: Translation 0.048011 -0.005463 0.497446 Pitch(X) -0.221154 Yaw(Y) -0.129963 Roll(Z) -0.582447 Error 46.326806 Los errores vuelven a ser despreciables y el movimiento es correcto. 50 Fig. 9 Fig. 10 Entre las figuras 8 y 9 volvemos a tener un avance de 50 cm. Los resultados obtenidos son: Translation 0.037613 0.021406 0.396241 Pitch(X) -0.065476 Yaw(Y) -0.928274 Roll(Z) -1.460958 Error 98.816665 En este caso vemos que los errores son más grandes. Estos errores se producen por la dificultad de emparejar los puntos de ambas imágenes. Entre las imágenes 9 y 10 se ha realizado un giro, supuestamente de 5 grados hacia la izquierda, dado que el robot da todo el terreno por explorado al físicamente no caber por los espacios libres que quedan. Los resultados retornados por el algoritmo de auto localización son: Translation -0.005865 -0.000867 -0.001479 Pitch(X) 0.029426 Yaw(Y) 2.473567 Roll(Z) -0.070617 Error 37.417012 Con lo que vemos que el giro real captado por el algoritmo de auto localización es de 2.47 grados. Fig. 11 Fig. 12 Entre las imágenes 10 y 11 volvemos a tener un giro de 5 grados al ser imposible seguir recto y encontrar todo el espacio ocupado. Los resultados devueltos son: 51 Translation 0.019949 0.002149 -0.003365 Pitch(X) -0.048192 Yaw(Y) 3.623149 Roll(Z) 0.040991 Error 57.270717 Por lo que el giro captado es de 3.6 grados. Viendo los resultados en esta prueba podemos afirmar que el programa de auto localización funciona de forma correcta en la zona del laboratorio explorada. Prueba 6 En esta prueba se ha querido comprobar el correcto funcionamiento del programa de auto localización en el exterior. Los resultados obtenidos han sido los siguientes: Fig. 1 Fig. 2 Entre las imágenes 1 y 2 de este apartado tenemos un giro hacia la derecha, por lo tanto deberíamos tener un giro negativo sobre el eje Y. Los resultados que obtenemos con la auto localización son: Translation 0.017254 -0.002822 -0.000592 Pitch(X) 0.125885 Yaw(Y) -1.750777 Roll(Z) -0.314134 Error 27.874151 Como podemos observar, en este caso se realiza de forma correcta la auto localización, ya que como se ha dicho con anterioridad los grados que gira el robot no son exactamente los que le pasamos como parámetro a su función de movimiento. 52 Fig. 3 Fig. 4 Entre las imágenes 2 y 3 se realiza otro giro a la derecha, que comprobando el fichero de movimientos vemos que ha sido de 2.56 grados. Los resultados son: Translation 0.012204 -0.003518 -0.017699 Pitch(X) 0.040960 Yaw(Y) -2.560892 Roll(Z) -0.070926 Error 34.030696 Entre las imágenes 3 y 4 se debería haber hecho un giro hacia la izquierda de 5 grados, pero dado que el robot se había quedado prácticamente sin batería, apenas hizo el giro de unas décimas de grado. Esto ha sido captado por la cámara y, por medio del algoritmo de auto localización, ha quedado reflejado en el fichero de movimientos. El resultado obtenido es: Translation -0.008246 0.005014 0.011453 Pitch(X) -0.095971 Yaw(Y) 0.329339 Roll(Z) 0.072624 Error 30.488299 Como se puede ver el giro tan sólo ha sido de 0.3 grados, por lo que en este caso también ha funcionado de forma correcta el algoritmo de auto localización. Prueba 7 Para finalizar haremos una prueba en el exterior. Debemos tener en cuenta cuando el robot está funcionando en el exterior que la cámara tan sólo guarda los puntos encontrados a una cierta distancia máxima, ya que los encontrados a una distancia mayor se considerará que tienen un nivel de ruido demasiado elevado para tenerlos en cuenta. Por lo tanto, para que funcione de forma correcta la auto localización, debería haber puntos captados por debajo de esa distancia. En esta prueba podemos comprobar lo que ocurre cuando los puntos que se pueden captar son muy lejanos, con lo que los errores que encontraremos serán muy grandes. 53 Fig. 1 Fig. 2 Entre las imágenes 1 y 2 tenemos un avance de 50 cm. En cambio los resultados que obtenemos del fichero de movimientos son: Translation -0.415303 -0.009779 -0.003948 Pitch(X) -0.443149 Yaw(Y) -6.867904 Roll(Z) -0.758718 Error 51.638584 Como podemos ver, los datos son completamente erróneos, ya que el algoritmo de auto localización detecta un giro de 6.8 grados hacia la derecha. Esto se debe a que prácticamente no tenemos puntos, y los pocos que tenemos están a una distancia que linda con la distancia máxima seleccionada para considerar que un punto es erróneo. Fig. 3 Fig. 4 A medida que nos vamos acercando más hacia la barandilla los errores cometidos son menores. Pongamos el ejemplo entre las imágenes 2 y 3, cuyos resultados son: Translation 0.039373 -0.015876 0.160861 Pitch(X) 1.133256 Yaw(Y) -0.952863 Roll(Z) 0.676681 Error 61.111252 Como vemos en este caso los errores en los grados son más pequeños y ya se acerca más al movimiento correcto, ya que se detecta que se ha movido el robot 16 cm. hacia delante. 54 Entre las figuras 3 y 4 ya podemos encontrar un número de puntos mayor, por lo que es de esperar que los resultados sean mejores que los anteriores. Los resultados obtenidos son: Translation -0.154947 -0.023010 0.208938 Pitch(X) 0.427681 Yaw(Y) -1.895136 Roll(Z) -0.636744 Error 55.529031 En este caso ya se detecta el avance de 20 cm. Para solucionar estos casos se debería aumentar la distancia máxima a la que consideramos un punto erróneo, ya que será mejor tener puntos con alguna desviación que trabajar con los puntos con los que se ha trabajado en la última prueba. Además de aumentar la distancia se debería adecuar el algoritmo de autolocalización para situaciones donde tenemos varios puntos similares con un entorno también similar. Una vez hemos validado el algoritmo de autolocalización, encontrando sus errores, procedemos a hacer las pruebas pertinentes por todo el laboratorio y algunas más por el exterior. Prueba 8 El lugar desde donde empieza el robot es el siguiente: Fig. 1 Por el pasillo el robot pasa sobrándole aproximadamente 20 cm. Por cada uno de sus lados. Los movimientos que realiza el robot y los valores retornados por la auto localización son: 55 Movimiento Hacia delante Hacia delante Hacia delante Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Hacia delante Translación slam3D Rotación slam3D X: -0.025038 Pitch(X): 1.530729 Y: -0.041356 Yaw(Y): -0.917444 Z: 0.447597 Roll(Z): -0.251264 X:-0.000261 Pitch(X): 0.151632 Y:0.003035 Yaw(Y): -0.655127 Z:0.495578 Roll(Z): -0.056774 X:0.152546 Pitch(X):15.025910 Y:-0.357287 Yaw(Y): 4.586093 Z:0.548065 Roll(Z): 0.216975 X:0.016702 Pitch(X): 0.322939 Y:-0.003961 Yaw(Y): -2.953108 Z: 0.036942 Roll(Z): -0.031830 X: 0.009525 Pitch(X):-0.128891 Y: -0.000326 Yaw(Y): -2.979805 Z: -0.004046 Roll(Z): 0.236883 X: 0.023951 Pitch(X):0.028099 Y: -0.000304 Yaw(Y): -2.787259 Z: -0.002946 Roll(Z): -0.043730 X:0.008387 Pitch(X):-0.115697 Y:0.002596 Yaw(Y): -2.738749 Z:-0.001597 Roll(Z): -0.034546 X:0.015092 Pitch(X):0.082448 Y:-0.001518 Yaw(Y):-2.872979 Z:-0.003204 Roll(Z): 0.086270 X:0.014409 Pitch(X):-0.076194 Y:0.001566 Yaw(Y):-3.066295 Z:-0.003205 Roll(Z): 0.145288 X:0.014150 Pitch(X):0.303518 Y:-0.007633 Yaw(Y):-2.782358 Z:-0.004601 Roll(Z) 0.108322 X:0.031233 Pitch(X):1.697234 Y:-0.032387 Yaw(Y):-2.701975 56 Giro hacia la izquierda Giro hacia la izquierda Giro hacia la derecha Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Z:0.435373 Roll(Z): 0.443707 X:-0.011302 Pitch(X):0.047806 Y:0.004318 Yaw(Y):1.596241 Z:0.046500 Roll(Z):-0.021628 X:-0.012500 Pitch(X): 0.337493 Y:-0.007526 Yaw(Y): 2.082374 Z:-0.001652 Roll(Z): 0.190753 X:0.002375 Pitch(X):-0.107273 Y:0.003868 Yaw(Y): -2.566985 Z:-0.003146 Roll(Z): -0.050412 X:-0.006064 Pitch(X):0.242432 Y:-0.004750 Yaw(Y): 2.087975 Z:0.003089 Roll(Z): -0.096117 X:-0.007886 Pitch(X):-0.310060 Y:0.008515 Yaw(Y): 2.502797 Z:0.002685 Roll(Z): -0.273324 X:-0.030975 Pitch(X):0.173393 Y:-0.001595 Yaw(Y):1.846465 Z:0.003786 Roll(Z): -0.022388 X:-0.026542 Pitch(X):0.022794 Y:-0.000161 Yaw(Y): 2.416044 Z:0.003742 Roll(Z): -0.034487 X:-0.010614 Pitch(X):-0.364546 Y:0.007571 Yaw(Y): 2.641406 Z:0.007740 Roll(Z): -0.367083 X:-0.027216 Pitch(X):-0.238927 Y:0.004899 Yaw(Y): 2.048639 Z:-0.000320 Roll(Z): -0.120321 X:-0.021957 Pitch(X):-0.038846 Y:0.002313 Yaw(Y):2.287533 Z:0.002534 Roll(Z):-0.207727 X:-0.013860 Pitch(X):-0.583342 Y:0.013607 Yaw(Y): 1.941597 57 Z:0.008025 Roll(Z): -0.354822 De esta prueba debemos destacar el error que se produce en la tercera línea, donde nos devuelve un giro sobre el eje X de más de 15 grados, cuando sabemos por la naturaleza de nuestro robot que es imposible que gire en torno al eje X. Las fotografías en las que se produce el error son las que podemos ver en las figuras 2 y 3 de la prueba actual. Fig. 2 Fig. 3 El error se produce al tener 2 sillas iguales con un entorno prácticamente idéntico, por lo que emparejar los puntos se transforma en una tarea realmente difícil y provoca el error que se ha detectado. Prueba 9 En este caso hacemos pruebas en otra zona del laboratorio, en la que tenemos más espacio libre. Sin embargo el robot empieza desde una zona donde no puede seguir recto, por lo que tendrá que ir girando hasta encontrar vía libre. La zona desde donde empezamos la podemos ver en la figura 1 de esta prueba. Fig. 1 58 Como podemos observar en la imagen anterior, si el robot siguiera recto colisionaría contra las sillas, por lo que presumiblemente deberá esquivarlas para seguir recto más tarde. En la secuencia de movimientos tenemos los movimientos necesarios para esquivar las sillas, además de los de avance hasta el robot y los de esquivar el armario que se ve al fondo de la fotografía. Los movimientos que realiza el robot con su auto localización son: Movimiento Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Giro hacia la derecha Hacia delante Hacia delante Hacia delante Hacia delante Hacia delante Translación Rotación X:-0.008464 Pitch(X): 0.206373 Y:-0.009837 Yaw(Y): -3.499528 Z:-0.013407 Roll(Z): 0.135861 X:0.017624 Pitch(X): -0.108570 Y:0.005676 Yaw(Y): -2.674218 Z:-0.003458 Roll(Z): 0.035734 X:0.016179 Pitch(X): -0.016018 Y:-0.001528 Yaw(Y): -2.194870 Z:0.000259 Roll(Z): 0.120674 X:0.011527 Pitch(X): -0.056364 Y:0.003214 Yaw(Y): -2.833463 Z:-0.000084 Roll(Z): 0.142104 X:0.032906 Pitch(X): 0.149844 Y:-0.011344 Yaw(Y): -2.824960 Z:0.437085 Roll(Z): -0.066952 X: 0.006324 Pitch(X): 0.522445 Y:0.001572 Yaw(Y): -0.663863 Z:0.518604 Roll(Z): -0.313589 X:-0.003225 Pitch(X): -0.111430 Y:0.001382 Yaw(Y): -0.345013 Z:0.478284 Roll(Z): -0.100012 X:-0.000289 Pitch(X): 0.881197 Y:-0.007702 Yaw(Y): -0.366739 Z:0.473848 Roll(Z): 0.037479 X: 0.014157 Pitch(X): 0.484769 Y:0.009000 Yaw(Y): -0.646705 59 Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Hacia delante Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Z:0.479544 Roll(Z): -0.198617 X:-0.020714 Pitch(X): -0.282561 Y:0.010239 Yaw(Y): 1.341480 Z:0.047776 Roll(Z): -0.248864 X:-0.017116 Pitch(X): -0.028086 Y:0.000470 Yaw(Y): 2.258349 Z:0.005944 Roll(Z): -0.118047 X:-0.029529 Pitch(X): -0.184490 Y:0.004300 Yaw(Y): 2.575656 Z:0.005846 Roll(Z): -0.384130 X:-0.019713 Pitch(X): 0.039008 Y:-0.003640 Yaw(Y): 2.589590 Z:0.007850 Roll(Z): 0.073263 X:-0.015931 Pitch(X): -0.041041 Y:-0.001004 Yaw(Y): 2.261453 Z:0.004858 Roll(Z): 0.026471 X:-0.018715 Pitch(X): -0.126588 Y:0.004432 Yaw(Y): 2.537314 Z:0.004815 Roll(Z): -0.130764 X:-0.032430 Pitch(X): 0.085974 Y:0.004710 Yaw(Y): 1.400738 Z:0.420759 Roll(Z): 0.050912 X:-0.021249 Pitch(X): -0.014443 Y:0.007920 Yaw(Y): 2.257303 Z:0.049472 Roll(Z): -0.297495 X:-0.013605 Pitch(X): -0.077481 Y:0.002592 Yaw(Y): 1.953237 Z:0.005734 Roll(Z): -0.050468 X:-0.016127 Pitch(X): -0.003222 Y:-0.000855 Yaw(Y): 2.978340 Z:0.004932 Roll(Z): 0.012368 X:-0.011962 Pitch(X): -0.040714 Y:-0.000514 Yaw(Y): 2.624778 60 Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Hacia delante Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Z:0.005540 Roll(Z): -0.135780 X:-0.016622 Pitch(X): -0.015565 Y:0.000605 Yaw(Y): 2.285519 Z:0.001454 Roll(Z): -0.166826 X:-0.008364 Pitch(X): -0.070454 Y:0.002874 Yaw(Y): 2.373462 Z:0.001760 Roll(Z): -0.147454 X:-0.010063 Pitch(X): -0.075650 Y:0.002426 Yaw(Y): 2.443271 Z:0.005477 Roll(Z): -0.025933 X:-0.018750 Pitch(X): -0.114749 Y:0.002391 Yaw(Y): 2.813545 Z:0.004095 Roll(Z): 0.064643 X:-0.002088 Pitch(X): 8.767062 Y:-0.212291 Yaw(Y): 0.534738 Z:0.436182 Roll(Z): 4.885701 X:-0.016291 Pitch(X): 0.289157 Y:-0.004206 Yaw(Y): 2.054444 Z:0.039724 Roll(Z): -0.140736 X:-0.018345 Pitch(X): -0.443216 Y:0.013810 Yaw(Y): 2.536514 Z:0.004181 Roll(Z): -0.203841 X:-0.018402 Pitch(X): -0.032516 Y:-0.002540 Yaw(Y): 2.018926 Z:0.003280 Roll(Z): -0.234619 X:-0.016293 Pitch(X): -0.649780 Y:0.017532 Yaw(Y): 2.706855 Z:0.009547 Roll(Z): -0.102938 X:-0.009688 Pitch(X): -0.532750 Y:0.018160 Yaw(Y): 2.695436 Z:0.009766 Roll(Z): -0.294966 X:-0.009544 Pitch(X): -0.258879 Y:0.006807 Yaw(Y): 2.811025 61 Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la derecha Giro hacia la izquierda Giro hacia la derecha Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Z:0.010118 Roll(Z): -0.111452 X:0.013013 Pitch(X): -0.886490 Y:0.025139 Yaw(Y): 3.109017 Z:0.013043 Roll(Z): -0.314300 X:-0.050033 Pitch(X): 0.373021 Y:-0.011109 Yaw(Y): 1.870358 Z:0.000015 Roll(Z): -0.197722 X:-0.049667 Pitch(X): 0.539470 Y:-0.015402 Yaw(Y): 1.639693 Z:-0.002665 Roll(Z): -0.360696 X:-0.027077 Pitch(X): -0.298857 Y:0.011118 Yaw(Y): 2.113409 Z:0.008479 Roll(Z): 0.041772 X:0.027025 Pitch(X): 0.366275 Y:-0.011215 Yaw(Y): -1.698186 Z:-0.002630 Roll(Z): 0.274576 X:-0.028186 Pitch(X): -0.029412 Y:0.001285 Yaw(Y): 1.363482 Z:0.001716 Roll(Z): -0.253117 X:0.033481 Pitch(X): 0.559590 Y:-0.016004 Yaw(Y): -1.849041 Z:-0.002355 Roll(Z): 0.250714 X:-0.016087 Pitch(X): 0.055361 Y:0.013350 Yaw(Y): 1.756979 Z:0.004345 Roll(Z): -0.315693 X:-0.020593 Pitch(X): -0.462842 Y:0.005908 Yaw(Y): 2.417547 Z:0.003727 Roll(Z): -0.000572 X:-0.029408 Pitch(X): -0.184237 Y:0.003423 Yaw(Y): 2.068764 Z:0.002278 Roll(Z): -0.045393 X:-0.028596 Pitch(X): -0.094882 Y:-0.006986 Yaw(Y): 2.010413 62 Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Giro hacia la izquierda Z:0.002976 Roll(Z): -0.097220 X:0.019792 Pitch(X): 0.367856 Y:0.003300 Yaw(Y): 2.684426 Z:0.006545 Roll(Z): 0.169335 X:-0.016093 Pitch(X): -0.049689 Y:0.003561 Yaw(Y): 2.611442 Z:0.002147 Roll(Z): -0.166791 X:-0.044757 Pitch(X): -0.101333 Y:-0.001190 Yaw(Y): 2.009313 Z:0.002745 Roll(Z): -0.191360 X:-0.020338 Pitch(X): 0.055970 Y:-0.008697 Yaw(Y): 2.090032 Z:0.002248 Roll(Z): -0.301255 X:-0.016959 Pitch(X): -0.005345 Y:-0.003320 Yaw(Y): 2.392916 Z:0.009253 Roll(Z): 0.282268 De esta prueba demos destacar los errores que se producen en el movimiento número 24, en el que tan sólo se avanza hacia delante y el programa de auto localización nos devuelve que además de avanzar hacia delante se ha producido un giro de más de 8 grados en el eje X y de más de 4 en el eje Z. Las fotografías que recibe el programa de auto localización para este movimiento se pueden ver en las figuras 2 y 3 de esta prueba. Fig. 2 Fig. 3 El emparejamiento de puntos se complica al distinguir pocos puntos, además de que de una fotografía a la otra perdemos gran cantidad de éstos. 63 Prueba 10 Esta prueba es similar a la número 8, pero se ha realizado en otro de los pasillos creados por el mobiliario del laboratorio. El punto de comienzo de la prueba se puede apreciar en la figura 1 de esta prueba. Fig. 1 Previsiblemente el robot debería girar hacia la derecha para esquivar la silla y después ir hacia delante. Los resultados obtenidos son: Movimiento Translación Rotación Giro hacia la derecha X:0.024296 Pitch(X): 0.016355 Y:-0.002108 Yaw(Y): -2.214464 Z:-0.009255 Roll(Z): -0.094992 X:0.006795 Pitch(X): 0.023487 Y:-0.000911 Yaw(Y): -2.621179 Z:-0.000886 Roll(Z): 0.188704 X:0.012102 Pitch(X): -0.017062 Y:0.000117 Yaw(Y): -2.918615 Z:0.003572 Roll(Z): 0.236591 X:0.018864 Pitch(X): 0.446560 Y:0.005693 Yaw(Y): -3.244366 Z:0.457929 Roll(Z): 0.054514 X:0.014985 Pitch(X): 4.266712 Giro hacia la derecha Giro hacia la derecha Hacia delante Hacia delante 64 Giro hacia la izquierda Giro hacia la izquierda Hacia delante Giro hacia la derecha Giro hacia la izquierda Giro hacia la derecha Y:-0.150680 Yaw(Y): -0.088075 Z:0.452636 Roll(Z): -0.338958 X:-0.008750 Pitch(X): 0.128865 Y:0.001055 Yaw(Y): 1.045870 Z:0.056689 Roll(Z): 0.000553 X:-0.011389 Pitch(X): -0.181417 Y:0.005258 Yaw(Y): 1.925846 Z:0.005539 Roll(Z): 0.085577 X:-0.031805 Pitch(X): 0.333823 Y:0.007668 Yaw(Y): 1.529216 Z:0.438251 Roll(Z): -0.082681 X:0.005964 Pitch(X): 0.041448 Y:0.001413 Yaw(Y): -2.135647 Z:0.049728 Roll(Z): 0.072588 X:-0.006323 Pitch(X): 0.009628 Y:0.000293 Yaw(Y): 0.852487 Z:0.001972 Roll(Z): -0.064549 X:0.009369 Pitch(X): 0.104028 Y:-0.003194 Yaw(Y): -1.332013 Z:0.000430 Roll(Z): -0.016068 En este caso volvemos a tener errores en la auto localización, aunque son más pequeños. En este caso en concreto, en los movimientos 4 y 5 vemos esas desviaciones. Para el movimiento 4 miraremos las figuras 2 y 3 de esta prueba y para el movimiento 5 las figuras 4 y 5. Fig. 2 Fig. 3 65 Fig. 4 Fig. 5 Prueba 11 Se realiza una nueva prueba empezando desde otra zona del laboratorio, en concreto desde la puerta de éste, como se puede observar en la figura 1 de esta prueba. Fig. 1 Como podemos ver en la imagen el robot deberá esquivar el perchero y avanzar hacia la izquierda. En este caso en concreto el robot avanzará y detectará que hay mayor espacio libre hacia la derecha, por lo que girará a la derecha y caerá en un mínimo local, que solucionará girando hacia la izquierda y posteriormente salvará el obstáculo. Los movimientos, así como la auto localización son: Movimiento Hacia delante Translación Slam3D Rotación Slam3D X:0.008790 Pitch(X): 0.449356 Y:-0.012070 Yaw(Y): -0.250765 Z:0.422387 Roll(Z): -0.782364 66 Giro a la derecha Giro a la derecha Giro a la derecha Giro a la derecha Giro a la derecha Giro a la izquierda Giro a la derecha Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda X:0.017938 Pitch(X): 0.192059 Y:-0.002266 Yaw(Y): -2.586682 Z:0.040288 Roll(Z): -0.021082 X:0.021149 Pitch(X): -0.251528 Y:0.005315 Yaw(Y): -2.592897 Z:-0.010444 Roll(Z): 0.705127 X:0.015587 Pitch(X): 0.255954 Y:-0.006918 Yaw(Y): -2.810922 Z:-0.006634 Roll(Z): 0.220313 X:0.040242 Pitch(X): -0.086052 Y:0.003352 Yaw(Y): -1.515175 Z:-0.004482 Roll(Z): 0.206257 X:0.021375 Pitch(X): 0.349125 Y:-0.008429 Yaw(Y): -3.409425 Z:-0.003832 Roll(Z): -0.074440 X:-0.001255 Pitch(X): 0.117872 Y:-0.002510 Yaw(Y): 1.285112 Z:0.002184 Roll(Z): -0.105863 X:0.007434 Pitch(X): 0.138158 Y:-0.003784 Yaw(Y): -1.315951 Z:0.001655 Roll(Z): -0.137169 X:-0.005083 Pitch(X): -0.179901 Y:0.005501 Yaw(Y): 0.949683 Z:0.003466 Roll(Z): 0.175105 X:-0.017796 Pitch(X): 0.017606 Y:0.001233 Yaw(Y): 1.525284 Z:0.009295 Roll(Z): -0.144648 X:-0.020315 Pitch(X): -0.232936 Y:0.007096 Yaw(Y): 3.176463 Z:0.012905 Roll(Z): -0.134274 X:-0.014188 Pitch(X): -0.096401 Y:0.003784 Yaw(Y): 1.725375 Z:0.008763 Roll(Z): -0.148562 67 Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda Giro a la izquierda Hacia delante Hacia delante X:-0.022328 Pitch(X): -0.034796 Y:0.000188 Yaw(Y): 3.205469 Z:0.013493 Roll(Z): -0.127265 X:-0.018254 Pitch(X): -0.217144 Y:0.007782 Yaw(Y): 2.450013 Z:0.009899 Roll(Z): 0.177632 X:-0.018184 Pitch(X): 0.112559 Y:-0.003159 Yaw(Y): 2.323020 Z:0.005326 Roll(Z): -0.401418 X:-0.013580 Pitch(X): 0.156751 Y:-0.005373 Yaw(Y): 2.624120 Z:0.008248 Roll(Z): -0.243435 X:-0.020341 Pitch(X): -0.018165 Y:-0.000209 Yaw(Y): 3.034866 Z:0.008628 Roll(Z): -0.165243 X:-0.004203 Pitch(X): 0.009998 Y:-0.000405 Yaw(Y): 2.294029 Z:0.023536 Roll(Z): -0.077538 X:-0.032601 Pitch(X): -0.015718 Y:0.001893 Yaw(Y): 2.649333 Z:0.013788 Roll(Z): -0.236554 X:-0.017784 Pitch(X): -0.089299 Y:0.006430 Yaw(Y): 2.719156 Z:0.005875 Roll(Z): -0.431660 X:-0.034401 Pitch(X): 0.576032 Y:-0.020239 Yaw(Y): 2.112059 Z:0.456717 Roll(Z): 1.093568 X:0.101660 Pitch(X): 1.255960 Y:-0.078064 Yaw(Y): 2.081599 Z:0.472147 Roll(Z): 2.449282 En esta prueba debemos destacar los 2 últimos movimientos hacia delante. En ellos vemos giros en sus 3 ejes. Esto tiene una explicación, ya que el robot en estos 68 movimientos está encima de la base del colgador (Fig. 2 y 3), por lo que realmente hay variaciones en los 3 ejes del robot. Fig. 2 Fig. 3 Prueba 12 Por último realizaremos otra prueba en el exterior, comprobando que la auto localización funciona de forma correcta en el caso en el que los puntos no sean extremadamente difíciles de emparejar dado su entorno. El lugar desde donde empezamos es: Fig. 1 El robot deberá seguir recto y girar para esquivar la pared. Los resultados obtenidos son: Movimiento Translación Rotación Hacia delante X:-0.014839 Pitch(X): 0.363264 Y:-0.001578 Yaw(Y) :-0.477288 Z:0.440937 Roll(Z): -0.021270 X:0.038989 Pitch(X): 0.661094 Y:0.016720 Yaw(Y): 0.655985 Hacia delante 69 Giro a la izquierda Giro a la izquierda Z:0.363177 Roll(Z): 0.420870 X:-0.007091 Pitch(X): -0.097834 Y:0.006682 Yaw(Y): 0.890368 Z:0.054636 Roll(Z): 0.029278 X:-0.034172 Pitch(X): 0.121757 Y:-0.010514 Yaw(Y): 5.355554 Z:-0.017462 Roll(Z): -0.166978 Como podemos observar en este caso y, a diferencia del anterior hecho en exteriores, vemos que funciona de forma correcta el algoritmo de auto localización. La diferencia entre el anterior, que no funcionaba de forma correcta, y éste es que en este caso los puntos y su entorno son diferentes, mientras que en el anterior no lo era, por lo que se emparejaba de forma incorrecta. 70 6.8 Conclusiones En este apartado hemos realizado la integración y la evaluación del algoritmo de auto localización que se nos ha cedido. Se ha realizado de forma correcta y satisfactoria la integración del programa de auto localización slam3D, demostrando por medio de las pruebas que no hay ningún problema en la integración de dicho programa. Se han realizado todas las pruebas necesarias para poder afirmar que el programa de auto localización funciona de forma correcta con pequeñas limitaciones. Estas limitaciones son: 1. Fotografías en la que no somos capaces de identificar ningún punto. 2. Pequeños errores producidos por el algoritmo y por las distancias captadas por la cámara. 3. Trabajar con puntos muy lejanos. Se ha propuesto e implementado soluciones para la primera de las limitaciones, que consiste en no pasar el fichero al programa, sino que realizaremos otro movimiento y haremos la auto localización respecto a 2 movimientos. Para la segunda de las limitaciones no podemos encontrar una solución, ya que depende de la calibración de la cámara y de su resolución, así como de su SDK Triclops. Con todo lo visto podemos afirmar que se han cumplido todos los objetivos marcados en este apartado. 71 7. Desarrollo del proyecto El proyecto se ha dividido en los siguientes apartados que se han ido realizando de forma cronológica: 1. Estudio de las librerías del robot, así como las de la cámara. 2. Implementación del algoritmo que controla los movimientos del robot. 3. Pruebas y evaluación del programa que controla los movimientos del robot. 4. Soluciones a los problemas detectados en el programa controlador de los movimientos del robot. 5. Estudio del algoritmo Slam3D para la auto localización del robot. 6. Integración del programa de auto localización con el programa controlador de los movimientos del robot. 7. Pruebas y evaluación de los programas de control de movimientos y auto localización integrados en el robot. 8. Soluciones a los problemas encontrados. 72 8 Conclusiones Atendiendo a los objetivos que teníamos al empezar el proyecto podemos decir que se han cumplido de forma satisfactoria, de manera que: - Se ha realizado el programa para controlar los movimientos del robot realizando antes un estudio exhaustivo de las librerías que controlan el movimiento del robot, su estado y el funcionamiento de los sensores de ultrasonidos. - Se han realizado las pruebas pertinentes para poder comprobar que funciona el programa controlador de los movimientos del robot, incluyendo pruebas en casos extremos como por ejemplo los mínimos locales. - Se han buscado los errores del programa que controla los movimientos del robot así como sus limitaciones, aplicado las soluciones pertinentes a los problemas encontrados. - Se ha realizado el estudio del programa de auto localización cedido para la realización del proyecto, encontrando en él sus limitaciones. - Se ha integrado de forma satisfactoria el programa de auto localización del robot con el programa controlador de sus movimientos. - Se ha comprobado, mediante diversas pruebas en diferentes entornos, el correcto funcionamiento del programa de auto localización del robot. 73 9. Bibliografía y recursos utilizados Programación en C, Luis Joyanes Aguilar e Ignacio Zahonero Martínez. Editorial McGrawHill. Manuales Aria-Reference y Saphira-Reference, que hacen referencia al uso del robot Pioneer P2AT. Páginas web consultadas: Información sobre la medición de distancias mediante ultrasonidos: http://picmania.garcia-cuervo.com/Proyectos_AUX_SRF04.htm Información sobre el robot Pioneer P2AT: http://www.activrobots.com/ROBOTS/p2at.html Información sobre la cámara digiclops: http://www.robosoft.fr/SHEET/06Vision/1004DIGICLOPS/digiclops.pdf Programas auxiliares: Programas de visión de imágenes: 1. Gimp 2. XinView Programas para visualización de gráficas a partir de puntos: 1. Gnuplot. 74 10. Manual de uso Para el uso del programa lo primero será encender el robot por el interruptor situado en uno de sus laterales (fig. 7). Fig. 7 Cuando el robot haya arrancado se deberá establecer una conexión por medio de buletooth con el robot. La conexión no debe ser necesariamente por blue-tooth, pero no es recomendable hacerla directamente por cable, ya que el robot se mueve, con lo que los movimientos estarán limitados por la longitud del cable. Una vez realizada la conexión, nos deberemos conectar por medio de una conexión ssh a la I.P. del robot, que por defecto será 10.0.0.2, de manera que el comando será: ssh [email protected] Una vez hayamos establecido la conexión por medio del túnel ssh compilaremos el programa en el caso en el que sea la primera vez que se ejecuta. Para compilarlo se ha realizado un fichero de makefile, de manera que para compilarlo simplemente tendremos que utilizar el comando make. Una vez compilado el programa lo ejecutaremos: ./example7 > fichero_resultados Redireccionamos la salida a un fichero para liberar a la conexión blue-tooth de la transmisión de datos y que tenga que transmitir lo mínimo. esto no es estrictamente necesario, y depende más de las necesidades del usuario para guardar los movimientos que supuestamente ha realizado el robot. Una vez ejecutado el programa se nos preguntará si queremos ejecutarlo en el modo guiado o en el modo no guiado. En el caso del modo guiado podremos consultar para cada movimiento todos los ficheros temporales. En el modo no guiado sólo podremos consultar el Fichero_movimientos, que guardará todos los movimientos realizados por el robot que se han detectado por la auto localización. 75 En el caso del modo guiado se consultará al usuario antes de realizar cada movimiento si se puede realizar dicho movimiento, por lo que se podrán consultar los siguientes ficheros: Puntos_sensor Æ fichero de puntos seleccionados por el robot para hacer el mapa para la decisión del giro. Out*.ppm Æ imagen captada. Out*.pts Æ puntos de la imagen. C0,c1 Æ ficheros con las imágenes utilizadas para la auto localización con los puntos identificados y emparejados. d0,d1,f0,f1 Æ los mismos ficheros que c0 y c1 pero filtrados los puntos erróneos y con la ponderación para encontrar la distancia correcta que se ha movido el robot. A parte de estos ficheros se podrá consultar el fichero de movimientos en cualquier momento. En el modo guiado para salir se deberá pulsar la tecla ‘n’ cuando se pida antes de cualquier movimiento. En el modo no guiado para salir pulsar cualquier tecla en cualquier momento. 76