Towards an obstacle detection system for robot obstacle negotiation

Mobile robots equipped for obstacle negotiation are commonly used in applications such as rescue and disaster relief (Wang et al., 2023; Bai et al., 2023a, 2023b), scientific exploration (Zhu et al., 2021) and military operations due to their robust environmental adaptability (Bai et al., 2023a, 2023b). Obstacle negotiation is the premise for robots to overcome obstacles (Zhu et al., 2021), and the methods for mobile robots to achieve autonomous obstacle negotiation can be divided into two categories: environment-based motion planning obstacle negotiation method (Li et al., 2016) and information-based online control obstacle negotiation method (Vincent and Sun, 2012; Sokolov et al., 2017). Either method is inseparable from the perception of the surroundings (Xie et al., 2018; Gao et al., 2021a, 2021b), and the environment-based motion planning obstacle negotiation method has higher requirements for obstacle recognition (Gao et al., 2022, 2021a, 2021b). The result of obstacle detection largely determines the success or failure of autonomous obstacle negotiation.

Therefore, obstacle detection is crucial for autonomous obstacle negotiation.

Owing to the difference in the method of implementation, obstacle detection can be divided into sensor-based and map-based methods. Among the sensor-based methods, the vision-based method (Shi et al., 2022; Mancini et al., 2018) was developed earlier, but this method has a limited ability to detect the specific size of obstacles. Naujoks et al. (Naujoks and Wuensche, 2018) calculated the three-dimensional direction correction frame of an obstacle using point cloud data of a three-dimensional lidar, which can obtain the spatial pose and size of obstacles. Based on the geometric characteristics of obstacles, Larson and Trivedi (2011) and Shang et al. (2014) achieved the identification of negative obstacles through the relationship between the data of 3D lidar scan lines. The sensor-based methods perform obstacle detection based on the sensor’s limited frame data, which is likely to cause problems such as incomplete recognition results. These drawbacks can be avoided by map-based methods, of which elevation maps (Florin et al., 2007; Drulea et al., 2016) are used more. Dornhege and Kleiner (2007) established a behavior map for obstacle crossing based on the elevation map, which stores a set of skill descriptions corresponding to the robot in the grid according to the elevation information. Wermelinger et al. (2016) built a traversability map for quadruped robot navigation and obstacle crossing based on the elevation map, in which the traversability is determined by the weighting of factors such as slope, roughness and steps of terrain. In the existing methods, there are few methods that can simultaneously detect positive obstacles and negative obstacles and can calculate specific information such as sizes.

We propose an obstacle detection system based on elevation maps that covers the entire process, from environmental mapping to obstacle recognition and condition analysis of obstacle crossing. The system can detect obstacle sizes with high efficiency and accuracy in a range of scenarios. The research mainly focuses on three types of obstacles: positive obstacles, negative obstacles and trench obstacles. Positive obstacles refer to obstacles above the ground where the robot is currently located. Negative obstacles refer to areas below the ground, such as obvious sinking terrain. Trench obstacles refer to terrain with a certain span and depth, and the front and rear edges are approximately parallel. The system will mainly solve the problem of detecting the above three types of obstacles. For positive obstacles, an algorithm based on the smallest rectangular bounding box is proposed. For negative obstacles and trench obstacles, the algorithm is based on the information absence in the elevation map to initially locate them, and then through further detection to determine their final type. Generally speaking, this work provides the robot with the ability to determine whether it has the conditions to overcome obstacles, thereby supporting the robot to perform high-level command tasks such as obstacle navigation in complex environments. It has high application value in the field of robotics.

This paper is organized as follows. After giving a brief review of the related work in Section 2, we will introduce the principles and technical details of the system in Section 3. Section 4 then presents related evaluation experiments. Finally, conclusions and prospects will be drawn in Section 5.

