Введение

Динамическое планирование для автономных транспортных средств обычно выполняется с помощью локального планировщика движения. Локальный планировщик движения отвечает за формирование оптимальной траектории на основе глобального плана и информации об окружающей среде. Информация об окружающей среде может быть описана в основном двумя способами:

При наличии динамических препятствий в окружающей среде локальному планировщику движения требуется краткосрочное прогнозирование информации об окружении для оценки достоверности планируемых траекторий. Выбор представления среды обычно определяется алгоритмом восприятия восходящего потока. Для алгоритмов планирования представление на основе объектов обеспечивает эффективное с точки зрения памяти описание среды. Это также позволяет упростить определение межобъектных отношений для прогнозирования поведения. С другой стороны, подход, основанный на сетке, допускает представление без объектной модели, что способствует эффективной проверке коллизий в сложных сценариях с большим количеством объектов. Представление на основе сетки также менее чувствительно к несовершенствам извлечения объектов, таким как ложные и пропущенные цели. Гибрид этих двух подходов также возможен путем извлечения гипотезы объекта из представления на основе сетки.

В этом примере окружающая среда представляется как динамическая сетка занятости. Пример использования дискретного набора объектов см. в примере «Планирование траектории магистрали с использованием ссылочного пути Frenet» (панель инструментов навигации). Динамическая сетка занятости представляет собой основанную на сетке оценку локальной среды вокруг эго-транспортного средства. В дополнение к оценке вероятности занятости динамическая сетка занятости также оценивает кинематические атрибуты каждой ячейки, такие как скорость, скорость поворота и ускорение. Кроме того, оценки по динамической сетке можно прогнозировать на короткое время в будущем для оценки заполненности местной окружающей среды в ближайшем будущем. В этом примере можно получить оценку среды на основе сетки путем сплавления точечных облаков из шести лидаров, установленных на эго-транспортном средстве.

Настройка сценария и Grid-Based Tracker

Сценарий, используемый в этом примере, представляет сцену пересечения городов и содержит множество объектов, включая пешеходов, велосипедистов, легковые и грузовые автомобили. Эго-транспортное средство оснащено шестью однородными лидарными датчиками, каждый с полем зрения 90 градусов, обеспечивающими 360-градусное покрытие вокруг эго-транспортного средства. Дополнительные сведения о сценариях и моделях датчиков см. в примере «Отслеживание на основе сетки в городской среде с использованием нескольких лидаров». Определение сценария и датчиков заключено в функцию помощника helperGridBasedPlanningScenario.

% For reproducible results
rng(2020);

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

Теперь определите трекер на основе сетки с помощью trackerGridRFS object™ системы. Трекер выводит оценку среды как на уровне объекта, так и на уровне сетки. Оценка уровня сетки описывает занятость и состояние локальной среды и может быть получена в качестве четвертого выходного сигнала от трекера. Дополнительные сведения о настройке отслеживания на основе сетки см. в примере Отслеживание на основе сетки в городских средах с использованием нескольких лидаров.

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

Настройка планировщика движения

Настройте локальный алгоритм планирования движения для планирования оптимальных траекторий в координатах Френета вдоль глобальной опорной траектории.

Определите глобальный путь ссылки с помощью referencePathFrenet(Панель инструментов навигации) путем предоставления ППМ в декартовой координатной рамке ведущего сценария. Опорная траектория, используемая в этом примере, определяет траекторию, которая поворачивается прямо на пересечении.

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;

Алгоритм локального планирования движения в этом примере состоит из трех основных шагов:

В следующих разделах рассматриваются каждый шаг локального алгоритма планирования и вспомогательные функции, используемые для выполнения каждого шага.

Образец локальных траекторий

На каждом этапе моделирования алгоритм планирования генерирует список выборок траекторий, которые может выбрать эго-транспортное средство. Локальные траектории отбираются путем соединения текущего состояния эго-транспортного средства с требуемыми терминальными состояниями. Используйте trajectoryGeneratorFrenetОбъект (Панель инструментов навигации) для соединения текущего и конечного состояний для создания локальных траекторий. Определите объект, указав траекторию привязки и требуемое разрешение по времени для траектории. Объект соединяет начальное и конечное состояния в координатах Френета с помощью многочленов пятого порядка.

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

