直方体シミュレーションを使用した自動バレーパーキングの可視化

この例では次を使用します。

この例では、直方体ドライビングシナリオ環境で駐車場を移動している車両の運動を可視化する方法について説明します。これはUnreal Engine シミュレーションを使用した自動バレーパーキングの可視化の例に忠実に従っています。

可視化のワークフロー

Automated Driving Toolbox™ は、迅速にシナリオを作成し、低忠実度のレーダーおよびカメラセンサーを使用して検出を生成し、MATLAB® と Simulink® の両方でコントローラー、追跡アルゴリズム、およびセンサーフュージョンアルゴリズムをテストできる、直方体ドライビングシナリオ環境を提供します。Simulink での自動バレーパーキングの例では、Simulink で自動バレーパーキングシステムのためにパスプランニングおよび車両制御アルゴリズムを設計する方法について説明しています。この例では、そのアルゴリズムを拡張し、drivingScenario オブジェクトを使用してシナリオ内の車両運動を可視化する方法について説明します。このワークフローの手順は、次のとおりです。

駐車場を含むドライビングシナリオを作成します。
ドライビングシナリオから vehicleCostmap を作成します。
シナリオ内での自車のルート計画を作成します。
Simulink でシナリオをシミュレートし、ドライビングシナリオでの車両の運動を可視化します。

ドライビングシナリオの作成

drivingScenario オブジェクトを使用すると、低忠実度のシーンを作成できます。drivingScenario オブジェクトを作成し、駐車の操縦をモデル化するために必要な要素を追加します。

駐車スペースのグリッドを含む駐車場を作成する。
自車を追加する。
障害物として機能するように、駐車場のさまざまな位置に静的な車両を挿入する。

% Get a list of systems that are open now so any systems opened during this
% example can be closed at the end.
startingOpenSystems = find_system('MatchFilter', @Simulink.match.allVariants);

% Create drivingScenario object
scenario = drivingScenario;

% Create parking lot
lot = parkingLot(scenario,[3 -5; 60 -5; 60 -48; 3 -48]);

% Create parking spaces
cars = parkingSpace(Width=3.3);
accessible = parkingSpace(Type="Accessible",Width=3.3);
accessLane = parkingSpace(Type="NoParking",MarkingColor=[1 1 1],Width=1.5);
fireLane = parkingSpace(Type="NoParking",Length=2,Width=40);

% Insert parking spaces
insertParkingSpaces(lot,cars,Edge=2);
insertParkingSpaces(lot,cars,Edge=4);
insertParkingSpaces(lot,[cars accessLane accessible accessLane accessible], ...
    [5 1 1 1 1],Rows=2,Position=[42 -12]);
insertParkingSpaces(lot,[cars accessLane accessible accessLane accessible], ...
    [5 1 1 1 1],Rows=2,Position=[23 -12]);
insertParkingSpaces(lot,fireLane,1,Edge=3,Offset=8);

% Add the ego vehicle
egoVehicle = vehicle(scenario, ...
    ClassID=1, ...
    Position=[13 8 0].*[1 -1 1], ...
    Yaw=270, ...
    Mesh=driving.scenario.carMesh, ...
    Name="Ego");

% Add stationary vehicles
car1 = vehicle(scenario, ...
    ClassID=1, ...
    Position=[7.5 -42.7 0.01], ...
    Yaw=180, ...
    Mesh=driving.scenario.carMesh);

car2 = vehicle(scenario, ...
    ClassID=1, ...
    Position=[19 -17.5 0.01], ...
    Mesh=driving.scenario.carMesh);

car3 = vehicle(scenario, ...
    ClassID=1, ...
    Position=[27.5 -38.3 0.01], ...
    Yaw=180, ...
    Mesh=driving.scenario.carMesh);

car4 = vehicle(scenario, ...
    ClassID=1, ...
    Position=[55.5 -13.8 0.01], ...
    Mesh=driving.scenario.carMesh);

car5 = vehicle(scenario, ...
    ClassID=1, ...
    Position=[38 -20.8 0.01], ...
    Mesh=driving.scenario.carMesh);

% Plot drivingScenario
plot(scenario)

ドライビングシナリオからのコストマップの作成

ドライビングシナリオからコストマップを作成するには、次の手順に従います。

