# Quadrotor Trajectory Tracking Based on Quasi-LPV System and Internal Model Control

Authors: ZeFang He; Long Zhao
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

Internal model control (IMC) design method based on quasi-LPV (Linear Parameter Varying) system is proposed. In this method, the nonlinear model is firstly transformed to the linear model based on quasi-LPV method; then, the quadrotor nonlinear motion function is transformed to transfer function matrix based on the transformation model from the state space to the transfer function; further, IMC is designed to control the controlled object represented by transfer function matrix and realize quadrotor trajectory tracking. The performance of the controller proposed in this paper is tested by tracking for three reference trajectories with drastic changes. The simulation results indicate that the control method proposed in this paper has stronger robustness to parameters uncertainty and disturbance rejection performance.


---

## Body

## 1. Introduction

In the last few years, quadrotor helicopters have received widespread attention for their many advantages and relevant technologies have also been important research topics [1]. A quadrotor helicopter is a complex system with high nonlinearities, strong couplings, and underactuation, and it is constantly affected by aerodynamic disturbance, unmodeled dynamics, and parametric uncertainty. Quadrotor helicopter control is a challenging problem [2]. Therefore, the research for the control systems of quadrotor helicopters has been widely conducted in the automatic control field [3].Among all the control systems, robust nonlinear controllers have good control effect compared to classic controllers. The common robust control methods include sliding mode control [4, 5] and nonlinear control [6, 7]. These methods especially sliding mode control can effectively control the quadrotor, provide good dynamic performance, and assure robust stability. Sliding mode control and its improved methods are used in wider control field except for quadrotor. Sliding mode control has been used in underactuated two-wheeled mobile robot [8], steer-by-wire systems with AC motors [9], orbital stabilization of inverted-pendulum systems [10], stochastic polynomial systems with unmeasured states [11], electric drive [12], multimachine power system [13], and motion control [14]. But these methods directly rely on quadrotor nonlinear equations to achieve the control for the quadrotor, so the design method is complex and it is difficult to be mastered by engineers. For this reason, in this paper, the nonlinear model is firstly transformed to the linear model based on quasi-LPV (Linear Parameter Varying) method; the quadrotor model is then represented by the transfer function using the transformation model from the state space to the transfer function; finally, IMC (internal model control) is designed. IMC can inhibit the time-delay and has strong robustness.In order to analyze and design gain scheduled control, Shamma proposed the LPV system in 1988 [15]. Later, Shamma and Athans further studied and improved the LPV system [16, 17]. In the last decades, the LPV system which is independent of both LTI (Linear Time Invariant) and LTV (Linear Time Varying) systems has become a standard formalism in systems analysis and controller synthesis. In 2012, Shamma summarizes the research results for the LPV systems [18]. Quasi-LPV [19, 20] is an extension of the LPV. The LPV controller designs based on the LPV system can be found in the literature in [21, 22] and their references.IMC (internal model control) is proposed by Garcia and Morari in 1982 [23] for analyzing the two predictive control systems MAC (model algorithmic control) [24] and DMC (dynamic matrix control) [25] and as an extension for the Smith prediction assessment which is also called Smith time-delay compensator. IMC makes the Smith time-delay compensator design simpler and has stronger robustness and interference rejection ability. IMC is widely used in process control; for example, an internal model control method is designed for controlling the adiabatic reaction temperature of autothermal reforming (ATR) reactor [26], inverted decoupling internal model control method is used to control square stable multivariable time-delay systems [27], and an improvement method of the IMC is used to control MIMO (multiple input-multiple output) first order time-delay nonsquare systems [28]. In recent years, IMC has been expanded to the robotic arm, smart car control systems, and so forth. For example, IMC is used to control hydraulically driven robotic arm [29] and smart car’s speed [30]; modified IMC schemes with fuzzy supervisor are proposed to control the speed of heavy duty vehicle (HDV) [31]. But IMC is rarely used to control quadrotor; only the literature in [32] is found. In the literature in [32], discrete-time IMC is used for quadrotor trajectory tracking control. In this paper, aiming at the characteristics of quadrotor, a controller design strategy combining quasi-LPV system and IMC is proposed to track the given trajectory, where the quasi-LPV system is only used to transform the nonlinear model to the linear model, which is convenient to get the transfer function of the quadrotor and further control quadrotor.Quasi-LPV system is an extension of the LPV system. Correspondingly, the control method for LPV system can also be applied to quasi-LPV system. IMC design for LPV system can be found in the literature in [33, 34]. IMC in the literature in [33] is a one degree of freedom IMC which is designed to do compromise between the performance and robustness. Practically, we can use two degrees of freedom IMC as shown in this paper and the literature in [34]. In two degrees of freedom IMC, feedforward IMC only needs to consider tracking performance and feedback filter only needs to consider robustness and disturbance rejection performance. In addition, the author of the literature in [33] points out that the benefit of design presented in his paper is eliminating the need for adjusting IMC filter. But it can be seen from the literature in [33] that design weights need to be chosen for solving the LMI problem. The choice of the design weights is also a process of trial and error. Therefore, the literature in [33] eliminates the need to adjust IMC filter but at the same time increases the need to choose design weights. In the literature in [34], the author uses the generalized IMC (GIMC) based on LMI to control LPV system. The feedback controller in GIMC controller is equivalent to feedforward controller in IMC, and the conditional controller in GIMC is equivalent to feedback filter part in two degrees of freedom IMC. According to the literature in [35], the parameter of feedback filter is often set as the half of loop delay time, which has been verified to be correct in many applications. Therefore, it is not necessary to use LMI to adjust the filter parameter. In addition, the author of the literature in [34] points out that LPV system cannot be used to treat transfer functions, but it can be seen from the proof of Lemma  1 that the state matrices of LPV system can do matrix addition, multiplication, and inverse operations. Therefore, the concept of the transfer function in linear system can be generalized to LPV system and further generalized to quasi-LPV system.The motivation of this paper is to design a controller which is simple but has strong robustness to parameters uncertainty and disturbance rejection performance. The control strategy in this paper which integrates the advantages of the quasi-LPV system and IMC can realize the above purpose. The contributions of this paper are as follows: the integration of the quasi-LPV system and IMC is firstly proposed to control quadrotor and this design method can be generalized to control other nonlinear systems; the concept of the transfer function in linear system is generalized to quasi-LPV system and the two degrees of freedom IMC in which the parameter of feedback filter is adjusted according to the literature in [35] are used to control quadrotor; It can be seen from the transfer function matrix of quadrotor that the relationships of three attitude angles are as follows: yaw angle is decoupled with roll and pitch angles, roll angle is coupled with pitch angle, and when ψ
˙
=
0, roll angle is decoupled with pitch angle; the virtual inputs are proposed to simplify the position loop transfer function and a new calculation method for the desired values of roll and pitch angles is proposed, which is not restricted by the yaw angle, and therefore the control performance of controller adopted in this paper is better, which can make quadrotor fly with better mobility.The remainder of the paper is organized as follows: quadrotor helicopter model is described in Section2. The calculation of quasi-LPV model and the establishment of the quadrotor system transfer function are given in Section 3. In Section 4, the IMC design and the calculation of the desired roll angle, pitch angle, and total thrust are presented. Simulation results are reported in Section 5. Finally, we draw some conclusions and shed light on future work in Section 6.


## 2. Quadrotor Model

The structure of a quadrotor helicopter is shown in Figure1, where B represents the body coordinate system and E represents the ground coordinate system.Figure 1
The structure of a quadrotor helicopter.Given an aircraft, the positivex direction in the body coordinate frame is usually defined as the moving direction, the positive y direction as the right side of the moving direction, and the positive z direction as the vertical downward direction; this is also termed forward-right-downward coordinate frame [36].Because the fly altitude of quadrotor helicopters is limited in the atmosphere, the “flat earth hypothesis” can be adopted to consider the ground coordinate frame as an inertial coordinate frame to simplify the modeling complexity. In order to facilitate navigation and way-point tracking, the axis directions of the ground coordinate frame are chosen as north-east-down navigation frame directions, namely, that theX
g axis points to the north, the Y
g axis points to the east, and the Z
g axis is perpendicular to the ground and points to the center of the earth.The two main mechanism-modeling methods for the quadrotor include Newton-Euler formalism and Lagrange-Euler formalism [37–39]. In this paper, quadrotor helicopter nonlinear model is obtained by the Newton-Euler formalism and the rotor dynamics is not considered during the model establishment.The procedure of the Newton-Euler formalism modeling is as follows: firstly projecting lift forces acting on the aircraft to the ground coordinate frame; secondly analyzing the linear motion of the aircraft with Newton’s second law in the inertial coordinate system and the angular motion of the aircraft with the law of moment of momentum in the body coordinate system. Some assumptions are made in the process of quadrotor modeling as follows: quadrotor is a rigid body; the structure is symmetric; the center of gravity and the origin of body coordinate system are coincident and ground effect is ignored.


