SC-KF Mobile Robot Localization: A Stochastic Cloning Kalman Filter for Processing Relative-State Measurements

This paper presents a new method to optimally combine motion measurements provided by proprioceptive sensors, with relative-state estimates inferred from feature-based matching. Two key challenges arise in such pose tracking problems: 1) the displacement estimates relate the state of the robot at two different time instants and 2) the same exteroceptive measurements are often used for computing consecutive displacement estimates, a process that renders the errors in these correlated. We present a novel stochastic cloning Kalman filtering (SC-KF) estimation algorithm that successfully addresses these challenges, while still allowing for efficient calculation of the filter gains and covariances. The proposed algorithm is not intended to compete with simultaneous localization and mapping (SLAM) approaches. Instead, it can be merged with any extended-Kalman- filter-based SLAM algorithm to increase its precision. In this respect, the SC-KF provides a robust framework for leveraging additional motion information extracted from dense point features that most SLAM algorithms do not treat as landmarks. Extensive experimental and simulation results are presented to verify the validity of the proposed method and to demonstrate that its performance is superior to that of alternative position-tracking approaches.

[1]  Michael Bosse,et al.  Mapping Partially Observable Features from Multiple Uncertain Vantage Points , 2002, Int. J. Robotics Res..

[2]  D.S. Bayard,et al.  An estimation algorithm for vision-based exploration of small bodies in space , 2005, Proceedings of the 2005, American Control Conference, 2005..

[3]  Philip H. S. Torr,et al.  The Development and Comparison of Robust Methods for Estimating the Fundamental Matrix , 1997, International Journal of Computer Vision.

[4]  David M. Bradley,et al.  Scan matching for flooded subterranean voids , 2004, IEEE Conference on Robotics, Automation and Mechatronics, 2004..

[5]  John J. Leonard,et al.  Explore and return: experimental validation of real-time concurrent mapping and localization , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[6]  Sebastian Thrun,et al.  FastSLAM: a factored solution to the simultaneous localization and mapping problem , 2002, AAAI/IAAI.

[7]  Stergios I. Roumeliotis,et al.  Weighted line fitting algorithms for mobile robot map building and efficient data representation , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[8]  Clark F. Olson,et al.  Robust stereo ego-motion for long distance navigation , 2000, Proceedings IEEE Conference on Computer Vision and Pattern Recognition. CVPR 2000 (Cat. No.PR00662).

[9]  Paul S. Schenker,et al.  Improved Rover State Estimation in Challenging Terrain , 1999, Auton. Robots.

[10]  Jeffrey K. Uhlmann,et al.  Simultaneous localisation and map building using split covariance intersection , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[11]  Xavier Cufí,et al.  Augmented state Kalman filtering for AUV navigation , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[12]  Randall Smith,et al.  Estimating Uncertain Spatial Relationships in Robotics , 1987, Autonomous Robot Vehicles.

[13]  Peter C. Cheeseman,et al.  Estimating uncertain spatial relationships in robotics , 1986, Proceedings. 1987 IEEE International Conference on Robotics and Automation.

[14]  Juan D. Tardós,et al.  Data association in stochastic mapping using the joint compatibility test , 2001, IEEE Trans. Robotics Autom..

[15]  Andrew Howard,et al.  Multi-robot mapping using manifold representations , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[16]  Stergios I. Roumeliotis,et al.  Weighted range sensor matching algorithms for mobile robot displacement estimation , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[17]  Alonzo Kelly.  General solution for linearized systematic error propagation in vehicle odometry , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[18]  Stergios I. Roumeliotis,et al.  Stochastic cloning: a generalized framework for processing relative state measurements , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[19]  Kurt Konolige,et al.  Large-Scale Map-Making , 2004, AAAI.

[20]  Juan D. Tardós,et al.  Fast localization of avalanche victims using sum of Gaussians , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[21]  Evangelos E. Milios,et al.  Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans , 1994, 1994 Proceedings of IEEE Conference on Computer Vision and Pattern Recognition.

[22]  Hugh F. Durrant-Whyte,et al.  Simultaneous Mapping and Localization with Sparse Extended Information Filters: Theory and Initial Results , 2004, WAFR.

[23]  John J. Leonard,et al.  Incorporation of Delayed Decision Making into Stochastic Mapping , 2000, ISER.

[24]  Saied Moezzi,et al.  Dynamic stereo vision , 1992 .

[25]  Stergios I. Roumeliotis,et al.  A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[26]  Tim Bailey.  Constrained initialisation for bearing-only SLAM , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[27]  Jeffrey K. Uhlmann,et al.  A non-divergent estimation algorithm in the presence of unknown correlations , 1997, Proceedings of the 1997 American Control Conference (Cat. No.97CH36041).

[28]  E. J. Lefferts,et al.  Kalman Filtering for Spacecraft Attitude Estimation , 1982 .

[29]  Ingemar J. Cox,et al.  Autonomous Robot Vehicles , 1990, Springer New York.

[30]  Stergios I. Roumeliotis,et al.  Augmenting inertial navigation with image-based motion estimation , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[31]  Evangelos E. Milios,et al.  Globally Consistent Range Scan Alignment for Environment Mapping , 1997, Auton. Robots.

[32]  Hanumant Singh,et al.  Exactly Sparse Delayed-State Filters , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[33]  Stergios I. Roumeliotis,et al.  On the treatment of relative-pose measurements for mobile robot localization , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[34]  Hanumant Singh,et al.  Visually augmented navigation in an unstructured environment using a delayed state history , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[35]  Gamini Dissanayake,et al.  An efficient multiple hypothesis filter for bearing-only SLAM , 2004, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566).