Exponenta Banner
exponenta event banner

Обнаружение переулка в 3D облаке пункта лидара

Открыть сценарий в реальном времени

В этом примере показано, как обнаруживать полосы движения в облаках опорных точек. Для обнаружения полос движения транспортных средств ego можно использовать значения интенсивности, возвращаемые облаками точек lidar. Можно дополнительно улучшить обнаружение полосы движения с помощью алгоритма подгонки кривой и отслеживания параметров кривой. Обнаружение полосы движения Lidar позволяет создавать сложные рабочие процессы, такие как поддержка по поддержанию полосы движения, предупреждение о выходе из полосы движения и адаптивный круиз-контроль для автономного вождения. Испытываемое транспортное средство собирает данные лидара с помощью датчика лидара, установленного на его крыше.

Введение

Обнаружение полосы движения в лидаре включает в себя обнаружение непосредственной левой и правой полос движения, также известных как полосы движения эго-транспортного средства, относительно датчика лидара. Она включает в себя следующие этапы:

  • Регион извлечения процентов

  • Сегментация наземной плоскости

  • Обнаружение пиковой интенсивности

  • Определение полосы движения с помощью поиска в окне

  • Параболическая полиномиальная подгонка

  • Фитинг параллельной полосы движения

  • Отслеживание полосы движения

На этой блок-схеме представлен обзор рабочего процесса, представленного в этом примере.

Преимущества использования лидарных данных для обнаружения дорожек:

  • Точечные облака Lidar дают лучшее 3-D представление поверхности дороги, чем данные изображения, таким образом уменьшая требуемые калибровочные параметры для поиска вида с высоты птичьего полета.

  • Лидар более устойчив к неблагоприятным климатическим условиям, чем обнаружение на основе изображений.

  • Данные Лидара имеют сантиметровый уровень точности, приводящий к точной локализации полосы движения.

Загрузка и подготовка набора данных Lidar

Данные лидара, использованные в этом примере, были собраны с помощью датчика лидара канала Ouster OS1-64, создающего облака точек высокого разрешения. Этот набор данных содержит облака точек, хранящиеся как массив ячеек pointCloud объект. Каждое облако точек 3-D состоит из местоположений XYZ вместе с информацией об интенсивности.

Примечание.Время загрузки данных зависит от вашего подключения к Интернету. MATLAB будет временно не отвечать во время выполнения этого блока кода.

% Download lidar data
lidarData = helperGetDataset;

Выбор первого кадра набора данных для дальнейшей обработки.

% Select first frame 
ptCloud = lidarData{1};

% Visualize input point cloud
figure
pcshow(ptCloud)
title('Input Lidar Point Cloud')
axis([0 50 -15 15 -5 5])
view([-42 35])

Предварительная обработка

Чтобы оценить точки полосы движения, сначала выполните предварительную обработку облаков лидарных точек. Предварительная обработка включает в себя следующие шаги:

  • Регион извлечения процентов

  • Сегментация наземной плоскости

% Define ROI in meters
xlim = [5 55];
ylim = [-3 3];
zlim = [-4 1];
roi = [xlim ylim zlim];

% Crop point cloud using ROI
indices = findPointsInROI(ptCloud,roi);
croppedPtCloud = select(ptCloud,indices);

% Remove ground plane
maxDistance = 0.1;
referenceVector = [0 0 1];
[model,inliers,outliers] = pcfitplane(croppedPtCloud,maxDistance,referenceVector);
groundPts = select(croppedPtCloud,inliers);

figure
pcshow(groundPts)
title('Ground Plane')
view(3)

Обнаружение точек полосы движения

Обнаружение точек полосы движения с помощью поиска скользящего окна, где начальные оценки для скользящих окон выполняются с использованием гистограммы на основе интенсивности.

Обнаружение точки полосы движения состоит главным образом из следующих двух этапов:

  • Обнаружение пиковой интенсивности

  • Поиск в окне

Обнаружение пиковой интенсивности

Точки полосы движения в облаке точек лидара имеют четкое распределение интенсивностей. Обычно эти интенсивности занимают верхнюю область в распределении гистограммы и проявляются как высокие пики. Вычислите гистограмму интенсивностей от обнаруженной нулевой плоскости вдоль оси эго транспортного средства (положительная ось X). helperComputeHistogram вспомогательная функция создает гистограмму точек интенсивности. Управление количеством ячеек для гистограммы путем указания разрешения ячейки.

histBinResolution = 0.2;
[histVal,yvals] = helperComputeHistogram(groundPts,histBinResolution);

