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
,
relative to the Cartesian coordinate system of the sensor. The solution is:
In the formulae, is the laser beam; is the angle of the ith laser beam in the rectangular coordinate system; is the angular resolution of the lidar; is the starting angle of lidar measurement; and 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
in turn. The ring band filter uses two bands. The inner ring used in this paper consists of three laser points
, and the outer ring is seven laser points
. In
Figure 1a, the candidate laser point
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
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
in
Figure 1c is a noise point. However, due to the corner feature
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
, obtained by making straight lines from point
to each laser point in the ring, is:
In the formula,
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:
In the above formula, is the coordinate value after the center candidate laser point is inferred; is the coordinate value before the point is inferred; , are the number of slope outliers in the inner and outer layers of the ring band filter; is the threshold of the number of slope outliers in the inner layer; is the threshold of the number of slope outliers in the outer layer; and 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
threshold, and the number of outliers in the outer annulus is less than the
threshold. When the number of outliers in the inner ring is less than the
threshold, and the number of outliers in the outer ring is greater than the
threshold, the coordinate value of the candidate laser point is retained. When the number of outliers in the inner ring is greater than the
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
and the breakpoint detection threshold
are compared. If
, 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
should be adaptively changed according to the distance measured by the laser, such as in Equation (5):
is the distance between two adjacent points, and is the distance measured by the laser beam i. is the angular resolution of the lidar, and is the fixed value which needs to be determined by the lidar currently. When distance measured by the laser increases, distance of the adjacent two points also increases, and 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
calculation formula at the two adjacent laser data points is as follows:
In the formula, is the angle resolution of the laser radar. Point is the corner of the two lines when is the intersection of line L1 and line L2. When , and , point i is the corner point: that is, the end point of the previous line and the starting point of the next line. 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 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):
In the formula, SSE is the sum of squared errors of the corresponding points of predicted data and original data, is the weight, is the original data and 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 and , respectively. How to describe these two groups of sample points for correlation is the first problem to be considered. According to data sets and, the transformation relationship between two adjacent positions of the mobile robot is calculated, denoted as rotation angle and translation matrix , respectively, where the translation matrix is .
The registration problem of 2D lidar data can be expressed as: to find an optimal translation matrix
t and rotation angle
, so that dataset
can align dataset
:
Data point
in data set
corresponds to data point
in data set
. This section simulates two sets of 180° laser data with open point cloud data from Intel Lab. Point set
is obtained from point set
after transformation (−1.5, 0.8, 0.3), as shown in
Figure 4. The blue solid point is the data point in set
, and the red circle points are the data points in set
.
2.2.1. Estimation of Rotation Angle
Figure 5 shows the linear feature extraction effect diagram of data sets
and
, respectively, represented by a group of line segments. The blue line segment is the pseudo-synthesized environmental feature of data set
, and the red line segment is the pseudo-synthesized environmental feature of data set
.
Make a difference between any two angles of two groups of line segments:
In the formula,
is the angle of straight line
in dataset
, and
is the angle of straight line
in dataset
. All one-dimensional angle differences
are converted into two-dimensional variables
, 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 of the sample field and the threshold of the number of samples in the neighborhood of distance . Check whether the current is established or not. If yes, is the core object and is added to the core object collection , where is the set of points in a circular region with as the center and as the radius.
- (2)
Randomly select a core object from set , look for all points reachable by its density, add it to a new set , form the first cluster and remove the core object contained in from .
- (3)
Repeat the above process until all the points in the dataset are processed and clusters 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
of data sets
and
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
graph proposed in reference [
28]. Calculate the distance from each point to the point nearest to its
, and sort these distances from large to small for drawing. The distance to find the inflection point is the value of
, and the value of
is
. After testing, it is found that when the parameters of the DBSCAN algorithm are set as
, ideal results can be obtained.
Figure 7 shows the distribution of all data points in data set
after rotation angle
and all line segments in
.
2.2.2. Estimation of the Translation Matrix
The rotation angle
of data sets
to
is obtained above, and the rotation angle
of all data points of data set
is obtained. The new data set
is:
Then, data sets
and
can be represented by a group of straight-line segments. The straight-line segments in the two data sets have approximately parallel parts:
In the formulae,
is the number of line segments in data set
, and
is the number of line segments in data set
. For the straight-line segment
in data set
and the straight-line segment
in data set
, 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
, and the average angle of the two approximately parallel lines is taken as the direction angle
of the straight-line segment pair.
In the formula, is the angle difference between the two line segments, and 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 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 is less, which is not conducive to subsequent clustering. If the threshold of 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 and control the number of selected line segment pairs within the range of the total number of line segments in data set . In this case, the probability of mismatching is small, and a certain amount of data is obtained.
For the line segment pair
, if the direction angle of any two line segment pairs can satisfy Equation (13), it can be regarded as a line segment pair equation
:
In the formula, , , is the threshold for judging how non-parallel two line segment pairs are, and all line segment pairs are screened to form the line segment pair equation .
The translation matrix is in the form
. According to Equation (8), the two parallel lines in the line segment pair
can be set as
and
. 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):
where
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.
A line segment pair can obtain an equation as shown in (15), where the unknown quantity is . Therefore, two non-parallel line segment pairs are required to solve the equation: that is, the line segment pair equation . Each line segment pair equation can solve a translation matrix .
After solving all the line segment pair equations
, 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
,
, 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
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
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
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
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
be independent identically distributed samples extracted from the population, and their density function is
; then the kernel density estimator
is:
In the formula,
is the sample size,
is the bandwidth and
is the kernel function. As the core of the kernel density estimation method, its kernel function should meet the following conditions: (1) non-negative:
; (2) symmetry:
=
; (3) normality:
. For the kernel density estimator
, through observation, it is found that this method mainly emphasizes that the smaller the absolute value of random variable
and variable
, the smaller the distance between them, and the greater the influence of random variable
on the density function value at point
. 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
into the samples in the key clusters of the rotation matrix and the translation matrix, and obtain data set
and data set
, respectively, and carry out the kernel density estimation:
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:
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
and
. Since data set
is obtained by
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
are more concentrated in the 0–1 interval, and the probability exceeds 0.3, while data set
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
are more concentrated in the 3–4 interval, and the probability does not exceed 0.2, and data set
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
and
. Data set
is obtained from the angle estimated by the rotation of
. 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
are more concentrated in the 0–1 interval, while data set
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
is first transformed by translation matrix
of region 1 in
Figure 8 to obtain data set
. As shown in
Figure 11, the
xy-axis sample data distribution and probability density function of data sets
and
can be seen. The coordinate values of the corresponding points of data sets
and
in the figure are very different. Since Equation (15) is a squared equation, the clustered translation matrix
will be two opposite clusters. Therefore, translation matrix
of the key cluster in region 1 in
Figure 8 is incorrect, and it is opposite to the real value symbol of translation matrix
.
Data set
is transformed by translation matrix
of region 2 in
Figure 8 to obtain data set
.
Figure 12 shows the
xy-axis sample data distribution and probability density function of data sets
and
. It can be seen that the
xy-axis of data sets
and
basically overlap together, indicating that translation matrix
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
is −1.500567, the
x-axis coordinates of the 60th laser point of data sets
and
are different by 0.0006 m. When the value of
is −1.506931, the
x-axis coordinates of the 60th laser point of data sets
and
are different by 0.0058 m. In
Figure 12b, when the value of
is 0.800261, the
x-axis coordinate difference of the 60th laser point of data sets
and
is 0.00025 m, and when the value of
is 0.811266, the
x-axis coordinate difference of the 60th laser point of data sets
and
is 0.0112 m.
To address the problem that different values in the key cluster of translation matrix
have an influence on the estimation results, this paper uses K-L divergence to evaluate the estimation effect of translation matrix
. 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
and
, respectively, and then the K-L distance
from
to
is defined as:
It can also be obtained that the K-L distance
of
relative to
is:
Finally, the K-L divergence value
can be obtained by calculating the K-L distance
and
of the two probability distributions:
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
. The data with the minimum K-L divergence value are the optimal solution of the true value of the translation matrix
; 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.