A Fast Obstacle Detection Algorithm Based on 3D LiDAR and Multiple Depth Cameras for Unmanned Ground Vehicles

Abstract

With the advancement of technology, unmanned ground vehicles (UGVs) have shown increasing application value in various tasks, such as food delivery and cleaning. A key capability of UGVs is obstacle detection, which is essential for avoiding collisions during movement. Current mainstream methods use point cloud information from onboard sensors, such as light detection and ranging (LiDAR) and depth cameras, for obstacle perception. However, the substantial volume of point clouds generated by these sensors, coupled with the presence of noise, poses significant challenges for efficient obstacle detection. Therefore, this paper presents a fast obstacle detection algorithm designed to ensure the safe operation of UGVs. Building on multi-sensor point cloud fusion, an efficient ground segmentation algorithm based on multi-plane fitting and plane combination is proposed in order to prevent them from being considered as obstacles. Additionally, instead of point cloud clustering, a vertical projection method is used to count the distribution of the potential obstacle points through converting the point cloud to a 2D polar coordinate system. Points in the fan-shaped area with a density lower than a certain threshold will be considered as noise. To verify the effectiveness of the proposed algorithm, a cleaning UGV equipped with one LiDAR sensor and four depth cameras is used to test the performance of obstacle detection in various environments. Several experiments have demonstrated the effectiveness and real-time capability of the proposed algorithm. The experimental results show that the proposed algorithm achieves an over 90% detection rate within a 20 m sensing area and has an average processing time of just 14.1 ms per frame.

1. Introduction

In recent years, with the advancement of technology in the field of robotics, a growing number of unmanned ground vehicles (UGVs) have been developed to replace human beings to complete many tedious or dangerous tasks. Most of the application scenarios are crowded places with a variety of static or dynamic objects, such as cleaning, food distribution, personnel transportation, rescue and disaster relief, etc., which require unmanned vehicles to have the ability to move autonomously in these complex environments. How to utilize the onboard sensors to detect obstacles to avoid collisions during movement is crucial for UGVs [1,2].

In order to perceive the environment stably and reliably in real time to avoid collisions, the vehicle must use a variety of sensors to accurately perceive the surrounding environment. Due to its accurate range information, 3D light detection and ranging (LiDAR) has the ability to operate in low-light conditions [3]. However, although 3D LiDAR has a wide FOV horizontally, it has a relatively small FOV vertically. For example, the Velodyne HDL-32 LiDAR has a horizontal FOV of 360$°$, but a vertical FOV of only 40$°$ (−25$°$~+15$°$), which is not able to sense lower obstacles in the proximal part of the unmanned vehicle. In addition, the LiDAR sensor is usually installed in the center of the UGV. If the UGV is too high or the LiDAR sensor cannot achieve 360$°$ horizontal coverage due to the body of the UGV, it is necessary to add new sensors in the blind spots of the LiDAR sensor. Therefore, in order to ensure the safety and reliability of UGVs during operation, multiple sensors are usually integrated to collect point clouds to realize seamless environment sensing. Three-dimensional LiDAR and multiple depth cameras combination sensing are widely used as the sensing source for various types of UGVs [4]. However, the large amount of data collected by 3D LiDAR and multiple depth cameras can be overwhelming. To be specific, one 32-wire LiDAR sensor and four depth cameras generate more than one million data points per frame. These data usually include not only information about surrounding objects, but also ground data, which can be considered redundant for object detection, classification, and tracking [5,6]. On the one hand, these large numbers of point clouds will lead to the inability to detect obstacles in real time, which increases the risk of a collision. On the other hand, there are non-negligible noise points in the point cloud, which may lead to incorrect identification of obstacles. It is difficult for existing methods to achieve real-time performance and accuracy simultaneously, which makes rapid and accurate obstacle detection from dense point clouds a significant challenge [7].

To solve this problem, this paper proposes a fast obstacle detection algorithm utilizing one 3D LiDAR sensor and multiple depth cameras from the UGV. The experimental results show that the algorithm can effectively detect various obstacles within ±20 m within 15 ms, which meets the safety and real-time requirements for UGV operation. The main contributions of the proposed algorithm are as follows:

• An efficient ground segmentation algorithm based on multi-plane fitting and plane combination is proposed in order to prevent them from being considered as obstacles.
• Instead of point cloud clustering, a vertical projection method is used to count the distribution of the potential obstacle points through converting the point cloud to a 2D polar coordinate system. Then, points in the fan-shaped area with a density lower than a certain threshold will be considered as noise.
• To verify the effectiveness of the proposed algorithm, a cleaning UGV equipped with one LiDAR sensor and four depth cameras is used to test the performance of obstacle detection in various environments.

The paper is structured as follows: The related work of ground segmentation and obstacle detection is presented in Section 2. The details of the proposed algorithm are illustrated in Section 3. Experimental results on several datasets are shown in Section 4. Conclusions and future work are discussed in Section 5.

2. Related Works

This section will mainly focus on obstacle detection algorithms based on point clouds and the sources of point clouds, including LiDAR sensors and depth cameras. In general, these obstacle detection algorithms can be divided into two categories [8]: data-driven algorithms based on deep learning and statistical algorithms based on hand-crafted features. Each of them will be discussed below.