figure
plot(yvals,histVal,'--k')
set(gca,'XDir','reverse')
hold on

Получение пиков в гистограмме с помощью helperfindpeaks функция помощника. Дальнейшая фильтрация возможных точек полосы на основе ширины полосы с помощью helperInitialWindow функция помощника.

[peaks,locs] = helperfindpeaks(histVal);
startYs = yvals(locs);

laneWidth = 4;
[startLanePoints,detectedPeaks] = helperInitialWindow(startYs,peaks,laneWidth);

plot(startYs,peaks,'*r')
plot(startLanePoints,detectedPeaks,'og')
legend('Histogram','Peak','Detected Peaks','Location','North')
title('Peak Detection')
hold off

Поиск в окне

Поиск в окне используется для определения точек полосы движения путем перемещения окон вдоль направления кривизны полосы движения. Поиск в окне состоит из двух шагов:

  • Инициализация окна

  • Скользящее окно

Инициализация окна

Обнаруженные пики помогают в инициализации окна поиска. Инициализируйте окна поиска как несколько ячеек с красным и синим цветами для левой и правой полос соответственно. 

vBinRes = 1;
hBinRes = 0.8;
numVerticalBins = ceil((groundPts.XLimits(2) - groundPts.XLimits(1))/vBinRes);
laneStartX = linspace(groundPts.XLimits(1),groundPts.XLimits(2),numVerticalBins);

% Display bin windows
figure
pcshow(groundPts)
view(2)
helperDisplayBins(laneStartX,startLanePoints(1),hBinRes/2,groundPts,'red');
helperDisplayBins(laneStartX,startLanePoints(2),hBinRes/2,groundPts,'blue');
title('Initialized Sliding Windows')

Скользящее окно

Сдвижные бункеры инициализируются из мест расположения бункеров и итеративно перемещаются в направлении эго-транспортного средства (положительная ось X). helperDetectLanes функция помощника определяет точки полосы движения и визуализирует скользящие бункеры. Последовательные бункеры скользят вдоль оси Y на основе значений интенсивности внутри предыдущего бункера.

В областях, где отсутствуют точки полосы движения, функция предсказывает скользящие ячейки с помощью многочлена второй степени. Это условие обычно возникает, когда движущийся объект пересекает полосы движения. Скользящие ячейки желтые, а ячейки, предсказанные с помощью полинома, зеленые. 

display = true;
lanes = helperDetectLanes(groundPts,hBinRes, ...
    numVerticalBins,startLanePoints,display);

% Plot final lane points
lane1 = lanes{1};
lane2 = lanes{2};

figure
pcshow(groundPts)
title('Detected Lane Points')
hold on
p1 = plot3(lane1(:, 1),lane1(:, 2),lane1(:, 3),'*y');
p2 = plot3(lane2(:, 1),lane2(:, 2),lane2(:, 3),'*r');
hold off
view(2)
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White','Location','southoutside')

Фитинг полосы движения

Фитинг полосы движения включает в себя оценку полиномиальной кривой в обнаруженных точках полосы движения. Эти многочлены используются вместе с зависимостью параллельной полосы движения для фитинга полосы движения.

Параболическая полиномиальная подгонка

Многочлен устанавливается на точках X-Y с использованием 2-градусного многочлена, представленного как ось 2 + bx + c, где a, b и c - параметры многочлена. Для выполнения фитинга кривой используйте командуhelperFitPolynomial вспомогательная функция, которая также обрабатывает отклонения с помощью алгоритма консенсуса случайных выборок (RANSAC). Чтобы оценить 3-D точки полосы движения, извлеките параметры из модели плоскости, созданной во время этапа предварительной обработки. Модель плоскости представляется как ax + на + cz + d = 0, где координата Z неизвестна. Оцените координату Z, подставив координаты X и Y в уравнение плоскости.

[P1,error1] = helperFitPolynomial(lane1(:,1:2),2,0.1);
[P2,error2] = helperFitPolynomial(lane2(:,1:2),2,0.1);

xval = linspace(5,40,80);
yval1 = polyval(P1,xval);
yval2 = polyval(P2,xval);

% Z-coordinate estimation
modelParams = model.Parameters;
zWorld1 = (-modelParams(1)*xval - modelParams(2)*yval1 - modelParams(4))/modelParams(3);
zWorld2 = (-modelParams(1)*xval - modelParams(2)*yval2 - modelParams(4))/modelParams(3);

