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 movimiento en entornos urbanos utilizando un mapa de cuadrícula de ocupación dinámica

Este ejemplo utiliza:

Este ejemplo le muestra cómo realizar una replanificación dinámica en una escena de conducción urbana utilizando una ruta de referencia de Frenet. En este ejemplo, se utiliza una estimación de un mapa de cuadrícula de ocupación dinámica del entorno local para encontrar trayectorias locales óptimas.

Introducción

La replanificación dinámica para vehículos autónomos normalmente se realiza con un planificador de movimiento local. El planificador de movimiento local es responsable de generar una trayectoria óptima basada en el plan global y la información sobre el entorno circundante. La información sobre el entorno circundante se puede describir principalmente de dos maneras:

Conjunto discreto de objetos del entorno circundante con geometrías definidas.
Cuadrícula discretizada con estimación de regiones libres y ocupadas en el entorno circundante.

En presencia de obstáculos dinámicos en el entorno, un planificador de movimiento local requiere predicciones a corto plazo de la información sobre el entorno para evaluar la validez de las trayectorias planificadas. La elección de la representación del entorno suele estar gobernada por el algoritmo de percepción ascendente. Para los algoritmos de planificación, la representación basada en objetos ofrece una descripción del entorno que ahorra memoria. También permite una forma más sencilla de definir relaciones entre objetos para la predicción de comportamiento. Por otro lado, un enfoque basado en cuadrículas permite una representación sin modelo de objetos, lo que ayuda a comprobar de manera eficiente las colisiones en escenarios complejos con una gran cantidad de objetos. La representación basada en cuadrículas también es menos sensible a las imperfecciones de la extracción de objetos, como objetivos falsos y perdidos. También es posible un híbrido de estos dos enfoques extrayendo hipótesis de objetos de la representación basada en cuadrículas.

En este ejemplo, representará el entorno circundante como un mapa de cuadrícula de ocupación dinámica. Para ver un ejemplo que utiliza el conjunto discreto de objetos, consulte el ejemplo Planificación de la trayectoria de la carretera mediante la ruta de referencia de Frenet . Un mapa de cuadrícula de ocupación dinámica es una estimación basada en cuadrícula del entorno local alrededor del vehículo ego. Además de estimar la probabilidad de ocupación, la cuadrícula de ocupación dinámica también estima los atributos cinemáticos de cada celda, como la velocidad, la velocidad de giro y la aceleración. Además, las estimaciones de la red dinámica se pueden predecir para un corto período de tiempo en el futuro para evaluar la ocupación del entorno local en el futuro cercano. En este ejemplo, se obtiene la estimación del entorno basada en cuadrícula fusionando nubes de puntos de seis lidars montados en el vehículo ego.

Configurar escenario y rastreador basado en cuadrícula

El escenario utilizado en este ejemplo representa una escena de intersección urbana y contiene una variedad de objetos, incluidos peatones, ciclistas, automóviles y camiones. El vehículo ego está equipado con seis sensores lidar homogéneos, cada uno con un campo de visión de 90 grados, lo que proporciona una cobertura de 360 grados alrededor del vehículo ego. Para obtener más detalles sobre el escenario y los modelos de sensores, consulte el ejemplo Grid-Based Tracking in Urban Environments Using Multiple Lidars (Sensor Fusion and Tracking Toolbox) . La definición de escenario y sensores está incluida en la función auxiliar helperGridBasedPlanningScenario.

% For reproducible results
rng(2020);

% Create scenario, ego vehicle and simulated lidar sensors
[scenario, egoVehicle, lidars] = helperGridBasedPlanningScenario;

Ahora, defina un rastreador basado en cuadrícula utilizando el objeto trackerGridRFS (Sensor Fusion and Tracking Toolbox) System object™. El rastreador genera una estimación del entorno tanto a nivel de objeto como a nivel de cuadrícula. La estimación a nivel de cuadrícula describe la ocupación y el estado del entorno local y se puede obtener como el cuarto resultado del rastreador. Para obtener más detalles sobre cómo configurar un rastreador basado en cuadrícula, consulte el ejemplo Grid-Based Tracking in Urban Environments Using Multiple Lidars (Sensor Fusion and Tracking Toolbox) .

