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

Evite obstáculos utilizando el Planificador Local TEB en Simulink

Desde R2025a

Este ejemplo utiliza:

Este ejemplo muestra cómo utilizar el bloque Timed Elastic Band en Simulink® para la planificación de trayectorias locales para evitar obstáculos. El bloque implementa el algoritmo Timed Elastic Band (TEB) para calcular la trayectoria óptima teniendo en cuenta las restricciones cinemáticas del vehículo. La salida SolnInfo del bloque especifica si es factible una trayectoria óptima es viable. Los campos LastFeasibleIdx y ExitFlag en el bus de salida SolnInfo indican si una trayectoria óptima es viable en una marca de tiempo determinada. LastFeasibleIdx especifica una pose a lo largo de la trayectoria óptima y denota la marca de tiempo hasta la cual la trayectoria es viable. Para cada marca de tiempo después de la especificada por LastFeasibleIdx,, el valor de ExitFlag es mayor que cero e indica un error en el cálculo de la trayectoria óptima. Este ejemplo muestra cómo abordar estos errores y generar una trayectoria óptima para maniobrar el vehículo desde la posición inicial hasta la posición objetivo.

Un valor ExitFlag de 1 indica una colisión con un obstáculo en la trayectoria después de que LastFeasibleIdx se encuentre sobre los obstáculos. El modelo establece LastfeasibleIdx como la pose actual y continúa optimizando la trayectoria local.
El valor ExitFlag de 2 indica que el vehículo no mantiene una distancia segura de los obstáculos en la marca de tiempo correspondiente, infringiendo la restricción del margen de seguridad de obstáculos en más del 10%.
El valor ExitFlag de 3 indica que el radio de giro del vehículo al moverse desde la posición actual a una posición posterior infringe la restricción del radio de giro mínimo en más del 10 %.
El valor ExitFlag de 4 indica que la marca de tiempo del planificador no se ha actualizado correctamente.
Si el valor de LastFeasibleIdx es 1, la trayectoria se vuelve inviable inmediatamente después del primer estado. Esto indica que el planificador no pudo encontrar una trayectoria válida más allá de la posición actual.

La simulación resalta el seguimiento de trayectoria y la evasión de obstáculos, donde el robot se adhiere a la trayectoria planificada mientras navega por el entorno.

Descripción general del modelo

Abra el modelo de Simulink.

open_system("PathFollowingTEB.slx")

El modelo de Simulink para la planificación de trayectorias locales utilizando el algoritmo TEB consta principalmente de cuatro componentes principales: Global Path Planning, Obstacle Detection, Local Path Planning, y Visualization.

El modelo también utiliza un bloque Bicycle Kinematic Model para calcular la posición actual del vehículo utilizando los comandos de velocidad calculados durante la planificación local. El bloque Bicycle Kinematic Model considera las restricciones cinemáticas del vehículo mientras calcula la pose actual.
El bloque Timed Elastic Band devuelve la salida SolnInfo como un bus. Debe inicializar el bus desde las Propiedades del modelo antes de simular el modelo. Desde la barra de herramientas de Simulink, en la pestaña Modeling, haga clic en la flecha Model Settings y, a continuación, haga clic en Model Properties. En el cuadro de diálogo Model Properties, seleccione la pestaña Callbacks y, en el panel Model callbacks, seleccione InitFcn. Cree el bus para el puerto de salida SolnInfo especificando esta función de inicialización del modelo.

extraInfo_bus = nav.slalgs.internal.utils.extraInfo();
extraInfo_bus.Elements(2).Dimensions = [1000 1];
extraInfo_bus.Elements(2).DimensionsMode = "Variable";

Los bloques Exit Flag y Last Feasible Index muestran los valores de los campos ExitFlag y LastFeasibleIdx en el bus SolnInfo, respectivamente, según los calcula el bloque Timed Elastic Band en cada marca de tiempo.

El resto del ejemplo explica cada componente del modelo, describe los pasos involucrados en la planificación de la trayectoria local y la corrección de errores para generar una trayectoria óptima entre la pose inicial y la pose objetivo.

Planificación de trayectorias globales

El componente Global Path Planning calcula una trayectoria de referencia para que el vehículo navegue desde una posición inicial hasta una posición objetivo en un entorno de mapa específico. El componente utiliza el bloque de función de MATLAB® Global Path Planning para ejecutar la estrategia de planificación de trayectoria. Utilice el bloque de función de MATLAB Global Path Planning para calcular la trayectoria de referencia siguiendo estos pasos:

Cargue un mapa que represente el entorno del vehículo. Para cargar un entorno de mapa personalizado, edite el bloque de función de MATLAB Global Path Planning y especifique el nombre del archivo MAT que contiene el mapa deseado.

function referencePath = exampleHelperGlobalPlanner(startPose,goalPose)
persistent refPath mapMatrix resolution

if isempty(refPath)
    % Load Map Matrix
    data = load("offroadMap.mat","map");
    mapMatrix = data.map;
    resolution = 1;
end

Cree un mapa de ocupación 2-D para el entorno proporcionado.
Cree un objeto de espacio de estados Dubins y establezca el radio de giro mínimo para el vehículo. En este ejemplo, el radio de giro mínimo se establece en 2 m.
Cree un objeto validador de estado para comprobar la validez de los estados.
Configure el planificador de trayectoria RRT* y planifique una trayectoria desde la pose inicial hasta la pose objetivo utilizando el planificador de trayectoria RRT*.
Optimice la trayectoria calculada eliminando waypoints innecesarios.

Los componentes Global Path Planning contienen un disparador que vuelve a planificar la trayectoria de referencia si el vehículo no puede seguirla para navegar desde la posición inicial a la posición objetivo. Esto se activa si la pose actual estimada por el planificador de trayectoria local de TEB es la misma que la pose anterior.

Detección de obstáculos

El componente Obstacle Detection genera una matriz de ocupación local que indica qué áreas alrededor del vehículo están ocupadas o libres.

El componente Obstacle Detection utiliza el bloque de función de MATLAB Local Occupancy Matrix para generar un mapa local egocéntrico. El mapa está centrado en el vehículo y se extiende para cubrir un área definida que incluye el campo de visión inmediato del vehículo. En este ejemplo, el área se determina como el doble de la distancia máxima que el vehículo puede recorrer en una sola iteración y la distancia máxima se establece en 50 m. El bloque de función de MATLAB Local Occupancy Matrix actualiza continuamente el mapa local para reflejar la posición actual del vehículo. Esto implica mover el mapa para alinearlo con la posición actual del vehículo y sincronizarlo con el mapa global para garantizar la coherencia. El bloque Bicycle Kinematics Model proporciona la pose actual del vehículo en cada paso de tiempo. El mapa global proporciona una vista completa de toda el área, y el mapa local se centra en el entorno inmediato del vehículo y se actualiza en tiempo real para reflejar los cambios dinámicos.
El bloque de función de MATLAB Local Occupancy Matrix genera la matriz de ocupación del mapa local y proporciona la ubicación de la cuadrícula en el sistema de coordenadas mundial.

function [occupancyMatrix,gridLocationInWorld] = exampleHelperLocalMap(state)
% The exampleHelperLocalMap block generates an occupancy matrix to detect ...
% obstacles within the field of view. % It simulates sensors like lidar or ...
% cameras to produce occupancy data in a matrix format for further processing.

persistent globalMap localMap

if isempty(globalMap)
 % Load an occupancy map.
 data = load("offroadMap.mat","map");
 globalMap =   binaryOccupancyMap(data.map);

El componente Obstacle Detection almacena la información del mapa del bloque de función de MATLAB Local Occupancy Matrix como una señal de bus. El componente utiliza el bloque de función de MATLAB Convert to Bus para almacenar la matriz de ocupación y la información de la red en una señal de bus. Utilice los parámetros de bloque para especificar el tamaño de la cuadrícula, la resolución del mapa y el origen de la cuadrícula en coordenadas locales.

function mapInfobus = exampleHelperMapInfo(occupancyMatrix,gridLocationInWorld,GridSize,Resolution,GridOriginInLocal)
persistent mapInfoStruct
if isempty(mapInfoStruct)

    % Create occupancy matrix from grid size.
    occupancyMatrix = false(GridSize);

    % Create struct to contain essential information to create map
    mapInfoStruct = struct('Resolution',Resolution,'GridLocationInWorld', ...
        gridLocationInWorld,'OccupancyMatrix' occupancyMatrix,'GridSize', ...
        GridSize,'GridOriginInLocal', GridOriginInLocal);
end

mapInfoStruct.GridLocationInWorld = gridLocationInWorld; % Update map location in world.
mapInfoStruct.OccupancyMatrix = occupancyMatrix; % Update occupancy map
mapInfobus = mapInfoStruct;
end

