Detect Low Obstacles using Tilted 2D Lidar
By: Marshal Shi
Tags: Lidar, Python, Robotics, Ros, Programming

Algorithm

1. Keep the useful lidar data.

Since we are pointing lidar down to the ground, the useful data for us is the intersection parts between the lidar plane and the ground plane. We will define a HORIZONTAL_ANGLE_RANGE value to indicate the useful data range.

2. Convert lidar result in the movebase coordinate system

Lidar only returns the value measurements and the included angle between each measurement. But when we try to recognize a lidar point to be an obstacle, we will use the “height” of the object to determine whether it’s an obstacle. And this “height” is related to the movebase. SO, we need to convert the points data from the lidar coordinate system to the movebase coordinate system.

3. Line detection

It’s hard to tell whether points are obstacles since the lidar may raise some noise occasionally. We will convert points into segmented lines and then use them later to determine which parts are obstacles.

Since at step2 we have converted points data from 2D lidar coordinate system to 3D movebase coordinate system, points have their “height” on the z-axis. A threshold Z_DELTA (δ) around the first point of the line will determine whether a point belongs to the line if the distance on the z-axis from point to the first point is smaller than Z_DELTA (δ) . Do this because it has more chances to be an obstacle if the point has a much higher height.

Besides, we will define a max distance Y_MAX_DIST (d) between the first point to point. If the distance between the first point to point is larger than it, the point will belong to another line. Of course, it’s possible that a small group of points form a line, but the line is too small which won’t be an obstacle or we can ignore that size of the obstacle. We define MIN_LINE_PTS_NUM to control this min points number in a line.

Image from Detection and Classification of Obstacles Using a 2D LiDAR Sensor

4. Refine lines

Because of the Y_MAX_DIST (d) , all points are assigned into a line segmentation. It’s obvious that some of the lines can be merged into a long line since most of them are the ground when movebase is running. We will calculate the slope of the line between the first point and the last point in the line group. If two adjacency lines are similar, the difference of the slopes is smaller than REFINE_DELTA , then we will merge both as a long line.

Now we have segmented our points into different “classes”.

5. Classification

Based on the segmented points in the former 4 steps, we will use the mean height value of each group as group height, and use this group height to compare our threshold Z_OBS_EPS (obstacle epsilon threshold height on the z-axis). If the group height is larger than it, the group points are an obstacle and we will publish the PointCloud to costmap.

Sorry that I cannot show code here because of the legal reason. Please implement it by your-self, it’s simple.

When I tested the code on Gazebo, it shows very positive results. I put 4 cans on the ground at different distances to the robot. The lidar points heights on the cans are around 3cm ~8cm. All 4 cans are successfully recognized as obstacles which show on RVIZ.

Images by author. Cans in gazebo with lidar lines