In this study, an optimal autopilot algorithm is developed for 3D motion of SAGA which is an unmanned underwater survey vehicle. Firstly, a nonlinear mathematical model for SAGA is obtained. The structure of the mathematical model of the vehicle comes from a Newton-Euler formulation. The resultant nonlinear system is then controlled by PID controllers. These PID controllers arc designed for 3D motion which is realized by a suitable combination of right, left and vertical thrusters. The optimal control problem is that the vehicle is supposed to reach the desired position and rotation with desired velocity by consuming minimum energy. This problem is solved with a genetic algorithm. All of this study is performed in a Matlab/Simulink environment. (C) 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved.