Main Content

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

isStateValid

Vérifiez si l'état est valide

Description

isValid = isStateValid(validator,states) vérifie si un ensemble d'états donnés est valide.

exemple

Exemples

réduire tout

Cet exemple montre comment valider les chemins à travers un environnement.

Chargez des exemples de cartes. Utilisez la carte simple pour créer une carte d'occupation binaire.

load exampleMaps.mat
map = occupancyMap(simpleMap);
show(map)

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains an object of type image.

Spécifiez un chemin grossier à travers la carte.

path = [2 2 pi/2; 10 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 2 objects of type image, line.

Créez un validateur d'état en utilisant la définition stateSpaceSE2 . Spécifiez la carte et la distance pour interpoler et valider les segments de chemin.

validator = validatorOccupancyMap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Vérifiez que les points du chemin sont des états valides. Les trois points se trouvent dans un espace libre et sont donc considérés comme valides.

isValid = isStateValid(validator,path)
isValid = 3x1 logical array

   1
   1
   1

Vérifiez le mouvement entre chaque état de chemin séquentiel. La fonction isMotionValid interpole le long du chemin entre les états. Si un segment de chemin n'est pas valide, tracez le dernier point valide le long du chemin.

startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
    for i = 1:2
        [isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
        if ~isPathValid
            plot(lastValid(1),lastValid(2),'or')
        end
    end
hold off

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 3 objects of type image, line. One or more of the lines displays its values using only markers

Cet exemple montre comment valider les chemins à travers un environnement.

Chargez des exemples de cartes. Utilisez la carte simple pour créer une carte des coûts des véhicules. Précisez un rayon de gonflage de 1 mètre.

load exampleMaps.mat
map = vehicleCostmap(double(simpleMap));
map.CollisionChecker = inflationCollisionChecker("InflationRadius",1);
plot(map)

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

Spécifiez un chemin grossier à travers la carte.

path = [3 3 pi/2; 8 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 3 objects of type image, patch, line. This object represents Inflated Areas.

Créez un validateur d'état en utilisant la définition stateSpaceSE2 . Spécifiez la carte et la distance pour interpoler et valider les segments de chemin.

validator = validatorVehicleCostmap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Vérifiez que les points du chemin sont des états valides. Les trois points se trouvent dans un espace libre et sont donc considérés comme valides.

isValid = isStateValid(validator,path)
isValid = 3x1 logical array

   1
   1
   1

Vérifiez le mouvement entre chaque état de chemin séquentiel. La fonction isMotionValid interpole le long du chemin entre les états. Si un segment de chemin n'est pas valide, tracez le dernier point valide le long du chemin.

startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
    for i = 1:2
        [isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
        if ~isPathValid
            plot(lastValid(1),lastValid(2),'or')
        end
    end
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 4 objects of type image, patch, line. One or more of the lines displays its values using only markers This object represents Inflated Areas.

Créez une carte d'occupation 3D et un validateur d'état associé. Planifiez, validez et visualisez un chemin à travers la carte d'occupation.

Charger et attribuer une carte au validateur d'état

Chargez une carte d'occupation 3D d'un pâté de maisons dans l'espace de travail. Spécifiez un seuil pour lequel les cellules doivent être considérées 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([-20 220;
    -20 220;
    -10 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éé.

sv = validatorOccupancyMap3D(ss);

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

sv.Map = omap;
sv.ValidationDistance = 0.1;

Planifier et visualiser le chemin

Créez un planificateur de chemin avec une distance de connexion maximale augmentée. Réduisez le nombre maximum d’itérations.

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.MaxIterations = 1000;

Créez une fonction d'évaluation définie par l'utilisateur pour déterminer si le chemin atteint l'objectif. Spécifiez la probabilité de choisir l’état objectif lors de l’échantillonnage.

planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5);
planner.GoalBias = 0.1;

Définissez les états 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];

Planifiez un chemin en utilisant le début, l'objectif et le planificateur spécifiés.

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

Vérifiez que les points du chemin sont des états valides.

isValid = isStateValid(sv,pthObj.States)
isValid = 7x1 logical array

   1
   1
   1
   1
   1
   1
   1

Vérifiez que le mouvement entre chaque état de chemin séquentiel est valide.

isPathValid = zeros(size(pthObj.States,1)-1,1,'logical');
for i = 1:size(pthObj.States,1)-1
    [isPathValid(i),~] = isMotionValid(sv,pthObj.States(i,:),...
        pthObj.States(i+1,:));
end
isPathValid
isPathValid = 6x1 logical array

   1
   1
   1
   1
   1
   1

Visualisez les résultats.

show(omap)
hold on
scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state
scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled')    % draw goal state
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),...
    'r-','LineWidth',2) % draw path

Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 4 objects of type patch, scatter, line.

Arguments d'entrée

réduire tout

Objet validateur d'état, spécifié comme objet de sous-classe de nav.StateValidator. Voici les objets de validation d'état prédéfinis :

Positions d'état, spécifiées sous la forme d'un vecteur de ligne d'éléments n ou d'une matrice m-par- n . n est la dimension de l'espace d'état spécifié dans validator. m est le nombre d’états à valider.

Types de données : single | double

Arguments de sortie

réduire tout

États valides, renvoyés sous la forme d'un vecteur de colonne logique d'élément m.

Types de données : logical

Historique des versions

Introduit dans R2019b