Statistical algorithms for obstacle detection do not require training datasets and have good generalization abilities. Overall, these algorithms have two main processes, namely ground segmentation and point cloud clustering. The process of ground segmentation reduces the point cloud size and narrows the query scope of obstacles. Douillard et al. [9] projected the original point cloud into a set plane grid and extracted feature information from each grid for ground point cloud classification. When there are relatively few LiDAR wire harnesses, missed detection or false detection may occur. Himmelsbach et al. [10] proposed a fast ground segmentation algorithm. The purpose of the algorithm is to apply the two-dimensional line extraction algorithm to the unorganized three-dimensional point domain, quickly estimate the local ground plane, and compare the single point with the obtained ground plane line, which can reliably separate ground from non-ground places, but this algorithm cannot identify the ground and obstacles well. Asvadi et al. [11] designed a segmented approach by utilizing the Random Sample Consensus (RANSAC) algorithm to piecewise fit the ground estimate. Narksri et al. [12] proposed an environment-aware algorithm that filters the majority of non-ground points using the geometry of the sensor and the distance between consecutive rings in the scan. In the second step, multi-region RANSAC plane fitting is used to separate the remaining non-ground points from ground points in the scan. Zermas et al. [13] used the lowest altitude point of each frame of LiDAR to fit the ground equation, and iteratively optimized the normal vector of the ground plane through principal component analysis (PCA). This algorithm is only applicable to flat ground environments. Based on that algorithm, Lim et al. proposed a series of works including Patchwork [14] and Patchwork++ [15], which model point clouds based on a concentric region model and combine ground normal vector direction, height, and flatness information to perform ground segmentation. Wen et al. [16] gave up the idea of ground fitting on 3D point clouds, but projected point clouds onto two cylindrical images and used a pixel-based image method for segmentation. After the process of ground segmentation, the remaining point cloud needs to be clustered to distinguish between real obstacles and noise points. It is obvious that the point cloud that belongs to obstacles is dense while the noise point cloud is sparse. Due to the unknown number of obstacles, it will be difficult to effectively detect obstacles using partition-based clustering methods represented by k-means clustering. Gao et al. [17] proposed an improved density-based spatial clustering that dynamically adjusts the neighborhood radius on the basis of describing the local environment with an elliptic model. In contrast, Jiang et al. [18] proposed an adaptive neighborhood search radius clustering algorithm, which uses the horizontal resolution and pitch resolution of LiDAR to determine the clustering distance threshold. By using Euclidean distance clustering, Peng et al. [19] introduced two additional constraints to identify target obstacles. One is the number of points within each cluster, and the other is the volume of the bounding box containing these clusters. Nan et al. [20] proposed a tunable threshold Euclidean clustering method by designing a modulation function related to the clustering center. Although these sorts of clustering algorithms can effectively distinguish point clouds, they require a significant amount of time when dealing with dense point clouds, which makes it difficult to meet real-time requirements. Oh et al. [21] proposed an algorithm called TRAVEL, which performs traversable ground detection and simultaneous object clustering of 3D point clouds using graphical representation, taking full advantage of the node edge structure and ground segmentation to ensure real-time operation and reduce over-segmentation. However, there is a relatively large number of calculations that must be performed which limits the real-time performance.

With the enhancement of computing power and the surge of annotated data, deep learning has allowed for tremendous progress in the field of 3D point cloud-based object detection [22,23,24]. According to the different input formats of point clouds, these data-driven algorithms based on deep learning can be divided into two categories: voxel-based methods and point-based methods. Voxel-based methods divide the input point cloud into a unified feature description form, such as 3D voxels [25] or 2D bird’s eye views (BEVs) [26]. Later, convolutional neural networks (CNNs) were designed to learn valid point cloud features. This type of method greatly improves the computational efficiency of CNNs, but the sparse parts of the point cloud may be ignored during data conversion, leading to detection errors. Focal voxel R-CNN [27] replaced the original 3D sparse convolution with focal sparse convolution, and a branch of sub-manifold sparse convolution was added upstream of the backbone convolution network, which increased the weight of foreground points and retained more valuable information, which could provide more reliable obstacle detection results. Vote3Deep [28] used convolutional neural networks (CNNs) to locally detect objects in 3D point clouds, which was achieved by leveraging a feature-centric voting scheme to implement novel convolutional layers. In contrast, point-based methods learned feature representations of point clouds directly without any middleware, as exemplified by PointNet [29] and PointNet++ [30]. PointNet directly took the point cloud as input and could output labels for each point individually, which solved the disorder problem of point clouds. Due to the inadequacy of PointNet in extracting fine features, PointNet++ effectively extracted local fine features of different scales through the hierarchical set abstraction module. Lee et al. [31] proposed the Simultaneous Segmentation and Detection Network (SSADNet) algorithm, which performed simultaneous segmentation and detection based on a single neural network to recognize drivable areas and obstacles. The structure of SSADNet consisted of a branch for segmentation, a branch for detection, and a bridge connecting the two parts. Yin et al. [32] put forward a novel CNN model named CenterPoint that does not require manual setting of the anchor and has higher accuracy when facing scenes with objects of varying sizes. Objectively speaking, this type of method based on deep learning performs well in scenarios similar to the training data. However, it requires high computing power, has weak generalization, and has difficulty meeting real-time requirements.

3. Method

3.1. Algorithm Overview

The overall flow of the proposed fast obstacle detection algorithm based on point clouds is shown in Figure 1. It contains three modules: multi-sensor point cloud fusion, ground point cloud segmentation, and point cloud projection and denoising based on a fan-shaped area. In the first module, the point cloud from the depth camera will be transferred to the LiDAR coordinate system under the condition of time consistency. In the second module, in order to remove the ground point cloud that is not related to obstacles, an efficient ground segmentation algorithm is designed to adaptively fit multiple ground features. In the third module, the potential obstacle points will be converted into a 2D polar coordinate system. Points in the fan-shaped area with a density lower than a certain threshold will be considered as noise.

3.2. Multi-Sensor Point Cloud Fusion

