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

Траектория робота является 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)

Создайте объект Lidar SLAM

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

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

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

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