Планирование оптимальной траектории
[ вычисляет возможную траекторию, traj,index,cost,flag] = plan(planner,start)traj, из списка возможных траекторий, сгенерированных из trajectoryOptimalFrenet объект, planner. start указывается как шестиэлементный вектор [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2]где s - длина дуги от первой точки на траектории привязки, а l - нормальное расстояние от ближайшей точки на траектории привязки.
Выходная траектория, traj, также имеет связанный cost и index для свойства планировщика TrajectiveList. flag - числовой флаг выхода, указывающий состояние решения.
Для улучшения результатов выходных данных планирования измените параметры на planner объект.
В этом примере показано, как планировать оптимальную траекторию с помощью 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
Создание границ полос движения
Вычислите конец опорного пути как состояние Frenet.
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','--')
