Gsr: Geometrical Scan Registration Algorithm for Robust and Fast Robot Pose Estimation

Undoubtedly robot localization in indoors environment where environmental information is obtained by laser range finders, which provide partial and simple information about surrounding of robots, is a challenging task. In this paper, an attempt has been made to develop an algorithm equipped with geometric pattern registration techniques to perform exact, robust, and fast robot localization purely based on laser range data. The expected pose of the robot on a pre-calculated map is in the form of simulated sensor readings. In order to obtain the exact pose of the robot, segmentation of both real laser range and simulated laser range readings is performed. Critical points on two scan sets are extracted from the segmented range data, and thereby the pose difference is computed by matching similar parts of the scans and calculating the relative translation. In contrast to other self-localization algorithms based on particle filters and scan matching, our proposed method, in common positioning scenarios, provides a linear cost with respect to the number of sensor particles, making it applicable to real-time resource limited embedded robots. The proposed method is able to obtain a sensibly accurate estimate of the relative pose of the robot even in non- occluded, but partially visible segments conditions. A comparison of State-of-the-Art localization techniques has shown that GSR algorithm is superior to the other localization methods based on scan matching in accuracy, processing speed, and robustness to large positioning errors. Effectiveness of the proposed method has been demonstrated by conducting a series of simulations.


Introduction
Autonomous navigation of a mobile robot is generally defined as control of robot motion to arrive at a given position in its environment without human intervention. This navigation task can be decomposed into the various aspects such as robot pose estimation (localization) and path planning. To generate a path from an initial position to a target position, the localization is vital which frequently provides and updates a reliable estimate of the position and orientation of the robot given a global coordinate system in a structured or semi structured environment. Maintaining an estimate of the subsequent location of a robot using odometry, while the position and orientation of the robot is known at any time, is a common solution. But this method is sensitive to errors and the outcome is not reliable because of the integration of errors over time. Thus, the need arises for developing an algorithm to overcome the challenge. Many approaches have been proposed in the literature [1][2][3][4][5] to address the self-localization problem. One possible strategy to tackle the problem of pose estimation is to use scan matching, in which the geometric representation of the current scan is frequently compared to a reference scan until an optimal geometric overlap with the reference scan is obtained. A robust and accurate localization approach using Kalman filtering, which is based on scan matching is discussed by [6]. The earliest efficient methods based on scan matching, which try to match sensor readings to the line segments extracted from set of scanned points, and then reconstruct the map at the same time, are proposed by [7,8]. Another scan matching method is proposed by [9], in which at first a histogram is assigned to each of the scans and then a histogram registration algorithm transforms (shift and rotate) the pair of histograms into one coordinate system. A point-to-point scan matching algorithm which takes two scans as an input and then using a heuristic algorithm tries to compute the transformation between the two scans has been presented by [10]. An efficient multisensory approach which uses the geometric features from a laser rangefinder and a CCD camera to pose estimation is proposed by [11]. The method has proven to be successful in environments with more uncertainty, where a laser rangefinder, by itself, cannot visualize all the peripheral edges facing the robot (e.g. long corridors). An algorithm for self-localization in polygonal environments is suggested by [12]. The method is shown to be efficient, fast, and flexible in dynamic and polygonal small environments such as robotic soccer. [13] use the principal components of range data sets to form the structure of robot environment, and then tries to infer the most probable pose of robot in the Eigen space of the range data sets. A particle filtering pose estimation algorithm is proposed by [14]. Although the method is accurate, a set of assumptions and limitations have to be defined to make it work well. For example, when the robot has a high pose error, and where the robot environment is partly cluttered the method is useless.
The most famous position estimation algorithm based on Bayesian estimation that works through a non-Gaussian distribution of particles in a previously mapped environment is Monte Carlo Localization (MCL) [15][16][17]. Each sample (particle) is weighted using a reference map represented by vectors, and when the robot is confronted with the changes in the environment, the set of weighted samples are updated. This method highly relies on particles sampling. A large number of samples are required to achieve the desired result; Otherwise the result becomes inaccurate. Hence, to achieve an appropriate degree of accuracy this method requires a high computational resources.
One of the most popular methods for dealing with the pose estimation problem is based on the study of techniques which predominantly try to use exterocep-tive sensors' readings to localize the robot in a map and to obtain a representation of the map at the same time; like the known Simultaneous Localization and Mapping problem (SLAM) [18,19]. Another self-localization strategies which use Kalman filters to perform sensor fusion are introduced by [20,21]. Pose estimation problem can also be addressed by frequently recognizing landmarks, in which in order to determine the probable position of the landmarks, robot observation, in the form of laser range reading, continually is matched with the features of the landmarks. A position estimation algorithm based on recognition of landmarks is proposed by [22]. Probabilistic approaches such as Bayesian methods [23][24][25], and Markov localization [26][27][28][29][30] are another major strategies to solve robot localization problem.
It turned out that all existing positioning strategies do not meet the key property of robustness, accuracy, and being fast. For this reason, a new method, which is able to correct large position and orientation errors of a robot in a liner time complexity has been developed. According to the reviews of literatures, scan matching, which is concerned with matching sensed data against map information, is an obvious choice in dealing with the self-localization problems. In efficient model-based approaches equipped with scan matching algorithms, by computing the best similarity between the robot observation, and an accurately-known or reconstructed map (i.e. the model) of the environment, it is possible to obtain a sensibly accurate estimate of the relative position and orientation of robot. Hence, in our proposed algorithm, firstly, a spatial description of the expected pose of the robot on a totally known or reconstructed environmental map is simulated, and then the simulated model is matched to the spatial description from laser range data. The approach proposed in this study is inspired from scan matching approaches [31][32][33] and particle filter approaches [14,34,35].
Various types of sensors such as laser rangefinders, sonars, Kinekt, and vision can be used to address the self-localization problem in indoors environments. Although laser rangefinder suffers from poor calibration, but in the study it would be prefered, since it is much better than other types of exteroceptive sensors in case of speed of distance traveled and angular resolution, also provides an accurate data, which can be interpreted faster, and with much less computational effort than a vision system. The rest of the paper is organized as follows: In Section 2 the proposed method has been briefly described. The mathematical statement of the problem, also a general overview of our algorithm are provided in Section 3. In Section 4, the proposed method, including five different stages, as well as complexity analysis of the algorithm are explained. In Section 5, the experimental results of our algorithm are presented. In Section 6, a comparison between the proposed method and some state-of-the-art localization methods is performed, and finally our conclusions are presented in Section 7.

