Formation control of quadrotors via potential field and geometric techniques

In this paper, two leader-follower formation control methods are proposed for a fleet of quadrotors. The linear quadratic regulator (LQR) control technique is designed to stabilize the linear models generated at different points in the operating space. The leader-follower approach is used to achieve formation control where the position and the heading of the leader are used with offsets as references to be tracked by the followers. In the first method, the potential field technique is proposed to achieve the desired formation. The attractive potential field attracts the followers towards the leader while the repulsive potential field repulses every two neighboring followers in order to keep a required distance between them and also avoid the collision. In the second method, the followers’ positions are dictated by the geometry of the desired formation shape. These followers’ positions are expressed in equations that relate them to the leader position and then given to the followers as references to be followed. An extensive simulation is conducted to examine the validity of the results, and the quadrotor nonlinear model is used in all simulations.


Introduction
*Mathematics Unmanned aerial vehicles (UAVs) have gained an extensive attention because of their various uses in many fields. UAVs are reliable to replace manned aerial vehicles in many areas such as military, civilian communities, and agriculture.
In military tasks, UAVs are used to carry different payloads such as radars, cameras, and even weapons. In addition, UAVs can be used for observation and exploring hostile environments (Di, 2011). In civilian environments, UAVs are useful for several applications such as watching natural resources, home security, search and rescue operations, and scientific research (Marasco, 2013).
Recently, the use and control of multiple UAVs to achieve tasks cooperatively has attracted much interest from researchers in the related fields. This is because of the advantages that are obtained from the use of multiple vehicles instead of using a more elaborated single vehicle. When comparing the task outcome of a multiple unmanned vehicles team with that of an individual vehicle, one can realize that the multiple vehicles overall performance can develop mission allocation, the system safety, and efficiency, and the required time, (Gerkey and Matarić, 2004;Zhang and Mehrjerdi, 2013).
Multi-UAV formation flight is a combination of the study of both UAV and coordination, so it has received significant interest from both unmanned systems and control fields. Cooperative coordination requires that a team of UAVs track a prescribed path for flight tasks while acquiring useful information using their on-board sensors and keeping a specified formation shape. The flight trajectory could be a set of waypoints or a prescribed region of fly with boundaries (Di, 2011).
Different approaches such as leader-follower, behavioral, and virtual structure approaches are used to achieve formation flight. The concept of an artificial potential field-based approach was explained in Khatib (1986). The main idea of this methodology is that the vehicles move in a field of forces that affect them. This field is similar to the electric field produced by the positive and negative charges. The desired destination is considered as an attractive charge, while obstacles are considered as a repulsive charge.
In the literature, many kinds of research have studied the quadrotors formation control design, and different proposed control methods have resulted in various performances. Guerrero et al. (2012) investigated the formation of flight and trajectory following control problems for multiple mini rotorcrafts. First, Newton-Euler formalism was used to derive the dynamic model for a single mini rotorcraft, and then the authors used a leader/follower structure to control formation flight and designed a nonlinear control bounded inputs based on separated saturations and a single integrator consensus control for flight formation. The x and y positions for each rotorcraft were considered as dynamical agents with full information access. The virtual center of mass of the agents was used to achieve trajectory tracking for the group of agents. Mercado et al. (2013) studied the trajectory tracking and flight formation control problem in a horizontal plane for multiple unmanned aerial vehicles (UAVs) and presented a control strategy using the leader-follower scheme. They used time scale separation of the translational and rotational quadrotor dynamics for trajectory tracking and introduced a sliding mode controller for the translational dynamic to provide the desired orientation for the UAV. The attitude of each is stabilized by using a PID controller. Formation error dynamics are used by sliding mode control law to keep the formation of the follower with respect to a leader.
In Wang and Gu (2010), the authors discussed a leader-follower flocking system in which few agents are leaders provided with knowledge on the desired trajectory while the remaining are followers. The followers do not have global knowledge and also do not know who the leaders are among the flocking agents, but they have the ability to communicate their neighbors. To maintain the flocking group connected, a consensus algorithm via local communication is used by all the members to estimate the flocking center position.
The authors in Guerrero and Lozano (2010) investigated a leader/follower type approach based on the combination of nested saturations and a multi-agent consensus control to design a nonlinear controller in order to achieve flight formation for a multiple mini rotorcraft system. Abbas and Wu (2015) used two controllers to achieve formation tracking in the X-Y plane for quadrotors that are all at the same height Z. Two controllers are used in this study. The first is a PID controller used to ensure that the leader quadrotor tracks the desired trajectory and also used by the followers to keep formation. Then they used a directed Lyapunov controller to achieve quadrotors formation in the X-Y plane. The artificial fish swarm algorithm has been used to enhance the controller performance, and the controller parameters have been optimized dynamically.
Choi and Ahn (2011) used a three-dimensional formation controller based on inter-agent distances. The controller is constructed from the time derivative of the Euclidean distance matrix related to the team realization. Pilz et al. (2009) proposed a theory-based graph scheme for robust distributed controller design for formations and showed a method of making the design containing performance requirements. The stability is guaranteed with the proposed technique for all possible formations and arbitrary fast changes in the communication topology. Turpin et al. (2012) studied formation control for a team of quadrotors where the vehicles track a specified group trajectory with the ability to change the shape of the formation safely according to specifications. Shape vectors prescribe the formation and command the relative separations and bearings between the quadrotors. Lee et al. (2013) discussed the tracking control for cooperating quadrotor unmanned aerial vehicles with a suspended load, which is considered as a point mass connected to the quadrotors, and for an arbitrary number of vehicles. They assumed that the point mass is linked to multiple vehicles by rigid massless links and designed a control approach for the quadrotors to make the point mass asymptotically tracks the desired trajectory, and the quadrotors keep a prescribed formation, with respect to either the point mass or to the inertial frame. To avoid singularities and complexities associated with local parameterizations, a coordinate-free fashion was used. Eskandarpour and Majd (2014) introduced an approach to achieve cooperative formation control with obstacle and self-collisions avoidance for a group of quadrotors based on hierarchical model predictive control (MPC). In this study, the control structure is divided into two layers, which are lowerlayer and upper-layer control. At the lower-layer, a linear MPC was designed to stabilize quadrotors and to accomplish trajectory tracking. Two MPC controllers were used for stabilization, one of them for translational motion and the other for rotational motion. At the upper-layer, a linear MPC controller is also designed to produce the optimized reference trajectory that is used by the lower-layer. To keep the vehicles within the desired formation besides avoiding obstacles and self-collisions, three cost functions were considered. Guney and Unel (2013) proposed a decentralized method for formation control of a team of quadrotors (UAVs). Both position and attitude of each quadrotor were controlled by using PID controllers, which are considered as low-level controllers. Formation control, which is considered as high-level control, was achieved by presenting virtual springs and dampers between vehicles to generate reference trajectories for each vehicle. Coordination and target forces are described using spring and damping forces, where springs have adaptable parameters.
De Vries and Subbarao (2011) addressed a method to achieve cooperative control for swarms of quadrotors (UAVs). First, the linearized model was used to develop the inner loop controller, which is a backstepping based controller with a nested multiloop structure. This controller is used to stabilize individual quadrotors and to accomplish position tracking. Then, potential functions were used to design a cooperative controller in order to achieve the swarming behavior. Finally, obstacle avoidance was added to the system, where an obstacle was modeled as a repulsive potential function. Delgado and Lopez (2009) offered a decentralized formation control strategy for a team of quadrotor rotorcrafts. In this work, the nested saturation control is used to control each rotorcraft, while the formation control strategy is based on the potential field theory. Obstacle avoidance is guaranteed by using a potential repulsive function.
The Mamdani-type of fuzzy logic control is used to stabilize the system by equipping each vehicle with the fuzzy controller, which is composed of two cascaded blocks. The quadrotor's three-dimensional coordinates and the roll, pitch, and yaw angles are used as the crisp inputs to the fuzzy controller, while the outputs are the voltages of the motors which produce the rotors speed. The relative positions of the quadrotors are assumed to be known, and the roll and pitch angles are assumed to be maintained within a limited domain to avoid vehicle overturn. Rezaee et al. (2013) proposed a leader/follower formation control of Unmanned Aerial Vehicles (UAVs) based on fuzzy logic control. Kinematic equations were used to design a formation strategy in order to maintain a follower at a relative position from a leader in the body frame of the leader. Then, minimizing the kinematic formation errors was achieved by designing fuzzy logic controllers which control the follower speed and attitude. The UAVs' dynamical models were not considered in this work.
Imran (2015) proposed a formation control for a heterogeneous system consists of a group of nonholonomic mobile robots and one quadrotor UAV. In this work, the Immersion and Invariance (I&I) adaptive control technique has been used to stabilize all the vehicles, while the potential field algorithm was used to generate the desired formation path.
In this paper, two leader-follower based formation control techniques are proposed to obtain formation flight control for a group of quadrotors. One of these vehicles is considered as a leader while the others follow it in a prescribed formation. All the quadrotors are stabilized by using the linear quadratic regulator (LQR) control technique. The LQR controller was designed by linearizing the nonlinear model of the quadrotor and then applied to the nonlinear system. The leader is set to track a reference path while the followers are set to track the position and to head of the leader with specified offsets. By setting these offsets, we can determine the shape formed by the group. According to the current position of the leader and the neighboring followers, the formation control algorithms are employed to compute the offsets and obtain the paths that navigate the followers in the desired formation. To obtain a prescribed shape formation, we use two methods to produce the followers' paths. In the first method, the potential field technique is used to achieve the desired formation. The attractive potential field attracts the followers towards the leader while the repulsive potential field repulses every two adjacent agents in order to keep a distance between them. In the second method, the followers' positions that achieve the required formation are obtained from the geometry of the desired formation shape. These positions are expressed in equations that relate them to the leader position and then given to the followers as references to be followed. The controller will force this criterion.
This paper is organized as follows. Section 2 presents the dynamic model and its linearized equations, which are used to design the controller. The linear-quadratic LQ controller design is explained in Section 3. The design of formation flight control based on the potential field method with the simulation that shows the effectiveness of the designed controller is described in Section 4. Section 5 describes the formation of flight control using the geometric method with simulation results. The comparison between the results of the two techniques is given in Section 6. Finally, the work conclusion is given in section 7.

