Information Technology Journal

Year: 2010 | Volume: 9 | Issue: 1 | Page No.: 27-33
DOI: 10.3923/itj.2010.27.33

Lidar Scan-Matching for Mobile Robot Localization

Xia Yuan, Chun-Xia Zhao and Zhen-Min Tang

Abstract: Problem of mobile robot localization is usually solved by using GPS and INS system, but the system error of this kind of system has to be corrected by other sensors such as lidar. This study proposes an algorithm to do lidar scan-matching. The method employs a fuzzy clustering algorithm to segment points of lidar scans first and then do weight least-square linear fitting for each segment. Segments that satisfy linear distributed are picked out to calculate rotation between two lidar scans. Then the algorithm computes translation by calculating shifting of matched points. A principle called matching-range-point rule is used to find matching points belong to two scans. The characteristic of this proposed method it abandons iteration when calculate rotation and translation. It works fast and reliably and adapts to correct the error of GPS and INS system to localize a mobile robot accurately.

Keywords: lidar, localization, Robotics and scan-matching

INTRODUCTION

Ground unmanned mobile robots should navigate in different environment (Meyrowitz et al., 1996). Simultaneous Localization and Mapping (SLAM) is a basic problem for a mobile robot and it attracts lots of attentions since nineties of last century (Castellanos et al., 2000; Dissanayake et al., 2000; Bailey, 2002; Luo and Hong, 2004).

The robot understands the world outside depending on sensors it carries. When an unmanned robot reaches an unknown place two basic questions it has to answer: Where am I? and Where to go? A robot usually uses GPS combining INS system to answer the first question. Algorithms proposed by researchers to deal with the problem of localization including KF, EKF, UKF, MKF, MCL and MM, etc. The error of GPS and INS system is unavoidable. One more important thing is that in many cases there is no GPS signal and the error of INS system can be too large to be corrected without GPS. We use other sensors to correct result of localization in this situation. Matching points of lidar scans is a fast and good way to do localization accurately and we call this scan-matching in this paper. Iterative Closest Point (ICP) algorithm (Lu, 1995; Nieto et al., 2007) is a common scan-matching method. The resolution of lidar is limited and it is impossible for us to match points of two scans one-to-one so the convergent speed or even if the ICP algorithm is convergent is uncertain sometimes.

We proposed a scan-matching algorithm based on geometry-matching which abandon iteration totally. It has three mainly differences comparing to ICP: firstly, there is no iterative steps during computing translation and rotation between two lidar scans in our algorithm; secondly, our matching algorithm is based on geometry-matching which means we extract geometrical features from sampling points first and these features have less data quantity and will get more reliable result than finding corresponding points one-to-one from two scans; thirdly, we use fuzzy clustering method based on maximum entropy theory to do data segmentation.

APPROACH OF THE ALGORITHM

We give an overview of the algorithm first:

Clustering of pints: We employ a fuzzy cluster method which based on Maximum Entropy Theory (MEP) to cluster points in a lidar scan (Liu and Meng, 2004; Yuan et al., 2008). It is assumed the threshold of prediction error is δ, the next predicted lidar point’s position from a same laser detector is and the real measured position is (x_i, y_i).

The Euclidean distance between and (x_i, y_i) is defined as:

The point will be considered to be the start of another trend if e_i>δ and it means this point will be a critical point between different clusters. Since sample points collected by lidar are asymmetrical as shown in Fig. 1 the parameter δ should be determined according to the distance between lidar and the target. We use Eq. 2 to settle this problem.

Parameter a in Eq. 2 is a scaling factor to counteract noise. Parameter θ is half of angular resolution of azimuth of HDL64-E. (x_i, y_i) is the coordinate of a point p_i.

Suppose there are N points in a lidar data frame and the system model can be characterized by M cluster centers in an N-dimension space. Each cluster center can be represented by a vector that is composed of a pair of component vectors: the input vector and the output vector . For let the first p dimensions correspond to p input variables that constitute the input vector and the other m-p dimensions correspond to m-p output variables that form the output vector .

There will be a prediction error at each time when predict the position of the next point and the accumulated error problem is on the table if we ignore this error. In our algorithm, each vector in input vector is presented as , where error item e_i take part in prediction process too and then minus the last predicted error in next prediction to reduce the effect of accumulate errors. Then our cluster centers are formed as follows:

Data acquired by ladar is presented in polar coordinates as (ρ, θ). We converse it into grid coordinates first:

For data point measured at time t, the probability of it belongs to the cluster centers can be viewed as its fuzzy membership μ_i, where, μ_iε [0, 1] and i = 1, 2, ..., M. The summation of all μ_i’s is equal to one, i.e.,

