smoothPathSpline
Smooth vehicle path using cubic spline interpolation
Syntax
Description
[
generates a smooth vehicle path, consisting of poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses)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 C1-continuous vehicle path to
a C2-continuous path. C1-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.
[
specifies a minimum separation threshold between poses. If the distance between two poses is
smaller than poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation)minSeparation, the function uses only one of the poses for
interpolation.
[___,
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. cumLengths,curvatures] = smoothPathSpline(___)
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
Output Arguments
More About
C1-Continuous and C2-Continuous Paths
A path is C1-continuous if its derivative exists and is continuous. Paths that are only C1-continuous have discontinuities in their curvature. For example, a path composed of Dubins or Reeds-Sheep 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 C2-continuous if its second derivative exists and is continuous. C2-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
checkPathValidityfunction.
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.