Trabajo Fin de Grado - bibing.us.esbibing.us.es/proyectos/abreproy/90085/fichero/TFG_Alejandro_Munoz... · presenta un método orientado a la navegación en entornos de dos dimensiones

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

ii

iii

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


Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2014

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


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

vi

El Secretario del Tribunal

i

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.

ii

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.

iii

Índice

Agradecimientos i

Resumen ii

Índice iii

Índice de Figuras v

1 Introducción 1 1.1. Objetivo del trabajo 1 1.2. Estructura del trabajo 2

2 Estado del arte 5 2.1. Dijkstra 9 2.2. Algoritmo A* 9 2.3. Bellman-Ford 9 2.4. Algoritmo D* 10 2.5. Búsqueda en profundidad 11 2.6. Búsqueda en amplitud 11

3 Diagramas de Voronoi y modelado de obstáculos 13 3.1. Introducción 13 3.2. Diagramas de Voronoi 14

3.2.1. Construcción de un diagrama de Voronoi 14 3.2.2. Problemas al crear el diagrama de Voronoi 15

3.3. Modelado de obstáculos 15 3.3.1. Selección de vértices 16 3.3.2. Proyección de los vértices 20 3.3.3. Añadido de puntos adicionales 22

4 Selección del camino y suavizado con curvas beta-spline 27 4.1. Introducción 27 4.2. Algoritmos de planificación 29

4.2.1. Construcción de la matriz de costes 29 4.2.2. Algoritmo de Dijkstra 32 4.2.3. Algoritmo A* 34

4.3. Interpolación mediante curvas β-spline 35 4.3.1. Acotación cartesiana 39 4.3.2. Condiciones iniciales y finales de la curva β-spline 41

4.4. Selección del camino y suavizado 44 4.4.1. Selección del camino 45 4.4.2. Suavizado del camino 52

5 Simulaciones 63 5.1. Introducción 63 5.2. Simulaciones 65

5.2.1. Diferencia entre Dijkstra y A* 67

iv

5.2.2. Simulaciones variando el modelo 68 5.2.3. Trayectorias prohibidas por el suavizado 77

6 Conclusiones y trabajo futuro 83

Referencias 86

v

Í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

vi

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 vi. 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-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 Rmin=4 m 69

Figura 5-11 Simulación de [95,8] a [20,80]. d=2 m, Rmin=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, Rmin=2 m 71

Figura 5-15 Mismo caso, pero con d=2 m 72

Figura 5-16 Mismo caso, pero con Rmin=4 m 72

Figura 5-17 Trayectoria de [95,55] a [5,95] en el mapa 3 73

Figura 5-18 Mismo caso, con Rmin=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 Rmin=2 m 74

Figura 5-21 Trayectoria de [5,85] a [95,5], con d=2 m y Rmin=2 m. 75

Figura 5-22 Trayectoria de [5,95] a [95,5], con d=2 m y Rmin=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 Rmin=6 m 78

Figura 5-26 Primera de las trayectorias prohibidas, con las colisiones resaltadas 79

vii

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

1

1 INTRODUCCIÓN

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.

1.1. 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

E

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

2

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.

1.2. 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.

3

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.


4

5

2 ESTADO DEL ARTE

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

L


6

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

7

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 {p1,…,pn}, 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 {v1,…,vm}

son los vértices del diagrama de Voronoi, que coinciden con los puntos más alejados de los 3 obstáculos más

cercanos.


8

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)

9

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.

2.2. 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.

2.3. 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


10

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.

2.4. 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.

11

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.

2.6. 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]


12

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.

13

3 DIAGRAMAS DE VORONOI Y MODELADO DE

OBSTÁCULOS

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.

3.1. 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.

E


14

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.

3.2. 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.

3.2.1. 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 {p1,…,pn} en el plano P, con n≥2, y a cada pj le asociamos aquellos puntos del plano que están

más cerca o igual suya que de cualquier otro de los pi con i distinto de j. Todo punto del plano queda así

asociado a algún punto pi, 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 pi 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 {p1,…,pn} 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

15

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.

3.2.2. 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.

3.3. 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.


16

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.

3.3.1. 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.

17

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


18

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.

19

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


20

técnicas de modelado.

3.3.2. 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 va

comprobamos que existe algún otro vértice vb perteneciente a otro obstáculo distinto a una distancia menor, la

línea del diagrama no será equidistante al vértice va y a su proyección, sino a la proyección de va y al vértice vb.

Este sería el caso que se muestra en la Figura 3.8:

21

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 va 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

vb. 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:


22

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.

3.3.3. 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:

23

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.


24

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 (vx,vy), 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 (vx - d/2, vy + h/2). Por tanto, la distancia entre los

puntos P y V será la norma del vector que los une, de modulo:

