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

step

Calcular comandos de velocidad y trayectoria óptima para pasos de tiempo posteriores.

Desde R2023a

Sintaxis

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

[___,extraInfo] = step(___)

Descripción

[velcmds,timestamps,optPath] = step(controller,curState,curVel) calcula los comandos de velocidad lineal y angular velcmds, con su correspondiente timestamps y su correspondiente ruta optimizada optPath, para la pose actual especificada curState y la velocidad actual curVel de un robot.

[___,extraInfo] = step(___) devuelve información adicional, extraInfo, para evaluar la solución, además de todos los argumentos de la sintaxis anterior.

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 establezca 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

Cree un objeto occupancyMap local 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;

Cree 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

controller — controlador TEB

controllerTEB objeto

Controlador TEB, especificado como un objeto controllerTEB .

curState — Pose actual del robot.

vector de tres elementos de la forma [x y theta]

Pose actual del robot, especificada como un vector de tres elementos de la forma [x y theta]. x y y especifican la posición del robot en metros. theta especifica la orientación del robot en radianes.

Tipos de datos: single | double

curVel — Velocidad actual del robot.

vector de dos elementos de la forma [v w]

Velocidad actual del robot, especificada como un vector de dos elementos de la forma [v w]. v especifica la velocidad lineal del robot en metros por segundo. w especifica la velocidad angular del robot en radianes por segundo.

Tipos de datos: single | double

Argumentos de salida

velcmds — Comandos de velocidad

N-por-2 matriz

Comandos de velocidad, devueltos como una matriz N por 2. La primera columna es la velocidad lineal en metros por segundo y la segunda columna es la velocidad angular en radianes por segundo.

Tipos de datos: double

timestamps — Marcas de tiempo correspondientes a los comandos de velocidad.

N-elemento vector columna

Marcas de tiempo correspondientes a los comandos de velocidad, devueltas como un vector columna de elemento N .

Tipos de datos: double

optPath — Ruta optimizada

N-por-3 matriz

Ruta optimizada, devuelta como una matriz N-por-3. Cada fila tiene la forma [x y theta], que define la posición xy y ángulo de orientación theta en un punto de la ruta.

N se ve afectado por las propiedades ReferenceDeltaTime y LookAheadTime de controller. El algoritmo intenta mantener la diferencia entre dos timestamps consecutivos cerca de ReferenceDeltaTime. Si la brecha entre un par de marcas de tiempo consecutivas es mayor que ReferenceDeltaTime, la función agrega poses y marcas de tiempo a la ruta. Si la brecha es menor que ReferenceDeltaTime, la función elimina poses y marcas de tiempo de la ruta. Además, el algoritmo intenta mantener el valor final de timestamps cerca de LookAheadTime, por lo que al aumentar LookAheadTime aumenta N también.

Tipos de datos: double

extraInfo — Información extra

estructura

Información adicional, devuelta como una estructura. Los campos de la estructura son:

Campo	Descripción
LastFeasibleIdx	Índice que especifica un elemento en la ruta optimizada y las salidas de marca de tiempo hasta el cual la trayectoria es factible. Más allá de este índice, el valor de ExitFlag será mayor que cero. Esto implica que se han producido uno o más escenarios correspondientes a ExitFlag > 0.
DistanceFromStartPose	Distancia de cada pose en optPath desde la primera pose en optPath. El valor de curState es siempre la primera pose en optPath.
HasReachedGoal	Indica si el robot ha alcanzado con éxito la última pose en ReferencePath dentro de una tolerancia y regresa como true si tiene éxito. De lo contrario, este valor devuelve false.
TrajectoryCost	Coste de la trayectoria optimizada para funciones de coste en el algoritmo Timed Elastic Band.
ExitFlag	Valor escalar que indica la condición de salida de la función step . 0: indica que la función step devolvió comandos de velocidad y trayectoria factibles. 1: indica que la salida de trayectoria devuelta por la función step está libre de colisiones solo hasta el valor del índice especificado usando LastFeasibleIdx. 2 — Indica que las poses correspondientes a los índices posteriores al LastFeasibleIdx infringen la restricción del margen de seguridad de obstáculos del controlador TEB en más del 10%. 3: Indica que el radio de giro del vehículo mientras se mueve desde una pose en LastFeasibleIdx a una pose posterior infringe la restricción del radio de giro mínimo en más del 10%. 4: Indica que una o más marcas de tiempo devueltas por la función step son más antiguas que la marca de tiempo anterior. 5: indica que el robot no alcanzará la ruta de referencia dentro de un tiempo de anticipación específico incluso si viaja a una velocidad lineal máxima desde su posición actual.

Tipos de datos: struct

Historial de versiones

Introducido en R2023a

R2023b: Argumento de salida extraInfo devuelve la condición de salida

El argumento de salida extraInfo ahora devuelve un campo de suma ExitFlag que especifica más información sobre los comandos de velocidad, marcas de tiempo y ruta devueltas por step función $ .

Consulte también

Objetos

• controllerTEB

Funciones

• clone