Выполните SLAM Используя 3-D облака точек лидара

Этот пример использует:

Скрипт Open Live Script

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

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

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

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

load pClouds.mat

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

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

maxLidarRange = 20;

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

  • referenceVector - Нормальный к наземной плоскости.

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

  • maxAngularDistance - Максимальное угловое отклонение от ссылочного вектора при подборе кривой земле и потолке плоскостей.

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

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

randomSampleRatio = 0.25;

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

gridStep = 2.5;
distanceMovedThreshold = 0.3;

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

Алгоритм оценки закрытия цикла for параметров

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

loopClosureSearchRadius = 3;

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

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;

Установите случайный seed гарантировать сопоставимую случайную выборку.

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

Оценка траектории и улучшение Используя оптимизацию графика положения

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

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

  • Субдискретизация облака точек

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

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

  • Изложите оптимизацию графика

Итеративно обработайте облака точек, чтобы оценить траекторию.

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

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

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

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

  • Каждая подкарта представляет nScansPerSubmap последовательные сканирования. Предыдущие сканирования, представленные всеми подкартами соответствия, рассматриваются как соответствие текущему сканированию (вероятные кандидаты цикла).

  • Относительное положение между текущим сканированием и вероятным кандидатом цикла вычисляется с помощью регистрационного алгоритма NDT. Кандидат цикла с наименее относительной ошибкой оценки положения (RMSE) рассматривается как лучшее соответствие для текущего сканирования.

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

    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.

Создайте и визуализируйте 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
Документация Navigation Toolbox
Поддержка
Для просмотра документации необходимо авторизоваться на сайте