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
Description
crée un objet contrôleur TEB, controller
= controllerTEB(refpath
)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 .
tente d'éviter les obstacles dans la carte d'occupation spécifiée controller
= controllerTEB(refpath
,map
)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 .
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.controller
= controllerTEB(___,Name=Value
)
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 Remarque Lorsque vous définissez |
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
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 R2023aR2023b: 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
Fonctions
Commande MATLAB
Vous avez cliqué sur un lien qui correspond à cette commande MATLAB :
Pour exécuter la commande, saisissez-la dans la fenêtre de commande de MATLAB. Les navigateurs web ne supportent pas les commandes MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)