LiDAR を使用した地面および障害物の検出

スクリプトを開く

この例では、地面 (車両より下の面) をセグメント化し、近くの障害物を検出することで、車両に取り付けられたセンサーからの 3 次元 LiDAR データを処理する方法を示します。これにより、車両ナビゲーションのための走行可能なパスプランニングが容易になります。また、この例では、ストリーミング LiDAR データを可視化する方法も示します。

Velodyne ファイルリーダーの作成

この例で使用している LiDAR データは、車両に取り付けられた Velodyne® HDL32E センサーを使用して記録されたものです。記録した PCAP ファイルを読み取るように velodyneFileReader オブジェクトを設定します。

fileName    = 'lidarData_ConstructionRoad.pcap';
deviceModel = 'HDL32E';

veloReader = velodyneFileReader(fileName, deviceModel);

LiDAR スキャンの読み取り

LiDAR データの各スキャンは、3 次元点群として格納されます。高速なインデックス付けと探索を使用してこのデータを効率的に処理することが、センサー処理パイプラインのパフォーマンスにとって重要です。この効率性を達成するために、pointCloud オブジェクトを使用します。このオブジェクトは、kd 木データ構造を使用してデータを内部的に整理します。

veloReader は、LiDAR スキャンごとにオーガナイズド pointCloud を作成します。pointCloud の Location プロパティは、M×N×3 の行列であり、点の XYZ 座標 (メートル単位) が含まれます。点強度は Intensity に格納されます。

% Read a scan of lidar data
ptCloud = readFrame(veloReader) %#ok<NOPTS>

ptCloud = 

  pointCloud with properties:

     Location: [32x1083x3 single]
        Count: 34656
      XLimits: [-80.0444 87.1780]
      YLimits: [-85.6287 92.8721]
      ZLimits: [-21.6060 14.3558]
        Color: []
       Normal: []
    Intensity: [32x1083 uint8]

ストリーミング点群表示の設定

pcplayer を使用して、ストリーミング点群データを可視化できます。pcplayer を構成して、車両の周囲の表示する領域を設定します。

% Specify limits of point cloud display
xlimits = [-25 45]; % meters
ylimits = [-25 45];
zlimits = [-20 20];

% Create a pcplayer
lidarViewer = pcplayer(xlimits, ylimits, zlimits);

% Customize player axes labels
xlabel(lidarViewer.Axes, 'X (m)')
ylabel(lidarViewer.Axes, 'Y (m)')
zlabel(lidarViewer.Axes, 'Z (m)')

% Display the raw lidar scan
view(lidarViewer, ptCloud)

この例では、地面、自車、および近くの障害物に属している点をセグメント化します。これらの点にラベルを付けるためのカラーマップを設定します。

% Define labels to use for segmented points
colorLabels = [...
    0      0.4470 0.7410; ... % Unlabeled points, specified as [R,G,B]
    0.4660 0.6740 0.1880; ... % Ground points
    0.9290 0.6940 0.1250; ... % Ego points
    0.6350 0.0780 0.1840];    % Obstacle points

% Define indices for each label
colors.Unlabeled = 1;
colors.Ground    = 2;
colors.Ego       = 3;
colors.Obstacle  = 4;

% Set the colormap
colormap(lidarViewer.Axes, colorLabels)

自車のセグメント化

LiDAR は車両の上部に取り付けられ、点群には車両自体に属している点が含まれている場合があります (ルーフやフード上など)。車両の寸法がわかっているため、車両に最も近い点をセグメント化できます。

車両の寸法を格納する vehicleDimensions オブジェクトを作成します。

vehicleDims = vehicleDimensions(); % Typical vehicle 4.7m by 1.8m by 1.4m

車両座標系で LiDAR の取り付け位置を指定します。車両座標系は、後車軸の中央の下にある地面上を原点とします。正の X 方向が前を指し、正の Y 方向が左を指し、正の Z 方向が上を指します。この例では、LiDAR は、地面に平行に、車両の上部の中央に取り付けられています。

mountLocation = [...
    vehicleDims.Length/2 - vehicleDims.RearOverhang, ... % x
    0, ...                                               % y
    vehicleDims.Height];                                 % z

補助関数 helperSegmentEgoFromLidarData を使用して自車をセグメント化します。この関数は、自車で定義されている直方体内のすべての点をセグメント化します。セグメント化された点を struct points に格納します。

points = struct();
points.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

セグメント化した自車の点群を可視化します。補助関数 helperUpdateView を使用します。

closePlayer = false;
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

地面および近くの障害物のセグメント化

LiDAR データから障害物を特定するために、まず、関数 segmentGroundFromLidarData を使用して地面をセグメント化することでこれを実現します。この関数は、オーガナイズド LiDAR データから地面に属している点をセグメント化します。

elevationDelta = 10;
points.GroundPoints = segmentGroundFromLidarData(ptCloud, 'ElevationAngleDelta', elevationDelta);

% Visualize the segmented ground plane.
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

点群で関数 select を使用して、自車および地面に属している点を削除します。'OutputSize' を 'full' として指定して、点群のオーガナイズドな性質を保持します。

nonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints;
ptCloudSegmented = select(ptCloud, nonEgoGroundPoints, 'OutputSize', 'full');

