  Navigation underwater traditionally is done by keeping a safe distance from
obstacles, resulting in "fly-overs" of the area of interest. Movement of an
autonomous underwater vehicle (AUV) through a cluttered space, such as a
shipwreck or a decorated cave, is an extremely challenging problem that has not
been addressed in the past. This paper proposes a novel navigation framework
utilizing an enhanced version of Trajopt for fast 3D path-optimization planning
for AUVs. A sampling-based correction procedure ensures that the planning is
not constrained by local minima, enabling navigation through narrow spaces. Two
different modalities are proposed: planning with a known map results in
efficient trajectories through cluttered spaces; operating in an unknown
environment utilizes the point cloud from the visual features detected to
navigate efficiently while avoiding the detected obstacles. The proposed
approach is rigorously tested, both on simulation and in-pool experiments,
proven to be fast enough to enable safe real-time 3D autonomous navigation for
an AUV.
