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

controllerTEB

Évitez les obstacles invisibles grâce à des trajectoires optimales dans le temps

Depuis R2023a

Description

L'objet controllerTEB crée un contrôleur (planificateur local) à l'aide de l'algorithme Timed Elastic Band (TEB). Le contrôleur permet à un robot de suivre un chemin de référence généralement généré par un planificateur global, tel que RRT ou Hybrid A*. De plus, le planificateur évite les obstacles et adoucit le chemin tout en optimisant le temps de trajet, et maintient une distance de sécurité par rapport aux obstacles connus ou inconnus du planificateur global. L'objet calcule également des commandes de vitesse et une trajectoire optimale en utilisant la pose actuelle du robot et ses vitesses linéaires et angulaires actuelles.

Création

Syntaxe

controller = controllerTEB(refpath)

controller = controllerTEB(refpath,map)

controller = controllerTEB(___,Name=Value)

Description

controller = controllerTEB(refpath) crée un objet contrôleur TEB, controller, qui calcule les commandes de vitesse linéaire et angulaire pour qu'un robot à entraînement différentiel suive le chemin de référence refpath et voyagez pendant 5 secondes dans un environnement sans obstacle. L'entrée refpath définit la valeur de la propriété ReferencePath .

controller = controllerTEB(refpath,map) tente d'éviter les obstacles dans la carte d'occupation spécifiée map. Le contrôleur suppose que l’espace en dehors des limites de la carte est libre. L'entrée map définit la valeur de la propriété Map .

controller = controllerTEB(___,Name=Value) spécifie les propriétés en utilisant un ou plusieurs arguments nom-valeur en plus de toute combinaison d'arguments d'entrée des syntaxes précédentes.

Propriétés

ReferencePath — Chemin de référence à suivre

N-matrice par 2 | N-matrice par 3 | Objet navPath avec espace d'état SE(2)

Chemin de référence à suivre, spécifié comme une matrice N-par-2, N-par-3 matrice, ou un objet navPath avec un espace d’état SE(2). Lorsqu'elle est spécifiée sous forme de matrice, chaque ligne représente une pose sur le chemin. Utilisez la propriété LookAheadTime pour sélectionner une partie du ReferencePath pour laquelle optimiser la trajectoire et générer des commandes de vitesse.

Remarque

Si vous spécifiez le chemin de référence en tant que matrice N-by-2, alors l'objet calcule l'orientation à l'aide de la fonction headingFromXY et l'ajoute comme troisième colonne.

Types de données : single | double

Map — Carte d'occupation représentant l'environnement

binaryOccupancyMap() (par défaut) | Objet binaryOccupancyMap | occupancyMap objet

Carte d'occupation représentant l'environnement, spécifiée comme un objet binaryOccupancyMap ou un objet occupancyMap contenant les obstacles à proximité du robot. Lors de l'optimisation de la trajectoire, le contrôleur considère comme libre l'espace situé en dehors des limites de la carte. Des cartes plus grandes peuvent entraîner un ralentissement des performances.

CostWeights — Pondérations pour l'optimisation de la fonction de coût

struct("Time",10,"Smoothness",1000,"Obstacle",50) (par défaut) | structure

Pondérations pour l'optimisation de la fonction de coût, spécifiées sous forme de structure. Les champs de la structure sont :

Champ	Description
Time	Poids de la fonction de coût en fonction du temps, spécifié sous la forme d'un scalaire positif. Pour réduire le temps de trajet, augmentez cette valeur de poids.
Smoothness	Poids de la fonction de coût pour un mouvement fluide, spécifié sous forme de scalaire positif. Pour obtenir un parcours plus fluide, augmentez cette valeur de poids.
Obstacles	Poids de la fonction de coût pour maintenir une distance de sécurité par rapport aux obstacles, spécifié sous la forme d'un scalaire positif. Pour privilégier le maintien d'une distance de sécurité par rapport aux obstacles, augmentez cette valeur de poids.

Types de données : struct

RobotInformation — Informations sur la géométrie du robot pour la vérification des collisions

struct("Dimension",[1 0.67],"Shape","Rectangle") (par défaut) | structure

Informations géométriques du robot pour la vérification des collisions, spécifiées sous forme de structure. Les champs de la structure sont :

Champ	Description
Dimension	Taille du robot, spécifiée sous la forme d'un vecteur positif à deux éléments de la forme [length width], en mètres.
Shape	Forme du robot, spécifiée comme "Rectangle" ou "Point". Remarque Lorsque vous définissez Shape sur "Point", le champ Dimension est défini sur [0 0].

Types de données : struct

MinTurningRadius — Rayon de braquage minimum pour véhicule sur trajectoire optimisée

0 (par défaut) | scalaire non négatif

Depuis R2023b

Rayon de braquage minimum pour le véhicule sur la trajectoire optimisée, spécifié sous forme de scalaire non négatif. Cette valeur correspond au rayon du rayon de braquage à l'angle de braquage maximum du véhicule. Les unités sont en mètres.

Diminuez cette valeur pour permettre des virages serrés et des rotations sur place. Augmentez cette valeur pour limiter les virages serrés. Lorsque vous augmentez la valeur, le véhicule effectuera davantage de mouvements avant et arrière pour tourner dans un espace restreint.

Types de données : single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

ObstacleSafetyMargin — Distance de sécurité entre le robot et les obstacles

0.5 (par défaut) | scalaire positif

Distance de sécurité entre le robot et les obstacles, spécifiée sous forme d'un scalaire positif, en mètres. Notez qu'il s'agit d'une contrainte souple que le planificateur peut ignorer.

Types de données : single | double

NumIteration — Nombre d'itérations pour optimiser la trajectoire

2 (par défaut) | entier positif

Nombre d'itérations pour optimiser la trajectoire, spécifié sous forme d'entier positif. Cette valeur correspond au nombre de fois où l'interpolation se produit et où le contrôleur appelle le solveur pour l'optimisation de la trajectoire.

Types de données : single | double

MaxVelocity — Limites maximales de vitesse linéaire et angulaire

[0.8 1.6] (par défaut) | vecteur positif à deux éléments

Limites maximales de vitesse linéaire et angulaire pour les commandes de vitesse, spécifiées sous la forme d'un vecteur positif à deux éléments. Le premier élément est la limite de vitesse linéaire, en mètres par seconde, et le deuxième élément est la limite de vitesse angulaire, en radians par seconde.

Types de données : single | double

MaxReverseVelocity — Vitesse maximale pour le mouvement inverse

NaN (par défaut) | scalaire positif

Depuis R2023b

Vitesse maximale du véhicule lors d'un déplacement en sens inverse, spécifiée sous la forme d'un scalaire positif. La valeur par défaut est NaN. Lorsque la propriété est définie sur NaN, la valeur de la vitesse inverse maximale est la même que celle de la vitesse linéaire maximale.

Types de données : single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

MaxAcceleration — Limites maximales d'accélération linéaire et angulaire

[2.4 4.8] (par défaut) | vecteur positif à deux éléments

Limites maximales de l'accélération linéaire et angulaire pour les commandes de vitesse, spécifiées sous la forme d'un vecteur positif à deux éléments. Le premier élément est la limite d'accélération linéaire, en mètres par seconde carrée, et le deuxième élément est la limite d'accélération angulaire, en radians par seconde carrée.

Types de données : single | double

ReferenceDeltaTime — Temps de trajet de référence entre des poses consécutives

0.3 (par défaut) | scalaire positif

Temps de trajet de référence entre des poses consécutives, spécifié sous forme d'un scalaire positif en secondes. Cette propriété affecte l'ajout et la suppression de poses pour la trajectoire optimisée. Augmentez la valeur de cette propriété pour avoir moins de poses et réduisez-la pour avoir plus de poses dans le chemin de sortie.

