closestPoint
Syntax
Description
finds the closest point on the reference path to each of the specified
(x,y)-positions pathPoints
= closestPoint(refPath
,points
)points
.
[
optionally returns a logical vector pathPoints
,inWindow
] = closestPoint(refPath
,points
)inWindow
, specifying whether each
point for the corresponding xy coordinate in points
is projected within the search window..
[_] = closestPoint(
optionally accepts a nondecreasing row vector refPath
,points
,searchWindow
)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
See Also
Objects
Functions
Abrir ejemplo
Tiene una versión modificada de este ejemplo. ¿Desea abrir este ejemplo con sus modificaciones?
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)