Algorithm Description
This paper presents a new scan matching method to yield a robust and fast pose estimation algorithm. The GSR algorithm proposed in this paper takes two visualizations extracted from 2D laser range data, namely RSD (real spatial description), and V SD (virtual spatial description), and then tries to maximize the similarity between the two visualizations by transforming them (shift and rotate) into one coordinate system, and in this way, calculates actual difference between the two poses. V SD is a simulated visualization of the expected pose of the robot on a pre-calculated environmental map or, say, a simulation of sensor particles, and RSD is a visualization of the real pose of the robot from laser range data. The GSR algorithm proposed in this paper assumes that the environmental map is known at any time. Figure 1 illustrates the concept of virtual sensor and real robot on a known-map.
3 Problem statement Figure 2 shows the configuration of a mobile robot in a two-dimensional Cartesian space. In order to specify the position of the robot, we select a point O, denoted by x r , y r , on the rigid body of the robot as the position reference point. The angle between the line which passes through this point and the positive x-axis specifies the robot orientation θ r . In the same way as real robot, the pose of the virtual sensor is defined. Hence, the virtual sensor and the real robot poses illustrated in Figure 2 are defined as: T , the estimation of the relative translation (i.e. T = RP −V P ), which holds the values of the pose difference is defined as: where δ xi and δ yj are the two position difference components at times i and j, respectively, and δ θ k is the orientation difference component at time k.
The type of the sensor which is used for both virtual sensor and robot sensor is the same (i.e. Sick laser scanner LMS200-30106), and is defined as g(m, f, q), where m represents the maximum range, f is the field of view, and q is the angular resolution of the sensor. D r = [d r 1 , d r 2 , . . . , d r n ] is the set of distances measured by the robot sensor, is the set of distances measured by the virtual sensor, and Φ = [φ 1 , φ 2 , . . . , φ n ] is the set of beams angles, where n = (f /q) + 1 is the number of distances measured by the sensor, and beam angle φ i of distance d i is calculated as follows: Each point of the relative visualizations, RSD and V SD, is obtained by the following linear transformation: (4) Figure 3 depicts RSD and V SD for the robot sensor and the virtual sensor illustrated in Figure 2. RSD and V SD are used in next stages to perform scans registration calculations. Given the D r , D v , and V P , the problem to be solved is to determine T so that the final components of the pose difference, δ x I , δ y J , δ θ K are below some threshold. In other words, the robot pose is estimated as: Therefore, the three components of the pose estimation error are defined as follows: where angdif f (a, b) returns the difference between the angles a and b so that the result is wrapped on the interval [−π, π]. Figure 4 shows an overview of the proposed algorithm. The pose estimation algorithm proposed in this paper which determine an exact value of T runs in an iterative process in four different stages: corners detection, corners matching, calculating orientation difference and position difference. Once the corners of both scans are detected, the registration cycle starts by matching corner points on the two sets and finding similar parts of the scans, and thereby computing the orientation difference δ θ . The virtual sensor is then simulated for the first value of δ θ , and we check whether the updated virtual sensor pose meet the threshold condition. Next, The virtual sensor is simulated for the values of δ x , and δ y , and then it is checked for the threshold condition.

