This research presents a novel approach for 3D robot formation motion planning. The methodology presented is based on the standard Fast Marching Square (FM2 ) path planning method and its application to robot formations motion planning. For the formation coordination, a leader-followers scheme is used, which means that the reference pose for the follower robots is defined by geometric equations that place the goal pose of each follower as a function of the leader’s pose. The use of the Frenet-Serret frame in order to control the orientation of the formation is introduced. Thanks to the combination of these methods, the configuration of the formation is able to adapt its shape depending on the environment conditions. This adaptation is based on the velocities map calculated in the first step of the FM2 algorithm. Also, robot priorities within the formations are introduced. This is an important contribution since it provides different behaviours to the formation members in special situations. Using this information, simulations show that the method is able to achieve good performance in difficult environments.