Предупреждение столкновения Используя 2D лидар

Скрипт Open Live Script

В этом примере показано, как обнаружить препятствия и предупредить о возможных столкновениях с помощью 2D данных о лидаре.

Обзор

Склады логистики все больше монтируют 2D лидары на автоматических ведомых транспортных средствах (AGV) в целях навигации, из-за доступности, большого расстояния и высокого разрешения датчика. Датчики помогают в обнаружении столкновений, которое является важной задачей для безопасности плавания AGVs в сложных средах. В этом примере показано, как представлять рабочую область робота, заполненную препятствиями, сгенерируйте 2D данные о лидаре, обнаружьте препятствия и обеспечьте предупреждение перед предстоящим столкновением.

Создайте складскую карту

Чтобы представлять среду рабочей области робота, задайте binaryOccupancyMap Объект (Navigation Toolbox), который содержит план здания склада. Каждая ячейка в сетке заполнения имеет логическое значение. Занятое местоположение представлено как 1, и свободное местоположение представлено как 0. Можно использовать информацию о заполнении, чтобы сгенерировать синтетические 2D данные о лидаре.

Добавьте препятствия карте близко к заданному маршруту AGV.

% Create a binary warehouse map and place obstacles at defined locations
map = helperCreateBinaryOccupancyMap;

% Visualize map with obstacles and AGV
figure
show(map)
title('Warehouse Floor Plan With Obstacles and AGV')

% Add AGV to the map
pose = [5 40 0];
helperPlotRobot(gca, pose);

Симулируйте 2D датчик лидара

Симулируйте 2D датчик лидара с помощью rangeSensor объект собрать показания лидара для сгенерированной карты. Загрузите MAT-файл, содержащий предопределенный waypoints AGV в рабочую область. Используйте симулированный датчик лидара, чтобы возвратить область значений и угловые показания для положения AGV, и затем использовать области значений и углы, чтобы сгенерировать lidarScan объект, который содержит 2D лидар scan.

% Simulate lidar sensor and set the detection angles to [-pi/2 pi/2]
lidar = rangeSensor;
lidar.HorizontalAngle = [-pi/2 pi/2];
% Set min and max values of the detectable range of the sensor in meters
lidar.Range = [0 5];

% Load waypoints through which AGV moves
load waypoints.mat
traj = waypointsMap;

% Select a waypoint to visualize scan data
Vehiclepose = traj(350, :);

% Generate lidar readings
[ranges, angles] = lidar(Vehiclepose, map);

% Store and visualize 2-D lidar scan
scan = lidarScan(ranges, angles);
plot(scan)
title('Ego View')
helperPlotRobot(gca, [0 0 Vehiclepose(3)]);

Настройте визуализацию

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

% Set up display
display = helperVisualizer;

% Plot warehouse map in the display window
hRobot = plotBinaryMap(display, map, pose);

Столкновение, предупреждающее на основе зон

Предупреждения столкновения только появляются, если препятствие находится в пределах области обнаружения AGV.

Задайте область обнаружения

Создайте пользовательскую обнаруживаемую область с различными цветами, формами, и измените область цветных областей на графический интерфейсе пользователя фигуры. Запустите ниже фрагмента кода и измените указатели многоугольника, чтобы вместить ваши требования области обнаружения. Код ниже создает 3 указателя многоугольника полукруглых областей с радиусом 5, 2, 1 в метрах, и AGV расположен в [0 0]. Измените радиус или измените объекты многоугольника создать пользовательскую область обнаружения.

figure
detAxes = gca;
title(detAxes,'Define Detection Area')
axis(detAxes, [-2 10 -2 4])
xlabel(detAxes, 'X')
ylabel(detAxes, 'Y')
axis(detAxes, 'equal')
grid(detAxes, 'minor')
t = linspace(-pi/2, pi/2, 30)';
% Specify color values - white, yellow, orange, red
colors = [1 1 1; 1 1 0; 1 0.5 0; 1 0 0];

% Specify radius in meters
radius = [5 2 1];

% Create a 3x1 matrix of type Polygon
detAreaHandles = repmat(images.roi.Polygon, [3 1]);

