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

Planificación de la trayectoria de la carretera mediante la ruta de referencia de Frenet

Este ejemplo demuestra cómo planificar una trayectoria local en un escenario de conducción en carretera. Este ejemplo utiliza una ruta de referencia y una lista dinámica de obstáculos para generar trayectorias alternativas para un vehículo ego. El vehículo ego navega a través del tráfico definido en un escenario de conducción proporcionado a partir de un objeto drivingScenario . El vehículo alterna entre control de crucero adaptativo, cambio de carril y maniobras de seguimiento del vehículo según el coste, la viabilidad y el movimiento sin colisiones.

Escenario de conducción de carga

Comience cargando el objeto drivingScenario proporcionado, que define las propiedades del vehículo y la carretera en el espacio de trabajo actual. Este escenario se generó utilizando la app Driving Scenario Designer (Automated Driving Toolbox) y se exportó a una función de MATLAB® , drivingScenarioTrafficExample. Para obtener más información, consulte Create Driving Scenario Variations Programmatically (Automated Driving Toolbox).

scenario = drivingScenarioTrafficExample;
% Default car properties
carLen   = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;

% Define road dimensions
laneWidth   = carWidth*2; % in meters

plot(scenario);

Construir ruta de referencia

Toda la planificación local en este ejemplo se realiza con respecto a una ruta de referencia, representada por un objeto referencePathFrenet . Este objeto puede devolver el estado de la curva en longitudes dadas a lo largo de la ruta, encontrar el punto más cercano a lo largo de la ruta a alguna ubicación xy global y facilita las transformaciones de coordenadas entre los marcos de referencia global y Frenet.

Para este ejemplo, la ruta de referencia se trata como el centro de una autopista de cuatro carriles y los puntos de referencia coinciden con la carretera definida en el objeto drivingScenario proporcionado.

waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; % in meters
refPath = referencePathFrenet(waypoints);
ax = show(refPath);
axis(ax,'equal'); xlabel('X'); ylabel('Y');

Construir generador de trayectoria

Para un planificador local, el objetivo suele ser muestrear una variedad de movimientos posibles que avanzan hacia un objetivo final y al mismo tiempo satisfacen las condiciones cinemáticas y dinámicas actuales. El objeto trajectoryGeneratorFrenet logra esto conectando el estado inicial con un conjunto de estados terminales utilizando trayectorias polinómicas de cuarto o quinto orden. Los estados inicial y terminal se definen en el sistema de coordenadas de Frenet, y cada solución polinómica satisface las condiciones límite de posición lateral y longitudinal, velocidad y aceleración al tiempo que minimiza la sacudida.

Los estados terminales a menudo se calculan mediante comportamientos personalizados. Estos comportamientos aprovechan la información que se encuentra en el entorno local, como la información del carril, el límite de velocidad y las predicciones actuales o futuras de los actores en las proximidades del vehículo ego.

Construya un objeto trajectoryGeneratorFrenet usando la ruta de referencia

connector = trajectoryGeneratorFrenet(refPath);

Construir un verificador de colisiones dinámicas

El objeto dynamicCapsuleList es una estructura de datos que representa el estado de un entorno dinámico en un conjunto discreto de pasos de tiempo. Este entorno puede utilizarse luego para validar de manera eficiente múltiples trayectorias potenciales para el vehículo ego. Cada objeto en la escena está representado por:

• Identificador único con valor entero

• Propiedades de la geometría de una cápsula utilizada para una comprobación eficiente de colisiones

• Secuencia de estados SE2, donde cada estado representa una instantánea discreta en el tiempo.

En este ejemplo, las trayectorias generadas por el objeto trajectoryGeneratorFrenet ocurren durante un período de tiempo, conocido como horizonte temporal. Para garantizar que la verificación de colisiones cubra todas las trayectorias posibles, el objeto dynamicCapsuleList debe contener trayectorias previstas de todos los actores que abarquen el horizonte temporal máximo esperado.

capList = dynamicCapsuleList;

Crea una estructura geométrica para el vehículo ego con los parámetros dados.

egoID = 1;
[egoID, egoGeom] = egoGeometry(capList,egoID);

egoGeom.Geometry.Length = carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters

Agregue el vehículo ego a la lista de cápsulas dinámicas.

updateEgoGeometry(capList,egoID,egoGeom);

