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

contraer todo en la página

    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 la ruta optimizada correspondiente optPath, para la pose actual especificada curState y la velocidad actual curVel de un robot.

    ejemplo

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

    Ejemplos

    contraer todo

    Abrir script en vivo

    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

    Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains an object of type image.

    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

    Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains 2 objects of type image, line.

    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

    Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, quiver. These objects represent Reference Path, Current Path, Start Pose, Current Robot Pose.

    Argumentos de entrada

    contraer todo

    Controlador TEB, especificado como un objeto controllerTEB.

    Pose actual del robot, especificada como un vector de tres elementos con 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

    Velocidad actual del robot, especificada como un vector de dos elementos con 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

    contraer todo

    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

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

    Tipos de datos: double

    Ruta optimizada, devuelta como una matriz N por 3. Cada fila tiene el formato [x y theta], que define la posición xy y el á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 el espacio es menor que ReferenceDeltaTime, la función elimina las poses y las 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 también aumenta N.

    Tipos de datos: double

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

    CampoDescripció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 ha ocurrido 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 retorna 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 de índice especificado mediante LastFeasibleIdx.

    • 2 — Indica que las poses correspondientes a los índices posteriores a LastFeasibleIdx violan 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 al moverse desde una pose en LastFeasibleIdx a una pose posterior viola la restricción de 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

    expandir todo

    Consulte también

    Objetos

    Funciones