Сглаживайте ссылку путь, подгонка к путевым точкам
The referencePathFrenet объект подходит к гладкой, кусочно-непрерывной кривой для набора путевых точек, заданных как [x y] или [x y theta]. После подбора кривой точек вдоль кривой точки пути выражаются как [x y theta kappa dkappa s], где:
x
y и theta- состояние SE (2), выраженное в глобальных координатах, с x и y в метрах и theta в радианах
kappa - Кривизна, или обратная величине радиуса, в метрах
dkappa - Производная кривизны по длине дуги в метрах в секунду
s - Длина дуги, или расстояние вдоль пути от источника пути, в метрах
Используя этот объект, преобразуйте траектории между глобальной и Френдской системами координат, интерполируйте состояния вдоль пути на основе длины дуги и запросите ближайшую точку на пути из глобального состояния.
Объект выражает состояния Френе как вектор формы [S dS ddS L dL ddL], где S - длина дуги и L - перпендикулярное отклонение от направления опорного пути. Производные S относятся ко времени. Производные L относятся к длине дуги, S.
refPathObj = referencePathFrenet( подбирает кусочно-непрерывный набор кривых к заданным путевым точкам. The waypoints)waypoints аргумент устанавливает Waypoints свойство.
refPathObj = referencePathFrenet( подбирает кусочно-непрерывный набор кривых к путевым точкам, используя заданное расстояние между интерполированными точками пути. The waypoints,'DiscretizationDistance',discretionDist)discretionDist аргумент устанавливает DiscretizationDistance свойство.
closestPoint | Нахождение ближайшей точки на ссылку пути к глобальной точке |
frenet2global | Преобразуйте состояния Френе в глобальные состояния |
global2frenet | Преобразуйте глобальные состояния в состояния Френе |
interpolate | Интерполяция опорного пути при заданных длинах дуги |
show | Отображение ссылки путь на рисунке |
Сгенерируйте альтернативные траектории для пути ссылки с помощью координат Френе. Задайте различные начальное и конечные состояния для ваших траекторий. Настройте свои состояния на основе сгенерированных траекторий.
Сгенерируйте ссылку путь из набора путевых точек. Создайте 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
