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

clone

Créer un clone profond de l'objet controllerTEB

Depuis R2023a

Syntaxe

controller2 = clone(controller1)

Description

controller2 = clone(controller1) crée un clone profond de l'objet controllerTEB controller1.

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 d'une largeur et d'une hauteur de 15 mètres et de 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

controller1 — Contrôleur TEB

Objet controllerTEB

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

Arguments de sortie

controller2 — Clone du contrôleur TEB

Objet controllerTEB

Clone du contrôleur TEB, renvoyé sous forme d'objet controllerTEB .

Historique des versions

Introduit dans R2023a

Voir aussi

Objets

• controllerTEB

Fonctions

• step