Main Content

Collision Warning Using 2-D Lidar

This example shows how to detect obstacles and warn of possible collisions using 2-D lidar data.

Overview

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 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

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 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, 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)

Run Simulation

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,:);

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 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);

Segment Point Cloud Data into Obstacle Clusters

Use the 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,currentOrientation);
    % 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.

    % 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

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.

        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

Supporting Files

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

See also

binaryOccupancyMap (Navigation Toolbox) | lidarScan | rangeSensor | pcsegdist