Hybrid Sampling Method for Motion Planning in Warehouse Environment

Open Live Script

This example shows how to combine uniform sampling and Gaussian sampling approaches for motion planning in a warehouse scenario that contains narrow passages and wide spaces. In this example, you:

Create a custom sampler that implements a hybrid approach by combining uniform sampling and Gaussian sampling approaches to sample an input state space.
Use the computed state samples as seeds for motion planning. Compute an optimal path between a start state and goal state by using a probabilistic roadmap (PRM) path planner.
Compare the results of the hybrid sampling approach with that of the uniform sampling and Gaussian sampling methods.

Introduction

Robotics and autonomous systems commonly use sampling-based planning algorithms, such as probabilistic roadmap (PRM) and rapidly-exploring random trees (RRT), to find obstacle-free paths for a robot to travel from its starting position to a desired goal. The effectiveness of these approaches depends on the strategy used to sample the configuration space of the robot. Common sampling techniques include uniform and Gaussian sampling.

Uniform sampling generates samples uniformly across the entire configuration space of the robot. This method is simple, easy to implement and works well for environments with wide spaces. However, the sampler requires a greater number of iterations to generate optimal samples for spaces with narrow passages. You can use the stateSamplerUniform function to perform uniform state sampling.
Gaussian sampling uses a Gaussian distribution to generate samples for path planning. The sampler generates samples more concentrated along the obstacle boundary. As a result, the Gaussian sampling strategy is efficient for complex workspaces that contain high obstacle density or narrow regions. In wide spaces, because the samples are more concentrated along the boundary, Gaussian sampling can result in longer paths. You can use the stateSamplerGaussian function to perform Gaussian state sampling.

For sampling environments that have both narrow passages and wide spaces, you can use a hybrid approach that combines the uniform and Gaussian sampling strategies. This example uses an input environment consisting of a warehouse scenario that has narrow passages and wide spaces, to validate the performance of the hybrid approach. The figure shows the floor plan of the warehouse scenario used in this example. The scenario has two sorting stations, three storage areas, and off-limits areas like offices. The robot must pick packages from a sorting station and deliver them to a given location in the storage area without colliding with any obstacles.

The rest of the example explains how to create a custom sampler and use it with the PRM planner to compute optimal paths for the robot to navigate from a given start state to the goal state, without colliding with any obstacles.

Load Warehouse Map

Load a binaryOccupancyMap object, which contains the floor plan for the warehouse, into the workspace.

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

Display and inspect the map. The shelves in the storage areas are the obstacles, or occupied areas, in the input map environment. The robot must not collide with the obstacles while navigating from one location to another in the given environment.

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

Validate Input States

Define the lower and upper limits of the state space variables x, y, and theta from the occupancy map.

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

Create a state space SE(2) object using the specified state space variables. Check the validity of states in the input state space by using a state validator. Set the validation distance to 0.01.

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

Create Custom State Sampler

Use the helperStateSamplerHybrid helper function to create a custom sampler that is a subclass of the nav.StateSampler class. Specify a uniform state sampler and Gaussian state sampler as inputs to the helperStateSamplerHybrid helper function. Specify the percentage of total samples to be generated using uniform sampling. The helperStateSamplerHybrid helper function generates the remaining samples using the Gaussian sampling approach. The values of standard deviation and maximum number of attempts properties of the Gaussian state sampler have been set experimentally. The custom sampler returns a hybrid sampler object that stores the uniform and Gaussian state samplers and their respective properties. Use the hybrid sampler object as an input to a PRM path planner for motion planning.

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

Configure PRM Path Planner

To configure the PRM path planner, use the custom state sampler to sample the input state space, and set the MaxNumNodes property to 2000. You can increase the value of the MaxNumNodes property to increase the chance of finding a path. However, this also increases the computation time for the path planner. Use the tic and toc commands to verify that the computation time of the planner is high, due to the maximum number of nodes considered.

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

