Path Planning Using MPNet for Automated Parking Valet System

Generate Parking Valet Scenario

Read a parking scene image into the MATLAB workspace. The input image must be a binary image.

inputScene = imread("parkingScene.bmp");

Specify the cell size to create a binary occupancy map of the parking scene image. The cell size determines the resolution of the occupancy map.

cellSize = 0.5;

Create a binary occupancy map with a resolution of 1/cellSize.

inputMap = binaryOccupancyMap((inputScene),Resolution=1/cellSize);

Specify the center of potential parking spots within the map.

xLoc = [14, 18.5, 23, 27.25, 31.5, 36, 40.5, 44.75, 49, 53.5, 57.75];
yLoc = [46.5, 28.25, 21.5, 3.5];
[x,y] = meshgrid(xLoc,yLoc);

Display the parking scene image and the binary occupancy map. Plot the x and y coordinates of the center of the parking spots on the map.

fig = figure(Position=[0 0 600 300]);
subplot(Position=[0 0 0.4 1],Parent=fig)
imshow(inputScene)
title("Parking Scene Image")
subplot(Position=[0.5 0 0.5 1],Parent=fig)
show(inputMap)
hold on
plot(x,y,'*r')
hold off

Specify the number of parking spots to be occupied in the modified map.

numParkedSpots = 10;

Generate a modified parking lot map with the specified number of parked spots using the helperParkingLotMap function. The function randomly selects which slots in the parking lot will be occupied and places vehicles of three different sizes in those spots. By varying the sizes of the vehicles, the function creates a more realistic and diverse scenarios for training the MPNet deep learning network.

[outputMap,parkingStates] = helperParkingLotMap(inputMap,numParkedSpots,cellSize,xLoc,yLoc);

Display the generated parking lot map.

figure
show(outputMap)

Generate an entry or exit scenario with random start and goal states using the helperSampleStartGoal function. For an entry scenario, specify the scenario variable as "entry". For an exit scenario, specify the scenario variable as "exit". The helperSampleStartGoal function generates maps for entry and exit scenarios with equal probability. To achieve this, the function uses the rand function to generate a random number uniformly distributed between 0 and 1. If the generated random number is greater than 0.5, the scenario is set to "entry"; otherwise, it is set to "exit". This ensures that both scenarios are equally likely, each with a probability of 50%.

scenario = "entry";
[start,goal] = helperSampleStartGoal(parkingStates,scenario);

Display the generated parking valet scenario with the ego vehicle by using the helperPlotEgoVehicle function.

hold on
helperPlotEgoVehicle(start,DisplayName='start')
helperPlotEgoVehicle(goal,DisplayName='goal')
legend
hold off
title("Parking Valet Scenario")

Generate Training Dataset

Download and unzip a file containing the training dataset.

URL = "https://ssd.mathworks.com/supportfiles/nav/data/parkingLotDataset.zip";
filename = "./parkingLotDataset.zip";
websave(filename,URL);
unzip parkingLotDataset.zip

The downloaded dataset contains 200,000 optimal paths computed using the helperGenerateParkingLotDataset function. Here is an overview of the key functionalities of the helperGenerateParkingLotDataset function.

Alternatively, to generate your own dataset, set the generateData variable to true and specify the number of optimal paths you want to generate for the training dataset.

generateData = false;
if generateData==true
    numPaths = 20;
    helperGenerateParkingLotDataset(inputMap,cellSize,xLoc,yLoc,numPaths);

    files = dir("data/parkingValetPath*.mat");
    numSamples = length(files);
    dataset = cell(numSamples,1);
    for i = 1:length(files)
        load(fullfile(files(i).folder,files(i).name),'pathStates')
        dataset{i,1} = pathStates;
    end
    dataset = cell2table(dataset,'VariableNames',"Path");
    save("MyParkingDataset","dataset")
end

Load and Visualize Training Dataset

Load the downloaded training dataset into the MATLAB® workspace and visualize samples of the optimal paths stored in it.

load("parkingLotDataset.mat","dataset");
figure
for i=1:4
    subplot(2,2,i)
    % Select a random sample
    ind = randi(height(dataset));
    % Get path from Path column of the table
    pathStates = dataset(ind,:).Path{1};
    % Get start and goal states from the path
    start = pathStates(1,:);
    goal = pathStates(end,:);
    show(inputMap);
    hold on
    plot(pathStates(:,1),pathStates(:,2),plannerLineSpec.path{:})
    plot(start(1),start(2),plannerLineSpec.start{:})
    plot(goal(1),goal(2),plannerLineSpec.goal{:})
    hold off
end
legend(Position=[0.85 0.002 0.13 0.11]);

Create MPNet

Create a MPNet object for SE(2) state space by using the mpnetSE2 object.

mpnet = mpnetSE2;

Set the StateBounds property of the mpnetSE2 object to the limits of the input map used for generating the dataset.

x = inputMap.XWorldLimits;
y = inputMap.YWorldLimits;
theta = [-pi pi];
stateBounds = [x; y; theta];
mpnet.StateBounds = stateBounds;

Specify the weights for each state space variables using the LossWeights property of the mpnetSE2 object. For higher weight values, the network takes more epochs for convergence. For this example, the weight values for each state space variable must be non-zero.

mpnet.LossWeights = [100 100 50];

Set the EncodingSize property value to zero because the MPNet is to be trained using data generated from a single map environment.

mpnet.EncodingSize = [0 0];

Prepare Data For Training

Perform data augmentation by adding flipped versions of the paths to the dataset. This helps increase the size and diversity of the training samples. Data augmentation increases the robustness of the network for bidirectional planning.

