Robust 3D Distributed Formation Control with Application to Quadrotors

09/01/2018 ∙ by Kaveh Fathian, et al. ∙ The University of Texas at Dallas 0

We present a distributed control strategy for a team of quadrotors to autonomously achieve a desired 3D formation. Our approach is based on local relative position measurements and does not require global position information or inter-vehicle communication. We assume that quadrotors have a common sense of direction, which is chosen as the direction of gravitational force measured by their onboard IMU sensors. However, this assumption is not crucial, and our approach is robust to inaccuracies and effects of acceleration on gravitational measurements. In particular, converge to the desired formation is unaffected if each quadrotor has a velocity vector that projects positively onto the desired velocity vector provided by the formation control strategy. We demonstrate the validity of proposed approach in an experimental setup and show that a team of quadrotors achieve a desired 3D formation.



There are no comments yet.


This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

Supplementary Material

Video of the experiments is available at

I Introduction

Thanks to technological advances in recent years, it is now possible to deploy a team of unmanned aerial vehicles to collaboratively map and monitor an environment [1], inspect infrastructures [2], deliver goods [3], or manipulate objects [4]. In these applications, the ability to bring the vehicles to a desired geometric shape (i.e., formation) is a fundamental building block upon which more sophisticated maneuvering and navigation policies can be constructed.

There exists a large body of work on formation control of autonomous vehicles [5]. However, many methods rely on a centralized motion planning scheme or a global positioning/communication paradigm [6, 7, 8]. Fully distributed formation control strategies [9, 10, 11, 12], on the other hand, do not have these requirements and compared to the centralized methods have better scalability, naturally parallelized computation, and resiliency to global positioning signal jamming or loss.

Fig. 1: Crazyflie quadrotors achieving a 3D formation.

In this abstract, we present a distributed control strategy for a team of quadrotors to achieve a desired 3D formation. We assume that quadrotors have a common sense of direction, which is chosen as the direction of gravitational force measured by their onboard IMU sensors and is used to align the -axes of their local coordinate frames. The local relative positions of neighboring vehicles are multiplied by a set of constant gain matrices that are provided to each quadrotor before the mission, and summed to derive a desired direction of motion.

The main contribution of this abstract include extension of our previous work on planar formations [13, 14]

to 3D formations and a novel approach to design the required control gain matrices via semidefinite programming. Our approach can be applied on existing quadrotor platforms and is robust to noise, disturbances, and forces that can affect the estimated direction of gravity

111In particular, our approach is robust to accelerometer measurement inaccuracies discussed in [15].. Furthermore, no global position information is needed, and a distributed collision avoidance scheme naturally arises from our strategy.

Ii Methodology

In this section, we formulate the control strategy followed by a brief discussion on its design and properties. Due to space constraints and nature of this manuscript, mathematical proofs are omitted and will be presented in a future work.

Ii-a Distributed Formation Control

To bring a team of quadrotors to a desired 3D formation, we define the desired direction of motion for each quadrotor as


where denotes the index of each quadrotor, is the index set of neighboring vehicles for the -th quadrotor, denotes the position of quadrotor in quadrotor ’s local coordinate frame, and are constant control gain matrices that have the form


and are provided to quadrotors before the mission. The local coordinate frame of each quadrotor is adjusted so that its -axis is along the negative gravitational force direction that is measured by the onboard IMU sensor. Note that (2) can be considered as a rotation matrix about the -axis and a scaling along the -direction of each quadrotor’s local coordinate frame. Hence, (1) can be implemented using only the local relative position measurements.

We assume that the low-level flight controllers of the quadrotors regulate their linear velocities to the desired values given by (1), and the orientation of quadrotors (including yaw motion) is not considered. We point out that (1) is robust to unmodeled dynamics and disturbances, and can be extended to incorporate quadrotor dynamics and directly control propellers’ thrusts (see our previous work [14].)

If the matrices are chosen properly, it can be shown that under control (1) the desired formation is achieved up to a translation in , a rotation and scale factor along the -axis, and a non-negative overall scale factor. To fix the scale factor, (1

) can be augmented by a bounded, odd, and smooth map