𝐿 = √(𝑑

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 3.3.2.

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.

25

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

3.14. 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:


26

-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.

27

4 SELECCIÓN DEL CAMINO Y SUAVIZADO CON

CURVAS BETA-SPLINE

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.

4.1. 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

E


28

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.

29

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.

4.2.1. 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 {v1,…,vn}, 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 Di,j reflejará el coste que supone ir del vértice vi al vj, que será el mismo que Dj,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 Di,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:


30

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 Di,j:

- Comprobar que i≠j.

- Comprobar que vi está conectado con vj 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 Di,j=Dj,i.

- Si no se cumple la distancia de seguridad, o los vértices no están conectados, Di,j y Dj,i siguen valiendo

infinito.

-

Para calcular el coste, suponemos que tenemos los vértices vi y vj, 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 vi con vj, será definir este

segmento de la siguiente forma:

31

𝑤(𝜎) = (1 − 𝜎) ∗ 𝑣𝑖 + 𝜎 ∗ 𝑣𝑗

𝜎 ∈ [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:

𝜎∗ =(𝑣1 − 𝑝) ∗ (𝑣1 − 𝑣2)

𝑇

||𝑣1 − 𝑣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 vi y vj (llamado segmento w).

Por tanto, según el valor de σ*, la distancia entre el punto p y el vector que une vi con vj se calculará de forma

diferente:

Si σ* ≤ 0 → d = || p-vi ||

Si σ* ≥ 1 → d = || p-vj ||

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 vi a vj, y viceversa, se obtiene según la siguiente expresión:

𝐷𝑖,𝑗 = 𝐾1 ∗ ‖𝑣𝑖 − 𝑣𝑗‖ +𝐾2𝑑

( 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 K1 y K2, 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*.


32

4.2.2. 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.

33

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:

𝐷 =

(

0 6 56 0 4

8 ∞5 2

5 4 08 5 6∞ 3 ∞

6 ∞0 ∞2 0)

El nodo inicial es A, el final E, y la salida que devuelve la función en este sencillo caso es [A, B, E].


34

4.2.3. 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:

𝑓(𝑛) = 𝑔(𝑛) + 𝑕(𝑛) ( 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

35

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 5.

4.3. 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:

𝑝(𝜆0) = 𝑝0

𝑝(𝜆𝑓) = 𝑝𝑓

𝑝′(𝜆0) = 𝑝′0

𝑝′(𝜆𝑓) = 𝑝′𝑓

( 4-6 )

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:

𝑝(𝜆) = 𝑎0 + 𝑎1𝜆 + 𝑎2𝜆2 + 𝑎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.


36

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 Ti, en el punto inicial pi, el parámetro vale λ=0, y en el punto final pi+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]

𝑝𝑖(𝜆) = ∑ 𝐵𝑖,𝑘(𝜆)𝑉𝑖

𝑁−1

𝑖=0

( 4-8 )

Siendo N el número de vértices del polígono de control, {V0, V1,…, VN-1}, y Bi,k las funciones peso de orden k-

1, las cuales garantizan la continuidad Ck-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.

37

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, 𝛽2, 𝜆)𝑉𝑖+𝑟

1

𝑟=−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 Br 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 Br 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


38

expresiones:

𝑃𝑖+1(0) = 𝑃𝑖(1)

𝑃′𝑖+1(0) = 𝛽1 𝑃′𝑖(1)

𝑃′′𝑖+1(0) = 𝛽1 𝑃′′𝑖(1) + 𝛽2 𝑃′𝑖(1)

( 4-10 )

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]

3

2

1

0

1-

2- 1

6/1000

2/12/12/16/1

2/1103/2

6/12/12/16/1

)(B

)(B

)(B

)(B

x

( 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 Br 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 {v1,v2,v3,v4}, {v2,v3,v4,v5} y {v3,v4,v5,v6}.

Figura 4-8 β-spline de 6 vértices y 3 tramos

39

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.

4.3.1. 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, v0, v1} 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 4.10.


40

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:

𝑝(𝜆) = 𝐵−2(𝜆) 𝑣−2 + 𝐵−1(𝜆) 𝑣−1 +𝐵0(𝜆) 𝑣0 + 𝐵1(𝜆) 𝑣1

( 4-12 )

Particularizando las expresiones (4-11) para el punto inicial (𝜆 = 0), éstas quedan de la siguiente forma:

𝐵−2(0) =1

6

𝐵−1(0) =2

3

𝐵0(0) =1

6

𝐵1(0) = 0

( 4-13 )

Como se ve en estas expresiones, la posición del vértice v1 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.

𝑝𝑥(0) = −1

6𝑥 +

1

6𝑥 = 0

𝑝𝑦(0) =1

6𝑦 +

1

6𝑦 =

1

3𝑦

( 4-14 )

41

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:

𝑦 = 𝛿2

2𝑅

( 4-15 )

Y por lo tanto, es inmediato despejar:

𝑑 = 𝑅 − 𝑅′ = 𝛿2

6𝑅

( 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.

4.3.2. 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, v0} que hagan cumplir a la curva generada determinadas condiciones

iniciales. Siendo pi, 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)=pi

, 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:

𝑝𝑖(0) = 𝐵−2(0) 𝑣−2 + 𝐵−1(0) 𝑣−1 + 𝐵0(0) 𝑣0 + 𝐵1(0) 𝑣1

𝑝′𝑖(0) = 𝐵′−2(0) 𝑣−2 +𝐵′−1(0) 𝑣−1 + 𝐵′0(0) 𝑣0 + 𝐵′1(0) 𝑣1

𝑝′′𝑖(0) = 𝐵′′−2(0) 𝑣−2 + 𝐵′′−1(0) 𝑣−1 + 𝐵′′0(0) 𝑣0 + 𝐵′′1(0) 𝑣1

( 4-17 )

Para λ=0, tanto B1 como B’1 y B’’1 valen 0, luego la posición de v1 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):


42

𝑣−2 = 𝑝𝑖 − 𝑝′𝑖 +1

3𝑝′′𝑖

𝑣−1 = 𝑝𝑖 −1

6𝑝′′𝑖

𝑣0 = 𝑝𝑖 + 𝑝′𝑖 +1

3𝑝′′𝑖

( 4-18 )

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, v0, v1} que hagan cumplir a

la curva generada unas condiciones finales determinadas. En este caso, que debemos buscar que f(1)=pf ,

f’(1)=p’f y f’’(1)=p’’f, siendo pf, 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 B1 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:

𝑣−1 = 𝑝𝑓 − 𝑝′𝑓 +

1

3𝑝′′𝑓

𝑣0 = 𝑝𝑓 −1

6𝑝′′𝑓

𝑣1 = 𝑝𝑓 + 𝑝′𝑓 +

1

3𝑝′′𝑓

( 4-19 )

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 (𝑦′

𝑥′)

( 4-20 )

𝜅 =𝑥′𝑦′′ − 𝑥′′𝑦′

(√𝑥′2 + 𝑦′2)

3

( 4-21 )

En las expresiones anteriores, 𝜃 representa la orientación y 𝜅 la curvatura. Además, la separación entre los

43

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:

‖𝑣−1 − 𝑣−2‖2 = ∆𝑥2 + ∆𝑦2 = 𝑥′2 +

1

4𝑥′′2 − 𝑥′𝑥′′ + 𝑦′

2+1

4𝑦′′

2− 𝑦′𝑦′′ = 𝛿2

