Предупреждение о столкновении с 2-D лидар

Этот пример использует:

Открыть Live Script

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

Обзор

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

Создание карты склада

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

Добавить препятствия на карту рядом с определенным маршрутом 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);

Figure contains an axes. The axes with title Warehouse Floor Plan With Obstacles and AGV contains 2 objects of type image, patch.

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

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

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

Figure contains an axes. The axes with title Ego View contains 2 objects of type line, patch.

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

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

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

Figure contains an axes. The axes with title Define Detection Area contains 4 objects of type images.roi.polygon, image.

% 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. Это основные шаги, связанные с предупреждением столкновения:

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

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

  • Цикл над каждым препятствием для проверки на возможные столкновения.

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

  • Отображение препятствий, близких к AGV.

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

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

Соберите лидарные показания карты с помощью моделируемого датчика. Загрузите текущее положение AGV из файла путевых точек. Используйте 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

Figure Collision warning display contains 4 axes and other objects of type uipanel. Axes 1 with title Warehouse Floor Plan contains 4 objects of type image, patch, line. Axes 2 with title 2-D lidar scan contains 2 objects of type line, patch. Axes 3 with title Display obstacles contains 3 objects of type image, scatter. These objects represent AGV, Obstacle. Axes 4 contains an object of type scatter.

Figure Collision warning display contains 4 axes and other objects of type uipanel. Axes 1 with title Warehouse Floor Plan contains 686 objects of type image, patch, line. Axes 2 with title 2-D lidar scan contains 2 objects of type line, patch. Axes 3 with title Display obstacles contains 3 objects of type image, scatter. These objects represent AGV, Obstacle. Axes 4 contains an object of type scatter.

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

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

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

Поддержка

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