Стратегия выборки терминальных состояний в координатах Френета часто зависит от дорожной сети и желаемого поведения эго-транспортного средства на различных фазах глобального пути. Для получения более подробных примеров использования различных эго-моделей, таких как круиз-контроль и слежение за автомобилем, см. раздел «Планирование адаптивных маршрутов через движение» в примере «Планирование траектории шоссе с использованием эталонного пути Frenet» (панель инструментов навигации). В этом примере выполняется выборка состояний терминала с использованием двух различных стратегий, в зависимости от местоположения транспортного средства на опорном пути, показанном синими и зелеными областями на следующем рисунке.

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

Когда эго-транспортное средство находится в зеленой области, для выборки локальных траекторий используется следующая стратегия. Терминальное состояние эго-транспортного средства после $\mathit{\Delta T}$ времени определяется как:

где дискретные выборки для переменных получают с использованием следующих предварительно определенных наборов:

Использование NaN в состоянии терминала включает trajectoryGeneratorFrenet объект для автоматического вычисления продольного расстояния, пройденного по траектории минимального рывка. Эта стратегия создает набор траекторий, которые позволяют эго-транспортному средству ускоряться до максимальной скорости (${\stackrel{}{\mathit{}}}_{\mathrm{s\dot{}max}}$) или замедляться до полной остановки при различных скоростях. Кроме того, выбранные варианты бокового смещения (${\mathit{}}_{\mathrm{ddes}}$) позволяют эго-транспортному средству изменять полосы движения во время этих маневров.

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

Когда эго-транспортное средство находится в синей области траектории, для выборки локальных траекторий используется следующая стратегия:

${\mathit{}}_{\mathrm{xEgo}\text{(}}\mathrm{\Delta T}\mathit{)}={\mathit{[}}_{\mathrm{}}\text{stop}\mathrm{}\text{0}\mathrm{}\text{0}\mathrm{}\text{0}\mathrm{}\text{0}\mathrm{}0$ ];

где $\mathit{\Delta T}$ выбирается для минимизации рывка во время траектории. Эта стратегия позволяет транспортному средству останавливаться на требуемом расстоянии (${\mathit{}}_{\mathrm{stop}}$) в правой полосе с минимальной траекторией рывка. Алгоритм выборки траектории заключен в вспомогательную функцию, helperGenerateTrajectory, прилагается с этим примером.

Поиск осуществимых и свободных от столкновений траекторий

Процесс выборки, описанный в предыдущем разделе, может создавать траектории, которые являются кинематически неосуществимыми и превышают пороги кинематических атрибутов, таких как ускорение и кривизна. Поэтому максимальное ускорение и скорость эго-транспортного средства можно ограничить с помощью вспомогательной функции helperKinematicEcasibility, которая проверяет выполнимость каждой траектории с учетом этих кинематических ограничений.

% Define kinematic constraints
accMax = 15;

Кроме того, вы настроили валидатор столкновения, чтобы оценить, может ли эго-транспортное средство маневрировать на кинематически возможной траектории, не сталкиваясь с какими-либо другими препятствиями в окружающей среде. Для определения средства проверки используйте вспомогательный класс HelperDynamicMapValidator. Этот класс использует predictMapToTime функции trackerGridRFS изобретение позволяет получить краткосрочные прогнозы заполняемости окружающей среды. Поскольку неопределенность в оценке увеличивается со временем, настройте средство проверки с максимальным временным горизонтом 2 секунды.

Прогнозируемая заполняемость окружающей среды преобразуется в завышенную карту затрат на каждом этапе для учета размера эго-транспортного средства. Планировщик тракта использует временной интервал 0,1 секунды с временным горизонтом прогнозирования 2 секунды. Для уменьшения вычислительной сложности предполагается, что заполняемость окружающей среды действительна в течение 5 временных шагов или 0,5 секунды. В результате в 2-секундном горизонте планирования требуется только 4 прогноза. В дополнение к принятию двоичных решений о столкновении или отсутствии столкновения, валидатор также предоставляет меру вероятности столкновения эго-транспортного средства. Эта вероятность может быть включена в функцию затрат для критериев оптимальности для учета неопределенности в системе и принятия лучших решений без увеличения временного горизонта плановика.

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

Выбор критерия оптимальности

