This example shows how to detect obstacles and warn of possible collisions using 2-D lidar data.
Logistics warehouses are increasingly mounting 2-D lidars on automatic guided vehicles (AGV) for navigation purposes, due to the affordability, long range, and high resolution of the sensor. The sensors assist in collision detection, which is an important task for the safe navigation of AGVs in complex environments. This example shows how to represent a robot workspace populated with obstacles, generate 2-D lidar data, detect obstacles, and provide a warning before an impending collision.
To represent the environment of the robot workspace, define a
binaryOccupancyMap object that contains the floor plan of the warehouse. Each cell in the occupancy grid has a logical value. An occupied location is represented as 1 and a free location is represented as 0. You can use the occupancy information to generate synthetic 2-D lidar data.
Add obstacles to the map near to the defined route saved in the
waypoints.mat file that the AGV traverses.
% Create a binary warehouse map and place obstacles at defined locations map = helperCreateBinaryOccupancyMap; % Visualize map with obstacles figure show(map) title('Warehouse Floor Plan With Obstacles') % Add AGV to the map pose = [5 40 0]; helperPlotRobot(gca,pose);
Simulate a 2-D lidar sensor by using a
rangeSensor System object™ to gather lidar readings for the generated map. Load a MAT-file containing the predefined waypoints of the AGV into the workspace. Use the simulated
lidar sensor to return range and angle readings for a pose of the AGV, and then use the ranges and angles to generate a
lidarScan object that contains the 2-D lidar scan
% Simulate lidar sensor. Use default detection angle of [-pi pi] lidar = rangeSensor; % 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 annd visualize 2-D lidar scan scan = lidarScan(ranges,angles); plot(scan) title('Ego View') helperPlotRobot(gca,[0 0 Vehiclepose(3)]);
Set up a figure window that displays AGV movement in the warehouse, the associated lidar scans of the environment, displays obstacles as filled circles in bird's eye view, and color-coded collision warning messages. The color used for each warning signifies the likelihood of a collision based on the detection area zone that the obstacle occupies at that waypoint.
% Set up display display = helperVisualizer; % Plot warehouse map in the display window hRobot = plotBinaryMap(display,map,pose);
Collision warnings only appear if an obstacle falls within the detection area of the AGV.
Create a custom detectable area with different colors, and shapes, and modify the area of color regions on figure GUI. Run the below portion of code and modify the polygon handles to accommodate your requirements of the detection area. The code below creates 3 polygon handles of semi-circular regions with a radius of 1.5, 1, 0.5 in meters and AGV is positioned at [0 0]. Modify the radius or change the polygon objects to create a custom detection area.
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,11)'; % Specify colors colors = ["yellow" "red" "black"]; % Specify radius radius = [1.5 1 0.5]; % Create a 3x1 matrix of type Polygon detAreaHandles = repmat(images.roi.Polygon,[3 1]); % Create a polygon with a tunnel shape pos = [cos(t) sin(t)]*radius(1); posLine = [6.5 -0.5; 6.5 0.5]; pos = [pos(1:4,:); posLine; pos(end-3:end,:)]; detAreaHandles(1) = drawpolygon( ... 'Parent',detAxes, ... 'InteractionsAllowed','reshape', ... 'Position',pos, ... 'StripeColor','black', ... 'Color',colors(1)); % Create semicircle polygons for k = 2:numel(colors) pos = [cos(t) sin(t)]*radius(k); detAreaHandles(k) = drawpolygon( ... 'Parent',detAxes, ... 'InteractionsAllowed','reshape', ... 'Position',pos, ... 'StripeColor','black', ... 'Color',colors(k)); end pause(2) % Pausing for the detection area window to load
To save the created detection area, run the
helperSaveDetectionArea function. Use the axes handle of the figure with the detection area polygons and the associated
detAreaHandles variable as input arguments. The function outputs the detection area, as a matrix of datatype
uint8, and a bounding box. The violet rectangle around the detection area represents the bounding box.
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,'AlphaData',alphadata); colormap(ax3,[1 1 1; 1 1 0; 1 0 0; 0 0 0]); % Plot detection area plotObstacleDisplay(display)
The detection area is divided into three levels as: black, red, and yellow. Each region is associated with a specific degree of danger:
Black — Collision is imminent
Red — High chance of collision
Yellow — Apply caution measures
All obstacles that do not fall within the detection range as they are far from AGV. These are the primary steps involved in collision warning:
Simulate 2-D lidar and extract point cloud data
Segment point cloud data into obstacle clusters
Loop over each obstacle to check for possible collisions
Issue a warning based on the danger level of each obstacle
Display obstacles close to the AGV
% Move AGV through waypoints for ij = 27:size(traj,1) currentPose = traj(ij,:);
Gather lidar readings of the map using the simulated sensor. Load the current pose of the AGV from the waypoints file. Use the
rangeSensor System object you created to get range and angle measurement.
currentPosition = currentPose(1:2); currentOrientation = currentPose(3); robotCurrentPose = [currentPosition(1:2) currentOrientation]; % Retrieve lidar scans [ranges,angles] = lidar(robotCurrentPose,map); % Store 2-D scan as point cloud scan = lidarScan(ranges,angles); cart = scan.Cartesian; cart(:,3) = 0; pc = pointCloud(cart);
pcsegdist function to segment the scanned point cloud into clusters, using minimum Euclidean distance between the points as the segmentation criterion.
% 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,currentOrientation); % Delete obstacles from last scan to plot next scan line if exist('sc','var') delete(sc) clear sc end
Loop through the clusters based on their labels, to extract the points located inside them.
% 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 the world position of each obstacle into the camera coordinate system.
% 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]));
Extract the indices of only the obstacle points that lie in the detection area.
% 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);
For each valid obstacle point, find the associated value in the detection matrix. The maximum value of all associated points in the detection matrix determines the level of danger represented by that obstacle. Display a colored circle based on the danger level of the obstacle in the Warning Color pane of the visualization window.
% Rounding the index and getting the level of each obstacle from detArea cols = idx(validIdx,1); rows = idx(validIdx,2); levels = double(detArea(sub2ind(size(detArea),rows,cols))); % Display a warning color representing the danger level. If the % obstacle does not fall in the detection area, do not display a color. circleDisplay(display,'white') if~isempty(levels) [level,index] = max(levels); % Get the coordinates of obstacle that is in detection area rs = rows(index); cs = cols(index); nearxy = xy(idx(:,2) == rs & idx(:,1) == cs,:); switch level % Black region case 3 circleDisplay(display,'black') % Red region case 2 circleDisplay(display,'red') % Yellow region case 1 circleDisplay(display,'yellow') % Default case otherwise circleDisplay(display,'white') end end
As most of the obstacles in the warehouse are linear and long, display only the point of each obstacle cluster closest to the AGV. Obstacles display as filled circles in the Bird's-Eye Plot pane of the visualization window.
if(isempty(levels)) % Get nearest data item of each cluster nearxy = helperNearObstacles(xy); end % Display obstacles if exist in the mentioned range of axes3 sc(i,:) = displayObstacles(display,nearxy); end updateDisplay(display) pause(0.01) end
helperCreateBinaryOccupancyMap creates a warehouse map of the robot workspace
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