Плавное вписывание опорной траектории в ППМ
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( соответствует кусочковому непрерывному набору кривых указанным ППМ. waypoints)waypoints аргумент задает Waypoints собственность.
refPathObj = referencePathFrenet( подгоняет кусочно-непрерывный набор кривых к ППМ, используя заданное расстояние между интерполированными точками траектории. waypoints,'DiscretizationDistance',discretionDist)discretionDist аргумент задает DiscretizationDistance собственность.
closestPoint | Поиск ближайшей точки на пути привязки к глобальной точке |
frenet2global | Преобразование состояний Frenet в глобальные состояния |
global2frenet | Преобразовать глобальные состояния в состояния Френета |
interpolate | Интерполяция опорного пути при заданных длинах дуги |
show | Отображение ссылочного пути на рисунке |
Создайте альтернативные траектории для траектории привязки с помощью координат 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
