interpolate

Interpolate reference path at provided arc lengths

Since R2020b

Syntax

pathPoints = interpolate(refPath,arclengths)

Description

pathPoints = interpolate(refPath,arclengths) interpolates the reference path at the provided arc lengths and returns the interpolated points on the path in global coordinates.

Examples

Generate Trajectory from Reference Path

Generate a reference path from a set of waypoints.

waypoints = [0 0; 50 20; 100 0; 150 10];
refPath = referencePathFrenet(waypoints);

Create a trajectoryGeneratorFrenet object from the reference path.

connector = trajectoryGeneratorFrenet(refPath);

Generate a five-second trajectory between the path origin and a point 30 meters down the path as Frenet states.

initCartState = refPath.SegmentParameters(1,:);
initFrenetState = global2frenet(refPath,initCartState);
termFrenetState = initFrenetState + [30 zeros(1,5)];
frenetTraj = connect(connector,initFrenetState,termFrenetState,5);

Convert the trajectory to the global states.

globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

Display the reference path and the trajectory.

show(refPath);
axis equal
hold on
plot(globalTraj(:,1),globalTraj(:,2),'b')

Specify global points and find the closest points on reference path.

globalPoints = waypoints(2:end,:) + [20 -50];
nearestPathPoint = closestPoint(refPath,globalPoints);

Display the global points and the closest points on reference path.

plot(globalPoints(:,1),globalPoints(:,2),'r*','MarkerSize',10)
plot(nearestPathPoint(:,1),nearestPathPoint(:,2),'b*','MarkerSize',10)

Interpolate between the arc lengths of the first two closest points along the reference path.

arclengths = linspace(nearestPathPoint(1,6),nearestPathPoint(2,6),10);
pathStates = interpolate(refPath,arclengths);

Display the interpolated path points.

plot(pathStates(:,1),pathStates(:,2),'g')
legend(["Waypoints","Reference Path","Trajectory to 30m",...
        "Global Points","Closest Points","Interpolated Path Points"])

Input Arguments

refPath — Reference path

referencePathFrenet object

Reference path, specified as a referencePathFrenet object.

arclengths — Distances along reference path

N-element vector

Distances along the reference path, specified as an N-element vector. N is the number of arc lengths at which to sample points. Each distance is in meters.

Output Arguments

pathPoints — Points on reference path

N-by-6 numeric matrix

Points on the reference path , returned as an N-by-6 numeric matrix with rows of form [x y theta kappa dkappa s], where:

• x y and theta — SE(2) state expressed in global coordinates, with x and y in meters and theta in radians

• kappa — Curvature, or inverse of the radius, in m-1

• dkappa — Derivative of curvature with respect to arc length in m-2

• s — Arc length, or distance along path from path origin, in meters

N is the number of points sampled along the reference path.

Extended Capabilities

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

Version History

Introduced in R2020b