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