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

plannerRRTStar

Créer un planificateur de chemin RRT optimal (RRT*)

Description

L'objet plannerRRTStar crée un planificateur RRT asymptotiquement optimal, RRT*. L'algorithme RRT* converge vers une solution optimale en termes de distance dans l'espace d'état. De plus, son temps d'exécution est un facteur constant du temps d'exécution de l'algorithme RRT. RRT* est utilisé pour résoudre des problèmes de planification géométrique. Un problème de planification géométrique nécessite que deux états aléatoires tirés de l’espace d’état puissent être connectés.

Création

Syntaxe

planner = plannerRRTStar(stateSpace,stateVal)

planner = plannerRRTStar(___,Name=Value)

Description

planner = plannerRRTStar(stateSpace,stateVal) crée un planificateur RRT* à partir d'un objet d'espace d'état, stateSpace, et d'un objet validateur d'état, stateVal. L'espace d'état de stateVal doit être le même que celui de stateSpace. stateSpace et stateVal définissent également les propriétés StateSpace et StateValidator du planner Objet $ .

planner = plannerRRTStar(___,Name=Value) définit les propriétés en utilisant un ou plusieurs arguments nom-valeur en plus des arguments d'entrée dans la syntaxe précédente. Vous pouvez spécifier le StateSampler, BallRadiusConstant, ContinueAfterGoalReached, MaxNumTreeNodes, MaxIterations, MaxConnectionDistance, GoalReachedFcn et GoalBias propriétés comme arguments nom-valeur.

Propriétés

StateSpace — Espace d'état pour le planificateur

objet d'espace d'état

Espace d'état pour le planificateur, spécifié comme objet d'espace d'état. Vous pouvez utiliser des objets d'espace d'état tels que stateSpaceSE2, stateSpaceDubins, stateSpaceReedsShepp et stateSpaceSE3. Vous pouvez également personnaliser un objet d'espace d'état à l'aide de l'objet nav.StateSpace .

StateValidator — Validateur d'état pour planificateur

objet validateur d'état

Validateur d'état pour le planificateur, spécifié en tant qu'objet validateur d'état. Vous pouvez utiliser des objets de validation d'état tels que validatorOccupancyMap, validatorVehicleCostmap et validatorOccupancyMap3D.

StateSampler — Échantillonneur d'espace d'état pour échantillonner l'espace d'entrée

stateSamplerUniform objet (par défaut) | stateSamplerGaussian objet | stateSamplerMPNET objet | nav.StateSampler objet

Depuis R2023b

Échantillonneur d'espace d'état utilisé pour rechercher des échantillons d'état dans l'espace d'entrée, spécifié comme objet stateSamplerUniform , objet stateSamplerGaussian , objet stateSamplerMPNET ou Objet nav.StateSampler . Par défaut, le plannerRRTStar utilise un échantillonnage d'état uniforme.

BallRadiusConstant — Constante utilisée pour estimer le rayon de recherche des voisins proches

100 (par défaut) | scalaire positif

Constante utilisée pour estimer le rayon de recherche des voisins proches, spécifié comme un scalaire positif. Le rayon est estimé comme suit :

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

où:

• γ — La valeur de la propriété BallRadiusConstant

• n — Nombre actuel de nœuds dans l'arborescence

• d — Dimension de l'espace d'état

• η — La valeur de la propriété MaxConnectionDistance

γ est défini comme suit :

$$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

où:

• V_Free — Volume libre approximatif dans l'espace de recherche

• V_Ball — Volume de la bille unitaire dans les dimensions d

Les formules ci-dessus définissent un BallRadiusConstant de taille "appropriée" pour un espace donné, ce qui signifie qu'à mesure que le nombre de nœuds remplissant l'espace augmente et que le rayon diminue, le nombre attendu de voisins augmente de manière logarithmique. Des valeurs plus élevées entraîneront un nombre moyen plus élevé de voisins dans la boule d par itération, conduisant à davantage de candidats au recâblage. Cependant, des valeurs inférieures à ce minimum suggéré pourraient conduire à un seul voisin proche, ce qui ne produirait pas de résultats asymptotiquement optimaux.

Exemple : BallRadiusConstant=80

Types de données : single | double

ContinueAfterGoalReached — Continuer à optimiser une fois l’objectif atteint

false (par défaut) | true

Décidez si le planificateur continue d'optimiser une fois l'objectif atteint, spécifié comme false ou true. Le planificateur se termine également quelle que soit la valeur de cette propriété si le nombre maximum d'itérations ou le nombre maximum de nœuds d'arborescence est atteint.

Exemple : ContinueAfterGoalReached=true

Types de données : logical

MaxNumTreeNodes — Nombre maximum de nœuds dans l'arborescence de recherche

1e4 (par défaut) | entier positif

Nombre maximum de nœuds dans l'arborescence de recherche (à l'exclusion du nœud racine), spécifié sous forme d'entier positif.