% Set up sensor configurations for each lidar
sensorConfigs = cell(numel(lidars),1);

% Fill in sensor configurations
for i = 1:numel(sensorConfigs)
    sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle);
end

% Set up tracker
tracker = trackerGridRFS('SensorConfigurations',sensorConfigs,...
    'HasSensorConfigurationsInput',true,...
    'GridLength',120,...
    'GridWidth',120,...
    'GridResolution',2,...
    'GridOriginInLocal',[-60 -60],...
    'NumParticles',1e5,...
    'NumBirthParticles',2e4,...
    'VelocityLimits',[-15 15;-15 15],...
    'BirthProbability',0.025,...
    'ProcessNoise',5*eye(2),...
    'DeathRate',1e-3,...
    'FreeSpaceDiscountFactor',1e-2,...
    'AssignmentThreshold',8,...
    'MinNumCellsPerCluster',4,...
    'ClusteringThreshold',4,...
    'ConfirmationThreshold',[3 4],...
    'DeletionThreshold',[4 4]);

Configurar el planificador de movimiento

Configure un algoritmo de planificación de movimiento local para planificar trayectorias óptimas en coordenadas Frenet a lo largo de una ruta de referencia global.

Defina la ruta de referencia global utilizando el objeto referencePathFrenet proporcionando los puntos de referencia en el marco de coordenadas cartesianas del escenario de conducción. La ruta de referencia utilizada en este ejemplo define una ruta que gira a la derecha en la intersección.

waypoints = [-110.6 -4.5 0;
            49 -4.5 0;
            55.5 -17.7 -pi/2;
            55.5 -130.6 -pi/2]; % [x y theta]

% Create a reference path using waypoints
refPath = referencePathFrenet(waypoints);

% Visualize the reference path
fig = figure('Units','normalized','Position',[0.1 0.1 0.8 0.8]);
ax = axes(fig);
hold(ax,'on');
plot(scenario,'Parent',ax);
show(refPath,'Parent',ax);
xlim(ax,[-120 80]);
ylim(ax,[-160 40]);

snapnow;

El algoritmo de planificación de movimiento local en este ejemplo consta de tres pasos principales:

Ejemplos de trayectorias locales
Encuentre trayectorias factibles y libres de colisiones
Elija el criterio de optimización y seleccione la trayectoria óptima.

Las siguientes secciones analizan cada paso del algoritmo de planificación local y las funciones auxiliares utilizadas para ejecutar cada paso.

Muestras de trayectorias locales

En cada paso de la simulación, el algoritmo de planificación genera una lista de trayectorias de muestra que el vehículo ego puede elegir. Las trayectorias locales se muestrean conectando el estado actual del vehículo ego con los estados terminales deseados. Utilice el objeto trajectoryGeneratorFrenet para conectar los estados actual y terminal para generar trayectorias locales. Defina el objeto proporcionando la ruta de referencia y la resolución deseada en el tiempo para la trayectoria. El objeto conecta estados iniciales y finales en coordenadas de Frenet utilizando polinomios de quinto orden.

connector = trajectoryGeneratorFrenet(refPath,'TimeResolution',0.1);

La estrategia para muestrear estados terminales en coordenadas de Frenet a menudo depende de la red de carreteras y del comportamiento deseado del vehículo ego durante las diferentes fases de la ruta global. Para obtener ejemplos más detallados del uso de diferentes comportamientos del ego, como el control de crucero y el seguimiento de automóviles, consulte la sección "Planificación de rutas adaptativas a través del tráfico" del ejemplo Planificación de la trayectoria de la carretera mediante la ruta de referencia de Frenet . En este ejemplo, muestra los estados de la terminal utilizando dos estrategias diferentes, según la ubicación del vehículo en la ruta de referencia, que se muestran como regiones azul y verde en la siguiente figura.

% Visualize path regions for sampling strategy visualization
pathPoints = closestPoint(refPath, refPath.Waypoints(:,1:2));
roadS = pathPoints(:,end);
intersectionS = roadS(2,end);
intersectionBuffer = 20;
pathGreen = [interpolate(refPath,linspace(0,intersectionS-intersectionBuffer,20));...
            nan(1,6);...
            interpolate(refPath,linspace(intersectionS,roadS(end),100))];
pathBlue = interpolate(refPath,linspace(intersectionS-intersectionBuffer,roadS(2,end),20));
hold(ax,'on');
plot(ax,pathGreen(:,1),pathGreen(:,2),'Color',[0 1 0],'LineWidth',5);
plot(ax,pathBlue(:,1),pathBlue(:,2),'Color',[0 0 1],'LineWidth',5);

snapnow;

Cuando el vehículo ego está en la región verde, se utiliza la siguiente estrategia para muestrear trayectorias locales. El estado terminal del vehículo ego después $Δ T$ el tiempo se define como:

$x_{Ego} (Δ T) = [NaN \dot{s} 0 d 0 0];$

donde las muestras discretas para variables se obtienen utilizando los siguientes conjuntos predefinidos:

${Δ T \in {linspace (2, 4, 6)}, \dot{s} \in {linspace (0, {\dot{s}}_{\max,} 10)}, d \in {0 w_{lane}}}$

El uso de NaN en el estado terminal permite que el objeto trajectoryGeneratorFrenet calcule automáticamente la distancia longitudinal recorrida en una trayectoria de sacudida mínima. Esta estrategia produce un conjunto de trayectorias que permiten al vehículo ego acelerar hasta el límite máximo de velocidad ( ${\dot{s}}_{\max}$ ) velocidades o desacelerar hasta detenerse por completo a diferentes velocidades. Además, las opciones muestradas de desplazamiento lateral ( $d_{des}$ ) permiten que el vehículo ego cambie de carril durante estas maniobras.

% Define smax and wlane
speedLimit = 15;
laneWidth = 2.975;

Cuando el vehículo ego está en la región azul de la trayectoria, se utiliza la siguiente estrategia para muestrear trayectorias locales:

$x_{Ego} (Δ T) = [s_{stop} 0 0 0 0 0]$ ;

dónde $Δ T$ Se elige para minimizar el tirón durante la trayectoria. Esta estrategia permite que el vehículo se detenga a la distancia deseada ( $s_{stop}$ ) en el carril derecho con una trayectoria de mínima sacudida. El algoritmo de muestreo de trayectoria está incluido dentro de la función auxiliar, helperGenerateTrajectory, adjunta a este ejemplo.

Encontrar trayectorias factibles y libres de colisiones

El proceso de muestreo descrito en la sección anterior puede producir trayectorias que son cinemáticamente inviables y exceden los umbrales de atributos cinemáticos como la aceleración y la curvatura. Por lo tanto, limitas la aceleración y velocidad máximas del vehículo ego utilizando la función auxiliar helperKinematicFeasibility, que verifica la viabilidad de cada trayectoria frente a estas restricciones cinemáticas.

% Define kinematic constraints
accMax = 15;

Además, configura un validador de colisiones para evaluar si el vehículo ego puede maniobrar en una trayectoria cinemáticamente factible sin chocar con ningún otro obstáculo en el entorno. Para definir el validador, utilice la clase auxiliar HelperDynamicMapValidator. Esta clase utiliza la función predictMapToTime (Sensor Fusion and Tracking Toolbox) del objeto trackerGridRFS para obtener predicciones a corto plazo de la ocupación del entorno circundante. Dado que la incertidumbre en la estimación aumenta con el tiempo, configure el validador con un horizonte temporal máximo de 2 segundos.