GSR algorithm
In each cycle of running the algorithm the difference between the two poses is reduced, and the process continues until the algorithm converges to a minimum that satisfies the imposed criterion. Algorithm 1 illustrates this process.
In this study, the registration is carried out using a geometrical polyline matching method. Corner points as the most widely used critical points in shape matching problems are important features that provide remarkable information about the global structure of a shape. Corner points are points, at which the angle between the two intersecting line segments is above/below a threshold. Thus, the problem to be solved in this stage is to find all corner points of RSD and V SD.

Corners detection
The first section of GSR algorithm is to detect and extract all corners from the shape of the scans. A significant number of algorithms are developed to perform polyline corner finding [36][37][38][39], but all the algorithms rely on more advanced statistical and mathematical formulation which makes them computationally intensive. In this paper ISS algorithm (Improved ShortStraw), a slightly modified version of the known ShortStraw algorithm [40] which is a robust, fast and highly accurate polyline corner finder founded on a simple concept is introduced. The first step in ShortStraw algorithm is to resample the points of a shape. The goal of resampling is to redrawing original points of a polyline so that the new points are placed equidistant from each other as much as possible and in this way can fully describe the shape. The resampled RSD and V SD shown in Figure 3 are illustrated in Figure 5.
The strength of ShortStraw is greatly dependent on resampling stage. In our ISS algorithm, a new method for point resampling is proposed, which has improved the effectiveness of the ShortStraw algorithm for using in scan matching. This method ensures that all the resampled points of the shape of a scan are placed based on an absolutely equal distance with a constant length from each other. This constant length is named interspacing distance S, and for our positioning problem is set to 1. The main idea in the resampling procedure is illustrated in Algorithm 2. After resampling the relative visualizations, ShortStraw detects corners using length of the straws. A straw for a resampled point at p i is calculated as: where ω is a constant window, and in proposed ISS algorithm we set ω = 1. All the straws are first computed GSR: Geometrical Scan Registration algorithm for robust and fast robot pose estimation if δ θ k > θ then 10: k + +; 12: Repeat the lines 5-7; 13: [δ x i , δ y j ] ← Calculating P ositionDif f erence(M atchedCorners) 14: if δ x i > x then 15: x i + +; 17: if δ y j > y then 18: for points P ω+1 to P N −ω , where N is the number of resampled points. As we reach to a curvature in the shape, the length of the straws will begin to shorten. Thus, corner points are points at which the length of the associated straws are below a threshold. Algorithm 3 illustrates the process of finding corners set of resampled RSD and V SD. The quality of the process of finding corners is dependent on the value of the threshold constant factor Z. In addition to Z, the value of the interspacing distance S affects the strength of the shortstraw. For example, the less the value of S is, the more accurate visualization is gained, but smaller values of S cause too much noise, whereas greater values of S create oversmoothed strokes. However, this is simply possible to experimentally determine appropriate values of S and Z. In our experiment by setting the values of S and Z to 0.99 and 1, respectively, the best possible trade-off has been achieved. Figure 6 shows an example of correctly segmented strokes of resampled RSD and V SD by the proposed ISS algorithm.

