Simultaneous self-calibration and navigation using trajectory optimization

We describe a trajectory optimization framework that maximizes observability of one or more user-chosen states in a nonlinear system. Our framework is based on a novel metric for quality of observability that is state-estimator agnostic and offers improved numerical stability over prior methods in some cases where the states of interest do not appear directly in the observation. We apply this metric to trajectory optimization problems for closed-loop self-calibration, maintaining observability while navigating through an environment, and rapidly modifying an already-planned trajectory for online recalibration. We include a statistical procedure to balance observability of several states with heterogeneous units and magnitudes. As an example, we apply our framework to online calibration of GPS–IMU and visual–inertial navigation systems on a quadrotor helicopter. Extensive simulations and a real-robot experiment demonstrate the effectiveness of our framework, showing better convergence of the states and the resulting higher precision in navigation.

[1]  Gaurav S. Sukhatme,et al.  Trajectory Optimization for Self-Calibration and Navigation , 2017, Robotics: Science and Systems.

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

[3]  Roland Siegwart,et al.  A robust and modular multi-sensor fusion approach applied to MAV navigation , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[4]  Hannes Sommer,et al.  Online self-calibration for robotic systems , 2016, Int. J. Robotics Res..

[5]  S. Sukkarieh,et al.  Observability analysis and active control for airborne SLAM , 2008, IEEE Transactions on Aerospace and Electronic Systems.

[6]  Fernando Nobre,et al.  Drift-correcting self-calibration for visual-inertial SLAM , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[7]  Robin Deits,et al.  Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming , 2014, WAFR.

[8]  Arthur J. Krener,et al.  Measures of unobservability , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[9]  B. Brumback,et al.  A Chi-square test for fault-detection in Kalman filters , 1987 .

[10]  G. Klein,et al.  Parallel Tracking and Mapping for Small AR Workspaces , 2007, 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.

[11]  Kristi A. Morgansen,et al.  Observability optimization for the nonholonomic integrator , 2013, 2013 American Control Conference.

[12]  Howie Choset,et al.  Use of the nonlinear observability rank condition for improved parametric estimation , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[13]  M. E. Flores.  Real-Time Trajectory Generation for Constrained Nonlinear Dynamical Systems Using Non-Uniform Rational B-Spline Basis Functions , 2008 .

[14]  Vijay Kumar,et al.  Minimum snap trajectory generation and control for quadrotors , 2011, 2011 IEEE International Conference on Robotics and Automation.

[15]  Gaurav S. Sukhatme,et al.  Observability-Aware Trajectory Optimization for Self-Calibration With Application to UAVs , 2016, IEEE Robotics and Automation Letters.

[16]  Nicholas Roy,et al.  Rapidly-exploring Random Belief Trees for motion planning under uncertainty , 2011, 2011 IEEE International Conference on Robotics and Automation.

[17]  Kristi A. Morgansen,et al.  Path planning to optimize observability in a planar uniform flow field , 2013, 2013 American Control Conference.

[18]  Roland Siegwart,et al.  Path planning for motion dependent state estimation on micro aerial vehicles , 2013, 2013 IEEE International Conference on Robotics and Automation.

[19]  Ron Alterovitz,et al.  High-Frequency Replanning Under Uncertainty Using Parallel Sampling-Based Motion Planning , 2015, IEEE Transactions on Robotics.

[20]  Philipp Birken,et al.  Numerical Linear Algebra , 2011, Encyclopedia of Parallel Computing.

[21]  Gaurav S. Sukhatme,et al.  Self-calibrating multi-sensor fusion with probabilistic measurement validation for seamless sensor switching on a UAV , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[22]  Robert L. Smith,et al.  Hit-and-Run Algorithms for Generating Multivariate Distributions , 1993, Math. Oper. Res..

[23]  Stefano Soatto,et al.  Observability, identifiability and sensitivity of vision-aided inertial navigation , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[24]  Vijay Kumar,et al.  Safe and complete trajectory generation for robot teams with higher-order dynamics , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[25]  Z. Botev.  The normal law under linear restrictions: simulation and estimation via minimax tilting , 2016, 1603.04166.

[26]  Dimitrios G. Kottas,et al.  Camera-IMU-based localization: Observability analysis and consistency improvement , 2014, Int. J. Robotics Res..

[27]  Roland Siegwart,et al.  Visual-inertial self-calibration on informative motion segments , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[28]  Emilio Frazzoli,et al.  Sampling-based algorithms for optimal motion planning , 2011, Int. J. Robotics Res..

[29]  Roland Siegwart,et al.  Sampling-based motion planning for active multirotor system identification , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[30]  Mircea D. Farcas,et al.  About Bernstein polynomials , 2008 .

[31]  Gaurav S. Sukhatme,et al.  Risk-aware trajectory generation with application to safe quadrotor landing , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[32]  Gaurav S. Sukhatme,et al.  Cooperative multi-robot control for target tracking with onboard sensing 1 , 2015, Int. J. Robotics Res..

[33]  John C. Butcher,et al.  A history of Runge-Kutta methods , 1996 .

[34]  Charles Richter,et al.  Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments , 2016, ISRR.

[35]  Frank Dellaert,et al.  Planning in the continuous domain: A generalized belief space approach for autonomous navigation in unknown environments , 2015, Int. J. Robotics Res..

[36]  Geoffrey A. Hollinger,et al.  Sampling-based Motion Planning for Robotic Information Gathering , 2013, Robotics: Science and Systems.

[37]  Agostino Martinelli,et al.  State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: The Case of Calibration , 2011, IEEE Transactions on Robotics.

[38]  Wolfram Burgard,et al.  Robust landmark selection for mobile robot navigation , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[39]  Stephan Weiss,et al.  Vision based navigation for micro helicopters , 2012 .

[40]  Gaurav S. Sukhatme,et al.  Visual-Inertial Sensor Fusion: Localization, Mapping and Sensor-to-Sensor Self-calibration , 2011, Int. J. Robotics Res..

[41]  Roland Siegwart,et al.  Monocular Vision for Long‐term Micro Aerial Vehicle State Estimation: A Compendium , 2013, J. Field Robotics.

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

[43]  Mac Schwager,et al.  Distributed robotic sensor networks: An information-theoretic approach , 2012, Int. J. Robotics Res..

[44]  Siddhartha S. Srinivasa,et al.  CHOMP: Gradient optimization techniques for efficient motion planning , 2009, 2009 IEEE International Conference on Robotics and Automation.

[45]  Paolo Valigi,et al.  Perception-aware Path Planning , 2016, ArXiv.

[46]  Steven Lake Waslander,et al.  Degenerate motions in multicamera cluster SLAM with non-overlapping fields of view , 2016, Image Vis. Comput..

[47]  Gaurav S. Sukhatme,et al.  Designing Sparse Reliable Pose-Graph SLAM: A Graph-Theoretic Approach , 2016, WAFR.

[48]  R. Murray,et al.  Flat systems, equivalence and trajectory generation , 2003 .

[49]  R. Siegwart,et al.  Observability Properties and Optimal Trajectories for On-line Odometry Self-Calibration , 2006, Proceedings of the 45th IEEE Conference on Decision and Control.

[50]  Jan Vondrák,et al.  Submodularity in Combinatorial Optimization , 2007 .

[51]  A. Krener,et al.  Nonlinear controllability and observability , 1977 .