La ocupación prevista del entorno se convierte en un mapa de costes inflado en cada paso para tener en cuenta el tamaño del vehículo ego. El planificador de ruta utiliza un paso de tiempo de 0,1 segundos con un horizonte temporal de predicción de 2 segundos. Para reducir la complejidad computacional, se supone que la ocupación del entorno circundante es válida durante 5 pasos de tiempo, o 0,5 segundos. Como resultado, sólo se requieren 4 predicciones en el horizonte de planificación de 2 segundos. Además de tomar decisiones binarias sobre colisión o no colisión, el validador también proporciona una medida de la probabilidad de colisión del vehículo ego. Esta probabilidad se puede incorporar a la función de costes para que los criterios de optimización tengan en cuenta la incertidumbre en el sistema y tomen mejores decisiones sin aumentar el horizonte temporal del planificador.

vehDims = vehicleDimensions(egoVehicle.Length,egoVehicle.Width);
collisionValidator = HelperDynamicMapValidator('MaxTimeHorizon',2, ... % Maximum horizon for validation
    'TimeResolution',connector.TimeResolution, ... % Time steps between trajectory samples
    'Tracker',tracker, ... % Provide tracker for prediction
    'ValidPredictionSpan',5, ... % Prediction valid for 5 steps
    'VehicleDimensions',vehDims); % Provide dimensions of ego

Elegir criterio de optimización

Después de validar las trayectorias factibles frente a obstáculos o regiones ocupadas del entorno, elija un criterio de optimización para cada trayectoria válida definiendo una función de coste para las trayectorias. Se espera que diferentes funciones de costes produzcan diferentes comportamientos en el vehículo ego. En este ejemplo, usted define el coste de cada trayectoria como

$C = J_{s} + J_{d} + 1000 P_{c} + 100 {({\dot{s}}_{(Δ T)} - {\dot{s}}_{Limit})}^{2}$

donde:

$J_{s}$ es el tirón en la dirección longitudinal de la trayectoria de referencia

$J_{d}$ es el tirón en la dirección lateral de la trayectoria de referencia

$P_{c}$ es la probabilidad de colisión obtenida por el validador

El cálculo del coste para cada trayectoria se define utilizando la función auxiliar helperCalculateTrajectoryCosts. De la lista de trayectorias válidas, la trayectoria con el coste mínimo se considera la trayectoria óptima.

Ejecutar escenario, estimar mapa dinámico y planificar trayectorias locales

Ejecute el escenario, genere nubes de puntos a partir de todos los sensores lidar y estime el mapa de cuadrícula de ocupación dinámica. Utilice la estimación del mapa dinámico y sus predicciones para planificar una trayectoria local para el vehículo ego.

% Close original figure and initialize a new display
close(fig);
display = helperGridBasedPlanningDisplay;

% Initial ego state
currentEgoState = [-110.6 -1.5 0 0 15 0];
helperMoveEgoVehicleToState(egoVehicle, currentEgoState);

% Initialize pointCloud outputs from each sensor
ptClouds = cell(numel(lidars),1);
sensorConfigs = cell(numel(lidars),1);

