Main Content

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

plannerBiRRT

Créer un planificateur RRT bidirectionnel pour la planification géométrique

Depuis R2021a

    Description

    L'objet plannerBiRRT est un planificateur de requête unique qui utilise l'algorithme d'arbre aléatoire d'exploration rapide bidirectionnel (RRT) avec une heuristique de connexion facultative pour une vitesse accrue.

    Le planificateur RRT bidirectionnel crée un arbre avec un nœud racine à l'état de départ spécifié et un autre arbre avec un nœud racine à l'état objectif spécifié. Pour étendre chaque arbre, le planificateur génère un état aléatoire et, s'il est valide, fait un pas à partir du nœud le plus proche en fonction de la propriété MaxConnectionDistance . Les arbres de départ et d'objectif alternent ce processus d'extension jusqu'à ce que les deux arbres soient connectés. Si la propriété EnableConnectHeuristic est activée, le processus d'extension ignore la propriété MaxConnectionDistance . Les états invalides ou les connexions qui entrent en collision avec l'environnement ne sont pas ajoutés à l'arborescence.

    Création

    Description

    planner = plannerBiRRT(stateSpace,stateVal) crée un planificateur RRT bidirectionnel à 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. Les arguments stateSpace et stateVal définissent également les propriétés StateSpace et StateValidator , respectivement, du planificateur. .

    exemple

    planner = plannerBiRRT(___,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 les propriétés MaxConnectionDistance, MaxIterations, MaxNumTreeNodes et EnableConnectHeuristic comme arguments nom-valeur. .

    Propriétés

    développer tout

    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 la classe nav.StateSpace .

    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.

    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 plannerBiRRT utilise un échantillonnage d'état uniforme.

    Longueur maximale entre les configurations planifiées, spécifiée sous forme de scalaire positif.

    Si la propriété EnableConnectHeuristic est définie sur true, l'objet ignore cette distance lors de la connexion des deux arbres lors de l'étape de connexion.

    Exemple : MaxConnectionDistance=0.3

    Types de données : single | double

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

    Exemple : MaxIterations=2500

    Types de données : single | double

    Nombre maximum de nœuds dans l'arborescence de recherche, spécifié sous forme d'entier positif.

    Exemple : MaxNumTreeNodes=2500

    Types de données : single | double

    Rejoignez directement les arbres pendant la phase de connexion du planificateur, spécifié comme un 0 (false) ou 1 (true).

    Si vous définissez cette propriété sur true , l'objet ignore la propriété MaxConnectionDistance lorsqu'il tente de connecter les deux arbres ensemble.

    Exemple : EnableConnectHeuristic=true

    Types de données : logical

    Fonctions d'objet

    planPlanifier le chemin entre deux états
    copyCréer une copie complète de l'objet du planificateur

    Exemples

    réduire tout

    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.

    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.

    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.

    Références

    [1] Kuffner, J. J., and S. M. LaValle. “RRT-Connect: An Efficient Approach to Single-Query Path Planning.” In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), 2:995–1001. San Francisco, CA, USA: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.

    Capacités étendues

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

    Historique des versions

    Introduit dans R2021a

    développer tout