Agregue los actores drivingScenario al objeto dynamicCapsuleList . La geometría se establece aquí y los estados se definen durante el ciclo de planificación. Puedes ver que el dynamicCapsuleList ahora contiene un vehículo ego y cinco obstáculos.

actorID = (1:5)';
actorGeom = repelem(egoGeom,5,1);
updateObstacleGeometry(capList,actorID,actorGeom)

ans = 
  dynamicCapsuleList with properties:

     MaxNumSteps: 31
          EgoIDs: 1
     ObstacleIDs: [5x1 double]
    NumObstacles: 5
         NumEgos: 1

Planificación de rutas adaptativas a través del tráfico

Las utilidades de planificación respaldan una estrategia de planificación local que toma muestras de un conjunto de trayectorias locales basadas en el estado actual y previsto del medio ambiente antes de elegir la trayectoria más óptima. El bucle de simulación se ha organizado en las siguientes secciones:

1. Advance the Ground Truth Scenario

2. Generate Terminal States

3. Evaluate Cost of Terminal States

4. Generate Trajectories and Check for Kinematic Feasibility

5. Check Trajectories for Collision and Select the Optimal Trajectory

6. Display the Sampled Trajectories and Animate Scene

Haga clic en los títulos de cada sección para navegar hasta el código relevante en el bucle de simulación.

Advance Ground Truth Scenario

Cuando se planifica en un entorno dinámico, a menudo es necesario estimar el estado del medio ambiente o predecir su estado en el futuro próximo. Para simplificar, este ejemplo utiliza drivingScenario como fuente real de trayectorias para cada actor a lo largo del horizonte de planificación. Para probar un algoritmo de predicción personalizado, puede reemplazar o modificar la función exampleHelperRetrieveActorGroundTruth con código personalizado.

Generate Terminal States

Un objetivo común en la conducción autónoma es garantizar que las trayectorias planificadas no sólo sean factibles sino también naturales. La conducción típica en carretera implica elementos de mantener el carril, mantener el límite de velocidad, cambiar de carril, adaptar la velocidad al tráfico, etc. Cada comportamiento personalizado puede requerir información de entorno diferente. Este ejemplo demuestra cómo generar estados terminales que implementen tres de estos comportamientos: control de crucero, cambios de carril y seguimiento del vehículo líder.

control de crucero

La función exampleHelperBasicCruiseControl genera estados terminales que realizan un comportamiento de control de crucero. Esta función utiliza la velocidad lateral del vehículo ego y un horizonte temporal para predecir el carril esperado N segundos del vehículo ego en el futuro. El centro del carril se calcula y se convierte en la desviación lateral del estado terminal, y la velocidad lateral y la aceleración se establecen en cero.

Para condiciones de contorno longitudinal, la velocidad terminal se establece en el límite de velocidad de la carretera y la aceleración terminal se establece en cero. La posición longitudinal no tiene restricciones y se especifica como NaN. Al eliminar la restricción de longitud, trajectoryGeneratorFrenet puede usar un polinomio de cuarto orden inferior para resolver el problema del valor del límite longitudinal, lo que da como resultado una trayectoria que coincide suavemente con la velocidad de la carretera en el horizonte de tiempo dado:

$$cruiseControlState=\left[\mathrm{NaN }{\dot{s}}_{des} 0 l_{expLane} 0 0\right]$$

cambio de carril

La función exampleHelperBasicLaneChange genera estados terminales que hacen la transición del vehículo del carril actual a cualquier carril adyacente. La función determina primero el carril actual del vehículo ego y luego comprueba si existen los carriles izquierdo y derecho. Para cada carril existente, el estado terminal se define de la misma manera que el comportamiento del control de crucero, con la excepción de que la velocidad terminal se establece en la velocidad actual en lugar del límite de velocidad de la carretera:

$$laneChangeState=\left[\mathrm{NaN}{\dot{s}}_{cur} 0 l_{desLane} 0 0\right]$$

Seguir el vehículo líder

La función exampleHelperBasicLeadVehicleFollow genera estados terminales que intentan rastrear un vehículo que se encuentra delante del vehículo ego. La función determina primero el carril actual del vehículo ego. Para cada timeHorizon proporcionado, la función predice el estado futuro de cada actor, encuentra todos los actores que ocupan el mismo carril que el vehículo ego y determina cuál es el vehículo líder más cercano (si no hay ningún líder). se encuentran vehículos, la función no devuelve nada).

