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(
crea un objeto refPath
,validator
)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
.
establece propiedades adicionales utilizando uno o más pares nombre-valor en cualquier orden.planner
= trajectoryOptimalFrenet(___,Name,Value
)
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/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
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/s2.
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
ypi/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
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)