### 2.1. Kinematics Model

The transformation matrix between two rectangular coordinate systems is orthogonal.R
x
,
ϕ, R
y
,
θ, and R
z
,
ψ denote rotation matrices produced by the ground coordinate frame rotating roll angle ϕ, pitch angle θ, and yaw angle ψ around x, y, and z axes, respectively, and the expressions are as follows:(1)
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
.The rotation matrix from the ground coordinate system to the body coordinate system is the product of formulae (1), which denote rotation around the z axis followed by rotation around y axis and finally followed by rotation around x axis; namely,(2)
R
E
→
B
=
R
x
,
ϕ
R
y
,
θ
R
z
,
ψ
.Therefore, the transformation matrix from the ground coordinate system to the body coordinate system is given by(3)
R
B
→
E
=
R
E
→
B
T
.The specific expression is given by(4)
R
B
→
E
=
c
θ
c
ψ
s
ϕ
s
θ
c
ψ
-
c
ϕ
s
ψ
c
ϕ
s
θ
c
ψ
+
s
ϕ
s
ψ
c
θ
s
ψ
s
ϕ
s
θ
s
ψ
+
c
ϕ
c
ψ
c
ϕ
s
θ
s
ψ
-
s
ϕ
c
ψ
-
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
,where c
·
=
cos
⁡
(
·
) and s
·
=
sin
⁡
(
·
).The angular velocity componentsp, q, and r are the projection values on the body coordinate system of rotation angular velocity ω which denotes the rotation from the ground coordinate system to the body coordinate system. The relationships between angular velocity components and the attitude angle change rates are shown in Figure 2.Figure 2
The relationships between angular velocity components and the attitude angle change rate.The transformation matrix fromϕ
˙
θ
˙
ψ
˙
T to p
q
r
T is given by(5)
p
q
r
=
R
r
ϕ
˙
θ
˙
ψ
˙
,where(6)
R
r
=
1
0
-
sin
⁡
θ
0
cos
⁡
ϕ
sin
⁡
ϕ
cos
⁡
θ
0
-
sin
⁡
ϕ
cos
⁡
ϕ
cos
⁡
θ
.Around hovering position, R
r is assumed as a unit matrix [36, 40].


### 2.2. Dynamic Model

The dynamics model is composed of the rotational and translational motions. The rotational motion is fully actuated, while the translational motion is underactuated. In the body coordinate system, the rotational motion equations are derived according to the law of momentum theorem and gyroscopic effect of quadrotor; they are given by(7)
J
ω
˙
+
ω
×
J
ω
+
ω
×
0
0
J
r
Ω
r
=
M
B
,where J is the inertia matrix of quadrotor which is diagonal under the hypothesis of structure symmetry and the elements I
x, I
y, and I
z are, respectively, inertia matrices of x, y, and z axes. The last item on the left side of (7) represents the gyroscopic effect which is caused by the inertia of the rotors J
r and relative speed Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4, where Ω
i(i
=
1,2
,
3,4) represents the ith rotor speed. The aerodynamic force and moment produced by the ith rotor are directly proportional to the square of the rotor speed. The relationships are given by(8)
F
i
=
b
Ω
i
2
M
i
=
d
Ω
i
2
,where b and d are the aerodynamic force and moment constants, respectively. The moments acting on the quadrotor in the body coordinate system are given by(9)
M
B
=
l
·
b
-
Ω
2
2
+
Ω
4
2
l
·
b
Ω
1
2
-
Ω
3
2
d
Ω
1
2
-
Ω
2
2
+
Ω
3
2
-
Ω
4
2
,where l is the moment arm which represents the distance from the axis of a rotor to the center of quadrotor.The translational motion equations are obtained in the ground coordinate system by the method of Newton’s second law(10)
m
r
¨
=
0
0
m
g
T
+
R
F
B
,where r
=
x
y
z
T is the position of quadrotor in the ground coordinate system, m is the mass of quadrotor, g is the acceleration of gravity, and F
B is the total lift force acting on quadrotor in the body coordinate system; namely,(11)
F
B
=
0
0
-
b
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2
.


### 2.3. The Motion Equations of Quadrotor

Synthesizing the kinematics and dynamics models of quadrotor, the motion equations of quadrotor can be derived as follows:(12)
ϕ
¨
=
θ
˙
ψ
˙
I
y
-
I
z
I
x
-
J
r
I
x
θ
˙
Ω
r
+
L
I
x
U
2
,
θ
¨
=
ϕ
˙
ψ
˙
I
z
-
I
x
I
y
+
J
r
I
y
ϕ
˙
Ω
r
+
L
I
y
U
3
,
ψ
¨
=
ϕ
˙
θ
˙
I
x
-
I
y
I
z
+
1
I
z
U
4
,
x
¨
=
-
U
1
m
cos
⁡
ϕ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
ϕ
sin
⁡
ψ
,
y
¨
=
-
U
1
m
cos
⁡
ϕ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
ϕ
cos
⁡
ψ
,
z
¨
=
g
-
U
1
m
cos
⁡
ϕ
cos
⁡
θ
,where ϕ, θ, and φ are the attitude angle of quadrotor; x, y, and z are the position of quadrotor; U
1, U
2, U
3, and U
4 are the control input variables, which can be, respectively, calculated by U
1
=
b
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2, U
2
=
b
-
Ω
2
2
+
Ω
4
2, U
3
=
b
Ω
1
2
-
Ω
3
2, and U
4
=
d
Ω
1
2
-
Ω
2
2
+
Ω
3
2
-
Ω
4
2 in which b and d are the aerodynamic force and moment constants, respectively; I
x, I
y, and I
z are, respectively, inertia matrices of x, y, and z axes. J
r is the inertia matrix of the rotor and Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4 is rotors relative speed, where Ω
i(i
=
1,2
,
3,4) represents the ith rotor speed; l is the moment arm which represents the distance from the axis of a rotor to the center of quadrotor; m is the mass of quadrotor; and g is the acceleration of gravity.


## 2.1. Kinematics Model

The transformation matrix between two rectangular coordinate systems is orthogonal.R
x
,
ϕ, R
y
,
θ, and R
z
,
ψ denote rotation matrices produced by the ground coordinate frame rotating roll angle ϕ, pitch angle θ, and yaw angle ψ around x, y, and z axes, respectively, and the expressions are as follows:(1)
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
.The rotation matrix from the ground coordinate system to the body coordinate system is the product of formulae (1), which denote rotation around the z axis followed by rotation around y axis and finally followed by rotation around x axis; namely,(2)
R
E
→
B
=
R
x
,
ϕ
R
y
,
θ
R
z
,
ψ
.Therefore, the transformation matrix from the ground coordinate system to the body coordinate system is given by(3)
R
B
→
E
=
R
E
→
B
T
.The specific expression is given by(4)
R
B
→
E
=
c
θ
c
ψ
s
ϕ
s
θ
c
ψ
-
c
ϕ
s
ψ
c
ϕ
s
θ
c
ψ
+
s
ϕ
s
ψ
c
θ
s
ψ
s
ϕ
s
θ
s
ψ
+
c
ϕ
c
ψ
c
ϕ
s
θ
s
ψ
-
s
ϕ
c
ψ
-
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
,where c
·
=
cos
⁡
(
·
) and s
·
=
sin
⁡
(
·
).The angular velocity componentsp, q, and r are the projection values on the body coordinate system of rotation angular velocity ω which denotes the rotation from the ground coordinate system to the body coordinate system. The relationships between angular velocity components and the attitude angle change rates are shown in Figure 2.Figure 2
The relationships between angular velocity components and the attitude angle change rate.The transformation matrix fromϕ
˙
θ
˙
ψ
˙
T to p
q
r
T is given by(5)
p
q
r
=
R
r
ϕ
˙
θ
˙
ψ
˙
,where(6)
R
r
=
1
0
-
sin
⁡
θ
0
cos
⁡
ϕ
sin
⁡
ϕ
cos
⁡
θ
0
-
sin
⁡
ϕ
cos
⁡
ϕ
cos
⁡
θ
.Around hovering position, R
r is assumed as a unit matrix [36, 40].


