Этот пример демонстрирует, как реализовать алгоритм Одновременной локализации и отображения (SLAM) на собранной серии сканирований лидара с помощью оптимизации графика положения. Цель этого примера состоит в том, чтобы создать карту среды с помощью сканирований лидара и получить траекторию робота.
Чтобы создать карту среды, алгоритм SLAM инкрементно обрабатывает сканирования лидара и создает график положения, который соединяет эти сканирования. Робот распознает ранее посещаемое место посредством соответствия сканирования и может установить одно или несколько закрытий цикла вдоль его движущегося пути. Алгоритм SLAM использует информацию о закрытии цикла, чтобы обновить карту и настроить предполагаемую траекторию робота.
Загрузите прореженный набор данных, состоящий из лазерных сканирований, собранных из мобильного робота во внутренней среде. Среднее смещение между каждыми двумя сканированиями составляет приблизительно 0,6 метра.
offlineSlamData.mat
файл содержит scans
переменная, которая содержит все лазерные сканирования, используемые в этом примере
load('offlineSlamData.mat');
План здания и аппроксимированный путь робота предоставлены для иллюстративных целей. Это изображение показывает относительную сопоставляемую среду и аппроксимированная траектория робота.
Создайте a lidarSLAM
объект и набор разрешение карты и макс. область значений лидара. Этот пример использует робота Jackal™ от Clearpath Robotics™. Робот оборудован сканером лазера SICK™ TiM-511 с макс. областью значений 10 метров. Установите макс. область значений лидара, немного меньшую, чем макс. область значений сканирования (8 м), когда лазерные показания являются менее точной близкой макс. областью значений. Установите разрешение карты сетки 20 ячеек на метр, который дает точность на 5 см.
maxLidarRange = 8; mapResolution = 20; slamAlg = lidarSLAM(mapResolution, maxLidarRange);
Следующие параметры закрытия цикла устанавливаются опытным путем. Используя более высокий цикл порог закрытия помогает отклонить ложные положительные стороны в идентификационном процессе закрытия цикла. Однако имейте в виду, что соответствие высокого счета может все еще быть плохим соответствием. Например, сканирования, собранные в среде, которая имеет подобные или повторные функции, более вероятно, произведут ложные положительные стороны. Используя более высокий цикл радиус поиска закрытия позволяет алгоритму искать более широкую область значений карты вокруг текущих оценочных закрытий цикла for положения.
slamAlg.LoopClosureThreshold = 210; slamAlg.LoopClosureSearchRadius = 8;
Инкрементно добавьте сканирования в slamAlg
объект. Числа сканирования распечатаны, если добавлено к карте. Объект отклоняет сканирования, если расстояние между сканированиями слишком мало. Добавьте первые 10 сканирований сначала, чтобы протестировать ваш алгоритм.
for i=1:10 [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i}); if isScanAccepted fprintf('Added scan %d \n', i); end end
Added scan 1 Added scan 2 Added scan 3 Added scan 4 Added scan 5 Added scan 6 Added scan 7 Added scan 8 Added scan 9 Added scan 10
Восстановите сцену путем графического вывода сканирований и положений, прослеженных slamAlg
.
figure; show(slamAlg); title({'Map of the Environment','Pose Graph for Initial 10 Scans'});
Продолжите добавлять сканирования в цикле. Закрытия цикла должны быть автоматически обнаружены, когда робот перемещается. Позируйте оптимизация графика выполняется каждый раз, когда закрытие цикла идентифицировано. Выход optimizationInfo
имеет поле, IsPerformed
, это указывает, когда оптимизация графика положения происходит..
Постройте сканирования и положения каждый раз, когда закрытие цикла идентифицировано, и проверьте результаты визуально. Этот график показывает наложенные сканирования и оптимизированный график положения для первого закрытия цикла. Ребро закрытия цикла добавляется как красная ссылка.
firstTimeLCDetected = false; figure; for i=10:length(scans) [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i}); if ~isScanAccepted continue; end % visualize the first detected loop closure, if you want to see the % complete map building process, remove the if condition below if optimizationInfo.IsPerformed && ~firstTimeLCDetected show(slamAlg, 'Poses', 'off'); hold on; show(slamAlg.PoseGraph); hold off; firstTimeLCDetected = true; drawnow end end title('First loop closure');
Постройте созданную карту финала после того, как все сканирования будут добавлены к slamAlg
объект. Предыдущий for
цикл должен был добавить все сканирования только несмотря на графический вывод начального закрытия цикла.
figure show(slamAlg); title({'Final Built Map of the Environment', 'Trajectory of the Robot'});
Изображение сканирований и графика положения наложено на исходной планировке. Вы видите, что карта совпадает с исходным планом здания много позже добавления всех сканирований и оптимизации графика положения.
Оптимизированные сканирования и положения могут использоваться, чтобы сгенерировать a occupancyMap
, который представляет среду как вероятностную сетку заполнения.
[scans, optimizedPoses] = scansAndPoses(slamAlg); map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);
Визуализируйте карту сетки заполнения, заполненную с лазерными сканированиями и оптимизированным графиком положения.
figure; show(map); hold on show(slamAlg.PoseGraph, 'IDs', 'off'); hold off title('Occupancy Grid Map Built Using Lidar SLAM');