# The Quadrotor Dynamic Modeling and Indoor Target Tracking Control Method

Authors: Dewei Zhang; Hui Qi; Xiande Wu; Yaen Xie; Jiangtao Xu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

A reliable nonlinear dynamic model of the quadrotor is presented. The nonlinear dynamic model includes actuator dynamic and aerodynamic effect. Since the rotors run near a constant hovering speed, the dynamic model is simplified at hovering operating point. Based on the simplified nonlinear dynamic model, the PID controllers with feedback linearization and feedforward control are proposed using the backstepping method. These controllers are used to control both the attitude and position of the quadrotor. A fully custom quadrotor is developed to verify the correctness of the dynamic model and control algorithms. The attitude of the quadrotor is measured by inertia measurement unit (IMU). The position of the quadrotor in a GPS-denied environment, especially indoor environment, is estimated from the downward camera and ultrasonic sensor measurements. The validity and effectiveness of the proposed dynamic model and control algorithms are demonstrated by experimental results. It is shown that the vehicle achieves robust vision-based hovering and moving target tracking control.


---

## Body

## 1. Introduction

As an emerging platform for unmanned aerial vehicle (UAV) research, the quadrotor has recently gained most attention from the community. With some specific capabilities, such as vertical take-off and landing (VTOL), hovering, fly alone or in team, autonomously fly, it has been envisaged for a wide of applications including military reconnaissance, search and rescue, meteorological survey, environmental monitoring, and wireless mobile senor networks [1]. The quadrotor has several advantages compared to other rotorcrafts. First, the quadrotor does not require swash plate and mechanical linkages as it equips fixed pitch propellers and uses speed variation for vehicle control. This makes it more convenient to design, manufacture, maintain, and recover from incidents. Second, its propellers are smaller in diameter relative to the airframe size and can be enclosed within a frame. This makes it safer and brings more benefit for indoor flight and in obstacle-dense environments. In addition, the quadrotor has greater thrust-weight ratio and thus better maneuver performance. All these advantages promote the development of a number of commercial and research quadrotor platforms [2–5].Although the quadrotor has a series of advantages, it is an absolutely unstable and underactuated dynamic system with sophisticated nonlinearity and strong coupling. Moreover, it is easily affected by near-surface airstream. Because of these difficulties, the intensive study on dynamical modeling, analysis, and advanced control of quadrotor needs to be done to improve the flight quality. In particular, the actuator dynamic and aerodynamic effects must be investigated to establish a reliable dynamic model of quadrotor. Control method dealing with the nonlinearity and coupling property of quadrotor has to be proposed for precise flight control. Bouabdallah and Siegwart mentioned the importance of actuator dynamic and analyzed forces and moments caused by aerodynamic effects. But they simplified the dynamic model and omitted those effects [6]. Huang et al. researched two important aerodynamic effects and presented control techniques compensating for them accordingly. However, only the altitude controller was designed based on nonlinear method [7]. Minh and Ha linearized the nonlinear dynamic model about a trim in hover and applied the LQG method to stabilize the quadrotor with vision-based pose estimation. Nevertheless, only simulation results were displayed [8]. A variety of control algorithms have been attempted to handle the nonlinearity and coupling property, such as neural networks control [9], integral predictive/nonlinear H
∞ control [10], sliding mode control [11, 12], and fuzzy tracking control [13]. Although simulation results demonstrated effectiveness of those control algorithms, most of them were developed based on dynamic model without the actuator dynamic and aerodynamic effect. Besides, the stochastic method and fault detection algorithms are investigated [14–18], whereas those methods are difficult to be implemented in the real system. As the quadrotor is a cascade system, it is proven that the backstepping control method has an excellent performance [19]. Recently, interests in the quadrotor research have transferred to autonomous flight. While the success of laser-based autonomous indoor flight [20–22] has made a huge impact on the development of quadrotor, the vision-based autonomous indoor flight [23] is an immediate area of research focus. Moreover, heterogeneous multiagent problems such as UAV and unmanned ground vehicle (UGV) indoor/outdoor coordination control are expected to be the next technical breakthrough. Based on the research actuality and trend on quadrotor, this paper studies the quadrotor dynamic modeling and indoor target tracking control method. These research results will build foundations for precise flight control and heterogeneous multiagent study.The main contributions of this paper are the following. First, a reliable nonlinear dynamic model is presented based on the analysis of actuator dynamic, aerodynamic effect, and rigid body dynamic. The gyroscope effect of the rotors is considered by dividing the quadrotor into body part and rotor part. It makes the dynamic model more reliable to take actuator dynamic and aerodynamic effect into account. Second, the PID controllers with feedback linearization and feedforward control are proposed to control both the attitude and position of the quadrotor. The dynamic model is explicitly expressed as a cascade system of three subsystems to be suitable for the backstepping method. The control algorithms are realized on a fully custom quadrotor and vision-based autonomous indoor moving target tracking flight is achieved.This paper is structured as follows. In Section2, we first analyze the actuator dynamic and aerodynamic effect. The actuator dynamic is the derivation of the Kirchhoff laws and the law of rotation. The aerodynamic effect is mainly about blade flapping which has a significant effect on attitude tracking control. Then, a reliable nonlinear dynamic model is addressed using Newton-Euler method. The dynamic model is a combination of actuator dynamic, aerodynamic effect, and rigid body dynamic. In Section 3, a general PID controller with feedforward control is proposed. Then, based on the simplified nonlinear dynamic model, decoupling nonlinear control laws are presented using feedback linearization and the backstepping control strategy is applied to the position control. Section 4 describes the system design of our fully custom quadrotor and discusses the experimental results. The fully custom quadrotor is equipped with an IMU, a downward camera, and a downward ultrasonic sensor. Full control experiments are executed in the order of attitude control, altitude control, hovering control, and tracking control. At last, we outline the conclusion in Section 5.


## 2. Mathematical Modeling

Most researchers used to regard the whole quadrotor as a rigid model [24], neglecting the propeller gyroscope effect and aerodynamic effect. Moreover, models containing actuator dynamic are rarely investigated. However, studies show that aerodynamic effect is obvious even with moderate speed [7] and that actuator dynamic has a strong influence on the attitude stabilization [6]. Thus, a detailed analysis of those effects is necessary.The coordinate system is defined in Figure1. E is an earth-fixed frame and B is a body-fixed frame. The body fixed coordinates origin locates at the center of gravity (CoG) of the quadrotor. h is the distance between propeller plane and CoG.Figure 1
Body-fixed frame and Earth-fixed frame.


### 2.1. Actuator Dynamic

Actuator dynamic describes the relationship between rotor speed and actuator voltage. The latter is our real control input. Basically, the actuator response speed is most interested by designers. Based on Kirchhoff laws and the law of rotation, the simplified actuator dynamic model is [25]
(1)
Ω
˙
i
=
-
1
τ
Ω
i
+
k
Ω
τ
u
i
,
where Ω
i, τ, and K
Ω represent the speed of actuator i, delay coefficient, and gain coefficient, respectively.Actuator delay is curial especially when the attitude control loop runs at a low frequency.


### 2.2. Aerodynamic Effect

Aerodynamic effect dramatically increases with the variation from equilibrium state. Some literatures show excellent performance on a test bench [11]. Whereas, the attitude control result without the test bench is worse than the attitude control result on the test bench. This is because the aerodynamic effect occurs when flight without the test bench. Aerodynamic effect is mainly caused by blade flapping, as shown in Figure 2.Figure 2
Motor 1 is the front of the quadrotor; speed-up motor 3 and speed-down motor 1 will result in forward flight.During forward flight, the advancing blade has a higher velocity relative to the free stream and the retreating blade sees a lower effective airspeed. This brings imbalance of lift and results in the propeller plane deflecting from position 1 to position 2. The deflection angleα
x is proportional to the velocity in body x-axis v
x
b:
(2)
α
x
=
k
α
v
x
b
.The deflection of the propeller plane causes an extra moment on they-axis [7]:
(3)
M
b
f
,
y
=
4
(
k
β
α
x
+
T
h
sin
α
x
)
≈
4
(
k
β
α
x
+
T
h
α
x
)
,
where T is the total thrust and k
α and k
β are experimentally measured constants. The blade flapping effect on the x-axis is the same.Air friction is relative to the velocity of quadrotor and can be expressed as(4)
f
=
-
1
2
C
A
c
ρ
v
|
v
|
,
where C, A
c,  ρ, and v represent damping coefficient, active area, air density, and relative speed, respectively.Theoretical and experimental results demonstrate that aerodynamic effect is not trivial even with moderate speed and will be crucially important in aerobatic flight.


### 2.3. Nonlinear Dynamic Model

We separate the quadrotor into two portions, the body part and the rotor part. The body part includes the frame structure and equipments. The rotor part includes motors and propellers. Manifestly, the relative position between the rotor part and the body part varies as the rotor spins. Hence we cannot assume the whole quadrotor as a rigid body. The mathematical model is based on following assumptions.(1)
The body part and rotor part are rigid, respectively.
(2)
The quadrotor is symmetric.
(3)
Thrust and drag are proportional to the square of propeller’s speed.
(4)
Actuator dynamic is identical.
(5)
The center of gravity (CoG) coincides with the body fixed coordinates origin.Apply Newton-Euler equation to body part:(5)
F
E
=
m
X
¨
E
,
M
B
=
I
b
ω
˙
B
+
ω
B
×
I
b
ω
B
,
where F
E is the total force and X
E is the position of the quadrotor expressed in earth-fixed frame E. M
B is the total moment expressed in body-fixed frame B. I
b is the rotational inertia of the body part. ω
B
=
[
p
q
r
]
T is the angular speed expressed in body-fixed frame.Here we employ the Euler angle representation of orientation. We consequently rotate aboutZ
-
X
-
Y axes by yaw angle ψ, roll angle ϕ. and pitch angle θ.Letη
=
[
ϕ
θ
ψ
]
T; the relationship between η
˙ and ω
B is
(6)
η
˙
=
[
c
θ
0
s
θ
s
ϕ
s
θ
c
ϕ
1
-
s
ϕ
c
θ
c
ϕ
-
s
θ
c
ϕ
0
c
θ
c
ϕ
]
ω
B
,
where s
(
·
) and c
(
·
) represent sin
(
·
) and cos
⁡
(
·
), respectively.According to the third assumption, rotor thrustT and drag Q are
(7)
T
=
∑
i
=
1
4
T
i
=
k
T
Ω
i
2
,
Q
=
∑
i
=
1
4
Q
i
=
k
Q
Ω
i
2
.Utilizing the analysis of aerodynamic effect, the total force and moment on the body part are(8)
F
E
=
[
F
z
F
x
F
y
]
=
[
m
g
-
(
c
ψ
c
ϕ
)
T
c
α
x
c
α
y
(
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
)
T
c
α
x
c
α
y
-
1
2
C
x
A
c
ρ
x
˙
|
x
|
(
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
)
T
c
α
x
c
α
y
-
1
2
C
x
A
c
ρ
y
˙
|
y
|
]
,
M
B
=
[
M
x
M
y
M
z
]
=
[
J
r
q
Ω
r
+
l
(
T
4
-
T
2
)
+
M
b
f
,
x
J
r
p
Ω
r
+
l
(
T
1
-
T
3
)
+
M
b
f
,
y
Q
+
l
(
T
2
-
T
4
)
s
α
y
+
l
(
T
3
-
T
1
)
s
α
x
]
≈
[
J
r
q
Ω
r
+
l
(
T
4
-
T
2
)
+
M
b
f
,
x
J
r
p
Ω
r
+
l
(
T
1
-
T
3
)
+
M
b
f
,
y
Q
+
l
(
T
2
-
T
4
)
α
y
+
l
(
T
3
-
T
1
)
α
x
]
,
where J
r is rotational inertia of the rotor part and Ω
r
=
Ω
1
-
Ω
2
+
Ω
3
-
Ω
4 is the sum of rotor speed. Note that the moment for the rotor angular acceleration J
r
Ω
˙
r is produced by electromagnetic force and unrelated to the body part. Based on the second assumption, I
i
j
=
0, i
≠
j;  i
,
j
∈
{
x
y
z
} and I
x
x
=
I
y
y. We regard the body part as controlled object and by integrating the body part dynamic and actuator dynamic we can reach the global nonlinear dynamic model. Here we simplify the nonlinear dynamic model about the hovering operating point. Ω
h is the speed of rotor at the hovering operating point
(9)
Ω
h
=
m
g
4
k
T
.Let(10)
Δ
Ω
ϕ
=
Ω
4
-
Ω
2
,
Δ
Ω
θ
=
Ω
1
-
Ω
3
,
Δ
Ω
ψ
=
Ω
1
-
Ω
2
+
Ω
3
-
Ω
4
,
λ
=
Δ
Ω
T
4
Ω
h
,
Δ
Ω
T
=
Ω
1
-
Ω
h
+
Ω
2
-
Ω
h
+
Ω
3
-
Ω
h
+
Ω
4
-
Ω
h
,
U
2
=
[
u
1
u
2
u
3
u
4
]
T
,
U
1
=
X
2
=
[
Δ
Ω
ϕ
Δ
Ω
θ
Δ
Ω
ψ
Δ
Ω
T
]
T
,
X
1
=
[
ϕ
p
θ
q
ψ
r
z
z
˙
x
x
˙
y
y
˙
]
T
.Hence(11)
X
˙
2
=
f
(
X
2
,
U
2
)
=
[
-
1
τ
0
0
0
0
-
1
τ
0
0
0
0
-
1
τ
0
0
0
0
-
λ
+
1
λ
τ
]
[
Δ
Ω
ϕ
Δ
Ω
θ
Δ
Ω
ψ
Δ
Ω
T
]
+
[
0
-
k
Ω
τ
0
k
Ω
τ
k
Ω
τ
0
-
k
Ω
τ
0
k
Ω
τ
-
k
Ω
τ
k
Ω
τ
-
k
Ω
τ
k
Ω
τ
k
Ω
τ
k
Ω
τ
k
Ω
τ
]
[
u
1
u
2
u
3
u
4
]
,
(12)
X
˙
1
=
f
(
X
1
,
U
1
)
=
[
p
c
θ
+
r
s
θ
[
(
I
y
y
-
I
z
z
)
q
r
+
J
r
q
Δ
Ω
ψ
+
2
l
k
T
Ω
h
Δ
Ω
ϕ
+
k
α
k
β
x
˙
+
T
h
k
α
x
˙
]
I
x
x
(
p
s
ϕ
s
θ
+
q
c
ϕ
-
r
s
ϕ
c
θ
)
c
ϕ
[
(
I
z
z
-
I
x
x
)
p
r
+
J
r
p
Δ
Ω
ψ
+
2
l
k
T
Ω
h
Δ
Ω
θ
+
k
α
k
β
y
˙
+
T
h
k
α
y
˙
]
I
y
y
(
r
c
θ
-
p
s
θ
)
c
ϕ
2
[
k
Q
Ω
h
Δ
Ω
ψ
+
l
k
α
k
T
(
Δ
Ω
ϕ
x
˙
-
Δ
Ω
θ
y
˙
)
]
I
z
z
z
˙
m
g
-
2
k
T
Ω
h
c
ϕ
c
ψ
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
x
˙
2
k
T
Ω
h
(
s
ϕ
s
ψ
+
s
θ
c
ϕ
c
ψ
)
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
-
1
2
C
x
A
c
ρ
x
˙
|
x
˙
|
y
˙
2
k
T
Ω
h
(
-
s
ϕ
c
ψ
+
s
θ
c
ϕ
s
ψ
)
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
-
1
2
C
y
A
c
ρ
y
˙
|
y
˙
|
]
.


## 2.1. Actuator Dynamic

Actuator dynamic describes the relationship between rotor speed and actuator voltage. The latter is our real control input. Basically, the actuator response speed is most interested by designers. Based on Kirchhoff laws and the law of rotation, the simplified actuator dynamic model is [25]
(1)
Ω
˙
i
=
-
1
τ
Ω
i
+
k
Ω
τ
u
i
,
where Ω
i, τ, and K
Ω represent the speed of actuator i, delay coefficient, and gain coefficient, respectively.Actuator delay is curial especially when the attitude control loop runs at a low frequency.


## 2.2. Aerodynamic Effect

Aerodynamic effect dramatically increases with the variation from equilibrium state. Some literatures show excellent performance on a test bench [11]. Whereas, the attitude control result without the test bench is worse than the attitude control result on the test bench. This is because the aerodynamic effect occurs when flight without the test bench. Aerodynamic effect is mainly caused by blade flapping, as shown in Figure 2.Figure 2
Motor 1 is the front of the quadrotor; speed-up motor 3 and speed-down motor 1 will result in forward flight.During forward flight, the advancing blade has a higher velocity relative to the free stream and the retreating blade sees a lower effective airspeed. This brings imbalance of lift and results in the propeller plane deflecting from position 1 to position 2. The deflection angleα
x is proportional to the velocity in body x-axis v
x
b:
(2)
α
x
=
k
α
v
x
b
.The deflection of the propeller plane causes an extra moment on they-axis [7]:
(3)
M
b
f
,
y
=
4
(
k
β
α
x
+
T
h
sin
α
x
)
≈
4
(
k
β
α
x
+
T
h
α
x
)
,
where T is the total thrust and k
α and k
β are experimentally measured constants. The blade flapping effect on the x-axis is the same.Air friction is relative to the velocity of quadrotor and can be expressed as(4)
f
=
-
1
2
C
A
c
ρ
v
|
v
|
,
where C, A
c,  ρ, and v represent damping coefficient, active area, air density, and relative speed, respectively.Theoretical and experimental results demonstrate that aerodynamic effect is not trivial even with moderate speed and will be crucially important in aerobatic flight.


## 2.3. Nonlinear Dynamic Model

We separate the quadrotor into two portions, the body part and the rotor part. The body part includes the frame structure and equipments. The rotor part includes motors and propellers. Manifestly, the relative position between the rotor part and the body part varies as the rotor spins. Hence we cannot assume the whole quadrotor as a rigid body. The mathematical model is based on following assumptions.(1)
The body part and rotor part are rigid, respectively.
(2)
The quadrotor is symmetric.
(3)
Thrust and drag are proportional to the square of propeller’s speed.
(4)
Actuator dynamic is identical.
(5)
The center of gravity (CoG) coincides with the body fixed coordinates origin.Apply Newton-Euler equation to body part:(5)
F
E
=
m
X
¨
E
,
M
B
=
I
b
ω
˙
B
+
ω
B
×
I
b
ω
B
,
where F
E is the total force and X
E is the position of the quadrotor expressed in earth-fixed frame E. M
B is the total moment expressed in body-fixed frame B. I
b is the rotational inertia of the body part. ω
B
=
[
p
q
r
]
T is the angular speed expressed in body-fixed frame.Here we employ the Euler angle representation of orientation. We consequently rotate aboutZ
-
X
-
Y axes by yaw angle ψ, roll angle ϕ. and pitch angle θ.Letη
=
[
ϕ
θ
ψ
]
T; the relationship between η
˙ and ω
B is
(6)
η
˙
=
[
c
θ
0
s
θ
s
ϕ
s
θ
c
ϕ
1
-
s
ϕ
c
θ
c
ϕ
-
s
θ
c
ϕ
0
c
θ
c
ϕ
]
ω
B
,
where s
(
·
) and c
(
·
) represent sin
(
·
) and cos
⁡
(
·
), respectively.According to the third assumption, rotor thrustT and drag Q are
(7)
T
=
∑
i
=
1
4
T
i
=
k
T
Ω
i
2
,
Q
=
∑
i
=
1
4
Q
i
=
k
Q
Ω
i
2
.Utilizing the analysis of aerodynamic effect, the total force and moment on the body part are(8)
F
E
=
[
F
z
F
x
F
y
]
=
[
m
g
-
(
c
ψ
c
ϕ
)
T
c
α
x
c
α
y
(
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
)
T
c
α
x
c
α
y
-
1
2
C
x
A
c
ρ
x
˙
|
x
|
(
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
)
T
c
α
x
c
α
y
-
1
2
C
x
A
c
ρ
y
˙
|
y
|
]
,
M
B
=
[
M
x
M
y
M
z
]
=
[
J
r
q
Ω
r
+
l
(
T
4
-
T
2
)
+
M
b
f
,
x
J
r
p
Ω
r
+
l
(
T
1
-
T
3
)
+
M
b
f
,
y
Q
+
l
(
T
2
-
T
4
)
s
α
y
+
l
(
T
3
-
T
1
)
s
α
x
]
≈
[
J
r
q
Ω
r
+
l
(
T
4
-
T
2
)
+
M
b
f
,
x
J
r
p
Ω
r
+
l
(
T
1
-
T
3
)
+
M
b
f
,
y
Q
+
l
(
T
2
-
T
4
)
α
y
+
l
(
T
3
-
T
1
)
α
x
]
,
where J
r is rotational inertia of the rotor part and Ω
r
=
Ω
1
-
Ω
2
+
Ω
3
-
Ω
4 is the sum of rotor speed. Note that the moment for the rotor angular acceleration J
r
Ω
˙
r is produced by electromagnetic force and unrelated to the body part. Based on the second assumption, I
i
j
=
0, i
≠
j;  i
,
j
∈
{
x
y
z
} and I
x
x
=
I
y
y. We regard the body part as controlled object and by integrating the body part dynamic and actuator dynamic we can reach the global nonlinear dynamic model. Here we simplify the nonlinear dynamic model about the hovering operating point. Ω
h is the speed of rotor at the hovering operating point
(9)
Ω
h
=
m
g
4
k
T
.Let(10)
Δ
Ω
ϕ
=
Ω
4
-
Ω
2
,
Δ
Ω
θ
=
Ω
1
-
Ω
3
,
Δ
Ω
ψ
=
Ω
1
-
Ω
2
+
Ω
3
-
Ω
4
,
λ
=
Δ
Ω
T
4
Ω
h
,
Δ
Ω
T
=
Ω
1
-
Ω
h
+
Ω
2
-
Ω
h
+
Ω
3
-
Ω
h
+
Ω
4
-
Ω
h
,
U
2
=
[
u
1
u
2
u
3
u
4
]
T
,
U
1
=
X
2
=
[
Δ
Ω
ϕ
Δ
Ω
θ
Δ
Ω
ψ
Δ
Ω
T
]
T
,
X
1
=
[
ϕ
p
θ
q
ψ
r
z
z
˙
x
x
˙
y
y
˙
]
T
.Hence(11)
X
˙
2
=
f
(
X
2
,
U
2
)
=
[
-
1
τ
0
0
0
0
-
1
τ
0
0
0
0
-
1
τ
0
0
0
0
-
λ
+
1
λ
τ
]
[
Δ
Ω
ϕ
Δ
Ω
θ
Δ
Ω
ψ
Δ
Ω
T
]
+
[
0
-
k
Ω
τ
0
k
Ω
τ
k
Ω
τ
0
-
k
Ω
τ
0
k
Ω
τ
-
k
Ω
τ
k
Ω
τ
-
k
Ω
τ
k
Ω
τ
k
Ω
τ
k
Ω
τ
k
Ω
τ
]
[
u
1
u
2
u
3
u
4
]
,
(12)
X
˙
1
=
f
(
X
1
,
U
1
)
=
[
p
c
θ
+
r
s
θ
[
(
I
y
y
-
I
z
z
)
q
r
+
J
r
q
Δ
Ω
ψ
+
2
l
k
T
Ω
h
Δ
Ω
ϕ
+
k
α
k
β
x
˙
+
T
h
k
α
x
˙
]
I
x
x
(
p
s
ϕ
s
θ
+
q
c
ϕ
-
r
s
ϕ
c
θ
)
c
ϕ
[
(
I
z
z
-
I
x
x
)
p
r
+
J
r
p
Δ
Ω
ψ
+
2
l
k
T
Ω
h
Δ
Ω
θ
+
k
α
k
β
y
˙
+
T
h
k
α
y
˙
]
I
y
y
(
r
c
θ
-
p
s
θ
)
c
ϕ
2
[
k
Q
Ω
h
Δ
Ω
ψ
+
l
k
α
k
T
(
Δ
Ω
ϕ
x
˙
-
Δ
Ω
θ
y
˙
)
]
I
z
z
z
˙
m
g
-
2
k
T
Ω
h
c
ϕ
c
ψ
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
x
˙
2
k
T
Ω
h
(
s
ϕ
s
ψ
+
s
θ
c
ϕ
c
ψ
)
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
-
1
2
C
x
A
c
ρ
x
˙
|
x
˙
|
y
˙
2
k
T
Ω
h
(
-
s
ϕ
c
ψ
+
s
θ
c
ϕ
s
ψ
)
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
-
1
2
C
y
A
c
ρ
y
˙
|
y
˙
|
]
.


## 3. Control Implementation

First, we define a general PID controller with feedforward control(13)
PID
(
ζ
)
=
K
p
(
ζ
d
-
ζ
)
+
k
d
(
ζ
˙
d
-
ζ
˙
)
+
k
I
∫
(
ζ
d
-
ζ
)
d
t
+
ζ
¨
d
,
in which ζ
¨
d is the feedforward part.Then we apply feedback linearization to design the attitude and position controllers. Finally, we describe the backstepping control scheme.


### 3.1. Position Controllers

The outputs of the position controllers areθ, ϕ, Δ
Ω
T. During the hovering and tracking flight, ψ
=
0; thus, we choose the control law as
(14)
s
θ
=
PID
(
x
)
+
C
x
A
c
ρ
x
˙
|
x
˙
|
/
2
2
k
T
Ω
h
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
,
s
ϕ
=
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
/
2
-
2
k
T
Ω
h
c
θ
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
,
Δ
Ω
T
=
m
g
-
PID
(
z
)
2
k
T
Ω
h
c
ϕ
c
θ
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
.Substituting (14) into (12), we get
(15)
x
¨
=
PID
(
x
)
,
y
¨
=
PID
(
y
)
,
z
¨
=
PID
(
z
)
.This guarantees asymptotic stability and has robustness to some uncertainties. Solving (14), we get the decoupling form:
(16)
θ
=
-
arctan
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
)
-
1
(
PID
(
x
)
+
C
x
A
c
ρ
x
˙
|
x
˙
|
2
)
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
[
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
(
PID
(
z
)
-
m
g
)
2
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
)
-
1
)
,
ϕ
=
arctan
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
/
2
PID
(
z
)
-
m
g
)
,
Δ
Ω
T
=
(
m
g
-
PID
(
z
)
)
3
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
2
k
T
Ω
h
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
×
[
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
(
PID
(
z
)
-
m
g
)
2
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
)
.
-
1


### 3.2. Attitude Controllers

The outputs of the position controllers are the inputs of the attitude controllers. The outputs of the attitude controllers areΔ
Ω
ϕ, Δ
Ω
θ, and Δ
Ω
ψ. Utilizing the similar design method, we choose the control law as
(17)
Δ
Ω
ϕ
=
1
2
l
k
T
Ω
h
×
{
[
·
]
ϕ
-
J
r
q
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
(
PID
(
ψ
)
I
z
z
-
k
α
x
˙
[
·
]
ϕ
Ω
h
+
k
α
y
˙
[
·
]
θ
Ω
h
)
}
,
Δ
Ω
θ
=
1
2
l
k
T
Ω
h
×
{
[
·
]
θ
-
J
r
q
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
(
PID
(
ψ
)
I
z
z
-
k
α
x
˙
[
·
]
ϕ
Ω
h
+
k
α
y
˙
[
·
]
θ
Ω
h
)
}
,
Δ
Ω
ψ
=
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
{
PID
(
ψ
)
I
z
z
-
k
α
x
˙
Ω
h
[
·
]
ϕ
+
k
α
y
˙
Ω
h
[
·
]
θ
}
,
[
·
]
ϕ
=
PID
(
ϕ
)
I
x
x
-
k
α
k
β
x
˙
-
T
h
k
α
x
˙
-
(
I
y
y
-
I
z
z
)
q
r
,
[
·
]
θ
=
PID
(
θ
)
I
y
y
-
k
α
k
β
y
˙
-
T
h
k
α
y
˙
-
(
I
y
y
-
I
z
z
)
p
r
.


### 3.3. Backstepping Control Algorithms

The outputs of attitude controllers are the inputs of the actuator controllers. The outputs of the actuator controllers areU
2
=
[
u
1
u
2
u
3
u
4
]
T. Considering the relationship between position controllers, attitude controllers, and actuator controllers, we proposed the backstepping control scheme shown in Figure 3.Figure 3
Backstepping control scheme.


## 3.1. Position Controllers

The outputs of the position controllers areθ, ϕ, Δ
Ω
T. During the hovering and tracking flight, ψ
=
0; thus, we choose the control law as
(14)
s
θ
=
PID
(
x
)
+
C
x
A
c
ρ
x
˙
|
x
˙
|
/
2
2
k
T
Ω
h
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
,
s
ϕ
=
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
/
2
-
2
k
T
Ω
h
c
θ
Δ
Ω
T
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
,
Δ
Ω
T
=
m
g
-
PID
(
z
)
2
k
T
Ω
h
c
ϕ
c
θ
c
(
k
α
x
˙
)
c
(
k
β
y
˙
)
.Substituting (14) into (12), we get
(15)
x
¨
=
PID
(
x
)
,
y
¨
=
PID
(
y
)
,
z
¨
=
PID
(
z
)
.This guarantees asymptotic stability and has robustness to some uncertainties. Solving (14), we get the decoupling form:
(16)
θ
=
-
arctan
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
)
-
1
(
PID
(
x
)
+
C
x
A
c
ρ
x
˙
|
x
˙
|
2
)
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
[
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
(
PID
(
z
)
-
m
g
)
2
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
1
/
2
)
-
1
)
,
ϕ
=
arctan
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
/
2
PID
(
z
)
-
m
g
)
,
Δ
Ω
T
=
(
m
g
-
PID
(
z
)
)
3
×
(
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
2
k
T
Ω
h
c
(
k
α
x
˙
)
c
(
k
α
y
˙
)
×
[
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
(
PID
(
z
)
-
m
g
)
2
+
(
PID
(
y
)
+
C
y
A
c
ρ
y
˙
|
y
˙
|
2
)
2
]
)
.
-
1