El estado terminal del vehículo ego se calcula tomando la posición y la velocidad del vehículo líder y reduciendo la posición longitudinal terminal en cierta distancia de seguridad:

$$closestLeadVehicl\mathrm{eState}=\left[s_{lead} {\dot{s}}_{lead} 0 l_{lead} {\dot{l}}_{lead} 0\right]$$

$$\mathit{followState}=\left[\left({\mathit{s}}_{\mathit{l}\mathit{e}\mathit{a}\mathit{d}}-{\mathit{d}}_{\mathit{safety}}\right)\mathit{ }{\dot{\mathit{s}}}_{\mathit{l}\mathit{e}\mathit{a}\mathit{d}}\mathit{ }0\mathit{ }{\mathit{l}}_{\mathit{l}\mathit{e}\mathit{a}\mathit{d}}\mathit{ }{\dot{\mathit{l}}}_{\mathit{l}\mathit{e}\mathit{a}\mathit{d}}\mathit{ }0\right]$$

Evaluate Cost of Terminal States

Una vez generados los estados terminales, se puede evaluar su coste. La evaluación de la trayectoria y las formas de priorizar las posibles soluciones es muy subjetiva. En aras de la simplicidad, la función exampleHelperEvaluateTSCost define el coste como la combinación de tres sumas ponderadas.

• Coste de desviación lateral ($C_{latDev}$)—Un peso positivo que penaliza a los estados que se desvían del centro de un carril.

$C_{latDev}=w_{\mathrm{\Delta }L}*\mathrm{\Delta }\mathrm{L}$

$$\mathrm{\Delta }L={\mathrm{argmin}}_{\mathit{i}}\left(|{\mathit{L}}_{\mathrm{termState}}-{\mathit{L}}_{{\mathrm{lane}}_{\mathit{i}}}|\right)$$

• Coste de tiempo ($C_{time}$) —Un peso negativo que prioriza los movimientos que ocurren durante un intervalo más largo, lo que resulta en trayectorias más suaves.

$$C_{time}=w_{\mathrm{\Delta }t}*\mathrm{\Delta }t$$

• Coste de velocidad terminal ($C_{speed}$) — Un peso positivo que prioriza los movimientos que mantienen el límite de velocidad, lo que resulta en maniobras menos dinámicas.

$C_{speed}=w_{\mathrm{\Delta }v}*\mathrm{\Delta }v$

Generate Trajectories and Check for Kinematic Feasibility

Además de tener estados terminales con un coste mínimo, una trayectoria óptima a menudo debe satisfacer restricciones adicionales relacionadas con la viabilidad cinemática y la comodidad de marcha. Las restricciones de trayectoria son una forma de imponer una calidad de conducción deseada, pero lo hacen a expensas de una envolvente de conducción reducida.

En este ejemplo, la función exampleHelperEvaluateTrajectory verifica que cada trayectoria satisfaga las siguientes restricciones:

• MaxAcceleration: La aceleración absoluta a lo largo de la trayectoria debe estar por debajo de un valor especificado. Los valores más pequeños reducen la agresividad de la conducción y eliminan trayectorias cinemáticamente inviables. Esta restricción puede eliminar maniobras que de otro modo podrían ser realizadas por el vehículo.

• MaxCurvature: El radio de giro mínimo permitido a lo largo de una trayectoria. Al igual que con MaxAcceleration, la reducción de este valor da como resultado una experiencia de conducción más suave, pero puede eliminar trayectorias que de otro modo serían factibles.

• MinVelocity: Este ejemplo limita el vehículo ego a moverse únicamente hacia adelante estableciendo una velocidad mínima. Esta restricción es deseable en escenarios de conducción en autopistas y elimina trayectorias que se ajustan a valores límite demasiado restringidos o mal condicionados.

Check Trajectories for Collision and Select Optimal Trajectory

El último paso en el proceso de planificación es elegir la mejor trayectoria que también resulte en una ruta libre de colisiones. La verificación de colisiones a menudo se pospone hasta el final porque es una operación costesa, por lo que al evaluar el coste y analizar las restricciones primero, se pueden eliminar de la consideración las trayectorias no válidas. Luego se puede comprobar la colisión de las trayectorias restantes en el orden óptimo hasta que se encuentre una trayectoria libre de colisiones o se hayan evaluado todas las trayectorias.

Definir los parámetros del simulador y la planificación

Esta sección define las propiedades necesarias para ejecutar el simulador y los parámetros que utilizan el planificador y las utilidades de comportamiento. Propiedades como scenario.SampleTime and connector.TimeResolution están sincronizadas de modo que los estados en las trayectorias del actor de ground-truth y las trayectorias planificadas del ego ocurren en los mismos pasos de tiempo. De manera similar, replanRate, timeHorizons y maxHorizon se eligen de manera que sean múltiplos enteros de la tasa de simulación.

Como se mencionó en la sección anterior, los pesos y las restricciones se seleccionan para promover trayectorias de conducción suaves y al mismo tiempo cumplir con las reglas de la carretera.

Por último, defina los parámetros speedLimit y safetyGap , que se utilizan para generar estados terminales para el planificador.

% Synchronize the simulator's update rate to match the trajectory generator's
% discretization interval.
scenario.SampleTime = connector.TimeResolution; % in seconds

% Define planning parameters.
replanRate = 10; % Hz

% Define the time intervals between current and planned states.
timeHorizons = 1:3; % in seconds
maxHorizon = max(timeHorizons); % in seconds

% Define cost parameters.
latDevWeight    =  1;
timeWeight      = -1;
speedWeight     =  1;

% Reject trajectories that violate the following constraints.
maxAcceleration =  15; % in meters/second^2
maxCurvature    =   1; % 1/meters, or radians/meter
minVelocity     =   0; % in meters/second

% Desired velocity setpoint, used by the cruise control behavior and when
% evaluating the cost of trajectories.
speedLimit = 11; % in meters/second

% Minimum distance the planner should target for following behavior.
safetyGap = 10; % in meters

Inicializar simulador

Inicialice el simulador y cree un visor chasePlot (Automated Driving Toolbox) .

[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
    exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen);

Ejecutar simulación de conducción

tic
while isRunning
    % Retrieve the current state of actor vehicles and their trajectory over
    % the planning horizon.
    [curActorState,futureTrajectory,isRunning] = ...
        exampleHelperRetrieveActorGroundTruth(scenario,futureTrajectory,replanRate,maxHorizon);

    % Generate cruise control states.
    [termStatesCC,timesCC] = exampleHelperBasicCruiseControl(...
        refPath,laneWidth,egoState,speedLimit,timeHorizons);
    
    % Generate lane change states.
    [termStatesLC,timesLC] = exampleHelperBasicLaneChange(...
        refPath,laneWidth,egoState,timeHorizons);
    
    % Generate vehicle following states.
    [termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(...
        refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);

    % Combine the terminal states and times.
    allTS = [termStatesCC; termStatesLC; termStatesF];
    allDT = [timesCC; timesLC; timesF];
    numTS = [numel(timesCC); numel(timesLC); numel(timesF)];
    
    % Evaluate cost of all terminal states.
    costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit,...
        speedWeight, latDevWeight, timeWeight);

    % Generate trajectories.
    egoFrenetState = global2frenet(refPath,egoState);
    [frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT);
    
    % Eliminate trajectories that violate constraints.
    isValid = exampleHelperEvaluateTrajectory(globalTraj,maxAcceleration,maxCurvature,minVelocity);

    % Update the collision checker with the predicted trajectories
    % of all actors in the scene.
    for i = 1:numel(actorPoses)
        actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3);
    end
    updateObstaclePose(capList,actorID,actorPoses);
    
    % Determine evaluation order.
    [cost, idx] = sort(costTS);
    optimalTrajectory = [];
    
    trajectoryEvaluation = nan(numel(isValid),1);
    
    % Check each trajectory for collisions starting with least cost.
    for i = 1:numel(idx)
        if isValid(idx(i))
            % Update capsule list with the ego object's candidate trajectory.
            egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3);
            updateEgoPose(capList,egoID,egoPoses);
            
            % Check for collisions.
            isColliding = checkCollision(capList);
            
            if all(~isColliding)
                % If no collisions are found, this is the optimal.
                % trajectory.
                trajectoryEvaluation(idx(i)) = 1;
                optimalTrajectory = globalTraj(idx(i)).Trajectory;
                break;
            else
                trajectoryEvaluation(idx(i)) = 0;
            end
        end
    end

    % Display the sampled trajectories.
    lineHandles = exampleHelperVisualizeScene(lineHandles,globalTraj,isValid,trajectoryEvaluation);
    
    hold on;
    show(capList,'TimeStep',1:capList.MaxNumSteps,'FastUpdate',1);
    hold off;
    
    if isempty(optimalTrajectory)
        % All trajectories either violated a constraint or resulted in collision.
        %
        %   If planning failed immediately, revisit the simulator, planner,
        %   and behavior properties.
        %
        %   If the planner failed midway through a simulation, additional
        %   behaviors can be introduced to handle more complicated planning conditions.
        error('No valid trajectory has been found.');
    else
        % Visualize the scene between replanning.
        for i = (2+(0:(stepPerUpdate-1)))
            % Approximate realtime visualization.
            dt = toc;
            if scenario.SampleTime-dt > 0
                pause(scenario.SampleTime-dt);
            end
            
            egoState = optimalTrajectory(i,:);
            scenarioViewer.Actors(1).Position(1:2) = egoState(1:2);
            scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);
            scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi;
            scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
            
            % Update capsule visualization.
            hold on;
            show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);
            hold off;
            
            % Update driving scenario.
            advance(scenarioViewer);
            tic;
        end
    end
