# A Leader-Follower Trajectory Tracking Controller for Multi-Quadrotor Formation Flight

^{1}, Paulo Oliveira

^{2}, Afzal Suleman

^{3}

^{1}Instituto Superior Técnico, Lisbon, Portugal

^{2}Instituto Superior Técnico, Department of Mechanical Engineering, Lisbon, Portugal

^{3}University of Victoria, Department of Mechanical Engineering, Victoria, BC, Canada

Received 16 January 2022

Revised until 20 March 2022

Accepted 9 April 2022

Online date 28 June 2022

# Abstract

The aim of this work is to design a control system based on modern control methods to control flight formations of quadrotor unmanned aerial vehicles. A leader-follower methodology is implemented where the leader vehicle has some predefined trajectory, and the follower vehicles are controlled in order to track the leader while keeping a constant displacement. The formation control system, responsible for the vehicle formation, considers, at first, only the motion at a constant height, and secondly, the three-dimensional motion. In both cases, the nonlinear control laws are derived based on Lyapunov stability theory and the Backstepping method. The control laws are validated in simulation, resorting to a realistic environment and vehicle models.

## Keywords

- Unmanned Aerial Vehicle
- Leader-follower
- Lyapunov stability
- Backstepping

# 1. Introduction

Unmanned Air Vehicles (UAVs), which were originally developed for military purposes, have now been devoted to a myriad of other uses, ranging from aerial photography, goods delivery, agriculture, mapping and surveillance, pollution monitoring, or infrastructure inspections. When appropriately synchronized, a swarm of UAVs can perform much more complex tasks with gains in efficiency and robustness. As an example, (Bacelar, Cardeira, & Oliveira, 2019) describes how it is possible to deploy two UAVs to carry heavy loads cooperatively. (Rosalie, et al., 2017) presents a strategy for area exploration and mapping carried out by a swarm of autonomous UAVs. For policing and surveillance missions in areas where the communication range is limited, (Scherer & Rinner, 2020) discusses how efficient a network of UAVs can be in covering the area. For agriculture applications, (Ju & Son, 2018) delve deeply into the advantages of using multiple UAVs with distributed control for better performance.

Most of the examples shown apply different concepts of formation and resort to different techniques of how to control it. The control structure can be either centralized or decentralized. The centralized solutions rely on only one agent performing all computations and assigning the other agents their respective tasks. The centralized algorithms are generally easier to design but more difficult to implement due to the heavy computational burden. Also, communication is critical as (Das, et al., 2002) consider that if the communication link between the central agent and any other agent fails, the entire formation is broken. The decentralized solutions break down the computational burden into smaller problems to be solved by each of the agents. In this case, the control laws are derived for each agent or subgroup of agents. The decentralized algorithms are expectedly more intricate to design, but their implementation is more reliable, efficient, and robust.

In terms of the approaches to formation control, the most relevant concepts are the leader-follower, the virtual leader, and the behavior-based. In the leader-follower case, as defined by (Shao, Xie, & Wang, 2007), a formation is achieved when each follower drives into the desired position with respect to the leader, which has some known trajectory. In the second case, the virtual leader describes a reference trajectory, and the formation is achieved when all the vehicles in the swarm follow the leader in a rigid structure, maintaining a rigid geometric shape with respect to one another and to a reference frame, according to (Leonard & Fiorelli, 2001). The behavior-based formation control approach defines different control behaviors for different situations of interest, and the control action for each vehicle is a weighted average of the control for each behavior, as explained by (Balch & Arkin, 1998).

Contrary to many strategies found in the literature, which define the displacement in the inertial frame, this paper implements this method for the 2D motion and expresses the displacement in the follower’s body frame through decentralized onboard sensors. Moreover, given that the follower vehicles track a real-time reference provided by their controllers, they do not need any clue about the leader’s path, which can be unparameterized. For the 3D case, a centralized approach has been chosen for computational efficiency.

# 2. Quadrotor Model

Let *{I}* be an orthonormal reference frame according to the North-East-Down (NED) coordinate system, fixed at some point constant along the time. Let *{B}* be another orthonormal reference frame centered at point . The orientation of *{B}* with respect to *{I}* is given by the roll, pitch, and yaw angles** ** that represent the rotation about their respective axes. The rotation matrix from *{B}* to *{I}* is given by an orthogonal matrix

(1)

From the definition of , its derivative is , with the angular velocity of *{B}* expressed in *{B}*. Let be the quadrotor’s position and its velocity in *{I}*. The kinematics of the rigid body, for any , can be written as