Elapsed time is 272.319997 seconds.

Extract the generated roadmap. Extract the nodes and edges from the roadmap.

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

Interpolate states between the two ends of the edges to get the search tree.

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

Display the nodes and the edges. The interlinked edges show collision-free paths to use for computing the path solution.

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

Specify Start and Goal States

Set the seed value to ensure you generate repeatable results.

rng("default")

Specify the number of start and goal states for the robot to navigate. You can either specify values for the start and goal states or use the helperGenerateRandomStartGoals helper function to randomly generate the start and goal states. This example uses the helper function to generate the start and goal states.

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

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

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

Compute Path from Start State to Goal State

Select a start state and goal state between which you want to compute the path. Set index1 to your desired start state and index2 to your desired goal state pairs to use for motion planning.

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

Compute the path between the selected start and goal states by using the PRM planner. The path planner uses the state samples returned by the hybrid sampling approach as seeds to compute the path between the two states.

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

Display the results. Specify the default color and line properties for plotting the start and goal points by using the plannerLineSpec.start and plannerLineSpec.goal functions, respectively. Specify the default color and line properties for plotting the path states by using the plannerLineSpec.path function.

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 Hybrid Approach with Uniform and Gaussian Sampling Methods

Compare the samples generated by the hybrid sampling approach with those of the uniform sampling and Gaussian sampling methods. Because of the high computational time for path planning, the example includes precomputed PRM planner outputs for the uniform sampling and Gaussian sampling methods, stored as .mat files.

Load the planner output for the uniform sampling method. The MaxNumNodes property of the planner has been experimentally set to 3000.

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

Extract the nodes and edges, and interpolate states between the two ends of the edges to get the search tree.

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

Load the planner output for the Gaussian sampling method. The MaxNumNodes property of the planner has been experimentally set to 1500.

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

Extract the nodes and edges, and interpolate states between the two ends of the edges to get the search tree.

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

Plot the search trees and nodes obtained from the uniform, Gaussian, and hybrid sampling approaches. Use the helperDisplaySamples helper function to plot the search trees and nodes.

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

Select a start state and goal state between which you want to compute the path. Set index1 to your desired start state and index2 to your desired goal state pairs to use for motion planning.

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

Compute the path between the selected start and goal states by using the PRM path planner.

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

Plot and inspect the paths generated using the uniform, Gaussian, and hybrid sampling approaches. Use the helperDisplayStates helper function to plot the paths.

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

Compare the path lengths.

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

From the results, note that:

The uniform sampler generates more samples in the wide spaces than in the narrow passages. However, to find collision-free connections between the nodes in wide spaces and those in narrow passages, it must generate a greater number of nodes. This results in higher computation time than the Gaussian and hybrid sampling approaches, even though uniform sampling finds shorter paths than the Gaussian sampling approach.
The Gaussian sampler generates samples only along the obstacle boundaries. As a result, the Gaussian sampler returns longer paths than the uniform or hybrid samplers.
The hybrid sampling approach generates samples both along the obstacle boundaries and in the wide spaces. The hybrid approach takes less computation time compared to the uniform sampler, and generates shorter paths than the Gaussian sampler when the start state is not close to an obstacle boundary and the distance between the start and goal states is long.

Helper Functions

This example includes helper functions to set the properties of the figures used for plotting the planner results obtained using the uniform, Gaussian, and hybrid sampling approaches.

Use the helperDisplaySamples helper function to plot the search tree and nodes for each sampling approach. Specify the default color and line properties for plotting the search tree and nodes by using the plannerLineSpec.tree and plannerLineSpec.state functions, respectively.

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

Use the helperDisplayPaths helper function to plot the generated paths. Specify the default color and line properties for plotting the paths by using the plannerLineSpec.path function. Use the plannerLineSpec.start and plannerLineSpec.goal functions to plot the start and goal states, respectively.

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