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

plan

Planificar la trayectoria óptima

contraer todo en la página

Sintaxis

[traj,index,cost,flag] = plan(planner,start)

Descripción

[traj,index,cost,flag] = plan(planner,start) calcula una trayectoria factible, traj, a partir de una lista de trayectorias candidatas generadas a partir del objeto trajectoryOptimalFrenet, planner. start se especifica como un vector de seis elementos [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2], donde s es la longitud del arco desde el primer punto en la ruta de referencia y l es la distancia normal desde el punto más cercano en s en la ruta de referencia.

La trayectoria de salida, traj, también tiene un cost y index asociado para la propiedad TrajectoryList del planificador. flag es un indicador de salida numérico que indica el estado de la solución.

Para mejorar los resultados de la salida de planificación, modifique los parámetros en el objeto planner.

ejemplo

Ejemplos

contraer todo

Abrir script en vivo

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 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];

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

Abrir script en vivo

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.

Argumentos de entrada

contraer todo

Planificador de trayectoria óptima en el espacio Frenet, especificado como un objeto trajectoryOptimalFrenet.

Estado inicial de Frenet, especificado como un vector de 1 por 6 [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2].

  • s especifica la longitud del arco desde el primer punto en la ruta de referencia en metros.

  • ds/dt especifica la primera derivada de la longitud del arco.

  • d2s/dt2 especifica la segunda derivada de la longitud del arco.

  • l especifica la distancia normal desde el punto más cercano en la ruta de referencia en metros.

  • dl/ds especifica la primera derivada de la distancia normal.

  • d2l/ds2 especifica la segunda derivada de la distancia normal.

Argumentos de salida

contraer todo

Trayectoria factible con costo mínimo, devuelta como una matriz n por 7 de [x, y, theta, kappa, speed, acceleration, time], donde n es el número de puntos de referencia de la trayectoria.

  • x y y especifican la posición en metros.

  • theta especifica el ángulo de orientación en radianes.

  • kappa especifica la curvatura en m-1.

  • speed especifica la velocidad en m/s.

  • acceleration especifica la aceleración en m/s2.

  • time especifica la hora en s.

Índice de trayectoria factible con coste mínimo, devuelto como un escalar entero positivo.

Menor coste de la trayectoria factible, devuelto como un escalar positivo.

Indicador de salida que indica el estado de la solución, devuelto como 0 o 1.

  • 0 — Se encontró la trayectoria óptima.

  • 1 — No existe ninguna trayectoria factible.

Cuando no existe una trayectoria factible, el planificador devuelve una trayectoria vacía.

Capacidades ampliadas

expandir todo

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

|