Dynamic model
The quadrotor non-linear and linearized dynamic models that were derived and identified in Oliveira (2011) will be used in this work. To define reference frames, two right-handed coordinate systems were considered, as shown in Fig. 1. One of the coordinate systems is an earth-fixed inertial frame coordinate system, while the other is a body-fixed frame. The body-fixed frame has a linear velocity vector ⃗ = [ ] and an angular velocity vector Ω ⃗⃗⃗ = [ ] . Initially the two frames are coincident. The bodyfixed frame attitude is described by serial rotations of its three axes about the axes of INF. Euler's angles: (roll), (pitch), (yaw) will be used to express these rotations while the transformation from the body-fixed frame to the earth-fixed inertial frame will be achieved by means of the following matrix: where s represents sine and c represents cosine.

Non-linear model
The quadrotor airframe non-linear model is represented by using the moment and force equations as follows (Oliveira, 2011;Rinaldi et al., 2013;Saif, 2017): where L is the lever length of each of the quadrotor arms, is the air drag torque coefficient, is the linear friction torque coefficient, ω is the angular speed, g is the gravity acceleration, and , and are aircraft's moments of inertia around X, Y, and Z axes, respectively. γ is the ℎ cubic regression coefficient obtained by Oliveira (2011) from experimental identification for the thrust model. By assuming that = , the term − will be excluded (Oliveira, 2011). For the purpose of representing the attitude dynamics in the INF, we need the Euler angles ψ(t), θ(t) and ψ(t) and this can be obtained from the Euler kinematic equations which are expressed as:

