Создайте карту из 2-D сканов Лидара с использованием SLAM

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

Алгоритм SLAM в этом примере постепенно обрабатывает сканы лидара и строит график положения, чтобы создать карту окружения. Чтобы преодолеть дрейф, накопленный в оценочной траектории робота, пример распознает ранее посещенные места через сопоставление сканов и использует информацию замыкания цикла, чтобы оптимизировать положения и обновить карту окружения. Чтобы оптимизировать график положения, этот пример использует оптимизацию 2-D графика положения из Navigation Toolbox™.

В этом примере вы узнаете, как:

  • Оцените траекторию робота из ряда сканов с помощью скана алгоритмов регистрации.

  • Оптимизируйте дрейф в предполагаемой траектории робота путем определения ранее посещенных мест (циклы).

  • Визуализируйте карту окружения, используя сканы и их абсолютные положения.

Загрузка лазерных сканов

Этот пример использует данные, собранные в помещении с помощью робота- Jackal™ из Robotics™ Clearpath. Робот оснащён SICK™ TiM-511 лазерным сканером с максимальной областью значений 10 метров. Загрузите offlineSlamData.mat файл, содержащий сканы лазера в рабочую область.

data = load('offlineSlamData.mat');
scans = data.scans;

Оценка траектории робота

В примере используется matchScansGrid и matchScans функции для оценки относительного положения между последовательными сканами. The matchScansGrid функция предоставляет начальную оценку для относительного положения, которая точна до заданного разрешения. The matchScans функция использует оценку как начальное предположение и уточняет относительное положение для лучшей оценки.

% Set maximum lidar range to be slightly smaller than maximum range of the
% scanner, as the laser readings are less accurate near maximum range
maxLidarRange = 8;
% Set the map resolution to 10 cells per meter, which gives a precision of
% 10cm
mapResolution = 10;
% Create a pose graph object and define information matrix
pGraph = poseGraph;
infoMat = [1 0 0 1 0 1];
% Loop over each scan and estimate relative pose
prevScan = scans{1};
for i = 2:numel(scans)
    currScan = scans{i};
    % Estimate relative pose between current scan and previous scan
    [relPose,stats] = matchScansGrid(currScan,prevScan, ...
        'MaxRange',maxLidarRange,'Resolution',mapResolution);
    % Refine the relative pose
    relPoseRefined = matchScans(currScan,prevScan,'initialPose',relPose);
    % Add relative pose to the pose graph object
    pGraph.addRelativePose(relPoseRefined,infoMat);
    ax = show(pGraph,'IDs','off');
    title(ax,'Estimated Robot Trajectory')
    drawnow
    prevScan = currScan;
end

Figure contains an axes. The axes with title Estimated Robot Trajectory contains 2 objects of type line.

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

  • Шумные сканы с датчика без достаточного перекрытия

  • Отсутствие существенных функций

  • Неточное начальное преобразование, особенно когда вращение значительно

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

hFigMap = figure;
axMap = axes('Parent',hFigMap);
helperShow(scans,pGraph,maxLidarRange,axMap);
title(axMap,'Map of the Environment and Robot Trajectory')

Figure contains an axes. The axes with title Map of the Environment and Robot Trajectory contains 72 objects of type line.

Коррекция дрейфа

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

Обнаружение замыкания цикла

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

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

loopClosureThreshold = 110;
loopClosureSearchRadius = 2;
[loopClosureEdgeIds,loopClosurePoses] = helperDetectLoop(scans,pGraph, ...
    loopClosureSearchRadius,loopClosureThreshold);

Оптимизация траектории

Добавьте обнаруженные ребра замыкания цикла к графику положения, чтобы исправить дрейф в предполагаемой траектории. Используйте optimizePoseGraph (Navigation Toolbox) функция для оптимизации графика положения.

% Add loop closure edges to pose graph
if ~isempty(loopClosureEdgeIds)
    for k = 1:size(loopClosureEdgeIds,1)
        pGraph.addRelativePose(loopClosurePoses(k,:),infoMat, ...
            loopClosureEdgeIds(k,1),loopClosureEdgeIds(k,2));
    end
end
% Optimize pose graph
updatedPGraph = optimizePoseGraph(pGraph);

Визуализация

Визуализируйте изменение траектории робота до и после оптимизации графика положения. Красные линии представляют ребра замыкания цикла.

hFigTraj = figure('Position',[0 0 900 450]);

% Visualize robot trajectory before optimization
axPGraph = subplot(1,2,1,'Parent',hFigTraj);
axPGraph.Position = [0.04 0.1 0.45 0.8];
show(pGraph,'IDs','off','Parent',axPGraph);
title(axPGraph,'Before PGO')

% Visualize robot trajectory after optimization
axUpdatedPGraph = subplot(1,2,2,'Parent',hFigTraj);
axUpdatedPGraph.Position = [0.54 0.1 0.45 0.8];
show(updatedPGraph,'IDs','off','Parent',axUpdatedPGraph);
title(axUpdatedPGraph,'After PGO')
axis([axPGraph axUpdatedPGraph],[-6 10 -7 3])
sgtitle('Robot Trajectory','FontWeight','bold')

Figure contains 2 axes and another object of type subplottext. Axes 1 with title Before PGO contains 3 objects of type line. Axes 2 with title After PGO contains 3 objects of type line.

Визуализируйте карту окружения и траекторию робота до и после оптимизации графика положения.

hFigMapTraj = figure('Position',[0 0 900 450]);

% Visualize map and robot trajectory before optimization
axOldMap = subplot(1,2,1,'Parent',hFigMapTraj);
axOldMap.Position = [0.05 0.1 0.44 0.8];
helperShow(scans,pGraph,maxLidarRange,axOldMap)
title(axOldMap,'Before PGO')

% Visualize map and robot trajectory after optimization
axUpdatedMap = subplot(1,2,2,'Parent',hFigMapTraj);
axUpdatedMap.Position = [0.56 0.1 0.44 0.8];
helperShow(scans,updatedPGraph,maxLidarRange,axUpdatedMap)
title(axUpdatedMap,'After PGO')
axis([axOldMap axUpdatedMap],[-9 18 -10 9])
sgtitle('Map of the Environment and Robot Trajectory','FontWeight','bold')

Figure contains 2 axes and another object of type subplottext. Axes 1 with title Before PGO contains 72 objects of type line. Axes 2 with title After PGO contains 72 objects of type line.

Вспомогательные функции

The helperShow Функция helper визуализирует карту окружения и траекторию робота. Функция преобразует сканы лидара, используя их соответствующие положения, чтобы создать карту окружения.

function helperShow(scans,pGraph,maxRange,ax)
    hold(ax,'on')
    for i = 1:numel(scans)
        sc = transformScan(scans{i}.removeInvalidData('RangeLimits',[0.02 maxRange]), ...
            pGraph.nodes(i));
        scPoints = sc.Cartesian;
        plot(ax,scPoints(:,1),scPoints(:,2),'.','MarkerSize',3,'color','m')
    end
    nds = pGraph.nodes;
    plot(ax,nds(:,1),nds(:,2),'.-','MarkerSize',5,'color','b')
    hold(ax,'off')
    axis(ax,'equal')
    box(ax,'on')
    grid(ax,'on')
    xlabel('X')
    ylabel('Y')
end

См. также

Функции

matchScansGrid | matchScans | addRelativePose (Navigation Toolbox) | show (Navigation Toolbox) | optimizePoseGraph (Navigation Toolbox)

Объекты

lidarScan | poseGraph (Navigation Toolbox)

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