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];
road(scenario, roadCenters, 'lanes',lanespec(2));

Создайте автомобиль, оборудованный датчиком и три автомобиля вокруг этого: тот, который настигает автомобиль, оборудованный датчиком и передает его слева, тот, который управляет прямо перед автомобилем, оборудованным датчиком и тем, который управляет прямо позади автомобиля, оборудованного датчиком. Все автомобили следуют за траекторией, заданной дорогой 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} = radarDetectionGenerator('SensorIndex', 1, 'Height', 0.2, 'MaxRange', 174, ...
    'SensorLocation', [egoCar.Wheelbase + egoCar.FrontOverhang, 0], 'FieldOfView', [20, 5]);

% Rear-facing long-range radar sensor at the center of the rear bumper of the car.
sensors{2} = radarDetectionGenerator('SensorIndex', 2, 'Height', 0.2, 'Yaw', 180, ...
    'SensorLocation', [-egoCar.RearOverhang, 0], 'MaxRange', 174, 'FieldOfView', [20, 5]);

% Rear-left-facing short-range radar sensor at the left rear wheel well of the car.
sensors{3} = radarDetectionGenerator('SensorIndex', 3, 'Height', 0.2, 'Yaw', 120, ...
    'SensorLocation', [0, egoCar.Width/2], 'MaxRange', 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} = radarDetectionGenerator('SensorIndex', 4, 'Height', 0.2, 'Yaw', -120, ...
    'SensorLocation', [0, -egoCar.Width/2], 'MaxRange', 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} = radarDetectionGenerator('SensorIndex', 5, 'Height', 0.2, 'Yaw', 60, ...
    'SensorLocation', [egoCar.Wheelbase, egoCar.Width/2], 'MaxRange', 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} = radarDetectionGenerator('SensorIndex', 6, 'Height', 0.2, 'Yaw', -60, ...
    'SensorLocation', [egoCar.Wheelbase, -egoCar.Width/2], 'MaxRange', 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)
    sensors{m}.ActorProfiles = profiles;
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, это верно, если датчики сгенерировали обнаружения. Этот флаг используется, чтобы вызвать средство отслеживания только, когда существуют обнаружения.

Другое важное примечание - то, что датчики могут симулировать несколько обнаружений на цель, в частности когда цели очень близко к радарным датчикам. Поскольку средство отслеживания принимает одно обнаружение на цель от каждого датчика, необходимо кластеризировать обнаружения, прежде чем средство отслеживания обработает их. Это сделано функциональным clusterDetections. Смотрите раздел 'Supporting Functions'.

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
    detections = {};
    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
            end
            detections = [detections; sensorDets]; %#ok<AGROW>
        end
    end

    % Update the tracker if there are new detections
    if any(isValidTime)
        vehicleLength = sensors{1}.ActorProfiles.Length;
        detectionClusters = clusterDetections(detections, vehicleLength);
        confirmedTracks = updateTracks(tracker, detectionClusters, time);

        % Update bird's-eye plot
        updateBEP(BEP, egoCar, detections, 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

clusterDetections

Эта функция объединяет несколько обнаружений, которые, как подозревают, были того же транспортного средства к одному обнаружению. Функция ищет обнаружения, которые ближе, чем размер транспортного средства. Обнаружения, которые соответствуют этому критерию, рассматриваются кластером и объединены к одному обнаружению в центроиде кластера. Шумы измерения изменяются, чтобы представлять возможность, что каждое обнаружение может быть где угодно на транспортном средстве. Поэтому шум должен иметь тот же размер как размер транспортного средства.

Кроме того, эта функция удаляет третью размерность измерения (высота) и уменьшает вектор измерения до [x; y;; vy].

function detectionClusters = clusterDetections(detections, vehicleSize)
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);
    meas2D = [meas(1:2);meas(4:5)];
    i = i + 1;
    detectionClusters{i} = detections{detInds(1)};
    detectionClusters{i}.Measurement = meas2D;
    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(1:2,1:2) = vehicleSize^2 * eye(2);
    measNoise(3:4,3:4) = eye(2) * 100 * vehicleSize^2;
    detectionClusters{i}.MeasurementNoise = measNoise;
end
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');
        plotCoverageArea(cap, sensors{i}.SensorLocation,...
            sensors{i}.MaxRange, sensors{i}.Yaw, sensors{i}.FieldOfView(1));
    end

    % Plot the coverage areas for vision sensors
    for i = 7:8
        cap = coverageAreaPlotter(BEP,'FaceColor','blue','EdgeColor','blue');
        plotCoverageArea(cap, sensors{i}.SensorLocation,...
            sensors{i}.MaxRange, sensors{i}.Yaw, 45);
    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);

    % 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,:));

    % 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

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

Приложения

Объекты

Функции

Похожие темы

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