Main Content

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

trajectoryOptimalFrenet

Encuentre la trayectoria óptima a lo largo de la ruta de referencia

Desde R2019b

Descripción

El objeto trajectoryOptimalFrenet es un planificador de rutas que muestrea y evalúa trayectorias locales basándose en una ruta de referencia. El planificador genera un conjunto de estados terminales basados ​​en la ruta de referencia y otros parámetros del objeto. Luego, el planificador conecta el estado con cada estado terminal utilizando polinomios de cuarto o quinto orden. Para elegir una ruta óptima, las trayectorias muestreadas se evalúan en cuanto a viabilidad cinemática, colisión y coste.

Creación

Descripción

trajectoryOptimalFrenet(refPath,validator) crea un objeto trajectoryOptimalFrenet con ruta de referencia, refPath, en la forma de un n- arreglo por 2 de [x y] waypoints y un validador de estado, validator, especificado como un objeto validatorOccupancyMap .

ejemplo

planner = trajectoryOptimalFrenet(___,Name,Value) establece propiedades adicionales utilizando uno o más pares nombre-valor en cualquier orden.

Argumentos de entrada

expandir todo

Ruta de referencia, especificada como una matriz n-por-2 de pares [x y] , donde n es el número de puntos de referencia.

Ejemplo: [100,100;400,400]

Tipos de datos: double

Objeto del validador de estado, especificado como un objeto validatorOccupancyMap .

Propiedades

expandir todo

Nota

Para las propiedades 'Weights' y 'FeasibilityParameters' , no puede especificar todas las estructuras a la vez. En su lugar, configure sus campos individualmente como pares de nombre-valor. Por ejemplo, trajectoryOptimalFrenet(refPath,validator,'Deviation',0) establece el campo 'Deviation' de la estructura 'Weights'.

Las ponderaciones de todos los costes de la trayectoria, especificadas como una estructura que contiene escalares para los multiplicadores de costes de los atributos de la trayectoria correspondientes. El coste total de la trayectoria es una suma de todos los atributos multiplicada por sus pesos. La estructura tiene estos campos.

La función de coste multiplica el peso por el tiempo total necesario para llegar al estado terminal. Especifique este valor como el par separado por comas de 'Time' y un escalar positivo en segundos.

Tipos de datos: double

La función de coste multiplica el peso por la longitud total de las trayectorias generadas. Especifique este valor como el par separado por comas de 'ArcLength' y un escalar positivo en metros.

Tipos de datos: double

La función de coste multiplica el peso por la integral del tirón lateral al cuadrado. Este valor determina la agresividad de la trayectoria en dirección lateral (perpendicular a la trayectoria de referencia). Especifique este valor como el par separado por comas de 'LateralSmoothness' y un escalar positivo. Para penalizar el tirón lateral en la trayectoria planificada aumentar este valor de coste.

Tipos de datos: double

La función de coste multiplica el peso por la integral del tirón longitudinal al cuadrado. Este valor determina la agresividad de las trayectorias en dirección longitudinal (dirección de la trayectoria de referencia). Especifique este valor como el par separado por comas de 'LongitudinalSmoothness' y un escalar positivo. Para penalizar grandes cambios en la aceleración hacia adelante y hacia atrás, aumente este valor de coste.

Tipos de datos: double

La función de coste multiplica el peso por la distancia perpendicular desde la ruta de referencia al final de la trayectoria en metros. Especifique este valor como el par separado por comas de 'Deviation' y un escalar positivo en metros.

Tipos de datos: double

Tipos de datos: struct

Parámetros de viabilidad, especificados como una estructura que contiene valores escalares para comprobar la validez de una trayectoria. La estructura tiene estos campos.

Curvatura máxima que puede ejecutar el vehículo. Especifique este valor como el par separado por comas de 'MaxCurvature' y un escalar real positivo en m-1. Este valor determina la viabilidad cinemática de la trayectoria.

Tipos de datos: double

Aceleración máxima en la dirección de movimiento del vehículo. Especifique este valor como el par separado por comas de 'MaxAcceleration' y un escalar real positivo en m/s2. Para reducir el límite de aceleración del vehículo en dirección hacia adelante o hacia atrás, disminuya este valor.

Tipos de datos: double

Tipos de datos: struct

Intervalo de tiempo entre estados discretizados de la trayectoria. Especifique este valor como el par separado por comas de 'TimeResolution' y un escalar real positivo en segundos. Estos estados discretizados determinan la validez del estado y la función de costes.

Tipos de datos: double

La función de coste definida por el usuario, especificada como un identificador de función. La función debe aceptar una matriz de n-por-7 estados, TRAJSTATES, para cada trayectoria y devolver un valor de coste como escalar. La función plan devuelve la ruta con el menor coste.

