Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla

Transcripción

1 Trabajo Fin de Grado Grado en Ingeniería en Tecnologías Industriales Generación global de trayectorias para robots móviles, basada en curvas betaspline Autor: Alejandro Muñoz Cueva Tutores: Aníbal Ollero Baturone José Antonio Cobano Suárez Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2014

2 ii

3 Trabajo Fin de Grado Grado en Ingeniería en Tecnologías Industriales Generación global de trayectorias para robots móviles, basada en curvas betaspline Autor: Alejandro Muñoz Cueva Tutores: Aníbal Ollero Baturone José Antonio Cobano Suárez Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2014 iii

4

5 Trabajo Fin de Grado: Generación global de trayectorias para robots móviles, basada en curvas betaspline Autor: Alejandro Muñoz Cueva Tutores: Aníbal Ollero Baturone José Antonio Cobano Suárez El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2014

6 vi El Secretario del Tribunal

7 Agradecimientos A los tutores que me han orientado en la elaboración de este trabajo. A D. Aníbal Ollero por introducirme en el ámbito de la robótica, así como darme la oportunidad de realizar este trabajo, y a D. José Antonio Cobano por su continua ayuda y atención a lo largo de todo el tiempo invertido en el trabajo. i

8 Resumen En este trabajo se presenta un método de planificación global de trayectorias en entornos de dos dimensiones, conocidos y con obstáculos estáticos. El método busca combinar seguridad y flexibilidad, de forma que se obtengan trayectorias seguras entre un punto inicial y otro punto final dados. Se pretende que el software presentado sea capaz de calcular una trayectoria segura y realizable por un robot móvil cualquiera, a velocidad constante, introduciendo una serie de parámetros que definan las características del mismo, y que se tendrán en cuenta para calcular la trayectoria final. Para ello, el método integra el uso de los diagramas de Voronoi y el suavizado mediante el uso de curvas β-spline. ii

9 Índice Agradecimientos Resumen Índice Índice de Figuras 1 Introducción Objetivo del trabajo Estructura del trabajo 2 2 Estado del arte Dijkstra Algoritmo A* Bellman-Ford Algoritmo D* Búsqueda en profundidad Búsqueda en amplitud 11 3 Diagramas de Voronoi y modelado de obstáculos Introducción Diagramas de Voronoi Construcción de un diagrama de Voronoi Problemas al crear el diagrama de Voronoi Modelado de obstáculos Selección de vértices Proyección de los vértices Añadido de puntos adicionales 22 4 Selección del camino y suavizado con curvas beta-spline Introducción Algoritmos de planificación Construcción de la matriz de costes Algoritmo de Dijkstra Algoritmo A* Interpolación mediante curvas β-spline Acotación cartesiana Condiciones iniciales y finales de la curva β-spline Selección del camino y suavizado Selección del camino Suavizado del camino 52 5 Simulaciones Introducción Simulaciones Diferencia entre Dijkstra y A* 67 i ii iii v iii

10 Simulaciones variando el modelo Trayectorias prohibidas por el suavizado 77 6 Conclusiones y trabajo futuro 83 Referencias 86 iv

11 ÍNDICE DE FIGURAS Figura 2-1 Ejemplo de cuadrícula uniforme 5 Figura 2-2 Ejemplo de cuadrícula no uniforme 6 Figura 2-3 Descomposición exacta en celdas trapezoidales 6 Figura 2-4 Grafo de visibilidad con dos obstáculos 7 Figura 2-5 Diagrama de Voronoi generado 8 Figura 2-6 Representación de un ciclo de coste negativo 10 Figura 2-7 Posible orden de visita de los nodos en una búsqueda en profundidad 11 Figura 2-8 Posible orden de visita de los nodos en una búsqueda en anchura 12 Figura 3-1 Representación de los obstáculos de un mapa 16 Figura 3-2 Rejilla de 16 elementos 17 Figura 3-3 Mapa con tres obstáculos 17 Figura 3-4 Matriz de 4x4 18 Figura 3-5 Representación de los obstáculos y sus vértices 19 Figura 3-6 Diagrama de Voronoi modelando sólo con los vértices 19 Figura 3-7 Diagrama de Voronoi tras un mal modelado 20 Figura 3-8 Caso en el que proyectar sería inútil 21 Figura 3-9 Diagrama de Voronoi tras proyectar los vértices 21 Figura 3-10 : Diagrama de Voronoi resultante 22 Figura 3-11 Diagrama de Voronoi tras los dos pasos expuestos 23 Figura 3-12 Situación en la que existe deformación de una línea del diagrama 24 Figura 3-13 Diagrama de Voronoi tras el modelado 25 Figura 3-14 Diagrama de Voronoi resultante 25 Figura 4-1 Diferencia entre camino y trayectoria, y separación entre ellos 28 Figura 4-2 Celda de Voronoi 30 Figura 4-3 Distancia de un tramo a un obstáculo 30 Figura 4-4 Grafo de 5 nodos, con pesos representados 32 Figura 4-5 Resolución del grafo mediante Dijkstra 33 Figura 4-6 Spline cúbica con 4 nodos y 3 tramos 36 Figura 4-7 Curva de Bezier de grado 4 y su polígono de control 37 Figura 4-8 β-spline de 6 vértices y 3 tramos 38 Figura 4-9 Ilustración de la acotación cartesiana 39 Figura 4-10 Escenario para calcular la distancia d 40 Figura 4-11 Representación del polígono de control para suavizar un camino de 5 vértices 45 Figura 4-12 Situación en la que dos circunferencias aparecen secantes 46 v

