A Guidance and Control Law for Autonomous Formation of

Quadrotors

Xun Zhu and Shuguang Zhang

School of Transportation Science and Engineering, Beihang University, Beijing 100191, China

Keywords: Formation flying, Quadrotor, Relative kinematic, Formation algorithm.

Abstract: Formation flying means two or more flight vehicles maintain an organizational flight mode. In autonomous

formation, the guidance is normally divided into formation configuration and task assignment. This paper is

focused on the formation configuration of quadrotors from their initial random positions to an expected

configuration in response of the leader quadrotor. Based on the position and velocity of the leader, the

formation controller will generate the guidance and control instructions from the relative dynamic system.

The updated positions of the followers are compared with the expected positions and furtherly processed by

PI controllers to form speed instructions for the followers, and feedforward controllers are included to shape

commands to provide better tracking performance. Simulations with five quadrotors result show that the

designed guidance and control algorithm can help quickly achieve and keep the desired formation

configuration even to follow complex motions of the leader quadrotor.

1 INTRODUCTION

The multi rotor unmanned aerial vehicle (UAV), as

an important member of the UAV family, has been

evolving rapidly with the development of the control

theory and high technology (Giulietti F, 2005).

Compared with fixed wing unmanned aerial vehicle

and unmanned helicopter, the multi rotor aircraft has

the advantages in terms of vertical take-off and

landing, hovering, simple mechanical structures, easy

maintenance, flexible operability and so on, in spite

of the disadvantage of poor load capability due to

limits in blade size, speed, and flapping. Therefore,

the idea of multi UAV cooperative formation to

accomplish complex mission has been proposed to

employ its potential value while avoid the

disadvantage (Sun N P, 2014; Samaneh H S, 2015).

Multi UAV cooperative formation flying has the

following advantages over single UAV (Escareno J,

2013; Rudio J D 2014):

(1) Redundancy is increased, because any UAV in

the formation can take the place of the others;

(2) Formation flying is more adaptive to complex

tasks;

(3) Information perceived in formation is more

stereoscopic and more accurate

In autonomous formation, the guidance can be

divided into two phases of formation configuration

and task assignment, in which the formation

configuration can be further divided into three sub-

phases of configuration generating, configuration

keeping and configuration adjustment. Current

formation control methods include behaviour mode

based formation, ‘leader-follower’ structure based

formation, virtual structure based formation, artificial

potential field based formation, etc (

Salim N D, 2014;

Karimoddini A, 2013

), as compared in Table 1.

The behaviour mode based formation divides the

behaviour response of each member to its input

information into a number of fixed modes, and

completes formation control by assigning weight. In

the ‘leader-follower’ structure based formation, the

follower sense the information of the leader and form

and keep the configuration by changing its speed. The

virtual structure based formation let the members in

formation follow their respective trajectories by

defining a virtual leader. The artificial potential field

based formation keep the configuration and avoid

collision by imitating the repulsion and gravitation

between particles in a molecular structure.

204

Zhu, X. and Zhang, S.

A Guidance and Control Law for Autonomous Formation of Quadrotors.

In 3rd International Conference on Electromechanical Control Technology and Transportation (ICECTT 2018), pages 204-209

ISBN: 978-989-758-312-4

Table 1: Current mainstream formation algorithms

orithm Advanta

es Disadvanta

Behaviour

mode

Formation

flexibility;

High reliability;

Strong robustness

Poor formation

keeping

performance

Leader-

follower’

structure

Simple principle;

Easy to

implement

Poor

environmental

adaptability;

Poor reliability

Virtual

structure

Accurate

formation keeping

Hard to expand

Artificial

potential

fiel

High reliability

Slow response

In this paper, to achieve good formation flexibility

and keeping performance, while to be easy to

implement, a guidance and control law for

autonomous formation of quadrotors is based on

combination of PI control and feedforward control,

and the followers track the guidance instructions

generated by the formation controller in order to keep

the formation configuration.

2 MODEL DESCRIPTION

The quadrotors in this paper are small and driven by

batteries. Besides the rotors, all airframe parts are

taken as rigid. So, the control models include rigid

body kinematics model, rigid body dynamic model,

control allocation model and power system model as

shown in Figure 1.

2.1 Power system model

The input and output relation of the battery, throttle,

ESC and motor is described by the power system

model. Among them, the motor throttle

is input,

the motor speed

is output,

is the dynamic

response constant of motor. The power system model

is shown in Figure 2, and the mathematical model is

described as:

rigid body

kinematics

model

rigid body

dynamic model

control

allocation

model

powersystem

model

Position and attitude

Veloc ity and angu lar velocity

Force and moment

Propeller speed

PWM

Figure 1: Quadrotor control model

()

ϖσϖ

(1)

Figure 2: Signal transmission diagram of power system

2.2 Control allocation model

The input and output relation of the motor speed and

force and moment is described by the control

allocation model. There are two ways to control

distribution of the ‘

’ configuration and the ‘

’

configuration, as shown in Figure 3.

Figure 3: The ’

’ configuration and ‘

’ configuration

A Guidance and Control Law for Autonomous Formation of Quadrotors

