trajectoryOptimalFrenet

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

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

Описание

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

plan функция вычисляет оптимальную траекторию между start и терминальные состояния. Функциональные выборки несколько траекторий для каждой пары состояний и выбирают выполнимую траекторию с наименьшим количеством стоимостью.

Создание

Синтаксис

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

Описание

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

пример

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

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

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

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

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

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

stateValidator объект, заданный как 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, для каждой траектории и возвращают величину затрат как скаляр. Подпись функции стоимости:

cost = CostFunction(TRAJSTATES)

plan функция возвращает путь с самой низкой ценой.

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

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

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

  • Trajectory

  • 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, которые действуют как ссылка для планирования альтернативных траекторий, оптимизированных этим планировщиком.

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

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

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

Примеры

свернуть все

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

Скрипт Open Live Script

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

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

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

stateValidator = validatorOccupancyMap;

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

grid = zeros(700,700);
grid(296:305,150:175) = 1;
grid(286:295,300:325) = 1;
grid(306:315,300:325) = 1;
grid(296:305,450:475) = 1;
grid(286:295,600:625) = 1;
grid(306:315,600:625) = 1;

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

map = binaryOccupancyMap(grid);

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

stateValidator.Map = map;

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

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

refPath = [10,400;700,400];

Объявите, что указатель функции стоимости приоритизирует оставленный изменения маршрута.

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

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

plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

Присвойте боковые значения отклонения.

plannerObj.TerminalStates.Lateral = -10:10:10;

Генерация траектории

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

previousDeviationValue = 0;
deviationValue = 0;
initState = zeros(1,6);

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

for j = 1:150
    
    % Deviation from the current reference path
    if initState(4) > 5
        deviationValue = 10;
    elseif initState(4) < -5
        deviationValue = -10;
    end
    
    % Move reference path to the current lane
    if previousDeviationValue ~= deviationValue
        % Shift the waypoints by the deviation value
        plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue];
        
        % Shift the terminal states such that they remain fixed in world frame
        plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue;
        
        % Store the deviation value
        previousDeviationValue = deviationValue;
        
        % Update initState variable with the new planner object
        initState = cart2frenet(plannerObj,trajectory(10,1:6));
    end
    
    % Generate a trajectory from initState
    trajectory = plan(plannerObj,initState);
    
    % Use 10th state of the current trajectory as a starting point for
    % replanning
    initState = cart2frenet(plannerObj,trajectory(10,1:6));
    
    % Visualize every 5th iteration
    if mod(j,5)==0
        show(map)
        hold on;
        show(plannerObj,'TrajectoryColor','none');
        drawnow
    end
end

Ограничения

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

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

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

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

Ссылки

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

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

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

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

|

Введенный в R2019b

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