Quadrotor Formation Inversion Control Method Based on Unit Quaternion

## Publications

/ Export Citation / / / Text size:

#### International Journal of Advanced Network, Monitoring and Controls

Xi'an Technological University

Subject: Computer Science , Software Engineering

eISSN: 2470-8038

29
60
Visit(s)
0
Comment(s)
0
Share(s)

SEARCH WITHIN CONTENT

FIND ARTICLE

Volume / Issue / page

Archive
Volume 5 (2020)
Volume 4 (2019)
Volume 3 (2018)
Volume 2 (2017)
Volume 1 (2016)
Related articles

VOLUME 3 , ISSUE 1 (May 2018) > List of articles

### Quadrotor Formation Inversion Control Method Based on Unit Quaternion

Citation Information : International Journal of Advanced Network, Monitoring and Controls. Volume 3, Issue 1, Pages 27-34, DOI: https://doi.org/10.21307/ijanmc-2018-006

Published Online: 07-May-2018

### ARTICLE

#### 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 independent 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.

## 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 environment. However, faced with diverse combat missions, single UAV is more and more difficult to meet the needs of the multi-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 formation 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 contains leader-follower method, virtual structure, graph theory and behavior-based[1]. At present, the mainstream is the integration 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 central 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 flexibility. 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.

## II. THE PROPERTY OF UNIT QUATERNION[12]

Quaternion is a number such as a + bi + cj + dk, where a b c d is real numbers and i, j, k is imaginary units. And a set of quaternion H = {a + bi + cj + dk|a, b, c, dR} is a four-dimensional vector space on real field R. η = a is the quaternion scalar part, and $q⇀=bi+cj+dk$ is the quaternion vector part. The quaternion can be written as follows: $q=(η,q⇀)$ The mold of the unit quaternion is a constant one, that is:

##### (1)
$η2+q⇀2=1$

Unit quaternary can be seen as a point on the unit sphere S3. 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 $q=(cosα2,sinα2n⇀)α∈[0,2π]$, therein $n⇀$ is unit vector.

Given a unit quaternion, the relationship between the corresponding attitude matrix and the unit quaternion is:

##### (2)
$R(q)=(η2+q⇀2)I3+2ηS(q⇀)+2q⇀Tq⇀.$

Where I3R3×3 is the unit matrix. The Euler angle is Θ = [φ θ Ψ], the attitude matrix can be expressed as:

##### (3)
$R(Θ)=[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θ]$

The error between the actual attitude matrix R and the expected attitude matrix Rd is defined as:

##### (4)
$R˜=RRdT$

Let $q=(η,q⇀)=[η q1 q2 q3]$ and $p=(ε,p⇀)=[ε p1 p2 p3]$ be two units of quaternions. the quaternion multiplication is defined as:

##### (5)
$q⊗p=(ηε−q⇀p⇀T, ηp⇀+εq⇀+S(q⇀)p⇀T)$