Similarity Measures
After the set of corners for both RSD and V SD are computed in the previews stage, some processes are used to obtain a good matching between the two sets . Let C R = [c r 1 , c r 2 , . . . , c r n ] be the set of corner points of RSD, and if D > S then 8: P 3,y = P 1,y + (P 2,y − P 1,y )/D * S P 3 is located exactly S Euclidean distance away from the P 1 in the slop direction of the straight line from P i−1 to P i . 10: Append P 3 to the resampled array 11: ing definitions apply to the relative visualizations.
if straws(i−ω) < threshold then Append(corners, i) Append(corners, number of points) and V SD, respectively. Figure 7 illustrates the concept of CA and edge. As shown in the figure, each ca i has two edges as follows: The task to do in this stage is to detect all correct correspondences between the sets of CAs of RSD and V SD using a similarity function. The method is to take each CA from RSD set and compare it with each of the CAs in the V SD set. Each comparison yields a score, a similarity measure. The best similarities are assigned to the pair of CAs with the highest similarity score. From the correspondences between the similar CAs of RSD and V SD, the set of matched CAs are obtained, and then using a robust matching technique an estimate of the difference between the two poses is archived. Therefore, given the sets of CAs and corners, the similarity matrix is defined as: GSR: Geometrical Scan Registration algorithm for robust and fast robot pose estimation The function f measures the length similarity of RSD edges and V SD edges: where El r i (i = 1, 2, 3, . . . , n) and El v j (j = 1, 2, 3, . . . , m) are the length values of edges of RSD and V SD, respectively. It must be noted that we do not use the Euclidean distance between the end points of an edge to compute its length, but rather simply calculate the difference between the number of the two end points (e.g. El r i = c r i+1 − c r i ). Using Euclidean distance, in addition to increase the algorithm's computation time to perform the square root calculations, leads to impossibility of measuring the length of curved edges.  g is the similarity measure between interior angles of two different CAs in RSD and V SD, and it is formulated as: where S is the attitude similarity function which computes the angle between an edge in ca r i and its corresponding edge in ca v j , and in this way measures the difference between the orientation or angular position of the two CAs. Figure 9 illustrates the concepts of the interior angle and the attitude angle. The function S is formulated as: where h(i, j) is the position similarity function which simply specifies the horizontal area (the area along the x-axis) occupied by ca r i and ca v j , and can be computed as: where .
(17) Figure 8 shows the horizontal areas occupied by two CAs.

Correspondence matching
After the similarity matrix is computed in the previews stage, the similarity scores are used to match the CAs. As the number of matched points increases, the estimation of the difference between the two poses becomes more reliable. Here, in an ideal case, all the CA points of the set with fewer member should be matched with the CA points of the other set. But, as it will be further discussed, in some cases it could be impossible. However, a lower bound must be determined for the number of CA points that should be corresponded between the RSD and V SD sets. Through experiment, we came to believe that in order to get a good enough estimate of the difference between the two poses at least three consecutive CA points of RSD must be matched with three consecutive CA points of V SD. Algorithm 4 illustrates the process of matching CAs. Table 1 (i.e. a similarity matrix) illustrates the result of the matching procedure for RSD and V SD shown in Figure 3. In each raw of the similarity matrix, the matching is assigned to the pair of CAs with the highest similarity score. The correspondence matching between the CAs of V SD and RSD is illustrated in Figure 10.

