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

Sintaxis

trajectoryOptimalFrenet(refPath,validator)

planner = trajectoryOptimalFrenet(___,Name,Value)

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 .

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

Argumentos de entrada

refPath — Ruta de referencia

n-por-2 matriz

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

validator — Objeto del validador de estado

validatorOccupancyMap objeto

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

Propiedades

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'.

Weights — Pesos para todos los costes de trayectoria.

estructura

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.

Time — Peso por coste de tiempo

0 (predeterminado) | escalar positivo

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

ArcLength — Peso por coste de longitud de arco

0 (predeterminado) | escalar positivo

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

LateralSmoothness — Peso por coste de tirón lateral

0 (predeterminado) | escalar positivo

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

LongitudinalSmoothness — Peso por coste de tirón longitudinal

0 (predeterminado) | escalar positivo

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

Deviation — Peso por desviación de la ruta de referencia

1 (predeterminado) | escalar positivo

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

FeasibilityParameters — Estructura que contiene parámetros de viabilidad.

estructura

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

MaxCurvature — Curvatura máxima que puede ejecutar el vehículo

0.1 (predeterminado) | escalar real positivo

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

MaxAcceleration — Aceleración máxima en la dirección del movimiento del vehículo.

2.5 (predeterminado) | escalar real positivo

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/s². 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

TimeResolution — Intervalo de discretización de trayectoria

0.1 (predeterminado) | escalar real positivo

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

CostFunction — Función de coste definida por el usuario

nuloCoste (predeterminado) | mango de función

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

TrajectoryList — Lista de todas las trayectorias posibles.

arreglo de estructuras

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

TerminalStates — Estructura de todos los estados objetivo.

estructura

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.

Longitudinal — Longitudes del segmento de trayectoria.

30:15:90 (predeterminado) | vector

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

Tipos de datos: double

Lateral — Conjunto de desviaciones de la trayectoria de referencia en dirección perpendicular al estado objetivo

-2:1:2 (predeterminado) | vector

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

Speed — Velocidad en el estado objetivo en la dirección del movimiento.

10 (predeterminado) | escalar positivo

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

Tipos de datos: double

Acceleration — Aceleración en el estado objetivo en la dirección del movimiento.

0 (predeterminado) | escalar positivo

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

Tipos de datos: double

Time — Conjunto de tiempos finales para ejecutar el segmento de trayectoria

7 (predeterminado) | vector positivo

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

Waypoints — Waypoints de la ruta de referencia

[ ] (predeterminado) | n-por-2 matriz

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

NumSegments — Número de segmentos longitudinales para cada trayectoria

1 (predeterminado) | escalar positivo

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

DeviationOffset — Desviación de la trayectoria de referencia en dirección lateral

0 (predeterminado) | escalar

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

cart2frenet	Convertir estados cartesianos a estados de Frenet
copy	Crear copia profunda del objeto
frenet2cart	Convertir estados de Frenet a estados cartesianos
plan	Planificar la trayectoria óptima
show	Visualizar trayectorias

Ejemplos

Planificación óptima de trayectoria en el espacio Frenet

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')

Partición de estados terminales longitudinales en la generación de trayectorias

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','--')

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

Generación de ruta de referencia

Los objetos trajectoryOptimalFrenet y referencePathFrenet generan una ruta de referencia utilizando los puntos de referencia especificados ajustando una spline clotoide continua por partes a los puntos de referencia [2].

• Una clotoide conecta cada par de puntos de referencia adyacentes (el cambio en la curvatura es constante con respecto a la longitud del arco).

• Estas condiciones de contorno definen los parámetros de la clotoide:

  • Si se especifica como un arreglo n por 2 de [ x y] waypoints, entonces el solver producirá una spline continua C2 (Los puntos finales de la clotoide son tangentes y tienen una curvatura coincidente con la de sus vecinos).

  • Si se especifica como un arreglo n por 3 de [ x y theta] waypoints, luego se elimina un grado de libertad del solver, por lo que la spline es solo C1 continua (los puntos finales de la clotoide son tangentes a sus vecinos, pero la curvatura puede cambiar abruptamente).

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

Consulte también

validatorOccupancyMap | nav.StateValidator | plannerHybridAStar