Предупреждение столкновения Используя 2D лидар
В этом примере показано, как обнаружить препятствия и предупредить о возможных столкновениях с помощью 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