The quaternion does not satisfy the exchange law, which is qppq, but it satisfies the law of union, which is (qp)s = q(ps), and also satisfies the distribution law, which is (p + q)s = ps + qs. Defining $q*=(η,−q⇀)$ as the conjugate of the quaternion q. The conjugate of qp Satisfies (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 aircraft by controlling the speed of four independent motors and propellers. Quaternion is a simple and effective mathematical 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 installation 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 decomposed into the position subsystem Σ1 and the attitude subsystem Σ2, and getting the following model:

##### (6)
${∑1:{ξ˙=v,v˙=ge3−TmR(q)Te3,∑2:{q˙=12q⊗ω¯,ω˙=τ−S(ω)ω,$

In the formula, ξ, vR3 is the position and velocity of the quadrotor in the inertial coordinate system, g and m are the mass and gravitational acceleration of the quadrotor. Defining vector $ω¯=(0,ωT)T$, Where ωR3 represents the angular velocity in the body coordinate system. The unit vector for the z-axis is e3 [0 0 1]T. T is the lift to provide power system, τR3 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:

##### (7)
$S(ω)=[0−ω3ω2ω30−ω1−ω2ω10]$

## IV. CONTROLLER DESIGN AND CONVERGENCE ANALYSIS

### A. Problem description

Considering the UAV formation has N + 1 member, number 1 is assigned for the formation of unmanned aerial vehicles. Assuming that each UAV can access the status information of itself and other members of the formation through its own sensor and wireless communication network. According to the information exchange among unmanned aerial vehicles, UAV formation can be in the form of modeling. Assuming 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, making the UAVs close to the scheduled formation center aggregation. After reaching the specified spacing, the formation of the task is finished.

##### Figure 1.

Schematic diagram of quadrotor formation

Regardless of the difference in the quadrotor in the formation, the quadrotor is isomorphic and conforms to the model (6):

##### (8)
${∑i1:{ξ˙i=vi,v˙i=ge3−TimiR(qi)Te3,∑i2:{q˙i=12qi⊗ω¯i,ω˙i=τi−S(ωi)ωi,$

Define the position error of the i UAV is $ξ˜i=ξi−ξd−δi$, the velocity error is $v˜i=vi−ξ˙id$, the angular velocity error is $ω˜=ω−Rωd$, and the error system model of the i unmanned aerial vehicle which derivatived from above parameters is:

##### (9)
${ξ˜˙i=v˜iv˜˙i=ge3−1mRide3Tid−ξ¨id+1m(I−R˜id)e3Tidq˜˙i=12(S(q˜i)+ηiI3)ω˜iω˜˙i=J−1(τid−(ω˜i+R˜iωid)×J(ω˜i+R˜iωid)) −R˜i(ω˙id−S(ω˜i)ωid)$

In the formula, nid = Ride3 is the last column of the expected attitude matrix, Tid and τid is the parameter of control for the system. $q˜=(n˜,q˜⇀)$ is the error quaternion, and its calculation satisfies the quaternion multiplication algorithm:

##### (10)
$q˜=q⊗qd=(ηηd−q⇀q⇀dT,ηq⇀d+ηdq⇀+S(q⇀)q⇀dT)$

$R˜$ is the attitude error matrix, By the formula (2) available:

##### (11)
$q˜⇀$

The work which is done in this paper is to design the virtual control volume Ti and the control torque τi for the i th four-rotor unmanned aerial vehicle, so that the four-rotor formation maintains a fixed formation and a fixed distance. That is:.

$vi→vj→vd, ξi−ξj→δi−δj=δij.$

In the formula, vd is the expected speed for the formation reference, δij is the desired distance between the i th unmanned 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 quadrotor error system model is decomposed into position error subsystem and attitude error subsystem.

##### (12)
$∑1:{ξ˜˙i=v˜iv˜˙i=ge3−1mnidTid−ξ¨id$
##### (13)
$∑2:{q˜˙=12(S(q˜i)+ηiI3)ω˜iω˜˙i=J−1(τid−(ω˜i+R˜ωid)+J(ω˜i+R˜iωid)) −R˜i(ω˙id−S(ω˜i)ωid)$

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:

##### (14)
${z1=ξ˜iz2=v˜i−α1(z1)$

In the formula, α1 is the virtual control. V function is defined for each virtual feedback so that each state component has an appropriate progressive. The equation (14) is essentially a differential homeomorphism. To stabilize the original system, it is only necessary to stabilize the error between the original system state and the virtual feedback.

Step 1: Find the derivative of x

##### (15)
$z˙1=ξ˜˙i=v˜i$

Define the first Lyapunov function: $V1=z1Tz1/2$:

##### (16)
${z˙1=α1(z1)+z2=−k1z1+z2V˙1=z1Tz˙1=z1T(α1(z1)+z2)=−k1z1Tz1+z1Tz2$

Obviously, if z2 = 0, z1 asymptotically stable by the above formula, but in general z2 ≠ 0, therefore it needs to proceed to the next step, so that z2 has the desired progressive characteristics. V2 the derivative of time is:

##### (17)
$V˙2=−k1z1Tz1+z1Tz2 +z2T(ge3−1mnidTid−ξ¨id+k1(z2−k1z1))$

take $1mnidTid=ge3−ξ¨id+(1−k2)z1+(1+k1k2)z2$, now $V˙2=−k1z1Tz1−k2z2Tz2$ There are positive numbers k1, k2 in the formula. Thus, the z1 and z2 exponents converge to 0, which is globally exponentially stable. According to the nature of the attitude matrix, nid is a unit vector ||nid|| = 1. so each member of the formation of the expected lift Tid and virtual control of the amount of nid is acquired.

##### (18)
${Tid=m‖ge3−ξ¨id+(k1+k2)v˜i+(1+k1k2)ξ˜i‖nid=(ge3−ξ¨id+(k1+k2)v˜i+(1+k1k2)ξ˜i)/(Tid/m)$

In the formula, nid satisfy the constraint equation ||nid|| = 1, Since nid 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, Ψid = 0. ψid and θid can be obtained by calculation. The desired attitude matrix Rid is obtained according to equation (3) and then the desired quaternion (ηid and $q⇀id$) is obtained according to equation (2). Quaternion error $η˜i$ and $q⇀˜i$ can be obtained through the formula (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 defined as

##### (19)
${z3=q⇀˜iz4=ω˜−α2(z3)$

α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 derivation process reference literature [10].

##### (20)
$τid=ωi×Jωi+J(R˜iω˙id−R˜iS(ω˜i)ωid)+α˙2(ε3)−k4ε4−12(−S(ε3)+1−ε3Tε3I3)ε3$

The reference signal is negotiated by the formation members through a coherence algorithm. The reference signal needs to meet certain constraints. Assuming that the reference signals ξid, $ξid, ξ˙id, ξ¨id$ and $ξid(3)$ are bounded and visible to all formation members.

The desired position ξid and the desired velocity vid are obtained by taking the weighted average of all the members in the formation

##### (21)
$ξid=1Ni∑j=1Niξj$
##### (22)
$vid=1Ni∑j=1Nivj$

It translates the formation problem into a trajectory tracking problem for a given reference signal. In the formula, Ni is the number of aircraft in the formation with the i th unmanned aerial vehicle as a neighbor. Considering the situation that the formation communication topology is no omnidirectional graph, at this time, in addition 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:

##### (23)
$ξd=1N∑j=1Nξj, vd=1N∑j=1Nvj$

The formation of the ith UAV can be guaranteed through the inversion control law. ξiξd + δi, vivd, ξiξjδiδj = δij, vivj can be guaranteed.

## V. EXPERIMENTAL SIMULATION

### A. Parameter setting

Regardless of the differences among the quadrotor systems, the system quality is assumed m = 0.6 kg. The moment of inertia is Jx = Jy = 0.2kg·m2, Jz = 0.04kg·m2.

Setting the formation size N = 5, k1 = 10, k2 = 10. Specify the number five of the quadrotor for the pilot, the remaining four for the followers. Assuming that the pilot broadcasts 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 P0 is:

$P0=[ξ10 ξ20 ξ30 ξ40] =[0.8−1.5−0.411.50.9−0.5−1.20−2−1.50]$

The pilot does uniform circular motion in a fixed height, the movement trajectory is:

${x=cosπty=sinπtz=5$

The relative positional deviation of the formation is described by:

$δ=[δ1 δ2 δ3 δ4]=[0−0.500.50.50−0.500000]$

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 algorithm is verified. Figure 2 is the three-dimensional map of the formation simulation. In the figure, the black curve is the trajectory of the pilot, and the triangle “△” indicates the final position of each quadrotor UAV. At the beginning of the simulation, the pilot does circular motion in the air, followers locates 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.

##### Figure 2.

Quadrotor formation track simulation curve

Fig. 3-Fig. 5 shows the change of the position and velocity of the fleet members in the formation and maneuvering process. It can be seen from the figure that the followers quickly move closer to the pilot after takeoff. At t = 1s, the distance 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 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 trajectory 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

## 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 formation. At the beginning of the formation, team members calculate the formation of the geometric center as a reference signal through the wireless network exchange position, speed and other state information. Each UAV is designed 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.
[CROSSREF]
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]. 95th 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.

### FIGURES & TABLES

Figure 1.

Schematic diagram of quadrotor formation

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

Figure 6.

Reference trajectory and pilot trajectory error

### 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.
[CROSSREF]
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]. 95th 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.