Этот пример показывает Вам, как осуществить алгоритм одновременной локализации и отображения (SLAM) на ряде 2-х просмотров лидара, используя алгоритмы обработки просмотра и оптимизацию графика позы (PGO). Цель этого примера - оценить траекторию робота и построить карту окружающей среды.
Алгоритм SLAM в этом примере выполняет инкрементную обработку сканирования лидара и строит график позы для создания карты среды. Для преодоления дрейфа, накопленного в расчетной траектории робота, пример распознает ранее посещенные места посредством сопоставления сканирования и использует информацию о замыкании контура для оптимизации поз и обновления карты окружающей среды. Чтобы оптимизировать график позы, 2-D этом примере используется оптимизация графов позы из 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 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 значение для отклонения ложных срабатываний при обнаружении замыкания цикла, но функция может по-прежнему возвращать неправильные совпадения в средах с аналогичными или повторяющимися функциями. Чтобы решить эту проблему, увеличьте loopClosureSearchRadius значение для поиска большего радиуса вокруг текущей оценки позы для замыканий контура, хотя это увеличивает время вычислений.
loopClosureThreshold = 110;
loopClosureSearchRadius = 2;
[loopClosureEdgeIds,loopClosurePoses] = helperDetectLoop(scans,pGraph, ...
loopClosureSearchRadius,loopClosureThreshold);Добавьте обнаруженные кромки замыкания контура к графу позы для коррекции дрейфа в расчетной траектории. Используйте optimizePoseGraph(Панель инструментов навигации) для оптимизации графика позы.
% 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 (Панель инструментов навигации) | show(Панель инструментов навигации) | optimizePoseGraph(Панель инструментов навигации)