addScan

Добавьте сканирование, чтобы лоцировать карту SLAM

Описание

addScan(slamObj,currScan) добавляет сканирование лидара, currScan, к объекту SLAM лидара, slamObj. Функциональное сканирование использования, соответствующее, чтобы коррелировать это сканирование к новому, затем добавляет его в график положения, заданный в slamObj. Если сканирование принято, addScan обнаруживает закрытия цикла и оптимизирует на основе настроек в slamObj.

пример

addScan(slamObj,currScan,relPoseEst) также задает относительное положение к последнему положению сканирования лидара в slamObj. Это относительное положение улучшает соответствие сканирования.

[isAccepted,loopClosureInfo,optimInfo] = addScan(___) выходная подробная информация о добавлении сканирования к объекту SLAM. isAccepted указывает, добавляется ли сканирование или отклоняется. loopClosureInfo и optimInfo укажите, обнаруживается ли закрытие цикла, или график положения оптимизирован.

Примеры

свернуть все

Используйте lidarSLAM объект итеративно добавлять и сравнить сканирования лидара и создать оптимизированный график положения траектории робота. Чтобы получить карту заполнения от связанных положений и сканирований, используйте buildMap функция.

Загрузите данные и настроенный алгоритм SLAM

Загрузите массив ячеек lidarScan объекты. Сканирования лидара были собраны в гараже на роботе Husky® от ClearPath Robotics®. Как правило, лоцируйте сканирования, взяты в высокой частоте, и каждое сканирование не нужно для SLAM. Поэтому вниз произведите сканирования путем выбора только каждого 40-го сканирования.

load garage_fl1_southend.mat scans
scans = scans(1:40:end);

Чтобы настроить алгоритм SLAM, укажите диапазон лидара, сопоставьте разрешение, порог закрытия цикла и поисковый радиус. Настройте эти параметры для своего определенного робота и среды. Создайте lidarSLAM объект этими параметрами.

maxRange = 19.2; % meters
resolution = 10; % cells per meter

slamObj = lidarSLAM(resolution,maxRange);
slamObj.LoopClosureThreshold = 360;
slamObj.LoopClosureSearchRadius = 8;

Добавьте сканирования итеративно

Используя for цикл, добавляют сканирования к объекту SLAM. Объект использует сканирование, соответствующее, чтобы сравнить каждое добавленное сканирование с ранее добавленными единицами. Чтобы улучшить карту, объект оптимизирует график положения каждый раз, когда это обнаруживает закрытие цикла. Каждые 10 сканирований, отобразите сохраненные положения и сканирования.

for i = 1:numel(scans)

    addScan(slamObj,scans{i});
    
    if rem(i,10) == 0
        show(slamObj);
    end
end

Просмотрите карту заполнения

После добавления всех сканирований к объекту SLAM создайте occupancyMap карта путем вызова buildMap со сканированиями и положениями. Используйте то же разрешение карты и макс. область значений, которую вы использовали с объектом SLAM.

[scansSLAM,poses] = scansAndPoses(slamObj);
occMap = buildMap(scansSLAM,poses,resolution,maxRange);
figure
show(occMap)
title('Occupancy Map of Garage')

Входные параметры

свернуть все

Лоцируйте объект SLAM, заданный как lidarSLAM объект. Объект содержит параметры алгоритма SLAM, данные о датчике, и базовый график положения раньше создавал карту.

Лоцируйте чтение сканирования, заданное как lidarScan объект. Это сканирование коррелируется к новому сканированию в slamObj использование соответствия сканирования.

Относительная оценка положения сканирования, заданного как [x y theta] вектор. Это относительное положение улучшает соответствие сканирования.

Выходные аргументы

свернуть все

Указывает, принято ли сканирование, возвращено как true или false. Если относительное положение между сканированиями ниже MovementThreshold свойство slamObj, сканирование отклоняется. По умолчанию все сканирования приняты.

Детали закрытия цикла, возвращенные как структура с этими полями:

  • EdgeIDs – Идентификаторы недавно связанных ребер в графике положения, возвращенном как вектор.

  • Edges – Недавно добавленные ребра закрытия цикла, возвращенные как n-by-2 матрица идентификаторов узла, которые соединяет каждое ребро.

  • Scores – Множество недавно связанных ребер в графике положения, возвращенном в соответствие сканирования, возвращенное как вектор.

Примечание

Если LoopClosureAutoRollback свойство установлено в true в slamObj, ребра закрытия цикла могут быть удалены из графика положения. Это свойство отклоняет закрытия циклов, если остаточная ошибка изменяется решительно после оптимизации. Поэтому некоторые идентификаторы ребра, перечисленные в этой структуре, не могут существовать в фактическом графике положения.

Изложите детали оптимизации графика, возвращенные как структура с этими полями:

  • IsPerformed – Булевская переменная, указывающая, выполняется ли оптимизация при добавлении этого сканирования. Производительность оптимизации зависит от OptimizationInterval свойство в slamObj.

  • IsAccepted – Булевская переменная, указывающая, была ли оптимизация принята на основе ResidualError.

  • ResidualError – Ошибка сопоставлена с оптимизацией, возвращенной как скаляр.

  • LoopClosureRemoved – Список идентификаторов ребер закрытия цикла, удаленных во время оптимизации, возвращенной как вектор.

Расширенные возможности

Введенный в R2019b

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