closestPoint

Find closest point on reference path to global point

Since R2020b

Syntax

pathPoints = closestPoint(refPath,points)

[pathPoints,inWindow] = closestPoint(refPath,points)

[_] = closestPoint(refPath,points,searchWindow)

Description

pathPoints = closestPoint(refPath,points) finds the closest point on the reference path to each of the specified (x,y)-positions points.

[pathPoints,inWindow] = closestPoint(refPath,points) optionally returns a logical vector inWindow, specifying whether each point for the corresponding xy coordinate in points is projected within the search window..

[_] = closestPoint(refPath,points,searchWindow) optionally accepts a nondecreasing row vector searchWindow, which defines the interval of the path to use to find the closest points.

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"])

Get Closest Points at Path Intersections

Create a self-intersecting reference path.

refPath = referencePathFrenet([0 100 -pi/4; ...
                               50 50 -pi/4;...
                               75 50  pi/2; ...
                               50 50 -3*pi/4; ...
                               0  0  -3*pi/4]);

Show the reference path.

figure
show(refPath); 
title("Closest Points Around Intersection")
xlim([0 125])
ylim([0 125])
hold on

Find the arclength at which the intersection occurs.

sIntersection = refPath.SegmentParameters(2,end);

Generate the Frenet states lying just before and after the intersection.

f0 = [sIntersection-20 5 0 10 0 0]; % [S dS ddS L Lp Lpp]
f1 = [sIntersection+20 5 0 -5 0 0]; % [S dS ddS L Lp Lpp]

Calculate the time to travel with constant longitudinal velocity.

T = (f1(1)-f0(1))/f1(2);

Create trajectory generator using the reference path.

generator = trajectoryGeneratorFrenet(refPath);

Generate a trajectory between the points.

[fTraj,gTraj] = connect(generator,f0,f1,T);
pts = gTraj.Trajectory;

Define the plot helper functions.

mergeFcn = @(v1,v2)reshape([v1 v2 nan(size(v1,1),size(v2,2))]',[],1);
plotFcn = @(L1,L2,linespec)plot(mergeFcn(L1(:,1),L2(:,1:min(1,size(L2,2)))),mergeFcn(L1(:,2),L2(:,2:min(2,size(L2,2)))),linespec{:});
plotInterval = @(bounds,nPt,linespec)plotFcn(interpolate(refPath,linspace(bounds(1),bounds(2),nPt)'),[],linespec);

Plot the trajectory.

plotFcn(pts,[],{"k.-"});

Calculate closest point on path to each global state.

closestPts = closestPoint(refPath,pts);

Plot the closest point vectors.

plotFcn(pts,closestPts,{"b"});

Define a window in which to search for closest points.

buffWindow = [f0(1)-5 f1(1)+5];

plotInterval(buffWindow,100,{"Color",[.5 .5 .5],"LineWidth",5});

Find closest points within window.

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

Display windowed results.

plotFcn(pts,closestPtsInWindow,{"g","LineWidth",3});

Find closest points using a window that is too small.

smallWindow = [f0(1)+5 f1(1)-5];
[closestPtsSmall,inWindow] = closestPoint(refPath,pts,smallWindow);

Overlay small window results.

plotInterval(smallWindow,100,{"m","LineWidth",3});
plotFcn(pts(inWindow,:),closestPtsSmall(inWindow,:),{"Color",[.5 1 .5]});
plotFcn(pts(~inWindow,:),closestPtsSmall(~inWindow,:),{"r"});
legend({"Waypoints","ReferencePath","Trajectory","ClosestPoints",...
    "BuffWindow","ClosestInsideBuffWindow","SmallWindow",...
    "ClosestInsideSmallWindow","ClosestOutsideSmallWindow"});

Input Arguments

refPath — Reference path

referencePathFrenet object

Reference path, specified as a referencePathFrenet object.

points — Global points

P-by-2 numeric matrix

Global points, specified as a P-by-2 numeric matrix with rows of the form [x y]. P is the number of points. Positions are in meters.

searchWindow — Search window

two-element row vector

Search window on path to determine closest points, specified as an two-element row vector of arclengths.

Output Arguments

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:

• 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.

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 column vector, where N is the number of points in points. Points being projected within the search window are true, or false if they lie at the end of a window.

Extended Capabilities

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

Version History

Introduced in R2020b