% Visualize fitted lane
figure
pcshow(croppedPtCloud)
title('Fitted Lane Polynomial')
hold on
p1 = plot3(xval,yval1,zWorld1,'y','LineWidth',0.2);
p2 = plot3(xval,yval2,zWorld2,'r','LineWidth',0.2);
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White','Location','southoutside')
view(2)
hold off

Фитинг параллельной полосы движения

Полосы движения обычно параллельны друг другу вдоль дороги. Чтобы сделать фитинг полосы надежным, используйте эту зависимость параллельности. При подгонке многочленов helperFitPolynomial вспомогательная функция также вычисляет ошибку фитинга. Обновите полосы с ошибочными точками с помощью нового полинома. Обновите этот многочлен, сдвинув его вдоль оси Y.

lane3d1 = [xval' yval1' zWorld1'];
lane3d2 = [xval' yval2' zWorld2'];

% Shift the polynomial with a high score along the Y-axis towards
% the polynomial with a low score
if error1 > error2
    lanePolynomial = P2;
    if lane3d1(1,2) > 0
        lanePolynomial(3) = lane3d2(1,2) + laneWidth;
    else
        lanePolynomial(3) = lane3d2(1,2) - laneWidth;
    end
    lane3d1(:,2) = polyval(lanePolynomial,lane3d1(:,1));
    lanePolynomials = [lanePolynomial; P2];
else
    lanePolynomial = P1;
    if lane3d2(1,2) > 0
        lanePolynomial(3) = lane3d1(1,2) + laneWidth;
    else
        lanePolynomial(3) = lane3d1(1,2) - laneWidth;
    end
    lane3d2(:,2) = polyval(lanePolynomial,lane3d2(:,1));
    lanePolynomials = [P1; lanePolynomial]
end

% Visualize lanes after parallel fitting
figure
pcshow(ptCloud)
axis([0 50 -15 15 -5 5])
hold on
p1 = plot3(lane3d1(:,1),lane3d1(:,2),lane3d1(:,3),'y','LineWidth',0.2);
p2 = plot3(lane3d2(:,1),lane3d2(:,2),lane3d2(:,3),'r','LineWidth',0.2);
view([-90 90])
title('Fitted Lanes')
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White',"Location","southoutside")
hold off

Отслеживание полосы движения

Отслеживание полосы помогает стабилизировать кривизну полосы, вызванную внезапными рывками и заносами. Эти изменения могут произойти из-за отсутствия точек полосы движения, движения транспортных средств по полосам движения и ошибочного обнаружения полосы движения. Отслеживание полосы движения представляет собой двухэтапный процесс:

  • Отслеживание параметров многочлена полосы движения (a, b) для управления кривизной многочлена.

  • Отслеживание начальных точек, полученных при обнаружении пика. Этот параметр обозначается в многочлене как c.

Эти параметры обновляются с помощью фильтра Калмана с моделью движения с постоянным ускорением. Чтобы инициировать фильтр Калмана, используйте параметр configureKalmanFilter.

% Initial values
curveInitialParameters = lanePolynomials(1,1:2);
driftInitialParameters = lanePolynomials(:,3)';
initialEstimateError = [1 1 1] * 1e-1;
motionNoise = [1 1 1] * 1e-7;
measurementNoise = 10;

% Configure Kalman filter
curveFilter = configureKalmanFilter('ConstantAcceleration', ...
    curveInitialParameters,initialEstimateError,motionNoise,measurementNoise);
driftFilter = configureKalmanFilter('ConstantAcceleration', ...
    driftInitialParameters,initialEstimateError,motionNoise,measurementNoise);

Закольцовывание данных

Закольцовывание и обработка данных лидара с помощью helperLaneDetector класс помощника. Этот вспомогательный класс реализует все предыдущие шаги, а также выполняет дополнительную предварительную обработку для удаления транспортных средств из облака точек. Это гарантирует, что обнаруженные точки заземления являются плоскими и модель плоскости является точной. Метод класса detectLanes обнаруживает и извлекает точки полосы для левой и правой полосы в виде двухэлементного массива ячеек, где первый элемент соответствует левой полосе, а второй элемент - правой полосе.

% Initialize the random number generator
rng(2020)
numFrames = numel(lidarData);
detector = helperLaneDetector('ROI',[5 40 -3 3 -4 1]);

% Turn on display
player = pcplayer([0 50],[-15 15],[-5 5]);

drift = zeros(numFrames,1);
filteredDrift = zeros(numFrames,1);
curveSmoothness = zeros(numFrames,1);
filteredCurveSmoothness = zeros(numFrames,1);
for i = 1:numFrames
    ptCloud = lidarData{i};

    % Detect lanes
    detectLanes(detector,ptCloud);

    % Predict polynomial from Kalman filter
    predict(curveFilter);
    predict(driftFilter);

    % Correct polynomial using Kalman filter
    lanePolynomials = detector.LanePolynomial;
    drift(i) = mean(lanePolynomials(:,3));
    curveSmoothness(i) = mean(lanePolynomials(:,1));
    updatedCurveParams = correct(curveFilter,lanePolynomials(1,1:2));
    updatedDriftParams = correct(driftFilter,lanePolynomials(:,3)');

    % Update lane polynomials
    updatedLanePolynomials = [repmat(updatedCurveParams,[2 1]),updatedDriftParams'];

    % Estimate new lane points with updated polynomial
    lanes = updateLanePolynomial(detector,updatedLanePolynomials);
    filteredDrift(i) = mean(updatedLanePolynomials(:,3));
    filteredCurveSmoothness(i) = mean(updatedLanePolynomials(:,1));

    % Visualize lanes after parallel fitting
    ptCloud.Color = uint8(repmat([0 0 255],ptCloud.Count,1));
    lane3dPc1 = pointCloud(lanes{1});
    lane3dPc1.Color = uint8(repmat([255 0 0],lane3dPc1.Count,1));
    lanePc = pccat([ptCloud lane3dPc1]);
    lane3dPc2 = pointCloud(lanes{2});
    lane3dPc2.Color = uint8(repmat([255 255 0],lane3dPc2.Count,1));
    lanePc = pccat([lanePc lane3dPc2]);
    view(player,lanePc)
end

Результаты

Чтобы проанализировать результаты обнаружения полосы движения, сравните их с отслеживаемыми многочленами полосы движения, выведя их на рисунки. Каждый график сравнивает параметры с фильтром Калмана и без него. Первый график сравнивает дрейф полос вдоль оси Y, а второй график сравнивает гладкость многочленов полос. Гладкость определяется как скорость изменения уклона кривой полосы движения. 

figure
plot(drift)
hold on
plot(filteredDrift)
hold off
title('Lane Drift Along Y-axis')
legend('Drift Values','Filtered Drift Values')

figure
plot(curveSmoothness)
hold on
plot(filteredCurveSmoothness)
hold off
title('Curve Smoothness')
legend('Curve Smoothness','Filtered Curve Smoothness')

Резюме

В этом примере показано, как обнаруживать полосы на канале интенсивности точечных облаков, поступающих от лидарного датчика. Также вы научились подгонять многочлен 2-D к обнаруженным точкам полосы движения и использовать модель нулевой плоскости для оценки 3-D точек полосы движения. Вы также использовали отслеживание фильтра Калмана для дальнейшего улучшения обнаружения полос движения.

Вспомогательные функции

helperLoadData вспомогательная функция загружает набор данных лидара в рабочую область MATLAB.

function reflidarData = helperGetDataset()
outputFolder = fullfile(tempdir,'WPI');
url = 'https://www.mathworks.com/supportfiles/lidar/data/WPI_LidarData.tar.gz';
lidarDataTarFile = fullfile(outputFolder,'WPI_LidarData.tar.gz');
if ~exist(lidarDataTarFile,'file')
    mkdir(outputFolder);
    disp('Downloading WPI Lidar driving data (760 MB)...');
    websave(lidarDataTarFile,url);
    untar(lidarDataTarFile,outputFolder);
end

% Check if tar.gz file is downloaded, but not uncompressed
if ~exist(fullfile(outputFolder,'WPI_LidarData.mat'),'file')
    untar(lidarDataTarFile,outputFolder);
end

% Load lidar data
load(fullfile(outputFolder,'WPI_LidarData.mat'), 'lidarData');

% Select region with a prominent intensity value
reflidarData = cell(300,1);
count = 1;
roi = [-50 50 -30 30 -inf inf];
for i = 81:380
    pc = lidarData{i};
    ind = findPointsInROI(pc,roi);
    reflidarData{count} = select(pc,ind);
    count = count + 1;
end
end

Это helperInitialWindow вспомогательная функция вычисляет начальные точки окна поиска с помощью обнаруженных пиков гистограммы.

function [yval,detectedPeaks] = helperInitialWindow(yvals,peaks,laneWidth)
leftLanesIndices = yvals >= 0;
rightLanesIndices = yvals < 0;
leftLaneYs = yvals(leftLanesIndices);
rightLaneYs = yvals(rightLanesIndices);
peaksLeft = peaks(leftLanesIndices);
peaksRight = peaks(rightLanesIndices);
diff = zeros(sum(leftLanesIndices),sum(rightLanesIndices));
for i = 1:sum(leftLanesIndices)
    for j = 1:sum(rightLanesIndices)
        diff(i,j) = abs(laneWidth - (leftLaneYs(i) - rightLaneYs(j)));
    end
end
[~,minIndex] = min(diff(:));
[row,col] = ind2sub(size(diff),minIndex);
yval = [leftLaneYs(row) rightLaneYs(col)];
detectedPeaks = [peaksLeft(row) peaksRight(col)];
estimatedLaneWidth = leftLaneYs(row) - rightLaneYs(col);

% If the calculated lane width is not within the bounds,
% return the lane with highest peak
if abs(estimatedLaneWidth - laneWidth) > 0.5
    if max(peaksLeft) > max(peaksRight)
        yval = [leftLaneYs(maxLeftInd) NaN];
        detectedPeaks = [peaksLeft(maxLeftInd) NaN];
    else
        yval = [NaN rightLaneYs(maxRightInd)];
        detectedPeaks = [NaN rightLaneYs(maxRightInd)];
    end
end
end

Это helperFitPolynomial вспомогательная функция подгоняет многочлен на основе RANSAC к обнаруженным точкам полосы движения и вычисляет оценку фитинга.

function [P,score] = helperFitPolynomial(pts,degree,resolution)
P = fitPolynomialRANSAC(pts,degree,resolution);
ptsSquare = (polyval(P,pts(:,1)) - pts(:,2)).^2;
score =  sqrt(mean(ptsSquare));
end

Это helperComputeHistogram вспомогательная функция вычисляет гистограмму значений интенсивности точечных облаков.

function [histVal,yvals] = helperComputeHistogram(ptCloud,histogramBinResolution)
numBins = ceil((ptCloud.YLimits(2) - ptCloud.YLimits(1))/histogramBinResolution);
histVal = zeros(1,numBins-1);
binStartY = linspace(ptCloud.YLimits(1),ptCloud.YLimits(2),numBins);
yvals = zeros(1,numBins-1);
for i = 1:numBins-1
    roi = [-inf 15 binStartY(i) binStartY(i+1) -inf inf];
    ind = findPointsInROI(ptCloud,roi);
    subPc = select(ptCloud,ind);
    if subPc.Count
        histVal(i) = sum(subPc.Intensity);
        yvals(i) = (binStartY(i) + binStartY(i+1))/2;
    end
end
end

Это helperfindpeaks вспомогательная функция извлекает пики из значений гистограммы. 

function [pkHistVal,pkIdx] = helperfindpeaks(histVal)
pkIdxTemp = (1:size(histVal,2))';
histValTemp = [NaN; histVal'; NaN];
tempIdx = (1:length(histValTemp)).';

% keep only the first of any adjacent pairs of equal values (including NaN)
yFinite = ~isnan(histValTemp);
iNeq = [1; 1 + find((histValTemp(1:end-1) ~= histValTemp(2:end)) & ...
    (yFinite(1:end-1) | yFinite(2:end)))];
tempIdx = tempIdx(iNeq);

% Take the sign of the first sample derivative
s = sign(diff(histValTemp(tempIdx)));

% Find local maxima
maxIdx = 1 + find(diff(s)<0);

% Index into the original index vector without the NaN bookend
pkIdx = tempIdx(maxIdx) - 1;

% Fetch the coordinates of the peak
pkHistVal = histVal(pkIdx);
pkIdx = pkIdxTemp(pkIdx)';
end

Ссылки

[1] Галлаби, Фарук, Фавзи Нашашиби, Гайатх Эль-Хадж-Шаде и Мари-Анн Митте. «Обнаружение разметки полосы движения на основе LIDAR для позиционирования транспортного средства в карте высокой четкости». В 2018 году 21-я Международная конференция по интеллектуальным транспортным системам (ITSC), 2209-14. Мауи: IEEE, 2018. https://doi.org/10.1109/ITSC.2018.8569951.

[2] Туи, Микаэль и Фернандо Леон. «Обнаружение и отслеживание полосы движения на основе данных Лидара». Метрологические и измерительные системы 17, № 3 (2010): 311-322. https://doi.org/10.2478/v10178-010-0027-3.

Документация по панели инструментов Lidar

Поддержка