Motion planning for two 3D-Dubins vehicles with distance constraint

In this paper we consider the motion planning problem for a 3D-Dubins system consisting of a pair of Dubins vehicles moving in a 3D space while maintaining constant distance. We provide a motion planning algorithm and sufficient conditions on the initial and final configurations that guarantee the existence of admissible controls, moving a first step towards the complete controllability characterization of such type of systems. Results obtained in this paper are particularly relevant in order to solve formation control problem for multiple robots as aerial or underwater vehicles, which move in 3D spaces. Simulation results demonstrate the feasibility of the motion planning algorithm proposed in this paper.