trajectoryOptimalFrenet

Нахождение оптимальной траектории вдоль опорного пути

развернуть все на странице

Описание

The trajectoryOptimalFrenet объект является планировщиком пути, который выборки и оценивают локальные траектории на основе ссылки пути. Планировщик генерирует набор конечных состояний на основе пути ссылки и других параметров объекта. Затем планировщик соединяет состояние с каждым конечным состоянием, используя 4th или 5th Порядок полиномов. Чтобы выбрать оптимальный путь, выбранные траектории оцениваются на кинематическую осуществимость, столкновение и стоимость.

Создание

Синтаксис

trajectoryOptimalFrenet(refPath,validator)
planner = trajectoryOptimalFrenet(___,Name,Value)

Описание

trajectoryOptimalFrenet(refPath,validator) создает trajectoryOptimalFrenet объект со ссылочным путем, refPath, в виде n -на-2 массива [x y] путевые точки и валидатор состояния, validator, заданный как validatorOccupancyMap объект.

пример

planner = trajectoryOptimalFrenet(___,Name,Value) устанавливает дополнительные свойства с помощью одной или нескольких пар "имя-значение" в любом порядке.

Входные параметры

расширить все

Ссылочный путь, заданный как матрица n -by-2 [x y] пар, где n количество путевых точек.

Пример: [100 100; 400 400]

Типы данных: double

Объект валидатора состояний, заданный как validatorOccupancyMap объект.

Свойства

расширить все

Примечание

Для 'Weights' и 'FeasibilityParameters' свойства, вы не можете задать все структуры сразу. Вместо этого установите их поля по отдельности как пары "имя-значение". Для примера, trajectoryOptimalFrenet(refPath,validator,'Deviation',0) устанавливает 'Deviation' поле структуры 'Weights'.

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

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

Типы данных: double

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

Типы данных: double

Функция стоимости умножает вес на интеграл бокового рывка в квадрате. Это значение определяет агрессивность траектории в боковом направлении (перпендикулярно ссылке пути). Задайте это значение как разделенную разделенными запятой парами 'LateralSmoothness' и положительная скалярная величина. Чтобы оштрафовать боковой рывок в запланированной траектории, увеличьте это значение затрат.

Типы данных: double

Функция стоимости умножает вес на интеграл продольного рывка в квадрате. Это значение определяет агрессивность траекторий в продольном направлении (направлении опорного пути). Задайте это значение как разделенную разделенными запятой парами 'LongitudinalSmoothness' и положительная скалярная величина. Чтобы наказать большие изменения в прямом и обратном ускорении, увеличьте это значение затрат.

Типы данных: double

Функция стоимости умножает вес на перпендикулярное расстояние от пути ссылки в конце траектории в метрах. Задайте это значение как разделенную разделенными запятой парами 'Deviation' и положительная скалярная величина в метрах.

Типы данных: double

Типы данных: struct

Параметры выполнимости, заданные как структура, содержащая скалярные значения для проверки валидности траектории. Структура имеет эти поля.

Максимальная кривизна, которую может выполнить транспортное средство. Задайте это значение как разделенную разделенными запятой парами 'MaxCurvature' и положительный действительный скаляр в m-1. Это значение определяет кинематическую допустимость траектории.

Типы данных: double

Максимальное ускорение в направлении движения транспортного средства. Задайте это значение как разделенную разделенными запятой парами 'MaxAcceleration' и положительный действительный скаляр в м/с2. Чтобы снизить предел ускорения транспортного средства в прямом или обратном направлении, уменьшите это значение.

Типы данных: double

Типы данных: struct

Временной интервал между дискретизированными состояниями траектории. Задайте это значение как разделенную разделенными запятой парами 'TimeResolution' и положительный действительный скаляр в секундах. Эти дискретизированные состояния определяют валидность состояния и функцию затрат.

Типы данных: double