## 2.2. Dynamic Model

The dynamics model is composed of the rotational and translational motions. The rotational motion is fully actuated, while the translational motion is underactuated. In the body coordinate system, the rotational motion equations are derived according to the law of momentum theorem and gyroscopic effect of quadrotor; they are given by(7)
J
ω
˙
+
ω
×
J
ω
+
ω
×
0
0
J
r
Ω
r
=
M
B
,where J is the inertia matrix of quadrotor which is diagonal under the hypothesis of structure symmetry and the elements I
x, I
y, and I
z are, respectively, inertia matrices of x, y, and z axes. The last item on the left side of (7) represents the gyroscopic effect which is caused by the inertia of the rotors J
r and relative speed Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4, where Ω
i(i
=
1,2
,
3,4) represents the ith rotor speed. The aerodynamic force and moment produced by the ith rotor are directly proportional to the square of the rotor speed. The relationships are given by(8)
F
i
=
b
Ω
i
2
M
i
=
d
Ω
i
2
,where b and d are the aerodynamic force and moment constants, respectively. The moments acting on the quadrotor in the body coordinate system are given by(9)
M
B
=
l
·
b
-
Ω
2
2
+
Ω
4
2
l
·
b
Ω
1
2
-
Ω
3
2
d
Ω
1
2
-
Ω
2
2
+
Ω
3
2
-
Ω
4
2
,where l is the moment arm which represents the distance from the axis of a rotor to the center of quadrotor.The translational motion equations are obtained in the ground coordinate system by the method of Newton’s second law(10)
m
r
¨
=
0
0
m
g
T
+
R
F
B
,where r
=
x
y
z
T is the position of quadrotor in the ground coordinate system, m is the mass of quadrotor, g is the acceleration of gravity, and F
B is the total lift force acting on quadrotor in the body coordinate system; namely,(11)
F
B
=
0
0
-
b
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2
.


## 2.3. The Motion Equations of Quadrotor

Synthesizing the kinematics and dynamics models of quadrotor, the motion equations of quadrotor can be derived as follows:(12)
ϕ
¨
=
θ
˙
ψ
˙
I
y
-
I
z
I
x
-
J
r
I
x
θ
˙
Ω
r
+
L
I
x
U
2
,
θ
¨
=
ϕ
˙
ψ
˙
I
z
-
I
x
I
y
+
J
r
I
y
ϕ
˙
Ω
r
+
L
I
y
U
3
,
ψ
¨
=
ϕ
˙
θ
˙
I
x
-
I
y
I
z
+
1
I
z
U
4
,
x
¨
=
-
U
1
m
cos
⁡
ϕ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
ϕ
sin
⁡
ψ
,
y
¨
=
-
U
1
m
cos
⁡
ϕ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
ϕ
cos
⁡
ψ
,
z
¨
=
g
-
U
1
m
cos
⁡
ϕ
cos
⁡
θ
,where ϕ, θ, and φ are the attitude angle of quadrotor; x, y, and z are the position of quadrotor; U
1, U
2, U
3, and U
4 are the control input variables, which can be, respectively, calculated by U
1
=
b
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2, U
2
=
b
-
Ω
2
2
+
Ω
4
2, U
3
=
b
Ω
1
2
-
Ω
3
2, and U
4
=
d
Ω
1
2
-
Ω
2
2
+
Ω
3
2
-
Ω
4
2 in which b and d are the aerodynamic force and moment constants, respectively; I
x, I
y, and I
z are, respectively, inertia matrices of x, y, and z axes. J
r is the inertia matrix of the rotor and Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4 is rotors relative speed, where Ω
i(i
=
1,2
,
3,4) represents the ith rotor speed; l is the moment arm which represents the distance from the axis of a rotor to the center of quadrotor; m is the mass of quadrotor; and g is the acceleration of gravity.


## 3. The Calculation of Quasi-LPV Model and the Establishment of the Quadrotor System Transfer Function

### 3.1. Brief Introduction of the Quasi-LPV Systems

Quasi-LPV is an extension of the LPV. A LPV system is a linear time-varying system whose matrices depend on a vector of time-varying parameters which are either measured in real time or estimated using some known scheduling function. The advantage of this class of systems is that it embeds the system nonlinearities in the varying parameters which make the nonlinear system become a linear system with the parameter varying. In the pure LPV system, the varying parameters only depend on exogenous signals and, in the quasi-LPV system, the varying parameters can be functions of the states, inputs or outputs. The state space model of LPV system is given by(13a)
x
˙
=
A
α
t
x
+
B
α
t
u
(13b)
y
=
C
α
t
x
+
D
α
t
u
,where the state space matrices A
α
t, B
α
t, C
α
t, and D
α
t are the function of the time-varying parameter α
t; x is the state vector; y is the system output; u is the control input vector.


### 3.2. The Calculation of Quasi-LPV Model

Quasi-LPV method is used to transform the quadrotor nonlinear model represented by (12) into linear model in this paper. Because position loop of quadrotor is related to the attitude angle state variables, attitude loop of quadrotor is related to the first derivative of yaw angle and rotor rotating angular velocity Ω
r which is input, and quasi-LPV model is used to represent the quadrotor nonlinear motion equations in this paper. Unlike LPV system, the state space matrix of quasi-LPV system is the function of time-varying states ϕ, θ, ψ, ψ
˙ and input Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4. The state vector is chosen as X
=
x
,
y
,
z
,
x
˙
,
y
˙
,
z
˙
,
g
,
ϕ
,
θ
,
ψ
,
ϕ
˙
,
θ
˙
,
ψ
˙
T. The output vector is chosen as Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T. The control input vector is represented as U
=
U
1
U
2
U
3
U
4. Considering the hypothesis of quadrotor structure symmetry, that is, I
x is equal to I
y and is the half of I
z, the coefficient matrix of the state equation derived by quasi-LPV method is given by(14)
A
=
0
3
×
3
I
3
×
3
0
3
×
1
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
L
1
0
3
×
3
0
3
×
3
0
1
×
3
0
1
×
3
0
1
×
1
0
1
×
3
0
1
×
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
3
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
3
L
2
,
B
=
0
3
×
1
0
3
×
3
L
3
0
3
×
3
0
4
×
1
0
4
×
3
0
3
×
1
L
4
,
C
=
I
3
×
3
0
3
×
4
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
4
I
3
×
3
0
3
×
3
,
D
=
0
,where (15)
L
1
=
0
0
1
,
L
2
=
0
-
ψ
˙
-
J
r
I
x
Ω
r
0
ψ
˙
+
J
r
I
y
Ω
r
0
0
0
0
0
,
L
3
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
-
cos
⁡
ϕ
cos
⁡
θ
m
,
L
4
=
L
I
x
0
0
0
L
I
y
0
0
0
1
I
z
.


### 3.3. The Establishment of the Quadrotor System Transfer Function

