shortenpath

Shorten path by removing redundant nodes

Since R2024b

Syntax

outputPath = shortenpath(inputPath,sv)

Description

outputPath = shortenpath(inputPath,sv) shortens the path between a start pose and goal pose by removing redundant nodes along the path. The function uses the input state validator sv to validate the shortened path returned at the output. Removing redundant nodes does not always reduce path length.

Examples

Shorten Dubins Path

Set the random seed to ensure the repeatability of results.

rng(100,"twister")

Create a Dubins state space object. Set the minimum turning radius for the Dubins vehicle to 0.2.

ss = stateSpaceDubins;
ss.MinTurningRadius = 0.2;

Load a binary matrix that specifies an occupancy grid.

load("exampleMaps.mat","simpleMap")

Create a 2-D occupancy map from the grid, and set the map resolution to 10 cells/meter.

map = occupancyMap(simpleMap,10);

Create a state validator using the Dubins state space. Specify the map and the distance required for validating path segments within the map.

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

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

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

Create an RRT path planner, and set the maximum connection distance to 0.3. 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 0.1 meter.

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

planner = 
  plannerRRT with properties:

               StateSpace: [1x1 stateSpaceDubins]
           StateValidator: [1x1 validatorOccupancyMap]
             StateSampler: [1x1 stateSamplerUniform]
          MaxNumTreeNodes: 10000
            MaxIterations: 10000
    MaxConnectionDistance: 0.3000
           GoalReachedFcn: @(~,s,g)(norm(s(1:3)-g(1:3))<0.1)
                 GoalBias: 0.0500

Set the start and goal states.

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

Compute the path between the start pose and goal pose using the RRT path planner. Note the number of states in the computed path.

inputPath = plan(planner,start,goal)

inputPath = 
  navPath with properties:

      StateSpace: [1x1 stateSpaceDubins]
          States: [35x3 double]
       NumStates: 35
    MaxNumStates: Inf

Remove redundant nodes along the computed path by using the shortenpath function. Note the number of states in the output path.

outputPath = shortenpath(inputPath,sv)

outputPath = 
  navPath with properties:

      StateSpace: [1x1 stateSpaceDubins]
          States: [4x3 double]
       NumStates: 4
    MaxNumStates: Inf

Compute the lengths of the original path and the shortened path.

lenPath = pathLength(inputPath)

lenPath = 
4.3670

lenShortened = pathLength(outputPath)

lenShortened = 
3.7560

Note that removing redundant nodes has decreased the path length.

If you want to increase the path resolution, you can perform interpolation to get the desired number of states. Increase the number of states in the shortened path to 50, and compute the path length.

interpolatedPath = copy(outputPath);
interpolate(interpolatedPath,50)
lenInterpolated = pathLength(interpolatedPath)

lenInterpolated = 
3.7560

Plot the original path, shortened path, and the interpolated path.

show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
legend(Location="bestoutside")
line(inputPath.States(:,1),inputPath.States(:,2),LineStyle="-",LineWidth=1.5,Color="red",Marker="o")
line(outputPath.States(:,1),outputPath.States(:,2),LineStyle="-",LineWidth=1.5,Color="blue",Marker="o")
line(interpolatedPath.States(:,1),interpolatedPath.States(:,2),LineStyle="--",LineWidth=0.5,Color="green",Marker="o")
legend("Start","Goal","Original Path","Shortened Path","Interpolated Path")
title("Path Comparison")
drawnow
hold off

Shorten Path in 3-D Occupancy Map Environment

Set the random seed to ensure repeatability of results.

rng(1,"twister")

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");
map = mapData.omap;
map.FreeThreshold = 0.5;

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

inflate(map,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=map,ValidationDistance=0.1);

Create an RRT path planner.

planner = plannerRRT(ss,sv);

Specify the start and goal poses.

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

Plan the path.

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

Shorten the path.

outputPath = shortenpath(inputPath,sv);

Visualize the original path and the shortened path.

show(map)
axis equal
view([30 55])
hold on
s1 = scatter3(start(1,1),start(1,2),start(1,3),[],"g","filled",DisplayName="Start");
s2 = scatter3(goal(1,1),goal(1,2),goal(1,3),[],"m","filled",DisplayName="Goal");
p1 = plot3(inputPath.States(:,1),inputPath.States(:,2),inputPath.States(:,3),"r-",LineWidth=2,DisplayName="Original Path");
p2 = plot3(outputPath.States(:,1),outputPath.States(:,2),outputPath.States(:,3),"y-",LineWidth=2,DisplayName="Shortened Path");
legend([s1 s2 p1 p2],Location="bestoutside")
title("Path Comparison")

Input Arguments

inputPath — Input path

navPath object

Input path, specified as a navPath object. The input path must contain valid states. You can use any of the path planners to get the input path. However, if the input path is already optimal, the function may not shorten it significantly.

sv — State validator

validatorOccupancyMap object | validatorVehicleCostmap object | validatorOccupancyMap3D object | object from a subclass of nav.StateValidator

State validator, specified as a validatorOccupancyMap object, validatorVehicleCostmap object, validatorOccupancyMap3D object, or an object from a subclass of the nav.StateValidator class. The state space associated with the state validator and the state space used to compute the input path must be the same. The configuration of the state validator determines whether the function will return a collision-free path.

Output Arguments

outputPath — Shortened path

navPath object

Shortened path, returned as a navPath object.

Version History

Introduced in R2024b