Interpolate reference path at provided arc lengths
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 m down the path as Frenet states.
initState = [0 0 0 0 0 0]; % [S dS ddS L dL ddL] termState = [30 0 0 0 0 0]; % [S dS ddS L dL ddL] frenetTraj = connect(connector,initState,termState,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.
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"])
refPath — Reference path
Reference path, specified as a
arclengths — Distances along reference path
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.
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:
theta— SE(2) state expressed in global coordinates, with
yin meters and
kappa— Curvature, or inverse of the radius, in meters
dkappa— Derivative of curvature with respect to arc length in meters per second
s— Arc length, or distance along path from path origin, in meters
N is the number of points sampled along the reference path.