A new algorithm of motion planning based on set-membership approach and inspired by optimal control methods is presented. The goal of this algorithm is to find a safe and optimal path taking into account various sources of uncertainties on the dynamical model of the plant, on the model of the environment, while being robust with respect to the numerical approximations introduced by numerical integration methods. The main approach is based on sliding horizon method in order to predict the behavior of the plant allowing the computation of an optimal path. As an example, the motion planning algorithm is applied on an Autonomous Underwater Vehicle (AUV) case study, showing the benefit of the proposed approach.