Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

clone

Crear un clon profundo del objeto controllerTEB

Desde R2023a

Sintaxis

controller2 = clone(controller1)

Descripción

controller2 = clone(controller1) crea un clon profundo del objeto controllerTEB controller1.

Ejemplos

Calcule los comandos de velocidad y la trayectoria óptima para un robot de accionamiento diferencial mediante un algoritmo de banda elástica cronometrado

Configurar el entorno del estacionamiento

Cree un objeto occupancyMap a partir de un mapa de estacionamiento y configure la resolución del mapa en 3 celdas por metro.

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

Visualiza el mapa. El mapa contiene el plano de un aparcamiento con algunas plazas ya ocupadas.

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

Configurar y ejecutar el Planificador global

Cree un validador de estado validatorOccupancyMap utilizando la definición stateSpaceSE2. Especifique el mapa y la distancia para interpolar y validar segmentos de ruta.

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

Cree un planificador de rutas RRT*. Aumente la distancia máxima de conexión.

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

Establezca los estados de inicio y objetivo.

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

Planifique una ruta con la configuración predeterminada.

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

RRT* utiliza una orientación aleatoria, lo que puede provocar giros innecesarios.

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

Alinee la orientación con la ruta, excepto en los estados inicial y objetivo.

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

Visualiza la ruta.

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

Configurar y ejecutar el planificador local

Crea un objeto local occupancyMap con un ancho y alto de 15 metros y la misma resolución que el mapa global.

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

Cree un objeto controllerTEB utilizando la ruta de referencia generada por el planificador global y el mapa local.

teb = controllerTEB(refpath,localmap);

Especifique las propiedades del objeto 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;

Crea un clon profundo del objeto controllerTEB.

teb2 = clone(teb);

Inicializar parámetros.

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

Calcular comandos de velocidad y trayectoria óptima.

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

Argumentos de entrada

controller1 — controlador TEB

objeto controllerTEB

Controlador TEB, especificado como un objeto controllerTEB.

Argumentos de salida

controller2 — Clon del controlador TEB

objeto controllerTEB

Clon del controlador TEB, devuelto como un objeto controllerTEB.

Historial de versiones

Introducido en R2023a

Consulte también

Objetos

• controllerTEB

Funciones

• step