Quadrotor Formation Inversion Control Method Based on Unit Quaternion

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.

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 simplification to quadrotor model by feedback linearization and small perturbation linearization. However, the quadrotor is a typical underactuated system, a cascade nonholonomic system with complex constraint equations, which has a strong nonlinearity and thus needs a higher requirement 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 network 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 described by quaternion and the intermediate control is introduced. The formation is stabilized by setting the appropriate intermediate control for each UAV. In literature [10], the error of the actual position and the expected position is introduced, and the tracking error model is established. 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 progressive 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 + 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, d ∈ R} is a four-dimensional vector space on real field R. η = a is the quaternion scalar part, and $\overset{⇀}{q} = b i + c j + d k$ is the quaternion vector part. The quaternion can be written as follows: $q = (η, \overset{⇀}{q})$ The mold of the unit quaternion is a constant one, that is:(1) $η^{2} + {\overset{⇀}{q}}^{2} = 1$

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

Given a unit quaternion, the relationship between the corresponding attitude matrix and the unit quaternion is:(2) $R (q) = (η^{2} + {\overset{⇀}{q}}^{2}) I_{3} + 2 η S (\overset{⇀}{q}) + 2 {\overset{⇀}{q}}^{T} \overset{⇀}{q} .$

Where I₃ ∈ R^3×3 is the unit matrix. The Euler angle is Θ = [φ θ Ψ], the attitude matrix can be expressed as:(3) $R (Θ) = [\begin{matrix} 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 θ \end{matrix}]$

The error between the actual attitude matrix R and the expected attitude matrix R_d is defined as:(4) $\tilde{R} = R R_{d}^{T}$

