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

frenet2cart

Convertir estados de Frenet a estados cartesianos

Sintaxis

frenet2cart(planner,frenetStates)

Descripción

frenet2cart(planner,frenetStates) convierte un vector de seis elementos de frenetStates [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2] en un vector de seis elementos de estados cartesianos [x, y, theta, kappa, speed, acceleration].

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

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

Argumentos de entrada

planner — Planificador de trayectoria óptimo en el espacio Frenet.

objeto trajectoryOptimalFrenet

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

frenetStates — Vector de estados de Frenet

vector de seis elementos

Vector de estados 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.

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

• d²l/ds² especifica la segunda derivada de la distancia normal.

Ejemplo: [10 1 0 3 0 0]

Tipos de datos: double

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

trajectoryOptimalFrenet | cart2frenet