smoothPathSpline

Smooth vehicle path using cubic spline interpolation

Syntax

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses)

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation)

[___,cumLengths,curvatures] = smoothPathSpline(___)

Description

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses) generates a smooth vehicle path, consisting of numSmoothPoses discretized poses, by fitting the input reference path poses to a cubic spline. Given the input reference path directions, smoothPathSpline also returns the directions that correspond to each pose.

Use this function to convert a C¹-continuous vehicle path to a C²-continuous path. C¹-continuous paths include the driving.DubinsPathSegment or driving.ReedsSheppPathSegment paths that you can plan using a pathPlannerRRT object. For more details on these path types, see C1-Continuous and C2-Continuous Paths.

You can use the returned poses and directions with a vehicle controller, such as the lateralControllerStanley function.

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation) specifies a minimum separation threshold between poses. If the distance between two poses is smaller than minSeparation, the function uses only one of the poses for interpolation.

[___,cumLengths,curvatures] = smoothPathSpline(___) also returns the cumulative path length and signed path curvature at each returned pose, using any of the previous syntaxes. Use these values to generate a velocity profile along the path.

Examples

Smooth a Planned Path

Smooth a path that was planned by an RRT* path planner.

Load and plot a costmap of a parking lot.

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

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 = [4,4,90]; % [meters, meters, degrees]
goalPose = [30,13,0];

Use a pathPlannerRRT object to plan a path from the start pose to the goal pose.

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

Plot and zoom in on the planned path. The path is composed of a sequence of Dubins curves. These curves include abrupt changes in curvature that are not suitable for driving with passengers.

hold on
plot(refPath,'Vehicle','off','DisplayName','Reference path')
xlim([3 31])
ylim([3 18])

Interpolate the transition poses of the path. Use these poses as the reference poses for interpolating the smooth path. Also return the motion directions at each pose.

[refPoses,refDirections] = interpolate(refPath);

Specify the number of poses to return in the smooth path. Return poses spaced about 0.1 meters apart, along the entire length of the path.

approxSeparation = 0.1; % meters
numSmoothPoses = round(refPath.Length / approxSeparation);

Generate the smooth path by fitting a cubic spline to the reference poses. smoothPathSpline returns the specified number of discretized poses along the smooth path.

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses);

Plot the smooth path. The more abrupt changes in curvature that were present in the reference path are now smoothed out.

plot(poses(:,1),poses(:,2),'LineWidth',2,'DisplayName','Smooth path')
hold off

Input Arguments

refPoses — Reference poses

M-by-3 matrix of [x, y, Θ] vectors

Reference poses of the vehicle along the path, specified as an M-by-3 matrix of [x, y, Θ] vectors, where M is the number of poses.

x and y specify the location of the vehicle in meters. Θ specifies the orientation angle of the vehicle in degrees.

Data Types: single | double

refDirections — Reference directions

M-by-1 column vector of 1s (forward motion) and –1s (reverse motion)

Reference directions of the vehicle along the path, specified as an M-by-1 column vector of 1s (forward motion) and –1s (reverse motion). M is the number of reference directions. Each element of refDirections corresponds to a pose in the refPoses input argument.

Data Types: single | double

numSmoothPoses — Number of smooth poses to return

positive integer

Number of smooth poses to return in the poses output argument, specified as a positive integer. To increase the granularity of the returned poses, increase numSmoothPoses.

minSeparation — Minimum separation between poses

1e-3 (default) | positive real scalar

Minimum separation between poses, in meters, specified as a positive real scalar. If the Euclidean (x, y) distance between two poses is less than this value, then the function uses only one of these poses for interpolation.

Output Arguments

poses — Discretized poses of smoothed path

numSmoothPoses-by-3 matrix of [x, y, Θ] vectors

Discretized poses of the smoothed path, returned as a numSmoothPoses-by-3 matrix of [x, y, Θ] vectors.

x and y specify the location of the vehicle in meters. Θ specifies the orientation angle of the vehicle in degrees.

The values in poses are of the same data type as the values in the refPoses input argument.

directions — Motion directions at each output pose

numSmoothPoses-by-1 column vector of 1s (forward motion) and –1s (reverse motion)

Motion directions at each output pose in poses, returned as a numSmoothPoses-by-1 column vector of 1s (forward motion) and –1s (reverse motion).

The values in directions are of the same data type as the values in the refDirections input argument.

cumLengths — Cumulative path lengths

numSmoothPoses-by-1 real-valued column vector

Cumulative path length at each output pose in poses, returned as a numSmoothPoses-by-1 real-valued column vector. Units are in meters.

You can use the cumLengths and curvatures outputs to generate a velocity profile of the vehicle along the smooth path. For more details, see the Automated Parking Valet example.

curvatures — Signed path curvatures

numSmoothPoses-by-1 real-valued column vector

Signed path curvatures at each output pose in poses, returned as a numSmoothPoses-by-1 real-valued column vector. Units are in radians per meter.

You can use the curvatures and cumLengths outputs to generate a velocity profile of the vehicle along the smooth path. For more details, see the Automated Parking Valet example.

More About

C¹-Continuous and C²-Continuous Paths

A path is C¹-continuous if its derivative exists and is continuous. Paths that are only C¹-continuous have discontinuities in their curvature. For example, a path composed of Dubins or Reeds-Shepp path segments has discontinuities in curvature at the points where the segments join. These discontinuities result in changes in direction that are not smooth enough for driving with passengers.

A path is also C²-continuous if its second derivative exists and is continuous. C²-continuous paths have continuous curvature and are smooth enough for driving with passengers.

Tips

• To check if a smooth path is collision-free, specify the smooth poses as an input to the checkPathValidity function.

Algorithms

• The path-smoothing algorithm interpolates a parametric cubic spline that passes through all input reference pose points. The parameter of the spline is the cumulative chord length at these points. [1]

• The tangent direction of the smoothed output path approximately matches the orientation angle of the vehicle at the starting and goal poses.

References

[1] Floater, Michael S. "On the Deviation of a Parametric Cubic Spline Interpolant from Its Data Polygon." Computer Aided Geometric Design. Vol. 25, Number 3, 2008, pp. 148–156.

[2] Lepetic, Marko, Gregor Klancar, Igor Skrjanc, Drago Matko, and Bostjan Potocnik. "Time Optimal Path Planning Considering Acceleration Limits." Robotics and Autonomous Systems. Vol. 45, Numbers 3–4, 2003, pp. 199–210.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019a