For the depth camera, since each pixel in the depth image corresponds to its detection distance, it is necessary to convert the depth information from the pixel coordinate system to the depth camera coordinate system. The conversion formulas are shown in Equations (1) and (2), where the detection distance of the point with pixel coordinates $(u,v)$ in the depth image is $d$, K is the intrinsic parameter of the depth camera, $c_x$ and $c_y$ are the offsets of the camera’s optical axis in the pixel coordinate system, and $f_x$ and $f_y$ are the focal lengths of the depth camera in the width and height directions, respectively.

$$P_{\mathrm{C}}=K^{-1}\left[\begin{array}{c}u\times d\\ v\times d\\ d\end{array}\right]$$

(1)

$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

(2)

Since point cloud data come from multiple sensors including LiDAR and multiple depth cameras, a multi-sensor fusion operation is required to obtain the fused point cloud in the same coordinate system almost simultaneously. The relative pose relationship between these sensors is known. For convenience, the LiDAR coordinate system is set as the main coordinate system, while each depth camera coordinate system is set as a sub-coordinate system. Point clouds obtained from the depth cameras will be converted to the main coordinate system. The definitions of these coordinate systems are described in Figure 2. ${\mathrm{C}}_i$ and L represent the coordinate systems of the i-th depth camera and LiDAR sensor. In order to ensure that the fused point cloud is basically consistent in time, the timestamp of the current frame of the LiDAR sensor is used as the standard. Each depth camera frame can only be fused within a certain time threshold.

3.3. Ground Point Cloud Segmentation

The spatial characteristics of the ground point cloud are related to the environment in which the UGVs are located. For most service robots, their main operation scenarios include office buildings, shopping malls, underground parking lots, etc., which usually comprise only one ground plane.

Therefore, based on the ground plane fitting proposed in the literature [13], the ground point cloud is extracted by using a single-sensor multi-plane combination of the plane fitting method, which follows the following principles. Details are described in Figure 3. Firstly, the ground point cloud has distinct planar features, which can be fitted by a planar formula. Secondly, for UGVs, the points with the lowest height values are most likely to belong to the ground point cloud, so there is no need to introduce the RANSAC process. Third, utilizing each individual sensor for plane fitting and then combining the plane information to form a new plane would enable the use of parallel computing to improve algorithmic efficiency. The fused point cloud $P$ consists of the LiDAR’s point cloud $P_{\mathrm{L}}$ and several depth cameras’ point clouds from $P_{{\mathrm{C}}_1}$ to $P_{{\mathrm{C}}_n}$. A single sensor’s point cloud $P_{\mathrm{L}}$ or $P_{{\mathrm{C}}_i}$ needs to be sorted in ascending order according to the height value. Taking $P_{\mathrm{L}}$ as an example, its ascending state is represented as $P_{\mathrm{L}}^{\mathrm{*}}$. A set of seed points with low height values are selected to estimate the initial plane formulation for the ground, described as $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$.

Later, it is necessary to calculate the mean point $p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$ of $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$ and the corresponding covariance matrix $C_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}$ to obtain the simple linear ground estimation model. Performing singular value decomposition (SVD) on $C_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}$ can result in three main directions of $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$. Obviously, the singular vector corresponding to the minimum singular value obtained by SVD is the normal vector perpendicular to the ground plane. The initial ground plane estimation of $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$ can be described in Equations (3)–(7):

$$a_{\mathrm{L}}x+b_{\mathrm{L}}y+c_{\mathrm{L}}z+d_L=0$$

(3)

$$C_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}=\sum _{j=1}^N\left(p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^j-p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}\right){\left(p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^j-p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}\right)}^{\mathrm{T}}$$

(4)

$$C_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}=\mathrm{U}\mathrm{S}{\mathrm{V}}^{\mathrm{T}},U=\left[u_1,u_2,u_3\right]$$

(5)

$${\left(a_{\mathrm{L}},b_{\mathrm{L}},c_{\mathrm{L}}\right)}^{\mathrm{T}}=u_3$$

(6)

$$d_{\mathrm{L}}=-{u_3}^{\mathrm{T}}·p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$$

(7)

where $a_{\mathrm{L}}$, $b_{\mathrm{L}}$, $c_{\mathrm{L}}$, and $d_{\mathrm{L}}$ are the parameters of the ground plane model, and $p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^j$ is the j-th point of $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$.

After obtaining the initial ground plane estimation, the next step is to calculate the distance from each point in $P_{\mathrm{L}}$ to the ground plane. This distance is compared with a threshold ${\mathrm{T}\mathrm{h}}_{\mathrm{d}}$ to determine whether it falls within the ground plane. The points belonging to the ground plane serve as the new seed points for updating the ground plane estimation. After repeating the above process $\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{N}\mathrm{u}\mathrm{m}$ times, the ground plane estimation of LiDAR will be stable. The ground plane estimation of each depth camera performs the same operation. The mean points including $p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$, $p_{{\mathrm{C}}_1,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$, $\dots$, $p_{{\mathrm{C}}_n,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$ will be combined to fit a new plane $a_{\mathrm{F}}x+b_{\mathrm{F}}y+c_{\mathrm{F}}z+1=0$ by using the Gauss–Newton method. Since the ground plane extracted by each sensor has different errors, it is not appropriate to give the same weight to these mean points. Inspired by the works [33,34] in weighted plane fitting, the indicator for evaluating the quality of a ground point cloud is its thickness, that is, its standard deviation in the z-axis direction. The smaller the standard deviation, the higher the quality of the ground point cloud extracted by the sensor. There is no doubt that the standard deviation ${\sigma }_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^z$ in the z-axis direction of $P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$ can be obtained from $C_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}$. The corresponding weight $w_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}$ is calculated in Equation (8), which ensures the normalization of weight coefficients.

