Main Content

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 inflar 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 prevenció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)

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains an object of type image.

Definir las dimensiones del robot e inflar el mapa

Para asegurarse de que el robot no choque con ningún obstáculo, debe inflar 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. A continuación, puede inflar 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 inflado 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 inflado

show(mapInflated)

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains an object of type image.

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 inflado 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)

Figure contains an axes object. The axes object with title Probabilistic Roadmap contains 4 objects of type image, line, scatter.

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)

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains an object of type image.

Inflar el mapa a partir de las dimensiones del robot

Copie e infle el mapa para tener en cuenta el tamaño del robot y evadir obstáculos

mapInflated = copy(map);
inflate(mapInflated, robotRadius);

Muestre el mapa inflado.

show(mapInflated)

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains an object of type image.

Asociar el objeto PRM existente con el nuevo mapa y los parámetros definidos

Actualice el objeto PRM con el mapa que acaba de inflar 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)

Figure contains an axes object. The axes object with title Probabilistic Roadmap contains 3 objects of type image, line, scatter.

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)

Figure contains an axes object. The axes object with title Probabilistic Roadmap contains 4 objects of type image, line, scatter.