( 4-22 )

De la misma forma, la separación entre v1 y v0 según la expresión (4-19) también debe ser δ. Forzando esto, se

obtiene la siguiente expresión:

‖𝑣1 − 𝑣0‖2 = ∆𝑥2 + ∆𝑦2 = 𝑥′2 +

1

4𝑥′′2 + 𝑥′𝑥′′ + 𝑦′

2+1

4𝑦′′

2+ 𝑦′𝑦′′ = 𝛿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:

𝑥′ =𝛿

√𝑡𝑎𝑛2(𝜃) + 1

( 4-24 )

𝑦′ = 𝑥′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:

𝑦′′ = −𝑥′

𝑦′𝑥′′ = −

𝑥′′

tan (𝜃)

( 4-26 )

Despejando (4-26) en (4-21), obtenemos:

𝑥′′ = −𝜅 √𝑥′2 + 𝑦′2

3

𝑥′tan (𝜃)

+ 𝑦′

( 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).


44

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.

4.4. 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 4.11. En dicha figura, la línea roja discontinua representa el

camino a suavizar, formado por los waypoints {v1, v2, v3, v4, v5}, 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).

45

Figura 4-11 Representación del polígono de control para suavizar un camino de 5 vértices

4.4.1. 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.

4.4.1.1. 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, wmax. De esta forma, sabiendo el

valor de wmax del robot, y la velocidad v a la que queremos movernos, la restricción cinemática del robot se


46

traduce en un radio mínimo de giro que puede seguir, según la expresión (4-28).

𝑟𝑚𝑖𝑛 =𝑣

𝑤𝑚𝑎𝑥