Определяемая пользователем функция затрат, заданная как указатель на функцию. Функция должна принять матрицу n -на-7 состояний, TRAJSTATES, для каждой траектории и возвращает значение стоимости в виде скаляра. plan функция возвращает путь с самой низкой стоимостью.

Для примера, leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10) создает указатель на функцию затрат для определения приоритетов изменений в левой полосе.

Типы данных: function handle

Это свойство доступно только для чтения.

The 'TrajectoryList' свойство, возвращенное как массив структур из всех траекторий кандидатов и их соответствующих параметров. Каждая структура имеет следующие поля:

  • Trajectory - матрица n -by-7 [x, y, theta, kappa, speed, acceleration, time], где n - количество траекторных точек пути.

  • Cost - Стоимость траектории.

  • MaxAcceleration - Максимальное ускорение траектории.

  • MaxCurvature - Максимальная кривизна траектории.

  • Feasible - Вектор с четырьмя элементами [velocity, acceleration, curvature, collision] указывает валидность траектории.

    Значение элементов может быть либо,

    • 1 - Траектория действительна.

    • 0 - Недопустимая траектория.

    • -1 - Траектория не проверяется.

Типы данных: struct

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

Длины сегмента траектории, заданные в виде вектора в метрах.

Типы данных: double

Массив отклонений от опорного пути в перпендикулярном направлении в состоянии цели, заданный как вектор в метрах.

Типы данных: double

Скорость в состоянии цели в направлении движения, заданная как положительная скалярная величина в м/с.

Типы данных: double

Ускорение в состоянии цели в направлении движения, заданное как положительная скалярная величина в м/с2.

Типы данных: double

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

Типы данных: double

Типы данных: struct

Точки пути опорного пути, заданные как матрица n -by-2 [x y] пар, где n количество путевых точек. Waypoints действует как ссылка для планирования альтернативных траекторий, оптимизированных этим планировщиком.

Типы данных: double

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

Для примера, 'NumSegments',2 создает две перегородки между каждым продольным конечным состоянием. Траектории генерируются, чтобы достичь промежуточных продольных состояний со всеми доступными боковыми конечными состояниями.

Типы данных: double

Смещение отклонения от траектории ссылки в боковом направлении. Задайте это значение как разделенную разделенными запятой парами 'DeviationOffset' и скаляром. Отрицательное значение смещает отклонение вправо, и положительное значение смещает отклонение налево от пути ссылки в боковом направлении. Установите это свойство, чтобы смещать ваше решение в определенное направление поворота, избегая препятствий в ссылку пути.

Типы данных: double

Функции объекта

cart2frenetПреобразуйте Декартовы состояния в состояния Френе
copyСоздайте глубокую копию объекта
frenet2cartПреобразуйте состояния Френе в Декартовы состояния
planПланируйте оптимальную траекторию
showВизуализация траекторий

Примеры

свернуть все

Открыть Live Script

В этом примере показано, как спланировать оптимальную траекторию с помощью trajectoryOptimalFrenet объект.

Создайте и назначьте карту для валидатора состояний

Создайте объект средства проверки состояния для проверки столкновения.

stateValidator = validatorOccupancyMap;

Создайте сетчатую карту препятствий.

grid = zeros(50,100);
grid(24:26,48:53) = 1;

Создайте binaryOccupancyMap с сетчатой картой.

map = binaryOccupancyMap(grid);

Присвойте карту валидатору состояния.

stateValidator.Map = map;

Планирование и визуализация траектории

Создайте ссылку пути для последующего планирования.

refPath = [0,25;100,25];

Инициализируйте объект планировщика с ссылка пути и средства проверки состояния.

planner = trajectoryOptimalFrenet(refPath,stateValidator);

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

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -10:5:10;
planner.FeasibilityParameters.MaxAcceleration = 10;

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

planner.DeviationOffset = 5;

Планирование траектории

Начальное декартовое состояние транспортного средства.

initCartState = [0 25 pi/9 0 0 0];

Преобразуйте декартовое состояние транспортного средства в состояние Френе.

initFrenetState = cart2frenet(planner,initCartState);

