Найдите оптимальную траекторию вдоль ссылочного пути
trajectoryGeneratorFrenet объект генерирует альтернативные траектории, использующие четвертый или полиномы пятого порядка относительно данного ссылочного пути. Каждая траектория задает движение между состояниями Frenet по промежутку требуемого времени.
Состояния Frenet описывают свое положение, скорость и ускорение относительно статического ссылочного пути в виде referencePathFrenet объект.
Объект описывает состояния Френета как вектор из формы [S dS ddS L dL ddL], где S длина дуги и L перпендикулярное отклонение от направления ссылочного пути. Производные S относительно времени. Производные L относительно длины дуги, S.
Чтобы сгенерировать альтернативные траектории, задайте начальные и терминальные состояния сети с бесплатным доступом с данным отрезком времени к connect объектная функция.
connectorFrenet = trajectoryGeneratorFrenet( генерирует траектории между начальной буквой и конечными состояниями относительно ссылочного пути refPath)refPath заданный как referencePathFrenet объект. refPath входной параметр устанавливает свойство ReferencePath.
connectorFrenet = trajectoryGeneratorFrenet( задает временной интервал для дискретизации. refPath,'TimeResolution',timeValue)timeValue аргумент устанавливает свойство TimeResolution.
connect | Соедините начальную букву и терминал состояния Frenet |
Сгенерируйте альтернативные траектории для ссылочного использования пути координаты Frenet. Задайте различную начальную букву и конечные состояния для ваших траекторий. Настройте свои состояния на основе сгенерированных траекторий.
Сгенерируйте ссылочный путь от набора waypoints. Создайте trajectoryGeneratorFrenet объект от ссылочного пути.
waypoints = [0 0; ... 50 20; ... 100 0; ... 150 10]; refPath = referencePathFrenet(waypoints); connector = trajectoryGeneratorFrenet(refPath);
Сгенерируйте пять вторых траекторий между источником пути и точкой 30 m вниз путь, как Frenet утверждает.
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,5); 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
closestPoint | connect | frenet2global | global2frenet | interpolate | show