Создание и назначение карты средству проверки состояния

Создание объекта проверки состояния для проверки конфликтов.

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')

Создание и назначение карты средству проверки состояния

Создание объекта проверки состояния для проверки конфликтов.

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','--')