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

trajectoryOptimalFrenet

Trouver la trajectoire optimale le long du chemin de référence

développer tous les éléments de la page

Description

L'objet trajectoryOptimalFrenet est un planificateur de chemin qui échantillonne et évalue les trajectoires locales en fonction d'un chemin de référence. Le planificateur génère un ensemble d'états terminaux basés sur le chemin de référence et d'autres paramètres de l'objet. Le planificateur connecte ensuite l'état à chaque état terminal à l'aide de polynômes du quatrième ou du cinquième ordre. Pour choisir un chemin optimal, les trajectoires échantillonnées sont évaluées en termes de faisabilité cinématique, de collision et de coût.

Création

Syntaxe

trajectoryOptimalFrenet(refPath,validator)
planner = trajectoryOptimalFrenet(___,Name,Value)

Description

trajectoryOptimalFrenet(refPath,validator) crée un objet trajectoryOptimalFrenet avec un chemin de référence, refPath, sous la forme d'un n- tableau by-2 de points de cheminement [x y] et un validateur d'état, validator, spécifié comme un objet validatorOccupancyMap .

planner = trajectoryOptimalFrenet(___,Name,Value) définit des propriétés supplémentaires en utilisant une ou plusieurs paires nom-valeur dans n'importe quel ordre.

exemple

Arguments en entrée

développer tout

Chemin de référence, spécifié sous la forme d'une matrice n-par 2 de paires [x y] , où n est le nombre de points de cheminement.

Exemple : [100 100 ;400 400]

Types de données : double

Objet validateur d'état, spécifié comme objet validatorOccupancyMap .

Propriétés

développer tout

Remarque

Pour les propriétés 'Weights' et 'FeasibilityParameters' , vous ne pouvez pas spécifier l'intégralité des structures à la fois. Au lieu de cela, définissez leurs champs individuellement sous forme de paires nom-valeur. Par exemple, trajectoryOptimalFrenet(refPath,validator,'Deviation',0) définit le champ 'Deviation' de la structure 'Weights'.

Les pondérations pour tous les coûts de trajectoire, spécifiées sous forme de structure contenant des scalaires pour les multiplicateurs de coûts des attributs de trajectoire correspondants. Le coût total de la trajectoire est la somme de tous les attributs multipliée par leurs poids. La structure possède ces champs.

La fonction de coût multiplie le poids par le temps total nécessaire pour atteindre l'état terminal. Spécifiez cette valeur sous la forme d'une paire de 'Time' séparés par des virgules et d'un scalaire positif en secondes.

Types de données : double

La fonction de coût multiplie le poids par la longueur totale des trajectoires générées. Spécifiez cette valeur sous la forme d'une paire de 'ArcLength' séparés par des virgules et d'un scalaire positif en mètres.

Types de données : double

La fonction de coût multiplie le poids par l'intégrale de l'à-coup latéral au carré. Cette valeur détermine l'agressivité de la trajectoire dans la direction latérale (perpendiculaire à la trajectoire de référence). Spécifiez cette valeur sous la forme d'une paire de 'LateralSmoothness' séparés par des virgules et d'un scalaire positif. Pour pénaliser l'à-coup latéral dans la trajectoire prévue, augmenter cette valeur de coût.

Types de données : double

La fonction de coût multiplie le poids par l'intégrale de l'à-coup longitudinal au carré. Cette valeur détermine l'agressivité des trajectoires dans le sens longitudinal (direction de la trajectoire de référence). Spécifiez cette valeur sous la forme d'une paire de 'LongitudinalSmoothness' séparés par des virgules et d'un scalaire positif. Pour pénaliser les changements importants d'accélération vers l'avant et vers l'arrière, augmentez cette valeur de coût.

Types de données : double

La fonction de coût multiplie le poids par la distance perpendiculaire au chemin de référence à la fin de la trajectoire en mètres. Spécifiez cette valeur sous la forme d'une paire de 'Deviation' séparés par des virgules et d'un scalaire positif en mètres.

Types de données : double

Types de données : struct

Paramètres de faisabilité, spécifiés sous forme de structure contenant des valeurs scalaires pour vérifier la validité d'une trajectoire. La structure possède ces champs.

Courbure maximale que le véhicule peut exécuter. Spécifiez cette valeur sous la forme d'une paire de 'MaxCurvature' séparés par des virgules et d'un scalaire réel positif en m-1. Cette valeur détermine la faisabilité cinématique de la trajectoire.

Types de données : double

Accélération maximale dans le sens de déplacement du véhicule. Spécifiez cette valeur sous la forme d'une paire de 'MaxAcceleration' séparés par des virgules et d'un scalaire réel positif en m/s2. Pour abaisser la limite d'accélération du véhicule en marche avant ou en marche arrière, diminuez cette valeur.

Types de données : double

Types de données : struct

