Планирование движения в городских окружениях с использованием динамической сетки заполнения

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

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

Введение

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

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

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

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

Настройте сценарий и основанный на сетке трекер

Сценарий, используемый в этом примере, представляет сцену городского перекрестка и содержит множество объектов, включая пешеходов, велосипедистов, легковые автомобили и грузовики. Автомобиль , оборудованный датчиком оборудован шестью однородными датчиками лидара, каждый из которых имеет поле зрения 90 степеней, обеспечивая 360-градусное покрытие вокруг автомобиля , оборудованного датчиком. Для получения дополнительной информации о сценарии и моделях датчиков см. Пример отслеживания на основе сетки в городских окружениях с использованием нескольких лидаров. Определение сценария и датчиков обернуто в функцию helper 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 Объект (Navigation Toolbox) путем задания путевых точек в Декартовой координатной системе координат сценария вождения. Ссылочный путь, используемый в этом примере, задает путь, который поворачивается вправо на пересечении.

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

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

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

% 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}$ ) в правой полосе с траекторией с минимальным рывком. Алгоритм выборки траектории заворачивается внутрь функции helper, helperGenerateTrajectory, прилагаемый к этому примеру.

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

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

% Define kinematic constraints
accMax = 15;

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

Предсказанная заполненность окружения преобразуется в надутую косметику на каждом шаге с учетом размера автомобиля , оборудованного датчиком. Планировщик пути использует временной интервал 0,1 секунды с временным горизонтом предсказания 2 секунды. Чтобы уменьшить вычислительную сложность, заполнение окружающего окружения принято действительным в течение 5 временных шагов или 0,5 секунды. В результате в двухсекундном горизонте планирования требуются только 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

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

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

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

где:

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

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

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

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

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

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

% 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

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

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

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

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