Преобразуйте Декартовы состояния в состояния Frenet
cart2frenet( преобразует вектор с шестью элементами planner,cartesianStates)cartesianStates
[x, y, theta, kappa, speed, acceleration] к вектору с шестью элементами Frenet утверждает [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2], где s является длиной дуги от первой точки в ссылочном пути, и l является нормальным расстоянием от самой близкой точки в s на ссылочном пути.
В этом примере показано, как запланировать оптимальную траекторию с помощью 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];
Объявите, что указатель функции стоимости приоритизирует оставленный изменения маршрута.
leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);
Инициализируйте объект планировщика ссылочным путем, блоком проверки допустимости состояния и пользовательской функцией стоимости.
planner = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);Присвойте продольное конечное состояние, боковое отклонение и максимальные ускоряющие значения.
planner.TerminalStates.Longitudinal = 100; planner.TerminalStates.Lateral = -10:10:10; planner.FeasibilityParameters.MaxAcceleration = 10;
Планирование траектории
Начальное состояние Frenet транспортного средства.
initFrenetState = zeros(1,6);
Запланируйте траекторию от начального состояния Frenet.
plan(planner,initFrenetState);
Визуализация траектории
Визуализируйте карту и траектории.
show(map) hold on show(planner,'Trajectory','all')
В этом примере показано, как разделить продольные конечные состояния в оптимальном планировании траектории с помощью trajectoryOptimalFrenet объект.
Создайте и присвойте карту блоку проверки допустимости состояния
Создайте объект блока проверки допустимости состояния для проверки столкновения.
statevalidator = validatorOccupancyMap;
Создайте карту сетки препятствия.
grid = zeros(50,100); grid(22:24,28:33) = 1; grid(18:20,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.FeasibilityParameters.MaxAcceleration = 10;
Планирование траектории
Начальное состояние Frenet транспортного средства.
initFrenetState = zeros(1,6);
Присвойте номер разделов для продольного конечного состояния.
planner.NumSegments = 3;
Запланируйте траекторию от начального состояния Frenet.
plan(planner,initFrenetState);
Визуализация траектории
Визуализируйте карту и траектории.
show(map) hold on show(planner,'Trajectory','all')
