# Mathematical Modeling of a Multilayered Drift-Stabilization Method for Micro-UAVs Using Inertial Navigation Unit Sensor

Authors: Hyeok-June Jeong; Myunggwon Hwang; Hanmin Jung; Young-guk Ha
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

This paper proposes a multilayered quadrotor control method that can move the quadrotor to the desired goal while resisting disturbance. The proposed control system is modular, convenient to design and verify, and easy to extend. It comprises three layers: a physical layer, a displacement control layer, and an attitude control layer. The displacement control layer considers the movement of the vehicle, while the attitude control layer controls its attitude. The physical layer deals with the physical operation of the vehicle. The two control layers use a mathematical method to provide minute step-by-step control. The proposed control system effectively combines the three layers to achieve drift stabilization.


---

## Body

## 1. Introduction

Unmanned aerial vehicles (UAVs) are expected to become a major part of the aviation industry as they can perform tasks such as traffic control, video recording, reconnaissance, and surveillance. Concomitant with developments in computer science, automatic control, sensors, and communications technologies, the quadrotor, in particular, is being evaluated as a suitable platform for small UAVs. Among its many advantages is the fact that it can move both vertically and horizontally, it can be made very small, and it can carry a variety of electrical devices.A quadrotor flies by means of four propellers, which are controlled by an automatic system programmed in a microprocessor. This means that the control system has minute control over its flight. Consequently, it is relatively easier to fly a quadrotor than other aircrafts such as helicopters and airplanes.However, controlling the desired behavior of the quadrotor is never easy. Because it flies in air, the quadrotor has to overcome inertia and undesirable wind disturbance.Consequently, in order for the quadrotor to be useful, it has to have an effective control system.Efforts have previously been made to solve this problem; however, this goal has not yet been definitively achieved. The control systems proposed thus far are more complicated than effective. Some researchers have proposed vision-based control systems; however, such systems are inefficient and heavy and therefore not suitable for small UAVs.In this paper, we propose a quadrotor control system that uses the four motors and nine EOF-axis inertial navigation sensors, which measure the quadrotor’s attitude and movement, to achieve minute control.The control system operates on the basis of positioning; that is, it can move the quadrotor to any desired point while resisting external forces such as wind. Of course, drift stabilization is possible in any current position. In particular, the proposed control system is implemented in a modular form, so its design efficiency and performance can be easily verified.The proposed controller significantly increases the quadrotor’s stability and hovering performance and thereby facilitates the use of the quadrotor for various applications.


## 2. Related Work

Over the past decade, much research has been devoted to quadrotor control systems, a large portion of which has focused on the use of vision. Grzonka et al. [1] pioneered work in this area. Their research focused on indoor quadrotor flight control systems that are able to pilot the quadrotor in indoor spaces via real-time image processing [2]. However, even though their research is excellent, the stability of their control system is in doubt. Bills et al. [3] also proposed a control system that uses vision; however, their proposed system cannot guarantee stable quadrotor movement.Romero et al. [4] proposed a control system that uses optical signals. The proposed system is stable because it only traces certain points. Gu et al. [5] focused their research on systems flying in formation, with each UAV designed hierarchically according to its role, such as flight leader or wingman. These systems have the advantage of stability but are limited to operating only in particular situations.Zhang et al. [6] proposed a control system that combines vision with an IMU sensor. The system is stable; however, the efficiency of the control system is poor—it performs well for small vehicles but is too large for microquadrotors.Consequently, in spite of research efforts expended to date, a suitable implantable control system for micro-UAVs has not been realized. We believe our proposed multilayered drift-stabilization method can provide an effective solution that resolves these problems.


## 3. Design of the Proposed Control System

In this section, we discuss the design of the proposed control system in terms of its layers and the algorithms used by each layer.


### 3.1. The Combined Layers

Our proposed multilayered control system has advantages in terms of its design, verifiability, and extendibility. These advantages result from the fact that each control layer is in charge of an assigned function. That is, each layer is considered to be abstracted.The architecture of our proposed multilayered drift-stabilization control system is depicted in Figure1. It comprises three layers. The first layer consists of physical components such as motor, frame, rotor, and battery. The rotation of the motor is controlled in accordance with a control value. The sensor calculates such parameters as the angular velocity and acceleration and passes the feedback values to the other layers.Figure 1
Architecture of the multilayered drift-stabilization control system.The second layer, the “attitude control layer,” is in charge of the attitude angle, “roll, pitch, and yaw.” The attitude control layer receives attitude feedback and an objective angle. However, the layer does not care about how the actual angle is calculated. This layer is simply responsible for ensuring that the quadrotor is at the correct angle received.Finally, the “displacement control layer” is in charge of the movement of the vehicle. This layer receives displacement feedback and control signals (reference) that it uses to calculate an appropriate angle to pass to the attitude control layer. The displacement control layer does not care how the angle of the vehicle is controlled. This is why we say that the control system design is abstracted.Consequently, the proposed multilayered control system can be a powerful and convenient system, in which each section only fulfills its specified roles.


### 3.2. The Physical Layer

A quadrotor has four rotors and four motors on a rigid framework. A sensor is mounted at the center of the frame for accurate operations. Other components, such as circuit and batteries, are mounted in the same position as the sensor to ensure a balanced center of gravity.Our proposed control system is designed for the shape shown in Figure2 and takes into consideration the dynamics of this shape.Figure 2
Physical appearance/shape of a quadrotor.Despite the shape of the camera, the quadrotor is assumed to be symmetrical in quality and structure. The physical characteristics of the quadrotor are listed in Table1.Table 1
Quadrotor physical parameters.
Parameters
Value
Inertia aroundX-axis
I
x
7.5
×
10
-
3 kg·m2
Inertia aroundY-axis
I
y
7.5
×
10
-
3 kg·m2
Inertia aroundZ-axis
I
z
1.5
×
10
-
3 kg·m2
Distance to the center of the quadrotor
L
0.23 m
Mass of the quadrotor
m
0.65 kg
Gravitational acceleration
g
0.98 m·s−2In order to establish the dynamic model of the quadrotor, we can make the following general assumptions.(i)
Gravity and resistance of the quadrotor are not affected by flight altitude and other factors.
(ii)
Thrust in all directions is proportional to the square of the rotor speed.
(iii)
The quadrotor is a symmetrical rigid body.
(iv)
The origin of the inertial coordinate system is in the same position as the geometric center and the centroid of the quadrotor.Two main effects are taken into consideration: generation of the thrust and the drag force. The thrust,T, produced by each motor is a force calculated as
(1)
F
i
=
ρ
C
t
A
ω
i
2
R
2
=
k
t
ω
i
2
,
where C
t is the thrust coefficient, ρ is the air density, A is the rotor disk area, and R is the blade radius. Further, the drag force is defined as
(2)
D
i
=
1
2
ρ
C
d
v
2
=
k
d
v
2
,
where D is the drag force, C
d is the drag force coefficient, and v is the speed of the quadrotor.In the fixed coordinates of the body, the direct inputs are revolutions per minute (RPM) commands for the motors. The resultant outputs areZ direction thrusts in these coordinates.However, the outputs under consideration are the attitude and position. To eliminate this gap, four control variables are defined as follows:(3)
[
U
1
U
2
U
3
U
4
]
=
[
F
1
+
F
2
+
F
3
+
F
4
l
(
F
4
-
F
2
)
l
(
F
3
-
F
1
)
F
2
+
F
4
-
F
3
-
F
1
]
=
[
k
i
∑
i
=
1
4
ω
i
2
k
t
(
ω
4
2
-
ω
2
2
)
k
t
(
ω
3
2
-
ω
1
2
)
k
d
(
ω
1
2
-
ω
2
2
+
ω
3
2
-
ω
4
2
)
]
,
where ω
1, ω
2, ω
3, and ω
4 are the respective rotational speed of each rotor, F
1, F
2, F
3, and F
4 are the lift forces of the motor’s axis, l is the length of the quadrotor, U
1 is the total lift force, U
2 is the rolling moment, U
3 is the pitching moment, and U
4 is the pitching moment.


### 3.3. The Attitude Control Layers

Let us now derive a mathematical model for the quadrotor shown in Figure2 (see Figure 4). But this layer is not our main interest. For that reason, this layer is designed using existing excellent research. Descriptions, expressions, sentences, and equations also are quoted in those papers (especially reference [7]).The origin of the inertial coordinate system E is the initial position of the quadrotor. The positive direction of theO
X axis is the designated heading of the quadrotor and is perpendicular to the horizontal plane.This coordinate system is used to study the relative movement of ground and quadrotor. TheO
Y axis is perpendicular to the O
X
Z plane. The quadrotor’s spatial coordinates (X, Y, Z) can be obtained through the inertial coordinate system.The origin of quadrotor coordinate system B (o
x
y
z) is the center of the quadrotor, and o
x is parallel to the center connection of the front and rear rotors and the positive direction points to the front. o
z is parallel to the center connection of the left and right rotors and the positive direction points to the right. The o
y axis is perpendicular to the o
x
z plane; the positive direction is the direction conforming to the right hand rule.These two coordinates can be converted to each other through transition matrixR.Euler angles are defined as follows.Pitch angleθ: angle between the Z-axis and the projection of O
z in the O
X
Y plane.Yaw angleφ: angle between the X-axis and the projection of O
x in the O
X
Y plane.Roll angleψ: angle between the Y-axis and the projection of O
y in the O
X
Y plane.Consequently, we can obtain the transition matrixR, which is from the quadrotor coordinate system to the inertial frame. Consider
(4)
R
x
=
[
1
0
0
0
cos
⁡
ϕ
-
sin
ϕ
0
sin
ϕ
cos
⁡
ϕ
]
,
R
y
=
[
cos
⁡
θ
0
sin
θ
0
1
0
-
sin
θ
0
cos
⁡
θ
]
,
R
z
=
[
cos
⁡
ψ
-
sin
ψ
0
sin
ψ
cos
⁡
ψ
0
0
0
1
]
.With attitude angles defined as in Figure2, the transformation matrix from the inertial coordinates to the fixed body coordinates is
(5)
R
(
ϕ
,
θ
,
ψ
)
=
R
x
·
R
y
·
R
z
=
[
cos
⁡
ψ
cos
⁡
ϕ
cos
⁡
ψ
sin
θ
sin
ϕ
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
sin
ψ
cos
⁡
θ
sin
ψ
sin
θ
sin
ϕ
sin
ψ
sin
θ
cos
⁡
ϕ
-
sin
ϕ
cos
⁡
ψ
-
sin
θ
cos
⁡
θ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
]
.Then, we can obtain(6)
F
x
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
,
F
y
=
k
t
∑
i
=
1
4
ω
t
2
(
sin
ψ
sin
ϕ
cos
⁡
ϕ
+
cos
⁡
ψ
sin
ϕ
)
,
F
z
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ϕ
cos
⁡
ϕ
)
.By Newton’s second law of motion(7)
F
→
=
m
a
=
m
d
V
→
d
t
.By Newton’s second law, the dynamic equation of the quadrotor, the line motion equation can be obtained. It is defined as follows:(8)
x
¨
=
(
F
x
-
K
1
x
˙
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
-
K
1
x
˙
m
,
y
¨
=
(
F
y
-
K
2
y
˙
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
sin
ψ
sin
θ
cos
⁡
ϕ
+
cos
⁡
ψ
sin
ϕ
)
-
K
2
y
˙
m
,
z
¨
=
(
F
z
-
K
3
z
˙
-
m
g
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ϕ
cos
⁡
ϕ
)
-
K
3
y
˙
m
-
g
,
where, K
1
x
˙, K
2
y
˙, and K
3
z
˙ is the air resistance.According to the relationship between Euler angle and angular velocity of the quadrotor, the following result can be obtained:(9)
[
p
q
r
]
=
[
ϕ
˙
-
ψ
˙
sin
θ
θ
˙
cos
⁡
ϕ
+
ψ
˙
sin
ϕ
cos
⁡
θ
-
θ
˙
sin
ϕ
+
ψ
cos
⁡
ϕ
cos
⁡
θ
]
.Expressed with regard toψ, we obtain
(10)
-
θ
˙
=
r
-
ψ
cos
⁡
ϕ
cos
⁡
θ
sin
ϕ
,
θ
˙
=
q
-
ψ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
,
ψ
cos
⁡
ϕ
cos
⁡
θ
-
r
sin
ϕ
=
q
-
ψ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
.Then, this equation can be expressed as follows:(11)
ψ
(
cos
⁡
2
⁡
ϕ
cos
⁡
θ
+
sin
2
⁡
ϕ
cos
⁡
θ
)
=
r
cos
⁡
ϕ
+
q
sin
ϕ
,
ψ
=
r
cos
⁡
ϕ
+
q
sin
ϕ
cos
⁡
θ
.Finally, the following expression is obtained:(12)
[
ϕ
˙
θ
˙
ψ
˙
]
=
[
p
cos
⁡
θ
+
q
sin
ϕ
sin
θ
+
r
cos
⁡
ϕ
sin
θ
cos
⁡
θ
q
cos
⁡
ϕ
+
r
sin
ϕ
r
cos
⁡
ϕ
+
q
sin
ϕ
cos
⁡
θ
]
.The quadrotor was previously assumed to be symmetrical in quality and structure, so the inertia matrix. So it can be defined as a diagonal matrix:(13)
I
[
I
x
I
y
I
z
]
,
where I
x
x, I
y
y, and I
z
z are the rotary inertia around the X, Y, and Z axes, respectively.By calculating the angular momentum, we can obtain the three axial components’ angular motion equations ofM in the quadrotor coordinate system: M
x, M
y, and M
z. Consider
(14)
M
x
=
p
˙
I
x
+
q
r
(
I
z
-
I
y
)
,
p
˙
=
M
x
+
q
r
(
I
y
-
I
z
)
I
x
,
(15)
M
y
=
q
˙
I
y
+
p
r
(
I
x
-
I
z
)
+
(
p
2
-
r
2
)
I
x
z
,
M
y
=
q
˙
I
y
+
p
r
(
I
x
-
I
z
)
,
q
˙
=
M
y
+
p
r
(
I
z
-
I
x
)
I
y
,
(16)
M
z
=
r
˙
I
z
-
p
˙
I
x
z
+
p
q
(
I
y
-
I
x
)
+
q
r
I
x
z
,
M
z
=
r
˙
I
z
+
p
q
(
I
y
-
I
x
)
,
r
˙
=
M
z
+
p
q
(
I
x
-
I
y
)
I
z
.After simplification, the formula becomes(17)
[
p
˙
q
˙
r
˙
]
=
[
M
x
+
q
r
(
I
y
-
I
z
)
I
x
M
y
+
p
r
(
I
z
-
I
x
)
I
y
M
z
+
p
q
(
I
x
-
I
y
)
I
z
]
.Equations (3)–(14) are mathematically considered state equations and can be implemented at the attitude control layer.


### 3.4. The Displacement Control Layer

The displacement control layer controls the movement of the quadrotor to the desired location while resisting disturbances. At this layer, the control system adjusts the attitude of the quadrotor to make it move. That is, this layer receives the input value for position, which it then uses to calculate the proper angle for the quadrotor​​.The algorithm is derived from situations such as those shown in Figure5. The U
1 is known, and the force of x-axis can be obtained by flowing equations. Consider
(18)
F
x
=
U
1
sin
θ
.The quadrotor is tilted to thez-axis. This angle is defined as theta. The force is generated in the x-axis by the rotation of the rotor:
(19)
F
x
=
m
a
x
.Equation (19) is Newton’s second law on the x-axis. Consider
(20)
sin
θ
=
a
x
m
U
1
,
(21)
θ
=
sin
-
1
⁡
(
a
x
m
U
1
)
,
(22)
-
1
≤
a
x
m
U
1
≤
1
.Equation (21) can be obtained by substituting in (18) and (19). Then, an angle can be obtained for the desired acceleration. The respective domain and range of the sine function must be satisfied (22). The physical meaning of (21) is as follows.(i)
In order to obtain a large acceleration, a large angle is necessary.
(ii)
A large mass requires a large angle.
(iii)
If the motor output is large, the desired angle is reduced (Figure3).Figure 3
Definition of axis and rotor output.Figure 4
Model of the structure of the quadrotor.Figure 5
Directional diagram of the control system.Given the angle of the quadrotor, the respective acceleration can be derived easily; however, this system needs the appropriate angle of the quadrotor’s respective displacement. Therefore, we propose another equation derived from (18).If the system is designed to simply resist the acceleration, it cannot achieve its desired target because of the value of the instantaneous acceleration. Our proposed control system needs to know the tendency of the movement due to acceleration. For this reason, the acceleration in (18) is replaced by a different function:
(23)
a
x
⟹
u
x
(
t
)
.The conversion of expression (23) does not use the mathematical meaning of equal. The transform function is inferred from (18):
(24)
e
x
(
t
)
=
d
ref
(
t
)
-
d
(
t
)
,
where d
ref is a desired location in the absolute coordinate system and d is the current position in the absolute coordinate system. The function (24) means the distance from the target point, in this sense called “error” in control engineering:
(25)
d
(
t
)
=
d
(
t
-
1
)
+
∬
a
x
di
(
t
)
d
t
+
∬
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
U
1
m
d
t
,
where d
(
t
-
1
) is the previous position in the absolute coordinate system and a
x
di is the acceleration of the disturbance of the x-axis. This means that the current position is the sum of the previous position, the current attitude of the aircraft, and the acceleration of the disturbance.That is, the transform function refers to the error value in the PID feedback control shown in Figure6, and the feedback control system can minimize the error by adjusting the process control outputs. Consider
(26)
u
(
t
)
=
K
p
e
(
t
)
+
1
T
i
∫
0
t
e
(
τ
)
d
τ
+
T
d
d
e
(
t
)
d
t
,
where τ is a variable of integration and takes values from time zero to the present time, t. K
p, T
i, and T
d signify the gain, a tuning parameter. Consider(27)
θ
=
sin
-
1
⁡
(
u
x
(
t
)
m
U
1
)
,
(28)
-
sin
θ
min
⁡
≤
u
x
(
t
)
m
U
1
≤
sin
θ
max
⁡
.Figure 6
Block diagram of the PID feedback.The appropriate angle can be calculated using (28). The physical meaning of this expression is as follows (Figure 7, left block).(i)
A large mass requires a large angle.
(ii)
If the motor output is large, the desired angle is reduced.
(iii)
The larger the control signal, the more the angle tilts.Figure 7
Implementation of the control system in MATLAB Simulink (layer 3 is excluded).The displacement control layer enables the quadrotor to move to the desired point while resisting disturbance. Invariably, it also makes it possible for the quadrotor to hover. However, this control layer calculates only the angle for the respective distance error. In other words, this layer is designed to faithfully conduct an indigenous mission.


## 3.1. The Combined Layers

Our proposed multilayered control system has advantages in terms of its design, verifiability, and extendibility. These advantages result from the fact that each control layer is in charge of an assigned function. That is, each layer is considered to be abstracted.The architecture of our proposed multilayered drift-stabilization control system is depicted in Figure1. It comprises three layers. The first layer consists of physical components such as motor, frame, rotor, and battery. The rotation of the motor is controlled in accordance with a control value. The sensor calculates such parameters as the angular velocity and acceleration and passes the feedback values to the other layers.Figure 1
Architecture of the multilayered drift-stabilization control system.The second layer, the “attitude control layer,” is in charge of the attitude angle, “roll, pitch, and yaw.” The attitude control layer receives attitude feedback and an objective angle. However, the layer does not care about how the actual angle is calculated. This layer is simply responsible for ensuring that the quadrotor is at the correct angle received.Finally, the “displacement control layer” is in charge of the movement of the vehicle. This layer receives displacement feedback and control signals (reference) that it uses to calculate an appropriate angle to pass to the attitude control layer. The displacement control layer does not care how the angle of the vehicle is controlled. This is why we say that the control system design is abstracted.Consequently, the proposed multilayered control system can be a powerful and convenient system, in which each section only fulfills its specified roles.


## 3.2. The Physical Layer

A quadrotor has four rotors and four motors on a rigid framework. A sensor is mounted at the center of the frame for accurate operations. Other components, such as circuit and batteries, are mounted in the same position as the sensor to ensure a balanced center of gravity.Our proposed control system is designed for the shape shown in Figure2 and takes into consideration the dynamics of this shape.Figure 2
Physical appearance/shape of a quadrotor.Despite the shape of the camera, the quadrotor is assumed to be symmetrical in quality and structure. The physical characteristics of the quadrotor are listed in Table1.Table 1
Quadrotor physical parameters.
Parameters
Value
Inertia aroundX-axis
I
x
7.5
×
10
-
3 kg·m2
Inertia aroundY-axis
I
y
7.5
×
10
-
3 kg·m2
Inertia aroundZ-axis
I
z
1.5
×
10
-
3 kg·m2
Distance to the center of the quadrotor
L
0.23 m
Mass of the quadrotor
m
0.65 kg
Gravitational acceleration
g
0.98 m·s−2In order to establish the dynamic model of the quadrotor, we can make the following general assumptions.(i)
Gravity and resistance of the quadrotor are not affected by flight altitude and other factors.
(ii)
Thrust in all directions is proportional to the square of the rotor speed.
(iii)
The quadrotor is a symmetrical rigid body.
(iv)
The origin of the inertial coordinate system is in the same position as the geometric center and the centroid of the quadrotor.Two main effects are taken into consideration: generation of the thrust and the drag force. The thrust,T, produced by each motor is a force calculated as
(1)
F
i
=
ρ
C
t
A
ω
i
2
R
2
=
k
t
ω
i
2
,
where C
t is the thrust coefficient, ρ is the air density, A is the rotor disk area, and R is the blade radius. Further, the drag force is defined as
(2)
D
i
=
1
2
ρ
C
d
v
2
=
k
d
v
2
,
where D is the drag force, C
d is the drag force coefficient, and v is the speed of the quadrotor.In the fixed coordinates of the body, the direct inputs are revolutions per minute (RPM) commands for the motors. The resultant outputs areZ direction thrusts in these coordinates.However, the outputs under consideration are the attitude and position. To eliminate this gap, four control variables are defined as follows:(3)
[
U
1
U
2
U
3
U
4
]
=
[
F
1
+
F
2
+
F
3
+
F
4
l
(
F
4
-
F
2
)
l
(
F
3
-
F
1
)
F
2
+
F
4
-
F
3
-
F
1
]
=
[
k
i
∑
i
=
1
4
ω
i
2
k
t
(
ω
4
2
-
ω
2
2
)
k
t
(
ω
3
2
-
ω
1
2
)
k
d
(
ω
1
2
-
ω
2
2
+
ω
3
2
-
ω
4
2
)
]
,
where ω
1, ω
2, ω
3, and ω
4 are the respective rotational speed of each rotor, F
1, F
2, F
3, and F
4 are the lift forces of the motor’s axis, l is the length of the quadrotor, U
1 is the total lift force, U
2 is the rolling moment, U
3 is the pitching moment, and U
4 is the pitching moment.


## 3.3. The Attitude Control Layers

Let us now derive a mathematical model for the quadrotor shown in Figure2 (see Figure 4). But this layer is not our main interest. For that reason, this layer is designed using existing excellent research. Descriptions, expressions, sentences, and equations also are quoted in those papers (especially reference [7]).The origin of the inertial coordinate system E is the initial position of the quadrotor. The positive direction of theO
X axis is the designated heading of the quadrotor and is perpendicular to the horizontal plane.This coordinate system is used to study the relative movement of ground and quadrotor. TheO
Y axis is perpendicular to the O
X
Z plane. The quadrotor’s spatial coordinates (X, Y, Z) can be obtained through the inertial coordinate system.The origin of quadrotor coordinate system B (o
x
y
z) is the center of the quadrotor, and o
x is parallel to the center connection of the front and rear rotors and the positive direction points to the front. o
z is parallel to the center connection of the left and right rotors and the positive direction points to the right. The o
y axis is perpendicular to the o
x
z plane; the positive direction is the direction conforming to the right hand rule.These two coordinates can be converted to each other through transition matrixR.Euler angles are defined as follows.Pitch angleθ: angle between the Z-axis and the projection of O
z in the O
X
Y plane.Yaw angleφ: angle between the X-axis and the projection of O
x in the O
X
Y plane.Roll angleψ: angle between the Y-axis and the projection of O
y in the O
X
Y plane.Consequently, we can obtain the transition matrixR, which is from the quadrotor coordinate system to the inertial frame. Consider
(4)
R
x
=
[
1
0
0
0
cos
⁡
ϕ
-
sin
ϕ
0
sin
ϕ
cos
⁡
ϕ
]
,
R
y
=
[
cos
⁡
θ
0
sin
θ
0
1
0
-
sin
θ
0
cos
⁡
θ
]
,
R
z
=
[
cos
⁡
ψ
-
sin
ψ
0
sin
ψ
cos
⁡
ψ
0
0
0
1
]
.With attitude angles defined as in Figure2, the transformation matrix from the inertial coordinates to the fixed body coordinates is
(5)
R
(
ϕ
,
θ
,
ψ
)
=
R
x
·
R
y
·
R
z
=
[
cos
⁡
ψ
cos
⁡
ϕ
cos
⁡
ψ
sin
θ
sin
ϕ
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
sin
ψ
cos
⁡
θ
sin
ψ
sin
θ
sin
ϕ
sin
ψ
sin
θ
cos
⁡
ϕ
-
sin
ϕ
cos
⁡
ψ
-
sin
θ
cos
⁡
θ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
]
.Then, we can obtain(6)
F
x
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
,
F
y
=
k
t
∑
i
=
1
4
ω
t
2
(
sin
ψ
sin
ϕ
cos
⁡
ϕ
+
cos
⁡
ψ
sin
ϕ
)
,
F
z
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ϕ
cos
⁡
ϕ
)
.By Newton’s second law of motion(7)
F
→
=
m
a
=
m
d
V
→
d
t
.By Newton’s second law, the dynamic equation of the quadrotor, the line motion equation can be obtained. It is defined as follows:(8)
x
¨
=
(
F
x
-
K
1
x
˙
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
-
K
1
x
˙
m
,
y
¨
=
(
F
y
-
K
2
y
˙
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
sin
ψ
sin
θ
cos
⁡
ϕ
+
cos
⁡
ψ
sin
ϕ
)
-
K
2
y
˙
m
,
z
¨
=
(
F
z
-
K
3
z
˙
-
m
g
)
m
=
k
t
∑
i
=
1
4
ω
t
2
(
cos
⁡
ϕ
cos
⁡
ϕ
)
-
K
3
y
˙
m
-
g
,
where, K
1
x
˙, K
2
y
˙, and K
3
z
˙ is the air resistance.According to the relationship between Euler angle and angular velocity of the quadrotor, the following result can be obtained:(9)
[
p
q
r
]
=
[
ϕ
˙
-
ψ
˙
sin
θ
θ
˙
cos
⁡
ϕ
+
ψ
˙
sin
ϕ
cos
⁡
θ
-
θ
˙
sin
ϕ
+
ψ
cos
⁡
ϕ
cos
⁡
θ
]
.Expressed with regard toψ, we obtain
(10)
-
θ
˙
=
r
-
ψ
cos
⁡
ϕ
cos
⁡
θ
sin
ϕ
,
θ
˙
=
q
-
ψ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
,
ψ
cos
⁡
ϕ
cos
⁡
θ
-
r
sin
ϕ
=
q
-
ψ
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
.Then, this equation can be expressed as follows:(11)
ψ
(
cos
⁡
2
⁡
ϕ
cos
⁡
θ
+
sin
2
⁡
ϕ
cos
⁡
θ
)
=
r
cos
⁡
ϕ
+
q
sin
ϕ
,
ψ
=
r
cos
⁡
ϕ
+
q
sin
ϕ
cos
⁡
θ
.Finally, the following expression is obtained:(12)
[
ϕ
˙
θ
˙
ψ
˙
]
=
[
p
cos
⁡
θ
+
q
sin
ϕ
sin
θ
+
r
cos
⁡
ϕ
sin
θ
cos
⁡
θ
q
cos
⁡
ϕ
+
r
sin
ϕ
r
cos
⁡
ϕ
+
q
sin
ϕ
cos
⁡
θ
]
.The quadrotor was previously assumed to be symmetrical in quality and structure, so the inertia matrix. So it can be defined as a diagonal matrix:(13)
I
[
I
x
I
y
I
z
]
,
where I
x
x, I
y
y, and I
z
z are the rotary inertia around the X, Y, and Z axes, respectively.By calculating the angular momentum, we can obtain the three axial components’ angular motion equations ofM in the quadrotor coordinate system: M
x, M
y, and M
z. Consider
(14)
M
x
=
p
˙
I
x
+
q
r
(
I
z
-
I
y
)
,
p
˙
=
M
x
+
q
r
(
I
y
-
I
z
)
I
x
,
(15)
M
y
=
q
˙
I
y
+
p
r
(
I
x
-
I
z
)
+
(
p
2
-
r
2
)
I
x
z
,
M
y
=
q
˙
I
y
+
p
r
(
I
x
-
I
z
)
,
q
˙
=
M
y
+
p
r
(
I
z
-
I
x
)
I
y
,
(16)
M
z
=
r
˙
I
z
-
p
˙
I
x
z
+
p
q
(
I
y
-
I
x
)
+
q
r
I
x
z
,
M
z
=
r
˙
I
z
+
p
q
(
I
y
-
I
x
)
,
r
˙
=
M
z
+
p
q
(
I
x
-
I
y
)
I
z
.After simplification, the formula becomes(17)
[
p
˙
q
˙
r
˙
]
=
[
M
x
+
q
r
(
I
y
-
I
z
)
I
x
M
y
+
p
r
(
I
z
-
I
x
)
I
y
M
z
+
p
q
(
I
x
-
I
y
)
I
z
]
.Equations (3)–(14) are mathematically considered state equations and can be implemented at the attitude control layer.


## 3.4. The Displacement Control Layer

The displacement control layer controls the movement of the quadrotor to the desired location while resisting disturbances. At this layer, the control system adjusts the attitude of the quadrotor to make it move. That is, this layer receives the input value for position, which it then uses to calculate the proper angle for the quadrotor​​.The algorithm is derived from situations such as those shown in Figure5. The U
1 is known, and the force of x-axis can be obtained by flowing equations. Consider
(18)
F
x
=
U
1
sin
θ
.The quadrotor is tilted to thez-axis. This angle is defined as theta. The force is generated in the x-axis by the rotation of the rotor:
(19)
F
x
=
m
a
x
.Equation (19) is Newton’s second law on the x-axis. Consider
(20)
sin
θ
=
a
x
m
U
1
,
(21)
θ
=
sin
-
1
⁡
(
a
x
m
U
1
)
,
(22)
-
1
≤
a
x
m
U
1
≤
1
.Equation (21) can be obtained by substituting in (18) and (19). Then, an angle can be obtained for the desired acceleration. The respective domain and range of the sine function must be satisfied (22). The physical meaning of (21) is as follows.(i)
In order to obtain a large acceleration, a large angle is necessary.
(ii)
A large mass requires a large angle.
(iii)
If the motor output is large, the desired angle is reduced (Figure3).Figure 3
Definition of axis and rotor output.Figure 4
Model of the structure of the quadrotor.Figure 5
Directional diagram of the control system.Given the angle of the quadrotor, the respective acceleration can be derived easily; however, this system needs the appropriate angle of the quadrotor’s respective displacement. Therefore, we propose another equation derived from (18).If the system is designed to simply resist the acceleration, it cannot achieve its desired target because of the value of the instantaneous acceleration. Our proposed control system needs to know the tendency of the movement due to acceleration. For this reason, the acceleration in (18) is replaced by a different function:
(23)
a
x
⟹
u
x
(
t
)
.The conversion of expression (23) does not use the mathematical meaning of equal. The transform function is inferred from (18):
(24)
e
x
(
t
)
=
d
ref
(
t
)
-
d
(
t
)
,
where d
ref is a desired location in the absolute coordinate system and d is the current position in the absolute coordinate system. The function (24) means the distance from the target point, in this sense called “error” in control engineering:
(25)
d
(
t
)
=
d
(
t
-
1
)
+
∬
a
x
di
(
t
)
d
t
+
∬
(
cos
⁡
ψ
sin
θ
cos
⁡
ϕ
+
sin
ψ
sin
ϕ
)
U
1
m
d
t
,
where d
(
t
-
1
) is the previous position in the absolute coordinate system and a
x
di is the acceleration of the disturbance of the x-axis. This means that the current position is the sum of the previous position, the current attitude of the aircraft, and the acceleration of the disturbance.That is, the transform function refers to the error value in the PID feedback control shown in Figure6, and the feedback control system can minimize the error by adjusting the process control outputs. Consider
(26)
u
(
t
)
=
K
p
e
(
t
)
+
1
T
i
∫
0
t
e
(
τ
)
d
τ
+
T
d
d
e
(
t
)
d
t
,
where τ is a variable of integration and takes values from time zero to the present time, t. K
p, T
i, and T
d signify the gain, a tuning parameter. Consider(27)
θ
=
sin
-
1
⁡
(
u
x
(
t
)
m
U
1
)
,
(28)
-
sin
θ
min
⁡
≤
u
x
(
t
)
m
U
1
≤
sin
θ
max
⁡
.Figure 6
Block diagram of the PID feedback.The appropriate angle can be calculated using (28). The physical meaning of this expression is as follows (Figure 7, left block).(i)
A large mass requires a large angle.
(ii)
If the motor output is large, the desired angle is reduced.
(iii)
The larger the control signal, the more the angle tilts.Figure 7
Implementation of the control system in MATLAB Simulink (layer 3 is excluded).The displacement control layer enables the quadrotor to move to the desired point while resisting disturbance. Invariably, it also makes it possible for the quadrotor to hover. However, this control layer calculates only the angle for the respective distance error. In other words, this layer is designed to faithfully conduct an indigenous mission.


## 4. Implementation and Experimentation

We implemented our proposed control system in MATLAB Simulink and conducted several simulations (Figure7). The experimental values obtained are listed in Tables 1 and 2.Table 2
Control parameter values.
Parameters
Value
P term
P
1.2
I term
I
0.1
D term
D
0.4Optimization is not within the scope of this paper. As a result, approximate values were used. To verify our proposed multilayered control system, only layers 1 and 2 were implemented in the first set of experiments.In Figure7, the left block is the attitude control layer and the right block is the physical layer. The remaining small blocks are the input and output gates.Figure8 shows the results of the first set of experiments. As expected, in situations where there is a continuous disturbance, the quadrotor is able to maintain its attitude. However, it can be seen that the quadrotor is being gradually pushed by the disturbance. The quadrotor control system is intended to give results such as those shown in Figure 8.Results from the partially simulated system (layer 3 is excluded).
(a)
Continuous disturbance
(b)
Position of quadrotor
(c)
Attitude of quadrotor (φ, θ)In the second set of experiments, we added layer 3 to the system.The left block is the displacement control layer. It receives position feedback and outputs the appropriate angle (Figure9).Figure 9
Implementation of the control system in MATLAB Simulink (full system).Figure10 shows the inside block that implements (26) and (27). With the addition of layer 3 to the system, the results depicted in the figure were obtained.Figure 10
Inside of layer 3 block.It can be seen in Figure11 that the attitude changes to resist the disturbance. This appropriate change in the attitude enables the quadrotor to have drift stabilization.Results for the full system (disturbance only).
(a)
Step disturbance of thex-axis
(b)
Position of quadrotor
(c)
Attitude of quadrotor (φ)The control system knows the acceleration due to the quadrotor control position; thus, control is possible. Figure12 shows that it is moved to a point that maintains its objective.Tracking displacement without noise.
(a)
Position of quadrotor (x-axis)
(b)
Position of quadrotor (y-axis)
(c)
Position of quadrotor (z-axis)Figure13 shows that it moves to a point to maintain its objective even though the disturbance is very irregular. When there are large angular changes it is designed to move to a certain point, whereas for small angular changes it is designed to resist the disturbance.Tracking displacement with noise.
(a)
Disturbance ofx-axis
(b)
Position of quadrotor (x-axis)
(c)
Attitude of quadrotor (φ)The final experimental results show that our proposed multilayered drift-stabilization control system can be a useful and powerful solution for microquadrotors.


## 5. Conclusion

The design and implementation of mathematical modeling of our multilayered drift-stabilization method were successful. Each of the layers is designed separately and drift stabilization is achieved by cooperation with each layer.Our proposed system provides powerful solutions for some of the problems that have not yet been solved. The system can be mounted on small vehicles and can make the quadrotor move to its objective even when there are irregular disturbances. The proposed control system has good performance and is easily expandable. Therefore, it is a useful micro-UAV control system.However, this control system presently has an integration error. In order to overcome this limitation, we plan to implement a process that corrects it using vision. The system also lacks optimization, particularly in the PID term. Therefore, we hope to conduct further studies from which we can present optimized results that further enhance the usefulness of our proposed control system.



---

*Source: 747134-2014-06-22.xml*