Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

レーダーおよびビジョンの合成データを使用したセンサー フュージョン

この例では、シナリオの生成、センサー検出のシミュレート、およびシミュレートする車両のセンサー フュージョンを使用した追跡を行う方法を説明します。センサーの記録に対してシナリオの生成およびセンサーのシミュレーションを使用する主な利点は、まれなイベントや危険な可能性のあるイベントを作成して、それらのイベントで車両アルゴリズムをテストできることです。

この例では、合成データを生成するためのプログラムによるワークフロー全体をカバーします。代わりに、合成データを対話的に生成するには、Driving Scenario Designerアプリを使用します。例については、Create Driving Scenario Interactively and Generate Synthetic Sensor Dataを参照してください。

シナリオの生成

シナリオの生成は、道路ネットワークの生成、道路上を移動する車両の定義、および車両の移動で構成されます。

この例では、自車の左側を通過する車両を追跡するセンサー フュージョンの機能をテストします。このシナリオではハイウェイの設定をシミュレートし、追加の車両が自車の前方と後方にあります。

% Define an empty scenario.
scenario = drivingScenario;
scenario.SampleTime = 0.01;

500 メートルにわたる 2 車線の典型的なハイウェイ道路を追加します。道路は、一連の点を使用して定義されます。ここで、各点は 3 次元空間での道路の中央を定義します。道路の右エッジにジャージ障壁を追加します。

roadCenters = [0 0; 50 0; 100 0; 250 20; 500 40];
mainRoad = road(scenario, roadCenters, 'lanes',lanespec(2));
barrier(scenario,mainRoad);

自車と、自車の周囲の 3 台の自動車 (自車を追い越して左側を通過する 1 台、自車のすぐ前を走行する 1 台、および自車のすぐ後ろを走行する 1 台) を作成します。これらの自動車はすべて、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 度の視野をカバーしている自車をシミュレートします。これらのセンサーにはオーバーラップやカバレッジ ギャップがあります。自車は、車両の前と後ろの両方に長距離レーダー センサーとビジョン センサーを備えています。車両の右側と左側にはそれぞれ、2 台の短距離レーダー センサーがあり、各センサーが 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 次元で行われます。センサーは 3 次元での測定を返しますが、運動自体は水平面に限定されるため、高さを追跡する必要はありません。

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);

シナリオのシミュレーション

以下のループは車両を移動させ、センサーのシミュレーションを呼び出して、追跡を実行します。

シナリオの生成とセンサーのシミュレーションに異なるタイム ステップを指定できることに注意してください。シナリオとセンサーに異なるタイム ステップを指定すると、シナリオのシミュレーションをセンサーのシミュレーションから分離できます。これは、センサーの測定頻度とは独立して、アクターの運動を高い精度でモデル化する場合に役立ちます。

もう 1 つの例は、各センサーが異なる更新頻度をもつ場合です。あるセンサーは 20 ミリ秒ごとに更新を提供し、別のセンサーは 50 ミリ秒ごとに更新を提供するとします。更新頻度 10 ミリ秒のシナリオを指定すると、センサーは更新を正しい時間で提供できます。

この例では、センサーは 0.1 秒ごとに検出しますが、シナリオの生成のタイム ステップは 0.01 秒です。センサーは論理フラグ isValidTime を返し、センサーが検出を生成した場合は true になります。このフラグは、検出がある場合にのみトラッカーを呼び出すために使用されます。

もう 1 つ重要な点は、センサーはターゲットごとに複数の検出をシミュレートできることです (特にターゲットがレーダー センサーに非常に近い場合)。トラッカーは、各センサーからの検出がターゲットごとに 1 つであることを前提としているため、トラッカーで処理される前に検出をクラスター化する必要があります。これを行うには、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

この関数は、3 パネルからなるディスプレイを作成します。

  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

参考

アプリ

オブジェクト

関数

関連するトピック