Intervalle de temps entre les états discrétisés de la trajectoire. Spécifiez cette valeur sous la forme d'une paire de 'TimeResolution' séparés par des virgules et d'un scalaire réel positif en secondes. Ces états discrétisés déterminent la validité de l'état et la fonction de coût.

Types de données : double

Fonction de coût définie par l'utilisateur, spécifiée comme descripteur de fonction. La fonction doit accepter une matrice d'états n-by-7, TRAJSTATES, pour chaque trajectoire et renvoyer une valeur de coût sous forme de scalaire. La fonction plan renvoie le chemin ayant le coût le plus bas.

Par exemple, leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10) crée un descripteur de fonction de coût pour prioriser les changements sur la voie de gauche.

Types de données : function handle

Ce propriété est en lecture seule.

La propriété 'TrajectoryList' , renvoyée sous forme de tableau de structure de toutes les trajectoires candidates et de leurs paramètres correspondants. Chaque structure possède ces champs :

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

  • Cost — Coût de la trajectoire.

  • MaxAcceleration — Accélération maximale de la trajectoire.

  • MaxCurvature — Courbure maximale de la trajectoire.

  • Feasible — Un vecteur à quatre éléments [velocity, acceleration, curvature, collision] indiquant la validité de la trajectoire.

    La valeur des éléments peut être soit,

    • 1 — La trajectoire est valide.

    • 0 — La trajectoire n'est pas valide.

    • -1 — La trajectoire n'est pas vérifiée.

Types de données : struct

Structure qui contient une liste d’états d’objectif par rapport au chemin de référence. Ces paramètres définissent le comportement d'échantillonnage pour générer des segments de trajectoire alternatifs entre le début et chaque état objectif. La structure possède ces champs.

Longueurs du segment de trajectoire, spécifiées sous forme de vecteur en mètres.

Types de données : double

Tableau des écarts par rapport au chemin de référence dans la direction perpendiculaire à l'état objectif, spécifié sous forme de vecteur en mètres.

Types de données : double

Vitesse à l'état final dans la direction du mouvement, spécifiée sous forme d'un scalaire positif en m/s.

Types de données : double

Accélération à l'état cible dans la direction du mouvement, spécifiée sous forme d'un scalaire positif en m/s2.

Types de données : double

Tableau d'heures de fin pour l'exécution du segment de trajectoire, spécifié sous forme de vecteur positif en secondes.

Types de données : double

Types de données : struct

Points de cheminement du chemin de référence, spécifiés sous la forme d'une matrice n-par 2 de paires [x y] , où n est le nombre de points de cheminement. Les waypoints servent de référence pour planifier des trajectoires alternatives optimisées par ce planificateur.

Types de données : double

Nombre de segments longitudinaux pour chaque trajectoire. Spécifiez cette valeur sous la forme d'une paire de 'NumSegments' séparés par des virgules et d'un scalaire positif. Cette propriété génère des états terminaux longitudinaux intermédiaires auxquels tous les états terminaux latéraux sont combinés pour générer davantage de primitives de mouvement pour chaque état terminal.

Par exemple, 'NumSegments',2 crée deux partitions entre chaque état terminal longitudinal. Des trajectoires sont générées pour atteindre les états longitudinaux intermédiaires avec tous les états terminaux latéraux disponibles.

Types de données : double

Déviation décalée par rapport à la trajectoire de référence dans la direction latérale. Spécifiez cette valeur sous la forme d'une paire de 'DeviationOffset' séparés par des virgules et d'un scalaire. Une valeur négative compense la déviation vers la droite et une valeur positive compense la déviation vers la gauche de la trajectoire de référence dans la direction latérale. Définissez cette propriété pour orienter votre solution vers une certaine direction de virage lorsque vous évitez les obstacles sur la trajectoire de référence.

Types de données : double

Fonctions d'objet

cart2frenetConvert Cartesian states to Frenet states
copyCréer une copie complète de l'objet
frenet2cartConvertir les états Frenet en états cartésiens
planPlanifier une trajectoire optimale
showVisualiser les trajectoires

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.

Limites

  • Les auto-intersections dans le chemin de référence peuvent entraîner un comportement inattendu.

  • Le planificateur ne prend pas en charge la conduite en marche arrière.

  • L'orientation initiale pour la planification doit se situer entre -pi/2 et pi/2 par rapport au chemin de référence.

  • Limitez le nombre de TerminalStates pour les applications en temps réel, car la complexité informatique augmente avec elle.

En savoir plus

développer tout

Références

[1] Werling, Moritz, Julius Ziegler, Sören Kammel, and Sebastian Thrun. "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame." 2010 IEEE International Conference on Robotics and Automation. 2010, pp. 987–993.

[2] Bertolazzi, Enrico, and Marco Frego. “Fast and Accurate Clothoid Fitting.” Mathematical Methods in the Applied Sciences 38, no. 5 (March 30, 2015): 881–97. https://doi.org/10.1002/mma.3114.

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

| |