После проверки возможных траекторий относительно препятствий или занятых областей окружающей среды выберите критерий оптимальности для каждой допустимой траектории путем определения функции затрат для траекторий. Ожидается, что различные функции затрат будут создавать различные модели поведения для эго-транспортного средства. В этом примере стоимость каждой траектории определяется как

где:

${\mathit{}}_{\mathit{Js}}$ - рывок в продольном направлении траектории отсчета;

${\mathit{}}_{\mathit{Jd}}$ - рывок в поперечном направлении опорной траектории;

${\mathit{}}_{\mathit{Pc}}$ - вероятность столкновения, полученная валидатором.

Расчет затрат для каждой траектории определяется с помощью вспомогательной функции helperCalculateTrajectureCostes. Из списка допустимых траекторий в качестве оптимальной траектории рассматривается траектория с минимальной стоимостью.

Выполнение сценария, оценка динамической карты и планирование локальных траекторий

Выполните сценарий, создайте облака точек из всех лидарных датчиков и оцените динамическую сетку занятости. Используйте динамическую оценку карты и ее прогнозы для планирования локальной траектории для эго-транспортного средства.

% 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

Результаты

Проанализируйте результаты из алгоритма планирования локального тракта и то, как прогнозы из карты помогли плановику. Эта анимация показывает результат алгоритма планирования в течение всего сценария. Обратите внимание, что эго-автомобиль успешно достиг нужного пункта назначения и при необходимости маневрировал вокруг различных динамических объектов. Эго-транспортное средство также вышло на остановку на перекрестке из-за региональных изменений, добавленных в политику отбора проб.

Затем проанализируйте алгоритм локального планирования во время первого изменения полосы. Снимки в этом разделе регистрируются в течение времени = 4,3 секунды во время моделирования.

В этом снимке эго-транспортное средство только что начало выполнять маневр смены полосы движения в правую полосу.

showSnaps(display, 3, 1);

На следующем снимке отображается оценка динамической сетки в один и тот же шаг. Цвет ячейки сетки обозначает направление движения объекта, занимающего эту ячейку сетки. Обратите внимание, что ячейки, представляющие автомобиль перед эго-транспортным средством, окрашены в красный цвет, что означает, что ячейки заняты динамическим объектом. Также автомобиль движется в положительном X направлении сценария, поэтому на основе цветового колеса цвет соответствующих ячеек сетки красный.

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

На основе предыдущего изображения планируемая траектория эго-транспортного средства проходит через занятые области пространства, представляя собой столкновение, если выполняется традиционная статическая проверка занятости. Динамическая карта занятости и средство проверки достоверности, однако, учитывают динамический характер сетки путем проверки состояния траектории относительно прогнозируемой занятости на каждом временном шаге. Следующий снимок показывает прогнозируемую карту затрат на различных этапах прогнозирования ($\mathit{\Delta T}$) вместе с запланированным положением эго-транспортного средства на траектории. Прогнозируемая карта затрат раздувается для учета размера эго-транспортного средства. Следовательно, если точечный объект, представляющий начало координат эго-транспортного средства, может быть помещен на карту занятости без какого-либо столкновения, можно интерпретировать, что эго-транспортное средство не сталкивается с каким-либо препятствием. Желтые области на costmap обозначают участки с гарантированными столкновениями с препятствием. Вероятность столкновения распадается вне желтых областей экспоненциально до конца области инфляции. Синие области указывают области с нулевой вероятностью столкновения согласно текущему прогнозу.

Обратите внимание, что желтая область, представляющая автомобиль перед эго-автомобилем, движется вперед по карте затрат, как и карта, предсказанная в будущем. Это отражает то, что при прогнозировании занятости учитывается скорость объектов в окружающей среде. Также обратите внимание, что клетки, классифицированные как статические объекты, оставались относительно статичными на сетке во время прогнозирования. Наконец, обратите внимание на то, что планируемая позиция происхождения эго-транспортного средства не сталкивается ни с какими оккупированными регионами на карте затрат. Это показывает, что эго-транспортное средство может успешно маневрировать на этой траектории.

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

Резюме

В этом примере вы научились использовать предсказания динамической карты от трекера на основе сетки, trackerGridRFSи как интегрировать динамическую карту с локальным алгоритмом планирования пути для генерации траекторий для эго-транспортного средства в динамических сложных средах. Вы также узнали, как динамический характер занятости можно использовать для более эффективного планирования траекторий в среде.

Вспомогательные функции

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