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

step

Calculer les commandes de vitesse et la trajectoire optimale pour les pas de temps ultérieurs

Depuis R2023a

Syntaxe

[velcmds,timestamps,optPath] = step(controller,curState,curVel)

[___,extraInfo] = step(___)

Description

[velcmds,timestamps,optPath] = step(controller,curState,curVel) calcule les commandes de vitesse linéaire et angulaire velcmds, avec leur timestamps correspondant et le chemin optimisé correspondant optPath, pour la pose actuelle spécifiée curState et la vitesse actuelle curVel d'un robot.

[___,extraInfo] = step(___) renvoie des informations supplémentaires, extraInfo, pour évaluer la solution, en plus de tous les arguments de la syntaxe précédente.

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

Arguments d'entrée

controller — Contrôleur TEB

Objet controllerTEB

Contrôleur TEB, spécifié comme objet controllerTEB .

curState — Pose actuelle du robot

vecteur de forme à trois éléments [x y theta]

Pose actuelle du robot, spécifiée comme un vecteur à trois éléments de la forme [x y theta]. x et y précisent la position du robot en mètres. theta spécifie l'orientation du robot en radians.

Types de données : single | double

curVel — Vitesse actuelle du robot

vecteur de forme à deux éléments [v w]

Vitesse actuelle du robot, spécifiée comme un vecteur à deux éléments de la forme [v w]. v spécifie la vitesse linéaire du robot en mètres par seconde. w spécifie la vitesse angulaire du robot en radians par seconde.

Types de données : single | double

Arguments de sortie

velcmds — Commandes de vitesse

N-matrice par 2

Commandes de vitesse, renvoyées sous forme de matrice N-by-2. La première colonne est la vitesse linéaire en mètres par seconde et la deuxième colonne est la vitesse angulaire en radians par seconde.

Types de données : double

timestamps — Horodatages correspondant aux commandes de vitesse

N-élément colonne vecteur

Horodatages correspondant aux commandes de vitesse, renvoyés sous forme de vecteur de colonne d'éléments N.

Types de données : double

optPath — Chemin optimisé

N-matrice par 3

Chemin optimisé, renvoyé sous la forme d'une matrice N-by-3. Chaque ligne est de la forme [x y theta], qui définit la position xy et l'angle d'orientation theta en un point du chemin.

N est affecté par les propriétés ReferenceDeltaTime et LookAheadTime de controller. L'algorithme essaie de garder la différence entre deux timestamps consécutifs proche de ReferenceDeltaTime. Si l'écart entre une paire d'horodatages consécutifs est supérieur à ReferenceDeltaTime, la fonction ajoute des poses et des horodatages au chemin. Si l'écart est inférieur à ReferenceDeltaTime, la fonction supprime les poses et les horodatages du chemin. De plus, l'algorithme essaie de garder la valeur finale de timestamps proche de LookAheadTime, donc augmenter LookAheadTime augmente N également.

Types de données : double

extraInfo — Informations supplémentaires

structure

Informations supplémentaires, renvoyées sous forme de structure. Les champs de la structure sont :

Champ	Description
LastFeasibleIdx	Index spécifiant un élément dans les sorties de chemin optimisé et d'horodatage jusqu'à lequel la trajectoire est réalisable. Au-delà de cet indice, la valeur de ExitFlag sera supérieure à zéro. Cela implique qu'un ou plusieurs scénarios correspondant à ExitFlag > 0 se sont produits.
DistanceFromStartPose	Distance de chaque pose dans optPath de la première pose dans optPath. La valeur de curState est toujours la première pose dans optPath.
HasReachedGoal	Indique si le robot a réussi à atteindre la dernière pose dans le ReferencePath dans une tolérance, et renvoie comme true en cas de succès. Sinon, cette valeur renvoie false.
TrajectoryCost	Coût de la trajectoire optimisée pour les fonctions de coût dans l'algorithme Timed Elastic Band.
ExitFlag	Valeur scalaire indiquant la condition de sortie de la fonction step . 0 — Indique que la fonction step a renvoyé des commandes de vitesse et une trajectoire réalisables. 1 — Indique que la sortie de trajectoire renvoyée par la fonction step est sans collision uniquement jusqu'à la valeur d'index spécifiée à l'aide de LastFeasibleIdx. 2 — Indique que les poses correspondant aux indices après le LastFeasibleIdx violent la contrainte de marge de sécurité obstacle du contrôleur TEB de plus de 10 %. 3 — Indique que le rayon de braquage du véhicule lors du passage d'une pose à LastFeasibleIdx à une pose ultérieure viole la contrainte de rayon de braquage minimum de plus de 10 %. 4 — Indique qu'un ou plusieurs horodatages renvoyés par la fonction step sont plus anciens que l'horodatage précédent. 5 — Indique que le robot n'atteindra pas la trajectoire de référence dans un délai d'anticipation spécifié même s'il se déplace à une vitesse linéaire maximale à partir de sa position actuelle.

Types de données : struct

Historique des versions

Introduit dans R2023a

R2023b: L'argument de sortie extraInfo renvoie la condition de sortie

L'argument de sortie extraInfo renvoie désormais un champ d'ajout ExitFlag qui spécifie plus d'informations sur les commandes de vitesse, les horodatages et le chemin renvoyés par le step. Fonction $ .

Voir aussi

Objets

• controllerTEB

Fonctions

• clone