## 3.2. Attitude Controllers

The outputs of the position controllers are the inputs of the attitude controllers. The outputs of the attitude controllers areΔ
Ω
ϕ, Δ
Ω
θ, and Δ
Ω
ψ. Utilizing the similar design method, we choose the control law as
(17)
Δ
Ω
ϕ
=
1
2
l
k
T
Ω
h
×
{
[
·
]
ϕ
-
J
r
q
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
(
PID
(
ψ
)
I
z
z
-
k
α
x
˙
[
·
]
ϕ
Ω
h
+
k
α
y
˙
[
·
]
θ
Ω
h
)
}
,
Δ
Ω
θ
=
1
2
l
k
T
Ω
h
×
{
[
·
]
θ
-
J
r
q
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
(
PID
(
ψ
)
I
z
z
-
k
α
x
˙
[
·
]
ϕ
Ω
h
+
k
α
y
˙
[
·
]
θ
Ω
h
)
}
,
Δ
Ω
ψ
=
Ω
h
2
k
Q
Ω
h
2
+
k
α
J
r
(
p
y
˙
-
q
x
˙
)
×
{
PID
(
ψ
)
I
z
z
-
k
α
x
˙
Ω
h
[
·
]
ϕ
+
k
α
y
˙
Ω
h
[
·
]
θ
}
,
[
·
]
ϕ
=
PID
(
ϕ
)
I
x
x
-
k
α
k
β
x
˙
-
T
h
k
α
x
˙
-
(
I
y
y
-
I
z
z
)
q
r
,
[
·
]
θ
=
PID
(
θ
)
I
y
y
-
k
α
k
β
y
˙
-
T
h
k
α
y
˙
-
(
I
y
y
-
I
z
z
)
p
r
.


## 3.3. Backstepping Control Algorithms

The outputs of attitude controllers are the inputs of the actuator controllers. The outputs of the actuator controllers areU
2
=
[
u
1
u
2
u
3
u
4
]
T. Considering the relationship between position controllers, attitude controllers, and actuator controllers, we proposed the backstepping control scheme shown in Figure 3.Figure 3
Backstepping control scheme.


## 4. Verification and Results

The nonlinear dynamic model and control algorithms are verified on a fully custom quadrotor. First, the design and manufacture of our fully custom quadrotor is briefly described. Then, results of different experiments are discussed consequently.


### 4.1. Verification

We developed a fully custom quadrotor using the optimal design algorithm, shown in Figure4. The frame is made from carbon fiber composite (CFC), and the structure is designed with CATIA v5. The inherent frequency of the frame is more than 100 Hz while the vibration frequency caused by actuators is about 60 Hz. This avoids resonance and reduces the accelerometer measurement noise. The downward camera and ultrasonic sensor are utilized to obtain the physical position, running at 25 Hz and 10 Hz, respectively. The image is off-board processed as shown in Figure 4. Furthermore, there is a remote control UGV with a colored mark, playing the role as a moving target, Figure 5.Figure 4
Off-board process of the image.A fully custom quadrotor and autonomous moving target tracking.
(a)
(b)The parameters of the rigid body dynamic are calculated during the design process. The parameters of the aerodynamic effect are estimated by empirical model and data fitting. The actuator dynamic is obtained by system identification. The parameters used in the verification experiments are listed in Table1.Table 1
Parameters used in the verification experiments.
Parameter
Description
Value
Units
g
Gravity
9.81
m/
s
2
m
Mass
1.17
kg
l
Distance between CoG and motor
0.25
m
h
Distance between CoG and propeller plane
0.05
m
I
x
x
Roll inertia
1.27
×
1
0
-
2
kg·m2
I
y
y
Pitch inertia
1.27
×
1
0
-
2
kg·m2
I
z
z
Yaw inertia
2.29
×
1
0
-
2
kg·m2
J
r
Rotor inertia
3.8
×
1
0
-
5
kg·m2
k
T
Thrust coefficient
2.1
×
1
0
-
5
N·s2
k
Q
Drag coefficient
1.2
×
1
0
-
6
N·s2
τ
Motor time constant
0.09
s
k
α
Velocity to angle constant
6.1
×
1
0
-
3
rad·s/m
k
β
Angle to moment constant
6.1
×
1
0
-
3
N·m/rad
A
c
Active area
0.25
m
2
ρ
Air density
1.205
kg/
m
3
C
Damping coefficient
0.09


### 4.2. Experiment Results

