Fusion датчика Используя синтетические данные о радаре и видении

В этом примере показано, как сгенерировать сценарий, симулируйте обнаружения датчика и используйте сплав датчика, чтобы отследить симулированные транспортные средства. Основное преимущество использования генерации сценария и симуляции датчика по записи датчика является способностью создать редкие и потенциально опасные события и протестировать алгоритмы транспортного средства с ними.

Этот пример покрывает целый программируемый рабочий процесс для генерации синтетических данных. Чтобы сгенерировать синтетические данные в интерактивном режиме вместо этого, используйте приложение 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);

Создайте автомобиль, оборудованный датчиком и три автомобиля вокруг этого: тот, который настигает автомобиль, оборудованный датчиком и передает его слева, тот, который управляет прямо перед автомобилем, оборудованным датчиком и тем, который управляет прямо позади автомобиля, оборудованного датчиком. Все автомобили следуют за траекторией, заданной дорогой waypoints при помощи 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 поддерживание функции, чтобы инициализировать постоянную скорость линейный Фильтр Калмана, который работает с положением и скоростью.

В отслеживании выполняют 2D. Несмотря на то, что датчики возвращают измерения в 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 к 'Кластеризованным обнаружениям', который является значением по умолчанию. Модель датчика может также вывести необработанные данные об обнаружении или отследить обновления с помощью внутреннего средства отслеживания.

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

Эта функция создает отображение с тремя панелями:

  1. Верхний левый угол отображения: вид сверху, который следует за автомобилем, оборудованным датчиком.

  2. Нижний левый угол отображения: поле зрения камеры преследования, которое следует за автомобилем, оборудованным датчиком.

  3. Правильная половина отображения: 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

Смотрите также

Приложения

Объекты

Функции

Похожие темы

Для просмотра документации необходимо авторизоваться на сайте