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

plan

Planifier le chemin entre deux états

Depuis R2021a

Syntaxe

path = plan(planner,startState,goalState)

[path,solnInfo] = plan(planner,startState,goalState)

Description

path = plan(planner,startState,goalState) renvoie un chemin bidirectionnel d'arbre aléatoire à exploration rapide (RRT) depuis l'état de départ jusqu'à l'état final en tant qu'objet navPath .

[path,solnInfo] = plan(planner,startState,goalState) renvoie également les informations de solution issues de la planification du chemin.

Exemples

Planifier un chemin entre deux états à l'aide de RRT bidirectionnel

Utilisez l'objet plannerBiRRT pour planifier un chemin entre deux états dans un environnement comportant des obstacles. Visualisez le chemin prévu avec les états interpolés.

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 par mètre.

load exampleMaps
map = occupancyMap(ternaryMap,10);

Attribuez la carte d'occupation à l'objet validateur d'état. Spécifiez l’intervalle de distance d’échantillonnage.

sv.Map = map;
sv.ValidationDistance = 0.01;

Mettez à 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 le planificateur de chemin et augmentez la distance de connexion maximale.

planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 0.3;

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

start = [20 10 0];
goal = [40 40 0];

Planifiez un chemin. En raison du caractère aléatoire de l'algorithme RRT, définissez la valeur de départ rng pour la répétabilité.

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

Affiche le nombre d'itérations nécessaires pour que l'arbre converge.

fprintf("Number of iterations: %d\n",solnInfo.NumIterations)

Number of iterations: 346

Visualisez les résultats.

show(map)
hold on
% Plot start pose and goal pose
plot(start(1), start(2),plannerLineSpec.start{:});
plot(goal(1), goal(2), plannerLineSpec.goal{:});
% Start tree expansion
plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ...
    plannerLineSpec.tree{:})
% Goal tree expansion
plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ...
    plannerLineSpec.goalTree{:})
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})
legend
hold off

Replanifiez le chemin avec la propriété EnableConnectHeuristic définie sur true.

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

Affiche le nombre d'itérations nécessaires pour que l'arbre converge. Notez que le planificateur nécessite beaucoup moins d'itérations que lorsque la propriété EnableConnectHeuristic est définie sur false.

fprintf("Number of iterations: %d\n",solnInfo.NumIterations)

Number of iterations: 192

Visualisez les résultats.

figure
show(map)
hold on
% Start tree expansion
% Plot start pose and goal pose
plot(start(1), start(2),plannerLineSpec.start{:});
plot(goal(1), goal(2), plannerLineSpec.goal{:});
plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ...
    plannerLineSpec.tree{:})
% Goal tree expansion
plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ...
   plannerLineSpec.goalTree{:})
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})
legend
hold off

Planifier le chemin à travers la carte d'occupation 3D à l'aide du planificateur RRT bidirectionnel

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 RRT bidirectionnel avec une distance de connexion maximale accrue et un nombre maximal d'itérations réduit. Définissez la propriété EnableConnectHeuristic sur true.

planner = plannerBiRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          EnableConnectHeuristic = true);

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")
% Start tree expansion
plot3(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ...
      solnInfo.StartTreeData(:,3),".-",Color="g")
% Goal state
scatter3(goal(1,1),goal(1,2),goal(1,3),"y","filled")
% Goal tree expansion
plot3(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ...
      solnInfo.GoalTreeData(:,3),".-",Color="y")
% Path
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ...
      "m-",LineWidth=2)

Arguments d'entrée

planner — Planificateur de chemin

plannerBiRRT objet

Planificateur de chemin, spécifié comme objet plannerBiRRT .

startState — État de départ du chemin

N-élément vecteur à valeur réelle

État de départ du chemin, spécifié comme un vecteur à valeur réelle d'élément N. N est le nombre de dimensions dans l'espace d'état.

Exemple : [1 1 pi/6]

Exemple : [40 180 25 0.7 0.2 0 0.1]

Types de données : single | double

goalState — État objectif du chemin

N-élément vecteur à valeur réelle

État objectif du chemin, spécifié sous la forme d'un vecteur à valeur réelle d'élément N. N est le nombre de dimensions dans l'espace d'état.

Exemple : [2 2 pi/3]

Exemple : [150 33 35 0.3 0 0.1 0.6]

Types de données : single | double

Arguments de sortie

path — Informations sur le chemin prévu

navPath objet

Informations sur le chemin prévu, renvoyées sous la forme d'un objet navPath .

solnInfo — Informations sur la solution

structure

Informations sur la solution, renvoyées sous forme de structure. La structure contient ces champs :

Champ	Description
IsPathFound	Indique si un chemin est trouvé. Il renvoie 1 (true) si un chemin est trouvé. Sinon, il renvoie 0 (false).
ExitFlag	Indique la cause de résiliation du planificateur, renvoyée sous la forme : 1 — Le planificateur atteint l'objectif. 2 — Le planificateur atteint le nombre maximum d'itérations. 3 — Le planificateur atteint le nombre maximum de nœuds.
StartTreeNumNodes	Nombre de nœuds dans l'arborescence de recherche de démarrage lorsque le planificateur se termine, à l'exclusion du nœud racine.
GoalTreeNumNodes	Nombre de nœuds dans l'arborescence de recherche d'objectifs à la fin du planificateur, à l'exclusion du nœud racine.
NumIterations	Nombre d'itérations combinées par l'arbre de départ et l'arbre d'objectifs.
StartTreeData	Collection d'états explorés qui reflètent l'état de l'arborescence de recherche de démarrage lorsque le planificateur se termine. Notez que les valeurs NaN sont insérées comme délimiteurs pour séparer chaque arête individuelle.
GoalTreeData	Collection d'états explorés qui reflètent l'état de l'arbre de recherche d'objectifs lorsque le planificateur se termine. Notez que les valeurs NaN sont insérées comme délimiteurs pour séparer chaque arête individuelle.

Types de données : structure

Historique des versions

Introduit dans R2021a

Voir aussi

Objets

• plannerBiRRT | navPath

Fonctions

• copy