Exemple : MaxNumTreeNodes=2500

Types de données : single | double

MaxIterations — Nombre maximum d'itérations

1e4 (par défaut) | entier positif

Nombre maximum d'itérations, spécifié sous forme d'entier positif.

Exemple : MaxIterations=2500

Types de données : single | double

MaxConnectionDistance — Durée maximale du mouvement

0.1 (par défaut) | scalaire positif

Longueur maximale d'un mouvement autorisé dans l'arborescence, spécifiée sous forme de scalaire.

Exemple : MaxConnectionDistance=0.3

Types de données : single | double

GoalReachedFcn — Fonction de rappel pour déterminer si l'objectif est atteint

@nav.algs.checkIfGoalIsReached | poignée de fonction

Fonction de rappel pour déterminer si l'objectif est atteint, spécifiée comme handle de fonction. Vous pouvez créer votre propre fonction d’objectif atteint. La fonction doit suivre cette syntaxe :

 function isReached = myGoalReachedFcn(planner,currentState,goalState)

où:

• planner — L'objet de planificateur créé, spécifié comme objet plannerRRTStar .

• currentState — L'état actuel, spécifié comme un vecteur réel à trois éléments.

• goalState — L'état objectif, spécifié comme un vecteur réel à trois éléments.

• isReached — Une variable booléenne pour indiquer si l'état actuel a atteint l'état objectif, renvoyée sous la forme true ou false.

Pour utiliser le GoalReachedFcn personnalisé dans le flux de travail de génération de code, cette propriété doit être définie sur un handle de fonction personnalisé avant d'appeler la fonction de plan et elle ne peut pas être modifiée après l'initialisation.

Types de données : function handle

GoalBias — Probabilité de choisir l'état objectif lors de l'échantillonnage de l'état

0.05 (par défaut) | scalaire réel dans la plage [0,1]

Probabilité de choisir l'état objectif lors de l'échantillonnage d'état, spécifiée comme un scalaire réel dans la plage [0,1]. La propriété définit la probabilité de choisir l’état objectif réel pendant le processus de sélection aléatoire d’états dans l’espace d’états. Vous pouvez commencer par définir la probabilité sur une petite valeur telle que 0.05.

Exemple : GoalBias=0.1

Types de données : single | double

Fonctions d'objet

plan	Planifier le chemin entre deux états
copy	Create copy of planner object

Exemples

Planifier le chemin optimal entre deux États

Créez un espace d'état.

ss = stateSpaceSE2;

Créez un validateur d'état basé sur occupancyMap en utilisant l'espace d'état créé.

sv = validatorOccupancyMap(ss);

Créez une carte d'occupation à partir d'un exemple de carte et définissez la résolution de la carte sur 10 cellules/mètre.

load exampleMaps.mat
map = occupancyMap(simpleMap,10);
sv.Map = map;

Définissez la distance de validation pour le validateur.

sv.ValidationDistance = 0.01;

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

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

Créez un planificateur de chemin RRT* et permettez une optimisation supplémentaire une fois l'objectif atteint. Réduisez le nombre maximal d'itérations et augmentez la distance de connexion maximale.

planner = plannerRRTStar(ss,sv, ...
          ContinueAfterGoalReached=true, ...
          MaxIterations=2500, ...
          MaxConnectionDistance=0.3);

Définissez les états de départ et d’objectif.

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

Planifiez un chemin avec les paramètres par défaut.

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

Visualisez les résultats.

map.show
hold on
% Tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

Planifier le chemin à travers la carte d'occupation 3D à l'aide de RRT Star Planner

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);

Créez un planificateur de chemin en étoile 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 = plannerRRTStar(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 = [40 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);

Visualisez le chemin prévu.

show(omap)
axis equal
view([-10 55])
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)

En savoir plus

Constante de rayon de balle

La différence majeure entre RRT et RRT* réside dans le comportement de recâblage, qui garantit l'optimalité asymptotique de l'algorithme RRT*. Lorsque les planificateurs basés sur RRT génèrent un nouveau nœud, le planificateur trouve le nœud le plus proche dans l'arborescence. Si le chemin entre les nœuds est sans collision et autrement valide, l'algorithme RRT connecte les nœuds, mais l'algorithme RRT* effectue des étapes supplémentaires pour optimiser l'arborescence après avoir connecté les nœuds. Tout d'abord, RRT* trouve tous les nœuds de l'arborescence à une certaine distance du nouveau nœud, puis RRT* trouve le nœud qui fournit au nouveau nœud le chemin valide le plus court vers le nœud de départ et ajoute un bord entre le nœud et le nouveau. nœud. Enfin, le planificateur effectue une opération de recâblage, qui vérifie si le nouveau nœud peut fournir à chacun des nœuds les plus proches un itinéraire plus court vers le nœud de départ. Dans le cas où il existe un chemin plus court, le nœud est déconnecté du parent actuel et reparenté vers le nœud le plus proche.