Планируйте траекторию от начального состояния Френе.

plan(planner,initFrenetState);

Визуализация траектории

Визуализируйте карту и траектории.

show(map)
hold on
show(planner,'Trajectory','all')

Figure contains an axes. The axes with title Binary Occupancy Grid contains 5 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory.

Открыть Live Script

Этот пример показывает, как разбить продольные конечные состояния в оптимальном планировании траектории с помощью trajectoryOptimalFrenet объект.

Создайте и назначьте карту для валидатора состояний

Создайте объект средства проверки состояния для проверки столкновения.

statevalidator = validatorOccupancyMap;

Создайте сетчатую карту препятствий.

grid = zeros(50,100);
grid(25:27,28:33) = 1;
grid(16:18,37:42) = 1;
grid(29:31,72:77) = 1;

Создайте binaryOccupancyMap с сетчатой картой.

map = binaryOccupancyMap(grid);

Присвойте карту валидатору состояния.

statevalidator.Map = map;

Планирование и визуализация траектории

Создайте ссылку пути для последующего планирования. 

refPath = [0,25;30,30;75,20;100,25];

Инициализируйте объект планировщика с ссылка пути и средства проверки состояния.

planner = trajectoryOptimalFrenet(refPath,statevalidator);

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

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -5:5:5;
planner.FeasibilityParameters.MaxAcceleration = 10;

Назначьте количество перегородок для продольного конечного состояния.

planner.NumSegments = 3;

Планирование траектории

Начальное состояние транспортного средства.

initFrenetState = zeros(1,6);

Планируйте траекторию от начального состояния Френе.

plan(planner,initFrenetState);

Визуализация траектории

Визуализируйте карту и траектории.

show(map)
hold on
show(planner,'Trajectory','all')
hold on

Сгенерируйте контуры маршрута

Вычислите конец ссылки пути как состояние Френе.

refPathEnd = cart2frenet(planner,[planner.Waypoints(end,:) 0 0 0 0]);

Вычислите смещения маршрута по обеим сторонам боковых конечных состояний со значением ширины половины маршрута.

laneOffsets = unique([planner.TerminalStates.Lateral+2.5 planner.TerminalStates.Lateral-2.5]);

Вычислите положения полос в Декартовом состоянии.

numLaneOffsets = numel(laneOffsets);
xRefPathEnd = ceil(refPathEnd(1));
laneXY = zeros((numLaneOffsets*xRefPathEnd)+numLaneOffsets,2);
xIndex = 0;

for laneID = 1:numLaneOffsets
    for x = 1:xRefPathEnd
        laneCart = frenet2cart(planner,[x 0 0 laneOffsets(laneID) 0 0]);
        xIndex = xIndex + 1;
        laneXY(xIndex,:) = laneCart(1:2);
    end
    xIndex = xIndex + 1;
    laneXY(xIndex,:) = NaN(1,2);
end

Постройте контуры маршрута.

plot(laneXY(:,1),laneXY(:,2),'LineWidth',0.5,'Color',[0.5 0.5 0.5],'DisplayName','Lane Boundaries','LineStyle','--')

Figure contains an axes. The axes with title Binary Occupancy Grid contains 6 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory, Lane Boundaries.

Ограничения

  • Самопересечения в пути ссылки могут привести к неожиданному поведению.

  • Плановик не поддерживает реверс вождения.

  • Первоначальная ориентация на планирование должна быть в пределах -pi/2 и pi/2 к ссылке пути.

  • Ограничьте количество TerminalStates для приложений реального времени, так как вычислительная сложность увеличивается вместе с ним.

Ссылки

[1] Верлинг, Мориц, Юлиус Циглер, Сёрен Каммель и Себастьян Трун. «Оптимальная генерация траектории для динамических уличных сценариев на французской Системе координат». Международная конференция IEEE по робототехнике и автоматизации 2010. 2010, стр 987–993.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

.

См. также

| |

Введенный в R2019b

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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте