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

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.

Arguments en entrée

refPath — Chemin de référence

n-matrice par 2

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

validator — Objet validateur d’état

validatorOccupancyMap objet

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

Propriétés

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'.

Weights — Pondérations pour tous les coûts de trajectoire

structure

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.

Time — Poids pour le coût du temps

0 (par défaut) | scalaire positif

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

ArcLength — Poids pour le coût de la longueur de l'arc

0 (par défaut) | scalaire positif

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

LateralSmoothness — Poids pour coût de secousse latérale

0 (par défaut) | scalaire positif

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

LongitudinalSmoothness — Poids pour le coût de la secousse longitudinale

0 (par défaut) | scalaire positif

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

Deviation — Poids pour écart par rapport au chemin de référence

1 (par défaut) | scalaire positif

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

FeasibilityParameters — Structure contenant les paramètres de faisabilité

structure

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.

MaxCurvature — Courbure maximale que le véhicule peut exécuter

0.1 (par défaut) | scalaire réel positif

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

MaxAcceleration — Accélération maximale dans le sens de déplacement du véhicule

2.5 (par défaut) | scalaire réel positif

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/s². 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

TimeResolution — Intervalle de discrétisation de la trajectoire

0.1 (par défaut) | scalaire réel positif

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

CostFunction — Fonction de coût définie par l'utilisateur

coût nul (par défaut) | poignée de fonction

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

TrajectoryList — Liste de toutes les trajectoires possibles

tableau de structures

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

TerminalStates — Structure de tous les états d'objectif

structure

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.

Longitudinal — Longueurs du segment de trajectoire

30:15:90 (par défaut) | vecteur

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

Types de données : double

Lateral — Tableau des écarts par rapport au chemin de référence dans la direction perpendiculaire à l'état objectif

-2:1:2 (par défaut) | vecteur

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

Speed — Vitesse à l'état cible dans la direction du mouvement

10 (par défaut) | scalaire positif

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

Acceleration — Accélération à l'état cible dans la direction du mouvement

0 (par défaut) | scalaire positif

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

Types de données : double

Time — Tableau d'heures de fin pour l'exécution du segment de trajectoire

7 (par défaut) | vecteur positif

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

Waypoints — Waypoints du chemin de référence

[ ] (par défaut) | n-matrice par 2

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

NumSegments — Nombre de segments longitudinaux pour chaque trajectoire

1 (par défaut) | scalaire positif

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

DeviationOffset — Décalage d'écart par rapport à la trajectoire de référence dans la direction latérale

0 (par défaut) | scalaire

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

cart2frenet	Convert Cartesian states to Frenet states
copy	Créer une copie complète de l'objet
frenet2cart	Convertir les états Frenet en états cartésiens
plan	Planifier une trajectoire optimale
show	Visualiser les trajectoires

Exemples

Planification de trajectoire optimale dans l'espace Frenet

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

Partitionnement des états terminaux longitudinaux dans la génération de trajectoire

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','--')

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

Génération de chemin de référence

Les objets trajectoryOptimalFrenet et referencePathFrenet génèrent un chemin de référence en utilisant les points de cheminement spécifiés en ajustant une spline Clothoïde continue par morceaux aux points de cheminement [2].

• Une Clothoïde relie chaque paire de waypoints adjacents (le changement de courbure est constant par rapport à la longueur de l'arc).

• Ces conditions aux limites définissent les paramètres de la Clothoïde :

  • S'il est spécifié comme un n-par 2 de points de cheminement [x y], alors le solveur produira une spline continue C2. (les extrémités Clothoïdes sont tangentes et ont une courbure correspondante à celle de leurs voisins).

  • S'il est spécifié comme un tableau n-par 3 de [x y theta] waypoints, Ensuite, un degré de liberté est supprimé du solveur, de sorte que la spline n'est continue que C1 (les extrémités de la Clothoïde sont tangentes à leurs voisines, mais la courbure peut changer brusquement).

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

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

validatorOccupancyMap | nav.StateValidator | plannerHybridAStar