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

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