pos = [cos(t) sin(t)] * radius(1);
pos = [0 -2; pos(14:17, :); 0 2];
detAreaHandles(1) = drawpolygon(...
	'Parent', detAxes, ...
	'InteractionsAllowed', 'reshape', ...
	'Position', pos, ...
	'StripeColor', 'black', ...
	'Color', colors(2, :));

pos = [cos(t) sin(t)] * radius(2);
pos = [0 -1.5; pos(12:19, :); 0 1.5];
detAreaHandles(2) = drawpolygon(...
	'Parent', detAxes, ...
	'InteractionsAllowed', 'reshape', ...
	'Position', pos, ...
	'StripeColor', 'black', ...
	'Color', colors(3, :));

pos = [cos(t) sin(t)] * radius(3);
pos = [0 -1; pos(10:21, :); 0 1];
detAreaHandles(3) = drawpolygon(...
	'Parent', detAxes, ...
	'InteractionsAllowed', 'reshape', ...
	'Position', pos, ...
	'StripeColor', 'black', ...
	'Color', colors(4, :));

pause(2) % Pausing for the detection area window to load

Чтобы сохранить созданную область обнаружения, запустите helperSaveDetectionArea функция. Используйте указатель осей фигуры с многоугольниками области обнаружения и связанным detAreaHandles переменная как входные параметры. Функциональные выходные параметры область обнаружения, как матрица типа данных uint8, и ограничительная рамка. Синий прямоугольник вокруг области обнаружения представляет ограничительную рамку.

axesDet = gca; % Axes of the figure window containing the polygon handles
[detArea,bbox] = helperSaveDetectionArea(axesDet, detAreaHandles);

% Make detection area transparent by scaling colors
%alphadata = double(detArea ~= 0) * 0.5;
ax3 = getDetectionAreaAxes(display);
h = imagesc(ax3, [bbox(1) (bbox(1) + bbox(3))], ...
    -[bbox(2) (bbox(2) + bbox(4))], ...
    detArea);

colormap(ax3, colors);

% Plot detection area
plotObstacleDisplay(display)

Запустите симуляцию

Область обнаружения разделена на три уровня как: красный, оранжевый, и желтый. Каждая область сопоставлена с определенной степенью опасности:

  • Красный — Столкновение неизбежно

  • Оранжевый — Высокий шанс столкновения

  • Желтый — Применяют меры по осторожности

Препятствия, которые не находятся в пределах области значений обнаружения, на безопасном расстоянии от AGV. Это первичные шаги, вовлеченные в столкновение, предупреждающее:

  • Симулируйте 2D лидар и извлеките данные об облаке точек.

  • Данные об облаке точек сегмента в кластеры препятствия.

  • Цикл по каждому препятствию, чтобы проверять на возможные столкновения.

  • Выдайте предупреждение на основе опасного уровня препятствий.

  • Отобразите препятствия близко к AGV.

% Move AGV through waypoints
for ij = 27:size(traj, 1)
    currentPose = traj(ij, :);

Симулируйте 2D лидар и извлеките данные об облаке точек

Соберите показания лидара карты с помощью симулированного датчика. Загрузите текущее положение AGV из waypoints файла. Используйте rangeSensor возразите, что вы создали, чтобы получить угловое измерение и область значений.

    % Retrieve lidar scans
    [ranges, angles] = lidar(currentPose, map);
    scan = lidarScan(ranges, angles);
    
    % Store 2-D scan as point cloud
    cart = scan.Cartesian;
    cart(:, 3) = 0;
    pc = pointCloud(cart);

Данные об облаке точек сегмента в кластеры препятствия

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

    % Segment point cloud into clusters based on euclidean distance
    minDistance = 0.9;
    [labels, numClusters] = pcsegdist(pc, minDistance);

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

    % Update display map
    updateMapDisplay(display, hRobot, currentPose);
    
    % Plot 2-D lidar scans
    plotLidarScan(display, scan, currentPose(3));
    
    % Delete obstacles from last scan to plot next scan line
    if exist('sc', 'var')
        delete(sc)
        clear sc
    end

Цикл по каждому препятствию, чтобы найти вероятность столкновений

Цикл через кластеры на основе их меток, чтобы извлечь точки, расположенные в них.

    nearxy = zeros(numClusters, 2);
    maxlevel = -inf;
    % Loop through all the clusters in pc
    for i = 1:numClusters
        c = find(labels == i);
        % XY coordinate extraction of obstacle
        xy = pc.Location(c, 1:2);

