Generación de trayectorias en tiempo real a partir de diagramas de Voronoi

Equation Chapter 1 Section 1 Trabajo Fín de Grado Grado en Ingeniería de las Tecnologías Industriales Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Autor: Ángel Moya Carrasco Tutores: José Antonio Cobano Suárez Anibal Ollero Baturone Dep. Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 iii Trabajo Fín de Grado Grado en Ingeniería de las Tecnologías Industriales Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Autor: Ángel Moya Carrasco Tutor: José Antonio Cobano Suárez Aníbal Ollero Baturone Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 v Trabajo Fin de Grado: Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Autor: Ángel Moya Carrasco Tutor: José Antonio Cobano Aníbal Ollero Baturone El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2016 El Secretario del Tribunal vii A los que aún están A los que se han ido ix Agradecimientos A mis padres, por todo el empeño que han tenido en que me esfuerce y de lo máximo de mí, a mi novia, por apoyarme en todos los momentos difíciles, y a mis tutores, tanto a D. Aníbal Ollero por enseñarme las bases de la robótica que han despertado mi interés en este ámbito, como a D. José Antonio Cobano que ha dedicado su tiempo y dedicación para orientarme en este proyecto. xvii ÍNDICE DE FIGURAS Figura 3-1. Divide y vencerás 8 Figura 3-2. Divide y vencerás 1 10 Figura 3-3. Divide y vencerás 2 10 Figura 3-4. Divide y vencerás 3 11 Figura 3-5. Divide y vencerás 4 11 Figura 3-6. Divide y vencerás 5 12 Figura 3-7. Fortune (Barrido recta) 13 Figura 3-8. Fortune Progreso 1 13 Figura 3-9. Fortune Progreso 2 14 Figura 3-10. Fortune Progreso 3 14 Figura 3-11. Fortune Progreso 4 14 Figura 3-12. Fortune Progreso 5 15 Figura 3-13. Fortune Progreso 6 15 Figura 3-14. Dijkstra pseudocódigo 16 Figura 3-15. Dijkstra ejemplo 1.1 16 Figura 3-16. Dijkstra ejemplo 1.2 17 Figura 3-17. Dijkstra ejemplo 1.3 17 Figura 3-18. Dijkstra ejemplo 1.4 17 Figura 3-19. Dijkstra ejemplo 1.5 18 Figura 3-20. Dijkstra ejemplo 1.6 18 Figura 4-1. Diagrama Voronoi 19 Figura 4-2. Modelado obstáculos 20 Figura 4-3. Sin duplicado 1 21 Figura 4-4. Con duplicado 1 21 Figura 4-5. Sin duplicado 2 22 Figura 4-6. Con duplicado 2 22 Figura 4-7. Lmaximo =10 Npuntos=244 a 23 Figura 4-8. Lmaximo =10 Npuntos =244 b 23 Figura 4-9. Lmaximo =5 Npuntos =386 a 24 Figura 4-10. Lmaximo =5 Npuntos =386 b 24 Figura 4-11. Lmaximo =1 Npuntos =2992 a 25 Figura 4-12. Lmaximo =1 Npuntos =2992 b 25 Figura 4-13. Sin largo máximo 26 Figura 4-14. Lmaximo=10 27 Figura 4-15. Comprobación por líneas 28 Figura 4-16. Comprobación por líneas y sin largo máximo 28 Figura 4-17. Distancia de seguridad 1 29 Figura 4-18. Distancia de seguridad 2 30 Figura 4-19. Sin distancia de seguridad 31 Figura 4-20. Distancia de seguridad 5x5 31 Figura 4-21. Distancia de seguridad 7x7 32 Figura 4-22. Sin distancia de seguridad 32 Figura 4-23. Distancia de seguridad 22x22 33 Figura 4-24. Distancia de seguridad 33x33 33 Figura 4-25. Tabla de tiempos para mapa de 100x100 34 Figura 4-26. Tabla de tiempos para mapa de 500x500 34 Figura 4-27. Esquema Dijkstra 35 Figura 4-28. Ejemplo Dijkstra 1 36 Figura 4-29. Ejemplo Dijkstra 2 37 Figura 4-30. Ejemplo Dijkstra 3 37 Figura 4-31. Ejemplo Dijkstra 4 38 Figura 4-32. Robot Turtlebot 39 Figura 4-33. Rango de visión Sensor 40 Figura 4-34. Rango de acotado 40 Figura 4-35. Modelado obstáculo 42 Figura 4-36. Plan de generación de trayectorias 43 Figura 4-37. Cambio de Coordenadas respecto al sensor a respecto al origen 44 Figura 4-38. Cálculo velocidad angular 45 Figura 4-39. Robotics System Toolbox 45 Figura 4-40. Árbol de conexiones 46 Figura 5-1. Traza del recorrido inicial 48 Figura 5-2. Recorrido ejemplo 1.1 48 Figura 5-3. Recorrido ejemplo 1.2 49 Figura 5-4. Recorrido ejemplo 1.3 49 Figura 5-5. Recorrido ejemplo 1.4 50 Figura 5-6. Recorrido ejemplo 1.5 50 Figura 5-7. Recorrido ejemplo 1.5 51 Figura 5-8. Recorrido ejemplo 1.6 51 Figura 5-9. Recorrido ejemplo 1.7 52 Figura 5-10. Recorrido ejemplo 2.1 52 Figura 5-11. Recorrido ejemplo 2.2 53 xix Figura 5-12. Recorrido ejemplo 2.3 53 Figura 5-13. Recorrido ejemplo 2.4 54 Figura 5-14. Recorrido ejemplo 2.6 54 Figura 5-15. Recorrido ejemplo 2.7 55 Figura 5-16. Recorrido ejemplo 2.8 55 xxi Notación e π ϴ α β ⊆ Ε número e número pi Theta Alfa Beta Contenido o igual Perteneciente a sen Función seno Tg Función tangente arctg Función arco tangente sen Función seno sinxy Función seno de x elevado a y cosxy log Función coseno de x elevado a y Función logaritmo en base 10 : Tal que < Menor o igual > Mayor o igual \ Backslash ⇔ min max Si y sólo si Mínimo Máximo 1 1 INTRODUCCIÓN Y ALCANCE DEL PROYECTO l avance científico en el campo de la robótica y automática busca dotar de la mayor autonomía posible a los robots y autómatas programables. Su objetivo es la mejora en la calidad de vida de las personas, que se logra gracias a la liberación de funciones del ser humano en su propio hogar, con aspiradoras autónomas por ejemplo, hasta el transporte de productos en naves de fabricación industrial de forma autónoma con sistemas de AGVs, o de posicionamiento por balizas. En este proyecto se propone una forma de resolver estos problemas que pueden presentarse en diversos campos, para poder aportar nuevas ideas en el campo de la autonomía de robots, y la planificación de trayectorias en entornos desconocidos. 1.1. Objetivos y motivaciones del Proyecto A la hora de generar un camino o trayectoria segura que seguir en entornos desconocidos, debemos recoger información del entorno a partir de sensores para poder solucionar el problema, es decir, posibles colisiones con obstáculos. También en necesario localizar nuestro vehiculo en el entorno. Esto require importantes esfueros donde en un primer instante no conocemos más que el estado dinámico del robot, a través del estado de sus sensores internos. Para obtener información del medio, son necesarios ciertos sensores de detección de objetos. La precisión de estos sensores está muy condicionada por el presupuesto y las condiciones de nuestro robot. Este proyecto considera un enfoque económico que pueda resolver el problema para pequeños robots que puedan ser programados para probar la fiabilidad del método de forma real, por lo que se utilizará el sensor de una pequeña cámara Kinect como detector de objetos. Esta es una cámara muy utilizada en entornos de investigación y académicos por su gran flexibilidad y reducido precio, otorgando una lectura de obstáculos en 2D en un rango de distancia de entre 0.5 y 5 metros, y un rango angular de 57 grados. La generación de trayectorias requiere un primer paso en el que se detectan los obstáculos. Una vez detectados, deberemos modelar la forma de los obstáculos para poder saber con precisión la posición de los obstáculos. Un requisito fundamental del Sistema de generación de trayectorias será la presencia de una alta eficiencia, pues E Que extraño, cuanto más me esfuerzo mas suerte tengo. - Henry Ford - Introducción y alcance del proyecto 2 2 la simplicidad de computación permitirá unos buenos resultados ya que al ser un Sistema que estará trabajando a tiempo real debe tener una capacidad de reacción elevada. Además una menor carga computacional permitirá abaratar costes para unas futuras implementaciones en robots reales. El fin del proyecto es servir como antecedente para implementar el Sistema de generación de trayectorias que se va a desarrollar en aplicaciones reales de robots, utilizando únicamente un Sistema de posicionamiento como pueda ser un GPS, o el posicionamiento por balizas, y un simple sensor de detección de obstáculos para poder recorrer trayectorias seguras en entornos desconocidos. Se trabajará con el entorno de programación de robots ROS (Robot Operating System), herramienta ampliamente utilizada en el entorno robótico, con la que implementar el sistema aquí desarrollado se convierte en algo trivial si se dispone del hardware adecuado. 1.2. Estructura del Proyecto El desarrollo del proyecto puede ser explicado en las siguientes partes:  Método propuesto (Capítulo 3): En esta parte se explican los algoritmos elegidos para desarrollar las dos partes fundamentales de la generación de trayectorias: la generación del grafo de puntos conectados; y la generación del camino más adecuado en función de este grafo.  Implementación (Capítulo 4): Esta es la parte principal del proyecto, en la que se explican todas las simulaciones, desde las pruebas iniciales para entornos conocidos en la herramienta Matlab, hasta el paso al sistema ROS, donde también se establecerá una comunicación con Matlab a través del Robotic System Toolbox incluido en la última versión de Matlab para permitir la comunicación entre ambas plataformas.  Experimentación (Capítulo 5): En esta parte se mostrarán algunos ejemplo del resultado obtenido, para lo que se utiliza la herramienta de ROS de simulación de robots Gazebo, en la que se puede representar de forma muy precisa la respuesta del robot, incluyendo factores de deslizamiento, errores de posicionamiento, y toda la dinámica y características físicas del vehículo. Todas las pruebas engloban los resultados de todo el desarrollo del proyecto, por lo que es una parte fundamental del mismo. Además muestra la respuesta de todo el sistema y permite observar donde se encuentran los puntos fuertes y los defectos del sistema.  Conclusiones y posibles trabajos futuros (Capítulo 5): En esta parte se resumen todas las impresiones que ha generado la realización del proyecto, las conclusiones sacadas de todo el desarrollo, las posibles mejoras, modificaciones, e implementaciones que pueden realizarse en vistas a un posible Trabajo Fin de Master o investigaciones futuras para sistemas reales. 3 3 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi 2 ESTADO DEL ARTE a planificación de trayectorias o caminos eficientes y seguros para aplicaciones con robots móviles autónomos juega un papel fundamental en todos las funcionalidades que se están abordando en estos últimos años. Concretamente, los robots deben calcular sus trayectorias de manera eficiente para llevar a cabo las misiones de forma segura a partir de la información sobre el entorno dado por los sensores de abordo. Una clasificación de algoritmos de planificación y evitación de colisiones es presentada en [1] y [15] respectivamente. Entre los más utilizados en la literatura destacan los algoritmos evolutivos como genéticos [2], optimización de enjambres de partículas (Particle Swarm Optimization) [7] o basados en fuegos artificiales (Fireworks Algorithms) [13], métodos de programación no-lineal [3], mixed-integer linear programming [4][5], métodos de colocación [6], algoritmos de planificación RRT (Rapidly-Exploring Random Trees) [8], RRT* [14], cocido simulado (Simulated Annealing) [9], colonias de hormigas (Ant Colony) [10] y otros. La representación del entorno es fundamental en cualquier algoritmo de planificación de caminos. Atendiendo a este requisito, los algoritmos se pueden dividir en: utilización de cuadrículas (grids en inglés) uniformes o nouniformes, una descomposición exacta de celdas, roadmaps probabilísticos, grafos de visibilidad o diagramas de Voronoi. Estos dos últimos se pueden englobar en los métodos de roadmaps. En cuanto a los métodos que representan el entorno mediante cuadrículas, se distinguen los que trabajan con grids uniformes, y no uniformes. Las cuadrículas uniformes dividen el entorno en una serie de celdas con las mismas dimensiones mientras que las no uniformes dividen el entorno en celdas cuyo tamaño es variable. La descomposición exacta de celdas suele utilizar la descomposición trapezoidal. Este método consiste en dividir el entorno libre de obstáculos en celdas, de forma que éstas representen el espacio seguro. La forma y posición de las celdas, por tanto, vienen determinadas por la posición de los obstáculos. El uso de la descomposición trapezoidal viene determinado por posición de los obstáculos que puede dar lugar a celdas trapezoidales, que representan el espacio libre. Los métodos basados en roadmaps convierten el entorno multidimensional en una red de curvas unidimensionales, que se encuentran en una zona libre de obstáculos. A estas curvas se les denomina roadmaps. L El fracaso es, a veces, más fructífero que el éxito. - Henry Ford - Método propuesto 10 10 Figura 3-2. Divide y vencerás 1 2. Paso 2: Se calcula el diagrama de Voronoi de la izquierda Figura 3-3. Divide y vencerás 2 3. Paso 3: Se calcula el diagrama de Voronoi de la derecha 11 11 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 3-4. Divide y vencerás 3 4. Paso 4: Se calcula la cadena divisoria Figura 3-5. Divide y vencerás 4 5. Paso 5: Se eliminan todas las líneas de cada diagrama que no caen a su lado de la cadena divisoria Método propuesto 12 12 Figura 3-6. Divide y vencerás 5 3.3.2.3 Algoritmo de Fortune (barrido de recta) El algoritmo de fuerza bruta previamente revisado tiene una complejidad O(n2 log n) , sin embargo, teniendo en cuenta la propiedad siguiente: Para n ≥ 3, el número de vértices en el diagrama de Voronoi de un conjunto de n sitios en el plano es a lo más 2n - 5 y el número de aristas es a lo más 3n - 6. Se puede suponer por tanto que hay una forma mucho más eficiente de encontrar el diagrama de Voronoi pues sus elementos constituyentes tienen complejidad O(n). En efecto, existe este algoritmo y es llamado Algoritmo de Fortune en honor a Steven Fortune quien lo inventó en 1986 y cuya complejidad es: 𝑂(𝑛log𝑛) El algoritmo de Fortune está basado en una de las técnicas clave dentro de la geometría computacional denominada barrido de recta. La esencia de esta técnica yace en suponer que existe una recta L que recorre el plano de arriba hacia abajo (o de izquierda a derecha, incluso en direcciones opuestas) y que a lo largo de su recorrido se interseca con las estructuras que deseamos procesar. Cuando se da esta intersección, se guarda cierta información de tal forma que ayude en los cálculos. El problema con el diagrama de Voronoi es el de predecir cuándo y dónde se producirán los próximos eventos. Imagine que detrás de la línea de barrido ya se ha construido el diagrama de Voronoi basándose en los sitios que se han encontrado hasta ahora en el barrido. La dificultad es que un sitio que queda por delante de la línea de barrido podría generar un vértice de Voronoi que se encuentre detrás de la línea de barrido. Son precisamente estos eventos no anticipados los que complican el diseño de un algoritmo de barrido del plano. Fortune hizo la inteligente observación de que podía calcularse el diagrama de Voronoi mediante barrido del plano construyendo una versión “distorsionada” de éste pero que es topológicamente equivalente. Esta versión distorsionada del diagrama se basa en una transformación que modifica la forma en que las distancias son medidas en el plano. El diagrama resultante tiene la misma estructura topológica que el diagrama de Voronoi, pero sus aristas son arcos parabólicos, en vez de segmentos de línea recta. 13 13 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 3-7. Fortune (Barrido recta) Es necesario que la información que se haya obtenido en regiones ya visitadas por la recta sea invariante. Es muy común que está técnica utilice dos tipos de estructuras de datos: cola de prioridades donde se guardan eventos que no son más que puntos donde la recta debe detenerse y un árbol binario de búsqueda donde se almacenan los elementos geométricos que se han intersecado con la recta y se necesita recordar para el procesamiento futuro. Cabe resaltar que debido a que en la computadora no se puede emular tal cual el movimiento continuo de la recta de barrido, se requiere idear una forma de discretización del movimiento de la recta que sea procesable en la computadora, de ahí que los eventos sean tal discretización. En la siguiente secuencia de imágenes se puede ver la evolución de la recta de barrido durante el proceso: Figura 3-8. Fortune Progreso 1 Método propuesto 14 14 Figura 3-9. Fortune Progreso 2 Figura 3-10. Fortune Progreso 3 Figura 3-11. Fortune Progreso 4 15 15 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 3-12. Fortune Progreso 5 Figura 3-13. Fortune Progreso 6 Este algoritmo tendrá un coste computacional igual al algoritmo de Divide y Vencerás, sin embargo su implementación es mucho más sencilla, por lo que actualmente es el método más extendido para generar el diagrama. 3.4 Algoritmo Dijkstra Es un algoritmo de búsqueda en grafos para la determinación del camino más corto dado un vértice origen f al resto de vértices en un grafo con pesos en cada arista. Cobra su nombre en honor a su descubridor Edsger Dijkstra. La idea de este algoritmo consiste en ir explorando todos los caminos más cortos que parten del vértice origen y que llevan a todos los demás vértices; cuando se obtiene el camino más corto desde el vértice origen, al resto de vértices que componen el grafo, el algoritmo se detiene. De manera iterativa se construye un conjunto de vértices seleccionados S que se toman del conjunto de vértices candidatos C según el menor peso (distancia) desde f. El algoritmo termina cuando no hay más vértices de G fuera del conjunto formado. El paradigma usado corresponde al método voraz, en el que se trata de optimizar una función sobre una colección de objetos (menor peso). Se usa un vector d[v] para almacenar la distancia de v a f, cuando se añade un vértice al conjunto S, el valor de d[v] contiene la distancia de f a v. Cuando se añade un nuevo vértice u al conjunto S, es necesario comprobar si Método propuesto 16 16 u es una mejor ruta para sus vértices adyacentes z que están en el conjunto C. Para ello se actualiza d con la relajación de la arista (u,z): 𝑑[𝑧]=min (𝑑[𝑧],𝑑[𝑢]+𝑤[𝑢,𝑧]) En forma de pseudocódigo, el desarrollo del algoritmo queda: Figura 3-14. Dijkstra pseudocódigo El método gráfico es el más adecuado para comprender como trabaja este algoritmo, así, para un simple grafo de 5 puntos dirigido podemos ver el desarrollo del algoritmo: Figura 3-15. Dijkstra ejemplo 1.1 17 17 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 3-16. Dijkstra ejemplo 1.2 Figura 3-17. Dijkstra ejemplo 1.3 Figura 3-18. Dijkstra ejemplo 1.4 Método propuesto 18 18 Figura 3-19. Dijkstra ejemplo 1.5 Figura 3-20. Dijkstra ejemplo 1.6 Para reconstruir los vértices del camino más corto desde el vértice fuente a cada vértice se usa otro array p[v] que contiene el vértice anterior a v en el camino se inicializa p[v] = f para todo v  f , se actualiza p siempre que d[v] > d[w] + w(w, v) El camino a cada vértice se halla mediante una traza hacia atrás en el array p. 19 19 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi 4 IMPLEMENTACIÓN l objetivo del proyecto es ser capaz de implementar un algoritmo que funcione en tiempo real para esquivar obstáculos cuyas ubicaciones son en principio desconocidas, hasta llegar a un destino especificado. La forma de proceder constará de varias etapas para poder ir comprendiendo bien cómo funcionan los algoritmos que queremos utilizar, y sobre todo comprobar los tiempos de computación de estos para ver si pueden ser usados en tiempo real y cómo modificarlos para reducir el tiempo de cálculo al mínimo. Para generar el grafo utilizaremos los diagramas de Voronoi. La idea del diagrama de Voronoi se basa fundamentalmente en la proximidad. Dado un conjunto finito de puntos en el plano P = {p1,...,pn} (con n mayor o igual que dos) y a cada pj le asociamos aquellos puntos del plano que están más cerca o igual suya que de cualquier otro de los pi con i distinto de j. Todo punto del plano queda así asociado a algún pi, formándose conjuntos que recubren a éste. Existirán puntos que disten lo mismo de dos elementos de P y que formarán la frontera de cada región. Los conjuntos resultantes forman una teselación del plano, en el sentido de que son exhaustivos (todo punto del plano pertenece a alguno de ellos) y mutuamente excluyentes salvo en su frontera. Figura 4-1. Diagrama Voronoi E La imaginación es más importante que el conocimiento. - Albert Einstein - Implementación 26 26 Como se puede observar, a medida que reducimos el largo máximo, el número de puntos de modelado aumenta de forma importante a la vez que la suavidad y precisión de nuestro grafo aumenta. De forma ideal, a mejor modelación todo son ventajas, sin embargo a la hora de implementarlo en un vehículo móvil que trabaja con restricciones temporales y depende del tiempo de respuesta y de la capacidad de generar una trayectoria a cada cambio de situación de un computador, deberemos tomar una decisión de compromiso que vendrá determinada por las condiciones de diseño. 4.1.2 Comprobación de caminos Una vez tenemos nuestro diagrama de Voronoi, determinado por dos matrices, una para las coordenadas Y otra para las coordenadas en X de los puntos extremos de cada segmento del grafo, tendremos que eliminar todas las líneas que pasen por algún obstáculo, para ello he utilizado dos métodos para a continuación decidir el más apropiado: 4.1.2.1 Comprobación de puntos del grafo: Conlleva en principio un menor coste computacional, y es muy sencillo de implementar: Figura 4-13. Sin largo máximo 27 27 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 4-14. Lmaximo=10 Como se puede ver, para un modelado muy preciso de los obstáculos este método se presenta correcto, cumpliendo su función, además de ser poco costoso computacionalmente. Sin embargo, para obstáculos con un peor modelado tiene fallos claros en algunas líneas que podrían tener consecuencias desastrosas en un caso real. Por lo tanto, no es un método adecuado para implementar pues no conocemos el resultado final hasta probarlo, y nunca tendremos garantías de su correcto funcionamiento. 4.1.2.2 Comprobación línea por línea Método más costoso que el anterior pero con garantías de efectividad, consiste en ir comprobando cada segmento del grafo con salto de unidades entre comprobación y comprobación, que serán redondeadas a la casilla de la matriz mapa más cercana: Implementación 28 28 Figura 4-15. Comprobación por líneas Figura 4-16. Comprobación por líneas y sin largo máximo Como era lógico no presenta ningún problema en la eliminación de las rectas, ni en el caso arriba representado ni en ningún otro de todas las pruebas realizadas. 29 29 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi En conclusión, el método elegido será el de comprobación línea a línea para poder tener una implementación futura en sistemas reales sin que pueda suponer un problema la incertidumbre que se genera sin la comprobación por línea. 4.1.3 Distancia de seguridad Otro aspecto importante a tener en cuenta es la geometría del vehículo que pueda seguir la trayectoria, pues hay zonas por las que aunque no existan obstáculos le podría resultar imposible pasar. Por tanto se presenta interesante establecer una distancia de seguridad en función de la geometría del vehículo. Al igual que para la eliminación de líneas, esta comprobación se realizará línea por línea para garantizar una distancia de seguridad homogénea en toda la trayectoria. He implementado dos formas de realizar esta comprobación: 4.1.3.1 Comprobando todas las celdas adyacentes Constituye un método útil para vehículos con la forma lo más circular posible: Figura 4-17. Distancia de seguridad 1 Se hace necesario comprobar una gran cantidad de puntos con este método y requiere sobredimensionar el vehículo para que se cumpla la distancia de seguridad, pues en función de la dirección protegerá una distancia igual a media diagonal del cuadro de puntos adyacentes, y en otros solo medio lado de dicho cuadrado. 4.1.3.2 Comprobando perpendicularmente a la dirección del movimiento Comprueba una menor cantidad de puntos y se adapta a la dirección de la trayectoria: Implementación 30 30 Figura 4-18. Distancia de seguridad 2 La Figura 3-18 muestra cómo se va comprobando la distancia de seguridad, ilustrando en morado los puntos que se comprueban. 4.1.3.3 Conclusiones y pruebas En cualquiera de los dos métodos, cuando se encuentren con un obstáculo se eliminará la línea completa y se pasará a comprobar la siguiente.Cuando acabe la comprobación, todas las líneas que queden “sueltas”, por no estar unidas a ninguna otra, serán también eliminadas pues no resultan de interés porque el vehículo no podrá acceder a ellas de ninguna manera. El resultado de establecer determinadas distancias de seguridad es el siguiente: 31 31 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 4-19. Sin distancia de seguridad Figura 4-20. Distancia de seguridad 5x5 Implementación 32 32 Figura 4-21. Distancia de seguridad 7x7 En otro mapa de mayor tamaño, o de mayor resolución, según como quiera interpretarse: Figura 4-22. Sin distancia de seguridad 33 33 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 4-23. Distancia de seguridad 22x22 Figura 4-24. Distancia de seguridad 33x33 Se observa como al aumentar la distancia de seguridad las zonas de paso se vuelven mas restrictivas, a la vez que necesita un mayor tiempo de cálculo ya que tiene que comprobar un gran número de celdas. 4.1.4 Tiempos de cálculo Para comprobar el coste computacional aproximado de cada operación he realizado varias pruebas con distintas Implementación 34 34 configuraciones. Así, he elaborado dos tablas de tiempos de pruebas realizadas al mismo mapa, pero con su resolución aumentada, pasando de ser un mapa 100x100 a un mapa 500x500, siendo L el número de celdas en los bordes de los obstáculos entre los que se establecen puntos clave: Tiempo de calculo puntos obstáculo (seg) Tiempo en calcular diagrama Voronoi (seg) Número de puntos obstáculo Tiempo en comprobar línea a línea (seg) Tiempo en establecer distancia de seguridad=4 (seg) Tiempo en establecer distancia de seguridad=2 (seg) Tiempo en establecer distancia de seguridad=1 (seg) Tiempo en generar matriz de adyacencia (seg) Tiempo total (seg) L máx=1 0.037 0.029 2992 0.027 0.268 0.153 0.088 1.016 <1,40 L máx=5 0.016 0.022 386 0.019 0.242 0.153 0.088 0.128 <0.50 L máx=10 0.015 0.021 244 0.018 0.242 0.149 0.088 0.063 <0.40 Sín L máx 0.008 0.019 136 0.018 0.233 0.148 0.088 0.038 <0.35 *Valores calculados realizando un ensayo de 6 resultados, descartando valores que excedan un 5% de la media aritmética. Figura 4-25. Tabla de tiempos para mapa de 100x100 Tiempo de calculo puntos obstáculo (seg) Tiempo en calcular diagrama Voronoi (seg) Número de puntos obstáculo Tiempo en comprobar línea a línea (seg) Tiempo en establecer distancia de seguridad=4 (seg) Tiempo en establecer distancia de seguridad=2 (seg) Tiempo en establecer distancia de seguridad=1 (seg) Tiempo en generar matriz de adyacencia (seg) Tiempo total (seg) L máx=1 0.902 0.059 15400 0.061 6.086 3.833 0.169 16.530 <24.0 L máx=5 0.352 0.029 1592 0.033 5.850 3.775 0.168 1.149 <7.50 L máx=50 0.310 0.025 242 0.0028 5.535 3.821 0.168 0.046 <6.0 Sín L máx 0.298 0.019 136 0.031 5.657 3.654 0.169 0.043 <6.0 *Valores calculados realizando un ensayo de 6 resultados, descartando valores que excedan un 5% de la media aritmética. Figura 4-26. Tabla de tiempos para mapa de 500x500 35 35 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi 4.1.4.1 Conclusiones Las tablas permiten sacar muchas conclusiones, la primera de ellas es que establecer una distancia de seguridad se antoja como la operación más costosa cuando esta distancia de seguridad aumenta ligeramente, llegando a límites de más de 6 segundos de cálculo en las peores situaciones, lo cual no se podría aplicar en tiempo real. La distancia límite también provoca un gran retraso en el cálculo total, ya que aumenta en gran medida los puntos obstáculos a calcular y por tanto el tiempo de cálculo. Esto se puede solucionar en general controlando la resolución del mapa, ya que con un mapa de 100x100 el máximo tiempo de cálculo no excede el segundo y medio, lo que podría permitir operaciones en tiempo real con un tiempo de respuesta aceptable. Sin distancia de seguridad todos los tiempos son muy reducidos, por lo que se puede decir que la generación del diagrama de Voronoi y su posterior eliminado de líneas que pasan por obstáculos no tiene apenas coste. Para un número elevado de puntos obstáculos (>1000), los tiempos de cálculo de la matriz de adyacencia aumentan demasiado (>1seg), por lo que conviene tener controlados los parámetros, pues el cálculo de la matriz de adyacencia es el último paso para poder generar planificación de trayectorias utilizando algoritmos de búsqueda del camino óptimo. 4.1.5 Matriz de adyacencia y algoritmo Dijkstra La matriz de adyacencia es una matriz cuadrada que establece relaciones entre distintos puntos. En función de si estos puntos están o no conectados en el lugar correspondiente aparecerá un 0, o la distancia entre ambos puntos. Figura 4-27. Esquema Dijkstra Para nuestro caso la matriz será no dirigida, es decir, no influye si vas de A a B, o al contrario pues consideraremos todos los caminos como bidireccionales, por lo que nuestra matriz será simétrica. Para obtener la matriz de adyacencia en Matlab he tenido que utilizar variables tipo cell para crear una celda de puntos en la que cada celda almacene los puntos a los que está conectado, y a continuación realizar una comprobación en bucle para ir rellenando valores en nuestra matriz, que inicialmente será una matriz de npuntos x npuntos de ceros. La gran ventaja de este algoritmo es que depende únicamente de una variable de coste, por lo que funciona muy bien para pocos nodos, sin embargo cuando el número de nodos aumenta mucho el algoritmo se hace lento de cálculo debido a la gran cantidad de nodos que tiene que explorar. Le pasaremos a una función que implementa este algoritmo en Matlab la matriz de adyacencia junto con el punto inicial y el de destino, y nos devolverá la secuencia de puntos que debemos seguir, dando por ejemplo como Implementación 42 42 Figura 4-35. Modelado obstáculo Una vez obtenido el modelado del espacio visible, se pasa a establecer los puntos clave que serán los utilizados para generar el diagrama de Voronoi, en este caso la selección de estos puntos clase será trivial pues ya se ha realizado la selección en Matlab. 4.2.2 Generación de trayectorias Una vez modelados los obstáculos detectados, se debe establecer un plan de actuación según la situación del robot y estos obstáculos. Al comienzo el robot se dirige al punto de destino. Durante la navegación del robot y con una cierta frecuencia se irán analizando las lecturas del sensor, para comprobar si el robot puede alcanzar el destino o puede colisionar con un obstáculo. En caso de posible colisión, se lanza el algoritmo de Voronoi y el algoritmo Dijsktra para actualizar camino generando nuevos waypoints que aseguren la evitación de la colisión. De esta forma, solo se computará el algoritmo de evitación de obstáculos cuando sea necesario, evitando una posible ralentización del sistema. Respecto al seguimiento de caminos, cuando el robot esté a una distancia menor de 10 cm del waypoint actual se dirigirá al próximo waypoint. Esto aporta fluidez y continuidad a las trayectorias, pues la velocidad lineal es reducida y no da lugar a problemas de aproximamiento excesivo a los obstáculos. Se puede entender de forma más visual con el siguiente diagrama de flujo: 43 43 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 4-36. Plan de generación de trayectorias Para determinar la posición de los waypoints respecto al sistema de coordenadas global debemos realizar una traslación y rotación de los ejes coordenados, pues el sensor da las coordenadas en su sistema de referencia local en coordenadas polares (módulo y ángulo), que posteriormente hemos pasado a un sistema de coordenadas Implementación 44 44 cartesianas local del sensor con la matriz de obstáculos La transformación es la siguiente: 𝑃𝑂𝑆𝑥 𝑔𝑙𝑜𝑏𝑎𝑙𝑒𝑠 𝑊𝑃 =𝑃𝑂𝑆𝑥 𝑔𝑙𝑜𝑏𝑎𝑙𝑒𝑠 𝑟𝑜𝑏𝑜𝑡 +(𝑃𝑂𝑆𝑥 𝑙𝑜𝑐𝑎𝑙𝑒𝑠 𝑊𝑃 −2.5)∗cos(𝑡ℎ𝑒𝑡𝑎−𝜋 2) − 𝑃𝑂𝑆𝑦 𝑙𝑜𝑐𝑎𝑙𝑒𝑠 𝑊𝑃 ∗sin(𝑡ℎ𝑒𝑡𝑎−𝜋 2) 𝑃𝑂𝑆𝑦 𝑔𝑙𝑜𝑏𝑎𝑙𝑒𝑠 𝑊𝑃 =𝑃𝑂𝑆𝑦 𝑔𝑙𝑜𝑏𝑎𝑙𝑒𝑠 𝑟𝑜𝑏𝑜𝑡 +𝑃𝑂𝑆𝑦 𝑙𝑜𝑐𝑎𝑙𝑒𝑠 𝑊𝑃∗cos(𝑡ℎ𝑒𝑡𝑎−𝜋 2)+(𝑃𝑂𝑆𝑥 𝑙𝑜𝑐𝑎𝑙𝑒𝑠 𝑊𝑃−2.5)∗sin(𝑡ℎ𝑒𝑡𝑎−𝜋 2) Los cambios de coordenadas se realizan después de restar a las coordenadas locales en x una distancia de 2.5 metros, ya que la posición del sensor en su sistema de coordenadas locales son (2.5 0), es decir, el sensor se encuentra situado en la mitad de la matriz de obstáculos respecto al eje x. En la imagen podemos ver una representación en Gazebo de las coordenadas: Figura 4-37. Cambio de Coordenadas respecto al sensor a respecto al origen Para el guiado del vehiculo se establece una velocidad lineal constante, y una velocidad angular proporcional a la diferencia entre la orientación del robot, y la recta que une el robot con el siguiente waypoint (que sería la orientación deseada), siendo α la orientación del vehículo respecto al origen y β la del waypoints respecto al vehículo, es decir: 𝑤=𝐾∗(𝛼−𝛽)𝑟𝑎𝑑 𝑠,𝑝𝑎𝑟𝑎 𝐾=𝑐𝑡𝑒 𝑣=𝑐𝑡𝑒 Para comprender la diferencia angular entre el obstáculo se puede observar la siguiente representación gráfica: 45 45 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 4-38. Cálculo velocidad angular 4.2.3 Toolbox de Matlab para conexionado con ROS y árbol de conexiones Para obtener el diagrama de Voronoi se va a hacer uso de la novedosa herramienta de Matlab Robotics System Toolbox que ha incorporado en su versión 2015.a.Eesta herramienta comunicar Matlab y ROS, con lo que se puede trabajar con sistemas en tiempo real, se puede monitorizar su estado, analizar los resultados, visualizar gráficas de estado, o interaccionar con Gazebo y V-REP para simulaciones de sistemas. La Figura 3-37 muestra sus utilidades. Figura 4-39. Robotics System Toolbox Implementación 46 46 Para nuestro caso concreto, estableceremos un conexionado entre nuestro nodo main, en donde se va comandando al vehiculo y calculando las trayectorias, y un nodo global que generará Matlab de forma automática. Ambos están conectados mediante un canal de comunicación “topic”, a través del cual ROS solicitará el cálculo del diagrama de Voronoi a Matlab cada cierto tiempo, enviando un vector con los puntos clave con los que se pretende calcular el diagrama, y Matlab devolverá una matriz de [n][4], donde cada fila constituye una recta del diagrama, determinada por las coordenadas X1n y Y1n del punto inicial del segmento, y los puntos X2n y Y2n. Podemos observar en el siguiente diagrama de conexiones el árbol de nodos y topics que constituyen todo el sistema: Figura 4-40. Árbol de conexiones En el esquema se pueden observar los topics de conexión entre Matlab y el nodo robot_control, que son /mytopic y /mytopicback, el topics /scan para conectar las lecturas del láser que obtiene a partir de Gazebo con robot_control, y el topic /odom para enviar el estado del robot desde Gazebo al control del robot. La zona superior del esquema computa la dinámica del Turtlebot a partir de la velocidad impuesta por el control del robot, que envía a través del topic /cmd_vel_mux/imput/navi que tras procesarla se la envía a Gazebo con el topic /mobile_base/commands/velocity. 47 5 EXPERIMENTACIÓN n este capítulo vamos a mostrar algunos ejemplos de trayectorias seguidas por el vehiculo para distintos escenarios y para ver cómo evita obstáculos y llega al punto objetivo . Se ha utiilzado el entorno de simulación Gazebo de ROS para las pruebas, en vistas a poder probar el sistema para entornos reales. Una representación en video plasma de mejor forma el comportamiento dinámico del robot y la evitación de obstáculos, sin embargo con una serie de imágenes de varios experimentos se puede tener una idea del comportamiento del sistema. 5.1 Experimento 1 En el primer experimento hemos fijado el objetivo a 18 metros del punto inicial del robot. Cada cuadrado de la superficie del terreno tiene unas dimensiones de 1x1 metros. Al iniciar el recorrido, el robot toma inicialmente la dirección del punto objetivo, y nada más detectar un obstáculo que le impide llegar al objetivo, lanza Voronoi y calcula el camino óptimo con Dijsktra para evitar colisionar con el obstáculo detectado. En un primer instante se genera un vector de waypoints que determinan el recorrido que se va a realizar en principio, y durante el trayecto se va a ir comprobando si se puede llegar al objetivo directamente. Esto hará que se acorte camino y sea más eficiente el recorrido: Como ya se explicó, para esto el sistema va a ir comprobando si en la dirección de movimiento existe algún obstáculo. Se considera una anchura de comprobación de unos 60 cm, para evitar pasar demasiado cerca de los obstáculos. En la siguiente imagen podemos observar el recorrido calculado inicialmente: E El valor de una idea radica en el uso de la misma. - Tomas A. Edison - Experimentación 48 48 Figura 5-1. Traza del recorrido inicial El robot va pasando por los waypoints mientras el camino calculado sea el correcto y no se pueda llegar al objetivo directamente: Figura 5-2. Recorrido ejemplo 1.1 En el mismo instante en que el camino al objetivo sea posible, se establece el destino como el siguiente waypoint, como se puede observar en la sFigura 4-3. 49 49 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 5-3. Recorrido ejemplo 1.2 Ahora se sitúa en tiempo real un obstáculo justo delante del recorrido, e inmediatamente recalcula el camino y genera una nueva trayectoria: Figura 5-4. Recorrido ejemplo 1.3 Como se observa, el robot va trazando las trayectorias eligiendo el camino más corto y de forma que queda lejos de los obstáculos del camino para generar trayectorias seguras: Experimentación 50 50 Figura 5-5. Recorrido ejemplo 1.4 De nuevo, una vez que el objetivo es accesible directamente, el vehiculo toma su dirección: Figura 5-6. Recorrido ejemplo 1.5 Ahora vamos a introducir un obstáculo cercano al anterior justo delante del robot cuando el anterior ha salido del rango del sensor, en ese momento el robot cambia de nuevo de trayectoria y se acerca demasiado al obstáculo anterior, por lo que se puede determinar que el rango angular del sensor podría ser demasiado reducido para ciertos casos, además de que el modelado de las zonas muertas en la matriz mapa debe ser más preciso para evitar el acercamiento a zonas desconocidas: 51 51 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi Figura 5-7. Recorrido ejemplo 1.5 Una vez más , el robot toma el camino directo al no detectar obstáculos en la dirección hacía el destino: Figura 5-8. Recorrido ejemplo 1.6 Una vez que llega al destino el vehiculo se detiene y el sistema avisa por pantalla de la llegada al destino con el mensaje goal reached. Conclusiones y márgenes de ampliación 58 58 La herramienta Gazebo es muy útil evaluar los algoritmos implementados y permite reducir el tiempo de desarrollo de nuevos sistemas sin la necesidad de probar los algoritmos en sistemas reales. Sin embargo para observar estados intermedios del algoritmo, como puede ser el modelado de obstáculos o la trayectoria calculada, el uso de la herramienta Rviz permitiría estas opciones ya través del conexionado de los datos del sistema con Rviz a través de topics. Con las mejoras sugeridas anteriormente, el sistema estaría listo para unas posibles pruebas con un Turtlebot real, ya que todo el sistema está preparado para la implementación directa en una plataforma real, teniendo en cuenta la necesidad de un sistema de posicionamiento del vehículo en el entorno, para lo que podría utilizarse un GPS o GPS diferencial para entornos al aire libre, o sistemas de posicionamiento por balizas de posición, cámaras, o emisores y receptores de luz infrarroja para interiores. 59 59 Generación de trayectorias en tiempo real a partir de diagramas de Voronoi REFERENCIAS [1] C. Goerzen, Z. Kong, and B. Mettler, “A survey of motion planning algorithms from the perspective of autonomous uav guidance,” Journal of Intelligent and Robotic Systems, vol. 57, no. 1, pp. 65-100, 2010. [2] Conde, R., Alejo, D., Cobano, J.A., Viguria, A., and Ollero, A. “Conflict detection and resolution method for cooperating unmanned aerial vehicles”, Journal of Intelligent & Robotic Systems, 2012. [3] Prasanna, H.M., Ghosey, D., Bhat, M.S., Bhattacharyya, C., and Umakant, J., “Interpolation-aware trajectory optimization for a hypersonic vehicle using nonlinear programming”, In AIAA Guidance, Navigation, and Control Conference and Exhibit, San Francisco, USA, 2005. [4] Vela, A., Solak, S., Singhose, W., and Clarke, J.P., “A mixed integer program for flight-level assignment and speed control for conflict resolution”, In Proceedings of the 48th IEEE Conference on Decision and Control, 5219 –5226, 2009. [5] Pallottino, L., Feron, E., and Bicchi, A., “Conflict resolution problems for air traffic management systems solved with mixed integer programming”, Intelligent Transportation Systems, IEEE Transactions on, 3(1), 3– 11, 2002. [6] Geiger, B., “Unmanned Aerial Vehicle Trajectory Planning with direct methods”, Ph.D. thesis, The Pennsylvania State University, Pennsylvania, USA, 2009. [7] Alejo, D., Cobano, J. A., Heredia, G., and Ollero, A., “Conflict-free 4D Trajectory Planning in Unmanned Aerial Vehicles for Assembly and Structure Construction”, Journal Intelligent and Robotic Systems, Vol. 73, pp. 783-795, 2014. [8] Lavalle, “S.M., Rapidly-exploring random trees: A new tool for path planning”, In Computer Science Dept, Iowa State University, Tech. Rep. TR: 9811, 1998. [9] Xue, M. y Atkins, E.M., “Terminal area trajectory optimization using simulated annealing”, In 44th AIAA Aerospace Sciences Meeting and Exhibit. Reno, Nevada, USA, 2006. [10] Durand, N. and Alliot, J., “Ant colony optimization for air traffic conflict resolution”, In Proceedings of the Eighth USA/Europe Air Traffic Management Research and Development Seminar (ATM2009). Napa, (CA, USA), 2009. [11] Foskey, .M, Garber, M., Lin, M., and Manocha, D., “A voronoi-based hybrid motion planner”, In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2001. Referencias 60 60 [12] Rao, N., Stoltzfus, N., and Iyengar, S. S., “A retraction method for learned navigation in unknown terrains for a circular robot”, IEEE Transactions on Robotics and Automation, 7(5), October 1991. [13] S. Zheng, A. Janecek, J. Li, and T. Ying, “Dynamic search in fireworks algorithm,” in Proc. 2014 IEEE Congress on Evolutionary computation, Beijing, China, 6-11 July 2014, pp. 3222–3229. [14] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” International Journal of Robotics Research, vol. 30, 2011. [15] J. K. Kuchar and L. C. Yang, “A review of conflict detection and resolution modeling methods,” IEEE Transactions on Intelligent Transportation Systems, vol. 1, pp. 179–189, 2000. [16] Wikipedia. https://es.wikipedia.org/wiki/Grafo. [En línea] [17] Ferran Hurtado, Belen Palop y Vera Sacristian, “Diagramas de Voronoi con funciones temporales,” Universitat Politecnica de Catalunya, Barcelona, 1996.