2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 27 Quadrotor Formation Inversion Control Method Based on Unit Quaternion Wang Zhongsheng School of Computer Science and Engineering Xi'an Technological University Xi’an, 710032, China e-mail: 59483672@qq.com Yang Sen Department of UAV Engineering, Ordance Engineering College, Shijiazhuang, China School of Automation Science and Electrical Engineer- ing, Beihang University, Beijing, China Dong Hairui Department of UAV Engineering Ordance Engineering College Shijiazhuang, China Abstract—In this paper, the formation control problem of quadrotor is studied under ideal communication condition. The quadrotor has a complex mathematical model. First, the unit quaternion method is used to describe its dynamic model and kinematic model. It is decomposed into two in- dependent subsystems of position and attitude. The tracking error model is established by introducing the error between the ture trajectory and the desired trajectory. And then appointing a member of formation as a pilot, formation members get the geometric center position as the desired trajectory through the consistency algorithm. The back- stepping method is used to design the time-varying feedback control law for each four-rotor, so that the formation is stabilized. Finally, the effectiveness of the control method is verified by simulation experiments. Keyword-Formation Control; Quadrotor; Quaternion; Inversion Method; Leader-follower Method I. INTRODUCTION In recent years, with the rapid development of small unmanned aerial vehicles, researchers are concerned with quadrotor because it has simple structure and is easy to control. Quadrotor can not only carry out vertical takeoff, landing and autonomous hover function, but also the task efficiently under the uncertain and dangerous environ- ment. However, faced with diverse combat missions, single UAV is more and more difficult to meet the needs of the mul- ti-UAV collaborative control concept come into being. UAV formation control is an important basic research track in the field of multi-UAV collaborative control. Reasonable forma- tion control method can make the UAV formation quickly get the air superiority and finish the combat task more efficiently. On the future battlefield, UAV will play an irreplaceable role. The research of methods of formation control mainly con- tains leader-follower method, virtual structure, graph theory and behavior-based[1]. At present, the mainstream is the inte- gration of the above methods. Early formation control mainly uses centralized control method that is characterized by high precision and easy to control, but depends on the calculation capabilities and global communication capabilities of the cen- tral control unit. With the increasing number of the formation members, the calculation of the central control unit increases exponentially, therefore this method lacks scalability and flex- ibility. Later, people propose the distributed control method, in which each UAV only communicates with the adjacent UAV. The surrounding UAV relative position relationship is acquired, compared to the expected formation by using of UAV's own computing power. The actual position of the UAV is corrected to eliminate the formation error. 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 28 In recent years, many scholars aim at problems of the quadrotor formation , and they do a lot of research and design different types of controllers. In literature [2-6], the controller is designed with different degree of simpli- fication to quadrotor model by feedback linearization and small perturbation linearization. However, the quadrotor is a typical underactuated system, a cascade nonholo- nomic system with complex constraint equations, which has a strong nonlinearity and thus needs a higher re- quirement for the control system. In literature [7], the quadrotor is also modeled by unit quaternion and the concept of manifold is introduced. The attitude control algorithm is designed under the differential geometry framework and gets a nice effect. Literature [8] combines the nonlinear part of the system identified by neural net- work online and leader-follower method to realize the formation control of the quadrotor. In literature [9], the kinetics and kinematics models of the quadrotor are de- scribed by quaternion and the intermediate control is in- troduced. The formation is stabilized by setting the ap- propriate intermediate control for each UAV. In literature [10], the error of the actual position and the expected po- sition is introduced, and the tracking error model is estab- lished. Literature [11] also divides quadrotor model into two independent subsystems of position and attitude, and each of them uses backstepping method to design the time-varying feedback in order to make the system stable. The backstepping method is a design method of forward and backward recursion. It makes the system error pro- gressive and stable, and it can also reduce the difficulty of design by designing the Lyapunov function step by step. Based on the above research, the quadrotor mathematical model is established by using the unit quaternion method. Then the geometric center position of the formation is calculated by the consistency algorithm and is used as the expected trajectory. Finally the whole formation is stable by designing Backstepping Control for each quadrotor. II. THE PROPERTY OF UNIT QUATERNION [12] Quaternion is a number such as a b c d  i j k , where , , ,a b c d is real numbers and , ,i j k is imaginary units. And a set of quaternion  , , , RH a b c d a b c d    i j k is a four-dimensional vector space on real field R. a  is the quaternion scalar part, and q b c d    i j k is the quater- nion vector part. The quaternion can be written as follows:  , qq =  The mold of the unit quaternion is a constant one, that is: 2 2 q 1     Unit quaternary can be seen as a point on the unit sphere 3 S . Quaternion is a powerful tool that represents rotation in three-dimensional space. Based on Euler theorem, any rotation in a three-dimensional space can be obtained by rotating an angle about the characteristic axis and the corresponding unit quaternion is cos , sin 2 2 n        q =   0, 2  ,therein n  is unit vector. Given a unit quaternion, the relationship between the cor- responding attitude matrix and the unit quaternion is:      2 2 3 2 2 T R q I S q q q    q      Where 3 3 3 I R   is the unit matrix. The Euler angle is      , the attitude matrix can be expressed as: 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 R                                                The error between the actual attitude matrix R and the ex- pected attitude matrix d R is defined as: T d R RR  Let    1 2 3, q q q q  q  and    1 2 3, p p p p p =  be two units of quaternions. the quaternion multiplication is defined as:   ,T Tq p qp p q S q p             The quaternion does not satisfy the exchange law, which is qp pq , but it satisfies the law of union, which is    qp s q ps , and also satisfies the distribution law, which is 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 29  p q s ps qs   . Defining  * , q   q as the conjugate of the quaternion q .The conjugate of qp Satis- fies   * * * qp p q . III. QUADROTOR MODEL BASED ON UNIT QUATERNION The quadrotor UAVs are usually divided into Type X and Type + , with four inputs and six outputs, which are typical underactuated systems. The quadrotor carries out the movements, pitch, roll and yaw of the air- craft by controlling the speed of four independent motors and propellers. Quaternion is a simple and effective ma- thematical tool that describes the rotation and movement of rigid bodies in three-dimensional space, which can effectively avoid the appearance of singularity and have the characteristics of high efficiency etc. The quadrotor UAV is regarded as the rigid body structure. Assuming that the center of gravity is located at the origin of the coordinate system of the body. The motor has no installa- tion error angle, and the motor lift surface is located on the same plane as the aircraft center of mass. Using the quaternion to establish the quadrotor of the dynamics model and kinematics model, the quadrotor is decom- posed into the position subsystem 1  and the attitude subsystem 2  , and getting the following model:     1 3 3 2 , : , 1 , : 2 , T v T v ge R e m S                          q q q      In the formula, 3 , v R  is the position and velocity of the quadrotor in the inertial coordinate system, g and m are the mass and gravitational acceleration of the qua- drotor. Defining vector  0, T T   , Where 3 R represents the angular velocity in the body coordinate system. The unit vector for the z-axis is 3 [0 0 1] T e  .T is the lift to provide power system, 3 R  is the role which has effect on the body in the rolling moment, pitch torque and yaw moment.  S  is the antisymmetric matrix for  . For  1 2 3 T     , there is:   3 2 3 1 2 1 0 0 0 S                    IV. CONTROLLER DESIGN AND CONVERGENCE ANALYSIS A. Problem description Considering the UAV formation has 1N  member, number 1 is assigned for the formation of unmanned aerial vehicles. Assuming that each UAV can access the status in- formation of itself and other members of the formation through its own sensor and wireless communication network. Accord- ing to the information exchange among unmanned aerial ve- hicles, UAV formation can be in the form of modeling. As- suming that the UAV communication topology is an inbound connection diagram. As shown in Fig.1. At the beginning of the formation, the formation members obtain their own and other members of the formation of state information through the wireless communication network, then the formation of the desired location center and the expected speed are obtained by consistency algorithm, and output to the pose controller, mak- ing the UAVs close to the scheduled formation center aggrega- tion. After reaching the specified spacing, the formation of the task is finished. Figure 1. Schematic diagram of quadrotor formation 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 30 Regardless of the difference in the quadrotor in the formation, the quadrotor is isomorphic and conforms to the model (6):     1 3 3 2 , : - , 1 , : 2 , i i Ti i i i i i i i i i i i i v T v ge R e m S                          q q q      Define the position error of the i UAV is i i d i       , the velocity error is i i idv v    , the angular velocity error is d R    , and the error system model of the i unmanned aerial vehicle which derivatived from above parameters is:              3 3 3 3 1 1 1 1 2 i i i id id id id id i i i i i id i i id i i id i id i id v v g R T I R T m m q S q I J R J R R S                                                      e e e  In the formula, 3id id n R e is the last column of the expected attitude matrix, id T and id  is the parameter of control for the system.  ,q q    is the error quaternion, and its calculation satisfies the quaternion multiplication algorithm:   ,T Td d d d d dq q q qq q q S q q                R is the attitude error matrix, By the formula (2) available:    2 2 3 2 2 T R q I S q q q                The work which is done in this paper is to design the virtual control volume i T and the control torque i  for the i th four-rotor unmanned aerial vehicle, so that the four-rotor for- mation maintains a fixed formation and a fixed distance. That is：. i j d v v v  , i j i j ij         . In the formula, d v is the expected speed for the formation reference, ij  is the desired distance between the i th un- manned aerial vehicle and the j th unmanned aerial vehicle, and is satisfied with ij ji    . B. Controller design According to the cascade system analysis method, the qua- drotor error system model is decomposed into position error subsystem and attitude error subsystem. 1 3 : 1 i i i id id id v v g n T m               e             3 1 2 1 2 : i i i i id i id i i id i id i id q S q I J R J R R S                                         The following reference [] uses the BackStepping method to design the position subsystem controller and the attitude subsystem controller separately. First step is to define the error between the system state and the virtual feedback:   1 2 1 1 i i z z v z           In the formula, 1  is the virtual control. V function is de- fined for each virtual feedback so that each state component 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 31 has an appropriate progressive. The equation (14) is es- sentially a differential homeomorphism. To stabilize the original system, it is only necessary to stabilize the error between the original system state and the virtual feed- back. Step 1: Find the derivative of x 1 i i z v     Define the first Lyapunov function: 1 1 1 /2 T V z z ,and 1 1 1 1i k z k      :      1 1 1 2 1 1 2 1 1 1 1 1 1 2 1 1 1 1 2 T T T T z z z k z z V z z z z z k z z z z                    Obviously, if 2 0z  , 1 z asymptotically stable by the above formula, but in general 2 0z  , therefore it needs to proceed to the next step, so that 2 z has the desired progressive characteristics. 2 V the derivative of time is:   2 1 1 1 1 2 2 3 1 2 1 1 1 T T T id id id V k z z z z z g n T k z k z m                e   take    23 1 1 2 2 1 1 1 id id id n T g k z k k z m      e ,now 2 1 1 1 2 2 2 T T V k z z k z z   There are positive numbers 1 2 ,k k in the formula. Thus, the 1 z and 2 z exponents converge to 0, which is globally exponentially stable. According to the nature of the attitude matrix, id n is a unit vector 1 id n  .so each member of the formation of the expected lift id T and virtual control of the amount of id n is acquired.            3 1 2 1 2 3 1 2 1 2 1 1 id id i i id id i i id T m g k k v k k n g k k v k k T m                        e e  In the formula, idn satisfy the constraint equation 1idn  , Since idn does not contain all the desired gesture information Where the aircraft can be in the course of flight to maintain the heading angle of 0°, that is, 0 id   . id  and id  can be ob- tained by calculation. The desired attitude matrix id R is ob- tained according to equation (3) and then the desired quater- nion( id  and id q  ) is obtained according to equation (2). Qu- aternion error i  and i q  can be obtained through the formu- la (10). The same method is used to design the control law for the attitude subsystem to stabilize the global exponent, and the error between the system state and the virtual feedback is de- fined as   3 4 2 3 i z q z z          2  is the virtual control amount. Formation control is mainly concerned with the position control subsystem of the aircraft. Therefore, the quadrotor UAV i posture subsystem inversion control law is given directly next, the specific deriva- tion process reference literature [10].          2 3 4 4 3 3 3 3 3 1 1 2 id i i i id i i id T J J R R S k S I                            The reference signal is negotiated by the formation mem- bers through a coherence algorithm. The reference signal needs to meet certain constraints. Assuming that the reference signals 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 32 id  ， id  ， id  and  3 id  are bounded and visible to all formation members. The desired position id  and the desired velocity id v are obtained by taking the weighted average of all the members in the formation  1 1 i N id j ji N       1 1 i N id j ji v v N     It translates the formation problem into a trajectory tracking problem for a given reference signal. In the for- mula, i N is the number of aircraft in the formation with the i th unmanned aerial vehicle as a neighbor. Consi- dering the situation that the formation communication topology is no omnidirectional graph, at this time, in ad- dition to the pilot, any aircraft in the formation has the same status, so the subscript i can be ignored. Formula (21) and (22) are rewritten as follows: 1 1 N d j jN       1 1 N d j j v v N     The formation of the ith UAV can be guaranteed through the inversion control law. i d i     , i d v v , i j i j ij         , i j v v can be guaranteed. V. EXPERIMENTAL SIMULATION A. Parameter setting Regardless of the differences among the quadrotor systems, the system quality is assumed 0.6m kg .The moment of inertia is 2 0.2 x y J J kg m   ， 2 0.04 kg z J m  . Setting the formation size 5N  ， 1 10k  ， 2 10k  . Specify the number five of the quadrotor for the pilot, the re- maining four for the followers. Assuming that the pilot broad- casts its own state information, the followers are able to receive that in real time, and the communication topology between the followers is an undirected graph. The follower initial position matrix 0P is:  0 10 20 30 40 0.8 1.5 0.4 1 1.5 0.9 0.5 1.2 0 2 1.5 0 P                   The pilot does uniform circular motion in a fixed height, the movement trajectory is: cos sin 5 x t y t z        The relative positional deviation of the formation is de- scribed by:  1 2 3 4 0 0.5 0 0.5 0.5 0 0.5 0 0 0 0 0                  The simulation time is 9.5s and the simulation step is 0.001s B. Simulation results According to the above parameters, the system model is built with Simulink module in MATLAB, and the control algo- rithm is verified. Figure 2 is the three-dimensional map of the formation simulation. In the figure, the black curve is the tra- jectory of the pilot, and the triangle "△ " indicates the final position of each quadrotor UAV. At the beginning of the simu- lation, the pilot does circular motion in the air, followers lo- cates on the different positions on the ground, taking off after receiving the state information of the pilot, the first team members calculate the formation center position through the negotiation of calculation , and as a reference tracking signal, Algorithm(18) eventually converges the five quadrilateral to the intended formation. A variety of formation can be easily designed by setting a different relative position error. Fig.3-Fig.5 shows the change of the position and velocity of the fleet members in the formation and maneuvering process. 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 33 It can be seen from the figure that the followers quickly move closer to the pilot after takeoff. At t = 1s, the dis- tance from the pilot tends to be the designated position and remains at the same speed as the pilot, and is well tracked by the pilot. The validity of the tactics proposed in this paper is verified. Figure 2. Quadrotor formation track simulation curve Figure 3. X-direction UAV position and speed curve Figure 4. Y-direction UAV position and speed curve Figure 5. Z-direction UAV position and speed curve Fig.6 shows the error between the formation center trajec- tory and the pilot's trajectory calculated by the formation member's negotiation. It can be seen that the formation center finally converges to the position of the pilot. Figure 6. Reference trajectory and pilot trajectory error 2018 International Conference on Sensor Network and Computer Engineering (ICSNCE 2018) 34 VI. CONCLUSION Aiming at the control problem of quadrotor formation, the nonlinear dynamic model and kinematic model are described by unit quaternion. The formation control is achieved by tracking the geometric center of the forma- tion. At the beginning of the formation, team members calculate the formation of the geometric center as a refer- ence signal through the wireless network exchange posi- tion, speed and other state information. Each UAV is de- signed with time-varying feedback control law through the Back stepping method which translates the formation problem into a tracking problem for a given reference signal. The simulation results of matlab show that the method is fast and accurate and the convergence speed of the formation system is improved. The simulation results show that the proposed method is fast and accurate. At present, the method has not yet considered the formation problem and the formation problem with the maximum velocity constraint. The subsequent study will consider the problem of formation, maintenance and reconstruction of the formation under the constraint of uniformity and maximum speed . ACKNOWLEDGMENT Fund support: Shaanxi Education Department Special Fund Project number: Shaanxi Education Finance[2013] 23 REFERENCES [1] Survey of developments on multi-agent formation control related problems[J]. Control and Decision, 2013,28(11):1601-1612. [2] F.Rinaldi, S.Chiesa, F.Quagliotti. Linear Quadratic Control for Quadrotors UAVs Dynamics and Formation Flight [J]. Intell Robot Syst. 2013(70): 203-220. [3] Xia Liang-sheng. Formation Control for Multi-Robot Based on Flocking[J]. Fire Control & Command Control, 2013,38(1):18-21. [4] Yu B C, Dong X W, shi ZY, et al. Formation control for quadrotor swarm systems: Algorithms and experiments [A]. Proceedings of the 32nd Chinese Control Conference[C]. 26-28, July 2013, xi’an China. [5] Wang Honghao, Chen Ming etc. Attitude estimation of quadrotor based on quaternion and Kalman filtering [J]. Microcomputer and Application，2016,35(14):71-76. [6] Xing Guan-sheng, Du Chun-yan et al. Consensus-based distributed motion planning for autonomous formation of miniature quadrotor groups[J]. Control and Decision, 2014,29(11):2081-2094. [7] Fu Changyou, Cai Hongbin etc. Design of Micro quad-rotor aircraft based on IOT[]. Modern Electronic Technique, 2016,39(13):108-111. [8] Choi InSung, Choi JongSuk. Leader-Follower Formation Control Using PID Controller[A]. 5th International Conference [C]. 3-5, October, 2012, Montreal, Canada. [9] Wu Cheng-fu, Liu Xiao-qi etc. Modeling and control for a quadrotor UAV based on quaternion[J]. FLIGHT DYNAMICS, 2013,31(2):122-125. [10] Dierks T, Jagannathan S. Neural network control of quadrotor UAV formations [A]. American Control Conference [C]. St.Louis, USA, 2009. [11] Abdessameud A, Tayebi A. Formation control of VTOL-UAVs [A]. Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference [C]. 2009. [12] Li Guang-chun, Wang Lu, Wang Zhao-long et al. Trajectory Tracking Control of Quad-rotor UAV Based on Quaternion[J]. JOURNAL OF APPLIED SCIENCES, 2012, 30(4):415-422.