Документация

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

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