Laser Radar Data Registration Algorithm Based on DBSCAN Clustering

Abstract

At present, the core of lidar data registration algorithms depends on search correspondence, which has become the core factor limiting the performance of this kind of algorithm. For point-based algorithms, the data coincidence rate is too low, and for line-based algorithms, the method of searching the correspondence is too complex and unstable. In this paper, a laser radar data registration algorithm based on DBSCAN (Density-Based Spatial Clustering of Applications with Noise) clustering is proposed, which avoids the search and establishment of the corresponding relationship. Firstly, a ring band filter is designed to process the outliers with noise in a point cloud. Then, the adaptive threshold is used to extract the line segment features in the laser radar point cloud. For the point cloud to be registered, a DBSCAN density clustering algorithm is used to obtain the key clusters of the rotation angle and translation matrix. In order to evaluate the similarity of the two frames of the point cloud in the key clusters after data registration, a kernel density estimation method is proposed to describe the registered point cloud, and K-L divergence is used to find the optimal value in the key clusters. The experimental results show that the proposed algorithm avoids the direct search of the correspondence between points or lines in complex scenes with many outliers in laser point clouds, which can effectively improve the robustness of the algorithm and suppress the influence of outliers on the algorithm. The relative error between the registration result and the actual value is within 10%, and the accuracy is better than the ICP algorithm.

1. Introduction

In the process of scanning the environment, lidar generates an environmental map [1] by measuring the distance and angle of an obstacle relative to the sensor, and can obtain the position of the sensor itself in the environment. Reference [2] designed a mathematically tractable analytical model for a vehicle with arbitrary shape. The effects of extended-vehicle shape were correctly captured by lidar, and the main dependencies between receiving energy, bandwidth and array size were revealed. In the framework of a probabilistic multi-hypothesis tracker (PMHT), reference [3] proposed a new algorithm for lasers to track multiple point targets and extended targets simultaneously in the presence of clutter and missed detections, which can adapt to spatio-temporally varying target sizes or extents of extended targets and temporally varying target cardinality. Frequency-modulated continuous wave (FMCW) lidar can be studied for remote vital-sign monitoring. Reference [4] proposed an impulse denoising operation and spectral estimation decision method, which provides high-quality, repeatable respiration and heart rates. Compared with cameras, sonars and ultrasonic sensors, lidar has the advantages of high measurement accuracy, good directivity and immunity to environmental illumination. It plays an important role in target tracking, three-dimensional reconstruction [5], target detection and other fields.

Synchronous positioning and mapping based on lidar [6] positioning in an unknown environment [7] are essentially the registration of lidar data. Point cloud data registration [8], as an extremely important part of two-dimensional point cloud data processing [9], can convert data obtained from different perspectives into the same coordinate system through a series of calculations. By rotating and translating the scanning lidar data at the current moment, it has a large degree of overlap with the reference scanning data, so that changes in the robot pose [10] can be obtained. The registration effect of point cloud data will affect the subsequent multi-sensor fusion positioning [11] and loop detection directly [12]. In practical applications, due to the existence of the wheel-slip phenomenon of mobile robots, the radar point cloud will be distorted [13]; that is, a series of discrete obstacle points obtained by scanning have a certain deviation from their real coordinates. Therefore, the effect of the lidar data registration algorithm will be limited.

At present, the classical laser radar data registration method is based on correspondence. The process is to search and construct correspondence, calculate the transformation matrix according to the correspondence, use the transformation matrix to perform rigid body transformation on the point set, construct the square error function, calculate the current error and iterate to find the optimal solution. In point-based algorithms, the most widely used point cloud registration algorithm is the traditional iterative closest point (ICP) algorithm proposed by Besl [14]. The algorithm searches for the corresponding points of each point according to the minimum Euclidean distance standard and forms point pairs. The error function is constructed according to the square sum of the distances between the point pairs to minimize it, thereby correcting the pose transformation value of the robot. The ICP algorithm [15] provides a classical framework for the registration of lidar data, but it is easy to fall into a local optimum when the initial positions of the point cloud are quite different. Aiming at addressing the shortcomings of the ICP algorithm, Dong [16] proposed a Lie group parameterization method based on the ICP algorithm. This method can provide robust registration for a low-overlapping point cloud [17], but this method is time-consuming. Reference [18] applied a structure similar to a K-D tree [19] to search the nearest point, which improved the speed of searching the nearest point greatly, but the algorithm cannot solve the registration problem of a partially overlapping point cloud. An iterative dual matching (IDC) algorithm [20] takes a point in the reference scan data as the reference point and searches the matching point of that point in the current data. This algorithm does not require the robot to be in a structured environment. However, due to the movement of the mobile robot, the point cloud coincidence rate becomes lower and the corresponding point distance becomes larger, so it is easy to cause mismatch between points. In a line-based registration algorithm, the line has better stability than the point, and it has better robustness. Iterative closest line (ICL) [21] extracts straight lines from point cloud data, and then searches the corresponding line segments to register. The method of searching the corresponding relationship is too complicated and unstable. Holy [22] constructed an error function according to the area included between the extracted lines, and then used the gradient descent method [23] to solve the corresponding coordinate transformation parameters, but this method has a poor effect in complex indoor environments.

Table 1. The merits and demerits of point cloud registration algorithms.

Point Cloud Registration Algorithm	Merits	Demerits
ICP algorithm	Point cloud registration with high coincidence rate has a better effect.	Point cloud registration with low coincidence rate is easy to fall into local optimal solution.
IDC algorithm	Point cloud registration does not require a structured environment.	It is easy to cause mismatch between points and points.
ICL algorithm	Point cloud registration suitable for low-texture environment.	The method of searching line correspondence is complex and unstable.

shows the merits and demerits of the point cloud registration algorithms mentioned above. It can be seen that the core of the above laser radar data registration algorithms depends on search correspondence, but the correspondence has also become a core factor limiting the performance of this type of algorithm, which is limited in the application of laser point cloud outliers and complex scenes. Therefore, this paper proposes a lidar data registration algorithm based on DBSCAN clustering. The algorithm can avoid directly searching the correspondence between points or lines, effectively improving the robustness of the algorithm and suppressing the influence of outliers on the algorithm.

The rest of this paper is organized as follows: Section 2 describes a lidar data registration algorithm based on DBSCAN clustering. Firstly, a ring band filter is designed to process the outliers with noise in the point cloud, and then the adaptive threshold is used to extract the line segment features in the laser radar point cloud. Using the DBSCAN density clustering algorithm, the key clusters of the rotation angle and translation matrix of the point cloud to be registered are obtained. In order to evaluate the similarity of the two frames of point cloud after data registration in the key clusters, a kernel density estimation method is proposed to describe the registered point cloud. The optimal value in the key cluster is found by combining K-L divergence. Section 3 tests the feasibility of the algorithm in the data set and the actual environment, and compares the data registration results with the ICP algorithm. Section 4summarizes the lidar data registration algorithm based on DBSCAN clustering.

2. Method of Solution

2.1. Radar Data Preprocessing

In the process of analyzing lidar data, it is found that since lidar data acquisition is a real-time process, the sensor will inevitably generate noise due to its applied environment and its own accuracy, resulting in some isolated points in the data. These noise points will introduce a negative impact on the robustness, accuracy and timeliness of the data registration algorithm. Before the registration of laser scanning data, it is necessary to denoise the current and reference scanning data: that is, to preprocess the data. The laser sensor data are highly discretized. When the target is far away from the laser sensor, the discretization error is also greater. Therefore, it is necessary to process the noise adaptively for data with a long distance to prevent interference algorithm results. In order to facilitate the search for noise, the polar coordinate data collected by each frame of lidar are converted to the Cartesian coordinate system. The measured distance is represented by $x_i$, $y_i$ relative to the Cartesian coordinate system of the sensor. The solution is:

$${\theta }_i=\left(\Delta \theta ·i-\alpha \right)·\frac{\pi }{180}$$

(1)

