Introduction

Automated Driving Toolbox™ integrates an Unreal Engine simulation environment in Simulink®. You can use this environment to visualize the motion of a vehicle in a prebuilt scene. This environment provides an intuitive way to analyze the performance of path planning and vehicle control algorithms. The Automated Parking Valet in Simulink example shows how to design a path planning and vehicle control algorithm for an automated parking valet system in Simulink. This example shows how to augment the model to visualize the vehicle motion in a scene using the visualization engine. The steps in this workflow are:

Create Costmap from 3D Scene

The visualization integration comes with a number of prebuilt scenes. Each scene comes with a high-resolution image that can be used to explore the scene. Use the helperShowSceneImage function to display the images. This example uses the Large Parking Lot scene.

% Load and display the image of the parking lot
sceneName = 'LargeParkingLot';
[sceneImage, sceneRef] = helperGetSceneImage(sceneName);

% Visualize the scene image
figure
helperShowSceneImage(sceneImage, sceneRef)

Such a high-resolution image is an accurate depiction of the environment up to some resolution. You can use this image to create a vehicleCostmap for path planning and navigation.

First, estimate free space from the image. Free space is the area where a vehicle can drive without collision with other static objects, such as parked cars, cones, and road boundaries, and without crossing marked lines. In this example, you can estimate the free space based on the color of the image. Use the Color Thresholder (Image Processing Toolbox) app from Image Processing Toolbox to perform the segmentation and generate a binary image from the image. You can also use the helper function helperCreateCostmapFromImage at the end of the example to generate the binary image:

sceneImageBinary = helperCreateCostmapFromImage(sceneImage);

Alternatively, load a pregenerated binary image.

sceneImageBinary = imread('sim3d_LargeParkingLotBinary.bmp');

Next, create a costmap from the binary image. Use the binary image to specify the cost value at each cell.

% Get the left-bottom corner location of the map
mapLocation = [sceneRef.XWorldLimits(1), sceneRef.YWorldLimits(1)]; % [meters, meters]

% Compute resolution
mapWidth = sceneRef.XWorldLimits(2)-sceneRef.XWorldLimits(1); % meters
cellSize = mapWidth/size(sceneImageBinary, 2);

% Create the costmap
costmap = vehicleCostmap(im2single(sceneImageBinary), 'CellSize', cellSize, 'MapLocation', mapLocation);

figure
plot(costmap, 'Inflation', 'off');
legend off

You also need to specify the dimensions of the vehicle that will park automatically based on the vehicles available in the 3D scene. This example uses the dimension of a Hatchback. These dimensions need to be consistent between the costmap and the Simulink model.

centerToFront = 1.104; % meters
centerToRear  = 1.343; % meters
frontOverhang = 0.828; % meters
rearOverhang  = 0.589; % meters
vehicleWidth  = 1.653; % meters
vehicleHeight = 1.513; % meters
vehicleLength = centerToFront + centerToRear + frontOverhang + rearOverhang;

vehicleDims = vehicleDimensions(vehicleLength, vehicleWidth, vehicleHeight,...
    'FrontOverhang', frontOverhang, 'RearOverhang', rearOverhang);
costmap.CollisionChecker.VehicleDimensions = vehicleDims;

Set the inflation radius by specifying the number of circles enclosing the vehicle.

costmap.CollisionChecker.NumCircles = 5;

Create Route Plan from a 3D Scene

The global route plan is described as a sequence of lane segments to traverse in order to reach a parking spot. You can interactively select intermediate goal positions from the scene image using the tool described in Select Waypoints for Unreal Engine Simulation. In this example, the route plan has been created and stored in a table. Before simulation, the PreLoadFcn callback function of the model loads the route plan.

data      = load('routePlanUnreal.mat');
routePlan = data.routePlan %#ok<NOPTS>

% Plot vehicle at the starting pose
startPose = routePlan.StartPose(1,:);
hold on
helperPlotVehicle(startPose, vehicleDims, 'DisplayName', 'Current Pose')
legend

for n = 1 : height(routePlan)
    % Extract the goal waypoint
    vehiclePose = routePlan{n, 'EndPose'};

    % Plot the pose
    legendEntry = sprintf('Goal %i', n);
    helperPlotVehicle(vehiclePose, vehicleDims, 'DisplayName', legendEntry);
    hold on
end
hold off

routePlan =

  5×3 table

           StartPose                   EndPose             Attributes 
    _______________________    _______________________    ____________

     44.5        8      180    -33.5      8.5      180    [1×1 struct]
    -33.5      8.5      180    -45.2     -0.7      250    [1×1 struct]
    -45.2     -0.7      250    -33.5    -13.5        0    [1×1 struct]
    -33.5    -13.5        0    -20.3    -13.5       -7    [1×1 struct]
    -20.3    -13.5       -7    -13.5     -6.8       90    [1×1 struct]

Configure 3D Scene and Ego Vehicle

Close the figures and open the model.

helperCloseFigures

if ~ispc
    error(['3D Simulation is only supported on Microsoft', char(174), ' Windows', char(174), '.']);
end

modelName = 'AutomatedParkingValetWith3DSimulation';
open_system(modelName)
snapnow

This model extends the one used in the Automated Parking Valet in Simulink example by adding two blocks for visualizing the vehicle in the 3D scene:

Visualize Vehicle Motion in 3D Scene

Simulate the model to see how the vehicle drives into the desired parking spot.

sim(modelName)

As the simulation runs, the Simulink environment updates the position and orientation of the vehicle in the 3D visualization engine through the Simulation 3D Vehicle with Ground Following block. A new window shows the ego vehicle in the 3D visualization engine. The Automated Parking Valet figure displays the planned path in blue and the actual path of the vehicle in red. The Parking Maneuver figure shows a local costmap used in searching for the final parking maneuver.

Explore Enhanced Path Planning System

The Path Planner block plans a feasible path through the environment map using the optimal rapidly exploring random tree (RRT*) algorithm. To ensure the performance of the planning algorithm, the path planning module is modified to include two separate modules:

Open the Path Planner subsystem.

open_system([modelName, '/Path Planner'], 'force')

The two path planner modules are implemented as Enabled Subsystem (Simulink) blocks. The enable signal is from the IsParkingManeuver signal in the input Config bus sent from the Behavior Planner block. When the Parking Mode subsystem is enabled, a local costmap is created with the center as the current position of the vehicle.

Close the model and the figures.

bdclose all
helperCloseFigures

Conclusions

This example showed how to integrate 3D simulation with the existing Automated Parking Valet in Simulink example to visualize the motion of vehicle in a 3D parking lot scene.

Supporting Functions

helperCreateCostmapFromImage

function BW = helperCreateCostmapFromImage(sceneImage) %#ok<DEFNU>
%helperCreateCostmapFromImage Create a costmap from an RGB image.

% Flip the scene image
sceneImage = flipud(sceneImage);

% Call the autogenerated code from the Color Thresholder app
BW = helperCreateMask(sceneImage);

% Smooth the image
BW = im2uint8(medfilt2(BW));

% Resize
BW = imresize(BW, 0.5);

% Compute complement
BW = imcomplement(BW);
end

helperCreateMask

function [BW,maskedRGBImage] = helperCreateMask(RGB)
%helperCreateMask  Threshold RGB image using auto-generated code from Color Thresholder app.
%  [BW,maskedRGBImage] = createMask(RGB) thresholds image RGB using
%  autogenerated code from the Color Thresholder app. The colorspace and
%  range for each channel of the colorspace were set within the app. The
%  segmentation mask is returned in BW, and a composite of the mask and
%  original RGB image is returned in maskedRGBImage.

% Convert RGB image to chosen colorspace
I = RGB;

% Define thresholds for channel 1 based on histogram settings
channel1Min = 67.000;
channel1Max = 216.000;

% Define thresholds for channel 2 based on histogram settings
channel2Min = 68.000;
channel2Max = 171.000;

% Define thresholds for channel 3 based on histogram settings
channel3Min = 69.000;
channel3Max = 160.000;

% Create mask based on chosen histogram thresholds
sliderBW = (I(:,:,1) >= channel1Min ) & (I(:,:,1) <= channel1Max) & ...
    (I(:,:,2) >= channel2Min ) & (I(:,:,2) <= channel2Max) & ...
    (I(:,:,3) >= channel3Min ) & (I(:,:,3) <= channel3Max);
BW = sliderBW;

% Initialize output masked image based on input image
maskedRGBImage = I;

% When BW is false, set background pixels to zero
maskedRGBImage(repmat(~BW,[1 1 3])) = 0;
end

helperCloseFigures

function helperCloseFigures()
%helperCloseFigures Close all the figures except the simulation visualization

% Find all the figure objects
figHandles = findobj('Type', 'figure');

% Close the figures
for i = 1: length(figHandles)
    close(figHandles(i));
end
end