Por ejemplo, leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10) crea un identificador de función de coste para priorizar los cambios de carril izquierdo.

Tipos de datos: function handle

Esta propiedad o parámetro es de solo lectura.

La propiedad 'TrajectoryList' , devuelta como un arreglo de estructura de todas las trayectorias candidatas y sus parámetros correspondientes. Cada estructura tiene estos campos:

  • Trajectory — Una matriz n-por 7 de [x, y, theta, kappa, speed, acceleration, time], donde n es el número de waypoints de la trayectoria.

  • Cost — Coste de la trayectoria.

  • MaxAcceleration — Aceleración máxima de la trayectoria.

  • MaxCurvature — Curvatura máxima de la trayectoria.

  • Feasible — Un vector de cuatro elementos [velocity, acceleration, curvature, collision] que indica la validez de la trayectoria.

    El valor de los elementos puede ser,

    • 1 — La trayectoria es válida.

    • 0 — La trayectoria no es válida.

    • -1 — La trayectoria no está marcada.

Tipos de datos: struct

Estructura que contiene una lista de estados objetivo relacionados con la ruta de referencia. Estos parámetros definen el comportamiento de muestreo para generar segmentos de trayectoria alternativos entre el inicio y cada estado objetivo. La estructura tiene estos campos.

Longitudes del segmento de trayectoria, especificadas como vector en metros.

Tipos de datos: double

Conjunto de desviaciones de la trayectoria de referencia en dirección perpendicular al estado objetivo, especificada como un vector en metros.

Tipos de datos: double

Velocidad en el estado objetivo en la dirección del movimiento, especificada como un escalar positivo en m/s.

Tipos de datos: double

Aceleración en el estado objetivo en la dirección del movimiento, especificada como un escalar positivo en m/s2.

Tipos de datos: double

Arreglo de tiempos de finalización para ejecutar el segmento de trayectoria, especificado como un vector positivo en segundos.

Tipos de datos: double

Tipos de datos: struct

Puntos de referencia de la ruta de referencia, especificados como una matriz n-por 2 de pares [x y] , donde n es el número de puntos de referencia. Los waypoints actúan como referencia para planificar trayectorias alternativas optimizadas por este planificador.

Tipos de datos: double

Número de segmentos longitudinales para cada trayectoria. Especifique este valor como el par de 'NumSegments' separados por comas y un escalar positivo. Esta propiedad genera estados terminales longitudinales intermedios con los que se combinan todos los estados terminales laterales para generar más primitivas de movimiento para cada estado terminal.

Por ejemplo, 'NumSegments',2 crea dos particiones entre cada estado terminal longitudinal. Se generan trayectorias para alcanzar los estados longitudinales intermedios con todos los estados terminales laterales disponibles.

Tipos de datos: double

Desviación de la trayectoria de referencia en dirección lateral. Especifique este valor como el par separado por comas de 'DeviationOffset' y un escalar. Un valor negativo compensa la desviación hacia la derecha y un valor positivo compensa la desviación hacia la izquierda de la ruta de referencia en la dirección lateral. Establezca esta propiedad para desviar su solución hacia una determinada dirección de giro al evitar obstáculos en la ruta de referencia.

Tipos de datos: double

Funciones del objeto

cart2frenetConvertir estados cartesianos a estados de Frenet
copyCrear copia profunda del objeto
frenet2cartConvertir estados de Frenet a estados cartesianos
planPlanificar la trayectoria óptima
showVisualizar trayectorias

Ejemplos

contraer todo

Este ejemplo muestra cómo planificar una trayectoria óptima utilizando un objeto trajectoryOptimalFrenet .

Crear y asignar mapa al validador de estado

Cree un objeto de validación de estado para verificar colisiones.

stateValidator = validatorOccupancyMap;

Crea un mapa de cuadrícula de obstáculos.

grid = zeros(50,100);
grid(24:26,48:53) = 1;

Crea un binaryOccupancyMap con el mapa de cuadrícula.

map = binaryOccupancyMap(grid);

Asigne el mapa y los límites estatales al validador estatal.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Planificar y visualizar la trayectoria

Cree una ruta de referencia para que la siga el planificador.

refPath = [0,25;100,25];

Inicialice el objeto del planificador con la ruta de referencia y el validador de estado.

planner = trajectoryOptimalFrenet(refPath,stateValidator);

Asigne valores de estado terminal longitudinal, desviación lateral y aceleración máxima.

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -10:5:10;
planner.FeasibilityParameters.MaxAcceleration = 10;

