Загрузить траекторию робота из файла

Траектория робота - это ППМ, заданные роботу для перемещения в моделируемой среде. В этом примере представлена траектория робота.

load slamRobotTrajectory.mat

План этажа и приблизительная траектория робота приведены в иллюстративных целях. На этом рисунке показана отображаемая среда и приблизительная траектория робота.

Загрузка и просмотр виртуального мира

В этом примере используется виртуальная сцена с двумя транспортными средствами и четырьмя стенами в качестве препятствий и робот, оснащенный лидарным сканером, показанным в Simulink 3D Animation Viewer. Навигацию в виртуальной сцене можно осуществлять с помощью строки меню, панели инструментов, панели навигации, мыши и клавиатуры. Основные особенности зрителя проиллюстрированы в примере манипуляции плоскостью с помощью космической мыши MATLAB ® Object (Simulink 3D Animation).

Создание и открытие vrworld объект.

w = vrworld('slamSimulatedWorld.x3d');
open(w)

Создание фигуры, показывающей виртуальную сцену

vrf = vrfigure(w)

vrf = 

	vrfigure object: 1-by-1

	Differential Wheeled Robot with LIDAR Sensor

Инициализация положения и вращения робота в виртуальном мире

Виртуальная сцена представлена в виде иерархической структуры файла VRML, используемого Simulink 3D Animation. Положение и ориентация дочерних объектов находятся относительно родительского объекта. Робот vrnode используется для управления положением и ориентацией робота в виртуальной сцене.

Для доступа к узлу VRML, соответствующий vrnode необходимо создать объект. Узел идентифицируется по его названию и миру, которому он принадлежит.

Создать vrnode ручка для робота в виртуальной среде.

robotVRNode = vrnode(w,'Robot');

Задайте начальное положение робота от первой точки траектории и установите начальный поворот на 0 рад вокруг оси Y.

robotVRNode.children.translation = [trajectory(1,1) 0 trajectory(1,2)];
robotVRNode.children.rotation = [0 1 0 0];

Создайте ручку для лидарного датчика на роботе, создав vrnode.

lidarVRNode = vrnode(w,'LIDAR_Sensor');

Моделируемый лидар использует в общей сложности 240 лазерных линий, и угол между этими линиями составляет 1,5 градуса.

angles  = 180:-1.5:-178.5;
angles = deg2rad(angles)';

Ожидание обновления и инициализации виртуальной сцены

pause(1)

Создание объекта Lidar Slam

Создать lidarSLAM и задайте разрешение карты и максимальный диапазон лидаров. В этом примере используется смоделированная виртуальная среда. Робот в этом vrworld имеет лидарный датчик с диапазоном от 0 до 10 метров. Установите максимальный диапазон лидара (8 м) меньше максимального диапазона сканирования, так как показания лазера менее точны вблизи максимального диапазона. Установите разрешение карты сетки 20 ячеек на метр, что обеспечивает точность 5 см. Эти два параметра используются во всем примере.

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution,maxLidarRange);

Параметры замыкания контура задаются эмпирически. Использование более высокого порога замыкания цикла помогает отклонить ложные срабатывания в процессе идентификации замыкания цикла. Имейте в виду, что матч с высоким счетом все еще может быть плохим матчем. Например, сканирование, собранное в среде с подобными или повторяющимися функциями, с большей вероятностью приведет к ложному положительному результату. Использование более высокого радиуса поиска замыкания контура позволяет алгоритму искать более широкий диапазон карты вокруг текущей оценки позы для замыканий контура.

slamAlg.LoopClosureThreshold = 200;
slamAlg.LoopClosureSearchRadius = 3;
controlRate = rateControl(10);

Наблюдение за эффектом процесса замыкания контура и оптимизации

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

Замыкания петель обнаруживаются автоматически при перемещении робота. Оптимизация графика позы выполняется всякий раз, когда обнаруживается замыкание контура. Это можно проверить с помощью выходных данных. optimizationInfo.IsPerformed значение из addScan.

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

Окончательная построенная карта будет представлена после сбора и обработки всех сканирований.

График постоянно обновляется по мере перемещения робота по виртуальной сцене

firstLoopClosure = false;
scans = cell(length(trajectory),1);

figure
for i=1:length(trajectory)
    % Use translation property to move the robot. 
    robotVRNode.children.translation = [trajectory(i,1) 0 trajectory(i,2)];
    vrdrawnow;
    
    % Read the range readings obtained from lidar sensor of the robot.
    range = lidarVRNode.pickedRange;
    
    % The simulated lidar readings will give -1 values if the objects are
    % out of range. Make all these value to the greater than
    % maxLidarRange.
    range(range==-1) = maxLidarRange+2;

    % Create a lidarScan object from the ranges and angles. 
    scans{i} = lidarScan(range,angles);
    
    [isScanAccepted,loopClosureInfo,optimizationInfo] = addScan(slamAlg,scans{i});
    if isScanAccepted
        % Visualize how scans plot and poses are updated as robot navigates
        % through virtual scene
        show(slamAlg);
        
        % Visualize the first detected loop closure
        % firstLoopClosure flag is used to capture the first loop closure event
        if optimizationInfo.IsPerformed && ~firstLoopClosure
            firstLoopClosure = true;
            show(slamAlg,'Poses','off');
            hold on;
            show(slamAlg.PoseGraph);
            hold off;
            title('First loop closure');
            snapnow
        end
    end

    waitfor(controlRate);
end

Постройте график окончательной построенной карты после добавления всех сканирований к slamAlg объект.

show(slamAlg,'Poses','off');
hold on
show(slamAlg.PoseGraph); 
hold off
title({'Final Built Map of the Environment','Trajectory of the Robot'});

Построить сетку занятости

Оптимизированные сканирования и позы можно использовать для создания occupancyMap которая представляет среду как вероятностную сетку занятости.

[scans,optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans,optimizedPoses,mapResolution,maxLidarRange);

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

figure; 
show(map);
hold on
show(slamAlg.PoseGraph,'IDs','off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');

Закройте виртуальную сцену.

close(vrf);
close(w);
delete(w);