Linearized model
The linear approximation of the Taylor series is used here to obtain the linearized quadrotor model . Before doing that, we need to express the thrust of each rotor and its derivative ̇ in terms of the angular speed . Although (ω) can be modeled mathematically, experimental tests that were conducted in the literature to identify the thrust model led to the following more accurate model (Oliveira, 2011): and its derivative at the operational speed j0 , ̇0 = 1.3 • 10 −3 + 2(−1.44 • 10 −6 ) 0 + 3(5.19 • 10 −9 ) 0 2 Now, linearizing the moment, force, and the Euler kinematic equations results in the following linearized airframe dynamics. The obtained linearized moment equations are: The linearized force equations are: The linearized Euler kinematic equations are: The above model represents only the linearized airframe dynamics. To obtain the complete linearized quadrotor dynamics, the linearized rotor dynamics is needed. For this purpose, we will use the linearized rotor dynamics with its identified parameters obtained in Oliveira (2011). This model was simplified, after applying the Laplace transform, from second to the first-order model by discarding the very fast pole, it becomes: In the time domain, this can be written as: The point around which the rotor dynamics was linearized is defined by: where is the voltage applied to the armature of the j th rotor DC motor, is the resistance of the armature, is the electric torque constant, is the speed constant (Oliveira, 2011). Table 1 shows the parameters of the quadrotor model. Now, defining the vector of states as: x ⃗ = [x ∆u y ∆v z ∆w ∆φ ∆p ∆θ ∆q ψ ∆r ∆ω 1 ∆ω 2 ∆ω 3 ∆ω 4 ] T , and the input vector as ⃗ ⃗ = [ 1 2 3 4 ] , the quadrotor linearized system can be expressed in the state space description as: The matrices A, B, C, and D are defined as:

