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

Planificadores de ruta de referencia para robots de accionamiento diferencial en el mapa de almacén

Este ejemplo muestra cómo elegir el mejor planificador de rutas 2-D para un robot de accionamiento diferencial en un entorno de almacén entre los planificadores de rutas disponibles. Utilice el objeto plannerBenchmark para comparar los planificadores de ruta plannerRRT, plannerRRTStar, plannerBiRRT, plannerPRM y plannerHybridAstar en el entorno del almacén con las poses de inicio y objetivo elegidas al azar. Compare los planificadores de rutas en función de su capacidad para encontrar una ruta válida, la distancia a los obstáculos, el tiempo necesario para inicializar un planificador, el tiempo necesario para encontrar una ruta, la longitud de la ruta y la suavidad de la ruta. Se elige un planificador adecuado en función del desempeño de cada planificador de ruta en las métricas mencionadas anteriormente.

Configurar el entorno

Cree un objeto binaryOccupancyMap a partir de un mapa de almacén. En el almacén, el robot móvil se desplaza desde la estación de carga a la estación de descarga para colocar la mercancía.

map = load("wareHouseMap.mat").logicalMap;
map = binaryOccupancyMap(map);

Visualiza el mapa.

figure
show(map)
title("Warehouse Floor Plan")
% Set the location of text displayed on the map.
loadingStationTextLoc = [40 9 0];
unloadingStationTextLoc = [7.5 42 0];
hold on
text(loadingStationTextLoc(1),loadingStationTextLoc(2),1,"Loading Station");
text(unloadingStationTextLoc(1),unloadingStationTextLoc(2),1,"Unloading Stations");
hold off

Definir entorno y planificadores para el benchmarking

Especifique el radio de giro mínimo para el robot de accionamiento diferencial.

minTurningRadius = 2.2; % meters

Cree un objeto stateSpaceDubins con los límites del espacio de estados para que sean los mismos que los límites del mapa. establezca el radio mínimo de giro.

stateSpace = stateSpaceDubins([map.XWorldLimits; map.YWorldLimits; [-pi pi]]);
stateSpace.MinTurningRadius = minTurningRadius;

Cree un validador de estado validatorOccupancyMap con el espacio de estado de Dubins usando el mapa. Especifique la distancia para interpolar y validar segmentos de ruta.

validator = validatorOccupancyMap(stateSpace,Map=map);
validator.ValidationDistance = 0.01*(1/map.Resolution); % meters

Defina los identificadores de funciones para las funciones de inicialización de cada planificador. Para obtener más información sobre estas funciones de inicialización, consulte Initialization Functions for Planners.

rrtInit = @(validator) plannerRRTWrapper(validator);
rrtStarInit = @(validator) plannerRRTStarWrapper(validator);
birrtInit = @(validator) plannerBiRRTWrapper(validator);
haStarInit = @(validator) plannerHybridAStarWrapper(validator,minTurningRadius);
prmInit = @(validator) plannerPRM(validator.StateSpace,validator);

Defina el identificador de función para la función del plan, que es común para todos los planificadores.

planFcn = @(initOutput,start,goal) plan(initOutput,start,goal);

Seleccione aleatoriamente pares de inicio-objetivo en el mapa del almacén

Las ubicaciones de inicio y meta se toman aleatoriamente del área de la estación de carga y descarga, respectivamente. Especifique el número de pares inicio-meta que se deben generar aleatoriamente. En este ejemplo, solo se eligen tres pares inicio-meta para reducir el tiempo de ejecución de este ejemplo. Aumente el número del par inicio-meta para obtener suficiente cobertura del mapa.

% Set default random number for repeatability of results.
rng("default")
% Select the number of start-goal pairs.
numStartGoalPairs = 3;

La ubicación inicial del robot se toma como muestra del área del rectángulo marcada como estación de carga y la ubicación de destino se toma como muestra del área del rectángulo marcada como área de la estación de descarga. Las ubicaciones de los robots se muestrean uniformemente en el área marcada. El área del rectángulo se especifica como un vector de la forma [x y w h]. x y y especifican la coordenada de la esquina inferior izquierda del rectángulo. w y h especifican el ancho y alto del rectángulo.

loadingArea = [51.5 11 5 10];
startLocations = helperSampleSelectedAreaOnMap(validator,loadingArea,numStartGoalPairs);

unloadingArea = [2 43.5 27 15];
goalLocations = helperSampleSelectedAreaOnMap(validator,unloadingArea,numStartGoalPairs);

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

show(map)
title("Warehouse Floor Plan")
% Indicate the loading and unloading station on the map.
hold on
text(loadingStationTextLoc(1),loadingStationTextLoc(2),1,"Loading Station");
rectangle(Position=loadingArea)