Compared with two-dimensional map, 2.5D elevation map can express richer ground elevation information. At the same time, the calculation and storage are more efficient than 3D map. The early elevation mapping was mainly realized by sparse two-dimensional lidar data (Ye and Borenstein, 2003; Belter and Skrzypczyński, 2011), while using a filter to fill the elevation map to deal with the problem of invalid and missing original data (Ye and Borenstein, 2004). Recently, approaches to elevation mapping in rough terrain have tended to focus either on probabilistic estimating (Belter et al., 2012; Fankhauser et al., 2018) and considering the measurement noise of the sensor and the uncertainty of robot localization (Fankhauser et al., 2014), or on efficient update of elevation map (Kleiner and Dornhege, 2007). These approaches provide a strong guarantee for the map-based obstacle detection methods.

To calculate the specific location and size of the obstacle, the ground must be segmented. A grid-based approach was introduced, which divides the grid cells into ground and nonground according to the maximum absolute difference between the height of points in the grid. Similarly, (Chu et al., 2017) provides a fast and effective method for ground segmentation. It determines the threshold point depending on three features: gradient, lost threshold points and abnormalities in the distance between the sensor and a particular threshold point. Then, it estimates ground points and nonground points based on threshold points. On the other hand, Moosmann et al. (2009) create an undirected graph and compare local changes in the plane normal to characterize the change in slope.

Obstacle clustering: The approaches used for obstacle clustering are k-medoid algorithm (Hou, 1999), AK-means clustering algorithm (Xu et al., 2012), density-based approach (Zaiane and Lee, 2002) and elevation-reference connected component labeling algorithm (Tian et al., 2020), etc. The idea of obstacle clustering we adopted is very close to the Euclidean clustering algorithm. Euclidean clustering algorithm is a method for neighborhood clustering of points in three-dimensional space.

In the field of obstacle recognition, there are common positive obstacle recognition and negative obstacle recognition. Ortigosa et al. (2011) recognizes positive obstacles by detecting whether the column direction depth of the image column matches the linear model. Labayrade et al. (2002) propose the concept of V parallax to identify positive obstacles. In the V parallax space, the ideal road plane and vertical obstacles are shown as an inclined straight line and vertical line, respectively. Hu and Uchimura (2005) further introduce the concept of U-V parallax to adapt to a more general environment. For negative obstacles, methods based on synthetic aperture radar interference (InSAR) and based on multisensor fusion are more common (Wang et al., 2016; Yuan et al., 2015; Forouher et al., 2016). Our obstacle detection system takes multiple scenarios and multiple obstacle types into account, and uses methods based on the minimum rectangle bounding box and based on the characteristics of data absence to identify three types of obstacles.

The detection algorithm proposed in this paper, as shown in Figure 1, conducts obstacle recognition research on the basis of elevation maps, including ground segmentation algorithms, obstacle clustering algorithms and specific recognition algorithms for positive obstacles, negative obstacles and trench obstacles.

This paper will take the robot’s posture and sensor’s point cloud data as input, and develop an elevation map algorithm based on Grid Map Library (Fankhauser and Hutter, 2016). In this paper, the setting of the coordinate system for mapping is similar to (Fankhauser et al., 2018), and a local elevation map that follows the movement of the robot is established. This method can avoid the discontinuity of the mapping as much as possible, as shown in Figure 2. In the algorithm, the associated coordinate system is related to the world coordinate system, and the relationship is shown $xyz$ in (1):

where, x, y, z and ψ, θ, φ are, respectively, the translation and rotation angles along each axis among the coordinate systems. R represents the rotation transformation matrix, r represents the translation transformation and I is the unit matrix. The elevation map $Mele$ constructed in this paper can be expressed by (2):

where $ci$ is the grid point in the elevation map, whose quantity is m. $xi$ and $yi$ is the position of the grid point $ci$ in the world coordinate system, corresponding to the index i in the map, $hi$ is the elevation value at this grid point and $σi2$ is the variance of the elevation value.

The general idea of our algorithm is the same as that in Fankhauser et al. (2018), which will consider the measurement noise of the sensor and the localization uncertainty of the robot, and the mapping process is divided into two stages: measurement update and movement update. Different from the literature Fankhauser et al. (2018), we have improved the variance calculation formula of measurement update and movement update formula, corresponding to (4) and (5), respectively:

• Measurement update. The relationship between the sensor scan point $PSi$ converted to the elevation value $pi$ in the map and the variance formula of the elevation value are shown in (3) and (4), respectively, where p = [0 0 1] is a mapping vector that extracted the z value of the point cloud as the elevation value at that point. $ΣS$ is the covariance matrix measured by the sensor. $ΣR$ and $Σr$ are the position posture covariance matrix and position covariance matrix, respectively. The solution method of the Jacobian matrix and the update process based on the one-dimensional Kalman filter are the same as in Fankhauser et al. (2018).

  $pi=P(RSMrSSPi+rMMS)$

  (3)
  $σpi2=JSΣSJST+JRΣRJRT+JrΣrJrT$

  (4)

• Movement update. To reduce the amount of calculation and simplify the update process, this paper linearly combines the uncertainty of positioning and the corresponding amount of motion, and only updates the variance of the elevation value at a certain frequency. The elevation value remains unchanged during this process, updated equation is as follows:

  ${hk+1=hkσk+12=σk2+|xk+1−xk|Σr,k+1(1,1)+|yk+1−yk|Σr,k+1(2,2)$

  (5)

where, k and k + 1 represent two adjacent update moments, $|xk+1−xk|$ and $|yk+1−yk|$ respectively, represent the displacement of the robot in the corresponding direction in the world coordinate system.

Ground segmentation is a necessary link before obstacle recognition, used to eliminate the interference of ground information on obstacle detection. In this paper, the ground segmentation algorithm based on plane fitting proposed in Zermas et al. (2017) is used to separate the ground and nonground parts in the elevation map.

Because the operating environment of the robot in this paper is three-dimensional, the ground part is not always on the map. So, we made two improvements to the algorithm: the current height of the robot is used as the reference point for the selection of seed points, so that the ground will change with the movement of the robot, which can ensure that the obstacles above the ground are positive obstacles relative to the robot and those below the ground height are negative obstacles. After segmenting the original ground, for areas where obstacles exist, the theoretical ground height will be calculated based on the final ground model.

We converted the Euclide-clustering algorithm from three-dimensional to two-dimensional for this paper.

We realize the recognition of positive obstacles by calculating the three-dimensional bounding box of them. The whole process can be divided into three steps: convex hull, minimum area rectangle and bounding box:

1. Convex hull calculation. In this paper, the Graham scanning method (Hu and Uchimura, 2005) is used to calculate the convex hull according to the grid coordinates of the elevation map occupied by positive obstacles.

2. Minimum area rectangle calculation. The minimum area rectangle refers to the smallest circumscribed rectangle formed by convex hull points, which is closest to the actual shape of the obstacle. The general idea of the calculation of the minimum area rectangle is: connecting two adjacent points in the convex hull to form a straight line $l1$ , using this line as the bottom edge, searching for the parallel line $l2$ that passes other convex hull points and is farthest from $l1$ and two perpendicular lines $l3$ and $l4$ farthest apart from each other. The four straight lines intersect to form a rectangle. Traverse the con-vex hull and connect two adjacent points in sequence to obtain different bottom edges, and then obtain a rectangle corresponding to it, in which the smallest area is the desired one. After the bottom edge is determined, how to efficiently find the other three edges is the key to the calculation of this part. We use the corner judgment method (Arnon and Gieselmann, 1983) to solve this problem. It can complete the traversal calculation in linear time.

The calculation method of the rectangular area in this paper is different from that in Graham (1972). As shown in Figure 3, the key to the area calculation is to solve the points p, q and m. These three points can be obtained by the nature of vector dot product. Taking point p as an example, the formula is as follows:

According to the meaning of vector dot product, (6) can be written as:

The formula for solving the position of the center point of the rectangle is as follows:

Taking the straight line $l1$ passing through points $pi$ and $pi+1$ as the bottom, the dotted lines intersect to form a rectangle, whose area is the product of the length and width. The length is the length of pq, and the width is the length of $pkm$ , where p, q and m are the projection points of $pj$ , $pm$ and $pk$ on line $l1$ . (Source: Authors’ own work)

• 3

  Bounding box generation

Take the minimum area rectangle as the bottom surface and the maximum height of the obstacle relative to the ground as high, a bounding box can be generated.

• Obstacle discovery. The purpose of this step is to determine whether there are potential negative obstacles or trench obstacles, and to calculate the location of the potential obstacle edge. In this paper, multiple detection units are designated in the map space to detect negative obstacles and trench obstacles. A detection unit contains a detection base point and a detection area associated with the base point.

The following describes the detection process by taking the detection unit closest to the robot shown in Figure 4 as an example. Since the sensor blind area also shows no elevation value in the map, certain conditions need to be made when clustering grid points, which can be described as follows: when clustering starts, the traversal area is only limited to the green rectangle. For grid points belonging to negative obstacles or trench obstacles, the Euclidean distance from the point to the detection base point must be less than a certain distance threshold $Thdist$ .

When there is a negative obstacle or a trench obstacle in the detection unit, the cluster area will be similar to the purple fan-shaped obstacle area in Figure 5. If the coordinates of points $v1$ and $v2$ can be calculated, the position C of the obstacle’s edge and the normal V of the obstacle can be calculated. Based on the shape characteristics of the fan-shaped area, two points can be found by sorting and filtering the points in the fan-shaped area. The process is as follows:

• Convert the two-dimensional coordinates of the grid points in the obstacle area to the robot coordinate system R and put them into the queue $Punkown$ .

• Sort $Punkown$ according to the rule of increasing X coordinate value. Points with the same X coordinate value are arranged from small to large according to the Y coordinate value. The first element of the team is the vertex $v1$ , and $v1$ is the smaller X coordinate value of the two vertices.

• Then sort $Punkown$ according to the rule of increasing Y coordinate value. Points with the same Y coordinate value are arranged from small to large according to the X coordinate value. If the head element and $v1$ are the same point or the European distance between the two points is small, take the tail element as the vertex $v2$ , corresponding to the situation in Figure 5 (b) and Figure 5 (c). Otherwise, take the head element as $v2$ , corresponding to the situation in Figure 5 (a).

The angle $θ$ between the normal V and the forward direction of the robot and the position C are obtained by the following formula:

When the width $|v1v2|$ of the obstacle’s edge is greater than the wheelbase of the robot, it is necessary to further confirm the type of obstacle and enter the next link for secondary detection.

• Type confirmation. The purpose of this step is to determine whether the obstacle is a negative obstacle or a trench obstacle. The secondary detection area is still rectangular, as shown by the blue rectangular frame in Figure 4. The location of the rectangle is related to point C.

The height of the terrain on both sides of the trench in the trench obstacle tends to be consistent, and the terrain height of the negative obstacle is much lower than the height of the ground where the robot is located. We use this to confirm the type of obstacle. The specific process is as follows:

• Traverse the secondary detection area to find grid points with elevation values, and set the height judgment threshold $Th$ based on the height of the detection base point. When the height value of the grid points is within the range of $Th$ above and below the reference. The points are classified as a trench obstacle point set $PT$ , otherwise they are classified to the negative obstacle point set $PN$ .

• After the traversal, judge the number N_T of the points in the trench obstacle point set $PT$ . If N_T exceeds the set threshold N, the place is considered to be a trench obstacle. The convex hull algorithm in D and the minimum area rectangle algorithm are used to solve the minimum area rectangle of point set $PT$ , and the span of the trench can be further calculated.

• If N_T does not exceed the threshold N, it is regarded as a negative obstacle and the average height $h¯$ of the point set $PN$ is calculated. The difference between $h¯$ and the height of the identified base point is taken as the depth of the negative obstacle.

In summary, the detection process of negative obstacles and trench obstacles is shown in Figure 6.

The proposed detection system was tested on the ROS-Gazebo simulation platform, indoor real environment and outdoor real environment. The validation is performed in three scenarios with different types of mobile robot platforms, namely, six-wheeled, tracked and four-wheeled. The resolution of the elevation map is 0.1 m. The feasibility of the algorithm is demonstrated by testing the success rate of detecting obstacles, and the accuracy of the algorithm is demonstrated by detecting the length or depth of the obstacles. Each scenario was repeated multiple times to ensure the validity of the data and conclusions. The success rate is only used to judge the type of obstacle. For irregular obstacles, the outermost boundary is used as the size standard for judging the accuracy.

The obstacle detection system was tested in simulations of various obstacles. The proposed detection algorithm is carried on a six-wheeled mobile robot. Figure 7 (top) shows five scenarios containing three types of obstacles constructed from various barricades. Figure 7 (middle) shows the effect of elevation mapping. Figure 7 (bottom) shows the detection results of the obstacle detection system.

The detection results are shown in Table 1. It can be seen from the figure that the proposed algorithm successfully detects various obstacles and completes the distinction of three kinds of obstacles with the success rate of the algorithm has reached 94.29%. It can be seen from Table 1 that the detection errors of the length and width of the obstacles are 0.1 m and 0.18 m, the error percentage is 2.5% and 9%, respectively. The detection accuracy of the height is 0.003 m, which reaches the millimeter level and 0.6% error rate. The average detection error of the algorithm for negative obstacle depth and trench width is 0.020 m and 0.039 m. While completing the above detection, the algorithm also maintains excellent real-time performance, with an average detection time of 16.8 ms. This is due to the low noise of the simulated environment and the result of the ideal obstacle scene.

This system is also verified experimentally in an indoor real environment. We created multiple obstacle environments with cardboard boxes, wooden boards, etc. Algorithms are validated on tracked vehicles. Figure 8 (top) shows the built indoor obstacle environment, and Figure 8 (bottom) shows the elevation map and obstacle detection results. Consistent with the simulation results, our work can accurately detect the type of each obstacle. Results of indoor experiments are shown in Table 2. In the real environment, the error of the system for obstacle size detection increases slightly. The accuracy of the detection algorithm is still excellent, reaching 96.5%. Due to the influence of sensor noise and environment, the detection accuracy (3.6%–6.5%) and time (21.5 ms) is not as good as the simulation result.

The system is finally verified by experiments in an outdoor real environment. Corresponding to the obstacle type, we selected three scenarios for outdoor verification experiments. The algorithm was verified on a four-wheeled vehicle. Figure 9 (top) shows the outdoor obstacle environment, and Figure 9 (bottom) shows the elevation map and obstacle detection results. The results are shown in Table 3. Due to the complexity of the outdoor environment and the irregularity of obstacles, the detection error (4.0%–6.6%) and time (22.2 ms) slightly increases compared to the simulation and indoor experiments. However, the algorithm has completed multiple identifications of obstacles with a high success rate (95%).

This paper presents an obstacle detection system based on elevation maps capable of detecting positive obstacles, negative obstacles and trench obstacles in various environments. The paper initially introduces the elevation mapping algorithm based on the Grid Map Library. The mapping algorithm takes the measurement noise of sensors and the localization uncertainties of robot into account. The mapping is realized by two steps: measurement update and movement update. Before identifying obstacles, the scheme completes the ground segmentation and obstacle clustering based on plane fitting algorithm and the idea of Euclidean clustering. For positive obstacles, an algorithm based on the smallest rectangular bounding box is proposed. For negative obstacles and trench obstacles, the algorithm is conducted based on the feature of data absence in the elevation map so as to initially locate them, while final types are determined in accordance with further detection. The proposed detection system is verified in Gazebo simulation environment, real indoor environment and real outdoor environment. Experiments show that in a real outdoor environment, the algorithm efficiently and accurately completes the obstacle detection task with an average time of 22.2 ms and a success rate of 95%. The error range of obstacle size detection is 4%–6.6%, which meets the next step obstacle-crossing requirements.

Future work will involve the optimization of the algorithm to improve efficiency, as well as the improvement of the algorithm’s functions to detect negative obstacles and trench obstacles to expand its applicability and meet more needs stemming from different scenarios.

This work is supported in part by the National Natural Science Foundation of China under Grant 62273123.