次に、自車を中心とした一定の半径内で地面にも自車にも属していないすべての点を探して、近くの障害物をセグメント化します。この半径は、LiDAR の範囲および詳細に処理する対象領域に基づいて決定できます。

sensorLocation  = [0, 0, 0]; % Sensor is at the center of the coordinate system
radius          = 40; % meters

points.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, ...
    sensorLocation, radius);

% Visualize the segmented obstacles
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

LiDAR シーケンスの処理

これで単一の LiDAR スキャンの点群処理パイプラインが用意できたので、これをまとめて、記録されたデータのシーケンスを処理します。主要なパラメーターは以前のステップで定義したため、下記のコードは省略したものです。ここでは、パラメーターは詳細な説明なしで使用しています。

% Rewind the |veloReader| to start from the beginning of the sequence
reset(veloReader);

isPlayerOpen = true;
while hasFrame(veloReader) && isPlayerOpen

    % Grab the next lidar scan
    ptCloud = readFrame(veloReader);

    % Segment points belonging to the ego vehicle
    points.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

    % Segment points belonging to the ground plane
    points.GroundPoints = segmentGroundFromLidarData(ptCloud, 'ElevationAngleDelta', elevationDelta);

    % Remove points belonging to the ego vehicle and ground plane
    nonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints;
    ptCloudSegmented = select(ptCloud, nonEgoGroundPoints, 'OutputSize', 'full');

    % Segment obstacles
    points.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, sensorLocation, radius);

    closePlayer = ~hasFrame(veloReader);

    % Update lidar display
    isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);
end
snapnow

サポート関数

helperSegmentEgoFromLidarData は、車両および取り付け位置の寸法を指定すると、自車に属している点をセグメント化します。

function egoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation)
%helperSegmentEgoFromLidarData segment ego vehicle points from lidar data
%   egoPoints = helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation)
%   segments points belonging to the ego vehicle of dimensions vehicleDims
%   from the lidar scan ptCloud. The lidar is mounted at location specified
%   by mountLocation in the vehicle coordinate system. ptCloud is a
%   pointCloud object. vehicleDimensions is a vehicleDimensions object.
%   mountLocation is a 3-element vector specifying XYZ location of the
%   lidar in the vehicle coordinate system.
%
%   This function assumes that the lidar is mounted parallel to the ground
%   plane, with positive X direction pointing ahead of the vehicle,
%   positive Y direction pointing to the left of the vehicle in a
%   right-handed system.

% Buffer around ego vehicle
bufferZone = [0.1, 0.1, 0.1]; % in meters

% Define ego vehicle limits in vehicle coordinates
egoXMin = -vehicleDims.RearOverhang - bufferZone(1);
egoXMax = egoXMin + vehicleDims.Length + bufferZone(1);
egoYMin = -vehicleDims.Width/2 - bufferZone(2);
egoYMax = egoYMin + vehicleDims.Width + bufferZone(2);
egoZMin = 0 - bufferZone(3);
egoZMax = egoZMin + vehicleDims.Height + bufferZone(3);

egoXLimits = [egoXMin, egoXMax];
egoYLimits = [egoYMin, egoYMax];
egoZLimits = [egoZMin, egoZMax];

% Transform to lidar coordinates
egoXLimits = egoXLimits - mountLocation(1);
egoYLimits = egoYLimits - mountLocation(2);
egoZLimits = egoZLimits - mountLocation(3);

% Use logical indexing to select points inside ego vehicle cube
egoPoints = ptCloud.Location(:,:,1) > egoXLimits(1) ...
    & ptCloud.Location(:,:,1) < egoXLimits(2) ...
    & ptCloud.Location(:,:,2) > egoYLimits(1) ...
    & ptCloud.Location(:,:,2) < egoYLimits(2) ...
    & ptCloud.Location(:,:,3) > egoZLimits(1) ...
    & ptCloud.Location(:,:,3) < egoZLimits(2);
end

helperUpdateView は、最新の点群および関連カラーラベルを使用してストリーミング点群表示を更新します。

function isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer)
%helperUpdateView update streaming point cloud display
%   isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)
%   updates the pcplayer object specified in lidarViewer with a new point
%   cloud ptCloud. Points specified in the struct points are colored
%   according to the colormap of lidarViewer using the labels specified by
%   the struct colors. closePlayer is a flag indicating whether to close
%   the lidarViewer.

if closePlayer
    hide(lidarViewer);
    isPlayerOpen = false;
    return;
end

scanSize = size(ptCloud.Location);
scanSize = scanSize(1:2);

% Initialize colormap
colormapValues = ones(scanSize, 'like', ptCloud.Location) * colors.Unlabeled;

if isfield(points, 'GroundPoints')
    colormapValues(points.GroundPoints) = colors.Ground;
end

if isfield(points, 'EgoPoints')
    colormapValues(points.EgoPoints) = colors.Ego;
end

if isfield(points, 'ObstaclePoints')
    colormapValues(points.ObstaclePoints) = colors.Obstacle;
end

% Update view
view(lidarViewer, ptCloud.Location, colormapValues)

% Check if player is open
isPlayerOpen = isOpen(lidarViewer);

end