Discrete Dynamics in Nature and Society

Volume 2016, Article ID 2028414, 20 pages

http://dx.doi.org/10.1155/2016/2028414

## Polar Metric-Weighted Norm-Based Scan Matching for Robot Pose Estimation

^{1}State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150001, China^{2}School of Software Engineering, Beijing University of Technology, Beijing 100871, China

Received 17 November 2015; Accepted 6 January 2016

Academic Editor: Daniele Fournier-Prunaret

Copyright © 2016 Guanglei Huo et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

A novel point-to-point scan matching approach is proposed to address pose estimation and map building issues of mobile robots. Polar Scan Matching (PSM) and Metric-Based Iterative Closest Point (Mb-ICP) are usually employed for point-to-point scan matching tasks. However, due to the facts that PSM considers the distribution similarity of polar radii in irrelevant region of reference and current scans and Mb-ICP assumes a constant weight in the norm about rotation angle, they may lead to a mismatching of the reference and current scan in real-world scenarios. In order to obtain better match results and accurate estimation of the robot pose, we introduce a new metric rule, Polar Metric-Weighted Norm (PMWN), which takes both rotation and translation into account to match the reference and current scan. For robot pose estimation, the heading rotation angle is estimated by correspondences establishing results and further corrected by an absolute-value function, and then the geometric property of PMWN called projected circle is used to estimate the robot translation. The extensive experiments are conducted to evaluate the performance of PMWN-based approach. The results show that the proposed approach outperforms PSM and Mb-ICP in terms of accuracy, efficiency, and loop closure error of mapping.

#### 1. Introduction

Localization and environment mapping are foundational functions of mobile robots. The dominant way to implement these functionalities is to use a scan matching approach [1–4], where the robot pose is iteratively estimated by the established correspondences between reference and current scans. In each iteration, the most popular way is to apply a set of predefined nearest neighbor rules to three types of features [5], that is, segments (represented as lines or curves), corners (environmental significant feature changes), and raw points, to achieve the scan matching.

Generally, scan matching approaches will fall into three categories based on the feature representation [6], that is, feature-to-feature approaches [7, 8], point-to-feature approaches [9, 10], and point-to-point approaches [11–14]. Feature-to-feature approaches adopt the location criteria to match features in both the reference and current scans, while point-to-feature approaches are utilized in geometric distance criteria to match segments in the reference scan with current scan points. However, both approaches suffer from uncertain or misrecognized features, which inevitably lead to rapid declination of pose estimation accuracy in a real indoor environment. Therefore, those approaches are only applicable in structural environments with simple feature representation. Point-to-point approaches, in contrast, are more suitable for unknown environments because they are independent of the environmental features. The earliest point-to-point scan matching approach is Iterative Closest Points (ICP) [11], which is limited in its real-world application because of the long runtime and low convergence rate. Currently, the two most popular point-to-point algorithms are Mb-ICP [13] and PSM [14]. The former assumes a constant weight of the norm about the rotation angle, but the rotation angle is actually different in every iteration. This may cause mismatching between reference and current scans. Moreover, it has a high computational cost to match and calculate each pair of laser scanning points between the reference and current scan. The latter mainly considers rotation of mobile robot and sometimes might lead to mismatching since the distribution of polar radii in irrelevant regions of the reference and current scans is similar.

In order to solve problems of Mb-ICP and PSM mentioned above, a novel point-to-point scan matching approach called PMWN is proposed in this paper. This approach treats the scans as a whole during the correspondences establishing process, rather than point to point. And correspondences between reference and current scans are established by means of one adaptive metric weight. Furthermore, the rotation and translation estimations are separated, where the former is achieved by linear absolute function method, and the latter is implemented by a projected circle method.

The rest of our paper is organized as follows: some related works are discussed in Section 2. The scan matching method based on PMWN is described in Section 3, where the relevant derivation about PMWN is interpreted to facilitate understanding of the proposed methods, and, then, the proposed methods for rotation and translation estimation are introduced. The empirical experiments are reported in Section 4, and conclusions are presented in Section 5.

