2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 54 Quadrotor Formation Control Method Based on Graph and Consistency Theory Yang Sen 1,2 1 Department of UAV Engineering, Ordance Engineering College, Shijiazhuang, China 2 School of Automation Science and Electrical Engineering, Beihang University, Beijing, China e-mail: 568657132@qq.com Xi Leiping Department of UAV Engineering Ordance Engineering College Shijiazhuang, China Abstract—This paper introduces graph and group system consistency theory and puts forward a quadrotor formation control method. The quadrotor is described as a second-order integrator dynamic system, and the relative position deviation of different quadrotors is used to describe formation. According to the communication topology relationship between quadrotors, the formation is modeled by graph theory. The fusion of the pilot-follow and graph theory method is analyzed, and a second-order coherence algorithm with a pilot is presented. With this algorithm, the quadrotors can complete behaviors just as formation rally and formation mobility etc. Finally the paper verifies the availability of the proposed method through simulation tests. Keywords-Formation Control; Graph Theory; Consistency Theory; Second-order Integrator; Pilot-follow Method I. INTRODUCTION In recent years, quadrotors become more and more popular among scientific researchers because of their small size, light weight, easy manipulation and high efficiency while performing tasks in uncertain and dangerous environment. Due to the shortage of its load and the sensor performance, a single quadrotor is highly restricted in complex tasks. Thus, more and more quadrotors are applied to work together. Multiple quadrotors formation control is a major and basic research subject in the studies of quadrotors cooperative control. It has attracted the interests of many researchers both at home and abroad. The main formation control methods are pilot-followed method, virtual structure method, behavior-based method and graph theory method. Graph theory method means to model the formation to directed graphs or undirected graphs according to the communication network topology relationship and the formation conditions, and then analyze and design the formation information flow and the formation control algorithm through mature graph theory. The method of graph theory has gradually become the mainstream of formation control as it is a fusion of several methods talked before [1]. Consistency theory has been applied in formation control research under a variety of circumstances such as the fixed topology, time-varying topology, communication time delay of ground robots, underwater vehicles and satellite systems etc [2]. Literature [3] proposes a model of multi-robot formation control, which takes a robot as a single integrator dynamics model and analyzes the multi-robot system formation and its stability with consistency theory. As shown in literature [4], in the GRASP laboratory of the Pennsylvania University, a team led by Kumar describes the formation with the relative position and relative direction of the quadrotors. By introducing the error of relative position, they control the formation flight of four quadrotors with consistency algorithm. However, the control structure they describe is a centralized control structure which demands high calculation capability of the central control unit but has poor extensibility. In literature [5], in the study of the automatic quadrotors formation, a communication topology solution is put forward based on the Hamilton loop in the cascade control system. At present, most of the literature describes intelligence agents as first-order integrator power systems, which is relatively simple. But many complex systems must be described by second-order or higher order system so as to 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 55 realize accurate and effective control. This article combines graph theory method and pilot-follow method and describes a as s second order integrator system. Through the second-order coherence algorithms, the formation of control method is studied under the topological structure that only one quadrotor is informed of the flight path and all the follower quadrotors can receive information from the pilot. II. GRAPH THEORY AND CONSISTENCY OF THE GROUP SYSTEM INFORMATION A. Graph theory Graph is a data structure composed by a vertex set and the binary relation between vertexes (i.e., the set of edge), usually referred to as ( , )G V E . The collection of vertices and edges are represented as: 1 2 ( ) { , , , } ( ) {( , ) : , ( )} n i j i j V G v v v E G v v v v V G      In the diagram, if node i and node j has information exchange, the edge ( , ) i j v v exists; If there is no directional information exchange, then the following exists: ( , ) ( , ) i j j i v v E v v E   The diagram is called undirected graph. If the information stream is flowing from node i to node j, then the edge is directional and the diagram is a directed graph. Undirected graph can be considered as a special case of directed graph. Directed graph is more practical as it takes the one-way flow of information into consideration. Adjacency matrix uses algebra matrix to describe the location relationship between the nodes, note adjacency matrix as [ ] n n ij A a R    , thereinto 1, 0, other i j ij v v E a     The neighbor nodes for the node i is the set ( ) i N V G , namely { : ( , ) } i j i j N v V v v E   In undirected graph, the degree of any node i is defined as the number of neighbor nodes of node i; in directed graph, the node degree is equal to the sum of the inward degree and the outward degree of node i, namely, ( ) ( ) ( ) i in i out i d v d v d v  The inward degree and the outward degree of node i v are respectively defined as: 1 1 ( ) , ( ) n n in i ji out i ij j j d v a d v a      The undirected graph/directed graph Laplacian matrix adopts the following definition: 1 , , n ik kij ij a i j l a i j         B. Group system information consistency Assuming that number of flight vehicles in the formation is n, we use ( , ) n n n G v  to indicate the communication topology among various vehicles, in which ={1 2 , } n v n，， is the node set, n n n v v   is the edge set. The matrix [ ] n n n ij A a R    and [ ] n n n ij L l R    are respectively the adjacency matrix and asymmetric Laplacian of graph n G . Consider a simple single integrator system: , 1, 2, i i u i n     In the equation above, m i R  and m i u R are respectively the information state and control input of vehile i. The common continuous time consistency algorithm of first-order integrator system is:    1 , 1, 2, n i ij i j j u a t i n         The above two equations are rewritten into matrix form as below:  n mL t I        Theorem 1 [6] : Under the condition of fixed time invariant communication topology, if and only if directed communication topology is a bunch of directed spanning tree, the flight vehicles formation can gradually reach agreement; Under the condition of time-varying communication topology, the conditions for flight vehicles 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 56 formation reaching agreement are the existence of an infinite number of uniformly bounded adjacency periods which enables the set of the directed communication topology of the flight vehicles formation to exist in all these periods and contain a bunch of directed spanning tree. When considering the maneuver of the flight vehicles formation, we indicate the information equation with a second order integral dynamic system. The second-order dynamic systems integrator algorithm is an extension of the first-order integrator power system consistency algorithm. The existence of a tufted spanning tree in the directed graph is conditions of second-order dynamic systems integrator agreement necessary rather than sufficient conditions. Taking the following second order integrator power system as an example: i i   i u   m i R  is the state of system information; m i R  is system information state derivative; m i u R is the system information input control of member i. Assuming that the same communication topology about the transfer between i  and i  exists among different quadrotors, and also uses  ,n n nG v  to represent, and n A and n L respectively represent the adjacency matrix and asymmetric Laplace matrix. A basic second-order dynamic systems integrator consistency algorithm is as follows:        1 [ ] n i ij i j i j j u a t t             1, 2, ,i n  .For any time t is a positive number. If i  and i  respectively signify the position and speed of vehicle i for all (0) i  and (0) i  , through operation type (5), when ,t   0 i j    and 0 i j    , the formation may reach an agreement at this time. If 1 2 [ ] , T T T T n      1 2 [ ] , T T T T n      the equation above can be rewritten as:  ( ) mt I                      While 0( ) ( ) ( ) ( ) n n n n n I t L t t L t          . Theorem 2: Algorithms (6) asymptotically may reach consistent if and only if there are only two zero eigenvalues in  , and other nonzero eigenvalues are all negative real part, Particularly for large enough t, 1 1 ( ) (0)+ (0) ( ) (0) n n n i i i i i i i i i a i i t p t p t p           ， Thereinto T 1 2 [ ] 0 n p p p p , T 1 n p 1 , T 0 n p L .Relevant attests for the Theorem refers to the literature [7]. III. SYSTEM MODELING AND DESIGN OF QUADROTORS FORMATION Quadrotor is a typical underactuated system which has four input and six output. They are usually divided into “X” type and“+ ”type. Quadrotor realizes pitch, yaw and roll motion of the plane by controlling the four motor and the speed of the propeller. The definition of inertia coordinate system  I OXYZ and body coordinate system  B oxyz , as shown in Fig.1. x z y Q Z X Y O F B L R  Figure 1. The definition of quadrotor’s position coordinate system 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 57 3 , v R  is the position and velocity of the quadrotor under the inertial coordinates, and 3R is the varying attitude angular velocity of the vehicle under coordinate system. 3 3R   is referred to as the rotation matrix from the coordinate system to the inertial coordinate system, c c c s s s c s s c s c c s c c s s s s c c s s s s c c c                                               sins   and  cosc   represent the sine and cosine.   3 T R     is the euler angle of quadrotor in the vehicle coordinate system. [0 0 1] T z e is the unit vector of z shaft. z n e  is column 3 of attitude matrix. m is the weight of the quadrotor. 1 2 3 3 3J diag J J J R   = is the system rotation inertia matrix. g is the acceleration of gravity. T is the total lift of the four propellers that drives the quadrotors’ position change.  1 2 3 T    respectively represent the control matrix of roll, pitch and yaw direction. It is assumed that the center of the aircraft is the origin of the coordinate system. Driving motor has no installation angle error. Excepting the rotor rotation, the rest of a quadrotor is supposed as rigid. Taking the north, east, and the ground coordinate system as the inertial coordinate system, this paper adopts the system model in the literature [8]:   z v mv mg n S                       e T R R J J  The quadrotor position control comprises the inner and outer loop control structure, in which the inner ring controls posture and the outer one controls position. The controller can be seen in literature [8]. The main concern here is the formation control algorithm. The formation controller receives state information of itself and the other members of the formation through its own sensors and the wireless communication network, puts out the respective expected position and posture to position controller, and drives actuator to perform formation task, as shown in Fig.2. The key study is the design of formation controller based on second order integrator dynamic model. Position controller Attitude controller Formation controller Dynamics of four rotor UAV Expected attitude Position and speed Attitude and angular velocity Moment Lift Expected position Expected orientation UAV i Position controller Attitude controller Formation controller Dynamics of four rotor UAV Expected attitude Position and speed Attitude and angular velocity Moment Lift Expected position Expected orientation UAV j State interaction Figure 2. Structure of the formation control system If the formation size is N, according to the information interaction relationship between quadrotors, the quadrotors formation can be modeled as a directed graph. i is the node i V in the directed graph, information flowing from i to j is marked as edge , , (1, 2, , ) ij e i j N  . Number 1 is specified as the pilot and the rest are followers. In formation control layer, we can see each of the quadrotor as a particle in three-dimensional space, and the quadrotors system meet the type (4). ( ) n i t R  and ( ) ( 1, 2,3)n i t R n   represent position and speed information of quadrotor i. ( ) n i u t R is the corresponding input control. Assuming that the pilot only broadcast its own information r  and r  , the rest followers are able to receive state information from the pilot. The formation topology structure is shown in Fig.3. Regardless of the pilot, the topological relationship between followers can be arbitrary directed graph. 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 58 2 3 4 … N r  Figure 3. Communication topology graph In the process of formation, communication topology remains unchanged and maintains fixed formation. For the convenience of analysis, we consider the simple case of one dimension, the analysis of the three dimensional space can be obtained through the Kronecker product. The center of the formation is defined as c V , and its coordinate is ( , , ) c c c c R x y z . cV is the consistent balance point for consistency algorithm. If there is a leading vehicle, c V is the position of the leader, and when without a leader it is the geometric center. We adopt relative position vector to describe the formation and define the relative position deviation as * * * * ( , , ) T i i i i R x y z . The spatial location relations are shown in Fig.4. For the condition of the existence of a pilot, the following consistency algorithm is shown as:               * * * 1 d d i L i i ix x n ij i i j j ix jx j u t v x x x v v x x x x v v                          d x and d x x are respectively the information of the pilot’s position and speed.  is the adjacency relation between followers and the pilot. When a follower i can receive the pilot's state information, 0  ，otherwise 0  . Here we take 1  . In this algorithm, the followers also need to obtain the second order derivative information about the pilot status, namely the pilot input control. i R d R j R * i R * j R * * i j R R  x z y 0 Figure 4. The position relationship among formation members Theorem 3: If i  is the i characteristic values of n L , and i i      , 0  . when all of the 1n  nonzero eigenvalues of n L are all negative real part if   ,otherwise,         Re 0 Im 0 2 max Im cos arctan Re i i i and i i i v                 Therefore in the consistency of the algorithm (9), when t  , then *( ) d i i x x x  and d ix x v v , 1, 2, ,i n  . When t  , algorithm (9) will ensure *d i i x x x  and 0 d ix x v v  . At this time the formation reaches consistency and generates the specified array. On the stabilization issue of the formation, each quadrotor must agree on the formation center. The center’s position is changeless and the velocity is zero, which means 0 d x v  . Through the consistency algorithm, different formation can be designed only by changing the communication topology G and setting up different location deviation * i R . 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 59 IV. EXPERIMENT SIMULATION A. Parameter settings Quadrotor control adopts traditional PID control algorithm. The inner ring control is for posture stability while the outer ring for position control. The experiment assumes that the underlying controller is well performed. On that basis, the formation control module is inserted. The formation number is N = 5, and there are no obstacles in the flight space, the system mass m = 0.6kg, and the moment of inertia are 2 20.2 0.04 x y z J J kg m J kg m    ， the communication topological structure is shown in Fig.5. The Specific parameter is set as 1, 2   . Quadrotor numbers 1 is the pilot and the remaining four are the followers. The deviation to describe the relative position of the expectation formation is set to * * * * * 1 2 3 4 0 0.5 0 0.5 [ ] 0.5 0 0.5 0 0 0 0 0 R R R R R            32 5 4 1 Figure 5. Quadrotor’s communication topology in the simulation experiments This experiment sets two circumstances: formation rally and formation maneuvers. Case 1: the pilot hovers in the air waiting for formation in the designated position. At this point, the Setting 0 d v  ; Case 2: the pilot make circular motion at fixed height in the air, while the other four quadrotors track the movement of the pilot and ultimately form a set mobile fleet. The trajectory of the pilot is as follows: cos sin 5 d d d v t y t z        The simulation step is 0.001 s, and the simulation time is 15 s. B. Simulation results Case 1: Quadrotors formations are assembled as shown in Fig. 6. At the beginning of the simulation, the pilot waits at a position in the air while the other four take off from the ground. Each quadrotor maneuvers in the pre-selected direction and after a period of time they construct a diamond formation with the pilot as the center. That verifies the effectiveness of algorithm (9) in rallying the formation. Figure 6. Simulation curve of the generation of the quadrotors formation Case 2: Quadrotors formation maneuvers as shown in Fig.7. At the beginning of the simulation, the pilot do circular motion in the air, the followers in different positions on the ground, take off and receive information about the pilot status. in algorithm (9), each follower will apply the second order derivative of the pilot’s status information, which means the followers know the pilot’s control input at any time. The follower quadrotors do not simply gather towards the pilot, but do circular motion in the plane direction like the pilot. As a result they rise in a spiral and eventually converge to a stabile formation with the pilot as the center. 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 60 Figure 7. Simulation curve of quadrotor formation maneuver Fig. 8 and Fig. 9 show more clearly the rule of change in position and speed of each members of the formation from the X direction and Z direction. From Fig.8 (a), we can see the quadrotors rapidly gather to the pilot (the black curve) after they take off. At 4t s the followers get close to the specified location and they are able to track the trajectory change of the pilot. From Fig.8 (b), we can see after 5t s with consistency algorithm, the following quadrotors is consistent with the pilot’s speed and are able to track speed changes of the pilot. Fig.9 describes quadrotors’ position and speed change curve in Z direction. The changing rule is roughly the same to that in the X direction, so we do not talk about it in detail. Under the guidance of second order coherence algorithm, quadrotors formation converges at a higher speed and realizes better synchronization. (a) Position change in X-direction (b) Velocity change in X-direction Figure 8. Curve of each quadrotor’s position and velocity change in X-direction (a) Position change in Z-direction (b) Velocity change in Z-direction Figure 9. Curve of each quadrotor’s positon and velocity change in Z-direction V. CONCLUSION According to quadrotor aircraft fleet formation problem, the quadrotor is described as a second order integrator dynamics model and a method called distributed formation control method is designed based on graph theory and leading to follow method. The research is mainly focused on the second-order coherence algorithm in the application of the quadrotor aircraft fleet formation. In formation control layer, the quadrotor is regard as a particle in three-dimensional space, and by using the consistent algorithm with leader, the control requirements of marshalling and marshalling maneuver are realized; Finally, the validity of the consistency marshalling algorithm is verified by Matlab simulation. REFERENCES [1] WANG X.K., LI X., ZHENG Z.Q., Survey of Develop- ments on Multi-agent Formation Control Related Problems[J]. Control and Decision, 2013, 28 (11):1601-1613. [2] Ren W., Beard R.W., Distributed consensus in multi-vehicle cooperative control theory and applications[M]. New York: Spring, 2008. [3] WU Z.P., GUAN Z.H.,WU X.Y., Consensus based Formation Control of Multi-rotor System[J]. Control and Decision, 2007, 22(11):1241-1244. 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 61 [4] Matthew T, Nathan M, Vijay K. Trajectory Design and Control for Aggressive Formation Flight with Quadrotors [J]. Auton Robot, 2012, 33:143-156. [5] XING G.S., DU C.Y., ZONG Q., CHEN H.Y., SUN H.X., Consensus-based distributed motion planning for autonomous formation of miniature quadrotor groups[J]. Control and Decision, 2014, 29(11): 2081-2084. [6] Olfati-Saber R, Fax J A, Murray R M. Consensus and cooperation in Networked Multi-agent system[C]. Proc of the IEEE. 2007:215-233. [7] Wei R, Ella A, Distributed multi-vehicle coordinated control via local information exchange[J].Robust Nonlinear Control. 2007, 17:1002-1033. [8] Li G.C., Wang L., Wang Z.L., Xu D.X., Trajectory Tracking Control of Quad-rotor UAV Based on Quaternion[J]. Journal of Applied Sciences. 2012, 30(4):415-422.