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

stateSpaceSE3

Espace d’état SE(3)

Depuis R2020b

Description

L'objet stateSpaceSE3 stocke les paramètres et les états dans l'espace d'état SE(3), qui est composé de vecteurs d'état représentés par [x, y, z, qw, qx, qy, qz]. x, y et z sont des coordonnées cartésiennes. qw, qx, qy et qz représentent l'orientation dans un quaternion. L'objet utilise le calcul de la distance euclidienne et l'interpolation linéaire pour la composante de traduction de l'état. L'objet utilise le calcul de la distance des quaternions et l'interpolation linéaire sphérique pour la composante de rotation de l'état.

Création

Syntaxe

space = stateSpaceSE3

space = stateSpaceSE3(bounds)

Description

space = stateSpaceSE3 crée un objet d'espace d'état SE(3) avec des limites d'état par défaut pour x, y et z. Les variables d'état qw, qx, qy, et qz correspondant à l'orientation ne sont pas bornées.

space = stateSpaceSE3(bounds) crée un objet d'espace d'état SE(3) avec des limites d'état spécifiées sous la forme d'une matrice 7 x 2. Chaque ligne spécifie la valeur minimale et maximale d'une dimension de l'état dans l'ordre x, y, z, qw, qx, qy et qz. L'entrée bounds définit la propriété StateBounds .

Propriétés

Name — Nom de l'espace d'état

'SE3' (par défaut) | vecteur de caractère

Ce propriété est en lecture seule.

Nom de l'espace d'état, spécifié sous forme de vecteur de caractères.

Types de données : char

NumStateVariables — Nombre de dimensions de l'espace d'état

7 (par défaut) | entier positif

Ce propriété est en lecture seule.

Nombre de dimensions de l'espace d'état, dimensions spécifiées, renvoyées sous forme d'entier positif.

Types de données : double

StateBounds — Limites des variables d'état

[-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf] (par défaut) | Matrice 7x2 de valeurs réelles

Limites des variables d'état, spécifiées sous la forme d'une matrice 7 x 2 de valeurs réelles.

• La première ligne spécifie les limites inférieure et supérieure de l'état x en mètres.

• La deuxième ligne spécifie les limites inférieure et supérieure de l'état y en mètres.

• La troisième ligne spécifie les limites inférieure et supérieure de l'état z en mètres.

• Les quatrième à septième lignes spécifient les limites inférieure et supérieure des variables d'état qw, qx, qy et qz respectivement, correspondant à l'orientation en quaternion.

Remarque

La propriété StateBounds n'affecte que les composantes cartésiennes de l'état. Les variables d'état correspondant à l'orientation ne sont pas bornées.

Exemple : stateSpaceSE3([-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf])

Exemple : space.StateBounds = [-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf]

Types de données : double

WeightXYZ — Poids appliqué au calcul de distance x, y et z

1 (par défaut) | scalaire réel positif

Poids appliqué au x, y et z , spécifié comme un scalaire réel positif. Par défaut, le poids pour la translation est choisi supérieur au poids pour la rotation.

L'objet calcule la distance comme suit :

$$d=\sqrt{w_{xyz}(d_x^2+d_y^2+d_z^2)+w_q{d}_q^2}$$

,

où w xyz est le poids appliqué à x, y et z coordonnées, et w q est le poids appliqué à l'orientation en quaternion. d x , d y et d z sont les distances dans les directions x, y et z , respectivement. d q est la distance du quaternion.

Exemple : space.WeightXYZ = 2

Types de données : double

WeightQuaternion — Poids appliqué au calcul de la distance du quaternion

0.1 (par défaut) | scalaire réel positif

Poids appliqué au calcul de la distance du quaternion, spécifié comme un scalaire réel positif. Par défaut, le poids pour la rotation est choisi inférieur au poids pour la translation.

L'objet calcule la distance comme suit :

$$d=\sqrt{w_{xyz}(d_x^2+d_y^2+d_z^2)+w_q{d}_q^2}$$

,

où w xyz est le poids appliqué à x, y et z coordonnées, et w q est le poids appliqué à l'orientation en quaternion. d x , d y et d z sont les distances dans les directions x, y et z , respectivement. d q est la distance du quaternion.

Exemple : space.WeightQuaternion = 0.5

Types de données : double

Fonctions d'objet

copy	Créer une copie complète de l'objet de l'espace d'état
distance	Distance entre deux états
enforceStateBounds	Reduce state to state bounds
interpolate	Interpoler entre les états
sampleUniform	Exemple d'état utilisant une distribution uniforme

Exemples

Valider le chemin via l'environnement de carte d'occupation 3D

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

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 R2020b

Voir aussi

stateSpaceSE2 | validatorOccupancyMap3D