(2)

with

(3)

Let be the mass of the quadrotor and its inertia tensor. From the conservation of linear and angular momentum in inertial frames, the complete dynamics of the rigid body in *{I}* is given by

(4)

where is the sum of external forces applied on the quadrotor expressed in *{I}* and the sum of external moments expressed in *{B}* and is a skew-symmetric matrix such that for any .

A quadrotor is made of two pairs of counter-rotating rotors, assumed equal and equally spaced, as represented in figure 1. The forces applied on the quadrotor include its weight, aligned with the inertial frame axis pointing downwards, and the total thrust force along the body axis, pointing upwards. Relative to *{I}*, it is given by

(5)

The moments applied on the quadrotor originated from the different thrust forces produced by each rotor and the reaction torque generated by the rotors rotating. For the purpose of this work, which by no means intends to be a fastidious description of the quadrotor dynamics, the thrust force and reaction torque of each rotor are assumed proportional to its angular speed squared, such that the sum of external moments relative to *{B}* is

(6)

where is the quadrotor radius, i.e. the distance between the center of mass and the center of each rotor.

**Fig. 1.** Simplified representation of a quadrotor with forces and moments on each rotor.

# 3.Horizontal Formation Control

A trajectory tracking controller is implemented to make the follower track the leader while keeping a constant offset in its reference frame. Assuming the motion at constant height, Eqs. (2) and (4) can be simplified for a purely kinematic model given by

(7)

with only two independent equations. Given that we ultimately wish to control the force and the torque, it is wise to select the input vector (or alternatively ). So, the kinematics can also be written as

(8)

with** ** the horizontal velocity and the horizontal rotation matrix from the body-fixed reference frame to the inertial frame. If is the desired displacement and the follower’s and leader’s positions, respectively, the position error , expressed in the follower’s reference frame, can be written as

(9)

and its derivative as

(10)

An equilibrium point different from zero for the error system above makes . By making the appropriate change of coordinates, one can obtain the error system with an equilibrium point at the origin. We now want to derive a control law to stabilize the system around this equilibrium point. Let be a continuously differentiable Lyapunov function such that

and given by . Its derivative is

(11)

Adding and subtracting a term yields

(12)

where a new error was introduced. To apply backstepping with one step, we define the continuously differentiable Lyapunov function such that and given by . Its derivative is

(13)

If the accelerations are considered inputs of the system, the control law should be

(14)

which is well-defined for . Under this control law, the error system can be written in the strict-feedback form (Khalil, 2014) as

(15)

and the derivative of becomes

(16)

which is negative for if . Thus, according to the Barbashin-Krasovskii theorem (Khalil, 2014), the error system is globally asymptotically stable around the origin.

Consider the existence of an unknown external acceleration disturbance expressed in *{B}* such that

(17)

Additionally, assume the controller has an estimator such that

(18)

Expressing the estimation error by and the error state the error dynamics can be written in state-space as with

Let be a continuously differentiable Lyapunov function such that and given by . Its derivative is

(19)

The first term of is negative for all as it has already been proved the error system converges under the control law from Eq. (14). As of the remaining two terms, gets negative for all if they sum to zero. If the disturbance is assumed constant, then ** **and the adaptation law for the estimator is

(20)

Now that we have the tracking control and the disturbance estimation, we must study the stability of the system comprised of both the position error and the disturbance estimation error simultaneously. This system is given by

(21)

Let for any . The set is bounded since is radially unbounded and, from Lyapunov’s direct method (Khalil, 2014), it is positively invariant with respect to the dynamics (21). Let be the set of all points in where . This set is given by . Let be the largest invariant set contained in . By LaSalle’s theorem (Khalil, 2014), every solution with the initial condition in approaches as . Since for any there exists a such that , we have that any solution converges to . From its invariance, we have that for all . Therefore, is the only element in and the system is globally asymptotically stable around the origin.

## 3.1. Closed-loop system

After deriving a control law, it is of interest to study the stability of the closed-loop system, i.e., the formation of one leader and one follower. When the position error is identically zero, , and Eq. (10) becomes

(22)

Assuming a general leader’s trajectory , the closed-loop equation (22) can be expanded to isolate the control variables as

(23)

These equations describe a nonlinear periodic system with dynamics for and output . It is asymptotically stable around the points

(24)

within the region of convergence

(25)

In conclusion, the follower can have a heading difference relative to the leader of up to . The bigger the difference, the slower the convergence is to the desired heading. In the limit, if a follower is set to track a leader describing a linear path, starting in the opposite heading, it will not converge.

