Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Método de muestreo híbrido para planificación de movimiento en entornos de almacén

Este ejemplo muestra cómo combinar enfoques de muestreo uniforme y muestreo gaussiano para la planificación del movimiento en un escenario de almacén que contiene pasillos estrechos y espacios amplios. En este ejemplo, usted:

Cree un muestreador personalizado que implemente un enfoque híbrido combinando enfoques de muestreo uniforme y de muestreo gaussiano para muestrear un espacio de estados de entrada.
Utilice las muestras de estado calculadas como semillas para la planificación del movimiento. Calcule una ruta óptima entre un estado inicial y un estado objetivo utilizando un planificador de ruta de hoja de ruta probabilística (PRM).
Compare los resultados del método de muestreo híbrido con los de los métodos de muestreo uniforme y gaussiano.

Introducción

La robótica y los sistemas autónomos suelen utilizar algoritmos de planificación basados en muestreo, como la hoja de ruta probabilística (PRM) y los árboles aleatorios de exploración rápida (RRT), para encontrar rutas libres de obstáculos para que un robot viaje desde su posición inicial hasta un objetivo deseado. La efectividad de estos enfoques depende de la estrategia utilizada para muestrear el espacio de configuración del robot. Las técnicas de muestreo comunes incluyen el muestreo uniforme y gaussiano.

El muestreo uniforme genera muestras uniformemente en todo el espacio de configuración del robot. Este método es sencillo, fácil de implementar y funciona bien para ambientes con espacios amplios. Sin embargo, el muestreador requiere una mayor cantidad de iteraciones para generar muestras óptimas para espacios con pasajes estrechos. Puede utilizar la función stateSamplerUniform para realizar un muestreo de estado uniforme.
El muestreo gaussiano utiliza una distribución gaussiana para generar muestras para la planificación de rutas. El muestreador genera muestras más concentradas a lo largo del límite del obstáculo. Como resultado, la estrategia de muestreo gaussiana es eficaz para espacios de trabajo complejos que contienen una alta densidad de obstáculos o regiones estrechas. En espacios amplios, debido a que las muestras están más concentradas a lo largo del límite, el muestreo gaussiano puede generar rutas más largas. Puede utilizar la función stateSamplerGaussian para realizar un muestreo de estado gaussiano.

Para entornos de muestreo que tienen pasillos estrechos y espacios amplios, puede utilizar un enfoque híbrido que combine las estrategias de muestreo uniforme y gaussiana. Este ejemplo utiliza un entorno de entrada que consta de un escenario de almacén que tiene pasillos estrechos y espacios amplios, para validar el rendimiento del enfoque híbrido. La figura muestra el plano del escenario del almacén utilizado en este ejemplo. El escenario tiene dos estaciones de clasificación, tres áreas de almacenamiento y áreas prohibidas como oficinas. El robot debe recoger los paquetes de una estación de clasificación y entregarlos en un lugar determinado del área de almacenamiento sin chocar con ningún obstáculo.

El resto del ejemplo explica cómo crear un muestreador personalizado y usarlo con el planificador PRM para calcular rutas óptimas para que el robot navegue desde un estado inicial determinado hasta el estado objetivo, sin chocar con ningún obstáculo.

Mapa de almacén de carga

Cargue un objeto binaryOccupancyMap, que contiene el plano del almacén, en el espacio de trabajo.

data = load("warehouseMapData.mat","warehouseMap");
map =  data.warehouseMap;

Mostrar e inspeccionar el mapa. Los estantes en las áreas de almacenamiento son los obstáculos, o áreas ocupadas, en el entorno del mapa de entrada. El robot no debe chocar con los obstáculos mientras navega de un lugar a otro en el entorno determinado.

figure
show(map)
title("Input Map")

Validar estados de entrada

Define los límites inferior y superior de las variables de espacio de estados x, y y theta del mapa de ocupación.

x = map.XWorldLimits;
y = map.YWorldLimits;
theta = [-pi pi];
StateBounds=[x; y; theta];

Cree un objeto SE(2) de espacio de estados utilizando las variables de espacio de estados especificadas. Verifique la validez de los estados en el espacio de estados de entrada utilizando un validador de estados. Establezca la distancia de validación en 0,01.

ss = stateSpaceSE2(StateBounds);
sv = validatorOccupancyMap(ss,Map=map);
sv.ValidationDistance = 0.01;

Crear muestra de estado personalizado

Utilice la función auxiliar helperStateSamplerHybrid para crear un muestreador personalizado que sea una subclase de la clase nav.StateSampler. Especifique un muestreador de estado uniforme y un muestreador de estado gaussiano como entradas para la función auxiliar helperStateSamplerHybrid. Especifique el porcentaje del total de muestras que se generarán mediante muestreo uniforme. La función auxiliar helperStateSamplerHybrid genera las muestras restantes utilizando el enfoque de muestreo gaussiano. Los valores de las propiedades de desviación estándar y número máximo de intentos del muestreador de estado gaussiano se han establecido experimentalmente. El muestreador personalizado devuelve un objeto de muestreo híbrido que almacena los muestreadores de estado uniforme y gaussiano y sus respectivas propiedades. Utilice el objeto de muestra híbrido como entrada a un planificador de ruta PRM para la planificación del movimiento.

uniformSampler = stateSamplerUniform(sv.StateSpace);
gaussianSampler = stateSamplerGaussian(sv,StandardDeviation=[1 1 1],MaxAttempts=50);
percentage = 50;
hybridSampler = helperStateSamplerHybrid(uniformSampler,gaussianSampler,percentage);

Configurar el planificador de rutas PRM

Para configurar el planificador de rutas PRM, utilice el muestreador de estados personalizado para muestrear el espacio de estados de entrada y configure la propiedad MaxNumNodes en 2000. Puede aumentar el valor de la propiedad MaxNumNodes para aumentar la posibilidad de encontrar una ruta. Sin embargo, esto también aumenta el tiempo de cálculo del planificador de rutas. Utilice los comandos tic y toc para verificar que el tiempo de cálculo del planificador sea alto, debido al número máximo de nodos considerados.

tic
hybridPlanner = plannerPRM(ss,sv,StateSampler=hybridSampler,MaxNumNodes=2000);
toc

Elapsed time is 272.319997 seconds.

Extraiga la hoja de ruta generada. Extraiga los nodos y bordes de la hoja de ruta.

digraphObj = hybridPlanner.graphData;
edgesHybrid = table2array(digraphObj.Edges);
nodesHybrid = table2array(digraphObj.Nodes);

Interpola estados entre los dos extremos de los bordes para obtener el árbol de búsqueda.

linksHybrid = [];
for i = 1:size(edgesHybrid,1)
    % Samples states interpolated at distance 0.02 meters
    states = interpolate(ss,nodesHybrid(edgesHybrid(i,1),:),nodesHybrid(edgesHybrid(i,2),:),0:0.02:1);
    % Add [NaN NaN] to the end to break the line plot
    linksHybrid = [linksHybrid; [states(:,1) states(:,2)]; NaN NaN];
end

Muestra los nodos y las aristas. Los bordes interconectados muestran rutas libres de colisiones que se pueden usar para calcular la solución de la ruta.

figure
show(map)
hold on
plot(linksHybrid(:,1),linksHybrid(:,2),plannerLineSpec.tree{:},DisplayName="Edges")
plot(nodesHybrid(:,1),nodesHybrid(:,2),plannerLineSpec.state{:},DisplayName="Nodes")
legend(Location="bestoutside")
hold off

Especificar estados de inicio y objetivo

Establezca el valor inicial para asegurarse de generar resultados repetibles.

rng("default")

Especifique el número de estados de inicio y objetivo para que navegue el robot. Puede especificar valores para los estados de inicio y objetivo o utilizar la función auxiliar helperGenerateRandomStartGoals para generar aleatoriamente los estados de inicio y objetivo. Este ejemplo utiliza la función auxiliar para generar los estados inicial y objetivo.

numStartGoal = 3;
[startLocations,goalLocations] = helperGenerateRandomStartGoals(sv,numStartGoal);

Visualice el mapa con todas las ubicaciones de inicio y meta seleccionadas al azar.

figure
show(map)
title("Selected Start and Goal States")
hold on
plot(startLocations(:,1),startLocations(:,2),plannerLineSpec.start{:})
plot(goalLocations(:,1),goalLocations(:,2),plannerLineSpec.goal{:})
hold off
legend(Location="bestoutside")

Calcular la ruta desde el estado inicial hasta el estado objetivo

Seleccione un estado inicial y un estado objetivo entre los cuales desea calcular la ruta. Establezca index1 en el estado de inicio deseado y index2 en los pares de estados de destino deseados para usar en la planificación del movimiento.

index1 = 2;
start = startLocations(index1,:);
index2 = 3;
goal = goalLocations(index2,:);

Calcule la ruta entre los estados de inicio y objetivo seleccionados utilizando el planificador PRM. El planificador de rutas utiliza las muestras de estado devueltas por el enfoque de muestreo híbrido como semillas para calcular la ruta entre los dos estados.

clear pathHybrid solnInfo
[pathHybrid,solnInfo] = plan(hybridPlanner,start,goal);

Mostrar los resultados. Especifique el color y las propiedades de línea predeterminados para trazar los puntos de inicio y destino utilizando las funciones plannerLineSpec.start y plannerLineSpec.goal, respectivamente. Especifique el color predeterminado y las propiedades de línea para trazar los estados de la ruta utilizando la función plannerLineSpec.path.

