Simultaneous Localization, Mapping and Moving Object Tracking

Simultaneous localization, mapping and moving object tracking (SLAMMOT) involves both simultaneous localization and mapping (SLAM) in dynamic environments and detecting and tracking these dynamic objects. In this paper, a mathematical framework is established to integrate SLAM and moving object tracking. Two solutions are described: SLAM with generalized objects, and SLAM with detection and tracking of moving objects (DATMO). SLAM with generalized objects calculates a joint posterior over all generalized objects and the robot. Such an approach is similar to existing SLAM algorithms, but with additional structure to allow for motion modeling of generalized objects. Unfortunately, it is computationally demanding and generally infeasible. SLAM with DATMO decomposes the estimation problem into two separate estimators. By maintaining separate posteriors for stationary objects and moving objects, the resulting estimation problems are much lower dimensional than SLAM with generalized objects. Both SLAM and moving object tracking from a moving vehicle in crowded urban areas are daunting tasks. Based on the SLAM with DATMO framework, practical algorithms are proposed which deal with issues of perception modeling, data association, and moving object detection. The implementation of SLAM with DATMO was demonstrated using data collected from the CMU Navlab11 vehicle at high speeds in crowded urban environments. Ample experimental results shows the feasibility of the proposed theory and algorithms.

[1]  Stuart J. Russell,et al.  Dynamic bayesian networks: representation, inference and learning , 2002 .

[2]  Hugh F. Durrant-Whyte,et al.  Field Robots , 2001, ISRR.

[3]  Wolfram Burgard,et al.  A real-time algorithm for mobile robot mapping with applications to multi-robot and 3D mapping , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[4]  Aubrey B. Poore,et al.  Multidimensional assignment formulation of data association problems arising from multitarget and multisensor tracking , 1994, Comput. Optim. Appl..

[5]  Takeo Kanade,et al.  Fast and accurate shape-based registration , 1996 .

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

[7]  Michael Isard,et al.  Learning Multi-Class Dynamics , 1998, NIPS.

[8]  Sebastian Thrun,et al.  Multi-robot SLAM with Sparse Extended Information Filers , 2003, ISRR.

[9]  Rudolph van der Merwe,et al.  The unscented Kalman filter for nonlinear estimation , 2000, Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373).

[10]  Robert W. Sittler,et al.  An Optimal Data Association Problem in Surveillance Theory , 1964, IEEE Transactions on Military Electronics.

[11]  S. R. Rogers,et al.  Tracking multiple targets with correlated measurements and maneuvers , 1988 .

[12]  Martial Hebert.  Active and passive range sensing for robotics , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[13]  Paul J. Besl,et al.  A Method for Registration of 3-D Shapes , 1992, IEEE Trans. Pattern Anal. Mach. Intell..

[14]  Gaurav S. Sukhatme,et al.  Mobile Robot Simultaneous Localization and Mapping in Dynamic Environments , 2005, Auton. Robots.

[15]  Randall Smith,et al.  Estimating uncertain spatial relationships in robotics , 1986, Proceedings. 1987 IEEE International Conference on Robotics and Automation.

[16]  Sebastian Thrun,et al.  Learning Hierarchical Object Maps of Non-Stationary Environments with Mobile Robots , 2002, UAI.

[17]  Joelle Pineau,et al.  An integrated approach to hierarchy and abstraction for pomdps , 2002 .

[18]  Wolfram Burgard,et al.  Monte Carlo Localization: Efficient Position Estimation for Mobile Robots , 1999, AAAI/IAAI.

[19]  Trevor Hastie,et al.  The Elements of Statistical Learning , 2001 .

[20]  Howie Choset,et al.  Accurate relative localization using odometry , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[21]  Michael Bosse,et al.  An Atlas framework for scalable mapping , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[22]  S. W. Shaw,et al.  Efficient target tracking using dynamic programming , 1993 .

[23]  Albert-Jan Baerveldt,et al.  Localization in changing environments - estimation of a covariance matrix for the IDC algorithm , 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).

[24]  Eduardo Mario Nebot,et al.  Optimization of the simultaneous localization and map-building algorithm for real-time implementation , 2001, IEEE Trans. Robotics Autom..