Linear quadratic (LQ) control
The control goal here is to design a control system for a single quadrotor to track the desired position and altitude references ( , , ℎ ) on the navigation frame. One of the most attractive approaches to MIMO control systems is the linear quadratic regulator (LQR or LQ). Lewis et al. (2012) explained this topic in detail. The LQ is a kind of state-feedback control that is used with the statespace description of the plant: The LQ methodology is used to obtain the statefeedback gain matrix as following: with the situation of the infinite horizon.
To design a tracking controller using the LQR, artificial states are needed to be added as the integral of the control error to guarantee asymptotic tracking. This produces augmented matrices and . Thus, four new states are needed to be added to the original (16×1) state vector as follows: where, ( , , ) is the three-dimensional reference position that is required to be tracked by the quadrotor, ( , , ) is the current threedimensional position of the quadrotor, is the yaw angle reference that is required to be followed by the quadrotor, is the current yaw angle of the quadrotor, and This will result in an augmented system with augmented matrices. Fig. 3 shows the scheme of applying the state feedback LQ control technique on the augmented system.

Formation flight control using potential field method
In this section, formation control using a potential field technique is applied to a group of (UAV) quadrotors that are required to navigate in the desired flight formation. One of the group quadrotors is considered as a leader and tracks a given reference path. The others follow the leader in a prescribed formation shape.

Shape formation
Let each quadrotor has a sensing range of D, as shown in Fig. 4. Each quadrotor has the ability to determine the positions of all its neighbors that are located inside this sensing range. A prescribed polygon with circumcircle of radius R is required to be tracked by the fleet quadrotors during their individual motion such that the leader is at the center of the formation at all times. Every two adjacent agents, and have to be at a distant d ≤ D from each other. From the basic geometry, R can be defined as: where m represents the number of agents (Al-Buraiki and El Ferik, 2015).

