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

controllerTEB

Evite obstáculos invisibles con trayectorias óptimas en el tiempo

Desde R2023a

Descripción

El objeto controllerTEB crea un controlador (planificador local) utilizando el algoritmo de banda elástica temporizada (TEB). El controlador permite que un robot siga una ruta de referencia generalmente generada por un planificador global, como RRT o Hybrid A*. Además, el planificador evita obstáculos y suaviza la ruta mientras optimiza el duración del recorrido y mantiene una distancia segura de obstáculos conocidos o desconocidos por el planificador global. El objeto también calcula comandos de velocidad y una trayectoria óptima utilizando la pose actual del robot y sus velocidades lineales y angulares actuales.

Creación

Sintaxis

controller = controllerTEB(refpath)

controller = controllerTEB(refpath,map)

controller = controllerTEB(___,PropertyName=Value)

Descripción

controller = controllerTEB(refpath) crea un objeto controlador TEB, controller, que calcula los comandos de velocidad lineal y angular para que un robot tipo automóvil o con tracción diferencial siga la ruta de referencia refpath y viaje durante 5 segundos en un entorno sin obstáculos. La entrada refpath establece el valor de la propiedad ReferencePath.

controller = controllerTEB(refpath,map) intenta evitar obstáculos en el mapa de ocupación especificado map. El controlador asume que el espacio fuera del límite del mapa está libre. La entrada map establece el valor de la propiedad Map.

controller = controllerTEB(___,PropertyName=Value) especifica propiedades utilizando uno o más argumentos de nombre-valor además de cualquier combinación de argumentos de entrada de las sintaxis anteriores.

Propiedades

ReferencePath — Ruta de referencia a seguir

matriz de N por 2 | matriz de N por 3 | objeto navPath con espacio de estados SE(2)

Ruta de referencia a seguir, especificada como una matriz N por 2, una matriz N por 3 o un objeto navPath con un espacio de estados SE(2). Cuando se especifica como matriz, cada fila representa una pose en la ruta. Utilice la propiedad LookAheadTime para seleccionar una parte de ReferencePath para la cual optimizar la trayectoria y generar comandos de velocidad.

Nota

Si especifica la ruta de referencia como una matriz N por 2, el objeto calcula la orientación utilizando la función headingFromXY y la agrega como la tercera columna.

Tipos de datos: single | double

Map — Mapa de ocupación que representa el medio ambiente.

binaryOccupancyMap() (predeterminado) | objeto binaryOccupancyMap | objeto occupancyMap

Mapa de ocupación que representa el entorno, especificado como un objeto binaryOccupancyMap o un objeto occupancyMap que contiene los obstáculos en la proximidad del robot. Al optimizar la trayectoria, el controlador considera libre el espacio fuera del límite del mapa. Los mapas más grandes pueden provocar un rendimiento más lento.

CostWeights — Pesos para la optimización de la función de costes.

struct("Time",10,"Smoothness",1000,"Obstacle",50) (predeterminado) | estructura

Ponderaciones para la optimización de la función de costes, especificadas como una estructura. Los campos de la estructura son:

Campo	Descripción
Time	Ponderación de la función de coste por tiempo, especificada como un escalar positivo. Para reducir el duración del recorrido, aumente este valor de peso.
Smoothness	Peso de la función de coste para un movimiento suave, especificado como un escalar positivo. Para obtener una ruta más suave, aumente este valor de peso.
Obstacles	Peso de la función de coste para mantener una distancia segura de los obstáculos, especificado como un escalar positivo. Para priorizar el mantenimiento de una distancia segura de los obstáculos, aumente este valor de peso.

Tipos de datos: struct

RobotInformation — Información de geometría del robot para comprobar colisiones.

struct("Dimension",[1 0.67],"Shape","Rectangle","FixedTransform",[1 0.5 0.5]) (predeterminado) | estructura

Información de geometría del robot para verificación de colisiones, especificada como estructura. Los campos de la estructura son:

Campo	Descripción
Dimension	Tamaño del robot, especificado como un vector positivo de dos elementos con la forma [length width], en metros.
Shape	Forma del robot, especificada como "Rectangle" o "Point".
FixedTransform	Origen del robot, especificado como un vector fila de tres elementos con la forma [x, y, theta] o un objeto se2 que define una matriz de transformación SE(2). Cuando se especifica el origen como [x, y, theta], el objeto controllerTEB calcula la matriz de transformación SE(2) asociada y la almacena en el campo FixedTransform. El origen predeterminado está en el centro de la parte trasera del robot. Puede utilizar este campo para especificar un nuevo origen para el robot en relación con el origen predeterminado. Este campo sólo es válido si el robot tiene forma rectangular.

• Si establece Shape como "Point", el campo Dimension se establece en [0 0].

• Si establece Shape como "Rectangle", la función considera el centro del borde trasero del robot como el origen del robot.

Tipos de datos: struct

MinTurningRadius — Radio de giro mínimo para el vehículo en trayectoria optimizada

0 (predeterminado) | escalar no negativo

Desde R2023b

Radio de giro mínimo para el vehículo en la ruta optimizada, especificado como un escalar no negativo. Este valor corresponde al radio del círculo de giro en el ángulo máximo de dirección del vehículo. Las unidades se establecen en metros.

Disminuya este valor para permitir giros bruscos y rotaciones en el lugar. Aumente este valor para limitar los giros bruscos. Cuando aumenta el valor, el vehículo realizará más movimientos hacia adelante y hacia atrás para girar en un espacio restringido.

Tipos de datos: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

ObstacleSafetyMargin — Distancia segura entre el robot y los obstáculos.

0.5 (predeterminado) | escalar positivo

