Этот пример показывает вам, как выполнить динамическое перепланирование в городской ведущей сцене с помощью пути к ссылке Frenet. В этом примере вы используете динамическую оценку карты сетки заполнения окружения, чтобы найти оптимальные локальные траектории.
Динамическое перепланирование автономных транспортных средств обычно делается с локальным планировщиком движения. Локальный планировщик движения ответственен за генерацию оптимальной траектории на основе глобального плана и информации об окружающей среде. Информация об окружающей среде может быть описана в основном двумя способами:
Дискретный набор объектов в окружающей среде с заданными конфигурациями.
Дискретизированная сетка с оценкой о свободных и занятых областях в окружающей среде.
В присутствии динамических препятствий в среде локальный планировщик движения требует, чтобы краткосрочные предсказания информации о среде оценили валидность запланированных траекторий. Выбором представления среды обычно управляет восходящий алгоритм восприятия. Для планирования алгоритмов основанное на объектах представление предлагает эффективное памятью описание среды. Это также допускает более легкий путь к отношениям межобъекта define для предсказания поведения. С другой стороны, основанный на сетке подход допускает представление без объектных моделей, которое помогает в эффективных регистрирующихся в столкновении комплексных сценариях с большим количеством объектов. Основанное на сетке представление также менее чувствительно к недостаткам объектной экстракции, таким как ложь и пропущенные цели. Гибрид этих двух подходов также возможен путем извлечения объектной гипотезы из основанного на сетке представления.
В этом примере вы представляете окружающую среду как динамическую карту сетки заполнения. Для примера с помощью дискретного набора объектов отошлите к Магистральному Планированию Траектории Используя Путь к Ссылке Frenet (Navigation Toolbox) пример. Динамическая карта сетки заполнения является основанной на сетке оценкой окружения вокруг автомобиля, оборудованного датчиком. В дополнение к оценке вероятности заполнения динамическая сетка заполнения также оценивает кинематические атрибуты каждой ячейки, такие как скорость, угловая скорость вращения и ускорение. Далее, оценки от динамической сетки могут быть предсказаны в течение короткого времени в будущем, чтобы оценить заполнение окружения в ближайшем будущем. В этом примере вы получаете основанную на сетке оценку среды путем плавления облаков точек от шести лидаров, смонтированных на автомобиле, оборудованном датчиком.
Сценарий, используемый в этом примере, представляет городскую перекрестную сцену и содержит множество объектов, включая пешеходов, велосипедистов, автомобили и грузовики. Автомобиль, оборудованный датчиком оборудован шестью однородными датчиками лидара, каждым с полем зрения 90 градусов, предоставив полную страховую защиту вокруг автомобиля, оборудованного датчиком. Для получения дополнительной информации о сценарии и моделях датчика, отошлите к Основанному на сетке Отслеживанию в Городских Средах Используя Несколько Лидаров (Sensor Fusion and Tracking Toolbox) пример. Определение сценария и датчиков перенесено в функцию помощника helperGridBasedPlanningScenario
.
% For reproducible results rng(2020); % Create scenario, ego vehicle and simulated lidar sensors [scenario, egoVehicle, lidars] = helperGridBasedPlanningScenario;
Теперь задайте основанное на сетке средство отслеживания с помощью trackerGridRFS
(Sensor Fusion and Tracking Toolbox) Система object™. Средство отслеживания выходные параметры и уровень объектов и оценка уровня сети среды. Оценка уровня сети описывает заполнение и состояние окружения и может быть получена как четвертый выход из средства отслеживания. Для получения дополнительной информации о том, как настроить основанное на сетке средство отслеживания, отошлите к Основанному на сетке Отслеживанию в Городских Средах Используя Несколько Лидаров (Sensor Fusion and Tracking Toolbox) пример.
% Set up sensor configurations for each lidar sensorConfigs = cell(numel(lidars),1); % Fill in sensor configurations for i = 1:numel(sensorConfigs) sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle); end % Set up tracker tracker = trackerGridRFS('SensorConfigurations',sensorConfigs,... 'HasSensorConfigurationsInput',true,... 'GridLength',120,... 'GridWidth',120,... 'GridResolution',2,... 'GridOriginInLocal',[-60 -60],... 'NumParticles',1e5,... 'NumBirthParticles',2e4,... 'VelocityLimits',[-15 15;-15 15],... 'BirthProbability',0.025,... 'ProcessNoise',5*eye(2),... 'DeathRate',1e-3,... 'FreeSpaceDiscountFactor',1e-2,... 'AssignmentThreshold',8,... 'MinNumCellsPerCluster',4,... 'ClusteringThreshold',4,... 'ConfirmationThreshold',[3 4],... 'DeletionThreshold',[4 4]);
Настройте локальный алгоритм планирования движения, чтобы запланировать оптимальные траектории в координатах Frenet вдоль глобального ссылочного пути.
Задайте глобальный ссылочный путь с помощью referencePathFrenet
Объект (Navigation Toolbox) путем обеспечения waypoints в системе координат Декартовой координаты ведущего сценария. Ссылочный путь, используемый в этом примере, задает путь, который поворачивает направо на пересечении.
waypoints = [-110.6 -4.5 0; 49 -4.5 0; 55.5 -17.7 -pi/2; 55.5 -130.6 -pi/2]; % [x y theta] % Create a reference path using waypoints refPath = referencePathFrenet(waypoints); % Visualize the reference path fig = figure('Units','normalized','Position',[0.1 0.1 0.8 0.8]); ax = axes(fig); hold(ax,'on'); plot(scenario,'Parent',ax); show(refPath,'Parent',ax); xlim(ax,[-120 80]); ylim(ax,[-160 40]);
snapnow;
Локальный алгоритм планирования движения в этом примере состоит из трех основных шагов:
Демонстрационные локальные траектории
Найдите выполнимые и траектории без коллизий
Выберите критерий оптимальности и выберите оптимальную траекторию
Следующие разделы обсуждают каждый шаг локального алгоритма планирования, и функции помощника раньше выполняли каждый шаг.
На каждом шаге симуляции алгоритм планирования генерирует список демонстрационных траекторий, которые может выбрать автомобиль, оборудованный датчиком. Локальные траектории производятся путем соединения текущего состояния автомобиля, оборудованного датчиком к желаемым конечным состояниям. Используйте trajectoryGeneratorFrenet
Объект (Navigation Toolbox) соединиться текущий и конечные состояния для генерации локальных траекторий. Задайте объект путем обеспечения ссылочного пути и нужного разрешения как раз к траектории. Объект соединяет начальные и конечные состояния в координатах Frenet с помощью полиномов пятого порядка.
connector = trajectoryGeneratorFrenet(refPath,'TimeResolution',0.1);
Стратегия выборки конечных состояний в координатах Frenet часто зависит от дорожной сети и желаемого поведения автомобиля, оборудованного датчиком во время различных фаз глобального пути. Для более подробных примеров использования различного поведения эго, таких как круиз-контроль и следование автомобиля, отсылают к разделу "Planning Adaptive Routes Through Traffic" Магистрального Планирования Траектории Используя Путь к Ссылке Frenet (Navigation Toolbox) пример. В этом примере вы производите конечные состояния с помощью двух различных стратегий, в зависимости от местоположения транспортного средства на ссылочном пути, показавшем синими и зелеными областями в следующем рисунке.
% Visualize path regions for sampling strategy visualization pathPoints = closestPoint(refPath, refPath.Waypoints(:,1:2)); roadS = pathPoints(:,end); intersectionS = roadS(2,end); intersectionBuffer = 20; pathGreen = [interpolate(refPath,linspace(0,intersectionS-intersectionBuffer,20));... nan(1,6);... interpolate(refPath,linspace(intersectionS,roadS(end),100))]; pathBlue = interpolate(refPath,linspace(intersectionS-intersectionBuffer,roadS(2,end),20)); hold(ax,'on'); plot(ax,pathGreen(:,1),pathGreen(:,2),'Color',[0 1 0],'LineWidth',5); plot(ax,pathBlue(:,1),pathBlue(:,2),'Color',[0 0 1],'LineWidth',5);
snapnow;
Когда автомобиль, оборудованный датчиком находится в зеленой области, следующая стратегия используется, чтобы произвести локальные траектории. Конечное состояние автомобиля, оборудованного датчиком после время задано как:
где дискретные выборки для переменных получены с помощью следующих предопределенных наборов:
Использование NaN
в конечном состоянии включает trajectoryGeneratorFrenet
возразите, чтобы автоматически вычислить продольное расстояние, путешествовавшее по траектории минимального толчка. Эта стратегия производит набор траекторий, которые позволяют автомобилю, оборудованному датчиком ускориться до предела максимальной скорости () уровни или замедляются к точке на различных уровнях. Кроме того, произведенный выбор бокового смещения () позвольте автомобилю, оборудованному датчиком перестраиваться на другую полосу во время этих маневров.
% Define smax and wlane
speedLimit = 15;
laneWidth = 2.975;
Когда автомобиль, оборудованный датчиком находится в синей области траектории, следующая стратегия используется, чтобы произвести локальные траектории:
;
где выбран, чтобы минимизировать толчок во время траектории. Эта стратегия позволяет транспортному средству остановиться на желаемом расстоянии () в правильном маршруте с траекторией минимального толчка. Алгоритм выборки траектории перенесен в функции помощника, helperGenerateTrajectory
, присоединенный с этим примером.
Процесс выборки, описанный в предыдущем разделе, может произвести траектории, которые кинематическим образом неосуществимы и превышают пороги кинематических атрибутов, такие как ускорение и искривление. Поэтому вы ограничиваете максимальное ускорение и скорость автомобиля, оборудованного датчиком с помощью функции помощника helperKinematicFeasibility, который проверяет выполнимость каждой траектории против этих кинематических ограничений.
% Define kinematic constraints
accMax = 15;
Далее, вы настраиваете блок проверки допустимости столкновения, чтобы оценить, если автомобиль, оборудованный датчиком может маневрировать на кинематическим образом выполнимой траектории, не сталкиваясь ни с какими другими препятствиями в среде. Чтобы задать блок проверки допустимости, используйте класс помощника HelperDynamicMapValidator
. Этот класс использует predictMapToTime
(Sensor Fusion and Tracking Toolbox) функция trackerGridRFS
объект получить краткосрочные предсказания заполнения окружающей среды. Начиная с неопределенности в оценочных увеличениях со временем сконфигурируйте блок проверки допустимости период времени имеющий 2 секунд.
Предсказанное заполнение среды преобразовано в расширенный costmap на каждом шаге с учетом размера автомобиля, оборудованного датчиком. Планировщик пути использует такт 0,1 секунд с периодом времени предсказания 2 секунд. Чтобы уменьшать вычислительную сложность, заполнение окружающей среды принято, чтобы быть допустимым для 5 временных шагов, или 0,5 секунды. В результате только 4 предсказания требуются в 2-секундном горизонте планирования. В дополнение к принятию решений из двух альтернатив относительно столкновения или никакого столкновения, блок проверки допустимости также обеспечивает меру вероятности столкновения автомобиля, оборудованного датчиком. Эта вероятность может быть включена в функцию стоимости для критериев оптимальности с учетом неопределенности в системе и принять лучшие решения, не увеличивая период времени планировщика.
vehDims = vehicleDimensions(egoVehicle.Length,egoVehicle.Width); collisionValidator = HelperDynamicMapValidator('MaxTimeHorizon',2, ... % Maximum horizon for validation 'TimeResolution',connector.TimeResolution, ... % Time steps between trajectory samples 'Tracker',tracker, ... % Provide tracker for prediction 'ValidPredictionSpan',5, ... % Prediction valid for 5 steps 'VehicleDimensions',vehDims); % Provide dimensions of ego
После проверки выполнимых траекторий против препятствий или занятых областей среды, выберите критерий оптимальности для каждой допустимой траектории путем определения функции стоимости для траекторий. Различные функции стоимости, как ожидают, произведут различные поведения из автомобиля, оборудованного датчиком. В этом примере вы задаете стоимость каждой траектории как
где:
толчок в продольном направлении ссылочного пути
толчок в боковом направлении ссылочного пути
вероятность столкновения, полученная блоком проверки допустимости
Расчет стоимости для каждой траектории задан с помощью функции помощника helperCalculateTrajectoryCosts. Из списка допустимых траекторий траектория с минимальной стоимостью рассматривается как оптимальную траекторию.
Запустите сценарий, сгенерируйте облака точек от всех датчиков лидара и оцените динамическую карту сетки заполнения. Используйте динамическую оценку карты и ее предсказания, чтобы запланировать локальную траекторию автомобиль, оборудованный датчиком.
% Close original figure and initialize a new display close(fig); display = helperGridBasedPlanningDisplay; % Initial ego state currentEgoState = [-110.6 -1.5 0 0 15 0]; helperMoveEgoVehicleToState(egoVehicle, currentEgoState); % Initialize pointCloud outputs from each sensor ptClouds = cell(numel(lidars),1); sensorConfigs = cell(numel(lidars),1); % Simulation Loop while advance(scenario) % Current simulation time time = scenario.SimulationTime; % Poses of objects with respect to ego vehicle tgtPoses = targetPoses(egoVehicle); % Simulate point cloud from each sensor for i = 1:numel(lidars) [ptClouds{i}, isValidTime] = step(lidars{i},tgtPoses,time); sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle); end % Pack point clouds as sensor data format required by the tracker sensorData = packAsSensorData(ptClouds,sensorConfigs,time); % Call the tracker [tracks, ~, ~, map] = tracker(sensorData,sensorConfigs,time); % Update validator's future predictions using current estimate step(collisionValidator, currentEgoState, map, time); % Sample trajectories using current ego state and some kinematic % parameters [frenetTrajectories, globalTrajectories] = helperGenerateTrajectory(connector, refPath, currentEgoState, speedLimit, laneWidth, intersectionS, intersectionBuffer); % Calculate kinematic feasibility of generated trajectories isKinematicsFeasible = helperKinematicFeasibility(frenetTrajectories,speedLimit,accMax); % Calculate collision validity of feasible trajectories feasibleGlobalTrajectories = globalTrajectories(isKinematicsFeasible); feasibleFrenetTrajectories = frenetTrajectories(isKinematicsFeasible); [isCollisionFree, collisionProb] = isTrajectoryValid(collisionValidator, feasibleGlobalTrajectories); % Calculate costs and final optimal trajectory nonCollidingGlobalTrajectories = feasibleGlobalTrajectories(isCollisionFree); nonCollidingFrenetTrajectories = feasibleFrenetTrajectories(isCollisionFree); nonCollodingCollisionProb = collisionProb(isCollisionFree); costs = helperCalculateTrajectoryCosts(nonCollidingFrenetTrajectories, nonCollodingCollisionProb, speedLimit); % Find optimal trajectory [~,idx] = min(costs); optimalTrajectory = nonCollidingGlobalTrajectories(idx); % Assemble for plotting trajectories = helperAssembleTrajectoryForPlotting(globalTrajectories, ... isKinematicsFeasible, isCollisionFree, idx); % Update display display(scenario, egoVehicle, lidars, ptClouds, tracker, tracks, trajectories, collisionValidator); % Move ego with optimal trajectory if ~isempty(optimalTrajectory) currentEgoState = optimalTrajectory.Trajectory(2,:); helperMoveEgoVehicleToState(egoVehicle, currentEgoState); else % All trajectories either violated kinematic feasibility % constraints or resulted in a collision. More behaviors on % trajectory sampling may be needed. error('Unable to compute optimal trajectory'); end end
Анализируйте результаты локального алгоритма планирования пути и как предсказания из карты помогли планировщику. Эта анимация показывает результат алгоритма планирования во время целого сценария. Заметьте, что автомобиль, оборудованный датчиком успешно достиг своего желаемого места назначения и маневрировал вокруг различных динамических объектов, каждый раз, когда необходимо. Автомобиль, оборудованный датчиком также остановился на пересечении из-за региональных изменений, добавленных к политике выборки.
Затем анализируйте локальный алгоритм планирования во время первого изменения маршрута. Снимки состояния в этом разделе получены во время = 4,3 секунды во время симуляции.
В этом снимке состояния автомобиль, оборудованный датчиком только что начал выполнять маневр изменения маршрута в правильный маршрут.
showSnaps(display, 3, 1);
Снимок состояния, который следует, показывает, что оценка динамической сетки одновременно продвигается. Цвет ячейки сетки обозначает направление движения объекта, занимающего ту ячейку сетки. Заметьте, что ячейки, представляющие автомобиль перед автомобилем, оборудованным датчиком, окрашены в красный, обозначив, что ячейки заняты динамическим объектом. Кроме того, автомобиль перемещается в положительное направление X сценария, таким образом, на основе цветового диска, цвет соответствующих ячеек сетки является красным.
f = showSnaps(display, 2, 1); if ~isempty(f) ax = findall(f,'Type','Axes'); ax.XLim = [0 40]; ax.YLim = [-20 20]; s = findall(ax,'Type','Surf'); s.XData = 36 + 1/3*(s.XData - mean(s.XData(:))); s.YData = 16 + 1/3*(s.YData - mean(s.YData(:))); end
На основе предыдущего изображения запланированная траектория автомобиля, оборудованного датчиком проходит через занятые области пробела, представляя столкновение, если вы выполнили традиционную статическую валидацию заполнения. Динамическая карта заполнения и блок проверки допустимости, однако, составляют динамический характер сетки путем проверки состояния траектории против предсказанного заполнения на каждом временном шаге. Следующий снимок состояния показывает предсказанный costmap на различных шагах предсказания (), наряду с запланированным положением автомобиля, оборудованного датчиком на траектории. Предсказанный costmap раздут с учетом размера автомобиля, оборудованного датчиком. Поэтому, если точечный объект, представляющий источник автомобиля, оборудованного датчиком, может быть помещен в карту заполнения без столкновения, это может быть интерпретировано, что автомобиль, оборудованный датчиком не сталкивается ни с каким препятствием. Желтые области на costmap обозначают области с гарантируемыми столкновениями с препятствием. Вероятность столкновения затухает за пределами желтых областей экспоненциально до конца области инфляции. Синие области указывают на области с нулевой вероятностью столкновения согласно текущему предсказанию.
Заметьте, что желтая область, представляющая автомобиль перед автомобилем, оборудованным датчиком, продвигается на costmap, когда карта предсказана в будущем. Это отражает, что предсказание заполнения рассматривает скорость объектов в окружающей среде. Кроме того, заметьте, что ячейки, классифицированные как статические объекты, остались относительно статическими на сетке во время предсказания. Наконец, заметьте, что запланированное положение источника автомобиля, оборудованного датчиком не сталкивается ни с какими занятыми областями в карте стоимости. Это показывает, что автомобиль, оборудованный датчиком может успешно маневрировать на этой траектории.
f = showSnaps(display, 1, 1); if ~isempty(f) ax = findall(f,'Type','Axes'); for i = 1:numel(ax) ax(i).XLim = [0 40]; ax(i).YLim = [-20 20]; end end
В этом примере вы изучили, как использовать динамические предсказания карты от основанного на сетке средства отслеживания, trackerGridRFS
, и как интегрировать динамическую карту с локальным алгоритмом планирования пути, чтобы сгенерировать траектории для автомобиля, оборудованного датчиком в динамических сложных средах. Вы также изучили, как динамический характер заполнения может использоваться, чтобы запланировать траектории более эффективно в среде.
function sensorData = packAsSensorData(ptCloud, configs, time) % Pack the sensor data as format required by the tracker % % ptCloud - cell array of pointCloud object % configs - cell array of sensor configurations % time - Current simulation time %The lidar simulation returns outputs as pointCloud objects. The Location %property of the point cloud is used to extract x,y, and z locations of %returns and pack them as structures with information required by a tracker. sensorData = struct('SensorIndex',{},... 'Time', {},... 'Measurement', {},... 'MeasurementParameters', {}); for i = 1:numel(ptCloud) % This sensor's point cloud thisPtCloud = ptCloud{i}; % Allows mapping between data and configurations without forcing an % ordered input and requiring configuration input for static sensors. sensorData(i).SensorIndex = configs{i}.SensorIndex; % Current time sensorData(i).Time = time; % Exctract Measurement as a 3-by-N defining locations of points sensorData(i).Measurement = reshape(thisPtCloud.Location,[],3)'; % Data is reported in the sensor coordinate frame and hence measurement % parameters are same as sensor transform parameters. sensorData(i).MeasurementParameters = configs{i}.SensorTransformParameters; end end function config = helperGetLidarConfig(lidar, ego) % Get configuration of the lidar sensor for tracker % % config - Configuration of the lidar sensor in the world frame % lidar - lidarPointCloudGeneration object % ego - driving.scenario.Actor in the scenario % Define transformation from sensor to ego senToEgo = struct('Frame',fusionCoordinateFrameType(1),... 'OriginPosition',[lidar.SensorLocation(:);lidar.Height],... 'Orientation',rotmat(quaternion([lidar.Yaw lidar.Pitch lidar.Roll],'eulerd','ZYX','frame'),'frame'),... 'IsParentToChild',true); % Define transformation from ego to tracking coordinates egoToScenario = struct('Frame',fusionCoordinateFrameType(1),... 'OriginPosition',ego.Position(:),... 'Orientation',rotmat(quaternion([ego.Yaw ego.Pitch ego.Roll],'eulerd','ZYX','frame'),'frame'),... 'IsParentToChild',true); % Assemble using trackingSensorConfiguration. config = trackingSensorConfiguration(... 'SensorIndex',lidar.SensorIndex,... 'IsValidTime', true,... 'SensorLimits',[lidar.AzimuthLimits;0 lidar.MaxRange],... 'SensorTransformParameters',[senToEgo;egoToScenario],... 'DetectionProbability',0.95); end function helperMoveEgoVehicleToState(egoVehicle, currentEgoState) % Move ego vehicle in scenario to a state calculated by the planner % % egoVehicle - driving.scenario.Actor in the scenario % currentEgoState - [x y theta kappa speed acc] % Set 2-D Position egoVehicle.Position(1:2) = currentEgoState(1:2); % Set 2-D Velocity (s*cos(yaw) s*sin(yaw)) egoVehicle.Velocity(1:2) = [cos(currentEgoState(3)) sin(currentEgoState(3))]*currentEgoState(5); % Set Yaw in degrees egoVehicle.Yaw = currentEgoState(3)*180/pi; % Set angular velocity in Z (yaw rate) as v/r egoVehicle.AngularVelocity(3) = currentEgoState(4)*currentEgoState(5); end function isFeasible = helperKinematicFeasibility(frenetTrajectories, speedLimit, aMax) % Check kinematic feasibility of trajectories % % frenetTrajectories - Array of trajectories in Frenet coordinates % speedLimit - Speed limit (m/s) % aMax - Maximum acceleration (m/s^2) isFeasible = false(numel(frenetTrajectories),1); for i = 1:numel(frenetTrajectories) % Speed of the trajectory speed = frenetTrajectories(i).Trajectory(:,2); % Acceleration of the trajectory acc = frenetTrajectories(i).Trajectory(:,3); % Is speed valid? isSpeedValid = ~any(speed < -0.1 | speed > speedLimit + 1); % Is acceleration valid? isAccelerationValid = ~any(abs(acc) > aMax); % Trajectory feasible if both speed and acc valid isFeasible(i) = isSpeedValid & isAccelerationValid; end end function cost = helperCalculateTrajectoryCosts(frenetTrajectories, Pc, smax) % Calculate cost for each trajectory. % % frenetTrajectories - Array of trajectories in Frenet coordinates % Pc - Probability of collision for each trajectory calculated by validator n = numel(frenetTrajectories); Jd = zeros(n,1); Js = zeros(n,1); s = zeros(n,1); for i = 1:n % Time time = frenetTrajectories(i).Times; % resolution dT = time(2) - time(1); % Jerk along the path dds = frenetTrajectories(i).Trajectory(:,3); Js(i) = sum(gradient(dds,time).^2)*dT; % Jerk perpendicular to path % d2L/dt2 = d/dt(dL/ds*ds/dt) ds = frenetTrajectories(i).Trajectory(:,2); ddL = frenetTrajectories(i).Trajectory(:,6).*(ds.^2) + frenetTrajectories(i).Trajectory(:,5).*dds; Jd(i) = sum(gradient(ddL,time).^2)*dT; s(i) = frenetTrajectories(i).Trajectory(end,2); end cost = Js + Jd + 1000*Pc(:) + 100*(s - smax).^2; end