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

frenet2cart

Convertir les états Frenet en états cartésiens

Syntaxe

frenet2cart(planner,frenetStates)

Description

frenet2cart(planner,frenetStates) convertit un vecteur à six éléments de frenetStates [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2] en un vecteur à six éléments d'états cartésiens [x, y, theta, kappa, speed, acceleration].

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

Arguments d'entrée

planner — Planificateur de trajectoire optimale dans l'espace Frenet

Objet trajectoryOptimalFrenet

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

frenetStates — Vecteur des états Frenet

vecteur à six éléments

Vecteur des états Frenet, spécifié comme 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.

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

• d²l/ds² spécifie la dérivée seconde de la distance normale.

Exemple : [10 1 0 3 0 0]

Types de données : double

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

trajectoryOptimalFrenet | cart2frenet