Trajectory Optimization for Self-Calibration and Navigation

Trajectory generation approaches for mobile robots generally aim to optimize with respect to a cost function such as energy, execution time, or other mission-relevant parameters within the constraints of vehicle dynamics and obstacles in the environment. We propose to add the cost of state observability to the trajectory optimization in order to ensure fast and accurate state estimation throughout the mission while still respecting the constraints of vehicle dynamics and the environment. Our approach finds a dynamically feasible estimation-optimized trajectory in a sequence of connected convex polytopes representing free space in the environment. In addition, we show a statistical procedure that enables observability-aware trajectory optimization for heterogeneous states in the system both in magnitude and units, which was not supported in previous formulations. We validate our approach with extensive simulations of a visualinertial state estimator on an aerial platform as a specific realization of our general method. We show that the optimized trajectories lead to more accurate navigation while eliminating the need for a separate calibration procedure.

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

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

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

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

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

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

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

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

[9]  Roland Siegwart,et al.  Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments , 2012, 2012 IEEE International Conference on Robotics and Automation.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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