[25]  John J. Leonard,et al.  Pure range-only sub-sea SLAM , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[26]  Hugh Durrnat-Whyte,et al.  A Critical Review of the State-of-the-Art in Autonomous Land Vehicle Systems and Technology , 2001 .

[27]  Robert Fitzgerald.  Simple Tracking Filters: Steady-State Filtering and Smoothing Performance , 1980, IEEE Transactions on Aerospace and Electronic Systems.

[28]  Thiagalingam Kirubarajan,et al.  Tracking evasive move-stop-move targets with a MTI radar using a VS-IMM estimator , 2000, SPIE Defense + Commercial Sensing.

[29]  Marc Levoy,et al.  Zippered polygon meshes from range images , 1994, SIGGRAPH.

[30]  Henning Biermann,et al.  Recovering non-rigid 3D shape from image streams , 2000, Proceedings IEEE Conference on Computer Vision and Pattern Recognition. CVPR 2000 (Cat. No.PR00662).

[31]  Wolfram Burgard,et al.  A Probabilistic Approach to Concurrent Mapping and Localization for Mobile Robots , 1998, Auton. Robots.

[32]  Naonori Ueda,et al.  Bayesian model search for mixture models based on optimizing variational bounds , 2002, Neural Networks.

[33]  Kurt Konolige,et al.  Markov Localization using Correlation , 1999, IJCAI.

[34]  Eduardo Mario Nebot,et al.  High accuracy navigation using laser range sensors in outdoor applications , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[35]  G. Champleboux,et al.  From accurate range imaging sensor calibration to accurate model-based 3D object localization , 1992, Proceedings 1992 IEEE Computer Society Conference on Computer Vision and Pattern Recognition.

[36]  Clark F. Olson,et al.  Probabilistic self-localization for mobile robots , 2000, IEEE Trans. Robotics Autom..

[37]  Thor I. Fossen,et al.  Marine Control Systems Guidance, Navigation, and Control of Ships, Rigs and Underwater Vehicles , 2002 .

[38]  Herbert Gish,et al.  Target State Estimation in a Multitarget Environment Using Multiple Sensors , 1987, IEEE Transactions on Aerospace and Electronic Systems.

[39]  Hong-Tzong Yau,et al.  Automated precision measurement of surface profile in CAD-directed inspection , 1992, IEEE Trans. Robotics Autom..

[40]  John J. Leonard,et al.  A Computationally Efficient Method for Large-Scale Concurrent Mapping and Localization , 2000 .

[41]  Yakov Bar-Shalom,et al.  Multitarget-Multisensor Tracking: Principles and Techniques , 1995 .

[42]  Larry H. Matthies,et al.  Integration of sonar and stereo range data using a grid-based representation , 1988, Proceedings. 1988 IEEE International Conference on Robotics and Automation.

[43]  Wolfram Burgard,et al.  Map building with mobile robots in dynamic environments , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[44]  Jing Xiao,et al.  A Closed-Form Solution to Non-rigid Shape and Motion Recovery , 2004, ECCV.

[45]  Charles E. Thorpe,et al.  Simultaneous localization and mapping with detection and tracking of moving objects , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[46]  David J. C. MacKay,et al.  Information Theory, Inference, and Learning Algorithms , 2004, IEEE Transactions on Information Theory.

[47]  Hugh F. Durrant-Whyte,et al.  Simultaneous map building and localization for an autonomous mobile robot , 1991, Proceedings IROS '91:IEEE/RSJ International Workshop on Intelligent Robots and Systems '91.

[48]  Donald McLean,et al.  Automatic Flight Control Systems , 1990 .

[49]  Ingemar J. Cox,et al.  An Efficient Implementation of Reid's Multiple Hypothesis Tracking Algorithm and Its Evaluation for the Purpose of Visual Tracking , 1996, IEEE Trans. Pattern Anal. Mach. Intell..

[50]  Wolfram Burgard,et al.  Towards Lazy Data Association in SLAM , 2003, ISRR.

[51]  Hugh F. Durrant-Whyte,et al.  A Bayesian Algorithm for Simultaneous Localisation and Map Building , 2001, ISRR.