#### 2. Related Works

The objective of the scan matching techniques is to estimate the relative motion of a mobile robot between two consecutive sensor scans. More precisely, given a reference scan , the current scan , and a rough intermediate pose estimation, the objective is to estimate the real pose , where and indicate the translation displacement, and is the rotation angle of the robot. Currently, many typical localization methods are conducted in Cartesian coordinate frames or polar coordinate frames.

##### 2.1. Scan Matching Method in Cartesian Coordinate Frame

In a Cartesian coordinate frame, a lot of researches have been conducted on point-to-point scan matching approaches, such as ICP [11], Iterative Dual Correspondence (IDC) [12], and Mb-ICP [13]. The classical ICP is an iterative algorithm for robot pose estimation, where each iteration includes two steps, that is, searching correspondences and calculating relative pose. The first step is to establish the correspondence between the reference and current scan. Then, a minimization process is adopted in the second step to improve the estimation of relative pose until convergence. The family of ICP algorithms uses the minimum Euclidean distance criteria to establish the correspondences and to apply the least squares for estimating the pose. However, many factors limit the applications of ICP [12]. For example, the Euclidean distance used by ICP does not take the sensor rotation into account, and correspondences of ICP are established by matching every pair of points between the reference and current scans, which may lead to a high computational burden. Otherwise, the convergence rate of ICP is slow in structured indoor environments because the consecutive scans are less distinguishable in a small region.

To address the inherent rotation issue in traditional ICP methods, Lu and Milios proposed the IDC algorithm to estimate the translation and rotation components in robot pose. The novelty of IDC is that it evaluates translation with ICP and rotation with IMRP [12], which guarantees that the translation and rotation estimation is estimated accurately. In fact, the efficiency of IDC is lower than that of ICP since IDC integrates the IMRP and ICP. To improve IDC, Gutmann and Schlegel proposed a special filter, which projects the original points into the center [15]. Unfortunately, its accuracy of pose estimation depends on the center estimation.

Mb-ICP is designed for the rotation estimating issue of ICP. Unlike ICP, Mb-ICP adopts a new metric distance to establish the correspondences, which is defined in the configuration space of the sensor. It takes into account both the translation and rotation error of the sensor as shown in formula (1). Zhuang et al. presented a hybrid sensing system for mobile robot localization in an indoor, semistructured environment based on the Mb-ICP algorithm, which is a typical application of Mb-ICP [16]:

However, for Mb-ICP, as the number of pose estimation iterations increases, the rotation angle is changeable whereas remains constant; this discrepancy may lead to mismatching during the pose estimation process. Moreover, like ICP, Mb-ICP prefers unstructured environments.

In recent years, some scan matching applications in Cartesian coordinate frame have been proposed. For example, Pedraza et al. proposed an approach called BS-SLAM [17], which uses B-spline [18] to describe the environment in which equal-interval control points are viewed as landmarks. His correspondences establishing method is realized by calculating the smallest Euclidean distance between the control points of the reference and current scan. Its performance depends on the selected endpoints and the controlled points of the spline. This algorithm is suitable for environments that cannot be partially occluded. Olson proposed a probabilistically motivated scan matching algorithm [19], which produces higher quality and more robust results at the cost of additional computational time. Grzonka et al. use this laser scan matching method to solve the indoor navigation of aerial vehicles [20]. The 6-degree-of-freedom navigation problem can be solved using laser scan matching because the aerial vehicle they used carried multiple sensors.

##### 2.2. Scan Matching in Polar Coordinate Frame

The rotation and scaling of data in polar coordinate frame are easier to be accomplished than in Cartesian coordinate frame [21]. For example, three squares in Cartesian coordinate frame in Figure 1(a) are converted to the curves in the polar coordinate frame in Figure 1(b). A rotation or a scaling in Cartesian coordinate frame can be viewed as the translation in direction or direction in polar coordinate frame shown in Figure 1.