Planificación de trayectorias en entornos de diferente complejidad

Este ejemplo muestra cómo calcular una trayectoria libre de obstáculos entre dos ubicaciones en un determinado mapa utilizando el planificador de trayectorias basado en roadmap probabilístico (PRM) El planificador de trayectorias PRM construye un roadmap 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 construido el roadmap, puede hacer una consulta para encontrar una trayectoria 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 trayectoria 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 trayectoria 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 trayectorias PRM encuentre una trayectoria libre de obstáculos.

Importar mapas de ejemplo para planificar una trayectoria

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 trayectorias 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 trayectorias. 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 un roadmap 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 un roadmap denso y aumenta la probabilidad de encontrar una trayectoria. En cambio, tener más nodos aumenta el tiempo de cálculo para crear el roadmap 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 trayectoria más fácilmente, pero puede aumentar también el tiempo de cálculo de la creación del roadmap.

prm.ConnectionDistance = 5;

Encontrar una trayectoria factible en el PRM construido

Defina la ubicación inicial y final en el mapa que utilizará el planificador de trayectorias.

startLocation = [2 1];
endLocation = [12 10];

Busque una trayectoria 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 trayectoria factible en el PRM construido

Defina la ubicación inicial y final en el mapa para encontrar una trayectoria 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 trayectoria factible para un determinado número de nodos (devuelve una trayectoria vacía).

path = findpath(prm, startLocation, endLocation);

Como está planificando una trayectoria 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 trayectoria 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 trayectoria.

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
   44.3936   34.8592
   45.0000   35.0000

Muestre la solución de PRM.

show(prm)