$$\left\{\begin{array}{c}{x}_i={\rho }_i\mathit{cos}{\theta }_i\\ y_i={\rho }_i\mathit{sin}{\theta }_i\end{array}\right.$$

(2)

In the formulae, $i$ is the laser beam; ${\theta }_i$ is the angle of the ith laser beam in the rectangular coordinate system; $\Delta \theta$ is the angular resolution of the lidar; $\alpha$ is the starting angle of lidar measurement; and ${\rho }_i$ is the distance from the reflection point to the emission point measured by the ith laser beam.

In order to reduce the influence of the above factors on feature extraction, it is necessary to preprocess radar data. The essence of radar data preprocessing is point cloud filtering [24]. Common point cloud filtering algorithms include median filtering and Gaussian filtering [25]. Median filtering has higher requirements for the selected points. If the selected coordinate points are not representative, the filtering accuracy will be affected. Gaussian filtering cannot preserve the corner features of the original point cloud well, and the edge point cloud will be smoothed greatly. In this paper, a ring band filter is designed. Firstly, abnormal values with noise in the laser point cloud are judged by a ring band filter. Then, the mean filter is used to reduce noise while retaining the corner features of the original point cloud.

As shown in Figure 1, some of the original data collected by the lidar are $P_1,P_2,P_3,P_4,P_5,P_6,P_7,P_8$ in turn. The ring band filter uses two bands. The inner ring used in this paper consists of three laser points $P_4,P_5,P_6$, and the outer ring is seven laser points $P_2,P_3,P_4,P_5,P_6,P_7,P_8$. In Figure 1a, the candidate laser point $P_5$ is the data point distorted by noise, which is distributed in the point cloud unevenly and should be replaced by the estimated laser point coordinate value. In Figure 1b, the candidate laser point $P_5$ is the corner point, and the corner point is the inflection point where two line segments intersect in the obstacle contour. It is a very important environmental feature for the segmentation of the point set and should be retained. The candidate laser point $P_5$ in Figure 1c is a noise point. However, due to the corner feature $P_6$ in its band, the estimated laser point coordinate value is quite different from the actual value, which affects the angle of the point cloud fitting line segment and should be filtered.

In Figure 1, slope $k_i$, obtained by making straight lines from point $P_1$ to each laser point in the ring, is:

$$k_i=\frac{y_i-y_1}{x_i-x_1}$$

(3)

In the formula, $\left(x_i,y_i\right)$ are the coordinates of the laser point i. In the ring to be denoised, the laser points which are judged to be outliers are defined as candidate laser points. The candidate laser points are centered using a ring band filter shown in the graph. The algorithm can be expressed as:

$$P_{center}\left(i\right)=\left\{\begin{array}{c}M,C_1<{\mathrm{T}}_1\cap C_2<{\mathrm{T}}_2\\ P_{before}\left(i\right),C_1<{\mathrm{T}}_1\cap C_2>{\mathrm{T}}_2\\ inf,C_1>{\mathrm{T}}_1\\ P_{before}\left(i\right),C_1=0\end{array}\right.$$

(4)

In the above formula, $P_{center}\left(i\right)$ is the coordinate value after the center candidate laser point is inferred; $P_{center}\left(i\right)$ is the coordinate value before the point is inferred; $C_1$, $C_2$ are the number of slope outliers in the inner and outer layers of the ring band filter; ${\mathrm{T}}_1$ is the threshold of the number of slope outliers in the inner layer; ${\mathrm{T}}_2$ is the threshold of the number of slope outliers in the outer layer; and $M$ is the mean value of the laser point coordinates in the inner layer. The outlier refers to the fact that one or several values in the data are quite different from other values, and the slope value representing a laser point in the band deviates from the slope value of other laser points.

Therefore, the coordinate value of the candidate laser points is set as the mean of the coordinates of the inner laser points only when the number of outliers in the inner annulus is less than the ${\mathrm{T}}_1$ threshold, and the number of outliers in the outer annulus is less than the ${\mathrm{T}}_2$ threshold. When the number of outliers in the inner ring is less than the ${\mathrm{T}}_2$ threshold, and the number of outliers in the outer ring is greater than the ${\mathrm{T}}_2$ threshold, the coordinate value of the candidate laser point is retained. When the number of outliers in the inner ring is greater than the ${\mathrm{T}}_1$ threshold, the slope in the laser point will produce multiple clusters. It is precisely because of the corner points in the ring that the estimation of the coordinate value of the candidate laser point will be interfered by other laser points. The laser point is removed when the line segment feature is maximized without affecting the line segment fitting angle. Figure 2 is the effect of preprocessing laser radar data. Region 1 and region 2 in Figure 2a are the data points distorted by noise, which are unevenly distributed in the point cloud and should be replaced by the estimated laser point coordinate value. Region 1 and region 2 in Figure 2b set their coordinate values as the mean value of the inner laser point coordinate to make the point cloud data smoother. Region 3 in Figure 2a is the corner feature of the intersection of two line segments in the obstacle contour. The corner feature of region 3 in Figure 2b is retained without changing the coordinate value. Region 4 in Figure 2a is a data point distorted by noise. However, due to the corner feature near the data point, in order to prevent the estimated laser point coordinates from being affected by other points to deviate from the true value, region 4 in Figure 2b is removed.

2.2. Laser Radar Data Registration

Before data registration of laser data, it is necessary to extract line segment features from the filtered point set to make it better in line with environmental characteristics. In this paper, previous research work is used to extract the line segment features: that is, adaptive-threshold laser radar scanning environment line segment feature extraction algorithm research [26]. The principle of the algorithm is to use the adaptive nearest-neighbor algorithm to detect the breakpoint, and segment the point set according to the adaptive-threshold segmentation point set.

The distance between two adjacent measurement points $d_i$ and the breakpoint detection threshold $D$ are compared. If $d_i<D$, the two adjacent measurement points are classified as the point set of the same obstacle. On the contrary, breakpoint i is extracted; that is, point i is the end point of the previous line, point i + 1 is the starting point of the latter line, and the point set of the original data is preliminarily segmented. The selected breakpoint detection threshold $D$ should be adaptively changed according to the distance measured by the laser, such as in Equation (5):

$$D=k·\Delta d_i=k·{\rho }_i·\Delta \theta ·\pi /180$$

(5)

$\Delta d_i$ is the distance between two adjacent points, and ${\rho }_i$ is the distance measured by the laser beam i. $\Delta \theta$ is the angular resolution of the lidar, and $k$ is the fixed value which needs to be determined by the lidar currently. When distance ${\rho }_i$ measured by the laser increases, distance $\Delta d_i$ of the adjacent two points also increases, and $k$ is the amplification coefficient of the current breakpoint detection threshold.

After completing the initial segmentation of the original point set, it is necessary to segment the point set of each part to find out the corner feature. The slope difference $\Delta k\left(i\right)$ calculation formula at the two adjacent laser data points is as follows:

$$\Delta k\left(i\right)=\frac{\left|{\rho }_i-{\rho }_{i-1}\right|}{{\rho }_{i-1}·\Delta \theta }-\frac{\left|{\rho }_{i+1}-{\rho }_i\right|}{{\rho }_i·\Delta \theta }$$

(6)

In the formula, $\Delta \theta$ is the angle resolution of the laser radar. Point $P_i$ is the corner of the two lines when $P_i$ is the intersection of line L1 and line L2. When $\left|\Delta k\left(i\right)\right|>d{k}_{th}$, $\left|\Delta k\left(i\right)\right|>\left|\Delta k\left(i-1\right)\right|$ and $\left|\Delta k\left(i\right)\right|>\left|\Delta k\left(i+1\right)\right|$, point i is the corner point: that is, the end point of the previous line and the starting point of the next line. $d{k}_{th}$ is the threshold for corner extraction. The algorithm evaluates the segmentation effect according to the fitting error of the segment segmentation point set until the most appropriate threshold $d{k}_{th}$ is found, so that the average fitting error of all segments is minimum, and the segmentation point set is output.

The formula for calculating the sum of the squared errors (SSE) in line segment fitting is shown in (7):

$$SSE=\sum _{i=1}^n{W}_i\left(y_i-y_i^{\prime }\right)$$

(7)

In the formula, SSE is the sum of squared errors of the corresponding points of predicted data and original data, $W_i$ is the weight, $y_i$ is the original data and $y_i^{\prime }$ is the predicted data. In order to reduce the amount of calculation, the point set to be fitted is divided into five sections. Then, the average value of the coordinates of each part of the point set is calculated as a new point. Finally, the five points are used to fit the line.

Figure 3 shows the simulation results of the line segment feature extraction algorithm on an Intel laboratory data set. The simulation results show that the adaptive nearest-neighbor algorithm in this paper has a good effect on breakpoint detection. All 22 breakpoints in this frame are extracted without wrong fitting, and the adaptive threshold segmentation algorithm accurately extracts the diagonal feature. Since the data were preprocessed before line segment segmentation, the data were smoother and no segmentation or undersegmentation occurred.

The lidar scanning data points can be properly registered, but it is difficult to search the corresponding relation of data points when encountering a similar environmental terrain, and the distance between corresponding data points increases with a longer laser scanning distance. After completing linear feature extraction of lidar data, a frame of lidar data can be represented by a set of straight-line segments, and the straight-line segments of two frames can be registered, which improves the accuracy of data registration.

In an unknown static environment, the association between data points will further deteriorate, and the classical laser data registration algorithm will be greatly affected. A mobile robot collects two frames of point cloud data at two adjacent locations by means of its self-carried lidar, which are recorded as sets $O_j=\left\{\left({\theta }_j,{\rho }_j\right)\left|j=1,\cdots N\right.\right\}$ and $R_i=\left\{\left({\theta }_i,{\rho }_i\right)\left|i=1,\cdots ,M\right.\right\}$, respectively. How to describe these two groups of sample points for correlation is the first problem to be considered. According to data sets $O_j$ and, the transformation relationship between two adjacent positions of the mobile robot is calculated, denoted as rotation angle $\phi$ and translation matrix $t$, respectively, where the translation matrix is $t={\left(t_x,t_y\right)}^{\mathrm{T}}$.

The registration problem of 2D lidar data can be expressed as: to find an optimal translation matrix t and rotation angle $\phi$, so that dataset $O_j$ can align dataset $R_i$:

$$\left\{\begin{array}{c}{x}_i^R={\rho }_i^{R}cos{\theta }_i^R={\rho }_j^{O}cos\left({\theta }_j^O+\phi \right)+t_x\\ y_i^R={\rho }_i^{R}sin{\theta }_i^R={\rho }_j^{O}sin\left({\theta }_j^O+\phi \right)+t_y\end{array}\right.$$

(8)

Data point $j$ in data set $O_j$ corresponds to data point $i$ in data set $R_i$. This section simulates two sets of 180° laser data with open point cloud data from Intel Lab. Point set $R_i$ is obtained from point set $O_j$ after transformation (−1.5, 0.8, 0.3), as shown in Figure 4. The blue solid point is the data point in set $O_j$, and the red circle points are the data points in set $R_i$.

2.2.1. Estimation of Rotation Angle

Figure 5 shows the linear feature extraction effect diagram of data sets $O_j$ and $R_i$, respectively, represented by a group of line segments. The blue line segment is the pseudo-synthesized environmental feature of data set $O_j$, and the red line segment is the pseudo-synthesized environmental feature of data set $R_i$.

Make a difference between any two angles of two groups of line segments:

$$\Delta {\theta }_i={\theta }_k^R-{\theta }_p^O$$

(9)

In the formula, ${\theta }_k^R$ is the angle of straight line $k$ in dataset $R_i$, and ${\theta }_p^O$ is the angle of straight line $p$ in dataset $O_j$. All one-dimensional angle differences $\Delta {\theta }_i$ are converted into two-dimensional variables $\left(\Delta {\theta }_i,\Delta {\theta }_i\right)$, and a DBSCAN algorithm [27] based on density clustering is adopted for clustering. The specific steps of the DBSCAN algorithm are as follows:

(1)

Determine the distance threshold $\sigma$ of the sample field and the threshold $MinPts$ of the number of samples in the neighborhood of distance $\sigma$. Check whether the current $\left|N_p\left(\Delta {\theta }_i\right)\right|\ge MinPts$ is established or not. If yes, $\Delta {\theta }_i$ is the core object and is added to the core object collection $B$, where $N_p\left(\Delta {\theta }_i\right)$ is the set of points in a circular region with $\left(\Delta {\theta }_i,\Delta {\theta }_i\right)$ as the center and $\sigma$ as the radius.

(2)

Randomly select a core object $\Delta {\theta }_i$ from set $B$, look for all points reachable by its density, add it to a new set $C_1$, form the first cluster and remove the core object contained in $C_1$ from $B$.

(3)

Repeat the above process until all the points in the dataset are processed and $m$ clusters $C_m$ are obtained.

Figure 6 shows the distribution of each cluster after DBSCAN density clustering of all samples. It can be seen that the distribution of key clusters clustered in the black box is relatively concentrated, and the enlarged results are mainly concentrated around 0.3. The cluster with the largest number of sampled elements is taken as the key cluster. Finally, the weighted average of all samples of the key cluster can be obtained, and the rotation angle $\phi$ of data sets $O_j$ and $R_i$ is 0.3.

Because the matrix is a two-dimensional variable, for the selection of DBSCAN clustering algorithm parameters, this paper adopts the idea of a $k-dist$ graph proposed in reference [28]. Calculate the distance from each point to the point nearest to its $k$, and sort these distances from large to small for drawing. The distance to find the inflection point is the value of $\sigma$, and the value of $MinPts$ is $k+1$. After testing, it is found that when the parameters of the DBSCAN algorithm are set as $MinPts=1,\sigma =0.08 \mathrm{m}$, ideal results can be obtained. Figure 7 shows the distribution of all data points in data set $O_j$ after rotation angle $\phi$ and all line segments in $R_i$.

2.2.2. Estimation of the Translation Matrix

The rotation angle $\phi$ of data sets $O_j$ to $R_i$ is obtained above, and the rotation angle $\phi$ of all data points of data set $O_j$ is obtained. The new data set $O_j^{\prime }$ is:

$$\left\{\begin{array}{c}{x}_j^{O\prime }={\rho }_j^{O}cos{\theta }_j^O={\rho }_j^{O}cos\left({\theta }_j^O+\phi \right)\\ y_j^{O\prime }={\rho }_j^{O}sin{\theta }_j^O={\rho }_j^{O}sin\left({\theta }_j^O+\phi \right)\end{array}\right.$$

(10)

Then, data sets $O_j^{\prime }$ and $R_i$ can be represented by a group of straight-line segments. The straight-line segments in the two data sets have approximately parallel parts:

$$\left\{\begin{array}{c}{O}_j^{\prime }=\left\{O^{p\prime }\left|p=1,2\cdots \right.\right\}\\ R_i=\left\{R^k\left|k=1,2\cdots \right.\right\}\end{array}\right.$$

(11)

In the formulae, $p$ is the number of line segments in data set $O_j^{\prime }$, and $k$ is the number of line segments in data set $R_i$. For the straight-line segment $O^{p\prime }$ in data set $O_j^{\prime }$ and the straight-line segment $R^k$ in data set $R_i$, if Equation (12) can be satisfied, it means that the two straight lines in the two data sets are approximately parallel. They are assigned as a straight-line segment pair $L$, and the average angle of the two approximately parallel lines is taken as the direction angle ${\theta }_{p,k}$ of the straight-line segment pair.

$$\Delta {\theta }_{p,k}=\left|O^{p^{,}}-R^k\right|<{\epsilon }_1$$

(12)

In the formula, $\Delta {\theta }_{p,k}$ is the angle difference between the two line segments, and ${\epsilon }_1$ is the threshold for judging the approximate parallel of the two line segments, so as to remove the angle outliers that appear when feature extraction fits the line segments. When the threshold of ${\epsilon }_1$ is too small, the matching degree of nearly parallel-line segments of the two data sets is high, but the number of successfully matched line segment pairs $L$ is less, which is not conducive to subsequent clustering. If the threshold of ${\epsilon }_1$ is too large, the line segments in the two data sets will match incorrectly, resulting in errors in subsequent calculations. Therefore, we should adjust the value of ${\epsilon }_1$ and control the number of selected line segment pairs $L$ within the range of the total number of line segments in data set $O_j^{\prime }$. In this case, the probability of mismatching is small, and a certain amount of data is obtained.

For the line segment pair $L$, if the direction angle of any two line segment pairs can satisfy Equation (13), it can be regarded as a line segment pair equation $L_{op}$:

$$\left|{\theta }_{p,k}{}^{L_q}-{\theta }_{p,k}{}^{L_t}\right|>{\epsilon }_2$$

(13)

In the formula, $L_qϵL$, $L_tϵL$, ${\epsilon }_2$ is the threshold for judging how non-parallel two line segment pairs are, and all line segment pairs $L$ are screened to form the line segment pair equation $L_{op}$.

The translation matrix is in the form $t={\left(t_x,t_y\right)}^T$. According to Equation (8), the two parallel lines in the line segment pair $L$ can be set as $y=kx+b$ and $y=k\left(x-t_x\right)+b+t_y$. Therefore, the relationship between the distance of the two parallel lines and the distance from a point in the line segment to another line segment can be obtained, as shown in Equation (14):

$$\frac{\left|-k{t}_x+t_y\right|}{\sqrt{k^2+1}}=\frac{\left|k{x}_0-y_0+b\right|}{\sqrt{k^2+1}}$$

(14)

where $\left(x_0,y_0\right)$ is a point in the straight-line segment. Due to error in the accuracy of the fit of the line, this experiment selects the midpoint of the straight line segment; a simplification can be obtained as follows.

$${\left(-k{t}_x+t_y\right)}^2={\left(k{x}_0-y_0+b\right)}^2$$

(15)

A line segment pair $L$ can obtain an equation as shown in (15), where the unknown quantity is $t={\left(t_x,t_y\right)}^T$. Therefore, two non-parallel line segment pairs $L$ are required to solve the equation: that is, the line segment pair equation $L_{op}$. Each line segment pair equation $L_{op}$ can solve a translation matrix $t={\left(t_x,t_y\right)}^T$.

After solving all the line segment pair equations $L_{op}$, this paper uses the DBSCAN algorithm to cluster all the translation matrices as samples, and then samples the cluster with the largest number of samples as the key cluster. Because the matrix is a two-dimensional variable, the test found that when the parameter of the DBSACN algorithm is set to $MinPts=1$, $\sigma =0.05 \mathrm{m}$, the ideal result can be obtained; that is, two-dimensional variables with similar values in both dimensions are gathered together. Figure 8 shows the distribution diagram of each cluster after DBSCAN density clustering of all the translation matrix $t$ samples. The clusters of density clustering are mainly concentrated in region 1 and region 2 areas. This is because Equation (15) is an equation with squares, and the translation matrix $t$ will be two opposite clusters. Then, it is impossible to determine which key cluster to use. The samples in the key clusters shown in the enlarged image of region 1 and region 2 are scattered. Because of outlier interference in the extraction of line segment features and the estimated rotation matrix is not the true value, the samples of the translation matrix $t$ key clusters cannot be simply weighted averages. In order to evaluate the similarity of the two frames of point cloud after data registration in the key clusters, this paper proposes a kernel density estimation method, which brings the sample size in the key clusters into the probability density function. The optimal solution of the translation matrix $t$ is found by combining the K-L divergence.

2.3. Kernel Density Estimation and K-L Divergence

There are many methods to analyze and describe sample points. The probability density function (PDF) [29] is used as a means to describe sample points. This paper proposes to use kernel density estimation (KDE) to obtain the probability density function of a laser point cloud. As one of the classical methods of nonparametric density estimation, the kernel density estimation method plays an extremely important role in big data processing. When the probability distribution of an event is unknown, the density function is estimated using the observed data. In addition, the distance between data will also have different effects, so it is believed that the data with a relatively close distance will have a greater impact on each other, while the data with a relatively long distance will have a smaller impact.

The kernel density estimation method can estimate the density of arbitrarily distributed sample points without the prior knowledge of sample distribution. It is suitable for analyzing the laser scanning data obtained by mobile robots in any environment. Let random variables $X_1,X_2,\cdots ,X_n$ be independent identically distributed samples extracted from the population, and their density function is $f\left(x\right)$; then the kernel density estimator $\widehat{f}\left(x\right)$ is:

$$\widehat{f}\left(x\right)=\frac{1}{nh}\sum _{i=1}^{n}K(\frac{x-X_i}{h})$$

(16)

In the formula, $n$ is the sample size, $\mathrm{h}$ is the bandwidth and $K\left(x\right)$ is the kernel function. As the core of the kernel density estimation method, its kernel function should meet the following conditions: (1) non-negative: $K\left(x\right)\ge 0$; (2) symmetry: $K\left(x\right)$ = $K\left(-x\right)$; (3) normality: ${\int }_R^{ }K\left(x\right)dx = 1$. For the kernel density estimator $\widehat{f}\left(x\right)$, through observation, it is found that this method mainly emphasizes that the smaller the absolute value of random variable $X_i$ and variable $x$, the smaller the distance between them, and the greater the influence of random variable $X_i$ on the density function value at point $x$. In addition, the kernel density estimator only depends on the sample data, bandwidth and kernel function, and does not require whether the sample data meets the specific model or rule. Bring data set $O_j$ into the samples in the key clusters of the rotation matrix and the translation matrix, and obtain data set $O_j^{″}$ and data set $R_i$, respectively, and carry out the kernel density estimation:

$$\left\{\begin{array}{c}{P}_{O^{″}}\left(x\right)=\frac{1}{nh}{\displaystyle \sum _{i=1}^n}K(\frac{x-O_j^{″}}{h})\\ P_R\left(x\right)=\frac{1}{nh}{\displaystyle \sum _{i=1}^n}K(\frac{x-R_i}{h})\end{array}\right.$$

(17)

For the kernel density estimator, as long as its kernel function and bandwidth are properly selected, the kernel density estimation method can approach the real density function with arbitrary accuracy. The commonly used kernel functions that meet the above properties mainly include the Gaussian kernel function, the Epanechnikov kernel function and the Triangle kernel function. For the convenience of calculation, the Gaussian kernel function is selected, and its mathematical expression is:

$$K\left(x\right)=\frac{1}{\sqrt{2\pi }}{e}^{-\frac{x^2}{2}}$$

(18)

The Gaussian kernel function is substituted into the kernel density estimation of the data set to obtain the probability density function of the sample points. Figure 9 shows the xy-axis sample data distribution and probability density function of the data sets $O_j$ and $R_i$. Since data set $R_i$ is obtained by $O_j$ through transformation (−1.5, 0.8, 0.3), the x-axis coordinate distribution of the two data sets in Figure 9a is quite different. The distance between the corresponding laser points in the first half of the data set is large, and the distance between the corresponding laser points in the second half is small, resulting in different probability density function waveforms. The x-axis coordinates of the data set $R_i$ are more concentrated in the 0–1 interval, and the probability exceeds 0.3, while data set $O_j$ is more concentrated in the 2–3 interval, and the probability does not exceed 0.3. The y-axis coordinates of the two data sets in Figure 9b are more compact, but the distance at the laser point in the first half is small, and the distance at the laser point in the second half is large. From the waveform of the probability density function, the y-axis coordinates of data set $R_i$ are more concentrated in the 3–4 interval, and the probability does not exceed 0.2, and data set $O_j$ is more concentrated in the 1–2 interval, and the probability exceeds 0.2.

Figure 10 shows the xy-axis sample data distribution and probability density function of data sets $O_j^{\prime }$ and $R_i$. Data set $O_j^{\prime }$ is obtained from the angle estimated by the rotation of $O_j$. There is a certain distance between the x-axis coordinate distribution of the two data sets in Figure 10a, and the distance at the corresponding laser point is equal. The probability density function waveforms of the two are the same, but the x-axis coordinates of data set $R_i$ are more concentrated in the 0–1 interval, while data set $O_j^{\prime }$ is more concentrated in the 2–3 interval. The y-axis coordinates of the two data sets in Figure 10b are more compact, and the distances at the corresponding laser points are equal. From the waveform of the probability density function, the y-axis coordinates of the two data sets are concentrated in the interval 2–4.

For the two key clusters using DBSCAN to solve the translation matrix clustering, data set $O_j^{\prime }$ is first transformed by translation matrix $t$ of region 1 in Figure 8 to obtain data set $O_j^{″}$. As shown in Figure 11, the xy-axis sample data distribution and probability density function of data sets $O_j^{″}$ and $R_i$ can be seen. The coordinate values of the corresponding points of data sets $O_j^{″}$ and $R_i$ in the figure are very different. Since Equation (15) is a squared equation, the clustered translation matrix $t$ will be two opposite clusters. Therefore, translation matrix $t$ of the key cluster in region 1 in Figure 8 is incorrect, and it is opposite to the real value symbol of translation matrix $t$.

Data set $O_j^{\prime }$ is transformed by translation matrix $t$ of region 2 in Figure 8 to obtain data set $O_j^{″}$. Figure 12 shows the xy-axis sample data distribution and probability density function of data sets $O_j^{″}$ and $R_i$. It can be seen that the xy-axis of data sets $O_j^{″}$ and $R_i$ basically overlap together, indicating that translation matrix $t$ of the key cluster in region 2 is close to the real value, but the different values in the key cluster have an impact on the estimation results. In Figure 12a, when the value of $t_x$ is −1.500567, the x-axis coordinates of the 60th laser point of data sets $O_j^{″}$ and $R_i$ are different by 0.0006 m. When the value of $t_x$ is −1.506931, the x-axis coordinates of the 60th laser point of data sets $O_j^{″}$ and $R_i$ are different by 0.0058 m. In Figure 12b, when the value of $t_y$ is 0.800261, the x-axis coordinate difference of the 60th laser point of data sets $O_j^{″}$ and $R_i$ is 0.00025 m, and when the value of $t_y$ is 0.811266, the x-axis coordinate difference of the 60th laser point of data sets $O_j^{″}$ and $R_i$ is 0.0112 m.

To address the problem that different values in the key cluster of translation matrix $t$ have an influence on the estimation results, this paper uses K-L divergence to evaluate the estimation effect of translation matrix $t$. K-L divergence, also known as relative entropy, can be used to measure the degree of difference between two signals and is an index to quantify the similarity between two probability distributions. Let the two probability distributions be $p\left(x\right)$ and $q\left(x\right)$, respectively, and then the K-L distance ${\delta }_{K-L}\left(p,q\right)$ from $q$ to $p$ is defined as:

$${\delta }_{K-L}\left(p,q\right)=\sum _{i=1}^{N}p\left(x_i\right)·log\frac{p\left(x_i\right)}{q\left(x_i\right)}$$

(19)

It can also be obtained that the K-L distance ${\delta }_{K-L}\left(q,p\right)$ of $p$ relative to $q$ is:

$${\delta }_{K-L}\left(q,p\right)=\sum _{i=1}^{N}q\left(x_i\right)·log\frac{q\left(x_i\right)}{p\left(x_i\right)}$$

(20)

Finally, the K-L divergence value $D_{K-L}\left(p,q\right)$ can be obtained by calculating the K-L distance ${\delta }_{K-L}\left(p,q\right)$ and ${\delta }_{K-L}\left(q,p\right)$ of the two probability distributions:

$$D_{K-L}\left(p,q\right) = {\delta }_{K-L}\left(p,q\right) + {\delta }_{K-L}\left(q,p\right)$$

(21)

K-L divergence is used to measure the difference between two probability distributions, and its physical significance is a measure of the angle between them. Therefore, a large K-L divergence value indicates a large difference between two probability distributions. The smaller the K-L divergence value, the smaller the difference between the two probability distributions; that is, the higher the similarity. When the two probability distributions are exactly the same, the value is 0. Therefore, the probability density function of the data in the key cluster of region 2 in Figure 8 is calculated, and the K-L divergence value is calculated with the probability density function of data set $R_i$. The data with the minimum K-L divergence value are the optimal solution of the true value of the translation matrix $t$; that is, the transformation is (−1.50057, 0.80026, 0.3). However, the true value is (−1.5, 0.8, 0.3). It can be seen that the lidar data registration algorithm proposed in this paper based on DBSCAN clustering has high accuracy in estimating the rotation angle and translation matrix of the laser point cloud of two frames, which is basically close to the true value.

2.4. Description of the Algorithm

The lidar returns a set of ordered two-dimensional lidar data once every scanning environment. The collected two frames of point cloud data are recorded as sets $O_j=\left\{\left({\theta }_j,{\rho }_j\right)\left|j=1,\cdots N\right.\right\}$ and $R_i=\left\{\left({\theta }_i,{\rho }_i\right)\left|i=1,\cdots M\right.\right\}$, respectively. A detailed description of the algorithm is shown in Figure 13.

3. Discussion and Results

3.1. Open-Source Dataset Simulation

In order to verify the performance of the lidar data registration algorithm based on DBSCAN clustering in this paper, the algorithm is used to register two-dimensional lidar data in the MATLAB simulation environment. The open point cloud data of the Intel Lab provided by Dirk Hahnel [30] are selected in the experiment. The laser radar scanning angle used in this data set is 180$°$, and the angular resolution is 1$°$. In this experiment, two consecutive frames of laser information in the data set are selected, and the experimental results are shown in Figure 14. In Figure 14a, the line segment features of two frames of laser point cloud $O_j$ and point cloud $R_i$ are selected. The environment with multiple breakpoints and multiple corners is selected, and the line segment features are extracted. Due to the change of the position of the lidar in the map, the position of the corresponding line segment also changes. In the figure, region 1 is the newly emerged point cloud after the position transformation of laser point cloud $O_j$, while region 2 is the missing point cloud of laser point cloud $O_j$. However, since the initial point cloud has been rotated and translated, the two frames of point cloud can be approximately coincident by the data registration algorithm to obtain the lidar rotation angle and translation matrix. Figure 14b shows the point cloud line segment features after data registration of the algorithm in this paper. In the figure, except for the new and missing line segments of the position transformation, the rest of the line segments coincide with each other through estimated rotation angle and translation matrix transformation. It can be seen that the estimation of rotation angle and shift matrix of the lidar data registration algorithm proposed in this paper based on DBSCAN clustering is basically close to the real odometer results, which can be effectively used in complex environments with multiple breakpoints and multiple corners.

Figure 15 is a local enlarged view of region 3 in Figure 14. Figure 15a is the distribution of the line segments of the laser point cloud after rotation. It can be seen that the line segments of the two frames of the laser point cloud are approximately parallel. The experimental rotation angle obtained is 0.0737, and the actual rotation angle in the open-source data set is 0.0676. Figure 15b is the distribution of the line segments of the laser point cloud after rotation and translation. It can be seen that the line segments of the two frames of the laser point cloud are approximately coincident. The translation matrix obtained from the experimental results is (−0.00644, −0.003122), and the actual translation matrix in the open-source data set is (−0.001, −0.001).

Figure 16 selects two frames of point cloud with noise from Intel LABS open point cloud data. Figure 16a includes the mobile robot scanning the environmental laser point cloud $O_j$ at a certain time, and the laser point cloud $R_i$ obtained after moving forward and rotating the car body. The laser point cloud $O_j$ contains more environmental noise and the environmental angle scanned by lidar is larger, while the laser points in the laser point cloud $R_i$ are relatively dense and the environmental noise is less. Figure 16b shows the registration result of lidar data obtained by the ICP algorithm. It can be seen that the two frames of the point cloud do not coincide, mainly because the data registration carried out by the ICP algorithm is not global optimal matching, but is local optimal matching. The experimental rotation angle is 0.4904 and the translation matrix is (0.5071, −0.0122), but the actual experimental rotation angle is 0.08 and the actual translation matrix is (−0.9523, −1.1598). Figure 16c shows the line segment feature extraction results of two frames of the laser point cloud. For the case of environmental noise in the point cloud, the main line segment in the point cloud is fitted. Figure 16d shows the registration results of lidar data using the algorithm in this paper, and the two frames of laser point cloud almost coincide. The experimental rotation angle is 0.0876 and the translation matrix is (−0.9842, −1.1413), which is close to the actual value. The experiment shows that for the ICP algorithm, it is better to have one-to-one correspondence between points. When there are partial overlaps or outliers, the point sets cannot correspond well, resulting in matching errors. However, the algorithm in this paper can extract the main line segment in the point cloud for lidar data registration to avoid mismatching caused by too many outliers or missing environmental information.

3.2. Real Environment Simulation

Simulation in a real environment adopts the LMS511 Lidar of SICK to collect environmental data. The specific parameters of the radar are shown in

Table 2. SICK LMS511 parameters.

Parameter	Value
Measurement range	0.7–80 m
Scanning angle	190°
Angular resolution	0.5°
Scanning frequency	25 Hz
System error	±25 ms (1–10 m)

.

In this study, the lidar sets the starting angle of measurement as −50° and the ending angle as 140° through SOPAS software, and a total of 381 groups of data are collected. This study uses the MATLAB 2016b simulation environment for algorithm simulation, and the computer CPU is Intel Core i5-6300U.

Figure 17a shows the corridor environment with flat ground. In the figure, the tracked robot equipped with lidar constantly scans the terrain. In the process of robot moving, the lidar scanning data with time stamp information and the tracked odometer data are obtained. The time stamps of two frames of lidar scanning data are mainly used. The tracked odometer whose time stamps are close to each other is used as time-weighted linear interpolation to obtain the time-stamp-synchronous lidar data and odometer. Figure 17b shows the conversion of polar coordinates to cartesian coordinates of two frames of lidar scanning. It can be seen that the corner points in the figure correspond to the turning part of the wall in Figure 17a, and the two frames of the point cloud cross but do not coincide. Figure 17c shows the data registration results after extracting the features of the point cloud line segments in Figure 17b. The line segments of the two frames of lidar approximately coincide, and the point cloud $R_i$ in region 1 has more line segments than the point cloud $O_j$, which is due to the extra environmental features of the vehicle body after counterclockwise rotation. The rotation angle obtained in the experiment is 0.0639, and the translation matrix is (0.0155, −0.0265). The actual rotation angle of the track odometer is 0.0583, and the translation matrix is (0.0124, −0.0285). Experiments show that the lidar data registration algorithm based on DBSCAN clustering in this paper can accurately estimate the rotation angle and migration matrix of two frames of the laser point cloud, meeting the accuracy requirements of the laser odometer.

In the experiment, four groups of laser point cloud frames were extracted from the Dirk Hahnel Intel laboratory data set and the real environment for data registration. Most point cloud registration algorithms use mean square error as a metric of the lidar data registration effect, and calculate the mean square error of the distance between the current measurement point and the real corresponding point. However, in the actual scene, due to occlusion, movement and low coincidence rate, not all measurement points have one-to-one corresponding real points. Therefore, the lower the data coincidence rate, the more unreliable the evaluation standard of mean square error. In the case of large changes in lidar pose, not all search methods can find accurate real corresponding points, and there may be a large number of mismatched point pairs.

In order to evaluate the effect of data registration more effectively, this paper introduces absolute error $E_A\left(t_x,t_y,\theta \right)$ and relative error $E_R\left(t_x,t_y,\theta \right)$ as the main measurement standards. Absolute error $E_A\left(t_x,t_y,\theta \right)$ can directly reflect the size of the error, but it cannot accurately reflect the accuracy of the registration algorithm. The relative error $E_R\left(t_x,t_y,\theta \right)$ here can not reflect the size of the error, but can more accurately reflect the accuracy of the registration algorithm. Therefore, this paper uses these two errors at the same time to make use of the advantages of the two to evaluate the registration effect of the data registration algorithm more directly and accurately.

Table 3. Comparison of lidar data registration results.

Number	$\left({\mathit{t}}_{\mathit{x}},{\mathit{t}}_{\mathit{y}},\mathit{\theta }\right)$	${\mathit{E}}_{\mathit{A}}\left({\mathit{t}}_{\mathit{x}},{\mathit{t}}_{\mathit{y}},\mathit{\theta }\right)$		${\mathit{E}}_{\mathit{R}}\left({\mathit{t}}_{\mathit{x}},{\mathit{t}}_{\mathit{y}},\mathit{\theta }\right)$
		ICP Algorithm	Proposed Algorithm	ICP Algorithm	Proposed Algorithm
1	(−0.9523, −1.1598,0.08)	(1.4594, 1.1476, 0.4104)	(0.0319, 0.0185, 0.0076)	(153.3%, 98.9%, 513%)	(3.3%, 1.6%, 9.5%)
2	(0.0124, −0.0285,0.0583)	(0.0262, 0.0464, 0.0171)	(0.0031, 0.002, 0.0056)	(211.3%, 162.8%, 29.3%)	(25.0%, 7.0%, 9.6%)
3	(0.4389, −0.2463,0.0475)	(0.3055, 0.1594, 0.0249)	(0.0232, 0.0065, 0.0027)	(69.6%, 64.7%, 52.4%)	(5.3%, 2.6%, 5.7%)
4	(−0.068, 0.3961, 0.0579)	(0.0645, 0.2625, 0.1284)	(0.0011, 0.007, 0.0038)	(94.9%, 66.3%, 221.8%)	(1.6%, 1.8%, 6.6%)

compares the rotation angle and translation matrix estimated by the proposed algorithm with the traditional ICP algorithm. Among them, $\left(t_x,t_y,\theta \right)$ is the actual value of translation and rotation, $E_A\left(t_x,t_y,\theta \right)$ is the absolute error measured by the algorithm and $E_R\left(t_x,t_y,\theta \right)$ is the relative error measured by the algorithm. The results show that the proposed algorithm can obtain accurate registration results compared with the ICP algorithm. The relative error between the registration results and the actual values is within 10%, and the accuracy is better than the ICP algorithm. The absolute error and relative error of the registration results obtained by the ICP algorithm are very large. This is due to the fact that a new part of the environment is prone to appear in the point cloud, coupled with the influence of outliers and noises. At this time, it is difficult for the ICP algorithm to find a matching relationship between points. The algorithm in this paper can still extract the main line segment features in the environment for point cloud data registration, avoiding the problem of low coincidence rate of point cloud matching due to the emergence of a new part of the environment.

4. Conclusions

The core of lidar data registration algorithms depends on searching correspondence. For a point-based algorithm, the data coincidence rate is too low, while for a line-based algorithm, the method of searching correspondence is too complex and unstable. Therefore, this paper proposes a lidar data registration algorithm based on DBSCAN clustering.

(1)

In the radar data preprocessing stage, a ring band filter is designed. According to the ring band filter, the outliers with noise in the laser point cloud are judged, retained or removed, or the coordinates of the laser points to be denoised are estimated using the surrounding laser point coordinates.

(2)

In the radar data registration stage, for the environmental line segment features required for registration, the adaptive nearest-neighbor algorithm is used to detect the breakpoints. The point set is segmented and fitted according to the adaptive threshold segmentation point set. Then, the fitted two frames of laser data sets are clustered by a DBSCAN algorithm based on density clustering. Thus, the key clusters of rotation angle and translation matrix of point cloud to be registered are obtained.

(3)

In the key cluster data selection stage, in order to evaluate the similarity of two frames of point cloud after data registration in the key cluster, a kernel density estimation method is proposed to describe the registered point cloud, and K-L divergence is used to find the optimal value in the key cluster.

The experimental results show that the laser radar data registration algorithm based on DBSCAN clustering proposed in this paper avoids the direct search of the correspondence between points or lines in the complex scene with many outliers of the laser point cloud, which can effectively improve the robustness of the algorithm and suppress the influence of outliers on the algorithm. The relative error between the registration result and the actual value is within 10%, and the accuracy is better than the ICP algorithm, which realizes the accurate self-localization of the mobile robot. On this basis, further improving the calculation speed of the algorithm and processing high-complexity 3D laser point clouds are the main topics to be studied in the future.