Планирование траектории шоссе с использованием траектории Ссылки 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-местоположению и облегчает преобразования координат между глобальной и Френдской опорными системами координат.
В данном примере ссылки путь рассматривается как центр четырехполосной магистрали, и путевые точки совпадают с дорогой, заданной в предоставленном 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');
Конструкция генератора траектории
Для локального планировщика целью обычно является выборка множества возможных движений, которые движутся к конечной цели с учетом текущих кинематических и динамических условий. The trajectoryGeneratorFrenet объект достигает этого путем соединения начального состояния с набором конечных состояний с помощью траекторий полинома 4-го или 5-го порядка. Начальное и конечные состояния заданы в системе координат Френе, и каждое полиномиальное решение удовлетворяет боковым и продольным положениям, скорости и граничным условиям ускорения при минимизации рывка.
Конечные состояния часто вычисляются с помощью пользовательских поведений. Эти модели поведения используют информацию, найденную в локальных окружениях, такую как информация о маршруте, пределе скорости и текущих или будущих предсказаниях актёров в непосредственной близости от автомобиля , оборудованного датчиком.
Создайте trajectoryGeneratorFrenet объект с использованием ссылки пути
connector = trajectoryGeneratorFrenet(refPath);
Создайте динамическую проверку столкновения
The 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 функция с пользовательским кодом.
Сгенерируйте Конечные состояния
Общая цель в автоматизированном вождении состоит в том, чтобы гарантировать, что запланированные траектории не просто выполнимы, но и естественны. Типичное движение по шоссе включает элементы поддержания маршрута, поддержания предела, перестроения, адаптации скорости к движению и так далее. Каждое пользовательское поведение может потребовать различной информации об окружении. Этот пример демонстрирует, как сгенерировать конечные состояния, которые реализуют три таких поведения: круиз-контроль, изменения маршрута и следование ведущему транспортному средству.
Круиз-контроль
The exampleHelperBasicCruiseControl функция генерирует конечные состояния, которые выполняют поведение круиз-контроля. Эта функция использует автомобиль , оборудованный датчиком поперечной скорости и временной горизонт, чтобы предсказать автомобиль , оборудованный датчиком ожидаемую полосу N-секунд в будущем. Центр маршрута вычисляется и становится боковым отклонением конечного состояния, и поперечная скорость и ускорение устанавливаются в нуль.
Для продольных граничных условий конечная скорость устанавливается на предельную скорость дороги, а ускорение терминала устанавливается на нуль. Продольное положение неограниченно, что задается как NaN. Отбрасывая ограничение долготы, trajectoryGeneratorFrenet может использовать более низкий полином 4-го порядка, чтобы решить продольную краевую задачу, приводящую к траектории, которая плавно совпадает со скоростью дороги на заданном временном горизонте:
Изменение маршрута
The exampleHelperBasicLaneChange функция генерирует конечные состояния, которые переключают транспортное средство с текущей полосы на любую смежную полосу. Функция сначала определяет автомобиль , оборудованный датчиком текущую полосу, а затем проверяет, существуют ли левая и правая полосы. Для каждого существующего маршрута конечное состояние определяется таким же образом, как и поведение круиз-контроля, за исключением того, что конечная скорость устанавливается на текущую скорость, а не на предел скорости дороги:
Следуйте за ведущим транспортным средством
The 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);
ax = gca;
ax.ZLim = [-100 100];
tic;
end
end
endИндивидуальные настройки планировщика и дополнительные факторы
Пользовательские решения часто включают много настраиваемых параметров, каждый из которых способен изменять окончательное поведение способами, которые трудно предсказать. В этом разделе описываются некоторые свойства элемента и их эффект на вышеуказанный планировщик. Затем предложения обеспечивают способы настройки или увеличения пользовательской логики.
Список динамических капсул
Как упоминалось ранее, dynamicCapsuleList объект действует как временная база данных, которая кэширует предсказанные траектории препятствий. Вы можете выполнить проверку столкновения с одним или несколькими ego телами в течение некоторого периода времени. The MaxNumSteps свойство определяет общее количество временных шагов, проверенных объектом. В вышеописанном цикле симуляции свойство было установлено на 31. Это значение означает, что планировщик проверяет весь 1-3-секундный диапазон любых траекторий (дискретизируется каждые 0,1 секунды). Теперь увеличьте максимальное значение в timeHorizons:
timeHorizons = 1:5; % in seconds maxTimeHorizon = max(timeHorizons); % in seconds
Сейчас существуют две опции:
The
MaxNumStepsсвойство остается неизменным.The
MaxNumStepsсвойство обновляется с учетом нового максимального временного интервала.
Если свойство остается неизменным, то список капсул только подтверждает первые 3 секунды любой траектории, что может быть предпочтительнее, если вычислительная эффективность является первостепенной или достоверность предсказания падает быстро.
Альтернативно, можно работать с достоверными данными (как показано выше), или будущее состояние окружение хорошо известно (например, полностью автоматизированная окружение с централизованным управлением). Поскольку этот пример использует достоверные данные для актёров, обновляйте свойство.
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); ax = gca; ax.ZLim = [-100 100]; tic; end end end
