smoothness

Smoothness of path

Description

example

smoothness(pathMetricsObj) evaluates the smoothness of the planned path. Values close to 0 indicate a smoother path. Straight-line paths return a value of 0.

smoothness(pathMetricsObj,'Type','segments') returns individual smoothness calculations between each set of three poses on the path, in the form of a (n–2)-element vector, where n is the number of poses.

Examples

collapse all

Compute smoothness, clearance, and validity of a planned path based on a set of poses and the associated map environment.

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 = 
          0.21

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 = 
          2.21

Visualize the minimum clearance of the path.

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

Input Arguments

collapse all

Information for path metrics, specified as a pathmetrics object.

References

[1] Lindemann, Stephen R., and Steven M. LaValle. "Simple and efficient algorithms for computing smooth, collision-free feedback laws over given cell decompositions." The International Journal of Robotics Research 28, no. 5. 2009, pp. 600-621.

See Also

Objects

Functions

Introduced in R2019b