Этот пример показов, как обнаружить полосы движения в лидар облаков точек. Можно использовать значения интенсивности, возвращенные из облаков лидарных точек, для обнаружения полос автомобиля , оборудованного датчиком. Можно дополнительно улучшить обнаружение маршрута с помощью алгоритма аппроксимирования кривыми и отслеживания параметров кривой. Обнаружение лидара маршрута позволяет вам создавать сложные рабочие процессы, такие как помощь по хранению маршрута, предупреждение о выезде из маршрута и адаптивный круиз-контроль для автономного управления автомобилем. Тестовое транспортное средство собирает данные лидара с помощью датчика лидара, установленного на его крыше.
Обнаружение маршрута в лидаре включает обнаружение непосредственных левой и правой дорожек, также известных как автомобиль , оборудованный датчиком дорожки, относительно датчика лидара. Он включает следующие шаги:
Необходимая область
Сегментация наземной плоскости
Обнаружение пиковой интенсивности
Обнаружение маршрута с помощью поиска в окне
Параболический полиномиальный подбор кривой
Параллельные подборы кривой маршрута
Отслеживание маршрута
Эта блок-схема дает обзор рабочего процесса, представленного в этом примере.
Преимуществами использования лидарных данных для обнаружения маршрута являются:
Облака точек Лидара дают лучшее 3-D представление поверхности дороги, чем данные изображений, таким образом уменьшая необходимые параметры калибровки, чтобы найти вид птичьего полета.
Лидар является более устойчивым к неблагоприятным климатическим условиям, чем обнаружение на основе изображений.
Данные Лидара имеют сантиметровый уровень точности, приводящий к точной локализации маршрута.
Данные лидара, используемые в этом примере, были собраны с помощью датчика лидара канала 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)
Обнаружите точки маршрута с помощью поиска в скользящем окне, где начальные оценки для скользящих окон сделаны с помощью гистограммы на основе интенсивности.
Обнаружение точек маршрута состоит, в основном, из этих двух шагов:
Обнаружение пиковой интенсивности
Поиск окна
Точки маршрута в облаке точек лидара имеют отчетливое распределение интенсивности. Обычно эти интенсивности занимают верхнюю область в распределении гистограмм и появляются как высокий peaks. Вычислите гистограмму интенсивности от обнаруженной плоскости земли вдоль оси автомобиль , оборудованный датчиком (положительная ось X). The helperComputeHistogram Функция helper создает гистограмму точек интенсивности. Управление количеством интервалов для гистограммы путем определения разрешения интервала.
histBinResolution = 0.2; [histVal,yvals] = helperComputeHistogram(groundPts,histBinResolution); figure plot(yvals,histVal,'--k') set(gca,'XDir','reverse') hold on
Получите peaks в гистограмме при помощи 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
Поиск окна используется, чтобы обнаружить точки маршрута путем скольжения окон вдоль направления кривизны маршрута. Поиск окна состоит из двух шагов:
Инициализация окна
Скользящее окно
Обнаруженный peaks помогают в инициализации окна поиска. Инициализируйте окна поиска как несколько интервалов с красным и синим цветами для левой и правой линий соответственно.
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). The helperDetectLanes Функция helper обнаруживает точки маршрута и визуализирует скользящие интервалы. Последовательные интервалы скользят вдоль оси 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 степени, представленный как , где a, b и c являются полиномиальными параметрами. Для выполнения аппроксимирования кривыми используйте helperFitPolynomial вспомогательная функция, которая также обрабатывает выбросы с помощью алгоритма консенсуса случайной выборки (RANSAC). Чтобы оценить 3-D точки маршрута, извлеките параметры из модели плоскости, созданной на шаге предварительной обработки. Модель плоскости представлена как , где 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 helper функция также вычисляет, аппроксимация ошибки. Обновите полосы движения, имеющие ошибочные точки, новым полиномом. Обновите этот полином путем сдвига его вдоль оси 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
Отслеживание маршрута помогает в стабилизации кривизны маршрута, вызванной внезапными рывками и дрейфами. Эти изменения могут произойти из-за отсутствия точек маршрута, транспортных средств, движущихся по полосам, и ошибочного обнаружения маршрута. Отслеживание маршрута является двухэтапным процессом:
Проследите за полиномиальными параметрами маршрутадля управления кривизной полинома.
Отслеживайте начальные точки, поступающие от пикового обнаружения. Этот параметр обозначается как в полиноме.
Эти параметры обновляются с помощью фильтра Калмана с постоянной моделью движения ускорения. Чтобы инициировать фильтр Калмана, используйте 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 класс helper. Этот класс helper реализует все предыдущие шаги, а также выполняет дополнительную предварительную обработку, чтобы удалить транспортные средства из облака точек. Это гарантирует, что обнаруженные точки Земли являются плоскими, и модель плоскости является точной. Метод класса 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 точек маршрута. Вы также использовали отслеживание фильтра Калмана для дальнейшего улучшения обнаружения маршрута.
The helperLoadData Функция helper загружает набор данных лидара в рабочее пространство 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 Функция helper вычисляет начальные точки окна поиска, используя обнаруженный peaks гистограммы.
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 Функция helper подбирает полином на основе 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 Функция helper вычисляет гистограмму значений интенсивности облаков точек.
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 Функция helper извлекает peaks из значений гистограммы.
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] Ghallabi, Farouk, Fawzi Nashashibi, Ghayath El-Haj-Shhhade, и Marie-Anne Mitte. «Обнаружение маркировки маршрута на основе LIDAR для позиционирования транспортного средства в HD-карте». В 2018 году 21-я Международная конференция по интеллектуальным транспортным системам (ITSC), 2209-14. Maui: 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.