205

For the ‘

’ configuration quadrotor, the

mathematical model is as follows:

TT T T

xTT

yT T

zMMMM

fcccc

dc dc

ccc c

⎡⎤

⎡⎤⎡ ⎤

⎢⎥

⎢⎥⎢ ⎥

−

⎢⎥

⎢⎥⎢ ⎥

⎢⎥

⎢⎥⎢ ⎥

−

⎢⎥

⎢⎥⎢ ⎥

−−

⎢⎥

⎣⎦⎣ ⎦

⎣⎦

(2)

For the’

’ configuration quadrotor, the

mathematical model is as follows:

2222

22 2 2

TTTT

TT T T

MMM M

cccc

dc dc dc dc

ccc c

⎡⎤

⎢⎥

⎡⎤

⎢⎥

−−

⎢⎥

−−⎢⎥

⎢⎥

⎣⎦

⎢⎥

−−

⎣⎦

(3)

Where,

is the total tension acting on the body,

while

, and

are the moments produced by

propellers along the pitching, roll and yaw axes

respectively.

2.3 Control rigid body model

The control rigid body model contains dynamic and

kinematic parts to determine the Euler angles

θφψ

as in the following mathematical model:

[]

()

bbb T

drag

bb b

vvgReeKv

JJG

ωω ω τ

Θ=

=− × + − −

=− × + +

(4)

Where,

represent position, attitude

angle and angular velocity respectively, the left tag e

is expressed in the inertial system and the left tag b is

expressed in the body coordinate system. The

the coordinate transformation matrix of the body

coordinate system to the inertial system. The

and

are the mass and inertia of the quadrotor.

is the

gyroscopic moment and it is overlooked here. All

these quantities are vector forms.

3 FORMATION ALGORITHM

3.1 Formation objective

The formation configuration is defined in the leader’s

speed coordinate system as

and

, it can also

be converted to distance and azimuth angle. The

relative kinematic model is shown in Figure 4.

Figure 4: Geometric relation diagram of formation flying

The mathematical model is described as the

following model:

cos( )

sin( )

LLWWW

LLWW

xV yV

yV x

ψψ ψ

=−+−

=−−

(5)

In the inertial coordinate system, the expected

position of followers can be computed through the

measured leader’s position and relative kinematic

relational expression as:

sin cos

cos sin

WEx L L L

Wx Ex Lx

Wy Ex Ly

YYx y

XXx y

ψψ

=− −

(6)

The state error of the follower can be calculated

by its expected state and current state, as shown in

Figure 5.

ICECTT 2018 - 3rd International Conference on Electromechanical Control Technology and Transportation

206

Figure 5: Expected and current states of the follower

arctan

()()

cos

sin

WEx w

WEx w WEx w

xWxExWxLxWx

yWyExWyLyWy

DX X YY

xxx D

yyy D

vV V V V

βαψ

−

=−

=−+−

Δ= − =

Δ= − = −

(7)

The control objective is for the follower to fly

from current state to the expected state with minimal

errors:

( , , , ) (0, 0, 0, 0)

xyv vΔΔΔ Δ ≡

(8)

3.2 Formation algorithm

To achieve the above control objective, the formation

controller is designed according to the forward and

lateral channels.

(1) The forward channel. The control amount is

, and the differential effects of follower’s

expected position is added to the speed control as a

feedforward control:

WEx

Wxc xp xl

Vkxkxdt

=Δ+ Δ+

∫

(9)

(2) The lateral channel. Similar to the forward

channel, the lateral control is as:

WEx

Wyc yp yl

Vkykydt

=Δ+ Δ+

∫

(10)

4 SIMULATIONS

The control structure of the bottom adopts double

loop PID, the formation controller of the top adopts

the combination of PI and position instruction

differential feedforward. The block diagram of the

formation control structure is shown in Figure 6.

The parameters of the quadrotor used in the

simulation are shown in Table 2.

Table 2: Quadrotor parameters

Parameter Value Parameter Value

/mkg

1.15

1.57*10

−

/lm

0.177

7.5*10

−

kg m⋅

0.0109

kg m⋅

0.021

kg m⋅

0.0109

drag

0.0013

Figure 6: Block diagram of formation control

A Guidance and Control Law for Autonomous Formation of Quadrotors

207

4.1 Simulations of single quadrotor

control

To verify the single quadrotor control law, the initial

position of quadrotor is set to

(0,0,3)

m and the

initial state of quadrotor is hover. The quadrotor starts

from the initial position and tracks the given

trajectory instructions. The 3D graphs of trajectory

tracking are shown in Figure 7.

Figure 7: 3D graph of trajectory tracking

It can be seen from Figure 7, the actual tracking

trajectory is approximately coincident with the

desired trajectory, the height changes when the

quadrotor turns, but the change is so small that can be

ignored.

4.2 Formation flight simulation of five

quadrotors

To verify the performance of multi aircraft formation,

five quadrotors are taken as examples. Table 3

presents the initial and desired formation position of

five quadrotor, followers read leader for reference to

guide. The detailed formation configuration is shown

Figure