[52]  Rae-Hong Park,et al.  An Orientation Reliability Matrix for the Iterative Closest Point Algorithm , 2000, IEEE Trans. Pattern Anal. Mach. Intell..

[53]  Sebastian Thrun,et al.  Detecting and modeling doors with mobile robots , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[54]  Peter Cheeseman,et al.  On the Representation and Estimation of Spatial Uncertainty , 1986 .

[55]  Charles E. Thorpe,et al.  A hierarchical object based representation for simultaneous localization and mapping , 2004, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566).

[56]  I. Kadar,et al.  Tracking ground targets with road constraints using an IMM estimator , 1998, 1998 IEEE Aerospace Conference Proceedings (Cat. No.98TH8339).

[57]  Wolfram Burgard,et al.  Map building with mobile robots in populated environments , 2002, IEEE/RSJ International Conference on Intelligent Robots and Systems.

[58]  Hans P. Moravec,et al.  Robot Evidence Grids. , 1996 .

[59]  Alonzo Kelly,et al.  Efficient Construction of Globally Consistent Ladar Maps Using Pose Network Topology and Nonlinear Programming , 2003, ISRR.

[60]  Sebastian Thrun,et al.  An Approximate Bayesian Method for Simultaneous Localisation and Mapping , 2002 .

[61]  Amir Averbuch,et al.  Interacting Multiple Model Methods in Target Tracking: A Survey , 1988 .

[62]  Andrew W. Fitzgibbon,et al.  An Experimental Comparison of Range Image Segmentation Algorithms , 1996, IEEE Trans. Pattern Anal. Mach. Intell..

[63]  T. Zadra,et al.  Precision tracking of ground targets , 2000, 2000 IEEE Aerospace Conference. Proceedings (Cat. No.00TH8484).

[64]  Geoffrey E. Hinton,et al.  Variational Learning for Switching State-Space Models , 2000, Neural Computation.

[65]  Adrian Hilton,et al.  Estimating pose uncertainty for surface registration , 1998, Image Vis. Comput..

[66]  Marc Levoy,et al.  Efficient variants of the ICP algorithm , 2001, Proceedings Third International Conference on 3-D Digital Imaging and Modeling.

[67]  Chieh-Chih Wang,et al.  LADAR-based detection and tracking of moving objects from a ground vehicle at high speeds , 2003, IEEE IV2003 Intelligent Vehicles Symposium. Proceedings (Cat. No.03TH8683).

[68]  Stefano Coraluppi,et al.  Multiple-Hypothesis IMM ( MH-IMM ) Filter for Moving and Stationary Targets , 2001 .

[69]  Yaakov Bar-Shalom,et al.  Estimation and Tracking: Principles, Techniques, and Software , 1993 .

[70]  Sebastian Thrun,et al.  Online simultaneous localization and mapping with detection and tracking of moving objects: theory and results from a ground vehicle in crowded urban areas , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[71]  Jo Yung Wong,et al.  Theory of ground vehicles , 1978 .

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

[73]  Kurt Konolige,et al.  Incremental mapping of large cyclic environments , 1999, Proceedings 1999 IEEE International Symposium on Computational Intelligence in Robotics and Automation. CIRA'99 (Cat. No.99EX375).

[74]  Y. Bar-Shalom,et al.  Multiple-model estimation with variable structure , 1996, IEEE Trans. Autom. Control..

[75]  Kurt Konolige,et al.  The Revisiting Problem in Mobile Robot Map Building: A Hierarchical Bayesian Approach , 2002, UAI.

[76]  X. R. Li,et al.  Tracking in clutter with strongest neighbor measurements. I. Theoretical analysis , 1998, IEEE Trans. Autom. Control..

[77]  Alberto Elfes,et al.  Sonar-based real-world mapping and navigation , 1987, IEEE J. Robotics Autom..

[78]  Keiji Nagatani,et al.  Topological simultaneous localization and mapping (SLAM): toward exact localization without explicit localization , 2001, IEEE Trans. Robotics Autom..

