Нахождение оптимальной траектории вдоль опорного пути
The trajectoryOptimalFrenet объект является планировщиком пути, который выборки и оценивают локальные траектории на основе ссылки пути. Планировщик генерирует набор конечных состояний на основе пути ссылки и других параметров объекта. Затем планировщик соединяет состояние с каждым конечным состоянием, используя 4th или 5th Порядок полиномов. Чтобы выбрать оптимальный путь, выбранные траектории оцениваются на кинематическую осуществимость, столкновение и стоимость.
trajectoryOptimalFrenet( создает refPath,validator)trajectoryOptimalFrenet объект со ссылочным путем, refPath, в виде n -на-2 массива [x y]
путевые точки и валидатор состояния, validator, заданный как validatorOccupancyMap объект.
planner = trajectoryOptimalFrenet(___,Name,Value)
cart2frenet | Преобразуйте Декартовы состояния в состояния Френе |
copy | Создайте глубокую копию объекта |
frenet2cart | Преобразуйте состояния Френе в Декартовы состояния |
plan | Планируйте оптимальную траекторию |
show | Визуализация траекторий |
В этом примере показано, как спланировать оптимальную траекторию с помощью trajectoryOptimalFrenet объект.
Создайте и назначьте карту для валидатора состояний
Создайте объект средства проверки состояния для проверки столкновения.
stateValidator = validatorOccupancyMap;
Создайте сетчатую карту препятствий.
grid = zeros(50,100); grid(24:26,48:53) = 1;
Создайте binaryOccupancyMap с сетчатой картой.
map = binaryOccupancyMap(grid);
Присвойте карту валидатору состояния.
stateValidator.Map = map;
Планирование и визуализация траектории
Создайте ссылку пути для последующего планирования.
refPath = [0,25;100,25];
Инициализируйте объект планировщика с ссылка пути и средства проверки состояния.
planner = trajectoryOptimalFrenet(refPath,stateValidator);
Присвойте продольное конечное состояние, боковое отклонение и максимальные значения ускорения.
planner.TerminalStates.Longitudinal = 100; planner.TerminalStates.Lateral = -10:5:10; planner.FeasibilityParameters.MaxAcceleration = 10;
Задайте значение смещения отклонения близко к левому боковому конечному состоянию, чтобы расставить приоритеты изменениям в левой полосе.
planner.DeviationOffset = 5;
Планирование траектории
Начальное декартовое состояние транспортного средства.
initCartState = [0 25 pi/9 0 0 0];
Преобразуйте декартовое состояние транспортного средства в состояние Френе.
initFrenetState = cart2frenet(planner,initCartState);
Планируйте траекторию от начального состояния Френе.
plan(planner,initFrenetState);
Визуализация траектории
Визуализируйте карту и траектории.
show(map) hold on show(planner,'Trajectory','all')
Этот пример показывает, как разбить продольные конечные состояния в оптимальном планировании траектории с помощью trajectoryOptimalFrenet объект.
Создайте и назначьте карту для валидатора состояний
Создайте объект средства проверки состояния для проверки столкновения.
statevalidator = validatorOccupancyMap;
Создайте сетчатую карту препятствий.
grid = zeros(50,100); grid(25:27,28:33) = 1; grid(16:18,37:42) = 1; grid(29:31,72:77) = 1;
Создайте binaryOccupancyMap с сетчатой картой.
map = binaryOccupancyMap(grid);
Присвойте карту валидатору состояния.
statevalidator.Map = map;
Планирование и визуализация траектории
Создайте ссылку пути для последующего планирования.
refPath = [0,25;30,30;75,20;100,25];
Инициализируйте объект планировщика с ссылка пути и средства проверки состояния.
planner = trajectoryOptimalFrenet(refPath,statevalidator);
Присвойте продольное конечное состояние, боковое отклонение и максимальные значения ускорения.
planner.TerminalStates.Longitudinal = 100; planner.TerminalStates.Lateral = -5:5:5; planner.FeasibilityParameters.MaxAcceleration = 10;
Назначьте количество перегородок для продольного конечного состояния.
planner.NumSegments = 3;
Планирование траектории
Начальное состояние транспортного средства.
initFrenetState = zeros(1,6);
Планируйте траекторию от начального состояния Френе.
plan(planner,initFrenetState);
Визуализация траектории
Визуализируйте карту и траектории.
show(map) hold on show(planner,'Trajectory','all') hold on
Сгенерируйте контуры маршрута
Вычислите конец ссылки пути как состояние Френе.
refPathEnd = cart2frenet(planner,[planner.Waypoints(end,:) 0 0 0 0]);
Вычислите смещения маршрута по обеим сторонам боковых конечных состояний со значением ширины половины маршрута.
laneOffsets = unique([planner.TerminalStates.Lateral+2.5 planner.TerminalStates.Lateral-2.5]);
Вычислите положения полос в Декартовом состоянии.
numLaneOffsets = numel(laneOffsets); xRefPathEnd = ceil(refPathEnd(1)); laneXY = zeros((numLaneOffsets*xRefPathEnd)+numLaneOffsets,2); xIndex = 0; for laneID = 1:numLaneOffsets for x = 1:xRefPathEnd laneCart = frenet2cart(planner,[x 0 0 laneOffsets(laneID) 0 0]); xIndex = xIndex + 1; laneXY(xIndex,:) = laneCart(1:2); end xIndex = xIndex + 1; laneXY(xIndex,:) = NaN(1,2); end
Постройте контуры маршрута.
plot(laneXY(:,1),laneXY(:,2),'LineWidth',0.5,'Color',[0.5 0.5 0.5],'DisplayName','Lane Boundaries','LineStyle','--')
Самопересечения в пути ссылки могут привести к неожиданному поведению.
Плановик не поддерживает реверс вождения.
Первоначальная ориентация на планирование должна быть в пределах -pi/2 и pi/2 к ссылке пути.
Ограничьте количество TerminalStates для приложений реального времени, так как вычислительная сложность увеличивается вместе с ним.
[1] Верлинг, Мориц, Юлиус Циглер, Сёрен Каммель и Себастьян Трун. «Оптимальная генерация траектории для динамических уличных сценариев на французской Системе координат». Международная конференция IEEE по робототехнике и автоматизации 2010. 2010, стр 987–993.
nav.StateValidator | plannerHybridAStar | validatorOccupancyMap