Planificación de trayectorias locales

El componente Local Path Planning utiliza el bloque Timed Elastic Band para calcular los comandos de velocidad que indican al vehículo que debe seguir la trayectoria de referencia especificada mientras evita obstáculos. El bloque Timed Elastic Band:

Utiliza la información del mapa del componente Obstacle Detection para detectar y evitar obstáculos.
Modifica la trayectoria del vehículo para mantener la proximidad a la trayectoria de referencia evitando obstáculos. El bloque obtiene la trayectoria de referencia del componente Global Path Planning.
Considera las restricciones cinemáticas del vehículo, como la velocidad máxima, el radio de giro mínimo y los límites de aceleración, para garantizar un movimiento viable. Utilice los parámetros del bloque Timed Elastic Band para especificar los parámetros de la trayectoria. Para obtener detalles sobre estos parámetros y cómo establecer sus valores, consulte la sección Parámetros del bloque Timed Elastic Band.

El bloque Timed Elastic Band devuelve estos valores:

Comandos de velocidad en cada marca de tiempo, VelCmds, que incluyen tanto la velocidad lineal como la velocidad angular.
Marcas de tiempo correspondientes a los comandos de velocidad.
Ruta optimizada, OptPath, que especifica la posición xy y el ángulo de orientación (theta) de los waypoints a lo largo de la trayectoria en cada marca de tiempo.
Información adicional, SolnInfo, que proporciona información sobre la solución derivada utilizando el bloque Timed Elastic Band. Los campos LastFeasibleIdx y ExitFlag en el bus de salida SolnInfo indican el estado de la planificación de la trayectoria local.

Corrección de errores

El componente Local Path Planning utiliza el bloque de función de MATLAB Process TEB Error Codes y la función auxiliar exampleHelperProcessTEBErrorFlags para corregir errores y guiar el vehículo hacia la posición objetivo.

Margen de seguridad de obstáculos infringido en más del 10 % (ExitFlag = 2)

Si la trayectoria generada por el algoritmo TEB hace que el vehículo navegue demasiado cerca de un obstáculo, excediendo la restricción del margen de seguridad, el valor ExitFlag para la marca de tiempo correspondiente es 2. Si el margen de seguridad frente a obstáculos especificado no es significativamente mayor que las dimensiones reales del vehículo, infringir este margen implica una posible colisión. Dado que el margen de seguridad del obstáculo define un espacio adicional alrededor del vehículo, se debe determinar si una infracción de este margen realmente indica una colisión. Si la infracción afecta únicamente a la zona de amortiguación y no impacta al vehículo en sí, implica que el vehículo puede circular sin colisión.

Utilice la función auxiliar exampleHelperProcessTEBErrorFlags para verificar si el vehículo está demasiado cerca del obstáculo y puede causar una colisión, y tomar las medidas adecuadas para evitar la colisión. Siga estos pasos para corregir el error y generar la trayectoria optimizada.

Defina un umbral mínimo para la duración de desplazamiento viable mediante el uso del tiempo de anticipación.
Cree una configuración de detección de colisiones teniendo en cuenta las dimensiones del vehículo. La función auxiliar exampleHelperProcessTEBErrorFlags utiliza la función inflationCollisionChecker para determinar la configuración de verificación de colisiones. La función auxiliar exampleHelperVehicleSpecs especifica las dimensiones del vehículo para la detección de colisiones.
Genere un mapa de distancia firmado a partir de la matriz de ocupación calculada por el componente Obstacle Detection.
Seleccione puntos de referencia 2-D a lo largo del vehículo para utilizarlos como límite máximo para la detección de colisiones.
Utilice el mapa de distancias señalizadas para calcular la distancia desde cada punto de referencia del vehículo hasta el obstáculo más cercano. Calcula la distancia mínima entre todos los puntos de referencia para cada pose a lo largo de la trayectoria. Si la distancia es mayor que cero o NaN, la pose correspondiente está libre de colisiones. La distancia positiva indica un margen seguro de obstáculos y NaN representa áreas no definidas donde no se detecta ninguna colisión.
Encuentre las poses para las que la distancia calculada es menor o igual a cero. Puede considerar estas poses como puntos de colisión. Encuentre el índice del primer punto de colisión y verifique su marca de tiempo.
Si la marca de tiempo del primer punto de colisión es menor que el umbral mínimo para la duración de desplazamiento viable, la colisión ocurre demasiado pronto en la trayectoria para la corrección local y la función activa la replanificación de la trayectoria global.
Si la marca de tiempo del primer punto de colisión es mayor que el umbral mínimo para la duración de desplazamiento viable, recorte la trayectoria y los comandos asociados hasta el último índice viable e inicie la planificación de la trayectoria local utilizando el último índice viable como la posición actual.