UsingG
s
=
C
S
I
-
A
-
1
B
+
D, the transfer function matrix of quadrotor helicopter can be obtained as(16)
G
s
=
-
u
x
m
s
2
0
0
0
-
u
y
m
s
2
0
0
0
-
u
z
m
s
2
0
0
0
0
I
y
M
-
J
r
Ω
r
+
I
x
ψ
˙
s
M
0
0
J
r
Ω
r
+
I
y
ψ
˙
s
M
I
x
M
0
0
0
0
1
/
I
z
s
2
,where u
x
=
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ, u
y
=
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ, u
z
=
cos
⁡
ϕ
cos
⁡
θ, and M
=
J
r
2
Ω
r
2
+
I
x
I
y
ψ
˙
2
+
I
x
I
y
s
2
+
I
x
J
r
Ω
r
ψ
˙
+
I
y
J
r
Ω
r
ψ
˙.It can be seen from the transfer function matrix of quadrotor that the position loop is underactuated with three outputs and only one input; the attitude loop is fully actuated; the transfer function of position loop is time-varying, which varies with the attitude angle; yaw angle is decoupled with roll angle and pitch angle; roll angle is coupled with pitch angle. The coupling transfer function of roll angle and pitch angle can be extracted and written as(17)
G
ϕ
θ
=
I
y
M
-
J
r
Ω
r
+
I
x
ψ
˙
s
M
J
r
Ω
r
+
I
y
ψ
˙
s
M
I
x
M
.BecauseJ
r is much smaller than I
x and I
y, the gyroscopic effect item can be ignored. Under the hypothesis of quadrotor structure symmetry, I
x is equal to I
y. The simplified coupling transfer function of roll and pitch angles can be obtained by eliminating common factor of (17) and substituting I
y with I
x. It is given by (18)
G
ϕ
θ
=
1
I
x
ψ
˙
2
+
I
x
s
2
ψ
˙
s
I
x
ψ
˙
2
+
I
x
s
2
ψ
˙
s
I
x
ψ
˙
2
+
I
x
s
2
1
I
x
ψ
˙
2
+
I
x
s
2
.In this paper, the control design is conducted only inψ
˙
=
0. In this situation, (18) can be rewritten as(19)
G
ϕ
θ
=
1
I
x
s
2
0
0
1
I
x
s
2
.It can be seen from (19), that the roll angle and pitch angle are decoupled. Therefore, the attitude angle stability of quadrotor can be realized by individually designed controllers for three attitude angles. For such a simple second-order system represented by yaw angle loop in (16) and (19), the internal model controller can be used. Only two controller parameters need to be adjusted in IMC.The position transfer function in (12) can be extracted and written as(20)
x
¨
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
U
1
,
y
¨
=
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
U
1
,
z
¨
=
g
-
cos
⁡
ϕ
cos
⁡
θ
m
U
1
.If the virtual inputs are defined as(21)
V
x
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
U
1
,
V
y
=
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
U
1
,
V
z
=
g
-
cos
⁡
ϕ
cos
⁡
θ
m
U
1
,then the transfer function of position loop can be written as(22)
x
s
V
x
=
1
s
2
,
x
s
V
y
=
1
s
2
,
x
s
V
z
=
1
s
2
.To sum up, in six inputsU
=
V
x
,
V
y
,
V
z
,
U
2
,
U
3
,
U
4
T and six outputs Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T, the transfer function of quadrotor is given by(23)
G
U
→
Y
=
1
s
2
0
0
0
0
0
0
1
s
2
0
0
0
0
0
0
1
s
2
0
0
0
0
0
0
1
I
x
s
2
0
0
0
0
0
0
1
I
x
s
2
0
0
0
0
0
0
1
I
z
s
2
.


## 3.1. Brief Introduction of the Quasi-LPV Systems

Quasi-LPV is an extension of the LPV. A LPV system is a linear time-varying system whose matrices depend on a vector of time-varying parameters which are either measured in real time or estimated using some known scheduling function. The advantage of this class of systems is that it embeds the system nonlinearities in the varying parameters which make the nonlinear system become a linear system with the parameter varying. In the pure LPV system, the varying parameters only depend on exogenous signals and, in the quasi-LPV system, the varying parameters can be functions of the states, inputs or outputs. The state space model of LPV system is given by(13a)
x
˙
=
A
α
t
x
+
B
α
t
u
(13b)
y
=
C
α
t
x
+
D
α
t
u
,where the state space matrices A
α
t, B
α
t, C
α
t, and D
α
t are the function of the time-varying parameter α
t; x is the state vector; y is the system output; u is the control input vector.


## 3.2. The Calculation of Quasi-LPV Model

Quasi-LPV method is used to transform the quadrotor nonlinear model represented by (12) into linear model in this paper. Because position loop of quadrotor is related to the attitude angle state variables, attitude loop of quadrotor is related to the first derivative of yaw angle and rotor rotating angular velocity Ω
r which is input, and quasi-LPV model is used to represent the quadrotor nonlinear motion equations in this paper. Unlike LPV system, the state space matrix of quasi-LPV system is the function of time-varying states ϕ, θ, ψ, ψ
˙ and input Ω
r
=
-
Ω
1
+
Ω
2
-
Ω
3
+
Ω
4. The state vector is chosen as X
=
x
,
y
,
z
,
x
˙
,
y
˙
,
z
˙
,
g
,
ϕ
,
θ
,
ψ
,
ϕ
˙
,
θ
˙
,
ψ
˙
T. The output vector is chosen as Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T. The control input vector is represented as U
=
U
1
U
2
U
3
U
4. Considering the hypothesis of quadrotor structure symmetry, that is, I
x is equal to I
y and is the half of I
z, the coefficient matrix of the state equation derived by quasi-LPV method is given by(14)
A
=
0
3
×
3
I
3
×
3
0
3
×
1
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
L
1
0
3
×
3
0
3
×
3
0
1
×
3
0
1
×
3
0
1
×
1
0
1
×
3
0
1
×
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
3
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
3
L
2
,
B
=
0
3
×
1
0
3
×
3
L
3
0
3
×
3
0
4
×
1
0
4
×
3
0
3
×
1
L
4
,
C
=
I
3
×
3
0
3
×
4
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
4
I
3
×
3
0
3
×
3
,
D
=
0
,where (15)
L
1
=
0
0
1
,
L
2
=
0
-
ψ
˙
-
J
r
I
x
Ω
r
0
ψ
˙
+
J
r
I
y
Ω
r
0
0
0
0
0
,
L
3
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
-
cos
⁡
ϕ
cos
⁡
θ
m
,
L
4
=
L
I
x
0
0
0
L
I
y
0
0
0
1
I
z
.


## 3.3. The Establishment of the Quadrotor System Transfer Function

UsingG
s
=
C
S
I
-
A
-
1
B
+
D, the transfer function matrix of quadrotor helicopter can be obtained as(16)
G
s
=
-
u
x
m
s
2
0
0
0
-
u
y
m
s
2
0
0
0
-
u
z
m
s
2
0
0
0
0
I
y
M
-
J
r
Ω
r
+
I
x
ψ
˙
s
M
0
0
J
r
Ω
r
+
I
y
ψ
˙
s
M
I
x
M
0
0
0
0
1
/
I
z
s
2
,where u
x
=
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ, u
y
=
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ, u
z
=
cos
⁡
ϕ
cos
⁡
θ, and M
=
J
r
2
Ω
r
2
+
I
x
I
y
ψ
˙
2
+
I
x
I
y
s
2
+
I
x
J
r
Ω
r
ψ
˙
+
I
y
J
r
Ω
r
ψ
˙.It can be seen from the transfer function matrix of quadrotor that the position loop is underactuated with three outputs and only one input; the attitude loop is fully actuated; the transfer function of position loop is time-varying, which varies with the attitude angle; yaw angle is decoupled with roll angle and pitch angle; roll angle is coupled with pitch angle. The coupling transfer function of roll angle and pitch angle can be extracted and written as(17)
G
ϕ
θ
=
I
y
M
-
J
r
Ω
r
+
I
x
ψ
˙
s
M
J
r
Ω
r
+
I
y
ψ
˙
s
M
I
x
M
.BecauseJ
r is much smaller than I
x and I
y, the gyroscopic effect item can be ignored. Under the hypothesis of quadrotor structure symmetry, I
x is equal to I
y. The simplified coupling transfer function of roll and pitch angles can be obtained by eliminating common factor of (17) and substituting I
y with I
x. It is given by (18)
G
ϕ
θ
=
1
I
x
ψ
˙
2
+
I
x
s
2
ψ
˙
s
I
x
ψ
˙
2
+
I
x
s
2
ψ
˙
s
I
x
ψ
˙
2
+
I
x
s
2
1
I
x
ψ
˙
2
+
I
x
s
2
.In this paper, the control design is conducted only inψ
˙
=
0. In this situation, (18) can be rewritten as(19)
G
ϕ
θ
=
1
I
x
s
2
0
0
1
I
x
s
2
.It can be seen from (19), that the roll angle and pitch angle are decoupled. Therefore, the attitude angle stability of quadrotor can be realized by individually designed controllers for three attitude angles. For such a simple second-order system represented by yaw angle loop in (16) and (19), the internal model controller can be used. Only two controller parameters need to be adjusted in IMC.The position transfer function in (12) can be extracted and written as(20)
x
¨
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
U
1
,
y
¨
=
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
U
1
,
z
¨
=
g
-
cos
⁡
ϕ
cos
⁡
θ
m
U
1
.If the virtual inputs are defined as(21)
V
x
=
-
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
m
U
1
,
V
y
=
-
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
m
U
1
,
V
z
=
g
-
cos
⁡
ϕ
cos
⁡
θ
m
U
1
,then the transfer function of position loop can be written as(22)
x
s
V
x
=
1
s
2
,
x
s
V
y
=
1
s
2
,
x
s
V
z
=
1
s
2
.To sum up, in six inputsU
=
V
x
,
V
y
,
V
z
,
U
2
,
U
3
,
U
4
T and six outputs Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T, the transfer function of quadrotor is given by(23)
G
U
→
Y
=
1
s
2
0
0
0
0
0
0
1
s
2
0
0
0
0
0
0
1
s
2
0
0
0
0
0
0
1
I
x
s
2
0
0
0
0
0
0
1
I
x
s
2
0
0
0
0
0
0
1
I
z
s
2
.