The clustering process can be formulated as an optimization problem and the corresponding cost function to be minimized is defined as:

where, is the input vector of fuzzy centers.

The distribution of μ_i is unknown in our problem. According to the information theory, the MEP is the most unbiased prescription to choose the values of the membership, μ_i, for which Shannon entropy, i.e., the expression:

is maximal under the constrains 7. This optimization problem can be reformulated as the maximization of the Lagrange:

where, λ is the Lagrange multiplier.

The final solved probabilities are of the Gibbs distribution (Rose et al., 1990; Rose, 1998), i.e.,

Take Eq. 10 into Eq. 6 and versus λ differentiate and then take the experiential parameter we get:

Weighting reasoning mechanism is used to compute the out put vector as our system present a linear trend. The following formula will account for that trend well:

where, w_i is the weighting term:

Here, we have introduced the clustering algorithm we used in this study. This method does not require training which enables it to work online completely. It has the ability to trace the trend of points. From the experiment we can see fuzzy clustering gets better result than some simple clustering algorithms (Liu and Mang, 2004; Nieto et al., 2007). After clustering we do linear fitting to find linear distributed segments.

Find linear distributed segment of points: Usually, we decide points in a segment are linear distributed or not by considering the value of mean μ and variance σ of the distance between points and the fitted line. But points sampled by lidar are asymmetrical so it is hard to decide the threshold of μ and σ to differentiate linear or nonlinear distribution.

We use a multi-time weight least-square linear fitting algorithm to find linear distributed segments. For each segment the fitting process contains three steps.

Firstly, we execute a regular least-square linear fitting algorithm on a segment pf points and get initial fitting line L₁.

Secondly, the algorithm do weight least-square linear fitting based on L₁. Suppose the function of a line is:

and the sum of square of least-square dispersion of its simple linear regression is:

it has been proved (He and Liu, 2007) that the weight least squares estimation of (β₀, β₁) will be:

in Eq. 15 is defined as:

We compute the weight of a point as follow steps. Suppose there are two adjacent points P_i and P_i+1. The weight of P_i equals to ’s projection on L₁ as showing in Fig. 2. We normalize the weight of every point w_i that belongs to the same segment using (17):

Symbol (L₁) in Eq. 17 denotes the direction of L₁. Weight least-squire linear fitting was executed after weight computing and then we get fitting line L₂.

Thirdly, we recomputed the weight of each point. The new weight of P_i equals to ’s projection on L₂. Then the algorithm take 80% points whose weight are bigger and dose the third times weight least-squire linear fitting to get L₃.

This linear fitting method has considered the direction between the fitting line and the line liked by two neighbors. A point gets bigger weight if its direction is along the main distributed direction of the points in a segment.

Our experiment show values of slope of L₁ and L₃ are close if points in a segment satisfy linear distribution, otherwise the include angle of L₁ and L₃ is bigger than a threshold θ_line.

Matching linear distributed segments: We do scan-matching based on matching geometrical feature of linear distributed segments that extracted from two lidar scans. It is easy to calculate the include angle between two matched linear segments since linear distributed segments have been extracted. The angle of rotation between two scans can be calculated according to include angles of matched segments. To find matched linear segments one-to-one is easier than to find matched points one-to-one. Then the rotate angle can be computed once without iteration.

The key of the idea is how to find matched segments one-to-one reliably. The weight least-square linear fitting algorithm can ensure linear distributed segments are picked out and then we propose some rules to find matched linear distributed segments one-to-one.

There are little differences between two lidar scans as the data updating rate is so, fast that the robot moves a very short distance during two data scans. We believe two matched segments are close in position according to their own coordinate so we compute the middle point p_n (n = 1,..., k) of every linear distributed segments s₁, s₂,…, s_k. A threshold of distance Dist is chosen. There are two matching rules should be satisfied simultaneously:

Suppose there are r matched segments and the include angle of each pair of fitting line of matched segments is β_m (m = 1,..., r), then the rotation angle of two scans α can be calculated by minimizing Q in Eq. 18:

Match points between two scans: We introduce how to calculate translation of two lidar scans in this section. The method we used is called Matching-range-point rule (Lu, 1995) which is illustrated by Fig. 3.

Consider a data point P and its corresponding point P = Pα+T. If we ignore the translation, we have P≈P. On the other hand, the polar angle θ of P and the polar angle of P' are related by:

This implies that the correspondence of P under a rotation is a point which has the same polar range as that of P and the polar angles of the corresponding point differ by the rotation angle α. Now in the presence of a small translation, we can still expect that the point P with the same range as that of P is possibly a good approximation to the true correspondence of P, ant this approximate correspondence provides rich information about the rotation angle α.

