Abstract: This study presents a novel method for 2D laser scan matching called Genetic Polar Scan Matching (GPSM). The method combined the Genetic algorithm that used to solve automatic Pre-alignment two 2D scan data represented by sets of points with a Polar Scan Matching (PSM). The GPSM not only avoid searching for point associations by simply matching points with the same bearing, but also produce accurate results without limiting a small orientation between a reference scan and current scan. The experiments illustrate how the performances of this method are better than PSM and Iterate Closest Point (ICP) in terms of robustness and accuracy.
INTRODUCTION
Localization and map making is an important function of mobile robots. One possible task to assist with this functionality is to use laser scan matching. Scan matching is the process of matching two range scans obtained from a range device such as a Laser Range Finder (LRF). Because of its high accuracy and reliability, scan matching has been applied to many SLAM algorithms (Hähnel et al., 2003; Grisetti et al., 2005). In laser scan matching, the position and orientation or pose of the current scan is sought with respect to a reference laser scan by adjusting the pose of the current scan until the best overlap with the reference scan is achieved.
Scan matching approaches can be categorized based on their association method such as feature-to-feature, point to feature and point-to-point. Point-to-point matching can be used in non-polygonal environments. Due to the applied association rules in the matching, matching points have to be searched across 2 scans, resulting in O(n2) complexity, where n is the number of scan points. The Cross Correlation Function (CCF) (Weiss and Puttkamer, 1995) method matches two scans using the correlation between angle histogram of the scans and also using the correlations between x and y histograms, reduces the searching problem of 3D to three problems of 1D, decreases greatly the computation complexity. Tangent Based Angle Histogram and Iterative Closest Point (TAHICP) in (Ming et al., 2004) that consists iterative tangent weighted closest point and Hough transform based tangent angle histogram algorithms to performance real-time pose estimation. But all of point-to-point approaches operate in a Cartesian coordinate frame and therefore do not take advantage of the native polar coordinate system of a laser scan. Polar Scan Matching (PSM) algorithm (Diosi and Kleeman, 2005) can eliminate the search for corresponding points using the matching bearing rule thereby achieving O (n) computational complexity for translation estimation and orientation estimation if a limited orientation estimation accuracy is acceptable. But this Iterative registration method like ICP and PSM work with roughly pre-registered sets: the relative rotation of the two sets must be small (e.g., less than 20 degree) for the iterations to converge to a good alignment. In mobile robot fields, scan matching algorithm’s approximate alignment is often obtained from odometry and imperfect because it leads to unbounded position error, the pre-registered manually is also impossible because of movement of robot. Genetic algorithms have already been used for registration of 3D data. Yamany et al. (1999) used a genetic algorithm for registration of partially overlapping 2D and 3D data by minimizing the mean square error cost function. Robertson and Fisher (2002) applied Gassuian to the registration of range data. They use the mean square error objective function and perform optimization in the six-parameter space. Their genetic algorithm uses four different mutation operators and two crossover operators. Chetverikov et al. (2005), they use a genetic algorithm to solve the problem of Euclidean alignment of two arbitrarily oriented, partially overlapping measured surfaces in presence of noise and outliers.
In this study we combine precision and robustness of PSM with generality of genetic algorithms. Arbitrarily oriented 2D laser scan point datasets are considered and no manual Pre-alignment is needed. The complete alignment is performed in two steps. First, a GA searches for Pre-alignment of the two data sets with approximate alignment from odometry while tuning the overlap. Second, the result of the GA is then refined by PSM. These results, in a general, precise and fast converge to global minimum to avoid local minima without limiting a small orientation between a reference scan and current scan.
Pre-alignment of arbitrarily oriented 2D scan points using a genetic algorithm: ICP-based scan matching algorithm is sometimes effective, a good initial guess is essential to find the correct solution. If the initial guess is far from the actual solution, incorrect solution or mismatching is very likely. The scan matching of two data sets can be formulated as a search or an optimization problem. This leads to a three-dimensional optimization problem with many local extrema. GA’s are good at optimizing functions with many local optimal points and have no restriction on the form of objective functions. Brunnstrom and Stoddart, 1996) used the GA yielded only an approximate transformation and used a further process such as ICP to find the accurate transformation with the approximate solution as an initial guess.
This section deals with genetic pre-alignment of two arbitrarily oriented scan points, Sref and Scur. The task is to quickly obtain a rough pre-alignment suitable for subsequent application of PSM. In (Diosi and Kleeman, 2005), the PSM algorithm can cope with initial angular misalignments up to 20 degree; 5 degree is certainly sufficient. This means that the genetic Pre-alignment should provide an angular accuracy of 5 degree, or better. The initial translation misalignment is less critical.
First let place each point p″i of Scur in the coordination of reference Sref using the estimation qk, p′i,k = qk (p″i,k). Then, due to the discrete nature of the data, it is assumed a local structure in Sref between successive points (pi,k, pi+l,k) of Sref. Thus, the correspondent point to p′j,k is the closest point xj.k belonging to one of the segments (pi,k, pi+l,k):
min {d (p′j.k, pi+l,k)} |
(1) |
In laser scan matching processing, there usually involves working with outliers and partially overlapping data. The overlap, that is, the ratio of the points that can be paired, is a critical parameter. ICP assumes that the overlap is 100%, which is rarely true. Using outliers, or correct but non-overlapping data, leads to artificially large residuals that spoil the cost function and the alignment. To achieve high accuracy and robustness, a good strategy is then to use as much overlapping data as possible.
Denote the overlap by ξ. Then the number of point in Scur that can be paired is Nop = ξNp″ where Np″ is number of current scan points. The result is a set C of Nop correspondences (pj,k, p′j,k).
We define the Euclidean distance between the Nop correspondences pair:
(2) |
Where
d21:Np″≤d22:Np″≤...≤d2Npo:Np″≤...≤d2Np″:Np″.The subset of the Nop paired points is updated after the robot motion.
A GA uses a fitness function to determine the performance of each artificially created chromosome. Our genetic Pre-alignment process minimize the objective function:
(3) |
Since the geometric relation between two 2D scan points can be represented by three parameters, in additionally the overlap ξ, we define this set of parameters as a chromosome. Each parameter then corresponds to one of the genes in the chromosome. They are defined as:
They form a chromosome [Tx, Ty, R, ξ] represent the relation between two scan points.
During the cross-over operation, given two chromosomes CMSj = [Tjx, Tjy, Rj, ξj] and CMSj = [Tkx, Tky, Rk, ξk], we simple randomly select one of genes to be swapped. The new off-springs generated using cross-over operation could be significantly different from its parents so cross-over facilitates the far-searching process in searching for the optimum.
Similar to cross-over, mutation is another standard operation in genetic algorithms.
Under mutation, each gene has a certain probability to change its value. In
our implementation, we let this probability be equal and be 0.25. Each real-valued
gene in a chromosome will be accumulated with a small value. The value to be
accumulated is generated randomly within the dynamic range
GENETIC POLAR SCAN MATCHING (GPSM)
Scan preprocessing: Scanning is noisy and small errors may occur, namely Gaussian noise and salt and pepper noise. The latter one arises for example at edges where the laser beam of the scanner hits two surfaces, resulting in a mean and erroneous data value. Further reflections, e.g., at glass surfaces, lead to suspicious data. Consequently prior to matching, the current and the reference scans are preprocessed. Preprocessing helps to remove erroneous measurements, clutter or to group measurements of the same object to increase the accuracy and robustness of scan matching. First a median filter with a window of 5 is used to take all points in a certain radius, group them together to one new point which have the average distance from the points in the group, replace outliers for removing objects that are likely to move, such as table and chair legs. Uses the median filter can be reduce the error in the distance measurement that comes with uneven walls and uncertainty in the sensor. Second these point which are further than a predefined threshold are not used in segmentation which have two advantages. The first advantage is that interpolation between 2 separate objects can be avoided if one knows that the objects are separate. Such interpolation is useful when one wants to know how a scan would look from a different location. The second advantage is that if laser scans are segmented and the segments are tracked in consecutive scans then certain types of moving objects such as peoples can be identified. Two criteria are used in the segmentation process. According to the first criterion, a range reading which less than predefined from the previous range reading belongs to the same segmentation. Therefore a second criterion is also applied according to which if 3 consecutive range readings lie approximately on the same polar line, then they belong to the same segment.
Scan projection: The objective of scan matching is to estimate the relative displacement q (R,t). A common approach just like ICP is to perform an iterative process in two steps: computing the correspondences and estimation of relative location. The problem of establishing the correspondence between scans is crucial and is difficult. The original ICP algorithm assumes outlier-free data and current scan data being a subset of reference scan data, in the sense that each point of current scan data has a corresponding point in reference. In practice, these conditions are often not fulfilled. In order to solve the correspondence between each point in the current scan and the point in the reference scan, we choose an association rule by simply matching points with the same bearing similar to IDC.
The current scan Scur is described as C = (xc, yc, θc, {rci, φci}ni,=l),where {rci, φci}ni,=l describe n range measurements of obstacle distance rci at bearing φci which is the angle relative to a predefined direction (e.g., the x-axis) and i is a numbered index, expressed in the current scans coordinate system. {rci, φci}ni,= l are ordered by the bearings in ascending order in a counterclockwise direction as they are received from a SICK laser scanner. The reference scan Sref is described as R = {rci, φci}ni,= l.
In order to discard those non-visible points and outlier, we transforms the current scan readings (φci, rci) into the reference scan’s polar coordinate frame, while using the current frame pose (xc, yc, θc) expressed in the reference frame:
Next ranges r″ci at the reference scan bearing φri are calculated using interpolation. Note that if bearing where range measurements are taken are unchanged in current and reference scans then φri = φci. By linear interpolation a range value is calculated for each sample bearing. If a range value is smaller than an already stored range value at the same bearing, then the stored range is overwritten with the new one to handle occlusion.
Translation estimation: We propose a simply rule that finds the correspondences in such a way that they significantly reveal the rotation component. After scan projection, for each bearing φri, there is at most one r″ci from the projected current scan and a corresponding rri from the reference scan. The objective is to find (xc, yc) which minimizes ∑wi (rri-r″ci)2, where wi is a weight used to reduce weighting of bad matches. To minimize the weighted sum of square residuals we applied Levenberg Marguard (LM) algorithm (Cai et al., 2006).
The LM algorithm is an optimization procedure which is particularly suited to functions such as
which are expressed as a sum of squared residuals, where neff is a number of effective corresponding pair. Each iteration of the LM algorithm comprise the following three steps,
Compute the vector of residuals e (xc, yc) and its matrix of derivatives J with respect to the components of xc, yc.
Compute the update rule
set
where the value of λ controls the distance traveled along the gradient direction.
According to the recommendations of Diosi and Kleeman (2005):
Where di = r″ci-rri is the error between projected current scan range measurements and reference scan range measurements, parameter c determines where the sigmoid changes from 1 to 0 and m determines how quickly the sigmoid function changes from 1 to 0.
Orientation estimation: Charge of orientation of the current scan is represented in a polar coordinate system by a left or right shift of the range measurements. Suppose that we can estimate a bound Bw for the rotation ω, |ω|≤Bw. We have: φri ε (φci-Bw, φci+Bw). This means that we can assume that the correct location of the current scan is known and the reference and current scans contain measurements of the same static objects, the correct orientation of the current scan can be found by shifting the projected current scan (r″ci, φri) until it covers the reference scan. For each shift angle, the average absolute range residual e (xc, yc) is calculated. Orientation correction is estimated by fitting a parabola to the 5 closest points to the smallest average absolute error and calculating the abscissa of the minimum. Assume that the 5 points of the error function are: (-2, e-2), (-1, e-1), (0, e0), (1, e1), (2, e2).
The least-squares fit to a parabola described as e = at2+bt+c with t = (-2, -1, 0, 1, 2) yields:
Then the abscissa of the minimum average absolute error e is:
Assuming the orientation correction corresponding to 0 is Δθt, the bearing distance between t = 0 and t = 1 is Δφ, then the estimated orientation correction will be:
Δθc = Δθl+mΔφ |
RESULTS
The results of 2 experiments are presented. In the first experiment simulated laser scans are matched and evaluated. The remaining experiments use a SICK LMS 221 laser range finder which fixed on AS-R robot in center intelligent robotic of Harbin institute of technology, with a 0.5o bearing resolution in indoor environments.
Simulation: Figure 1 shows two simulated scans of our environments. The scans were taken of the same location, but the x and y position of the current scan was altered by 100 cm. Orientation was altered by 15o. In the experiment, we choose an environment model and two poses, Pref and Pcur = (-100, -100, 15) from which to take the scans Sref and Scurrent. Figure 2- 4 show the evolution of errors of PSM, ICP and GPSM in the simulated experiment, respectively.
In order to determine how the polar scan matching using Genetic Algorithm variants cope with different types Pre-alignment, we assumed the x and y position of the current was altered by 100 cm. but orientation was altered by 5o, 10o, 15o, 20o, 30o, 40o, 60o and 80o, respectively. The evolution of final errors of PSM and ICP algorithm can be see on Fig. 5 and 6, respectively. From the Fig. 5 and Fig. 6, we can see that the scan matching errors are dramatically increase with the increasing of orientation. When the orientation is bigger than 20 degree, the PSM and ICP will diverge and no enough pair points.
The final errors of GPSM algorithm can be see on Table 1. For larger orientation, the final errors are still very small.
| Fig. 1: | The reference scan and the actual scan in the same coordinate system. Grid size in 1x1 m |
| Fig. 2: | The evolution of error of PSM |
| Fig. 3: | The evolution of errors of ICP |
| Fig. 4: | The evolution of errors of GPSM |
| Fig. 5: | The evolution of errors of PSM vs a variety of orientation |
From Fig. 2-6 and Table 1, it is clear that the algorithm can be solve the automatic pre-alignment of arbitrarily oriented scan data while preserving precision and robustness of PSM algorithm. In the navigation field of mobile robot, the effections of the robot rotation errors are often bigger than translation error.
| Table 1: | Scan matching results of the simulated room for GPSM algorithm (x = 100 cm, y = 100 cm) |
| Table 2: | Combinations of scans taken at different corners for the ground truth experiment |
| Table 3: | Absolute errors of x (cm) y (cm) and orientation (°) of
scans taken at different corners for the ground truth experiment |
| Fig. 6: | The evolution of errors of ICP vs a variety of orientation |
The number of this algorithm has been only successfully applied to initial relative rotations of up to 20o. But the GPSM will converage to the same values like the 15o.
Experiment with ground truth: To determine how the GPSM variants cope with different types of environments, we use the range data of an office scene. The data sets consist of four scans that were scanned from four different corners. On 4 corners of a 80x80 cm rectangle, 4 laser scan data were drawn from different orientations. The scene consists of the entrance area of an office with some paper boxes, a door and a bookcase. The combinations of scans taken at corners that take part in the scan matching are shown in Table 2. The carefully determined ground truth values for current scan poses in reference scan frames which also corresponded to the initial errors are also displayed in Table 2. From Table 2 one can see that the initial errors were up to 80 cm displacement and up to 34.5o orientation. During the experiments the environment remained static. From Table 3, we can see that the performance of GPSM and PSM are almost the same because the six scan matching contain a small initial error in orientation (less than 25 degree), are clearly outperformed the performance of ICP.
CONCLUSIONS
We presented a novel method GPSM which applying the genetic Pre-alignment prior to the novel Polar Scan Matching (PSM) approach belongs to the class of point to point matching algorithms. In order to solve automatic pre-registration two 2D scan data represented by sets of points, assuming that the overlapping part is informative enough for unambiguous registration. We employ a genetic algorithm perform search in the 4-parameter space formed by two translation parameters, one rotation parameter and one overlap parameter. GPSM takes advantage of the structure of laser scanner’s polar coordinate system. The direct use of range and bearing measurements coupled with a matching bearing association rule and a weighted range residual minimization, resulted in a fast and robust scan matching algorithm at unknown relative rotation and translation. Simulation of matching scans in a room demonstrates that the current scan pose error decreases more quickly with GPSM to a small value for arbitrary orientations between a reference scan and current scan. But the ICP and PSM can not solved the divergence of the current scan pose error for arbitrary orientations. The results with ground truth revealed that the performance of GPSM and PSM excel that of ICP in accuracy. The GPSM algorithm can cope with arbitrary orientations and producing accurate results without limiting a small orientation between a reference scan and current scan.