Benchmark Path Planners for Differential Drive Robots in Warehouse Map

Open Live Script

This example shows how to choose the best 2-D path planner for a differential drive robot in a warehouse environment from the available path planners. Use the plannerBenchmark object to benchmark the path planners plannerRRT, plannerRRTStar, plannerBiRRT, plannerPRM, and plannerHybridAstar on the warehouse environment with the randomly chosen start and goal poses. Compare the path planners based on their ability to find a valid path, clearance from the obstacles, time taken to initialize a planner, time taken to find a path, length of the path, and smoothness of the path. A suitable planner is chosen based on the performance of each path planner on the above mentioned metrics.

Set Up Environment

Create a binaryOccupancyMap object from a warehouse map. In the warehouse, the mobile robot moves from the loading station to the unloading station to place the goods.

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

Visualize the map.

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

Define Environment and Planners for Benchmarking

Specify the minimum turning radius for the differential drive robot.

minTurningRadius = 2.2; % meters

Create a stateSpaceDubins object with the state space bounds to be the same as map limits. set the minimum turning radius.

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

Create a validatorOccupancyMap state validator with the Dubins state space using the map. Specify the distance for interpolating and validating path segments.

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

Define the function handles for the initialization functions of each planners. For more information on these initialization functions, see 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);

Define the function handle for the plan function, which is common for all the planners.

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

Randomly Select Start-Goal Pairs on Warehouse Map

The start and goal locations are randomly sampled from the loading and unloading station area, respectively. Specify the number of start-goal pairs that must be randomly generated. In this example only three start-goal pair are chosen to reduce the execution time of this example. Increase the start-goal pair number to get sufficient map coverage.

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

The start location of the robot is sampled from the rectangle area marked as loading station and goal location is sampled from the rectangle area marked as unloading station area. Robot locations are sampled uniformly in the marked area. The rectangle area is specified as a vector of form [x y w h]. x and y specify the coordinate of the bottom left corner of the rectangle. w and h specify the width and height of the rectangle.

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

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

Visualize the map with all the randomly sampled start and goal locations.

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

Planner Benchmark Object Creation and Running Benchmark

Create a plannerBenchmark object for each start-goal pair and add the planners for benchmarking. The planner are executed twice on the same environment and start-goal pair by setting the runCount value to 2. This ensures the metrics results are accurate since sampling based planners like plannerRRT, plannerRRTStar, plannerBiRRT, and plannerPRM produce different output on the same set of start-goal pair. In this example runCount value is set to 2 to reduce the execution time of this example. Increase the runCount value to get more accurate benchmark results.

% 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.

Average Metrics Across all Start-Goal Pairs

All the planner are executed runCount times for each start-goal pair. Also all the planners are executed for all the start-goal pairs. This means that all planners are executed runCount*numStartGoalPairs times. The plots and tables below show the average metric value across all the start-goal pairs. The tables represent the Mean, Median, and Standard Deviation of each metric averaged across all the start-goal pairs.

The clearance metric represent the minimum distance of the path from the obstacles in the environment. The plot shows that plannerPRM has the highest clearance.

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

The isPathValid metric represent the success rate of each planner expressed in percentage. The plot shows that all the path planners produced valid path for all the start-goal pairs.

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

The executionTime metric represent the time taken by the plan function to execute. The plot shows that plannerPRM took the least time, followed by plannerBiRRT. We could also note that plannerRRTStar took lesser time than plannerRRT, this could be due to the lesser number of start-goal pairs used for benchmarking.

helperPlotAveragedMetrics(benchmarkList,runCount,"executionTime")

execTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"executionTime")

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

    rrt              0.4558     0.45447     0.12857
    rrtStar         0.22664     0.23817    0.062804
    biRRT           0.13557     0.10835    0.039044
    plannerPRM     0.032204    0.025505    0.013205
    hybridAstar     0.43528      0.4846     0.24292

The initializationTime metric indicate the time taken to execute the initialization function of each planner. Hence total execution time is the sum of plan function execution time and initialization time. plannerPRM has the longest initialization time. Hence plannerBiRRT took the least total execution time.

helperPlotAveragedMetrics(benchmarkList,runCount,"initializationTime")

initTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"initializationTime")

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

    rrt            0.012779    0.012702    0.0029436
    rrtStar        0.027303    0.030691    0.0099035
    biRRT          0.033718    0.018219     0.028493
    plannerPRM        2.195      2.2824      0.66862
    hybridAstar    0.021325    0.017558    0.0088038

The pathLength metric represent the length of the generated path. plannerHybridAstar has the shortest path, followed by 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

The smoothness metric represent the smoothness of the path for all poses. plannerBiRRT produced the smoothest path (lower smoothness value indicate smoother path) followed by plannerPRM. Observe that plannerRRTStar produced considerably smoother path compared to 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

If the slightly longer path is not a concern then consider using plannerBiRRT for path planning in a warehouse map setup as shown in this example, since it has the smoothest path and lesser execution time. If the path travelled by the robot should be the least then plannerHybridAstar could be considered.

As it can be seen, the inconsistency in execution time like, plannerRRTStar taking lesser time than plannerRRT, increasing the number of start-goal pairs and runCount to a considerably larger value will produce more accurate metrics results and better judgement of which path planner to choose.

Visualize Metric Results for Specific Start-Goal Pair

The above tables and graphs showed the metric results averaged across all the start-goal pairs. plannerBiRRT produced smoother path overall but the path length was slightly longer than plannerHybridAstar, which produced the least path length. Probe the start-goal pairs to see which produced the longest path length for 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

Visualize Path From All Planners for Specific Start-Goal Pair

Visualize the path output from all the planners for the start-goal pair that produced the longest path length for 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

Initialization Functions for Planners

Initialization function for 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

Initialization function for plannerBiRRT.

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

Initialization function for plannerRRTStar.

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

Initialization function for plannerRRT.

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