To ensure that the rule finds a unique and reliable correspondence, we only search for the matching-range point within a local region of the model near P. Suppose that we can estimate a bound B_α for the rotation α, i.e., |α|<B_α. We have:

This means that P' should lie within the sector bounded by 0±B_α.

Therefore, the matching-range-point rule is expressed as follows: For a data point P, its corresponding point is P' on the model where, P' satisfies:

and

|P'|.*P|

Suppose there are g pair corresponding points between two lidar frames and the translation of each pair of points is t_q (q = 1,..., g), then the translation of two frames T can be calculated by minimizing O in Eq. 19.

Scan-matching: We can do scan-matching by rotate and transfer the coordinate between two scans. Suppose the data of k-th lidar scan is F_k, then the (k+1)-th scan F_k+1 will be:

RESULTS AND DISCUSSION

We use SICK LMS-291 lidar and its angle resolution is set to 1 degree. A GPS and INS system is used to locate the robot cursorily.

Discussion Results of clustering and linear fitting: Our clustering algorithm is better than simple ones. A simple algorithm may choose a distance threshold and points belong to two different segments when distance between two points is bigger than the threshold.

Figure 4a is the result of a simple clustering algorithm and Fig. 4b is followed by our method based on fuzzy.

The result of weight least-square linear fitting is theory. We can see our method get better result than simple ones since it is better in tracing the trend of points. The result of simple method get more fragments than ours. illustrated by Fig. 5a. L₁ is shown in blue and L₃ in red. For linear distributed segments L₁ and L₃ are almost overlapped but there exist obvious include angle when points of the segment are nonlinear distributed. We choose θ_line = 1 and the picked out linear distributed segments is shown in Fig. 5b.

Discussion Results of scan-matching: From Fig. 6a and 7a we can see the results of localization of GPS and INS system has obvious errors. By using our algorithm to refine the result of GPS and INS the error has been corrected a lot (Fig. 6c, 7b). When we calculate translation between two lidar scans the X coordinate is aligned with the moving direction of the robot.

We also tried an iterative based method to do scan-matching as shown in Fig. 8. In this method we find the neatest two points in two scans as matched points and then calculate the rotation and translation matrix by using least-square algorithm. The result is not well. We think one reason is value of rotation and translation is small and another reason is it is hard to find reliable corresponding points according to the nearest distance.

We know the error is small when the robot moves straightly. Error becomes large when the curvature of the path is big like in crossroad area. Our experiments proved this situation. We can see the rotation angle is bigger in Fig. 6b than in Fig. 7b.

It is proved by Fig. 6c and 7b that our algorithm gets good result without iteration. The algorithm decouples the computation of rotation and translation. We use a reliable method to calculate the rotation. Traditional methods find corresponding points between two scan frames to calculate the rotation. But corresponding pair of points found by this way is unreliable so it has to do it as an iterative procedure to refine the result repeat. We extract lines from a lidar scan and calculate rotation by matching these lines between two scans. Linear feature is more reliable than laser sampling points so we can compute rotation without iteration and the result is exact enough for robot navigation task. But theoretically to compute rotation and translation is an iterative process. We try to compute translation by 5 times iterating and we can find in Table 1 that the result of the first time is good enough for robot navigation. So, there is no iteration in our algorithm.

To compare the cost factor of our method and iterative one we test run time of these. Our method cost 0.66 sec to segment points and do weight least-square linear fitting of a lidar scan and 0.09 sec to calculate rotation and translation.

The iterative method showed in Fig. 8 costs 5.69 sec to calculate rotation and translation by 1000 iterations. The cost time is get in platform of matlab 7.1 without any code optimization so, it just for comparing these two kind of methods.

CONCLUSIONS

Traditional localization algorithms employ plenty filters to correct the error of GPS and INS system, but intrinsic system error can not be corrected by data filtering. So, we propose this lidar scan-matching method to help to correct the error of GPS and INS system. In actual implementation we may do scan-matching like every ten scans to counteract the cumulate error of system. This is helpful to improve efficiency.

We separate the computation of rotation and translation to avoid iteration to satisfy the requirement of fast computing of unmanned mobile robot navigation. The algorithm increase the reliability of the result of finding correspondence between two scans by matching geometrical feature instead of matching points directly to calculate the rotation.

Now we only use linear feature to match frames. There are not so many linear features in cross-country area so we will extract more geometrical features in our future work to improve the environmental adaptability of the algorithm.

REFERENCES