直方体シミュレーションを使用した自動バレー パーキングの可視化
この例では、直方体ドライビング シナリオ環境で駐車場を移動している車両の運動を可視化する方法について説明します。これは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)
ドライビング シナリオからのコストマップの作成
ドライビング シナリオからコストマップを作成するには、次の手順に従います。
その後、
を使用してイメージを表示できます。この例では、以前にキャプチャした、シナリオおよび空間参照オブジェクトの高解像度のスクリーンショットを提供します。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
参考
parkingLot
| drivingScenario
| vehicleCostmap