$$w_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}={\displaystyle \frac{{\mathrm{e}}^{-{\sigma }_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^z}}{{\mathrm{e}}^{-{\sigma }_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^z}+{\mathrm{e}}^{-{\sigma }_{{\mathrm{C}}_1,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^z}+\cdots +{\mathrm{e}}^{-{\mathsf{\sigma }}_{{\mathrm{C}}_n,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^z}}}$$

(8)

Later, the new plane estimation can be calculated by the minimizing the weight-based distances between the mean points in the new plane, which is shown in the following Equations (9) and (10).

$$d_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}=w_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}\times {\left({p_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{c}}}^{\mathrm{T}}\left[\begin{array}{c}{a}_{\mathrm{F}}\\ b_{\mathrm{F}}\\ c_{\mathrm{F}}\end{array}\right]+1\right)}^2$$

(9)

$$\underset{a_{\mathrm{F}},b_{\mathrm{F}},c_{\mathrm{F}}}{\min }\left(\sum _{X\in \left\{\mathrm{L},{\mathrm{C}}_1,\cdots ,{\mathrm{C}}_n\right\}}{d}_{X,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}\right)$$

(10)

The point in the fused point cloud $P$ whose distance to the new plane does not exceed the threshold ${\mathrm{T}\mathrm{h}}_{\mathrm{d}}$ will be considered as a ground point, otherwise it is a potential obstacle point. Finally, the ground point cloud $P_{\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}$ and the potential obstacle point cloud ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ will be obtained. The flow chart of the ground point cloud segmentation method is shown in Algorithm 1.

Algorithm 1 Ground Point Cloud Segmentation

$\mathbf{Input}:P$$\left(\mathrm{The}\mathrm{fused}\mathrm{point}\mathrm{cloud}\right)\mathrm{includes}{P_{\mathrm{L}},P}_{{\mathrm{C}}_1},\dots ,P_{{\mathrm{C}}_n}$

