A Unified Closed-Loop Motion Planning Approach For An I-AUV In Cluttered Environment With Localization Uncertainty

Abstract

This paper presents a unified motion planning approach for an Intervention Autonomous Underwater Vehicle (I-AUV) in a cluttered environment with localization uncertainty. With the uncertainty being propagated by an information filter, a trajectory optimization problem closed by a Linear-Quadratic-Gaussian controller is formulated for a coupled design of optimal trajectory, localization, and control. Due to the presence of obstacles or complexity of the cluttered environment, a set of feasible initial I-AUV trajectories covering multiple homotopy classes are required by optimization solvers. Parameterized through polynomials, the initial base trajectories are from solving quasi-quadratic optimization problems that are linearly constrained by waypoints from RRTconnect, while the initial trajectories of the manipulator are generated by a null space saturation controller. Simulations on an I-AUV with a 3 DOF manipulator in cluttered underwater environments demonstrated that initial trajectories are generated efficiently and that optimal and collision-free I-AUV trajectories with low state uncertainty are obtained.