plannerRRTStar
Créer un planificateur de chemin RRT optimal (RRT*)
Depuis R2019b
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
Description
crée un planificateur RRT* à partir d'un objet d'espace d'état, planner
= plannerRRTStar(stateSpace
,stateVal
)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 $ .
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.planner
= plannerRRTStar(___,Name=Value
)
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 :
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 :
où:
VFree — Volume libre approximatif dans l'espace de recherche
VBall — 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 objetplannerRRTStar
.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 formetrue
oufalse
.
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
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 :
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 :
où:
VFree — Mesure de Lebesgue, le volume libre approximatif dans l'espace de recherche
VBall — Volume de la bille unitaire dans les dimensions d
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.
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:
où,
n1,d — Nombre attendu de points à l'intérieur de la boule unitaire des dimensions d
nr,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 nr,d=log(n) et résoudre r:
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 R2019bR2023b: 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
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)