Find closest point on reference path to global point
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"])
pathPoints — Closest points on reference path
N-by-6 numeric matrix
Closest 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.
inWindow — Indicator of points being in search window
N-element logical column vector
Indication whether each point nearest to the corresponding xy coordinate in
points, is projected within the search window, returned as an N-element logical row vector, where N is the number of points in
points. Points being projected within the search window are
false if they lie at the end of a window.
searchWindow — Search window
N-element nondecreasing row vector
Search window on path to determine closest points, specified as an
N-element nondecreasing row vector, where N is
the number of points in