В этом примере показано, как реализовать алгоритм одновременной локализации и картографии (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
Заметьте, что предполагаемая траектория робота дрейфует с течением времени. Дрейф может быть вызван любой из следующих причин:
Шумные сканы с датчика без достаточного перекрытия
Отсутствие существенных функций
Неточное начальное преобразование, особенно когда вращение значительно
Дрейф в предполагаемой траектории приводит к неточной карте окружения. Визуализируйте траекторию карты и робота с помощью функции helperShow helper, заданной в разделе Вспомогательные функции этого примера.
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 значение для поиска большего радиуса вокруг оценки положения тока для закрытий цикла, хотя это увеличивает время расчета.
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')
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)