text(unloadingStationTextLoc(1),unloadingStationTextLoc(2),1,"Unloading Stations");
rectangle(Position=unloadingArea)

% Set the length of the line representing the pose in the start-goal visualization.
r = 2;

% Display all the start-goal pairs on the map.
for i=1:numStartGoalPairs
    start = startLocations(i,:);
    goal = goalLocations(i,:);
    
    % Define the legend displayed on the map    
    startString = strcat("start location",num2str(i));
    goalString = strcat("goal location",num2str(i));
    % Display start and goal location of robot.
    p1 = plot(start(1,1),start(1,2),"o",DisplayName=startString);
    c1 = p1.Color;
    p2 = plot(goal(1,1),goal(1,2),"o",DisplayName=goalString);
    c2 = p2.Color;
    % Display start and goal headings.
    plot([start(1) start(1)+r*cos(start(3))],[start(2) start(2)+r*sin(start(3))],...
         "-",Color=c1,HandleVisibility="off")
    plot([goal(1) goal(1)+r*cos(goal(3))],[goal(2) goal(2)+r*sin(goal(3))],...
         "-",Color=c2,HandleVisibility="off")
        
end
hold off
legend(Location="northeastoutside")

Punto de referencia del planificador Creación de objetos y punto de referencia en ejecución

Cree un objeto plannerBenchmark para cada par de inicio-meta y agregue los planificadores para la evaluación comparativa. El planificador se ejecuta dos veces en el mismo entorno y par inicio-objetivo estableciendo el valor runCount en 2. Esto garantiza que los resultados de las métricas sean precisos, ya que los planificadores basados ​​en muestreos como plannerRRT, plannerRRTStar, plannerBiRRT y plannerPRM produce resultados diferentes en el mismo conjunto de par inicio-meta. En este ejemplo, el valor runCount se establece en 2 para reducir el tiempo de ejecución de este ejemplo. Aumente el valor runCount para obtener resultados de referencia más precisos.

% To store the generated benchmark objects for each start-goal pair.
benchmarkList = cell(1,numStartGoalPairs);
% Specify the number of times each planner to be executed on the same
% set of start-goal pair.
runCount = 2;

for i=1:size(startLocations,1)
    % Get each start and goal location from all the sampled locations.
    start = startLocations(i,:);
    goal = goalLocations(i,:);

    % Set default random number for repeatability of results.
    rng("default")
    % Construct benchmark object.
    benchmark = plannerBenchmark(validator,start,goal);
    % Add planners for benchmarking using initialization and plan function
    % handles. Additional optional input NumPlanOutput define the number of
    % outputs returned from the plan function.
    addPlanner(benchmark,planFcn,rrtInit,PlannerName="rrt",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,rrtStarInit,PlannerName="rrtStar",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,birrtInit,PlannerName="biRRT",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,prmInit,PlannerName="plannerPRM",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,haStarInit,PlannerName="hybridAstar",NumPlanOutput=2)
    % Run the benchmark.
    runPlanner(benchmark,runCount)
    % Store the benchmark for further analysis.
    benchmarkList{i} = benchmark;
end

Initializing rrt ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using biRRT.
Executing run 1.
Executing run 2.
Initializing plannerPRM ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using plannerPRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using hybridAstar.
Executing run 1.
Executing run 2.
Initializing rrt ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using biRRT.
Executing run 1.
Executing run 2.
Initializing plannerPRM ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using plannerPRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using hybridAstar.
Executing run 1.
Executing run 2.
Initializing rrt ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using biRRT.
Executing run 1.
Executing run 2.
Initializing plannerPRM ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using plannerPRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using hybridAstar.
Executing run 1.
Executing run 2.

Métricas promedio en todos los pares de inicio-meta

Todos los planificadores se ejecutan runCount veces para cada par inicio-meta. Además, se ejecutan todos los planificadores para todos los pares inicio-meta. Esto significa que todos los planificadores se ejecutan runCount*numStartGoalPairs veces. Los gráficos y tablas a continuación muestran el valor métrico promedio en todos los pares de inicio-meta. Las tablas representan la media, la mediana y la desviación estándar de cada métrica promediadas en todos los pares inicio-meta.

La métrica clearance representa la distancia mínima de la ruta desde los obstáculos en el entorno. El gráfico muestra que plannerPRM tiene la autorización más alta.

helperPlotAveragedMetrics(benchmarkList,runCount,"clearance")

clearanceAverage = helperCalculateAverageMetricTable(benchmarkList,"clearance")

clearanceAverage=5×3 table
                    Mean     Median     stdDev 
                   ______    ______    ________

    rrt             1.069      1       0.097631
    rrtStar             1      1              0
    biRRT               1      1              0
    plannerPRM     1.3333      1         0.4714
    hybridAstar         1      1              0

La métrica isPathValid representa la tasa de éxito de cada planificador expresada en porcentaje. El gráfico muestra que todos los planificadores de rutas produjeron una ruta válida para todos los pares inicio-meta.

helperPlotAveragedMetrics(benchmarkList,runCount,"isPathValid")

isPathValidAverage = helperCalculateAverageMetricTable(benchmarkList,"isPathValid")

isPathValidAverage=5×3 table
                   Mean    Median    stdDev
                   ____    ______    ______

    rrt            100      100        0   
    rrtStar        100      100        0   
    biRRT          100      100        0   
    plannerPRM     100      100        0   
    hybridAstar    100      100        0   

La métrica executionTime representa el tiempo que tarda la función del plan en ejecutarse. El gráfico muestra que plannerPRM tomó el menor tiempo, seguido de plannerBiRRT. También podríamos notar que plannerRRTStar tomó menos tiempo que plannerRRT, esto podría deberse al menor número de pares inicio-meta utilizados para la evaluación comparativa.

helperPlotAveragedMetrics(benchmarkList,runCount,"executionTime")

execTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"executionTime")

execTimeAverage=5×3 table
                     Mean       Median      stdDev 
                   ________    ________    ________

    rrt             0.33969     0.32277    0.045932
    rrtStar         0.13195    0.097166    0.067152
    biRRT           0.12853     0.16041    0.046992
    plannerPRM     0.023943    0.019155    0.007343
    hybridAstar     0.28662     0.24865     0.17631

La métrica initializationTime indica el tiempo necesario para ejecutar la función de inicialización de cada planificador. Por lo tanto, el tiempo total de ejecución es la suma del tiempo de ejecución de la función del plan y el tiempo de inicialización. plannerPRM tiene el tiempo de inicialización más largo. Por lo tanto , plannerBiRRT tomó el menor tiempo total de ejecución.

helperPlotAveragedMetrics(benchmarkList,runCount,"initializationTime")

initTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"initializationTime")

initTimeAverage=5×3 table
                     Mean       Median      stdDev  
                   ________    ________    _________

    rrt            0.014618    0.014967    0.0013914
    rrtStar        0.019332    0.017205    0.0081274
    biRRT          0.054229    0.016885     0.057827
    plannerPRM       2.1401      1.4704       1.0155
    hybridAstar    0.020806    0.016017    0.0099022

La métrica pathLength representa la longitud de la ruta generada. plannerHybridAstar tiene la ruta más corta, seguido de plannerBiRRT.

helperPlotAveragedMetrics(benchmarkList,runCount,"pathLength")

pathLengthAverage = helperCalculateAverageMetricTable(benchmarkList,"pathLength")

pathLengthAverage=5×3 table
                    Mean     Median    stdDev
                   ______    ______    ______

    rrt            65.992    69.037    7.3361
    rrtStar        64.426    67.992    5.3652
    biRRT           59.68    63.763    8.4716
    plannerPRM     99.498    101.43    4.1558
    hybridAstar    52.575    50.487    5.1352

La métrica smoothness representa la suavidad de la ruta para todas las poses. plannerBiRRT produjo la ruta más suave (un valor de suavidad más bajo indica una ruta más suave) seguida de plannerPRM. Observe que plannerRRTStar produjo una ruta considerablemente más suave en comparación con plannerRRT.

helperPlotAveragedMetrics(benchmarkList,runCount,"smoothness")

smoothnessAverage = helperCalculateAverageMetricTable(benchmarkList,"smoothness")

smoothnessAverage=5×3 table
                     Mean       Median      stdDev 
                   ________    ________    ________

    rrt             0.32247     0.31304    0.054903
    rrtStar         0.21445     0.23351    0.038925
    biRRT          0.064741    0.029512    0.069532
    plannerPRM     0.087386    0.090682    0.009506
    hybridAstar      1.5048       1.647     0.21204

Si la ruta un poco más larga no es una preocupación, considere usar plannerBiRRT para la planificación de rutas en una configuración de mapa de almacén como se muestra en este ejemplo, ya que tiene la ruta más fluida y el menor tiempo de ejecución. Si la ruta recorrida por el robot fuera el mínimo, entonces se podría considerar plannerHybridAstar .

