# An Improved MbICP Algorithm for Mobile Robot Pose Estimation
## 1 Introduction  
Simultaneous localization and mapping (SLAM) is one of the most important issues in mobile robotics research [1; 2; 3; 4; 5]. SLAM is a process by which a mobile robot can build a map of its environment and simultaneously use this map to locate itself [6]. Therefore, an accurate motion estimation of the robot will greatly facilitate the realization of this process and has attracted considerable attention from researchers in recent decades [1; 2; 7; 8; 9]. Scan-matching techniques are widely used to estimate mobile robot motion by matching successive range measurements from a laser range finder (LRF) [1; 2; 4; 9; 10; 11; 12; 13; 14; 15; 16; 17]. The iterative closest point (ICP) algorithm is one of the most popular scan-matching techniques because of its efficiency and simplicity [18; 19; 20; 21; 22]. This algorithm iteratively establishes correspondences based on the criterion of the closest point and calculates a rigid transformation that minimizes the distance error until convergence. However, ICP tends to fall into local optima when large sensor rotation is introduced, which frequently arises when a robot rotates around itself or unexpected slippage occurs [1]. Additionally, ICP utilizes a point-to-point metric to determine correspondences, which is less accurate because of the discreteness of laser scans. In addition, ICP relies on a questionable assumption that successive scans perfectly overlap [19; 20], which can hardly stand because of changes in the environment and occlusions fromvehicle motion [3; 20; 23]. False correspondences will arise in non-overlapping areas and thus cause the algorithm to converge upon an incorrect solution [20]. To solve the problem of scan matching with large rotation, [PERSON] proposed a metric-based ICP algorithm (MbICP) [10] that establishes correspondences and calculates transformations based on its defined metric-based distance, which simultaneously considers both translation and rotation. This algorithm is claimed [1; 2; 10] to be robust and accurate when the rotation change is large but still limited to matching scans that perfectly overlap. Considerable research has been conducted to improve ICP and its variants in matching partially overlapping scans by enhancing the performance of correspondence establishment and rejecting incorrect correspondences [19; 20; 22; 23; 24; 25; 26]. In terms of establishing correspondence, [PERSON] proposed a point-to-plane approach, which determines the correspondence by finding the projection of the source point onto the tangent plane at a destination surface point that is the intersection of the normal vector of the source point [27], but this approach is only available for scan matching with three-dimension (3D) laser scans. [PERSON] proposed a biunique correspondence ICP (BC-ICP) [20], which guarantees unique correspondence by searching for multiple closest points instead of one point. However, this strategy assumes that the corresponding point search begins from an overlapping area, which cannot be guaranteed in practice. In terms of rejecting incorrect correspondences, [PERSON] proposed a rejection strategy that was based on a statistical analysis of the correspondence distance, which adopted \(\mu+\sigma\) as the rejection threshold [28; 29; 30]. Nevertheless, this strategy assumes that the distribution of distances is Gaussian, which is not a reasonable assumption in a dynamic environment [22]. [PERSON] proposed a trimmed ICP algorithm (TrICP) [26], which uses golden-section searching to estimate an overlapping ratio and uses a trimmed least-squares approach to discard incorrect correspondence pairs. Despite the claimed robustness of this algorithm, TrICP is time consuming because it repeatedly searches for the best overlapping ratio [22; 24]. This algorithm may also be misled by a large change in the overlap because of occlusions from a moving platform.