関数 saveas または任意のツールを使用して、ドライビングシナリオのプロットのスクリーンショットをキャプチャします。スクリーンショットを PNG ファイルとして作業ディレクトリに保存します。
関数 imref2d を使用して空間参照オブジェクトを作成します。駐車場の寸法を使用して "X" の範囲と "Y" の範囲を指定します。
関数 imread を使用して PNG イメージをベースワークスペースの変数に読み取ります。

その後、imshow を使用してイメージを表示できます。この例では、以前にキャプチャした、シナリオおよび空間参照オブジェクトの高解像度のスクリーンショットを提供します。

% Load the image data
load("SceneImageData.mat","sceneImage","sceneRef")

% Visualize the scene image
figure
imshow(sceneImage, sceneRef);
xlabel('X (m)');
ylabel('Y (m)');

スクリーンショットは、ある程度の解像度まで環境を正確に描いたものです。このイメージを使用して、パスプランニングとナビゲーションのための vehicleCostmap を作成できます。

まず、イメージを使用してフリースペースを推定します。"フリースペース" とは、車両が駐車中の車、コーン、道路の境界などの静的オブジェクトと衝突することなく、またマークが付けられた線を横切ることなく走行できる領域です。この例では、イメージの色に基づいてフリースペースを推定できます。色のしきい値アプリを使用してセグメンテーションを実行し、イメージからバイナリイメージを生成します。この例の最後に詳述されている補助関数 helperCreateCostmapFromImage を使用してバイナリイメージを生成することもできます。

sceneImageBinary = helperCreateCostmapFromImage(sceneImage);

次に、バイナリイメージからコストマップを作成します。バイナリイメージを使用して、各セルでコスト値を指定します。

% Get the left-bottom corner location of the map
mapLocation = [sceneRef.XWorldLimits(1) sceneRef.YWorldLimits(1)]; % [meters meters]

% Compute resolution
mapWidth = sceneRef.XWorldLimits(2) - sceneRef.XWorldLimits(1); % meters
cellSize = mapWidth/size(sceneImageBinary,2);

% Create the costmap
costmap = vehicleCostmap(im2single(sceneImageBinary),CellSize=cellSize,MapLocation=mapLocation);

figure
plot(costmap,Inflation='off')
legend off

ドライビングシナリオで駐車する車両の寸法を指定する必要もあります。この例では、Create Actor and Vehicle Trajectories Programmaticallyの例に記載されている車両寸法を使用します。

frontOverhang = egoVehicle.FrontOverhang;
rearOverhang = egoVehicle.RearOverhang;
vehicleWidth = egoVehicle.Width;
vehicleHeight = egoVehicle.Height;
vehicleLength = egoVehicle.Length;
centerToFront = (vehicleLength/2) - frontOverhang;
centerToRear = (vehicleLength/2) - rearOverhang;

vehicleDims = vehicleDimensions(vehicleLength,vehicleWidth,vehicleHeight, ...
    FrontOverhang=frontOverhang,RearOverhang=rearOverhang);
costmap.CollisionChecker.VehicleDimensions = vehicleDims;

車両を囲む円の数を指定して、インフレーション半径を設定します。

costmap.CollisionChecker.NumCircles = 5;

自車のルート計画の作成

グローバルルート計画は、駐車スポットに到達するために車両が移動しなければならない車線セグメントのシーケンスです。この例では、ルート計画は作成済みで、table に格納されています。ルート計画の作成の詳細については、Simulink での自動バレーパーキングの例を確認してください。シミュレーションを行う前に、モデルのコールバック関数 PreLoadFcn により、ルート計画を読み込みます。

data = load("routePlanDrivingScenario.mat");
routePlan = data.routePlan %#ok<NOPTS>

% Plot vehicle at the starting pose
egoVehicle.Position = [routePlan.StartPose(1,2) routePlan.StartPose(1,1) 0].*[1 -1 1];
egoVehicle.Yaw = routePlan.StartPose(1,3) - 90;

startPose = routePlan.StartPose(1,:);
hold on
helperPlotVehicle(startPose,vehicleDims,DisplayName="Current Pose")
legend

for n = 1:height(routePlan)
    % Extract the goal waypoint
    vehiclePose = routePlan{n, 'EndPose'};

    % Plot the pose
    legendEntry = sprintf("Goal %i",n);
    helperPlotVehicle(vehiclePose,vehicleDims,DisplayName=legendEntry);
    hold on
