Выполните 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

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

Запрос закрытия цикла определяет, посетили ли текущее местоположение робота ранее. Поиск выполняется путем соответствия с текущим сканом с предыдущими сканами в маленьком радиусе вокруг текущего местоположения робота, заданного 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.

Создайте и визуализируйте 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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте