Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

closestPoint

Trouver le point le plus proche sur le chemin de référence jusqu'au point global

Depuis R2020b

Syntaxe

pathPoints = closestPoint(refPath,points)

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

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

Description

pathPoints = closestPoint(refPath,points) trouve le point le plus proche sur le chemin de référence de chacune des positions spécifiées (x, y)-positions points.

[pathPoints,inWindow] = closestPoint(refPath,points) renvoie éventuellement un vecteur logique inWindow, spécifiant si chaque point de la coordonnée xy correspondante dans points est projeté dans la fenêtre de recherche.

[_] = closestPoint(refPath,points,searchWindow) accepte éventuellement un vecteur ligne non décroissant searchWindow, qui définit l'intervalle du chemin à utiliser pour trouver les points les plus proches.

Exemples

Générer une trajectoire à partir du chemin de référence

Générez un chemin de référence à partir d'un ensemble de waypoints.

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

Créez un objet trajectoryGeneratorFrenet à partir du chemin de référence.

connector = trajectoryGeneratorFrenet(refPath);

Générez une trajectoire de cinq secondes entre l'origine du chemin et un point situé à 30 mètres du chemin, comme l'indique Frenet.

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

Convertissez la trajectoire en états globaux.

globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

Afficher le chemin de référence et la trajectoire.

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

Spécifiez les points globaux et recherchez les points les plus proches sur le chemin de référence.

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

Affichez les points globaux et les points les plus proches sur le chemin de référence.

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

Interpolez entre les longueurs d’arc des deux premiers points les plus proches le long du chemin de référence.

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

Affichez les points de chemin interpolés.

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

Obtenez les points les plus proches aux intersections de chemins

Créez un chemin de référence auto-entrecroisé.

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

Afficher le chemin de référence.

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

Trouvez la longueur de l'arc à laquelle l'intersection se produit.

sIntersection = refPath.SegmentParameters(2,end);

Générez les états de Frenet situés juste avant et après l'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]

Calculez le temps nécessaire pour voyager à vitesse longitudinale constante.

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

Créez un générateur de trajectoire en utilisant le chemin de référence.

generator = trajectoryGeneratorFrenet(refPath);

Générez une trajectoire entre les points.

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

Définissez les fonctions d'assistance au tracé.

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

Tracez la trajectoire.

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

Calculez le point le plus proche sur le chemin de chaque état global.

closestPts = closestPoint(refPath,pts);

Tracez les vecteurs de points les plus proches.

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

Définissez une fenêtre dans laquelle rechercher les points les plus proches.

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

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

Trouvez les points les plus proches dans la fenêtre.

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

Afficher les résultats fenêtrés.

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

Trouvez les points les plus proches en utilisant une fenêtre trop petite.

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

Superposez les résultats d’une petite fenêtre.

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

Arguments d'entrée

refPath — Chemin de référence

referencePathFrenet objet

Chemin de référence, spécifié en tant qu'objet referencePathFrenet .

points — Points globaux

P-matrice numérique par 2

Points globaux, spécifiés sous la forme d'une matrice numérique P-par 2 avec des lignes de la forme [x y]. P est le nombre de points. Les positions sont en mètres.

searchWindow — Fenêtre de recherche

vecteur de ligne à deux éléments

Fenêtre de recherche sur le chemin pour déterminer les points les plus proches, spécifiés sous la forme d'un vecteur ligne à deux éléments de longueurs d'arc.

Arguments de sortie

pathPoints — Points les plus proches sur le chemin de référence

N-matrice numérique par 6

Points les plus proches sur le chemin de référence, renvoyés sous la forme d'une matrice numérique N-par 6 avec des lignes de la forme [x y theta kappa dkappa s], où :

• x y et theta — État SE(2) exprimé en coordonnées globales, avec x et y en mètres et theta en radians

• kappa — Courbure, ou inverse du rayon, en m-1

• dkappa — Dérivée de la courbure par rapport à la longueur de l'arc dans m-2

• s — Longueur de l'arc ou distance le long du chemin depuis l'origine du chemin, en mètres

N est le nombre de points échantillonnés le long du chemin de référence.

inWindow — Indicateur des points étant dans la fenêtre de recherche

N-élément vecteur de colonne logique

Indique si chaque point le plus proche de la coordonnée xy correspondante dans points est projeté dans la fenêtre de recherche, renvoyé sous la forme d'un élément N vecteur de colonne logique, où N est le nombre de points dans points. Les points projetés dans la fenêtre de recherche sont true, ou false s'ils se trouvent au bout d'une fenêtre.

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2020b

Voir aussi

Objets

• referencePathFrenet | trajectoryGeneratorFrenet | navPath

Fonctions

• frenet2global | global2frenet | closestPointsToSequence | closestProjections | curvature | changeInCurvature | tangentAngle | interpolate | position | show

Rubriques

• Highway Trajectory Planning Using Frenet Reference Path