Exponenta Banner
exponenta event banner

Выполните ХЛОПОК Используя 3D облака пункта лидара

В этом примере используются:

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

Этот пример демонстрирует, как осуществить алгоритм одновременной локализации и отображения (SLAM) на собранных 3D данных о датчике лидара, используя облако пункта обработка алгоритмов и оптимизации графика позы. Цель этого примера состоит в том, чтобы оценить траекторию робота и создать 3D карту занятия окружающей среды от 3D облаков пункта лидара и оцененной траектории.

Продемонстрированный алгоритм SLAM оценивает траекторию, используя алгоритм регистрации облака точек на основе нормального преобразования распределения (NDT), и уменьшает дрейф, используя оптимизацию графов позы SE3 используя решатель областей доверия, всякий раз, когда робот пересматривает место.

Загрузка данных и настройка перестраиваемых параметров

Загрузите данные 3-D лидара, собранные от робота Clearpath™ Хаски, в гараж. Данные лидара содержат массив ячеек n-на-3 матрицы, где n - количество точек 3-D захваченных данных лидара, а столбцы представляют координаты xyz, связанные с каждой захваченной точкой.

load pClouds.mat

Параметры алгоритма регистрации облака точек

Задайте параметры для оценки траектории с помощью алгоритма регистрации облака точек. maxLidarRange задает максимальный диапазон 3-D лазерного сканера.

maxLidarRange = 20;

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

  • referenceVector - По нормали к плоскости земли.

  • maxDistance - Максимальное расстояние для врезок при снятии плоскости грунта и потолка.

  • maxAngularDistance - максимальное отклонение угла от опорного вектора при установке плоскостей грунта и потолка.

referenceVector = [0 0 1];
maxDistance = 0.5;
maxAngularDistance = 15;

Для повышения эффективности и точности алгоритма регистрации точечные облака понижают выборку с использованием случайной выборки с коэффициентом выборки, заданным randomSampleRatio.

randomSampleRatio = 0.25;

gridStep определяет размеры сетки вокселя, используемые в алгоритме регистрации неразрушающего контроля. Сканирование принимается только после перемещения робота на расстояние, большее, чем distanceMovedThreshold.

gridStep = 2.5;
distanceMovedThreshold = 0.3;

Настройте эти параметры для конкретного робота и среды.

Параметры алгоритма оценки замыкания контура

Укажите параметры для алгоритма оценки замыкания контура. Поиск замыкателей контура выполняется только в радиусе вокруг текущего местоположения робота, указанного loopClosureSearchRadius.

loopClosureSearchRadius = 3;

Алгоритм закрытия петли основан на 2-й подкарте и соответствии просмотра. Частичная карта создается после каждого nScansPerSubmap (Количество сканирований на подкарту). Алгоритм замыкания контура игнорирует самые последние subMapThresh сканирует при поиске кандидатов цикла. 

nScansPerSubmap = 3;
subMapThresh = 50;

Кольцевая область с z-границами, заданными annularRegionLimits извлекается из облаков точек. Точки за пределами этих пределов на полу и потолке удаляются после того, как алгоритмы соответствия плоскости облака точек идентифицируют интересующую область.

annularRegionLimits = [-0.75 0.75];

Максимально допустимая среднеквадратичная ошибка корня (RMSE) при оценке относительной позы между кандидатами цикла определяется rmseThreshold. Выберите меньшее значение для оценки точных кромок замыкания контура, что оказывает большое влияние на оптимизацию графика позы.

rmseThreshold = 0.26;

Пороговое значение показателя соответствия сканирования для принятия замыкания цикла определяется loopClosureThreshold. Оптимизация графика позы вызывается после вставки optimizationInterval ребер замыкания контура в график позы.

loopClosureThreshold = 150;
optimizationInterval = 2;

Инициализация переменных

Настройте график позы, карту занятости и необходимые переменные.

% 3D Posegraph object for storing estimated relative poses
pGraph = poseGraph3D;
% Default serialized upper-right triangle of 6-by-6 Information Matrix
infoMat = [1,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1];
% Number of loop closure edges added since last pose graph optimization and map refinement
numLoopClosuresSinceLastOptimization = 0; 
% True after pose graph optimization until the next scan
mapUpdated = false;
% Equals to 1 if the scan is accepted
scanAccepted = 0;

% 3D Occupancy grid object for creating and visualizing 3D map
mapResolution = 8; % cells per meter
omap = occupancyMap3D(mapResolution);

Предварительно распределить переменные для обработанных облаков точек, сканирований лидаров и частичных карт. Создайте пониженный набор облаков точек для быстрой визуализации карты.