Преобразуйте мировое положение каждого препятствия в систему координат камеры.

        % Convert to normalized coordinate system (0-> minimum location of detection
        % area, 1->maximum position of detection area)
        a = [xy(:, 1) xy(:, 2)] - repmat(bbox([1 2]), [size(xy, 1) 1]);
        b = repmat(bbox([3 4]), [size(xy, 1) 1]);
        xy_org = a./b;
        % Coordinate system (0, 0)->(0, 0), (1, 1)->(width, height) of detArea
        idx = floor(xy_org.*repmat([size(detArea, 2) size(detArea, 1)],[size(xy_org, 1), 1]));

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

        % Extracts as an index only the coordinates in detArea
        validIdx = 1 <= idx(:, 1) & 1 <= idx(:, 2) & ...
            idx(:, 1) <= size(detArea, 2) & idx(:, 2) <= size(detArea, 1);

Для каждой допустимой точки препятствия найдите присваиваемое значение в матрице обнаружения. Максимальное значение всех связанных точек в матрице обнаружения определяет уровень опасности, представленной тем препятствием. Отобразите цветной круг на основе опасного уровня препятствия в панели цвета предупреждения окна визуализации.

        % Round the index and get the level of each obstacle from detArea
        cols = idx(validIdx, 1);
        rows = idx(validIdx, 2);
        levels = double(detArea(sub2ind(size(detArea), rows, cols)));

        if ~isempty(levels)
            level = max(levels);
            maxlevel = max(maxlevel, level);
            xyInds = find(validIdx);
            xyInds = xyInds(levels == level);
            % Get the nearest coordinates of obstacle in detection area
            nearxy(i, :) = helperNearObstacles(xy(xyInds, :));
        else
            % Get the nearest coordinates of obstacle in the cluster
            nearxy(i, :) = helperNearObstacles(xy);
        end
    end
    % Display a warning color representing the danger level. If the
    % obstacle does not fall in the detection area, do not display a color.
    switch maxlevel
        % Red region
        case 3
            circleDisplay(display, colors(4, :))
        % Orange region
        case 2
            circleDisplay(display, colors(3, :))
        % Yellow region
        case 1
            circleDisplay(display, colors(2, :))
        % Default case
        otherwise
            circleDisplay(display, [])
    end

Отобразите точки препятствий, самых близких к AGV

Когда большинство препятствий на складе линейно и долго, отображает только точку каждого кластера препятствия, самого близкого к AGV. Препятствия отображаются как заполненные круги в Видимой с большого расстояния панели Графика окна визуализации.

    for i = 1:numClusters
        % Display obstacles if exist in the mentioned range of axes3
        sc(i, :) = displayObstacles(display, nearxy(i, :));
    end
    updateDisplay(display)
    pause(0.01)
end

Вспомогательные файлы

helperCreateBinaryOccupancyMap создает складскую карту рабочей области робота

function map = helperCreateBinaryOccupancyMap()
% helperCreateBinaryOccupancyMap Creates a warehouse map with specific
% resolution passed as arguments to binaryOccupancyMap

map = binaryOccupancyMap(100, 80, 1);
occ = zeros(80, 100);
occ(1, :) = 1;
occ(end, :) = 1;
occ([1:30, 51:80], 1) = 1;
occ([1:30, 51:80], end) = 1;
occ(40, 20:80) = 1;
occ(28:52, [20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
occ(1:12, [20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
occ(end-12:end, [20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
% Set occupancy value of locations
setOccupancy(map, occ);

% Add obstacles to the map at specific locations. Inputs to
% helperAddObstacle are obstacleWidth, obstacleHeight and obstacleLocation.
helperAddObstacle(map, 5, 5, [10, 30]);
helperAddObstacle(map, 5, 5, [20, 17]);
helperAddObstacle(map, 5, 5, [40, 17]);
end

%helperAddObstacle Adds obstacles to the occupancy map
function helperAddObstacle(map, obstacleWidth, obstacleHeight, obstacleLocation)
values = ones(obstacleHeight, obstacleWidth);
setOccupancy(map, obstacleLocation, values)
end

Смотрите также

binaryOccupancyMap (Navigation Toolbox) | lidarScan | rangeSensor | pcsegdist

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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте