This work presents a path planning algorithm for 3D robot formations based on the standard Fast Marching Square (FM2) path planning method. This method is enlarged in order to apply it to robot formations motion planning. The algorithm is based on a leader-followers scheme, 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. Besides, the Frenet-Serret frame is used to control the orientation of the formation. The algorithm presented allows the formation to adapt its shape so that the obstacles are avoided. Additionally, an approach to model mobile obstacles in a 3D environment is described. This model modifies the information used by the FM2 algorithm in favor of the robots to be able to avoid obstacles. The shape deformation scheme allows to easily change the behaviour of the
formation. Finally, simulations are performed in different scenarios and a quantitative analysis of the results has been carried out. The tests show that the proposed shape deformation method, in combination with the FM2 path planner, is robust enough to manage autonomous movements through an indoor 3D environment.