LIDAR を使用したグランドプレーンと障害物の検出

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

この例では、グランドプレーン (車両の下の平面) をセグメント化して、車両に取り付けられたセンサーからの 3 次元 LiDAR データを処理し、近くの障害物を見つける方法を説明します。これにより、車両ナビゲーション向けの運転可能経路の計画を簡素化できます。この例では、ストリーミング LIDAR データを可視化する方法も示します。

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

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

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

veloReader = velodyneFileReader(fileName, deviceModel);

LIDAR スキャンの読み取り

LIDAR データの各スキャンが 3 次元点群として格納されます。このデータを高速インデックス作成と検索を使用して効率的に処理することが、センサー処理パイプラインのパフォーマンスの鍵となります。K-d 木のデータ構造を使用してデータを内部的に整理する pointCloud オブジェクトを使用して、この効率性を達成します。

veloReader は、LIDAR スキャンごとに、オーガナイズド pointCloud を構築します。pointCloud の Location プロパティは、M x N x 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 (Automated Driving Toolbox)オブジェクトを作成します。

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 シーケンスの処理

1 回の 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

LIDAR を使用したグランド プレーンと障害物の検出

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