$\mathbf{Output}:P_{\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}$$(\mathrm{ground}\mathrm{point}\mathrm{cloud}),{\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ (potential obstacle point cloud) $1:\mathrm{Sort}\mathrm{each}\mathrm{one}\mathrm{of}{P_{\mathrm{L}},P}_{{\mathrm{C}}_1},\dots ,P_{{\mathrm{C}}_n}$ from low to high along the height value; $2:\mathrm{Select}\mathrm{a}\mathrm{set}\mathrm{of}\mathrm{seed}\mathrm{points}\mathrm{with}\mathrm{low}\mathrm{height}\mathrm{values}\mathrm{as}{P}_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}},P_{{\mathrm{C}}_1,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}},\dots ,P_{{\mathrm{C}}_n,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$; $3:\mathbf{foreach}{P}_{X,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}\in \left\{P_{\mathrm{L},\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}},P_{{\mathrm{C}}_1,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}},\dots ,P_{{\mathrm{C}}_n,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}\right\}$ do $4:\mathrm{Plane}\mathrm{Fitting}\mathrm{and}\mathrm{obtain}\mathrm{ground}\mathrm{plane}\mathrm{model}{a}_{X}x+b_{X}y+c_{X}z+d_X=0$; $5:\mathbf{end}$ $6:\mathbf{for}i=1:\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{N}\mathrm{u}\mathrm{m}$$\mathbf{do}(\mathrm{parallel}\mathrm{computing}\mathrm{for}\mathrm{each}{P}_X$$\mathrm{in}\left\{{P_{\mathrm{L}},P}_{{\mathrm{C}}_1},\dots ,P_{{\mathrm{C}}_n}\right\}$) $7:\mathbf{foreach}p\in P_X$ do $8:\mathrm{Calculate}\mathrm{the}\mathrm{distance}d$ to corresponding plane model; $9:\mathbf{if}d<{\mathrm{T}\mathrm{h}}_{\mathrm{d}}$then $10:\mathrm{Put}p$$\mathrm{into}{P}_{X,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$; 11:        else  $12:\mathrm{Remove}p$$\mathrm{from}{P}_{X,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$; $13:\mathbf{end}$ $14:\mathrm{Do}\mathrm{plane}\mathrm{Fitting}\mathrm{for}{P}_{X,\mathrm{s}\mathrm{e}\mathrm{e}\mathrm{d}}^{\mathrm{*}}$ and obtain the ground plane model; $15:\mathbf{end}$ $16:\mathrm{Plane}\mathrm{fitting}\mathrm{for}\mathrm{getting}\mathrm{a}\mathrm{new}\mathrm{plane}{a}_{\mathrm{F}}x+b_{\mathrm{F}}y+c_{\mathrm{F}}z+1=0$ $17:\mathbf{foreach}p\in P$ do $18:\mathrm{Calculate}\mathrm{the}\mathrm{distance}d$ to the new plane model; $19:\mathbf{if}d<{\mathrm{T}\mathrm{h}}_{\mathrm{d}}$ then $20:\mathrm{Put}p$$\mathrm{into}{P}_{\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}$; $21:\mathbf{else}$ $22:\mathrm{put}p$$\mathrm{into}{\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$; $23:\mathbf{end}$ $24:\mathbf{return}{P}_{\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}$$\mathrm{and}{\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$

3.4. Point Cloud Projection and Denoising Based on Fan-Shaped Area

After receiving the potential obstacle point cloud ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$, it is vital to count its distribution for distinguishing the noise points. Obviously, regions with dense point clouds are more likely to have obstacles, while regions with only sporadic point clouds are likely to be just noise. A clustering algorithm is an unsupervised learning method mainly used in data mining and data analysis. Its purpose is to automatically group a set of objects into different categories, with high similarity of data within each class and low similarity between different classes. The existing mainstream method is to cluster ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ to determine the scale and distribution of point clouds in each class. Most clustering algorithms require setting hyperparameters such as the number of cluster centers and boundary distance in advance, and are very time-consuming when the point cloud is dense. Therefore, a new statistical method for the point cloud distribution of ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ is proposed.

Firstly, the potential obstacle point cloud ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ will be projected vertically, transforming it from 3D to 2D. Within a certain horizontal range, the real obstacle point cloud is continuous in the vertical direction, and it becomes dense after projecting it into 2D space. For each point $p\left(x,y,z\right)$ in ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$, its corresponding polar coordinate value $\left(r,\theta \right)$ can be calculated in the following Equations (11) and (12):

$$r=\sqrt[2]{x^2+y^2}$$

(11)

$$\theta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}{\displaystyle \frac{y}{x}}$$

(12)

Later, a grid system based on the fan-shaped area will be established so that $r\in \left[r_{\mathrm{m}\mathrm{i}\mathrm{n}},r_{\mathrm{m}\mathrm{a}\mathrm{x}}\right]$ and $\theta \in \left[{\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\theta }_{\mathrm{m}\mathrm{a}\mathrm{x}}\right]$. Discretize the polar diameter with step size $∆r$ and the polar angle with step size $∆\theta$ so that the point in this grid system can be represented as $\left(r_i,{\theta }_j\right)$. The specific description is in the following Equations (13) and (14).

$$r_i=r_{\mathrm{m}\mathrm{i}\mathrm{n}}+i\times ∆r,i=\mathrm{0,1},2,\cdots ,N_r,N_r=⌊{\displaystyle \frac{r_{\mathrm{m}\mathrm{a}\mathrm{x}}-r_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆r}}⌋$$

(13)

$${\theta }_j={\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}}+j\times ∆\theta ,j=\mathrm{0,1},2,\cdots ,N_{\theta },N_{\theta }=⌊{\displaystyle \frac{{\theta }_{\mathrm{m}\mathrm{a}\mathrm{x}}-{\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆\theta }}⌋$$

(14)

All the points in ${\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ need to have their corresponding grid values calculated. For the point with polar coordinate value $\left(r,\theta \right)$, the corresponding grid value is $\left(⌊{\displaystyle \frac{r-r_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆r}}⌋,⌊{\displaystyle \frac{\theta -{\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆\theta }}⌋\right)$. The grid system will count the number of points that fall into each grid. In order to accelerate the retrieval process of the grid system, a hash table is used to store the number of points that each grid falls into.

Because the grid division is delicate, it is unscientific to judge whether there is an obstacle in the grid only by the number of points falling into the grid. Including neighboring grids in the statistics will reflect the true condition of the current grid. Specifically, with the current grid as the center, expand $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$ steps forward and backward on the polar diameter and $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$ steps forward and backward on the polar angle. The number of points falling into the expanding grids is defined as $N_f$. The schematic diagram of the above process is shown in Figure 4. For the convenience of explanation, the parameters $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$ and $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$ are both set to 1.

According to the analysis of LiDAR and depth camera measurement noise in [33,34], the uncertainty of point cloud is positively correlated with its detection distance. It is not reasonable to set a uniform threshold to determine whether the point cloud within a grid is a real obstacle. Setting a dynamic threshold related to distance is a feasible method. When the distance between the grid and the UGV is closer, the corresponding grid’s area is smaller and the threshold value needs to be set smaller. It is necessary to judge the condition of a grid according to the density information of the points falling into the grid. The area of expanding grids by using $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$ and $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$ can be calculated in Equation (15), and the density value is calculated in Equation (16). If the density value is greater than the threshold ${\mathrm{T}\mathrm{h}}_{\mathrm{p}}$, the corresponding points in the current grid will be treated as obstacle points.

$$t_{\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{a}}={\displaystyle \frac{\mathsf{\pi }{\left(r+N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}\times ∆r\right)}^2}{2\mathsf{\pi }∕\left(2N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }\times ∆\theta \right)}}-{\displaystyle \frac{\mathsf{\pi }{\left(r-N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}\times ∆r\right)}^2}{2\mathsf{\pi }∕\left(2N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }\times ∆\theta \right)}}$$

(15)

$${Th}_{\mathrm{d}\mathrm{e}\mathrm{n}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{y}}={\displaystyle \frac{N_f}{t_{\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{a}}}}$$

(16)

Combined with the content of the previous section, the flow chart of the noise filtering method is shown in Algorithm 2. Through a series of operations, the real obstacle point cloud is obtained.

Algorithm 2 Noise Filtering

$\mathbf{Input}:{\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ (The potential obstacle point cloud)

$\mathbf{Output}:P_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ (Real obstacle point cloud) $1:\mathrm{Initialize}\mathrm{hash}\mathrm{table}{H}_t$$\mathrm{and}{P}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ $2:\mathbf{foreach}p\in {\widehat{P}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ do $3:\mathrm{Calculate}\mathrm{its}\mathrm{corresponding}\mathrm{polar}\mathrm{coordinate}\mathrm{value}\left(r,\theta \right)$; $4:\mathrm{Calculate}\mathrm{its}\mathrm{corresponding}\mathrm{grid}\mathrm{value}\left(⌊{\displaystyle \frac{r-r_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆r}}⌋,⌊{\displaystyle \frac{\theta -{\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆\theta }}⌋\right)$; $5:\mathrm{Add}1\mathrm{to}{H}_t\left(⌊{\displaystyle \frac{r-r_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆r}}⌋,⌊{\displaystyle \frac{\theta -{\theta }_{\mathrm{m}\mathrm{i}\mathrm{n}}}{∆\theta }}⌋\right)$; $6:\mathbf{end}$; $7:\mathbf{for}i=0:N_r$ do  $8:\mathbf{for}j=0:N_{\theta }$ do $9:\mathbf{if}{H}_t\left(i,j\right)>0$ then $10:\mathrm{Determine}\mathrm{the}\mathrm{expanding}\mathrm{grids}\mathrm{based}\mathrm{on}{N}_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$$\mathrm{and}{N}_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$; $11:\mathrm{Calculate}\mathrm{the}\mathrm{number}\mathrm{of}\mathrm{points}{N}_f$ falling into the expanding grids; $12:\mathrm{Calculate}\mathrm{the}\mathrm{density}\mathrm{information}\mathrm{of}\mathrm{the}\mathrm{expanding}\mathrm{grids};$ $13:\mathbf{if}{Th}_{\mathrm{d}\mathrm{e}\mathrm{n}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{y}}>{\mathrm{T}\mathrm{h}}_{\mathrm{p}}$ then $14:\mathrm{Put}\mathrm{the}\mathrm{corresponding}\mathrm{points}\mathrm{into}{P}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$ $15:\mathbf{end}$ $16:\mathbf{end}$ $17:\mathbf{end}$ $18:\mathbf{end}$ $19:\mathbf{return}{P}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}$

4. Experiments and Results

4.1. Experiment Overview

In order to verify the effectiveness of the proposed method, the authors selected the cleaning UGV to test the effectiveness and real-time capability of the proposed algorithm. This UGV is equipped with one Leishen C32W LiDAR in its top center and four depth cameras in front, behind, left, and right of it. Specifically, the camera model installed in the front and behind directions is the Orbbec DaBai DCW2, which has a detection range of 0.2 m to 3 m and provides a depth resolution of 640 × 400. Conversely, the camera model installed in the left and right directions is the Orbbec Dabai MAX (Orbbec, Shenzhen, China), which has a detection range of 0.2 m to 2.5 m and provides a depth resolution of 640 × 320. The frequency of all the sensors is set to 10 hz for maintaining time consistency. The loaded sensors are shown in Figure 5 and the cleaning UGV is shown in Figure 6.

The UGV is equipped with an Intel Core i7 (Santa Clara, CA, USA) processor with 3.1GHz and 16G RAM, which has enough computing ability in point cloud calculation. The transmission of sensor data is achieved through the Robot Operating System (ROS) (Stanford, CA, USA) framework [35]. This UGV will be tested in two environments; one is a warehouse and the other is a parking scene. The total acquisition time of the warehouse scene is 431 s. Its main static obstacles include boxes and walls in various scenes. During the acquisition process, people walking in the warehouse are its main dynamic obstacles. The noise mainly comes from the unstable point cloud information obtained by the depth camera. The acquisition time of the parking scene is 1204 s. Its main static obstacles include pillars, walls, and stationary vehicles. Since drivers get out of cars and vehicles start moving during the acquisition process, its main dynamic obstacles include pedestrians and moving vehicles. Similarly, its noise source is mainly the unstable observation value of the depth camera. After the process of multi-sensor point cloud fusion, the vertical view of the fused point cloud in the main coordinate system is shown in Figure 7 and Figure 8. The red, orange, blue, green, and purple point clouds come from the front-view depth camera, left-view depth camera, behind-view depth camera, right-view depth camera, and LiDAR sensor, respectively. The UGV is located in the middle of the wrapped area of four depth camera point clouds. It can be clearly observed that the detection distance of the depth camera is smaller but the point cloud is dense, while the detection distance of the LiDAR sensor is longer but the point cloud is relatively sparse.

Ground point cloud segmentation is a classic binary classification problem that requires dividing the point cloud into a ground portion and non-ground portion. Similarly, obstacle detection is used to determine whether the point is a noise point or an obstacle. Therefore, evaluation indicators including the precision, recall, and F1_score are used to evaluate the classification performance. In this task, correct matches are true positives (TPs), incorrect matches are false positives (FPs), wrongly discarded matches are false negative (FNs), and the correctly discarded matches are true negative (TNs). Precision measures the proportion of samples correctly predicted as positive to all samples predicted as positive. High precision means that the model rarely misclassifies positive classes. Recall measures the ratio of the number of samples that are actually predicted to be positive to all actual positive samples. High recall means that the model is able to find all positive classes well. These two indicators often need to be weighed in practical applications, because improving one may sacrifice the other. Coincidentally, the F1_score is the harmonic mean of precision and recall. The precision, recall, and F1_score can be calculated in the following Equations (17)–(19).

$$f_{\mathrm{p}\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}}={\displaystyle \frac{\mathrm{T}\mathrm{P}}{\mathrm{T}\mathrm{P}+\mathrm{F}\mathrm{P}}}$$

(17)

$$f_{\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{a}\mathrm{l}\mathrm{l}}={\displaystyle \frac{\mathrm{T}\mathrm{P}}{\mathrm{T}\mathrm{P}+\mathrm{F}\mathrm{N}}}$$

(18)

$${F1}_{\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{e}}={\displaystyle \frac{2\times f_{\mathrm{p}\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}}\times f_{\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{a}\mathrm{l}\mathrm{l}}}{f_{\mathrm{p}\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}}+f_{\mathrm{r}\mathrm{e}\mathrm{c}\mathrm{a}\mathrm{l}\mathrm{l}}}}$$

