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

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

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

В этом примере вы учитесь как:

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

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

  • Визуализируйте карту среды с помощью сканов и их абсолютных положений.

Загрузите лазерные сканы

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

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

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

Пример использует matchScansGrid и matchScans функции, чтобы оценить относительное положение между последовательными сканами. matchScansGrid функция обеспечивает первоначальную оценку для относительного положения, которое точно до заданного разрешения. 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 функции помощника, заданной в разделе Supporting Functions этого примера.

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

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.

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

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

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)

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