Le rayon auquel le recâblage se produit est appelé rayon constant de la boule. La sélection d'une constante de rayon de balle appropriée est importante car l'objectif de RRT* est de garantir l'optimalité asymptotique tout en limitant tout calcul supplémentaire. Si la constante du rayon de la balle est trop grande, la durée d'exécution du RRT* augmente. Si la constante du rayon de la balle est trop petite, l’algorithme risque de ne pas parvenir à converger vers un résultat optimal.

plannerRRTStar utilise cette formule de distance, adaptée de [1], pour trouver les voisins les plus proches :

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

où:

• n — Nombre de nœuds dans l'arborescence

• d — Nombre de dimensions de l'espace d'état

• η — Distance de connexion maximale

• γ — Constante de rayon de balle, définie comme :

  $$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

  où:

  • V_Free — Mesure de Lebesgue, le volume libre approximatif dans l'espace de recherche

  • V_Ball — Volume de la bille unitaire dans les dimensions d

Signification et intuition de la constante du rayon de la balle

Les formules pour r et γ définissent un rayon de taille appropriée pour un espace et une densité d'échantillonnage donnés. Et à mesure que le nombre de nœuds remplissant l’espace augmente de manière linéaire, le rayon doit diminuer et le nombre de voisins à l’intérieur de la boule qui rétrécit augmente de manière logarithmique.

Cette intuition part de l’hypothèse que tous les points nouvellement échantillonnés dans l’arborescence ont été échantillonnés de manière uniforme et indépendante à partir d’une partie libre de l’espace de configuration. En échantillonnant les points de cette manière, vous pouvez dire qu'ils ont été générés à l'aide d'un processus de points de Poisson homogène. Cela signifie qu'à chaque itération de RRT*, les points n ont été uniformément échantillonnés dans l'espace libre, il devrait donc y avoir une densité moyenne de points par unité de volume, λ. Pour les espaces de dimensions arbitraires, il existe une intensité de points par unité de mesure.

$$\lambda =\frac{N}{V_{Free}}$$

Par conséquent, le nombre de points que vous pouvez vous attendre à voir dans n’importe quelle partie de l’espace de planification correspond au volume de cette partie, multiplié par la densité. Pour RRT*, l'accent est mis sur le nombre de points dans une boule de d dimensions du rayon r:

$$n_{1,d}=V_{Ball}·\lambda =\frac{V_{Ball}}{V_{Free}}·n$$

$${\text{n}}_{1,d}={\text{n}}_{1,d}·r^d$$

où,

• n_1,d — Nombre attendu de points à l'intérieur de la boule unitaire des dimensions d

• n_r,d — Nombre attendu de points à l'intérieur de la boule des dimensions d avec un rayon r

Et en rappelant que l'objectif du nombre de voisins est de croître de manière logarithmique à mesure que n s'approche de l'infini, vous pouvez définir n_r,d=log(n) et résoudre r:

$$\text{r=}{\left(\frac{V_{Free}}{V_{Ball}}·\frac{\log (n)}{n}\right)}^{\frac{1}{d}}$$

Les coefficients restants de la formule 2 sont dérivés de la preuve de convergence dans [1]. Cependant, avec n supprimé, vous pouvez voir que la constante du rayon de la balle est un rapport entre l'espace libre dans la région d'échantillon et la mesure de la balle unitaire multipliée par une constante spécifique à la dimension.

Références

[1] Karaman, S. and E. Frazzoli. "Sampling-Based Algorithms for Optimal Motion Planning." The International Journal of Robotics Research. Vol. 30, Number 7, 2011, pp 846 – 894.

Capacités étendues

Génération de code C/C++

Générez du code C et C++ avec MATLAB® Coder™.

Notes d'utilisation et limitations :

• Pour utiliser le GoalReachedFcn personnalisé dans le flux de travail de génération de code, cette propriété doit être définie sur un handle de fonction personnalisé avant d'appeler la fonction de plan et elle ne peut pas être modifiée après l'initialisation.

Historique des versions

Introduit dans R2019b

R2023b: Spécifier l'approche d'échantillonnage pour la planification du chemin

Vous pouvez désormais spécifier un échantillonnage uniforme, un échantillonnage gaussien, un échantillonnage MPNet ou une approche d'échantillonnage personnalisée pour générer des échantillons pour la planification du chemin. Utilisez le nom et l'argument de valeur StateSampler pour spécifier l'approche d'échantillonnage.

Voir aussi

Objets

• plannerRRT | plannerBiRRT | stateSpaceReedsShepp | stateSpaceDubins | stateSpaceSE2 | stateSpaceSE3 | validatorOccupancyMap | validatorVehicleCostmap | validatorOccupancyMap3D | stateSamplerUniform

Fonctions

• plan | copy

Rubriques

• Calculating an Appropriate BallRadiusConstant for PlannerRRStar