(19)

4.2. Experimental Results in Ground Point Cloud Segmentation

Because the proposed algorithm is a statistical algorithm based on hand-crafted features, it is necessary to compare the effect of ground point cloud segmentation with other advanced algorithms, including Patchwork++ [15] and DipG-Seg [16]. In this part, the performance of the algorithms will be tested using the single ground plane case. Before conducting comparative experiments, it is necessary to explain the configuration of algorithm-related hyperparameters. For this algorithm, the threshold ${\mathrm{T}\mathrm{h}}_{\mathrm{d}}$ to determine whether a point falls within the corresponding ground plane is set as 0.1 m, and the iteration optimization time $\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{N}\mathrm{u}\mathrm{m}$ is set as 10. In the initial stage, the lowest points in the first 10% are selected as seed points. In order to ensure good performance of ground segmentation, the above hyperparameter settings are all based on reference [13].

The ground point cloud segmentation effect is tested in both the warehouse and parking environments. After inputting the fused point cloud, the performance of the ground point cloud segmentation effect using several algorithms is shown in Figure 9, Figure 10, Figure 11, Figure 12, Figure 13 and Figure 14. It is obvious that the proposed method achieves the best ground segmentation effect that basically reflects the spatial information of the ground point cloud. In sharp contrast, the performance of Patchwork++ and DipG-Seg are not satisfactory. The ground point clouds they extract are quite different from the real scenes and do not distinguish between ground point clouds and non-ground obstacles. As shown in

