clearance
Dégagement minimum du chemin
Description
clearance( renvoie le dégagement minimum du chemin. Le dégagement est mesuré comme la distance minimale entre les centres des cellules de la grille des états sur le chemin et les obstacles dans l'environnement cartographique spécifié.pathMetricsObj)
Remarque
Le jeu calculé est précis jusqu'à sqrt(2) multiplié par la taille des cellules de la carte de la grille.
clearance( renvoie l'ensemble des distances minimales pour chaque état du chemin, sous la forme d'un vecteur n-par-1, où n est le nombre d'états.pathMetricsObj,'Type','states')
Exemples
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')
![Figure contains an axes object. The axes object with xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, patch, line. These objects represent Planned Path, Minimum Clearance.](computepathmetricsexample_01_fr.png)
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; % metersCré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)')
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
Historique des versions
Introduit dans R2019b