Distancia de seguridad entre el robot y los obstáculos, especificada como escalar positivo, en metros. Tenga en cuenta que ésta es una restricción suave que el planificador puede ignorar.

Tipos de datos: single | double

NumIteration — Número de iteraciones para optimizar la trayectoria.

2 (predeterminado) | número entero positivo

Número de iteraciones para optimizar la trayectoria, especificado como un entero positivo. Este valor es el número de veces que se produce la interpolación y el controlador llama al solver para optimizar la trayectoria.

Tipos de datos: single | double

MaxVelocity — Límites máximos de velocidad lineal y angular.

[0.8 1.6] (predeterminado) | vector positivo de dos elementos

Límites máximos de velocidad lineal y angular para comandos de velocidad, especificados como un vector positivo de dos elementos. El primer elemento es el límite de velocidad lineal, en metros por segundo, y el segundo elemento es el límite de velocidad angular, en radianes por segundo.

Tipos de datos: single | double

MaxReverseVelocity — Velocidad máxima para movimiento inverso

NaN (predeterminado) | escalar positivo

Desde R2023b

Velocidad máxima del vehículo mientras se mueve en dirección inversa, especificada como un escalar positivo. El valor predeterminado es NaN. Cuando la propiedad se establece en NaN, el valor de la velocidad inversa máxima es el mismo que el de la velocidad lineal máxima.

Tipos de datos: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

MaxAcceleration — Límites máximos de aceleración lineal y angular.

[2.4 4.8] (predeterminado) | vector positivo de dos elementos

Límites máximos de aceleración lineal y angular para comandos de velocidad, especificados como un vector positivo de dos elementos. El primer elemento es el límite de aceleración lineal, en metros por segundo al cuadrado, y el segundo elemento es el límite de aceleración angular, en radianes por segundo al cuadrado.

Tipos de datos: single | double

ReferenceDeltaTime — Duración del recorrido de referencia entre poses consecutivas.

0.3 (predeterminado) | escalar positivo

Duración del recorrido de referencia entre poses consecutivas, especificado como un escalar positivo en segundos. Esta propiedad afecta la adición y eliminación de poses para la trayectoria optimizada. Aumente el valor de esta propiedad para tener menos poses y redúzcalo para tener más poses en la ruta de salida.

Tipos de datos: single | double

LookAheadTime — tiempo de anticipación

5 (predeterminado) | escalar positivo

Tiempo de anticipación, especificado como un escalar positivo en segundos. El controlador genera comandos de velocidad y optimiza la trayectoria hasta que el controlador alcanza el tiempo de anticipación. Un tiempo de anticipación más alto genera comandos de velocidad más en el futuro. Esto permite que el robot reaccione antes ante obstáculos invisibles, pero aumenta el tiempo de ejecución del controlador. Por el contrario, un tiempo de anticipación más corto reduce el tiempo disponible para reaccionar ante obstáculos nuevos y desconocidos, pero permite que el controlador funcione a un ritmo más rápido.

Nota

Esta propiedad afecta la cantidad de comandos de velocidad, marcas de tiempo y poses en la ruta.

Tipos de datos: single | double

GoalTolerance — Tolerancia en torno a la pose de objetivo.

[0.1 0.1 0.1] (predeterminado) | vector de tres elementos

Desde R2023b

Tolerancia alrededor de la pose objetivo, especificada como un vector de tres elementos con la forma [x y θ]. x y y denotan la posición del robot en las direcciones x y y, respectivamente. Las unidades están en metros. θ es el ángulo de rumbo del robot en radianes. Este valor de tolerancia objetivo especifica el límite para determinar si el robot ha alcanzado la pose objetivo.

Tipos de datos: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Funciones del objeto

step	Calcular comandos de velocidad y trayectoria óptima para pasos de tiempo posteriores.
clone	Crear un clon profundo del objeto controllerTEB

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

Capacidades ampliadas

Generación de código C/C++

Genere código C y C++ mediante MATLAB® Coder™.

Indicaciones y limitaciones de uso:

Para la generación de código, el tamaño del mapa de entrada debe ser una constante en tiempo de compilación.

Historial de versiones

Introducido en R2023a

R2024b: Especificar el origen del robot

Ahora puede especificar el origen del robot en relación con el origen predeterminado utilizando el campo FixedTransform de la propiedad RobotInformation. Puede especificar el nuevo origen como un vector fila de tres elementos o una matriz de transformación SE(2). En cualquier caso, el campo FixedTransform almacena el origen especificado como una matriz de transformación SE(2).

Por ejemplo, especifique el origen [1.0, 0.5, pi/4] como un vector fila de tres elementos.

origin = [1.0, 0.5, pi/4];
info = struct(Dimension=[1 1],Shape="Rectangle",FixedTransform=origin);
teb = controllerTEB(refpath,RobotInformation=info);

Alternativamente, especifique el origen [1.0, 0.5, pi/4] utilizando una matriz de transformación SE(2).

tform = se2(pi/4,"theta",[1.0 0.5]);
info = struct(Dimension=[1 1],Shape="Rectangle",FixedTransform=tform);
teb = controllerTEB(refpath,RobotInformation=info);

R2023b: Especifique el radio de giro mínimo, la velocidad inversa máxima y la tolerancia objetivo

Ahora puedes especificar

• Radio de giro mínimo de un vehículo en la ruta optimizada mediante la propiedad MinTurningRadius.

• Velocidad máxima para el movimiento inverso utilizando la propiedad MaxReverseVelocity.

• El límite para determinar si el robot ha alcanzado la pose objetivo mediante la propiedad GoalTolerance.

Consulte también

Bloques

• Timed Elastic Band

Objetos

• controllerPurePursuit | plannerAStarGrid

Funciones

• step | clone | headingFromXY | velocityCommand