# 4. Three-dimensional Formation Control

The motion of the quadrotor at constant height has been studied, and a controller for the simplified model has been derived using the backstepping method applied to the position error. This method is now used to derive a similar nonlinear controller for the complete model. The dynamics from Eqs. (2) and (4) can be written in a state-space form by introducing the state vector and the input vector , according to (Bouabdallah & Siegwart, 2005). Defining the constants , , , , and , the dynamics becomes

(26)

with and . The system as it is posed highlights an important relationship between the position and attitude of the quadrotor: the position components depend on the angles; however, the opposite is not true. In other words, the way the position evolves is a consequence of the attitude of the quadrotor. However, the attitude is oblivious to its position. The overall system can be thought of as the result of two semi-decoupled subsystems: the translation and the rotation – for which two controllers are designed separately.

## 4.1. Attitude Control

Let be the roll angle error. Let be a continuously differentiable Lyapunov function such that and given by . Its derivative is

(27)

If is controlled to be

(28)

then . Let now be the roll rate error given by

(29)

and the augmented Lyapunov function , in the same conditions as above, given by

(30)

with its derivative given by

(31)

If the control law for is chosen to be

(32)

Then

(33)

which is negative for if . Thus, according to the Barbashin-Krasovskii theorem (Khalil, 2014), the roll error system is globally asymptotically stable around the origin. Following the same backstepping procedure for the remaining angular variables, an attitude controller is derived as

(34)

with gains .

As stated in Section 2, the quadrotor model is well defined for any . It is important to remember that, because of the singularities of the Euler angles and the topological limitations of group, this attitude controller is almost globally stable.

## 4.2. Position Control

A similar backstepping approach will next be followed for controlling the quadrotor position. Let be the altitude error. Let be a Lyapunov function, in the same conditions as above, with derivative

(35)

If is controlled to be

(36)

then . Let now z be the vertical speed error given by

(37)

and the augmented Lyapunov function , in the same conditions as above, given by

(38)

with its derivative given by

(39)

If the control law for is chosen to be

(40)

Then

(41)

which is negative for if . Thus, according to the Barbashin-Krasovskii theorem (Khalil, 2014), the altitude error system is globally asymptotically stable around the origin. Following the same backstepping procedure for the remaining position variables, a position controller is derived as

(42)

with gains

To feed the attitude controller with the roll and pitch reference values, it is assumed the quadrotor does not perform complex maneuvers, thereby keeping and small enough. From the definitions of and it can be written

(43)

The attitude controller also requires the derivative and second derivative of the reference Euler angles. These derivatives could be explicitly computed from the system (43); however, that would require measurements of the acceleration and its derivative, which are in all likelihood unavailable. Numerical differentiation of the reference roll and pitch is suggested by (Alcocer, Valenzuela, & Colorado, 2016) and adapted to

(44)

And

(45)

respectively, where is the sampling period. The second derivative of the reference angles can be computed likewise. The controller scheme is depicted in Fig. 2.

# 5. Simulation Results

Several simulations have been carried out for the developed architectures. This section presents an illustrative simulation for both the 2D and 3D controllers, where the physical parameters are shown in SI units.

The first simulation consists of a formation of two vehicles – one leader and one follower – in the two-dimensional space. The follower is intended to track a leader in a circular path with a radius and angular speed . The displacement is and the gains are . The disturbance intensity is . Figure 3 shows the simulation position as the follower starts from outside the leader’s circle and converges to a trajectory where it sees the leader at the position in its reference frame. Position and estimation error convergence is guaranteed after about 10 seconds. Figure 4 shows the speed convergence for a uniform circular motion and the heading angle bounded between and .

**Fig.3**. 2D simulation – position and tracking and estimation error.

**Fig.4**. 2D simulation – speed and heading angle.

For the three-dimensional case, the complete model simulation considers a formation of one leader and two followers, each of them with equal controllers and departing from the same place. The heavy quadrotor model developed by (Pounds, Mahony, & Corke, 2010) was used. The attitude gains are all equal to and the position gains are . The first follower keeps a displacement of and the second follower keeps a displacement of . White Gaussian noise is added to the sensors of the followers with a signal-to-noise ratio equal to 45 dB. Figure 5 displays the position of the vehicles and the actuation on each rotor, respectively, for a 100-second simulation. It is possible to see the thrust forces for each rotor converging to a constant value as the quadrotor tilts and banks. Figure 6 shows the horizontal speed – converging to a uniform circular motion at constant height – and the distance from the followers to the leader at each time instant. This plot converges to the desired value after 50 seconds, which bears testimony to the fact that the position error system converges to zero.