% Simulation Loop
while advance(scenario)
    % Current simulation time
    time = scenario.SimulationTime;
    
    % Poses of objects with respect to ego vehicle
    tgtPoses = targetPoses(egoVehicle);
    
    % Simulate point cloud from each sensor
    for i = 1:numel(lidars)
        [ptClouds{i}, isValidTime] = step(lidars{i},tgtPoses,time);
        sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle);
    end
    
    % Pack point clouds as sensor data format required by the tracker
    sensorData = packAsSensorData(ptClouds,sensorConfigs,time);
    
    % Call the tracker
    [tracks, ~, ~, map] = tracker(sensorData,sensorConfigs,time);
    
    % Update validator's future predictions using current estimate
    step(collisionValidator, currentEgoState, map, time);
    
    % Sample trajectories using current ego state and some kinematic
    % parameters
    [frenetTrajectories, globalTrajectories] = helperGenerateTrajectory(connector, refPath, currentEgoState, speedLimit, laneWidth, intersectionS, intersectionBuffer);
    
    % Calculate kinematic feasibility of generated trajectories
    isKinematicsFeasible = helperKinematicFeasibility(frenetTrajectories,speedLimit,accMax);
    
    % Calculate collision validity of feasible trajectories
    feasibleGlobalTrajectories = globalTrajectories(isKinematicsFeasible);
    feasibleFrenetTrajectories = frenetTrajectories(isKinematicsFeasible);
    [isCollisionFree, collisionProb] = isTrajectoryValid(collisionValidator, feasibleGlobalTrajectories);
    
    % Calculate costs and final optimal trajectory
    nonCollidingGlobalTrajectories = feasibleGlobalTrajectories(isCollisionFree);
    nonCollidingFrenetTrajectories = feasibleFrenetTrajectories(isCollisionFree);
    nonCollodingCollisionProb = collisionProb(isCollisionFree);
    costs = helperCalculateTrajectoryCosts(nonCollidingFrenetTrajectories, nonCollodingCollisionProb, speedLimit);
    
    % Find optimal trajectory
    [~,idx] = min(costs);
    optimalTrajectory = nonCollidingGlobalTrajectories(idx);
    
    % Assemble for plotting
    trajectories = helperAssembleTrajectoryForPlotting(globalTrajectories, ...
        isKinematicsFeasible, isCollisionFree, idx);
    
    % Update display
    display(scenario, egoVehicle, lidars, ptClouds, tracker, tracks, trajectories, collisionValidator);
    
    % Move ego with optimal trajectory
    if ~isempty(optimalTrajectory)
        currentEgoState = optimalTrajectory.Trajectory(2,:);
        helperMoveEgoVehicleToState(egoVehicle, currentEgoState);
    else
        % All trajectories either violated kinematic feasibility
        % constraints or resulted in a collision. More behaviors on
        % trajectory sampling may be needed.
        error('Unable to compute optimal trajectory');
    end
end

Resultados

Analice los resultados del algoritmo de planificación de rutas locales y cómo las predicciones del mapa ayudaron al planificador. Esta animación muestra el resultado del algoritmo de planificación durante todo el escenario. Observe que el vehículo ego alcanzó con éxito su destino deseado y maniobró alrededor de diferentes objetos dinámicos, siempre que fue necesario. El vehículo ego también se detuvo en la intersección debido a los cambios regionales agregados a la política de muestreo.

A continuación, analice el algoritmo de planificación local durante el primer cambio de carril. Las instantáneas de esta sección se capturan en un tiempo = 4,3 segundos durante la simulación.

En esta instantánea, el vehículo ego acaba de comenzar a realizar una maniobra de cambio de carril hacia el carril derecho.

showSnaps(display, 3, 1);

La instantánea que sigue muestra la estimación de la cuadrícula dinámica en el mismo paso de tiempo. El color de la celda de la cuadrícula indica la dirección del movimiento del objeto que ocupa esa celda de la cuadrícula. Observe que las celdas que representan el automóvil frente al vehículo ego están coloreadas en rojo, lo que indica que están ocupadas con un objeto dinámico. Además, el automóvil se mueve en la dirección X positiva del escenario, por lo que, según la rueda de colores, el color de las celdas de la cuadrícula correspondientes es rojo.

f = showSnaps(display, 2, 1);
if ~isempty(f)
    ax = findall(f,'Type','Axes');
    ax.XLim = [0 40];
    ax.YLim = [-20 20];
    s = findall(ax,'Type','Surf');
    s.XData = 36 + 1/3*(s.XData - mean(s.XData(:)));
    s.YData = 16 + 1/3*(s.YData - mean(s.YData(:)));
end