[79]  Geoffrey J. Gordon,et al.  Finding Approximate POMDP solutions Through Belief Compression , 2011, J. Artif. Intell. Res..

[80]  Yaakov Bar-Shalom,et al.  Multitarget-Multisensor Tracking: Applications and Advances , 1992 .

[81]  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.

[82]  Shao-Wen Yang,et al.  Interacting Object Tracking in Crowded Urban Areas , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[83]  Radu Horaud,et al.  Motion-Egomotion Discrimination and Motion Segmentation from Image-Pair Streams , 2000, Comput. Vis. Image Underst..

[84]  Ryosuke Shibasaki,et al.  Reconstructing urban 3D model using vehicle-borne laser range scanners , 2001, Proceedings Third International Conference on 3-D Digital Imaging and Modeling.

[85]  Michael Garland,et al.  Simplifying surfaces with color and texture using quadric error metrics , 1998, Proceedings Visualization '98 (Cat. No.98CB36276).

[86]  Matthew Brand,et al.  Morphable 3D models from video , 2001, Proceedings of the 2001 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. CVPR 2001.

[87]  Wolfram Burgard,et al.  Tracking multiple moving targets with a mobile robot using particle filters and statistical data association , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[88]  X. Rong Li,et al.  Mode-Set Adaptation in Multiple-Model Estimators for Hybrid Systems , 1992, 1992 American Control Conference.

[89]  Luis Montesano,et al.  Modeling the Static and the Dynamic Parts of the Environment to Improve Sensor-based Navigation , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[90]  Jitendra Tugnait.  Detection and estimation for abruptly changing systems , 1981, CDC 1981.

[91]  Wolfram Burgard,et al.  Using the CONDENSATION algorithm for robust, vision-based mobile robot localization , 1999, Proceedings. 1999 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (Cat. No PR00149).

[92]  Mark A. Paskin,et al.  Thin Junction Tree Filters for Simultaneous Localization and Mapping , 2002, IJCAI.

[93]  Chee-Yee Chong,et al.  Ground target tracking-a historical perspective , 2000, 2000 IEEE Aerospace Conference. Proceedings (Cat. No.00TH8484).

[94]  Michael Potmesil,et al.  Generating Models of Solid Objects by Matching 3D Surface Segments , 1983, IJCAI.

[95]  Lorenzo Torresani,et al.  Tracking and modeling non-rigid objects with rank constraints , 2001, Proceedings of the 2001 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. CVPR 2001.

[96]  Xavier Pennec,et al.  A Framework for Uncertainty and Validation of 3-D Registration Methods Based on Points and Frames , 2004, International Journal of Computer Vision.

[97]  Zhengyou Zhang,et al.  Iterative point matching for registration of free-form curves and surfaces , 1994, International Journal of Computer Vision.

[98]  S. Thrun,et al.  Simultaneous Localization, Mapping and Moving Object Tracking Simultaneous Localization, Mapping and Moving Object Tracking , 2007 .

[99]  Martial Hebert,et al.  Bearings-only localization and mapping , 2005 .

[100]  Yaakov Bar-Shalom,et al.  Sonar tracking of multiple targets using joint probabilistic data association , 1983 .

[101]  Sanjiv Singh,et al.  Preliminary results in range-only localization and mapping , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[102]  Hugh F. Durrant-Whyte,et al.  On the role of process models in autonomous land vehicle navigation systems , 2003, IEEE Trans. Robotics Autom..

[103]  Y. Bar-Shalom,et al.  The interacting multiple model algorithm for systems with Markovian switching coefficients , 1988 .

[104]  Stefano Coraluppi,et al.  A hybrid-state estimation algorithm for multisensor target tracking , 2000, Proceedings of the Third International Conference on Information Fusion.

[105]  Vladimir Pavlovic,et al.  Time-series classification using mixed-state dynamic Bayesian networks , 1999, Proceedings. 1999 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (Cat. No PR00149).

[106]  Gérard G. Medioni,et al.  Object modeling by registration of multiple range images , 1991, Proceedings. 1991 IEEE International Conference on Robotics and Automation.

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

[108]  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).

[109]  Sebastian Thrun,et al.  Robotic mapping: a survey , 2003 .