Table 3: Initial and desired position for five quadrotor

formation

Quadrotor

000

,,/

yz m

,, /

yzm

ΔΔΔ

Leader

0, 0, 3

Follower 1

2, 3, 3−

1, 1, 0−−

Follower 2

1, 2 , 3−−

2, 2,

−−

Follower 3

2, 1, 3−−

1, 1, 0−

Follower 4

1, 4 , 3−−

2, 2, 0−

Figure 8: Five quadrotor formation configuration

In order to test the formation effect of various

motion situation in the leader quadrotor, let the leader

quadrotor carry out all kinds of movement including

uniform motion, uniform transmission motion in the

forward and lateral directions, curvilinear motion and

so on. Speed command and actual speed response of

the leader quadrotor in the forward and lateral

directions is shown in Figure 9.

Figure 9: Speed command and actual speed response of the

leader quadrotor in two direction

Figure 10 gives the result of the simulated five

quadrotor formation.

Figure 10: Five quadrotor formation configuration

As can be seen from Figure 10, in the forward and

lateral directions, four following quadrotors

accurately reach the desired positions in the formation.

-10

-5

-10

2.5

3.5

X/ m

Y/m

Z/m

Expected trajectory

Actual trajectory

0 5 10 15 20 25

0.1

0.2

0.3

0.4

0.5

0.6

t/s

Vx/(m/s)

Expected speed

Actual speed

15 20 25 30 35 40

0.2

0.4

0.6

0.8

1.2

t/s

Vy/(m/s)

Expected speed

Actual speed

0 5 10 15

-4

-2

X/ m

Y/m

Leader

Follower1

Follower2

Follower3

Follower4

ICECTT 2018 - 3rd International Conference on Electromechanical Control Technology and Transportation

208

The altitude and yaw angle channel is decoupled, and

the change is so small that it is negligible.

The variation of speed in the forward and lateral

directions of four follower quadrotor and the leader

quadrotor in the formation is shown in Figure 11.

Figure 11: The speed in the forward and lateral

directions of five quadrotors

As can be seen from Figure 10 and Figure 11,

leader and followers form a stable formation at about

7 seconds. After that, formation configuration can be

maintained nice in the case of a variety of movements

in the leader quadrotor. Among them, under the

condition of unidirectional linear uniform motion and

uniformly variable motion of the leader, the speed in

the forward and lateral directions of any follower will

converge to the speed value of the leader. Under the

condition of curvilinear motion of the leader, the

speed of the follower in the outer circle will become

larger in order to keep the formation configuration,

the speed of the follower in the inner circle will

become smaller and even move in reverse in order to

keep the formation configuration. In this process,

although the speed of any follower will not converge

to the speed value of the leader, the formation

configuration has been kept very well. If the leader

quadrotor is allowed to continue to do circular motion,

the speed in the forward and lateral directions of any

follower will converge to a corresponding value.

5 CONCLUSIONS

This paper uses the combination of classical PI

control and feedforward control to design the

guidance law of autonomous formation of multiple

quadrotors. Simulation results of single quadrotor

trajectory tracking and formation of five quadrotors

are given. The accuracy and response speed of

trajectory tracking are verified. The formation control

law as a top layer controller commands the position

and controller of single quadrotor. Simulation results

show that the guidance instruction generated by the

presented formation control law can guide the

followers to form expected formation configuration

and keep the formation quite well.

REFERENCES

Giulietti F, Innocenti M, Napolitano, et al. Dynamic and

control issues of formation flight[J]. Aerospace Science

and Technology, 2005, 36(9): 65-71.

Sun N P. An alternative flocking algorithm with additional

dynamic conditions[C]//Ninth International Conference

on Broadbrand and Wireless Computing. Guangdong:

IEEE, 2014: 491-496.

Samaneh H S. Semi-flocking algorithm for motion control

of mobile sensors in large-scale surveillance

systems[J]IEEE Transactions on Cybernetics, 2015,

45(1):129-135.

Escareno J, Salazar S, Romero H. Trajectory control

quadrotor subject to 2D wind disturbances: robust-adap-

tive approach[J]. Journal of Intelligent and Robotic

Systems: Theory and Applications, 2013, 70(1-4):51-63.

Rudio J D, Cruz J H P, Zamudio Z, et al. Comparison of

two quadrotor dynamic models[J]. IEEE Latin America

Transactions, 2014, 12(4): 531-537.

Salim N D, Derawi D, Abdullah S S, et al. PID plus LQR

attitude control for hexarotor MAV[C].IEEE

International Conference on Industrial Technology

(ICIT). Busan, Korea, 2014:85-90.

Karimoddini A, Lin H, Chen B M. Hybrid three-

dimensional formation control for unmanned

helicopters[J]. Automatica, 2013, 49(2):424-433.

0 10 20 30 40

-1

-0.5

0.5

1.5

t/s

Vx/(m/s)

Leader

Follower1

Follower2

Follower3

Follower4

0 10 20 30 40

-1.5

-1

-0.5

0.5

1.5

t/s

Vy/(m/s)

A Guidance and Control Law for Autonomous Formation of Quadrotors

209