Speeding up Gaussian Belief Space Planning for Underwater Robots through a Covariance Upper Bound

Existing belief space motion planning methods are not efficient for underwater robots that are subject to spatially varying motion and sensing uncertainties arising from the non-uniform current disturbances and landmark populations, respectively. Based on a closed-loop stochastic control framework, we propose a fast Gaussian belief space planning approach for coupled optimization of trajectory, localization and control, resulting in a non-linear programming problem (NLP). In particular, as opposed to advancing the covariance by a Kalman filter in the existing literature, we utilize an upper bound of the trace propagation of the covariance, thereby avoiding to solve Riccati equations and thus, reducing the computational complexity. The NLP is then efficiently solved by sequential quadratic programming based on the initial solutions obtained from RRT-connect. These initials lie in multiple homotopy classes guaranteed by H-signature discrimination, leading to global optimality with probability one as the number of samples in RRT-connect goes to infinity. Numerical simulations on holonomic and non-holonomic autonomous underwater vehicles (AUVs) and an Intervention-AUV with a manipulator in cluttered underwater environments demonstrate that optimal and collision-free trajectories with low localization uncertainty are obtained efficiently.