Движение, планирующее в городских средах Используя динамическую карту сетки заполнения

Этот пример использует:

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

Введение

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

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

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

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

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

Сценарий, используемый в этом примере, представляет городскую перекрестную сцену и содержит множество объектов, включая пешеходов, велосипедистов, автомобили и грузовики. Автомобиль, оборудованный датчиком оборудован шестью однородными датчиками лидара, каждым с полем зрения 90 градусов, предоставив полную страховую защиту вокруг автомобиля, оборудованного датчиком. Для получения дополнительной информации о сценарии и моделях датчика, отошлите к Основанному на сетке Отслеживанию в Городских Средах Используя Несколько Лидаров (Sensor Fusion and Tracking Toolbox) пример. Определение сценария и датчиков перенесено в функцию помощника helperGridBasedPlanningScenario.

% For reproducible results
rng(2020);

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

Теперь задайте основанное на сетке средство отслеживания с помощью trackerGridRFS (Sensor Fusion and Tracking Toolbox) Система object™. Средство отслеживания выходные параметры и уровень объектов и оценка уровня сети среды. Оценка уровня сети описывает заполнение и состояние окружения и может быть получена как четвертый выход из средства отслеживания. Для получения дополнительной информации о том, как настроить основанное на сетке средство отслеживания, отошлите к Основанному на сетке Отслеживанию в Городских Средах Используя Несколько Лидаров (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]);

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

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

Задайте глобальный ссылочный путь с помощью referencePathFrenet объект путем обеспечения waypoints в системе координат Декартовой координаты ведущего сценария. Ссылочный путь, используемый в этом примере, задает путь, который поворачивает направо на пересечении.

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 возразите, чтобы соединиться текущий и конечные состояния для генерации локальных траекторий. Задайте объект путем обеспечения ссылочного пути и нужного разрешения как раз к траектории. Объект соединяет начальные и конечные состояния в координатах Frenet с помощью полиномов пятого порядка.

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

Стратегия выборки конечных состояний в координатах Frenet часто зависит от дорожной сети и желаемого поведения автомобиля, оборудованного датчиком во время различных фаз глобального пути. Для более подробных примеров использования различного поведения эго, таких как круиз-контроль и следование автомобиля, относятся к разделу "Planning Adaptive Routes Through Traffic" Магистрального Планирования Траектории Используя пример Пути к Ссылке 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;

Когда автомобиль, оборудованный датчиком находится в зеленой области, следующая стратегия используется, чтобы произвести локальные траектории. Конечное состояние автомобиля, оборудованного датчиком после $Δ T$ время задано как:

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

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

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

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

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

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

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

где $Δ T$ выбран, чтобы минимизировать толчок во время траектории. Эта стратегия позволяет транспортному средству остановиться на желаемом расстоянии ( $s_{stop}$ ) в правильном маршруте с траекторией минимального толчка. Алгоритм выборки траектории перенесен в функции помощника, helperGenerateTrajectory, присоединенный с этим примером.

Нахождение выполнимых и траекторий без коллизий

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

% Define kinematic constraints
accMax = 15;

Далее, вы настраиваете блок проверки допустимости столкновения, чтобы оценить, если автомобиль, оборудованный датчиком может маневрировать на кинематическим образом выполнимой траектории, не сталкиваясь ни с какими другими препятствиями в среде. Чтобы задать блок проверки допустимости, используйте класс помощника HelperDynamicMapValidator. Этот класс использует predictMapToTime (Sensor Fusion and Tracking Toolbox) функция trackerGridRFS объект получить краткосрочные предсказания заполнения окружающей среды. Начиная с неопределенности в оценочных увеличениях со временем сконфигурируйте блок проверки допустимости период времени имеющий 2 секунд.

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

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

Выберите Optimality Criterion

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

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

где:

$J_{s}$ толчок в продольном направлении ссылочного пути

$J_{d}$ толчок в боковом направлении ссылочного пути

$P_{c}$ вероятность столкновения, полученная блоком проверки допустимости

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

Запустите сценарий, оцените динамическую карту и запланируйте локальные траектории

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

% 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

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

Заметьте, что желтая область, представляющая автомобиль перед автомобилем, оборудованным датчиком, продвигается на 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

Документация