Нахождение оптимальной траектории вдоль опорного пути
The trajectoryGeneratorFrenet объект генерирует альтернативные траектории, используя полиномы четвертого или пятого порядка относительно заданного опорного пути. Каждая траектория определяет движение между состояниями Френе в течение заданного временного интервала.
Состояния Френе описывают их положение, скорость и ускорение относительно статического эталонного пути, заданные как referencePathFrenet объект.
Объект выражает состояния Френе как вектор формы [S dS ddS L dL ddL], где S - длина дуги и L - перпендикулярное отклонение от направления опорного пути. Производные S относятся ко времени. Производные L относятся к длине дуги, S.
Чтобы сгенерировать альтернативные траектории, задайте начальное и терминальное состояния Френе с заданным периодом времени connect функция объекта.
connectorFrenet = trajectoryGeneratorFrenet( генерирует траектории между начальным и конечные состояния относительно пути ссылки refPath)refPath задается как referencePathFrenet объект. The refPath входной параметр устанавливает свойство ReferencePath.
connectorFrenet = trajectoryGeneratorFrenet( определяет временной интервал дискретизации. The refPath,'TimeResolution',timeValue)timeValue аргумент устанавливает свойство TimeResolution.
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
