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
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
José Antonio Cobano Suárez
Dep. de Ingeniería de Sistemas y Automática
Escuela Técnica Superior de Ingeniería
Universidad de Sevilla
Sevilla, 2014
Trabajo Fin de Grado: Generación global de trayectorias para robots móviles, basada en curvas betaspline
Autor: Alejandro Muñoz Cueva
Tutores: Aníbal Ollero Baturone
José Antonio Cobano Suárez
El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:
Presidente:
Vocales:
Secretario:
Acuerdan otorgarle la calificación de:
Sevilla, 2014
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-2 Mapa 2 64
Figura 5-3 Mapa 3 64
Figura 5-4 Diagrama de Voronoi sin filtrar y camino elegido 66
Figura 5-5 Circunferencias y tangentes sobre el camino 66
Figura 5-6 Trayectoria obtenida con el modelo 67
Figura 5-7 Solución obtenida con el algoritmo A* 67
Figura 5-8 Solución obtenida con el algoritmo de Dijkstra 68
Figura 5-9 Simulación de [10,10] a [94,80] con d=2 m 69
Figura 5-10 Mismo caso, con 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.
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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]
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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*.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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].
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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):
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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).
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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 )
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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*
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Figura 5-20 Trayectoria de [5,85] a [95,5], con d=1 m y Rmin=2 m
- 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:
Figura 5-22 Trayectoria de [5,95] a [95,5], con d=2 m y Rmin=4 m
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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:
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
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
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.
Planificación global de trayectorias para robots móviles, basada en curvas β-spline
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.