A Path Planning Algorithm for an AUV Guided with Homotopy Classes