Según la imagen anterior, la trayectoria planificada del vehículo ego pasa a través de las regiones ocupadas del espacio, lo que representa una colisión si se realiza una validación de ocupación estática tradicional. Sin embargo, el mapa de ocupación dinámica y el validador tienen en cuenta la naturaleza dinámica de la cuadrícula al validar el estado de la trayectoria con la ocupación prevista en cada paso de tiempo. La siguiente instantánea muestra el mapa de costes previsto en diferentes pasos de predicción ( $Δ T$ ), junto con la posición planificada del vehículo ego en la trayectoria. El mapa de costes previsto está inflado para tener en cuenta el tamaño del vehículo ego. Por lo tanto, si un objeto puntual que representa el origen del vehículo ego puede colocarse en el mapa de ocupación sin ninguna colisión, se puede interpretar que el vehículo ego no choca con ningún obstáculo. Las regiones amarillas en el mapa de costes indican áreas con colisiones garantizadas con un obstáculo. La probabilidad de colisión decae exponencialmente fuera de las regiones amarillas hasta el final de la región de inflación. Las regiones azules indican áreas con probabilidad cero de colisión según la predicción actual.

Observe que la región amarilla que representa el automóvil frente al vehículo ego avanza en el mapa de costes tal como se predice el mapa en el futuro. Esto refleja que la predicción de ocupación considera la velocidad de los objetos en el entorno circundante. Además, observe que las celdas clasificadas como objetos estáticos permanecieron relativamente estáticas en la cuadrícula durante la predicción. Por último, observe que la posición planificada del origen del vehículo ego no choca con ninguna región ocupada en el mapa de costes. Esto muestra que el vehículo ego puede maniobrar con éxito en esta trayectoria.

f = showSnaps(display, 1, 1);
if ~isempty(f)
ax = findall(f,'Type','Axes');
for i = 1:numel(ax)
    ax(i).XLim = [0 40];
    ax(i).YLim = [-20 20];
end
end

Resumen

En este ejemplo, aprendió a utilizar las predicciones del mapa dinámico del rastreador basado en cuadrícula, trackerGridRFS, y a integrar el mapa dinámico con un algoritmo de planificación de ruta local para generar trayectorias para el vehículo ego. en entornos dinámicos y complejos. También aprendió cómo se puede utilizar la naturaleza dinámica de la ocupación para planificar trayectorias de manera más eficiente en el entorno.

Funciones de apoyo

function sensorData = packAsSensorData(ptCloud, configs, time)
% Pack the sensor data as format required by the tracker
%
% ptCloud - cell array of pointCloud object
% configs - cell array of sensor configurations
% time    - Current simulation time

%The lidar simulation returns outputs as pointCloud objects. The Location
%property of the point cloud is used to extract x,y, and z locations of
%returns and pack them as structures with information required by a tracker.
sensorData = struct('SensorIndex',{},...
    'Time', {},...
    'Measurement', {},...
    'MeasurementParameters', {});

for i = 1:numel(ptCloud)
    % This sensor's point cloud
    thisPtCloud = ptCloud{i};
    
    % Allows mapping between data and configurations without forcing an
    % ordered input and requiring configuration input for static sensors.
    sensorData(i).SensorIndex = configs{i}.SensorIndex;
    
    % Current time
    sensorData(i).Time = time;
    
    % Exctract Measurement as a 3-by-N defining locations of points
    sensorData(i).Measurement = reshape(thisPtCloud.Location,[],3)';
    
    % Data is reported in the sensor coordinate frame and hence measurement
    % parameters are same as sensor transform parameters.
    sensorData(i).MeasurementParameters = configs{i}.SensorTransformParameters;
end

end

function config = helperGetLidarConfig(lidar, ego)
% Get configuration of the lidar sensor for tracker
%
% config - Configuration of the lidar sensor in the world frame
% lidar - lidarPointCloudGeneration object
% ego   - driving.scenario.Actor in the scenario

% Define transformation from sensor to ego
senToEgo = struct('Frame',fusionCoordinateFrameType(1),...
    'OriginPosition',[lidar.SensorLocation(:);lidar.Height],...
    'Orientation',rotmat(quaternion([lidar.Yaw lidar.Pitch lidar.Roll],'eulerd','ZYX','frame'),'frame'),...
    'IsParentToChild',true);

