Localización y generación de mapas del entorno (SLAM) de un robot por medio de una Kinect

Escola Tècnica Superior d’Enginyeria Informàtica Universitat Politècnica de València Localización y generación de mapas del entorno (SLAM) de un robot por medio de una Kinect. Proyecto Final de Carrera Ingeniería Informática Superior Autor: Joaquín Viñals Pons Director: Ángel Valera, Enrique Bernabeu 29 de septiembre de 2012 Resumen En este proyecto exploraremos las posibilidades del sensor Kinect de Microsoft en el ámbito de la robótica, así como la historia y el amplio repertorio de herramientas Open Source que nos permitirán desarrollar aplicaciones de navegación y mapeado haciendo uso del mismo. Palabras clave: robot móvil, Kinect, robótica, SLAM Índice general 1. Introducción 4 1.1. Justificación . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.2. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2. Desarrollo teórico 6 2.1. Conocimientos previos . . . . . . . . . . . . . . . . . . . . . . 6 2.1.1. Hardware . . . . . . . . . . . . . . . . . . . . . . . . . 6 2.1.2. Odometría . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.1.3. Dispositivo para la medición de distancias . . . . . . . 12 2.1.4. Modelos matemáticos . . . . . . . . . . . . . . . . . . . 16 2.2. SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.2.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . 17 2.2.2. Historia del problema del SLAM . . . . . . . . . . . . . 17 2.2.3. Modelado y solución . . . . . . . . . . . . . . . . . . . 19 2.2.4. Landmarks y extracción . . . . . . . . . . . . . . . . . 19 2.2.5. RANSAC . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.6. Asociación de datos . . . . . . . . . . . . . . . . . . . . 21 2.2.7. Mapeado . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.3. Navegación . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.3.1. Algoritmos de muestreo . . . . . . . . . . . . . . . . . 24 2.3.2. Trayectorias basadas en celdillas . . . . . . . . . . . . . 25 2.4. Algoritmos de búsqueda . . . . . . . . . . . . . . . . . . . . . 26 2.4.1. Búsqueda primero en anchura (BFS) . . . . . . . . . . 27 2.4.2. Búsqueda primero en profundidad (DFS) . . . . . . . . 27 2.4.3. Búsqueda informada. Algoritmos A*. . . . . . . . . . . 28 2.5. Historia de los controladores avanzados . . . . . . . . . . . . . 29 2.5.1. Primeros controladores . . . . . . . . . . . . . . . . . . 29 2.5.2. Controladores durante los 80 y 90 . . . . . . . . . . . . 30 2.5.3. Primeros intentos de control avanzado . . . . . . . . . 31 2.5.4. Finales de los 90 . . . . . . . . . . . . . . . . . . . . . 34 2.5.5. Nuevos intentos . . . . . . . . . . . . . . . . . . . . . . 35 2 Índice general Índice general 2.6. Generación actual . . . . . . . . . . . . . . . . . . . . . . . . . 37 2.6.1. Nintendo Wii . . . . . . . . . . . . . . . . . . . . . . . 37 2.6.2. Sony Playstation 3 . . . . . . . . . . . . . . . . . . . . 38 2.6.3. Microsoft Xbox 360 . . . . . . . . . . . . . . . . . . . . 38 2.7. Recursos y entornos de programación . . . . . . . . . . . . . . 41 2.7.1. LibFreenect. El conjunto de herramientas MRPT. . . . 41 2.7.2. OpenNI. El conjunto de herramientas ROS. . . . . . . . 42 2.8. Caso de estudio: RGBDSLAM . . . . . . . . . . . . . . . . . . 49 2.9. HOGMAN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 2.10. SURF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 2.11. ICP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 2.12. Entorno de desarrollo . . . . . . . . . . . . . . . . . . . . . . . 54 3. Desarrollo práctico 56 3.1. Preámbulos y configuraciones . . . . . . . . . . . . . . . . . . 56 3.1.1. Instalación del Sistema Operativo . . . . . . . . . . . . 56 3.1.2. Instalación del framework ROS . . . . . . . . . . . . . 60 3.1.3. Instalación de las dependencias necesarias para el proyecto 61 3.1.4. Instalación del programa RGBDSLAM . . . . . . . . . 65 3.1.5. Estudio de RGBDSLAM . . . . . . . . . . . . . . . . . 67 3.1.6. Instalación del entorno de desarrollo Eclipse y adaptación del proyecto 67 3.2. Desarrollo del proyecto . . . . . . . . . . . . . . . . . . . . . . 70 3.2.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . 70 3.2.2. Modelado . . . . . . . . . . . . . . . . . . . . . . . . . 71 3.2.3. Estructuras de datos . . . . . . . . . . . . . . . . . . . 71 3.2.4. Sistema de visión . . . . . . . . . . . . . . . . . . . . . 77 3.2.5. Modelado del sistema de visión . . . . . . . . . . . . . 79 3.2.6. Implementación del sistema de visión . . . . . . . . . . 80 3.2.7. Integración con RGBDSLAM . . . . . . . . . . . . . . 83 3.3. Plataforma final y adaptaciones . . . . . . . . . . . . . . . . . 84 3.4. Análisis de una ejecución del programa. . . . . . . . . . . . . . 87 4. Conclusiones y futuros proyectos 92 3 Capítulo 1 Introducción 1.1. Justificación Desde siempre, el ser humano ha utilizado los recursos a su disposición como herramientas para ayudarle en la realización de las tareas de la forma más efectiva, rápida y segura. A medida que la tecnología avanza, estas herramientas se convierten en maquinaria cada vez más compleja y capaz de realizar trabajos complicados de manera precisa y, muchas veces, mejor de lo que podría haberlo hecho un humano. Con el fin de eliminar la necesidad de un operario humano en trabajos repetitivos, peligrosos o realizados en entornos donde una persona podría resultar herida o para explotar al máximo las ventajas que ofrecen estas herramientas, nace el deseo de automatizar todo lo posible estos procesos. Una de las disciplinas que aparecen gracias a este deseo es la del estudio de los robots autónomos, máquinas capaces de tomar sus propias decisiones para conseguir cumplir un objetivo, como podría ser la exploración de entornos desconocidos. Aunque en principio parezcan disciplinas separadas, existe una cercana relación entre los campos de la electrónica con aplicaciones recreativas, como las consolas y la telefonía móvil, y la investigación científica, más orientada a la búsqueda de soluciones con un impacto práctico en nuestra vida cotidiana. Con un mercado en constante expansión, las empresas que se encargan del diseño y la construcción de video consolas y teléfonos móviles han dedicado grandes cantidades de dinero y recursos al desarrollo de dispositivos capaces de ofrecer al usuario una experiencia lo más inmersiva y completa posible, así como la posibilidad de diferenciarlas de la competencia. De esta forma, aparecen en el mercado herramientas con costes de produc4 Capítulo 1. Introducción 1.2. Objetivos ción reducidos y precios competitivos, pero con gran potencial en el ámbito científico. Herramientas como los diversos mandos de la consola Wii de Nintendo, o la cámara Kinect de Microsoft, han contribuido a que el público general tenga alcance a equipamiento para la investigación que no habría podido obtener de otra manera. En este proyecto utilizaremos una cámara Kinect y las herramientas a nuestra disposición para encontrar una aproximación a la solución del problema de la autolocalización y el mapeado de un robot móvil, uno de los principales problemas de la navegación autónoma de los robots móviles. 1.2. Objetivos El objetivo de este proyecto es explorar las posibilidades de la cámara Kinect como sensor en el ámbito de la robótica móvil, y conocer las diversas herramientas de desarrollo disponibles en este campo. Se pretende implementar un algoritmo que permita extraer la información disponible del sensor y con ella construir un mapa del entorno, al tiempo que el robot provisto con la cámara obtiene las órdenes de movimiento necesarias para cumplir su objetivo. Una lista de los objetivos que se han marcado para este proyecto: Estudio del sensor Kinect y sus restricciones físicas. Estudio del software disponible para el sensor. Estudio de las estructuras de datos proporcionadas por el sensor para su utilización en el proyecto. Estudio de algoritmos de generación de mapas y de navegación. Análisis de las imágenes capturadas por la cámara para la navegación y detección de obstáculos. Desarrollo de una aplicación que englobe los conocimientos adquirido. 5 Capítulo 2 Desarrollo teórico 2.1. Conocimientos previos SLAM, del Inglés Simultaneous Localization And Mapping, o en Español: Localización Y Mapeado Simultáneos o también Localización y Modelado Simultáneos. Es una técnica usada por robots y vehículos autónomos para construir un mapa de un entorno desconocido en el que se encuentra, a la vez que estima su trayectoria al desplazarse dentro de este entorno. Antes de explicar en qué consiste y cómo es posible resolver este problema, es necesario analizar las distintas alternativas que tenemos a nuestro alcance, ya que formaran parte de la solución[1]: 2.1.1. Hardware En esencia, tenemos dos componentes hardware necesarios para poder implementar un algoritmo que resuelva el problema del SLAM: el robot móvil y sus sensores, en particular los dispositivos usados para la medición de distancias. Robot móvil Un robot móvil es una máquina automática capaz de moverse en un determinado entorno. Los robots móviles, tienen la capacidad de desplazarse alrededor de un determinado entorno y no están fijos en una posición determinada. En contraposición, otros robots, como los industriales, generalmente consisten en un brazo articulado y una herramienta que está anclado en una superficie fija. Los robots móviles son foco de investigación actual, y casi todas las grandes universidades tienen un laboratorio dedicado al desarrollo de aplicaciones 6 Capítulo 2. Desarrollo teórico 2.1. Conocimientos previos en robótica móvil. Los robots móviles también pueden ser encontrados en la industria y en entornos militares y de seguridad. También posible encontrarlos en entornos domésticos, tanto como juguetes como realizando tareas domésticas (por ejemplo, el robot aspirador Roomba). Clasificación de los robots móviles Atendiendo a sus capacidades de movimiento, podemos clasificar los robots móviles en las siguientes categorías: Robots rodantes Son aquellos que, como su nombre indica, se desplazan haciendo uso de ruedas, generalmente montadas por pares en una configuración 2+2 como las de un vehículo por mera simplicidad. Habitualmente solo dos de sus ruedas presentan tracción y otras dos dirección, de forma que sea posible maniobrar el robot con un solo servomotor. Figura 2.1: Robot rodante con cuatro ruedas. También es frecuente encontrar distribuciones de ruedas montadas en modo triciclo, donde una rueda sirve para la dirección y las otras dos aportan la tracción. Otra opción es que la tercera rueda simplemente sea una rueda ‘loca’ y las otras dos aporten tanto la tracción como la dirección, mediante el método de las orugas tratado más adelante. 7 2.1. Conocimientos previos Capítulo 2. Desarrollo teórico Figura 2.2: Mini robot espía con seis ruedas. Existen algunos casos especiales en los que se usan otras configuraciones que dotan al robot de mejor adaptación a terrenos difíciles. En estos casos los algoritmos de control de movimiento adquieren una mayor complejidad, proporcional al número de elementos direccionables de forma independiente. Por último cabría considerar a los robots con orugas como un tipo de robot odante en el que se substituyen las ruedas por un mecanismo de oruga para la tracción. La dirección se consigue parando una de las orugas o haciéndolas girar en sentido contrario. Figura 2.3: Robot dotado de orugas. Robots andantes Respecto a los robots construidos a imagen y semejanza humana, con dos piernas, las técnicas de control necesarias son varias, pero todas ellas hacen uso de complejos algoritmos para poder mantener el equilibrio y caminar correctamente. Todos ellos son capaces de caminar bien sobre suelos planos y subir escaleras en algunos casos, pero no están preparados para caminar en suelos irregulares. Algunos incluso pueden realizar tareas como bailar, luchar o practicar deportes, pero esto requiere una programación sumamente compleja que no 8 Capítulo 2. Desarrollo teórico 2.1. Conocimientos previos Visión Una cosa que falta a todos los sensores de detección de distancias es la habilidad de extraer propiedades de las superficies observadas e identificar objetos. Las imágenes a color (o escala de grises) nos permiten utilizar un amplio repertorio de información para identificar y localizar componentes del entorno. Las principales ventajas de los sensores de visión son: Gran cantidad de información. Capacidad de extraer información en tres dimensiones del entorno. Las cámaras son sensores pasivos, que no necesitan emitir sonido o luz como los sónar y los escáneres láser. Sus inconvenientes son: Alto coste computacional para extraer la información de las imágenes. La visión está altamente influenciada por la cantidad de luz disponible. Son dispositivos caros. Es mucho más fácil interpretar la información obtenida por los otros tipos de sensores, con menor coste computacional y, por tanto, velocidad. Por este motivo la visión no suele usarse con tanta asiduidad. Figura 2.11: Cámara estereoscópica. El algoritmo que creemos estará íntimamente ligado a las elecciones de hardware que hagamos. La precisión, la tolerancia al ruido, el tipo de entorno en el que nuestro robot y nuestros sensores puedan trabajar nos condiciona en el desarrollo del modelo matemático que usaremos para buscar nuestra solución al problema del SLAM. 15 2.1. Conocimientos previos Capítulo 2. Desarrollo teórico 2.1.4. Modelos matemáticos Antes de adentrarnos en el la resolución del problema del SLAM, necesitamos conocer algunos fundamentos estadísticos en los que se basan sus soluciones. Teorema de Bayes P(A|B) = P(B|A)P(A) P(B).(2.1) El teorema de Bayes es importante ya que es capaz de vincular la probabilidad de que un suceso A suceda conociendo la probabilidad condicional de dicho suceso con respecto a otro suceso B, siempre que se conozca la distribución de probabilidad marginal de A[3]. Cuando tratamos de aplicar esta regla de Bayes a la resolución exitosa del SLAM, podemos considerar el suceso A como uno de los diferentes estados en los que se encontrará el sistema (explicado en 2.2.3), basado en la información del suceso B, por ejemplo, las medidas obtenidas por un sensor. La regla dice que podemos resolver este cálculo de manera muy sencilla multiplicando dos términos: la probabilidad (en nuestro modelo) de obtener la medida B en el estado A, y el grado de confianza que damos a que A sea precisamente el estado del sistema antes de recibir los datos. Filtro de Kalman El filtro de Kalman, o estimación lineal cuadrática, es un algoritmo que usa una serie de medidas observadas a lo largo del tiempo, que contienen un cierto nivel de ruido (como las medidas de un sensor o la propia estimación de la posición del robot) y otras inexactitudes, y produce estimaciones de variables desconocidas que tienden a ser más precisas de lo que serían basadas en una simple medición. Más formalmente, un filtro de Kalman opera recursivamente en flujos de datos con ruido para producir una estimación estadísticamente óptima del estado del sistema. El algoritmo funciona como un proceso en dos fases: en la fase de predicción, el filtro de Kalman produce estimaciones del estado actual de las variables, junto a algunas incertidumbres. Una vez capturada la salida del siguiente estado, el filtro actualiza sus estimaciones utlizando una media ponderada, con mayor peso a las predicciones con un mayor nivel de certeza. Este filtro asume que el sistema que intenta predecir se comporta de forma lineal, y que todas las medidas de error tienen una distribución Gaussiana. 16 Capítulo 2. Desarrollo teórico 2.2. SLAM Existen diversas variaciones de este filtro, entre ellas el filtro extendido de Kalman, usado comúnmente en la resolución del SLAM. Estas extensiones presentan algunas complicaciones con respecto al filtro original. Por ejemplo, el filtro extendido de Kalman no es un estimador estadístico óptimo, y si la estimación inicial del estado es errónea el filtro divergirá rápidamente del sistema real, debido a la linealización subyacente que el filtro lleva a cabo de estos sistemas no lineales. 2.2. SLAM 2.2.1. Introducción El problema de la localización y mapeado simultáneos consiste en descubrir si es posible para un robot móvil navegar a través de un entorno desconocido y construir, de manera incremental, un mapa consistente del mismo mientras que determina, al mismo tiempo, su posición dentro de este mapa. Una solución a este problema se ha considerado ”el Santo Grial” por la comunidad de especialistas en robótica móvil, ya que dotaría a los robots con las herramientas necesarias para ser completamente autónomos. La ”solución” del problema del SLAM ha sido uno de los sucesos más notables para la comunidad de los últimos años. SLAM ha sido formulado y resuelto como problema teórico en distintas formas. SLAM ha sido, también, implementado en diversos campos de la robótica móvil, como los robots de interiores, en exteriores, robots subacuáticos y robots voladores. A un nivel teórico y conceptual, SLAM puede considerarse un problema resuelto. Sin embargo, aún quedan varios problemas en la aplicación práctica de soluciones más generales de SLAM, y particularmente en la utilización y construcción de mapas perceptualmente ricos como parte de un algoritmo de SLAM[4]. 2.2.2. Historia del problema del SLAM El nacimiento del problema del SLAM ocurre durante la Conferencia sobre Robótica y Automática del IEEE, en la ciudad de San Francisco, en 1986. Durante estos años, los métodos probabilísticos comenzaban a ser introducidos en los campos de la robótica y la inteligencia artificial. Un grupo de asistentes mantuvieron una conversación sobre la aplicación de métodos de estimación a los problemas de mapeado y localización. Como resultado de esta conversación, se reconoció que el mapeado consistente probabílistico era un problema fundamental de la robótica que merecía ser tratado. Durante los siguientes años se describen en diferentes estudios 17 2.2. SLAM Capítulo 2. Desarrollo teórico las bases estadísticas para describir las relaciones entre los objetos de referencia (conocidos como landmarks) y la manipulación de la incertidumbre geómetrica. Un elemento clave de estos trabajos fue mostrar que debía existir una gran correlación entre las estimaciones de la localización de diferentes landmarks, y que ésta debía crecer con las sucesivas observaciones. Al mismo tiempo, se estaban produciendo los primeros avances en la navegación visual y en la navegación por sónar, usando algoritmos basados en filtros de Kalman. Estas dos ramas separadas de investigación combinadas ayudaron en la investigación de la navegación de robots móviles basada en landmarks. Estas investigaciones demostraron que para un robot móvil moviéndose a través de un entorno desconocido y capturando observaciones relativas de puntos de referencia, las estimaciones de éstas están necesariamente correlacionadas entre sí a causa del error común en la estimación de la posición del vehículo. La implicación de esto era profunda: una solución consistente al problema combinado de la localización y el mapeado requeriría un estado conjunto compuesto por la posición de cada una de los objetos de referencia, que debería ser actualizado con cada observación de unos de estos objetos. Esto requeriría que el estimador de posición almacenara un gran vector de estados (de orden igual al número de landmarks que se conserven del mapa), con un coste computacional escalado al doble del cuadrado de landmarks. Lamentablemente, estos primeros estudios no observaron las propiedades convergentes del error en las estimaciones o del mapa. De hecho, se asumió que los errores en la estimación del mapa no convergirían y que, por el contrario, exhibirían un comportamiento errático sin cota para el error. Así, dada la complejidad computacional del problema del mapeado y con un completo desconocimiento del comportamiento convergente del mapa, los investigadores se centraron en una serie de aproximaciones a la solución al problema del mapeado consistente que asumía, e incluso forzaba, la minimización de las correlaciones entre los objetos de referencia, reduciendo así el filtro completo en una serie de filtros vehículo-referencia sin relación entre sí. El descubrimiento más importante fue que el problema del mapeado y la localización, una vez formulado como un único problema de estimación, era de naturaleza convergente. Aun más, se reconoció que las correlaciones entre los objetos de referencia, que otros científicos intentaban minimizar o eliminar, eran la pieza clave del problema y que, de hecho, cuanto más ricas fueran estas correlaciones, mejor sería la solución[5]. 18 Capítulo 2. Desarrollo teórico 2.2. SLAM 2.2.3. Modelado y solución El proceso del SLAM consiste en un cierto número de pasos: extracción de características, asociación de datos, estimación del estado y actualización de las características. La finalidad del proceso es usar el entorno para actualizar la posición del robot. Dado que la odometría del robot no es enteramente fiable, no podemos depender directamente de ella. Debemos usar sensores de distancia para corregir esta posición. Esto se consigue extrayendo características del entorno y re-observándolas mientras el robot se mueve. Un filtro extendido de Kalman (EKF, por sus siglas en inglés) es responsable de actualizar la estimación de la posición del robot basándose en estas características, a las que hemos llamado puntos de referencia o landmarks. El filtro mantiene un estimado de la incertidumbre en la posición del robot y también de los landmarks que el robot ha visto en su entorno. 2.2.4. Landmarks y extracción Los puntos de referencia son características del entorno que pueden ser fácilmente distinguidas y re-observadas. Son utilizadas por el robot para poder saber dónde se encuentra. Hay una serie de requisitos para considerar una característica como un buen landmark[1]: Debe ser posible observarla desde distintas posiciones y ángulos. Debe ser lo suficientemente única como para poder ser distinguida de otras características. Es decir, si uno observa dos características y las re-observa en el futuro, debe ser posible identificar cuál es cuál. Cuanto más cerca se encuentran unas de otras, más difícil es realizar esta tarea. Aquellas características que decidamos deben ser landmarks, deben ser suficientes como para que el robot no circule durante largos periodos de tiempo sin observar ningún landmark, ya que el robot podría perderse. Si decidimos que algo debe ser un landmark, este debe permanecer estacionario. Una referencia en movimiento provocaría datos y estimaciones erróneas. La extracción de características puede conseguirse de diversas formas. Explicamos el método RANSAC, ya que será utilizado por el algoritmo que veremos en 2.8. 19 2.2. SLAM Capítulo 2. Desarrollo teórico 2.2.5. RANSAC RANSAC (del inglés, RANdom Sampling Consensus, consenso en un muestreo aleatorio) es un método iterativo que puede ser utilizado para extraer líneas rectas de una lectura de muestras de un sensor de distancias. Es usado en estadística como una herramienta para extraer datos anómalos, valores que difieren de la distribución normal de la población a la que pertenecen. Gracias a estas características, es posible utilizar este método para distinguir líneas rectas en un entorno en tres dimensiones, como las formadas por las paredes, techos, suelos y muebles que pueden encontrarse en un ambiente de interior. RANSAC encuentra estas líneas escogiendo una muestra aleatoria de lecturas del sensor de distancias y utiliza una técnica de análisis numérico que intenta encontrar la función que mejor se ajusta a la distribución de puntos. Esta técnica se conoce con el nombre de mínimos cuadrados, ya que pretende minimizar el cuadrado del error producido al restar la posición de la estimación con respecto a los puntos estimados. Al elevar el error al cuadrado obtenemos un conjunto de valores que siempre serán positivos, mientras que se otorga un mayor peso a los errores más grandes y un menor peso a los errores más pequeños. Una vez realizada esta estimación, RANSAC comprueba cuantas lecturas de su muestra aleatoria están cerca de esta aproximación. Si el número de coincidencias es superior a un límite, o consenso, podemos estar seguros de haber observado una línea recta, y por tanto un segmento de pared (o puerta o ventana). Figura 2.12: Filtrado de datos anómalos y extracción de línea por RANSAC. 20 Capítulo 2. Desarrollo teórico 2.2. SLAM 2.2.6. Asociación de datos El problema de la asociación de datos consiste en hacer corresponder las distintas observaciones de un mismo landmark. Es decir, ser capaces de identificar cuándo estamos observando una referencia ya extraída en una iteración anterior. En la práctica, pueden surgir los siguientes problemas: Es posible que no se re-observen landmarks en cada paso del algoritmo. Es posible observar y extraer un landmark, y luego no ser capaz de volverlo a ver en la duración de la ejecución del algoritmo. Es posible asociar, erróneamente, un landmark a otro visto anteriormente. Como se ha visto en 2.2.4 no debería ser posible extraer landmarks que causaran los dos primeros problemas, lo que quiere decir que hemos creado MALOS landmarks. Aun con buenos algoritmos de generación de landmarks, es posible crear malas referencias. Para poder lidiar con este problema, es necesario crear una política de asociación de datos que permita minimizar estos errores. Una solución común se basa en la creación de una base de datos que contendrá los objetos de referencia. Esta tabla está usualmente vacía. La primera regla que debe usarse es la de no considerar un objeto de esta tabla a no ser que se haya visto un número N de veces; de esta forma, eliminamos los casos en los que extraemos un landmark incorrecto. En cada nueva observación, extraemos todas las referencias que encontremos y las asociamos a la referencia más cercana que exista en nuestra base de datos (y que hayamos visto más de N veces). El cálculo de las distancias puede hacerse por distancia euclídea, por ejemplo. A esta técnica se la conoce como “nearest-neighbor”, vecino más cercano. Una vez asociada, la pareja de landmarks se analiza usando un filtro (como un filtro de Kalman) que nos dice si podemos considerar, dentro de un margen de incertidumbre, que ambas referencias se corresponden. De ser así, se incrementa el número de veces que se ha observado esta landmark en 1. En caso contrario, se incluye esta referencia como una nueva landmark en la base de datos vista 1 vez. Tan pronto el proceso de extracción de características y la asociación de datos finaliza, el proceso de SLAM puede considerarse dividido en tres fases: Actualizamos el estado estimado del robot usando la información dada por la odometría. Conociendo las acciones de control aplicadas al robot y la posición (estimada) anterior, es fácil estimar la posición actual. 21 2.2. SLAM Capítulo 2. Desarrollo teórico Corregir la estimación tras la re-observación de los landmarks. Usando la estimación de la posición es posible estimar dónde deberían estar ubicados. Usualmente, encontraremos diferencias. A estas diferencias las llamaremos ”innovación”. Básicamente, la innovación es la diferencia entre la posición estimada del robot y la posición real del robot, basada en lo que el robot es capaz de ”ver”. En este paso, la incertidumbre de cada landmark re-observado es actualizado también para reflejar los cambios recientes. Finalmente, comenzaremos nuevamente el ciclo, añadiendo nuevos landmarks al mapa del robot. Utilizaremos la información actualizada sobre posición e incertidumbre obtenida en este paso. Así pues, el proceso del SLAM puede esquematizarse de la siguiente manera: Figura 2.13: Esquema de un algoritmo de SLAM. 22 Capítulo 2. Desarrollo teórico 2.2. SLAM 2.2.7. Mapeado El objetivo del mapeado es representar la información obtenida del entorno de forma que sea posible utilizarla para planificar la navegación y la resolución de tareas. Existen dos enfoques diferentes: topológico y métrico. Las representaciones topológicas únicamente consideran “lugares” y las conexiones entre ellos. Estos mapas tienen forma de grafos, donde los nodos representan distintas posiciones alcanzables por el robot y las aristas, la posibilidad de acceder a uno desde el otro. Es posible almacenar las distancias entre posiciones como peso de las aristas que las unen. Las representaciones geométricas consideran el espacio bidimensional en el que se producirá el desplazamiento y ubica en éste los objetos. Las coordenadas de los objetos son elegidas con precisión. Ésta es la representación preferida al resolver el problema del SLAM. Un ejemplo de representación geométrica es la generación de mapas por celdillas de ocupación. El método se basa en discretizar el espacio, dividiéndolo en unidades de tamaño predefinido, que se clasifican como ocupadas o vacías con un determinado nivel de confianza o probabilidad. Estas soluciones parten de la hipótesis de que la posición del robot es conocida. En la práctica, se necesita de algún método de localización que estime la posición del robot en cada instante que, en este caso, no es considerada una variable estocástica. La precisión que alcanzan estos mapas en la descripción del entorno (tanto mayor cuanto más fina es la división del espacio), permite que el algoritmo de localización empleado acumule errores reducidos a lo largo de intervalos prolongados de tiempo. Así pues, la mayor desventaja de estos métodos es la pérdida de potencia que se deriva de no tener en cuenta la incertidumbre asociada a la posición del robot, lo cual origina que su capacidad para cerrar bucles correctamente se vea mermada. Entre sus ventajas cabe destacar las siguientes: El algoritmo es robusto y su implementación sencilla. No hace suposiciones acerca de la naturaleza geométrica de los elementos presentes en el entorno. Distingue entre zonas ocupadas y vacías, consiguiendo una partición y descripción completa del espacio explorado. Por esto motivo es popular en tareas de navegación, al facilitar la planificación y generación de trayectorias empleando métodos convencionales. Permite descripciones arbitrariamente densas o precisas del mundo, simplemente aumentando la resolución de la rejilla que lo divide (es 23 2.3. Navegación Capítulo 2. Desarrollo teórico decir, disminuyendo el tamaño de las celdillas individuales). Como es lógico, esto va en detrimento del rendimiento computacional del algoritmo. Permite una extensión conceptualmente simple al espacio tridimensional. Una vez estimada la posición del robot y elegida la representación del mapa, podemos elegir la estrategia para conseguir elegir el camino que habrá de seguir el robot durante su exploración. Se conoce a este problema como ”planificación de movimientos” o ”problema de la navegación”. 2.3. Navegación Una forma básica de este problema es la producción de un movimiento continuo que conecte una posición inicial y una posición final, evitando cualquier tipo de colisiones con los obstáculos que existan en el entorno (paredes, muebles, escaleras, etc). Cuando el número de dimensiones en las que puede desplazarse el robot es reducido, es posible resolver este problema con algoritmos que discretizan el espacio aplicando un patrón de celdillas de ocupación. En casos con mayor complejidad espacial, el tratamiento exacto de la planificación de caminos puede considerarse computacionalmente indecidibles. En estos casos, es posible crear algoritmos basados en el muestreo que han demostrado ser bastante eficaces en muchos casos. No es posible detectar los casos en los que no existe un camino entre el inicio y el final, pero la probabilidad que tienen estos algoritmos de fallar decrece cuanto mayor sea el tiempo empleado en la navegación. Los algoritmos de muestreo se consideran extremadamente buenos en planificación de movimientos en espacios multidimensionales, y se han aplicado a problemas con docenas e incluso cientos de dimensiones (manipuladores robóticos, moléculas biológicas, personajes animados digitales, robots humanoides). 2.3.1. Algoritmos de muestreo Un algoritmo básico toma muestras de N diferentes configuraciones (por ejemplo, posiciones de las articulaciones en un brazo robótico) y las proyecta al espacio del problema, C. De todas ellas, elegirá únicamente aquellas que permanezcan dentro de las restricciones del sistema (colisiones con obstáculos, por ejemplos) y las utilizará como marcas o milestones. 24 Capítulo 2. Desarrollo teórico 2.5. Historia de los controladores avanzados Figura 2.20: Controladores a principios de los años 90. Nintendo. También añadió botones de control extra, tres en un principio que luego se extendieron a seis. En respuesta a la nueva competencia, Nintendo comenzó a experimentar con otros tipos de control. Sin embargo, para su siguiente consola (Super NES o SNES), el mando era muy similar al anterior. A pesar de ello, se introdujo el diseño que serviría de inspiración para todos los controladores que existirían en un futuro: botones traseros o ”gatillo”. La idea era utilizar los dedos índice para operar dos botones extra que estaban en la parte trasera del mando, perpendiculares al resto de botones. 2.5.3. Primeros intentos de control avanzado Es durante estos años de rivalidad entre Nintendo y Sega en los que ambas compañías intentan diferenciarse mediante estilos de control novedosos, que utilizaban sensores y tecnologías más avanzados. Power Glove Figura 2.21: Power Glove, de Nintendo. Por primera vez en la historia, se diseña un artefacto capaz de replicar 31 2.5. Historia de los controladores avanzados Capítulo 2. Desarrollo teórico los movimientos de una mano, a tiempo real, en una televisión o monitor. El Power Glove es un guante de goma que incorpora un mando de NES en un lateral, así como un teclado numérico. Usando una tecnología basada en la patente de VPL DataGlove, este controlador puede detectar la posición de la mano gracias a un conjunto de tres micrófonos (situados sobre y a los lados del televisor) y dos pequeños altavoces ultrasónicos incorporados en el mando. Los altavoces se turnan emitiendo pulsos cortos de sonido, que los micrófonos reciben. El tiempo que tarda el sonido en viajar a cada uno de estos micrófonos y un cálculo por triangulación nos permiten conocer la posición X, Y y Z de la mano. Gracias a unos sensores recubiertos con tinta conductora situados en las articulaciones de los dedos, el guante también es capaz de distinguir con una precisión de 2 bits la posición de cuatro de los 5 dedos (por abaratar costes y complejidad hardware, el meñique se elimina ya que generalmente esta en la misma posición que el anular). Lamentablemente, debido a las limitaciones tanto de la consola como del guante, este controlador no fue lo suficientemente preciso como para funcionar correctamente y fue un fracaso en ventas. Sega Activator Figura 2.22: Activator, de Sega. Con el propósito de permitir a los jugadores participar con todo su cuerpo en el control del juego, Sega lanzó el Activator. El Activator consistía de un octágono negro de plástico de aproximadamente un metro de diámetro. El dispositivo debía situarse en el suelo, conectarse tanto a la consola como a una fuente de alimentación externa, y calibrarse. Un techo recto, sin luces directamente encima del controlador, y sin relieves era necesario para poder jugar. 32 Capítulo 2. Desarrollo teórico 2.5. Historia de los controladores avanzados Cada lado del octágono contiene dos emisores de infrarrojos, así como dos receptores. Un haz de luz se proyecta hasta el techo, rebota y es captado por el receptor. El jugador participa situándose en el centro del mando e interrumpiendo los haces con sus extremidades. Pese a que la idea del controlador era la de replicar el movimiento del jugador en la pantalla, en realidad, cada uno de los 16 haces representaba uno de los botones del mando al que reemplazaba. Ésto, añadido a la falta de juegos diseñados específicamente para el Activator lo convierte en una mala opción de reemplazo del mando original. NES Zapper Figura 2.23: Zapper, de Nintendo La Zapper es una pistola de luz para la NES. Se conecta a un puerto para mandos de la consola, reemplazando el mando del segundo jugador. La pistola consiste en un mecanismo muy simple: un botón hace las veces de gatillo, y dentro del ”cañón” hay un receptor de infrarrojos. Cuando el gatillo se presiona, la pantalla entera se vuelve negra durante un breve instante. Inmediatamente después, la zona objetivo se vuelve blanca, mientras que el resto de la pantalla permanece negra. Si el receptor en la punta del arma detecta este cambio de luz, el juego registra un “acierto”. El proceso de disparo es lo suficientemente rápido como para que el ojo humano no sea capaz de detectarlo (aunque es posible notar que la pantalla ”parpadea”). Debido a la naturaleza del efecto detectado por la pistola, sólo es posible utilizarla en pantallas CRT. 33 2.5. Historia de los controladores avanzados Capítulo 2. Desarrollo teórico 2.5.4. Finales de los 90 La era de las consolas de 32 bits comienza cuando Sega saca al mercado su consola Saturn. Ésta adopta la filosofía de Nintendo de usar mandos de reducido tamaño, con gatillos traseros. Quien traerá las primeras innovaciones será Sony con su consola Play Station y su mando Dual Shock. Por primera vez los mandos tienen un diseño ”con cuernos”, que permiten al jugador sujetar con comodidad el controlador. El nuevo diseño incluye un D-Pad, 4 botones de acción y cuatro gatillos traseros, así como dos joysticks analógicos en el centro. En respuesta a estos cambios, Nintendo pone a la venta su Nintendo 64 con su nuevo mando. Éste, como el Dual Shock, también tiene un diseño ”con cuernos”, tres, en este caso. Incluye un stick analógico para la dirección, un D-Pad, cuatro botones de acción, tres gatillos traseros y, por primera vez, un stick analógico como botón de acción. También incluye, como novedad, la posibilidad de añadir un accesorio de vibración, también llamada force-feedback, idea que sería incluida en posteriores versiones del Dual Shock de Sony. Sega decide sacar su propia versión de mando renovado, incluyendo dos joysticks (que ahora pasan a llamarse ThumbSticks, al poderse manejar con los pulgares) que, además, podían ser presionados como botones tradicionales. Este concepto será utilizado en muchos de los mandos posteriores. Durante los años siguientes, a pesar de tener una nueva competidora en el mercado (Microsoft) y una generación nueva de consolas, no se producen grandes cambios en el diseño de los controladores. Figura 2.24: Controladores a finales de los años 90. Surgen, sin embargo, algunas ideas interesantes de control alternativo. 34 Capítulo 2. Desarrollo teórico 2.5. Historia de los controladores avanzados 2.5.5. Nuevos intentos Plataformas de baile Figura 2.25: Plataforma de baile para el juego Dance, Dance, revolution. Una plataforma de baile es un controlador plano electrónico utilizado en videojuego para la entrada en los juegos de baile. La mayoría de las almohadillas de baile se divide en una matriz de 3 x 3 de paneles cuadrados para el jugador los pise, con algunos o todos de los paneles correspondientes a las instrucciones o acciones dentro del juego. Algunas también tienen botones adicionales fuera del área principal de paneles, como por ejemplo ”Inicio” y ”Seleccionar”. Los pares de plataformas de baile a menudo se unen de lado a lado para ciertos modos de juego. Muchas de estas plataformas de baile emplean sensores de presión, como galgas extensiométricas o sensores piezoeléctricos. Sin embargo, algunas plataformas de baile más avanzadas, llamadas plataformas de estado sólido, utilizan sensores de proximidad. De esta manera se elimina la necesidad de partes móviles, que pueden romperse con el uso. Eye Toy Eye Toy es una cámara a color, digital, similar a una webcam en prestaciones. La tecnología usa visión por ordenador y reconocimiento de gestos para procesar las imágenes adquiridas por la cámara. Esto permite a los jugadores interactuar con los juegos usando movimientos, detección de colores y, gracias al micrófono incluido, sonido. La cámara requiere una habitación bien iluminada para poder jugar correctamente. De no disponer de suficiente luz, se informa al jugador mediante un LED rojo parpadeante situado en la parte frontal. 35 2.5. Historia de los controladores avanzados Capítulo 2. Desarrollo teórico Figura 2.26: Eyetoy, de Sony Pistolas de luz Figura 2.27: Desert Eagle, de Trustmaster. Dadas las restricciones de las primeras pistolas de luz al trabajar con otras tecnologías que no sean CRT, estos dispositivos han evolucionado para adaptarse a estas limitaciones. Una de las soluciones empleadas requiere la utilización de uno (o más) emisores de infrarrojos, que la pistola recibe en unos sensores especiales dentro del ”cañón” del arma. Al presionar el gatillo, la pistola envía a la consola la intensidad de los receptores que puede recibir. Al ser esta intensidad tanto función de la posición como de la inclinación, un conjunto de acelerómetros se utilizan para poder calcular la inclinación del arma y poder, por tanto, calcular la posición. Es una evolución de esta tecnología la que se usará en la presente generación de consolas. 36 Capítulo 2. Desarrollo teórico 2.6. Generación actual 2.6. Generación actual Las tres consolas de la generación actual (Sony Playstation 3, Nintendo Wii, Microsoft Xbox 360) utilizan distintos tipos de controladores avanzados. Figura 2.28: Mandos a finales de la década pasada. 2.6.1. Nintendo Wii El Wii Remote tiene la capacidad de detectar la aceleración a lo largo de tres ejes mediante la utilización de un acelerómetro ADXL330. El Wiimote también cuenta con un sensor óptico PixArt, lo que le permite determinar el lugar al que el Wiimote está apuntando; además de agregar una brújula electrónica en el WiiMotionPlus. A diferencia de un mando que detecta la luz de una pantalla de televisión, el Wiimote detecta la luz de la Barra sensor de la consola, lo que permite el uso coherente, independientemente del tipo o tamaño de la televisión. La barra puede ser colocada por encima o por debajo de la televisión, y debe centrarse. Si está colocada por encima, el sensor debe estar alineado con la parte delantera de la televisión, y si coloca en la parte inferior, debe alinearse con la parte delantera de la superficie de la televisión en la que se coloca. No es necesario señalar directamente a la barra sensor, pero apuntar significativamente fuera de la barra de posición perturbará la capacidad de detección debido al limitado ángulo de visión del Wiimote. El uso de la barra de sensores permite al Wiimote ser utilizado como un dispositivo de señalamiento preciso de hasta 5 metros de distancia de la barra. El sensor de imagen del Wiimote se utiliza para localizar los puntos de luz de la barra con respecto al campo de visión del Wiimote. La barra de sensores es necesaria cuando el Wiimote está controlando movimientos arriba-abajo o izquierda-derecha de un cursor en la pantalla del televisor para apuntar a las opciones de menú u objetos como los enemigos en un juego. 37 2.6. Generación actual Capítulo 2. Desarrollo teórico 2.6.2. Sony Playstation 3 En la Playstation 3 llegó una importante revolución en el aspecto de controladores para consolas de Sony. El mando añadió la función de detección del movimiento. A este primer mando se le llamó Sixaxis (seis ejes), haciendo referencia a los seis ejes de detección de movimiento (3 para movimientos posicionales en el espacio mediante acelerómetros, y 3 para la detección de rotación). Más tarde, debido a las críticas de los usuarios hacia la falta de vibración, se hizo una revisión del mando con el nombre Dual Shock 3, que añade la función de vibración de nuevo al mando. Al principio se diseñó un mando con forma de boomerang pero fue abandonado su diseño, volviendo al diseño tradicional del Dual Shock. La diferencia con el Wii Mote es que el mando de la Playstation 3 usa un chip para detectar los vuelcos del mismo, mientras que el mando de la Wii usa un sistema de chip giroscopio para sentir movimientos de desplazamiento, vuelco y posicionamiento al apuntar gracias a los infrarrojos emitidos por una barra que se puede colocar encima o debajo de la televisión. 2.6.3. Microsoft Xbox 360 Figura 2.29: Kinect, de Microsoft. Finalmente, Kinect para Xbox 360, o simplemente Kinect es un controlador de juego libre y entretenimiento creado por Alex Kipman, desarrollado por Microsoft para la videoconsola Xbox 360, y desde junio del 2011 para PC a través de Windows 7 y Windows 8. Kinect permite a los usuarios controlar e interactuar con la consola sin necesidad de tener contacto físico con un controlador de videojuegos tradicional, mediante una interfaz natural de usuario que reconoce gestos, comandos de voz, y objetos e imágenes. El dispositivo tiene como objetivo primordial 38 Capítulo 2. Desarrollo teórico 2.6. Generación actual aumentar el uso de la Xbox 360, más allá de la base de jugadores que posee en la actualidad. Kinect fue lanzado en Norteamérica el 4 de noviembre de 2010 y en Europa el 10 de noviembre de 2010. Fue lanzado en Australia, Nueva Zelanda y Singapur el 18 de noviembre de 2010, y en Japón el 20 de noviembre de ese mismo año. Características El sensor Kinect es una barra horizontal conectado a una base con un pivote motorizado con un rango de movimiento de aproximadamente 30o[7]. Cámara RGB La captura de imágenes a color es realizada por Kinect mediante una pequeña cámara RGB situada en la parte central del artefacto. Estas cámaras funcionan utilizando unos sensores capaces de convertir la señal recibida en forma de fotones a una señal electrónica digital que descompone la luz capturada en tres componentes: rojo, verde y azul (o Red, Green, Blue, de donde extraemos sus sílabas en inglés). En el caso de Kinect, el sensor encargado de realizar esta conversión es un sensor dispositivo de carga acoplada o CCD. Estos sensores están compuestos por una matriz de condensadores. El número de condensadores contenidos en el sensor determina su capacidad de resolución, que mediremos en píxeles. Un sensor CCD está basado en el efecto fotoeléctrico, un fenómeno físico por el cual la luz recibida es convertida en corriente eléctrica en algunos materiales. De esta forma, alterando la composición del material en el que se fabrican cada uno de los condensadores, es posible crear cedldas que reaccionan ante determinadas frecuencias de la luz (en este caso el espectro verde, el rojo y el azul). De esta forma, para conseguir la conversión de la imagen física en imagen digital, la mayoría de cámaras CCD utilizan una máscara de Bayer que proporciona una trama para cada conjunto de cuatro píxeles de forma que un pixel registra luz roja, otro luz azul y dos píxeles se reservan para la luz verde (el ojo humano es más sensible a la luz verde que a los colores rojo o azul). El resultado final incluye información sobre la luminosidad en cada píxel pero con una resolución en color menor que la resolución de iluminación. La información de color para cada píxel de la fotografía digital formada se calcula interpolando la señal de dos píxeles verdes, uno rojo y uno azul. De esta manera, aunque se pierde resolución de color, se gana resolución de iluminación, reduciendo el tiempo necesario de exposición del sensor a la luz 39 2.6. Generación actual Capítulo 2. Desarrollo teórico Figura 2.30: Distribucion de colores en un filtro de Bayer. y, por tanto, el ruido generado por la sobrecarga de los propios condensadores por la temperatura generada por la corriente inducida que los atraviesa. En particular, la cámara RGB equipada por Kinect tiene una resolución de 640x480 píxeles (VGA) y es capaz de procesar imágenes a una velocidad de 30 fotogramas por segundo, con una resolución de color de 32 bits (1 byte por cada color, 16.7 millones de colores). Sensor de profundidad El sensor de profundidad está formado por dos componentes: un generador de láser de infrarrojos y un sensor CMOS monocromo de luz infrarroja. Esta composición permite a la cámara capturar vídeo con información en tres dimensiones bajo cualquier condición de iluminación. A diferencia de la cámara RGB, el sensor de profundidad emplea tecnología CMOS en su sensor de imagen. La principal diferencia radica en la velocidad de respuesta de un sensor CMOS, permitiendo un tiempo de exposición menor y, por tanto, una mayor sensibilidad a la luz. La imagen en tres dimensiones es construida capturando la luz infrarroja proyectada por el láser, en forma de parrilla, y calculando la profundidad en base al tiempo que ha tardado la luz en volver al sensor (ver 2.1.3). El sensor de distancia produce imágenes con una resolución de 320x240 (QVGA) con una información de profundidad de 11 bits, que es traducida a una imagen en escala de grises de 16 bits. Para facilitar la sincronización con la cámara RGB, también emite vídeo a 30 fotogramas por segundo. En ambos casos, el ancho de campo horizontal está limitado a 57oy el ancho de campo vertical a 43o. En la figura 2.6.3 se observan las distintas partes que componen el sensor Kinect. Señalado con ”1” se encuentran el emisor (izquierda) y el receptor (derecha) del sensor de infrarrojos. El número ”2” señala la cámara RGB. La 40 Capítulo 2. Desarrollo teórico 2.7. Recursos y entornos de programación tf tf es un paquete que permite al usuario monitorizar y controlar múltiples ejes de coordenadas y su evolución en el tiempo. tf crea una estructura jerárquica en forma de árbol y ofrece la posibilidad de realizar transformaciones de puntos, vectores, etc, entre estos distintos marcos de referencia. Generalmente, un sistema robotizado tiene muchos ejes de coordenadas que cambian con respecto al tiempo (un buen ejemplo serían las articulaciones de un brazo mecánico). Este paquete proporciona los medios al usuario para mantener de forma ordenada toda esta información, así como realizar diferentes consultas al sistema sobre ellas. Los datos para cada nodo del árbol de transformaciones se almacenan en diferentes estructuras de datos definidas en la biblioteca tf. Estas estructuras incluyen: cuaterniones, vectores tri-dimensionales, puntos tri-dimensionales y transformadas. Las transformadas contienen información para generar las matrices de transformación entre cada eje de coordenadas almacenado y el nodo raíz. Las transformaciones entre nodos se realizan transformando ambos sistemas de referencia al del nodo raíz y luego realizando la operación inicial. tf permite, también, la visualización de los distintos ejes de referencia en programas como rviz o imprimir el árbol jerárquico en formato PDF. Figura 2.34: Múltiples ejes de coordenadas en un robot. PCL Point-Cloud Library[10] es un proyecto independiente a ROS, que contiene numerosos algoritmos incluyendo el filtrado, cálculo de referencias, reconstrucción de superficies, reconocimiento de modelos y segmentación de imágenes. Esto algoritmos pueden usarse, por ejemplo, para extraer datos 47 2.7. Recursos y entornos de programación Capítulo 2. Desarrollo teórico anómalos en poblaciones con ruido, unir distintas nubes de puntos, segmentar partes relevantes de una imagen e identificar y extraer landmarks del mundo real para la navegación con robots. También es posible convertir esta información matemática en imágenes y se ofrecen diversos formatos y aplicaciones con los que visualizarlos. Las nubes de puntos son estructuras de datos que representan una colección de puntos multidimensionales, y son usadas comúnmente en la representación de información en tres dimensiones. En una nube de puntos 3D, los puntos generalmente representan las coordenadas geométricas X, Y y Z de una superficie muestreada. Las nubes de puntos pueden adquirirse a través de sensores como las cámaras estereoscópicas, escáners 3D o generadas artificialmente en un programa de ordenador. PCL soporta nativamente las interfaces proporcionadas por OpenNI, como el driver de Kinect. La información de un sensor se almacena en una estructura de datos definida en el paquete sensor_msg de ROS llamada PointCloud. Entre otros datos, se almacenan las coordenadas X, Y y Z y el color correspondiente a ese punto, siempre que sea posible. A este tipo de nubes se las llama nubes de puntos coloreadas. Aunque la información de la profundidad extraída por el sensor de Microsoft no contiene información de color, es posible realizar una transformación entre el sistema de referencia del sensor de profundidad y la cámara RGB y asignar a cada punto un píxel de la imagen bidimensional, extrayendo así la información de color correcta. Figura 2.35: Varias estructuras 3D formadas a partir de nubes de puntos. OpenNI_Kinect OpenNI contiene todo el código necesario para realizar la comunicación con el dispositivo, así como la conversión de los datos obtenidos de los sensores a los distintos formatos ofrecidos por ROS, como nubes 48 Capítulo 2. Desarrollo teórico 2.8. Caso de estudio: RGBDSLAM de puntos o imágenes planas. Figura 2.36: NITE en funcionamiento. Además del driver, el paquete incluye una implementación en ROS de la biblioteca NITE, que incluye detección de poses, gestos y seguimiento de ”esqueletos” humanos. OpenNI_tracker añade a esta funcionalidad ventajas propias de ROS como el uso de tf para publicar las posiciones de cada una de las articulaciones detectadas. Figura 2.37: Dispositivos que cumplen el estándar OpenNI. 2.8. Caso de estudio: RGBDSLAM RGBDSLAM es un programa desarrollado para ROS que es capaz de registrar las nubes de puntos generadas por sensores como Kinect o cámaras estereoscópicas y crear mapas en forma de nubes de puntos densas coloreadas. 49 2.8. Caso de estudio: RGBDSLAM Capítulo 2. Desarrollo teórico Figura 2.38: Esquema de ejecución de RGBDSLAM. El algoritmo utilizado en esa solución el problema del SLAM consta de cuatro pasos. Primero, se extraen las características y landmarks de la información obtenida por el sensor utilizando un algoritmo llamado SURF (ver 2.10). Después, se comparan estas características contra las extraídas en iteraciones anteriores. Una vez obtenido el conjunto de correspondencias, se utiliza un algoritmo RANSAC ( 2.2.5) para estimar la transformación relativa entre los pares de imágenes obtenidas. El tercer paso consiste en una refinación de esta estimación usando una implementación del algoritmo ICP (ver 2.11). Estas estimaciones de la posición basadas en la comparación de parejas de fotogramas no es necesariamente consistente con la representación global del mapa, así que se realiza un cuarto paso en el que se optimiza el grafo global utilizando un algoritmo de resolución de grafos llamado HOGMAN (ver 2.9). RGBDSLAM permite a un robot generar modelos en tres dimensiones del espacio en el que se desplaza. Aunque los autores también consideran que aplicaciones fuera del campo de la robótica son posibles, como el escaneado en tres dimensiones de viviendas para su posterior modelado y utilización por diseñadores de interiores. 50 Capítulo 2. Desarrollo teórico 2.9. HOGMAN 2.9. HOGMAN Como en nuestro caso de estudio, otros algoritmos utilizan una representación del problema del SLAM basada en grafos. En este tipo de formulaciones, la diferentes posiciones del robot se modelan como nodos en un grafo. La información de las restricciones espaciales calculadas a partir de la odometría y de las observaciones del entorno se codifican en las aristas que conectan los nodos. En este tipo de soluciones, el problema del SLAM se divide en dos ramas independientes: la extracción e identificación de las restricciones mediante los sensores disponibles, lo que se conoce como front-end, y, una vez obtenidas estas restricciones, el cálculo de la posición más probable y de la incertidumbre de este cómputo se realiza en el motor de optimización o back-end. La solución a la primera rama se explica en 2.2.3, HOGMAN es el algoritmo usado para la segunda. Para garantizar un equilibrio razonable entre coste computacional y fiabilidad de la respuesta del algoritmo, HOGMAN utiliza una representación del espacio del problema (en este caso, el grafo de posiciones y sus restricciones) en diferentes niveles. Utilizando una representación no-euclídea del espacio tridimensional como simplificación del modelado del entorno, el algoritmo consigue un modelo susceptible a ser dividido en capas. La complejidad del modelo (y su similitud con el problema original) avanza a medida que profundizamos en esta estructura en niveles, como se puede observar en 2.9. Figura 2.39: Estructura en capas de un grafo. Este concepto permite estimar las rotaciones en tres dimensiones usando un modelo de Gauss-Newton, lo que simplifica el cálculo de las transformaciones en comparación a modelos más tradicionales, como el de ángulos de Euler. El algoritmo se comporta de manera ”perezosa” para optimizar el tiempo de cálculo, de manera que los cambios en cualquier nivel sólo se extienden 51 2.10. SURF Capítulo 2. Desarrollo teórico a las zonas del mapa que están directamente conectadas, sin re-optimizar todos los nodos del grafo en cada iteración. Así, cada vez que se obtiene una nueva observación, sólo el nivel superior -el más simplificadonecesita ser optimizado por completo. Cuando el mapa superior se cambia, sólo las regiones necesarias de los niveles inferiores reciben cambios. De esta manera, se conserva la consistencia global mientras que el coste computacional se reduce al mínimo posible. El objetivo que persigue el algoritmo HOGMAN es el de obtener, de entre todas las posibles, la posición (nodo del grafo) que maximice la correspondencia con la información sensorial obtenida en el front-end. Para esto, los creadores del algoritmo[11] han adaptado algoritmos conocidos (Gauss-Newton y Levenberg-Marquardt ([12])) de minimización de errores para trabajar en su modelado del entorno como un espacio no-Euclideano. 2.10. SURF SURF (Speeded-Up Robust Features, extracción acelerada de características) es un algoritmo de extracción y descripción de características, independiente de la escala y la rotación. SURF es capaz de aproximarse a (o, bajo ciertas condiciones, superar) las prestaciones de otros algoritmos similares en repetibilidad, diferenciación y robustez, aunque es posible computar sus cálculos mucho más rápido[13]. Esto se consigue mediante técnicas estadísticas de tratamiento de imágenes, en particular matrices Hessianas para las medidas del detector y un descriptor basado en muestreos aleatorios. Para localizar puntos de interés en una imagen se utiliza un filtro basado en matriz Hessiana para identificar los sectores donde el determinante de este filtro es máximo (filtros hessianos). De esta forma se consigue, al eliminar las zonas de la imagen que no se maximizan por el filtro, extraer una máscara para la imagen original que contiene la información relativa a los puntos de interés almacenados en estructuras con forma de ”burbuja”. Una vez extraída esta información, el siguiente paso consiste en asignar a cada punto de interés una ”orientación”, de manera que se facilite la identificación y correspondencia entre landmarks cuando las imágenes capturadas presentan una rotación, como ocurre durante el desplazamiento de un robot, por ejemplo. Esta forma de computación permite obtener información robusta y fiable con una gran mejora en cuanto a velocidad con respecto a otras alternativas, aun sin tener en cuenta posibles optimizaciones para plataformas particulares. Esto hace de este algoritmo una elección perfecta para aplicaciones 52 Capítulo 2. Desarrollo teórico 2.11. ICP Figura 2.40: Puntos de interés extraidos y sus orientaciones. empotradas con extracción de características online, como un algoritmo de SLAM. 2.11. ICP Iterative Closest Point (punto más cercano iterativo) es un algoritmo empleado para minimizar las diferencias entre dos nubes de puntos. ICP es comúnmente utilizado para reconstruir superficies tridimiensionales a partir de diferentes capturas de imagen, para la localización de robots, y para conseguir una planificación de trayectorias óptima. La estructura del algoritmo, en todas sus versiones, es bastante simple: Se asocian los puntos siguiendo el criterio del vecino más cercano. Se estima la transformación entre nubes mediante una función de mínimos cuadrados. Se transforman los puntos utilizando los parámetros estimados en los puntos anteriores. Se repite el proceso, iterativamente. Una de los problemas de este algoritmo es que es frecuente la acumulación de errores entre iteraciones, lo que puede provocar fallos en los algoritmos de mapeado. En particular, RGBDSLAM utiliza la implementación proporcionada por la biblioteca PCL que combina las mejoras de dos algoritmos (standardICP ypoint-to-plane ICP) y que ha probado, mediante pruebas en entornos simulados y reales, que es capaz de ofrecer un rendimiento temporal superior 53 2.12. Entorno de desarrollo Capítulo 2. Desarrollo teórico al de ambos algoritmos por separado y, a su vez, ser más robusto frente a los errores causados por las correspondencias incorrectas entre puntos. Este nuevo enfoque recibe el nombre de ICP generalizado, y puede encontrarse una descripción detallada del algoritmo, así como las pruebas a las que ha sido sometido, en [14]. 2.12. Entorno de desarrollo Gracias a que ROS ofrece un alto grado de integración con el entorno de desarrollo Eclipse y dada la probada utilidad de éste, así como sus potentes herramientas de compilación y depuración, se ha elegido esta plataforma para realizar el desarrollo del proyecto. Eclipse fue desarrollado originalmente por IBM como el sucesor de su familia de herramientas para VisualAge. Hoy en día, lo desarrolla la Fundación Eclipse, una organización independiente sin ánimo de lucro que fomenta una comunidad de código abierto y un conjunto de productos complementarios, capacidades y servicios Figura 2.41: Estado actual del proyecto. El entorno de desarrollo integrado (IDE) de Eclipse emplea módulos (en inglés plug-in) para proporcionar toda su funcionalidad al frente de la plataforma de cliente enriquecido, a diferencia de otros entornos monolíticos donde las funcionalidades están todas incluidas, las necesite el usuario o no. Este mecanismo de módulos es una plataforma ligera para componentes de software. Adicionalmente a permitirle a Eclipse extenderse usando otros lenguajes de programación como son C/C++ y Python, permite a Eclipse trabajar con lenguajes para procesado de texto como LaTeX, aplicaciones en red como Telnet y Sistema de gestión de base de datos. La arquitectura plugin permite escribir cualquier extensión deseada en el ambiente, como sería Gestión de la configuración. Se provee soporte para Java y CVS en el SDK de Eclipse. 54 Capítulo 2. Desarrollo teórico 2.12. Entorno de desarrollo Y no tiene por qué ser usado únicamente para soportar otros lenguajes de programación.. Eclipse dispone de un Editor de texto con resaltado de sintaxis. La compilación es en tiempo real.Cuenta con asistentes (wizards) para creación de proyectos, clases, tests, etc., y refactorización del código. Es posible extender sus funciones (mediante plugins) para controlar versiones mediante SVN, así como trabajar con lenguajes de abstracción de bases de datos, como Hibernate. A continuación se muestra una tabla resumen de la situación actual del software. Figura 2.42: Estimación del volumen de código desarrollado con Eclipse. Un punto muy importante a notar son los diversos lenguajes de programación utilizados en el desarrollo del proyecto, de acuerdo al análisis realizado usando SLOCCount, el lenguaje más utilizado es Java, seguido de ANSI C. 55 Capítulo 3 Desarrollo práctico Una vez comprendidos los principios teóricos en los que se basa el proyecto, se procede a exponer el trabajo desarrollado. 3.1. Preámbulos y configuraciones 3.1.1. Instalación del Sistema Operativo Dado que se ha decidido que la plataforma de desarrollo de este proyecto será ROS, es necesario preparar el equipo disponible en el laboratorio para tal fin. Ya que el soporte completo para ROS sólo se ofrece para el sistema operativo Ubuntu, la instalación de éste es el primer paso que se debe dar. Entre las múltiples opciones disponibles, el instalador para Windows Wubi (Windows-Based Ubuntu Installer) resulta la opción más cómoda. Wubi crea una instalación que no necesita una partición física en el disco duro, sino que reserva un espacio virtual dentro del mismo. De esta forma, la instalación del sistema operativo funciona de manera similar a como lo haría una máquina virtual, pero sin todas las complicaciones asociadas a la utilización de éstas. El primer paso para la instalación es dirigirse a la página del instalador en la web de Ubuntu (http://www.ubuntu.com/download/) y descargar el programa. Una vez lanzado, el programa nos ofrece tres opciones: 56 Capítulo 3. Desarrollo práctico 3.1. Preámbulos y configuraciones Biblioteca tf Aunque tf es uno de los stacks que se instalan junto con ROS, es necesario construir el paquete antes de poder utilizarlo. Esto se consigue con la siguiente orden rosmake tf Esta biblioteca permite acceder a ciertas herramientas que serán de utilidad durante el desarrollo del proyecto, las más destacables son: tf_monitor. Permite conocer información sobre el árbol de transformaciones de los procesos en ejecución. tf_echo. Cuando se acompaña esta orden de dos nodos del árbol de transformadas, tf_echo vuelca la información relativa a la transformación entre ambos. En concreto, devuelve la traslación y la rotación tanto en raw/pitch/yaw como en cuaterniones. Estudio de las opciones del framework para el simulado Toda la información proporcionada por tf es susceptible de ser interpretada por rviz, un entorno de visualización 3D para robots usando ROS, y es posible utilizar una versión simplificada, llamada turtlesim, que representa un conjunto de robots moviéndose en el plano XY como tortugas desplazándose por un estanque. Esta herramienta, en principio simple, permite comprender cuál es la correspondencia entre los ejes de movimiento y los movimientos reales de la cámara, por ejemplo. Para instalar turtlesim se necesita tener previamente instalado tf e instalar rviz. Esto puede solucionarse con las siguientes líneas de comandos: rosdep install turtle_tf rviz rosmake turtle_tf rviz Se puede comprobar el funcionamiento correcto de la instalación ejecutando el programa completo que se desarrollará en los tutoriales (ros.org/wiki/tf). En éste, un ”robot”, representado por una tortuga, recorre una trayectoria aleatoria, mientras que un segundo robot lo sigue a una corta distancia. El algoritmo utilizado por la segunda tortuga es una simplificación del algoritmo de persecución pura, por lo que las trayectorias no coinciden exactamente. 63 3.1. Preámbulos y configuraciones Capítulo 3. Desarrollo práctico Figura 3.10: Ejecución ejemplo del programa turtlesim En la figura 3.1.3 se puede observar la estructura en forma de árbol que forman los distintos nodos publicadores con respecto a las coordenadas comunes del ”mundo”, en este caso, el centro de la pantalla. Figura 3.11: Estructura en árbol del conjunto de ejes que participan en turtlesim 64 Capítulo 3. Desarrollo práctico 3.1. Preámbulos y configuraciones 3.1.4. Instalación del programa RGBDSLAM Aunque existe una versión disponible de RGBDSLAM en www.openslam.org, el autor recomienda utilizar subversion para obtener la versión constantemente actualizada del proyecto. Una vez obtenido el código y añadido al PATH de ROS, es necesario descargar dos dependencias por separado HOGMANminimal y g2o[15]. Cuando los tres paquetes han sido añadidos al PATH de ROS, una orden en la consola de comandos permite instalar los tres paquetes y sus dependencias principales: rosmake –rosdep-install hogman g2o rgbdslam Una vez este proceso finaliza (puede tardar unos veinte minutos en completarse) y con el sensor Kinect conectado correctamente al PC y la alimentación, puede lanzarse a ejecución el programa: roslaunch rgbdslam kinect+rgbdslam.launch La orden utilizada se asegura de lanzar el núcleo de ROS, los drivers de Kinect y RGBDSLAM para el correcto funcionamiento del programa. En la imagen 3.1.4 se observan cuatro secciones diferentes en pantalla principal: La imagen superior muestra el mapa generado. El mapeado toma la forma de una nube de puntos coloreada que combina la información de la cámara RGB y del sensor de distancias para asignar un color a cada punto con información tridimensional. La propia cámara puede encargarse de publicar la correlación entre ambas imágenes, corrigiendo el desplazamiento que existe entre ambos sensores, si se configura para publicar nubes de puntos registradas antes de lanzar el programa. Esta imagen contiene también el grafo que representa las distintas posiciones estimadas del robot, y su recorrido por dentro del mapa. La imagen izquierda de la zona inferior corresponde al stream de datos recibido de la cámara RGB. La imagen central corresponde al stream de datos de la cámara IR. Los colores en escala de grises codifican la distancia detectada. 65 3.1. Preámbulos y configuraciones Capítulo 3. Desarrollo práctico La imagen de la derecha muestra los puntos de interés extraídos, con la información del algoritmo RANSAC 2.2.5 superpuesta. El tamaño de los círculos representa el grado de confianza en la estimación (menor cuanto mayor es el grado) y la flecha muestra la orientación estimada en dicho algoritmo. Figura 3.12: Captura de una ejecución del programa RGBDSLAM El mapa tridimensional permite el control del punto de vista y del zoom y la posición de la cámara. También es posible, una vez detenida la captura de imágenes, exportar la nube de puntos final para su posterior visualizado y exploración en cualquier programa con soporte para PCL, como rviz. 66 Capítulo 3. Desarrollo práctico 3.1. Preámbulos y configuraciones 3.1.5. Estudio de RGBDSLAM Una vez comprobado el correcto funcionamiento de la instalación, se procede a analizar las posibilidades de la utilización de este algoritmo en la navegación de robots móviles. Utilizando los métodos vistos en 3.1.3 y 3.1.3, así como la documentación que acompaña al programa y la información obtenida al contactar con el autor, se obtienen las siguientes conclusiones: No es posible obtener información sobre el mapa de manera online. El tamaño de la nube de puntos generada hace imposible el publicado de la misma durante la ejecución del algoritmo y por tanto no puede utilizarse como referencia para la navegación. La solución a este problema se encuentra en 3.2.5. Sí es posible conocer en tiempo real la posición actual del robot con respecto a una posición inicial, considerada (0, 0, 0). La relación entre ambas se publica mediante tf, y es posible suscribirse a los topics /map y /openni_camera para obtener la transformada entre ambas. Para poder obtener un equilibrio entre exactitud y rendimiento, el algoritmo basa sus cálculos no en toda la historia de capturas obtenidas (a un ritmo de 20/30 capturas por segundo) sino en las últimas diez. Esto, sumado al hecho de que el backend utilizado 2.2.5 no puede detectar rotaciones inferiores a 10oni traslaciones inferiores a 5 centímetros de manera consistente, limita la velocidad de rotación a 20o/s y la de traslación a 0.4m/s. 3.1.6. Instalación del entorno de desarrollo Eclipse y adaptación del proyecto Para poder comenzar con el desarrollo del proyecto es necesario instalar el entorno de desarrollo (IDE) Eclipse. Aunque Ubuntu incluye una instalación del entorno por defecto, las instrucciones de ROS especifican explícitamente que es necesario utilizar una versión separada descargada desde la web de Eclipse. La versión escogida será la última disponible para desarrolladores de C/C++. Una vez descargada y descomprimida la versión, es necesario crear un paquete de ROS en el que comenzar el desarrollo y conseguir importarlo correctamente en Eclipse. Hay varias formas de realizar este trabajo (Cmake es la más utilizada), pero la forma más simple resulta ser la creación de un nuevo paquete vacío en ROS mediante el comando 67 3.1. Preámbulos y configuraciones Capítulo 3. Desarrollo práctico roscreate-pkg [nombre del proyecto] Si, como en el caso de este proyecto, las dependencias se conocen de antemano también puede modificarse la orden para incluirlas, aunque siempre es posible modificar el manifiesto en la carpeta del proyecto para agregarlas a mano. Por ejemplo, para la creación de este proyecto la orden completa sería: roscreate-pkg PFC_ROS rospy roscpp tf pcl pcl_ros sensor_msgs std_msgs Y la lista de dependencias en el manifiesto sería <depend package=”rospy”/> <depend package=”roscpp”/> <depend package=”tf”/> <depend package=”pcl”/> <depend package=”pcl_ros”/> <depend package=”sensor_msgs”/> <depend package=”std_msgs”/> Esto crea la estructura de carpetas adecuada para que ROS pueda indexar correctamente el paquete. Una vez hecho esto, se comprueba el correcto funcionamiento del paquete intentando cambiar al directorio que lo contiene usando roscd en la consola de comandos. Si todo ha funcionado correctamente, en este punto es posible crear un archivo vacío (llamado main.cpp, por ejemplo) e introducirlo en la carpeta ”src”. Luego, debido a ciertas incompatibilidades entre el IDE yROS, es necesario modificar manualmente el archivo ”CmakeLists.txt” que se habrá creado automáticamente. En concreto, la línea #rosbuild_add_executable(example examples/example.cpp) Debe quedar de la siguiente manera 68 Capítulo 3. Desarrollo práctico 3.1. Preámbulos y configuraciones rosbuild_add_executable(main src/main.cpp src/nodo.cpp) Ahora el proyecto está preparado para ser importado a Eclipse directamente. Para ello, en Eclipse es necesario clickar en ”Archivo->importar...” y seleccionar ”Proyectos existentes al Espacio de Trabajo”. Esto ofrece todas las comodidades del desarrollo en un IDE, como autocompletar texto, indentación automática y resaltado de sintaxis. Para poder compilar y ejecutar el proyecto es necesario establecer las variables del entorno necesarias. Esto se consigue haciendo click con el botón derecho sobre el proyecto y eligiendo ”Propiedades ->C/C++ Make Project” y comprobar que las variables ROS_ROOT, ROS_PACKAGE_PATH, PYTHONPATH y PATH están asignadas a los valores correctos (que pueden comprobarse escribiendo echo $[nombre de la variable] en consola). Figura 3.13: Configuración de las variables del entorno. También es necesario informar a Eclipse de dónde establecer las conexiones para poder realizar la conexión con ROS cambiando los valores de las va69 3.2. Desarrollo del proyecto Capítulo 3. Desarrollo práctico riables ROS_ROOT y ROS_MASTER_URI en ”Ejecutar ->Configuración de la ejecución ->Proyecto C/C++.” Figura 3.14: Configuración de las variables de ejecución. Si un ejecutable existe previamente (si hemos hecho ”make” en la carpeta del proyecto antes de importar) es necesario seleccionar el ejecutable correcto en la pestaña ”principal” del diálogo anterior. 3.2. Desarrollo del proyecto 3.2.1. Introducción Una vez preparado el entorno, es necesario establecer unos objetivos antes de comenzar el desarrollo. En primer lugar, se necesita que el robot que incorpore el sensor Kinect sea capaz de recibir e interpretar los datos proporcionados por la cámara, así como utilizarlos para desplazarse por el entorno. 70 Capítulo 3. Desarrollo práctico 3.2. Desarrollo del proyecto En segundo lugar, el robot ha de ser capaz de comunicarse con el algoritmo RGBDSLAM y utilizar la salida del mismo para localizarse en el entorno y ser capaz de cumplir una serie de objetivos, como alcanzar ciertas destinaciones, realizar un mapa completo, etc. Por último, es necesario adaptar la funcionalidad del algoritmo de SLAM a la navegación de robots móviles, ya que no está diseñado para este fin. No es posible conocer variables como la certidumbre de una determinada estimación de la posición, ni las acciones de control necesarias para alcanzar un destino. 3.2.2. Modelado Con estos objetivos en mente, se procede a escoger la forma de representar la información necesaria para la localización y la navegación. La estrategia elegida para gestionar el mapeado se corresponde con la versión basada en celdillas de navegación vista en 2.3.2 por su equilibrio entre simplicidad de implementación y la potencia de los posibles algoritmos de búsqueda de caminos que pueden crearse para este diseño. 3.2.3. Estructuras de datos Mapa Ya que se va a tratar con un diseño basado en mapas de ocupación[16], el problema del diseño de la estructura de datos para almacenar la información del mapeado tiene una solución, en principio, trivial: podemos representar el mapa dentro de una matriz de las dimensiones que nos permita el hardware almacenar. Esta matriz estará compuesta de elementos binarios, que indicarán si la porción del mundo representada por cada casilla se encuentra libre (con un valor de true) u ocupada (false). A esta información es necesario añadir una variable de incertidumbre, que nos diga qué tan fiable es la información que poseemos sobre este fragmento del entorno. En nuestro diseño, esta variable adquiere dos posibles valores: observado y no observado. El mapa, por tanto, se almacenará en memoria con la forma de un registro compuesto por dos valores binarios, que se corresponden con variables de tipo Bool en C++. La estructura en forma en forma de matriz bidimensional permite una conversión directa desde el espacio cartesiano, asignando columnas al eje de las X y filas al de las Y. Esta representación es trivialmente ampliable al espacio tridimensional, pero el robot que se utilizará en este proyecto 71 3.2. Desarrollo del proyecto Capítulo 3. Desarrollo práctico se trata de un robot móvil con ruedas, limitando las trayectorias al plano correspondiente al suelo. La resolución de la matriz, definida como la dimensión en metros cuadrados a los que corresponde cada celdilla de la matriz, está almacenada en una variable de tipo float que puede variar su tamaño según las posibilidades del hardware utilizado, pero que debe permanecer constante durante toda la ejecución del programa. Trayectorias Se define una trayectoria en este modelado como la lista de casillas que forman el camino desde una posición inicial hasta una posición final. Así pues, cada elemento de dicha lista debe contener un identificador único correspondiente a su ubicación en el espacio cartesiano. En concreto, cada elemento contiene una variable de tipo entero para su coordenada X y una para su coordenada Y. Se ha decidido que, para las aplicaciones de este proyecto, puede definirse la contigüidad de cada casilla como el conjunto de los casillas 4-conectadas, requiriendo numerosas modificaciones del algoritmo principal cualquier cambio que se haga en este respecto. La razón por la que se ha escogido 4-conexión frente a 8-conexión (u otras opciones más potentes) se explica en 3.2.5. Para la implementación de los algoritmos de búsqueda, y por la naturaleza dinámica del problema a resolver, se escoge una estructura de datos en forma de lista enlazada para almacenar el grafo de búsqueda. En dicho grafo, cada nodo corresponde con una casilla de la matriz y las aristas representan contigüidad. Al no tratarse de un mapa topológico 3.2.3 no es necesario almacenar el peso de las aristas, ya que es constante para todas ellas por la propiedad que representan. Cabe destacar que, aunque la contigüidad sea una cualidad inherentemente simétrica, la estructura en forma de lista y la forma en la que los punteros se comportan en C++ hacen de éste un grafo dirigido. Para facilitar el diseño del algoritmo de búsqueda de caminos, así como otras funciones del programa principal, se han realizado algunas modificaciones a la implementación común de la lista enlazada. La primera de ellas es la creación de una súper-clase que agrupa todas las funciones de acceso a la estructura de datos y que consiste en un simple puntero al primer nodo, que se corresponde con la casilla de inicio de la búsqueda. De esta forma, junto a la manera de introducir los nodos dentro de la estructura (como se explicará en 3.2.3) se puede crear una cierta jerarquía interna en la lista. Esto será útil en la gestión del árbol de búsqueda. La siguiente modificación realizada es la conversión de la lista de simple a 72 Capítulo 3. Desarrollo práctico 3.2. Desarrollo del proyecto 3.2.5. Modelado del sistema de visión Una vez investigadas las posibilidades de Kinect como sensor de distancias, se puede proceder al diseño de un modelo simplificado del cono de la cámara que funcione en el contexto de un mapeado basado en celdillas de ocupación. Debido al límite inferior de la distancia, es posible establecer una resolución de matriz de 0.4x0.4 metros. Usando esta información y la relativa al ancho de campo de la cámara, se puede establecer un modelado del cono del sensor similar al representado en la figura 3.2.5. Figura 3.21: Distribución de casillas en el cono de visión. Debido a las dimensiones del entorno en el que se desarrolló este proyecto, se ha limitado la profundidad del modelo en tres niveles, aunque es posible extenderlo al menos en dos niveles dadas las características de la cámara. El algoritmo de búsqueda debe ser modificado para adaptarse a la nueva manera de obtener la información de la ocupación de las celdillas. En principio, se considera el mapa completo como libre de obstáculos. En cada iteración, se comprueban todas las casillas posibles alrededor de la actual, actualizando todos los cambios detectados en el mapa. El algoritmo de búsqueda siempre tendrá, de esta manera, información fiable de todas las casillas 4-conectadas y podrá calcular el siguiente destino de la misma forma que lo hacía con anterioridad sí, y sólo sí, se obtiene información alrededor de la posición inicial antes de realizar el primer cálculo. Para reducir el número de giros que habría que realizar y, por tanto, el tiempo en que el algoritmo está esperando la información relativa al entorno los giros pasan por un proceso de selección similar al de los nodos (ver 3.2.3). 79 3.2. Desarrollo del proyecto Capítulo 3. Desarrollo práctico El algoritmo de minimización no incluirá un giro en la cola de movimientos a realizar si: El giro llevaría a la cámara a intentar obtener información de posiciones inalcanzables (fuera de los límites del mapa), siempre y cuando no se observe al menos una posición relevante en el proceso. El giro no aportaría nueva información. Es decir, el giro nos llevaría a mirar hacia el camino recorrido. Las siete casillas susceptibles de ser observadas ya han sido examinadas. Una casilla que se encuentra en un nivel superior (”detrás”) a una casilla con obstáculo se considera imposible de examinar. Una vez finalizado este proceso de selección se obtiene la información del entorno mediante el algoritmo de visión (ver 3.2.6), codificada en un vector de siete posiciones que se corresponden con la numeración vista en la figura 3.2.5. Con éste, se rellenan las celdillas correspondientes en el mapa y puede comenzar una nueva iteración del bucle de control: planificación de ruta, desplazamiento y obtención de información. 3.2.6. Implementación del sistema de visión Haciendo uso de las herramientas ofrecidas por la biblioteca PCL y la información obtenida de la cámara (2.7.2 y 3.1.3, respectivamente) es posible diseñar un filtro que cumpla dos objetivos: realizar correctamente la segmentación de la nube de puntos generada por el sensor y reducir la cantidad de datos que son procesados, optimizando los parámetros espaciales y temporales del programa. Partiendo de una nube de puntos que abarca todo el espacio cubierto por la cámara, se eliminan todos aquellos puntos que quedan fuera del área relevante para el robot, es decir, todos aquellos puntos que no representan información relativa a posibles obstáculos a considerar. Para esto, hacemos uso de un filtro pasa-banda proporcionado por la biblioteca PCL. El filtro funciona guardando en una nube de puntos auxiliar sólo aquellos puntos cuyas características se encuentren dentro de unos márgenes determinados. Estas características pueden ser tanto las coordenadas X, Y o Z como otros parámetros, como el color RGB del punto, por ejemplo. El primer filtrado de la nube de puntos elimina todos los puntos que se encuentran por encima del nivel de la cámara, punto más elevado del robot empleado. Se mantiene un pequeño margen (10 cms) para evitar posibles 80 Capítulo 3. Desarrollo práctico 3.2. Desarrollo del proyecto errores de lectura causados por el ruido del sensor. Asimismo, se eliminan los puntos correspondientes al suelo, que forman parte de la nube de puntos, pero no deben considerarse obstáculos. De esta forma se elimina un conjunto de puntos significativamente grande, ya que el suelo se almacena de forma densa en la nube, y se elimina toda la información respectiva a la parte superior de la imagen. Para obtener la información relativa a la profundidad inferior a la mínima detectada por el sensor – 40 centímetros – es necesario conocer una propiedad de la cámara: si el objeto es lo suficientemente ancho con respecto al campo de visión, la ”sombra” proyectada por éste bloqueará casi por completo el sensor. Aprovechando esta información, el siguiente filtrado elimina el conjunto de puntos que se encuentra fuera de la celdilla ubicada frente a la cámara, ya que no pueden formar parte de un obstáculo localizado delante del robot. Si el tamaño de la nube de puntos después del filtrado es mayor que cero, podemos asegurar que no hay ningún objeto obstruyendo el sensor y, por tanto, no hay obstáculos en la casilla observada. Partiendo de la nube filtrada original, los filtros para las siguientes profundidades funcionan de manera idéntica entre sí. Se eliminan todos los puntos que no corresponden a la profundidad deseada (de 40 a 80 centímetros en un caso y de 80 a 120 en el otro), y la nube de puntos resultante se segmenta en tres partes: nube de puntos izquierda, derecha y central. Si el tamaño de la nube de puntos de alguna de estas partes es mayor que cero, se marca como ocupada la posición correspondiente en el vector que se devuelve al algoritmo encargado de volcar esta información en el mapa. Control de acceso Para obtener acceso completo a la nube de puntos, es necesario subscribirse al topic en el que ésta publica su información. Para esto, necesitamos crear una función que ROS conoce como callback. A efectos prácticos, estas funciones se comportan como hilos separados de ejecución, que entran en acción cada vez que el bucle principal del programa termina una iteración y realiza una llamada a la orden ros::Spin(). Como no es necesario, ni deseable, que el algoritmo de filtrado se ejecute constantemente, es necesario controlar la ejecución con semáforos. Los semáforos son variables que se marcan a determinados valores para permitir el avance de un programa. Un sistema con dos (o más) semáforos se utiliza cuando alguno de los hilos de ejecución necesita de la información proporcionada por otros, pero debe controlar cuándo y cómo se obtiene ésta. En este proyecto, el acceso al algoritmo de filtrado debe estar cerrado 81 3.2. Desarrollo del proyecto Capítulo 3. Desarrollo práctico hasta que la detección de obstáculos necesite obtener datos nuevamente sobre su entorno y, a su vez, no puede continuar con su ejecución hasta que los datos no hayan acabado de obtenerse correctamente, por lo que son necesarios dos semáforos. Así, cuando se alcanza la orientación correcta de la cámara y es necesaria una captura de datos nueva, se activa la variable que impide al filtro ejecutarse en cada iteración del algoritmo. En una implementación normal, el filtro obtendría el acceso de forma inmediata y podría continuar su ejecución, pero en ROS es necesario forzar una iteración del nodo principal, lo que se logra mediante la llamada a función ros::SpinOnce(). Una vez el filtro ha terminado de rellenar los datos necesarios, desactiva su variable de acceso y activa la variable que deja saber al algoritmo principal que ha terminado su ejecución. Éste puede entonces continuar. Figura 3.22: Esquema del funcionamiento del control de accesos. 82 Capítulo 3. Desarrollo práctico 3.2. Desarrollo del proyecto 3.2.7. Integración con RGBDSLAM Llegado a este punto, lo único que resta al algoritmo es desplazar físicamente al robot dentro del entorno. Con este fin se utilizan dos herramientas proporcionadas por la biblioteca tf ( 3.1.3): una transformada ”estampada”, es decir, que posee información sobre el tiempo en que ha sido capturada, y un escuchador para realizar la suscripción a la salida del programa RGBDSLAM. La salida del programa no comienza hasta que la cámara ha captado su primer movimiento, algo que suele suceder incluso con la cámara estática debido al ruido del sensor, así que la primera transformada de la posición recibida tendrá un pequeño desplazamiento con respecto a la posición asumida por el algoritmo (0, 0, 0). Por tanto, el programa principal debe esperar a recibir la primera información proporcionada por RGBDSLAM y almacenar ese valor como offset, que se aplicará a todos los valores obtenidos a partir de ese momento. Cuando se tiene un flujo constante de datos y un desplazamiento fiable, el bucle principal procede con los cálculos correspondientes a rutas, obstáculos y mapeado deteniéndose únicamente cuando necesite de los datos de la posición y orientación actuales. En ambos casos, el procedimiento es similar: Se espera a que el escuchador tenga primera la transformada. Se calcula la resta de la transformada actual y el desplazamiento inicial. Se envía la orden de movimiento (rotación, traslación) correspondiente al robot (ver 3.3). Se compara la posición actualizada calculada a partir de la transformada con la posición destino y si no coinciden el bucle detiene su ejecución durante 50 milisegundos, para sincronizar con el bucle de control del robot. Una vez alcanzada la posición deseada se devuelve el control al bucle principal. Las posiciones destino corresponden con el centro de las ”casillas” en las que se ha dividido el entorno. Las rotaciones de la cámara se calculan en incrementos de noventa grados para observar las casillas 4-conectadas y las posiciones observables tras éstas. El bucle principal continúa hasta que se ha alcanzado la casilla de destino o se detecta una casilla de destino inalcanzable. 83 3.3. Plataforma final y adaptaciones Capítulo 3. Desarrollo práctico 3.3. Plataforma final y adaptaciones La información sobre la plataforma física en la que finalmente se ha implementado el proyecto, así como toda la información relacionada con los algoritmos de comunicación, control y movimiento del robot se encuentra detallada en [17]. El robot empleado fue construido usando la tecnología LEGO NXT MINDSTORM. Esta plataforma incluye: el brick NXT, múltiples sensores (luz/color, ultrasonidos, contacto, sonido) motores y cableado eléctrico, y el set de construcción habitual de la línea Technic, formado por miles de piezas de construcción, engranajes, ejes correas y poleas. El robot LEGO Mindstorms NXT es la segunda versión del sistema desarrollado en colaboración entre LEGO y el MIT, presentado en enero de 2006 en el International Consumer Electronics Show. La nueva versión además de otros cambios menores en los sensores electrónicos y las piezas de construcción, incorpora una unidad de control nueva: el NXT. Combinando los bloques de construcción, la fácil programación del NXT y su interfaz de entrada y salida se puede obtener un sistema de prototipado rápido para el desarrollo de una gran variedad de actividades, lo que ha permitido que este sistema haya sido ampliamente aceptado como una herramienta para la investigación y la educación universitaria como se demuestra con la publicación de ediciones especiales de revistas como IEEE Robotics and Automation Magazine o IEEE Control Systems Magazine. La unidad de control, definida como ladrillo inteligente NXT, está basada en el ARM7, un potente microcontrolador de 32 bits, con 256 Kbytes FLASH y 64 Kbytes de memoria RAM. Para la programación y las comunicaciones, el NXT está equipado con un puerto USB 2.0 y con un dispositivo inalámbrico Bluetooth clase II, V2.0. Así mismo, el NXT dispone de 4 entradas (una de ellas incluye una expansión IEC 61158 Type 4/EN 50 170 para usos futuros) y 3 salidas analógicas. La nueva versión del LEGO proporciona 4 tipos de sensores electrónicos: de contacto, de luz, de sonido y de distancia. Además, existen varias compañías que han desarrollado numerosos sensores compatibles con el NXT, como giróscopos, acelerómetros, buscadores de infrarrojos, cámaras de visión artificial, etc. De entre los distintos disponibles, en este trabajo se ha optado por utilizar el firmware LeJOS, el cual permite la programación de los NXT en lenguaje Java, ofreciendo librerías orientadas a la navegación, a la comunicación Bluetooth, al uso de trigonometría y funciones matemáticas complejas, a la programación multithreading, al soporte de programación orientada a objetos... Además proporciona distintos niveles de abstracción a la hora de 84 Capítulo 3. Desarrollo práctico 3.3. Plataforma final y adaptaciones programar los movimientos de los NXT, desde funciones de alto nivel que definen rectas y curvas, al tratamiento del movimiento de los servos de corriente continua a nivel del voltaje que se les aplica. A continuación una lista de las características electrónicas del brick NXT, extraídas del manual de LEGO: Figura 3.23: Especificaciones técnicas. Aunque estas características son lo suficientemente potentes para ejecutar gran parte del algoritmo principal, no cumplen con los requisitos para el tratamiento de imágenes online, ni para la ejecución del programa RGBDSLAM. Por tanto, la parte software se ejecutará en un PC, y toda la comunicación se realizará por medio de USB. Será el propio robot quien decida la mejor forma de generar la trayectoria, mientras que el PC decidirá los distintos destinos a los que éste deberá desplazarse. La información de la cámara es tratada directamente por el PC, mientras que el control de la posición de la cámara se realiza en el robot. A este fin, es necesario realizar ciertas modificaciones sobre el código original. La primera de ellas es definir qué tipo de órdenes van a intercambiarse entre el programa y el robot: 85 3.3. Plataforma final y adaptaciones Capítulo 3. Desarrollo práctico //Definiciones de las órdenes posibles para el robot #define DESPLAZAMIENTO 0 #define ROTACION 1 #define FRENAR 2 #define FIN 3 #define POSICION 4 #define FRENAR_ROTACION 5 #define DESPLAZAMIENTO_GIRO 6 Gracias a la configuración del robot, donde la cámara está sujeta a una plataforma independiente que puede rotar mientras el robot permanece estático o desplazándose en una línea recta, es posible separar las órdenes de rotación y de traslación. Por este motivo existen, a su vez, dos órdenes de frenado independientes. Para suavizar las trayectorias en las que el robot, además de avanzar, debe girar, se crea una orden que avisa al robot para que éste decida cuál es la curva más adecuada para alcanzar su destino. La orden llamada “FIN” es la que avisa al robot que el programa ha terminado, para que comience el protocolo de desconexión y finalización de ejecución internamente. Los segmentos del programa que tratan con el desplazamiento y la rotación ahora deben esperar confirmación del robot antes de continuar su ejecución, y deben actualizar, en cada iteración, la información del robot sobre su posición y orientación actuales, obtenidas mediante RGBDSLAM. La rutina que sufre un cambio más drástico con respecto a su implementación original es la de backtracking, ya que debe informar al robot de todos los pasos que debe dar para deshacer el camino recorrido, porque éste no almacena la trayectoria seguida. Para realizar correctamente las comunicaciones, se han desarrollado dos funciones: enviar que gestiona las comunicaciones PC-robot y recibir que se encarga de las comunicaciones robot-PC. La comunicación entre ambos dispositivos se realiza usando una implementación de la biblioteca libusb y ha de ser inicializada al comienzo del bucle principal: dev = nxt_find_nth(cnt, name); hdl = nxt_open(dev); Las variables cnt yname contienen información específica del brick que se utiliza para ejecutar el código y pueden obtenerse sus valores mediante 86 Capítulo 3. Desarrollo práctico3.4. Análisis de una ejecución del programa. la órden lsusb en la línea de comandos. Las llamadas a ambas funciones son bloqueantes, es decir, impiden el avance del programa hasta que han acabado su ejecución y ambas deben terminar la ejecución del programa si no consiguen conectar exitósamente con el brick. Una vez inicializada la comunicación, enviar codifica la información que se ha de enviar al robot utilizando los defines mostrados anteriormente. Además, el robot recibe los valores de X e Y correspondientes a su casilla actual en la matriz de ocupación, y la orientación deseada al final del movimiento. Recibir es una función bloqueante que detiene la ejecución del bucle principal hasta que recibe una señal del robot, confirmando que ha finalizado correctamente de ejecutar la orden recibida. 3.4. Análisis de una ejecución del programa. Figura 3.24: Laberinto en el que se realizan las pruebas del programa. La figura 3.4 presenta un ”laberinto” de ejemplo en el que el robot habrá de alcanzar un cierto destino desde una casilla inicial, de manera completamente autónoma. El modelado del problema puede observarse en la siguiente figura: En este modelado, la casilla inicial del robot (parámetro del programa) está señalada en color azul y la casilla final en rojo. La primera tarea que debe realizar el programa es elegir la cantidad de giros necesarios para conocer toda la información relativa a la posición inicial del robot. Dada la posición inicial del robot y el tamaño de la matriz, se 87 3.4. Análisis de una ejecución del programa.Capítulo 3. Desarrollo práctico Figura 3.25: Modelado correcto del laberinto. realizarán 3 giros: delante (o ”arriba”), izquierda y derecha. Para realizar esto, el robot recibe tres órdenes separadas de ”ROTACIÓN” y el programa espera en la orden ”recibir” hasta que consigue una confirmación de que el giro ha sido realizado. Cuando se ha obtenido la información relativa a las 3 casillas adyacentes del robot (todas las casillas de las que se disponga información serán señaladas con el color gris en las siguientes figuras) el algoritmo puede comenzar con el proceso de selección de destinos parciales y la obtención de información del entorno. En este caso, la heurística dice que la casilla que ofrece mejores posibilidades para alcanzar el destino de forma óptima es la casilla de ”arriba” de la actual, ya que el objetivo se encuentra a 6 casillas de distancia hacia arriba y sólo 2 hacia la derecha. Se envía al robot una orden de desplazamiento de 40 centímetros en la misma dirección en la que se encuentra el robot actualmente. A partir de la segunda iteración, se aplica una optimización extra: si no se tiene información de una casilla adyacente a la actual, pero dicha casilla no forma parte del (posible) camino ésta no se explorará, para reducir el número de giros necesarios. Así pues, una vez elegido el primer destino y realizado el desplazamiento, el proceso se repite. El algoritmo selecciona los giros necesarios para conocer 88 Bibliografía Bibliografía [14] Aleksandr V. Segal, Dirk Haehnel, and Sebastian Thrun. Generalized icp. [15] Rainer Kürmele, Giorgio Grisetti, and Hauke Strasdata. "g2o: A general framework for graph optimization". [16] Sebastian Thrun and Arno Bücken. Integrating grid-based and topological maps for mobile robot navigation. [17] Vicent Mayans Roca. Desarrollo multiplataforma de aplicaciones de control y comunicación para robots móviles. Master’s thesis, Universitat Polit’ecnica de Val’encia, 2012. 95 Índice de figuras 2.1. Robot rodante con cuatro ruedas. . . . . . . . . . . . . . . . . 7 2.2. Mini robot espía con seis ruedas. . . . . . . . . . . . . . . . . 8 2.3. Robot dotado de orugas. . . . . . . . . . . . . . . . . . . . . . 8 2.4. Robot bípedo humanoide. . . . . . . . . . . . . . . . . . . . . 9 2.5. Robot reptador. . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.6. Robot nadador con forma de pez. . . . . . . . . . . . . . . . . 10 2.7. Quadracoptero. . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.8. Avión espía no tripulado o drone . . . . . . . . . . . . . . . . 11 2.9. Sensor láser Hokuyo. . . . . . . . . . . . . . . . . . . . . . . . 13 2.10. Sensor de distancias por sonar. . . . . . . . . . . . . . . . . . 14 2.11. Cámara estereoscópica. . . . . . . . . . . . . . . . . . . . . . . 15 2.12. Filtrado de datos anómalos y extracción de línea por RANSAC. 20 2.13. Esquema de un algoritmo de SLAM. . . . . . . . . . . . . . . . 22 2.14. Estimación de la trayectoria óptima por un algoritmo de muestreo. 25 2.15. A la izquierda, casillas 4-conectadas. A la derecha, casillas 8-conectadas. 26 2.16. Órden en el que se despliegan los nodos (BFS) . . . . . . . . . 27 2.17. Órden en el que se despliegan los nodos (DFS) . . . . . . . . . 28 2.18. Los primeros controladores. . . . . . . . . . . . . . . . . . . . 30 2.19. Controladores a finales de los años 80. . . . . . . . . . . . . . 30 2.20. Controladores a principios de los años 90. . . . . . . . . . . . . 31 2.21. Power Glove, de Nintendo. . . . . . . . . . . . . . . . . . . . . 31 2.22. Activator, de Sega. . . . . . . . . . . . . . . . . . . . . . . . . 32 2.23. Zapper, de Nintendo . . . . . . . . . . . . . . . . . . . . . . . 33 2.24. Controladores a finales de los años 90. . . . . . . . . . . . . . 34 2.25. Plataforma de baile para el juego Dance, Dance, revolution. . 35 2.26. Eyetoy, de Sony . . . . . . . . . . . . . . . . . . . . . . . . . . 36 2.27. Desert Eagle, de Trustmaster. . . . . . . . . . . . . . . . . . . 36 2.28. Mandos a finales de la década pasada. . . . . . . . . . . . . . 37 2.29. Kinect, de Microsoft. . . . . . . . . . . . . . . . . . . . . . . . 38 2.30. Distribucion de colores en un filtro de Bayer. . . . . . . . . . . 40 2.31. Componentes de Kinect . . . . . . . . . . . . . . . . . . . . . 41 96 Índice de figuras Índice de figuras 2.32. Interfaz de ordenador controlada por gestos con Kinect. . . . . 43 2.33. Estructura del framework OpenNI. . . . . . . . . . . . . . . . 44 2.34. Múltiples ejes de coordenadas en un robot. . . . . . . . . . . . 47 2.35. Varias estructuras 3D formadas a partir de nubes de puntos. . 48 2.36. NITE en funcionamiento. . . . . . . . . . . . . . . . . . . . . . 49 2.37. Dispositivos que cumplen el estándar OpenNI. . . . . . . . . . 49 2.38. Esquema de ejecución de RGBDSLAM. . . . . . . . . . . . . . 50 2.39. Estructura en capas de un grafo. . . . . . . . . . . . . . . . . . 51 2.40. Puntos de interés extraidos y sus orientaciones. . . . . . . . . 53 2.41. Estado actual del proyecto. . . . . . . . . . . . . . . . . . . . . 54 2.42. Estimación del volumen de código desarrollado con Eclipse. . . 55 3.1. Diálogo inicial de Wubi. . . . . . . . . . . . . . . . . . . . . . 57 3.2. Diálogo principal de la instalación. . . . . . . . . . . . . . . . 57 3.3. Instalación finalizada. . . . . . . . . . . . . . . . . . . . . . . . 58 3.4. Panel de control de Windows. . . . . . . . . . . . . . . . . . . 58 3.5. La instalación se selecciona como un programa normal. . . . . 59 3.6. Diálogo de confirmación . . . . . . . . . . . . . . . . . . . . . 59 3.7. La desinstalación ha sido completada. . . . . . . . . . . . . . . 60 3.8. Selección de los repositorios. . . . . . . . . . . . . . . . . . . . 60 3.9. Agregar un nuevo repositorio. . . . . . . . . . . . . . . . . . . 61 3.10. Ejecución ejemplo del programa turtlesim . . . . . . . . . . . . 64 3.11. Estructura en árbol del conjunto de ejes que participan en turtlesim 64 3.12. Captura de una ejecución del programa RGBDSLAM . . . . . . 66 3.13. Configuración de las variables del entorno. . . . . . . . . . . . 69 3.14. Configuración de las variables de ejecución. . . . . . . . . . . . 70 3.15. Representación de un posible árbol de búsqueda. . . . . . . . . 73 3.16. Camino estimado por el algoritmo. . . . . . . . . . . . . . . . 76 3.17. Casillas sobrantes. . . . . . . . . . . . . . . . . . . . . . . . . 76 3.18. Casillas sobrantes. . . . . . . . . . . . . . . . . . . . . . . . . 77 3.19. Datos obtenidos de manera empírica. Las casillas tienen una dimensión de 40x40 centímetros. 78 3.20. Información disponible. . . . . . . . . . . . . . . . . . . . . . . 78 3.21. Distribución de casillas en el cono de visión. . . . . . . . . . . 79 3.22. Esquema del funcionamiento del control de accesos. . . . . . . 82 3.23. Especificaciones técnicas. . . . . . . . . . . . . . . . . . . . . . 85 3.24. Laberinto en el que se realizan las pruebas del programa. . . . 87 3.25. Modelado correcto del laberinto. . . . . . . . . . . . . . . . . . 88 3.26. Iteración inicial. De izquierda a derecha: captura de la cámara, información del robot y desplazamien 3.27. Segunda iteración. . . . . . . . . . . . . . . . . . . . . . . . . 89 3.28. Tercera iteración. . . . . . . . . . . . . . . . . . . . . . . . . . 90 3.29. Novena iteración. . . . . . . . . . . . . . . . . . . . . . . . . . 91 97 Índice de figuras Índice de figuras 3.30. Laberinto completado. . . . . . . . . . . . . . . . . . . . . . . 91 98