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

Generación de trayectoria óptima para la conducción urbana

Este ejemplo utiliza:

Este ejemplo muestra cómo realizar una replanificación dinámica en un escenario urbano usando trajectoryOptimalFrenet.

En este ejemplo, usted:

Explora un escenario urbano con vehículos predefinidos.
Utilice trajectoryOptimalFrenet para realizar una planificación local para navegar por el escenario urbano.

Contenido

Introducción

La conducción autónoma en un escenario urbano necesita una planificación en dos niveles, global y local. El planificador global encuentra la ruta más factible entre el punto de partida y el de meta. El planificador local realiza una replanificación dinámica para generar una trayectoria óptima basada en el plan global y la información circundante. En este ejemplo, un vehículo ego (cuadro verde) sigue un plan global (línea azul punteada). Se realiza la planificación local (línea naranja continua) mientras se intenta evitar otro vehículo (rectángulo negro).

Los planificadores locales utilizan la información de los obstáculos para generar trayectorias óptimas sin colisiones. La información de obstáculos de varios sensores a bordo como cámara, radar y lidar se fusiona para mantener actualizado el mapa de ocupación. Este mapa de ocupación es egocéntrico, donde el marco local se centra en el vehículo ego. El mapa se utiliza para la planificación local cuando los sensores detectan obstáculos y los colocan en el mapa.

Explorar un escenario urbano para la planificación local

Este escenario de ejemplo tiene otros cuatro vehículos (rectángulos azules), que se mueven en rutas predefinidas a diferentes velocidades. La siguiente figura ilustra este escenario y el plan global (línea roja continua) utilizado en este ejemplo. Los puntos rojos sólidos en la siguiente figura representan los puntos de referencia del plan global entre las posiciones inicial y final. El rectángulo verde representa el vehículo ego.

El vehículo ego utiliza trajectoryOptimalFrenet para navegar desde la posición A a la posición D en tres segmentos con tres parámetros de configuración diferentes.

Primero (A a B), el vehículo demuestra el comportamiento del Control de crucero adaptativo (ACC).
En segundo lugar (B a C), el vehículo negocia un giro para seguir el plan global.
En tercer lugar (C a D), el vehículo realiza una maniobra de cambio de carril.

Configure los datos necesarios y las variables de entorno:

