frenet2cart

Преобразуйте состояния Frenet в Декартовы состояния

Описание

пример

frenet2cart(planner,frenetStates) преобразует вектор с шестью элементами из frenetStates [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2] к вектору с шестью элементами из Декартовых состояний [x, y, theta, kappa, speed, acceleration].

Примеры

свернуть все

В этом примере показано, как запланировать оптимальную траекторию с помощью trajectoryOptimalFrenet объект.

Создайте и присвойте карту блоку проверки допустимости состояния

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

stateValidator = validatorOccupancyMap;

Создайте карту сетки препятствия.

grid = zeros(50,100);
grid(24:26,48:53) = 1;

Создайте binaryOccupancyMap с картой сетки.

map = binaryOccupancyMap(grid);

Присвойте карту и границы состояния к блоку проверки допустимости состояния.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Запланируйте и визуализируйте траекторию

Создайте ссылочный путь для планировщика, чтобы следовать.

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];

Преобразуйте декартово состояние транспортного средства к состоянию Frenet.

initFrenetState = cart2frenet(planner,initCartState);

Запланируйте траекторию от начального состояния Frenet.

plan(planner,initFrenetState);

Визуализация траектории

Визуализируйте карту и траектории.

show(map)
hold on
show(planner,'Trajectory','all')

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 5 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory.

В этом примере показано, как разделить продольные конечные состояния в оптимальном планировании траектории с помощью 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;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

Запланируйте и визуализируйте траекторию

Создайте ссылочный путь для планировщика, чтобы следовать.

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;

Планирование траектории

Начальное состояние Frenet транспортного средства.

initFrenetState = zeros(1,6);

Запланируйте траекторию от начального состояния Frenet.

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

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 6 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory, Lane Boundaries.

Входные параметры

свернуть все

Оптимальный планировщик траектории на пробеле Frenet в виде trajectoryOptimalFrenet объект.

Вектор из Frenet утверждает в виде 1 6 вектор, [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2].

  • s задает длину дуги от первой точки в ссылочном пути в метрах.

  • ds/dt задает первую производную длины дуги.

  • d2s/dt2 задает вторую производную длины дуги.

  • l задает нормальное расстояние от самой близкой точки в ссылочном пути в метрах.

  • dl/ds задает первую производную нормального расстояния.

  • d2l/ds2 задает вторую производную нормального расстояния.

Пример: [10 1 0 3 0 0]

Типы данных: double

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Смотрите также

|

Введенный в R2019b
Для просмотра документации необходимо авторизоваться на сайте