Этот пример показывает, как обнаружить препятствия и предупредить о возможных столкновениях, используя 2-е данные о лидаре.
Логистические склады все чаще монтируют 2-D лидары на автоматических управляемых автомобилях (AGV) в навигационных целях из-за доступности, большой дальности и высокой разрешающей способности датчика. Датчики помогают обнаруживать столкновения, что является важной задачей для безопасной навигации AGV в сложных средах. Этот пример шоу, как представлять рабочее пространство робота, населенное с препятствиями, произведите 2-е данные о лидаре, обнаружьте препятствия и обеспечьте предупреждение перед нависшим столкновением.
Чтобы представить среду рабочего пространства робота, определите binaryOccupancyMap(Панель инструментов навигации), содержащий план этажа склада. Каждая ячейка в сетке занятости имеет логическое значение. Занятое место представлено как 1, а свободное - как 0. Вы можете использовать информацию о занятии, чтобы произвести синтетические 2-е данные о лидаре.
Добавьте препятствия на карте рядом с определенным маршрутом 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);
Моделируйте 2-й датчик лидара, используя a rangeSensor объект для сбора показаний лидара для создаваемой карты. Загрузите MAT-файл, содержащий предопределенные ППМ AGV, в рабочее пространство. Используйте моделируемый лидарный датчик для возврата показаний диапазона и угла для позы AGV, а затем используйте диапазоны и углы для генерации lidarScan объект, содержащий 2-D lidar 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 на складе, связанные с ним лидарные сканирования среды, отображаются препятствия в виде закрашенных окружностей на виде птичьего глаза и цветные предупреждения о столкновениях. Цвет, используемый для каждого предупреждения, означает вероятность столкновения на основе зоны зоны обнаружения, которую препятствие занимает в этом ППМ.
% 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)
Область обнаружения разделена на три уровня: красный, оранжевый и желтый. Каждый регион связан с определенной степенью опасности:
Красный - столкновение неизбежно
Оранжевый - высокая вероятность столкновения
Желтый - применение мер предосторожности
Препятствия, не попадающие в зону обнаружения, находятся на безопасном расстоянии от АРМ. Ниже перечислены основные шаги, связанные с предупреждением о столкновении:
Моделирование 2-D лидара и извлечение данных облака точек.
Сегментировать данные облака точек в кластеры препятствий.
Закольцовывайте каждое препятствие, чтобы проверить возможные столкновения.
Выдайте предупреждение, исходя из уровня опасности препятствий.
Отображение препятствий рядом с AGV.
% Move AGV through waypoints for ij = 27:size(traj, 1) currentPose = traj(ij, :);
Соберите показания лидара на карте с помощью смоделированного датчика. Загрузите текущую позу 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. Препятствия отображаются в виде закрашенных кругов на панели «График птичьего глаза» окна визуализации.
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
helperCreateBineyOccupancyMap создает карту склада рабочего пространства робота
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(Панель инструментов навигации) | lidarScan | rangeSensor | pcsegdist