В этом примере показано, как выполнить динамическое перепланирование в городском сценарии с помощью trajectoryOptimalFrenet
.
В этом примере вы будете:
Исследуйте городской сценарий с предопределенными транспортными средствами.
Используйте trajectoryOptimalFrenet
сделать локальное планирование навигации по Городскому сценарию.
Для автоматизированного управления в городском сценарии нужно планирование двух уровней, глобальной переменной и локальный. Глобальный планировщик находит самый выполнимый путь между запуском и целевыми точками. Локальный планировщик делает динамическое перепланирование сгенерировать оптимальную траекторию на основе глобального плана и окружающей информации. В этом примере следует автомобиль, оборудованный датчиком (зеленое поле), глобальный план (отметил точкой синюю линию). Локальное планирование сделано (чисто оранжевая линия), стараясь избегать другого транспортного средства (черный прямоугольник).
Локальные планировщики используют информацию о препятствии, чтобы сгенерировать оптимальные траектории без коллизий. Информация о препятствии от различных встроенных датчиков как камера, радар и лидар сплавлена, чтобы держать карту заполнения в курсе. Эта карта заполнения эгоцентрична, где локальная система координат сосредоточена на автомобиле, оборудованном датчиком. Карта используется в локальном планировании, когда препятствия обнаруживаются от датчиков и помещаются в карту.
Этот сценарий в качестве примера имеет четыре других транспортных средства (синие прямоугольники), которые перемещаются в предопределенные пути при различных скоростях. Фигура ниже иллюстрирует этот сценарий и глобальный план (твердая красная линия) используемый в этом примере. Чисто красные точки в ниже фигуры представляют waypoints глобального плана между запуском и целевыми положениями. Зеленый прямоугольник представляет автомобиль, оборудованный датчиком.
Автомобиль, оборудованный датчиком использует trajectoryOptimalFrenet
перейти от положения A до положения D в трех сегментах с тремя различными параметрами конфигурации.
Сначала (К B), транспортное средство демонстрирует поведение Адаптивного круиз-контроля (ACC).
Второй (B к C), транспортное средство согласовывает поворот следовать глобальному плану.
Треть (C к D), транспортное средство выполняет маневр изменения маршрута.
Настройте необходимые данные и переменные окружения:
% Load data from urbanScenarioData.mat file, initialize required variables
[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;
otherVehicles: [1 x 4] Массив структур, содержащий поля: Position
, Yaw
, Velocity
, и SimulationTime
, из каждого транспортного средства в сценарии.
globalPlanPoints
:
[18 x 2] Матрица содержат предварительно вычисленный глобальный план, состоящий из восемнадцати waypoints, каждый представляющий положение в сценарии.
stateValidator
: validatorOccupancyMap
возразите, что действия как блок проверки допустимости состояния на основе данной 2D власти сопоставляют. Полностью занятая эгоцентрическая карта заполнения обновляется на основе информации о препятствии и дорожных контуров. Пользовательский блок проверки допустимости состояния может также использоваться на основе приложения. Для получения дополнительной информации смотрите nav.StateValidator
.
Постройте сценарий.
exampleHelperPlotUrbanScenario;
Укажите, что блок проверки допустимости состояния и глобальная переменная планируют создать локального планировщика, использующего trajectoryOptimalFrenet
.
localPlanner = trajectoryOptimalFrenet(globalPlanPoints,stateValidator);
localPlanner
localPlanner
имеет множество свойств, которые могут быть настроены, чтобы достигнуть желаемого поведения. Этот раздел исследует некоторые из этих свойств и их значений по умолчанию.
localPlanner.TerminalStates
Longitudinal:
[30 45 60 75 90]: Задает продольное расстояние выборки в метрах. Это значение может быть скаляром или вектором.
Lateral:
[-2 -1 0 1 2]:
Задает боковое отклонение в метрах от ссылочного пути (Глобальный план в нашем случае).
Time:
7: Время в секундах, чтобы достигнуть конца траектории.
Speed:
10: Скорость в метрах в секунду, чтобы быть достигнутым в конце траектории
Acceleration:
0:
Ускорение в конце траектории в м/с^2.
localPlanner.FeasibilityParameters
MaxCurvature:
0.1 : Максимальное выполнимое значение для искривления в m^-1
MaxAcceleration:
2.5: Максимальное выполнимое ускорение в м/с^2.
localPlanner.TimeResolution:
0.1: Интервал дискретизации траектории в секундах
trajectoryOptimalFrenet
продемонстрировать поведение Адаптивного круиз-контроля (ACC) В этом разделе свойства присвоение, должен был сконфигурировать localPlanner
продемонстрировать поведение Адаптивного круиз-контроля (ACC).
Чтобы продемонстрировать ACC, автомобиль, оборудованный датчиком должен следовать за ведущим транспортным средством путем поддержания безопасного расстояния. Ведущее транспортное средство в этом сегменте выбирается using otherVehicles(1)
.
% Get leadVehicle in segment from Postion A to Position B leadVehicle = otherVehicles(1); % Define ACC safe distance ACCSafeDistance = 35; % in meters % Adjusting the time resolution of planner object to make the ego vehicle % travel smoothly timeResolution = 0.01; localPlanner.TimeResolution = timeResolution;
Настройте автомобиль, оборудованный датчиком в положении A и задайте его начальную скорость и ориентацию (Отклонение от курса).
% Set positions A, B, C and D positionA = [5.1, -1.8, 0]; positionB = [60, -1.8, 0]; positionC = [74.45, -30.0, 0]; positionD = [74.45, -135, 0]; goalPoint = [145.70, -151.8, 0]; % Set the initial state of the ego vehicle egoInitPose = positionA; egoInitVelocity = [10, -0.3, 0]; egoInitYaw = -0.165; currentEgoState = [egoInitPose(1), egoInitPose(2), deg2rad(egoInitYaw),... 0, norm(egoInitVelocity), 0]; vehicleLength = 4.7; % in meters % Replan interval in number of simulation steps % (default 50 simulation steps) replanInterval = 50;
Визуализируйте результаты симуляции.
% Initialize Visualization
exampleHelperInitializeVisualization;
Поведение ACC достигается путем установки TerminalStates
из localPlanner
как below:
Чтобы обеспечить безопасное расстояние от ведущего транспортного средства, установите localPlanner.TerminalStates.Longitudinal
к расстоянию, чтобы привести транспортное средство - длина транспортного средства;
Чтобы обеспечить относительную скорость относительно ведущего транспортного средства, установите localPlanner.TerminalStates.Speed
привести скорость транспортного средства;
Продолжать перейти на глобальном плане, set localPlanner.TerminalStates.Lateral
к 0;
В следующем фрагменте кода, localPlanner
генерирует trajectory
это выполняется и визуализировало использование exampleHelperUpdateVisualization
для каждого шага симуляции. Однако перепланирование сделано для каждого 50-го шага симуляции. Следующее является последовательностью событий во время перепланирования:
Обновите карту заполнения с помощью информации о транспортном средстве с помощью exampleHelperUpdateOccupancyMap
.
Обновите localPlanner.TerminalStates
.
Генерация траектории с помощью plan(localPlanner,currentStateInFrenet)
.
% Simulate till the ego vehicle reaches position B simStep = 1; % Check only for X as there is no change in Y. while currentEgoState(1) <= positionB(1) % Replan at every "replanInterval"th simulation step if rem(simStep, replanInterval) == 0 || simStep == 1 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState); % Compute distance to Lead Vehicle and leadVehicleVelocity distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ... currentEgoState(1:2)); leadVehicleVelocity = leadVehicle.Velocity(simStep,:); % Set localPlanner.TerminalStates for ACC behavior if distanceToLeadVehicle <= ACCSafeDistance localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength; localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... localPlanner.TerminalStates.Speed; localPlanner.TerminalStates.Time = desiredTimeBound; localPlanner.FeasibilityParameters.MaxCurvature = 0.5; end % Generate optimal trajectory for current set of parameters currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]); trajectory = plan(localPlanner, currentStateInFrenet); % Visualize the ego-centric occupancy map show(egoMap,"Parent",hAxes1) title("Ego Centric Occupancy Map","Parent",hAxes1) % Visualize ego vehicle on occupancy map egoCenter = currentEgoState(1:2); egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter); hold(hAxes1,"on") fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1) % Visualize the Trajectory reference path and trajectory show(localPlanner,"Trajectory","optimal","Parent",hAxes1) end % Execute and Update Visualization [isGoalReached, currentEgoState] = ... exampleHelperExecuteAndVisualize(currentEgoState,simStep,... trajectory,previousReplanTime); if(isGoalReached) break; end % Update the simulation step for the next iteration simStep = simStep + 1; pause(0.01); end
В конце этого выполнения автомобиль, оборудованный датчиком в положении B.
Затем сконфигурируйте trajectoryOptimalFrenet
для согласования поворота во втором сегменте от положения B до положения C.
trajectoryOptimalFrenet
согласовывать сглаженный поворотТекущие свойства набора localPlanner
достаточны, чтобы согласовать сглаженный поворот. Однако во втором сегменте, ведущее транспортное средство выбирается от otherVehicles(2)
.
% Set Lead Vehicle to correspond to the vehicle in second segment % from position B to position C leadVehicle = otherVehicles(2); % Simulate till the ego vehicle reaches position C % Check only for Y as there is no change in X at C while currentEgoState(2) >= positionC(2) % Replan at every "replanInterval"th simulation step if rem(simStep, replanInterval) == 0 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState); % Compute distance to Lead Vehicle and leadVehicleVelocity distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ... currentEgoState(1:2)); leadVehicleVelocity = leadVehicle.Velocity(simStep,:); if(distanceToLeadVehicle <= ACCSafeDistance) localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength; localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... localPlanner.TerminalStates.Speed; localPlanner.TerminalStates.Time = desiredTimeBound; localPlanner.FeasibilityParameters.MaxCurvature = 0.5; localPlanner.FeasibilityParameters.MaxAcceleration = 5; end % Generate optimal trajectory for current set of parameters currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]); trajectory = plan(localPlanner, currentStateInFrenet); % Visualize the ego-centric occupancy map show(egoMap,"Parent",hAxes1) title("Ego Centric Occupancy Map","Parent",hAxes1) % Visualize ego vehicle on occupancy map egoCenter = currentEgoState(1:2); egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter); hold(hAxes1,"on") fill(egoPolygon(1, :), egoPolygon(2, :), "g", "Parent", hAxes1) % Visualize the Trajectory reference path and trajectory show(localPlanner, "Trajectory", "optimal", "Parent", hAxes1) end % Execute and Update Visualization [isGoalReached, currentEgoState] = ... exampleHelperExecuteAndVisualize(currentEgoState,simStep,... trajectory,previousReplanTime); if(isGoalReached) break; end % Update the simulation step for the next iteration simStep = simStep + 1; pause(0.01); end
В конце этого выполнения автомобиль, оборудованный датчиком в положении C.
Затем сконфигурируйте trajectoryOptimalFrenet
для выполнения маршрута изменяют маневр от положения C до положения D.
trajectoryOptimalFrenet
выполнять маневр Изменения МаршрутаМаневр изменения маршрута может быть выполнен путем соответствующего конфигурирования Боковых терминальных состояний планировщика. Это может быть достигнуто путем установки бокового терминального состояния на ширину маршрута (3.6 м в этом примере) и предположения, что ссылочный путь выравнивается к центру текущего маршрута эго.
% Simulate till the ego vehicle reaches position D % Set Lane Width laneWidth = 3.6; % Check only for Y as there is no change in X at D while currentEgoState(2) >= positionD(2) % Replan at every "replanInterval" simulation step if rem(simStep, replanInterval) == 0 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState); % TerminalState settings for negotiating Lane change localPlanner.TerminalStates.Longitudinal = 20:5:40; localPlanner.TerminalStates.Lateral = laneWidth; localPlanner.TerminalStates.Speed = 10; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2); localPlanner.TerminalStates.Time = desiredTimeBound; localPlanner.FeasibilityParameters.MaxCurvature = 0.5; localPlanner.FeasibilityParameters.MaxAcceleration = 15; % Generate optimal trajectory for current set of parameters currentStateInFrenet = cart2frenet(localPlanner,[currentEgoState(1:5) 0]); trajectory = plan(localPlanner,currentStateInFrenet); % Visualize the ego-centric occupancy map show(egoMap,"Parent",hAxes1) title("Ego Centric Occupancy Map","Parent",hAxes1) % Visualize ego vehicle on occupancy map egoCenter = currentEgoState(1:2); egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter); hold(hAxes1,"on") fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1) % Visualize the Trajectory reference path and trajectory show(localPlanner,"Trajectory","optimal","Parent",hAxes1) end % Execute and Update Visualization [isGoalReached, currentEgoState] = ... exampleHelperExecuteAndVisualize(currentEgoState,simStep,... trajectory,previousReplanTime); if(isGoalReached) break; end % Update the simulation step for the next iteration simStep = simStep + 1; pause(0.01); end
localPlanner
теперь сконфигурирован, чтобы перейти от положения D до целевого положения.
% Simulate till the ego vehicle reaches Goal position % Check only for X as there is no change in Y at Goal. while currentEgoState(1) <= goalPoint(1) % Replan at every "replanInterval"th simulation step if rem(simStep, replanInterval) == 0 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState); localPlanner.TerminalStates.Longitudinal = 20; localPlanner.TerminalStates.Lateral = [-1 0 1]; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2); localPlanner.TerminalStates.Time = desiredTimeBound:0.2:desiredTimeBound+1; % Generate optimal trajectory for current set of parameters currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]); trajectory = plan(localPlanner, currentStateInFrenet); % Visualize the ego-centric occupancy map show(egoMap,"Parent",hAxes1) title("Ego Centric Occupancy Map","Parent",hAxes1) % Visualize ego vehicle on occupancy map egoCenter = currentEgoState(1:2); egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter); hold(hAxes1,"on") fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1) % Visualize the Trajectory reference path and trajectory show(localPlanner,"Trajectory","optimal","Parent",hAxes1) end % Execute and Update Visualization [isGoalReached, currentEgoState] = ... exampleHelperExecuteAndVisualize(currentEgoState,simStep,... trajectory,previousReplanTime); % Goal reached will be true only in this section. if(isGoalReached) break; end % Update the simulation step for the next iteration simStep = simStep + 1; pause(0.01); end
Регистрируйте положения автомобиля, оборудованного датчиком в egoPoses
переменная, которая доступна в базовом рабочем пространстве. Можно воспроизвести путь к транспортному средству в DrivingScenario с помощью exampleHelperPlayBackInDS(egoPoses)
.
% Clear workspace variables that were created during the example run. % This excludes egoPoses to allow the user to playback the simulation in DS exampleHelperUrbanCleanUp;
Этот пример объяснил, как выполнить динамическое перепланирование в городском сценарии с помощью trajectoryOptimalFrenet.
В частности, мы изучили, как использовать trajectoryOptimalFrenet
понять следующее поведение.
Адаптивный круиз-контроль
Согласование поворотов
Маневр изменения маршрута.