Этот пример показывает, как обнаружить препятствия и предупредить о возможных столкновениях с помощью данных о 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);
Симулируйте 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)]);
Настройте окно рисунка, в котором отображается движение 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)
Область обнаружения разделена на три уровня: красный, оранжевый и желтый. Каждая область связана с определенной степенью опасности:
Красный - Столкновение неизбежно
Оранжевый - Высокая вероятность столкновения
Желтый - Применить меры предосторожности
Препятствия, которые не попадают в область значений обнаружения, находятся на безопасном расстоянии от AGV. Это основные шаги, связанные с предупреждением столкновения:
Симулируйте 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
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