Simultaneous Trajectory Estimation and Planning via Probabilistic Inference

We provide a unified probabilistic framework for trajectory estimation and planning. The key idea is to view these two problems, usually considered separately, as a single problem. At each time-step the robot is tasked with finding the complete continuous-time trajectory from start to goal. This can be quite difficult; the robot must contend with a potentially high-degreeof-freedom (DOF) trajectory space, uncertainty due to limited sensing capabilities, model inaccuracy, and the stochastic effect of executing actions, and the robot must find the solution in (faster than) real time. To overcome these challenges, we build on recent probabilistic inference approaches to continuous-time localization and mapping and continuous-time motion planning. We solve the joint problem by iteratively recomputing the maximum a posteriori trajectory conditioned on all available sensor data and cost information. Finally, we evaluate our framework empirically in both simulation and on a mobile manipulator.

[1]  Frank Dellaert,et al.  A factor graph approach to estimation and model predictive control on Unmanned Aerial Vehicles , 2014, 2014 International Conference on Unmanned Aircraft Systems (ICUAS).

[2]  Byron Boots,et al.  Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups , 2017, ArXiv.

[3]  Nidhi Kalra,et al.  Replanning with RRTs , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[4]  Panganamala Ramana Kumar,et al.  Non-Gaussian SLAP: Simultaneous Localization and Planning Under Non-Gaussian Uncertainty in Static and Dynamic Environments , 2016, ArXiv.

[5]  Simo Särkkä,et al.  Batch nonlinear continuous-time trajectory estimation as exactly sparse Gaussian process regression , 2014, Autonomous Robots.

[6]  Frank Dellaert,et al.  The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping , 2010, WAFR.

[7]  Frank Dellaert,et al.  Incremental smoothing and mapping , 2008 .

[8]  Brendan J. Frey,et al.  Factor graphs and the sum-product algorithm , 2001, IEEE Trans. Inf. Theory.

[9]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.

[10]  Tim D. Barfoot,et al.  Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE(3) , 2015, 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[11]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[12]  Emanuel Todorov,et al.  General duality between optimal control and estimation , 2008, 2008 47th IEEE Conference on Decision and Control.

[13]  Marc Toussaint,et al.  Robot trajectory optimization using approximate inference , 2009, ICML '09.

[14]  Marc Toussaint,et al.  Probabilistic inference for solving discrete and continuous state Markov Decision Processes , 2006, ICML.

[15]  Byron Boots,et al.  Gaussian Process Motion planning , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[16]  Byron Boots,et al.  Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs , 2016, Robotics: Science and Systems.

[17]  Sergey Levine,et al.  Variational Policy Search via Trajectory Optimization , 2013, NIPS.

[18]  Morgan Quigley,et al.  ROS: an open-source Robot Operating System , 2009, ICRA 2009.

[19]  Kevin Skadron,et al.  Scalable parallel programming , 2008, 2008 IEEE Hot Chips 20 Symposium (HCS).

[20]  Andrew W. Fitzgibbon,et al.  KinectFusion: real-time 3D reconstruction and interaction using a moving depth camera , 2011, UIST.

[21]  Nancy M. Amato,et al.  Simultaneous Localization and Planning for Physical Mobile Robots via Enabling Dynamic Replanning in Belief Space , 2015, ArXiv.

[22]  Byron Boots,et al.  4D crop monitoring: Spatio-temporal reconstruction for agriculture , 2016, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[23]  Frank Dellaert,et al.  Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing , 2006, Int. J. Robotics Res..

[24]  Byron Boots,et al.  Incremental sparse GP regression for continuous-time trajectory estimation and mapping , 2017, Robotics Auton. Syst..

[25]  Byron Boots,et al.  Motion planning with graph-based trajectories and Gaussian process inference , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[26]  Daniel P. Huttenlocher,et al.  Distance Transforms of Sampled Functions , 2012, Theory Comput..

[27]  Sven Koenig,et al.  Fast replanning for navigation in unknown terrain , 2005, IEEE Transactions on Robotics.

[28]  Simo Särkkä,et al.  Batch Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression , 2014, Robotics: Science and Systems.

[29]  Marc Toussaint,et al.  On Stochastic Optimal Control and Reinforcement Learning by Approximate Inference , 2012, Robotics: Science and Systems.

[30]  Hagai Attias,et al.  Planning by Probabilistic Inference , 2003, AISTATS.

[31]  T. Başar,et al.  A New Approach to Linear Filtering and Prediction Problems , 2001 .

[32]  Paul Timothy Furgale,et al.  Gaussian Process Gauss–Newton for non-parametric simultaneous localization and mapping , 2013, Int. J. Robotics Res..

[33]  Byron Boots,et al.  Approximately optimal continuous-time motion planning and control via Probabilistic Inference , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[34]  William D. Penny.  Simultaneous Localisation and Planning , 2014, 2014 4th International Workshop on Cognitive Information Processing (CIP).

[35]  Frank Dellaert,et al.  iSAM: Incremental Smoothing and Mapping , 2008, IEEE Transactions on Robotics.