Table 1. Comparison of evaluation indicators for different algorithms in ground segmentation (warehouse).

Algorithm	Precision (%)	Recall (%)	F1score (%)	Runtime (ms)
Patchwork++	75.3	30.6	43.5	22.4
DipG-Seg	48.7	19.3	27.6	17.7
Proposed	88.2	92.5	90.3	11.3

and

Table 2. Comparison of evaluation indicators for different algorithms in ground segmentation (parking).

Algorithm	Precision (%)	Recall (%)	F1score (%)	Runtime (ms)
Patchwork++	63.4	28.1	38.9	23.0
DipG-Seg	52.7	26.2	35.0	18.1
Proposed	94.4	95.3	94.8	11.6

, it is obvious that the proposed algorithm achieves the highest score from the evaluation metrics.

The outstanding performance of the proposed algorithm in ground point cloud segmentation can be attributed to the inclusion of the point cloud generated by the depth camera. The point cloud generated by the depth camera is affected by factors such as ambient lighting, texture contrast, and camera hardware performance [36]. If the ambient light changes greatly or the surface of the object is smooth, the depth estimation may be inaccurate. In summary, the point cloud data provided by LiDAR are more stable in most cases [37], especially in complex lighting and dynamic environments. The proposed ground segmentation method is able to overcome the unstable point cloud vibration, which reflects its robustness for fused point cloud data. Since Patchwork++ and DipG-Seg were originally designed for ground segmentation of the LiDAR point cloud, they are sensitive to the unstable point cloud from depth cameras. Patchwork++ uses region-wise vertical plane fitting (R-VPF) to divide the fused point cloud into several subsets and estimates the plane based on the principle of concentric circles. Because the concentric circle assumption does not hold in this fused point cloud, its idea is not applicable in this scenario. As for DipG-Seg based on image processing, this algorithm extracts the ground point cloud by filtering the image obtained by projecting the point cloud data. In the process of projecting point clouds to generate images, the noise from the depth camera will have a non-negligible interference on the image quality. There is no doubt that pixel filling and mean filtering have limited effects on improving image quality. Ultimately, ground point cloud segmentation based on pixel features make it difficult to ensure the effectiveness of the algorithm.

However, although the proposed algorithm has a better performance in ground segmentation, there are still some shortcomings in the proposed algorithm, as shown in Figure 15. Since there is a height threshold based on plane fitting, this will cause the points where obstacles are close to the ground plane to be incorrectly classified into the ground point cloud. This problem is more obvious in scenes with many small objects. Meanwhile, some remote points generated by depth cameras are mistakenly identified as obstacle points.

The ground segmentation efficiency of these algorithms is compared on the unified platform without using GPU-related devices. For the sake of fairness, the average running time over a period of time was chosen as the comparison. Table 1 and Table 2 illustrate that the runtimes of Patchwork++ and DipG-Seg are slightly greater than that of the proposed algorithm. In general, these methods basically meet the real-time requirement when the frequency of the receiving point cloud is 10 hz. On the basis of Patchwork, Patchwork++ divides the point cloud into a few regions for fitting the ground plane and adds some outlier suppression modules to optimize the segmentation effect. Therefore, Patchwork++ is relatively more time-consuming. DipG-Seg does not use point cloud-based fitting operations, and its processing flow is based on image filtering and segmentation. This image-based processing method further reduces the time required for ground segmentation. The proposed method performs parallel ground plane estimation based on the number of sensors and only requires a small amount of iterative optimization to achieve a stable segmentation effect. In general, the proposed method is slightly better in efficiency.

4.3. Experimental Results in Obstacle Detection

Next, on the basis of the ground point cloud segmentation by the proposed algorithm, the real obstacle point cloud is obtained by using a point cloud clustering-based method or a point cloud denoising-based method. In this part, a typical Euclidean clustering algorithm [19] is used to compare the proposed fan-shaped area retrieval. In addition, the CenterPoint model [32] based on deep learning also adds a comparison experiment. As for the proposed algorithm, the step size $∆r$ of polar diameter and the step size $∆\theta$ of polar angle are respectively set as 0.1m and 0.01rad. The neighboring parameters $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$ and $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$ are set as 2 and 10. This smaller hyperparameter setting is to prevent erroneous binding to adjacent noise points. To verify the above functions, a larger set of hyperparameter settings where $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},r}$ = 5 and $N_{\mathrm{n}\mathrm{e}\mathrm{i}\mathrm{b}\mathrm{o},\theta }$ = 20 will also be compared. The density threshold ${\mathrm{T}\mathrm{h}}_{\mathrm{p}}$ to determine the obstacle is set as 80. Its setting is mainly based on the work of reference [18]. The obstacle avoidance function only requires the point cloud within a certain range of this kind of low-speed UGV. Therefore, the obstacle detection radius centered on the vehicle body is set to 20 m.

