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

isPathValid

Déterminer si le chemin prévu est sans obstacle

Depuis R2019b

Syntaxe

isPathValid(pathMetricsObj)

Description

isPathValid(pathMetricsObj) renvoie soit un 1 (true) logique si le chemin prévu est sans obstacle, soit un 0 (false) logique si le chemin n'est pas valide.

Exemples

Calculer les métriques du chemin

Calculez la fluidité, le dégagement et la validité d'un chemin planifié en fonction d'un ensemble de poses et de l'environnement cartographique associé.

 Charger et attribuer une carte au validateur d'état 

Créez une carte d'occupation à partir d'un exemple de carte et définissez la résolution de la carte.

load exampleMaps.mat; % simpleMap
mapResolution = 1; % cells/meter
map = occupancyMap(simpleMap,mapResolution);

Créez un espace d'état Dubins.

statespace = stateSpaceDubins;

Créez un validateur d'état basé sur la carte d'occupation pour stocker les paramètres et les états dans l'espace d'état Dubins.

statevalidator = validatorOccupancyMap(statespace);

Attribuez la carte au validateur.

statevalidator.Map = map;

Définissez la distance de validation pour le validateur.

statevalidator.ValidationDistance = 0.01;

Mettez à jour les limites de l’espace d’état pour qu’elles soient identiques aux limites de la carte.

statespace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

Planifier le chemin

Créez un planificateur de chemin RRT* et permettez une optimisation plus poussée.

planner = plannerRRTStar(statespace,statevalidator);
planner.ContinueAfterGoalReached = true;

Réduisez le nombre maximum d'itérations et augmentez la distance de connexion maximale.

planner.MaxIterations = 2500;
planner.MaxConnectionDistance = 0.3;

Définissez les états de départ et d'objectif pour le planificateur de chemin sous forme de vecteurs [x, y, theta] . x et y sont les coordonnées cartésiennes et thêta est l'angle d'orientation.

start = [2.5, 2.5, 0]; % [meters, meters, radians]
goal = [22.5, 8.75, 0];

Planifiez un chemin depuis l’état de départ jusqu’à l’état final. La fonction plan renvoie un objet navPath .

rng(100,'twister') % repeatable result
[path,solutionInfo] = plan(planner,start,goal);

Calculer et visualiser les métriques de chemin

Créez un objet de métriques de chemin.

pathMetricsObj = pathmetrics(path,statevalidator);

Vérifiez la validité du chemin. Le résultat est 1 (true) si le chemin prévu est sans obstacle. 0 (false) indique un chemin non valide.

isPathValid(pathMetricsObj)

ans = logical
   1

Calculez le dégagement minimum du chemin.

clearance(pathMetricsObj)

ans = 1.4142

Évaluez la douceur du chemin. Les valeurs proches de 0 indiquent un chemin plus fluide. Les chemins en ligne droite renvoient une valeur de 0.

smoothness(pathMetricsObj)

ans = 1.7318

Visualisez le dégagement minimum du chemin.

show(pathMetricsObj)
legend('Planned Path','Minimum Clearance')

Planification du trajet des véhicules et calcul des métriques dans un environnement de carte de coûts 2D

Planifiez le trajet d'un véhicule dans un parking à l'aide de l'algorithme RRT*. Calculez et visualisez la fluidité, le dégagement et la validité du chemin prévu.

 Charger et attribuer une carte au validateur d'état 

Chargez une carte des coûts d'un parking. Tracez la carte des coûts pour voir le parking et les zones gonflées que le véhicule doit éviter.

load parkingLotCostmap.mat;
costmap = parkingLotCostmap;
plot(costmap)
xlabel('X (meters)')
ylabel('Y (meters)')

Créez un objet stateSpaceDubins et augmentez le rayon de rotation minimum à 4 mètres.

statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % meters

Créez un objet validatorVehicleCostmap en utilisant l'espace d'état créé.

statevalidator = validatorVehicleCostmap(statespace);

Attribuez la carte des coûts du parking à l’objet validateur d’état.

statevalidator.Map = costmap;

 Planifier le chemin 

Définissez les poses de départ et d'objectif pour le véhicule comme [x, y, Θ] vecteurs. Les unités mondiales pour les emplacements (x, y) sont en mètres. Les unités mondiales pour les angles d'orientation Θ sont en degrés.

startPose = [5, 5, 90]; % [meters, meters, degrees]
goalPose = [40, 38, 180]; % [meters, meters, degrees]

Utilisez un objet pathPlannerRRT (Automated Driving Toolbox) et la fonction plan (Automated Driving Toolbox) pour planifier le trajet du véhicule depuis la pose de départ jusqu'à la pose d'objectif.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