Especifique el valor de compensación de desviación cerca del estado del terminal lateral izquierdo para priorizar los cambios de carril izquierdo.

planner.DeviationOffset = 5;

Planificación de trayectoria

Estado cartesiano inicial del vehículo.

initCartState = [0 25 pi/9 0 0 0];

Convierte el estado cartesiano del vehículo al estado de Frenet.

initFrenetState = cart2frenet(planner,initCartState);

Planifique una trayectoria desde el estado inicial de Frenet.

plan(planner,initFrenetState);

Visualización de trayectoria

Visualiza el mapa y las trayectorias.

show(map)
hold on
show(planner,'Trajectory','all')

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, patch. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Optimal Trajectory.

Este ejemplo muestra cómo dividir los estados terminales longitudinales en la planificación de trayectoria óptima utilizando un objeto trajectoryOptimalFrenet .

Crear y asignar mapa al validador de estado

Cree un objeto de validación de estado para verificar colisiones.

stateValidator = validatorOccupancyMap; 

Crea un mapa de cuadrícula de obstáculos.

grid = zeros(50,100);
grid(25:27,28:33) = 1;
grid(16:18,37:42) = 1;
grid(29:31,72:77) = 1;

Crea un binaryOccupancyMap con el mapa de cuadrícula.

map = binaryOccupancyMap(grid);

Asigne el mapa y los límites estatales al validador estatal.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Planificar y visualizar la trayectoria

Cree una ruta de referencia para que la siga el planificador.

refPath = [0,25;30,30;75,20;100,25];

Inicialice el objeto del planificador con la ruta de referencia y el validador de estado.

planner = trajectoryOptimalFrenet(refPath,stateValidator);

Asigne valores de estado terminal longitudinal, desviación lateral y aceleración máxima.

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -5:5:5;
planner.FeasibilityParameters.MaxAcceleration = 10;

Asigne el número de particiones para el estado terminal longitudinal.

planner.NumSegments = 3;

Planificación de trayectoria

Estado Frenet inicial del vehículo.

initFrenetState = zeros(1,6);

Planifique una trayectoria desde el estado inicial de Frenet.

plan(planner,initFrenetState);

Visualización de trayectoria

Visualiza el mapa y las trayectorias.

show(map)
hold on
show(planner,'Trajectory','all')
hold on

Generar límites de carril

Calcule el final de la ruta de referencia como estado de Frenet.

refPathEnd = cart2frenet(planner,[planner.Waypoints(end,:) 0 0 0 0]);

Calcule los desplazamientos de carril en ambos lados de los estados terminales laterales con un valor de ancho de medio carril.

laneOffsets = unique([planner.TerminalStates.Lateral+2.5 planner.TerminalStates.Lateral-2.5]);

Calcular posiciones de carriles en estado cartesiano.

numLaneOffsets = numel(laneOffsets);
xRefPathEnd = ceil(refPathEnd(1));
laneXY = zeros((numLaneOffsets*xRefPathEnd)+numLaneOffsets,2);
xIndex = 0;

for laneID = 1:numLaneOffsets
    for x = 1:xRefPathEnd
        laneCart = frenet2cart(planner,[x 0 0 laneOffsets(laneID) 0 0]);
        xIndex = xIndex + 1;
        laneXY(xIndex,:) = laneCart(1:2);
    end
    xIndex = xIndex + 1;
    laneXY(xIndex,:) = NaN(1,2);
end

Trazar los límites de los carriles.

plot(laneXY(:,1),laneXY(:,2),'LineWidth',0.5,'Color',[0.5 0.5 0.5],'DisplayName','Lane Boundaries','LineStyle','--')

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 6 objects of type image, line, patch. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Optimal Trajectory, Lane Boundaries.

Limitaciones

  • Las autointersecciones en la ruta de referencia pueden provocar comportamientos inesperados.

  • El planificador no admite la marcha atrás.

  • La orientación inicial para la planificación debe estar dentro de -pi/2 y pi/2 con respecto a la ruta de referencia.

  • Limite el número de TerminalStates para aplicaciones en tiempo real, ya que la complejidad computacional crece con ello.

Más acerca de

expandir todo

Referencias

[1] Werling, Moritz, Julius Ziegler, Sören Kammel, and Sebastian Thrun. "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame." 2010 IEEE International Conference on Robotics and Automation. 2010, pp. 987–993.

[2] Bertolazzi, Enrico, and Marco Frego. “Fast and Accurate Clothoid Fitting.” Mathematical Methods in the Applied Sciences 38, no. 5 (March 30, 2015): 881–97. https://doi.org/10.1002/mma.3114.

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2019b