pcProcessed = cell(1,length(pClouds));
lidarScans2d = cell(1,length(pClouds)); 
submaps = cell(1,length(pClouds)/nScansPerSubmap);

pcsToView = cell(1,length(pClouds));

Создание переменных для отображения.

% Set to 1 to visualize created map and posegraph during build process
viewMap = 1; 
% Set to 1 to visualize processed point clouds during build process
viewPC = 0;

Задайте случайное начальное число, чтобы гарантировать последовательную случайную выборку.

rng(0);

При необходимости инициализируйте окна рисунков.

% If you want to view the point clouds while processing them sequentially
if viewPC==1
    pplayer = pcplayer([-50 50],[-50 50],[-10 10],'MarkerSize',10);
end

% If you want to view the created map and posegraph during build process
if viewMap==1
    ax = newplot; % Figure axis handle
    view(20,50);
    grid on;
end

Figure contains an axes. The axes is empty.

Оценка и уточнение траектории с использованием оптимизации графика позы

Траектория робота представляет собой совокупность поз робота (расположение и ориентация в 3-D пространстве). Поза робота оценена в каждый 3D случай приобретения просмотра лидара, используя 3D алгоритм ХЛОПКА лидара. Алгоритм SLAM 3-D лидара состоит из следующих шагов:

  • Фильтрация облака точек

  • Понижение дискретизации облака точек

  • Регистрация облака точек

  • Запрос на закрытие цикла

  • Оптимизация графика позы

Итеративная обработка облаков точек для оценки траектории.

count = 0; % Counter to track number of scans added
disp('Estimating robot trajectory...');
Estimating robot trajectory...

for i=1:length(pClouds)
    % Read point clouds in sequence
    pc = pClouds{i};

Фильтрация облака точек

Фильтрация облака точек выполняется для извлечения области, представляющей интерес, из полученного сканирования. В этом примере интересующей областью является кольцевая область со снятыми грунтом и потолком.

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

    ind = (-maxLidarRange < pc(:,1) & pc(:,1) < maxLidarRange ...
        & -maxLidarRange  < pc(:,2) & pc(:,2) < maxLidarRange ...
        & (abs(pc(:,2))>abs(0.5*pc(:,1)) | pc(:,1)>0));
    
    pcl = pointCloud(pc(ind,:));

Удалите точки на нулевой плоскости.

    [~, ~, outliers] = ...
        pcfitplane(pcl, maxDistance,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl,outliers,'OutputSize','full');

Удаление точек на плоскости потолка.

    [~, ~, outliers] = ...
        pcfitplane(pcl_wogrd,0.2,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl_wogrd,outliers,'OutputSize','full');

Выберите точки в кольцевой области.

    ind = (pcl_wogrd.Location(:,3)<annularRegionLimits(2))&(pcl_wogrd.Location(:,3)>annularRegionLimits(1));
    pcl_wogrd = select(pcl_wogrd,ind,'OutputSize','full');

Понижение дискретизации облака точек

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

    pcl_wogrd_sampled = pcdownsample(pcl_wogrd,'random',randomSampleRatio);
    
    if viewPC==1
        % Visualize down sampled point cloud
        view(pplayer,pcl_wogrd_sampled);
        pause(0.001)
    end

Регистрация облака точек

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

    if count == 0
        % First can
        tform = [];
        scanAccepted = 1;
    else
        if count == 1
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep);
        else
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep,...
                'InitialTransform',prevTform);
        end
        
        relPose = [tform2trvec(tform.T') tform2quat(tform.T')];
        
        if sqrt(norm(relPose(1:3))) > distanceMovedThreshold
            addRelativePose(pGraph,relPose);
            scanAccepted = 1;
        else
            scanAccepted = 0;
        end
    end

Запрос замыкания цикла

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

  • Создание частичных карт из nScansPerSubmap последовательных сканирований.

  • Сопоставьте текущее сканирование с подкартами в пределах loopClosureSearchRadius.

  • Принимать совпадения, если оценка совпадений больше, чем loopClosureThreshold. Все сканирования, представляющие принятую подкарту, рассматриваются как вероятные кандидаты цикла.

  • Оценка относительной позы между вероятными кандидатами на цикл и текущим сканированием. Относительная поза принимается как ограничение замыкания контура, только когда RMSE меньше, чем rmseThreshold. 

    if scanAccepted == 1
        count = count + 1;
        
        pcProcessed{count} = pcl_wogrd_sampled;
        
        lidarScans2d{count} = exampleHelperCreate2DScan(pcl_wogrd_sampled);
        
        % Submaps are created for faster loop closure query. 
        if rem(count,nScansPerSubmap)==0
            submaps{count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d,...
                pGraph,count,nScansPerSubmap,maxLidarRange);
        end
        
        % loopSubmapIds contains matching submap ids if any otherwise empty.   
        if (floor(count/nScansPerSubmap)>subMapThresh)
            [loopSubmapIds,~] = exampleHelperEstimateLoopCandidates(pGraph,...
                count,submaps,lidarScans2d{count},nScansPerSubmap,...
                loopClosureSearchRadius,loopClosureThreshold,subMapThresh);
            
            if ~isempty(loopSubmapIds)
                rmseMin = inf;
                
                % Estimate best match to the current scan
                for k = 1:length(loopSubmapIds)
                    % For every scan within the submap
                    for j = 1:nScansPerSubmap
                        probableLoopCandidate = ...
                            loopSubmapIds(k)*nScansPerSubmap - j + 1;
                        [loopTform,~,rmse] = pcregisterndt(pcl_wogrd_sampled,...
                            pcProcessed{probableLoopCandidate},gridStep);
                        % Update best Loop Closure Candidate
                        if rmse < rmseMin
                            loopCandidate = probableLoopCandidate;
                            rmseMin = rmse;
                        end
                        if rmseMin < rmseThreshold
                            break;
                        end
                    end
                end
                
                % Check if loop candidate is valid
                if rmseMin < rmseThreshold
                    % loop closure constraint
                    relPose = [tform2trvec(loopTform.T') tform2quat(loopTform.T')];
                    
                    addRelativePose(pGraph,relPose,infoMat,...
                        loopCandidate,count);
                    numLoopClosuresSinceLastOptimization = numLoopClosuresSinceLastOptimization + 1;
                end
                     
            end
        end

Оптимизация графика позы

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

        if (numLoopClosuresSinceLastOptimization == optimizationInterval)||...
                ((numLoopClosuresSinceLastOptimization>0)&&(i==length(pClouds)))
            if loopClosureSearchRadius ~=1
                disp('Doing Pose Graph Optimization to reduce drift.');
            end
            % pose graph optimization
            pGraph = optimizePoseGraph(pGraph);
            loopClosureSearchRadius = 1;
            if viewMap == 1
                position = pGraph.nodes;
                % Rebuild map after pose graph optimization
                omap = occupancyMap3D(mapResolution);
                for n = 1:(pGraph.NumNodes-1)
                    insertPointCloud(omap,position(n,:),pcsToView{n}.removeInvalidPoints,maxLidarRange);
                end
                mapUpdated = true;
                ax = newplot;
                grid on;
            end
            numLoopClosuresSinceLastOptimization = 0;
            % Reduce the frequency of optimization after optimizing
            % the trjectory
            optimizationInterval = optimizationInterval*7;
        end

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

        pcToView = pcdownsample(pcl_wogrd_sampled, 'random', 0.5);
        pcsToView{count} = pcToView;
        
        if viewMap==1
            % Insert point cloud to the occupance map in the right position
            position = pGraph.nodes(count);
            insertPointCloud(omap,position,pcToView.removeInvalidPoints,maxLidarRange);
            
            if (rem(count-1,15)==0)||mapUpdated
                exampleHelperVisualizeMapAndPoseGraph(omap, pGraph, ax);
            end
            mapUpdated = false;
        else
            % Give feedback to know that example is running
            if (rem(count-1,15)==0)
                fprintf('.');
            end
        end

Обновление предыдущей оценки относительной позы и облака точек.

        prevPc = pcl_wogrd_sampled;
        prevTform = tform;
    end
end
Doing Pose Graph Optimization to reduce drift.

Figure contains an axes. The axes with title Occupancy Map contains 4 objects of type patch, line.

Построение и визуализация карты занятости 3-D

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

if (viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>0)
    nodesPositions = nodes(pGraph);
    % Create 3D Occupancy grid
    omapToView = occupancyMap3D(mapResolution);
    
    for i = 1:(size(nodesPositions,1)-1)
        pc = pcsToView{i};
        position = nodesPositions(i,:);
        
        % Insert point cloud to the occupance map in the right position
        insertPointCloud(omapToView,position,pc.removeInvalidPoints,maxLidarRange);
    end
    
    figure;
    axisFinal = newplot;
    exampleHelperVisualizeMapAndPoseGraph(omapToView, pGraph, axisFinal);
end

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

Поддержка