Collision Warning Using 2-D Lidar
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.
Create a Warehouse Map
To represent the environment of the robot workspace, define a
binaryOccupancyMap (Navigation Toolbox) 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 of 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);
Simulate 2-D Lidar Sensor
Simulate 2-D lidar sensor using a
rangeSensor 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 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)]);
Set Up Visualization
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 Warning Based on Zones
Collision warnings only appear if an obstacle falls within the detection area of the AGV.
Define the Detection Area
Create a custom detectable area with different colors, 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 5, 2, 1 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, 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
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 blue 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); colormap(ax3, colors); % Plot detection area plotObstacleDisplay(display)
The detection area is divided into three levels as: red, orange, and yellow. Each region is associated with a specific degree of danger:
Red — Collision is imminent
Orange — High chance of collision
Yellow — Apply caution measures
Obstacles that do not fall within the detection range are at safe distance 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 obstacles.
Display obstacles close to the AGV.
% Move AGV through waypoints for ij = 27:size(traj, 1) currentPose = traj(ij, :);
Simulate 2-D Lidar and Extract Point Cloud Data
Gather lidar readings of the map using the simulated sensor. Load the current pose of the AGV from the waypoints file. Use the
rangeSensor object you created to get range and angle measurement.
% 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);
Segment Point Cloud Data into Obstacle Clusters
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 Visualization Window with Map and Scan Data
% 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
Loop Over Each Obstacle to Find the Likelihood of Collisions
Loop through the clusters based on their labels, to extract the points located inside them.
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 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 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.
% 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
Display Points of Obstacles Closest to the AGV
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.
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 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