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