The obstacle detection effect is tested in the same scenes as the previous part. After inputting the potential obstacle point cloud, the performance of obstacle detection by two algorithms is shown in Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22 and Figure 23. The Euclidean clustering method calculates the Euclidean distance between points to determine if they belong to the same cluster. Then, the noise point cloud will be eliminated by the number of points in the cluster and the size of the bounding box corresponding to the cluster. This clustering method can combine the point cloud with similar spatial distances and filter out some sparse points. However, when facing relatively dense noise, it will cluster them together and mistakenly believe that these noise points belong to obstacles. Figure 16 and Figure 20 confirm the above phenomenon. In fact, the point cloud data in the edge area detected by depth cameras are relatively unstable. This leads to a relatively uniform and dense distribution of noise. Due to their large quantity and relatively large spatial volume, clustering-based methods have difficultly distinguishing whether they belong to obstacles or noise. CenterPoint has a good recognition of obstacles that are labeled in the training set, but the effect is not satisfactory for unlabeled obstacles. Admittedly, this will avoid many false detections of noise points as obstacles, but it will cause many real obstacles to be ignored by the UGV. In the above experiment, it treated all identified obstacles as cars. The proposed fan-shaped area retrieval also detects various static and dynamic obstacles. Compared with the Euclidean clustering method, it basically overcomes the noise interference problem. Due to the fan-shaped segmentation of the point cloud into small areas, the real obstacle point cloud presents a very dense scene, while there are obvious gaps between the noise points. This provides a good pattern for distinguishing between the real obstacle points and noise points. If the density of the point cloud in the area does not reach a certain level, it will not be considered as an obstacle. As is shown in Figure 18, Figure 19, Figure 22, and Figure 23, due to the design of the density threshold mechanism, the points belong to the persons near the UGV, and their number exceeds the smaller threshold. Some noise points are also relatively dense with remote distances. Thus, their quantity is not sufficient to meet the threshold requirement. Both of the comparisons between Figure 18 and Figure 19 and between Figure 22 and Figure 23 show that setting a smaller threshold can avoid the association of irrelevant point clouds and prevent noise points from being mistakenly included in the same category as real obstacles. Objectively speaking, the proposed algorithm effectively filters out most of the noise, but it can also lead to missed or false detections. Some smaller boxes placed in the long distance are overlooked, and the trailing points of LiDAR close to the UGV may be misidentified as an obstacle to some extent.

The obstacle detection efficiency of two algorithms is compared on the same platform. The average running time over a period of time is used for comparison.

Table 3. Comparison of evaluation indicators in different algorithms for smaller objects at close range (warehouse).

Algorithm	Precision (%)	Recall (%)	F1score (%)	Runtime (ms)
Euclidean Clustering	75.3	95.1	84.0	124.2
CenterPoint	94.4	12.3	21.8	1263.7
Proposed (Smaller Hyperparameter)	91.2	93.7	92.4	2.8
Proposed (Larger Hyperparameter)	80.9	94.1	87.0	2.9

and

Table 4. Comparison of evaluation indicators in different algorithms for smaller objects at close range (parking).

Algorithm	Precision (%)	Recall (%)	F1score (%)	Runtime (ms)
Euclidean Clustering	87.1	96.4	91.5	119.7
CenterPoint	96.8	16.7	28.5	1425.9
Proposed (Smaller Hyperparameter)	92.2	95.5	93.8	2.6
Proposed (Larger Hyperparameter)	88.4	95.9	92.0	2.8

illustrate that the runtime of the Euclidean clustering method is significantly greater than that of the proposed algorithm. Due to the need to control costs, the computing equipment performance of the proposed cleaning UGV is limited. Since CenterPoint is based on deep learning, it is difficult to run on computing devices without GPU configuration. Therefore, it takes the longest time to process. The proposed algorithm is able to meet the real-time requirement. According to the time complexity analysis of clustering algorithms [38], the time complexity of Euclidean clustering is $\mathrm{O}\left(lnkd\right)$, where $n$ is the number of point clouds, $k$ is the number of clusters, $d$ is the feature dimension, and $l$ is the number of iterations. When the number of points is particularly large, it will consume a lot of time. Due to abandoning the clustering operation, the proposed fan-shaped area retrieval greatly improves the efficiency of point cloud denoising, and the time complexity is only $\mathrm{O}\left(n\right)$.

5. Conclusions

In this paper, a fast obstacle detection algorithm based on 3D LiDAR and multiple depth cameras is proposed to improve the effectiveness and real-time capability of obstacle detection for UGVs. The fused point cloud is preliminarily filtered by efficient ground point cloud segmentation based on multi-plane fitting and plane combination, which ensures that the ground point cloud is not misidentified as an obstacle. Subsequently, the distribution of the remaining point cloud is counted by vertical projection, and the fan-shaped area retrieval will find the points belonging to the real obstacles on the basis of the adaptive threshold function. Experimental results demonstrate the effectiveness and real-time capability of the proposed algorithm. Compared with several state-of-the-art algorithms, the proposed algorithm can efficiently detect ground point clouds and complete separation operations in the case of multiple ground planes. Due to the adoption of the efficient point cloud data analysis method, the proposed algorithm can effectively avoid false detections caused by noise and detect obstacles of different sizes within various distance ranges. Specifically, the proposed algorithm can achieve an over 90% detection rate within a 20 m sensing area, with an average processing time of just 14.1 ms per frame.

However, the proposed algorithm still has some shortcomings. When faced with an unstructured multi-plane environment, the proposed algorithm will find it difficult to filter out all the ground point clouds and is likely to regard them as obstacles. Furthermore, due to the projection-based retrieval mechanism, thin and hollow objects are likely to be overlooked. In future work, the authors will make improvements to address the above issues, such as adding height information in projection statistics and designing a more robust method for detecting the number of ground planes.