% Load data from urbanScenarioData.mat file, initialize required variables
[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;

otrosVehículos: [1 x 4] arreglo de estructura que contiene campos: Position, Yaw, Velocity, y SimulationTime, de cada vehículo en el escenario.
globalPlanPoints: [18 x 2] La matriz contiene un plan global precalculado que consta de dieciocho puntos de referencia, cada uno de los cuales representa una posición en el escenario.
stateValidator: validatorOccupancyMap objeto que actúa como validador de estado en función de un mapa de agarre 2-D determinado. Un mapa de ocupación egocéntrico totalmente ocupado se actualiza en función de la información de obstáculos y los límites de las carreteras. También se puede utilizar un validador de estado personalizado según la aplicación. Para obtener más información, consulte nav.StateValidator.

Traza el escenario.

exampleHelperPlotUrbanScenario;

Figure contains an axes object and an object of type uipanel. The axes object contains 16 objects of type line, patch.

Crear planificador local

Especifique el validador de estado y el plan global para crear un planificador local usando trajectoryOptimalFrenet.

localPlanner = trajectoryOptimalFrenet(globalPlanPoints,stateValidator);

Explorar propiedades de `localPlanner`

El localPlanner tiene una variedad de propiedades que se pueden ajustar para lograr el comportamiento deseado. Esta sección explora algunas de estas propiedades y sus valores predeterminados.

localPlanner.TerminalStates

Longitudinal: [30 45 60 75 90]: Define la distancia de muestreo longitudinal en metros. Este valor puede ser un escalar o un vector.
Lateral: [-2 -1 0 1 2]: Define la desviación lateral en metros de la trayectoria de referencia (Plan Global en nuestro caso).
Time: 7: Tiempo en segundos para llegar al final de la trayectoria.
Speed: 10: Velocidad en metros por segundo, a alcanzar al final de la trayectoria
Acceleration: 0: Aceleración al final de la trayectoria en m/s^2.

localPlanner.FeasibilityParameters

MaxCurvature: 0.1 : Valor máximo factible para la curvatura en m^-1
MaxAcceleration: 2.5: Aceleración máxima factible en m/s^2.

localPlanner.TimeResolution: 0.1: Intervalo de discretización de la trayectoria en segundos

Utilice `trajectoryOptimalFrenet` para demostrar el comportamiento del control de crucero adaptativo (ACC)

En esta sección, asigne las propiedades necesarias para configurar localPlanner para demostrar el comportamiento del control de crucero adaptativo (ACC).

Para demostrar ACC, el vehículo ego necesita seguir a un vehículo líder manteniendo una distancia segura. El vehículo líder en este segmento se obtiene usando otherVehicles(1).

% Get leadVehicle in segment from Postion A to Position B
leadVehicle = otherVehicles(1);

% Define ACC safe distance
ACCSafeDistance = 35; % in meters
% Adjusting the time resolution of planner object to make the ego vehicle
% travel smoothly
timeResolution = 0.01;
localPlanner.TimeResolution = timeResolution;

Configure el vehículo ego en la posición A y defina su velocidad y orientación iniciales (Guía).

% Set positions A, B, C and D
positionA =  [5.1, -1.8, 0];
positionB =  [60, -1.8, 0];
positionC =  [74.45, -30.0, 0];
positionD =  [74.45, -135, 0];
goalPoint =  [145.70, -151.8, 0];

% Set the initial state of the ego vehicle
egoInitPose     = positionA;
egoInitVelocity = [10, -0.3, 0];
egoInitYaw      = -0.165;
currentEgoState = [egoInitPose(1), egoInitPose(2), deg2rad(egoInitYaw),...
    0, norm(egoInitVelocity), 0];
vehicleLength = 4.7; % in meters
% Replan interval in number of simulation steps
% (default 50 simulation steps)
replanInterval = 50;

Visualice los resultados de la simulación.

% Initialize Visualization
exampleHelperInitializeVisualization;

El comportamiento de ACC se logra configurando TerminalStates de localPlanner como se muestra a continuación:

Para mantener la distancia segura con el vehículo líder, establezca localPlanner.TerminalStates.Longitudinal en Distancia al vehículo líder - Longitud del vehículo;

Para mantener la velocidad relativa con respecto al vehículo líder, establezca localPlanner.TerminalStates.Speed en Velocidad del vehículo líder;

Para continuar navegando en el plan global, establezca localPlanner.TerminalStates.Lateral en 0;

En el siguiente fragmento de código, localPlanner genera trajectory que se ejecuta y visualiza usando exampleHelperUpdateVisualization para cada paso de la simulación. Sin embargo, la replanificación se realiza en cada paso de la simulación de replanInterval. La siguiente es la secuencia de eventos durante la replanificación:

Actualice el mapa de ocupación utilizando la información del vehículo usando exampleHelperUpdateOccupancyMap.
Actualice el localPlanner.TerminalStates.
Generación de trayectoria usando plan(localPlanner,currentStateInFrenet).

% Simulate till the ego vehicle reaches position B
simStep = 1;
% Check only for X as there is no change in Y.
while currentEgoState(1) <= positionB(1)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0 || simStep == 1
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);
        
        % Compute distance to Lead Vehicle and leadVehicleVelocity
        distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ...
            currentEgoState(1:2));
        leadVehicleVelocity = leadVehicle.Velocity(simStep,:);
        
        % Set localPlanner.TerminalStates for ACC behavior
        if distanceToLeadVehicle <= ACCSafeDistance
            localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength;
            localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity);
            localPlanner.TerminalStates.Lateral = 0;
            desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
                localPlanner.TerminalStates.Speed;
            localPlanner.TerminalStates.Time = desiredTimeBound;
            localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
        end
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Al final de esta ejecución, el vehículo ego se encuentra en la posición B.