end
hold off

routePlan =

  4x3 table

         StartPose                EndPose           Attributes
    ____________________    ____________________    __________

       8      13       0    37.5      13       0    1x1 struct
    37.5      13       0      43      22      90    1x1 struct
      43      22      90      35      34     180    1x1 struct
      35      34     180    24.3      29     270    1x1 struct

モデルの確認

APVWithDrivingScenario モデルは、ドライビングシナリオで車両を可視化するためのプロットを追加することで、Simulink での自動バレーパーキングの例で使用されているモデルを拡張します。モデルを開きます。

helperCloseFigures

modelName = "APVWithDrivingScenario";
open_system(modelName)

Simulink モデルの Path Planner ブロックは、以前に作成したルート計画を使用して自車の最適な軌跡を計算します。このモデルは、各タイムステップにおける自車の姿勢を生成します。モデルはこの姿勢を Visualization ブロックに送り、このブロックは drivingScenario オブジェクトおよびコストマップの可視化を更新します。ドライビングシナリオを自車のパースペクティブからプロットします。

chasePlot(egoVehicle,ViewHeight=3,ViewPitch=5,Meshes="on");
movegui("east");

車両運動の可視化

モデルをシミュレートし、車両がどのように走行して目的の駐車スポットに到達するのかを確認します。

sim(modelName)

シミュレーションの実行に伴い、Simulink 環境は Visualization ブロックを使用してドライビングシナリオにおける車両の位置と方向を更新します。"自動バレーパーキング" の図において、計画されたパスが青で表示され、車両の実際のパスが赤で表示されます。"駐車の操縦" の図は、最終的な駐車の操縦の探索で使用されたローカルコストマップを示しています。

モデルと図を閉じます。

endingOpenSystems = find_system('MatchFilter', @Simulink.match.allVariants);
bdclose(setdiff(endingOpenSystems,startingOpenSystems))
helperCloseFigures

サポート関数

helperCreateCostmapFromImage

function BW = helperCreateCostmapFromImage(sceneImage)
%helperCreateCostmapFromImage Create a costmap from an RGB image.

% Call the autogenerated code from the Color Thresholder app.
BW = helperCreateMask(sceneImage);

% Smooth the image.
BW = im2uint8(medfilt2(BW));

% Resize.
BW = imresize(BW,0.5);

% Compute complement.
BW = imcomplement(BW);
end

helperCreateMask

function [BW,maskedRGBImage] = helperCreateMask(RGB)
%helperCreateMask  Threshold RGB image using auto-generated code from
%Color Thresholder app.
%  [BW,maskedRGBImage] = createMask(RGB) thresholds image RGB using
%  autogenerated code from the Color Thresholder app. The colorspace
%  and range for each channel of the colorspace have been set within the
%  app. The segmentation mask is returned in BW, and a composite of the
%  mask and original RGB image is returned in maskedRGBImage.

% Convert RGB image to chosen color space.
I = RGB;

% Define thresholds for channel 1 based on histogram settings.
channel1Min = 115.000;
channel1Max = 246.000;

% Define thresholds for channel 2 based on histogram settings.
channel2Min = 155.000;
channel2Max = 225.000;

% Define thresholds for channel 3 based on histogram settings.
channel3Min = 193.000;
channel3Max = 209.000;

% Create mask based on chosen histogram thresholds.
sliderBW = (I(:,:,1) >= channel1Min) & (I(:,:,1) <= channel1Max) & ...
    (I(:,:,2) >= channel2Min) & (I(:,:,2) <= channel2Max) & ...
    (I(:,:,3) >= channel3Min) & (I(:,:,3) <= channel3Max);
BW = sliderBW;

% Initialize output masked image based on input image.
maskedRGBImage = RGB;

% Set background pixels where BW is false to zero.
maskedRGBImage(repmat(~BW,[1 1 3])) = 0;
end

helperCloseFigures

function helperCloseFigures()
%helperCloseFigures Close all the figures except the simulation
%visualization

% Find all the figure objects
figHandles = findobj("Type","figure");

% Close the figures
for i = 1:length(figHandles)
    close(figHandles(i));
end
end