Types de données : single | double

LookAheadTime — Temps d'anticipation

5 (par défaut) | scalaire positif

Temps d'anticipation, spécifié sous forme d'un scalaire positif en secondes. Le contrôleur génère des commandes de vitesse et optimise la trajectoire jusqu'à ce qu'il atteigne le temps d'anticipation. Un temps d’anticipation plus long génère des commandes de vitesse plus loin dans le futur. Cela permet au robot de réagir plus tôt aux obstacles invisibles, mais augmente le temps d'exécution du contrôleur. À l’inverse, un temps d’anticipation plus court réduit le temps disponible pour réagir à de nouveaux obstacles inconnus, mais permet au contrôleur de fonctionner plus rapidement.

Remarque

Cette propriété a un impact sur le nombre de commandes de vitesse, d'horodatages et de poses dans le chemin.

Types de données : single | double

GoalTolerance — Tolérance autour de la pose du but

[0.1 0.1 0.1] (par défaut) | vecteur à trois éléments

Depuis R2023b

Tolérance autour de la pose de l'objectif, spécifiée comme un vecteur à trois éléments de la forme [x y θ]. x et y désignent la position du robot dans les directions x et y , respectivement. Les unités sont en mètres. θ est l'angle de cap du robot en radians. Cette valeur de tolérance d'objectif spécifie la limite permettant de déterminer si le robot a atteint la pose d'objectif.

Types de données : single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Fonctions d'objet

step	Calculer les commandes de vitesse et la trajectoire optimale pour les pas de temps ultérieurs
clone	Créer un clone profond de l'objet controllerTEB

Exemples

Calculer les commandes de vitesse et la trajectoire optimale pour un robot à entraînement différentiel à l'aide d'un algorithme de bande élastique chronométré

 Mettre en place un environnement de stationnement 

Créez un objet occupancyMap à partir d'une carte de parking et définissez la résolution de la carte sur 3 cellules par mètre.

load parkingMap.mat;
resolution = 3;
map = occupancyMap(map,resolution);

Visualisez la carte. La carte contient le plan d'étage d'un parking avec quelques emplacements de stationnement déjà occupés.

show(map)
title("Parking Lot Map")
hold on

Configurer et exécuter le planificateur global

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

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

Créez un planificateur de trajet RRT*. Augmentez la distance de connexion maximale.

rrtstar = plannerRRTStar(validator.StateSpace,validator);
rrtstar.MaxConnectionDistance = 0.2;

Définissez les états de départ et d’objectif.

start = [2 9 0];
goal = [27 18 -pi/2];

Planifiez un chemin avec les paramètres par défaut.

rng(42,"twister") % Set random number generator seed for repeatable result.
route = plan(rrtstar,start,goal);
refpath = route.States;

RRT* utilise une orientation aléatoire, ce qui peut provoquer des virages inutiles.

headingToNextPose = headingFromXY(refpath(:,1:2));

Alignez l'orientation sur le chemin, sauf pour les états de départ et d'objectif.

refpath(2:end-1,3) = headingToNextPose(2:end-1);

Visualisez le chemin.

plot(refpath(:,1),refpath(:,2),"r-",LineWidth=2)
hold off

Configurer et exécuter le planificateur local

Créez un objet local occupancyMap avec une largeur et une hauteur de 15 mètres et la même résolution que la carte globale.

localmap = occupancyMap(15,15,map.Resolution);

Créez un objet controllerTEB en utilisant le chemin de référence généré par le planificateur global et la carte locale.

teb = controllerTEB(refpath,localmap);

Spécifiez les propriétés de l'objet controllerTEB .

teb.LookAheadTime = 10;         % sec
teb.ObstacleSafetyMargin = 0.4; % meters