## 4. The IMC Design and the Calculation of the Desired Roll Angle, Pitch Angle, and Total Thrust

### 4.1. Quadrotor Control Structure

The quadrotor control structure adopted in this paper is shown in Figure3. The inner and outer loops both adopt the internal model control.Figure 3
Quadrotor control structure.


### 4.2. IMC Design Principle

The general structure of IMC [35] is shown in Figure 3, in which G
P
(
s
) is the controlled object, G
m
(
s
) is the controlled object model, G
I
M
C
(
s
) is the feedforward item of IMC, G
F
(
s
) is the feedback filter, y
p and u are the output of the controlled object and controlled quantity, y
m is the output of controlled object model, r is a given value (reference trajectory), and d is the external disturbance. In IMC system, G
I
M
C
(
s
) is mainly used for the reliable tracking for a given input r and G
F
(
s
) is used to adjust the robustness and reject disturbance (see Figure 4).Figure 4
The general structure of IMC.According to the differentG
F
(
s
) values, IMC system can be called one degree of freedom IMC system or two degrees of freedom IMC system. When G
F
(
s
)
=
1, IMC system is called One degree of freedom IMC system, otherwise it is called two degrees of freedom IMC system. The basic structure of IMC which is proposed by Garcia and Morari in 1982 is shown in Figure 5. It is essentially one degree of freedom IMC system.Figure 5
The basic structure of IMC.In Figure5, G
d is the transfer function of disturbance channel, and usually G
d
=
1; the inner of the dotted box is the internal structure of the whole control system and the process model is also included besides the controller G
I
M
C in the inner of the dotted box; therefore, this control structure shown in Figure 5 is entitled internal model control. The equivalent structure of IMC can be derived by doing equivalent transformation for Figure 5, and it is shown in Figure 6. It can be seen from Figure 6 that it is a unit feedback control system.Figure 6
The equivalent structure of IMC.It can be seen from Figure6 that the relation between the feedback controller and IMC is given by(24)
G
C
s
=
G
I
M
C
s
I
-
G
I
M
C
s
G
m
s
.According to Figure6, the closed-loop transfer functions under the action of control and disturbance are respectively given by(25)
W
r
s
=
Y
s
R
s
=
G
c
s
G
p
s
I
+
G
c
s
G
P
s
=
G
I
M
C
s
G
p
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
,
(26)
W
d
s
=
Y
s
D
s
=
G
d
s
I
+
G
c
s
G
P
s
=
I
-
G
I
M
C
s
G
m
s
G
d
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
.The closed-loop transfer function for one degree of freedom IMC system is derived as(27)
Y
s
=
G
c
s
G
p
s
I
+
G
c
s
G
p
s
R
s
+
G
d
s
I
+
G
c
s
G
p
s
D
s
=
G
I
M
C
s
G
p
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
R
s
+
I
-
G
I
M
C
s
G
m
s
G
d
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
D
s
.From Figure 6, feedback signal is straightforwardly derived as(28)
D
~
s
=
G
p
s
-
G
m
s
U
s
+
G
d
s
D
s
.If the model is accurate, that is,G
p
(
s
)
=
G
m
(
s
), (27) and (28), respectively, become (29) and (30); namely,(29)
Y
s
=
G
I
M
C
s
G
p
s
R
s
+
I
-
G
I
M
C
s
G
m
s
G
d
s
D
s
,
(30)
D
~
s
=
G
d
s
D
s
.After introducing the controlled object model, it can be seen from (29) that the output of the controlled object only contains the product items but not division items. Therefore, IMC has good tracking performance. It can be seen from (30) that the amount of feedback becomes disturbance estimator G
d
(
s
)
D
(
s
) from full feedback of output. At the moment, IMC is equivalent to a disturbance estimator, and G
I
M
C
(
s
) can be designed to fully compensate the disturbance effect on output. Therefore, IMC has good disturbance rejection performance. When the model does not completely match with the controlled object, that is, G
p
(
s
)
≠
G
m
(
s
), the model error exists, the amount of feedback includes some information of the model mismatch, and thus the designed G
I
M
C
(
s
) can compensate the amount of model mismatch; that is to say, the IMC is robust to the model mismatch.In conclusion, IMC actually belongs to a kind of robust control. It has good tracking performance and ability to reject disturbance and has robustness to the model mismatch. When there is no feedback filter, that is, one degree of freedom IMC system, the tracking performance, disturbance rejection performance, and robustness can be traded off only by feedforward item of IMC. When there is feedback filter, that is, two degrees of freedom IMC system, feedback signal is given as(31)
D
~
s
=
G
p
s
-
G
m
s
G
F
s
U
s
+
G
d
s
D
s
G
F
s
.It can be seen from (31) that the design of G
I
M
C
(
s
) only needs to consider tracking performance which is the rapidity of response and the intensity of control, and robustness and disturbance rejection performance is adjusted by the feedback filter G
F
(
s
).


#### 4.2.1. The Design Procedure ofG
I
M
C
(
s
)

This paper uses the modified offset method [41] for the G
I
M
C
(
s
) design. The principle of the offset method is as follows: supposing the controlled object is stable, firstly use G
I
M
C
(
s
) to eliminate the minimum phase section of G
m
(
s
), and then add a first-order or second-order feedforward filter to regulate the rapidity of the response, the control intensity, and robustness and disturbance rejection performance. The advantage of this method is that the design and adjustment are intuitive and simple, and it can be applied to continuous and discrete systems. But for nonminimum phase object, due to the instable zero of G
m that cannot be eliminated, the response performance of the system cannot be guaranteed; that is to say, overshoot and negative overshoot likely appear. The design procedure of the modified offset method is as follows: firstly do decomposition for the controlled object model which is decomposed into minimum phase part G
m
-
(
s
) and all-pass filter part G
m
+
(
s
), and then use the offset method to design G
I
M
C
(
s
) of minimum phase part. The specific design procedure is given as follows.Step 1 (the decomposition of the controlled object modelG
m
(
s
)).
G
m
(
s
) can be decomposed into two items G
m
+
(
s
) and G
m
-
(
s
) and can be written as(32)
G
m
s
=
G
m
+
s
G
m
-
s
,where G
m
+
(
s
) is an all-pass filter which contains all time delays and zero in right half plane; G
m
-
(
s
) is a transfer function with minimum phase characteristic which is stable and does not contain the prediction term.Step 2 (IMC design).
IMC is designed as(33)
G
I
M
C
s
=
G
-
1
m
-
s
f
s
,
(34)
f
s
=
1
1
+
a
s
n
.
In (34), f
(
s
) is the low pass filter; the purpose of adding a filter into the controller is to guarantee the stability and robustness of the system. The principle of choosing f
(
s
) is making G
I
M
C
(
s
) rational; that is, G
I
M
C
(
s
) can be realized through the decomposition of (32) and filter choice of (34). a is filter parameters; it is the only parameter in G
I
M
C
(
s
) design.
In the filter design,n should be large enough to ensure that G
I
M
C
(
s
) is rational. The greater a is, the slower the closed-loop output response is and the softer the operating variables change. In addition, a is approximately proportional to the closed-loop bandwidth. Therefore, an initial estimate of the filter parameters can be got and, in practice, a can be adjusted on-line as needed.