Attractive-repulsive potential field-based control design
The concept of cooperative control is achieved based on a potential field from Imran (2015). With this technique, each follower can access the positions of the adjacent agents in addition to the leader position. The agents' path is obtained according to the current position of the leader and the corresponding positions of neighboring followers. The attractive and repulsive potential field concept will be applied in this work for formation control. The desired formation is obtained by using the potential field function to generate the paths that are needed to be tracked by the followers. To achieve this, we need to define two potential field functions, one of them is attractive, ( ), which attract the followers towards the leader and the other is the repulsive, ( ), which keep the distance between each two neighboring followers as equal to or greater than d. and are defined as follows: where, is The current distance between the ℎ follower and the leader, R is The desired distance between the ℎ follower and the leader (the circumcircle radius of the desired polygon), , is The current distance between the ℎ and ℎ followers, d is The desired distance between the ℎ and ℎ followers, and is Positive design constants.
The following associated force vector, which acts as the negative gradients of the potential fields, will be used to control the formation flight of the quadrotors group.
where, F o is the attractive potential (also called center potential), F ij is the repulsive potential (also called Interagent potential), D a is damping action. The formation of flight control based on LQR with potential field technique is illustrated in Fig. 5. The potential field control uses the center potential to attract the followers towards the center and the interagent potential to repulse every two neighboring followers preventing a collision. The formation of flight control is applied to each follower as a cascaded control in which the outer loop controller is the potential field control action, while the controller of the inner loop is the LQR.

Fig. 5:
The scheme of a follower quadrotor controlled with potential field and LQR formation control

Center potential
From EQS. 20 and 23, the attractive potential field of the i th follower is: , , , ] where is the current location and heading of the ℎ agent and can be calculated as: Now by differentiating given in Eq. 25 with respect to , the center potential between the ℎ follower and the leader can be obtained: and Using the chain rule, we can differentiate r with respect to as: and from Eq. 30 where = [ , , , ] is the current location and heading of the leader. Substituting Eqs. 32 and 33 in Eq. 31 to obtain Now, substituting Eqs. 28 and 34 in Eq. 27 results in attractive potential as:

Repulsive potential
From Eqs. 21 and 24, the repulsive potential field of the i th follower is: where , can be calculated as: Following the same steps of obtaining results in the repulsive potential between every two neighboring agents and as: where = [ , , , ] is the current location and heading of the ℎ agent.

Simulation results
In this section, we will show the results of applying the formation control using a potential field technique on a group of four quadrotors navigating in space as a leader and three agents. Each quadrotor has two stages of control. The first one is the outer stage, which is responsible for formation control. This controller produces a path that is required to be tracked by each follower. This path is generated according to the current position of the leader and the adjacent agents. The inner controller is the LQR controller that is responsible for the quadrotor stability and tracking tasks. The three agents are required to form a circle with a radius R around the leader, such that the position of the leader represents the center of the circle. Each agent has to be at a distance R from the center, and every two adjacent agents have to be at a distance d from each other.
Let R=1.5m and m=3, then d=2R sin (pi/m) =2.598m. The leader is required to track its reference trajectory while the followers depend on the formation control function to produce their paths that satisfy the formation requirements. The simulation is carried out firstly with considering that the leader moves in the x-direction and then with considering the movement in the x-y direction. To verify the efficiency of the proposed control system, the control method is applied to the nonlinear model, and the performance results have been obtained and shown in the next figures. It is clear from Figs. 6-13 that the three agents are able to track their leader with keeping the desired formation. Figs. 6-13 show the performance of the group when the leader moves through different points along the x-direction. The leader is given a reference input in the form of successive steps. While the leader follows its reference, the followers track it with keeping the required formation. Figs. 6-12 show the performance of the group when the leader moves from one step to another, while Fig. 13 shows the performance of the group when the leader moves through the full trajectory. Fig. 14 shows the complete navigation path for the group in x as well as y directions with the ability of the control system to achieve the prescribed formation flight.

Formation of flight control using a geometric approach
In this section, we propose a kinematic formation method with the possibility to select the appearance specifications for the shape that is required to be formed by the agents. The method deals with the geometric equations that relate the positions of the followers to the position of their leader. By considering the shape as a rigid body, two reference frames (fixed earth/inertial and body-fixed frames) are needed to be defined. As shown in Fig. 15, the axes X−Y−Z and x11−y11−z11 denote the global/inertial frame and the body frame of the desired shape, respectively. The roll, pitch, and yaw angles: (roll), (roll), (roll) are used to express the rotations of the shape around the global frame.

Rigid body motion
In order to determine the coordinates of a point that is located in a rigid body moving in space, two coordinate frames are needed to be defined. The first one is a fixed global/inertial frame n (OXYZ), while the other is a body-fixed one (oxyz). As shown in Fig.  15, the motion of the rigid body can be a combination of rotation in the global frame and a translation of the body frame origin (o) relative to the origin O of the inertial frame. where,

