Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

closestPoint

Encuentre el punto más cercano en la ruta de referencia al punto global

contraer todo en la página

Sintaxis

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

Descripción

pathPoints = closestPoint(refPath,points) encuentra el punto más cercano en la ruta de referencia a cada una de las posiciones (x,y) especificadas points.

ejemplo

[pathPoints,inWindow] = closestPoint(refPath,points) devuelve opcionalmente un vector lógico inWindow, que especifica si cada punto de la coordenada xy correspondiente en points se proyecta dentro de la ventana de búsqueda.

[_] = closestPoint(refPath,points,searchWindow) acepta opcionalmente un vector fila no decreciente searchWindow, que define el intervalo de la ruta a utilizar para encontrar los puntos más cercanos.

ejemplo

Ejemplos

contraer todo

Abrir script en vivo

Genere una ruta de referencia a partir de un conjunto de puntos de referencia.

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

Crea un objeto trajectoryGeneratorFrenet a partir de la ruta de referencia.

connector = trajectoryGeneratorFrenet(refPath);

Genera una trayectoria de cinco segundos entre el origen de la ruta y un punto a 30 metros de la ruta como afirma Frenet.

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

Convierta la trayectoria a los estados globales.

globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

Muestra la ruta de referencia y la trayectoria.

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

Especifique puntos globales y busque los puntos más cercanos en la ruta de referencia.

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

Muestra los puntos globales y los puntos más cercanos en la ruta de referencia.

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

Interpola entre las longitudes de arco de los dos primeros puntos más cercanos a lo largo de la ruta de referencia.

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

Muestra los waypoints interpolados.

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

Figure contains an axes object. The axes object contains 6 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Trajectory to 30m, Global Points, Closest Points, Interpolated Path Points.

Abrir script en vivo

Cree una ruta de referencia que se interseque sola.

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

Mostrar la ruta de referencia.

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

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 2 objects of type line. One or more of the lines displays its values using only markers

Encuentre la longitud del arco en la que ocurre la intersección.

sIntersection = refPath.SegmentParameters(2,end);

Genere los estados de Frenet que se encuentran justo antes y después de la intersección.

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]

Calcule el tiempo para viajar con velocidad longitudinal constante.

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

Cree un generador de trayectoria utilizando la ruta de referencia.

generator = trajectoryGeneratorFrenet(refPath);

Generar una trayectoria entre los puntos.

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

Defina las funciones auxiliares de la trama.

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);

Trazar la trayectoria.

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

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 3 objects of type line. One or more of the lines displays its values using only markers

Calcule el punto más cercano en la ruta a cada estado global.

closestPts = closestPoint(refPath,pts);

Trazar los vectores de puntos más cercanos.

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

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 4 objects of type line. One or more of the lines displays its values using only markers

Defina una ventana en la que buscar puntos más cercanos.

buffWindow = [f0(1)-5 f1(1)+5];
plotInterval(buffWindow,100,{"Color",[.5 .5 .5],"LineWidth",5});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 5 objects of type line. One or more of the lines displays its values using only markers

Encuentra los puntos más cercanos dentro de la ventana.

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

Mostrar resultados en ventanas.

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

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 6 objects of type line. One or more of the lines displays its values using only markers

Encuentre los puntos más cercanos usando una ventana que sea demasiado pequeña.

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

Superponer resultados de ventanas pequeñas.

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"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 9 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, ReferencePath, Trajectory, ClosestPoints, BuffWindow, ClosestInsideBuffWindow, SmallWindow, ClosestInsideSmallWindow, ClosestOutsideSmallWindow.

Argumentos de entrada

contraer todo

Ruta de referencia, especificada como un objeto referencePathFrenet.

Puntos globales, especificados como una matriz numérica de P por 2 con filas con la forma [x y]. P es el número de puntos. Las posiciones están en metros.

Ventana de búsqueda en la ruta para determinar los puntos más cercanos, especificados como un vector fila de dos elementos de longitudes de arco.

Argumentos de salida

contraer todo

Puntos más cercanos en la ruta de referencia, devueltos como una matriz numérica N por 6 con filas con la forma [x y theta kappa dkappa s], donde:

  • x y y theta — estado SE(2) expresado en coordenadas globales, con x y y en metros y theta en radianes

  • kappa — Curvatura, o inversa del radio, en m-1

  • dkappa — Derivada de la curvatura con respecto a la longitud del arco en m-2

  • s — Longitud del arco, o distancia a lo largo de la trayectoria desde el origen de la trayectoria, en metros

N es el número de puntos muestreados a lo largo de la ruta de referencia.

Indicación de si cada punto más cercano a la coordenada xy correspondiente en points se proyecta dentro de la ventana de búsqueda, y se devuelve como un vector columna lógico de elementos N, donde N es el número de puntos en points. Los puntos que se proyectan dentro de la ventana de búsqueda son true, o false si se encuentran al final de una ventana.

Capacidades ampliadas

expandir todo

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2020b

Consulte también

Objetos

Funciones

Temas