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

plan

Planifier une trajectoire optimale

réduire tous les éléments de la page

Syntaxe

[traj,index,cost,flag] = plan(planner,start)

Description

[traj,index,cost,flag] = plan(planner,start) calcule une trajectoire réalisable, traj, à partir d'une liste de trajectoires candidates générées à partir de l'objet trajectoryOptimalFrenet , planner. start est spécifié comme un vecteur à six éléments [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2], où s est la longueur de l'arc à partir du premier point du chemin de référence, et l est la distance normale du point le plus proche à s sur le chemin de référence.

La trajectoire de sortie, traj, a également un cost et un index associés pour la propriété TrajectoryList de le planificateur. flag est un indicateur de sortie numérique indiquant l'état de la solution.

Pour améliorer les résultats de la sortie de planification, modifiez les paramètres de l'objet planner .

exemple

Exemples

réduire tout

Ouvrir le live script

Cet exemple montre comment planifier une trajectoire optimale à l'aide d'un objet trajectoryOptimalFrenet .

Créer et attribuer une carte au validateur d'état

Créez un objet validateur d'état pour la vérification des collisions.

stateValidator = validatorOccupancyMap;

Créez une carte quadrillée d’obstacles.

grid = zeros(50,100);
grid(24:26,48:53) = 1;

Créez un binaryOccupancyMap avec la carte quadrillée.

map = binaryOccupancyMap(grid);

Attribuez la carte et les limites de l'état au validateur d'état.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Planifier et visualiser la trajectoire

Créez un chemin de référence que le planificateur devra suivre.

refPath = [0,25;100,25];

Initialisez l'objet planificateur avec le chemin de référence et le validateur d'état.

planner = trajectoryOptimalFrenet(refPath,stateValidator);

Attribuez des valeurs d’état terminal longitudinal, d’écart latéral et d’accélération maximale.

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -10:5:10;
planner.FeasibilityParameters.MaxAcceleration = 10;

Spécifiez la valeur de décalage de déviation proche de l’état terminal latéral gauche pour donner la priorité aux changements de voie de gauche.

planner.DeviationOffset = 5;

 Planification de trajectoire 

Etat cartésien initial du véhicule.

initCartState = [0 25 pi/9 0 0 0];

Convertir l'état cartésien du véhicule en état Frenet.

initFrenetState = cart2frenet(planner,initCartState);

Planifiez une trajectoire à partir de l'état initial de Frenet.

plan(planner,initFrenetState);

Visualisation de trajectoire

Visualisez la carte et les trajectoires.

show(map)
hold on
show(planner,'Trajectory','all')

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, patch. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Optimal Trajectory.

Ouvrir le live script

Cet exemple montre comment partitionner les états terminaux longitudinaux dans une planification de trajectoire optimale à l'aide d'un objet trajectoryOptimalFrenet .

Créer et attribuer une carte au validateur d'état

Créez un objet validateur d'état pour la vérification des collisions.

stateValidator = validatorOccupancyMap;

Créez une carte quadrillée d’obstacles.

grid = zeros(50,100);
grid(25:27,28:33) = 1;
grid(16:18,37:42) = 1;
grid(29:31,72:77) = 1;

Créez un binaryOccupancyMap avec la carte quadrillée.

map = binaryOccupancyMap(grid);

Attribuez la carte et les limites de l'état au validateur d'état.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Planifier et visualiser la trajectoire

Créez un chemin de référence que le planificateur devra suivre. 

refPath = [0,25;30,30;75,20;100,25];

Initialisez l'objet planificateur avec le chemin de référence et le validateur d'état.

planner = trajectoryOptimalFrenet(refPath,stateValidator);

Attribuez des valeurs d’état terminal longitudinal, d’écart latéral et d’accélération maximale.

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -5:5:5;
planner.FeasibilityParameters.MaxAcceleration = 10;

Attribuez le nombre de partitions pour l’état terminal longitudinal.

planner.NumSegments = 3;

 Planification de trajectoire 

Etat Frenet initial du véhicule.

initFrenetState = zeros(1,6);

Planifiez une trajectoire à partir de l'état initial de Frenet.

plan(planner,initFrenetState);

Visualisation de trajectoire

Visualisez la carte et les trajectoires.

show(map)
hold on
show(planner,'Trajectory','all')
hold on

 Générer des limites de voie 

Calculez la fin du chemin de référence en tant qu'état de Frenet.

refPathEnd = cart2frenet(planner,[planner.Waypoints(end,:) 0 0 0 0]);

Calculez les décalages de voie des deux côtés des états terminaux latéraux avec une valeur de demi-largeur de voie.

laneOffsets = unique([planner.TerminalStates.Lateral+2.5 planner.TerminalStates.Lateral-2.5]);

Calculez les positions des voies dans l'état cartésien.

numLaneOffsets = numel(laneOffsets);
xRefPathEnd = ceil(refPathEnd(1));
laneXY = zeros((numLaneOffsets*xRefPathEnd)+numLaneOffsets,2);
xIndex = 0;

for laneID = 1:numLaneOffsets
    for x = 1:xRefPathEnd
        laneCart = frenet2cart(planner,[x 0 0 laneOffsets(laneID) 0 0]);
        xIndex = xIndex + 1;
        laneXY(xIndex,:) = laneCart(1:2);
    end
    xIndex = xIndex + 1;
    laneXY(xIndex,:) = NaN(1,2);
end

Tracer les limites des voies.

plot(laneXY(:,1),laneXY(:,2),'LineWidth',0.5,'Color',[0.5 0.5 0.5],'DisplayName','Lane Boundaries','LineStyle','--')

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 6 objects of type image, line, patch. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Optimal Trajectory, Lane Boundaries.

Arguments d'entrée

réduire tout

Planificateur de trajectoire optimal dans l'espace Frenet, spécifié comme un objet trajectoryOptimalFrenet .

État Frenet initial, spécifié sous la forme d'un vecteur 1 x 6 [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2].

  • s spécifie la longueur de l'arc à partir du premier point du chemin de référence en mètres.

  • ds/dt spécifie la dérivée première de la longueur de l'arc.

  • d2s/dt2 spécifie la dérivée seconde de la longueur de l'arc.

  • l spécifie la distance normale du point le plus proche du chemin de référence en mètres.

  • dl/ds spécifie la dérivée première de la distance normale.

  • d2l/ds2 spécifie la dérivée seconde de la distance normale.

Arguments de sortie

réduire tout

Trajectoire réalisable avec un coût minimum, renvoyée sous la forme d'une matrice n-par 7 de [x, y, theta, kappa, speed, acceleration, time], où n est le nombre de points de cheminement de la trajectoire .

  • x et y précisent la position en mètres.

  • theta spécifie l'angle d'orientation en radians.

  • kappa spécifie la courbure dans m-1.

  • speed spécifie la vitesse en m/s.

  • acceleration spécifie l'accélération dans m/s2.

  • time spécifie l'heure dans s.

Indice de trajectoire réalisable avec un coût minimum, renvoyé sous forme de scalaire entier positif.

Le moindre coût de la trajectoire réalisable, renvoyé sous forme de scalaire positif.

Indicateur de sortie indiquant l'état de la solution, renvoyé soit sous la forme 0 ou 1.

  • 0 — La trajectoire optimale a été trouvée.

  • 1 — Aucune trajectoire réalisable n'existe.

Lorsqu'aucune trajectoire réalisable n'existe, le planificateur renvoie une trajectoire vide.

Capacités étendues

développer tout

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

Historique des versions

Introduit dans R2019b

Voir aussi

|