Оцените положение робота с соответствием сканирования

Этот пример демонстрирует, как совпадать с двумя лазерными сканированиями с помощью алгоритма Преобразования нормальных распределений (NDT) [1]. Цель сканирования, соответствующего, состоит в том, чтобы найти относительное положение (или преобразовать) между двумя положениями робота, где рентген был сделан. Сканирования могут быть выровнены на основе форм их перекрывающихся функций.

Чтобы оценить это положение, NDT подразделяет лазерное сканирование на 2D ячейки, и каждая ячейка присвоена соответствующее нормальное распределение. Распределение представляет вероятность измерения точки в той ячейке. Если плотность вероятности вычисляется, метод оптимизации находит относительное положение между текущим лазерным сканированием и ссылочным лазерным сканированием. Чтобы ускорить сходимость метода, исходное предположение положения может быть обеспечено. Как правило, одометрия робота используется, чтобы предоставить первоначальную оценку.

Если вы применяете сканирование, соответствующее к последовательности сканирований, можно использовать его, чтобы восстановить грубую карту среды, которую пересекает робот. Отсканируйте соответствие, также играет важную роль в других приложениях, таких как отслеживание положения и Одновременная локализация и отображение (SLAM).

Загрузите лазерные данные сканирования из файла

load scanMatchingData.mat

Лазерные данные сканирования были собраны мобильным роботом во внутренней среде. Аппроксимированную планировку области, наряду с путем робота через пробел, показывают в следующем изображении.

Постройте два лазерных сканирования

Выберите два лазерных сканирования, чтобы отсканировать соответствие из сообщений ROS laserMsg. Они должны совместно использовать типичные функции путем нахождения близко друг к другу в последовательности.

referenceScan = lidarScan(laserMsg{180});
currentScan = lidarScan(laserMsg{202});

Отобразите два сканирования. Уведомление там является переводными и вращательными смещениями, но некоторые функции все еще соответствуют.

currScanCart = currentScan.Cartesian;
refScanCart = referenceScan.Cartesian;
figure
plot(refScanCart(:,1), refScanCart(:,2), 'k.');
hold on
plot(currScanCart(:,1), currScanCart(:,2), 'r.');
legend('Reference laser scan', 'Current laser scan', 'Location', 'NorthWest');

Запустите алгоритм соответствия сканирования и отображение преобразованное сканирование

Передайте эти два сканирования функции соответствия сканирования. matchScans вычисляет относительное положение текущего сканирования относительно ссылочного сканирования.

transform = matchScans(currentScan, referenceScan)
transform = 1×3

    0.5348   -0.0065   -0.0336

Чтобы визуально проверить, что относительное положение было вычислено правильно, преобразуйте текущее сканирование расчетным положением с помощью transformScan. Это преобразованное лазерное сканирование может использоваться, чтобы визуализировать результат.

transScan = transformScan(currentScan, transform);

Отобразите ссылочное сканирование вместе с преобразованным текущим лазерным сканированием. Если бы сканирование, соответствующее, было успешно, два сканирования должны быть хорошо выровнены.

figure
plot(refScanCart(:,1), refScanCart(:,2), 'k.');
hold on
transScanCart = transScan.Cartesian;
plot(transScanCart(:,1), transScanCart(:,2), 'r.');
legend('Reference laser scan', 'Transformed current laser scan', 'Location', 'NorthWest');

Создайте карту сетки заполнения Используя итеративное соответствие сканирования

Если вы применяете сканирование, соответствующее к последовательности сканирований, можно использовать его, чтобы восстановить грубую карту среды. Используйте robotics.OccupancyGrid класс, чтобы создать вероятностную карту сетки заполнения среды.

Создайте объект сетки заполнения для 15 метров 15-метровой областью. Установите источник карты быть [-7.5 - 7.5].

map = robotics.OccupancyGrid(15, 15, 20);
map.GridLocationInWorld = [-7.5 -7.5]
map = 
  OccupancyGrid with properties:

        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
                 GridSize: [300 300]
               Resolution: 20
             XWorldLimits: [-7.5000 7.5000]
             YWorldLimits: [-7.5000 7.5000]
      GridLocationInWorld: [-7.5000 -7.5000]

Предварительно выделите массив, чтобы получить абсолютное перемещение робота. Инициализируйте первое, изображают из себя [0 0 0]. Все другие положения относительно первого измеренного сканирования.

numScans = numel(laserMsg);
initialPose = [0 0 0];
poseList = zeros(numScans,3);
poseList(1,:) = initialPose;
transform = initialPose;

Создайте цикл для обработки сканирований и отображения области. Лазерные сканирования обрабатываются в парах. Задайте первое сканирование как ссылочное сканирование и второе сканирование как текущее сканирование. Два сканирования затем передаются алгоритму соответствия сканирования, и относительное положение между двумя сканированиями вычисляется. Функция exampleHelperComposeTransform используется, чтобы вычислить совокупного абсолютного положения робота. Данные сканирования наряду с абсолютным положением робота могут затем быть переданы в insertRay функцию сетки заполнения.

% Loop through all the scans and calculate the relative poses between them
for idx = 2:numScans
    % Process the data in pairs.
    referenceScan = lidarScan(laserMsg{idx-1});
    currentScanMsg = laserMsg{idx};
    currentScan = lidarScan(currentScanMsg);
    
    % Run scan matching. Note that the scan angles stay the same and do 
    % not have to be recomputed. To increase accuracy, set the maximum 
    % number of iterations to 500. Use the transform from the last
    % iteration as the initial estimate.
    [transform, stats] = matchScans(currentScan, referenceScan, ...
        'MaxIterations', 500, 'InitialPose', transform);
    
    % The |Score| in the statistics structure is a good indication of the
    % quality of the scan match. 
    if stats.Score / currentScan.Count < 1.0
        disp(['Low scan match score for index ' num2str(idx) '. Score = ' num2str(stats.Score) '.']);
    end
    
    % Maintain the list of robot poses. 
    absolutePose = exampleHelperComposeTransform(poseList(idx-1,:), transform);
    poseList(idx,:) = absolutePose;
       
    % Integrate the current laser scan into the probabilistic occupancy
    % grid.
    insertRay(map, absolutePose, currentScan, double(currentScanMsg.RangeMax));

end

Визуализируйте карту

Визуализируйте карту сетки заполнения, заполненную с лазерными сканированиями.

figure
show(map);
title('Occupancy grid map built using scan matching results');

Постройте абсолютные положения робота, которые были вычислены алгоритмом соответствия сканирования. Это показывает путь, что робот взял через карту среды.

hold on
plot(poseList(:,1), poseList(:,2), 'bo', 'DisplayName', 'Estimated robot position');
legend('show', 'Location', 'NorthWest')

Смотрите также

Ссылки

[1] П. Бибер, В. Стрэссер, "Нормальные распределения преобразовывают: новый подход к лазерному соответствию сканирования", в Продолжениях Международной конференции IEEE/RSJ по вопросам Интеллектуальных Роботов и Систем (IROS), 2003, стр 2743-2748

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