В этом примере показано, как планировать локальную траекторию в сценарии движения по шоссе. В этом примере используется опорная траектория и динамический список препятствий для создания альтернативных траекторий для эго-транспортного средства. Эго-транспортное средство осуществляет навигацию по трафику, определенному в предоставленном сценарии вождения из drivingScenario объект. Транспортное средство чередуется между адаптивным круиз-контролем, изменением полосы движения и последующими маневрами транспортного средства в зависимости от стоимости, осуществимости и движения без столкновения.
Начните с загрузки предоставленного drivingScenario , определяющий свойства транспортного средства и дороги в текущем рабочем пространстве. Этот сценарий был создан с помощью приложения Driving Script Designer и экспортирован в функцию MATLAB ® ,drivingScenarioTrafficExample. Дополнительные сведения см. в разделе Программное создание вариантов сценария управления.
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');
Для локального планировщика целью обычно является выборка различных возможных движений, которые движутся к конечной цели при удовлетворении текущих кинематических и динамических условий. trajectoryGeneratorFrenet Объект (Navigation Toolbox) выполняет это путем соединения начального состояния с набором терминальных состояний с помощью полиномиальных траекторий 4-го или 5-го порядка. Начальное и конечное состояния определяются в системе координат Френета, и каждое полиномиальное решение удовлетворяет граничным условиям бокового и продольного положения, скорости и ускорения при минимизации рывка.
Состояния клемм часто вычисляются с использованием пользовательского поведения. Это поведение использует информацию, найденную в локальной среде, такую как информация о полосе движения, ограничение скорости и текущие или будущие прогнозы субъектов в окрестностях эго-транспортного средства.
Построить trajectoryGeneratorFrenet объект с использованием пути ссылки
connector = trajectoryGeneratorFrenet(refPath);
dynamicCapsuleList Объект (Navigation Toolbox) - это структура данных, представляющая состояние динамической среды в дискретном наборе временных шагов. Эта среда может затем использоваться для эффективной проверки нескольких потенциальных траекторий для эго-транспортного средства. Каждый объект в сцене представлен следующим образом:
Уникальный целочисленный идентификатор
Свойства геометрии капсулы, используемые для эффективной проверки столкновений
Последовательность состояний SE2, где каждое состояние представляет дискретный снимок во времени.
В этом примере траектории, сгенерированные trajectoryGeneratorFrenet объект происходит в течение некоторого периода времени, известного как временной горизонт. Чтобы убедиться, что проверка столкновений охватывает все возможные траектории, dynamicCapsuleList объект должен содержать прогнозируемые траектории всех действующих лиц, охватывающие максимально ожидаемый временной горизонт.
capList = dynamicCapsuleList;
Создайте структуру геометрии для транспортного средства ego с заданными параметрами.
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-го порядка для решения задачи продольного граничного значения, что приводит к траектории, плавно совпадающей со скоростью дороги на данном временном горизонте:
0]
exampleHelperBasicLaneChange функция генерирует терминальные состояния, которые переводят транспортное средство из текущей полосы движения в любую смежную полосу движения. Функция сначала определяет текущую полосу движения эго-транспортного средства, а затем проверяет наличие левой и правой полос движения. Для каждой существующей полосы конечное состояние определяется таким же образом, как и поведение круиз-контроля, за исключением того, что конечная скорость устанавливается на текущую скорость, а не на ограничение скорости дороги:
exampleHelperBasicLeadVehicleFollow функция генерирует состояния терминала, которые пытаются отследить транспортное средство, обнаруженное перед эго-транспортным средством. Функция сначала определяет текущую полосу движения эго-транспортного средства. Для каждого предоставленного timeHorizon, функция предсказывает будущее состояние каждого актера, находит всех актёров, которые занимают ту же полосу, что и эго-транспортное средство, и определяет, какое из них является ближайшим ведущим транспортным средством (если ведущие транспортные средства не найдены, функция ничего не возвращает).
Состояние терминала эго-транспортного средства рассчитывается путем определения положения и скорости ведущего транспортного средства и уменьшения продольного положения терминала на некоторое безопасное расстояние:
0]
После формирования состояний терминала их стоимость может быть оценена. Оценка траектории и способы определения приоритетности потенциальных решений являются весьма субъективными. Ради простоты, exampleHelperEvaluateTSCost функция определяет стоимость как комбинацию трех взвешенных сумм.
Стоимость бокового отклонения () - положительный вес, который штрафует состояния, которые отклоняются от центра полосы.
* ΔL
Llanei |)
Временная стоимость () - отрицательный вес, который определяет приоритеты движений, происходящих на более длинном интервале, что приводит к более плавным траекториям.
* Δt
Terminal Velocity Cost () - положительный вес, который определяет приоритеты движений, которые поддерживают ограничение скорости, что приводит к менее динамическим маневрам.
* Δv
Помимо наличия конечных состояний с минимальными затратами, оптимальная траектория часто должна удовлетворять дополнительным ограничениям, связанным с кинематической осуществимостью и комфортом езды. Ограничения траектории являются одним из способов обеспечения желаемого качества езды, но они делают это за счет уменьшенного диапазона вождения.
В этом примере 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 зритель.
[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 объект выступает в качестве временной базы данных, которая кэширует предсказанные траектории препятствий. Можно выполнить проверку столкновений с одним или несколькими эго-телами в течение некоторого периода времени. MaxNumSteps определяет общее количество шагов времени, проверяемых объектом. В приведенном выше цикле моделирования свойство было установлено равным 31. Это значение означает, что плановик проверяет весь 1-3-секундный диапазон любых траекторий (отбираемых через каждые 0,1 секунды). Теперь увеличьте максимальное значение в timeHorizons:
timeHorizons = 1:5; % in seconds maxTimeHorizon = max(timeHorizons); % in seconds
Теперь существует два варианта:
MaxNumSteps свойство остается неизменным.
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
drivingScenario | referencePathFrenet (Панель инструментов навигации) | trajectoryGeneratorFrenet(Панель инструментов навигации)