The hardware in loop (HIL) simulation is executed before flight experiment. Manually change the pitch angle, roll angle, and yaw angle of the quadrotor; the speed command of each motor changes, respectively. Manually change the altitude of the quadrotor; the throttle command for four motors changes. Manually change the position of the quadrotor; the attitude commands change. The simulation results verify the correctness of the dynamic model and control algorithms. The HIL simulation is shown in Figure6.Figure 6
The HIL simulation: manually change the quadrotor, the 3D model changes, and the curves show the outputs of the controllers.Attitude control, altitude control, hovering control, and moving target tracking control experiments are consequently performed. As shown in Figure7, the pitch and roll control error is less than 2 degrees while yaw control error is less than 5 degrees. Altitude control experiment is executed with a switch. The quadrotor takes off manually and then switches to autoaltitude control. From Figure 8 we can know that the altitude control error is less than 5 cm. Hovering and moving target tracking control experiments are conducted with a switch too. As shown in Figures 9 and 10, both the hovering control and tracking control errors are less than 10 cm.Attitude control results.
(a)
(b)
(c)Figure 8
Altitude control results.Figure 9
Hovering control results.Moving target tracking control results.
(a)
(b)


## 4.1. Verification

We developed a fully custom quadrotor using the optimal design algorithm, shown in Figure4. The frame is made from carbon fiber composite (CFC), and the structure is designed with CATIA v5. The inherent frequency of the frame is more than 100 Hz while the vibration frequency caused by actuators is about 60 Hz. This avoids resonance and reduces the accelerometer measurement noise. The downward camera and ultrasonic sensor are utilized to obtain the physical position, running at 25 Hz and 10 Hz, respectively. The image is off-board processed as shown in Figure 4. Furthermore, there is a remote control UGV with a colored mark, playing the role as a moving target, Figure 5.Figure 4
Off-board process of the image.A fully custom quadrotor and autonomous moving target tracking.
(a)
(b)The parameters of the rigid body dynamic are calculated during the design process. The parameters of the aerodynamic effect are estimated by empirical model and data fitting. The actuator dynamic is obtained by system identification. The parameters used in the verification experiments are listed in Table1.Table 1
Parameters used in the verification experiments.
Parameter
Description
Value
Units
g
Gravity
9.81
m/
s
2
m
Mass
1.17
kg
l
Distance between CoG and motor
0.25
m
h
Distance between CoG and propeller plane
0.05
m
I
x
x
Roll inertia
1.27
×
1
0
-
2
kg·m2
I
y
y
Pitch inertia
1.27
×
1
0
-
2
kg·m2
I
z
z
Yaw inertia
2.29
×
1
0
-
2
kg·m2
J
r
Rotor inertia
3.8
×
1
0
-
5
kg·m2
k
T
Thrust coefficient
2.1
×
1
0
-
5
N·s2
k
Q
Drag coefficient
1.2
×
1
0
-
6
N·s2
τ
Motor time constant
0.09
s
k
α
Velocity to angle constant
6.1
×
1
0
-
3
rad·s/m
k
β
Angle to moment constant
6.1
×
1
0
-
3
N·m/rad
A
c
Active area
0.25
m
2
ρ
Air density
1.205
kg/
m
3
C
Damping coefficient
0.09


## 4.2. Experiment Results

The hardware in loop (HIL) simulation is executed before flight experiment. Manually change the pitch angle, roll angle, and yaw angle of the quadrotor; the speed command of each motor changes, respectively. Manually change the altitude of the quadrotor; the throttle command for four motors changes. Manually change the position of the quadrotor; the attitude commands change. The simulation results verify the correctness of the dynamic model and control algorithms. The HIL simulation is shown in Figure6.Figure 6
The HIL simulation: manually change the quadrotor, the 3D model changes, and the curves show the outputs of the controllers.Attitude control, altitude control, hovering control, and moving target tracking control experiments are consequently performed. As shown in Figure7, the pitch and roll control error is less than 2 degrees while yaw control error is less than 5 degrees. Altitude control experiment is executed with a switch. The quadrotor takes off manually and then switches to autoaltitude control. From Figure 8 we can know that the altitude control error is less than 5 cm. Hovering and moving target tracking control experiments are conducted with a switch too. As shown in Figures 9 and 10, both the hovering control and tracking control errors are less than 10 cm.Attitude control results.
(a)
(b)
(c)Figure 8
Altitude control results.Figure 9
Hovering control results.Moving target tracking control results.
(a)
(b)


## 5. Conclusions and Future Works

We aim at precise modeling, analysis, and control of a sophisticated nonlinear system. This paper presented the newest research on quadrotor of our project. First, we analyzed the actuator dynamic and aerodynamic effect of the quadrotor. Then, we established a reliable nonlinear dynamic model of the quadrotor. As the backstepping control algorithm is well fit for the cascaded structured systems such as the quadrotor, we designed a series of PID controllers with feedforward control and feedback linearization using the backstepping method. Real experiments were executed and the effectiveness of the proposed dynamic model and control method is demonstrated by the experimental result. The future works include two directions. Firstly, the quaternion representation of orientation needs to be employed since the Euler angle representation is subject to problematic singularities. The global stable controllers are expected to be proposed based on the quaternion representation. Secondly, more efforts need to be done to promote the moving target tracking system more like a heterogeneous multiagent system. Problems within heterogeneous multiagent system are expected to be the next technical breakthrough.



---

*Source: 637034-2014-03-20.xml*