Этот пример показов, как сплавить дорожки от двух транспортных средств в порядок, чтобы предоставить более полную оценку окружения, которая может быть видна каждому транспортному средству. Пример демонстрирует использование фузера уровня дорожки и формата данных дорожки объекта. В этом примере вы используете сценарий вождения и модели из Automated Driving Toolbox™, а также модели слияния и отслеживания из Sensor Fusion and Tracking Toolbox™.
Приложения безопасности автомобиля полагаются на слияние данных из различных сенсорных систем, установленных на транспортном средстве. Отдельные транспортные средства взрывают обнаружения датчиков при помощи либо централизованного трекера, либо путем более децентрализованного подхода и сплавления треков, производимых отдельными датчиками. В дополнение к слиянию интравехикальных данных слияние данных из нескольких транспортных средств дает дополнительные преимущества, которые включают лучший охват, ситуационную осведомленность и безопасность. [1] Этот подход к слиянию датчиков межтранспортного средства использует преимущества разнообразия датчиков и обеспечивает лучшее покрытие для каждого автомобиля, потому что он использует данные, обновленные датчиками на других транспортных средствах в области. Правительства и производители транспортных средств давно признают необходимость обмена информацией между автотранспортными средствами в целях повышения безопасности автомобилей. Например, была создана Специализированная служба связи малой дальности (DSRC) для предоставления услуг связи для совместного использования информацией о межтранспортном средстве. [2]
Хотя слияние датчиков между несколькими транспортными средствами выгодно, большинство транспортных средств должны соответствовать определенным требованиям безопасности, даже если доступны только внутренние датчики. Поэтому транспортное средство, вероятно, будет оснащено трекером и/или трековым фьюзером, которые обеспечивают ситуационную осведомленность на уровне одного транспортного средства. В результате, предположение, сделанное в этом примере, заключается в том, что транспортные средства разделяют ситуационную осведомленность путем широковещательной передачи дорожек и выполнения слияния трека с дорожкой.
Этот пример демонстрирует преимущества сплавления дорожек с двух транспортных средств для повышения ситуационной осведомленности и безопасности. Обратите внимание, что этот пример не моделирует коммуникационных систем. Вместо этого пример принимает, что коммуникационная система обеспечивает полосу пропускания, необходимую для передачи дорожек между двумя транспортными средствами.
Следующий блок изображает основные функции в двух транспортных средствах.
Транспортное средство 1 имеет два датчика, каждый из которых обеспечивает обнаружение локального трекера. Трекер использует обнаружения от локальных датчиков, чтобы отслеживать объекты и выводит эти локальные дорожки в фузер трека транспортного средства. Транспортное средство 2 имеет один датчик, который подает обнаружения на локальный трекер на транспортное средство 2. Локальные дорожки от транспортного средства 2 являются входом в локальный трек-фьюзер на транспортном средстве 2.
Трековый фюзер на каждом транспортном средстве запирает локальные дорожки транспортного средства с дорожками, полученными от трекового фюзера другого транспортного средства. После каждого обновления трек-фьюзер на каждом транспортном средстве транслирует свои слитые дорожки, которые поступают в следующее обновление трека-фьюзера на другом транспортном средстве.
В этом примере вы используете trackerJPDA
(Sensor Fusion and Tracking Toolbox), чтобы определить каждый трекер транспортного средства.
% Create trackers for each vehicle v1Tracker = trackerJPDA('TrackerIndex',1, 'DeletionThreshold', [4 4]); % Vehicle 1 tracker v2Tracker = trackerJPDA('TrackerIndex',2, 'DeletionThreshold', [4 4]); % Vehicle 2 tracker posSelector = [1 0 0 0 0 0; 0 0 1 0 0 0];
Обратите внимание, что в этой архитектуре слитые дорожки от одного транспортного средства используются для обновления слитых дорожек от другого транспортного средства. Эти слитые дорожки затем транслируются обратно на первое транспортное средство. Чтобы избежать распространения слухов, будьте осторожны, как треки из другого транспортного средства используются для обновления фузера трека.
Рассмотрим следующий пример распространения слухов: на некотором шаге обновления транспортное средство 1 отслеживает объект с помощью его внутренних датчиков. Транспортное средство 1 затем сплавляет дорожку объекта и передает ее транспортному средству 2, которое теперь сплавляет дорожку со своими собственными дорожками и становится известно об объекте. До этого момента это как раз и является целью слияния треков в трек: повысить ситуационную осведомленность транспортного средства 2 с помощью информации от транспортного средства 1. Поскольку транспортное средство 2 теперь знает об объекте, оно также начинает транслировать дорожку, возможно, в интересах другого транспортного средства (не показано в примере).
Однако теперь транспортное средство 1 принимает информацию о дорожке от транспортного средства 2 о объекте, который фактически отслеживает только транспортное средство 1. Таким образом, трек-фюзер на транспортном средстве 1 должен знать, что треки, которые он получает от транспортного средства 2 об этом объекте, на самом деле не содержат никакой новой информации, обновленной независимым источником. Чтобы сделать различие между треками, которые содержат новую информацию, и треками, которые просто повторяют информацию, вы должны определить транспортное средство 2 как внешний источник для слежения за фюзером на транспортном средстве 1. Точно так же транспортное средство 1 должно быть определено как внешний источник для трекового фюзера на транспортном средстве 2. Кроме того, вы должны задать только треки, которые обновляются фузером трека на основе информации от внутреннего источника как самоотчетные. При этом трек-фьюзер в каждом транспортном средстве способен игнорировать обновления от треков, которые прыгают назад и вперед между треками-фьюзерами без какой-либо новой информации в них.
Локальный трекер каждого транспортного средства отслеживает объекты относительно исходной системы координат транспортного средства, называемой ego-системой координат. Слияние дорожки и дорожки выполняется в системе координат сценария, который является системой координат глобального уровня. Вспомогательный egoToScenario
функция преобразует треки из системы координат ego в систему координат сценария. Точно так же функция scenarioToEgo
преобразует дорожки из системы координат сценария в любой из системы координат. Оба преобразования полагаются на StateParameters
свойство objectTrack
(Sensor Fusion and Tracking Toolbox) объекты. Обратите внимание, что когда trackFuser
объект вычисляет расстояние между центральной дорожкой (в системе координат сценария) и локальной дорожкой (в любой системе координат), он использует StateParameters
локальной дорожки для выполнения преобразования координат.
Для достижения вышеописанного trackFuser
определения, задайте следующие источники как fuserSourceConfiguration
(Sensor Fusion and Tracking Toolbox) объект.
% Define sources for each vehicle v1TrackerConfiguration = fuserSourceConfiguration('SourceIndex',1,'IsInternalSource',true, ... % v1Tracker is internal to v1Fuser "CentralToLocalTransformFcn", @scenarioToEgo, 'LocalToCentralTransformFcn', @egoToScenario); % Coordinate transformation v2FuserConfiguration = fuserSourceConfiguration('SourceIndex',4,'IsInternalSource',false); % v2Fuser is external to v2Fuser v1Sources = {v1TrackerConfiguration; v2FuserConfiguration}; v2TrackerConfiguration = fuserSourceConfiguration('SourceIndex',2,'IsInternalSource',true, ... % v2Tracker is internal to v2Fuser "CentralToLocalTransformFcn", @scenarioToEgo, 'LocalToCentralTransformFcn', @egoToScenario); % Coordinate transformation v1FuserConfiguration = fuserSourceConfiguration('SourceIndex',3,'IsInternalSource',false); % v1Fuser is external to v2Fuser v2Sources = {v2TrackerConfiguration; v1FuserConfiguration};
Теперь можно задать каждое транспортное средство track fuser как trackFuser
(Sensor Fusion and Tracking Toolbox) объект.
stateParams = struct('Frame','Rectangular','Position',[0 0 0],'Velocity',[0 0 0]); v1Fuser = trackFuser('FuserIndex',3,... 'MaxNumSources',2,'SourceConfigurations',v1Sources,... 'StateFusion','Intersection','DeletionThreshold',[3 3],... 'StateParameters',stateParams); v2Fuser = trackFuser('FuserIndex',4,... 'MaxNumSources',2,'SourceConfigurations',v2Sources,'StateFusion',... 'Intersection','DeletionThreshold',[3 3],... 'StateParameters',stateParams); % Initialize the following variables fusedTracks1 = objectTrack.empty(0,1); fusedTracks2 = objectTrack.empty(0,1); wasFuser1Updated = false; wasFuser2Updated = false;
В следующем сценарии показаны два транспортных средств, движущиеся по улице. Лидирует транспортное средство 1, оснащённая двумя перспективными датчиками: радаром малой дальности и датчиком зрения. Транспортное средство 2, движущаяся на 10 метров позади транспортного средства 1, оснащено радаром большой дальности. Правая сторона улицы содержит припаркованные транспортные средства. Между транспортными средствами стоит пешехода. Этот велосипед показан в виде точки примерно на X = 60 метрах.
Из-за короткого расстояния между транспортным средством 2 и транспортным средством 1 большая часть покрытия радарного датчика транспортного средства 2 перекрывается транспортным средством 1. В результате большинство дорожек, которые поддерживает трек-фьюзер на транспортном средстве 2, сначала инициализируются дорожками, транслируемыми из транспортного средства 1.
% Create the drivingScenario object and the two vehicles [scenario, vehicle1, vehicle2] = createDrivingScenario; % Create all the sensors [sensors, numSensors, attachedVehicle] = createSensors(scenario); % Create display [f,plotters] = createT2TDisplay(scenario, sensors, attachedVehicle);
Следующий график погони рассматривается с точки зрения второго транспортного средства. В стреле указывается положение пешехода, который почти полностью закрыт припаркованными транспортными средствами и первым транспортным средством.
% Define each vehicle as a vehicle, sensors, a tracker, and plotters v1 = struct('Actor', {vehicle1}, 'Sensors', {sensors(attachedVehicle==1)}, 'Tracker', {v1Tracker}, 'DetPlotter', {plotters.veh1DetPlotter}, 'TrkPlotter', {plotters.veh1TrkPlotter}); v2 = struct('Actor', {vehicle2}, 'Sensors', {sensors(attachedVehicle==2)}, 'Tracker', {v2Tracker}, 'DetPlotter', {plotters.veh2DetPlotter}, 'TrkPlotter', {plotters.veh2TrkPlotter});
Следующий код запускает симуляцию.
running = true; % For repeatable results, set the random number seed s = rng; rng(2019) snaptimes = [0.5, 2.6, 4.4, 6.3, inf]; snaps = cell(numel(snaptimes,1)); i = 1; f.Visible = 'on'; while running && ishghandle(f) time = scenario.SimulationTime; % Detect and track at the vehicle level [tracks1,wasTracker1Updated] = detectAndTrack(v1,time,posSelector); [tracks2,wasTracker2Updated] = detectAndTrack(v2,time,posSelector); % Keep the tracks from the previous fuser update oldFusedTracks1 = fusedTracks1; oldFusedTracks2 = fusedTracks2; % Update the fusers if wasTracker1Updated || wasFuser2Updated tracksToFuse1 = [tracks1;oldFusedTracks2]; if isLocked(v1Fuser) || ~isempty(tracksToFuse1) [fusedTracks1,~,~,info1] = v1Fuser(tracksToFuse1,time); wasFuser1Updated = true; pos = getTrackPositions(fusedTracks1,posSelector); plotTrack(plotters.veh1FusePlotter,pos); else wasFuser1Updated = false; fusedTracks1 = objectTrack.empty(0,1); end else wasFuser1Updated = false; fusedTracks1 = objectTrack.empty(0,1); end if wasTracker2Updated || wasFuser1Updated tracksToFuse2 = [tracks2;oldFusedTracks1]; if isLocked(v2Fuser) || ~isempty(tracksToFuse2) [fusedTracks2,~,~,info2] = v2Fuser(tracksToFuse2,time); wasFuser2Updated = true; pos = getTrackPositions(fusedTracks2,posSelector); ids = string([fusedTracks2.TrackID]'); plotTrack(plotters.veh2FusePlotter,pos,ids); else wasFuser2Updated = false; fusedTracks2 = objectTrack.empty(0,1); end else wasFuser2Updated = false; fusedTracks2 = objectTrack.empty(0,1); end % Update the display updateT2TDisplay(plotters, scenario, sensors, attachedVehicle) % Advance the scenario one time step and exit the loop if the scenario is complete running = advance(scenario); % Snap a shot at required times if time >= snaptimes(i) snaps{i} = getframe(f); i = i + 1; end end
Рисунок показывает сцену и результаты отслеживания в конце сценария.
Когда начинается моделирование, транспортное средство 1 обнаруживает транспортные средства, припаркованные на правой стороне улицы, затем подтверждаются дорожки, сопоставленные с припаркованными транспортными средствами. В это время единственным объектом, обнаруженным и отслеживаемым трекером транспортного средства 2, является транспортное средство 1 непосредственно перед ней. Как только транспортное средство автомобиля 1 подтверждает дорожки, он транслирует их, и трек- фьюзер транспортного средства 2 запирает их. В результате транспортному средству 2 становится известно о припаркованных транспортных средствах, прежде чем он сможет обнаружить их самостоятельно.
showsnap(snaps, 1)
Когда симуляция продолжается, транспортное средство 2 способно также обнаруживать и отслеживать транспортные средства, припаркованные сбоку, и сплавляет их с дорожками, поступающими из транспортного средства 1. Транспортное средство 2 способно обнаруживать и отслеживать пешехода около 4 секунд в симуляции, а транспортное средство 2 запирает дорожку, связанную с пешеходом, около 4,4 секунд в симуляции (см. Снимок 2). Однако транспортное средство 2 занимает около двух секунд, прежде чем он сможет обнаружить и отследить пешехода с помощью собственных датчиков (см. Снимок 3). Эти две секунды могут оказать огромное влияние на безопасность пешехода, если он начнет пересекать улицу.
showsnap(snaps, 2)
showsnap(snaps, 3)
Наконец, обратите внимание, как транспортные средства передают объекты и эти объекты выходят из поля зрения, слитые дорожки, связанные с этими объектами, отбрасываются обоими трекерами (см. Снимок 4). Падение дорожек демонстрирует, что слитые дорожки, транслируемые туда и обратно между двумя транспортными средствами, не используются для распространения слухов.
showsnap(snaps, 4)
% Restart the driving scenario to return the actors to their initial positions. restart(scenario); % Release all the sensor objects so they can be used again. for sensorIndex = 1:numSensors release(sensors{sensorIndex}); end % Return the random seed to its previous value rng(s)
В этом примере вы увидели, как слияние треков в трек может улучшить ситуационную осведомленность и повысить безопасность в автомобильных приложениях. Вы видели, как настроить trackFuser
выполнить слияние трека-трека и как определить источники как внутренние или внешние при помощи fuserSourceConfiguration
объект. Делая это, вы избегаете распространения слухов и сохраняете только слитые дорожки, которые действительно наблюдаются каждым транспортным средством, чтобы быть обеспеченным.
[1] Bharanidhar Duraisamy, Tilo Schwartz, and Christian Wohler, «Track level fusion algoritms for automotive safety applications», 2013 International Conference on Signal Processing, Image Processing & Patticle Recognition, ionition, ience, iul.
[2] Федеральная комиссия по связи, «Специализированная служба связи малой области значений», https://www.fcc.gov/wireless/bureau-divisions/mobility-division/dedicated-short-range-communications-dsrc-service.
function [scenario, egoVehicle, secondVehicle] = createDrivingScenario % createDrivingScenario Returns the drivingScenario defined in the Designer % Construct a drivingScenario object. scenario = drivingScenario('SampleTime', 0.05); % Add all road segments roadCenters = [50.8 0.5 0; 253.4 1.5 0]; roadWidth = 12; road(scenario, roadCenters, roadWidth); roadCenters = [100.7 -100.6 0; 100.7 103.7 0]; road(scenario, roadCenters); roadCenters = [201.1 -99.2 0; 199.7 99.5 0]; road(scenario, roadCenters); % Add the ego vehicle egoVehicle = vehicle(scenario, 'ClassID', 1, 'Position', [65.1 -0.9 0]); waypoints = [71 -0.5 0; 148.7 -0.5 0]; speed = 12; trajectory(egoVehicle, waypoints, speed); % Add the second vehicle secondVehicle = vehicle(scenario, 'ClassID', 1, 'Position', [55.1 -0.9 0]); waypoints = [61 -0.5 0; 138.7 -0.5 0]; speed = 12; trajectory(secondVehicle, waypoints, speed); % Add the parked cars vehicle(scenario, 'ClassID', 1, 'Position', [111.0 -3.6 0]); vehicle(scenario, 'ClassID', 1, 'Position', [140.6 -3.6 0]); vehicle(scenario, 'ClassID', 1, 'Position', [182.6 -3.6 0]); vehicle(scenario, 'ClassID', 1, 'Position', [211.3 -4.1 0]); % Add pedestrian actor(scenario, 'ClassID', 4, 'Length', 0.5, 'Width', 0.5, ... 'Height', 1.7, 'Position', [130.3 -2.7 0], 'RCSPattern', [-8 -8;-8 -8]); % Add parked truck vehicle(scenario, 'ClassID', 2, 'Length', 8.2, 'Width', 2.5, ... 'Height', 3.5, 'Position', [117.5 -3.5 0]); end
function [sensors, numSensors, attachedVehicle] = createSensors(scenario) % createSensors Returns all sensor objects to generate detections % Units used in createSensors and createDrivingScenario % Distance/Position - meters % Speed - meters/second % Angles - degrees % RCS Pattern - dBsm % Assign into each sensor the physical and radar profiles for all actors profiles = actorProfiles(scenario); sensors{1} = radarDetectionGenerator('SensorIndex', 1, ... 'SensorLocation', [3.7 0], 'MaxRange', 50, 'FieldOfView', [60 5], ... 'ActorProfiles', profiles, 'HasOcclusion', true, 'HasFalseAlarms', false); sensors{2} = visionDetectionGenerator('SensorIndex', 2, ... 'MaxRange', 100, 'SensorLocation', [1.9 0], 'DetectorOutput', 'Objects only', ... 'ActorProfiles', profiles); sensors{3} = radarDetectionGenerator('SensorIndex', 3, ... 'SensorLocation', [3.7 0], 'MaxRange', 120, 'FieldOfView', [30 5], ... 'ActorProfiles', profiles, 'HasOcclusion', true, 'HasFalseAlarms', false); attachedVehicle = [1;1;2]; numSensors = numel(sensors); end
function trackInEgo = scenarioToEgo(trackInScenario) % Performs coordinate transformation from scenario to ego coordinates % trackInScenario has StateParameters defined to transform it from scenario % coordinates to ego coordinates % We assume a constant velocity model with state [x;vx;y;vy;z;vz] egoPosInScenario = trackInScenario.StateParameters.Position; egoVelInScenario = trackInScenario.StateParameters.Velocity; stateInScenario = trackInScenario.State; stateShift = [egoPosInScenario(1);egoVelInScenario(1);egoPosInScenario(2);egoVelInScenario(2);egoPosInScenario(3);egoVelInScenario(3)]; stateInEgo = stateInScenario - stateShift; trackInEgo = objectTrack('UpdateTime',trackInScenario.UpdateTime,'State',stateInEgo,'StateCovariance',trackInScenario.StateCovariance,'StateParameters',trackInScenario.StateParameters); end
function trackInScenario = egoToScenario(trackInEgo) % Performs coordinate transformation from ego to scenario coordinates % trackInEgo has StateParameters defined to transform it from ego % coordinates to scenario coordinates % We assume a constant velocity model with state [x;vx;y;vy;z;vz] egoPosInScenario = trackInEgo.StateParameters.Position; egoVelInScenario = trackInEgo.StateParameters.Velocity; stateInScenario = trackInEgo.State; stateShift = [egoPosInScenario(1);egoVelInScenario(1);egoPosInScenario(2);egoVelInScenario(2);egoPosInScenario(3);egoVelInScenario(3)]; stateInEgo = stateInScenario + stateShift; trackInScenario = objectTrack('UpdateTime',trackInEgo.UpdateTime,'State',stateInEgo,'StateCovariance',trackInEgo.StateCovariance,'StateParameters',trackInEgo.StateParameters); end
function [tracks,wasTrackerUpdated] = detectAndTrack(agent,time,posSelector) % Create detections from the vehicle poses = targetPoses(agent.Actor); [detections,isValid] = vehicleDetections(agent.Actor.Position,agent.Sensors,poses,time,agent.DetPlotter); % Update tracks for the vehicle if isValid agent.Tracker.StateParameters = struct(... 'Frame','Rectangular', ... 'Position', agent.Actor.Position, ... 'Velocity', agent.Actor.Velocity); tracks = agent.Tracker(detections,time); tracksInScenario = tracks; for i = 1:numel(tracks) tracksInScenario(i) = egoToScenario(tracks(i)); end pos = getTrackPositions(tracksInScenario,posSelector); plotTrack(agent.TrkPlotter,pos) wasTrackerUpdated = true; else tracks = objectTrack.empty(0,1); wasTrackerUpdated = false; end end function [objectDetections,isValid] = vehicleDetections(position, sensors, poses, time, plotter) % Provides the detections for each vehicle. numSensors = numel(sensors); objectDetections = {}; isValidTime = false(1, numSensors); % Generate detections for each sensor for sensorIndex = 1:numSensors sensor = sensors{sensorIndex}; [objectDets, ~, isValidTime(sensorIndex)] = sensor(poses, time); objectDets = cellfun(@(d) setAtt(d), objectDets, 'UniformOutput', false); if isa(sensors{sensorIndex},'radarDetectionGenerator') objectDets = helperClusterDetections(objectDets, 5); end numObjects = numel(objectDets); objectDetections = [objectDetections; objectDets(1:numObjects)]; %#ok<AGROW> end isValid = any(isValidTime); % Plot detections if numel(objectDetections)>0 detPos = cellfun(@(d)d.Measurement(1:2), objectDetections, 'UniformOutput', false); detPos = cell2mat(detPos')' + position(1:2); plotDetection(plotter, detPos); end end function d = setAtt(d) % Set the attributes to be struct d.ObjectAttributes = struct; end function detectionClusters = helperClusterDetections(detections, vehicleSize) % helperClusterDetections Helper to cluster detections in the example N = numel(detections); distances = zeros(N); for i = 1:N for j = i+1:N if detections{i}.SensorIndex == detections{j}.SensorIndex distances(i,j) = norm(detections{i}.Measurement(1:2) - detections{j}.Measurement(1:2)); else distances(i,j) = inf; end end end leftToCheck = 1:N; i = 0; detectionClusters = cell(N,1); while ~isempty(leftToCheck) % Remove the detections that are in the same cluster as the one under % consideration underConsideration = leftToCheck(1); clusterInds = (distances(underConsideration, leftToCheck) < vehicleSize); detInds = leftToCheck(clusterInds); clusterDets = [detections{detInds}]; clusterMeas = [clusterDets.Measurement]; meas = mean(clusterMeas, 2); i = i + 1; detectionClusters{i} = detections{detInds(1)}; detectionClusters{i}.Measurement = meas; leftToCheck(clusterInds) = []; end detectionClusters(i+1:end) = []; % Since the detections are now for clusters, modify the noise to represent % that they are of the whole car for i = 1:numel(detectionClusters) measNoise = eye(6); measNoise(1:2,1:2) = vehicleSize^2 * eye(2); measNoise(4:5,4:5) = eye(2) * vehicleSize^2; detectionClusters{i}.MeasurementNoise = measNoise; end end
fuserSourceConfiguration
(Sensor Fusion and Tracking Toolbox) | objectTrack
(Sensor Fusion and Tracking Toolbox) | trackFuser
(Sensor Fusion and Tracking Toolbox)