A New Collision Avoidance System for the Autosub6000 Autonomous Underwater Vehicle