#### 4.2.2. The Design ofG
F
(
s
)

It can be seen from Figure6 and (28), when there is no model mismatch and disturbance, that the feedback signal D
~
(
s
)
=
0, the original system is equal to open-loop system, and only the feedforward controller G
I
M
C
(
s
) works at this time. Therefore, the feedback filter G
F
(
s
) is set specifically for model mismatch and disturbance, the most common structure for G
F
(
s
) is first-order low-pass filter, and it can be expressed as(35)
G
F
s
=
1
λ
s
+
1
.Equation (35) only has one adjustable parameter; the structure is simple, but performance improvement effect is remarkable and there is a lot of mature conclusions. In the multivariable time-delay systems, λ is often set as the half of loop delay time [35].


### 4.3. IMC Design for the Attitude and Position Loops

According to the feedforward and feedback controllers of IMC design method introduced in Section4.2, when the position and attitude controlled object vector is Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T, the feedforward controller G
I
M
C
(
s
) is given as(36)
G
=
s
2
1
+
a
1
s
2
,
s
2
1
+
a
2
s
2
,
s
2
1
+
a
3
s
2
,
s
2
1
+
a
4
s
2
,
s
2
1
+
a
5
s
2
,
s
2
1
+
a
6
s
2
T
,where a
1, a
2, a
3, a
4, a
5, and a
6 are, respectively, controller design parameters corresponding to the controlled variables x, y, z, ϕ, θ, and ψ loops.


### 4.4. The Calculation of the Desired Roll Angle, Pitch Angle, and Total Thrust

Equation (21) can be transformed as (37)
-
m
V
x
U
1
=
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
,
(38)
-
m
V
y
U
1
=
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
,
(39)
-
m
V
z
-
g
U
1
=
cos
⁡
ϕ
cos
⁡
θ
.Subtracting the expression derived by multiplying (38) with cos
⁡
ψ from the expression derived by multiplying (37) with sin
⁡
ψ, -
(
m
V
x
/
U
1
)
·
sin
⁡
ψ
+
(
m
V
y
/
U
1
)
·
cos
⁡
ψ
=
sin
⁡
ϕ can be obtained; further, the desired value of pitch angle can be obtained as (40)
ϕ
d
=
a
r
c
s
i
n
-
m
V
x
U
1
·
sin
⁡
ψ
+
m
V
y
U
1
·
cos
⁡
ψ
.Adding the expression derived by multiplying (37) with cos
⁡
ψ and the expression derived by multiplying (38) with sin
⁡
ψ together, (41) can be obtained as(41)
-
m
V
x
U
1
·
cos
⁡
ψ
-
m
V
y
U
1
·
sin
⁡
ψ
=
cos
⁡
ϕ
sin
⁡
θ
.tan
⁡
θ
=
(
V
x
cos
⁡
ψ
+
V
y
sin
⁡
ψ
)
/
(
V
z
-
g
) can be derived by (41) dividing (39); further, the desired value of roll angle can be obtained as(42)
θ
d
=
a
r
c
t
a
n
V
x
cos
⁡
ψ
+
V
y
sin
⁡
ψ
V
z
-
g
.The total thrust can be obtained by adding the both sides square of (37), (38), and (39) together; it can be written as(43)
U
1
=
m
V
x
2
+
V
y
2
+
V
z
-
g
2
.To sum up, after the position loop controllersV
x, V
y, and V
z are got, the desired values of roll and pitch angles and the total thrust can be calculated by using (40), (42), and (43), which can complete the join of attitude loop and position loop. Finally, the control for quadrotor position and yaw angle can be realized by using (36), (40), (42), and (43) and quadrotor trajectory tracking can be further realized.


## 4.1. Quadrotor Control Structure

The quadrotor control structure adopted in this paper is shown in Figure3. The inner and outer loops both adopt the internal model control.Figure 3
Quadrotor control structure.


## 4.2. IMC Design Principle

The general structure of IMC [35] is shown in Figure 3, in which G
P
(
s
) is the controlled object, G
m
(
s
) is the controlled object model, G
I
M
C
(
s
) is the feedforward item of IMC, G
F
(
s
) is the feedback filter, y
p and u are the output of the controlled object and controlled quantity, y
m is the output of controlled object model, r is a given value (reference trajectory), and d is the external disturbance. In IMC system, G
I
M
C
(
s
) is mainly used for the reliable tracking for a given input r and G
F
(
s
) is used to adjust the robustness and reject disturbance (see Figure 4).Figure 4
The general structure of IMC.According to the differentG
F
(
s
) values, IMC system can be called one degree of freedom IMC system or two degrees of freedom IMC system. When G
F
(
s
)
=
1, IMC system is called One degree of freedom IMC system, otherwise it is called two degrees of freedom IMC system. The basic structure of IMC which is proposed by Garcia and Morari in 1982 is shown in Figure 5. It is essentially one degree of freedom IMC system.Figure 5
The basic structure of IMC.In Figure5, G
d is the transfer function of disturbance channel, and usually G
d
=
1; the inner of the dotted box is the internal structure of the whole control system and the process model is also included besides the controller G
I
M
C in the inner of the dotted box; therefore, this control structure shown in Figure 5 is entitled internal model control. The equivalent structure of IMC can be derived by doing equivalent transformation for Figure 5, and it is shown in Figure 6. It can be seen from Figure 6 that it is a unit feedback control system.Figure 6
The equivalent structure of IMC.It can be seen from Figure6 that the relation between the feedback controller and IMC is given by(24)
G
C
s
=
G
I
M
C
s
I
-
G
I
M
C
s
G
m
s
.According to Figure6, the closed-loop transfer functions under the action of control and disturbance are respectively given by(25)
W
r
s
=
Y
s
R
s
=
G
c
s
G
p
s
I
+
G
c
s
G
P
s
=
G
I
M
C
s
G
p
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
,
(26)
W
d
s
=
Y
s
D
s
=
G
d
s
I
+
G
c
s
G
P
s
=
I
-
G
I
M
C
s
G
m
s
G
d
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
.The closed-loop transfer function for one degree of freedom IMC system is derived as(27)
Y
s
=
G
c
s
G
p
s
I
+
G
c
s
G
p
s
R
s
+
G
d
s
I
+
G
c
s
G
p
s
D
s
=
G
I
M
C
s
G
p
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
R
s
+
I
-
G
I
M
C
s
G
m
s
G
d
s
I
+
G
I
M
C
s
G
p
s
-
G
m
s
D
s
.From Figure 6, feedback signal is straightforwardly derived as(28)
D
~
s
=
G
p
s
-
G
m
s
U
s
+
G
d
s
D
s
.If the model is accurate, that is,G
p
(
s
)
=
G
m
(
s
), (27) and (28), respectively, become (29) and (30); namely,(29)
Y
s
=
G
I
M
C
s
G
p
s
R
s
+
I
-
G
I
M
C
s
G
m
s
G
d
s
D
s
,
(30)
D
~
s
=
G
d
s
D
s
.After introducing the controlled object model, it can be seen from (29) that the output of the controlled object only contains the product items but not division items. Therefore, IMC has good tracking performance. It can be seen from (30) that the amount of feedback becomes disturbance estimator G
d
(
s
)
D
(
s
) from full feedback of output. At the moment, IMC is equivalent to a disturbance estimator, and G
I
M
C
(
s
) can be designed to fully compensate the disturbance effect on output. Therefore, IMC has good disturbance rejection performance. When the model does not completely match with the controlled object, that is, G
p
(
s
)
≠
G
m
(
s
), the model error exists, the amount of feedback includes some information of the model mismatch, and thus the designed G
I
M
C
(
s
) can compensate the amount of model mismatch; that is to say, the IMC is robust to the model mismatch.In conclusion, IMC actually belongs to a kind of robust control. It has good tracking performance and ability to reject disturbance and has robustness to the model mismatch. When there is no feedback filter, that is, one degree of freedom IMC system, the tracking performance, disturbance rejection performance, and robustness can be traded off only by feedforward item of IMC. When there is feedback filter, that is, two degrees of freedom IMC system, feedback signal is given as(31)
D
~
s
=
G
p
s
-
G
m
s
G
F
s
U
s
+
G
d
s
D
s
G
F
s
.It can be seen from (31) that the design of G
I
M
C
(
s
) only needs to consider tracking performance which is the rapidity of response and the intensity of control, and robustness and disturbance rejection performance is adjusted by the feedback filter G
F
(
s
).


### 4.2.1. The Design Procedure ofG
I
M
C
(
s
)

This paper uses the modified offset method [41] for the G
I
M
C
(
s
) design. The principle of the offset method is as follows: supposing the controlled object is stable, firstly use G
I
M
C
(
s
) to eliminate the minimum phase section of G
m
(
s
), and then add a first-order or second-order feedforward filter to regulate the rapidity of the response, the control intensity, and robustness and disturbance rejection performance. The advantage of this method is that the design and adjustment are intuitive and simple, and it can be applied to continuous and discrete systems. But for nonminimum phase object, due to the instable zero of G
m that cannot be eliminated, the response performance of the system cannot be guaranteed; that is to say, overshoot and negative overshoot likely appear. The design procedure of the modified offset method is as follows: firstly do decomposition for the controlled object model which is decomposed into minimum phase part G
m
-
(
s
) and all-pass filter part G
m
+
(
s
), and then use the offset method to design G
I
M
C
(
s
) of minimum phase part. The specific design procedure is given as follows.Step 1 (the decomposition of the controlled object modelG
m
(
s
)).
G
m
(
s
) can be decomposed into two items G
m
+
(
s
) and G
m
-
(
s
) and can be written as(32)
G
m
s
=
G
m
+
s
G
m
-
s
,where G
m
+
(
s
) is an all-pass filter which contains all time delays and zero in right half plane; G
m
-
(
s
) is a transfer function with minimum phase characteristic which is stable and does not contain the prediction term.Step 2 (IMC design).
IMC is designed as(33)
G
I
M
C
s
=
G
-
1
m
-
s
f
s
,
(34)
f
s
=
1
1
+
a
s
n
.
In (34), f
(
s
) is the low pass filter; the purpose of adding a filter into the controller is to guarantee the stability and robustness of the system. The principle of choosing f
(
s
) is making G
I
M
C
(
s
) rational; that is, G
I
M
C
(
s
) can be realized through the decomposition of (32) and filter choice of (34). a is filter parameters; it is the only parameter in G
I
M
C
(
s
) design.
In the filter design,n should be large enough to ensure that G
I
M
C
(
s
) is rational. The greater a is, the slower the closed-loop output response is and the softer the operating variables change. In addition, a is approximately proportional to the closed-loop bandwidth. Therefore, an initial estimate of the filter parameters can be got and, in practice, a can be adjusted on-line as needed.


### 4.2.2. The Design ofG
F
(
s
)

It can be seen from Figure6 and (28), when there is no model mismatch and disturbance, that the feedback signal D
~
(
s
)
=
0, the original system is equal to open-loop system, and only the feedforward controller G
I
M
C
(
s
) works at this time. Therefore, the feedback filter G
F
(
s
) is set specifically for model mismatch and disturbance, the most common structure for G
F
(
s
) is first-order low-pass filter, and it can be expressed as(35)
G
F
s
=
1
λ
s
+
1
.Equation (35) only has one adjustable parameter; the structure is simple, but performance improvement effect is remarkable and there is a lot of mature conclusions. In the multivariable time-delay systems, λ is often set as the half of loop delay time [35].


## 4.2.1. The Design Procedure ofG
I
M
C
(
s
)

This paper uses the modified offset method [41] for the G
I
M
C
(
s
) design. The principle of the offset method is as follows: supposing the controlled object is stable, firstly use G
I
M
C
(
s
) to eliminate the minimum phase section of G
m
(
s
), and then add a first-order or second-order feedforward filter to regulate the rapidity of the response, the control intensity, and robustness and disturbance rejection performance. The advantage of this method is that the design and adjustment are intuitive and simple, and it can be applied to continuous and discrete systems. But for nonminimum phase object, due to the instable zero of G
m that cannot be eliminated, the response performance of the system cannot be guaranteed; that is to say, overshoot and negative overshoot likely appear. The design procedure of the modified offset method is as follows: firstly do decomposition for the controlled object model which is decomposed into minimum phase part G
m
-
(
s
) and all-pass filter part G
m
+
(
s
), and then use the offset method to design G
I
M
C
(
s
) of minimum phase part. The specific design procedure is given as follows.Step 1 (the decomposition of the controlled object modelG
m
(
s
)).
G
m
(
s
) can be decomposed into two items G
m
+
(
s
) and G
m
-
(
s
) and can be written as(32)
G
m
s
=
G
m
+
s
G
m
-
s
,where G
m
+
(
s
) is an all-pass filter which contains all time delays and zero in right half plane; G
m
-
(
s
) is a transfer function with minimum phase characteristic which is stable and does not contain the prediction term.Step 2 (IMC design).
IMC is designed as(33)
G
I
M
C
s
=
G
-
1
m
-
s
f
s
,
(34)
f
s
=
1
1
+
a
s
n
.
In (34), f
(
s
) is the low pass filter; the purpose of adding a filter into the controller is to guarantee the stability and robustness of the system. The principle of choosing f
(
s
) is making G
I
M
C
(
s
) rational; that is, G
I
M
C
(
s
) can be realized through the decomposition of (32) and filter choice of (34). a is filter parameters; it is the only parameter in G
I
M
C
(
s
) design.
In the filter design,n should be large enough to ensure that G
I
M
C
(
s
) is rational. The greater a is, the slower the closed-loop output response is and the softer the operating variables change. In addition, a is approximately proportional to the closed-loop bandwidth. Therefore, an initial estimate of the filter parameters can be got and, in practice, a can be adjusted on-line as needed.


## 4.2.2. The Design ofG
F
(
s
)

It can be seen from Figure6 and (28), when there is no model mismatch and disturbance, that the feedback signal D
~
(
s
)
=
0, the original system is equal to open-loop system, and only the feedforward controller G
I
M
C
(
s
) works at this time. Therefore, the feedback filter G
F
(
s
) is set specifically for model mismatch and disturbance, the most common structure for G
F
(
s
) is first-order low-pass filter, and it can be expressed as(35)
G
F
s
=
1
λ
s
+
1
.Equation (35) only has one adjustable parameter; the structure is simple, but performance improvement effect is remarkable and there is a lot of mature conclusions. In the multivariable time-delay systems, λ is often set as the half of loop delay time [35].


## 4.3. IMC Design for the Attitude and Position Loops

According to the feedforward and feedback controllers of IMC design method introduced in Section4.2, when the position and attitude controlled object vector is Y
=
x
,
y
,
z
,
ϕ
,
θ
,
ψ
T, the feedforward controller G
I
M
C
(
s
) is given as(36)
G
=
s
2
1
+
a
1
s
2
,
s
2
1
+
a
2
s
2
,
s
2
1
+
a
3
s
2
,
s
2
1
+
a
4
s
2
,
s
2
1
+
a
5
s
2
,
s
2
1
+
a
6
s
2
T
,where a
1, a
2, a
3, a
4, a
5, and a
6 are, respectively, controller design parameters corresponding to the controlled variables x, y, z, ϕ, θ, and ψ loops.


## 4.4. The Calculation of the Desired Roll Angle, Pitch Angle, and Total Thrust

