plan
Syntaxe
Description
path = plan(planner,startState,goalState)navPath .
Exemples
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
![Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 6 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Start, Goal, Tree, Goal Tree, Path.](planpathbetweentwostatesusingbidirectionalrrtexample_01_fr.png)
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
![Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 6 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Start, Goal, Tree, Goal Tree, Path.](planpathbetweentwostatesusingbidirectionalrrtexample_02_fr.png)
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)
![Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 6 objects of type patch, scatter, line.](planpaththrough3doccupancymapbidirectionalrrtplannerexample_01_fr.png)
Arguments d'entrée
Arguments de sortie
Historique des versions
Introduit dans R2021a
