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

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

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

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

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

load lidarScans.mat

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

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

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

referenceScan = lidarScans(180);
currentScan = lidarScans(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');

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

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

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

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

             XWorldLimits: [-7.5000 7.5000]
             YWorldLimits: [-7.5000 7.5000]
        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
      GridLocationInWorld: [-7.5000 -7.5000]
                 DataType: 'double'
             DefaultValue: 0.5000
               Resolution: 20
                 GridSize: [300 300]
             XLocalLimits: [0 15]
             YLocalLimits: [0 15]
        GridOriginInLocal: [0 0]
       LocalOriginInWorld: [-7.5000 -7.5000]

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

numScans = numel(lidarScans);
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 = lidarScans(idx-1);
    currentScan = lidarScans(idx);
    
    % 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,10);

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

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