end

Personalizaciones del planificador y consideraciones adicionales

Las soluciones personalizadas a menudo implican muchos parámetros ajustables, cada uno de ellos capaz de cambiar el comportamiento final de maneras que son difíciles de predecir. Esta sección destaca algunas de las propiedades específicas de las funciones y su efecto en el planificador anterior. Luego, las sugerencias brindan formas de ajustar o aumentar la lógica personalizada.

Lista de cápsulas dinámicas

Como se mencionó anteriormente, el objeto dynamicCapsuleList actúa como una base de datos temporal, que almacena en caché las trayectorias previstas de los obstáculos. Puedes realizar una comprobación de colisión con uno o más cuerpos del ego durante un período de tiempo. La propiedad MaxNumSteps determina el número total de pasos de tiempo que verifica el objeto. En el bucle de simulación anterior, la propiedad se estableció en 31. Este valor significa que el planificador verifica el lapso completo de 1 a 3 segundos de cualquier trayectoria (muestreado cada 0,1 segundo). Ahora, aumente el valor máximo en timeHorizons:

timeHorizons   = 1:5; % in seconds
maxTimeHorizon = max(timeHorizons); % in seconds

Ahora hay dos opciones:

1. La propiedad MaxNumSteps no se modifica.

2. La propiedad MaxNumSteps se actualiza para adaptarse al nuevo período de tiempo máximo.

Si la propiedad no se modifica, entonces la lista de cápsulas solo valida los primeros 3 segundos de cualquier trayectoria, lo que puede ser preferible si la eficiencia computacional es primordial o si la certeza de la predicción disminuye rápidamente.

Alternativamente, se puede estar trabajando con datos reales (como se muestra arriba), o se puede conocer bien el estado futuro del entorno (por ejemplo, un entorno totalmente automatizado con control centralizado). Dado que este ejemplo utiliza datos reales para los actores, actualice la propiedad.

capList.MaxNumSteps = 1+floor(maxTimeHorizon/scenario.SampleTime);

Otra propiedad de la lista que se puede ajustar indirectamente es la geometría de la cápsula. La geometría del vehículo ego o de los actores se puede inflar aumentando Radius, y se pueden agregar regiones de amortiguamiento a los vehículos modificando Length y FixedTransform propiedades.

Infla toda la superficie del vehículo ego aumentando el radio.

egoGeom.Geometry.Radius = laneWidth/2; % in meters
updateEgoGeometry(capList,egoID,egoGeom);

Agregue una región de amortiguación delantera y trasera a todos los actores.

actorGeom(1).Geometry.Length = carLen*1.5; % in meters
actorGeom(1).Geometry.FixedTransform(1,end) = -actorGeom(1).Geometry.Length*rearAxleRatio; % in meters
actorGeom = repmat(actorGeom(1),5,1);
updateObstacleGeometry(capList,actorID,actorGeom);

Vuelva a ejecutar la simulación con propiedades actualizadas

Vuelva a ejecutar la simulación. La simulación resultante tiene algunos desarrollos interesantes:

• El horizonte temporal más largo de cinco segundos da como resultado una experiencia de conducción mucho más fluida. El planificador aún prioriza las trayectorias más largas debido al negativo timeWeight.

• La propiedad MaxNumSteps actualizada ha permitido la verificación de colisiones en toda la trayectoria. Cuando se combina con el horizonte de planificación más largo, el planificador identifica y descarta el cambio de carril izquierdo previamente óptimo y regresa al carril original.

• Las cápsulas infladas detectan antes una colisión y rechazan una trayectoria, lo que da como resultado un comportamiento de conducción más conservador. Un posible inconveniente de esto es una envolvente de planificación reducida, que corre el riesgo de que el planificador no pueda encontrar una trayectoria válida.

% Initialize the simulator and create a chasePlot viewer.
[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
    exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen);
tic;
while isRunning
    % Retrieve the current state of actor vehicles and their trajectory over
    % the planning horizon.
    [curActorState,futureTrajectory,isRunning] = exampleHelperRetrieveActorGroundTruth(...
        scenario,futureTrajectory,replanRate,maxHorizon);
    
    % Generate cruise control states.
    [termStatesCC,timesCC] = exampleHelperBasicCruiseControl(...
        refPath,laneWidth,egoState,speedLimit,timeHorizons);
    
    % Generate lane change states.
    [termStatesLC,timesLC] = exampleHelperBasicLaneChange(...
        refPath,laneWidth,egoState,timeHorizons);
    
    % Generate vehicle following states.
    [termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(...
        refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);
    
    % Combine the terminal states and times.
    allTS = [termStatesCC; termStatesLC; termStatesF];
    allDT = [timesCC; timesLC; timesF];
    numTS = [numel(timesCC); numel(timesLC); numel(timesF)];
    
    % Evaluate cost of all terminal states.
    costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit, ...
        speedWeight,latDevWeight,timeWeight);
    
    % Generate trajectories.
    egoFrenetState = global2frenet(refPath,egoState);
    [frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT);
    
    % Eliminate trajectories that violate constraints.
    isValid = exampleHelperEvaluateTrajectory(...
        globalTraj, maxAcceleration, maxCurvature, minVelocity);
    
    % Update the collision checker with the predicted trajectories
    % of all actors in the scene.
    for i = 1:numel(actorPoses)
        actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3);
    end
    updateObstaclePose(capList, actorID, actorPoses);
    
    % Determine evaluation order.
    [cost, idx] = sort(costTS);
    optimalTrajectory = [];
    
    trajectoryEvaluation = nan(numel(isValid),1);
    
    % Check each trajectory for collisions starting with least cost.
    for i = 1:numel(idx)
        if isValid(idx(i))
            % Update capsule list with the ego object's candidate trajectory.
            egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3);
            updateEgoPose(capList, egoID, egoPoses);
            
            % Check for collisions.
            isColliding = checkCollision(capList);
            
            if all(~isColliding)
                % If no collisions are found, this is the optimal
                % trajectory.
                trajectoryEvaluation(idx(i)) = 1;
                optimalTrajectory = globalTraj(idx(i)).Trajectory;
                break;
            else
                trajectoryEvaluation(idx(i)) = 0;
            end
        end
    end
    
    % Display the sampled trajectories.
    lineHandles = exampleHelperVisualizeScene(lineHandles, globalTraj, isValid, trajectoryEvaluation);
    
    if isempty(optimalTrajectory)
        % All trajectories either violated a constraint or resulted in collision.
        %
        %   If planning failed immediately, revisit the simulator, planner,
        %   and behavior properties.
        %
        %   If the planner failed midway through a simulation, additional
        %   behaviors can be introduced to handle more complicated planning conditions.
        error('No valid trajectory has been found.');
    else
        % Visualize the scene between replanning.
        for i = (2+(0:(stepPerUpdate-1)))
            % Approximate realtime visualization.
            dt = toc;
            if scenario.SampleTime-dt > 0
                pause(scenario.SampleTime-dt);
            end
            
            egoState = optimalTrajectory(i,:);
            scenarioViewer.Actors(1).Position(1:2) = egoState(1:2);
            scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);
            scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi;
            scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
            
            % Update capsule visualization.
            hold on;
            show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);
            hold off;
            
            % Update driving scenario.
            advance(scenarioViewer);
            tic;
        end
    end
end