pathmetrics
Description
L'objet pathmetrics
contient des informations pour le calcul des métriques de chemin. Utilisez les fonctions d'objet pour calculer la fluidité, le dégagement et la validité du chemin en fonction d'un ensemble de poses et de l'environnement cartographique associé.
Création
Description
crée un objet basé sur l'objet d'entrée pathMetricsObj
= pathmetrics(path
)navPath
. Le validateur d'état est supposé être un objet validatorOccupancyMap
, si l'espace d'état de l'objet navPath
est un stateSpaceSE2
, stateSpaceDubins
, ou objet stateSpaceReedsShepp
. Sinon, le validateur d'état est supposé être un objet validatorOccupancyMap3D
, si l'espace d'état de l'objet navPath
est un objet stateSpaceSE3
. L'entrée path
définit la valeur de la propriété Path .
crée un objet basé sur l'objet d'entrée pathMetricsObj
= pathmetrics(path
,validator
)navPath
et le validateur d'état associé pour vérifier la validité du chemin. L'entrée validator
définit la valeur de la propriété StateValidator .
Propriétés
Path
— Structure des données de chemin
navPath
objet
La structure de données de chemin, spécifiée en tant qu'objet navPath
, est le chemin dont la métrique doit être calculée.
StateValidator
— Validateur pour les états sur le chemin
validatorOccupancyMap
objet | validatorVehicleCostmap
objet | validatorOccupancyMap3D
objet
Validateur pour les états sur le chemin, spécifié soit comme un objet validatorOccupancyMap
, validatorVehicleCostmap
ou validatorOccupancyMap3D
.
Pour les espaces d'état 2D, le validateur d'état par défaut est validatorOccupancyMap(stateSpaceSE2,binaryOccupancyMap(10))
.
Pour les espaces d'état 3D, le validateur d'état par défaut est validatorOccupancyMap3D(stateSpaceSE3)
.
Fonctions d'objet
clearance | Dégagement minimum du chemin |
isPathValid | Déterminer si le chemin prévu est sans obstacle |
show | Visualisez les métriques de chemin dans un environnement cartographique |
smoothness | Douceur du chemin |
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)
Historique des versions
Introduit dans R2019b
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)