Active SLAM over Continuous Trajectory and Control: A Covariance-Feedback Approach

This paper proposes a novel active Simultaneous Localization and Mapping (SLAM) method with continuous trajectory optimization over a stochastic robot dynamics model. The problem is formalized as a stochastic optimal control over the continuous robot kinematic model to minimize a cost function that involves the covariance matrix of the landmark states. We tackle the problem by separately obtaining an openloop control sequence subject to deterministic dynamics by iterative Covariance Regulation (iCR) and a closed-loop feedback control under stochastic robot and covariance dynamics by Linear Quadratic Regulator (LQR). The proposed optimization method captures the coupling between localization and mapping in predicting uncertainty evolution and synthesizes highly informative sensing trajectories. We demonstrate its performance in active landmark-based SLAM using relativeposition measurements with a limited field of view.

[1]  Vijay Kumar,et al.  Information-theoretic mapping using Cauchy-Schwarz Quadratic Mutual Information , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[2]  Nancy M. Amato,et al.  SLAP: Simultaneous Localization and Planning Under Uncertainty via Dynamic Replanning in Belief Space , 2018, IEEE Transactions on Robotics.

[3]  George J. Pappas,et al.  Asymptotically Optimal Planning for Non-Myopic Multi-Robot Information Gathering , 2019, Robotics: Science and Systems.

[4]  John J. Leonard,et al.  Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age , 2016, IEEE Transactions on Robotics.

[5]  Ron Alterovitz,et al.  Motion planning under uncertainty using iterative local optimization in belief space , 2012, Int. J. Robotics Res..

[6]  George J. Pappas,et al.  Decentralized active information acquisition: Theory and application to multi-robot SLAM , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[7]  Davide Scaramuzza,et al.  Active Autonomous Aerial Exploration for Ground Robot Path Planning , 2017, IEEE Robotics and Automation Letters.

[8]  Nancy M. Amato,et al.  FIRM: Feedback controller-based information-state roadmap - A framework for motion planning under uncertainty , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[9]  William Whittaker,et al.  Autonomous exploration using multiple sources of information , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[10]  Basilio Bona,et al.  Active SLAM and Exploration with Particle Filters Using Kullback-Leibler Divergence , 2014, J. Intell. Robotic Syst..

[11]  George J. Pappas,et al.  Information acquisition with sensing robots: Algorithms and error bounds , 2013, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[12]  Ben Grocholsky,et al.  Information-Theoretic Control of Multiple Sensor Platforms , 2002 .

[13]  Nikolay Atanasov,et al.  Active Exploration and Mapping via Iterative Covariance Regulation over Continuous SE(3) Trajectories , 2021, 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[14]  Nando de Freitas,et al.  A Bayesian exploration-exploitation approach for optimal online sensing and planning with a visually guided mobile robot , 2009, Auton. Robots.

[15]  E. Todorov,et al.  A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems , 2005, Proceedings of the 2005, American Control Conference, 2005..

[16]  George J. Pappas,et al.  On trajectory optimization for active sensing in Gaussian process models , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[17]  Geoffrey A. Hollinger,et al.  Sampling-based robotic information gathering algorithms , 2014, Int. J. Robotics Res..

[18]  Andrej Pázman,et al.  Foundations of Optimum Experimental Design , 1986 .

[19]  John J. Leonard,et al.  Advances in Inference and Representation for Simultaneous Localization and Mapping , 2021, Annu. Rev. Control. Robotics Auton. Syst..

[20]  Panagiotis Tsiotras,et al.  Belief Space Planning: a Covariance Steering Approach , 2021, 2022 International Conference on Robotics and Automation (ICRA).

[21]  Ian D. Reid,et al.  On the comparison of uncertainty criteria for active SLAM , 2012, 2012 IEEE International Conference on Robotics and Automation.

[22]  Gamini Dissanayake,et al.  Active SLAM using Model Predictive Control and Attractor based Exploration , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[23]  Alberto Elfes,et al.  Robot Navigation: Integrating Perception, Environmental Constraints and Task Execution Within a Probabilistic Framework , 1995, Reasoning with Uncertainty in Robotics.

[24]  Brian Yamauchi,et al.  A frontier-based approach for autonomous exploration , 1997, Proceedings 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA'97. 'Towards New Computational Principles for Robotics and Automation'.

[25]  Vivienne Sze,et al.  FSMI: Fast computation of Shannon mutual information for information-theoretic mapping , 2019, 2019 International Conference on Robotics and Automation (ICRA).

[26]  N. Roy,et al.  The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance , 2009, Int. J. Robotics Res..

[27]  George J. Pappas,et al.  Information Theoretic Active Exploration in Signed Distance Fields , 2020, 2020 IEEE International Conference on Robotics and Automation (ICRA).

[28]  Steven L. Waslander,et al.  Uncertainty-Constrained Differential Dynamic Programming in Belief Space for Vision Based Robots , 2020, IEEE Robotics and Automation Letters.

[29]  Nikolay Atanasov,et al.  Active Bayesian Multi-class Mapping from Range and Semantic Segmentation Observations , 2021, 2021 IEEE International Conference on Robotics and Automation (ICRA).