12 Figura 4-13 Intentos de adaptar el camino y hacerlo realizable 47 Figura 4-14 Representación de la distancia de seguridad 48 Figura 4-15 Representación de la separación máxima al suavizar 48 Figura 4-16 Camino de 6 vértices, donde se elimina el cuarto. 50 Figura 4-17 Camino de 6 vértices, donde se elimina el tercero. 51 Figura 4-18 Circunferencia sobre el vértice v i. 52 Figura 4-19 Tangentes comunes a dos circunferencias 54 Figura 4-20 Circunferencias y bisectrices en un tramo de 5 vértices 54 Figura 4-21 Tangente exterior a dos circunferencias 55 Figura 4-22 Situación anterior en coordenadas globales 56 Figura 4-23 Representación de una tangente interior 57 Figura 4-24 Representación de dos circunferencias y el vector bisectriz 58 Figura 4-25 Búsqueda de vértices separados una distancia δ 60 Figura 5-1 Mapa 1 64 Figura 5-2 Mapa 2 64 Figura 5-3 Mapa 3 64 Figura 5-4 Diagrama de Voronoi sin filtrar y camino elegido 66 Figura 5-5 Circunferencias y tangentes sobre el camino 66 Figura 5-6 Trayectoria obtenida con el modelo 67 Figura 5-7 Solución obtenida con el algoritmo A* 67 Figura 5-8 Solución obtenida con el algoritmo de Dijkstra 68 Figura 5-9 Simulación de [10,10] a [94,80] con d=2 m 69 Figura 5-10 Mismo caso, con R min =4 m 69 Figura 5-11 Simulación de [95,8] a [20,80]. d=2 m, R min =2 m 70 Figura 5-12 Mismo caso que en la Figura 5.11, pero d=3 m 70 Figura 5-13 Mismo caso, con d=0.5 m 71 Figura 5-14 De [5,95] a [95,5], con d=0.5 m, R min =2 m 71 Figura 5-15 Mismo caso, pero con d=2 m 72 Figura 5-16 Mismo caso, pero con R min =4 m 72 Figura 5-17 Trayectoria de [95,55] a [5,95] en el mapa 3 73 Figura 5-18 Mismo caso, con R min =4 m 73 Figura 5-19 Trayectoria de [65,80] a [75,35 74 Figura 5-20 Trayectoria de [5,85] a [95,5], con d=1 m y R min =2 m 74 Figura 5-21 Trayectoria de [5,85] a [95,5], con d=2 m y R min =2 m. 75 Figura 5-22 Trayectoria de [5,95] a [95,5], con d=2 m y R min =4 m 75 Figura 5-23 Trayectoria prohibida por suavizado inseguro. 77 Figura 5-24 Trayectoria final segura obtenida. 78 Figura 5-25 Trayectoria prohibida de [5,95] a [95,55], con d=1 m y R min =6 m 78 Figura 5-26 Primera de las trayectorias prohibidas, con las colisiones resaltadas 79 vi

13 Figura 5-27 Segunda de las trayectorias prohibidas, por salirse del mapa. 79 Figura 5-28 Trayectoria prohibida erróneamente, ya que es segura. 80 Figura 5-29 Ampliación del tramo conflictivo, y representación de todas las líneas. 80 vii

14

15 1 INTRODUCCIÓN E l uso de robots móviles para diversas aplicaciones se está convirtiendo en algo cada vez más común a medida que avanza la tecnología debido a las ventajas que ofrecen. Conforme se avanza en la complejidad de las tareas desempeñadas por dichos robots móviles, surge la necesidad de establecer estrategias de planificación de trayectorias, de forma que el robot sea capaz de generar una trayectoria segura para pasar por los puntos requeridos según la actividad a realizar. Se persigue así conseguir que el robot tenga la mayor autonomía posible Objetivo del trabajo Los métodos de planificación de caminos simplemente definen una secuencia de puntos objetivo o waypoints, y hacen que el seguidor de caminos se encargue de que el robot pase por dichos puntos. En este trabajo se presenta un método orientado a la navegación en entornos de dos dimensiones considerablemente poblados de obstáculos, por lo que la estrategia comentada anteriormente no es adecuada, ya que podría no asegurar la ausencia de colisiones en los tramos definidos por dos waypoints consecutivos. Resulta evidente por tanto la necesidad de generar una curva continua que una los waypoints y que el robot utilice como trayectoria a seguir, de forma que asegure la ausencia de colisiones a lo largo de la misma. Sin embargo, cada robot móvil tiene un tamaño y unas limitaciones cinemáticas distintas, por lo que un robot de características concretas no tiene por qué ser capaz de reproducir cualquier curva que pase por cada waypoint. Por lo tanto el planificador, encargado de encontrar la trayectoria que una los puntos inicial y final, debe considerar las limitaciones del robot móvil, así como sus dimensiones, para generar una curva que pueda ser seguida por el robot, y que suponga un camino seguro y sin colisiones. En este trabajo se presenta un método de planificación de trayectorias en mapas conocidos y estáticos de dos dimensiones, por lo que se trata de planificación global. El método presentado pretende conseguir dos objetivos fundamentales: Seguridad y flexibilidad. Al estar orientado a la navegación en entornos densos de obstáculos, la seguridad es un aspecto fundamental para evitar posibles colisiones. Por lo tanto, se premia la seguridad en la trayectoria, aunque esto implique un recorrido un poco mayor que en el caso de trayectorias más cortas pero más arriesgadas. Por otra parte, cuando hablamos de flexibilidad como uno de los objetivos principales, nos referimos a que el método se adapte a las características concretas de un robot móvil, y no en el sentido inverso. De esta forma, dado un robot móvil de ciertas características, no será necesario buscar un método de planificación compatible con ellas, sino que el mismo método presentado en este trabajo es capaz de buscar una trayectoria segura y realizable teniendo en cuenta las limitaciones del robot, de forma que pueda ser reproducida por el robot móvil 1

16 Planificación global de trayectorias para robots móviles, basada en curvas β-spline que se desee utilizar. Así, se presenta en este trabajo un método de planificación que permita, introduciendo una serie de parámetros que definan las limitaciones del robot, generar una curva continua en posición, orientación y curvatura que una los puntos inicial y final del trayecto deseado, de forma que se eviten colisiones con los obstáculos del entorno y pueda ser seguida perfectamente por el robot móvil. El objetivo es, por tanto, presentar un software adaptable a distintos robots móviles sólo variando algunos parámetros. Para obtener el resultado deseado, partiremos de un mapa que contenga la información del entorno. Dicho mapa vendrá representado por una cuadrícula, donde los obstáculos estarán modelados mediante polígonos. Para darle un papel predominante a la seguridad, nos basaremos en la generación de diagramas de Voronoi, ya que esta herramienta permite encontrar los caminos más alejados de los dos obstáculos más cercanos y, por tanto, más seguros. Una vez generado el diagrama de Voronoi, se escogerá el camino óptimo mediante los algoritmos de planificación Dijkstra o A*. Por último, la obtención de una curva continua en posición, orientación y curvatura se conseguirá mediante el uso de curvas β-spline. Además, el método presenta una ventaja ya que integra los diagramas de Voronoi con el suavizado mediante curvas β-spline. De esta forma, adapta el camino generado para que la curva β-spline resultante no incumpla las limitaciones cinemáticas del robot móvil al que se ha orientado, y siga siendo segura en todo momento Estructura del trabajo El capítulo 2 introduce algunos conceptos y estrategias que son comunes en los temas de planificación de trayectorias. En primer lugar, se explican brevemente algunos de los métodos más comunes de representación del entorno, así como algunas herramientas muy utilizadas en este tipo de trabajos, como son los grafos de visibilidad y los diagramas de Voronoi. Posteriormente se exponen algunos de los algoritmos de búsqueda del camino óptimo más comunes, así como las características principales de cada uno de ellos. El capítulo 3 describe el método presentado en este trabajo. Dicho método se explica entre los capítulos 3 y 4, ya que se compone de dos partes diferenciadas. En esta primera parte se expone la estrategia seguida para el modelado de obstáculos a partir de un mapa representado por una matriz. Este modelado se realiza teniendo como objetivo la generación de un diagrama de Voronoi seguro y fiable y, para ello, se profundiza en este capítulo en las características de los diagramas de Voronoi. El diagrama generado en este capítulo supone el punto de partida del capítulo 4. El capítulo 4 es el capítulo más importante de este trabajo. En él se exponen todos los pasos seguidos desde que se parte del diagrama de Voronoi obtenido en el capítulo 3, hasta que se obtiene la curva final que se propone como trayectoria a seguir. Para ello, el capítulo se divide en dos bloques principales. Por un lado, es necesario escoger el camino óptimo, para lo que se profundiza en los algoritmos de Dijkstra y A*, que son los utilizados en este trabajo. Una vez obtenido el camino óptimo, es necesario suavizarlo mediante la generación de curvas β-spline, cuyas propiedades destacadas se exponen en este capítulo. Sin embargo, para seleccionar el camino óptimo y suavizarlo es necesario tener en cuenta las características del robot. Estas características se traducen en conocer la distancia de seguridad necesaria y el radio mínimo de giro para una velocidad constante dada. En función de estos parámetros, se presenta una estrategia de adaptación del camino escogido para que la curva resultante no viole ninguna de las restricciones del robot, y sea una curva continua en posición, orientación y curvatura, y realizable por el robot móvil considerado. 2

17 El capítulo 5 muestra una serie de simulaciones que pretenden demostrar la validez del método presentado, y verificar que se cumplen los objetivos marcados. Incluye distintas simulaciones en tres mapas con densidades de obstáculos variables, y se varían los parámetros del robot que se desee utilizar para demostrar que el método se adapta a distintas restricciones, y acaba encontrando el camino más óptimo posible manteniendo la seguridad en todo momento. Se incluye también en este capítulo una tabla con los tiempos de cálculo invertidos en cada simulación, y una comparación del tiempo utilizando el algoritmo de Dijkstra y A*. El capítulo 6 resume las conclusiones que pueden sacarse de las simulaciones, valorando si se han conseguido los objetivos y, por último, se proponen algunas posibles ampliaciones que pudieran realizarse siguiendo la línea de este trabajo. 3

18 Planificación global de trayectorias para robots móviles, basada en curvas β-spline 4

19 2 ESTADO DEL ARTE L a planificación de caminos eficientes y seguros juega un papel fundamental en aplicaciones con robots móviles autónomos. A partir de la información sobre el entorno, los robots deben calcular trayectorias eficientemente para llevar a cabo las misiones de forma segura. Una clasificación de algoritmos de planificación es presentada en [1]. Entre los más utilizados en la literatura destacan los algoritmos genéticos [2], métodos de programación no-lineal [3], mixed-integer linear programming [4][5], métodos de colocación [6], optimización de enjambres de partículas (Particle Swarm Optimization) [7], algoritmos de planificación RRT (Rapidly-Exploring Random Trees) [8], cocido simulado (Simulated Annealing) [9], colonias de hormigas (Ant Colony) [10] y otros. Los algoritmos de planificación de caminos se pueden dividir dependiendo del tipo de representación del entorno usada. De este modo, se pueden utilizar cuadrículas (grids en inglés) uniformes o no-uniformes, 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. Como su propio nombre indica, el método de cuadrículas uniformes divide el entorno en una serie de celdas, donde todas tienen las mismas dimensiones. Esta disposición se muestra en la Figura 2.1. Por otra parte, el método de cuadrículas no uniformes divide en entorno en celdas, pero el tamaño de las mismas es variable, según las necesidades que se persigan. Un ejemplo de grids no uniformes se muestra en la Figura 2.2. Figura 2-1 Ejemplo de cuadrícula uniforme 5

20 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 2-2 Ejemplo de cuadrícula no uniforme En cuanto a la descomposición exacta de celdas, la herramienta más habitual es 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. Un ejemplo de descomposición exacta en celdas se muestra en la Figura 2.3: Figura 2-3 Descomposición exacta en celdas trapezoidales Como se puede observar en la Figura 2.3, la posición de los obstáculos da lugar a la formación de celdas trapezoidales, que representan el espacio libre. El algoritmo de planificación identifica las celdas que contienen los puntos inicial y objetivo, y planificará la secuencia de celdas por las que tiene que pasar. En la elección del camino a seguir para recorrer una determinada celda, es habitual hacer que el robot pase por el punto medio de cada frontera entre dos celdas, lo cual hace que deje a los obstáculos tan alejados como sea posible. 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. Los roadmaps probabilísticos buscan crear un mapa generado de forma aleatoria y libre de colisiones, de forma que conecte de forma rápida la posición del robot móvil con el objetivo. Sin embargo, los métodos basados en roadmaps más utilizados son los grafos de visibilidad y los diagramas de Voronoi. En los grafos de visibilidad, se representan inicialmente los vértices de los obstáculos para, posteriormente, unir cada vértice con el resto de vértices mediante líneas rectas, llamadas aristas, y comprobar 6

21 si se intercepta algún obstáculo o no. Esto equivale a comprobar qué vértices son visibles desde un determinado vértice. Una vez repetido este proceso para cada vértice, se obtiene una red de líneas o grafo que unen los distintos vértices cuya conexión no intercepta a ningún obstáculo. En la Figura 2.4 Se muestra un grafo de visibilidad. Figura 2-4 Grafo de visibilidad con dos obstáculos Para evitar la consideración de robot puntual, el método seguido habitualmente en los grafos de visibilidad es la expansión de los obstáculos. Así, dependiendo de las dimensiones del robot, los obstáculos se sobredimensionan de forma que no se produzcan colisiones. Una vez generado el grafo de visibilidad, el planificador seleccionará qué vértices conforman la secuencia a seguir para conectar el punto inicial y el punto final de forma óptima. Como se puede comprobar, la seña de identidad de este método es que pasa muy cerca de los obstáculos, ya que los puntos de paso del camino serán vértices de los obstáculos sobredimensionados. En el extremo opuesto se encuentran los diagramas de Voronoi. Un diagrama de Voronoi también genera un grafo, pero el objetivo en este caso es separarse lo máximo posible de los obstáculos. Dado un conjunto de obstáculos en el plano, el diagrama de Voronoi es el lugar geométrico de los puntos del plano que equidista de los dos obstáculos más cercanos en todo momento. Por ello, supone una estrategia en la que prima ante todo la seguridad. Así, dados un conjunto de obstáculos puntuales {p 1,,p n }, el diagrama de Voronoi representa las posibles trayectorias más seguras, al encontrarse lo más alejado posible de los obstáculos. En la Figura 2.5 Se muestra un diagrama de Voronoi, generado a partir del conjunto de obstáculos puntuales P, y donde {v 1,, v m } son los vértices del diagrama de Voronoi, que coinciden con los puntos más alejados de los 3 obstáculos más cercanos. 7

22 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 2-5 Diagrama de Voronoi generado Por otra parte, los diagramas de Voronoi suelen utilizarse en entornos muy poblados de obstáculos, lo cual también justifica su elección para este trabajo. La ventaja de los diagramas de Voronoi es que proporcionan un espacio libre máximo a partir de regiones con obstáculos. Tienen aplicaciones prácticas para planificación multi-robot, vigilancia y cobertura de áreas. Existen varios métodos para generar los diagramas de Voronoi. Se pueden construir en espacios continuos [11] [12] o discretos, por ejemplo grids. En navegación de robots móviles se suelen usar representaciones basadas en cuadrículas (grids) y los diagramas de Voronoi se generan sobre un conjunto finito de celdas. Tras generar un diagrama de Voronoi, los algoritmos de planificación deben calcular el camino para unir un punto inicial y final. El diagrama de Voronoi proporciona un grafo sobre el que buscar el camino más corto. Entre los algoritmos de búsqueda se destacan los siguientes: Dijkstra Bellman-Ford A* D* Búsqueda en profundidad (Deep-First Search) Búsqueda en amplitud (Breadth-First Search) 8

23 2.1. Dijkstra El algoritmo de Dijkstra es un algoritmo usado para la determinación del camino más corto desde un nodo origen de un grafo, al resto de nodos del mismo. Para ello, toma como entrada un grafo cuyas aristas poseen pesos, que representan el coste que supone cada tramo. El algoritmo de Dijkstra evalúa, desde el nodo inicial, el coste invertido en ir a cada uno de los nodos adyacentes, y se desplaza al que menor coste acumulado suponga, marcándolo como permanente y repitiendo el proceso a partir del mismo. De esta forma, en cada paso se desplaza al nodo adyacente que presente menor coste acumulado, si no se ha marcado previamente como permanente. Cuando un nodo ha sido marcado como permanente, quiere decir que se ha encontrado el camino de menor coste desde el nodo inicial hasta dicho nodo. De esta forma, el algoritmo de Dijkstra es capaz de encontrar el camino de menor coste desde el nodo inicial hasta cada uno de los nodos del grafo. El algoritmo de Dijkstra siempre encuentra el camino óptimo entre dos nodos, siempre que exista un camino posible. Sin embargo, la principal limitación de este método es que no permite que las aristas tengan coste negativo Algoritmo A* El algoritmo A* también se presenta como un método de resolución de grafos, ya que busca el camino óptimo entre el nodo inicial y un nodo marcado como objetivo final. Al igual que el algoritmo de Dijkstra, éste también recibe la información mediante un grafo, cuyas aristas tienen asociados unos costes. Sin embargo, la principal diferencia con el algoritmo de Dijkstra es que A* no sólo tiene en cuenta los costes de las aristas para encontrar la solución, sino que incluye un factor heurístico que le permita orientar la búsqueda hacia el objetivo final, reduciendo así el número de nodos visitados y por tanto el tiempo de computación. Este factor heurístico supone una estimación de la distancia a la que se encuentra el nodo visitado en cada momento, respecto al nodo final. De esta forma se orienta la búsqueda del camino óptimo teniendo en cuenta la posición del objetivo final, para acercarse a él en la medida de lo posible. Se evita así visitar nodos innecesarios, lo que, en grafos muy grandes, supone un ahorro de tiempo considerable respecto al algoritmo de Dijkstra, en el que a menudo se visitan demasiados nodos. Por contraposición, la calidad de la solución depende de la estimación de la distancia al nodo objetivo que realice la componente heurística. De esta forma, si fuera posible estimar perfectamente la distancia al nodo final, la solución encontrada siempre sería óptima. Sin embargo esto no es posible, por lo que existe una posibilidad de que la solución obtenida no sea exactamente la más óptima posible. Aun así, en caso de no encontrar la solución óptima, el algoritmo A* suele encontrar una solución muy similar a la óptima, por lo que a menudo resulta rentable utilizar este método Bellman-Ford El algoritmo de Bellman-Ford es un algoritmo que permite obtener el camino más corto desde un nodo inicial hacia el resto de nodos de un grafo. Para ello, el algoritmo necesita tomar como entrada un grafo donde sus aristas estén representadas por pesos. Es habitual comparar este algoritmo con el algoritmo de Dijkstra, ya que existen similitudes entre ambos. Sin embargo, la principal diferencia radica en que el algoritmo de Bellman- Ford permite que las aristas tengan peso negativo, cosa que el algoritmo de Dijkstra prohibía, debido a que esto podía inducirle a error. El algoritmo de Bellman-Ford es capaz de identificar ciclos de coste negativo, y 9

24 Planificación global de trayectorias para robots móviles, basada en curvas β-spline por ello permite que las aristas posean peso negativo. En la Figura 2.6 se muestra un ejemplo de ciclo de coste negativo. [13] Figura 2-6 Representación de un ciclo de coste negativo El algoritmo de Dijkstra no permite identificar ciclos de coste negativo, ya que, una vez visitado el nodo 2 en la Figura 2.6. se marcaría como evaluado y no contemplaría la posibilidad de ir al nodo 3 y volver. Esta es la razón de que Dijkstra no acepte costes negativos para las aristas. Por contraposición, el tiempo empleado en resolver el grafo suele ser mayor en el caso de Bellman-Ford, comparando éste con el algoritmo de Dijkstra, ya que contemplar la posibilidad de la existencia de ciclos con coste negativo resulta computacionalmente más costoso. Por ello, este método suele quedar reservado para cuando existen aristas con costes negativos Algoritmo D* El algoritmo D* es un método de planificación que pretende encontrar el camino óptimo desde el punto actual hasta un punto objetivo, cuando se desconoce parcial o totalmente la información del terreno. Para ello, realiza inicialmente alguna suposición sobre la zona desconocida del terreno, como por ejemplo que no contiene ningún obstáculo, y calcula un camino óptimo para llegar al punto final basándose en esta suposición. El robot comienza a seguir dicho camino, y cuando encuentra nueva información del mapa que antes era desconocida, la añade a la información que ya tenía y, si es necesario, recalcula un nuevo camino óptimo desde el punto actual hasta el objetivo final. El proceso se repite una y otra vez, hasta que se alcanza el objetivo final, o hasta que se verifica que dicho punto no puede ser alcanzado. Durante la navegación en un terreno semi-desconocido es habitual encontrar nuevos obstáculos que se desconocían inicialmente, lo que obliga a recalcular un camino óptimo desde el punto actual, por lo que el algoritmo de búsqueda del nuevo camino debe ser rápido para no entorpecer la navegación. Este método de búsqueda se utiliza habitualmente en mapas desconocidos o semi-desconocidos. Por tanto, no se utilizará en el método presentado en este trabajo, ya que se trabajará con entornos conocidos. 10

25 2.5. Búsqueda en profundidad Un algoritmo de búsqueda en profundidad, o Deep-First Search, es un algoritmo que visita todos los nodos de un grafo de forma ordenada. Para ello, se comienza en el nodo inicial, al que se considera con índice 1, y se marca como nodo activo. Se evalúan todos los nodos vecinos y se visita el nodo adyacente que tenga menor índice. Se marca ahora dicho nodo como nodo activo, y se repite el proceso hasta que todos los vértices adyacentes al nodo activo hayan sido visitados. En ese caso, se retrocede al nodo anterior, y se continúa con el proceso, buscando en cada caso el nodo adyacente al activo que no haya sido visitado. En la Figura 2.7 Se muestra un posible orden de visita de los nodos, desde el nodo inicial.[14] Figura 2-7 Posible orden de visita de los nodos en una búsqueda en profundidad De esta forma, el algoritmo visita en cada paso el nodo adyacente no visitado que tenga un índice menor, y cuando todos los nodos adyacentes hayan sido visitados, repite el proceso con el nodo anterior. El concepto de índice tiene sentido si se quiere seguir un orden concreto a la hora de visitar los nodos vecinos, pero habitualmente estos algoritmos escogen el índice por sí mismos, de forma que visitan el primer nodo vecino no visitado que localizan. Esta aclaración no tiene trascendencia, ya que el algoritmo no acaba hasta que no se han visitado todos los nodos Búsqueda en amplitud Al igual que el algoritmo de búsqueda en profundidad, un algoritmo de búsqueda en amplitud o Breadth-First Search, es un algoritmo que visita todos los nodos de un grafo de forma ordenada. Sin embargo, sigue una estrategia distinta que el método anterior a la hora de seleccionar qué nodo visitar en cada caso. Empezando en el nodo inicial, se visitan uno por uno todos los nodos adyacentes. Una vez visitados todos los vecinos, se visitan los vecinos de éstos, y así sucesivamente hasta que se hayan visitado todos los nodos. En la Figura 2.8 se muestra un posible orden de visita de los nodos en un algoritmo de búsqueda en amplitud.[15] 11

26 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 2-8 Posible orden de visita de los nodos en una búsqueda en anchura Viendo la Figura 2.8 Resulta evidente la diferencia respecto a los algoritmos de búsqueda en profundidad. Mientras que los algoritmos de búsqueda en profundidad siempre avanzan a un vecino del nodo actual, y cuando es imposible vuelven sobre sus pasos, los algoritmos de búsqueda en anchura visitan todos los nodos adyacentes al actual, y posteriormente visitan todos los vecinos de los vecinos, y así sucesivamente. 12

27 3 DIAGRAMAS DE VORONOI Y MODELADO DE OBSTÁCULOS E ste capítulo describe el método utilizado en este trabajo para la elaboración de diagramas de Voronoi. El objetivo último es la obtención de una trayectoria segura entre un punto inicial y otro punto final, para lo cual necesitamos encontrar un camino libre de obstáculos entre ambos puntos. El método está orientado a entornos muy poblados de obstáculos, por lo que el factor determinante será la seguridad de los posibles caminos encontrados, entendiendo seguridad como una distancia a los obstáculos mayor que una distancia de seguridad dada. Por tanto los caminos calculados deberían estar lo más alejados de los obstáculos. Los diagramas de Voronoi se elaboran para modelar los obstáculos. Primero se introduce el concepto de diagramas de Voronoi y después se describe el proceso de modelado de obstáculos. Finalmente, este capítulo acabará con la obtención de un grafo de posibles caminos, obtenido a partir del mapa en el que queramos que se mueva nuestro robot móvil Introducción La planificación consiste en la búsqueda de un camino libre de obstáculos entre un punto inicial y otro final. Para obtenerlo, necesitamos utilizar la información que tenemos del entorno, con el fin de obtener una red de posibles caminos seguros. El concepto de camino (path en inglés) no implica suavidad ni que éste sea cinemáticamente realizable por un robot móvil, sino que se entiende como una sucesión de puntos o waypoints y líneas en un grafo, que nos permiten simplemente conectar el punto inicial con el punto final sin interceptar ningún obstáculo. Es por tanto un primer enfoque puramente geométrico del problema que abordamos. Para obtener este camino, calculamos una sucesión de puntos intermedios conectados entre sí, que permitan ir del punto inicial al final de forma segura. Es en el modo de obtención de estos puntos intermedios donde aparece la primera decisión importante que se debe tomar, ya que se podrían seguir diferentes estrategias para obtenerlos, según los intereses que se persigan. En nuestro caso, vamos a trabajar con escenarios bastante poblados de obstáculos donde la seguridad es el objetivo principal. Por ello, el método de planificación elegido se basa en diagramas de Voronoi, que aseguran en todo momento pasar lo más alejado posible de los obstáculos. Este capítulo muestra una estrategia de modelado de los obstáculos a partir de un diagrama de Voronoi que nos permita evaluar de forma realista si un determinado camino es realizable o no, así como evitar puntos de paso innecesarios. 13

28 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Por otro lado, se consideran ciertas simplificaciones, como son las de robot móvil puntual y omnidireccional, es decir, que no tendremos en cuenta de momento las dimensiones del robot móvil en cuestión, ni las limitaciones cinemáticas que pudiera tener. En el capítulo cuatro se presentarán estrategias para tener en cuenta estas limitaciones, y que proporcionarán al método la flexibilidad necesaria para que sea válido con cualquier robot móvil y en cualquier entorno Diagramas de Voronoi Desde un punto de vista formal, un diagrama de Voronoi podría definirse como el lugar geométrico de los puntos del plano que equidistan en todo momento de los dos obstáculos más cercanos. Supone, por tanto, una estrategia totalmente opuesta a los grafos de visibilidad, que trataban de pasar lo más cerca posible de los obstáculos sin interceptarlos. La característica principal de los diagramas de Voronoi es que el grafo que generan los vértices calculados da lugar a una ruta lo más alejada posible de los obstáculos Construcción de un diagrama de Voronoi La elaboración de un diagrama de Voronoi se realiza de la siguiente forma: Suponemos dados un conjunto finito de puntos {p 1,,p n } en el plano P, con n 2, y a cada p j le asociamos aquellos puntos del plano que están más cerca o igual suya que de cualquier otro de los p i con i distinto de j. Todo punto del plano queda así asociado a algún punto p i, existiendo algunos puntos que equidistan de dos o más de los puntos del conjunto P. Son precisamente estos puntos equidistantes los que conforman el diagrama de Voronoi (ver Figura 2.5).[16] Por lo tanto, los diagramas de Voronoi dividen el plano en regiones, llamadas regiones de Voronoi, que están formadas por los puntos que están más cerca de algún punto p i que de cualquier otro. Sin embargo, para nuestra aplicación nos interesan precisamente las líneas que aparecen en el diagrama, es decir, las líneas equidistantes de los dos puntos más cercanos en cada caso. Suponiendo que el conjunto {p 1,,p n } reflejara en realidad la posición de los obstáculos, las líneas del diagrama reflejarían las posiciones más seguras, ya que estarían a la mayor distancia posible de los mismos. Los puntos de intersección de las distintas líneas del diagrama son los llamados vértices de Voronoi, que son los puntos que equidistan de los tres obstáculos más cercanos (ver Figura 2.5). Precisamente estos vértices son los que usaremos como puntos de paso intermedios para llegar del punto inicial al punto final, mediante unos algoritmos de planificación que elijan cuáles serán los vértices que conformen la secuencia a seguir para conectar los puntos inicial y final de forma óptima. Sin embargo, los puntos inicial y final de la trayectoria que deseamos realizar no tienen por qué pertenecer a alguna de las líneas del diagrama de Voronoi y de hecho, en la inmensa mayoría de casos, no pertenecerán al mismo. Es necesario, por tanto, conectar de algún modo los puntos inicial y final al diagrama, para lo cual podemos seguir distintas estrategias. En nuestro caso, lo que hacemos es conectar los puntos inicial y final a 14

29 los cinco vértices del diagrama de Voronoi más cercanos, siempre que no se intercepte ningún obstáculo y que haya suficiente número de vértices. El motivo de conectar dichos puntos a los cinco vértices más cercanos, es simplemente elegir una cantidad de conexiones adicionales que no sea demasiado pequeña, y que tampoco añada demasiado tiempo de cómputo. En cualquier caso, en entornos muy complicados en los que 5 conexiones son insuficientes, podemos añadir algunas más Problemas al crear el diagrama de Voronoi El problema más evidente del método de elaboración del diagrama de Voronoi expuesto hasta ahora, es la limitación de que los obstáculos no siempre podrán ser considerados puntuales. Normalmente habrá que tener en cuenta sus dimensiones de alguna manera. Se hace necesario por tanto el uso de alguna estrategia de modelado, lo cual no es un problema en absoluto trivial. Algunas estrategias simples consisten en considerar los obstáculos como un punto situado en el centro del mismo, pero esto supone una consideración demasiado arriesgada, ya que si no tenemos en cuenta el tamaño del obstáculo nada nos asegura que no se producirá una colisión, por lo que el uso de diagramas de Voronoi perdería su sentido, ya que el objetivo es precisamente obtener una trayectoria lo más segura posible. Otra posible opción sería modelar los obstáculos mediante polígonos en un primer paso, y posteriormente quedarnos únicamente con los vértices de los mismos. Esta estrategia proporciona una solución mejor que la anterior, pero puede que siga prohibiendo un gran número de trayectorias que en realidad serían posibles. Otro aspecto que debemos conseguir con el modelado es la equidistancia real del diagrama de Voronoi respecto a los obstáculos, en la medida de lo posible. Esto no siempre es fácil, ya que si nuestro obstáculo es un polígono, tendremos que elegir cuidadosamente qué puntos de su perímetro definen el obstáculo para conseguir este objetivo, ya que la entrada de los programas que calculan el diagrama de Voronoi son coordenadas de puntos, no rectas. Por tanto, es evidente la necesidad de elaborar una estrategia de modelado, que se expondrá en el siguiente apartado Modelado de obstáculos Como se ha comentado en la sección anterior, es necesario seguir una estrategia a la hora de modelar los obstáculos, ya que la elaboración de los diagramas de Voronoi toma como entrada las coordenadas de puntos que representarán nuestros obstáculos. Sin embargo, los obstáculos en la realidad no serán puntuales, por lo que es necesario elegir un criterio para modelarlos con varios puntos, con el objetivo de seguir conservando las propiedades de equidistancia de los diagramas de Voronoi. En este trabajo se consideran entornos conocidos y estáticos. El entorno es dividido en celdas y es definido por una matriz de 0 y 1, donde un 1 en la componente i,j de la matriz significará que hay un obstáculo en la fila i, columna j, y un 0 significará ausencia de obstáculo. Se desprende de esta forma de modelar el entorno, que los obstáculos vendrán representados por polígonos, cuyas aristas serán horizontales o verticales. En la figura 3.1 se muestra uno de los mapas que consideraremos a lo largo de este trabajo. 15

30 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 3-1 Representación de los obstáculos de un mapa Para obtener el diagrama de Voronoi, escogeremos en primer lugar los puntos que nos interesen de los obstáculos y, a continuación, filtraremos el diagrama resultante para eliminar las líneas que intercepten a los mismos, de forma que finalmente obtengamos un diagrama de Voronoi seguro y fiable. En los siguientes apartados se muestra el proceso de selección de puntos en los obstáculos Selección de vértices En primer lugar, seleccionamos los vértices de los polígonos que representan nuestros obstáculos. Este es un primer paso necesario, ya que nos dará una idea de las dimensiones de los obstáculos, y alertará de la posición de las esquinas, que suelen ser los puntos más conflictivos a la hora de evitar colisiones. Desde el punto de vista de la forma de identificación de los vértices, nos apoyaremos en el conocimiento de que el mapa está representado por una matriz. Entendiendo una matriz como una rejilla o cuadrícula, un determinado elemento estará rodeado por otros ocho elementos, a no ser que se encuentre en alguno de los límites del mapa, pero esa situación correspondería a un caso especial. La Figura 3.2 muestra una pequeña rejilla de 16 elementos, donde las casillas sombreadas indican que forman parte de un obstáculo, por lo que aparecería un 1 en dichas posiciones de la matriz correspondiente. Esta rejilla de 16 elementos se corresponde con un vértice de un obstáculo, situado en la posición (2,2), y los elementos que lo rodean. En realidad, para cada elemento de la matriz sólo nos interesa una rejilla de 9 elementos, de tal forma que incluya al elemento en cuestión, y a los 8 elementos circundantes. Sin embargo, en la Figura 3.2 se han representado también algunos elementos más, con el fin de demostrar que sólo el elemento situado en el vértice cumplirá las condiciones que se expondrán a continuación. 16

31 Figura 3-2 Rejilla de 16 elementos De la Figura se desprende fácilmente que la forma de identificar un vértice es, en primer lugar, comprobar que hay un 1 en la posición correspondiente de la matriz, para asegurarnos de que pertenece a un obstáculo y, posteriormente, que la suma de las componentes de la matriz en las ocho posiciones circundantes es igual a 3. Vemos cómo, en el caso de la figura 3.2, el cuadro correspondiente a la posición (2,2) está sombreado (pertenece a un obstáculo, hay un 1 en dicha posición de la matriz), y además hay tres de los ocho cuadros adyacentes que también pertenecen a dicho obstáculo, por lo que se verifica que la suma de los 8 cuadrados o celdas circundantes a la posición que estamos estudiando en la matriz es igual a 3. Comprobamos también que esta condición sólo se cumple cuando efectivamente se trata de un vértice. A continuación, la Figura 3.3 muestra los resultados obtenidos de aplicar este modo de identificación de vértices a un mapa sencillo, de sólo tres obstáculos. Los vértices seleccionados aparecen representados con un círculo verde. Figura 3-3 Mapa con tres obstáculos Además, como medida de delimitación del espacio, colocamos 4 vértices adicionales en las esquinas del mapa, para tener una idea del espacio del que disponemos. Sin embargo, el modo de identificación de vértices expuesto hasta ahora sólo sirve para identificar los vértices 17

32 Planificación global de trayectorias para robots móviles, basada en curvas β-spline exteriores de los obstáculos. Es posible también que éstos tengan vértices interiores, como es el caso del mapa mostrado en la Figura 3.1. Ilustraremos este caso con la matriz de 4x4 representada en la Figura 3.4: Figura 3-4 Matriz de 4x4 La Figura 3.4 representa un vértice interior, así como algunos de los elementos que lo rodean. La identificación de esté vértice interior no entraña más dificultad que en el caso de un vértice exterior, sino que en este caso, además de pertenecer a un obstáculo (1 en la posición de la matriz correspondiente), la suma de los 8 elementos circundantes debe ser igual a 7. Se comprueba que, siguiendo este criterio, el vértice interior estaría en la posición (3,3) de la matriz de la Figura 3.4. A modo de resumen, el proceso de identificación de los vértices se sintetiza en el cumplimiento de los siguientes pasos: -Cargar el mapa. -Añadir las esquinas. -Para cada punto, sumar las componentes de la matriz en las 8 posiciones colindantes. -Si la suma es 3 ó 7, y el punto pertenece a un obstáculo, añadirlo. En resumen, para identificar los vértices de los obstáculos, simplemente debemos comprobar que el punto en cuestión pertenece a un obstáculo, y que la suma de los 8 elementos circundantes en la matriz es igual a 3 (vértice exterior) o a 7 (vértice interior). La Figura 3.5 muestra los resultados de aplicar este criterio al mapa de la Figura 3.1, que es relativamente denso en cuanto a obstáculos. 18

33 Figura 3-5 Representación de los obstáculos y sus vértices Sin embargo, el modelado de los obstáculos únicamente mediante sus vértices no es suficientemente preciso, ya que se seguirán prohibiendo muchos caminos que en realidad si serían posibles, y la equidistancia a los obstáculos no tiene por qué cumplirse. De hecho, la Figura 3.6 muestra el diagrama de Voronoi obtenido si modelamos únicamente con los vértices en el mapa de la Figura 3.3, que a priori es un mapa muy poco exigente. Figura 3-6 Diagrama de Voronoi modelando sólo con los vértices En la Figura 3.6 se ve claramente cómo las líneas del diagrama de Voronoi sólo son equidistantes de los obstáculos cuando estos se encuentran alineados, cosa que por norma general no se cumplirá. De hecho, vemos que las líneas no son paralelas a los límites del mapa, lo cual sería deseable. Cabe destacar que, dentro de lo que cabe, el resultado obtenido con este mapa no es excesivamente pobre debido a que los obstáculos son muy pocos, y están todos alineados. No es por tanto difícil imaginar que el diagrama de Voronoi obtenido del mapa de la Figura 3.5 carecerá absolutamente de valor. Se hace por tanto necesario seguir avanzando en las 19

34 Planificación global de trayectorias para robots móviles, basada en curvas β-spline técnicas de modelado Proyección de los vértices Según la propia definición de diagrama de Voronoi, las líneas del mismo se encontrarán equidistantes a los dos puntos más cercanos. Por otra parte, queremos que estas líneas sean paralelas a los obstáculos, y evidentemente también equidistantes. Además, también se hace patente la necesidad de situar algunos puntos en los límites de mapa, para delimitar el espacio del que disponemos. Sin embargo, la posición en la que situemos los puntos en los límites del mapa delimitará las características del diagrama de Voronoi. En la Figura 3.7 se muestra un ejemplo de los problemas que podemos encontrar si no seguimos una estrategia adecuada a la hora de colocar los puntos en los límites del mapa. Figura 3-7 Diagrama de Voronoi tras un mal modelado En la Figura 3.7 los puntos en los límites del mapa se han colocado simplemente a una distancia constante entre ellos. Como se desprende de la misma, esta estrategia no es adecuada, ya que las líneas del diagrama de Voronoi no son en absoluto paralelas a los obstáculos, e incluso en este caso interceptan al mismo. Se hace necesario por ello el uso de una estrategia alternativa. Todos los problemas y necesidades expuestas anteriormente se pueden solucionar si simplemente proyectamos los puntos que ya tenemos, sobre los límites del mapa y sobre el resto de obstáculos. Al ser las aristas de los obstáculos siempre horizontales y verticales (esto se debe al concepto de mapa como cuadrícula), los vértices también se proyectarán horizontal y verticalmente, sobre el obstáculo o límite más cercano. Sin embargo, no siempre es necesario proyectar absolutamente todos los vértices, ya que a veces esta proyección carece de utilidad. No hay que perder de vista que el motivo de proyectar un vértice, es forzar que la línea del diagrama de Voronoi sea paralela a los obstáculos, y dicha línea es el lugar geométrico de los puntos del plano que equidista de los dos puntos más cercanos. Por tanto, si al proyectar algún vértice v a comprobamos que existe algún otro vértice v b perteneciente a otro obstáculo distinto a una distancia menor, la línea del diagrama no será equidistante al vértice v a y a su proyección, sino a la proyección de v a y al vértice v b. Este sería el caso que se muestra en la Figura 3.8: 20

35 Figura 3-8 Caso en el que proyectar sería inútil En el caso de la Figura 3.8, no tendría sentido proyectar el vértice v a según la línea verde, ya que la distancia entre vértice y proyección sería mayor que la de la línea amarilla, que conecta dicha proyección con el vértice v b. Por tanto, la línea del diagrama de Voronoi sería perpendicular a dicha línea amarilla, y no conseguiríamos el objetivo de que fuera paralela a los obstáculos. Puede parecer una trivialidad, pero el ahorro de puntos innecesarios es un aspecto importante, de cara a evitar la complejidad computacional al generar el diagrama de Voronoi. De la misma manera que proyectamos los vértices sobre los límites del mapa, podemos proyectarlos sobre otros obstáculos, para conseguir un diagrama de Voronoi equidistante de los obstáculos. Aplicando este segundo paso en el modelado de los obstáculos, obtenemos el siguiente resultado en la Figura 3.9: Figura 3-9 Diagrama de Voronoi tras proyectar los vértices Vemos cómo, efectivamente, las líneas del diagrama de Voronoi son paralelas a los obstáculos y equidistantes respecto a los mismos. El siguiente paso consiste simplemente en eliminar las líneas que interceptan a algún obstáculo o se salen del mapa, tras lo cual se obtiene el siguiente diagrama final, representado por las líneas verdes: 21

36 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 3-10 : Diagrama de Voronoi resultante Vemos que se ha conseguido el objetivo de crear un diagrama de Voronoi perfectamente seguro, y equidistante a los obstáculos. El proceso de proyección de los vértices puede resumirse en el seguimiento de los siguientes pasos: -Para cada vértice, proyectarlo horizontal y verticalmente, hasta que encontremos un obstáculo o un límite del mapa. Almacenamos la distancia entre vértice y proyección. -Para cada proyección, comprobar si hay algún vértice distinto del proyectado, situado a una distancia menor, y que no pertenezca al obstáculo o límite sobre el que se encuentra la proyección. -Si no hay ningún vértice así, añadimos la proyección a la colección de puntos. Si lo hay, desechamos la proyección y no la añadimos Añadido de puntos adicionales Tras los dos pasos expuestos en los apartados anteriores, la Figura 3.10 muestra que se alcanza el objetivo buscado. Sin embargo, es posible que en entornos más poblados de obstáculos sea necesario añadir algunos puntos más. Si aplicamos la estrategia expuesta hasta ahora a un mapa más poblado, obtenemos el siguiente resultado: 22

37 Figura 3-11 Diagrama de Voronoi tras los dos pasos expuestos En la Figura 3.11 queda de manifiesto que a veces, en entornos con una gran densidad de obstáculos, a pesar de haber proyectado los vértices no siempre obtenemos una línea del diagrama de Voronoi que sea equidistante a los mismos. Concretamente, en dicha figura aparecen dos casos del problema que se expone, (ver cuadros amarillos en la Figura 3.11). El caso de la izquierda es especialmente peligroso, ya que la deformación del diagrama provoca que se salga de los límites del mapa, por lo que, al realizar el filtrado de líneas posterior, se prohibirá pasar por un tramo que en es seguro. A continuación, procedemos a analizar cuándo se produce esta deformación del diagrama y a qué se debe, para así poder encontrar una solución. Para ello, tomaremos como ejemplo el comentado caso de la izquierda. Una vez más, debemos recordar que nuestro objetivo es crear un diagrama de Voronoi que sea equidistante a los obstáculos, y paralelo a los mismos. Además, recordamos que las líneas del diagrama de Voronoi son equidistantes a los dos puntos más cercanos. La Figura 3.12 muestra la línea blanca que representa la línea del diagrama de Voronoi que desearíamos obtener para cumplir los requisitos mencionados anteriormente. Para ello, en cualquier punto de la misma, la distancia a los vértices superior o inferior izquierdo (o a sus respectivas proyecciones sobre el límite del mapa) debería ser menor que a cualquier otro punto, representados por un círculo verde en todas las imágenes. Sin embargo, se aprecia en la imagen que en el punto central, la distancia a dichos vértices superior e inferior izquierdo, representada por la línea verde, es mayor que la distancia a un tercer punto, representada por la línea amarilla. Este tercer punto es resultado de una de las proyecciones de vértices cuya necesidad demostrábamos en el apartado anterior de este mismo capítulo. Hemos analizado dichas distancias en el punto correspondiente a la mitad de la altura del obstáculo, debido a que en el mismo la distancia a los dos vértices más cercanos es la mayor posible, convirtiéndose este punto en el más conflictivo del tramo. 23

38 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 3-12 Situación en la que existe deformación de una línea del diagrama Tras el análisis realizado en el párrafo anterior, podemos obtener alguna regla general que nos permita prever cuándo se producirá una deformación indeseada del diagrama de Voronoi, con el fin de evitarlo. Podemos concluir que, si desde el punto de la línea del diagrama de Voronoi deseada correspondiente a la mitad de la altura del obstáculo se encuentra algún vértice o proyección de vértice a una distancia menor que la distancia entre dicho punto central y el vértice que hemos proyectado para obtener la línea, se producirá una deformación de la misma. Esta engorrosa definición se traduce en la Figura 3.12 en la existencia de una línea amarilla más corta que la línea verde. Para esclarecer esta definición, podemos apoyarnos en expresiones matemáticas: Si suponemos que el vértice superior izquierdo del obstáculo, situado en unas coordenadas (v x,v y ), se encuentra a una distancia d del límite del mapa, y el obstáculo al que pertenece tiene una altura h, podemos deducir que el punto más conflictivo será el punto P de coordenadas (v x - d/2, v y + h/2). Por tanto, la distancia entre los puntos P y V será la norma del vector que los une, de modulo: L = ( d 2 )2 + ( 2 )2 ( 3-1 ) De esta forma, sabremos que se producirá una deformación indeseada cuando se encuentre otro vértice o proyección de vértice V tal que P-V < L, es decir, cuya separación del punto P sea menor que la ya definida distancia L. Como se ha comentado anteriormente, si esta restricción se cumple en el llamado punto P, se cumplirá en cualquier otro punto de la línea correspondiente, ya que éste es el punto más conflictivo. Una vez que hemos identificado cuándo aparece este problema, resulta inmediato inferir que la solución al mismo es simplemente colocar otro punto adicional entre los dos vértices correspondientes del obstáculo (en el caso concreto que estamos ejemplificando, entre los vértices superior e inferior izquierdos). Además, hay que proyectar dicho vértice adicional de la misma forma que se exponía en el apartado Una vez finalizado este proceso, mediante el cual añadimos los puntos adicionales necesarios, procederemos a mostrar los resultados obtenidos al generar el diagrama de Voronoi tomando como entrada los puntos de los obstáculos que hemos ido seleccionando a lo largo de este capítulo. 24

39 Figura 3-13 Diagrama de Voronoi tras el modelado A simple vista puede parecer un diagrama de Voronoi un poco caótico, pero si realizamos el correspondiente filtrado de los tramos que interceptan a algún obstáculo, obtendremos el resultado que se muestra en la Figura En la misma, queda demostrado que, aunque se trate de un mapa con una considerable densidad de obstáculos, si seguimos los tres pasos expuestos en esta sección, podemos obtener un diagrama de Voronoi que representa perfectamente una red de los posibles caminos equidistantes a los obstáculos, y paralelos a los mismos. Por otro lado, las consideraciones que hemos tenido en cuenta aseguran la no inclusión de puntos innecesarios, que ralentizarían la elaboración del diagrama y no supondrían ninguna ventaja. Figura 3-14 Diagrama de Voronoi resultante El proceso de comprobación de la necesidad de añadir algún vértice adicional se resume en el seguimiento de los siguientes pasos: 25

40 Planificación global de trayectorias para robots móviles, basada en curvas β-spline -Para cada vértice, medimos la distancia en X y en Y al siguiente vértice del mismo obstáculo. A la mayor de las dos magnitudes la llamamos h. -Desde el vértice que nos ocupa, medimos la distancia al siguiente obstáculo o límite, en la dirección perpendicular a la que se ha medido h. A esta distancia la llamamos d. -Calculamos las coordenadas del punto P, situado respecto al vértice que nos ocupa a una distancia d/2 en la dirección en la que se ha medido d, y a h/2 en la dirección en la que se ha medido h. -Comprobamos si hay algún punto a una distancia de P menor que el vértice que nos ocupa. -Si la respuesta es no, no se producirá deformación indeseada del diagrama. Si la respuesta es sí, colocamos un punto adicional sobre el obstáculo, a una distancia h/2 del vértice que estamos comprobando, así como su correspondiente proyección en la dirección de d. Cabe destacar que sólo usamos en los cálculos de este último bloque la mayor de las magnitudes del obstáculo, que llamamos h, ya que este fenómeno sólo puede ocurrir cuando hay una arista mucho mayor que la perpendicular. Por ejemplo, en obstáculos cuadrados nunca ocurriría. Una vez realizados estos pasos, ha quedado de manifiesto a lo largo del capítulo que se consigue el objetivo de conseguir un diagrama de Voronoi fiable. Lo que se busca con este método, al igual que con el trabajo en su totalidad, es la flexibilidad del mismo, para que sea capaz de adaptarse a cualquier mapa que se introduzca en forma de matriz, y efectivamente encuentre un diagrama de Voronoi seguro, sin importar la cantidad de obstáculos que haya. 26

41 4 SELECCIÓN DEL CAMINO Y SUAVIZADO CON CURVAS BETA-SPLINE E ste capítulo expone el método de obtención de trayectorias seguras y realizables entre un punto inicial y otro final. Para ello, partimos de los diagramas de Voronoi obtenidos en el capítulo anterior, a partir de los cuáles encontraremos el camino que conecta el punto inicial con el final de la trayectoria deseada de forma óptima. También se introducen parámetros que nos permiten eliminar las consideraciones de robot puntual y omnidireccional, así como la posibilidad de obtener trayectorias para distintos robots móviles, simplemente cambiando el valor de dichos parámetros y adecuándolos para cada robot en concreto. Los algoritmos de planificación utilizados son los llamados algoritmos de Dijkstra y de A*. Una vez seleccionado el camino óptimo a seguir, se procede al suavizado del mismo, de tal forma que finalmente obtengamos una curva suave que sea perfectamente realizable por el robot móvil. Para la obtención de la curva suave a partir del camino seleccionado, hacemos uso de las curvas β-spline, cuyas propiedades también se exponen en este capítulo Introducción A partir del diagrama de Voronoi generado (ver capítulo3), generamos una matriz, llamada matriz de costes, que refleja el coste invertido en ir de un vértice a otro del diagrama de Voronoi. El coste puede reflejar simplemente la distancia recorrida entre los nodos correspondientes, o puede también depender de la distancia a los obstáculos. Una vez construida la matriz de costes, ésta será la entrada del programa que calculará el camino óptimo, mediante los algoritmos de planificación de Dijkstra o A*. Una vez calculada la sucesión de waypoints que conforman el camino óptimo mediante una de estas herramientas, hay que proceder al suavizado del mismo. En nuestro caso, el suavizado supone el paso de camino a trayectoria. Como se ha comentado previamente, el camino es simplemente una sucesión de líneas que unen los waypoints por los que queremos pasar, pero no tiene por qué ser realizable cinemáticamente por un robot móvil que no sea omnidireccional, ya que un camino, por su propia definición, sólo debe ser continuo en posición. En cambio, el concepto de trayectoria implica una serie de propiedades que hacen que sea cinemáticamente posible reproducirla con un robot móvil. Las trayectorias se obtienen a partir de los caminos, normalmente mediante diversas técnicas de interpolación que aseguren el paso por los waypoints, pero que eviten las discontinuidades en la posición y sus derivadas 27

42 Planificación global de trayectorias para robots móviles, basada en curvas β-spline temporales. La Figura 4.1 muestra la diferencia entre camino, representado con línea discontinua roja, y trayectoria (línea continua negra). Figura 4-1 Diferencia entre camino y trayectoria, y separación entre ellos La Figura 4.1 muestra que el camino es simplemente la unión de los waypoints (representados en azul), mientras que la trayectoria, representada por la línea negra, consiste en una curva suave que nos permite interpolar a dichos puntos de paso. En este trabajo se obtienen las trayectorias mediante curvas β-spline que interpolan a los waypoints. La Figura 4.1 también muestra que, a veces, la separación entre trayectoria y camino es considerable, como indican las líneas verdes. Esta separación podría causar en algún caso la violación de la distancia de seguridad para algún tramo, por lo que, una vez obtenido el camino que conecta los puntos inicial y final, hay que considerar dicha separación para comprobar que no se viola la distancia de seguridad. A través de la distancia de seguridad que debemos mantener con los obstáculos eliminamos la restricción de robot puntual. Para eliminar la restricción de robot omnidireccional, hacemos uso de las especificaciones del robot y de la velocidad constante a la que queramos movernos. En este capítulo se expone la estrategia seguida para, en caso necesario, adaptar el camino según las características del robot. Por otra parte, las curvas β-spline aproximan a unos puntos llamados vértices del polígono de control. En este capítulo se explica el proceso de obtención de dichos vértices, que se colocarán sobre una serie de circunferencias y tangentes, de forma que la curva final interpole a los waypoints seleccionados. 28

43 4.2. Algoritmos de planificación En este trabajo se utilizan los algoritmos de Dijkstra y de A*para obtener el camino más corto a partir del diagrama de Voronoi calculado. Ambos algoritmos toman como entrada una matriz de costes que refleja el coste que supone ir del nodo a al nodo b del diagrama de Voronoi, suponiendo que éstos estén conectados en el diagrama, y devuelven como salida la sucesión de nodos o waypoints que conectan los puntos inicial y final con el menor coste posible Construcción de la matriz de costes Este trabajo se ha implementado en Matlab, programa que ya tiene implementada una función para mostrar los diagramas de Voronoi. Dicha función, llamada voronoi, toma como argumentos de entrada las coordenadas de los puntos, que para nosotros son obstáculos, y devuelve como salida la representación gráfica del diagrama. Sin embargo, también existe la función voronoin, que toma las mismas entradas, pero devuelve por un lado las coordenadas de los nodos del diagrama de Voronoi (intercepción de las líneas del diagrama), y además una matriz que contiene los vértices que conforman cada una de las celdas de Voronoi. De esta forma, con esta función podemos saber qué nodos están conectados entre sí, y utilizar esta información para elaborar la matriz de costes. El primer paso después del uso de la función voronoin es la eliminación de los vértices o nodos del diagrama de Voronoi que están situados dentro de algún obstáculo, ya que nunca formarán parte del camino óptimo (provocan colisiones) y sólo aumentarán el tiempo de cómputo. Suponemos que, tras este filtrado, nos quedamos con el conjunto de nodos {v 1,,v n }, formado por los n nodos del diagrama de Voronoi que no pertenecen a ningún obstáculo. La matriz de costes, que llamaremos D, será una matriz cuadrada de tamaño n+2 x n+2, donde todos los elementos tendrán inicialmente valor infinito, excepto los elementos de la diagonal, que valdrán 0. En esta matriz, la componente D i,j reflejará el coste que supone ir del vértice v i al v j, que será el mismo que D j,i. La matriz D es, por tanto, una matriz simétrica. Las dos filas y columnas adicionales corresponden a los puntos inicial (n+1) y final (n+2), y a sus posibles conexiones con el resto de vértices del diagrama de Voronoi. Inicialmente todos los elementos de la matriz D i,j, con i j, tienen valor infinito, lo que equivale a decir que los nodos correspondientes no están conectados entre sí. Procedemos ahora a calcular el coste de cada par de vértices, suponiendo que éstos estén conectados. La función voronoin devuelve, además de las coordenadas de los vértices del diagrama de Voronoi, la secuencia de vértices que conforman cada una de las celdas del diagrama, en forma de vectores. Se puede deducir de estos vectores qué vértices están unidos entre sí. Si sabemos que un vector está formado, por ejemplo, por la secuencia {3, 8, 14, 2, 27}, quiere decir que el vértice situado en la posición i está conectado con los situados en las posiciones i+1, e i-1, a no ser que i se corresponda con uno de los extremos del vector, en cuyo caso estaría conectado con el extremo opuesto. Esta deducción se muestra en la Figura 4.2: 29

44 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 4-2 Celda de Voronoi De esta forma, podemos saber qué nodos están conectados entre sí. Por tanto, el proceso para elaborar la matriz será el siguiente: Para cada D i,j : - Comprobar que i j. - Comprobar que v i está conectado con v j en el diagrama de Voronoi. - Si están conectados, calcular la distancia al obstáculo más cercano. - Si la distancia es mayor que la distancia de seguridad, calcular el coste y almacenarlo en D i,j =D j,i. - Si no se cumple la distancia de seguridad, o los vértices no están conectados, D i,j y D j,i siguen valiendo infinito. - Para calcular el coste, suponemos que tenemos los vértices v i y v j, conectados en el diagrama de Voronoi, y el punto p es el obstáculo más cercano a dicho tramo. La Figura 4.3 muestra esta situación. Figura 4-3 Distancia de un tramo a un obstáculo El primer paso para calcular la distancia entre el punto p y el segmento que une v i con v j, será definir este segmento de la siguiente forma: 30

45 w(σ) = (1 σ) v i + σ v j σ [0,1] ( 4-1) Definiendo el segmento w de esta forma [17], el valor del parámetro σ en el que la distancia entre el segmento w y el punto p es mínima es el siguiente: σ = (v 1 p) (v 1 v 2 ) T v 1 v 2 ( 4-2 ) Este σ * se corresponde con el valor de σ para el cual la línea que une w(σ * ) con el punto p es perpendicular al segmento que pasa por v i y v j (llamado segmento w). Por tanto, según el valor de σ *, la distancia entre el punto p y el vector que une v i con v j se calculará de forma diferente: Si σ * 0 d = p-v i Si σ * 1 d = p-v j Si 0 < σ * < 1 d = p - w(σ * ) ( 4-3 ) Repetimos este proceso para cada uno de los puntos que modelan nuestros obstáculos, y finalmente el valor de d será el menor de los obtenidos (distancia al obstáculo más cercano). Una vez obtenido d, el coste invertido en ir de v i a v j, y viceversa, se obtiene según la siguiente expresión: D i,j = K 1 v i v j + K 2 d ( 4-4 ) En la expresión anterior, el coste se calcula como una función de la separación entre ambos puntos, y de la distancia al obstáculo más cercano. La importancia de cada uno de estos dos factores en el coste final puede controlarse variando los parámetros K 1 y K 2, para dar más peso a la distancia recorrida o a la distancia a los obstáculos, respectivamente. Repitiendo este proceso, se rellena por completo la matriz de costes D, que nos permite encontrar el camino óptimo mediante Dijkstra o A*. 31

46 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Algoritmo de Dijkstra El algoritmo de Dijkstra es un algoritmo usado para la determinación del camino más corto desde un nodo origen de un grafo, al resto de nodos del mismo. Para ello, toma como entrada un grafo cuyas aristas poseen pesos. En otras palabras, el algoritmo de Dijkstra toma como entrada la matriz de costes. En la Figura 4.4 se muestra un grafo al que se podría aplicar el algoritmo de Dijkstra. En el mismo, están representados los pesos o costes correspondientes a cada una de las conexiones posibles. Figura 4-4 Grafo de 5 nodos, con pesos representados El algoritmo de Dijkstra evalúa, desde el nodo inicial, todos y cada uno de los nodos adyacentes para determinar el camino de mínimo coste para llegar a cada uno de los nodos. Por ello, siempre acaba encontrando el camino de mínimo coste desde un nodo inicial a otro final, ya que va expandiendo progresivamente todos los nodos, y almacenando el camino de mínimo coste en cada caso. Una restricción importante de este algoritmo es que los pesos de la matriz de costes deben ser positivos. La idea de este algoritmo es realizar iteraciones, desde el nodo origen hacia todos los adyacentes, marcando siempre el que tenga menor coste, y luego moviéndose al siguiente realizando la misma operación, hasta llegar al nodo final. El algoritmo de Dijkstra, de forma esquemática, es el siguiente: - Inicialmente, estamos en el nodo inicial, marcado como permanente. - Evaluamos el coste para cada nodo adyacente a éste, y se indica el nodo de procedencia. Dichos nodos se añaden al conjunto evaluados. - Se escoge el nodo del conjunto evaluados de menor coste, y se marca como permanente. - Desde este nodo, se evalúa el coste acumulado a los nodos adyacentes que no hayan sido marcados como permanentes, y se añaden al conjunto evaluados junto con el nodo de procedencia. - Se escoge el nodo de menor coste acumulado del conjunto evaluados y se marca como permanente. - El proceso se repite hasta que el nodo destino haya sido marcado como permanente. Cuando esto ocurre, se reconstruye el camino viendo el nodo de procedencia en cada caso. Cuando un nodo es marcado como permanente, significa que ya se ha encontrado el camino que lo une con el nodo inicial de forma óptima. En caso de evaluar un nodo que ya había sido evaluado, se almacena la conexión que menor coste acumulado provoca. Si el algoritmo de Dijkstra no se detiene al encontrar el camino óptimo para el nodo final, acabará encontrando el camino óptimo desde el nodo inicial a todos y cada uno de los nodos. 32

47 Sin embargo, el algoritmo de Dijkstra tiene el inconveniente de que a veces visita demasiados nodos, que no siempre serían necesarios. Esto se debe a que evalúa cada posible conexión con el fin de encontrar siempre el camino de menor coste, y no incluye ninguna componente heurística que le permita guiarse hasta el nodo final. Esta ausencia de componente heurística provoca que, en grafos que tengan una gran cantidad de conexiones, se evalúen demasiados nodos y se produzca, por tanto, un aumento del tiempo de cómputo. La Figura 4.5 muestra la resolución del grafo de la Figura 4.4, para ir del nodo A al E. Al lado de cada vértice, se muestra el coste acumulado para llegar a él por el camino de coste mínimo, así como el nodo de procedencia según dicho camino óptimo. Figura 4-5 Resolución del grafo mediante Dijkstra Como se ha comentado previamente, este algoritmo siempre encuentra el camino óptimo pero, en grafos con una gran cantidad de nodos, puede visitar demasiados nodos y aumentar el tiempo de cómputo innecesariamente. Matlab incluye una función que calcula el camino óptimo entre dos nodos inicial y final según el algoritmo de Dijkstra, llamada graphshortestpath. Esta función toma como entrada la matriz de costes, el nodo inicial, y el nodo final, y devuelve la secuencia de nodos que conforman el camino óptimo. En el caso del grafo mostrado en la Figura 4.4, la matriz de costes es la siguiente: D = ( ) El nodo inicial es A, el final E, y la salida que devuelve la función en este sencillo caso es [A, B, E]. 33

48 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Algoritmo A* El algoritmo A* también es un método de resolución de grafos, que busca el camino óptimo de un nodo inicial a otro final del mismo. Sin embargo, la principal diferencia con el algoritmo de Dijkstra, es que el algoritmo A* incluye una componente heurística, que estima la distancia al nodo destino para intentar acercarse a él en la medida de lo posible. De esta forma guía la búsqueda hacia el nodo destino y ahorra tiempo de computación, ya que se visitan menos nodos. Esto puede provocar que en algunos casos la solución que se encuentre no sea la óptima pero, si se da esta situación, la solución dada presumiblemente será muy similar a la óptima. En el algoritmo A*, el coste invertido en ir del nodo inicial a un nodo n se calcula según la siguiente expresión: f(n) = g(n) + (n) ( 4-5 ) Donde g(n) es el coste real acumulado en ir del nodo inicial al nodo n, entendiendo coste acumulado como la suma de las componentes correspondientes de la matriz de costes calculada en el apartado 4.2.1, y h(n) se corresponde con la mencionada componente heurística que guía la búsqueda de nodos. Esta componente h(n) se puede cuantificar según distintas estrategias, y en nuestro caso la definimos como la distancia euclídea entre el nodo n y el nodo final. De esta forma, estimamos lo lejos que nos encontramos del nodo final, para intentar acercarnos en la medida de lo posible. Así, la función de evaluación f(n) se calcula como la suma del coste acumulado para llegar al nodo n, y la distancia de dicho nodo al objetivo. Es esta función f(n) la que queremos minimizar entre el nodo inicial y el final. El algoritmo A* utiliza para seleccionar el camino dos conjuntos: el conjunto abiertos y el conjunto cerrados. En el conjunto abiertos se guardarán todos los nodos que han sido identificados como posibles movimientos, pero que aún no han sido visitados, mientras que en el conjunto cerrados se guarda la información de los nodos que ya han sido visitados. El conjunto abiertos se puede entender como una cola de prioridad, siendo más prioritarios los nodos con menor valor de coste total f(n). Al igual que en el algoritmo de Dijkstra, cada vez que evaluamos un nodo, es necesario almacenar desde qué nodo previo lo estamos evaluando, para poder reconstruir el camino una vez alcanzado el nodo final. El algoritmo A* sigue los siguientes pasos: - Se añade el nodo inicial a la lista abiertos. - Mientras la lista abiertos no esté vacía, se busca el nodo con menor coste total f(n). - Este nodo se pasa de la lista abiertos a la lista cerrados. - Se buscan todos los nodos adyacentes a éste, se calcula el coste total de cada uno, y se añaden al conjunto abiertos (a no ser que algún nodo esté en el conjunto cerrados, en cuyo caso ese nodo no se añadiría a abiertos). - Se busca en el conjunto abiertos el nodo con menor coste total, se pasa al conjunto cerrados, y se repite el proceso. - El proceso se repite hasta que se alcanza al nodo final, o hasta que la lista abiertos queda vacía, lo que significaría que no hay ningún camino posible. Como se desprende de este esquema, si hay un camino posible el algoritmo A* siempre lo encontrará. Por otra parte, en lugar de expandir todos los nodos, en cada iteración se expande sólo el nodo más prometedor (el de menor coste total de la lista abiertos), lo cual supone una reducción del tiempo de búsqueda en grafos con un gran número de nodos, ya que se realiza una búsqueda guiada por la función heurística. De esta forma, se 34

49 cambia de camino de búsqueda cada vez que existen nodos más prometedores. La función heurística h(n) tiene un papel muy importante en este algoritmo. En primer lugar, supone la diferencia fundamental entre Dijkstra y A*, ya que si elegimos h(n)=0, prácticamente estamos implementando el algoritmo de Dijkstra. Además, es la responsable de que a veces el camino encontrado no sea el más óptimo, ya que, por la propia definición de heurística, no es posible calcularla de forma exacta. Si idealmente fuéramos capaces de obtener una h(n) que reflejara fielmente la distancia al nodo final, el método de A* obtendría siempre la solución más óptima y en un tiempo muy reducido. Sin embargo, con la solución de compromiso de tomar h(n) como la distancia en línea recta del nodo n al nodo final, se comprueba experimentalmente que en la inmensa mayoría de casos encuentra el camino óptimo, y en caso de no ser exactamente éste, propone un camino muy similar. Las posibles diferencias entre las soluciones propuestas por ambos métodos se muestran en el capítulo Interpolación mediante curvas β-spline Una vez obtenida la sucesión de nodos del diagrama de Voronoi que conforman el camino óptimo, es necesario ajustar una curva que defina una trayectoria admisible entre el punto inicial y el punto final, y que pase por dichos nodos. Esta curva debe ser continua en posición, orientación y curvatura, para posibilitar su seguimiento por parte de un robot móvil. Además, resulta beneficioso que esté expresada en forma paramétrica, para posibilitar la representación funcional de cada una de sus dimensiones por separado. La generación de trayectorias hace necesario considerar al menos la continuidad de la primera derivada. Esta condición se traduce en las siguientes restricciones, que fuerzan el valor de la variable y su primera derivada en los puntos inicial y final de cada tramo: p(λ 0 ) = p 0 p(λ f ) = p f p (λ 0 ) = p 0 ( 4-6 ) p (λ f ) = p f Para que se cumplan estas restricciones, se necesita un polinomio de orden al menos tres (cúbico), como el que se muestra a continuación: p(λ) = a 0 + a 1 λ + a 2 λ 2 + a 3 λ 3 ( 4-7 ) Sin embargo, la consideración de las variaciones de curvatura empleando las técnicas básicas de interpolación resulta difícil. Además, se produce un aumento considerable de la complejidad de cálculo en función del número de nodos que se deseen interpolar. 35

50 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Por ello, resulta beneficioso el uso de funciones spline para la interpolación, cuando la curvatura juega un papel importante. Otra propiedad beneficiosa de las curvas spline es su definición por partes o tramos. La curva spline más sencilla es la llamada spline cúbica, que está compuesta por funciones de tercer grado que interpolan a los puntos de control dados. Cada tramo está definido por dos puntos de control consecutivos, de forma que en el tramo T i, en el punto inicial p i, el parámetro vale λ=0, y en el punto final p i+1 toma el valor λ=1. El problema de esta curva radica en la rigidez de su construcción, ya que sólo permite fijar la orientación inicial del primer punto del polígono de control, y la orientación final del último punto del mismo. Las orientaciones en los puntos intermedios vienen fijadas por construcción, para conseguir la continuidad hasta la segunda derivada. Esta rigidez limita el empleo de las spline cúbicas en el suavizado de caminos. Figura 4-6 Spline cúbica con 4 nodos y 3 tramos Se utilizan por ello otras funciones más complejas que ofrecen mayor flexibilidad, al precio de no tener que pasar necesariamente por todos los puntos que definen el camino. A estos puntos se les llama vértices del polígono de control. Estas curvas spline son por tanto curvas de aproximación, ya que no interpolan al polígono de control, y están definidas por unas funciones peso, que determinan la proximidad a la que pasa la curva respecto a los vértices del mismo. Entre estas curvas destacan las B-spline o splines básicas, que se describen según la siguiente expresión: [18] N 1 p i (λ) = B i,k (λ)v i i=0 ( 4-8 ) Siendo N el número de vértices del polígono de control, {V 0, V 1,, V N-1 }, y B i,k las funciones peso de orden k- 1, las cuales garantizan la continuidad C k-2 para la curva completa. Así, el orden mínimo para garantizar la continuidad en orientación y curvatura es k=4, lo que se corresponde con las B-spline cúbicas. 36

51 Cuando en una curva B-spline el orden de las funciones peso k es igual al número de vértices del polígono de control, ésta se comporta como una curva de Bezier, es decir, interpola al primer y último punto de polígono de control, mientras que sobre el resto realiza una aproximación. Este comportamiento se muestra en la Figura 4.7. Figura 4-7 Curva de Bezier de grado 4 y su polígono de control Estos dos tipos de curvas (B-spline y Bezier) solventan el problema de rigidez de las spline cúbicas, ya que aumentando el número de vértices de control aparecen variables libres que podemos utilizar para la imposición de posturas iniciales o finales.el problema principal de este tipo de curvas es que el crecimiento del número de vértices del polígono de control aumenta significativamente la complejidad computacional y, por tanto, el tiempo de cálculo. Además, la modificación de un sólo vértice de control obliga a la reconstrucción de toda la curva. Una clase de curvas que permite la construcción del camino con polinomios de orden reducido, así como añadir vértices adicionales para la imposición de condiciones iniciales, son las llamadas β-spline. De esta forma, este tipo de curvas aúna las ventajas de los tipos vistos anteriormente. En este trabajo se usan las β- spline cúbicas, que garantizan la continuidad en sus uniones hasta la segunda derivada. Las β-spline se definen por secciones, de forma similar a las B-spline. El tramo i de la curva se obtiene según la siguiente expresión: 1 p i (λ) = B r (β 1, β 2, λ)v i+r r= 2 ( 4-9 ) En la expresión anterior, la razón de que r varíe entre -2 y 1 es simplemente porque es la notación habitual, utilizada por ejemplo en [18]. Lo importante de la expresión es que un determinado tramo viene definido por el sumatorio de los 4 productos de cada función B r por las coordenadas de uno de los 4 vértices que definen dicho tramo.vemos como la expresión general es muy similar a una B-spline de orden 3, pero en este caso las funciones de peso B r dependen del parámetro de la curva λ, y de dos valores adicionales llamados parámetros de forma. Estos parámetros de forma son β1, llamado parámetro de sesgo, y β2, conocido como parámetro de tensión. La continuidad en las uniones entre dos segmentos consecutivos se reduce a las siguientes 37

52 Planificación global de trayectorias para robots móviles, basada en curvas β-spline expresiones: P i+1 (0) = P i (1) P i+1(0) = β1 P i (1) ( 4-10 ) P i+1(0) = β1 P i (1) + β2 P i (1) La elección más común es fijar los parámetros β1=1 y β2=0, ya que esto provoca una variación prácticamente lineal de la curvatura. Particularizando las expresiones de las funciones de peso para dichos valores de los parámetros, dichas funciones quedan de la siguiente forma: [18] B-2 ( ) 1/ 6 1/ 2 1/ 2 1/ 6 1 B-1 ( ) 2/ / 2 x 2 B 0 ( ) 1/ 6 1/ 2 1/ 2 1/ 2 3 B1( ) / 6 ( 4-11 ) Una de las propiedades más importantes de las curvas β-spline es que, al modificar un vértice del polígono de control, no es necesario recalcular toda la curva, sino solo los tramos afectados. Cada tramo de una curva β- spline queda definido por cuatro vértices, por lo que para N vértices, aparecerán N-3 tramos. De esta forma, al modificar un vértice, en el peor de los casos serán 4 los tramos afectados. Además, el orden de las funciones de coste B r es independiente del número de vértices, por lo que podemos añadir algunos para fijar condiciones iniciales sin aumentar significativamente la complejidad. La Figura 4.8 muestra una curva β-spline generada a partir de 6 vértices. Por tanto, la curva consta de 3 tramos, definidos respectivamente por los vértices {v 1,v 2,v 3,v 4 }, {v 2,v 3,v 4,v 5 } y {v 3,v 4,v 5,v 6 }. Figura 4-8 β-spline de 6 vértices y 3 tramos 38

53 Por todas las ventajas comentadas anteriormente, en este trabajo se utilizan las mencionadas curvas β-spline. Además, los vértices del polígono de control se colocarán separados entre sí por una distancia constante, ya que así se consigue un mejor control de las variaciones de la curvatura. Por otra parte, la separación constante entre ellos garantiza que la atracción que ejercen los vértices del polígono de control sobre la curva se reparte de forma homogénea, por lo que se evitan saltos innecesarios en la curvatura Acotación cartesiana Una propiedad importante de las curvas β-spline es la llamada acotación cartesiana. Esta propiedad consiste en que si tenemos un conjunto de vértices {v -2, v -1, v 0, v 1 } situados sobre un arco de circunferencia de radio R, separados entre sí por una distancia constante δ, la curva β-spline generada será un arco de circunferencia concéntrico con el anterior, y de radio R = R-d. Esta situación se ilustra en la Figura 4.9, donde la curva β- spline resultante se muestra en negrita, y también se indica la separación d. [19] Figura 4-9 Ilustración de la acotación cartesiana Una vez aceptada esta propiedad de acotación de la curva β-spline, resulta conveniente deducir la diferencia d entre el radio R que contiene a los vértices del polígono de control, y el radio R que contiene a la curva. Para ello, estudiaremos el escenario de la figura

54 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 4-10 Escenario para calcular la distancia d Debido a la propiedad de acotación cartesiana, toda la curva β-spline se encuentra sobre la circunferencia de radio R =R-d. Por ello, para calcular la distancia d podemos centrarnos en cualquier punto de la curva, y en este caso nos centraremos en el punto inicial. Recordamos que, según la definición de β-spline, la expresión de la curva es la siguiente: p(λ) = B 2 (λ) v 2 + B 1 (λ) v 1 + B 0 (λ) v 0 + B 1 (λ) v 1 ( 4-12 ) Particularizando las expresiones (4-11) para el punto inicial (λ = 0), éstas quedan de la siguiente forma: B 2 (0) = 1 6 B 1 (0) = 2 3 ( 4-13 ) B 0 (0) = 1 6 B 1 (0) = 0 Como se ve en estas expresiones, la posición del vértice v 1 no influye para el cálculo de la posición del punto inicial de la curva. Utilizando estos valores para las dos componentes que nos ocupan, obtenemos los valores de las coordenadas x e y para el punto inicial. p x (0) = 1 6 x x = 0 p y (0) = 1 6 y y = 1 3 y ( 4-14 ) 40

55 En la Figura 4.10 ya quedaba de manifiesto que la componente x del punto inicial iba a ser 0, tal y como se han definido los ejes, ya que se encuentra sobre el eje y. Sin embargo, el valor de la componente vertical sí que nos aporta información, ya que nos dice que la distancia entre ambos radios es d=y/3. Por lo tanto, el problema se reduce a la resolución de un sistema de ecuaciones donde el objetivo es calcular la intersección entre una circunferencia de radio R y centro en C, y una circunferencia de radio δ y centro en el origen de coordenadas. Resolviendo el mencionado problema, obtenemos: y = δ2 2R ( 4-15 ) Y por lo tanto, es inmediato despejar: d = R R = δ2 6R ( 4-16 ) Esta sencilla expresión nos permite saber el radio de la circunferencia que contiene a la curva β-spline cuando los vértices del polígono de control se encuentran equiespaciados sobre una circunferencia de radio R. Vemos que esta expresión sólo depende del radio de la circunferencia y de la separación entre los vértices. Esta propiedad aporta una ventaja más a la elección de los vértices del polígono de control de forma equidistante, ya que nos permite forzar a la curva a interpolar los waypoints que conforman nuestro camino, como se verá en los próximos apartados Condiciones iniciales y finales de la curva β-spline Conociendo las expresiones de las funciones peso de las curvas β-spline (4-11), podemos deducir la posición de unos vértices adicionales {v -2, v -1, v 0 } que hagan cumplir a la curva generada determinadas condiciones iniciales. Siendo p i, p i y p i el punto por donde se desea que pase la curva, y los valores de la primera y segunda derivada discreta que debe tomar la curva en ese punto, respectivamente, debemos buscar que f(0)=p i, f (0)=p i y f (0)=p i, siendo f(λ) el tramo de β-spline generado. Dichas condiciones iniciales vienen representadas por las siguientes expresiones: p i (0) = B 2 (0) v 2 + B 1 (0) v 1 + B 0 (0) v 0 + B 1 (0) v 1 p i (0) = B 2 (0) v 2 + B 1 (0) v 1 + B 0 (0) v 0 + B 1 (0) v 1 ( 4-17 ) p i (0) = B 2 (0) v 2 + B 1 (0) v 1 + B 0 (0) v 0 + B 1 (0) v 1 Para λ=0, tanto B 1 como B 1 y B 1 valen 0, luego la posición de v 1 no influye en el punto inicial de la curva (como se vio en el apartado anterior), y las expresiones anteriores resultan en un sistema de tres ecuaciones con tres incógnitas. Resolviendo este sistema, obtenemos las siguientes posiciones para los puntos (para cada coordenada por separado): 41

56 Planificación global de trayectorias para robots móviles, basada en curvas β-spline v 2 = p i p i p i v 1 = p i 1 6 p i ( 4-18 ) v 0 = p i + p i p i Con las expresiones anteriores, obtenemos fácilmente las posiciones en las que debemos colocar los 3 vértices adicionales para conseguir unas condiciones iniciales concretas, en función de la posición del punto y de las derivadas discretas primera y segunda en el mismo, para cada componente (en el plano, x e y). De la misma forma, podemos deducir la posición de tres vértices adicionales {v -1, v 0, v 1 } que hagan cumplir a la curva generada unas condiciones finales determinadas. En este caso, que debemos buscar que f(1)=p f, f (1)=p f y f (1)=p f, siendo p f, p f y p f los valores de la coordenada correspondiente y de las derivadas primera y segunda deseadas en el punto final. De la misma forma que en el caso de las condiciones iniciales para λ=0 tanto B 1 como B 1 y B 1 valían 0, en este caso, para λ=1 son B -2, B -2 y B -2 las que valen 0 y por lo tanto no influyen en las expresiones. Despejando de la misma forma que en el caso de las condiciones iniciales, obtenemos las siguientes expresiones: v 1 = p f p f p f v 0 = p f 1 6 p f ( 4-19 ) v 1 = p f + p f p f Como se puede comprobar, las expresiones son prácticamente análogas en (4-18) y (4-19), diferenciándose sólo en que en el primer caso las coordenadas de los puntos se corresponden con los tres primeros vértices, y en el segundo caso se corresponden con los tres últimos. Sin embargo, resulta conveniente expresar estos valores de p y p en función de parámetros más inmediatos, como serían la posición, orientación y curvatura iniciales deseadas. De la definición de orientación y curvatura, podemos relacionar dichos parámetros con las derivadas discretas de nuestras componentes: θ = arctan ( y x ) ( 4-20 ) κ = x y x y ( 4-21 ) 3 ( x 2 + y 2 ) En las expresiones anteriores, θ representa la orientación y κ la curvatura. Además, la separación entre los 42

57 vértices es conveniente que sea constante y de valor δ. De la expresión (4-18) podemos obtener la separación entre los vértices v -2 y v -1, aplicando dichas expresiones a las componentes x e y. Haciendo esto, obtenemos la siguiente expresión: v 1 v 2 2 = x 2 + y 2 = x x 2 x x + y y 2 y y = δ 2 ( 4-22 ) De la misma forma, la separación entre v 1 y v 0 según la expresión (4-19) también debe ser δ. Forzando esto, se obtiene la siguiente expresión: v 1 v 0 2 = x 2 + y 2 = x x 2 + x x + y y 2 + y y = δ 2 ( 4-23 ) Por otra parte, si queremos definir un vector primera derivada tal que su módulo sea la separación deseada δ, podemos definir las componentes del mismo de la siguiente forma: x δ = tan 2 (θ) + 1 ( 4-24 ) y = x tan (θ) ( 4-25 ) Donde la expresión (4-25) viene derivada de la (4-20), y la expresión (4-24) posibilita que el módulo del vector primera derivada sea δ.igualando las expresiones (4-22) y (4-23), obtenemos la siguiente igualdad, que asegura que la separación entre los vértices adicionales sea constante: y = x y x = x tan (θ) ( 4-26 ) Despejando (4-26) en (4-21), obtenemos: x = κ x 2 + y 2 3 x tan (θ) + y ( 4-27 ) De esta forma, si queremos imponer unas condiciones iniciales o finales de posición, orientación y curvatura, podemos utilizar las ecuaciones (4-24) a (4-27) para obtener las derivadas discretas x, x, y e y, y sustituir estos valores en las expresiones (4-18) ó (4-19) para obtener la posición de los vértices adicionales a añadir, según queramos forzar unas condiciones iniciales (4-18) o finales (4-19). 43

58 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Cabe destacar que, para cualquier valor de θ, se cumplirá que los vértices adicionales serán equidistantes y, además, cuando la curvatura sea 0, la distancia entre ellos será exactamente de valor δ, siendo muy similar a este valor cuando la curvatura sea distinta de 0. Para que el valor fuera exactamente δ para cualquier valor de la curvatura, habría que resolver en cada caso el sistema de ecuaciones no lineales formado por las ecuaciones (4.22) y (4.23), pero no merece la pena ya que la separación es similar a δ, y lo más importante es que dicha separación sea constante, cosa que sí se cumple en cualquier caso Selección del camino y suavizado En este apartado se explica el proceso de selección del camino óptimo que conecta el punto inicial con el punto final, y el posterior proceso de suavizado que permite la obtención de una trayectoria suave y realizable por el robot móvil que se quiera utilizar. Para ello, se utilizan las herramientas presentadas a lo largo de este capítulo, como los algoritmos de Dijkstra y A* para la obtención del camino, y las curvas β-spline para el suavizado del mismo. Se distinguen por ello dos partes diferenciadas, como son la obtención del camino y el suavizado del mismo. En cuanto a la obtención de la sucesión de waypoints que conforman el camino óptimo, se realizan una serie de consideraciones para asegurar que no habrá problema con dicho camino en el proceso de suavizado, y así evitar procesos iterativos entre ambas partes que aumenten el tiempo de cómputo. Se busca, por tanto, que el camino propuesto en el primer paso pueda ser suavizado sin ningún problema en el segundo, y la curva final sea realizable por el robot móvil que se desee. Como se ha expuesto en los apartados anteriores, el problema principal del suavizado se limita a la obtención de los vértices del polígono de control que define a la curva β-spline, separados entre sí una distancia δ, de forma que la curva resultante interpole a los waypoints del camino elegido. El polígono de control se situará sobre una serie de circunferencias, y sobre las rectas tangentes a dichas circunferencias, como se muestra en la Figura En dicha figura, la línea roja discontinua representa el camino a suavizar, formado por los waypoints {v 1, v 2, v 3, v 4, v 5 }, y en azul aparecen los vértices del polígono de control que definen la curva β-spline, colocados sobre una serie de arcos de circunferencias, unidos mediante rectas tangentes. Dichas circunferencias se colocan de tal forma que fuercen a la curva β-spline a pasar por los waypoints que definen el camino. El proceso se describe con mayor profundidad en el apartado (4.4.2). 44

59 Figura 4-11 Representación del polígono de control para suavizar un camino de 5 vértices Selección del camino Este apartado expone el método de selección de los waypoints que conforman el camino óptimo, a partir del diagrama de Voronoi generado según el capítulo 3. Este proceso de selección del camino se realiza mediante el algoritmo de Dijkstra (apartado 4.2.2) o el algoritmo A* (apartado 4.2.3). Sin embargo, estos algoritmos eligen el camino óptimo basándose en la matriz de costes (apartado 4.2.1), y a priori no tienen por qué garantizar que dicho camino pueda ser suavizado mediante curvas β-spline, y potencialmente seguido por un robot móvil de ciertas características. Por esta razón, se presenta en este apartado un método de obtención de un camino que pueda ser suavizado a continuación, y que permita alcanzar el objetivo de forma segura. En este método de obtención del camino ya se tienen en cuenta las características del robot móvil concreto con el que se quiere realizar la trayectoria, mediante la inclusión de algunos parámetros. De esta forma, el camino escogido irá orientado al robot móvil en cuestión, y será realizable por éste Adaptación del camino para que sea realizable Tras obtener el camino, es posible que las características y limitaciones cinemáticas de nuestro robot no permitan alcanzar todos los waypoints del camino propuesto, o al menos no de forma segura. Por esta razón, se procede a un proceso de adaptación del camino propuesto teniendo en cuenta las características cinemáticas del robot, que consiste en la eliminación de algunos de los waypoints que conforman dicho camino, para conseguir que sea realizable y seguro. El método presentado supone que se viaja a velocidad v constante. Además, la principal restricción cinemática en los robots móviles es la velocidad angular máxima que pueden alcanzar, w max. De esta forma, sabiendo el valor de w max del robot, y la velocidad v a la que queremos movernos, la restricción cinemática del robot se 45

60 Planificación global de trayectorias para robots móviles, basada en curvas β-spline traduce en un radio mínimo de giro que puede seguir, según la expresión (4-28). r min = v w max ( 4-28 ) Mediante el cumplimiento de esta restricción de radio mínimo de giro, se elimina la suposición de robot omnidireccional. Este valor de r min es fundamental a la hora de adaptar el camino obtenido mediante los algoritmos de Dijkstra o A*. Para justificar el método seguido para adaptar el camino al robot deseado, es necesario prever qué problemas podemos tener en el suavizado, con el fin de evitarlos. Como se ha comentado previamente, sobre los picos del camino se colocan una serie de circunferencias, que se unen mediante rectas tangentes y conforman la curva sobre la que se sitúa el polígono de control. Estas circunferencias no pasan exactamente por los waypoints, sino que están desplazadas una distancia d, dada por la ecuación (4-16), para que la curva final interpole a dichos puntos. Sin embargo, para la deducción que se presenta a continuación no se tiene en cuenta dicho desplazamiento, ya que dicho valor será muy pequeño para pequeños valores de δ (separación entre los vértices del polígono de control). Resulta evidente que, si dos waypoints consecutivos se encuentran separados por una distancia demasiado pequeña, al construir las circunferencias correspondientes resultará que son secantes, es decir, que se cortarán, y no será posible encontrar la recta tangente interior que enlace los dos arcos correspondientes. Este problema que surge en el suavizado, refleja en realidad la imposibilidad de pasar por dos waypoints consecutivos demasiado cercanos, cuando para ello se exigiría incumplir la restricción de radio mínimo. Este caso se muestra en la Figura 4.12, que refleja un caso similar al de la Figura 4.11, pero con el vértice v 3 demasiado cerca de v 2. Figura 4-12 Situación en la que dos circunferencias aparecen secantes La Figura 4.12 muestra el mismo camino que la Figura 4.11, pero en cada caso el valor del radio mínimo es distinto. Este caso es un ejemplo de que un mismo camino a veces es válido y a veces no, dependiendo del robot. En el caso de la Figura 4.12 es necesario, por tanto, adaptar dicho camino. Para evitar el caso presentado en la Figura 4.12, una vez conocido el camino propuesto por los algoritmos de Dijkstra o A*, comprobaremos que la distancia entre los distintos vértices sea mayor a 2.25r min, para evitar que las circunferencias construidas sean secantes. A priori podría parecer que sería suficiente con que la distancia 46

61 entre los vértices fuera de 2r min, pero no siempre es así, ya que los centros de las circunferencias se encuentran sobre las bisectrices de los distintos vértices, y éstas no tienen por qué ser paralelas. Por ello, se aumenta un poco la distancia como medida de seguridad. En caso de encontrar dos vértices consecutivos separados por una distancia menor a 2.25r min, hay que eliminar uno de ellos. En primer lugar, probamos a eliminar el segundo de los vértices que están demasiado próximos. Tomando como ejemplo el caso de la Figura 4.12, eliminaríamos el vértice v 3. Se comprueba por tanto la seguridad del tramo {v 2, v 4 } y, si se cumple la distancia de seguridad con los obstáculos en dicho tramo, se sigue con el proceso a partir de v 4. En caso de no cumplirse la distancia de seguridad entre v 2 y v 4, se probaría eliminando el vértice v 2. En este caso, habría que comprobar la seguridad del tramo que une v 1 con v 3. Si se cumple la distancia de seguridad, el proceso seguirá con normalidad a partir de v 3. Sin embargo, si no se cumple tampoco en este caso, significa que nuestro robot no puede completar este tramo de ninguna forma, por lo que habrá que prohibir la conexión entre v 2 y v 3 (el tramo conflictivo) en la matriz de costes, y volver a calcular otro camino óptimo. En la Figura 4.13 se muestra este caso. En rojo aparece el primer camino propuesto, en azul la primera modificación que probamos, y en negro la modificación que probamos en caso de fallar tanto la conexión en rojo como la conexión en azul. Por último, la cruz muestra el tramo que se prohibiría en caso de fallar todas las combinaciones propuestas. Figura 4-13 Intentos de adaptar el camino y hacerlo realizable Una vez prohibido dicho tramo en la matriz de costes, aseguramos así que el camino elegido por Dijkstra o A* no será el mismo Búsqueda de un suavizado seguro Llegados a este punto, nos hemos asegurado de que el camino es cinemáticamente realizable por el robot móvil seleccionado (mediante la elección de un r min adaptado al mismo). Por otra parte, se verifica que, en cada tramo, se cumple la distancia de seguridad con el fin de evitar colisiones. Sin embargo, la distancia al obstáculo más cercano se ha medido respecto a la línea que une los dos vértices que definen un tramo. Esta situación se muestra en la Figura

62 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 4-14 Representación de la distancia de seguridad En la Figura 4.14 se muestra la distancia de seguridad tal y como se ha medido hasta ahora. En ella, queda patente que se ha medido respecto a las líneas que componen el camino, por lo que en realidad aún sería posible que se produzca una colisión, debido a la separación que existe entre la curva suavizada y las líneas que componen el camino. Un ejemplo de la separación entre las líneas que componen el camino, y la trayectoria real que seguirá nuestro robot se muestra en la Figura 4.1, representada mediante las líneas verdes. Por tanto es necesario un estudio de la separación máxima que puede existir en cada tramo, para tenerla en cuenta a la hora de seleccionar un camino como seguro. Como se verá con más profundidad en el apartado correspondiente al suavizado, las circunferencias se colocan sobre las bisectrices de los ángulos que forman las líneas rectas que unen los waypoints. En la Figura 4.15 se muestra un caso genérico, sobre el cual estudiaremos dicha separación. Figura 4-15 Representación de la separación máxima al suavizar En la Figura 4.15 se representa el waypoint v i, así como las líneas que lo unen con los vértices v i-1 y v i+1, representadas en rojo, formando entre sí un ángulo α. El círculo de radio R que se genera tiene su centro en la 48

63 bisectriz de dicho ángulo α, y viene representado en la figura en azul. La circunferencia no contiene al vértice v i en su perímetro, sino que está desplazada una distancia d calculada en la expresión (4-16). En la Figura 4.15 se aprecia que la máxima separación entre la circunferencia de radio R y la línea que une v i con v i-1 se corresponde con la línea perpendicular a ésta que pasa por el centro de la circunferencia. Esta distancia es la llamada h max, y está representada en verde. Igualmente, para el tramo que une v i con v i+1, la separación máxima se corresponderá con la línea perpendicular a dicho tramo, que pasa por el centro de la circunferencia. Esta segunda separación máxima no se ha representado en la Figura 4.15 para no cargar excesivamente de líneas la misma. Para la situación genérica mostrada en la Figura 4.15, el valor de h max es: max = R (R d) sinα ( 4-29 ) Sin embargo, este valor de h max se corresponde con la separación máxima de la circunferencia que contiene los vértices del polígono de control. Por la propiedad de acotación cartesiana sabemos que el tramo correspondiente de la curva final estará contenido en dicha circunferencia, exactamente a una distancia d de la misma. Por tanto, la separación máxima real de la curva será: real = R (R d) sinα d ( 4-30 ) Sustituyendo el valor de d calculado en (4-16), obtenemos la siguiente expresión de la separación real: real = R (R δ2 δ2 ) sinα 6 R 6 R ( 4-31 ) La expresión (4-31) muestra la separación máxima real que experimentará la trayectoria generada respecto a la línea que une los waypoints en cada caso. Sin embargo, esta separación depende del radio de la circunferencia con la que suavizaremos posteriormente, que en este momento es desconocido. La estrategia seguida es, a partir de la separación con el obstáculo más cercano en determinado tramo, calcular la separación límite h real,max que no provocaría una colisión, y a partir de este valor calcular el radio máximo con el que podemos suavizar posteriormente. Si este radio máximo es menor que el radio mínimo de giro impuesto por la cinemática de nuestro robot, significa que es imposible recorrer dicho tramo de forma segura, y por lo tanto habrá que buscar un camino alternativo. Como se ve en la Figura 4.15, cada ángulo α está definido por dos tramos {v i, v i-1 } y {v i, v i+1 }. Se calcula la distancia al obstáculo más cercano en ambos tramos, y la diferencia entre el menor de estos dos valores y la distancia de seguridad, será lo que llamamos h real,max. A continuación, deducimos el valor de R max mediante la resolución de la ecuación de segundo grado (4-32), obtenida a partir de (4-31). 6 (1 sin ( α 2 )) R 2 max 6 real,max R max + δ 2 (sin( α 2 ) 1) = 0 ( 4-32 ) De esta forma, nos aseguramos que el suavizado de dicho tramo será seguro siempre que se cumpla que R<R max, y podrá ser seguido por nuestro robot siempre que R>R min. Como se ha comentado previamente, en caso de que para un conjunto de tres vértices consecutivos 49

64 Planificación global de trayectorias para robots móviles, basada en curvas β-spline obtengamos un valor de R max <R min, será imposible recorrer dicha secuencia de vértices de forma segura sin violar las restricciones cinemáticas del robot. Este problema puede derivar de que el tramo que se desea recorrer está demasiado cerca de los obstáculos, como un pasillo muy estrecho, caso en el que sería imposible adaptar el camino y habría que buscar otro camino alternativo (mediante Dijkstra o A*), que pase por otro tramo con más holgura. Sin embargo, el problema que aparece al suavizar también puede venir derivado de haber eliminado algún vértice intermedio por estar demasiado cerca de otro (distancia entre ellos <2.25R min ). Cuando sucede esto, se procede de forma similar al caso mostrado en la Figura 4.13, de forma que comprobamos si dicho problema también aparece al eliminar el primero de los vértices que se encuentran demasiado próximos (en un primer paso se habrá intentado eliminar el segundo de los vértices). En caso de que el suavizado siga sin ser seguro, se prohíbe el tramo correspondiente en la matriz de costes y se vuelve a buscar otro camino mediante los algoritmos de Dijkstra o A*. Este caso se muestra en la Figura 4.16, donde aparece una secuencia de 6 vértices, representados por la línea discontinua roja. Los vértices v 3 y v 4 están demasiado próximos, por lo que es necesario eliminar uno de los dos. En la Figura 4.16 se muestra con línea negra el conjunto de 3 vértices que comprobaremos si es seguro y, suponiendo que no lo sea, en la Figura 4.17 se muestra la combinación alternativa que se comprobaría, así como el tramo que se prohibiría en caso de que tampoco fuera segura dicha secuencia. Figura 4-16 Camino de 6 vértices, donde se elimina el cuarto. En el caso de que suavizar el tramo entre v 2, v 3 y v 4 no sea seguro, se comprueba el caso marcado por la línea negra en la Figura

65 Figura 4-17 Camino de 6 vértices, donde se elimina el tercero. En la Figura 4.17, en el caso de que el suavizado entre los tres vértices representados fuera seguro, se seguiría comprobando a partir de v 4 de la misma forma. En el caso de que de esta forma tampoco sea seguro, se prohíbe en la matriz de costes la conexión entre v 1 y v 2, ya que es la que precede al tramo conflictivo, y se vuelve a buscar otro camino alternativo mediante los algoritmos de Dijkstra o A*. Cabe destacar algunas consideraciones que se han tomado a la hora de calcular el radio máximo en cada vértice. Por un lado, no hemos considerado hacia qué lado se producirá la separación entre la curva suavizada que conforma la trayectoria, y la línea que une los waypoints en cada tramo. Esto no tiene ninguna trascendencia, ya que, gracias a las consideraciones tomadas a la hora de elaborar el diagrama de Voronoi en el capítulo 3, cada línea del diagrama se encuentra equidistante entre los dos obstáculos más cercanos. Por tanto, no importa hacia qué lado se produce la separación máxima. Por otro lado, para calcular el valor de R max en cada caso, hemos supuesto siempre que se cumple el caso más desfavorable, es decir, que el punto en el que se produce la separación máxima h max forma parte del arco de circunferencia que albergará los vértices del polígono de control. Esto supone una estrategia conservadora, ya que si el trayecto es seguro en el caso más desfavorable, lo será en cualquier caso. Sin embargo, también entraña el peligro de prohibir trayectos que son seguros, al no cumplirse este caso más desfavorable. En el capítulo 5, destinado a simulaciones, se realiza un estudio sobre la cantidad de veces que este método prohíbe trayectorias seguras. En el mismo se muestra que este problema se da en un número de casos muy reducido, por lo que queda así justificada la elección de este método Proceso final de elección del camino El proceso explicado se repite mientras que no se alcance el punto final, o hasta que se confirme que no existe un camino seguro entre dicho punto final, y el punto inicial. Gracias a las consideraciones tenidas en cuenta y a las precauciones tomadas, la sucesión de waypoints resultante de este proceso podrá ser suavizada en el siguiente paso y dará lugar a una trayectoria segura y realizable por el robot móvil que cumpla las especificaciones de distancia de seguridad y radio mínimo de giro que se han utilizado para la selección del camino. De esta forma, se evita un posible proceso de iteración entre la selección del camino óptimo y el suavizado, ya que la sucesión de waypoints resultante de este proceso irá acompañada de unos valores de radio máximo aceptables para que sea posible realizar el suavizado posteriormente, sin peligro de colisión alguno. El hecho de evitar dicha iteración entre suavizado y selección del camino supone un ahorro de tiempo de 51

66 Planificación global de trayectorias para robots móviles, basada en curvas β-spline computación importante Suavizado del camino En este apartado se aborda el problema de obtener una curva suave que pase por los waypoints seleccionados tras el proceso explicado en el apartado anterior Para suavizar dicho camino, la estrategia seguida es la búsqueda de curvas β-spline que interpolen a dichos waypoints. El problema principal a la hora de suavizar el camino de esta forma es la colocación de los vértices del polígono de control que definen a la curva β-spline resultante. Dichos vértices se colocan de forma equidistante sobre una serie de arcos de circunferencias, y las rectas tangentes que los unen. Un ejemplo de dicha configuración es el mostrado en la Figura A lo largo de este capítulo se han comentado las ventajas de que los vértices del polígono de control se encuentren separados entre sí por una distancia constante, como por ejemplo el mejor control de la curvatura y la posibilidad de forzar que la curva final interpole a los waypoints del camino, gracias a la propiedad de acotación cartesiana. Por ello, esta estrategia a la hora de situar el polígono de control ya se ha seguido para otros métodos de planificación, como el presentado en Planificación de Trayectorias para Robots Móviles (Víctor F. Muñoz, 1995)[19], por lo que no se trata de una estrategia innovadora. En cambio sí se trata de una estrategia útil, por lo que en este apartado se profundiza en la forma de obtener dicho polígono de control. Parte de la base teórica de las curvas β-spline mostrada anteriormente (ver apartados y 4.3.2) se basa en [19], pero en este trabajo se alcanzan y utilizan expresiones diferentes a las alcanzadas en dicha obra Construcción de las circunferencias El primer paso a la hora de suavizar consiste en la construcción de una serie de circunferencias sobre los waypoints del camino, y su posterior desplazamiento una distancia d para que, gracias a la propiedad acotación cartesiana de las curvas β-spline, la curva resultante interpole a dichos puntos. En la Figura 4.18 se muestra la situación comentada. Figura 4-18 Circunferencia sobre el vértice v i. 52

67 En la Figura 4.18 se ha representado el tramo formado por los vértices v i y v i-1, separados una distancia H i, y el tramo acotado por los vértices v i y v i+1, separados una distancia H i+1. Se construye en un primer paso la circunferencia de radio R i, que contiene inicialmente a v i en su perímetro, pero posteriormente se desplaza una distancia d en la dirección de la bisectriz del ángulo α i, donde la distancia d viene determinada por la expresión (4-16). Dicho desplazamiento permitirá que la curva β-spline generada interpole al vértice v i. En cuanto al valor del radio R i, del apartado anterior sabemos el valor máximo aceptable R i,max que evita una colisión, y por otra parte también sabemos el valor de R min que se corresponde con el radio mínimo que puede recorrer nuestro robot para una velocidad dada. Tenemos de esta forma acotados los posibles valores del radio de la circunferencia, pero es necesario idear una expresión que indique el valor exacto del radio. Por un lado, queremos que el radio de giro sea mayor mientras más amplio sea el ángulo α i, ya que cuanto mayor sea este ángulo, menos brusco tendrá que ser el giro. Por otro lado, el radio deberá ser menor cuanto menor sean los valores de H i y de H i+1, ya que los vértices v i+1 y v i-1 en ningún caso deben estar contenidos en la circunferencia de radio R i. Por tanto, el radio se calculará mediante la expresión (4-33), donde H i es un valor que depende de H i y H i+1, como se verá posteriormente. R i = H i 2 cos( α i 2 ) ( 4-33 ) En caso de que el valor resultante de R i sea mayor que R i,max o menor que R min, se le asignará el valor límite correspondiente. En cuanto al valor H i que aparece en la expresión (4-33), es conveniente definirlo de tal forma que dos circunferencias consecutivas no sean secantes, ya que en este caso no podrían unirse mediante una tangente interior. Sin embargo, no es posible obtener una fórmula cerrada para H i que evite que dos circunferencias consecutivas sean secantes, ya que esto dependerá de factores como la posición de los ángulos α i, que definirá las bisectrices sobre las que se encontrarán los centros, o el desplazamiento d sufrido por cada circunferencia, que a su vez depende de su radio. Se propone por tanto la expresión (4-34), obtenida experimentalmente. Se ha comprobado que con esta expresión no suele darse el fenómeno de que dos círculos consecutivos sean secantes. H i = min (H i, H i+1 ) 3.3 ( 4-34 ) Como se ha comentado, el valor de H i según la expresión (4-34) supone una propuesta inicial que no suele dar problemas. Sin embargo, existe la posibilidad de que en algún caso especial las circunferencias resultantes sigan siendo secantes. Si se necesita enlazar ambas circunferencias por una tangente exterior, este hecho carece de importancia. Sin embargo, si necesitamos enlazarlas mediante una tangente interior, la estrategia seguida consiste en disminuir el radio del mayor de los dos círculos (siempre respetando que sea mayor que el radio mínimo permitido), hasta que dichas circunferencias no sean secantes Construcción de las tangentes Una vez construidas las circunferencias, el siguiente paso es unirlas mediante tangentes. Sin embargo, ésta no es una tarea trivial, ya que entre dos circunferencias que no son secantes existen cuatro posibles tangentes 53

68 Planificación global de trayectorias para robots móviles, basada en curvas β-spline comunes, como se muestra en la Figura Es por tanto necesario idear una estrategia que permita escoger de forma automática qué tangente es la más adecuada en cada caso. Figura 4-19 Tangentes comunes a dos circunferencias En la Figura 4.19 se muestran las 4 rectas que son tangentes comunes a ambas circunferencias, es decir, que las cortan en un solo punto. Las rectas T 1 y T 2, representadas en negro, son las tangentes exteriores, mientras que T 3 y T 4, mostradas en rojo, son las tangentes interiores a ambas circunferencias. Como se ha comentado previamente, entre dos circunferencias secantes se pueden encontrar las tangentes exteriores, pero resulta imposible trazar las interiores. Por ello, gracias a las precauciones tomadas previamente, esta situación nunca se dará. La primera decisión que se debe tomar a la hora de enlazar dos circunferencias mediante una de sus tangentes comunes, es si se necesita una tangente interior o exterior. En la Figura 4.20 se muestra un tramo formado por cinco vértices {v 1,,v 5 }, que definen tres circunferencias. Figura 4-20 Circunferencias y bisectrices en un tramo de 5 vértices 54

69 En la Figura 4.20 aparecen también representados los vectores que apuntan en la dirección de cada bisectriz, b 1, b 2 y b 3. Precisamente estos vectores son los que aportan la información necesaria para saber si dos circunferencias necesitan ser enlazadas mediante una tangente exterior o interior. Así, si deseamos enlazar las circunferencias C i y C i+1, calcularemos el producto escalar de los vectores b i y b i+1, definidos éstos en la dirección de la bisectriz de cada ángulo, y de módulo 1. Si dicho producto escalar es negativo, necesitaremos enlazar las circunferencias mediante una tangente interior. En cambio, si el producto es positivo o igual a 0, necesitaremos enlazarlas mediante una tangente exterior. Esta deducción se desprende de que, siempre que necesitemos enlazar dos circunferencias mediante una tangente interior, los vectores unitarios que apuntan en la dirección de las bisectrices estarán enfrentados, por lo que su producto escalar será negativo. La deducción del párrafo puede comprobarse en la Figura 4.20, donde el producto escalar de los vectores b 1 y b 2 es negativo, por lo que unimos las circunferencias correspondientes mediante una tangente interior. Por otra parte, el producto escalar de los vectores b 2 y b 3 es positivo, por lo que se confirma que debemos utilizar una tangente exterior en este caso. Una vez deducido un método para identificar qué tipo de tangente necesitamos, el siguiente paso consiste en saber cuál de las dos tangentes del mismo tipo es la idónea, ya que entre dos circunferencias no secantes se pueden trazar dos tangentes interiores y dos exteriores. En la Figura 4.21 se muestran dos circunferencias cuyos centros se encuentran sobre la misma horizontal, así como una de las tangentes exteriores. Figura 4-21 Tangente exterior a dos circunferencias Una recta se define por su pendiente, y por un punto de la misma. La expresión (4-35) muestra una fórmula para obtener la pendiente de la tangente exterior superior a dos circunferencias, cuando sus centros se encuentran alineados según la misma horizontal. m = tanα = R i+1 R i C i+1, C i 2 (R i+1 R i ) 2 ( 4-35 ) 55

70 Planificación global de trayectorias para robots móviles, basada en curvas β-spline La expresión (4-35) [19] indica la pendiente de la tangente que enlaza ambas circunferencias por la mitad superior, pero si queremos obtener la pendiente de la inferior, es simplemente su valor negativo. Una vez obtenida la pendiente de la recta tangente, para definirla completamente hay que obtener el punto de tangencia. El vector que une el centro de la circunferencia con el punto de tangencia (llamado vector t), por definición, es perpendicular a la recta tangente. Por tanto, la pendiente de este vector será m perp = 1/m, siendo m la pendiente de la recta tangente. Sin embargo, sólo con el valor de la pendiente no está definido el punto de tangencia, ya que una pendiente define la orientación de un vector, y no su dirección. Es necesario deducir la siguiente correspondencia: Si R i >R i+1 : t = (1, 1 m )/ (1, 1 m ) Si R i <R i+1 : t = ( 1, 1 m )/ ( 1, 1 m ) ( 4-36 ) De esta forma, el vector unitario t, con origen en el centro C i, nos permite saber la posición del punto de tangencia. Sin embargo, tanto el vector t como la pendiente m están referidos a un sistema de coordenadas locales en el que el eje x está alineado con la línea que une los dos centros de las circunferencias. Es necesario transformar estos parámetros al sistema de coordenadas global que estamos siguiendo. Para ello, supongamos que la línea que une ambos centros forma un ángulo β con la horizontal, como se muestra en la Figura 4.22: Figura 4-22 Situación anterior en coordenadas globales 56

71 De esta forma, la pendiente real de la recta tangente será la que se indica en la expresión (4-37): m real = tan(α + β) = tan (atan(m) + β) ( 4-37 ) Suponiendo que el vector t en coordenadas locales sea (t 1, t 2 ), el vector en coordenadas globales será: t real = (t 1 cosβ t 2 sinβ, t 1 sinβ + t 2 cosβ) ( 4-38 ) La expresión (4-38) se deduce simplemente de girar un vector un determinado ángulo β. De esta forma, mediante el vector t real obtenemos el punto de tangencia, y con la pendiente m real queda totalmente definida la tangente. En el caso de que necesitemos una tangente interior, caso presentado en la Figura 4.23, la expresión (4-39) calcula la pendiente de dicha tangente en coordenadas locales. Figura 4-23 Representación de una tangente interior m = R i+1 ( R i+1 C i+1, C i ) R i + R 2 2 R i+1 i+1 ( 4-39 ) En este caso, la expresión (4-39) [19] calcula la pendiente en coordenadas locales de la recta tangente interior que enlaza por la mitad inferior del primer círculo. En caso de desearse la tangente que enlaza por la mitad superior, la pendiente será el valor negativo de la calculada en (4-39). En el caso de las tangentes interiores, el vector que une el centro de la primera circunferencia con el punto de 57

72 Planificación global de trayectorias para robots móviles, basada en curvas β-spline tangencia, en coordenadas locales, se obtiene según la expresión (4-40): t = (1, 1 m )/ (1, 1 m ) ( 4-40 ) En este caso no ocurre como con las tangentes exteriores, donde la orientación del vector dependía de qué radio era mayor, como se mostraba en las expresiones (4-36). En el caso de las tangentes interiores, si enlazamos por la mitad inferior de la primera circunferencia, la tangente siempre será ascendente, y si enlazamos por la mitad superior, siempre será descendente. Una vez obtenidos el vector t y la pendiente m, se procede de forma análoga al caso de las tangentes exteriores, es decir, se transforman dichos valores a las coordenadas globales mediante las expresiones (4-37) y (4-38). A continuación se expone el método seguido para identificar si necesitamos enlazar las circunferencias por la mitad superior o inferior de la primera de ellas. Según este método, el factor determinante es la orientación del vector bisectriz, mostrado en la Figura 4.20 como b i. La Figura 4.24 muestra una conexión entre dos circunferencias, donde los centros de las mismas se encuentran sobre la misma horizontal (coordenadas locales). Figura 4-24 Representación de dos circunferencias y el vector bisectriz Una vez alineados los centros de las dos circunferencias, la orientación del vector bisectriz respecto a la nueva línea horizontal determinará la mitad por la que debemos enlazar las circunferencias. Así, si el vector bisectriz en coordenadas locales de la primera circunferencia, b i, apunta hacia la mitad inferior, significa que el vértice a interpolar se encuentra en la mitad superior de la circunferencia, por lo que tendremos que enlazar ambas circunferencias mediante una tangente que pase por la mitad superior, como en el caso de la Figura

73 En cambio, si el vector bisectriz local b i apunta hacia la mitad superior, tendremos que enlazar las circunferencias mediante una tangente que pase por la mitad inferior de la primera circunferencia. Vemos que no es necesario determinar en qué mitad cortará la tangente a la segunda circunferencia, ya que esto vendrá determinado por el tipo de tangente que vamos a utilizar. Así, si se trata de una tangente exterior, cortará a ambas circunferencias por la misma mitad, superior o inferior. En cambio, si se trata de una tangente interior, cortará a cada circunferencia en una mitad distinta. Para comprobar la posición del vector bisectriz con respecto a la línea que une los centros, basta con realizar una rotación del vector b i un ángulo β, y ver si la componente horizontal del vector una vez girado es positiva o negativa. Al igual que en casos anteriores, β es el ángulo que forma la línea que une ambos centros con la horizontal. De esta forma, deducimos por qué mitad queremos que pase la recta tangente, y siguiente el método expuesto a lo largo de este apartado, queda perfectamente definida la recta tangente que enlaza las dos circunferencias de forma correcta. El proceso de construcción de las tangentes puede resumirse, por tanto, en los siguientes pasos: - Saber si necesitamos una tangente exterior o interior. - Selección de la mitad por la que queremos enlazar. - Obtención de la pendiente y el punto de tangencia, con el método explicado. En el caso del primer y último punto del camino, se enlazan con el primer y último círculo, respectivamente, mediante una tangente al círculo correspondiente que pase por el punto extremo (inicial o final). De esta forma, una vez definidas todas las circunferencias y tangentes, ya está definida la curva sobre la que se situarán los vértices del polígono de control Construcción del polígono de control El último paso para el suavizado es la colocación de los vértices del polígono de control sobre la curva compuesta por los arcos de circunferencia y las rectas tangentes. Consideramos que las tangentes vienen definidas por los puntos de tangencia inicial y final, y las circunferencias vienen definidas por su centro y su radio. De esta forma, empezando en el punto inicial o primer waypoint, seleccionamos los puntos de la recta tangente que se encuentran a una distancia δ del último vértice seleccionado, hasta que el último vértice seleccionado se encuentre a una distancia del punto de tangencia final menor que δ. En este punto debemos encontrar qué punto de la circunferencia se encuentra a una distancia δ del último vértice seleccionado. Para ello, basta con encontrar los puntos de intersección entre un círculo de radio R i y centro en C i, y un círculo de radio δ y centro en el último vértice seleccionado p j. La resolución de este problema da dos resultados, pero nos quedaremos con el valor más lejano del penúltimo vértice seleccionado p j-1, para asegurarnos de que vamos avanzando. Esta situación se ilustra en la Figura

74 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 4-25 Búsqueda de vértices separados una distancia δ Para colocar los puntos sobre una recta, podemos hacerlo inmediatamente si calculamos el vector director. Sin embargo, cuando los vértices no pertenecen a la misma recta, actuamos según la Figura En ella aparecen en azul los puntos que ya hemos añadido al polígono de control, y en rojo el vértice que interpolará la β-spline final. Para encontrar qué punto de la circunferencia dista una distancia δ, buscamos la intersección de las dos circunferencias que se muestran en dicha figura. Este proceso se repite con cada vértice mientras que el último vértice encontrado se encuentre a una distancia del punto de tangencia inicial de la siguiente recta tangente menor que δ. Cuando esto ocurre, significa que ya se ha recorrido el arco de la circunferencia y necesitamos enlazar con la siguiente recta tangente, por lo que se actúa de igual manera que con la tangente inicial. Este proceso se repite para cada arco y cada recta, hasta llegar al punto final. Una vez acabado el proceso, se obtiene una serie de vértices del polígono de control colocados de forma equidistante sobre una serie de rectas y arcos de circunferencia (ver Figura 4.11). A partir de este polígono de control, más los vértices adicionales correspondientes para forzar unas condiciones iniciales y finales (apartado 4.3.2), se asegura que la curva β-spline generada interpolará a los distintos waypoints, y será una curva continua en posición, orientación y curvatura. Además, gracias a la separación constante entre los vértices del polígono de control, la curvatura se encontrará acotada y presentará una variación aproximadamente lineal, lo que facilita la labor del seguidor de caminos. Gracias a todas las consideraciones tomadas a lo largo del proceso, la curva generada supondrá una trayectoria segura y perfectamente realizable por el robot móvil para el que se ha ideado. Por tanto, se consigue así el objetivo de idear un método seguro y flexible, ya que no está sujeto a un robot móvil en concreto, si no que se adapta a las características deseadas simplemente modificando los parámetros de distancia de seguridad y radio mínimo admisible. 60

75 En el capítulo 5 se muestran los resultados obtenidos al aplicar este método a distintos mapas de obstáculos, y con distintos modelos de robots móviles. 61

76 Planificación global de trayectorias para robots móviles, basada en curvas β-spline 62

77 5 SIMULACIONES E ste capítulo pretende validar el método presentado, mostrando para ello una serie de simulaciones en distintos entornos, y con robots móviles de distintas características. En estas simulaciones se pretende encontrar una trayectoria segura entre un punto inicial y un punto final, de forma que sea realizable por un robot móvil. El objetivo es mostrar que se cumplen los dos requisitos principales del método: seguridad y flexibilidad, entendiendo flexibilidad como la capacidad de adaptar un camino para que sea realizable por un robot de características concretas Introducción Las simulaciones llevadas a cabo tienen lugar en tres mapas distintos, variando el número de obstáculos. Además, se varían los parámetros que definen al robot, como son la distancia de seguridad y el radio mínimo, y se comprueba cómo reacciona el método ante estos cambios. También se comparan las soluciones proporcionadas por los algoritmos de Dijkstra y A*, y se muestra que en algunos casos proporcionan soluciones diferentes. Se incluye al final del capítulo una tabla que incluye el tiempo invertido en cada una de las simulaciones, tanto con Dijkstra como con A*. Este tiempo refleja el tiempo invertido desde la ejecución del programa, hasta que se obtiene la trayectoria final a seguir. En el capítulo 4 se expuso que para saber si el suavizado de determinado tramo era seguro o no, se consideraba el caso más desfavorable. En este capítulo se comprueba hasta qué punto esta consideración es acertada, mediante un estudio que verifica cuántas veces se prohíbe un camino que en realidad es seguro. Las Figuras 5.1, 5.2 y 5.3 muestran los mapas considerados en las simulaciones: 63

78 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 5-1 Mapa 1 Figura 5-2 Mapa 2 Figura 5-3 Mapa 3 64

79 Resulta evidente la disparidad entre los distintos mapas, lo cual pretende demostrar que el método se adapta a mapas muy diferentes. El Mapa 1, mostrado en la Figura 5.1, tiene muy pocos obstáculos y parece a priori un mapa muy sencillo, pero en cambio el mapa de la Figura 5.2 (Mapa2) supone un entorno mucho más poblado de obstáculos. Por último, el mapa dela Figura 5.3 (Mapa3) pretende representar un entorno bastante poblado de obstáculos, donde los mismos presentan formas muy irregulares y desordenadas. Los tres mapas tienen un tamaño de 100x100, donde cada casilla se considera que tiene 1 metro de lado Simulaciones Todas las simulaciones mostradas en este capítulo se han realizado considerando un modelo de UAV que se mantiene a altura constante. La implementación del método presentado está orientada a planificación en dos dimensiones. Las ecuaciones que definen el modelo son las siguientes: x i = v cosθ i y i = v sinθ i θ i = α θ (θ c θ i ) ( 5-1 ) Siendo θ el ángulo formado con la horizontal (heading), α θ = 1.32s -1 y θ c el ángulo necesario para apuntar al siguiente waypoint desde el punto actual. La restricción en velocidad angular de este modelo es w < θ i < w, siendo w=0.28 rad/s. Discretizando las expresiones de (5-1) para un período de muestreo T, se obtienen las siguientes ecuaciones discretas para la posición en cada instante: x k+1 = v T cos(θ k ) + x k y k+1 = v T sin(θ k ) + y k ( 5-2 ) θ k+1 = α θ T (θ c θ k ) + θ k Éste es el modelo utilizado en todas las simulaciones, pero cambiando los valores de distancia de seguridad y radio mínimo de giro se consigue el mismo resultado que cambiando el modelo de robot móvil, por lo que se puede seguir esta estrategia para demostrar la flexibilidad del método. Cambiar el radio mínimo de giro equivaldría a modificar la velocidad constante que se quiere mantener en el trayecto. En primer lugar, vamos a mostrar los distintos pasos que se realizan para acabar encontrando la trayectoria final entre un punto inicial y otro final. Para ello, utilizamos el mapa 1. El punto inicial lo situamos en las coordenadas [5,95], que equivale a la columna 5 (eje x) y la fila 95 (eje y) en la matriz que define nuestro mapa. El punto objetivo será el [95,5] que se encuentra en el extremo opuesto del mapa. En cuanto a los parámetros que definen nuestro robot móvil, exigimos en este caso una distancia de seguridad de 1 metro, y un radio mínimo de 2 metros (lo que en el modelo utilizado equivale a una velocidad constante de 0.5 m/s). En la Figura 5.4, aparece el diagrama de Voronoi sin filtrar, con línea azul, y el camino elegido con línea roja discontínua. Tanto con el algoritmo de Dijkstra como con el de A* se obtiene el mismo resultado. 65

80 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 5-4 Diagrama de Voronoi sin filtrar y camino elegido En la Figura 5.5, se muestran las circunferencias generadas para suavizar el camino, así como las líneas tangentes, representados todos estos elementos en verde. Los puntos de tangencia están representados en dicha figura en verde, y los vértices del camino están resaltados en rojo. Figura 5-5 Circunferencias y tangentes sobre el camino Sobre las rectas y los arcos de circunferencia correspondientes, se colocan los vértices del polígono de control equiespaciados una distancia δ. Para que la curva β-spline interpole exactamente a los vértices que conforman el camino, deben caer 4 puntos consecutivos del polígono de control sobre la misma circunferencia. Si esto no se cumple, la curva resultante no interpolará exactamente a dichos vértices. Por ello, debido a que este método está orientado a entornos muy poblados de obstáculos donde será necesario realizar giros bruscos, enlazando arcos de circunferencias de radios pequeños, se elegirá en todas las simulaciones un valor pequeño de δ, de forma que podamos asegurar que siempre van a aparecer al menos cuatro vértices del polígono de control sobre cada arco de circunferencia. Por esta razón, la separación entre dichos vértices se escogerá tal que δ=r min / 3. Sin embargo, esta elección supone un valor orientativo, ya que según la aplicación para la que se desee aplicar el método puede interesar un valor de δ mayor o menor. 66

81 En la Figura 5.6 se muestra la curva β-spline generada, en rojo, y la trayectoria seguida al simular con el modelo, en verde. Vemos que sólo se observa la línea verde ya que ambas coinciden perfectamente, lo cual significa que la curva generada puede ser reproducida perfectamente por el robot móvil utilizado, ya que es continua en posición, orientación y curvatura, y no incumple ninguna de las restricciones del modelo. Las líneas amarillas representan la distancia de seguridad. Figura 5-6 Trayectoria obtenida con el modelo Diferencia entre Dijkstra y A* En la simulación anterior se obtiene el mismo resultando al elegir el camino óptimo mediante los algoritmos de Dijkstra y A*, pero esto no siempre sucede. Para ilustrarlo, se muestran a continuación los resultados de simular con las mismas condiciones que en el caso anterior, pero tomando como punto inicial el [10,10] y como punto final el [90,90]. Figura 5-7 Solución obtenida con el algoritmo A* 67

82 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 5-8 Solución obtenida con el algoritmo de Dijkstra En la Figura 5.7 se observa la solución obtenida al elegir el camino óptimo mediante el algoritmo de A*, y en la Figura 5.8 la solución obtenida tras escoger el camino mediante el algoritmo de Dijkstra. Aunque a simple vista ambas trayectorias parezcan muy distintas, la realidad es que la distancia recorrida en ambos casos es prácticamente idéntica. La causa de aparezcan dos soluciones distintas es, precisamente, el factor heurístico que se incluye en el algoritmo de A*. Al ser los dos caminos prácticamente idénticos en cuanto a coste, el algoritmo A* intenta acercarse en la medida de lo posible al punto final, por lo que no pasa entre los dos obstáculos de la izquierda, sino entre los dos obstáculos de la derecha, ya que éstos se encuentran más próximos al punto final deseado. El caso anterior supone por tanto un claro ejemplo de las posibles diferencias entre Dijkstra y A*, aunque en cuanto a distancia recorrida, ambas soluciones son prácticamente iguales Simulaciones variando el modelo Como se ha comentado previamente, variar el modelo se traduce en variar los parámetros de radio mínimo de giro y distancia de seguridad. Por ello, se muestran a continuación una serie de simulaciones en las que se varían dichos parámetros, para ver cómo se adapta el método a los distintos modelos. En todos los casos mostrados a continuación, se obtienen los mismos resultados con Dijkstra y con A*. Además, para calcular el coste de cada tramo, en todas las simulaciones consideramos K 1 =0.8 y K 2 =0.2 (ver expresión (4-4), en el apartado 4.2.1). - La siguiente simulación tiene lugar en un mapa más adecuado para este método, ya que está más poblado de obstáculos. El trayecto va del punto [10,10] al punto [94,80]. La distancia de seguridad se considera de 2 metros, y el radio mínimo de giro también es de 2 metros. 68

83 Figura 5-9 Simulación de [10,10] a [94,80] con d=2 m - Mismo caso que en la Figura 5.9, pero con radio mínimo 4 metros: Figura 5-10 Mismo caso, con R min =4 m - Se muestra a continuación un tramo más complicado dentro del mismo mapa. El trayecto deseado va da [95,8] a [20,80], la distancia de seguridad es de 2 metros, y el radio mínimo también es de 2 metros. 69

84 Planificación global de trayectorias para robots móviles, basada en curvas β-spline Figura 5-11 Simulación de [95,8] a [20,80]. d=2 m, R min =2 m - Mismo trayecto que en el caso anterior, pero con una distancia de seguridad de 3 metros. Figura 5-12 Mismo caso que en la Figura 5.11, pero d=3 m - Mismo caso, pero con distancia de seguridad 0.5 metros. 70

85 Figura 5-13 Mismo caso, con d=0.5 m Con las tres últimas simulaciones, se puede observar cómo el método busca el camino óptimo realizable, y acaba dando la mejor solución que evite colisiones. Así, en el caso de la Figura 5.13, en la que el robot prácticamente no tiene dimensiones, escoge el camino más óptimo posible, mientras que en los casos de la Figura 5.11 y la Figura 5.12, tiene que buscar otros caminos alternativos que resulten seguros, para los mismos puntos inicial y final. Estas tres últimas simulaciones podrían ser un ejemplo de cálculo de trayectorias para tres robots distintos, con sólo variar un parámetro, en este caso la distancia de seguridad. - Trayecto que va de [5,95] a [95,5], con distancia de seguridad igual a 0.5 metros y radio mínimo de 2 metros: Figura 5-14 De [5,95] a [95,5], con d=0.5 m, R min =2 m 71