Equation (21) can be transformed as (37)
-
m
V
x
U
1
=
cos
⁡
φ
sin
⁡
θ
cos
⁡
ψ
+
sin
⁡
φ
sin
⁡
ψ
,
(38)
-
m
V
y
U
1
=
cos
⁡
φ
sin
⁡
θ
sin
⁡
ψ
-
sin
⁡
φ
cos
⁡
ψ
,
(39)
-
m
V
z
-
g
U
1
=
cos
⁡
ϕ
cos
⁡
θ
.Subtracting the expression derived by multiplying (38) with cos
⁡
ψ from the expression derived by multiplying (37) with sin
⁡
ψ, -
(
m
V
x
/
U
1
)
·
sin
⁡
ψ
+
(
m
V
y
/
U
1
)
·
cos
⁡
ψ
=
sin
⁡
ϕ can be obtained; further, the desired value of pitch angle can be obtained as (40)
ϕ
d
=
a
r
c
s
i
n
-
m
V
x
U
1
·
sin
⁡
ψ
+
m
V
y
U
1
·
cos
⁡
ψ
.Adding the expression derived by multiplying (37) with cos
⁡
ψ and the expression derived by multiplying (38) with sin
⁡
ψ together, (41) can be obtained as(41)
-
m
V
x
U
1
·
cos
⁡
ψ
-
m
V
y
U
1
·
sin
⁡
ψ
=
cos
⁡
ϕ
sin
⁡
θ
.tan
⁡
θ
=
(
V
x
cos
⁡
ψ
+
V
y
sin
⁡
ψ
)
/
(
V
z
-
g
) can be derived by (41) dividing (39); further, the desired value of roll angle can be obtained as(42)
θ
d
=
a
r
c
t
a
n
V
x
cos
⁡
ψ
+
V
y
sin
⁡
ψ
V
z
-
g
.The total thrust can be obtained by adding the both sides square of (37), (38), and (39) together; it can be written as(43)
U
1
=
m
V
x
2
+
V
y
2
+
V
z
-
g
2
.To sum up, after the position loop controllersV
x, V
y, and V
z are got, the desired values of roll and pitch angles and the total thrust can be calculated by using (40), (42), and (43), which can complete the join of attitude loop and position loop. Finally, the control for quadrotor position and yaw angle can be realized by using (36), (40), (42), and (43) and quadrotor trajectory tracking can be further realized.


## 5. Simulation Results

To test the trajectory tracking performance, the constant interference rejection performance, and robustness to parameter uncertainty of the control strategy proposed by this paper, the trajectory tracking experiments for the quadrotor flight path proposed in the literature in [42, 43] were carried out and constant interference and parameter uncertainty were added in the constant interference rejection performance and robustness tests. The quadrotor parameters used in this paper are as follows: m
=
0.4794 kg, g
=
9.81m/s2, l
=
0.225 m, I
x
=
I
y
=
0.0086kg·m2, I
z
=
0.0172kg·m2, J
r
o
t
o
r
=
3.7404
×
1
0
-
5 kg·m2, b
=
3.13
×
1
0
-
5, and d
=
9
×
1
0
-
7. In simulation, constant wind and ±30% uncertainties of inertia parameter were, respectively, added into three kinds of trajectory to test constant interference rejection performance and robustness to parameter uncertainty of the control strategy proposed in this paper. Constant wind was introduced at different times. At t
=
5s, constant wind in x direction w
x
=
0.5 m/s was introduced; at t
=
15s, constant wind in y direction w
y
=
0.5 m/s was introduced; while at t
=
2
5s, constant wind in z direction w
z
=
0.5 m/s was added. In this paper, the IMC parameters of position and attitude loop are the same; the parameters of feedforward controller G
I
M
C
(
s
) are as follows: a
1
=
a
2
=
a
3
=
a
4
=
a
5
=
a
6
=
0.2. The literature in [44] pointed out that the delay due to the electronic speed controller (ESC) driver is 0.2 s; therefore, the parameters of the feedback controller G
F
(
s
) in position and attitude loop are all chosen as λ
=
0.1.The reference trajectory used in the literature in [42] is given by(44a)
x
d
=
1
2
cos
⁡
t
2
,
(44b)
y
d
=
1
2
sin
⁡
t
2
,
(44c)
z
d
=
-
1
-
t
10
,
(44d)
ψ
d
=
π
3
,where the value of z
d is different from that in the literature in [42]. Because z axis direction in this paper is downward, the coordinate value of z axis is negative when quadrotor flies upward.Figures7, 8, and 9 are trajectory tracking, position, and attitude comparison diagrams with disturbance, respectively. Figure 10 is quadrotor trajectory tracking comparison diagrams with parameters uncertainty.Figure 7
Trajectory tracking with constant disturbance.Figure 8
Position with constant disturbance.Figure 9
Attitude angle with constant disturbance.Figure 10
Trajectory tracking with parameters uncertainty.The reference trajectory used in the literature in [43] is described as(45a)
x
d
=
1
2
cos
⁡
π
t
20
,
(45b)
y
d
=
1
2
sin
⁡
π
t
20
,
(45c)
z
d
=
3
-
2
cos
⁡
π
t
20
,
(45d)
ψ
d
=
0
.Similarly, Figures11, 12, and 13 are, respectively, trajectory tracking, position, and attitude comparison diagrams with disturbance. Figure 14 is quadrotor trajectory tracking comparison diagrams with parameters uncertainty.Figure 11
Trajectory tracking with constant disturbance.Figure 12
Position with constant disturbance.Figure 13
Attitude angle with constant disturbance.Figure 14
Trajectory tracking with parameters uncertainty.It can be clearly seen from Figures7 and 11 that a small disturbance overshoot firstly appears in x direction, but soon it is suppressed, and then disturbance overshoot and suppression process appear in y direction and finally are followed by z direction. Figures 8 and 12 show disturbance overshoot in x direction appear at t
=
5s, disturbance overshoot in y direction appear at t
=
15s, and disturbance overshoot in z direction appear at t
=
25s. It can be seen from Figures 9 and 13 that when the disturbances in x, y, and z directions are added, the roll angle and pitch angle generate corresponding change, but yaw angle does not change. This suggests that the expected values of the roll and pitch angle loops depend on the output of position controller. But the yaw angle is independent of position loop. It can be evidenced by (40), (42), (43), and (16). It can be seen from Figures 10 and 14 that IMC has strong robustness to parameter uncertainty. It can be seen from Figures 7 to 14, under the action of the controller proposed in this paper, that quadrotor can well track given trajectory in the literature in [42, 43] and has a strong constant interference rejection ability and robustness to parameter uncertainty.The above results of trajectory tracking experiments were obtained when gyroscope effect and sensor error are neglected. In order to test the robustness properties against neglected gyroscope effect and sensor error, the trajectory tracking experiments for the given trajectory in the literature in [42] are respectively done when gyroscope effect and sensor error are considered. Figure 15 is the comparison diagram between the reference trajectory and tracking trajectory with gyroscope effect. Figure 16 is the comparison diagram between the reference trajectory and tracking trajectory with sensor error. The sensor error is assumed to be white noise with zero means.Figure 15
Comparison diagram between the reference trajectory and tracking trajectory with gyroscope effect.Figure 16
Comparison diagram between the reference trajectory and tracking trajectory with sensor error.As shown in Figure15 the quadrotor has strong robustness properties against neglected gyroscope effect. It can be seen from Figure 16 that the quadrotor can track the trajectory with sensor error. In actual quadrotor control, the burr in tracking trajectory can be eliminated by Kalman filter or another alternative filter in quadrotor navigation process.


## 6. Conclusion

IMC design based on quasi-LPV system is proposed in this paper. In this method, the nonlinear model is firstly transformed to the Linear model using quasi-LPV (Linear Parameter Varying) method; the quadrotor nonlinear motion function is then transformed to transfer function matrix using the transformation model from the state space to the transfer function; finally, IMC is designed to control the controlled object represented by transfer function matrix, and further trajectory tracking can be realized. In this paper, the controllers for the inner and outer loops are both chosen IMC. It can be seen from the analysis for input and output relationship transfer function of IMC structure that IMC has strong robustness to disturbances and parameter uncertainty, which is verified by the simulation results. It can be seen from perfect tracking for three reference trajectories with drastic changes and robustness to disturbance and parameter uncertainty that the control method proposed in this paper is superior. In addition, the design method is simple and can be used in engineering practice.In this paper, the decoupling of roll and pitch angles is obtained under the hypothesis that the yaw angle is equal to zero. When the derivative of yaw angle is not equal to zero, the coupling of roll and pitch angle exists. At this moment, decoupling internal model controller is needed to control square system which is composed by roll angle and pitch angle loops; further, the roll and pitch angles can be controlled. The design of decoupling internal model controller can refer to the literature in [30]. Research results of this paper are realized through computer simulation; they have not yet been validated in real quadrotor helicopter. Further work will involve the implementation of this control strategy in embedded airborne control system, the acquirement of attitude and position information control needed using the method of the literature in [45, 46], and finally achieving quadrotor real-time trajectory tracking.



---

*Source: 857291-2015-08-04.xml*