Interpolez le long du chemin tous les mètres. Convertissez les angles d'orientation de degrés en radians.

poses = zeros(size(refPath.PathSegments,2)+1,3);
poses(1,:) = refPath.StartPose;
for i = 1:size(refPath.PathSegments,2) 
    poses(i+1,:) = refPath.PathSegments(i).GoalPose; 
end
poses(:,3) = deg2rad(poses(:,3));

Créez un objet navPath en utilisant l'objet espace d'état Dubins et les états spécifiés par poses.

path = navPath(statespace,poses);

 Calculer et visualiser les métriques de chemin 

Créez un objet pathmetrics .

pathMetricsObj = pathmetrics(path,statevalidator);

Vérifiez la validité du chemin. Le résultat est 1 (true) si le chemin prévu est sans obstacle. 0 (false) indique un chemin non valide.

isPathValid(pathMetricsObj)

ans = logical
   1

Calculez et visualisez le clearance minimum du chemin.

clearance(pathMetricsObj)

ans = 0.5000

show(pathMetricsObj)
legend('Inflated Areas','Planned Path','Minimum Clearance')
xlabel('X (meters)')
ylabel('Y (meters)')

Calculez et visualisez le smoothness du chemin. Les valeurs proches de 0 indiquent un chemin plus fluide. Les chemins en ligne droite renvoient une valeur de 0.

smoothness(pathMetricsObj)

ans = 0.0842

show(pathMetricsObj,'Metrics',{'Smoothness'})
legend('Inflated Areas','Path Smoothness')
xlabel('X (meters)')
ylabel('Y (meters)')

Visualisez le dégagement pour chaque état du chemin.

show(pathMetricsObj,'Metrics',{'StatesClearance'})
legend('Inflated Areas','Planned Path','Clearance of Path States')
xlabel('X (meters)')
ylabel('Y (meters)')

Planification de chemin et calcul de métriques dans un environnement 3D

Planifiez un chemin à travers un pâté de maisons à l'aide de l'algorithme RRT. Calculez et visualisez la fluidité, le dégagement et la validité du chemin prévu.

Charger et attribuer une carte au validateur d'état

Chargez une carte d'occupation 3D d'un pâté de maisons dans l'espace de travail. Spécifiez le seuil pour considérer les cellules comme sans obstacle.

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

Gonflez la carte d'occupation pour ajouter une zone tampon pour un fonctionnement sûr autour des obstacles.

inflate(omap,1)

Créez un objet d'espace d'état SE(3) avec des limites pour les variables d'état.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

Créez un validateur d'état de carte d'occupation 3D à l'aide de l'espace d'état créé. Attribuez la carte d'occupation à l'objet validateur d'état. Spécifiez l’intervalle de distance d’échantillonnage.

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

Planifier le chemin

Créez un planificateur de chemin RRT avec une distance de connexion maximale accrue et un nombre maximal d'itérations réduit. Spécifiez une fonction d'objectif personnalisée qui détermine qu'un chemin atteint l'objectif si la distance euclidienne jusqu'à la cible est inférieure à un seuil de 1 mètre.

planner = plannerRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

Spécifiez les poses de départ et d’objectif.

start = [50 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Configurez le générateur de nombres aléatoires pour un résultat reproductible.

rng(1,"twister")

Planifiez le chemin.

[pthObj,solnInfo] = plan(planner,start,goal);

Calculer et visualiser les métriques de chemin

Créez un objet de métriques de chemin.

pathMetricsObj = pathmetrics(pthObj,sv);

Vérifiez la validité du chemin. Le résultat est 1 (true) si le chemin prévu est sans obstacle. 0 (false) indique un chemin non valide.

isPathValid(pathMetricsObj)

ans = logical
   1

Calculez le dégagement minimum du chemin.

clearance(pathMetricsObj)

ans = 10

Évaluez la douceur du chemin. Les valeurs proches de 0 indiquent un chemin plus fluide. Les chemins en ligne droite renvoient une valeur de 0.

smoothness(pathMetricsObj)

ans = 0.0011

Visualisez le dégagement minimum du chemin.

show(pathMetricsObj)
axis equal
view([100 75])
hold on
% Start state
scatter3(start(1,1),start(1,2),start(1,3),"g","filled")
% Goal state
scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled")
% Path
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ...
      "r-",LineWidth=2)

Arguments d'entrée

pathMetricsObj — Informations sur les métriques de chemin

Objet pathmetrics

Informations sur les métriques de chemin, spécifiées en tant qu'objet pathmetrics .

Historique des versions

Introduit dans R2019b

Voir aussi

Objets

• pathmetrics

Fonctions

• clearance | show | smoothness