Orientation Estimation
Given the set of matching sequences M = {m 1 , . . . , m N }, the orientation difference between the two poses is estimated as: where and m i is the subsequence i from the set of matching sequences M which consists of n consecutive matched ca pairs (n is the number ca's of a subsequence). θ Rr i , the rotation angle of subsequence i from RSD set, is the angle between the line from the first ca to the last ca of the subsequence i and positive x-axis. Figure 11 illustrates the concept.

Position estimation
The problem of estimating position difference between the robot and our expectation can be considered as an optimization problem as : In our experiments, the simplex algorithm [41] has been utilized in order to make an estimate of relative position of the robot. Simplex algorithm is expected to converge to a local minimum in a finite number of iterations and a linear time complexity [42]. As shown in Algorithm 5, Simplex algorithm starts with three random points in a confidence interval. As it will be further illustrated by some experiments, the confidence interval for position difference in GSR algorithm for a laser range finder with 10 meters sensor range is [−300, 300]. Hence, using Simplex algorithm, in each itteration of GSR algorithm an approprite estimate of the relative position of robot is obtained. While the values of δ x and δ y are estimated, if (T emp(i, 2) = T emp(i − 1, 2) + 1) and (T emp(i, 1) = T emp(i − 1, 1) + 1) then 9: Append(Stack, (Temp(i))) 10: i ← i + 1 11: else 12: if length(stack) >= 3 then 13: if M is not empty then 14: if the first value of the current Stack does not exist in M then 15: Append(M, Stack) 16: else 17: Append(M, Stack) 18: Stack ← T emp(i) 19: i ← i + 1 20: if length(stack) >= 3 then 21: Append(M, Stack) Table 1 Similarity matrix of the two RSD and V SD sets we must check whether the new estimated pose of the virtual sensor is in a free space or not. In cases the new estimated pose makes the virtual sensor locate inside an obstacle then we can simply increase/decrease the Select three random points with an arbitrary x and y and call them P1, P2, P3 3: Err P 1 ← simm(P 1 + v, θ r , D r ) Err P 1 is the amount of error Err after simulation for P1. 4: Err P 2 ← simm(P 2 + v, θ r , D r ) 5: Err P 3 ← simm(P 3 + v, θ r , D r ) 6: while (Err P 1 > T reshhold)&(Err P 2 > T reshhold)&(Err P 3 > T reshhold) do 7: B ← find the point among P1, P2 and P3 with minimum error 8: N B ← find the point among P1, P2 and P3 with 2nd minimum error 9: W ← find the point among P1, P2 and P3 with maximum error 10: RB ← simm(B + v, θ r , D r ) 13: RW ← simm(W + v, θ r , D r ) 14: RR ← simm(R + v, θ r , D r ) 15: if RR < RW then 16: if RR > RB then 17: W ← R 18: if RR < RE then 22: W ← R 23: else 24: W ← E 25: W ← C 28: Err P 1 ← simm(B + v, θ r , D r ) 29: Err P 2 ← simm(N B + v, θ r , D r ) 30: Err P 3 ← simm(W + v, θ r , D r ) 31: B ← find the point among B, NB and W with minimum error 32: δ x ← B x 33: δ y ← B y 34: 35: values of δ x and δ y by an arbitrary amount as: After the final values of δ θ , δ x , and δ y are determined then we use them to perform a new simulation of virtual sensor, and the algorithm is terminated when an accurate enough registration between the two scans is achieved.

Computational cost
With a quick analysis it can be shown that the proposed ISS method costs O( n/S) in time and space to resample the points, where n is the number of distances measured by sensors, or original points of the scans, S is the interspacing distance, and is the maximum gap between two consecutive observed points in relative visualizations. In the best-case scenario equals 1, and in the average-case scenario, which is the common case, the value of is much less than n. However, in the worst-case scenario, which is exceedingly unusual and virtually impossible to happen, could be equals the maximum distance measured by the sensors (i.e. = max([d r 1 , d r 2 , . . . , d r n ])). The algorithm follows with the similarity function which is O(c 2 ), where c is the number of corners. Although it is not a common input but in the worst case which each point is a corner, that is a very unlikely case, the similarity function takes O(n 2 ) time. Since the correspondence matching procedure, in the worst case, has the same computational time as the similarity function, and the rest of the algorithm runs in O(1) we can conclude that the computational complexity of the one iteration step of GSR algorithm is O(c max(c/S, /S)) that in the worst case, in which c equals n, is O(n max(n/S, /S)).

Experiments
In this section, the performance of GSR algorithm is evaluated by conducting two experiments, both carried out in MATLAB environment. In the first experiment the robustness of the matching procedure is illustrated by a series of different simulations, and in the second experiment the performance of the localization algorithm is evaluated, also the impact of outliers on the performance of the algorithm is quantified.

Experiment 1
Significant pose differences between RSD and V SD, which lead to a large deformation between the CAs of the two scans, certainly, will affect the matching performance. However, it should be noted that, like any other point matching approach, it is possible to exists several matching candidates for some CAs, also possible to exists some CAs which cannot be matched. All of these problems lead to outliers in the set of matches. This is an inevitable phenomenon, which is always likely to exist and affects the process of CA matching. In spite of the fact that complete elimination of outliers is virtually impossible, it will be further discussed in detail, and followed by some examples that even despite the existence of outliers in the set of matched CAs, when there is no large depth pose difference between RSD and V SD, the algorithm is able to correctly estimate the difference between the two poses.
Through Experiment 1, 2704 various robot observations of the environment shown in Figure 1 have been collected from 100 different spots on the map. All the scans are captured with the 2D-laser scanner SICK LMS200-30106 with 10 meters sensor range and 180 degrees field of view when the Pioneer P3-DX robot was guided through the environment. It should be noted that the average number of CAs in each scan is 14 (i.e. scatter constant-1). By running the GSR algorithm for only one iteration step, the proportion of correctly matched CAs of two RSD and V SD in different situations has been measured. In order to better evaluate the performance of the matching procedure, also optimal readability, this experiment has been divided into four different scenarios, considering 676 samples, collected by the robot sensor and virtual sensor in different situations. In all scenarios, the x and y position differences are modified and range from 0 to 300 centimeters. In the first scenario, the orientation difference Table 2 The values of the pose error components after running GSR algorithm in the four different simulation scenarios scenario Iterations T ime(ms) ε x (cm) ε y (cm) ε θ (deg) is fixed and equals 0 • . Figure 12 is the error surface for the relative correspondence error in the first scenario. It can be seen from the figure that even in situations in which the position difference is too large (e.g. ∆ x = 250 and ∆ y = 250), the outlier rate is significantly low. In a wide range of error surface the correspondence rate is more than 90%. The simulation result for the first scenario shows that, the minimum correct matching rate is about 45%. The second scenario evaluates the performance of the matching procedure under the conditions in which, the orientation difference equals 15 • . The correspondence error surface in the second scenario is illustrated in Figure 13. As it has almost behaved the same on previous scenario, the result shows a significantly high performance of the method. In this scenario the minimum correct matching rate is about 35%.
The robustness of the matching procedure is more evident in the third scenario where the orientation difference equals 30 • . Despite the high orientation difference, the method is still able to match corners to an acceptable rate. In this scenario the minimum correct matching rate is about 30%. Figure 14 shows the result in the third scenario.
And finally the last scenario evaluates the performance of the method in conditions of large depth pose difference. In this case the orientation difference equals 45 • . It can be seen from the error surface showed in Figure 15 that even in spite of such a high orientation error the average correspondence rate is relatively high which shows the efficiency of the matching procedure. In this scenario the minimum correct matching rate is about 10% which has allocated a very small region of the error surface to itself.

Experiment 2
In the second experiment the performance of GSR algorithm will be evaluated by several sets of simulations, which have been conducted under different positioning error conditions. The simulation results have been obtained by running the algorithm in four different scenarios. The number of iterations, total elapsed time and the         final pose estimation errors in each scenario are demonstrated in Table 2. In all scenarios of the experiment all the three pose error thresholds, x , y , and θ is set to 1. The first scenario evaluates the performance of GSR algorithm in conditions in which we have curved edges. The actual pose difference between the real robot and the virtual sensor in this scenario is as follows: The simulation results of the first scenario are illustrated in Figure 16. The algorithm has successfully corrected the pose error between the RSD and V SD in four iterations. Figure 16(a) shows the correctly matched CAs of RSD and V SD after the first iteration step of running GSR algorithm. The process of pose error correction is evident from Figure 16(b). In this figure the virtual sensor readings are depicted after each iteration of running the algorithm. The actual pose different between the real robot and the virtual sensor in the second scenario is as follows: The simulation results of the second scenario are illustrated in Figure 17. GSR algorithm has corrected the pose error between the RSD and V SD in five iterations.
In the third scenario the actual pose different between the real robot and the virtual sensor is as follows: The simulation results of the third scenario are illustrated in Figure 18. In spite of a large pose difference between the RSD and V SD sets, after the first iteration step of running the algorithm, it correctly matches six CAs form RSD to six CAs from V SD. The algorithm has successfully corrected the pose error between the RSD and V SD in four iterations. In the fourth scenario the actual pose different between the real robot and the virtual sensor is as follows: The algorithm has successfully corrected the pose error between RSD and V SD in five iterations. The simulation results are illustrated in Figure 19.

Comparison
Since some of the outstanding localization techniques (e.g. MCL, PF, and etc.), in order to achieve the desired accuracy in pose estimation, highly relay on a large number of particles, it can be concluded that the proposed GSR algorithm outperforms these techniques in case of computational efficiency. However, in order to demonstrate the precision and convergence speed of GSR algorithm, it needs to be compared with the other group of the state-of-the-art localization methods under different positioning conditions. Hence, in this section a comparison between the proposed solution and some important solutions to the localization problem proposed in the recent past which are mainly based on landmarks recognition or scan registration is performed. For the reasons mentioned earlier we only concentrate on the methods that basically are based on laser rang data. For comparing the different methods we implemented each one, under 676 random positioning error conditions, on the environment shown in Figure 1. To better demonstrate the errors of GSR algorithm, Figure 20 illustrates the final pose estimations occurred in each simulation. The desired target location is centered at zero and the positions approached by the robot is marked by the solid circles. The results derived from this plot for the other localization methods are depicted in Figure 21. The figure summarizes the results of 676 positioning actions for each method in terms of average error, standard deviation, and maximum error in position and orientation estimation that occurred in each simulation. As can be seen from the figure, the error mean and variance associated with GSR algorithm is relatively low compared to the other methods of localization.
The result of the comparison of four terms, minimum and maximum AP E(i.e. accuracy in position estimation), minimum and maximum AOE (i.e. accuracy in orientation estimation), and AET (i.e. average elapsed time in each pose estimation operation), for the seven localization methods are demonstrated in Table 3. These results are obtained through 4732 (i.e. 676 × 7) simulations on the proposed map for a laser range sensor with 10 meters maximum range and 180 degrees field of view. The simulation results show that even in large depth pose error conditions the error of the GSR algorithm is small and never exceeds 12 mm/0.83 deg in any of our simulations. As shown in Table 3, the minimum accuracy in the estimated robot pose obtained through the simulations for GSR algorithm is 1.2 cm in position and 0.83 degree in orientation, and the maximum accuracy is 0.1 cm in position and 00.08 degree in orientation, whereas the average elapsed time for correcting the robot pose error in each pose estimation operation is approximately 75 millisecond making it applicable to real-time resource-limited embedded robots that require high precision.

Conclusions
This paper presents a new approach to pose estimation for a mobile robot based on geometrical and topological analysis of the shape of laser range scans. The problem of robot positioning has been stated as finding matchings between two visualizations extracted from 2D laser range data. The method, considering a simulated visualization of the expected pose of the robot on a known map as the prediction, continually searches for pairings between the features of the prediction and observation (i.e. corner points), and tries to maximize the similarity between the two sets by finding the relative translation in an iterative process, and in this way obtains a sensibly accurate estimate of the robot pose. Experimental results have been described to confirm the appropriateness of the matching procedure in finding the most similar parts of two scans even in large depth pose error conditions. Experiments also show that even in the presence of outliers in the set of matched corners, using an appropriate matching strategy, the algorithm is able to correctly estimate the difference between the two poses. The main advantages of GSR algorithm are the significantly low number of iterations to converge to a minimum error, computational efficiency, and robustness to large positioning errors. The minimum accuracy, obtained through the experiments, in the estimated robot pose for a laser range sensor with 10 meters maximum range and 180 degrees field of view was 1.2 cm in position and 0.83 degree in orientation, and the maximum accuracy was 0.1 cm in position and 0.08 degree in orientation, which demonstrates robustness and precision of the proposed algorithm.