Подключение начального и терминального состояний Frenet
frenetTrajectory = connect(connectorFrenet,initialState,terminalState,timeSpan)
[___, возвращает траектории в глобальных координатах в дополнение ко всем аргументам предыдущего синтаксиса.globalTrajectory] = connect(___)
Создайте альтернативные траектории для траектории привязки с помощью координат Frenet. Укажите различные начальное и конечное состояния для траекторий. Настройте состояния на основе сгенерированных траекторий.
Создание опорного пути из набора ППМ. Создать trajectoryGeneratorFrenet объект из пути ссылки.
waypoints = [0 0; ... 50 20; ... 100 0; ... 150 10]; refPath = referencePathFrenet(waypoints); connector = trajectoryGeneratorFrenet(refPath);
Создайте пятисекундную траекторию между началом траектории и точкой на 30 м вниз по траектории, как указывает Френет.
initState = [0 0 0 0 0 0]; % [S ds ddS L dL ddL] termState = [30 0 0 0 0 0]; % [S ds ddS L dL ddL] [~,trajGlobal] = connect(connector,initState,termState,5);
Отображение траектории в глобальных координатах.
show(refPath); hold on axis equal plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b') legend(["Waypoints","Reference Path","Trajectory to 30m"])
Создайте матрицу концевых состояний с боковыми отклонениями от -3 м до 3 м. Создайте траектории, охватывающие одну и ту же длину дуги за 10 секунд, но отклоняющиеся в боковом направлении от опорной траектории. Просмотрите новые альтернативные пути.
termStateDeviated = termState + ([-3:3]' * [0 0 0 1 0 0]); [~,trajGlobal] = connect(connector,initState,termStateDeviated,10); clf show(refPath); hold on axis equal for i = 1:length(trajGlobal) plot(trajGlobal(i).Trajectory(:,1),trajGlobal(i).Trajectory(:,2),'g') end legend(["Waypoints","Reference Path","Alternative Trajectories"]) hold off
Укажите новое состояние клеммы для создания новой траектории. Эта траектория нежелательна, поскольку для достижения поперечной скорости 10 м/с требуется обратное движение.
newTermState = [5 10 0 5 0 0]; [~,newTrajGlobal] = connect(connector,initState,newTermState,3); clf show(refPath); hold on axis equal plot(newTrajGlobal.Trajectory(:,1),newTrajGlobal.Trajectory(:,2),'b'); legend(["Waypoint","Reference Path","New Trajectory"]) hold off
Смягчить ограничение продольного состояния путем задания длины дуги NaN. Создайте и снова отобразите траекторию. Новое положение показывает хорошую альтернативную траекторию, которая отклоняется от опорной траектории.
relaxedTermState = [NaN 10 0 5 0 0]; [~,trajGlobalRelaxed] = connect(connector,initState,relaxedTermState,3); clf show(refPath); hold on axis equal plot(trajGlobalRelaxed.Trajectory(:,1),trajGlobalRelaxed.Trajectory(:,2),'g'); hold off
frenetTrajectory - Траектории ФренетаТраектории Frenet между всеми начальными и конечными состояниями, возвращаемые как массив структуры со следующими полями:
Trajectory - n-на-6 числовая матрица. Каждая строка матрицы представляет собой набор координат Френе для начального состояния траектории в виде [S dS ddS L dL ddL].
Time - Вектор положительных скаляров из 0 кому timeSpan в секундах.
Эта функция поддерживает соединения 1-n, n-to-1 или n-to-n попарной траектории на основе числа строк initialState и terminalState.
globalTrajectory - Глобальные траекторииГлобальные траектории между всеми начальными и конечными состояниями, возвращаемые как структура или структурный массив с полями:
Trajectory - n-на-6 числовая матрица. Каждая строка матрицы представляет собой набор глобальных состояний формы [x y theta kappa dkappa s].
Time - Вектор положительных скаляров из 0 кому timeSpan в секундах.
Эта функция поддерживает соединения 1-n, n-to-1 или n-to-n попарной траектории на основе числа строк initialState и terminalState.