connect
Соедините начальное и терминальное состояния Frenet
Синтаксис
Описание
frenetTrajectory = connect(connectorFrenet,initialState,terminalState,timeSpan)
[___, возвращает траектории в глобальных координатах в дополнение ко всем аргументам в предыдущем синтаксисе.globalTrajectory] = connect(___)
Примеры
Сгенерируйте альтернативные траектории для ссылочного пути
Сгенерируйте альтернативные траектории для пути ссылки с помощью координат Френе. Задайте различные начальное и конечные состояния для ваших траекторий. Настройте свои состояния на основе сгенерированных траекторий.
Сгенерируйте ссылку путь из набора путевых точек. Создайте 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 - траектории Френе
структура | массив структур
Траектории Френе между всеми начальным и конечным состояниями, возвращенные как массив структур с этими полями:
Trajectory- n -на-6 числовую матрицу. Каждая строка матрицы является множеством координат Френе для начального состояния траектории в форме[S dS ddS L dL ddL].Time- Вектор положительных скаляров из0наtimeSpanв секундах.
Эта функция поддерживает 1-то n, n-то-1 или n-то n парные траекторные соединения на основе количества строк initialState и terminalState.
globalTrajectory - Глобальные траектории
структура | массив структур
Глобальные траектории между всеми начальным и конечным состояниями, возвращенные как структура или массив структур с полями:
Trajectory- n -на-6 числовую матрицу. Каждая строка матрицы является набором глобальных состояний вида[x y theta kappa dkappa s].Time- Вектор положительных скаляров из0наtimeSpanв секундах.
Эта функция поддерживает 1-то n, n-то-1 или n-то n парные траекторные соединения на основе количества строк initialState и terminalState.