Como se puede observar, la inconsistencia en el tiempo de ejecución como, plannerRRTStar toma menos tiempo que plannerRRT, aumentando el número de pares inicio-meta y runCount a un valor considerablemente mayor producirá resultados de métricas más precisos y un mejor juicio sobre qué planificador de ruta elegir.

Visualice resultados de métricas para un par de inicio-meta específico

Las tablas y gráficos anteriores mostraron los resultados de las métricas promediados en todos los pares de inicio-meta. plannerBiRRT produjo una ruta más suave en general, pero la longitud de la ruta fue ligeramente más larga que plannerHybridAstar, que produjo la menor longitud de ruta. Sondee los pares inicio-meta para ver cuál produjo la longitud de ruta más larga para plannerBiRRT.

pathLengthMean = zeros(1,numStartGoalPairs);
% Iterate over all the benchmarks and store the mean path length for each
% start-goal pair.
for i=1:numStartGoalPairs
    benchmark = benchmarkList{i};
    pathLengthTable= benchmark.metric("pathLength");
    pathLengthMean(i) = pathLengthTable.Mean("biRRT"); 
end
% Find the index of the benchmark which produced largest mean path length
% value for plannerBiRRT.
[~,largestPathLengthIdx] = max(pathLengthMean);
benchmarkLargestPathlength = benchmarkList{largestPathLengthIdx};

show(benchmarkLargestPathlength,"pathLength")

pathLength = metric(benchmarkLargestPathlength,"pathLength")

pathLength=5×4 table
                    Mean     Median    StdDev     sampleSize
                   ______    ______    _______    __________

    rrt            73.058    73.058     3.3079        2     
    rrtStar        68.444    68.444    0.49012        2     
    biRRT          67.393    67.393     1.3109        2     
    plannerPRM     101.43    101.43          0        2     
    hybridAstar    59.643    59.643          0        2     

Visualice la ruta de todos los planificadores para un par de inicio-meta específico

Visualice el resultado de la ruta de todos los planificadores para el par inicio-meta que produjo la longitud de ruta más larga para plannerBiRRT.

% Retrieve the start and goal location from the benchmark object.
start = benchmarkLargestPathlength.Start;
goal = benchmarkLargestPathlength.Goal;

% Display the path from start location to goal location for all the path
% planners for all the runs in runCount.
for run=1:runCount
    figure
    show(map)
    title(['Path output from all planners for Run ' num2str(run)])
    hold on

    % show start and goal positions of robot.
    plot(start(1,1),start(1,2),"o")
    plot(goal(1,1),goal(1,2),"o")
    % Find the planner names used for benchmarking.
    plannerNames = fieldnames(benchmarkLargestPathlength.PlannerOutput);
    numPlanners = length(plannerNames);
    runString = strcat("Run",num2str(run));
    % Iterate and plot path of each planner for the specified run.
    for i=1:numPlanners
        plannerName = plannerNames{i};
        plannerOutput = benchmarkLargestPathlength.PlannerOutput.(plannerName).PlanOutput.(runString);
        pathObj = plannerOutput{1};
        plot(pathObj.States(:,1),pathObj.States(:,2),"-","LineWidth",2)
    end
    % Specify the legends.
    labels = [{"start location","goal location"} plannerNames'];
    legend(labels,location="northeastoutside")  
    hold off
end

Funciones de inicialización para planificadores

Función de inicialización para plannerHybridAStar.

function planner = plannerHybridAStarWrapper(validator,minTurningRadius)
    map = validator.Map;
    ss = stateSpaceSE2;
    sv = validatorOccupancyMap(ss,Map=map);
    sv.ValidationDistance = validator.ValidationDistance;
    planner = plannerHybridAStar(sv,MinTurningRadius=minTurningRadius);
end

Función de inicialización para plannerBiRRT.

function planner = plannerBiRRTWrapper(sv)
    planner = plannerBiRRT(sv.StateSpace,sv);
    planner.EnableConnectHeuristic = true;
    planner.MaxIterations = 5e4;
    planner.MaxNumTreeNodes = 5e4;
    planner.MaxConnectionDistance = 5;
end

Función de inicialización para plannerRRTStar.

function planner = plannerRRTStarWrapper(sv)
    planner = plannerRRTStar(sv.StateSpace,sv);
    planner.MaxIterations = 5e5;
    planner.MaxNumTreeNodes = 5e5;
    planner.MaxConnectionDistance = 3.8;
end

Función de inicialización para plannerRRT.

function planner = plannerRRTWrapper(sv)
    planner = plannerRRT(sv.StateSpace,sv);
    planner.MaxIterations = 5e5;
    planner.MaxNumTreeNodes = 5e5;
    planner.MaxConnectionDistance = 3.8;
end