A factor graph approach to estimation and model predictive control on Unmanned Aerial Vehicles

In this paper, we present a factor graph framework to solve both estimation and deterministic optimal control problems, and apply it to an obstacle avoidance task on Unmanned Aerial Vehicles (UAVs). We show that factor graphs allow us to consistently use the same optimization method, system dynamics, uncertainty models and other internal and external parameters, which potentially improves the UAV performance as a whole. To this end, we extended the modeling capabilities of factor graphs to represent nonlinear dynamics using constraint factors. For inference, we reformulate Sequential Quadratic Programming as an optimization algorithm on a factor graph with nonlinear constraints. We demonstrate our framework on a simulated quadrotor in an obstacle avoidance application.

[1]  W. Freeman,et al.  Generalized Belief Propagation , 2000, NIPS.

[2]  Frank Dellaert,et al.  Multi-level submap based SLAM using nested dissection , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[3]  Daniel P. Huttenlocher,et al.  Efficient Graph-Based Image Segmentation , 2004, International Journal of Computer Vision.

[4]  Robert F. Stengel,et al.  Optimal Control and Estimation , 1994 .

[5]  O. Khatib,et al.  Real-Time Obstacle Avoidance for Manipulators and Mobile Robots , 1985, Proceedings. 1985 IEEE International Conference on Robotics and Automation.

[6]  Peter S. Maybeck,et al.  Stochastic Models, Estimation And Control , 2012 .

[7]  Yoram Koren,et al.  Potential field methods and their inherent limitations for mobile robot navigation , 1991, Proceedings. 1991 IEEE International Conference on Robotics and Automation.

[8]  X. Jin.  Factor graphs and the Sum-Product Algorithm , 2002 .

[9]  E. Todorov.  Optimality principles in sensorimotor control , 2004, Nature Neuroscience.

[10]  Adnan Darwiche,et al.  Modeling and Reasoning with Bayesian Networks , 2009 .

[11]  Shuzhi Sam Ge,et al.  Dynamic Motion Planning for Mobile Robots Using Potential Field Method , 2002, Auton. Robots.

[12]  Frank Dellaert,et al.  Generalized subgraph preconditioners for large-scale bundle adjustment , 2011, Vision.

[13]  A. Iserles,et al.  Lie-group methods , 2000, Acta Numerica.

[14]  Anil V. Rao,et al.  Practical Methods for Optimal Control Using Nonlinear Programming , 1987 .

[15]  G. Chirikjian.  Stochastic models, information theory, and lie groups , 2012 .

[16]  Nicolas Petit,et al.  The Navigation and Control technology inside the AR.Drone micro UAV , 2011 .

[17]  Stephen J. Wright,et al.  Numerical Optimization , 2018, Fundamental Statistical Inference.

[18]  E. Celledoni,et al.  Lie group methods for rigid body dynamics and time integration on manifolds , 2003 .

[19]  Stephen J. Wright,et al.  Numerical Optimization (Springer Series in Operations Research and Financial Engineering) , 2000 .

[20]  E. Hairer,et al.  Geometric Numerical Integration , 2022, Oberwolfach Reports.

[21]  Wolfram Burgard,et al.  Probabilistic Robotics (Intelligent Robotics and Autonomous Agents) , 2005 .

[22]  Frank Dellaert,et al.  Subgraph-preconditioned conjugate gradients for large scale SLAM , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[23]  H. Kappen.  A path integral approach to agent planning , 2022 .

[24]  Hugh F. Durrant-Whyte,et al.  Simultaneous localization and mapping: part I , 2006, IEEE Robotics & Automation Magazine.

[25]  Frank Dellaert,et al.  Information fusion in navigation systems via factor graph based incremental smoothing , 2013, Robotics Auton. Syst..

[26]  Hugh Durrant-Whyte,et al.  Simultaneous Localisation and Mapping ( SLAM ) : Part I The Essential Algorithms , 2006 .

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

[28]  Magnus Egerstedt,et al.  Adaptive look-ahead for robotic navigation in unknown environments , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[29]  Nir Friedman,et al.  Probabilistic Graphical Models - Principles and Techniques , 2009 .

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

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

[32]  N. Roy,et al.  On Stochastic Optimal Control and Reinforcement Learning by Approximate Inference , 2013 .

[33]  M. Botvinick,et al.  Planning as inference , 2012, Trends in Cognitive Sciences.

[34]  Stefan Schaal,et al.  A Generalized Path Integral Control Approach to Reinforcement Learning , 2010, J. Mach. Learn. Res..

[35]  Hilbert J. Kappen,et al.  Graphical Model Inference in Optimal Control of Stochastic Multi-Agent Systems , 2008, J. Artif. Intell. Res..

[36]  Vicenç Gómez,et al.  Optimal control as a graphical model inference problem , 2009, Machine Learning.

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

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

[39]  G. Chirikjian.  Stochastic Models, Information Theory, and Lie Groups, Volume 2 , 2012 .

[40]  Paul J. Werbos,et al.  2009 Special Issue: Intelligence in the brain: A theory of how it works and how to build it , 2009 .

[41]  Frank Dellaert,et al.  Out-of-Core Bundle Adjustment for Large-Scale 3D Reconstruction , 2007, 2007 IEEE 11th International Conference on Computer Vision.

[42]  John T. Betts,et al.  Practical Methods for Optimal Control and Estimation Using Nonlinear Programming , 2009 .

[43]  Judea Pearl,et al.  Probabilistic reasoning in intelligent systems - networks of plausible inference , 1991, Morgan Kaufmann series in representation and reasoning.