trajectoryOptimalFrenet

Найдите оптимальную траекторию вдоль ссылочного пути

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

Описание

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

Создание

Синтаксис

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

Описание

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

пример

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

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

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

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

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

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

Объект блока проверки допустимости состояния в виде validatorOccupancyMap объект.

Свойства

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

Примечание

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  • Feasible — Четырехэлементный векторный [velocity, acceleration, curvature, collision] указание на валидность траектории.

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

    • 1 — Траектория допустима.

    • 0 — Траектория недопустима.

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

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

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

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

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

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

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

Скорость в целевом состоянии в направлении движения в виде положительной скалярной величины в m/s.

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

Ускорение в целевом состоянии в направлении движения в виде положительной скалярной величины в m/s2.

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

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

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

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

Скрипт Open Live Script

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

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

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

stateValidator = validatorOccupancyMap;

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

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

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

map = binaryOccupancyMap(grid);

Присвойте карту и границы состояния к блоку проверки допустимости состояния.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Запланируйте и визуализируйте траекторию

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

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

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

initFrenetState = cart2frenet(planner,initCartState);

Запланируйте траекторию от начального состояния Frenet.

plan(planner,initFrenetState);

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

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

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

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

Скрипт Open 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;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Запланируйте и визуализируйте траекторию

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

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;

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

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

initFrenetState = zeros(1,6);

Запланируйте траекторию от начального состояния Frenet.

plan(planner,initFrenetState);

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

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

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

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

Вычислите конец ссылочного пути, как Frenet утверждает.

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 object. The axes object 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] Werling, Мориц, Юлиус Циглер, Серен Каммель и Себастиан Трун. "Оптимальная Генерация Траектории для Сценариев Динэмик-Стрит в Системе координат Frenet". 2 010 Международных конференций IEEE по вопросам Робототехники и Автоматизации. 2010, стр 987–993.

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

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

Смотрите также

| |

Введенный в R2019b

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

Поддержка

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