In this work, the penalty functions technique together with the particle swarm optimization (PSO) algorithm are employed to develop a motion planning algorithm for autonomous underwater vehicles under real environment conditions. The objective functions include the traveling time, consumed energy and a combination of these quantities. The optimization procedure is conducted based on the optimal control using the trigonometric swarm. The constraints are implemented through the penalty functions approach by introducing the velocity approximation strategy to reduce the optimization process runtime. The method is verified by solving the problem under investigation in a simulated environment and performing comparison studies with the results of the other related methods. After that, its robustness and efficiency for real conditions are demonstrated by performing the motion planning of AUVs under disturbed conditions. The effects of real currents of an area in Persian Gulf (PG) on the proposed motion planning algorithm are investigated. Finally, it is shown that the use of the trigonometric swarm and also the velocity approximation strategy improve the motion planning results. In addition, it is shown that the velocity approximation strategy decreases the process runtime by up to almost 30%, and the trigonometric swarms instead if common swarms (spline) improves the objective function minimization by up to almost 31%.