Planificación de movimiento

Este algoritmo se apoya en el uso del grafo de visibilidad para obtener todas las trayectorias posibles que pasan por los vértices de los polígonos (obstáculos). Una vez que se tiene el grafo de visibilidad se aplica un algoritmo para encontrar el camino más corto, dicho algoritmo puede ser el de Dijkstra.^[2]

Algoritmo ObtenerCamino (${\displaystyle S,p_{inicio},p_{fina}l}$)

//${\displaystyle S}$ Conjunto de polígonos disjuntos //${\displaystyle p_{inicio}}$ Punto de partida del robot //${\displaystyle p_{final}}$ Punto de llegada del robot ${\displaystyle G}$ ← CalcularGrafoVisibilidad(${\displaystyle S,p_{inicio},p_{final}}$) Asignar como peso de cada arista de ${\displaystyle G}$ la distancia euclidiana entre los vértices de la arista. ${\displaystyle ShortestPath}$ ← DijkstraShortestPath(${\displaystyle G,p_{inicio},p_{final}}$) return ${\displaystyle ShortestPath}$

La complejidad de calcular el grafo de visibilidad es ${\displaystyle O(n^{2}log(n))}$,^[2] y la complejidad del algoritmo de Dijkstra es ${\displaystyle O(n^{2})}$, por lo tanto la complejidad del algoritmo ObtenerCamino queda acotada por ${\displaystyle O(n^{2}log(n))}$.

La descomposición en celdas exactas es utilizada para representar el espacio libre de obstáculos, una vez que se conoce basta con calcular una ruta a través de este espacio. Para generar las celdas se usa un mapa trapezoidal. La construcción de un mapa trapezoidal se realiza tomando un conjunto de polígonos disjuntos, estos son encerrados en una caja para delimitar el espacio en que se crearán los trapezoides. A continuación se trazan líneas verticales en los vértices de cada polígono hasta que la línea alcanza algún obstáculo, ya sea la frontera de algún polígono o la «caja». Una vez que se tiene el mapa trapezoidal se deben calcular las celdas que representan al espacio libre, esto se puede hacer simplemente removiendo los trapezoides que se encuentran en el interior de los polígonos.

Para encontrar una trayectoria libre de obstáculos desde el punto inicial del robot hasta la meta se construye un grafo de trayectoria utilizando como nodos los puntos ubicados en los centros de los trapezoides y de las líneas verticales que constituyen el mapa, las aristas serán las semirrectas que conecten tales puntos únicamente en los casos en que un punto esté en el centro de un trapezoide y el otro en el centro de una línea vertical que constituya la frontera de dicho trapezoide.

Una vez que se tiene el grafo descrito anteriormente la construcción de un camino libre de obstáculos se puede hacer de acuerdo al siguiente algoritmo.

Algoritmo ObtenerCamino (${\displaystyle M_{t},G,p_{inicio},p_{final}}$)

//${\displaystyle M_{t}}$ Mapa trapezoidal del espacio libre de obstáculos //${\displaystyle G}$ Grafo de trayectoria calculado a partir del mapa trapezoidal //${\displaystyle p_{inicio}}$ Punto de partida del robot //${\displaystyle p_{final}}$ Punto de llegada del robot Encontrar los trapezoides ${\displaystyle t_{i},t_{f}}$ que contienen a los puntos ${\displaystyle p_{i},p_{f}}$ if ${\displaystyle t_{i},t_{f}}$ no existen return "camino inexistente" Obtener los vértices ${\displaystyle v_{i},v_{f}}$ que corresponden al centro de los trapezoides ${\displaystyle t_{i},t_{f}}$ ${\displaystyle Path}$ ← BreadFirstSearch(${\displaystyle G,p_{inicio},p_{final}}$) if ${\displaystyle Path}$ no existe return "camino inexistente" return ${\displaystyle {\overline {p_{inicio}v_{i}}}\cup Path\cup {\overline {v_{f}p_{final}}}}$

En este algoritmo las celdas corresponden a los elementos formados por una rejilla de tamaño fijo, en este caso los obstáculos son representados como celdas completamente ocupadas.

Dependiendo del tamaño de la rejilla puede pasar que algunos caminos libres sean marcados como ocupados, en estos casos una opción puede ser disminuir el tamaño de las celdas de la rejilla para aumentar la precisión.