Этот пример показывает, как сгенерировать сценарий, симулировать обнаружения датчиков и использовать слияние датчиков для отслеживания моделируемых транспортных средств. Основным преимуществом использования генерации сценария и симуляции датчика над записью датчика является возможность создавать редкие и потенциально опасные события и тестировать алгоритмы транспортного средства с ними.
Этот пример охватывает весь программный рабочий процесс генерации синтетических данных. Чтобы сгенерировать синтетические данные в интерактивном режиме, используйте приложение Driving Scenario Designer. Для получения примера смотрите, Создают Сценарий Вождения в интерактивном режиме и Генерируют Синтетические Данные Датчика.
Генерация сценария включает в себя формирование дорожной сети, определение транспортных средств, которые движутся по дорогам, и перемещение транспортных средств.
В этом примере вы проверяете способность слияния датчиков отслеживать транспортное средство, которая проходит слева от автомобиля , оборудованного датчиком. Сценарий моделирует настройку шоссе, и дополнительные транспортные средства находятся перед и позади автомобиля , оборудованного датчиком.
% Define an empty scenario.
scenario = drivingScenario;
scenario.SampleTime = 0.01;
Добавить участок 500 метров типовой автомобильной дороги с двумя полосами. Дорога определяется с помощью набора точек, где каждая точка определяет центр дороги в трехмерном пространстве. Добавьте барьер Джерси на правое ребро дороги.
roadCenters = [0 0; 50 0; 100 0; 250 20; 500 40];
mainRoad = road(scenario, roadCenters, 'lanes',lanespec(2));
barrier(scenario,mainRoad);
Создайте автомобиль , оборудованный датчиком и три машины вокруг него: один, который обгоняет автомобиль , оборудованный датчиком и пропускает его слева, один, который движется прямо перед автомобиль , оборудованный датчиком и один, который движется прямо за автомобиль , оборудованный датчиком. Все автомобили следуют по траектории, заданной путевыми точками дороги, при помощи trajectory
политика вождения. Проезжающий мимо автомобиль стартует по правой полосе, переедет на левую полосу, чтобы пройти, и вернется на правую полосу.
% Create the ego vehicle that travels at 25 m/s along the road. Place the % vehicle on the right lane by subtracting off half a lane width (1.8 m) % from the centerline of the road. egoCar = vehicle(scenario, 'ClassID', 1); trajectory(egoCar, roadCenters(2:end,:) - [0 1.8], 25); % On right lane % Add a car in front of the ego vehicle leadCar = vehicle(scenario, 'ClassID', 1); trajectory(leadCar, [70 0; roadCenters(3:end,:)] - [0 1.8], 25); % On right lane % Add a car that travels at 35 m/s along the road and passes the ego vehicle passingCar = vehicle(scenario, 'ClassID', 1); waypoints = [0 -1.8; 50 1.8; 100 1.8; 250 21.8; 400 32.2; 500 38.2]; trajectory(passingCar, waypoints, 35); % Add a car behind the ego vehicle chaseCar = vehicle(scenario, 'ClassID', 1); trajectory(chaseCar, [25 0; roadCenters(2:end,:)] - [0 1.8], 25); % On right lane
В этом примере вы симулируете автомобиль , оборудованный датчиком, которая имеет 6 радарных датчиков и 2 датчика зрения, покрывающих поле зрения степеней 360. Датчики имеют некоторое перекрытие и некоторую погрешность покрытия. Автомобиль , оборудованный датчиком оснащён радарным датчиком большой дальности и датчиком зрения как на передней, так и на задней стороне транспортного средства. Каждая сторона транспортного средства имеет два радиолокационных датчика ближнего действия, каждый из которых охватывает 90 степени. Один датчик на каждой боковой крышке от середины транспортного средства до задней части. Другой датчик на каждой боковой стороне закрывается с середины транспортного средства вперед. Рисунок в следующем разделе показывает покрытие.
sensors = cell(8,1); % Front-facing long-range radar sensor at the center of the front bumper of the car. sensors{1} = drivingRadarDataGenerator('SensorIndex', 1, 'RangeLimits', [0 174], ... 'MountingLocation', [egoCar.Wheelbase + egoCar.FrontOverhang, 0, 0.2], 'FieldOfView', [20, 5]); % Rear-facing long-range radar sensor at the center of the rear bumper of the car. sensors{2} = drivingRadarDataGenerator('SensorIndex', 2, 'MountingAngles', [180 0 0], ... 'MountingLocation', [-egoCar.RearOverhang, 0, 0.2], 'RangeLimits', [0 30], 'FieldOfView', [20, 5]); % Rear-left-facing short-range radar sensor at the left rear wheel well of the car. sensors{3} = drivingRadarDataGenerator('SensorIndex', 3, 'MountingAngles', [120 0 0], ... 'MountingLocation', [0, egoCar.Width/2, 0.2], 'RangeLimits', [0 30], 'ReferenceRange', 50, ... 'FieldOfView', [90, 5], 'AzimuthResolution', 10, 'RangeResolution', 1.25); % Rear-right-facing short-range radar sensor at the right rear wheel well of the car. sensors{4} = drivingRadarDataGenerator('SensorIndex', 4, 'MountingAngles', [-120 0 0], ... 'MountingLocation', [0, -egoCar.Width/2, 0.2], 'RangeLimits', [0 30], 'ReferenceRange', 50, ... 'FieldOfView', [90, 5], 'AzimuthResolution', 10, 'RangeResolution', 1.25); % Front-left-facing short-range radar sensor at the left front wheel well of the car. sensors{5} = drivingRadarDataGenerator('SensorIndex', 5, 'MountingAngles', [60 0 0], ... 'MountingLocation', [egoCar.Wheelbase, egoCar.Width/2, 0.2], 'RangeLimits', [0 30], ... 'ReferenceRange', 50, 'FieldOfView', [90, 5], 'AzimuthResolution', 10, ... 'RangeResolution', 1.25); % Front-right-facing short-range radar sensor at the right front wheel well of the car. sensors{6} = drivingRadarDataGenerator('SensorIndex', 6, 'MountingAngles', [-60 0 0], ... 'MountingLocation', [egoCar.Wheelbase, -egoCar.Width/2, 0.2], 'RangeLimits', [0 30], ... 'ReferenceRange', 50, 'FieldOfView', [90, 5], 'AzimuthResolution', 10, ... 'RangeResolution', 1.25); % Front-facing camera located at front windshield. sensors{7} = visionDetectionGenerator('SensorIndex', 7, 'FalsePositivesPerImage', 0.1, ... 'SensorLocation', [0.75*egoCar.Wheelbase 0], 'Height', 1.1); % Rear-facing camera located at rear windshield. sensors{8} = visionDetectionGenerator('SensorIndex', 8, 'FalsePositivesPerImage', 0.1, ... 'SensorLocation', [0.2*egoCar.Wheelbase 0], 'Height', 1.1, 'Yaw', 180); % Register actor profiles with the sensors. profiles = actorProfiles(scenario); for m = 1:numel(sensors) if isa(sensors{m},'drivingRadarDataGenerator') sensors{m}.Profiles = profiles; else sensors{m}.ActorProfiles = profiles; end end
Создайте
отслеживать автомобили, которые находятся рядом с автомобилем , оборудованным датчиком. Трекер использует multiObjectTracker
initSimDemoFilter
поддерживающая функция для инициализации линейного фильтра Калмана с постоянной скоростью, который работает с положением и скоростью.
Отслеживание выполняется в 2-D. Хотя датчики возвращают измерения в 3-D, само движение ограничено горизонтальной плоскостью, поэтому нет необходимости отслеживать высоту.
tracker = multiObjectTracker('FilterInitializationFcn', @initSimDemoFilter, ... 'AssignmentThreshold', 30, 'ConfirmationThreshold', [4 5]); positionSelector = [1 0 0 0; 0 0 1 0]; % Position selector velocitySelector = [0 1 0 0; 0 0 0 1]; % Velocity selector % Create the display and return a handle to the bird's-eye plot BEP = createDemoDisplay(egoCar, sensors);
Следующий цикл перемещает транспортные средства, вызывает симуляцию датчика и выполняет отслеживание.
Обратите внимание, что генерация сценария и симуляция датчика могут иметь различные временные шаги. Определение различных временных шагов для сценария и датчиков позволяет вам отсоединить симуляцию сценария от симуляции датчика. Это полезно для моделирования движения актёра с высокой точностью независимо от скорости измерения датчика.
Другой пример - когда датчики имеют разные частоты обновления. Предположим, что один датчик обеспечивает обновления каждые 20 миллисекунд, а другой - каждые 50 миллисекунд. Можно задать сценарий со частотой обновления 10 миллисекунд и датчики предоставят свои обновления в правильное время.
В этом примере генерация сценария имеет временной шаг 0,01 секунды, в то время как датчики обнаруживают каждые 0,1 секунды. Датчики возвращают логический флаг, isValidTime
, это правда, если датчики генерировали обнаружения. Этот флаг используется для вызова трекера только при наличии обнаружений.
Другое важное примечание заключается в том, что датчики могут имитировать несколько обнаружений по каждой цели, в частности, когда цели очень близки к радарным датчикам. Поскольку трекер принимает одно обнаружение на цель от каждого датчика, вы должны кластеризировать обнаружения, прежде чем трекер обработает их. Это осуществляется путем настройки TargetReportFormat
к 'Clustered detections', который является значением по умолчанию. Модель датчика может также выводить необработанные данные обнаружения или отслеживать обновления с помощью внутреннего трекера.
toSnap = true; while advance(scenario) && ishghandle(BEP.Parent) % Get the scenario time time = scenario.SimulationTime; % Get the position of the other vehicle in ego vehicle coordinates ta = targetPoses(egoCar); % Simulate the sensors detectionClusters = {}; isValidTime = false(1,8); for i = 1:8 [sensorDets,numValidDets,isValidTime(i)] = sensors{i}(ta, time); if numValidDets for j = 1:numValidDets % Vision detections do not report SNR. The tracker requires % that they have the same object attributes as the radar % detections. This adds the SNR object attribute to vision % detections and sets it to a NaN. if ~isfield(sensorDets{j}.ObjectAttributes{1}, 'SNR') sensorDets{j}.ObjectAttributes{1}.SNR = NaN; end % Remove the Z-component of measured position and velocity % from the Measurement and MeasurementNoise fields sensorDets{j}.Measurement = sensorDets{j}.Measurement([1 2 4 5]); sensorDets{j}.MeasurementNoise = sensorDets{j}.MeasurementNoise([1 2 4 5],[1 2 4 5]); end detectionClusters = [detectionClusters; sensorDets]; %#ok<AGROW> end end % Update the tracker if there are new detections if any(isValidTime) if isa(sensors{1},'drivingRadarDataGenerator') vehicleLength = sensors{1}.Profiles.Length; else vehicleLength = sensors{1}.ActorProfiles.Length; end confirmedTracks = updateTracks(tracker, detectionClusters, time); % Update bird's-eye plot updateBEP(BEP, egoCar, detectionClusters, confirmedTracks, positionSelector, velocitySelector); end % Snap a figure for the document when the car passes the ego vehicle if ta(1).Position(1) > 0 && toSnap toSnap = false; snapnow end end
Этот пример показов, как сгенерировать сценарий, моделировать обнаружения датчиков и использовать эти обнаружения для отслеживания движущихся транспортных средств по автомобилю , оборудованному датчиком.
Вы можете попытаться изменить сценарий дороги, или добавить или удалить транспортные средства. Можно также попытаться добавить, удалить или изменить датчики на автомобиль , оборудованный датчиком или изменить параметры трекера.
initSimDemoFilter
Эта функция инициализирует фильтр постоянной скорости на основе обнаружения.
function filter = initSimDemoFilter(detection) % Use a 2-D constant velocity model to initialize a trackingKF filter. % The state vector is [x;vx;y;vy] % The detection measurement vector is [x;y;vx;vy] % As a result, the measurement model is H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1] H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1]; filter = trackingKF('MotionModel', '2D Constant Velocity', ... 'State', H' * detection.Measurement, ... 'MeasurementModel', H, ... 'StateCovariance', H' * detection.MeasurementNoise * H, ... 'MeasurementNoise', detection.MeasurementNoise); end
createDemoDisplay
Эта функция создает трехпанельное отображение:
Верхний левый угол отображения: вид сверху, следующий за автомобилем , оборудованным датчиком.
Нижний левый угол отображения: вид камеры погони, который следует за автомобилем , оборудованным датчиком.
Правая половина отображения: A
отображение.birdsEyePlot
function BEP = createDemoDisplay(egoCar, sensors) % Make a figure hFigure = figure('Position', [0, 0, 1200, 640], 'Name', 'Sensor Fusion with Synthetic Data Example'); movegui(hFigure, [0 -1]); % Moves the figure to the left and a little down from the top % Add a car plot that follows the ego vehicle from behind hCarViewPanel = uipanel(hFigure, 'Position', [0 0 0.5 0.5], 'Title', 'Chase Camera View'); hCarPlot = axes(hCarViewPanel); chasePlot(egoCar, 'Parent', hCarPlot); % Add a car plot that follows the ego vehicle from a top view hTopViewPanel = uipanel(hFigure, 'Position', [0 0.5 0.5 0.5], 'Title', 'Top View'); hCarPlot = axes(hTopViewPanel); chasePlot(egoCar, 'Parent', hCarPlot, 'ViewHeight', 130, 'ViewLocation', [0 0], 'ViewPitch', 90); % Add a panel for a bird's-eye plot hBEVPanel = uipanel(hFigure, 'Position', [0.5 0 0.5 1], 'Title', 'Bird''s-Eye Plot'); % Create bird's-eye plot for the ego vehicle and sensor coverage hBEVPlot = axes(hBEVPanel); frontBackLim = 60; BEP = birdsEyePlot('Parent', hBEVPlot, 'Xlimits', [-frontBackLim frontBackLim], 'Ylimits', [-35 35]); % Plot the coverage areas for radars for i = 1:6 cap = coverageAreaPlotter(BEP,'FaceColor','red','EdgeColor','red'); if isa(sensors{i},'drivingRadarDataGenerator') plotCoverageArea(cap, sensors{i}.MountingLocation(1:2),... sensors{i}.RangeLimits(2), sensors{i}.MountingAngles(1), sensors{i}.FieldOfView(1)); else plotCoverageArea(cap, sensors{i}.SensorLocation,... sensors{i}.MaxRange, sensors{i}.Yaw, sensors{i}.FieldOfView(1)); end end % Plot the coverage areas for vision sensors for i = 7:8 cap = coverageAreaPlotter(BEP,'FaceColor','blue','EdgeColor','blue'); if isa(sensors{i},'drivingRadarDataGenerator') plotCoverageArea(cap, sensors{i}.MountingLocation(1:2),... sensors{i}.RangeLimits(2), sensors{i}.MountingAngles(1), 45); else plotCoverageArea(cap, sensors{i}.SensorLocation,... sensors{i}.MaxRange, sensors{i}.Yaw, 45); end end % Create a vision detection plotter put it in a struct for future use detectionPlotter(BEP, 'DisplayName','vision', 'MarkerEdgeColor','blue', 'Marker','^'); % Combine all radar detections into one entry and store it for later update detectionPlotter(BEP, 'DisplayName','radar', 'MarkerEdgeColor','red'); % Add road borders to plot laneMarkingPlotter(BEP, 'DisplayName','lane markings'); % Add the tracks to the bird's-eye plot. Show last 10 track updates. trackPlotter(BEP, 'DisplayName','track', 'HistoryDepth',10); axis(BEP.Parent, 'equal'); xlim(BEP.Parent, [-frontBackLim frontBackLim]); ylim(BEP.Parent, [-40 40]); % Add an outline plotter for ground truth outlinePlotter(BEP, 'Tag', 'Ground truth'); end
updateBEP
Эта функция обновляет график птичьего глаза с помощью контуров дорог, обнаружений и треков.
function updateBEP(BEP, egoCar, detections, confirmedTracks, psel, vsel) % Update road boundaries and their display [lmv, lmf] = laneMarkingVertices(egoCar); plotLaneMarking(findPlotter(BEP,'DisplayName','lane markings'),lmv,lmf); % update ground truth data [position, yaw, length, width, originOffset, color] = targetOutlines(egoCar); plotOutline(findPlotter(BEP,'Tag','Ground truth'), position, yaw, length, width, 'OriginOffset', originOffset, 'Color', color); % update barrier data [bPosition,bYaw,bLength,bWidth,bOriginOffset,bColor,numBarrierSegments] = targetOutlines(egoCar, 'Barriers'); plotBarrierOutline(findPlotter(BEP,'Tag','Ground truth'),numBarrierSegments,bPosition,bYaw,bLength,bWidth,... 'OriginOffset',bOriginOffset,'Color',bColor); % Prepare and update detections display N = numel(detections); detPos = zeros(N,2); isRadar = true(N,1); for i = 1:N detPos(i,:) = detections{i}.Measurement(1:2)'; if detections{i}.SensorIndex > 6 % Vision detections isRadar(i) = false; end end plotDetection(findPlotter(BEP,'DisplayName','vision'), detPos(~isRadar,:)); plotDetection(findPlotter(BEP,'DisplayName','radar'), detPos(isRadar,:)); % Remove all object tracks that are unidentified by the vision detection % generators before updating the tracks display. These have the ObjectClassID % parameter value as 0 and include objects such as barriers. isNotBarrier = arrayfun(@(t)t.ObjectClassID,confirmedTracks)>0; confirmedTracks = confirmedTracks(isNotBarrier); % Prepare and update tracks display trackIDs = {confirmedTracks.TrackID}; labels = cellfun(@num2str, trackIDs, 'UniformOutput', false); [tracksPos, tracksCov] = getTrackPositions(confirmedTracks, psel); tracksVel = getTrackVelocities(confirmedTracks, vsel); plotTrack(findPlotter(BEP,'DisplayName','track'), tracksPos, tracksVel, tracksCov, labels); end
birdsEyePlot
| drivingRadarDataGenerator
| drivingScenario
| multiObjectTracker
| visionDetectionGenerator