**Fig.5**. 3D simulation – position and actuation.

**Fig.6**. 3D simulation – speed and displacement.

It is important to recall the assumption that the noise is both white and Gaussian. In fact, this assumption, however suitable for the simulation under analysis, is not completely accurate in a real environment. Firstly, the real noise is never completely white because the power spectral density is not necessarily constant for all frequencies. Secondly, the real noise is never completely Gaussian, given that a Gaussian distribution allows infinite frequency values, which is certainly not verifiable in a physical environment.

# 6. Concluding Remarks

This work proposes an approach to the problem of flight formation by solving the trajectory tracking problem of quadrotor unmanned aerial vehicles. The formation is conceived as a leader vehicle being followed by a follower vehicle that keeps a constant displacement between them. The backstepping method has been applied to derive nonlinear control laws, and the stability concerns have been addressed through Lyapunov stability theory.

The control solution is twofold. Firstly, only the motion at constant height was considered. In this case, the controller is purely kinematic but robust to constant acceleration disturbances, and the stability of the error system is globally asymptotic. The dynamic behavior of a closed-loop formation comprised of one leader and one follower is shown to be periodic. A formal proof of asymptotic stability within a certain region of convergence is also provided for this closed loop.

Secondly, a complete three-dimensional model was considered. This model is both kinematic and dynamic and takes into account the relevant inputs from the rotors spinning. The error system is also proved globally asymptotically stable.

A relevant simulation study has been carried out to attest to the performance of the control laws developed previously. For the motion at a constant height, the follower was intended to track a leader in a circular path. For the complete model simulation, two followers take part in the formation with noisy measurements from the sensors.

Possible avenues for future work could include validation in real quadcopters, estimating variables unavailable to the followers, incorporating collision avoidance techniques, and applying varying disturbances.

# CRrediT Author Statement

**Diogo Ferreira**: Conceptualization, Methodology, Software, Validation, Investigation, Writing – Original Draft. **Paulo Oliveira**: Conceptualization, Writing – Review & Editing, Supervision. **Afzal Suleman**: Conceptualization, Writing – Review & Editing, Supervision.

# References

Alcocer, R., Valenzuela, J., & Colorado, R. (2016). A robust approach for trajectory tracking control of a quadrotor with experimental validation. *ISA Transactions, 65*, 262-274.

Bacelar, T., Cardeira, C., & Oliveira, P. (2019). Cooperative Load Transportation with Quadrotors. *IEEE International Conference on Autonomous Robot Systems and Competions*, (pp. 1-6).

Balch, T., & Arkin, R. (1998). Behavior-based formation control for multirobot teams. *IEEE Transactions on Robotics and Automation, 14*, 926-939.

Bouabdallah, S., & Siegwart, R. (2005). Backstepping and Sliding-Mode Techniques Applied to an Indoor Micro Quadrotor. *International Conference on Robotics and Automation.*

Das, A., Fierro, R., Kumar, V., Ostrowski, J., Spletzer, J., & Taylor, C. (2002). A vision-based formation control framework. *IEEE Transactions on Robotics and Automation, 18*, 813-825.

Ju, C., & Son, H. (2018). Multiple UAV Systems for Agricultural Applications: Control, Implementation and Evaluation. *Electronics, 7*.

Khalil, H. (2014). *Nonlinear Systems.* Pearson.

Leonard, N., & Fiorelli, E. (2001). Virtual leaders, artificial potentials and coordinated control of groups. *Proceedings of the 40th IEEE Conference on Decision and Control*, *3*, pp. 2968-2973.

Pounds, P., Mahony, R., & Corke, P. (2010). Modelling and control of a large quadrotor robot. *Control Engineering Practice, 18*, 691-699.

Rosalie, M., Dentler, J., Danoy, G., Bouvry, P., Kannan, S., Mendez, M., & Voos, H. (2017). Area exploration with a swarm of UAVs combining deterministic chaotic ant colony mobility with position MPC. *International Conference on Unmanned Aircraft Systems*, (pp. 1392-1397).

Scherer, J., & Rinner, B. (2020). Multi-UAV Surveillance With Minimum Information Idleness and Latency Constraints. *IEEE Robotics and Automation Letters*, 4812-4819.

Shao, J., Xie, G., & Wang, L. (2007). Leader-following formation control of multiple mobile vehicles. *IET Control Theory and Applications, 1*, 545-552.