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

Траектория робота является путевыми точками, заданными роботу для перемещения в моделируемом окружении. В данном примере для вас предусмотрена траектория робота.

load slamRobotTrajectory.mat

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

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

Этот пример использует виртуальную сцену с двумя транспортными средствами и четырьмя стенками в качестве препятствий и робота, оснащенного сканером лидара, показанным в Simulink 3D Animation Viewer. Перемещаться в виртуальной сцене можно с помощью панели меню, панели инструментов, панели навигации, мыши и клавиатуры. Ключевые возможности средства просмотра проиллюстрированы в примере Манипуляция Using Space Mouse 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 метров. Установите максимальную область значений лидара (8m) меньше, чем максимальный скан области значений, так как показания лазера менее точны почти максимальная область значений. Установите разрешение сетки в 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);