where denotes the distance between quadrotors and , and is its desired value. Possible choices for are or , where is an arbitrary constant. The role of in (3) is to push quadrotors toward/away from their neighbors to achieve the desired scale.

Ii-B Control Gain Design

To design gain matrices that lead to desired formation, we propose a novel approach based on a semidefinite programming (SDP) formulation. Our approach is described in Algorithm 1, where is the aggregate gain matrix consisting of the blocks,

denotes the smallest eigenvalue of a matrix, and

respectively denote the aggregate vectors of desired formation coordinates, degree rotated coordinates about the -axis, and projected coordinates on the - plane. Further, , , , where is the vector of ’s, and denotes the Kronecker product.

It can be shown that if the sensing graph among quadrotors is undirected and universally rigid, Algorithm 1 is guaranteed to find the gain matrices that bring the quadrotors to the desired formation. If the sensing graph is time-varying, by adding additional constraints to the SDP problem one can ensure that the desired formation is achieved regardless of the switches in the sensing topology. This idea has been explained in more detail in our previous work [16].

input : Desired formation coordinates .
output : Aggregate gain matrix .
step 1: Let .
step 2: Compute SVD of .
step 3: Define as the last columns of .
step 4: Using a SDP solver, solve
subject to
Algorithm 1 Control gain design.

Ii-C Properties

By using the gains computed from Algorithm 1 in (1) (or (3)), it can be shown that the following properties hold:

  • The control is robust to perturbations, noise, and disturbances in the position measurements.

  • The control is robust to unmodeled dynamics and input saturations.

  • Convergence to the desired formation is unaffected by any positive scaling of the control vector.

  • Convergence to the desired formation is unaffected by any rotation of control vector up to degrees.

The last property indicates that the control is robust to any inaccuracies in the measured direction of gravitational force, which can be caused by noise or quadrotor acceleration. This is because such effects can be modeled as a small rotation in the desired direction of motion, for which convergence to the desired formation is not affected. This property can further be exploited to design a fully distributed collision avoidance scheme. Especially, each quadrotor is encapsulated in a safety cylinder to avoid both collisions and air flow disturbances. If the desired control direction points toward the safety region of another vehicle, the control is rotated to point outside of this region. If the required rotation is above degrees, the quadrotor stops until a feasible direction becomes available. Although gridlocks can occur due to the distributed nature of this strategy, in our simulations we have observed that if quadrotors are initially far apart, they can often overcome them and converge to the desired formation.

Iii Experimental Results

Fig. 2: (Top) Snapshots of experiment video for 5 quadrotors with a square pyramid desired formation. (Bottom) Coordinates of quadrotors associated to each snapshot plotted using Vicon data.

We present an experiment to show that a team of quadrotors can achieve a desired formation without collision. Our experiment is based on Crazyflie quadrotors, shown in Fig. 1, and Vicon motion capture system for localization. Although the position feedback provided by the Vicon system is given in a global coordinate frame, only the relative position measurements of neighboring vehicles are used to compute the control direction for each quadrotor. Furthermore, to mimic the direction of gravitational force measured by an IMU sensor, the -axes of quadrotors’ body frames returned by the Vicon system are chosen as the direction of gravity. Note that in general, the -axes are not perfectly aligned as the quadrotors move to achieve the desired formation. This effectively demonstrates the robustness of our method to measurement inaccuracies in the direction of gravitational force that may be present in a fully distributed implementation.

Fig. 2 shows snapshots of the experiment for 5 quadrotors while reaching a desired square pyramid formation. The sensing topology among the quadrotors is chosen as a complete graph. The quadrotors are initially placed on a line on the ground. They then move to an altitude of 70 centimeters and execute the control stratey (3) to achieve the desired formation. The location and trajectory of quadrotors constructed using Vicon measurements are shown below the associated snapshots, where the sensing topology among quadrotors is shown by gray lines connecting them. As can be seen from the figure, the quadrotors achieve the desired formation, where by rotating their desired direction of motion when necessary they avoid collision. Link to experiment videos are provided in the Supplementary Material section.