flippedPaths = cellfun(@(x) flipud(x), dataset.Path, UniformOutput=false);
dataset = [dataset.Path; flippedPaths];

Split the dataset into training and validation data in a ratio of 80:20. The training set, which comprises 80% of the data, is used to train the network by minimizing the training loss. The remaining 20% of the data forms the validation set, which is used to monitor the validation loss during the training process.

split = 0.8;
numTrain = ceil(split*height(dataset));
trainData = dataset(1:numTrain,1);
validationData = dataset(numTrain+1:end,1);

Preprocess the training and validation data. The mpnetPrepareData function preprocesses the data and stores it in a datastore. The mpnetPrepareData function replaces the state space variable theta with its cosine and sine components and normalizes the state space variables $\mathit{x}$ and$\mathit{y}$ to lie within the range [0, 1].

dsTrain = mpnetPrepareData(trainData,mpnet);
dsValidation = mpnetPrepareData(validationData,mpnet);

Train MPNet

Use the trainnet function to train the MPNet. Training this network might take a long time depending on the hardware you use. Set the doTraining value to true to train the network.

doTraining = false;

Specify trainingOptions for training the deep learning network:

if doTraining
    options = trainingOptions("adam",...
        MiniBatchSize=2048,...
        MaxEpochs=50,...
        Shuffle="every-epoch",...
        ValidationData=dsValidation,...
        ValidationFrequency=2000,...
        Plots="training-progress");
    % Train network
    [net,info] = trainnet(dsTrain,mpnet.Network,@mpnet.loss,options);
    % Update Network property of mpnet object with net
    mpnet.Network = net;
end

In this example, you will use a pretrained MPNet to compute the predictions. Load a .mat file containing the pretrained network. The network has been trained on the downloaded dataset, ParkingLotDataset.mat file.

if ~doTraining
    load("parkinglotTrainedMPNET.mat","trainedNetwork")
    mpnet.Network = trainedNetwork;
end

Plan Path with Trained MPNet

Generate a new, modified parking lot map from the input map used for generating the training dataset. Specify the number of parked spots as 15.

numParkedSpots = 15;
[map,parkingStates] = helperParkingLotMap(inputMap,numParkedSpots,cellSize,xLoc,yLoc);

Set the inflation radius to 1. Inflate the map to take into account the vehicle geometry during path planning.

inflationRadius = 1;
mapInflated = copy(map);
inflate(mapInflated,inflationRadius)

Generate random start and goal state by using the helperSampleStartGoal function. You can also specify whether the scenario should be an entry or exit scenario.

 
scenario = "Entry";
[start,goal] = helperSampleStartGoal(parkingStates,scenario);

Create Reeds-Shepp state space with minimum radius of the vehicle as 4. You must use the same state space for both training and inference.

stateSpace = stateSpaceReedsShepp(stateBounds);
stateSpace.MinTurningRadius = 4.0;

Create a state validator using the validatorOccupancyMap function to validate states based on an occupancy map. Set the validation distance to 0.1. Assign the inflated map to the state validator to ensure that the planned paths effectively avoid obstacles.

stateValidator = validatorOccupancyMap(stateSpace);
stateValidator.ValidationDistance = 0.1;
stateValidator.Map = mapInflated;

Create MPNet path planner using the state validator and the pretrained MPNet. Set the maximum number of learned states to be generated by the MPNet path planner to 20.

mpnetPlanner = plannerMPNET(stateValidator,mpnet);
mpnetPlanner.MaxLearnedStates = 20;

Plan a path between the start and goal states using the MPNet path planner.

[mpnetPath,solutionInfo] = plan(mpnetPlanner,start,goal);

If a path is found, interpolate it to increase its resolution and smoothness.

if solutionInfo.IsPathFound
    mpnetPathCopy = copy(mpnetPath);
    interpolate(mpnetPathCopy,100)
end

Plot the planned path.

figure
show(map)
hold on
if solutionInfo.IsPathFound
    plot(mpnetPathCopy.States(:,1),mpnetPathCopy.States(:,2),plannerLineSpec.path{:})
    plot(start(1),start(2),plannerLineSpec.start{:})
    plot(goal(1),goal(2),plannerLineSpec.goal{:})
    title(strcat("Path Planning using MPNet for ",scenario," Scenario"))
else
    plot(mpnetPath.States(:,1),mpnetPath.States(:,2),plannerLineSpec.path{:})
    plot(start(1),start(2),plannerLineSpec.start{:})
    plot(goal(1),goal(2),plannerLineSpec.goal{:})
    title("Path Not Found")
end

Display the learned states, beacon states, and classical states computed by the MPNet path planner.

displaySolutionInfo = true;
if displaySolutionInfo
    lstate = plannerLineSpec.state(DisplayName="Learned states",MarkerSize=3);
    cstate = plannerLineSpec.state(DisplayName="Classical states",MarkerSize=3,MarkerFaceColor="green",MarkerEdgeColor="green");
    bstate = plannerLineSpec.state(MarkerEdgeColor="magenta",MarkerSize=7,DisplayName="Beacon states",Marker="^");
    plot(solutionInfo.LearnedStates(:,1),solutionInfo.LearnedStates(:,2),lstate{:})
    plot(solutionInfo.ClassicalStates(:,1),solutionInfo.ClassicalStates(:,2),cstate{:})
    plot(solutionInfo.BeaconStates(:,1),solutionInfo.BeaconStates(:,2),bstate{:})
end
legend(Location="bestoutside")
hold off