% Define transformation from ego to tracking coordinates
egoToScenario = struct('Frame',fusionCoordinateFrameType(1),...
    'OriginPosition',ego.Position(:),...
    'Orientation',rotmat(quaternion([ego.Yaw ego.Pitch ego.Roll],'eulerd','ZYX','frame'),'frame'),...
    'IsParentToChild',true);

% Assemble using trackingSensorConfiguration.
config = trackingSensorConfiguration(...
    'SensorIndex',lidar.SensorIndex,...
    'IsValidTime', true,...
    'SensorLimits',[lidar.AzimuthLimits;0 lidar.MaxRange],...
    'SensorTransformParameters',[senToEgo;egoToScenario],...
    'DetectionProbability',0.95);

end

function helperMoveEgoVehicleToState(egoVehicle, currentEgoState)
% Move ego vehicle in scenario to a state calculated by the planner
% 
% egoVehicle - driving.scenario.Actor in the scenario
% currentEgoState - [x y theta kappa speed acc]

% Set 2-D Position
egoVehicle.Position(1:2) = currentEgoState(1:2);

% Set 2-D Velocity (s*cos(yaw) s*sin(yaw))
egoVehicle.Velocity(1:2) = [cos(currentEgoState(3)) sin(currentEgoState(3))]*currentEgoState(5);

% Set Yaw in degrees
egoVehicle.Yaw = currentEgoState(3)*180/pi;

% Set angular velocity in Z (yaw rate) as v/r
egoVehicle.AngularVelocity(3) = currentEgoState(4)*currentEgoState(5);

end

function isFeasible = helperKinematicFeasibility(frenetTrajectories, speedLimit, aMax)
% Check kinematic feasibility of trajectories
%
% frenetTrajectories - Array of trajectories in Frenet coordinates
% speedLimit - Speed limit (m/s)
% aMax - Maximum acceleration (m/s^2)

isFeasible = false(numel(frenetTrajectories),1);
for i = 1:numel(frenetTrajectories)
    % Speed of the trajectory
    speed = frenetTrajectories(i).Trajectory(:,2);
    
    % Acceleration of the trajectory
    acc = frenetTrajectories(i).Trajectory(:,3);
    
    % Is speed valid?
    isSpeedValid = ~any(speed < -0.1 | speed > speedLimit + 1);
    
    % Is acceleration valid?
    isAccelerationValid = ~any(abs(acc) > aMax);
    
    % Trajectory feasible if both speed and acc valid
    isFeasible(i) = isSpeedValid & isAccelerationValid;
end

end

function cost = helperCalculateTrajectoryCosts(frenetTrajectories, Pc, smax)
% Calculate cost for each trajectory.
%
% frenetTrajectories - Array of trajectories in Frenet coordinates
% Pc - Probability of collision for each trajectory calculated by validator

n = numel(frenetTrajectories);
Jd = zeros(n,1);
Js = zeros(n,1);
s = zeros(n,1);

for i = 1:n
    % Time
    time = frenetTrajectories(i).Times;
    
    % resolution
    dT = time(2) - time(1);
    
    % Jerk along the path
    dds = frenetTrajectories(i).Trajectory(:,3);
    Js(i) = sum(gradient(dds,time).^2)*dT;
    
    % Jerk perpendicular to path
    % d2L/dt2 = d/dt(dL/ds*ds/dt)
    ds = frenetTrajectories(i).Trajectory(:,2);
    ddL = frenetTrajectories(i).Trajectory(:,6).*(ds.^2) + frenetTrajectories(i).Trajectory(:,5).*dds;
    Jd(i) = sum(gradient(ddL,time).^2)*dT;
    
    s(i) = frenetTrajectories(i).Trajectory(end,2);
end

cost = Js + Jd + 1000*Pc(:) + 100*(s - smax).^2;

end