Iv Future Work

Due to the distributed nature of our collision avoidance strategy gridlocks can occur. Incorporating a communication scheme such that agents can detect/avoid the gridlock situations will be a topic of future work. Further future research include leveraging leader-follower strategies for cooperative navigation of multiple UAVs, hence, allowing a single human operator to navigate a team of UAVs while they autonomously travel in a specified formation.


  • [1] T. Cieslewski, S. Choudhary, and D. Scaramuzza, “Data-efficient decentralized visual SLAM,” in IEEE International Conference on Robotics and Automation, 2018.
  • [2] T. Ozaslan, G. Loianno, J. Keller, C. Taylor, V. Kumar, J. Wozencraft, and T. Hood, “Autonomous navigation and mapping for inspection of penstocks and tunnels with MAVs,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1740–1747, 2017.
  • [3] K. Dorling, J. Heinrichs, G. G. Messier, and S. Magierowski, “Vehicle routing problems for drone delivery,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 47, no. 1, pp. 70–85, 2017.
  • [4] R. B hnemann, D. Schindler, M. Kamel, R. Siegwart, and J. Nieto, “A decentralized multi-agent unmanned aerial system to search, pick up, and relocate objects,” in IEEE International Symposium on Safety, Security and Rescue Robotics, Oct 2017, pp. 123–128.
  • [5] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, 2015.
  • [6] N. Michael, M. M. Zavlanos, V. Kumar, and G. J. Pappas, “Distributed multi-robot task assignment and formation control,” in IEEE International Conference on Robotics and Automation, 2008, pp. 128–133.
  • [7] M. Aranda, G. López-Nicolás, C. Sagüés, and M. M. Zavlanos, “Coordinate-free formation stabilization based on relative position measurements,” Automatica, vol. 57, pp. 11–20, 2015.
  • [8] N.-s. P. Hyun, P. A. Vela, and E. I. Verriest, “Collision free and permutation invariant formation control using the root locus principle,” in American Control Conference, 2016, pp. 2572–2577.
  • [9] M. H. Trinh, S. Zhao, Z. Sun, D. Zelazo, B. D. Anderson, and H.-S. Ahn, “Bearing-based formation control of a group of agents with leader-first follower structure,” IEEE Transactions on Automatic Control, 2018.
  • [10] K. Fathian, D. I. Rachinskii, M. W. Spong, and N. R. Gans, “Globally asymptotically stable distributed control for distance and bearing based multi-agent formations,” in American Control Conference, 2016, pp. 4642–4648.
  • [11] K. Fathian, D. I. Rachinskii, T. H. Summers, and N. R. Gans, “Distributed control of cyclic formations with local relative position measurements,” in IEEE Conference on Decision and Control, 2016, pp. 49–56.
  • [12] T. Han, Z. Lin, R. Zheng, and M. Fu, “A barycentric coordinate-based approach to formation control under directed and switching sensing graphs,” IEEE Transactions on cybernetics, vol. 48, no. 4, pp. 1202–1215, 2018.
  • [13] K. Fathian, T. H. Summers, and N. R. Gans, “Robust distributed formation control of agents with higher-order dynamics,” Control Systems Letters, 2018.
  • [14] K. Fathian, S. Safaoui, T. H. Summers, and N. R. Gans, “Robust distributed planar formation control for higher-order holonomic and nonholonomic agents,” arXiv preprint, arXiv:1807.11058, 2018.
  • [15] R. C. Leishman, J. Macdonald, R. W. Beard, and T. W. McLain, “Quadrotors and accelerometers: State estimation with an improved dynamic model,” IEEE Control Systems Magazine, 2014.
  • [16] K. Fathian, D. I. Rachinskii, T. H. Summers, M. W. Spong, and N. R. Gans, “Distributed formation control under arbitrarily changing topology,” in American Control Conference, 2017, pp. 271–278.