Radio de giro mínimo superado en más del 10 %(ExitFlag = 3)

Si la trayectoria generada por el algoritmo TEB da como resultado un radio de giro para el vehículo que es menor que el radio de giro mínimo, el valor ExitFlag para la marca de tiempo correspondiente es 3. Esto implica que el vehículo está intentando realizar un giro más cerrado que el permitido por la restricción del radio de giro mínimo, excediendo esta restricción en más del 10%. La discrepancia a menudo surge de una desalineación entre el ángulo de rumbo real del vehículo y la trayectoria generada por el algoritmo TEB. En un esfuerzo por corregir esta desalineación y adherirse estrictamente a la trayectoria planificada, el algoritmo puede generar inadvertidamente comandos de velocidad que excedan las restricciones cinemáticas del vehículo.

Utilice la función auxiliar exampleHelperProcessTEBErrorFlags para ajustar la velocidad lineal y la velocidad angular del vehículo de tal manera que el radio de curvatura satisfaga la restricción del radio de giro mínimo. Realice estos pasos para corregir el error y generar la trayectoria optimizada.

Para ajustar la velocidad lineal:

Defina la velocidad inversa máxima como el límite de velocidad mínima permisible y la velocidad lineal máxima como el límite de velocidad máxima permisible para el vehículo. Debe configurar los valores de velocidad máxima como parámetros del bloque Timed Elastic Band.
Revise los comandos de velocidad en cada paso de tiempo para identificar cualquiera que exceda los límites de velocidad definidos.
Ajuste los comandos de velocidad fuera de rango para que se ajusten a los límites permitidos.

Para ajustar la velocidad angular,

Calcula el radio de giro mínimo como relación entre la velocidad lineal máxima del vehículo y su velocidad angular máxima.
Calcule el radio de curvatura para cada conjunto de comandos de velocidad tomando el valor absoluto de la velocidad lineal dividido por la velocidad angular.
Para un comando de velocidad dado, si el radio de curvatura es menor que el radio de giro mínimo, ajuste la velocidad angular configurándola en la velocidad lineal dividida por el radio de giro mínimo.

La planificación de trayectoria falla debido a puntos inalcanzables o mínimos locales (LastFeasibleIdx == 1)

Un LastFeasibleIdx es 1, indica que el controlador no ha generado una trayectoria o que los puntos de trayectoria de referencia cercanos son inalcanzables. Para mitigar este error, utilice el bloque de función de MATLAB Process TEB Error Codes para activar la replanificación global.

Modelo cinemático de bicicleta

El bloque Bicycle Kinematic Model crea un modelo de vehículo para simular la cinemática simplificada de un vehículo. Toma velocidades lineales y angulares como entradas de comando del bloque Timed Elastic Band y genera los estados de posición y velocidad actuales del vehículo. Luego, proporciona estos estados de velocidad y posición actuales como entradas al bloque Timed Elastic Band y al componente Obstacle Detection para calcular la siguiente pose a lo largo de la trayectoria óptima.

El modelo continúa realizando la planificación de la trayectoria local y la corrección de errores hasta que el vehículo alcanza la posición objetivo.

Visualización

El bloque de función de MATLAB Visualization traza los resultados de la planificación de la trayectoria local y el valor del indicador de salida en cada marca de tiempo. El bloque utiliza las funciones auxiliares exampleHelperPlotMap, exampleHelperPlotVehicle y exampleHelperBuildVehicleGraphic para trazar los resultados. El bloque de función de MATLAB Plot Exit Flag representa los valores ExitFlag obtenidos en cada marca de tiempo.

Ejecutar simulación

Establezca el tiempo de parada de la simulación en 100 segundos. Ejecute la simulación para visualizar los resultados del seguimiento de trayectoria. Al examinar ExitFlag, puede determinar con qué frecuencia el planificador de trayectorias local detectó trayectorias no viables y el modelo tomó medidas correctivas para calcular una trayectoria viable para que el robot navegue hasta la posición objetivo.

sim("PathFollowingTEB.slx");

Consulte también