Let $q = (η, \overset{⇀}{q}) = [η q_{1} q_{2} q_{3}]$ and $p = (ε, \overset{⇀}{p}) = [ε p_{1} p_{2} p_{3}]$ be two units of quaternions. the quaternion multiplication is defined as:(5) $q \otimes p = (η ε - \overset{⇀}{q} {\overset{⇀}{p}}^{T}, η \overset{⇀}{p} + ε \overset{⇀}{q} + S (\overset{⇀}{q}) {\overset{⇀}{p}}^{T})$

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 (p + q)s = ps + qs. Defining $q * = (η, - \overset{⇀}{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 Σ₁ and the attitude subsystem Σ₂, and getting the following model:(6) ${\begin{cases} \sum_{1} : {\begin{array}{l} \dot{ξ} = v, \\ \dot{v} = g e_{3} - \frac{T}{m} R {(q)}^{T} e_{3}, \end{array} \\ \sum_{2} : {\begin{array}{l} \dot{q} = \frac{1}{2} q \otimes \bar{ω}, \\ \dot{ω} = τ - S (ω) ω, \end{array} \end{cases}$

In the formula, ξ, 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 quadrotor. Defining vector $\bar{ω} = {(0, ω^{T})}^{T}$ , Where ω ∈ R³ represents the angular velocity in the body coordinate system. The unit vector for the z-axis is e₃ [0 0 1]^T. T is the lift to provide power system, τ ∈ 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 ω = [ω₁ω₂ω₃[^T, there is:(7) $S (ω) = [\begin{matrix} 0 & - ω_{3} & ω_{2} \\ ω_{3} & 0 & - ω_{1} \\ - ω_{2} & ω_{1} & 0 \end{matrix}]$

IV.

CONTROLLER DESIGN AND CONVERGENCE ANALYSIS

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.

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) ${\begin{cases} \sum_{i 1} : {\begin{array}{l} {\dot{ξ}}_{i} = v_{i}, \\ {\dot{v}}_{i} = g e_{3} - \frac{T_{i}}{m_{i}} R {(q_{i})}^{T} e_{3}, \end{array} \\ \sum_{i 2} : {\begin{array}{l} {\dot{q}}_{i} = \frac{1}{2} q_{i} \otimes {\bar{ω}}_{i}, \\ {\dot{ω}}_{i} = τ_{i} - S (ω_{i}) ω_{i}, \end{array} \end{cases}$

Define the position error of the i UAV is ${\tilde{ξ}}_{i} = ξ_{i} - ξ_{d} - δ_{i}$ , the velocity error is ${\tilde{v}}_{i} = v_{i} - {\dot{ξ}}_{i d}$ , the angular velocity error is $\tilde{ω} = ω - R ω_{d}$ , and the error system model of the i unmanned aerial vehicle which derivatived from above parameters is:(9) ${\begin{array}{l} {\dot{\tilde{ξ}}}_{i} = {\tilde{v}}_{i} \\ {\dot{\tilde{v}}}_{i} = g e_{3} - \frac{1}{m} R_{i d} e_{3} T_{i d} - {\ddot{ξ}}_{i d} + \frac{1}{m} (I - {\tilde{R}}_{i d}) e_{3} T_{i d} \\ {\dot{\tilde{q}}}_{i} = \frac{1}{2} (S ({\tilde{q}}_{i}) + η_{i} I_{3}) {\tilde{ω}}_{i} \\ \begin{array}{l} {\dot{\tilde{ω}}}_{i} = J^{- 1} (τ_{i d} - ({\tilde{ω}}_{i} + {\tilde{R}}_{i} ω_{i d}) \times J ({\tilde{ω}}_{i} + {\tilde{R}}_{i} ω_{i d})) \\ - {\tilde{R}}_{i} ({\dot{ω}}_{i d} - S ({\tilde{ω}}_{i}) ω_{i d}) \end{array} \end{array}$

In the formula, n_id = R_ide₃ is the last column of the expected attitude matrix, T_id and τ_id is the parameter of control for the system. $\tilde{q} = (\tilde{n}, \overset{⇀}{\tilde{q}})$ is the error quaternion, and its calculation satisfies the quaternion multiplication algorithm:(10) $\tilde{q} = q \otimes q_{d} = (η η_{d} - \overset{⇀}{q} {\overset{⇀}{q}}_{d}^{T}, η {\overset{⇀}{q}}_{d} + η_{d} \overset{⇀}{q} + S (\overset{⇀}{q}) {\overset{⇀}{q}}_{d}^{T})$

$\tilde{R}$ is the attitude error matrix, By the formula (2) available:(11) $\overset{⇀}{\tilde{q}}$

The work which is done in this paper is to design the virtual control volume T_i 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:. $v_{i} \to v_{j} \to v_{d}, ξ_{i} - ξ_{j} \to δ_{i} - δ_{j} = δ_{i j} .$

In the formula, v_d 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.

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)

\sum_{1} : {\begin{array}{l} {\dot{\tilde{ξ}}}_{i} = {\tilde{v}}_{i} \\ {\dot{\tilde{v}}}_{i} = g e_{3} - \frac{1}{m} n_{i d} T_{i d} - {\ddot{ξ}}_{i d} \end{array}

(13)

\sum_{2} : {\begin{array}{l} \dot{\tilde{q}} = \frac{1}{2} (S ({\tilde{q}}_{i}) + η_{i} I_{3}) {\tilde{ω}}_{i} \\ {\dot{\tilde{ω}}}_{i} = J^{- 1} (τ_{i d} - ({\tilde{ω}}_{i} + \tilde{R} ω_{i d}) + J ({\tilde{ω}}_{i} + {\tilde{R}}_{i} ω_{i d})) \\ - {\tilde{R}}_{i} ({\dot{ω}}_{i d} - S ({\tilde{ω}}_{i}) ω_{i d}) \end{array}

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) ${\begin{cases} z_{1} = {\tilde{ξ}}_{i} \\ z_{2} = {\tilde{v}}_{i} - α_{1} (z_{1}) \end{cases}$

In the formula, α₁ 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) ${\dot{z}}_{1} = {\dot{\tilde{ξ}}}_{i} = {\tilde{v}}_{i}$

Define the first Lyapunov function: $V_{1} = z_{1}^{T} z_{1} / 2$ :(16) ${\begin{cases} {\dot{z}}_{1} = α_{1} (z_{1}) + z_{2} = - k_{1} z_{1} + z_{2} \\ {\dot{V}}_{1} = z_{1}^{T} {\dot{z}}_{1} = z_{1}^{T} (α_{1} (z_{1}) + z_{2}) = - k_{1} z_{1}^{T} z_{1} + z_{1}^{T} z_{2} \end{cases}$

Obviously, if z₂ = 0, z₁ asymptotically stable by the above formula, but in general z₂ ≠ 0, therefore it needs to proceed to the next step, so that z₂ has the desired progressive characteristics. V₂ the derivative of time is:(17) $\begin{array}{l} {\dot{V}}_{2} = - k_{1} z_{1}^{T} z_{1} + z_{1}^{T} z_{2} \\ + z_{2}^{T} (g e_{3} - \frac{1}{m} n_{i d} T_{i d} - {\ddot{ξ}}_{i d} + k_{1} (z_{2} - k_{1} z_{1})) \end{array}$

take $\frac{1}{m} n_{i d} T_{i d} = g e_{3} - {\ddot{ξ}}_{i d} + (1 - k^{2}) z_{1} + (1 + k_{1} k_{2}) z_{2}$ , now ${\dot{V}}_{2} = - k_{1} z_{1}^{T} z_{1} - k_{2} z_{2}^{T} z_{2}$ There are positive numbers k₁, k₂ in the formula. Thus, the z₁ and z₂ exponents converge to 0, which is globally exponentially stable. According to the nature of the attitude matrix, n_id is a unit vector ||n_id|| = 1. so each member of the formation of the expected lift T_id and virtual control of the amount of n_id is acquired.

(18)

{\begin{cases} T_{i d} = m ‖ g e_{3} - {\ddot{ξ}}_{i d} + (k_{1} + k_{2}) {\tilde{v}}_{i} + (1 + k_{1} k_{2}) {\tilde{ξ}}_{i} ‖ \\ n_{i d} = (g e_{3} - {\ddot{ξ}}_{i d} + (k_{1} + k_{2}) {\tilde{v}}_{i} + (1 + k_{1} k_{2}) {\tilde{ξ}}_{i}) / (T_{i d} / m) \end{cases}

In the formula, n_id satisfy the constraint equation ||n_id|| = 1, Since n_id 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 R_id is obtained according to equation (3) and then the desired quaternion (η_id and ${\overset{⇀}{q}}_{i d}$ ) is obtained according to equation (2). Quaternion error ${\tilde{η}}_{i}$ and ${\tilde{\overset{⇀}{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) ${\begin{cases} z_{3} = {\tilde{\overset{⇀}{q}}}_{i} \\ z_{4} = \tilde{ω} - α_{2} (z_{3}) \end{cases}$

α² 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)

\begin{matrix} τ_{i d} = ω_{i} \times J ω_{i} + J ({\tilde{R}}_{i} {\dot{ω}}_{i d} - {\tilde{R}}_{i} S ({\tilde{ω}}_{i}) ω_{i d}) + {\dot{α}}_{2} (ε_{3}) \\ - k_{4} ε_{4} - \frac{1}{2} (- S (ε_{3}) + \sqrt{1 - ε_{3}^{T} ε_{3} I_{3}}) ε_{3} \end{matrix}

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, $ξ_{i d}, {\dot{ξ}}_{i d}, {\ddot{ξ}}_{i d}$ and $ξ_{i d}^{(3)}$ are bounded and visible to all formation members.

The desired position ξ_id and the desired velocity v_id are obtained by taking the weighted average of all the members in the formation(21) $ξ_{i d} = \frac{1}{N_{i}} \sum_{j = 1}^{N_{i}} ξ_{j}$ (22) $v_{i d} = \frac{1}{N_{i}} \sum_{j = 1}^{N_{i}} v_{j}$

It translates the formation problem into a trajectory tracking problem for a given reference signal. In the formula, N_i 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} = \frac{1}{N} \sum_{j = 1}^{N} ξ_{j}, v_{d} = \frac{1}{N} \sum_{j = 1}^{N} v_{j}$

The formation of the ith UAV can be guaranteed through the inversion control law. ξ_i → ξ_d + δ_i, v_i → v_d, ξ_i − ξ_j → δ_i − δ_j = δ_ij, v_i → v_j can be guaranteed.

EXPERIMENTAL SIMULATION

Parameter setting

Regardless of the differences among the quadrotor systems, the system quality is assumed m = 0.6 kg. The moment of inertia is J_x = J_y = 0.2kg·m², J_z = 0.04kg·m².

Setting the formation size N = 5, k₁ = 10, k₂ = 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 P₀ is: $\begin{array}{l} P_{0} = [ξ_{10} ξ_{20} ξ_{30} ξ_{40}] \\ = [\begin{array}{l} 0.8 & - 1.5 & - 0.4 & 1 \\ 1.5 & 0.9 & - 0.5 & - 1.2 \\ 0 & - 2 & - 1.5 & 0 \end{array}] \end{array}$

The pilot does uniform circular motion in a fixed height, the movement trajectory is: ${\begin{cases} x = \cos π t \\ y = \sin π t \\ z = 5 \end{cases}$

The relative positional deviation of the formation is described by: $δ = [δ_{1} δ_{2} δ_{3} δ_{4}] = [\begin{array}{l} 0 & - 0.5 & 0 & 0.5 \\ 0.5 & 0 & - 0.5 & 0 \\ 0 & 0 & 0 & 0 \end{array}]$

The simulation time is 9.5s and the simulation step is 0.001s

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.

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.

X-direction UAV position and speed curve

Y-direction UAV position and speed curve

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.

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.

eISSN:: 2470-8038
Język:: Angielski

Częstotliwość wydawania:: 4 razy w roku
Dziedziny czasopisma:: Computer Sciences, other

Kanał RSS czasopisma

Quadrotor Formation Inversion Control Method Based on Unit Quaternion

Data publikacji: 07 maj 2018

Zakres stron: 27 - 34

DOI: https://doi.org/10.21307/ijanmc-2018-006

© Wang Zhongsheng 2018 et al., published by Sciendo

This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License.

Figure 1.

Figure 2.

Figure 3.

Figure 4.

Figure 5.

Figure 6.