figure
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
if solnInfo.IsPathFound    
    plot(pathHybrid.States(:,1),pathHybrid.States(:,2),plannerLineSpec.path{:})    
    title("Path Computed Using Hybrid Sampling Approach and PRM Planner")
else
    title("Path not found")
end
hold off
legend(Location="bestoutside")

Compare el enfoque híbrido con los métodos de muestreo uniforme y gaussiano

Compare las muestras generadas por el enfoque de muestreo híbrido con las de los métodos de muestreo uniforme y gaussiano. Debido al alto tiempo de cálculo para la planificación de rutas, el ejemplo incluye salidas del planificador PRM precalculadas para los métodos de muestreo uniforme y muestreo gaussiano, almacenadas como archivos .mat.

Cargue la salida del planificador para el método de muestreo uniforme. La propiedad MaxNumNodes del planificador se ha establecido experimentalmente en 3000.

uniformPlanner = load("uniformPRM.mat");
uniformPlanner = uniformPlanner.planner;

Extraiga los nodos y los bordes, e interpola los estados entre los dos extremos de los bordes para obtener el árbol de búsqueda.

digraphObj = uniformPlanner.graphData;
edgesUniform = table2array(digraphObj.Edges);
nodesUniform = table2array(digraphObj.Nodes);
linksUniform = [];
for i = 1:size(edgesUniform,1)
    % Samples states interpolated at distance 0.02 meters
    states = interpolate(ss,nodesUniform(edgesUniform(i,1),:),nodesUniform(edgesUniform(i,2),:),0:0.02:1);
    % Add [NaN NaN] to the end to break line plot
    linksUniform = [linksUniform; [states(:,1) states(:,2)]; NaN NaN];
end

Cargue la salida del planificador para el método de muestreo gaussiano. La propiedad MaxNumNodes del planificador se ha establecido experimentalmente en 1500.

gaussianPlanner = load("GaussianPRM.mat");
gaussianPlanner = gaussianPlanner.planner;

Extraiga los nodos y los bordes, e interpola los estados entre los dos extremos de los bordes para obtener el árbol de búsqueda.

digraphObj = gaussianPlanner.graphData;
edgesGaussian= table2array(digraphObj.Edges);
nodesGaussian = table2array(digraphObj.Nodes);
linksGaussian = [];
for i = 1:size(edgesGaussian,1)
    % Samples states interpolated at distance 0.02 meters
    states = interpolate(ss,nodesGaussian(edgesGaussian(i,1),:),nodesGaussian(edgesGaussian(i,2),:),0:0.02:1);
    % Add [NaN NaN] to the end to break line plot
    linksGaussian = [linksGaussian; [states(:,1) states(:,2)]; NaN NaN];
end

Trazar los árboles y nodos de búsqueda obtenidos a partir de los enfoques de muestreo uniforme, gaussiano e híbrido. Utilice la función auxiliar helperDisplaySamples para trazar los árboles y nodos de búsqueda.

helperDisplaySamples(map,linksUniform,nodesUniform,linksGaussian,nodesGaussian,linksHybrid,nodesHybrid)

index1 = 2;
start = startLocations(index1,:);
index2 = 1;
goal = goalLocations(index2,:);

Calcule la ruta entre los estados inicial y objetivo seleccionados utilizando el planificador de ruta PRM.

clear uniformPath uniformInfo
[uniformPath,uniformInfo] = plan(uniformPlanner,start,goal);

clear gaussianPath gaussionInfo
[gaussianPath,gaussianInfo] = plan(gaussianPlanner,start,goal);

clear hybridPath hybridInfo
[hybridPath,hybridInfo] = plan(hybridPlanner,start,goal);

Trace e inspeccione las rutas generadas utilizando los enfoques de muestreo uniforme, gaussiano e híbrido. Utilice la función auxiliar helperDisplayStates para trazar las rutas.

helperDisplayPaths(map,start,goal,uniformPath.States,gaussianPath.States,hybridPath.States)

Compara las longitudes de las rutas.

uniformPathLength = pathLength(uniformPath);
gaussianPathLength = pathLength(gaussianPath);
hybridPathLength = pathLength(hybridPath);
table(uniformPathLength,gaussianPathLength,hybridPathLength)

ans=1×3 table
    uniformPathLength    gaussianPathLength    hybridPathLength
    _________________    __________________    ________________

         131.33                299.23               99.14

De los resultados, tenga en cuenta que:

El muestreador uniforme genera más muestras en los espacios amplios que en los pasajes estrechos. Sin embargo, para encontrar conexiones libres de colisiones entre los nodos en espacios amplios y los de pasajes estrechos, debe generar una mayor cantidad de nodos. Esto da como resultado un tiempo de cálculo mayor que los enfoques de muestreo gaussiano e híbrido, aunque el muestreo uniforme encuentra rutas más cortas que el enfoque de muestreo gaussiano.
El muestreador gaussiano genera muestras sólo a lo largo de los límites de los obstáculos. Como resultado, el muestreador gaussiano devuelve rutas más largas que los muestreadores uniformes o híbridos.
El enfoque de muestreo híbrido genera muestras tanto a lo largo de los límites de los obstáculos como en espacios amplios. El enfoque híbrido requiere menos tiempo de cálculo en comparación con el muestreador uniforme y genera rutas más cortas que el muestreador gaussiano cuando el estado inicial no está cerca del límite de un obstáculo y la distancia entre los estados inicial y objetivo es larga.

Funciones auxiliares

Este ejemplo incluye funciones auxiliares para establecer las propiedades de las figuras utilizadas para trazar los resultados del planificador obtenidos utilizando los enfoques de muestreo uniforme, gaussiano e híbrido.

Utilice la función auxiliar helperDisplaySamples para trazar el árbol de búsqueda y los nodos para cada enfoque de muestreo. Especifique el color predeterminado y las propiedades de línea para trazar el árbol de búsqueda y los nodos utilizando las funciones plannerLineSpec.tree y plannerLineSpec.state, respectivamente.

function helperDisplaySamples(map,linksUniform,nodesUniform,linksGaussian,nodesGaussian,linksHybrid,nodesHybrid)

fig_1 = figure(Position=[0 0 1400 300]);
movegui("center")
panel_1 = uipanel(fig_1, ...
    Position=[0 0 0.33 1]);
hPlot1 = axes(panel_1);
show(map,Parent=hPlot1)
hold on
plot(linksUniform(:,1),linksUniform(:,2),plannerLineSpec.tree{:},DisplayName="Edges")
plot(nodesUniform(:,1),nodesUniform(:,2),plannerLineSpec.state{:},DisplayName="Nodes")
title("Results Using Uniform Sampler")
hold off
legend(Location="bestoutside")

panel_2 = uipanel(fig_1, ...
    Position=[0.33 0 0.33 1]);
hPlot2 = axes(panel_2);
show(map,Parent=hPlot2)
hold on
plot(linksGaussian(:,1),linksGaussian(:,2),plannerLineSpec.tree{:},DisplayName="Edges")
plot(nodesGaussian(:,1),nodesGaussian(:,2),plannerLineSpec.state{:},DisplayName="Nodes")
title("Results Using Gaussian Sampler")
hold off
legend(Location="bestoutside")

panel_3 = uipanel(fig_1,...
    Position=[0.66 0 0.33 1]);
hPlot3 = axes(panel_3);
show(map,Parent=hPlot3);
hold on;
plot(linksHybrid(:,1),linksHybrid(:,2),plannerLineSpec.tree{:},DisplayName="Edges");
plot(nodesHybrid(:,1),nodesHybrid(:,2),plannerLineSpec.state{:},DisplayName="Nodes");
title("Results Using Hybrid Sampler")
hold off
legend(Location="bestoutside")
end

Utilice la función auxiliar helperDisplayPaths para trazar las rutas generadas. Especifique el color predeterminado y las propiedades de línea para trazar las rutas utilizando la función plannerLineSpec.path. Utilice las funciones plannerLineSpec.start y plannerLineSpec.goal para trazar los estados inicial y objetivo, respectivamente.

function helperDisplayPaths(map,start,goal,uniformStates,gaussianStates,hybridStates)
fig_2 = figure(Position=[0 0 1400 300]);
movegui("center")
panel_1 = uipanel(fig_2, ...
    Position=[0 0 0.33 1]);
hPlot1 = axes(panel_1);
show(map,Parent=hPlot1);
hold on
plot(start(1),start(2),plannerLineSpec.start{:}) 
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(uniformStates(:,1),uniformStates(:,2),plannerLineSpec.path{:})
title("Path Using Uniform Sampler")
hold off
legend(Location="bestoutside")

panel_2 = uipanel(fig_2, ...
    Position=[0.33 0 0.33 1]);
hPlot2 = axes(panel_2);
show(map,Parent=hPlot2);
hold on
plot(start(1),start(2),plannerLineSpec.start{:}) 
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(gaussianStates(:,1),gaussianStates(:,2),plannerLineSpec.path{:})
title("Path Using Gaussian Sampler")
hold off
legend(Location="bestoutside")

panel_3 = uipanel(fig_2, ...
    Position=[0.66 0 0.33 1]);
hPlot3 = axes(panel_3);
show(map,Parent=hPlot3)
hold on
plot(start(1),start(2),plannerLineSpec.start{:}) 
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(hybridStates(:,1),hybridStates(:,2),plannerLineSpec.path{:})
title("Path Using Hybrid Sampler")
hold off
legend(Location="bestoutside")
end

Consulte también