shortenpath
Description
shortens the path between a start pose and goal pose by removing redundant nodes along the
path. The function uses the input state validator outputPath
= shortenpath(inputPath
,sv
)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
See Also
MATLAB 명령
다음 MATLAB 명령에 해당하는 링크를 클릭했습니다.
명령을 실행하려면 MATLAB 명령 창에 입력하십시오. 웹 브라우저는 MATLAB 명령을 지원하지 않습니다.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)