plan
Description
[
calcule une trajectoire réalisable, traj
,index
,cost
,flag
] = plan(planner
,start
)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
.
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
trajectoryOptimalFrenet
objet
Planificateur de trajectoire optimal dans l'espace Frenet, spécifié comme un objet trajectoryOptimalFrenet
.
start
— État Frenet initial
vecteur à six éléments
É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
traj
— Trajectoire réalisable avec un coût minimum
n-matrice par 7
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
.
index
— Indice de trajectoire réalisable à coût minimum
scalaire entier positif
Indice de trajectoire réalisable avec un coût minimum, renvoyé sous forme de scalaire entier positif.
cost
— Le moindre coût de la trajectoire réalisable
scalaire positif
Le moindre coût de la trajectoire réalisable, renvoyé sous forme de scalaire positif.
flag
— Indicateur de sortie indiquant l'état de la solution
0
| 1
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
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
Commande MATLAB
Vous avez cliqué sur un lien qui correspond à cette commande MATLAB :
Pour exécuter la commande, saisissez-la dans la fenêtre de commande de MATLAB. Les navigateurs web ne supportent pas les commandes MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)