% To generate time-optimal trajectories, specify a larger weight value,
% like 100, for the cost function, Time. To follow the reference path
% closely, keep the weight to a smaller value like 1e-3.
teb.CostWeights.Time = 100;

Créez un clone profond de l'objet controllerTEB .

teb2 = clone(teb);

Initialisez les paramètres.

curpose = refpath(1,:);
curvel = [0 0];
simtime = 0;
% Reducing timestep can lead to more accurate path tracking.
timestep = 0.1;
itr = 0;
goalReached = false;

Calculez les commandes de vitesse et la trajectoire optimale.

while ~goalReached && simtime < 200
    % Update map to keep robot in the center of the map. Also update the
    % map with new information from the global map or sensor measurements.
    moveMapBy = curpose(1:2) - localmap.XLocalLimits(end)/2;
    localmap.move(moveMapBy,FillValue=0.5)
    syncWith(localmap,map)

    if mod(itr,10) == 0 % every 1 sec
        % Generate new vel commands with teb
        [velcmds,tstamps,curpath,info] = step(teb,curpose,curvel);
        goalReached = info.HasReachedGoal;
        feasibleDriveDuration = tstamps(info.LastFeasibleIdx);
        % If robot is far from goal and only less than third of trajectory
        % is feasible, then an option is to re-plan the path to follow to
        % reach the goal.
        if info.ExitFlag == 1 && ...
                feasibleDriveDuration < (teb.LookAheadTime/3)
            route = plan(rrtstar,curpose,[27 18 -pi/2]);
            refpath = route.States;
            headingToNextPose = headingFromXY(refpath(:,1:2));
            refpath(2:end-1,3) = headingToNextPose(2:end-1);
            teb.ReferencePath = refpath;
        end
        timestamps = tstamps + simtime;

        % Show the updated information input to or output
        % from controllerTEB
        clf
        show(localmap)
        hold on
        plot(refpath(:,1),refpath(:,2),".-",Color="#EDB120", ...
             DisplayName="Reference Path")
        quiver(curpath(:,1),curpath(:,2), ...
               cos(curpath(:,3)),sin(curpath(:,3)), ...
               0.2,Color="#A2142F",DisplayName="Current Path")
        quiver(curpose(:,1),curpose(:,2), ...
               cos(curpose(:,3)),sin(curpose(:,3)), ...
               0.5,"o",MarkerSize=20,ShowArrowHead="off", ...
               Color="#0072BD",DisplayName="Start Pose")
    end

    simtime = simtime+timestep;
    % Compute the instantaneous velocity to be sent to the robot from the
    % series of timestamped commands generated by controllerTEB
    velcmd = velocityCommand(velcmds,timestamps,simtime);
    % Very basic robot model, should be replaced by simulator.
    statedot = [velcmd(1)*cos(curpose(3)) ...
                velcmd(1)*sin(curpose(3)) ...
                velcmd(2)];
    curpose = curpose + statedot*timestep;

    if exist("hndl","var")
        delete(hndl)
    end
    hndl = quiver(curpose(:,1),curpose(:,2), ...
                  cos(curpose(:,3)),sin(curpose(:,3)), ...
                  0.5,"o",MarkerSize=20,ShowArrowHead="off", ...
                  Color="#D95319",DisplayName="Current Robot Pose");
    itr = itr + 1;
    drawnow
end
legend

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 R2023a

R2023b: Spécifiez le rayon de braquage minimum, la vitesse inverse maximale et la tolérance d'objectif

Vous pouvez maintenant préciser

• Rayon de braquage minimum pour un véhicule sur la trajectoire optimisée en utilisant la propriété MinTurningRadius .

• Vitesse maximale pour le mouvement inverse en utilisant la propriété MaxReverseVelocity .

• La limite pour déterminer si le robot a atteint l'objectif est posée en utilisant la propriété GoalTolerance .

Voir aussi

Objets

• controllerPurePursuit | plannerAStarGrid

Fonctions

• step | clone | headingFromXY | velocityCommand