Bearing and Distance Formation Control of Rigid Bodies in SE(3) with Bearing and Distance Constraints

Rigidity of the interaction graph is a fundamental condition for achieving the desired formation which can be defined in terms of distance or bearing constraints between agents. In this paper, for reaching a unique formation with the same scaling and orientation as the target formation, both distance and bearing constraints are considered for defining the desired formation. Besides, both distance and bearing measurements are also available. Each agent is able to gather the measurements with respect to other agents in its own body frame. So, the agents are coordinated-free concerning a global reference frame. On the other hand, the framework is embedded in SE(3). The control signal is designed based on a gradient descent method by introducing a cost function. Firstly, the formation problem is considered for bearing-only constraints in SE(3) configuration. Then, the formation control is expressed for the general case of both bearing and distance constraints. Furthermore, the essential conditions that guarantee reaching the desired formation is discussed. Finally, the validity of the proposed formation control is verified by numerical simulations.