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

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

Траектория робота является waypoints, данным роботу, чтобы переместиться в моделируемую среду. В данном примере траектория робота предоставляется вам.

load slamRobotTrajectory.mat

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

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

Этот пример использует виртуальную сцену с двумя автомобилями и четырьмя стенами как препятствия и робот, оборудованный сканером лазерного дальномера, показанным в Средстве просмотра Simulink 3D Animation. Можно перейти в виртуальной сцене с помощью панели меню, панели инструментов, панели навигации, мыши и клавиатуры. Ключевые возможности средства просмотра проиллюстрированы в VR example.

Создайте и откройте объект 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)

Создайте объект хлопка лазерного дальномера

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

maxLidarRange = 8;
mapResolution = 20;
slamAlg = robotics.LidarSLAM(mapResolution, maxLidarRange);

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

slamAlg.LoopClosureThreshold = 200;
slamAlg.LoopClosureSearchRadius = 3;
controlRate = robotics.Rate(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'});

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

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

[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);