Магистральное планирование траектории Используя путь к ссылке Frenet
Этот пример демонстрирует, как запланировать локальную траекторию в магистрали ведущий сценарий. Этот пример использует ссылочный путь и динамический список препятствий, чтобы сгенерировать альтернативные траектории для автомобиля, оборудованного датчиком. Автомобиль, оборудованный датчиком перешел через трафик, заданный в предоставленном ведущем сценарии от drivingScenario объект. Транспортное средство чередуется между адаптивным круиз-контролем, изменением маршрута и транспортным средством после маневров на основе стоимости, выполнимости и движения без коллизий.
Загрузите ведущий сценарий
Начните путем загрузки обеспеченного drivingScenario объект, который задает транспортное средство и дорожные свойства в текущей рабочей области. Этот сценарий был сгенерирован с помощью Driving Scenario Designer (Automated Driving Toolbox) приложение и экспортирован в функцию MATLAB®, drivingScenarioTrafficExample. Для получения дополнительной информации смотрите, Создают Ведущие Изменения Сценария Программно (Automated Driving Toolbox).
scenario = drivingScenarioTrafficExample; % Default car properties carLen = 4.7; % in meters carWidth = 1.8; % in meters rearAxleRatio = .25; % Define road dimensions laneWidth = carWidth*2; % in meters plot(scenario);
Создайте ссылочный путь
Все локальное планирование в этом примере выполняется относительно ссылочного пути, представленного referencePathFrenet объект. Этот объект может возвратить состояние кривой в данных длинах вдоль пути, найти самую близкую точку вдоль пути к некоторому глобальному xy-местоположению, и упрощает координатные преобразования между системами координат Frenet и глобальной переменной.
В данном примере ссылочный путь обработан как центр магистрали с четырьмя маршрутами, и waypoints совпадают с дорогой, заданной в обеспеченном drivingScenario объект.
waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; % in meters refPath = referencePathFrenet(waypoints); ax = show(refPath); axis(ax,'equal'); xlabel('X'); ylabel('Y');
Создайте генератор траектории
Для локального планировщика цель состоит в том, чтобы обычно производить множество возможных движений, которые двигают итоговую цель при удовлетворении текущим кинематическим и динамическим условиям. trajectoryGeneratorFrenet объект выполняет это путем соединения начального состояния с набором конечных состояний, использующих 4-й или траектории полинома 5-го порядка. Начальная буква и конечные состояния заданы в системе координат Frenet, и каждое полиномиальное решение удовлетворяет боковому и продольному положению, скорости и ускоряющим граничным условиям при минимизации толчка.
Конечные состояния часто вычисляются с помощью пользовательских поведений. Эти поведения усиливают информацию, найденную в окружении, таком как информация о маршруте, ограничение скорости и текущие или будущие предсказания агентов в близости автомобиля, оборудованного датчиком.
Создайте trajectoryGeneratorFrenet объект с помощью ссылочного пути
connector = trajectoryGeneratorFrenet(refPath);
Создайте динамическое средство проверки столкновения
dynamicCapsuleList объект является структурой данных, которая представляет состояние динамической среды по дискретному набору тактов. Эта среда может затем использоваться, чтобы эффективно подтвердить несколько потенциальных траекторий для автомобиля, оборудованного датчиком. Каждый объект в сцене представлен:
Уникальный идентификатор с целочисленным знаком
Свойства для краткой геометрии используются для эффективной проверки столкновения
Последовательность состояний SE2, где каждое состояние представляет дискретный снимок состояния вовремя.
В этом примере траектории сгенерированы trajectoryGeneratorFrenet объект происходит по некоторому промежутку времени, известного как период времени. Гарантировать, что проверка столкновения покрывает все возможные траектории, dynamicCapsuleList объект должен содержать предсказанные траектории всех агентов, охватывающих максимальный ожидаемый период времени.
capList = dynamicCapsuleList;
Создайте структуру геометрии для автомобиля, оборудованного датчиком данными параметрами.
egoID = 1; [egoID, egoGeom] = egoGeometry(capList,egoID); egoGeom.Geometry.Length = carLen; % in meters egoGeom.Geometry.Radius = carWidth/2; % in meters egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters
Добавьте автомобиль, оборудованный датчиком в динамический список капсул.
updateEgoGeometry(capList,egoID,egoGeom);
Добавьте drivingScenario агенты к dynamicCapsuleList объект. Геометрия установлена здесь, и состояния заданы во время цикла планирования. Вы видите что dynamicCapsuleList теперь содержит один автомобиль, оборудованный датчиком и пять препятствий.
actorID = (1:5)'; actorGeom = repelem(egoGeom,5,1); updateObstacleGeometry(capList,actorID,actorGeom)
ans =
dynamicCapsuleList with properties:
MaxNumSteps: 31
EgoIDs: 1
ObstacleIDs: [5x1 double]
NumObstacles: 5
NumEgos: 1
Планирование адаптивных маршрутов через трафик
Утилиты планирования поддерживают локальную стратегию планирования, которая производит набор локальных траекторий на основе текущего и предсказанного состояния среды прежде, чем выбрать самую оптимальную траекторию. Цикл симуляции был организован в следующие разделы:
Кликните по заголовкам каждого раздела, чтобы перейти к соответствующим нормам в цикле симуляции.
Усовершенствуйте сценарий основной истины
При планировании в динамической среде часто необходимо оценить состояние среды или предсказать ее состояние в ближайшем будущем. Для простоты этот пример использует drivingScenario как источник основной истины траекторий для каждого агента по горизонту планирования. Чтобы протестировать пользовательский алгоритм прогнозирования, можно заменить или изменить exampleHelperRetrieveActorGroundTruth функция с пользовательским кодом.
Сгенерируйте конечные состояния
Общая цель в автоматизированном управлении состоит в том, чтобы гарантировать, что запланированные траектории не являются только выполнимыми, но также и естественными. Типичное магистральное управление включает элементы хранения маршрута, поддержания ограничения скорости, перестраивания на другую полосу, адаптации скорости к трафику, и так далее. Каждое пользовательское поведение может потребовать различной информации о среде. Этот пример демонстрирует, как сгенерировать конечные состояния, которые реализуют три таких поведения: круиз-контроль, изменения маршрута, и следуют за ведущим транспортным средством.
Круиз-контроль
exampleHelperBasicCruiseControl функция генерирует конечные состояния, которые выполняют поведение круиз-контроля. Эта функция использует поперечную скорость автомобиля, оборудованного датчиком и период времени, чтобы предсказать ожидаемые N-секунды маршрута автомобиля, оборудованного датчиком в будущем. Центр маршрута вычисляется и становится боковым отклонением конечного состояния, и поперечная скорость и ускорение обнуляются.
Для продольных граничных условий терминальная скорость установлена в дорожное ограничение скорости, и терминальное ускорение обнуляется. Продольное положение неограниченно, который задан как NaN. Путем отбрасывания ограничения долготы, trajectoryGeneratorFrenet может использовать более низкий полином 4-го порядка, чтобы решить продольную краевую задачу, приводящую к траектории, которая гладко совпадает с дорожной скоростью за данный период времени:
Изменение маршрута
exampleHelperBasicLaneChange функция генерирует конечные состояния, которые переходят транспортное средство от текущего маршрута до любого смежного маршрута. Функция сначала определяет текущий маршрут автомобиля, оборудованного датчиком, и затем проверяет, существуют ли левые и правые маршруты. Для каждого существующего маршрута конечное состояние задано таким же образом как поведение круиз-контроля, за исключением того, что терминальная скорость установлена в текущую скорость, а не ограничение скорости дороги:
Следуйте за ведущим транспортным средством
exampleHelperBasicLeadVehicleFollow функция генерирует конечные состояния, которые пытаются запоздать транспортное средство, найденное перед автомобилем, оборудованным датчиком. Функция сначала определяет текущий маршрут автомобиля, оборудованного датчиком. Поскольку каждый обеспечил timeHorizon, функция предсказывает будущее состояние каждого агента, находит всех агентов, которые занимают тот же маршрут как автомобиль, оборудованный датчиком, и определяет, который является самым близким ведущим транспортным средством (если никакие ведущие транспортные средства не найдены, функция ничего не возвращает).
Конечное состояние автомобиля, оборудованного датчиком вычисляется, беря на себя инициативу положение и скорость транспортного средства и уменьшая терминальное продольное положение некоторым расстоянием безопасности:
Оцените стоимость конечных состояний
После того, как конечные состояния были сгенерированы, их стоимость может быть оценена. Оценка траектории и способы приоритизировать потенциальные решения очень субъективны. Ради простоты, exampleHelperEvaluateTSCost функция задает стоимость как комбинацию трех взвешенных сумм.
Боковая стоимость отклонения () — Положительный вес, который штрафует состояния, которые отклоняются от центра маршрута.
Стоимость времени () — Отрицательный вес, который приоритизирует движения, которые происходят на более длинном интервале, приводящем к более сглаженным траекториям.
Терминальная скоростная стоимость () — Положительный вес, который приоритизирует движения, которые обеспечивают ограничение скорости, приводящее к менее динамическим маневрам.
Сгенерируйте траектории и проверку на кинематическую выполнимость
В дополнение к наличию конечных состояний с минимальной стоимостью оптимальная траектория должна часто удовлетворять дополнительным ограничениям, связанным с кинематической выполнимостью и комфортом поездки. Ограничения траектории являются одним способом осуществить желаемое качество поездки, но они делают так за счет уменьшаемого ведущего конверта.
В этом примере, exampleHelperEvaluateTrajectory функция проверяет, что каждая траектория удовлетворяет следующим ограничениям:
MaxAcceleration: Абсолютное ускорение в траектории должно быть ниже заданного значения. Меньшие значения уменьшают ведущую агрессивность и устраняют кинематическим образом неосуществимые траектории. Это ограничение может устранить маневры, которые могли в противном случае быть выполнены транспортным средством.MaxCurvature: Радиус превращения минимума, который позволен в траектории. Как сMaxAcceleration, сокращение этого значения результаты в более сглаженном водительском стаже, но может устранить в противном случае выполнимые траектории.MinVelocity: Этот пример ограничивает автомобиль, оборудованный датчиком к движению только для форварда путем установки минимальной скорости. Это ограничение желаемо в магистрали ведущие сценарии и устраняет траектории, которые соответствуют сверхограниченным или плохо обусловленным граничным значениям.
Проверяйте траектории на столкновение и выберите оптимальную траекторию
Последний шаг в процессе планирования выбирает лучшую траекторию, которая также приводит к пути без коллизий. Проверка столкновения часто задерживается до конца, потому что это - дорогая операция, таким образом, путем оценки стоимости и анализа ограничений первые, недопустимые траектории могут быть удалены из фактора. Остающиеся траектории могут затем проверяться на столкновение в оптимальном порядке до столкновения, которым был найден свободный путь, или все траектории были оценены.
Задайте средство моделирования и планирование параметров
Этот раздел задает свойства, требуемые запускать средство моделирования и параметры, которые используются утилитами поведения и планировщиком. Свойства, такие как scenario.SampleTime and connector.TimeResolution синхронизируются так, чтобы состояния в траекториях агента основной истины и запланированных траекториях эго произошли в те же такты. Точно так же replanRate, timeHorizons, и maxHorizon выбраны таким образом, что они - целочисленные множители уровня симуляции.
Как упомянуто в предыдущем разделе, веса и ограничения выбраны, чтобы способствовать сглаженным ведущим траекториям при соблюдении правил дорожного движения.
Наконец, задайте speedLimit и safetyGap параметры, которые используются, чтобы сгенерировать конечные состояния для планировщика.
% Synchronize the simulator's update rate to match the trajectory generator's % discretization interval. scenario.SampleTime = connector.TimeResolution; % in seconds % Define planning parameters. replanRate = 10; % Hz % Define the time intervals between current and planned states. timeHorizons = 1:3; % in seconds maxHorizon = max(timeHorizons); % in seconds % Define cost parameters. latDevWeight = 1; timeWeight = -1; speedWeight = 1; % Reject trajectories that violate the following constraints. maxAcceleration = 15; % in meters/second^2 maxCurvature = 1; % 1/meters, or radians/meter minVelocity = 0; % in meters/second % Desired velocity setpoint, used by the cruise control behavior and when % evaluating the cost of trajectories. speedLimit = 11; % in meters/second % Minimum distance the planner should target for following behavior. safetyGap = 10; % in meters
Инициализируйте средство моделирования
Инициализируйте средство моделирования и создайте chasePlot (Automated Driving Toolbox) средство просмотра.
[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen);
Запустите ведущую симуляцию
tic while isRunning % Retrieve the current state of actor vehicles and their trajectory over % the planning horizon. [curActorState,futureTrajectory,isRunning] = ... exampleHelperRetrieveActorGroundTruth(scenario,futureTrajectory,replanRate,maxHorizon);
% Generate cruise control states. [termStatesCC,timesCC] = exampleHelperBasicCruiseControl(... refPath,laneWidth,egoState,speedLimit,timeHorizons); % Generate lane change states. [termStatesLC,timesLC] = exampleHelperBasicLaneChange(... refPath,laneWidth,egoState,timeHorizons); % Generate vehicle following states. [termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(... refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);
% Combine the terminal states and times. allTS = [termStatesCC; termStatesLC; termStatesF]; allDT = [timesCC; timesLC; timesF]; numTS = [numel(timesCC); numel(timesLC); numel(timesF)]; % Evaluate cost of all terminal states. costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit,... speedWeight, latDevWeight, timeWeight);
% Generate trajectories. egoFrenetState = global2frenet(refPath,egoState); [frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT); % Eliminate trajectories that violate constraints. isValid = exampleHelperEvaluateTrajectory(globalTraj,maxAcceleration,maxCurvature,minVelocity);
% Update the collision checker with the predicted trajectories % of all actors in the scene. for i = 1:numel(actorPoses) actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3); end updateObstaclePose(capList,actorID,actorPoses); % Determine evaluation order. [cost, idx] = sort(costTS); optimalTrajectory = []; trajectoryEvaluation = nan(numel(isValid),1); % Check each trajectory for collisions starting with least cost. for i = 1:numel(idx) if isValid(idx(i)) % Update capsule list with the ego object's candidate trajectory. egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3); updateEgoPose(capList,egoID,egoPoses); % Check for collisions. isColliding = checkCollision(capList); if all(~isColliding) % If no collisions are found, this is the optimal. % trajectory. trajectoryEvaluation(idx(i)) = 1; optimalTrajectory = globalTraj(idx(i)).Trajectory; break; else trajectoryEvaluation(idx(i)) = 0; end end end
% Display the sampled trajectories.
lineHandles = exampleHelperVisualizeScene(lineHandles,globalTraj,isValid,trajectoryEvaluation);
hold on;
show(capList,'TimeStep',1:capList.MaxNumSteps,'FastUpdate',1);
hold off;
if isempty(optimalTrajectory)
% All trajectories either violated a constraint or resulted in collision.
%
% If planning failed immediately, revisit the simulator, planner,
% and behavior properties.
%
% If the planner failed midway through a simulation, additional
% behaviors can be introduced to handle more complicated planning conditions.
error('No valid trajectory has been found.');
else
% Visualize the scene between replanning.
for i = (2+(0:(stepPerUpdate-1)))
% Approximate realtime visualization.
dt = toc;
if scenario.SampleTime-dt > 0
pause(scenario.SampleTime-dt);
end
egoState = optimalTrajectory(i,:);
scenarioViewer.Actors(1).Position(1:2) = egoState(1:2);
scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);
scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi;
scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
% Update capsule visualization.
hold on;
show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);
hold off;
% Update driving scenario.
advance(scenarioViewer);
tic;
end
end
endИндивидуальные настройки планировщика и дополнительные факторы
Настраиваемые решения часто включают много настраиваемых параметров, каждый способный к изменению итогового поведения способами, которые затрудняют, чтобы предсказать. Этот раздел подсвечивает некоторые специфичные для функции свойства и их эффект на вышеупомянутом планировщике. Затем предложения обеспечивают способы настроить или увеличить пользовательскую логику.
Динамический список капсул
Как упомянуто ранее, dynamicCapsuleList возразите действиям как временной базе данных, какие кэши предсказали траектории препятствий. Можно выполнить столкновение, сверяющееся с одним или несколькими телами эго по некоторому промежутку времени. MaxNumSteps свойство определяет общее количество тактов, которые проверяются объектом. В вышеупомянутом цикле симуляции свойство было установлено в 31. Это значение означает, что планировщик проверяет целые 1-3 вторых промежутка любых траекторий (произведенный в каждые 0,1 секунды). Теперь увеличьте максимальное значение в timeHorizons:
timeHorizons = 1:5; % in seconds maxTimeHorizon = max(timeHorizons); % in seconds
Существует теперь две опции:
MaxNumStepsсвойство оставлено без изменений.MaxNumStepsсвойство обновляется, чтобы вместить новый макс. промежуток.
Если свойство оставлено без изменений, то список капсул только подтверждает первые 3 секунды любой траектории, которая может быть предпочтительной, если вычислительный КПД является главным, или уверенность предсказания понижается быстро.
В качестве альтернативы можно работать с достоверными данными (как показан выше), или будущее состояние среды известно (e.g. полностью автоматизированная среда с централизованным управлением). Поскольку этот пример использует достоверные данные для агентов, обновите свойство.
capList.MaxNumSteps = 1+floor(maxTimeHorizon/scenario.SampleTime);
Другой, косвенно настраиваемый, свойство списка является краткой геометрией. Геометрия автомобиля, оборудованного датчиком или агентов может быть раздута путем увеличения Radius, и буферные области могут быть добавлены к транспортным средствам путем изменения Length и FixedTransform свойства.
Раздуйте целое место автомобиля, оборудованного датчиком путем увеличения радиуса.
egoGeom.Geometry.Radius = laneWidth/2; % in meters
updateEgoGeometry(capList,egoID,egoGeom);
Добавьте переднюю и заднюю буферную область во всех агентов.
actorGeom(1).Geometry.Length = carLen*1.5; % in meters actorGeom(1).Geometry.FixedTransform(1,end) = -actorGeom(1).Geometry.Length*rearAxleRatio; % in meters actorGeom = repmat(actorGeom(1),5,1); updateObstacleGeometry(capList,actorID,actorGeom);
Повторно выполните симуляцию с обновленными свойствами
Повторно выполните симуляцию. Получившаяся симуляция имеет несколько интересных разработок:
Более длинный горизонт с пятью вторым разом приводит к намного более сглаженному водительскому стажу. Планировщик все еще приоритизирует более длительные траектории из-за отрицательного
timeWeight.Обновленный
MaxNumStepsсвойство включило столкновение, проверяющее полную траекторию. Когда соединено с более длинным горизонтом планирования, планировщик идентифицирует и отменяет ранее оптимальное изменение лево-маршрута и возвращается к исходному маршруту.Расширенные капсулы находят столкновение ранее и отклоняют траекторию, которая приводит к более консервативному ведущему поведению. Один потенциальный недостаток к этому является уменьшаемым конвертом планирования, который рискует планировщиком, не бывшим способным найти допустимую траекторию.
% Initialize the simulator and create a chasePlot viewer. [scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ... exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen); tic; while isRunning % Retrieve the current state of actor vehicles and their trajectory over % the planning horizon. [curActorState,futureTrajectory,isRunning] = exampleHelperRetrieveActorGroundTruth(... scenario,futureTrajectory,replanRate,maxHorizon); % Generate cruise control states. [termStatesCC,timesCC] = exampleHelperBasicCruiseControl(... refPath,laneWidth,egoState,speedLimit,timeHorizons); % Generate lane change states. [termStatesLC,timesLC] = exampleHelperBasicLaneChange(... refPath,laneWidth,egoState,timeHorizons); % Generate vehicle following states. [termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(... refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons); % Combine the terminal states and times. allTS = [termStatesCC; termStatesLC; termStatesF]; allDT = [timesCC; timesLC; timesF]; numTS = [numel(timesCC); numel(timesLC); numel(timesF)]; % Evaluate cost of all terminal states. costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit, ... speedWeight,latDevWeight,timeWeight); % Generate trajectories. egoFrenetState = global2frenet(refPath,egoState); [frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT); % Eliminate trajectories that violate constraints. isValid = exampleHelperEvaluateTrajectory(... globalTraj, maxAcceleration, maxCurvature, minVelocity); % Update the collision checker with the predicted trajectories % of all actors in the scene. for i = 1:numel(actorPoses) actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3); end updateObstaclePose(capList, actorID, actorPoses); % Determine evaluation order. [cost, idx] = sort(costTS); optimalTrajectory = []; trajectoryEvaluation = nan(numel(isValid),1); % Check each trajectory for collisions starting with least cost. for i = 1:numel(idx) if isValid(idx(i)) % Update capsule list with the ego object's candidate trajectory. egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3); updateEgoPose(capList, egoID, egoPoses); % Check for collisions. isColliding = checkCollision(capList); if all(~isColliding) % If no collisions are found, this is the optimal % trajectory. trajectoryEvaluation(idx(i)) = 1; optimalTrajectory = globalTraj(idx(i)).Trajectory; break; else trajectoryEvaluation(idx(i)) = 0; end end end % Display the sampled trajectories. lineHandles = exampleHelperVisualizeScene(lineHandles, globalTraj, isValid, trajectoryEvaluation); if isempty(optimalTrajectory) % All trajectories either violated a constraint or resulted in collision. % % If planning failed immediately, revisit the simulator, planner, % and behavior properties. % % If the planner failed midway through a simulation, additional % behaviors can be introduced to handle more complicated planning conditions. error('No valid trajectory has been found.'); else % Visualize the scene between replanning. for i = (2+(0:(stepPerUpdate-1))) % Approximate realtime visualization. dt = toc; if scenario.SampleTime-dt > 0 pause(scenario.SampleTime-dt); end egoState = optimalTrajectory(i,:); scenarioViewer.Actors(1).Position(1:2) = egoState(1:2); scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5); scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi; scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5); % Update capsule visualization. hold on; show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1); hold off; % Update driving scenario. advance(scenarioViewer); tic; end end end