= [ ] = [ ]
The vector represents the displacement or translation of o relative to O, and is the rotation matrix that maps to when = 0. Eq. 39 combines the rotation and translation of the body frame with respect to the inertial frame. In other words, a rigid body location can be described by the position of its frame origin o and the orientation of the body frame relative to the global frame (Jazar, 2010).

Control design
Suppose that the quadrotors group is needed to form the circle shown in Fig. 16. The leader (L) is required to be located at the circle center while the followers are distributed around the leader to form the circle with radius R and interspatial distance of d between every two adjacent agents.
By means of the rigid body configuration concepts, the desired positions of the followers relative to the leader can be obtained as: where, is the rotation matrix from the body frame to the inertial frame given as: This matrix is used when the shape rotates around the inertial frame. However, if the shape rotates around its body frame, will be different.
are the coordinates of the leader position in the inertial frame?
are the desired position coordinates of the ℎ follower in the inertial frame, which accomplish the required formation.
are the desired position coordinates of the ℎ follower in the body frame.  By setting [ ] as a reference and using only the position equations in (18) in the vector form, the artificial augmented states of the follower can be obtained as: [ ] can be calculated from (40).

Collision avoidance
To avoid agent collision, we need to add a repulsive function that works when the distance between any two neighboring followers becomes less than d. Therefore, the repulsive potential field in Eq. 38 can be used and Eq. 43 becomes: Fig. 17 shows the scheme of the formation control with the geometric algorithm and the LQR control approach.

Simulation results
Using the same simulation conditions that have been used with the potential field method, the system is simulated here with the geometric method. The simulation is carried out firstly with considering that the leader moves in the x-direction and then with considering the movement in the x-y direction.
With this method, we have additional specifications that can be set to be achieved by the group of quadrotors. These specifications are the desired shape angels (roll, pitch, and yaw) that determine its configuration. Fig. 18-29 show the performance of the LQR with geometric formation control while navigating along a required formation path. The leader and followers start from their initial hovering positions, and then the leader moves while the agents track it with keeping the desired formation. Fig. 18 show that the leader starts moving from its initial hovering position (1, 0, 2) to the next position, which is (2, 0, 2). The followers track the paths obtained by the geometric functions to form the prescribed circle around the leader. Fig. 19-24 show the leader navigating among different positions while the followers track the desired formation around it. The complete navigation path for the quadrotors group is shown in Fig. 25. One can see the ability of the LQR with geometric formation control to navigate each agent and achieve the prescribed formation flight with keeping a desired configuration for the formation shape.
In Fig. 26, , and are set to be zeroes. The group achieved 2-D flight formation (in the X−Y plane), as shown in Fig. 26. By setting =−π/6, =−π/6 while keeping =0, the obtained group performance is shown in Fig. 27. By changing , all the agents are shifted around the z axis of the body frame by as shown in Fig. 28. Fig. 29 shows the complete navigation path for the group in x as well as y directions with ( = 0, = 6 , = 0).

Comparison
To assess the performance of the proposed methods, they are compared according to the accuracy of positioning the followers at positions that achieve the desired flight formation. Both the desired circle radius (R) and the required interspatial distance (d) are measured at different positions of the group for the proposed two formation and control methods. The required values of R and d are R=1.5m and d=2Rsin(pi/3)=2.5981m. Table 2 and Table 3 show the resulted measurements where represents the distance between the leader and the ℎ agent and , represents the distances between the agents and .  From Table 2 and Table 3, it can be concluded that the system with applying the LQR with a geometric formation control approach achieves the desired positions more accurately.

Conclusion
In this study, a cooperative flight control framework is designed for a fleet of an arbitrary number of quadrotors. The LQR control technique is employed as an inner controller that stabilize and achieve the tracking task of the fleet agents. Based on the leader-follower formation approach, two formation methodologies are used as an outer controller to accomplish a desired shape formation. The proposed control methodologies proved their ability to achieve the tracking task with keeping the desired team formation.