A continuación, configure trajectoryOptimalFrenet para negociar un giro en el segundo segmento desde la posición B a la posición C.

Usa `trajectoryOptimalFrenet` para Negociar un giro suave

Las propiedades actuales establecidas del localPlanner son suficientes para realizar un giro suave. Sin embargo, en el segundo segmento, el vehículo principal se obtiene de otrosVehículos(2).

% Set Lead Vehicle to correspond to the vehicle in second segment
% from position B to position C
leadVehicle = otherVehicles(2);

% Simulate till the ego vehicle reaches position C
% Check only for Y as there is no change in X at C
while currentEgoState(2) >= positionC(2)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState);
        
        % Compute distance to Lead Vehicle and leadVehicleVelocity
        distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ...
            currentEgoState(1:2));
        leadVehicleVelocity = leadVehicle.Velocity(simStep,:);
        
        if(distanceToLeadVehicle <= ACCSafeDistance)
            localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength;
            localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity);
            localPlanner.TerminalStates.Lateral = 0;
            desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
                localPlanner.TerminalStates.Speed;
            localPlanner.TerminalStates.Time = desiredTimeBound;
            localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
            localPlanner.FeasibilityParameters.MaxAcceleration = 5;
        end
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :), egoPolygon(2, :), "g", "Parent", hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner, "Trajectory", "optimal", "Parent", hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Al final de esta ejecución, el vehículo ego se encuentra en la posición C.

A continuación, configure trajectoryOptimalFrenet para realizar una maniobra de cambio de carril desde la posición C a la posición D.

Utilice `trajectoryOptimalFrenet` para realizar la maniobra de cambio de carril

La maniobra de cambio de carril se puede realizar configurando adecuadamente los estados del terminal lateral del planificador. Esto se puede lograr estableciendo el estado terminal lateral en el ancho del carril (3,6 m en este ejemplo) y asumiendo que la ruta de referencia está alineada con el centro del carril del ego actual.

% Simulate till the ego vehicle reaches position D
% Set Lane Width
laneWidth = 3.6;
% Check only for Y as there is no change in X at D
while currentEgoState(2) >= positionD(2)
    
    % Replan at every "replanInterval" simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);
        
        % TerminalState settings for negotiating Lane change
        localPlanner.TerminalStates.Longitudinal = 20:5:40;
        localPlanner.TerminalStates.Lateral = laneWidth;
        localPlanner.TerminalStates.Speed = 10;
        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);
        localPlanner.TerminalStates.Time = desiredTimeBound;
        localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
        localPlanner.FeasibilityParameters.MaxAcceleration = 15;
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner,[currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner,currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Simular la ejecución del vehículo ego para llegar al punto objetivo

El localPlanner ahora está configurado para navegar desde la posición D hasta la posición objetivo.

% Simulate till the ego vehicle reaches Goal position
% Check only for X as there is no change in Y at Goal.
while currentEgoState(1) <= goalPoint(1)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState);
        localPlanner.TerminalStates.Longitudinal = 20;
        localPlanner.TerminalStates.Lateral = [-1 0 1];
        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);
        localPlanner.TerminalStates.Time = desiredTimeBound:0.2:desiredTimeBound+1;
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    % Goal reached will be true only in this section.
    if(isGoalReached)        
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Registre las posiciones del vehículo ego en la variable egoPoses que está disponible en el espacio de trabajo base. Puede reproducir la ruta del vehículo en DrivingScenario usando exampleHelperPlayBackInDS(egoPoses).

% Clear workspace variables that were created during the example run.
% This excludes egoPoses to allow the user to playback the simulation in DS
exampleHelperUrbanCleanUp;

"Conclusión "

Este ejemplo ha explicado cómo realizar una replanificación dinámica en un escenario urbano usando trajectoryOptimalFrenet. En particular, hemos aprendido cómo usar trajectoryOptimalFrenet para realizar el siguiente comportamiento.