Этот пример показывает вам, как реализовать алгоритм одновременной локализации и картографии (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
Заметьте, что предполагаемая траектория робота дрейфует в зависимости от времени. Дрейф может произойти из-за любой из следующих причин:
Шумные сканы от датчика без достаточного перекрытия
Отсутствие значительных функций
Неточное начальное преобразование, особенно когда вращение является значительным
Дрейф в предполагаемой траектории приводит к неточной карте среды. Визуализируйте карту и траекторию робота с помощью helperShow функции помощника, заданной в разделе Supporting Functions этого примера.
hFigMap = figure; axMap = axes('Parent',hFigMap); helperShow(scans,pGraph,maxLidarRange,axMap); title(axMap,'Map of the Environment and Robot Trajectory')
Откорректируйте дрейф в траектории путем точного обнаружения циклов, которые являются местами, которые ранее посетил робот. Добавьте ребра закрытия цикла в график положения, который помогает откорректировать дрейф в траектории во время оптимизации графика положения.
Обнаружение закрытия цикла определяет, посетил ли робот ранее текущее местоположение. Поиск выполняется путем соответствия с текущим сканом против предыдущих сканов вокруг текущего местоположения робота в радиусе, заданном 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')
Визуализируйте карту среды и траектории робота до и после оптимизации графика положения.
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')
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)