**Cai Ze-Su**

Department of Computer Science and Technology, Harbin Institute of Technology, Harbin 150001, China

Hong Bing-Rong

Department of Computer Science and Technology, Harbin Institute of Technology, Harbin 150001, China

Li Hong

Not Available

Department of Computer Science and Technology, Harbin Institute of Technology, Harbin 150001, China

Hong Bing-Rong

Department of Computer Science and Technology, Harbin Institute of Technology, Harbin 150001, China

Li Hong

Not Available

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.

PDF Abstract XML References Citation

Cai Ze-Su, Hong Bing-Rong and Li Hong, 2007. An Improved Polar Scan Matching Using Genetic Algorithm. *Information Technology Journal, 6: 89-95.*

**DOI:** 10.3923/itj.2007.89.95

**URL:** https://scialert.net/abstract/?doi=itj.2007.89.95

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(n^{2}) 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, S_{ref} and S_{cur}. 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 S_{cur} in the coordination of reference S_{ref} using the estimation q_{k}, p′_{i,k }= q_{k} (p″_{i,k}). Then, due to the discrete nature of the data, it is assumed a local structure in S_{ref} between successive points (p_{i,k}, p_{i+l,k}) of S_{ref}. Thus, the correspondent point to p′_{j,k} is the closest point x_{j.k} belonging to one of the segments (p_{i,k}, p_{i+l,k}):

min {d (p′ _{j.k}, p_{i+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 S_{cur} that can be paired is N_{op} = ξN_{p″} where N_{p″} is number of current scan points. The result is a set C of N_{op} correspondences (p_{j,k}, p′_{j,k}).

We define the Euclidean distance between the N_{op} correspondences pair:

(2) |

Where are the sorted squared residuals. 1≤N_{op}≤N_{p’} are the parameter that can be tuned to discarded the outliers depending on the contamination of the scan data:

d^{2}_{1:Np″}≤d^{2}_{2:Np″}≤...≤d^{2}_{Npo:Np″}≤...≤d^{2}_{Np″:Np″}.The subset of the N_{op} 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 [T_{x}, T_{y}, R, ξ] represent the relation between two scan points.

During the cross-over operation, given two chromosomes CMS_{j} = [T^{j}_{x}, T^{j}_{y}, R^{j}, ξ^{j}] and CMS_{j} = [T^{k}_{x}, T^{k}_{y}, R^{k}, ξ^{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 where is the fitness of the parent chromosome. When the chromosome is far away from the global optimum, the mutation will implement a far jump, on the other hand, a chromosome with lower fitness implies that it is closed to the global optima and hence only small movement is needed. This dynamic mutation scheme is efficient in determining a suitable step size at various stages such that the GA will not terminate prematurely. Tournament selection was applied, as it is easy implement and helps avoid premature convergence. The GA terminates if the absolute error assessment of best chromosome less than a predefined threshold value.

**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 S_{cur} is described as C = (x_{c}, y_{c}, θ_{c}, {r_{ci}, φ_{ci}}^{n}_{i,=l}),where {r_{ci}, φ_{ci}}^{n}_{i,=l} describe n range measurements of obstacle distance r_{ci} 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. {r_{ci}, φ_{ci}}^{n}_{i,= 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 S_{ref} is described as R = {r_{ci}, φ_{ci}}^{n}_{i,= l}.

In order to discard those non-visible points and outlier, we transforms the current scan readings (φ_{ci}, r_{ci}) into the reference scan’s polar coordinate frame, while using the current frame pose (x_{c}, y_{c}, θ_{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 r_{ri} from the reference scan. The objective is to find (x_{c}, y_{c}) which minimizes ∑w_{i} (r_{ri}-r″_{ci})^{2}, where w_{i} 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 (x_{c}, y_{c}) and its matrix of derivatives J with respect to the components of x_{c}, y_{c}.

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}-r_{ri} 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 B_{w} for the rotation ω, |ω|≤B_{w}. We have: φ_{ri} ε (φ_{ci}-B_{w}, φ_{ci}+B_{w}). 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 (x_{c}, y_{c}) 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, e_{0}), (1, e_{1}), (2, e_{2}).

The least-squares fit to a parabola described as e = at^{2}+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Δφ |

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.5^{o} 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 15^{o}. In the experiment, we choose an environment model and two poses, P_{ref} and P_{cur} = (-100, -100, 15) from which to take the scans S_{ref} and S_{current}. 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 5^{o}, 10^{o}, 15^{o}, 20^{o}, 30^{o}, 40^{o}, 60^{o} and 80^{o}, 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 20^{o}. But the GPSM will converage to the same values like the 15^{o}.

**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.5^{o} 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.

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.

- Brunnstrom, K. and A. Stoddart, 1996. Genetic algorithms for free-form surface matching. Proceedings of the 13th International Conference on Pattern Recognition, August 25-29, 1996, Vienna, Austria, pp: 689-693.

CrossRefDirect Link - Grisetti, G., C. Stachniss and W. Burgard, 2005. Improving grid-based slam with Rao-Blackwellized particle filters by adaptive proposals and selective resampling. Proceedings of the International Conference on Robotics and Automation, April 18-22, 2005, ICRA, Barcelona, Spain, pp: 2443-2448.

Direct Link - Hahnel, D., W. Burgard, D. Fox and S. Thrun, 2003. An efficient Fast SLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements. Proceedings of the International Conference on Intelligent Robots and Systems, October 27-31, 2003, IEEE Computer Society, USA., pp: 206-211.

CrossRefDirect Link - Robertson, C. and R. Fisher, 2002. Parallel evolutionary registration of range data. Compu. Vision Image Understanding, 87: 39-55.

Direct Link - Weiss, G. and E. Puttkamer, 1995. A map based on laserscans without geometric interpretation. Proceedings of the International Conference on Intelligent Autonomous Systems, March 27-30, 1995, Karlsruhe, Germany, pp: 403-407.

Direct Link