В этом примере показано, как обнаружить препятствия и предупредить о возможных столкновениях с помощью 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 датчик лидара с помощью 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, :);
Соберите показания лидара карты с помощью симулированного датчика. Загрузите текущее положение 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. Препятствия отображаются как заполненные круги в Видимой с большого расстояния панели Графика окна визуализации.
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