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

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

Открыть Live Script

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

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

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

Загрузите 3-D данные лидара, собранные у робота Clearpath™ Хаски в гараже. Данные лидара содержат массив ячеек n-by-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 задает размеры воксели, используемые в алгоритме регистрации NDT. Скан принимается только после того, как робот перемещается на расстояние, больше distanceMovedThreshold.

gridStep = 2.5;
distanceMovedThreshold = 0.3;

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

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

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

loopClosureSearchRadius = 3;

Алгоритм замыкания контура основан на 2-D и сопоставление сканов. Подкарта создается после каждого 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;

Установите случайный 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

Figure contains an axes. The axes is empty.

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

Траектория робота - это набор положений робота (расположение и ориентация в трехмерном пространстве). Положение робота оценивается на каждом 3-D образце сбора скана лидара с помощью 3-D алгоритма SLAM лидара. Алгоритм 3-D lidar 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.

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

Документация по Navigation Toolbox

Поддержка

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