isPathValid

Load and Assign Map to State Validator

Create an occupancy map from an example map and set the map resolution.

load exampleMaps.mat; % simpleMap
mapResolution = 1; % cells/meter
map = occupancyMap(simpleMap,mapResolution);

Create a Dubins state space.

statespace = stateSpaceDubins;

Create a state validator based on occupancy map to store the parameters and states in the Dubins state space.

statevalidator = validatorOccupancyMap(statespace);

Assign the map to the validator.

statevalidator.Map = map;

Set the validation distance for the validator.

statevalidator.ValidationDistance = 0.01;

Update the state space bounds to be the same as the map limits.

statespace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

Plan Path

Create an RRT* path planner and allow further optimization.

planner = plannerRRTStar(statespace,statevalidator);
planner.ContinueAfterGoalReached = true;

Reduce the maximum number of iterations and increase the maximum connection distance.

planner.MaxIterations = 2500;
planner.MaxConnectionDistance = 0.3;

Define start and goal states for the path planner as [x, y, theta] vectors. x and y are the Cartesian coordinates, and theta is the orientation angle.

start = [2.5, 2.5, 0]; % [meters, meters, radians]
goal = [22.5, 8.75, 0];

Plan a path from the start state to the goal state. The plan function returns a navPath object.

rng(100,'twister') % repeatable result
[path,solutionInfo] = plan(planner,start,goal);

Compute and Visualize Path Metrics

Create a path metrics object.

pathMetricsObj = pathmetrics(path,statevalidator);

Check path validity. The result is 1 (true) if the planned path is obstacle free. 0 (false) indicates an invalid path.

isPathValid(pathMetricsObj)

ans = logical
   1

Calculate the minimum clearance of the path.

clearance(pathMetricsObj)

ans = 
1.4142

Evaluate the smoothness of the path. Values close to 0 indicate a smoother path. Straight-line paths return a value of 0.

smoothness(pathMetricsObj)

ans = 
1.7318

Visualize the minimum clearance of the path.

show(pathMetricsObj)
legend('Planned Path','Minimum Clearance')

Load and Assign Map to State Validator

Load a costmap of a parking lot. Plot the costmap to see the parking lot and the inflated areas that the vehicle should avoid.

load parkingLotCostmap.mat;
costmap = parkingLotCostmap;
plot(costmap)
xlabel('X (meters)')
ylabel('Y (meters)')

Create a stateSpaceDubins object and increase the minimum turing radius to 4 meters.

statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % meters

Create a validatorVehicleCostmap object using the created state space.

statevalidator = validatorVehicleCostmap(statespace);

Assign the parking lot costmap to the state validator object.

statevalidator.Map = costmap;

Plan Path

Define start and goal poses for the vehicle as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the orientation angles Θ are in degrees.

startPose = [5, 5, 90]; % [meters, meters, degrees]
goalPose = [40, 38, 180]; % [meters, meters, degrees]

Use a pathPlannerRRT (Automated Driving Toolbox) object and the plan (Automated Driving Toolbox) function to plan the vehicle path from the start pose to the goal pose.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

Interpolate along the path at every one meter. Convert the orientation angles from degrees to radians.

poses = zeros(size(refPath.PathSegments,2)+1,3);
poses(1,:) = refPath.StartPose;
for i = 1:size(refPath.PathSegments,2) 
    poses(i+1,:) = refPath.PathSegments(i).GoalPose; 
end
poses(:,3) = deg2rad(poses(:,3));

Create a navPath object using the Dubins state space object and the states specified by poses.

path = navPath(statespace,poses);

Compute and Visualize Path Metrics

Create a pathmetrics object.

pathMetricsObj = pathmetrics(path,statevalidator);

Check path validity. The result is 1 (true) if the planned path is obstacle free. 0 (false) indicates an invalid path.

isPathValid(pathMetricsObj)

ans = logical
   1

Compute and visualize the minimum clearance of the path.

clearance(pathMetricsObj)

ans = 
0.5000

show(pathMetricsObj)
legend('Inflated Areas','Planned Path','Minimum Clearance')
xlabel('X (meters)')
ylabel('Y (meters)')

Compute and visualize the smoothness of the path. Values close to 0 indicate a smoother path. Straight-line paths return a value of 0.

smoothness(pathMetricsObj)

ans = 
0.0842

show(pathMetricsObj,'Metrics',{'Smoothness'})
legend('Inflated Areas','Path Smoothness')
xlabel('X (meters)')
ylabel('Y (meters)')

Visualize the clearance for each state of the path.

show(pathMetricsObj,'Metrics',{'StatesClearance'})
legend('Inflated Areas','Planned Path','Clearance of Path States')
xlabel('X (meters)')
ylabel('Y (meters)')

Load and Assign Map to State Validator

Load a 3-D occupancy map of a city block into the workspace. Specify the threshold to consider cells as obstacle-free.

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

Inflate the occupancy map to add a buffer zone for safe operation around the obstacles.

inflate(omap,1)

Create an SE(3) state space object with bounds for state variables.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

Create a 3-D occupancy map state validator using the created state space. Assign the occupancy map to the state validator object. Specify the sampling distance interval.

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

Plan Path

Create a RRT path planner with increased maximum connection distance and reduced maximum number of iterations. Specify a custom goal function that determines that a path reaches the goal if the Euclidean distance to the target is below a threshold of 1 meter.

planner = plannerRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

Specify start and goal poses.

start = [50 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Configure the random number generator for repeatable result.

rng(1,"twister")

Plan the path.

[pthObj,solnInfo] = plan(planner,start,goal);

Compute and Visualize Path Metrics

Create a path metrics object.

pathMetricsObj = pathmetrics(pthObj,sv);

Check path validity. The result is 1 (true) if the planned path is obstacle free. 0 (false) indicates an invalid path.

isPathValid(pathMetricsObj)

ans = logical
   1

Calculate the minimum clearance of the path.

clearance(pathMetricsObj)

ans = 
10

Evaluate the smoothness of the path. Values close to 0 indicate a smoother path. Straight-line paths return a value of 0.

smoothness(pathMetricsObj)

ans = 
0.0011

Visualize the minimum clearance of the path.

show(pathMetricsObj)
axis equal
view([100 75])
hold on
% Start state
scatter3(start(1,1),start(1,2),start(1,3),"g","filled")
% Goal state
scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled")
% Path
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ...
      "r-",LineWidth=2)