( 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 rmin 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 v3 demasiado

cerca de v2.

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.25rmin, para evitar que

las circunferencias construidas sean secantes. A priori podría parecer que sería suficiente con que la distancia

47

entre los vértices fuera de 2rmin, 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.25rmin, 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 v3. Se comprueba por tanto la

seguridad del tramo {v2, v4} y, si se cumple la distancia de seguridad con los obstáculos en dicho tramo, se

sigue con el proceso a partir de v4. En caso de no cumplirse la distancia de seguridad entre v2 y v4, se probaría

eliminando el vértice v2. En este caso, habría que comprobar la seguridad del tramo que une v1 con v3. Si se

cumple la distancia de seguridad, el proceso seguirá con normalidad a partir de v3.

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 v2 y v3 (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.

4.4.1.2. 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 rmin 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 4.14.


48

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 vi, así como las líneas que lo unen con los vértices vi-1 y vi+1,

representadas en rojo, formando entre sí un ángulo α. El círculo de radio R que se genera tiene su centro en la

49

bisectriz de dicho ángulo α, y viene representado en la figura en azul. La circunferencia no contiene al vértice

vi 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 vi con vi-1 se

corresponde con la línea perpendicular a ésta que pasa por el centro de la circunferencia. Esta distancia es la

llamada hmax, y está representada en verde. Igualmente, para el tramo que une vi con vi+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 hmax es:

𝑕𝑚𝑎𝑥 = 𝑅 − (𝑅 − 𝑑) 𝑠𝑖𝑛𝛼

( 4-29 )

Sin embargo, este valor de hmax 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á:

𝑕𝑟𝑒𝑎𝑙 = 𝑅 − (𝑅 − 𝑑) 𝑠𝑖𝑛𝛼 − 𝑑

( 4-30 )

Sustituyendo el valor de d calculado en (4-16), obtenemos la siguiente expresión de la separación real:

𝑕𝑟𝑒𝑎𝑙 = 𝑅 − (𝑅 −𝛿2

6 𝑅) 𝑠𝑖𝑛𝛼 −

𝛿2

6 𝑅

( 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

hreal,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 {vi, vi-1} y {vi, vi+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 hreal,max. A continuación, deducimos el valor de Rmax mediante la

resolución de la ecuación de segundo grado (4-32), obtenida a partir de (4-31).

6 (1 − sin (𝛼

2))𝑅𝑚𝑎𝑥

2 − 6𝑕𝑟𝑒𝑎𝑙,𝑚𝑎𝑥𝑅𝑚𝑎𝑥 + 𝛿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<Rmax, y podrá ser seguido por nuestro robot siempre que R>Rmin.

Como se ha comentado previamente, en caso de que para un conjunto de tres vértices consecutivos


50

obtengamos un valor de Rmax<Rmin, 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.25Rmin).

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 v3 y v4 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 v2, v3 y v4 no sea seguro, se comprueba el caso marcado por la línea

negra en la Figura 4.17.

51

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 v4 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 v1 y v2, 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 Rmax 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 hmax 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.

4.4.1.3. 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


52

computación importante.

4.4.2. 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 4.4.1. 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 4.11.

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 4.3.1 y 4.3.2) se basa en

[19], pero en este trabajo se alcanzan y utilizan expresiones diferentes a las alcanzadas en dicha obra.

4.4.2.1. 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 vi.

53

En la Figura 4.18 se ha representado el tramo formado por los vértices vi y vi-1, separados una distancia Hi, y el

tramo acotado por los vértices vi y vi+1, separados una distancia Hi+1. Se construye en un primer paso la

circunferencia de radio Ri, que contiene inicialmente a vi 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 vi.

En cuanto al valor del radio Ri, del apartado anterior sabemos el valor máximo aceptable Ri,max que evita una

colisión, y por otra parte también sabemos el valor de Rmin 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 Hi y de Hi+1, ya que los vértices vi+1 y vi-1 en ningún caso deben estar contenidos en

la circunferencia de radio Ri. Por tanto, el radio se calculará mediante la expresión (4-33), donde H’i es un

valor que depende de Hi y Hi+1, como se verá posteriormente.

𝑅𝑖 =𝐻′𝑖

2 cos(𝛼𝑖2⁄ )

( 4-33 )

En caso de que el valor resultante de Ri sea mayor que Ri,max o menor que Rmin, 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

𝛼𝑖, 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.

𝐻′𝑖 =min (𝐻𝑖, 𝐻𝑖+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.

4.4.2.2. 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


54

comunes, como se muestra en la Figura 4.19. 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 T1 y T2, representadas en negro, son las tangentes exteriores, mientras

que T3 y T4, 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 {v1,…,v5}, que definen tres circunferencias.

Figura 4-20 Circunferencias y bisectrices en un tramo de 5 vértices

55

En la Figura 4.20 aparecen también representados los vectores que apuntan en la dirección de cada bisectriz,

𝑏1⃗⃗ ⃗, 𝑏2⃗⃗⃗⃗ 𝑦 𝑏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 Ci y Ci+1, calcularemos el producto escalar de los vectores bi y bi+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

𝑏1⃗⃗ ⃗ 𝑦 𝑏2⃗⃗⃗⃗ es negativo, por lo que unimos las circunferencias correspondientes mediante una tangente interior.

Por otra parte, el producto escalar de los vectores 𝑏2⃗⃗⃗⃗ 𝑦 𝑏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.

𝑚 = 𝑡𝑎𝑛𝛼 =𝑅𝑖+1 − 𝑅𝑖

√‖𝐶𝑖+1, 𝐶𝑖‖2 − (𝑅𝑖+1 − 𝑅𝑖)

2

( 4-35 )


56

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 𝑡 ), por

definición, es perpendicular a la recta tangente. Por tanto, la pendiente de este vector será 𝑚𝑝𝑒𝑟𝑝 = −1/𝑚,

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 Ri>Ri+1: 𝑡 = (1,−1

𝑚)/‖(1,−

1

𝑚)‖

Si Ri<Ri+1: 𝑡 = (−1,1

𝑚)/‖(−1,

1

𝑚)‖

( 4-36 )

De esta forma, el vector unitario 𝑡 , con origen en el centro Ci , nos permite saber la posición del punto de

tangencia. Sin embargo, tanto el vector 𝑡 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

57

De esta forma, la pendiente real de la recta tangente será la que se indica en la expresión (4-37):

𝑚𝑟𝑒𝑎𝑙 = tan(𝛼 + 𝛽) = tan (atan(𝑚) + 𝛽) ( 4-37 )

Suponiendo que el vector 𝑡 en coordenadas locales sea (t1, t2), el vector en coordenadas globales será:

𝑡𝑟𝑒𝑎𝑙⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑡1𝑐𝑜𝑠𝛽 − 𝑡2𝑠𝑖𝑛𝛽, 𝑡1𝑠𝑖𝑛𝛽 + 𝑡2𝑐𝑜𝑠𝛽) ( 4-38 )

La expresión (4-38) se deduce simplemente de girar un vector un determinado ángulo 𝛽. De esta forma,

mediante el vector 𝑡𝑟𝑒𝑎𝑙⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ obtenemos el punto de tangencia, y con la pendiente mreal 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

𝑚 =𝑅𝑖+1

√(𝑅𝑖+1‖𝐶𝑖+1, 𝐶𝑖‖𝑅𝑖 + 𝑅𝑖+1

)2 − 𝑅𝑖+12

( 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


58

tangencia, en coordenadas locales, se obtiene según la expresión (4-40):

𝑡 = (1,−1

𝑚)/ ‖(1,−

1

𝑚)‖

( 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 𝑡 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 bi.

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, bi, 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 4.24.

59

En cambio, si el vector bisectriz local bi 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 bi 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.

4.4.2.3. 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 Ri y centro en Ci, y un círculo de radio δ y centro

en el último vértice seleccionado pj. La resolución de este problema da dos resultados, pero nos quedaremos

con el valor más lejano del penúltimo vértice seleccionado pj-1, para asegurarnos de que vamos avanzando.

Esta situación se ilustra en la Figura 4.25.


60

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 4.25. 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.

61

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.


62

63

5 SIMULACIONES

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.

5.1. 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:

E


64

Figura 5-1 Mapa 1

Figura 5-2 Mapa 2

Figura 5-3 Mapa 3

65

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.

5.2. 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:

𝑥�̇� = 𝑣 𝑐𝑜𝑠𝜃𝑖

𝑦�̇� = 𝑣 𝑠𝑖𝑛𝜃𝑖

𝜃�̇� = 𝛼𝜃(𝜃𝑐 − 𝜃𝑖)

( 5-1 )

Siendo 𝜃 el ángulo formado con la horizontal (heading), 𝛼𝜃= 1.32s-1 y 𝜃𝑐 el ángulo necesario para apuntar al

siguiente waypoint desde el punto actual. La restricción en velocidad angular de este modelo es −𝑤 < 𝜃�̇� <𝑤, 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:

𝑥𝑘+1 = 𝑣 𝑇 cos(𝜃𝑘) + 𝑥𝑘

𝑦𝑘+1 = 𝑣 𝑇 sin(𝜃𝑘) + 𝑦𝑘

𝜃𝑘+1 = 𝛼𝜃 𝑇 (𝜃𝑐 − 𝜃𝑘) + 𝜃𝑘

( 5-2 )

É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.


66

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 δ=Rmin

/ 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.

67

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

5.2.1. 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*


68

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.

5.2.2. 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 K1=0.8 y K2=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.

69

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 Rmin=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.


70

Figura 5-11 Simulación de [95,8] a [20,80]. d=2 m, Rmin=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.

71

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, Rmin=2 m


72

- Mismo caso, pero con distancia de seguridad igual a 2 metros:

Figura 5-15 Mismo caso, pero con d=2 m

- A continuación, mostramos el mismo caso, pero con un radio mínimo de 4 metros.

Figura 5-16 Mismo caso, pero con Rmin=4 m

Se muestran a continuación una serie de simulaciones que tienen lugar en el Mapa 3. Este mapa es muy denso

de obstáculos y éstos tienen formas irregulares, por lo que supone un mapa complicado.

73

- Se busca una trayectoria con punto final en [5,95] y punto inicial en [95,55], para un robot móvil cuyo

radio mínimo de giro para cierta velocidad sea 2 metros, y que necesite una distancia de seguridad de

2 metros.

Figura 5-17 Trayectoria de [95,55] a [5,95] en el mapa 3

- Mismo caso que el anterior, pero con radio mínimo 4 metros.

Figura 5-18 Mismo caso, con Rmin=4 m

- Trayecto de [65,80] a [75,35], con distancia de seguridad de 1 metro y radio mínimo de giro dos

metros:


74

Figura 5-19 Trayectoria de [65,80] a [75,35], con d=1 m y Rmin=2 m

Finalmente, mostraremos a continuación tres simulaciones entre puntos situados en esquinas opuestas, con

distintos valores de distancia de seguridad y radio mínimo de giro, lo que equivale a usar tres modelos de

robots distintos.

- Distancia de seguridad de 1 metro, y radio mínimo 2 metros:


- Distancia de seguridad de 2 metros, y radio mínimo 2 metros.

75

Figura 5-21 Trayectoria de [5,85] a [95,5], con d=2 m y Rmin=2 m.

- Distancia de seguridad de 2 metros, y radio mínimo de 4 metros:


Con todas las simulaciones anteriores se demuestra la capacidad del método de adaptarse a distintos entornos

muy variados, así como a distintos modelos de robots móviles, traducidos en distintos valores de distancia de

seguridad y radio mínimo de giro. Además, se han representado los límites marcados por la distancia de

seguridad para demostrar que no se incumple en ningún caso, ya que una de las principales señas de identidad

del método es precisamente la seguridad.

La Tabla 5.1 refleja el tiempo invertido en obtener la solución final en cada una de las simulaciones

presentadas, tanto utilizando el algoritmo de Dijkstra como utilizando el de A*. El tiempo mostrado es el

transcurrido desde que se inicia el proceso hasta que se obtiene la curva β-spline propuesta como trayectoria,

es decir, el tiempo de cálculo completo.


76

Simulación Dijkstra (s) A* (s) Mapa

Figura 5.6 0.86 0.52 1

Figura 5.8/Figura 5.7 0.73 0.50 1

Figura 5.9 1.02 0.79 2

Figura 5.10 0.7 0.73 2

Figura 5.11 0.80 0.83 2

Figura 5.12 0.82 0.37 2

Figura 5.13 0.77 0.80 2

Figura 5.14 0.32 0.33 2

Figura 5.15 0.32 0.33 2

Figura 5.16 0.30 0.35 2

Figura 5.17 0.41 0.40 3

Figura 5.18 0.62 0.52 3

Figura 5.19 0.94 0.87 3

Figura 5.20 1.00 0.96 3

Figura 5.21 1.00 1.00 3

Figura 5.22 0.46 0.61 3

Tabla 5.1: Tiempo de cómputo para cada simulación, con Dijkstra y A*

Es importante destacar que dichas simulaciones, al igual que la implementación completa del método, se han

realizado en Matlab. Esto significa que los tiempos mostrados sólo pretenden ser orientativos, ya que el

programa Matlab está ejecutado en un PC, y la velocidad de éste en cada momento puede variar los tiempos de

cálculo. Así, para los mismos puntos inicial y final y los mismos parámetros del robot, se pueden obtener

valores bastante dispares de tiempo empleado en cada simulación, ya que no se puede controlar la velocidad

del PC en cada momento.

Teniendo en cuenta este factor, es posible sacar información útil de los tiempos mostrados en la Tabla 5.1. En

ella puede comprobarse que, a pesar de encontrarse valores de tiempo relativamente dispares, el tiempo de

cálculo prácticamente nunca supera un segundo. Esto supone una solución realmente buena, ya que el método

es capaz de encontrar una trayectoria óptima y segura, realizable en función de los parámetros considerados, y

el tiempo de cálculo no es para nada excesivo. Esto se consigue gracias a las consideraciones tomadas a lo

largo de todo el método para no aumentar innecesariamente el tiempo de cálculo.

Por otra parte, comparando entre los valores de tiempo para Dijkstra y A*, se comprueba que, en caso de

77

existir una diferencia de tiempo considerable entre ambos métodos para un mismo caso, generalmente es más

rápido el algoritmo A*. En el único caso de los presentados en el que el algoritmo de Dijkstra es

considerablemente más rápido es en de la Figura 5.22, y esto puede tener una explicación derivada de las

propiedades de cada algoritmo. El algoritmo A* en principio visita menos nodos, ya que orienta la búsqueda

hacia el punto de destino. Sin embargo, es posible que el número de caminos encontrados que no permitan un

suavizado seguro sea mayor con A* que con Dijkstra, lo que provocaría un mayor número de iteraciones y,

por tanto, un mayor tiempo de cómputo. Esto es lo que ha ocurrido en este caso, pero viendo el resto de

simulaciones se comprueba que, cuando hay un método más rápido que el otro, generalmente es el A* el que

encuentra la solución en el menor tiempo.

5.2.3. Trayectorias prohibidas por el suavizado

Por último, se muestran a continuación una serie de situaciones que han sido descartadas por nuestro

algoritmo, al predecirse que el suavizado no era seguro. En el capítulo 4 se comentó que, para prever si el

suavizado de determinado tramo era seguro o no, se consideraba el caso más desfavorable posible, y además

no se hacía distinción entre hacia qué lado se producía el suavizado. El objetivo de este apartado es verificar si

ésta es una suposición aceptable, o si por el contrario prohíbe un gran número de trayectorias que son seguras.

Para ello, se muestran a continuación algunas de las trayectorias descartadas en el proceso de selección del

camino óptimo para las situaciones mostradas en el apartado 5.2.2., y se comprueba si efectivamente dichas

trayectorias iban a provocar una colisión o no.

- En el mapa 1, se pretende ir del punto [10,10] al [65,80], con una distancia de seguridad de 5 metros,

y un radio mínimo de 6 metros. En la Figura 5.23 se muestra la trayectoria prohibida, y en la Figura

5.24 la trayectoria que finalmente se escogió.

Figura 5-23 Trayectoria prohibida por suavizado inseguro.


78

Figura 5-24 Trayectoria final segura obtenida.

- Trayectoria de [5,95] a [95,55], con distancia de seguridad de 1 metro y radio mínimo de giro de 6

metros:

Figura 5-25 Trayectoria prohibida de [5,95] a [95,55], con d=1 m y Rmin=6 m

- Trayecto de [5,95] a [95,5], con distancia de seguridad de 2 metros, y radio mínimo de 4 metros. Se

corresponde con la simulación mostrada en la Figura 5.22. La Figura 5.26 y la Figura 5.27 muestran

las dos trayectorias que se prohibieron antes de encontrar la trayectoria final escogida, mostrada en la

Figura 5.22:

79

Figura 5-26 Primera de las trayectorias prohibidas, con las colisiones resaltadas

Figura 5-27 Segunda de las trayectorias prohibidas, por salirse del mapa.

Hasta ahora se han mostrado algunas de las trayectorias que se han prohibido en las distintas simulaciones, y

vemos que siempre han sido prohibidas correctamente, ya que provocaban colisiones que han sido resaltadas

con círculos verdes en las figuras. Sin embargo, lanzando distintas simulaciones se ha encontrado un solo

caso en el que se prohíbe una trayectoria que resulta segura. Este caso se muestra en la Figura 5.28:


80

Figura 5-28 Trayectoria prohibida erróneamente, ya que es segura.

La Figura 5.28 muestra una trayectoria prohibida para ir del punto [5,95] al [95,5], con una distancia de

seguridad de 1 metro, y un radio mínimo de giro de 6 metros. Como se desprende de dicha figura, la

trayectoria pasa muy cerca del límite del mapa, pero no llega a violarse la distancia de seguridad. Para explicar

la causa de este error, se incluye la Figura 5.29, que muestra el tramo conflictivo.

Figura 5-29 Ampliación del tramo conflictivo, y representación de todas las líneas.

En la Figura 5.29 se muestran en azul las líneas del diagrama de Voronoi sin filtrar, la línea discontinua roja

muestra el camino elegido que se pretende suavizar, y la circunferencia verde es la generada para suavizar

dicho tramo. Cuando el diagrama de Voronoi es equidistante a los obstáculos, no hay problema en no

considerar hacia qué lado se produce la separación máxima al suavizar, ya que los obstáculos se sitúan a la

misma distancia a ambos lados. Sin embargo, en el caso mostrado en la Figura 5.29, aparecen dos vértices del

diagrama de Voronoi muy cercanos, por lo que se ha obviado uno de ellos, de forma que la línea del camino

correspondiente no es totalmente vertical, sino que presenta una pequeña inclinación, por lo que no es

exactamente equidistante respecto a los dos obstáculos más cercanos. Esto provoca que, si la separación se

81

produjera hacia el lado contrario, se violaría la distancia de seguridad por muy poco, y se produciría una

colisión. Por lo tanto, se prohíbe dicho tramo.

Resulta evidente que éste se trata de un caso muy especial, que no se dará con frecuencia. De hecho, vemos

que de todas las comprobaciones realizadas, éste ha sido el único caso en el que se ha prohibido una

trayectoria segura. Aun siendo segura, vemos que en este caso la diferencia entre ser segura e insegura es muy

pequeña (la separación de la línea que provoca el error con respecto a la vertical es mínima), por lo que la

consideración de ésta como una trayectoria peligrosa no supone un gran error, ya que es segura pero por muy

poco, por lo que el robot móvil pasaría muy cerca del límite del mapa.

Con las simulaciones mostradas en este apartado, queda justificada la consideración del peor caso posible a la

hora de prever si el suavizado es seguro, ya que evita procesos de iteración una vez que ya se ha iniciado el

suavizado, y prácticamente nunca induce a error, como se acaba de demostrar.


82

83

6 CONCLUSIONES Y TRABAJO FUTURO

lo largo de este trabajo se ha presentado un método para la generación de una trayectoria entre un

punto inicial y otro punto final, conociendo previamente el entorno en dos dimensiones en el que

queremos que se mueva el robot. Como se ha comentado en varias ocasiones a lo largo del trabajo, los

dos pilares sobre los que se cimenta el método son la seguridad y la flexibilidad.

El objetivo de encontrar una trayectoria lo más segura posible se consigue mediante la elección de los

diagramas de Voronoi como herramienta de partida. Sin embargo, tal y como se vio en el capítulo 3, los

programas que generan los diagramas de Voronoi toman como entrada una serie de puntos que definen a los

obstáculos, por lo que se hace necesario un estudio del modelado de los mismos, que se muestra en dicho

capítulo. Una vez seguido el proceso de modelado propuesto, se demuestra que el diagrama de Voronoi

resultante refleja fielmente los puntos del plano equidistantes de los dos obstáculos más cercanos, que era el

objetivo marcado al seleccionar los diagramas de Voronoi como herramienta. Por tanto, las líneas del

diagrama de Voronoi representan un grafo de líneas equidistantes de los obstáculos, y señalan por tanto los

posibles caminos más seguros.

Además, en el capítulo 3 se han mostrado los diagramas de Voronoi resultantes de dos mapas distintos una vez

aplicado el proceso de modelado presentado, donde ambos mapas presentan muy diferente densidad de

obstáculos. Se demuestra así que este método de modelado consigue un diagrama de Voronoi con buenas

características, independientemente de la cantidad de obstáculos que aparezcan en el mapa, siempre que éste

esté representado mediante una matriz de unos y ceros, lo que hace que las aristas de los obstáculos siempre

sean horizontales o verticales.

Además, cabe destacar que las consideraciones tomadas para evitar puntos innecesarios en el modelado de los

obstáculos supone un ahorro del tiempo de computación, ya que en mapas con una cantidad considerable de

obstáculos, la colocación de puntos que no cumplen ninguna función puede aumentar el tiempo de cómputo de

forma importante e innecesaria. Por ello, se ha buscado modelar los obstáculos sólo con los puntos necesarios

para que el diagrama de Voronoi resultante presente buenas características.

Para generar el diagrama de Voronoi sólo se ha utilizado la información del mapa, pero es a la hora de elegir el

camino que une los puntos inicial y final deseados cuando aparecen los parámetros que definen las

restricciones del robot. Así, nuestro programa necesita saber por un lado la distancia de seguridad deseada (que

depende de las dimensiones del robot), y por otro lado la velocidad angular máxima que puede alcanzar el

robot móvil y la velocidad constante a la que queremos movernos. Estas dos últimas características se traducen

en un parámetro de radio mínimo de giro que puede seguir el robot, para la velocidad determinada. Con los

parámetros de distancia de seguridad y radio mínimo de giro conseguimos obtener una secuencia de vértices

del diagrama de Voronoi o waypoints que definan un camino seguro, y que sea posible para el robot móvil

alcanzar todos los waypoints seleccionados.

A


84

El siguiente paso es obtener a partir de dicho camino una curva suave que interpole a los waypoints. Para ello

utilizamos las curvas β-spline de tal forma que, situando los vértices del polígono de control con determinada

disposición, conseguimos generar una curva continua en posición, orientación y curvatura, y que teóricamente

no viola las restricciones cinemáticas del robot móvil.

Cabe destacar que la elección de los diagramas de Voronoi como herramienta a partir de la cual obtener el

camino, también es beneficiosa para el posterior suavizado. Por ejemplo, en el caso de los grafos de

visibilidad, la cercanía de los waypoints a los obstáculos deja muy poco margen de maniobra al suavizado, ya

que se podría colisionar con un obstáculo al generar una curva suave que pase por dos vértices consecutivos

del mismo. Además, la estrategia seguida al eliminar puntos del diagrama de Voronoi para poder cumplir las

restricciones cinemáticas del robot se haría más complicada en el caso de los grafos de visibilidad, ya que

obviar un nodo supondría obviar un vértice de un obstáculo, provocando muy probablemente una colisión. Los

grafos de visibilidad presentarían por tanto complicaciones más difíciles de solventar cuando los obstáculos se

encuentran en determinadas configuraciones indeseadas.

En el capítulo 5 se demuestra, mediante la presentación de numerosas simulaciones, que el método presentado

consigue los objetivos marcados. Por un lado, la adecuada generación del diagrama de Voronoi suponía un

buen punto de partida para obtener una trayectoria segura, pero la forma de adaptar el camino según los

parámetros del robot móvil, manteniendo en todo momento la seguridad, se demuestra en el capítulo anterior

que consigue su objetivo de aportar flexibilidad al método. De esta forma, se demuestra que el programa

consigue generar una curva que puede ser seguida perfectamente como trayectoria, para distintos valores de

los parámetros y distintos trayectos. Se muestran además en algunos casos que, para un mismo punto inicial y

un mismo punto final, a veces las trayectorias proporcionadas como solución son distintas al cambiar los

parámetros del robot, ya que una curva que sirve como trayectoria a un robot de determinadas características

no tiene por qué servir para otro robot distinto, definido en última instancia por parámetros distintos. Se

demuestra así que se consigue el objetivo de obtener un planificador de trayectorias que se adapte a las

características del robot que se desee utilizar.

En todas las simulaciones mostradas en el capítulo 5 aparecen la curva generada y la trayectoria seguida al

simularlo con el modelo, y ambas coinciden perfectamente en todos los casos (por eso en todas las imágenes

parece que sólo aparece una curva). Esto ocurre gracias a que las curvas generadas son continuas en posición,

orientación y curvatura, y cumplen las restricciones cinemáticas impuestas por el robot en cuestión, por lo que

pueden ser seguidas por el seguidor de caminos perfectamente. Además, también se representan en todas las

figuras los límites de la distancia de seguridad, para demostrar que no se incumple en ningún caso.

Por otra parte, también se demuestra en el capítulo 5 que la actitud conservadora tomada al considerar el peor

caso posible para prever si al suavizar con la β-spline se producirá una colisión es acertada, ya que sólo en un

caso muy excepcional se prohibió una trayectoria segura. Además, se comprobó que dicha trayectoria era

segura por muy poco, por lo que no suponía un grave error, ya que el método presentado pretende pasar lo más

lejos posible de los obstáculos para maximizar la seguridad en entornos muy poblados de obstáculos. En el

resto de casos, las trayectorias prohibidas por prever un suavizado inseguro siempre han estado bien

prohibidas, como es el caso de los ejemplos mostrados en dicho capítulo 5.

De esta forma, mostrando las simulaciones realizadas en tres mapas muy distintos, y con distintos parámetros

del robot, se demuestra que el método ideado se adapta perfectamente tanto a diferentes entornos en dos

dimensiones, como a robots de distintas características, y acaba proporcionando trayectorias seguras.

Por último, en el capítulo 5 se muestra el tiempo de cómputo invertido desde que comienza el programa hasta

que ya se ha generado la curva β-spline propuesta como trayectoria, tanto para los algoritmos de Dijkstra como

85

A*. No se ha podido realizar un estudio exhaustivo sobre cómo crece el tiempo de cálculo al crecer el número

de obstáculos y la complejidad de los mismos, ya que todas las simulaciones se han realizado en Matlab (al

igual que el programa entero), y el tiempo de cálculo para un mismo caso variaba notablemente de una

simulación a otra, ya que depende en gran medida de la velocidad a la que esté operando el ordenador en cada

momento. Aun así, se pueden sacar algunas conclusiones. Al mirar la tabla 5.1 se comprueba que, de existir

una diferencia de tiempo considerable entre el proceso utilizando Dijkstra o utilizando A*, suele ser el

algoritmo A* el más rápido. En cualquier caso, se recuerda que el tiempo mostrado no corresponde al tiempo

que tardan Dijkstra o A* en encontrar el camino óptimo, sino que incluye todo el proceso de modelado de

obstáculos, generación del diagrama de Voronoi, selección del camino óptimo y suavizado del mismo.

Aunque, como se ha comentado previamente, el valor de los tiempos sea meramente orientativo, se comprueba

que en ningún caso se supera ampliamente el segundo de cálculo. Esto supone un resultado muy positivo, ya

que, por lo general, el proceso completo de planificación a partir de un mapa tarda menos de un segundo de

tiempo de computación. Este poco tiempo de computación se consigue gracias a las consideraciones tenidas en

cuenta a lo largo de todo el proceso, como la precaución de no incluir puntos innecesarios en el modelado de

los obstáculos, o intentar evitar iteraciones entre los procesos de selección de camino y suavizado del mismo,

de forma que se proceda a suavizar un camino cuando estemos seguros de que no va a dar ningún problema.

Queda así demostrado el éxito al conseguir un método que encuentre una trayectoria realizable en la que prime

la seguridad, y que se adapte a las características de un robot concreto.

El método presentado en este trabajo deja abierta la puerta a posibles mejoras. A continuación se proponen

algunos de los posibles trabajos futuros:

- Eliminar la restricción de mapas en dos dimensiones, y hacer el método extensible a planificación de

trayectorias en el espacio. Para ello, sería necesario escoger algún método para obtener los waypoints,

ya sea alguna estrategia análoga a los diagramas de Voronoi pero para tres dimensiones u otra

estrategia alternativa, y definir alguna estrategia para situar los vértices del polígono de control de

forma equidistante.

- Considerar una velocidad no constante en todo el trayecto. Esto podría traducirse en que no existiera

un radio mínimo constante en todo el trayecto, sino que hubiera una restricción de radio mínimo para

cada tramo, dependiendo de la velocidad que se siguiera en el mismo. En cualquier caso, en entornos

muy poblados de obstáculos suele trabajarse con velocidad constante, ya que prima la seguridad por

encima del tiempo empleado en el trayecto, pero se presenta en cualquier caso dicha posibilidad de

ampliación.

- Integrar el método presentado con algún método de planificación local. De esta forma, se podría

reaccionar ante entornos cambiantes u obstáculos inesperados. Gracias a la definición por partes de las

curvas β-spline, se podría generar una trayectoria inicial para alcanzar un punto final y, si durante la

ejecución de la misma el robot detecta algún obstáculo inesperado, sería posible recalcular un tramo

de β-spline alternativo, de forma que se esquivara el nuevo obstáculo y se enlazara de nuevo con la

trayectoria global calculada en primera instancia. Gracias a las propiedades de las curvas β-spline, esto

no supondría recalcular la curva completa, sino sólo el tramo afectado.


86

REFERENCIAS

[1] C. Goerzen, Z. Kong, and B. Mettler, “A survey of motion planning algorithms from the perspective of

autonomous uav guidance,” Journal of Intelligent and Robotic Systems, vol. 57, no. 1, pp. 65-100, 2010.

[2] Conde, R., Alejo, D., Cobano, J.A., Viguria, A., and Ollero, A. “Conflict detection and resolution method

for cooperating unmanned aerial vehicles”, Journal of Intelligent & Robotic Systems, 2012.

[3] Prasanna, H.M., Ghosey, D., Bhat, M.S., Bhattacharyya, C., and Umakant, J., “Interpolation-aware

trajectory optimization for a hypersonic vehicle using nonlinear programming”, In AIAA Guidance,

Navigation, and Control Conference and Exhibit, San Francisco, USA, 2005.

[4] Vela, A., Solak, S., Singhose, W., and Clarke, J.P., “A mixed integer program for flight-level assignment

and speed control for conflict resolution”, In Proceedings of the 48th IEEE Conference on Decision and

Control, 5219 –5226, 2009.

[5] Pallottino, L., Feron, E., and Bicchi, A., “Conflict resolution problems for air traffic management systems

solved with mixed integer programming”, Intelligent Transportation Systems, IEEE Transactions on, 3(1), 3–

11, 2002.

[6] Geiger, B., “Unmanned Aerial Vehicle Trajectory Planning with direct methods”, Ph.D. thesis, The

Pennsylvania State University, Pennsylvania, USA, 2009.

[7] Alejo, D., Cobano, J. A., Heredia, G., and Ollero, A., “Conflict-free 4D Trajectory Planning in Unmanned

Aerial Vehicles for Assembly and Structure Construction”, Journal Intelligent and Robotic Systems, Vol. 73,

pp. 783-795, 2014.

[8] Lavalle, S.M., ”Rapidly-exploring random trees: A new tool for path planning”, In Computer Science

Dept, Iowa State University, Tech. Rep. TR: 9811, 1998.

[9] Xue, M. y Atkins, E.M., “Terminal area trajectory optimization using simulated annealing”, In 44th AIAA

Aerospace Sciences Meeting and Exhibit. Reno, Nevada, USA, 2006.

[10] Durand, N. and Alliot, J., “Ant colony optimization for air traffic conflict resolution”, In Proceedings of

the Eighth USA/Europe Air Traffic Management Research and Development Seminar (ATM2009). Napa,

(CA, USA), 2009.

[11] Foskey, .M, Garber, M., Lin, M., and Manocha, D., “A voronoi-based hybrid motion planner”, In

Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2001.

87

[12] Rao, N., Stoltzfus, N., and Iyengar, S. S., “A retraction method for learned navigation in unknown terrains

for a circular robot”, IEEE Transactions on Robotics and Automation, 7(5), October 1991.

[13] Algorithms and more. http://jariasf.wordpress.com/2013/01/01/camino-mas-corto-algoritmo-de-bellman-

ford/. [En línea] 2013.

[14] Wikipedia. http://es.wikipedia.org/wiki/B%C3%BAsqueda_en_profundidad. [En línea]

[15] Wikipedia. http://es.wikipedia.org/wiki/B%C3%BAsqueda_en_anchura. [En línea]

[16] http://asignatura.us.es/fgcitig/contenidos/gctem3ma.htm. [En línea] Universidad de Sevilla

[17] Ollero, Aníbal. Robótica Avanzada; Planificación de robots. Universidad de Sevilla , 2014.

[18] Ollero Baturone, Aníbal, “Manipuladores y robots móviles”, Marcombo, 2001.

[19] Muñoz Martínez, D. Víctor Fernando, “Planificación de Trayectorias para Robots móviles”, Universidad

de Málaga, 1995.

Documents

Trabajo Fin de Grado - bibing.us.esbibing.us.es/proyectos/abreproy/90085/fichero/TFG_Alejandro_Munoz... · presenta un método orientado a la navegación en entornos de dos dimensiones