July 6, 2015 Robotica manuscript
To appear in the Robotica
Vol. 00, No. 00, Month 20XX, 1–17
Cascaded control for balancing an Inverted Pendulum on a Flying Quadrotor
Chao Zhang
ab∗
, Huosheng Hu
b
, DongBing Gu
b
and Jing Wang
a
a
Engineering Research Institute of USTB, University of Science and Technology Beijing, Beijing, China;
b
Department of Computer Science and Electronic Engineering, University of Essex, Colchester, UK
(Received 00 Month 20XX; final version received 00 Month 20XX)
Abstract: This paper focuses on the flying inverted pendulum problem, i.e balancing a pendulum on
a flying vehicle. After analyzing the system dynamic, a three loop cascade control strategy based on
active disturbance rejection control (ADRC) is proposed. Not only the pendulum balancing, but also the
trajectory tracking of the vehicle can be realized successfully even when strong disturbances exist. The
simulation results from a 3D mechanical systems simulation platform including a comparison to a LQR
controller are given to verify the control performance and robustness of the proposed strategy. Finally,
the practical flying experiments are achieved in our lab.
Keywords: Inverted pendulum, Micro aerial vehicles; ADRC; Cascade control.
1. Introduction
The inverted pendulum is a classical nonlinear control problem and is a common benchmark to
evaluate advanced control techniques. Recently, it has extended to many different scenarios, such as
an inverted pendulum on a cart, multi-degrees pendulum and Furuta pendulum
1
. The dynamics of
pendulum systems is related to two-wheels robots, rocket guidance, etc
2
. It has been investigated
for several decades. Many researchers regard it as a benchmark for advanced control strategies, e.g.
PID control
3
, neural networks
4
and controlled Lagrangians
5
.
With the recent advancement of MEMS (micro electro mechanical sensors) and energy stor-
age devices, Micro Aerial Vehicles (MAVs) have demonstrated a great potential, and been widely
used in many civil and military applications, e.g. wildfire monitoring, aerial filming, and pollution
assessment due to their easy construction and ability to take-off and land vertically (VTOL)
6
.
Meanwhile, these small flying machines, especially quadrotors have populated in the research insti-
tutes worldwide because of their simple steering principle and low cost
7
, such as the Autonomous
Systems Lab at Swiss Federal Institute of Technology
8
and the General Robotics, Automation,
Sensing and Perception (GRASP) Laboratory at University of Pennsylvania
9
.
Recently, the flying inverted pendulum has attracted much attention worldwide. It is a nonlin-
ear, under-actuated, coupled system with 8 Degrees of freedom (DOFs), i.e. a 6-DOF quadrotor
and a 2-DOF inverted pendulum. ETH Flying Lab has first investigated this problem
10
. After the
nominal pendulum equilibrium is realized, linear state feedback controllers are designed to sta-
bilize the whole system. Reinforcement learning was adopted to improve system performance by
decomposing the task into two subtasks
11
, i.e. initial balancing and balanced hover. In addition, a
global stabilizing controller for inverted pendulum derived from controlled Lagrangians is used to
command the desired angels with another parallel quadrotor position controller together
12
.
However, linear state feedback design is based on a linearization model of the equilibrium. This
∗
Corresponding author. Email: czhangd@essex.ac.uk
July 6, 2015 Robotica manuscript
is only valid in a small dynamics range and vulnerable to external disturbances. Meanwhile, the
parameters of two independent controllers for pendulum and quadrotor are difficult to adjust and
need many trials. Although reinforcement learning could be deployed in the controller design by
dividing the task into two subtasks, the designed controllers actually can not control the pendulum
and quadrotor positions together. The final quadrotor position may be dozens of meters away
from the starting point
11
. Other nonlinear control theory such as controlled lagrangian method
for a single inverted spherical pendulum shows very explicit in mathematical proof, however, the
controller form would be too complicated to be deployed practically. The research of classic inverted
pendulum problem may provide some ideas, but unable to be deployed directly.
The flying pendulum problem has an unstable equilibrium, i.e. the upwards vertical position of
the pendulum. Its internal and external disturbances are ubiquitous and difficult to be described in
mathematical methods. Other factors such as communication delay and aerodynamics may degrade
the control quality in practical systems
13
. Recently, many researchers analyzed practical complex
systems in terms of anti-disturbance view and proposed two kinds of disturbance estimation tech-
niques
14
. One is the disturbance observer technique, which is originally proposed by Ohnishi et
al.
15
, and has been widely applied to different systems, e.g., missile systems
16
, humanoid joint
17
,
precise motion control systems
18
. and so on. Another one is Active Disturbance Rejection Control
(ADRC), which lumps the internal uncertain dynamic and the external disturbances as a system
state and on-line estimate them by an extended state observer (ESO), then compensates for them
in control input signals. An ESO can estimate both the system states and the lumped disturbances.
Meanwhile, ADRC design does not require an accurate mathematical model and is relatively simple
to be realized in practical systems. It has been widely deployed in diverse examples of systems with
promising results
19
.
In
20
, a linear high order observer-based linear controller for the Furuta pendulum is designed
on the basis of a flat tangent linearization model. A two loop linear ADRC controller is designed
to solve the classic inverted pendulum on a cart problem
21
. From the results of their works,
ADRC shows great potential to solve underactuated nonlinear systems control problems. Therefore,
inspired by all the previous work in this subject, a three-loop cascade ADRC architecture for the
flying pendulum system is proposed to control the quadrotor track its reference trajectory while
the pendulum can always maintain its position.
The inner loop is a high bandwidth attitude controller on the quadrotor, which tracks desired
attitude angles using feedback from gyroscopes. The quadrotor has a fast tracking response to the
desired commands since it has very low rotational inertia and powerful brushless motors. Then,
on the basis of the fast inner loop, we design an ADRC controller for the pendulum position using
tangent angles as pseudo control inputs.
To design the outer loop controller for quadrotor horizontal position, time scale control concept
22
is adopted since the position control of the inverted pendulum has to be guaranteed first and thus
the position states of quadrotor has to be slower. An ADRC controller using the pendulum position
as control inputs is used here and with small parameters to meet the time-scale separation principle.
Another independent controller using total thrust as control input is designed to control the altitude
of the quadrotor. From simulation, we find system performance is sensitive to the gain coefficient of
control input, thus we introduce an adaptive gain parameter after analyzing the system. Through
this multi-level design, each sub-loop is a negative feedback controller, which is different from the
common LQR and PID controllers, i.e the vehicle loop is a positive feedback controller. This change
can utilize the natural connection between all sub-loops and can improve security in practical
experiments. The robustness of the system is validated by adding disturbances during path tracking.
In addition, many communications are involved in this system such as TCP/IP connection from
Vicon system to PC and wireless Zigbee connection from PC to the quadrotor. Time delay is
an important issue here and may affect the system stability. Inspired by Lupashin’s work
8
, we
proposed a compensation algorithm based on a tracking differentiator to generate the estimations
of the feedbacks and velocities. It is simple and effective in practical system.
2
July 6, 2015 Robotica manuscript
The remainder of this paper is organized as follows. Section 2 presents the dynamics models for
the quadrotor-pendulum system used in this research. In Section 3, a cascade ADRC controller
is designed for quadrotor trajectory tracking and balancing inverted pendulum. Simulation on
Matlab SimMechanics platform
23
and practical flight tests are carried out in Section 4 to show the
feasibility and effectiveness of the proposed control strategy. Finally, a brief conclusion and future
work are given in Section 5.
2. System Dynamics
A Hummingbird quadrotor made by Ascending Technology is used in this research
24
. Fig.1 shows
two right-handed coordinate systems used in describing the MAV pose. The world frame, W, is
defined by axes X
W
,Y
W
and Z
W
with Z
W
axis pointing upward. The body frame, B, coincides
with the center of mass and is defined by the axes X
B
,Y
B
and Z
B
. X
B
is always aligned with
the preferred forward direction and Z
B
perpendicular to the plane of four rotors. In the following
analysis, we assume that the body-fixed frame B is attached to the center of mass of the quadrotor
and the pendulum is rigidly attached to the center as well.
2.1. Quadrotor-Pendulum Model
As shown in Fig.2, the position vector of quadrotor is denoted by p = [x y z]
T
in the world frame
and the pendulum position is described by r and s, which represent the translational position of the
pendulum mass center relative to the supporting point, i.e., r along the X
W
-axis and s along the
Y
W
-axis. The relative height ζ can be calculated by
√
L
2
− r
2
− s
2
where L is the length from the
bottom to the mass center of the pendulum and its position can be given by p
p
= [x+r y+s z+ζ]
T
.
Since the pendulum has no moment of inertia about its z-axis, the rotation kinetic energy can be
given by
1
6
m
p
L
2
Ω
T
p,xy
Ω
p,xy
.
Figure 1. Fixed body and world coordinate systems.
x
y
R
S
,
×ÅR
Å
×ÅS
, m
p
Figure 2. Inverted pendulum on top of a quadrotor.
From the rigid body kinematics, we can derive Ω
p,xy
= p
r
×
˙
p
r
/L
2
where p
r
= [r s ζ]
T
. The
total kinetic energy T
p
and the potential energy V
p
of the pendulum are
T
p
=
1
2
m
p
(( ˙x + ˙r)
2
+ ( ˙y + ˙s)
2
+ ( ˙z −
r ˙r + s ˙s
ζ
)
2
) +
1
6
m
p
L
2
Ω
T
p,xy
Ω
p,xy
(1)
3
July 6, 2015 Robotica manuscript
V
p
= m
p
g(z + ζ) (2)
To derive the pendulum dynamics, applying Lagrangian mechanics as below
d
dt
(
∂L
p
∂( ˙r, ˙s)
) −
∂L
p
∂(r, s)
= 0 (3)
where L
p
= T
p
− V
p
. Then we obtain
¨r = −
3
4
(
ζ
2
L
2
− s
2
)¨x +
3rζ(g + ¨z)
4(L
2
− s
2
)
+
r
3
( ˙s
2
+ s¨s) − 2r
2
s ˙r ˙s + r(−L
2
s¨s + s
3
¨s + s
2
˙r
2
− L
2
˙r
2
− L
2
˙s
2
)
(L
2
− s
2
)ζ
2
(4)
¨s = −
3
4
(
ζ
2
L
2
− r
2
)¨y +
3sζ(g + ¨z)
4(L
2
− r
2
)
+
s
3
( ˙r
2
+ r¨r) − 2s
2
r ˙s ˙r + s(−L
2
r¨r + r
3
¨r + r
2
˙s
2
− L
2
˙s
2
− L
2
˙r
2
)
(L
2
− r
2
)ζ
2
(5)
From the dynamic equations, [¨x ¨y ¨z]
T
can be regarded as control input to the system. However,
for Z-axis, we have to adopt an independent controller to keep the height of the quadrotor at a
desired value. And considering the gain coefficient of ¨z is much smaller than that of ¨x and ¨y, it can
be regarded as a disturbance in the pendulum system when the height is changing. So the system
dynamics of the pendulum can be written in the following form
[¨r ¨s]
T
= B[¨x ¨y]
T
+ f
z
(r, s)¨z + f
n
(r, s, ˙r, ˙s) (6)
where f
z
and f
n
are the second term and third term of right hand in (4) and (5) respectively, and
B =
"
−
3
4
(
ζ
2
L
2
−s
2
) 0
−
3
4
(
ζ
2
L
2
−r
2
) 0
#
(7)
2.2. Quadrotor Dynamics
It is reasonable to assume that the dynamics of the quadrotor is not affected by the movements of
the pendulum since the mass and the inertia of the pendulum are one magnitude less than that
of the quadrotor. The quadrotor is described by six degrees of freedom: the translation position in
the world frame and the vehicle attitude parametered by XY Z-Euler angles (roll-φ,pitch-θ,yaw-
ψ). Using The Newton’s second law, translational equations of the MAV motion can be derived as
below
25
.
m
¨
p =
0
0
−mg
+ R
0
0
T
(8)
where m, g denote the total mass and the gravitational constant respectively, R is the transforma-
tion matrix between W and B and T is the total thrust produced by propellers.
The attitude is not directly controllable, but it is related to the angular velocity of the quadrotor
in the body frame and the angular acceleration is determined by three torques generated by four
propellers
˙
R = R
ˆ
Ω (9)
4
July 6, 2015 Robotica manuscript
J
˙
Ω = −Ω × JΩ + τ. (10)
where J denote the inertia matrix w.r.t the frame B, p is the position vector w.r.t the frame W.
The control input τ = [τ
1
τ
2
τ
3
]
T
represent the torques produced by propellers. Ω = [w
x
, w
y
, w
z
]
T
denotes the angular velocity vector w.r.t the frame B. The notation
ˆ
Ω denotes the skew-symmetric
matrix of Ω.
Based on these dynamic equations, we have done a high-bandwidth controller to track the desired
euler angle values to generate torques commands. In order to transfer these commands to motor
speeds, a brief motor model is introduced here. As seen in Fig.1, motor 1 is the motor on the +X
B
arm and the other three motors are allocated to +Y
B
, −X
B
, −Y
B
arms, respectively. For a typical
multi-rotor flying vehicle, each rotor produces a thrust force F
i
in its Z
B
-axis and a torque M
i
around its Z
B
-axis. A basic relationship between them and trotation speed n
i
is F
i
= k
f
n
2
i
, M
i
=
k
m
n
2
i
. Then, we can write the general relationship in matrix form for a quad-rotor used in this
paper.
τ
1
τ
2
τ
3
T
=
0 lk
f
0 −lk
f
−lk
f
0 lk
f
0
k
m
−k
m
k
m
−k
m
k
f
k
f
k
f
k
f
n
2
1
n
2
2
n
2
3
n
2
4
(11)
where l denotes the distance from rotor to the center of quadrotor, and the unit of motor speed
is revolutions per minute (rpm). The parameters k
f
and k
m
can be regarded as constants and be
determined from static thrust tests. Hence, we can achieve the required motor speeds by inverse
operation of (11). The attitude control loop and the calculation of the motor speed run on the
Asctec Autopilot board equipped on the quadrotor at 1KHz.
3. Controller Design
The quadrotor-pendulum control system is integrated by three loops, i.e. the onboard attitude
loop based on gyroscope feedbacks, the pendulum balancing loop and the quadrotor position loop.
Since the system is highly underactuated and the vertical position of the pendulum is a unstable
equilibrium, a cascaded control strategy is adopted, i.e. the mid loop is the pendulum control and
the quadrotor position loop is the outer loop. Desired attitude angles and thrust would be their
control inputs which are tracked by onboard inner loop.
3.1. Inverted Pendulum Loop
The target of the mid loop is to control the pendulum position (r, s) falling within a very small
region. From (8), we can derive
¨x =(sin ψ sin φ + cos ψ sin θ cos φ)T /m
¨y =(−cos ψ sin φ + sin ψ sin θ cos φ)T /m
¨z =(cos θ cos φ)T/m − g
(12)
Substitute (12) into (6) and take the attitude angles as control inputs
¨r = −
3
4
(1 −
r
2
L
2
− s
2
)g tan θ + f
r
(13)
5