Planificación de rutas en entornos de diferente complejidad
Este ejemplo muestra cómo calcular una ruta libre de obstáculos entre dos ubicaciones en un determinado mapa utilizando el planificador de rutas PRM (hoja de rutas probabilísticas). El planificador de rutas PRM construye una hoja de ruta en un espacio libre de un mapa determinado utilizando nodos de un muestreo aleatorio en el espacio libre y conectándolos entre sí. Una vez construida la hoja de ruta, puede hacer una consulta para encontrar una ruta desde una determinada ubicación inicial hasta una determinada ubicación final en el mapa.
En este ejemplo, el mapa se representa como un mapa de cuadrícula de ocupación que utiliza datos importados. Al realizar el muestreo de nodos en el espacio libre de un mapa, PRM utiliza esta representación de cuadrícula de ocupación binaria para deducir el espacio libre. Además, PRM no tiene en cuenta la dimensión del robot al calcular una ruta libre de obstáculos en un mapa. Por tanto, debe agrandar el mapa en la dimensión del robot para permitir el cálculo de una ruta libre de obstáculos que tenga en cuenta el tamaño del robot y garantice la evasión de colisiones para el robot real. Defina la ubicación inicial y final en el mapa para que el planificador de rutas PRM encuentre una ruta libre de obstáculos.
Importar mapas de ejemplo para planificar una ruta
load exampleMaps.mat
Los mapas importados son: simpleMap
, complexMap
y ternaryMap
.
whos *Map*
Name Size Bytes Class Attributes complexMap 41x52 2132 logical emptyMap 26x27 702 logical simpleMap 26x27 702 logical ternaryMap 501x501 2008008 double
Utilice los datos importados de simpleMap
y construya una representación de cuadrícula de ocupación utilizando el objeto binaryOccupancyMap
. Defina la resolución en 2 celdas por metro para este mapa.
map = binaryOccupancyMap(simpleMap,2);
Muestre el mapa utilizando la función show
en el objeto binaryOccupancyMap
.
show(map)
Definir las dimensiones del robot y agrandar el mapa
Para asegurarse de que el robot no choque con ningún obstáculo, debe agrandar el mapa en la dimensión del robot antes de enviarlo al planificador de rutas PRM.
Aquí, podemos asumir que la dimensión del robot es un círculo con un radio de 0,2 metros. Luego, puede agrandar el mapa en esta dimensión por medio de la función inflate
.
robotRadius = 0.2;
Como se ha señalado anteriormente, PRM no tiene en cuenta la dimensión del robot y, por tanto, enviar un mapa agrandado a PRM tiene en cuenta la dimensión del robot. Cree una copia del mapa antes de usar la función inflate
para conservar el mapa original.
mapInflated = copy(map); inflate(mapInflated,robotRadius);
Muestre el mapa agrandado.
show(mapInflated)
Construir PRM y definir los parámetros
Ahora debe definir un planificador de rutas. Cree un objeto mobileRobotPRM
y defina los atributos relacionados.
prm = mobileRobotPRM;
Asigne el mapa agrandado al objeto PRM.
prm.Map = mapInflated;
Defina el número de nodos de PRM que se utilizará durante la construcción de PRM. PRM construye una hoja de ruta utilizando el número de nodos indicado en un mapa determinado. Según la dimensión y la complejidad del mapa de entrada, este es uno de los atributos principales que hay que adaptar para obtener una solución entre dos puntos del mapa. Un número de nodos grande crea una hoja de ruta densa y aumenta la probabilidad de encontrar una ruta. En cambio, tener más nodos aumenta el tiempo de cálculo para crear la hoja de ruta y encontrar una solución.
prm.NumNodes = 50;
Defina la distancia máxima permitida entre dos nodos conectados en el mapa. PRM conecta todos los nodos separados por esta distancia (o inferior) en el mapa. Este es otro de los atributos que se deben ajustar cuando los mapas de entrada son grandes o complicados. Una distancia de conexión grande aumenta la conectividad entre nodos para encontrar una ruta más fácilmente, pero puede aumentar también el tiempo de cálculo de la creación de la hoja de ruta.
prm.ConnectionDistance = 5;
Encontrar una ruta factible en el PRM construido
Defina la ubicación inicial y final en el mapa que utilizará el planificador de rutas.
startLocation = [2 1]; endLocation = [12 10];
Busque una ruta entre la ubicación inicial y la final utilizando la función findpath
. La solución es un conjunto de waypoints de la ubicación inicial a la final. Tenga en cuenta que path
será diferente debido al carácter probabilístico del algoritmo PRM.
path = findpath(prm, startLocation, endLocation)
path = 7×2
2.0000 1.0000
1.9569 1.0546
1.8369 2.3856
3.2389 6.6106
7.8260 8.1330
11.4632 10.5857
12.0000 10.0000
Muestre la solución de PRM.
show(prm)
Usar PRM para un mapa grande y complicado
Utilice los datos importados de complexMap
, que representan un plano de planta grande y complicado, y construya una representación de cuadrícula de ocupación binaria con una determinada resolución (1 celda por metro).
map = binaryOccupancyMap(complexMap,1);
Muestre el mapa.
show(map)
Agrandar el mapa a partir de las dimensiones del robot
Copie y agrande el mapa para tener en cuenta el tamaño del robot y evadir obstáculos.
mapInflated = copy(map); inflate(mapInflated, robotRadius);
Muestre el mapa agrandado.
show(mapInflated)
Asociar el objeto PRM existente con el nuevo mapa y los parámetros definidos
Actualice el objeto PRM con el mapa que acaba de agrandar y defina otros atributos.
prm.Map = mapInflated;
Defina las propiedades NumNodes
y ConnectionDistance
.
prm.NumNodes = 20; prm.ConnectionDistance = 15;
Muestre la gráfica de PRM.
show(prm)
Encontrar una ruta factible en el PRM construido
Defina la ubicación inicial y final en el mapa para encontrar una ruta libre de obstáculos.
startLocation = [3 3]; endLocation = [45 35];
Busque una solución entre la ubicación inicial y la final. En los mapas complejos, es posible que no exista una ruta factible para un determinado número de nodos (devuelve una ruta vacía).
path = findpath(prm, startLocation, endLocation);
Como está planificando una ruta en un mapa grande y complicado, quizás necesite un mayor número de nodos. Sin embargo, a menudo resulta imposible determinar cuántos nodos serán suficientes. Ajuste el número de nodos para asegurarse de que exista una ruta factible entre la ubicación inicial y la final.
while isempty(path) % No feasible path found yet, increase the number of nodes prm.NumNodes = prm.NumNodes + 10; % Use the |update| function to re-create the PRM roadmap with the changed % attribute update(prm); % Search for a feasible path with the updated PRM path = findpath(prm, startLocation, endLocation); end
Muestre la ruta.
path
path = 12×2
3.0000 3.0000
4.2287 4.2628
7.7686 5.6520
6.8570 8.2389
19.5613 8.4030
33.1838 8.7614
31.3248 16.3874
41.3317 17.5090
48.3017 25.8527
49.4926 36.8804
⋮
Muestre la solución de PRM.
show(prm)