Design of Gyro Based Roll-Stabilization
Controller for a Concept Amphibious
Commuter Vehicle
IOANNIS KARAGIANNIS
Master’s Degree Project
Stockholm, Sweden June 2015
XR-EE-RT 2015:010
Design of Gyro Based Roll-Stabilization Controller
for a Concept Amphibious Commuter Vehicle
Ioannis Karagiannis
Master Thesis
Department of Automatic Control
KTH Royal Institute of Technology
Supervision
First Supervisor
Karl Henrik Johansson
Second Supervisor
Ivan Stenius
Stockholm, Sweden, June 2015
Abstract
In this master thesis the gyroscopic stabilization of a two-wheeled amphibious concept vehicle is
investigated. The key idea is to neutralize external torques applied on the vehicle by the countertorque produced from the two gyroscopes attached on the vehicle. Here the gyroscopes are used as
actuators, not as sensors. When a torque is applied in order to rotate a gyroscope whose flywheel
is spinning, then the gyroscope precesses and it generates a moment, orthogonal to both the torque
and the spinning axis. This phenomenon is known as gyroscopic precession. As the vehicle leans
from its upright position we expect to generate sufficient gyroscopic reaction moment to bring the
vehicle back and get it stabilized.
We first derive the equations of motion based on Lagrangian mechanics. It is worth mentioning that we only consider the control dynamics of a static vehicle. This is the so called regulator
problem where we try to counteract the effects of disturbances. The trajectory tracking (servo
problem) and the water-travelling can be considered as an extension of the current project. We
linearize the dynamics around an equilibrium and we study the stability of the linearized model.
We then design an LQG controller, a Glover-McFarlane controller and a cascade PID controller.
Regarding the implementation part, we only focus on the cascade PID controller. The results from
both simulations and experiments with a small-scale prototype are presented and discussed.
i
Acknowledgements
I could not start acknowledging anyone else first before my beloved parents, Makis and Ntina, for
bringing me into this world. It is them who supported me both psychologically and financially
during my unforgettable stay in Stockholm. Then of course my brother Panagiotis and his wife
Christina (poupe), who never stopped encouraging me, even when things were getting hard some
times. They are my soul-mates, my buddies.
I would also like to acknowledge my cousin Kostas, the teacher who introduced me to the fascinating and adventurous world of mathematics. He taught me the importance of methodology.
With respect to my home-university, I would like to thank prof. Anthony P. Tzes for his recommendation letter and for his invaluable advice when I left the Automation and Robotics laboratory,
Electrical and Computer Engineering Department, University of Patras, Greece in 2013. He kind
of passed me the ticket to this lovely academic-journey in Sweden. Thanks for trusting me professor.
Regarding KTH, there are a number of people I would like to acknowledge:
• My supervisor Ivan Stenius for sharing his passion about this project, and for his constant
feedback and support, especially with respsect to the mechanical and hardware part of the
project.
• Prof. Karl Henrik Johansson for giving me inspiration and encouragement. With his optimistic perspective and very precise directions I very soon realized what the path I should
follow was, which I have to admit, it was not a path paved with roses.
• Prof. Magnus Jansson, for always having time for me whenever a “signal-processing”-nature
problem was knocking my thesis door.
• My teachers Cristian Rojas, Dimos Dimarogonas, Elling Jacobsen and Alexandre Proutiere
who helped me to build a solid background in Control Theory.
• Antonio Adaldo for supervising the control theory part of my project and for sharing some
nice ideas.
Throughout my work on this thesis there were also colleagues, with whom we became close friends,
who stood by me and facilitated my efforts. Johan (Mr Phi) and Dirk, the restless minds as I call
them, never stopped shooting me with brilliant ideas. I would also like to thank Robert for his
valuable tips during my thesis.
I could not forget my friend Alekos. Thanks for helping me with the rehearsals of my presentation.
Last but not least, I would like to thank my uncle Elias for being a true mentor throughout
my entire life.
I truthfully thank you all!
iii
Contents
Abstract
i
Acknowledgements
iii
Contents
vi
1 Introduction
1.1 Motivation . . . . . . . . .
1.2 The NEWT Project . . . .
1.3 Problem Definition . . . . .
1.3.1 Strategic Objectives
1.4 Outline . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
1
3
3
4
4
2 Preliminaries
2.1 The Gyroscopic Effect . . . . . . . . . . .
2.2 Systems Theory . . . . . . . . . . . . . . .
2.2.1 Linear Time Invariant System . . .
2.2.2 Impulse Response and Convolution
2.2.3 State Space Model . . . . . . . . .
2.2.4 Stability . . . . . . . . . . . . . . .
2.3 FIR Filters . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
7
7
8
8
9
9
3 Equations of Motion
3.1 The Concept Vehicle . . . . . . . . . . .
3.2 Rigid Body Dynamics . . . . . . . . . .
3.2.1 Reliability Test of The Model . .
3.2.2 Linearized Model of Rigid Body
3.3 DC Motor Dynamics . . . . . . . . . . .
3.4 Full System Dynamics . . . . . . . . . .
3.5 Open-loop Transfer Function . . . . . .
3.6 Discretized System . . . . . . . . . . . .
3.6.1 Stability Analysis . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
13
13
14
18
19
20
22
23
23
24
4 Controller Design
4.1 Cascade Control Design . . . . . . . . . . .
4.2 LQG Design for the Discretized LTI System
4.2.1 Kalman Filter . . . . . . . . . . . . .
4.2.2 LQR Controller . . . . . . . . . . . .
4.2.3 LQG Controller . . . . . . . . . . . .
4.2.4 Further Tuning of the Parameters .
4.2.5 Penalize Change in Control Signals .
4.3 Glover-McFarlane Controller . . . . . . . .
4.3.1 Constant Gain Precompensator . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
25
25
28
29
31
34
36
38
39
42
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
v
.
.
.
.
.
.
.
.
.
4.3.2
4.3.3
Lead-lag Precompensator . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Two-degrees of Freedom Controller with Lead-lag Precompensator . . . . .
5 Controller Implementation
5.1 Revised Equations of Motion . . . . . . . . .
5.1.1 Acceptable Range of Initial Roll Angle
5.2 Cascade PID Control - Implementation . . .
5.2.1 Discrete-Time PID Controller . . . . .
5.3 Cascade PID Control - Tuning . . . . . . . .
5.3.1 Disturbance Rejection Test . . . . . .
5.4 Enhanced Model with Asymmetry Included .
44
48
.
.
.
.
.
.
.
53
53
54
55
56
57
59
59
6 Experiments with the Prototype
6.1 Servo-Parameters Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Filtering of the Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.3 Validation of the C++ code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
61
61
64
66
7 Conclusions and Remarks
7.1 Modelling and design-based considerations . . . . . . . . . . . . . . . . . . . . . . .
7.2 Hardware considerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3 Future Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
69
69
70
70
Bibliography
74
A Hardware
75
B Simulink Models
79
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Chapter 1
Introduction
“We can’t solve problems by using the
same kind of thinking we used when we
created them.”
Albert Einstein, 1879-1955
1.1
Motivation
The problem of gyroscopic stabilization is attached with two particular names; Louis Brennan [5]
and Pyotr Shilovsky [19],[18], who each built large-scale prototypes in the early of the twentieth
century. Despite the plethora of efforts, the gyro monorail and the gyro car were never developed
beyond the prototype stage. Figure 1.1 and Figure 1.2 illustrate some of these prototypes.
Figure 1.1: The Brennan’s Monorail.
1
2
1.1. Motivation
Figure 1.2: The Shilovsky’s Gyrocar.
The active gyroscopic roll-stabilizer for vehicles made its appearance again back in the 60s when
the single-seat Gyro-X was created by Alex Tremulis, who was working for Gyro Transport Systems
in California. Unfortunately, the company bankrupt and the Gyro-X never had the chance to be
taken into production. The particular prototype vehicle is shown in Figure 1.3
Figure 1.3: The Tremulis Gyro-X Prototype.
Mathematical analysis of the problem was first made by [7]. Significant attempts have been made
the last two decades into both analytically formulating the problem and finding gyro-based stabilizing controllers [8],[21],[3],[1],[12]. Although most of the simulations based on the mathematical
models performed quite well, the results from the experimentation with the physical systems were
not so breathtaking.
The last decade the problem of identifying a gyro-based stabilizing controller has become again
popular. The LIT-Motors company from San Francisco has been conducting significant research
on the field since 2010. Figure 1.4 illustrates the LIT-motor C1 prototype [14].
Chapter 1. Introduction
3
Figure 1.4: The LIT-Motors C1 Prototype.
1.2
The NEWT Project
The last decades there have been plenty of gyro-stabilizer applications, mainly for boats and yachts,
and recently for cars [14]. This project is part of the ongoing NEWT project. The objective of
the NEWT project is to merge the water-mode and the land-mode into one amphibious commuter
vehicle. A short-cut across water not only will reduce the travel-distance, but it will also enable
the use of roads outside the often highly congested main routes and it will offer the flexibility
to explore and enjoy new coastal areas. Figure 1.5 illustrates the design of the NEWT concept
amphibious commuter vehicle.
Figure 1.5: Land (left) and water (right) mode
1.3
Problem Definition
In this project, which is part of the NEWT project, we will focus on the design of an active gyrobased roll-stabilizer for the land-mode. In particular we will investigate the case where the vehicle
is standing still, and we will try to keep it in its upright position. So, the challenge is both to bring
the vehicle on its upright position starting from some tilted position, and to attenuate possible
disturbances. This is the regulator problem (track zero reference-signal).
Our aim is to neutralize any external torque applied on the vehicle by exploiting the counter
4
1.4. Outline
torque the gyroscopes can produce when they precess. More on the gyroscopic effect can be seen
in chapter 2.
The biggest challenge of this task is to deal with the delays coming from the hardware limitations, and the noise introduced by the sensor-measurements.
Due to power limitations the servo motors can only generate a bounded torque. Furthermore,
the servo motors cannot rotate fully. They usually operate within the range (−60o , 60o ). This is a
physical constraint which bounds the angles that the gyroscopes we are using can take. Considering all these limitations, we must come up with control signals that make the vehicle stand on its
upright position within a specified error band.
1.3.1
Strategic Objectives
(i) Derivation of vehicle’s mathematical model
(ii) Development of control strategy
(iii) Controller design and optimization
(iv) Test and evaluation of controller numerically in Simulink
(v) Implementation of the controller in C++ (to be loaded on an Arduino board)
(vi) Test and evaluation of controller experimentally on the small scale prototype
It should be clarified at this point that, although we design three different controllers, we do not
compare them. We only investigate how each and one of them behaves in isolation. For this reason,
we adjust accordingly the model of the system to meet the purposes of each strategy. Our aim is
to implement the controllers and find the one-to-one correlation between the simulated controllers
and the ones which will be translated and implemented in C++. Since the time frame of the
project was not long enough to implement them all, we only proceed with the implementation of
the cascade PID controller.
1.4
Outline
The particular master thesis is organized as follows. In chapter 2 we introduce the basic concepts
a reader should be aware of to follow the rest chapters. In chapter 3 we present the concept
vehicle we will be working with, and we derive the respective equations of motion. Furthermore, we
linearize around an equilibrium, we develop the mathematical state space model and we analyze the
stability of the open loop system. In chapter 4 we design a Cascade controller, an LQG controller
and a Glover-McFarlane controller. In addition, we evaluate their performance via simulations.
In chapter 5 we implement the cascade PID controller. In chapter 6 we present the experimental
results and we analyze the behaviour of the true system. Some critical parameters are identified in
this process. Finally, in chapter 7 we summarize and discuss the conclusions together with a list
of potential future research and extensions to the current work.
Chapter 2
Preliminaries
“To know, is to know that you know
nothing. That is the meaning of the true
knowledge.”
Socrates, 470-399 BC
In this chapter, we provide all the necessary theory, so the reader can go through the next chapters.
Above all, we consider that the most important thing to be explained is the gyroscopic effect.
Lacking this knowledge, the reader will not be able to understand what we are dealing with,
and what the challenge of this project is. Then, according to the strategic objectives, defined in
chapter 1, it might be useful to present some mathematical foundations upon which the following
chapters are built. Given that we want to derive the analytical model of the system and also design
controllers, it is essential to include some basics on Systems Theory. Moreover, we present the FIR
filters, which will be used in chapter 6, in order to deal with the high frequency noise coming with
the measurements.
2.1
The Gyroscopic Effect
In Figure 2.1 a simple gyroscope is presented. There are two requirements for the gyroscopic effect
to be observable. One is that the gyroscope is spinning around an axis and the second is that an
external torque is applied forcing it to rotate with respect to one of the other two perpendicular
axes. Let us take it step by step though. Based on Figure 2.1 assume that the flywheel inside the
gyroscope is spinning around z-axis with a spinning velocity Ω. According to Newton’s second law,
the angular momentum created by the spinning gyroscope will be given by
(2.1)
T = IG33 Ω,
where IG33 is the moment of inertia of the gyroscopic flywheel around the z-axis, and it is described
by
IG33 =
1
2
mG RG
,
2
(2.2)
where mG is the mass of the flywheel and RG is its respective radius. From Figure 2.1 we can
notice that the precession axis is locked. Now the τcontrol comes to rotate the gyroscope around
its precession angle. From Newton’s third law we know that every action has its reaction, and
therefore the gyroscope tries to resist on the applied torque. The result of this reaction is a rollopposing torque (τprec ) that tries to rotate the gyroscope in the opposite direction with an angular
velocity ωprec . The outcome is the gyro torque τgyro and it is described by
~τgyro = T~ × ω
~ prec .
(2.3)
5
6
2.1. The Gyroscopic Effect
The torque of Equation 2.3, while being the cross product of ωprec and T , it will be orthogonal to
both of them. This is the key idea of gyroscopic stabilization. So, the generated counter-torque will
be orthogonal to both the spinning axis and the applied control-torque. This is how the gyroscopes
can be used to roll-stabilize a vehicle.
Figure 2.1: The Gyroscopic Control System.
If we introduce αprec to be the precession angle of the gyroscope, then it apparently holds that
ωprec = dαprec /dt. Hence, the balancing torque generated by the gyroscope can now be expressed
by
τstab =
1
dαprec
2
mG RG
Ω
cos(αprec ).
2
dt
(2.4)
For αprec = 90o the balancing torque with respect to the roll-axis is zero. Therefore, it is only
meaningful to discuss about gyroscopic stabilization within some precession boundaries, e.g.: ±60o .
If we take the above-mentioned gyroscopic mechanism and place it on a vessel as shown in Figure
2.2, then by applying proper torque-signals on the gyroscope we can control the vessel’s orientation.
In this particular project the role of the vessel can be played by the vehicle which we try to rollstabilize.
Figure 2.2: The Gyroscopic Control System on a Vessel.
Chapter 2. Preliminaries
7
In general, the larger this gyroscopic torque τgyro is, the more control we have on the orientation
of the vessel with respect to the rolling x-axis. Usually this is more like a trade off, because very
large torques make the system hard to stabilize. Thus, should one want to design a gyro based roll
stabilizer, one should focus on the following four parameters:
(i) mass of the gyroscopic flywheel (mG ) ,
(ii) radius of the flywheel (RG ) ,
(iii) spinning velocity of the flywheel (Ω) ,
(iv) precession rate (ωprec ) .
The last two are highly depended on how much power we can supply the gyroscopes with. In
this project we will be working with two kinds of actuators. The first one is the servo motor
responsible for rotating the gyro-gimbal, and the second one is the DC motor responsible for
spinning the gyro-flywheel. The power we can supply our system with is specified and limited
from the characteristics of those two actuators. Apparently, larger mass and radius of the flywheel
results in larger balancing torque. However, the size and the weight of the flywheel is restricted
from the size of the vehicle one wants to attach it on. More on the gyroscopic precession can be
found in [23].
2.2
Systems Theory
For the purposes of this project we will only consider the discrete time systems which can be seen
as the discretized version of a continuous time system. A similar analysis hold for the continuous
time systems. Assume we can represent a system by an operator S{·} as shown in Figure 2.3. A
discrete time system accepts an input signal x(k) and produces an output signal y(k). That would
be
y[k] = S{x[k]}.
Figure 2.3: Generic Input-Output Model.
2.2.1
Linear Time Invariant System
A discrete time system is characterized as linear time-invariant (LTI) when the following properties
hold: linearity and time-invariance.
Linearity
Linearity consists of homogeneity and superposition. If we assume that y1 [k] and y2 [k] are the
outputs of the system S, excited from the inputs u1 [k] and u2 [k] respectively, then the system S
is linear when the following holds
S{a1 u1 [k] + a2 u2 [k]} = a1 S{u1 [k]} + a2 S{u2 [k]} = a1 y1 [k] + a2 y2 [k]
8
2.2. Systems Theory
Time-Invariance
Given that
y[k] = S{u[k]}
the system S system is time-invariant if it obeys the following time-shift invariance property
y[k − N ] = S{u[k − N ]}
for any integer N .
2.2.2
Impulse Response and Convolution
The behaviour of an LTI system is described by Equation 2.5.
y[k] = x[k] ∗ h[k] =
∞
X
x[k]h[n − k] =
k=−∞
∞
X
x[n − k]h[k]
(2.5)
k=−∞
where h[n] is the impulse response of the system and ∗ denotes the convolution operator. Since
the transfer function of the system is the Z-transform transform of its impulse response, we take
H(z) =
∞
X
h[m]z −m .
(2.6)
m=−∞
The convolution that gives the output of the system can be transformed to a multiplication in the
transform domain. So, Equation 2.5 would become
y[k] = x[k] ∗ h[k] = Z −1 {X(z)H(z)}.
(2.7)
It is now easier to analyze systems and gain insights into their behaviour through the frequency
domain.
2.2.3
State Space Model
All systems can be expressed by a set of differential equations. The state variable represents the
entire state of the system at any given time. The minimum amount of state variables required to
represent a system is usually equal to the order of the system’s defining differential equation. If
we consider x[k] ∈ Rn to be the state vector of the system, u[k] ∈ Rp to be the input in the system
and y[k] ∈ Rq to be the output of the system then the state space model of the LTI system can be
written as
x[k + 1] = Φx[k] + Γu[k]
(2.8)
y[k] = Cx[k] + Du[k].
The matrix Φ is known as the state-transition matrix and it can be used to derive the general
solution of linear dynamical systems. It also contains information with respect to the stability of
the open loop system. The solution of the above mentioned system given some initial conditions
x[0] is given by Equation 2.9.
x[k] = Φk x[0] +
k−1
X
Φk−j−1 Γu[j]
(2.9)
j=0
The relation between the state-space model described above and the transfer function of the system
is given in Equation 2.10.
H(z) = C(z1n − Φ)−1 Γ + D
(2.10)
Chapter 2. Preliminaries
2.2.4
9
Stability
The solution x∗ [k] of x[k + 1] = f (x[k]) is Lyapunov stable if for all > 0 there exists δ = δ()
such that for all solutions x[k]
kx[0] − x∗ [0]k< δ ⇒ kx[k] − x∗ [k]k< ,
k = 0, 1, ....
The solution is asymptotically stable if it is stable and it also holds that
kx[0] − x∗ [0]k< δ ⇒ lim (kx[k] − x∗ [k]k) = 0.
k→∞
A linear time invariant system having the state space form described in Equation 2.8 is asymptotically stable if and only if
|λi (Φ)|< 1,
i = 1, ...n,
where n is the length of the state vector and λi denotes the eigenvalues of the system which can
be calculated from the characteristic polynomial
det(λ1 − Φ) = 0
In general the eigenvalues of the transition-matrix coincide with the poles of the transfer function
described in Equation 2.10.
Controllability
Another important property of LTI systems is controllability. That is to test whether we can
drive the internal state of the system from any initial state to any other final state within a finite
time interval. An nth -order system described by Equation 2.8 is controllable if and only if its
controllability matrix (Cd ) ,described by Equation 2.11, has full row rank.
Cd (Φ, Γ) = [Γ
ΦΓ
Φ2 Γ
···
Φn−1 Γ]
(2.11)
Controllability is a very conservative criterion and that is why in most cases it is fair enough to
have a stabilizable system. That is a system whose unstable eigenvalues can be driven to the
stability region (unit circle). Further analysis of this goes beyond the scope of the current project.
Observability
A system is observable when we can estimate in finite time the states of the system using only
the outputs of the system. There is similarly an easy way to test this via the observability matrix
(Od ). An nth -order system is observable if and only if its observability matrix Od , described by
Equation 2.12, is full rank.




Od (Φ, C) = 


2.3
C
CΦ
CΦ2
..
.
CΦn−1







(2.12)
FIR Filters
In this section we will explain how an FIR (Finite Impulse Response) filter performs. In the particular project we are interested in the low pass filters for cancelling out the high frequency noise
from our signals.
10
2.3. FIR Filters
For a causal FIR filter of N th order, each value of the filtered (output) signal is a weighted sum of
the most recent input values and it can be written as
y[n] =
N
−1
X
(2.13)
h[k]x[n − k],
k=0
where N is the order of the filter, x[n] is the input signal, y[n] is the output signal and h[k] is the
values of the impulse response of the N th order FIR filter at the k th instant for 0 ≤ k ≤ N − 1.
Apparently this is a convolution in the discrete time, which corresponds to a multiplication in the
frequency domain. For time optimization reasons, which are out of the scope of the current thesis,
a rule of thumb is to select symmetric FIR filters.
Unless we increase significantly its order, the FIR filter is just an approximation of the ideal
low pass filter, and as such, it cannot attenuate the high frequency noise in the way the ideal one
does. In general the ideal low pass filter is described

1
|f |≤ fc


 H(f ) = F{h[n]} =

0 otherwise
sin(2πf n)
c

n 6= 0

πn

h[n] =

2fc ,
n=0
(2.14)
where |f |≤ 1/2 is the normalized frequency. Figure 6.7 illustrates how much closer to the ideal
filter we get as we increase the order of the FIR filter. The impulse response of an ideal filter has
a sync shape which is non-causal and infinite in duration. If we consider a sampling frequency
fs = 200Hz and a cut-off frequency fc = 10Hz then in the normalized frequency we would expect
to suppress all the frequencies above fc /(fs /2) = 0.05. Apparently FIR(96) approximates the ideal
low pass filter with high accuracy.
In general there exist four types of FIR filters. For more details see [15]. If we consider a linear phase N th order symmetric low pass filter, with N being even number, then we have the well
known Type-I FIR filter. Its impulse response can be written as:
h[n] = {h0 , h1 , · · · hN } = {bN/2 , · · · , b1 , b0 , b1 , · · · bN/2 }
The transfer function of the filter can be now written as its Z-transform:
H(z) =
+∞
X
k=−∞
N/2
h[k]z
−k
=
X
h[k]z −k = bN/2 z N/2 + · · · + b1 z 1 + b0 + b1 z −1 + · · · + bN/2 z −N/2
k=−N/2
Given that z = ejω with ω = 2πf we take:
N
2 −1
X
N
2h[k]cos(2kπf )
H(f ) = b0 + 2b1 cos(2πf ) + · · · + 2bN/2 cos( 2πf ) = h[N/2] +
2
k=0
Nevertheless, given that the FIR filter will be used for on-line filtering, we need to hold the
delay introduced by the filter relatively small. For instance, although we succeed a descent noiseattenuation with the 96th order filter, the introduced delay will cause significant problems to our
control signal later on. Therefore, there is always a trade off between the noise attenuation and
the introduced delay.
Chapter 2. Preliminaries
11
FIR(6)
FIR(12)
0.3
0.2
0.25
0.15
h[n]
0.2
0.15
0.1
0.1
0.05
0.05
0
-4
-2
0
2
4
0
-10
h[n]
FIR(48)
-5
0
5
10
FIR(96)
0.1
0.1
0.08
0.08
0.06
0.06
0.04
0.04
0.02
0.02
0
0
-0.02
-0.02
-20
-10
0
10
20
-50
0
50
Figure 2.4: Filter Comparison in Time Domain.
Figure 2.5 illustrates how the previously described low pass FIR filters attenuate the noise in
frequency.
FIR(6)
10 0
|H(f)|(dB)
10 0
10 -2
10 -4
10 -5
-0.5
10 0
|H(f)|(dB)
FIR(12)
0
0.5
FIR(48)
10 -6
-0.5
10 0
10 -2
10 -2
10 -4
10 -4
10 -6
-0.5
0
normalized frequency
0.5
10 -6
-0.5
0
0.5
FIR(96)
0
0.5
normalized frequency
Figure 2.5: Filter Comparison in Frequency Domain.
Before closing the filtering chapter it is worth-mentioning that there exist some window-techniques
with which one can suppress the side lobes which can be observed in Figure 2.5. Some very well
known are the Hamming window, the Chebyshev window, the Kaiser window which is used in
Figure 2.5, etc. Each and one of them has a different impact on suppressing the side lobes and on
the final resolution of the filtered signal. In particular the Kaiser window we have used is associated
with a non-negative parameter which determines the shape of the window. In the frequency domain,
12
2.3. FIR Filters
it determines the trade-off between main-lobe width and side lobe level, which is a central decision
in window design. For more details on the FIR filters and on the window-techniques one could
start from [15].
Chapter 3
Equations of Motion
“Science is not only a disciple of reason
but, also, one of romance and passion.”
Stephen Hawking
In this chapter we first present the concept vehicle we will be working with throughout this project,
together with the parts it consists of. Then, the vehicle’s equations of motion are derived based on
Lagrangian mechanics. Furthermore, the non-linear state space model will be calculated together
with its linearized version. In addition, the linearized rigid body dynamics together with the
DC motors dynamics are merged into one unified state space model. A similar approach for the
derivation of the equations of motion was made by [21], [12], [1] and [22], [11]. Finally, the stability
and controllability of the open loop system are studied.
3.1
The Concept Vehicle
The real prototype with which we will be working throughout this project is shown in Figure 3.1.
The basic parts this vehicle consists of are the following
(i) base of the vehicle
(ii) front and rear wheel
(iii) two gyroscopes
(iv) two servo motors to rotate the gyroscopes
(v) two DC motors to spin the gyroscopic flywheels
(vi) an Ardupilot board
(vii) encoders to measure the precession angle of the gyroscopes
(viii) battery for autonomy of the vehicle
(ix) remote controller
13
14
3.2. Rigid Body Dynamics
Figure 3.1: The Concept Amphibious Commuter Vehicle.
Given the dynamics and the size of the vehicle, we approximated the necessary balancing torque
required to stabilize the vehicle. We could do it in two ways. Either with one or with two gyroscopes. The reason we preferred to use two gyroscopes instead of one was in order to have mass
properties half of those corresponding to the single gyroscope case. By using smaller sized gyroscopes we affected less the symmetry of our system. For instance, the greater the radius of the
gyroscope is, the more effect it will have on the vehicle’s roll motion when the vehicle deviates a
lot from its upright position.
Although the title of this project mentions the property of the commuter vehicle to treat land
and waterways as equal options, we will only focus on the land case and in particular we will
investigate the control dynamics of the static vehicle. This is known in the control-literature as
the regulator problem where we try to track the zero reference-signal and counteract the effect
of the disturbances. Consequently, the wheels of the vehicle should be seen only as the touching
points with the ground. The trajectory tracking (servo problem) and the water-travelling can be
considered as extensions of the current project.
The reader can find more details on the technical characteristics of the above mentioned parts
in Appendix A.
3.2
Rigid Body Dynamics
There are two approaches to derive the equations of motion of a system: the Newtonian approach
and the Lagrangian approach. The Newtonian approach considers each individual component of
a system separately. Therefore, the calculation of interacting forces resulting from connections
among these sub-parts is required. These connections lead to kinematic constraints. In most cases
these interacting forces can be eliminated by the equations of motion. In this study we prefer to
use the Euler-Lagrange approach instead, which considers the system as a whole. Two key terms
in this process are the generalized variables and the generalized forces. The central idea of this
procedure is to find an expression for the total kinetic energy (T ) and total potential energy (V ) of
the system. Then, the equations of motion will be determined by solving the generalized equations
of motion
d ∂L ∂L
−
= Qi ,
(3.1)
dt ∂ q̇i
∂qi
where qi are the generalized coordinates, Qi are the generalized forces, and L = T − V is the
Lagrangian. In our case the generalized coordinates are the roll angle (q1 = φ) of the vehicle and
Chapter 3. Equations of Motion
15
the precession angle (q2 = α) of the gyroscopes. Hence, we obtain a set of two 2nd -order differential
equations described by
h d ∂T ∂T i h d ∂V ∂V i
−
−
−
= Q1
dt ∂ q̇1
∂q1
dt ∂ q̇1
∂q1
h d ∂T ∂T i h d ∂V ∂V i
−
−
−
= Q2 .
dt ∂ q̇2
∂q2
dt ∂ q̇2
∂q2
(3.2a)
(3.2b)
Figure 3.2 and Figure 3.3 illustrate the side and the back view of the vehicle respectively, inspired
by [21].
Figure 3.2: Concept Vehicle - Side View Schematic.
Figure 3.3: Concept Vehicle - Back View Schematic.
If we assume that the vehicle is composed of the main body (B), the gyro-cages (C), the gyrobases (GB), the gyro-wheel (G) and the servo motors, then the total kinetic energy and the total
potential can be expressed as
T = TB + TC1 + TGB1 + TG1 + TC2 + TGB2 + TG2 + 2Tservo
(3.3a)
V = VB + VC1 + VGB1 + VG1 + VC2 + VGB2 + VG2 + 2Vservo .
(3.3b)
Before defining the kinetic energy of each part we shall first list the parameters of the static vehicle
(see Table 3.1).
16
3.2. Rigid Body Dynamics
Parameter
Vehicle mass & height of COG
Servo mass & height of COG
Gyro-base mass & height of COG
Gyro-cage mass
Gyro-flywheel mass
Vehicle inertia
Servo inertia
Gyro-base inertia
Gyro-cage inertia
Gyro-flywheel inertia
Gravity constant
Flywheel spin velocity
Symbol
mB & hB
ms & hs
mGB & hG
mC
mG
IBx
Isx
IGBx
[ICx , ICy , ICz ]
[IGx , IGy , IGz ]
g
Ω
Table 3.1: Parameters of Static Vehicle.
Furthermore, the following variables shall be introduced
ω1 = φ̇
ω2 = ψ̇sin(φ)
ω3 = ψ̇cos(φ)
ω4 = φ̇cos(α) − ψ̇cos(φ)sin(α)
ω5 = ψ̇sin(φ) + α̇
ω6 = φ̇sin(α) + ψ̇cos(φ)cos(α)
where ω1 is the roll rate of the vehicle, ω2 is the pitch rate of the vehicle, ω3 is the yaw rate of
the vehicle, ω4 is the precession rate of the gyroscope, ω5 is the angular velocity of the gyroscope
with respect to the y-axis and ω6 is the angular velocity of the gyroscope with respect to the zaxis. There is a significant assumption we make at this point which simplifies our calculations and
analysis. The vehicle will only be able to rotate around the x-axis. That means the translational
motion will be neglected throughout our analysis. Therefore, the yaw rate (θ̇) and the pitch rate
(ψ̇) of the vehicle are considered to be zero. Hence, the above angular velocities become
ω1 = φ̇
ω2 = 0
ω3 = 0
ω4 = φ̇cos(α)
ω5 = α̇
ω6 = φ̇sin(α)
Chapter 3. Equations of Motion
17
The kinetic energies of each part can now be expressed by Equation 3.4.
1 2
ω IBx
(3.4a)
2
h
i
1
(3.4b)
TC1 = ω42 ICx + ω52 ICy + ω62 ICz
2
h
i
1
TG1 = ω42 IGx + ω52 IGy + (ω6 + Ω)2 IGz
(3.4c)
2
1
TGB1 = ω12 IGBx
(3.4d)
2
h
i
1
(3.4e)
TC2 = ω72 ICx + ω82 ICy + ω92 ICz
2
h
i
1
TG2 = ω72 IGx + ω82 IGy + (ω9 − Ω)2 IGz
(3.4f)
2
1
TGB2 = ω12 IGBx
(3.4g)
2
1
Tservo = ω12 Isx
(3.4h)
2
where ω7 , ω8 and ω9 are the respective angular velocities of the second gyroscope which is supposed
to be the mirrored version of the first one so they can both generate a double total gyroscopic
momentum compared to the single gyroscope case. Ω represents the spinning velocity of the
gyroscopic flywheels. In order to achieve double counter-torque they gyroscopes should spin in
opposite directions. Therefore
TB =
ω7 = φ̇cos(α)
ω8 = −α̇
ω9 = −φ̇sin(α)
The potential energy of the system is described by
V =
N
X
mi ghi = [hB mB + 2hG (mG + mC + mGB ) + 2hs ms ]gcos(φ).
(3.5)
i=1
If one would like to be coherent with the schematic we presented above, one should introduce the
asymmetry the servo motors induce in the system. So, the precise potential energy is described by
V =
N
X
mi ghi = [hB mB + 2hG (mG + mC + mGB )]gcos(φ) + 2hs ms gcos(φ + φs ),
(3.6)
i=1
where φs is the angle of the servos with respect to the yaw axis when the vehicle stands on its
upright position. While being on the upright position the center of gravity of the servos is not
aligned with the yaw axis. Nevertheless, since the servo motors constitute only one source of asymmetry among many, we prefer to keep our model as simple as possible and make all the necessary
modifications later on, at the implementation stage. Therefore, for our theoretical analysis we stay
with Equation 3.5.
By applying the derived kinetic and potential energies of the system on the Lagrange’s equations
of motion described in Equation 3.2, we obtain the two coupled nonlinear differential equations
φ̈[h2B mB + 2h2G (mG + mC + mGB ) + 2h2s ms + IBx + 2IGBx + 2Isx +
+ 2cos2 (α)(ICx + IGx ) + 2sin2 (α)(ICz + IGz )]−
− 4φ̇α̇cos(α)sin(α)[ICx + IGx − ICz − IGz ]+
+ 2Ωcos(α)α̇IGz − [hB mB + 2hG (mG + mC + mGB ) + 2hs ms ]gsin(φ) = Q1
(3.7)
18
3.2. Rigid Body Dynamics
2α̈(ICy + IGy ) + 2φ̇2 cos(α)sin(α)[ICx − ICz + IGx − IGz ] − 2φ̇cos(α)ΩIGz = Q2 .
(3.8)
For the sake of economy of space we can assume the following:
cos → c
sin → s
k1 = 2(ICx + IGx )
k2 = 2(ICy + IGy )
k3 = 2(ICz + IGz )
k4 = IBx + 2IGBx + 2Isx
k5 = k1 − k3
k6 = hB mB + 2hG (mG + mC + mGB ) + 2hs ms
k7 = h2B mB + 2h2G (mG + mC + mGB ) + 2h2s ms
where k7 represents the extra terms that have to be included according the Parallel Axis Theorem
(Huygens-Steiner Theorem [16]) due to the fact that the respective bodies rotate around the
reference roll axis, which is defined to be the axis between the two touching points of the wheels
with the ground. Thus, the equations of motion can now be written as
φ̈[k4 + k7 + k1 c2 α + k3 s2 α] − 2k5 φ̇α̇cαsα + 2Ωcαα̇IGz − k6 gsφ = Q1
2
k2 α̈ + k5 φ̇ cαsα − 2φ̇cαΩIGz = Q2
(3.9a)
(3.9b)
where
Q1 = hB Fd cφ,
Q2 = Mu
are the non-conservative generalized forces. Fd represents the horizontal disturbance applied on
the side of the vehicle as well as unmodeled dynamics, and Mu denotes the torque generated by
the servos.
3.2.1
Reliability Test of The Model
To ensure the reliability of the above calculated non-linear model we can perform two preliminary
tests. In both tests the spinning velocity of the gyroscopic flywheels are considered to be zero. For
the first test we set the roll angle of the vehicle to zero and we assume there is no input to the
system, so we set the initial conditions to x0 = [0o 0 0 0]T . If our model is correct, then no
movement should be produced. The respective simulation verified our predictions (Figure 3.4). In
the second test we release the vehicle with a non-zero roll angle and according to physics we should
expect an oscillating behaviour compared to that of a pendulum. We assume initial conditions
x0 = [20o 0 0 0]T . We have not specified any physical constraints yet and we completely
neglect the friction between the air and the vehicle. Thus, while having an isolated system, we
expect to observe an oscillating behaviour between the roll angles φ = 20o and φ = 340o , according
to the law of conservation of energy. Figure 3.4 verifies our predictions. Since the non-linear model
passed successfully the tests, we can claim that it can describe with high accuracy the true system.
Chapter 3. Equations of Motion
19
Vehicle released vertically (φ(0) = 0o , Ω=0 rpm) - Test1
1
Vehicle released tilted (φ(0) = 10o , Ω=0 rpm) - Test2
350
φ
φ
340o
20o
300
250
degrees
degrees
0.5
0
200
150
100
-0.5
50
0
-1
0
1
2
3
0
4
1
2
3
4
time (s)
time (s)
Figure 3.4: Reliability Test.
3.2.2
Linearized Model of Rigid Body
First we define the state-vector x = [φ α φ̇ α̇]T with x1 ∈ (−π/6, π/6) and x2 ∈ (−π/4, π/4).
Thus, we take the nonlinear state space equations described in Equation 3.10.


 
ẋ1
f1 (x)
 ẋ2   f2 (x)

 
 ẋ3  =  f3 (x)
ẋ4
f4 (x)



 
=
 




x3

x4
2k5 x3 x4 cx2 sx2 −2Ωcx2 x4 IGz +k6 gsx1 +hB Fd cx1
k4 +k7 +k1 c2 x2 +k3 s2 x2
−k5 x23 cx2 sx2 +2x3 cx2 ΩIGz +Mu
k2











(3.10)
By assuming there is no input (Mu = 0) and no disturbance (Fd = 0) in our system, we take that
the equilibrium points are x∗ = [0 x∗2 0 0]T . That means that the gyroscopes could have any
kind of orientation at the equilibrium point of the vehicle. For the sake of simplicity we will now
linearize about x∗ = [0 0 0 0]T to gain some intuition on the behaviour of the system. In the
following chapters we will explain the necessity of the gyroscopes having horizontal orientation at
the equilibrium. Based on [9], the respective Jacobians can now be calculated
 ∂f1 ∂f1 ∂f1 ∂f1 


0
0
1
0
∂x1
∂x2
∂x3
∂x4
 ∂f2 ∂f2 ∂f2 ∂f2  

0
0
0
1
∂f 
∂x2
∂x3
∂x4  

1
2ΩIGz
A=
x=x∗ =  ∂x
gk6
∂f3
∂f3
∂f3
∂f3  x=x∗ = 

0
0
−
∂x u=u∗  ∂x1 ∂x2 ∂x3 ∂x4  u=u∗
k1 +k7 +k4
k1 +k7 +k4
2ΩIGz
∂f4
∂f4
∂f4
∂f4
0
0
0
k2
∂x
∂x
∂x
∂x
1


∂f B=
x=x∗ = 
∂u u=u∗ 
∂f1
∂u
∂f2
∂u
∂f3
∂u
∂f4
∂u
2
3
4



0
  0 
 

 x=x∗∗ =  0 
u=u
1
k2
So, the linearized state-space model is described by




0
0
1
0
0




0
0
0
1
 x(t) +  0  Mu
2ΩIGz
ẋ(t) = 
gk6
 k +k



0
0
−
0
k1 +k7 +k4
1
7 +k4
1
2ΩIGz
0
0
0
k2
k2
y(t) =
1
0
0
1
0
0
0
0
x(t).
(3.11)
20
3.3. DC Motor Dynamics
The non-linear Simulink model is illustrated in Figure 3.5.
sin
u2
Trigonometric
Function
Math
Function
k3
cos
u2
Trigonometric
Function1
Math
Function1
x1dot
1
x1
1
s
Gain2
Roll angle
roll
Integrator
k1
2
x2dot
x2
1
s
Gain1
Gimbal angle
..
Integrator1
Add
k4+k7
Constant
sin
3
2*k5
Roll rate
...
x3dot
Trigonometric
Function2
Product6
Divide
cos
x3
1
s
Gain3
Integrator2
Product
cosinus
2*Omega*I_G33
x4dot
Gain4
1
s
x4
Integrator3
4
Product1
_
Gimbal rate
sin
sin5
k6
g
Constant2
Gain7
Product2
Add2
d1
Constant1
Disturbance
2
_.
cos
Product3
Trigonometric
Function4
Nonlinear Model of the Vehicle
u2
Math
Function2
k5
1/k2
Product4
1
Gain8
.
Input (Mu)
cos
Trigonometric
Function5
Gain15
Add3
2*Omega*I_G33
Gain9
Product5
Figure 3.5: Simulink Model of the Concept Vehicle.
3.3
DC Motor Dynamics
In the state space model of Equation 3.11, the input is the torque we apply on the gyroscopes.
But, this torque is generated when we apply a certain voltage to the DC motors. Therefore, it is
more intuitive if we can directly consider the input of the whole system as the voltage applied to
the DC motors. For this reason we introduce the dynamics of the DC motor, which we will later
merge with the state space model of Equation 3.11 to derive the full mathematical model of the
vehicle.
It is common to model the DC motor as a second order LTI and stable system. The dynamics governing the DC motor shown in Figure 3.6 can be summarized in
Vin (t) = i(t)R +
di(t)
L + Ve (t),
dt
(3.12)
where Vin is the terminal voltage of the motor, i(t) is the current through the motors windings, R
is the terminal resistance, L is the armature inductance and Ve is the back-emf of the motor.
Chapter 3. Equations of Motion
21
Figure 3.6: The DC Motor and its respective bond-graph.
If we assume that the relation between the produced torque and the current in the motor’s windings
is linear, then we obtain
Ve (t) = Ke ω(t)
(3.13a)
M (t) = Km i(t),
(3.13b)
where Ke is the back-emf constant, Km is the torque constant and M is the torque generated from
the DC motor. The angular velocity of the inertia connected to the motor is given by
dω(t)
1
= MJ (t),
dt
J
(3.14)
where MJ is the torque applied on the inertia. In our problem this MJ is the torque applied on
the gyroscopes. According to the respective bond graph in Figure 3.6 it holds:
MJ (t) = M (t) − MR (t) = Km i(t) − γω(t)
where γ is the mechanical damping. So the equation describing the angular velocity of the inertia,
which in our case will be the gyroscope, is
dω(t)
1
= (Km i(t) − γω(t)).
dt
J
If we now consider the state vector x̃(t) = [x̃1 (t) x̃2 (t)]T = [i(t) ω(t)]T and taking into account
Equation 3.12 and Equation 3.14 we obtain the state space model of the DC motor
˙
x̃(t)
=
y(t) =
−R
L
Ke
J
0
1
− KLm
− Jγ
x̃(t) +
1
L
0
Vin (t)
(3.15)
x̃(t),
where Vin is the voltage we feed the motor with in order to produce the desired torque and y = ω
is the measured output of the DC-motor.
22
3.4. Full System Dynamics
3.4
Full System Dynamics
Before proceeding with the state space model of the full-system, which will include both the linear
dynamics of the servo and the linearized dynamics of the rigid body, there is a property of basic
mechanics and physics which shall be reminded at this point. Two bodies which are connected in
series must share the same angular velocity. In our case the servos are connected with the gyroscopes. Thus, the angular velocities of the DC-motors must be equal with the respective precession
rates of the gyroscopes. That said, the following holds
ω(t) = α̇(t).
Therefore, the torque that will be applied on the gyroscopes will be
Mu (t) = MJ (t) = Km i(t) − γω(t) = Km i(t) − γ α̇(t)
and the Equation 3.12 becomes
di(t)
dα(t)
L + Ke
dt
dt
The new state vector will be x = [φ α φ̇ α̇
will be described by
Vin (t) = i(t)R +




ẋ(t) = 


y(t) =
0
0
0
0
0
0
0
gk6
k1 +k7 +k4
0
0
1
0
0
1
0
0
0
0
1
0
0
2ΩIGz
k2
0
0
0
0
1
2ΩI z
− k1 +k7G+k
4
− kγ2
− KLm
i]T and the state space model of the full system
0
0
0
Ke
k2
−R
L







 x(t) + 




0
0
0
0
1
L



 Vin (t)


(3.16)
x(t).
From now on
x1 :
x2 :
x3 :
x4 :
x5 :
roll angle of the vehicle
precession angle of the gyroscope
roll rate
precession rate
current in the motor’s armature
So, the generic closed loop system, without specifying the control strategy for the time, will look
like the one illustrated in Figure 3.7.
Figure 3.7: The Generic Closed-loop System.
Chapter 3. Equations of Motion
3.5
23
Open-loop Transfer Function
Given that we have identified the state space model of the system we can now explicitly calculate
the transfer function of the open loop system by
(3.17)
G(s) = C(s15 − A)−1 B + D.
Thus, the input-output relation can be written as
y(t) = G(p)u(t) ⇒
y1 (t)
y2 (t)
=
G1 (p)
G2 (p)
u(t),
where p denotes the time shift operator for continuous-time LTI systems.
There are only two minors and from the least common denominator (LCD) we can identify the
poles of the open-loop system which is unstable (one pole in the right half plane). It is worth
mentioning that the poles of the system coincide with the eigenvalues of the transition matrix A
shown in Equation 3.16. Although unstable, it can be verified that the system is controllable (full
rank controllability matrix).
3.6
Discretized System
Depending on the control technique one would like to investigate, one would prefer to work either
with the continuous time system, or with its discretized counterpart. Since measurements (y) can
only be taken at certain instances, the system has to be discretized, so the computer can interpret
how the states evolve in discrete time. If we assume that the input is piece-wise constant between
two sampling intervals, then the deterministic case of the discretized system will be described by
x[k + 1] = Φx[k] + Γu[k]
(3.18)
y[k] = Hx[k],
where
Φ = eATs
Z Ts
Γ=
eAτ dτ B
0
H=C
and Ts is the sampling period. Φ and Γ can be easily calculated at once by:


e
A
0
B
0

Ts
=
Φ
0
Γ
I
Figure 3.8 shows how the sampling interval affects the position of the poles in the open loop system.
24
3.6. Discretized System
Open-loop Zero-Pole Map
1
0.5π/T
0.6π/T
0.4π/T
0.8
0.6
0.8π/T
Imaginary Axis
0.4
0.2
0
-0.2
T=1
T=0.5
T=0.25
T=0.1
T=0.05
T=0.01
0.1
0.3π/T
0.2
0.3
0.4
0.2π/T
0.5
0.6
0.7
0.1π/T
0.8
0.9
0.7π/T
0.9π/T
1π/T
1π/T
0.9π/T
0.1π/T
-0.4
-0.6
-0.8
-1
-1
0.8π/T
0.2π/T
0.7π/T
0.3π/T
0.6π/T
0.4π/T
0.5π/T
-0.5
0
0.5
1
1.5
Real Axis
Figure 3.8: Root Locus of Discretized Open-loop System for Several Sampling Intervals.
3.6.1
Stability Analysis
Apparently, in all cases there will be one pole outside the unit circle and that pole is associated
with the unstable pole of the continuous-time system. So, according to the stability section in
chapter 2, the discretized system is also unstable. If we want to be more precise on this statement,
it is the equilibrium associated with the upright position of the vehicle which is unstable. The
mapping of continuous time poles to their discretized counterparts is given by
zi = epi Ts ,
(3.19)
where Ts is the sampling interval.
A general rule is that a sampled system is controllable if the underlying continuous time system is controllable. The sampling interval plays a crucial role in this. It can be confirmed that
this rule holds for Ts ≤ 1s. Given the fast dynamics of this unstable vehicle, we expect to sample
faster than Ts = 1s. Therefore, the system is controllable and we will reasonably search for proper
controllers to drive the poles of the system inside the stability region (unit circle). Moreover, the
observability matrix has also full rank, so with proper observers we can estimate the states only
from the system’s outputs. It can be verified that the controllability and observability matrices
described by Equation 2.11 and Equation 2.12 respectively are invertible.
In order for the reader to get a better intuition, we take the extreme scenario where Ω = 0rpm.
Without spinning, the gyroscopes cannot generate any balancing torque. Therefore, it would be
impossible to control the system. Theory coincides with our intuition, and the controllability
matrix for Ω = 0rpm is not full rank. Thus, the system would be uncontrollable.
Chapter 4
Controller Design
“It doesn’t matter how beautiful your
theory is, it doesn’t matter how smart you
are. If it doesn’t agree with experiment,
it’s wrong.”
Richard P. Feynman, 1918-1988
In this chapter three different control techniques are studied. For the purposes of each control
strategy, the mathematical model describing the physical system is accordingly adjusted. Since
the cascade PID controller is thoroughly studied in chapter 5, we only explain the basics of the
cascade control here. We also provide a comparison of the cascade PID closed-loop performance,
based in the analytical model we derived in chapter 3 and the SimMechanics model we have
developed. The LQG and the Glover-McFarlane controllers, although promising, they are only
studied from a theoretical perspective as the time frame of the project was not long enough to
implement them as well.
4.1
Cascade Control Design
In this section the design of the cascade PID controller both for the SimMechanics model and the
mathematical model is presented. Regarding the mathematical model, we have merged the equations of motion (EOM) described in Equation 3.9 together with the DC motor dynamics described
in Equation 3.15. The servo dynamics are neglected in this section as more emphasis is given on
the comparison of the two models describing the mechanical system. A brief introduction in the
cascade PID logic might be necessary at this point.
What is important to understand here is the variables we are trying to control. In control problems
where we have one input and multiple measurement signals, cascade control is a reasonable choice.
Given that the true system has asymmetries, we do not know in advance the roll angle φ associated
with the equilibrium. On the other hand, it is a fact that on the equilibrium the roll-rate has to be
zero. So, the roll-rate φ̇ will be the first control variable. Since we want our vehicle to be able to
compensate disturbances, it seems reasonable that we should keep the precession angles as close to
zero as possible, so the gyroscopes will have enough space to precess and generate the stabilizing
counter-torque. Therefore, the precession angle α is the second control variable.
The inner loop, which is also called slave loop, is responsible for attenuating possible disturbances
in the roll rate φ̇. An external force (e.g., side wind) could yield back a sudden change in the roll
rate and the vehicle would change roll angle rapidly, thus, making it hard to get stabilized. By
letting the inner loop being faster than the outer one, we achieve the desired disturbance rejection.
The outer loop, which is also called master loop, is responsible for controlling the precession angle
25
26
4.1. Cascade Control Design
which is a slower process. By guaranteeing that the disturbance of the inner loop will not be
propagated to the outer loop we achieve better control of the precession angle.
In order to verify that the mathematical model of the previous chapter and the SimMechanics-based
model behave in a similar manner, simulations under the same initial conditions and disturbances
will be presented. Figure 4.1 and Figure 4.2 illustrate the respective Simulink models.
torque.mat
kg-cm
To File1
-KNm --> kg.cm
Torque generated
by the servos
x1 (roll angle)
rad to degrees
roll
0
PID(s)
PID(s)
Roll reference
PID Prec
PID Roll
-K-
input
precession
Gain5
x2 (precession)
roll angle (yellow)
precession angle (purple)
roll rate
Group 1
Fd
Signal 1
disturbance
precession rate
disturbance
Signal Builder2
Non-linear Vehicle
Vehicle roll rate feedback
-K-
Gyro precession feedback
Gain2
nonlinearPID.mat
To File
x
Figure 4.1: The Mathematical Model with Cascade PID.
-K-
Servo sign (rad)
q_vehicle
Error
w_vehicle
T_GyroCage1
w_spin_G2
q_precession_G2
Actual q
Vehicle roll angle (deg)
-KActual w
w_precession_G2
1
Vehicle roll velocity (deg/sec)
w_spin_G1
Gain1
DistForce_GyroBase
q_precession_G1
Gyro spin velocity (rpm)
w_precession_G1
Group 1 q
q sin
Square
Reference Roll
PID(s)
PID(s)
Controller Prec
Controller Roll
a_vehicle
Servo sign (rad)
q_fw_swing
-KGain2
q_rw_swing
T_GyroCage2
Group 1
t_torque_G1
t_torque_G2
Square1
ScaleModelAssembly
Reference Gyro
-K-
Gyro precession angle (deg)
Actual G1 q
-KActual G2 q
Gyro precession velocity (deg/sec)
-KActual G1 wp
Swing angle (deg)
-KActual G2 wp
Vehicle roll rate feedback
-K-
Precession torque (kg cm)
Output plots
q fw swing
-Kq rw swing
Gyro precession feedback
Ivan_angles.mat
To File
-Kt torque G1
-Kt torque G2
Figure 4.2: The SimMechanics Model with Cascade PID.
We now consider initial conditions x0 = [10o 0 0 0], Ω = 10000rpm ≈ 1047.19rad/sec. For the
mathematical model we assume a disturbing wind-force which is approximated as a force applied
on the center of mass of the vehicle (roughly 16N). The formula to calculate the wind force is
Chapter 4. Controller Design
27
described by
Fd =
1
ρCd Av 2 ,
2
(4.1)
where ρ ≈ 1.2041kg/m3 is the density of air at 20o , A is the exposed cross section, Cd ≈ 1 is the
drag coefficient and v ≈ 16m/s is the wind velocity (roughly 7 Beaufort). This equation is based
on fluid dynamics. For more details see [4].
Figure 4.3 and Figure 4.4 reveal that the two models perform in a similar manner.
Mathematical Model Simulation
20
deg
φ
0
-20
deg/sec
0
2
4
6
8
10
100
φ̇
0
-100
0
2
4
6
8
10
deg
50
αG1
αG2
0
-50
0
2
4
6
8
10
time(s)
Figure 4.3: Mathematical Model Performance.
Sim-Mechanics Simulation
20
deg
φ
0
-20
0
2
4
6
8
10
deg/sec
100
φ̇
0
-100
0
2
4
6
8
10
deg
50
αG1
αG2
0
-50
0
2
4
6
8
10
time(s)
Figure 4.4: SimMechanics Performance.
Our analytical model behaves in a similar manner compared to the SimMechanics model. The
main difference is the equilibrium roll-angle, which in the SimMechanics model is not exactly zero.
That is because we have introduced some asymmetry to mimic the asymmetry coming from the
servo motors. Since we wanted to keep our analytical model as simple as possible, we neglected that
detail. At this stage we have neglected the servo motors. In chapter 5 one can find the complete
design and implementation of the cascade controller. From now on, we will only be working with
the analytical model for our control analysis. The SimMechanics model can be used to visualize
the performance of the closed loop system based on a CAD model.
28
4.2. LQG Design for the Discretized LTI System
4.2
LQG Design for the Discretized LTI System
In this section LQG controllers will be designed, based on the analytical model described in Equation 3.16. Given that we do not have full access to the state vector, but only on the measurable
states, we need an observer to estimate the state vector. The Kalman filter is used for the LQG
design, which exploits the Kalman gain in stationarity. Then, after having estimated the complete
state vector, we can apply the standard LQR approach. The control nodes of this approach are
the weighting matrices of the respective cost function, with which we try to penalize the control
effort and the control error.
Based on Equation 3.18 consider that the stochastic case of the discretized system can be described by Equation 4.2.
x[k + 1] = Φx[k] + Γu[k] + Gw[k]
(4.2)
y[k] = Hx[k] + v[k]
where the process noise w[k] and the measurement noise v[k] are considered to be zero mean white
Gaussian random sequences (see Figure 4.5). That is:
w[k] ∼ N (0, Rw )
v[k] ∼ N (0, Rv )
and
where



Rw = 


σx21
0
0
0
0
0
σx22
0
0
0
0
0
σx23
0
0
0
0
0
σx24
0

0
0
0
0
σx25





and
Rv =
Φ
..
_
K*u
1
z
σy21
0
0
σy22
Γ
u[k]
1
x[k]
x[k+1]
K*u
2
x[k]
u[k]
.
._
C
w[k]
K*u
Process noise
Gain4
K*u
Random
Number
y[k]
...
1
y[k]
.1
D
K*u
Measurement noise
v[k]
K*u
Gain3
Random
Number1
....
Figure 4.5: Open Loop of Discrete-Time LTI System.
where σxi is the standard deviation of the ith state and σyj is the standard deviation of the j th
measurement. These can be seen as design variables that need tuning. Our encoders have an
accuracy of ±0.0573o ≈ ±0.001rad (included in the specifications), hence it is reasonable to choose
Chapter 4. Controller Design
29
σyj = 0.001. We consider that the process noise and the measurement noise are independent, so
T
the cross spectrum between w and v is constant and equal to zero (Rwv = Rwv
= 0). Having said
this, we can stabilize the system by using the control law
u[k] = −K x̂[k|k − 1] + Kr r[k],
(4.3)
where K is the gain derived by the LQR method such that there is an optimal balance between
system errors and control effort, and Kr r[k] is the feed-forward control, which can be neglected as
we consider zero reference roll signal. If we want to track a constant roll-reference signal other than
r[k] = 0 some slight modifications shall be done in the following analysis. A convenient property of
linear systems is the separation principle according to which we first estimate the state vector, and
we then use it to derive the linear quadratic regulator (LQR). More on the separation principle
can be seen in the following sections.
4.2.1
Kalman Filter
As stated before we only have access to the measurable states φ and α. Therefore, if we want
to stabilize the closed-loop system with the LQG method we need to estimate the state vector
beforehand. This can be done via observers. The LQG process uses the Kalman filter, which is
the optimal observer for systems interrupted by white Gaussian noise. Based on the state space
model described in Equation 4.2 we present the following Kalman filter algorithm.
Kalman Filter Algorithm
Prediction Step
x̂k+1|k = Φx̂k|k + Γuk
Pk+1 = ΦQk ΦT + GRw GT
Update Step
Mk+1 = Pk+1 H T [HPk+1 H T + Rv ]−1
x̂k+1|k+1 = x̂k+1|k + Mk+1 (yk+1 − H x̂k+1|k )
Qk+1 = Pk+1 − Pk+1 H T [HPk+1 H T + Rv ]−1 HPk+1
In the particular problem we have assumed that the matrix G in front of the process noise is the
identity matrix. Lk = ΦMk represents the Kalman gain and Qk , Pk denote the error covariance.
Actually, the difference (Pk+1 − Qk+1 ) represents the reduction in mean squared error (MSE ) of
the state estimate due to the information in yk+1 . The update equation for P is known as the
Riccati difference equation. The difference (yk+1 − H x̂k+1|k ) is known as the innovation of the
measurement signal. The Kalman filter algorithm converges asymptotically when the system is
detectable and that is why many prefer to use the Stationary Kalman gain directly instead. That
is we solve the Algebraic Riccati equation described by
P̄ = ΦP̄ ΦT + GRw GT − ΦP̄ H T [H P̄ H T + Rv ]−1 H P̄ ΦT
for the stationary error covariance matrix
P̄ = lim E[(x̂(k + 1|k) − x(k + 1))(x̂(k + 1|n) − x(k + 1))T ]
k→∞
(4.6)
30
4.2. LQG Design for the Discretized LTI System
and then we obtain the Kalman gain by
L = ΦP̄ H T [H P̄ H T + Rv ]−1 .
(4.7)
The only thing required for the above algorithm to run properly is some initial conditions for the
estimate of the state vector x̂(0|0) and for the error-covariance Q(0). We usually have some basic
knowledge and intuition on the system whose states we try to estimate. Given that we expect a
smooth and linear behaviour only around the equilibrium of the non-linear system, it would make
sense to consider the initial estimate of the state being on the vicinity of the equilibrium. Given
that this is only a guess, a large error covariance should be considered so the estimates could easily
“escape” from the possibly erroneous initial guess. That is


 o 
10 0 0 0 0
5
 0 10 0 0 0 
 0 







and
Q(0) = 
x̂(0|0) =  0 
 0 0 10 0 0  .
 0 0 0 10 0 
 0 
0 0 0 0 10
0
In order to get a better intuition on how the Kalman filter algorithm behaves depending on the
Rw and Rv see Figure 4.6. It is clear that it is the ratio Rv /Rw that controls the trade-off between
the tracking ability and the noise rejection capability of the filter.
Kalman Filter for Rv /Rw ≫ 1
20
15
φ
φ̂
15
10
degrees
10
degrees
Kalman Filter for Rv /Rw ≪ 1
20
φ
φ̂
5
5
0
0
-5
-5
-10
-10
0
1
2
3
4
5
0
time (s)
1
2
3
4
5
time (s)
Figure 4.6: Estimate of Kalman Filter for Different Rv /Rw Ratios.
Therefore, the following rule holds:
(i) Rv /Rw 1 ⇒ good tracking but noise sensitive
(ii) Rv /Rw 1 ⇒ bad tracking but less noise sensitive
So, the milestone of the Kalman filter is to find the “golden ratio” Rv /Rw such that not only
we trust the developed model, but we also take into account the information coming from the
measurements.
By assuming Rv /Rw ≈ 100, with σyi ≈ 0.001, we obtain the stationary Kalman gain


0.0920
0.0265
 0.0327
0.1097 



0.0027 
L∞ =  0.0057
.
 0.3609
0.0916 
−0.0476 −0.0122
Simulations verified these results. Figure 4.7 illustrates some indicative Kalman gains for comparison.
Chapter 4. Controller Design
31
Kalman Gains for Rv /Rw ≈ 100
1
Kalman Gains for Rv /Rw ≈ 100
8
L(1, 1)
L(1, 2)
L(5, 1)
L(5, 2)
7
0.8
6
0.6
5
4
0.4
3
0.2
X: 0.756
Y: 0.02648
2
X: 0.9
Y: 0.09195
1
X: 0.918
X: 0.774
Y: -0.0476 Y: -0.01158
0
0
-0.2
-1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
time (s)
0.6
0.8
1
time (s)
Figure 4.7: Convergence of the Kalman Gains.
It is worth-mentioning that, by using directly the steady state Kalman gain, our estimations will
be slightly off in the first steps of the process. For instance L(5, 1) takes very large values in the
first steps. That means that the respective sub-innovation (y1 [k] − x̂1 [k]) would be multiplied
with a very large number and the correction at the measurement update step of the Kalman
filter algorithm would be significant. Thus, we can claim that the on-line Kalman filter performs
better for the first iteration steps, and it tracks the states better than the stationary one. Due
to computational complexity though, we prefer to use the steady state Kalman gain which can be
calculated off-line. Figure 4.8 compares the roll-tracking quality of the Kalman estimator when we
use the time-varying and the stationary Kalman gain respectively.
Time-varying Kalman Filter for Rv /Rw ≫ 1
20
φ
φ̂
15
φ
φ̂
15
10
degrees
10
degrees
Stationary Kalman Filter for Rv /Rw ≫ 1
20
5
5
0
0
-5
-5
-10
-10
0
0.5
1
1.5
2
2.5
time (s)
3
0
0.5
1
1.5
2
2.5
3
time (s)
Figure 4.8: Time-varying vs. Stationary Kalman Gain Roll-Estimate.
4.2.2
LQR Controller
Now that we have estimated the states via the Kalman filter we can use Equation 4.3 to stabilize
the discretized LTI system described in Equation 4.2. At this point we shall introduce the criterion
that is to be minimized using the LQ theory for discrete time systems. So the quadratic criterion
is described by Equation 4.8.
min(kxk2Q1 +kuk2Q2 ) = min
∞ X
k=1
subjected to the state dynamics
x[k + 1] = Φx[k] + Γu[k]
xT [k]Q1 x[k] + uT [k]Q2 u[k] + 2xT [k]Q12 u[k]
(4.8)
32
4.2. LQG Design for the Discretized LTI System
For the purposes of this project we can neglect the cross product part, that is Q12 = 0. The control
design will be such that the minimization will be a trade off between the control error (e = y − r)
and the control effort (u). Q2 has to be a positive definite matrix and Q1 positive semi-definite,
and like Rw and Rv from the Kalman filter algorithm these are also parameters that need tuning.
In order for the above mentioned optimization problem to have a solution, the system (Φ, Γ) has
to be stabilizable and the system (Φ, H) has to be detectable. In chapter 3 we have proved that
the system is both controllable and observable. Therefore, both stabilizability and detectability,
which are weaker conditions, are satisfied. As stated in [10], the controller which is derived from
this process is assumed to be causal and it contains a time delay (i.e., u[k] may only depend on
y[k − 1], u[k − 1], y[k − 2], u[k − 2], ...). Having said these, the cost function is given by
min(kek2Q1 +kuk2Q2 ) = min
∞ X
eT [k]Q1 e[k] + (u[k] − u∗ [r])T Q2 (u[k] − u∗ [r]) ,
(4.9)
k=1
where u∗ [r] is considered to be a well-defined constant control signal which in stationarity and
absence of disturbances yields e[k] = 0.
Then, the control gain K from Equation 4.3 is described by
(4.10)
K = (ΓT SΓ + Q2 )−1 ΓT SΦ,
where S is the unique, positive semi-definite, symmetric solution to the matrix equation
S = ΦT SΦ − ΦT SΓ(ΓT SΓ + Q2 )−1 ΓT SΦ + H T Q1 H.
(4.11)
We can easily verify that (Φ − ΓK) has all its eigenvalues inside the stability region (unit circle). In
this particular project we want to stabilize the vehicle around its vertical orientation. That means
we try to force the roll angle φ to track the zero reference signal. As explained in Section 4.1 of the
present chapter, we should keep the precession angles as close to zero as possible. Simultaneously
we must keep the input signal (in Volts) less than an upper bound (e.g.,|Vmax |≤ 8V ). Bryson’s
rule [6] can be used to tune Q1 and Q2 . Although Bryson’s rule sometimes gives good results,
often it is just the starting point to a trial-and-error iterative design procedure aimed at obtaining
desirable properties for the closed-loop system.
Bryson’s Rule:
Select Q1 and Q2 diagonal with
Q1 (i, i) =
1
||xi ||2∞
i ∈ {1, 2, · · · , N1 }
Q2 (j, j) =
1
||uj ||2∞
j ∈ {1, 2, · · · , N2 },
which corresponds to the following criterion
Z
JLQR :=
0
∞
N1
X
i=1
Q1 (i, i)x2i (t) +
N2
X
Q2 (j, j)u2j (t)
j=1
At this point we should consider what each state represents and what a reasonable maximum value
for |xi (t)| would be. An acceptable range for vehicle’s roll angle would be (−30o , 30o ) which can
be translated into radians |φ|= |x1 |≤ 0.6981rad. In order to have the desired gyroscopic effect
from the gyroscopes, their precession angles should lie in (−45o , 45o ), or |α|= |x2 |≤ 0.7854rad.
Chapter 4. Controller Design
33
Moreover, the input is the voltage fed in the DC motors and according to their specifications we
have |V |= |u|≤ 7.4V .
Thus, Q1 , Q2 would be
2.0518
0
Q1 =
and
Q2 = 0.0183.
0
1.6211
What needs to be taken into consideration at this point is what we try to penalize in our cost
function. By using large penalties for x1 = φ and x2 = α we achieve the desired convergence
of the respective states close to zero. Simultaneously the small Q2 relative to the large values in
Q1 implies low penalty on the control effort and therefore the optimal control u[k] can be large.
Solving Equation 4.11 for S we derive the following control gains
− 8.7573
K = [−21.3656
− 0.3552
0.0110
0.0173].
Figure 4.9 illustrates the input-output sequences following the Bryson’s rule for sampling period
Ts = 0.018s.
(Ts = 0.018, Q2 = 0.018, Q1 (1, 1) = 2.05, Q1 (2, 2) = 1.62)
V
Volts
4
2
0
-2
0
1
2
3
4
deg
40
φ
α
20
0
-20
0
1
2
3
4
time (s)
Figure 4.9: Response of the Discretized LTI System with LQG-controller (Bryson’s rule).
We can verify that the eigenvalues of (Φ − ΓK) are inside the stability region




−0.7104 + 0.3620i
0.7973
 −0.7104 − 0.3620i 
 0.7973 




 ⇒ |λi (Φ − ΓK)|=  0.0027 
0.0027
λi (Φ − ΓK) = 




 0.9838 + 0.0145 
 0.9839 
0.9838 − 0.0145
0.9839
However, as stated above, Bryson’s rule is only used to give us a basic intuition and it rarely yields
back the optimally performing controller. In fact the input only reaches a peak of 4V while there
is plenty of space up to 8V . That is why it takes longer to converge to the equilibrium. So, this
is not the optimal control effort. Trying to stay within the range of each signal we focus on fast
convergence of the roll and precession angles at zero. Simultaneously we try to keep the control
effort below the allowed maximum voltage of the DC motor. Keeping this in mind we come up
34
4.2. LQG Design for the Discretized LTI System
with the following Q1 , Q2
Q1 =
7500
0
0
4000
and
Q2 = 1,
which apparently lead to better closed-loop performance. Solving Equation 4.11 for S we derive
the control gains
− 33.4308
K = [−60.4333
− 0.6870
− 0.0148
0.0699].
Indeed the eigenvalues of (Φ − ΓK) are inside the stability region (unit circle) with



λi (Φ − ΓK) = 


−0.6224 + 0.2080i
−0.6224 − 0.2080i
0.0188
0.8142
0.9436


0.6562
0.6562
0.0188
0.8142
0.9436




 ⇒ |λi (Φ − ΓK)|= 







,


and they are faster compared to the ones derived from Bryson’s rule.
Figure 4.10 illustrates the input-output sequences for sampling period Ts = 0.018s.
(Ts = 0.018, Q2 = 1, Q1 (1, 1) = 7500, Q1 (2, 2) = 4000)
8
V
Volts
6
4
2
0
-2
0
1
2
3
4
φ
α
deg
20
0
-20
0
1
2
3
4
time (s)
Figure 4.10: Response of the Discretized LTI System with LQG-controller.
4.2.3
LQG Controller
Figure 4.11 which is taken from [17], illustrates the closed loop system with the LQG controller.
Based on the analysis that is conducted above we can now determine explicitly the linear controller
that minimizes the quadratic criterion described in Equation 4.8. The controller will be described
by
Chapter 4. Controller Design
35
Figure 4.11: The LQG controller Scheme [17].
(4.12)
u[k] = −F (q)y[k],
where q is the discrete time shift operator, F (q) is the transfer function of the LQG regulator
and, given that we reconstructed the states of the system based on the Kalman filter, it can be
calculated from
F (q) = K(q15 − A + BK + LC)−1 L.
(4.13)
By using Equation 4.13 we calculate the stable proper transfer matrix


 
5.606q 5 +3.8586q 4 −3.609q 3 −3.952q 2 +0.2447q
F1 (q)
q 5 −0.2721q 4 −0.8727q 3 +0.02312q 2 +0.2476q−0.00504

=
F T (q) = 


5
4
3
2
4.904q +3.151q −3.5q −3.613q +0.2245q
F2 (q)
5
4
3
2
q −0.2721q −0.8727q +0.02312q +0.2476q−0.00504
Finally, the control input u[k] can be expressed by
u[k] = θT ξ[k]
where θ are the parameters depended directly on the numerator and denominator of the transfer
function F (q) of the LQG regulator, and ξ[k] is the vector containing the corresponding regressors


y1 [k − n]
ξ[k] =  y2 [k − n] 
, n = 1, 2..., 5 ,
u[k − n]
where y1 is the measurement corresponding to the roll angle φ of the vehicle, and y2 is the measurement corresponding to the precession angle of the gyroscopes. So, the input signal can be
described by
h
1
−0.2476u[k−1]+0.2447y1 [k−1]+0.2245y2 [k−1]−0.02312u[k−2]−3.952y1 [k−2]−
0.00504
− 3.613y2 [k − 2] + 0.8727u[k − 3] − 3.609y1 [k − 3] − 3.5y2 [pk − 3] + 0.2721u[k − 4]+
i
+ 3.858y1 [k − 4] + 3.151y2 [k − 4] − u[k − 5] + 5.606y1 [k − 5] + 4.904y2 [k − 5]
u[k] =
36
4.2. LQG Design for the Discretized LTI System
Closed Loop System
The closed-loop system with the developed LQG controller is described by
(4.14a)
(4.14b)
x[k + 1] = Φx[k] + Γu[k] + w[k]
y[k] = Hx[k] + v[k]
u[k] = −K x̂[k|k − 1]
(4.14c)
x̂[k + 1|k] = Φx̂[k|k − 1] + Γu[k] + L(y[k] − H x̂[k|k − 1]).
(4.14d)
If we introduce x̃[k] = x[k] − x̂[k|k − 1] to be the error between the true and the estimated state
vector, the closed-loop system can be described by

x[k + 1]


Φ − ΓK
ΓK
0
Φ − LH
=

x̃[k + 1]

x[k]


15
+

x̃[k]


0
 w[k] + 
15

 v[k].
(4.15)
−L
Now it can be easily verified that the dynamics of the system are described by (Φ − ΓK) and
(Φ − LH) separately. This is the so called separation principle. Of course the dynamics in this
case are of order 2n.
4.2.4
Further Tuning of the Parameters
As shown in the previous sections there are five parameters requiring tuning. These are the
sampling period Ts , the Kalman filter noise variances Rw , Rv and the weighting matrices Q1 , Q2
from the discrete-time loss-function. In this project we can make some strong assumptions with
respect to the measurement noise, given that we know the specifications of the encoders we are
using. So, we know that the accuracy of the encoders is roughly ±0.05deg ≈ ±8.7 · 10−4 rad. That
is σy1 ≈ σy2 ≈ 8.7 · 10−4 . Regarding the uncertainty coming to our process, we tried to show some
more trust in our model, so, we assumed less process noise. That would be σx1 = σx2 = 4.3 · 10−4 ,
σx3 = σx4 = σx5 = 8.7 · 10−5 . Keeping that in mind we can try several combinations of sampling
period and weighting matrices to investigate the response of the closed-loop system. However,
before doing so, we determine four measures of quality which make a closed-loop response adequate.
(i) Asymptotic stability
(ii) Overshoot (|φ|< 30o , |α|< 45o )
(iii) Settling time (tset ≤ 5s)
(iv) Law frequency control signal with |V |< 8V
Figure 4.12-4.14 illustrate the optimal responses of the closed loop system for several sampling
intervals. Each case required its own tuning of the weighting matrices in order for the cost function
to get minimized.
Chapter 4. Controller Design
37
(Ts = 0.5, Q2 = 0.01, Q1 (1, 1) = 6000, Q1 (2, 2) = 200)
(Ts = 1, Q2 = 0.1, Q1 (1, 1) = 600, Q1 (2, 2) = 30)
V
0
0
-2
-2
0
20
40
60
0
80
200
φ
α
5
10
15
20
φ
α
100
100
deg
deg
V
2
Volts
Volts
2
50
0
0
-100
0
20
40
60
80
0
5
10
time (s)
15
20
time (s)
Figure 4.12: Optimal Response for T = 1 and T = 0.5.
(Ts = 0.25, Q2 = 0.01, Q1 (1, 1) = 2500, Q1 (2, 2) = 900)
V
4
Volts
Volts
(Ts = 0.1, Q2 = 0.01, Q1 (1, 1) = 250, Q1 (2, 2) = 370)
V
2
0
2
0
-2
-2
0
2
4
6
8
10
80
φ
α
60
40
20
0
0
-20
2
4
6
8
2
3
4
5
φ
α
20
-20
0
1
40
deg
deg
0
10
0
1
2
time (s)
3
4
5
time (s)
Figure 4.13: Optimal Response for T = 0.25 and T = 0.1.
(Ts = 0.01, Q2 = 0.3, Q1 (1, 1) = 600, Q1 (2, 2) = 330)
(Ts = 0.05, Q2 = 0.01, Q1 (1, 1) = 80, Q1 (2, 2) = 65)
6
8
V
2
0
4
2
0
-2
-2
0
1
2
3
4
40
0
5
φ
α
1
2
3
4
φ
α
20
20
5
deg
deg
V
6
Volts
Volts
4
0
0
-20
-20
0
1
2
3
time (s)
4
5
0
1
2
3
4
5
time (s)
Figure 4.14: Optimal Response for T = 0.05 and T = 0.01.
In order to meet the above-mentioned specifications one could choose any sampling period below
0.1s. We shall not forget the time required for computations, so, the sampling period cannot be
arbitrarily small. It is worth-mentioning that our CPU should be powerful enough to calculate
and yield back the control signal in less time than the sampling period. So, for the time, since we
have not determined the specifications coming from our hardware, any sampling period below 0.1s
seems to meet our specifications.
38
4.2. LQG Design for the Discretized LTI System
4.2.5
Penalize Change in Control Signals
Usually LQG controller generates high frequency control signals. One way to deal with such kind
of problems is to try to penalize the change in the control signals by including this requirement in
the loss function. If we introduce ∆u[k] = u[k] − u[k − 1], then the loss function becomes
min(kx̄k2Q1 +k∆uk2Q2 ) = min
∞ X
x̄T [k]Q̄1 x̄[k] + ∆uT [k]Q̄2 ∆u[k]
(4.16)
k=1
subjected to the new state dynamics

 

 

x[k + 1]
Φ Γ
x[k]
Γ

=

+
 ∆u[k]
u[k + 1]
0 1
u[k − 1]
1
with

Q1
0
Q̄1 = 

(
and

0
Q̄2 =
Q2
penalty for changes in control
0, conventional LQG problem
.
A conclusion that can be drawn from this analysis is that the LQG control is very similar to the
pole placement approach. The only difference is that in the LQG case, it is the cost function
described by Equation 4.9 that decides where the poles of the closed loop should be placed, in
order for the cost function to be minimized. Given that we consider sampled systems, the closedloop poles cannot be placed anywhere arbitrarily. Figure 4.15 illustrates the poles of the closed
loop system for each of the above mentioned cases. The basic drawback of this controller is that
we cannot guarantee for robustness of the closed loop system, even if we have chosen the optimal
Kalman gain L and the optimal feedback gain K seen in Equation 4.15. The stability margin may
in principle be arbitrarily poor. Therefore, if the true system is slightly different than the modelled
one, then there are no guarantees neither for the performance nor for the stability of the closed
loop system. For this reason, we proceed with the analysis of a robust controller based on the
Glover-McFarlane analysis.
Closed-loop Zero-Pole Map
1
0.6π/T
0.8
0.5π/T
0.4π/T
0.6
0.8π/T
0.4
Imaginary Axis
0.9π/T
0.2
0
T=1
T=0.5
0.1
0.3π/T
T=0.25
0.2
T=0.1
0.3
T=0.05
0.2π/T
0.4
T=0.01
0.5
0.6
0.7
0.1π/T
0.8
0.9
0.7π/T
1π/T
1π/T
-0.2
0.9π/T
0.1π/T
-0.4
-0.6
-0.8
0.8π/T
0.2π/T
0.7π/T
0.3π/T
0.6π/T
-1
-1
-0.5
0.5π/T
0.4π/T
0
Real Axis
0.5
1
Figure 4.15: Root locus of discretized closed loop system for several sampling intervals.
Chapter 4. Controller Design
4.3
39
Glover-McFarlane Controller
Before proceeding with the design of the particular controller we have to assure that we are using
the minimal realization of the system; that is a system with the minimum number of states. It
can be confirmed from the state space model developed in chapter 3 that the second column of
the transition matrix A of the system is a zero-column. That means that the second state, which
corresponds to the precession angle of the gyroscopes, does not affect the dynamics of the linearized
system. Therefore, the particular state can be removed, thus reducing the order of the system.
The only drawback of this model-reduction is that we cannot any longer control this variable. The
reduced-order model then is given by the state space model in Equation 4.17.
0


ẋ(t) = 

y(t) =
1
0
gk6
k1 +k7 +k4
2ΩIG33
k2
0
0
1
0
0
0
0
where xT (t) = [φ φ̇ α̇
0
G33
− k12ΩI
+k7 +k4
γ
− k2
− KLm
0
0
Ke
k2
−R
L



0



 x(t) +  0  Vin (t)
 0 

1
L
(4.17)
x(t)
i] is the new state vector.
For convenience we write the system described in Equation 4.17 in the form shown in Equation
4.18.
ẋ(t) = Ax(t) + Bu(t)
y(t) = Cx(t)
(4.18)
This system has three stable poles and one unstable pole. Though unstable, the system is controllable according to the controllability analysis that is made in chapter 2. Before proceeding with the
derivation of the Glover-McFarlane controller, we shall first define our specifications. If we want
our closed loop system to attenuate high frequency noise, we have to define a cut-off frequency.
During the following analysis we consider fc = 1Hz. Having defined this, the desired crossover
frequency of the loop gain can be approximated to be ωc ≈ 6.28rad/s. In order for the reader to
get a better intuition, according to [10], the sensitivity function S, the complementary sensitivity
function T and the sensitivity to input disturbances Gwu are defined as
S = (I + GF )−1
T = (I + GF )−1 GF
S+T =I
Gwu = −(I + F G)−1 F
The challenge of the loop shaping process is the so called water-bed effect. We make each sensitivity function small at a certain frequency-range, but at the cost of having a peak somewhere else.
With small S, disturbances and model errors have little impact on the output. Respectively, with
small T measurements disturbances (usually high frequency noise introduced by the sensors) have
insignificant impact on the output.
For the sake of completeness we present the method (taken from [10]) we use to derive our robustified controllers. In this method, the reason we reduced the order of our system is revealed in the
first step.
40
4.3. Glover-McFarlane Controller
Robust Controller Algorithm
1. Solve the Riccati equations
AZ + ZAT − ZC T CZ + BB T = 0
AT X + XA − XBB T X + C T C = 0
for positive definite matrices X and Z. Each equation has a unique
solution as long as Equation 4.18 is a minimal realization.
2. Let λm be the largest eigenvalue of XZ
3. Introduce
p
(1 + λm )
1
R = I − 2 (I + ZX)
γ
L = BT X
γ=α
K = R−1 ZC T
Here α is a scaling factor, slightly larger than 1. The controller is
now given by
x̂˙ = Ax̂ + Bu + K(y − C x̂)
u = −Lx̂
or in input-output form
F (s) = L(s1 − A + BL + KC)−1 K
Based on [10] and on the algorithm described above, we derive the controller that stabilizes the
system in a “maximally robust way”.
F (s) =
s4
−201.2s3 − 18120s2 − 1.2 · 106 s − 5.7 · 107
+ 170.6s3 + 1.345 · 104 s2 + 7.847 · 105 s + 2.379 · 107
The optimal gamma that we take when applying the Glover-McFarlane technique is γ = 2.74. If
we take the infinity norm of the closed loop transfer function we verify that
||Gec ||∞ = 1.758 ≤ γ
where


Wu (s)Gwu (s)
Gec (s) =  −WT (s)T (s) 
WS (s)S(s)
Since the above inequality holds, it also follows that
|WS (iω)S(iω)|≤ γ,
∀ω
|WT (iω)T (iω)|≤ γ,
∀ω
|Wu (iω)Gwu (iω)|≤ γ,
∀ω
where WS , WT , Wu are weighting functions and S, T , and Gwu denote the sensitivity function,
the complementary sensitivity function and the sensitivity of the system in input-disturbances
Chapter 4. Controller Design
41
respectively. For more details see [10]. Figure 4.18 illustrates the sensitivity functions and the
loop-gain characteristics. The cross-over frequency of the loop-gain is ωc = 1.38 rad/s.
Sensitivity Functions and Loop-Gain
Magnitude (dB)
50
0
S
T
L
-50
-100
-150
Phase (deg)
180
0
-180
-360
-540
10-2
10-1
100
101
102
103
Frequency (rad/s)
Figure 4.16: Sensitivity Functions without Compensation.
Figure 4.17 shows the response of the closed loop system for different initial conditions.
Input
Input
0.4
Volts
Volts
V
0.2
0
-0.2
0
2
4
6
8
0.8
0.6
0.4
0.2
0
-0.2
10
V
0
2
4
6
8
10
Output
15
10
5
0
-5
20
φ
deg
deg
Output
φ
10
0
0
2
4
6
8
10
0
2
4
time (s)
6
8
10
time (s)
Input
Volts
1
V
0.5
0
0
2
4
6
8
10
deg
Output
30
20
10
0
φ
0
2
4
6
8
10
time (s)
Figure 4.17: Closed-loop response with Glover McFarlane for Different Initial Conditions without
Compensator.
Apparently the controller stabilizes the system in all three cases, but the settling time is too
large. In order to deal with this we should try to increase the bandwidth of the loop-gain. An
easy way to do so, without increasing the complexity of the controller, is to introduce a constant
pre-compensator gain.
42
4.3. Glover-McFarlane Controller
4.3.1
Constant Gain Precompensator
Assume W (s) = 3. The Glover-McFarlane controller then becomes
F (s) =
s4
−193.8s3 − 17450s2 − 1.172 · 106 s − 5.508 · 107
+ 224.5s3 + 1.87 · 104 s2 + 1.147 · 106 s + 4.148 · 107
Figure 4.18 illustrates the sensitivity functions and the loop-gain characteristics. The cross-over
frequency of the loop-gain is now ωc = 2.448 rad/s.
Sensitivity Functions and Loop-Gain
Magnitude (dB)
100
0
S
T
L
-100
-200
-300
Phase (deg)
180
0
-180
-360
-540
-2
10
10
0
10
2
10
4
Frequency (rad/s)
Figure 4.18: Sensitivity Functions with compensation W1 = 3.
Figure 4.19 shows the response of the closed loop system for different initial conditions.
Input
Input
Volts
0.4
Volts
1
V
0.2
0
V
0.5
0
-0.2
0
1
2
3
4
5
0
1
2
4
5
Output
15
10
5
0
-5
20
φ
deg
deg
Output
3
φ
10
0
0
1
2
3
4
5
0
1
2
time (s)
3
4
5
time (s)
Volts
Input
V
1.5
1
0.5
0
0
1
2
3
4
5
deg
Output
30
20
10
0
φ
0
1
2
3
4
5
time (s)
Figure 4.19: Closed-loop response with Glover-McFarlane and Constant Compensator W1 = 3.
If we want to shape a faster loop gain without increasing the complexity of our robust controller, we
can just use a constant pre-compensator with larger gain, i.e.: W2 = 10. The sensitivity functions
Chapter 4. Controller Design
43
are illustrated in Figure 4.20. The loop gain indeed becomes faster with crossover frequency
ωc = 6.1 rad/s, but at the cost of allowing for more high frequency noise. This interpretation
can be verified from the complementary sensitivity function T which is responsible for the high
frequency noise. On the other hand, we suppress more aggressively the input-disturbances, which
are low frequency signals. This can be seen from the sensitivity function S. The fact that we cannot
suppress both T and S around the same frequencies is known in the literature as the water-bed
effect.
Sensitivity Functions and Loop-Gain
Magnitude (dB)
100
0
S
T
L
-100
-200
-300
Phase (deg)
180
0
-180
-360
-540
10-2
100
102
104
Frequency (rad/s)
Figure 4.20: Sensitivity Functions with Compensation W1 = 10.
The closed-loop response corresponding to this controller is presented in Figure 4.21.
Input
Input
V
Volts
Volts
1
0.5
0
0
1
2
3
4
3
2
1
0
-1
5
V
0
1
2
Output
4
5
Output
6
20
φ
4
deg
deg
3
2
φ
10
0
0
0
1
2
3
4
5
0
1
2
time (s)
3
4
5
time (s)
Volts
Input
V
4
2
0
0
1
2
3
4
5
deg
Output
30
20
10
0
φ
0
1
2
3
4
5
time (s)
Figure 4.21: Closed-loop response with Glover-McFarlane and Constant Compensator W2 = 10.
If we discretize the above mentioned controller with a sampling period Ts = 0.01s, then controller
44
4.3. Glover-McFarlane Controller
will be implementable as a filter by our CPU. The discrete time controller is shown below
F (z) =
z4
−1.112z 3 − 2.14z 2 − 1.65z − 0.4533
− 1.84z 3 + 1.451z 2 − 0.4134z + 0.015
For the sake of completeness we present the response of the discrete-time robustified controller
for the three cases, assuming that we have a pulse reference signal to track (see Figure 4.22).
Apparently the third controller tracks the reference signal better.
Input
Input
0.4
Volts
Volts
0.2
0
V
-0.2
0.2
0
V
-0.2
-0.4
0
5
10
15
20
25
30
0
5
10
15
20
25
30
Output
4
2
0
-2
-4
φ
rφ
φ
rφ
2
deg
deg
Output
0
-2
0
5
10
15
20
25
30
0
5
10
time (s)
15
20
25
30
time (s)
Input
Volts
1
0
V
-1
0
5
10
15
20
25
30
Output
φ
rφ
deg
2
0
-2
0
5
10
15
20
25
30
time (s)
Figure 4.22: No compensation (upper left), W1 = 3 (upper right), W2 = 10 (lower center).
So, with the third Glover-McFarlane controller we achieved good response and we forced the
closed loop system to have ||Gce ||∞ = 1.12 ≤ γ ≈ 1.62. That was easily done by just using a precompensator W , without having to tune separate weighting functions for each sensitivity function,
a process which usually leads to conflicts.
However, the steady state error is not zero. Moreover, although the controllers with the constant gain pre-compensator increase the crossover frequency, and thus the speed of the loop gain,
they on the other hand, do not have satisfactory performance with respect to the sensitivity functions. In particular, the sensitivity function S is not suppressed significantly in the low frequencies,
which means that the closed loop system will not be able to reject disturbances robustly. A lead
compensator could yield better results, but at the cost of increasing the order of the controller.
4.3.2
Lead-lag Precompensator
We introduce the lead-lag pre-compensator
Wp = 10
(s + 5) (s + 2)
(s + 10) s
We deliberately selected a lag term with pure integral, so we can drive the steady state error
of the system to zero. We focus on the desired crossover frequency ωc ≈ 6rad/s. That way,
Chapter 4. Controller Design
45
we can compare the performance of this controller with the one derived above for the W2 = 10
compensator. Figure 4.23 illustrates how the control scheme we work with looks like.
Figure 4.23: Robust Control Scheme - One Degree of Freedom.
Following the algorithm described above, we derive the robust controller
F (s) =
s6
−424.6s5 − 4.261s4 − 2.96 · 106 s3 − 1.473 · 108 s2 − 1.277 · s − 9.521 · 108
+ 413.9s5 + 4.201 · 104 s4 + 2.906 · 106 s3 + 1.365 · 108 s2 + 9.603 · 108 s + 1.339 · 109
Apparently, the complexity of the controller increased, but the performance of the closed-loop system gets improved accordingly. In order to make the controller implementable, we could discretize
it similarly to the constant pre-compensator case. Figure 4.24 presents the sensitivity functions,
the step response of the closed loop system, the response with initial roll-angle φ(0) = 25o , and the
tracking ability of our controller. We achieved a crossover frequency ωc = 5.92rad/s. Furthermore,
we achieved an optimal γ = 1.72 with better suppression of the sensitivity functions in the desired
regions. The characteristics of the step response can be seen in Table 4.1.
Input
Sensitivity Functions and Loop-Gain
Volts
Magnitude (dB)
100
0
S
T
L
-100
0
V
-0.1
-0.2
-200
0
2
4
-300
0
-180
10
1
φ
rφ
0.5
0
-360
-540
10-2
100
102
0
104
2
4
6
8
10
time (s)
Frequency (rad/s)
Input
Input
6
4
2
0
1
V
Volts
Volts
8
1.5
180
deg
Phase (deg)
360
0
V
-1
0
1
2
3
4
5
0
5
10
Output
15
20
25
30
Output
30
20
10
0
-10
φ
φ
rφ
2
deg
deg
6
Output
0
-2
0
1
2
3
time (s)
4
5
0
5
10
15
20
25
30
time (s)
Figure 4.24: Sensitivity functions (upper left) step response (upper right), initial roll angle response
(lower left) and tracking ability (lower right).
46
4.3. Glover-McFarlane Controller
Left Coprime Factorization
The controller derived above can stabilize a family of systems. Here we will see how 1/γ defines a
measure of how large the set of stabilizable systems can be. Having used the parameters shown in
table.4.2, at the end of this chapter, we evaluate the transfer function of the system.
−1.807 · 105
s4 + 89.37s3 + 5987s2 + 2.8 · 105 s − 1.875 · 105
G(s) =
The system G can be represented in its factored form as shown in Equation 4.19.
G(s) = Ml−1 (s)Nl (s)
MlT (−s)Ml (s)
+
(4.19a)
NlT (−s)Nl (s)
(4.19b)
=I
Therefore, we derive the stable transfer functions
Ml (s) =
Nl (s) =
s4 + 89.37s3 + 5987s2 + 2.8 · 105 s − 1.875 · 105
s4 + 90.95s3 + 6129s2 + 2.895 · 105 s + 2.604 · 105
s4
+
90.95s3
−1.807 · 105
+ 6129s2 + 2.895 · 105 s + 2.604 · 105
The controller we derived for the lead-lag compensator can guarantee stability for the family
of systems written in the form
Gp (s) = (Ml (s) + ∆M (s))−1 (Nl (s) + ∆N (s))
where ∆M and ∆N are stable transfer functions such that
k[∆N
∆M ]k∞ <
1
γ
We will now test 4 cases with different perturbing functions ∆N , ∆M . It will be verified that as
k[∆N ∆M ]k∞ approaches 1/γ, the system reaches its stability margins.
Case 1: ∆N = 0.3/(s + 2), ∆M = 0.2/(s + 3)
k[∆N
∆M ]k∞ = 0.16 < 1/γ ≈ 0.5795
In this case the perturbed system Gp became a nonminimum phase system, having a zero in the
right half plane. We expect to see that the gain of the system for rapid inputs (like step), has
opposite sign than the static gain. So, as we notice in Figure 4.25, the output does not track the
reference signal for a very short time in the beginning. The characteristics of the step response
can be seen in Table 4.1.
Input
Bode Diagram
0.1
nominal
perturbed
-50
Volts
Magnitude (dB)
0
-100
0
V
-0.1
-0.2
-150
0
2
4
0
-90
-180
10 -2
6
8
10
Output
deg
Phase (deg)
-200
90
10 -1
10 0
10 1
10 2
Frequency (rad/s)
10 3
10 4
1.5
1
0.5
0
φ
rφ
0
2
4
6
time (s)
Figure 4.25: Case 1 - Bode diagram (left) step response (right).
8
10
Chapter 4. Controller Design
47
Case 2: ∆N = 0.5/(s + 2), ∆M = 0.4/(s + 3)
k[∆N
∆M ]k∞ = 0.2833 < 1/γ ≈ 0.5795
In this case the perturbed system Gp became a nonminimum phase system as well. Since the
respective infinite norm is slightly larger, but still within the robustness limits, we expect to
observe a worse response compared to the one in the previous case (see Figure 4.26). From the
bode diagram we can also notice that the perturbed system differs more than the nominal one,
compared to the previous case. The characteristics of the step response can be seen in Table 4.1.
Input
Bode Diagram
nominal
perturbed
-50
Volts
Magnitude (dB)
0
-100
0.1
0
-0.1
-0.2
-150
V
0
2
4
-200
90
6
8
10
Output
deg
Phase (deg)
2
0
-90
-180
10 -2
1
φ
rφ
0
10 -1
10 0
10 1
10 2
10 3
0
10 4
2
4
6
8
10
time (s)
Frequency (rad/s)
Figure 4.26: Case 2 - Bode diagram (left) step response (right)).
Case 3: ∆N = 0.5/(s + 1), ∆M = 0.4/(s + 3)
k[∆N
∆M ]k∞ = 0.5174 < 1/γ ≈ 0.5795
In this case the perturbed system Gp became a nonminimum phase system also. Since the respective
infinite norm is very close to the robustness limits, the response of this case is expected to be even
worse compared to the previous two cases (see Figure 4.27). The characteristics of the step response
are also presented in Table 4.1.
Input
Bode Diagram
0.2
nominal
perturbed
-50
-100
Volts
Magnitude (dB)
0
V
0
-0.2
-150
0
5
10
15
20
Output
2
0
deg
Phase (deg)
-200
90
-90
-180
10 -2
1
φ
rφ
0
10 -1
10 0
10 1
10 2
10 3
10 4
Frequency (rad/s)
0
5
10
15
20
time (s)
Figure 4.27: Case 3 - Bode diagram (left) step response (right).
Case 4: ∆N = 0.58/(s + 1), ∆M = 0.4/(s + 3)
k[∆N
∆M ]k∞ = 0.5951 > 1/γ ≈ 0.5795
In this case we exceed the stability margins, thus, we expect to have an unstable behaviour. In
Figure 4.28 we observe the unstable oscillations.
48
4.3. Glover-McFarlane Controller
Input
Bode Diagram
1
nominal
perturbed
-50
Volts
Magnitude (dB)
0
V
0.5
0
-0.5
-100
-150
0
5
10
15
20
25
30
Output
0
deg
Phase (deg)
-200
90
-90
-180
10 -2
10 -1
10 0
10 1
10 2
10 3
10 4
φ
rφ
4
2
0
-2
-4
0
5
10
15
20
25
30
time (s)
Frequency (rad/s)
Figure 4.28: Case 4 - Bode diagram (left) step response (right).
Table 4.1 illustrates the step response characteristics of each case described above for the lead-lag
precompensator. We denote with tr the rising time, which is the time it takes for the output to
reach from 10% to 90% of its final value. We denote with tset the settling time, which is the time
required for the output to enter and remain within a specified error band, here ±5% of its final
value. Finally, we denote with M the overshoot, which is the occurrence of the output exceeding
its final value, expressed in percentage.
System
G
Gp case 1st
Gp case 2nd
Gp case 3rd
Gp case 4th
Closed Loop
stable
stable
stable
stable
unstable
tr (s)
0.213
0.3
0.356
0.613
-
tset (s)
2.2
2.15
3.3
8.87
-
M (%)
19.5
33
57.5
68.6
-
Table 4.1: Step Response Characteristics for Robust Controller
4.3.3
Two-degrees of Freedom Controller with Lead-lag Precompensator
One way to improve even more the performance of the robustified controller designed above is to
increase its degrees of freedom. So far, we had a one-degree of freedom controller. By introducing
a pref-filter as shown in Figure 4.29, we improve the response of the feedback system to changes
in the reference signal we try to track.
Figure 4.29: Robust Control Scheme with Pre-filter.
If we choose
Fr =
3
s+3
Chapter 4. Controller Design
49
to be the low-pass pre-filter, then we reduce the overshoots in the response. Figure 4.30 illustrates
the response of the closed-loop system with one-degree of freedom controller and two-degrees of
freedom controller respectively.
Input
Input
0.2
Volts
Volts
1
0
V
5
10
15
20
25
V
-0.2
-1
0
0
30
0
5
10
15
Output
φ
rφ
0
-2
30
φ
rφ
0
-2
0
5
10
15
20
25
30
0
5
10
time (s)
15
Volts
0
V
-0.2
2
4
25
30
Input
-0.1
0
20
time (s)
Input
Volts
25
2
deg
deg
2
6
8
0.02
0
-0.02
-0.04
10
V
0
2
4
Output
6
8
10
Output
1.5
1.5
1
deg
deg
20
Output
φ
rφ
0.5
φ
rφ
1
0.5
0
0
0
2
4
6
time (s)
8
10
0
2
4
6
8
10
time (s)
Figure 4.30: Tracking ability with lead-lag precompensator without prefilter (upper left) and with
prefilter (upper right) and step response without prefilter(lower left) and with filter (lower right).
We notice that the overshoot is significantly reduced when we use the pre-filter (M ≈ 13%), but
at the cost of slightly increasing the rise time and the settling time of the output. It depends on
what is more important for each case. Sometimes we want to penalize overshoots, whereas some
others we prefer to have a fast response with larger overshoots.
In this particular project our task is to stabilize the static vehicle starting from some initial rollangle. The aim is not to track a reference signal, but instead to reject disturbances and stabilize
the vehicle given some initial conditions. However, we present the above for the sake of a more
thorough analysis.
We have already revealed the necessity of driving the precession angles of the gyroscopes to zero.
Although we designed the controller based on the reduced order system, we can freely apply it on
the complete 5th -order system described in Equation 3.16. So, we only feed back the roll angle,
but we still have two outputs. Figure 4.31 illustrates the response of the system for two different
initial conditions.
50
4.3. Glover-McFarlane Controller
Input
Input
V
V
4
Volts
Volts
2
1
0
2
0
0
2
4
6
8
10
0
2
4
Output
8
10
Output
40
20
φ
α
10
deg
deg
6
0
φ
α
20
0
0
2
4
6
time (s)
8
10
0
2
4
6
8
10
time (s)
Figure 4.31: Glover-McFarlane for φ(0) = 10o (left) and φ(0) = 20o (right).
It is noteworthy that the gimbal angle is also driven to zero even though we do not directly control
it with the stabilizing controller we derived above. The full system is a SIMO system whose
transfer function is described by Equation 3.17. Figure 4.32 illustrates the control scheme for the
Glover-McFarlane controller for the full system case.
Figure 4.32: Glover-McFarlane for full SIMO system.
The closed loop transfer function associated with the roll angle of the vehicle φ is
H1 (s) =
G1 (s)F (s)W (s)
,
1 + G1 (s)F (s)W (s)
whereas the closed loop transfer function with respect to the gimbal angle α is
H2 (s) =
G2 (s)F (s)W (s)
.
1 + G1 (s)F (s)W (s)
The real part of all poles of H1 is strictly negative. It also holds that kH1 k∞ ≈ 1.24. Therefore,
the roll angle φ will asymptotically converge to zero for this regulator problem. H2 has the same
poles as H1 plus one pole at zero. So, we have marginal stability for the the closed loop system
H2 . Nevertheless, under the regulator-problem circumstances, H2 behaves in a stable manner.
Hence, the gimbal angle α converges to zero as well. Further investigation is required for the servo
problem, which is out of the scope of this thesis.
In order to evaluate the sensitivity of the closed loop system, we disturbed the input with two
consecutive disturbances. Figure 4.33 illustrates the response of the system under two different
input-disturbances. We can claim that the vehicle under the Glover-McFarlane controller is insensitive to input-disturbances. Further investigation is required with respect to the robustness in
perturbation of the nominal SIMO system described by Equation 3.17.
Chapter 4. Controller Design
51
Input
Input
5
V
Volts
Volts
5
0
-5
V
0
-5
0
5
10
15
0
5
Output
20
deg
deg
φ
α
0
-20
5
15
Output
40
0
10
10
15
time (s)
40
20
0
-20
-40
φ
α
0
5
10
15
time (s)
Figure 4.33: Glover-McFarlane sensitivity for different disturbances.
If one would like to have a direct access in controlling the gimbal angle, one could use a nonlinear
switching controller. By determining a condition for which the alternative control signal, dedicated
to the gimbal angle, is applied, we guarantee that the gimbal angle converges to zero in the way we
want it to. The system then becomes hybrid, and it has two discrete states, each one describing
the same system, but under different control signals. The condition that tells us which discrete
state we are in, is known in the literature as guard, and it can take the form
Guard = (|φ|≤ +
1
AN D
|φ̇|≤ +
2 ).
If this condition is satisfied, then we can apply a controller of the form
u = −Kα sign(α),
where the factor Kα regulates how slowly we will drive the gimbal angle back to zero. The particular guard we have used guarantees that in case the roll rate of the vehicle would drastically change
(disturbance occurrence), the controller would instantaneously switch to the Glover-McFarlane,
before it would be too late for the roll angle to react. Further research is required for this switching controller, but it goes beyond the scope of the current analysis.
Finally, in order for our simulations to be repeatable, we provide the values of the parameters we
used for our simulation setups (see Table 4.2).
Parameter
[mB , ms , mGB , mC , mG ]
[hB , hs , hG ]
[IBx , Isx , IGBx ]
[ICx , ICy , ICz ]
[IGx , IGy , IGz ]
g
Ω
Value
[1.82, 0.074, 0.053, 0.112, 0.276]
[7, 9.2, 12]
[135.97, 0.527, 0.725]
[1.288, 1.288, 2.534]
[1.56, 1.56, 3.12]
9.81
4000
Unit
[kg]
[cm]
[kg · cm2 ]
[kg · cm2 ]
[kg · cm2 ]
[m/s2 ]
[rpm]
Table 4.2: Parameter Values of Static Vehicle for Simulation Setups
52
4.3. Glover-McFarlane Controller
Chapter 5
Controller Implementation
“Failed plans should not be interpreted as
a failed vision. Visions don’t change, they
are only refined. Plans rarely stay the
same, and are scrapped or adjusted as
needed. Be stubborn about the vision, but
flexible with your plan.”
John C. Maxwell, 1831-1879
In this chapter we will explain how the cascade PID controller described in chapter 4 can be
implemented. The whole approach is based on our effort to link the simulated in Matlab cascade
PID controllers with the ones we will implement in C++. For this reason, we accordingly modify
the model describing our system. Different PID setting are investigated and the influence of
each gain is studied thoroughly. Moreover, the simulated controller appears to reject successfully
different kind of disturbances.
5.1
Revised Equations of Motion
Although the equations of motion have already been presented in chapter 3, we now present a
different reasoning which will simplify our calculations and will also be proven very useful for the
cascade loop analysis and implementation. For the sake of convenience we re-write the vehicle’s
equations of motion.
φ̈[k4 + k7 + k1 c2 α + k3 s2 α] − 2k5 φ̇α̇cαsα + 2Ωcαα̇IG33 − k6 gsφ = hB Fd cφ
(5.1)
The above equation can be written as in Equation 5.2 which is first expressed in this form in [24].
φ̈ =
k6 gsφ + hB Fd cφ
2k5 φ̇cαsα − 2ΩcαIG33
+
α̇
k4 + k7 + k1 c2 α + k3 s2 α k4 + k7 + k1 c2 α + k3 s2 α
(5.2)
Note at this point that the precession angle (α) of the gyroscopes is entirely depended on the
PWM signal we are feeding the servo motor with. So, the precession rate (α̇) can be considered as
the input of the system we are trying to describe, and the precession angle its integral. Remember
from chapter 2 that the higher the precession rate is, the larger the generated stabilizing torque
from the gyroscopes becomes. Therefore, the model summarizing the system dynamics can be
described by the nonlinear control-affine system shown in Equation 5.3.
φ̈ = f (φ, α) + g(φ̇, α)u
(5.3a)
y = [φ, α]
(5.3b)
53
54
5.1. Revised Equations of Motion
where u = α̇ and:
f=
k6 gsφ + hB Fd cφ
k4 + k7 + k1 c2 α + k3 s2 α
g=
2k5 φ̇cαsα − 2ΩcαIG33
k4 + k7 + k1 c2 α + k3 s2 α
Here Fd represents the disturbance and the unmodeled system dynamics.
5.1.1
Acceptable Range of Initial Roll Angle
In order to estimate the range of the initial roll angle within which we can stabilize the vehicle
we shall do the following. First, we have to approximate the maximum stabilizing torque the
gyroscopes can produce. For the sake of convenience we re-write the equation describing the
stabilizing torque in Equation 5.4.
τstab =
1
2
mG RG
Ωα̇ cos(α)
2
(5.4)
We have assumed α̇max ≈ 5rad/s (see A) and RG = 4.75cm. Thus, the maximum stabilizing
torque generated by the two gyroscopes together is
τstabmax ≈ 1.3N m.
Now, consider that the vehicle is leaned with a roll angle φ(0). The analogous of the vehicle is the
inverted pendulum as shown in Figure 5.1.
Figure 5.1: The Inverted Pendulum Analogous.
If we assume that the center of mass of the vehicle can be represented by the mass mtot in Figure
5.1, then the torque produced by the weight of this mass is given by Equation 5.5.
(5.5)
Tp = mtot g sin(φ)l
where l denotes the length of the pendulum, and it represents the distance between the center of
mass of the vehicle and the ground. We have considered l = 0.1m and mtot ≈ 3kg. In order to be
able to neutralize the weight-torque, the following should hold.
φ(0) ≤ arcsin
τ
stabmax
mtot gl
Therefore, |φ(0)|max ≈ 26o .
Chapter 5. Controller Implementation
5.2
55
Cascade PID Control - Implementation
The developed Simulink model describing the cascade PID strategy is shown in Figure 5.2. For
more details on what the block KTH-Vehicle consists of see Appendix B.
Figure 5.2: Block Diagram of the Cascade PID controller.
Assuming that our mathematical model represents with satisfactory accuracy the physical system,
the most important thing to be determined is the spinning velocity of the gyroscopic flywheels.
We have to assure that the one we use in simulations is very close to the one we apply in reality.
This parameter affects massively the gains of the PID controllers, especially the inner one. The
higher the spinning velocity, the smaller the P gain of the inner loop has to be. This coincides
with the theory presented in chapter 2. To achieve certain gyroscopic momentum we can either
use high spinning velocity and smaller precession rate or vice versa. Although we do not have an
encoder to measure the spinning velocity of the gyroscopic flywheels, it is fair enough to consider
in our analysis that Ω ≈ 4000rpm. For more information on this choice see Appendix A.
Our aim at this stage is to establish a one-to-one relation between the Simulink model and the
C++ code we develop in parallel. In other words we try to find the relationship between the
PID gains we identify in Simulink and the ones that should be used in the C++ code. The
sensor loop of the C++ code takes approximately 10ms to get executed. Thus, we consider a
10ms transport-delay for the measurements in our Simlink model. Due to the particular spinning
velocity of the gyroscopic flywheels, we can only attenuate short disturbances, and only after the
gyros have returned close to their horizontal orientation, so they can have enough space to rotate
and generate the necessary balancing counter-torque. For better performance we should be able
to apply larger spinning velocities. Figure 5.3 illustrates the response of the system given two
different spinning velocities, Ω1 = 4000rpm and Ω2 = 8000rpm respectively. Different tuning of
the PID controllers was required for each case, as Ω is a parameter that affects directly the whole
system. For a fair comparison we initialize our vehicle from φ(0) = 15o , and we also apply two
consecutive disturbances.
56
5.2. Cascade PID Control - Implementation
Cascade PID Response (Ω = 8000 rpm)
20
φ
α1
α2
deg
deg
Cascade PID Response (Ω = 4000 rpm)
40
20
0
-20
-40
φ
α1
α2
0
-20
0
5
10
15
0
5
Input
15
Input
20
10
rad/s
α̇
rad/s
10
0
-20
α̇
0
-10
0
5
10
15
0
time (s)
5
10
15
time (s)
Figure 5.3: PID-Response for Ω = 4000rpm(left) and Ω = 8000rpm(right).
Should one want to test how the system reacts in disturbances, one should first let it get stabilized
at its upright position, with the gyroscopes having almost horizontal position, and then apply a
disturbance. Apparently, with higher spinning velocity the precession angles do not have to be
that large in order for the gyroscopes to produce sufficient balancing torque. Thus, we can get a
more robust response for larger initial tilt-angle and while perturbing the system more aggressively.
Unfortunately, the motors we use are not powerful enough to yield back such high spinning velocity,
but we still present this for the sake of completeness and for the sake of a more thorough comparison.
5.2.1
Discrete-Time PID Controller
A typical computer implementation of a discrete time PID controller considers forward Euler approximation for the discretization of the integral part, and backward Euler approximation for the
derivative part. Moreover, a low pass filter is used for the derivative term in order to attenuate
high frequency noise coming from the encoders. A PID algorithm is described below.
Ideal PID Algorithm - Computer Implementation
P-part
yp [k] = e[k]
I-part (Forward Euler)
yi [k] = yi [k − 1] + Ki Ts e[k]
D-part (Backward Euler)
h
yd [k] = Kd yd [k − 1] +
e[k] − e[k − 1]
i
Ts
− yd [k − 1]
1/N + Ts
Ts
PID output
uP ID [k] = Kp (yp [k] + yi [k] + yd [k])
By taking the Z-transform of the last expression of the above algorithm one can derive the PID
described by Equation 5.6, which is the one we used for the inner and the outer loop. It is also the
PID one can find in Simulink.
1
N
UP ID (z) = Kp 1 + Ki Ts
+ Kd
E(z)
(5.6)
z
z−1
1 + N Ts z−1
Chapter 5. Controller Implementation
5.3
57
Cascade PID Control - Tuning
The way we tuned the cascade PID controllers is the following. We first tuned the inner loop while
having the outer loop in manual mode (open loop). Then, considering the inner loop as part of the
plant, we closed and tuned the outer loop. One rule of thumb is to keep the inner loop at least five
times faster than the outer one, so as for the disturbances to be attenuated before occurring in the
output. Furthermore, we usually use a P or a P I controller for the inner loop. Considering high
frequency disturbances, the derivative term in the inner PID controller would cause stability problems. In general, a P I controller would drive the steady state error (offset) to zero and it would
also track set points and reject disturbances better. However, such a sophisticated controller for
the slave-loop might have the tendency to oscillate. Thus, it might be wiser to start only with P
for the inner loop, at the cost of having a small steady state error. More on the cascade control
can be found in [2].
Table 5.1 shows eight different PID settings and their respective performance. We have defined
three measures of quality to characterize the respective performances. tφset and tαset represent
the times until the roll angle of the vehicle and the gyroscopic angle respectively, enter and never
exceed the error band ±0.5o . The third measure of quality is the maximum precession angle of
the gyroscopes and we denote it with |α|max . The quality of the performance decays as |α|max
approaches the physical boundaries. The sampling period of our discrete-time PID controllers in
Simulink is Ts = 1ms. It is directly linked to the speed of the control loop of our C++ code. Given
that we tried to avoid reaching close to the boundaries of the precession angles, we initialized our
vehicle from a tilted position that never exceeded the range (−18o , 18o ). At this comparison stage
we have neglected the external disturbances. Figure 5.4 and Figure 5.5 illustrate the behaviour of
each setting so that the reader can get an intuition on the effect of each PID gain.
In general, not only we want to stabilize the vehicle on its upright position, but we also want
to drive the gyroscopes to their horizontal position as fast as possible, so they can compensate
disturbances that might occur. However, we have to be careful on how fast we will drive the
gyroscopic angles to zero because we want to avoid producing significant counter-torque at this
stage. Otherwise, we would harm the stability of the vehicle.
Case
1
2
3
4
5
6
7
8
Loop
Inner
Outer
Inner
Outer
Inner
Outer
Inner
Outer
Inner
Outer
Inner
Outer
Inner
Outer
Inner
Outer
Kp
800
1.2
800
1.2
800
1.2
900
1.2
900
1.2
900
1.2
1100
1.2
1200
1.3
Ki
0.09
0.1
0.09
0.4
0.3
0.1
0.3
0.1
0.3
0.7
1
0.7
1
0.7
0.6
Kd
0.4
0.4
0.4
0.4
0.4
0.4
0.4
0.4
N
8
8
8
8
8
8
8
8
|α|max (deg)
39.5
tφset (s)
2.4
tαset (s)
7.8
40
4.5
8.5
39.6
2.1
5.2
38.6
1.97
5.9
39.5
4.6
6.5
39.8
3.8
5.35
38.3
2.8
5.2
37.7
2.3
4.65
Table 5.1: Cascade PID Performance for Different Settings
58
5.3. Cascade PID Control - Tuning
Cascade PID Response
Cascade PID Response
50
50
φ
α1
α2
deg
deg
φ
α1
α2
0
-50
0
5
10
0
-50
15
0
time (s)
Cascade PID Response
5
10
50
50
φ
α1
α2
deg
deg
φ
α1
α2
0
-50
15
time (s)
Cascade PID Response
0
5
10
0
-50
15
0
5
time (s)
10
15
time (s)
Figure 5.4: Cascade PID responses corresponding to Case 1(upper left), Case 2(upper right), Case
3 (lower left) and Case 4 (lower right).
Cascade PID Response
Cascade PID Response
50
50
φ
α1
α2
deg
deg
φ
α1
α2
0
-50
0
5
10
0
-50
15
0
time (s)
Cascade PID Response
5
10
Cascade PID Response (Ω = 4000 rpm)
50
50
φ
α1
α2
deg
deg
φ
α1
α2
0
-50
15
time (s)
0
5
10
time (s)
15
0
-50
0
5
10
15
Figure 5.5: Cascade PID responses corresponding to Case 5(upper left), Case 6(upper right), Case
7 (lower left) and Case 8 (lower right).
These simulation results coincide with the PID theory. Whenever we increase the integral gain
Chapter 5. Controller Implementation
59
Ki the settling time is decreased but at the cost of larger overshoots. Furthermore, the more
we increase the Kp gain the faster the response becomes up to a certain extent where we do not
have oscillations. Finally, the derivative term is responsible for suppressing the overshoot from the
integrator, but at the cost of introducing some oscillations. It is also verified that only P gain for
the inner loop (case 8) led to a less oscillatory response with a steady state error eφ ≈ 6 · 10−4 .
5.3.1
Disturbance Rejection Test
Depending on what one considers more important, one can choose any setting from the Table 5.1.
Here we will test the robustness of the 8th case when a disturbance occurs. We will consider two
types of disturbances. The first one will be an impulse-disturbance (i.e., someone pushes the vehicle
instantaneously), whereas the second will be a more persisting one (i.e., side-wind). Figure 5.6
illustrates the performance in each case. We also present the respective disturbances (see Figure
5.7), so the reader will have a better understanding of how aggressive they are. Our simulated
vehicle manages to confront both types of disturbances and it gets stabilized in its upright position.
Therefore we can claim that the system successfully rejects disturbances.
Cascade PID Response (Ω = 4000 rpm)
Cascade PID Response (Ω = 4000 rpm)
50
50
φ
α1
α2
deg
deg
φ
α1
α2
0
-50
0
-50
0
5
10
15
0
5
10
15
Figure 5.6: Cascade PID response under impulse disturbance (left) and under persisting disturbance (right).
Disturbance
10
5
0
0
5
10
time(s)
Disturbance
15
Force (N)
Force (N)
15
15
10
5
0
0
5
10
15
time(s)
Figure 5.7: Impulse disturbance (left) and persisting disturbance (right).
5.4
Enhanced Model with Asymmetry Included
As we have already discussed in chapter 3, the servo motors brake the asymmetry of our system.
If we consider the potential energy described in Equation 3.6, we take a more accurate differential
60
5.4. Enhanced Model with Asymmetry Included
equation describing the particular system. Equation 5.2 can now be re-written as
0
k gsφ + hB Fd cφ + k8 gcφ
2k5 φ̇cαsα − 2ΩcαIG33
φ̈ = 6
+
α̇,
k4 + k7 + k1 c2 α + k3 s2 α
k4 + k7 + k1 c2 α + k3 s2 α
(5.7)
where
0
k6 = hB mB + 2hG (mG + mC + mGB ) + 2hs ms cφs
k8 = 2hs ms gsφs ,
(5.8)
with φs denoting the angle of the servos with respect to the yaw axis, when the vehicle is stabilized
on its upright position. With this asymmetry introduced in our model we expect to have an
equilibrium different than zero. Therefore, the cascade PID controller, designed and implemented
above, will stabilize the system on the roll angle associated with the revised equilibrium. Figure
5.8 verifies this hypothesis, which also coincides with what we expect to happen in practice. Table
5.2 presents the setup we used for plots appearing in Figure 5.8 as well as the characteristics of the
responses, based on the analysis conducted above. We have assumed φ(0) = 15o and φs = 45o .
Case
9
10
Loop
Inner
Outer
Inner
Outer
Kp
750
1.1
1150
1.2
Ki
1.5
0.7
1
0.6
Kd
0.45
0.42
N
8
8
|α|max (deg)
37.13
tφset (s)
3.68
tαset (s)
5.18
33.96
2.86
3.07
Table 5.2: Cascade PID Gains with asymmetry introduced
Cascade PID Response (Ω = 4000 rpm)
Cascade PID Response (Ω = 4000 rpm)
40
40
φ
α1
α2
20
deg
20
deg
φ
α1
α2
0
-20
0
-20
-40
-40
0
2
4
6
8
0
2
4
6
8
Figure 5.8: Cascade PID responses corresponding to case 9 (left) and case 10 (right).
Assuming α̇∗ = α∗ = 0, the roll angle on the equilibrium is described by
φ∗ = tan−1
−h F − k g B d
8
.
0
k6 g
(5.9)
Chapter 6
Experiments with the Prototype
“By denying scientific principles, one may
maintain any paradox.”
Galileo Galilei, 1564-1642
In this chapter we present some experimental results. We first identify the parameters of the servo
motors by taking some input-output data, a step of crucial importance for increasing the validity
of our model. Then we present the low-pass filter we used for the noisy encoder-measurements.
Finally, we try to tune the cascade PID controller for the C++ code, but since PID-tuning is a
time consuming process, this stage is not completed. The conclusions that can be drawn though
are quite promising.
6.1
Servo-Parameters Identification
In this section we will try to identify some of the parameters of the DC motor which is part of the
servo-mechanism. Figure 6.1 illustrates the sub-parts of the digital servo motor. For the sake of
completeness we present the DC motor Simulink model in Figure 6.2. The gains of the inner PID
controller are unknown to us, but we can estimate them together with the DC motor’s parameters
by considering the servo motor as a black box. Figure 6.3. shows the input-output relationship for
the servo motor. The input is a Pulse Width Modulated (PWM) signal. The width of the pulse
signal can be translated into position (rad) of the shaft attached on the DC motor. The convention
we are using is the following:
• 1ms is associated with -π/2 rad
• 1.5ms is associated with 0 rad
• 2ms is associated with π/2 rad
Figure 6.1: Block Diagram of the Servo Motor.
61
62
6.1. Servo-Parameters Identification
Figure 6.2: Block Diagram of the DC motor.
Servo input-output relationship (RC mode + Low Pass)
1.5
rα
α
1
rad
0.5
0
-0.5
-1
-1.5
0
500
1000
1500
2000
2500
3000
3500
Time(sec)
Figure 6.3: Input-Output relationship for the servo motor - Estimation.
The acquisition of the input-output data illustrated in Figure 6.3 is done with the remote controller. So, we observe what is the desired angle, determined from the width of the applied P W M
signal, and what the measured angle from the encoder is. At this point it should be mentioned
that we use an off-line low pass filter to cancel out the high-frequency noise, coming together with
our measurement data. The low-pass filter is an 10th order FIR filter.
Via Simulink we managed to identify the parameters of the DC motor together with the PID
gains. The data we used to fit this model are called estimation-data. The identified parame-
Chapter 6. Experiments with the Prototype
63
ters are presented in table.6.1 and they were calculated via the Parameter Estimation toolbox in
Simulink. The way this Toolbox works can be summarized as follows. First we initialize the parameters to be identified. Then we take some input-output data from our physical system and we feed
the same input into our simulated system. Since, the dynamics behave in a similar manner, what
we usually observe in the simulated output is a scaled version of the measured output. Finally,
the Parameter Estimation toolbox optimize the selected parameters so that we have a satisfactory
fit of the simulated and the measured output. Pservo is the servo gain that bounds the gyroscopes
within the range (−50o , 50o ). Since the gyroscopes are attached on the servo motors during the
identification process, the values of these parameters will change drastically if we substitute or
completely remove the gyroscopes.
Parameter
Moment of inertia
Armature’s Inductance
Armature’s Resistance
Damping Coefficient
Back EMF constant
P gain
I gain
D gain
Derivative Filter Coefficient
Servo-Gain
Symbol
J
L
R
γ
Ke
Kp
Ki
Kd
N
Pservo
Estimated Value
8.8112 · 10−6
7.388
0.65627
3.0971 · 10−4
0.10837
1.5215
0.0769
0.14319
8.0743
0.6
Unit
kg · m2
mH
Ω
N · m · rad/s
V · s/rad
-
Table 6.1: Servo Motor - Estimated Parameters
Figure 6.4 illustrates the performance of our model on new experimental data. Those data are
called validation data. We can claim that our model mimics well the true system.
Angle of the Shaft
0.5
rad
simulated
true
0
-0.5
0
500
1000
1500
2500
3000
3500
2000
2500
3000
3500
PWM input
1.8
Pulse-width (ms)
2000
1.6
1.4
1.2
1
0
500
1000
1500
time (s)
Figure 6.4: Input-Output relationship for the servo motor - Validation.
In general, the digital servo motor behaves in a similar manner as the analogue one with one main
difference; instead of feeding the DC motor with voltage signals every 20ms (50Hz), it instead feeds
its respective DC motor with voltage signals every 3.33ms (300Hz). The pulses will be shorter in
length of course, but with so many voltage pulses occurring, the motor will speed up much quicker
64
6.2. Filtering of the Measurements
and provide constant torque.
The modelled discrete PID controller that we assume it is inside the servo-mechanism is described
by Equation 6.1.
P ID(z) = Kp + Ki Ts
1
N
E(z)
+ Kd
z
z−1
1 + N Ts z−1
(6.1)
where E(z) is the Z-transform of the error e(t) = r(t) − y(t). For N → ∞ the derivative part is
the one we already know. But if we use some values within the range (1,20) then we successfully
cut-off the high frequency noise. In order to get a better intuition on how the filtered derivative
works, we present the bode diagram of the PID controller of the servo with and without derivative
filter (Figure 6.5).
Servo PID Bode Diagram
Magnitude (dB)
100
PID with derivative filter
PID with no derivative filter
50
0
-50
Phase (deg)
-100
0
-90
-180
10 -6
10 -4
10 -2
10 0
Frequency (rad/s)
Figure 6.5: Bode Diagram of PID controller.
6.2
Filtering of the Measurements
Although we have not spent that much time yet, the filtering of the measurements is equally important as the tuning of the cascade PID controller. The measurement corresponding to the roll
rate (φ̇) of the vehicle is filtered with the help of the AHRS (Attitude and Heading Reference
System) library which combines measurements from the accelerometer and the gyroscope, which
are inside the ArduPilot board. We will not go through all the details, but we should mention that
in general the measurements from an accelerometer are trustworthy when the gravitational force is
the only force applied on the accelerometer. If this is not the case a gyroscope is used to calculate
a high accuracy estimate of the acceleration. Many tutorials on how one can combine the above
mentioned together with IMU (Inertial Measurement Unit) devices can be found on the internet.
Regarding the measurements corresponding to the precession angles of the gyroscopes a low pass
filter must be used to attenuate the high frequency noise. Unfortunately every filter introduces
delay to the signal. The better attenuation of the noise one tries to achieve, the more delay one
introduces in the system. In this project we decided to use a low order FIR filter which very successfully managed to attenuate the high frequency noise coming from the DS37 Netzer encoders.
Chapter 6. Experiments with the Prototype
65
Before one uses a low pass filter, one first has to minimize the probability of vibrations into his
system. So, before proceeding in the filtering process we carefully tested that all the sub-parts of
the vehicle were mounted properly, in order to eliminate probable source of vibrations. Then, we
realized that a 4th order FIR filter was enough to attenuate the high frequency noise coming from
the encoders. Unfortunately, due to the limited amount of time, we have not managed to identify
the source of this noise, so inevitably we had to use the filter. The filter we used is a linear phase
Type I 4th order FIR filter and it can be described by Equation 6.2.
(6.2)
h[n] = {0.0362, 0.2592, 0.4092, 0.2592, 0.0362}
Figure 6.6 illustrate the impulse response and the transfer function of this filter.
FIR(4)
10 0
0.4
10 -1
|H(f )|(dB)
0.5
h[n]
0.3
0.2
FIR(4)
10 -2
10 -3
0.1
0
-3
-2
-1
0
1
2
3
10 -4
-0.5
0
0.5
normalized frequency
Figure 6.6: FIR(4) with Kaiser window.
We have used a Kaiser window to suppress the side-lobes that are caused due to the poor approximation of the ideal filter. For the sake of a more thorough comparison we have implemented the
filter both in Matlab and in C++ and we can compare the off-line with the on-line processing. In
Figure 6.7 we observe that they behave in a similar manner and they both introduce delay in the
filtered signal.
Precession angle G1 (fc = 1H z)
Precession angle G2 (fc = 1H z)
3
4
Original
F IR(4) − C + +
F IR(4) − M atlab
2
Original
F IR(4) − C + +
F IR(4) − M atlab
3
rad
rad
2
1
1
0
0
-1
-1
0
1
2
3
Time(sec)
4
5
0
1
2
3
4
5
Time(sec)
Figure 6.7: FIR(4) with Kaiser window for both gyroscopes.
Apparently the filtered signals from the C++ code (on-line filtering) and the Matlab code (off-line
filtering) coincide. Since we use a 4th order filter the delay is two samples (order/2). We tested
the filtering process of the precession angles in isolation and we realized that the average time
between two consecutive measurements was 6ms. Therefore the delay introduced by the filters is
66
6.3. Validation of the C++ code
roughly 12ms. Figure 6.8 displays a zoomed-in version of Figure 6.7 so that the reader can notice
the approximate 14ms delay introduced by the filter.
Precession angle G1 (fc = 1H z)
Precession angle G2 (fc = 1H z)
1
X: 3.723
Y: 0.92
0.95
X: 1.681
Y: -0.765
-0.75
X: 3.736
Y: 0.92
0.9
X: 1.694
Y: -0.765
rad
rad
-0.7
0.85
0.8
-0.8
Original
F IR(4) − C + +
F IR(4) − M atlab
0.75
0.7
Original
F IR(4) − C + +
F IR(4) − M atlab
-0.85
1.6
1.65
1.7
1.75
3.6
Time(sec)
3.65
3.7
3.75
3.8
Time(sec)
Figure 6.8: FIR(4) with Kaiser window for both gyroscopes.
6.3
Validation of the C++ code
Although we did not have enough time to experiment with the physical system, we verified that the
C++ code we developed behaves in the desirable manner. As we did with the theoretical analysis,
we first try to tune the slave loop. We have already explained the reasons we will try either a P
or a P I controller. In particular we will avoid the I term which led to oscillatory behaviour. The
control scheme will now look like Figure 6.9.
Figure 6.9: Block Diagram of the Cascade PID controller (master loop in manual mode).
The PWM signal we are feeding our servos comes directly from the output of the PID controller,
and it is a scaled version of the roll-rate signal (φ̇). The PWM values should be interpreted as the
desired angles the gyroscopes should move into, in order to generate the necessary balancing torque.
When we exceeded a certain value for the Kp gain, the behaviour of the vehicle started being very
oscillatory. We notice that this gain is far away from the respective gain of our theoretical analysis.
So, we cannot just use the gains we came up with in our simulated system. We only had time
to experiment with the slave loop. Figure 6.10 and Figure 6.11 illustrate how the inner loop
behaves for certain Kp gains. For Kp = 400 we observe that the PWM signal becomes a very high
frequency signal. There are two main reasons one would avoid such high frequency signals. First,
the servos’ temperature will increase rapidly with the danger to get burned. Second, the low pass
filter smoothens erroneously the precession-angle signal, and therefore we will feed back the wrong
Chapter 6. Experiments with the Prototype
67
signal in the master-loop at a later stage. So, out of this experiment we get that the inner loop
Kp gain should be Kp < 400.
Tuning of slave-loop with Kp = 150
Tuning of slave-loop with Kp = 50
5
φ̇
rad/s
rad/s
5
0
φ̇
-5
-5
0
2
4
6
8
0
10
2
4
6
8
10
4
6
8
10
6
8
10
PWM
ms
PWM
ms
2
2
1.5
X: 2.19
Y: 1.2
1.5
1
1
0
1
2
4
6
8
0
10
1
α
rad
rad
X: 2.19
Y: -2.141
0
0
2
α
X: 2.21
Y: -0.524
0
-1
-1
0
2
4
6
8
0
10
2
4
time(s)
time(s)
Figure 6.10: Tuning of slave loop for Kp = 50 (left) and Kp = 150 (right).
Tuning of slave-loop with Kp = 200
Tuning of slave-loop with Kp = 400
5
φ̇
0
rad/s
rad/s
5
X: 3.18
Y: 1
-5
φ̇
0
-5
0
2
4
6
8
10
0
2
2
ms
ms
X: 3.18
Y: 1.7
0
4
6
8
0
1
α
0
X: 3.2
Y: 0.296
-1
0
2
4
6
time(s)
8
10
8
10
2
4
6
8
10
2
4
6
8
10
1
10
rad
rad
2
6
1.5
1
1
4
PWM
PWM
1.5
2
α
0
-1
0
time(s)
Figure 6.11: Tuning of slave loop for Kp = 200 (left) and Kp = 400 (right).
Moreover, we can confirm that the control-loop of our C++ code lasts less than 1ms, whereas the
sensor-loop lasts roughly 20ms (6ms calculations + 14ms delay from the filter). It is reminded
here that the P W M signal is the output of the control-loop and the precession angle is the output
of the sensor-loop. We have just picked a random peak of the signal φ̇ and we checked when the
respective peak occurred in the P W M signal and in the measured-filtered precession angle α.
In order to confirm once more that our simulated model of the servo mechanism, behaves in a
similar manner as the true one, we feed our servo model with the P W M signals from the Kp = 50
and Kp = 150 cases respectively, and we generate the simulated angles. Figure 6.12 illustrates the
simulated angles against the true ones. Apparently the simulated signals have no high frequency
noise. Since we do not filter the simulated signals, there is no delay either. Given that we have
used a 4th order FIR filter, the measured signals have a two-samples (order/2) delay.
68
6.3. Validation of the C++ code
Tuning of slave-loop with Kp = 50
Tuning of slave-loop with Kp = 150
0.4
measured
simulated
0.4
0.2
rad
rad
0.2
0
0
-0.2
-0.2
measured
simulated
-0.4
-0.4
-0.6
0
2
4
6
time(s)
8
10
0
2
4
6
8
10
time(s)
Figure 6.12: Validation of Modelled Servo Motor.
Given the limited amount of time we had to experiment with the concept vehicle, we have not
managed to stabilize it. However, we have brought the project to the final stage, which is the
fine tuning of the cascade PID controller. The C++ code including the control loop, the sensor loop, and the communication with the ArduPilot ports to receive data from the encoders is
developed. Furthermore, one could implement and experiment with the LQG and Glover McFarlane controllers we have designed in chapter 4, and make a fair comparison with respect to the
performance of the physical system.
Chapter 7
Conclusions and Remarks
“In so far as a scientific statement speaks
about reality, it must be falsifiable; and so
far as it is not falsifiable, it does not speak
about reality.”
Karl Popper, 1902-1994
In this master thesis, the gyroscopic stabilization problem is studied thoroughly. We derive the
nonlinear equations of motion which describe the concept vehicle, using Lagrangian mechanics.
Since we only test the control dynamics of the static vehicle, we accordingly simplify the equations
of motion to fulfil our purpose. We linearize the dynamics about an equilibrium. After analyzing
the stability of the open loop system we verify that, although unstable, the system is controllable.
Then we design three controllers; a cascade PID controller, an LQG controller, and a GloverMcFarlane controller. For each one, we adjust properly the model of the vehicle. The LQG and
the Glover-McFarlane controller are only studied from a theoretical point of view. We implement
the cascade PID controller and we test its functionality on the experimental platform.
It is the first time such controllers are proposed for a small-scale gyro-stabilized amphibious commuter vehicle. Most state-of-the-art papers have been focusing on the state feedback controller,
the PID controller, Lyapunov-based controllers and on the sliding mode controller. In addition,
the cascade PID controller has never been used before to gyroscopically stabilize a two-wheeled
vehicle. Finally, by using two mirrored gyroscopes, we managed to produce double gyroscopic
momentum, whilst keeping the dimensions of the gyroscopes reasonably small.
With respect to the strategic objectives defined in the chapter 1, all of them are met except
for the last one. The fine tuning of a cascade loop requires significant amount of time. However,
before one reaches the PID tuning stage, one has to guarantee that he has properly setup and
synchronized all the rest beforehand. This is what we actually achieved in this project. Both a
thorough theoretical analysis, and the implementation of the respective C++ code, with fine tuning of the PID being the last task to be accomplished. In case one would like to take this project
to the next level, we provide some tips on what should be taken into account before proceeding.
7.1
Modelling and design-based considerations
(i) Backlash: In this project we neglect the backlash effect. In general, backlash is present
in most mechanical systems and it sometimes induces oscillations (limit circles). Backlash
appears in the hardware as an inability to switch the gimbal rate instantaneously. For more
information on how one can treat such nonlinearities see [13],[10],[20]. One suggestion for
the impatient reader is to introduce a phase lead compensation which is known as linear
controller design.
69
70
7.2. Hardware considerations
(ii) Anti-windup for the PID integrator: A careful reader must have already noticed that the
PWM signals we feed our servos with, are saturated due to physical constraints. When we
deal with saturated control inputs, we have to assure that the integration state in the PID
controller tracks the proper state. The integral windup of the inner loop can be handled
relatively easy. Unfortunately, the avoidance of windup in the outer loop is not the most
trivial task. More on the anti-windup and the tuning of cascade loops can be found in [2].
(iii) Servo motors’ Placement: One could change the current setup so that that servo motors do
not harm that significantly the symmetry of the vehicle. In most applications the gyroscopes
together with their respective power suppliers (servos) are placed such that the symmetry of
the system is not broken. One could also use counterweights to eliminate the effect of the
servos in the symmetry of our system. The design of the vehicle is still on an experimentation
stage, so there is plenty of space for improvements.
(iv) Vehicle’s Height: If we increase the height of the center of mass of the vehicle, then the
dynamics will become somewhat slower, thus easier to get stabilized. One way of doing this
is by using larger wheels. An analogous paradigm is the inverted pendulum. The length of
the pendulum plays an important role on the control effort required to stabilize the pendulum
on its unstable equilibrium.
(v) DC Motor: One could slightly change the design of the current setup by using more powerful
DC motors to spin the gyroscopic flywheels faster. In general as Ω increases the flywheel
becomes more resistant to a change in vehicle’s orientation and thus, the gyroscopes will
produce larger balancing-torques. Hence, we would be able to stabilize the vehicle at greater
initial tilt angle and in the presence of more aggressive disturbances.
7.2
Hardware considerations
(i) Hardware: Initially the cables coming from the encoders appeared to have problems due to
sensitive soldering on the respective printed circuit board (PCB). When we substituted the
sensitive PCB with a new one, we realized that it was not the board, which made our task
extremely difficult, but the very thin encoder-cables. One could use some silicone glue on
the PCB where the cables are soldered as well as a plastic cover to protect them. Worst case
scenario; buy new encoders with less sensitive cables. Furthermore, we had to recalibrate the
sensors in our code, as the chips we used for the new PCB were different. In general, one
should be aware that embedded programming and interaction with a physical system takes
significant amount of time.
(ii) Noisy and Delayed Measurements: When we tested the encoder-measurements on an Arduino
board there was no high frequency noise in the signals. Unfortunately, this was not the case
for the ArduPilot board. Given the limited amount of time we had, we did not manage to
detect the source of this noise. Thus, we decided to use a low order low-pass FIR filter to
compensate with this noise. The drawback of using an on-line lowpass filter is the introduced
delay, which, fortunately, was not that significant. Delays on systems with fast dynamics
should be minimized. A word of wisdom is to always put some time on detecting the source
of noise before proceeding with unnecessary signal-filtering.
7.3
Future Research
During my work on this project I have come up with some promising extensions. I will try to
summarize some of them.
(i) Trajectory Tracking: In this thesis we only studied the control dynamics of the static vehicle.
This is indeed the most challenging part of the active gyro-based roll-stabilizer. Imagine
yourself trying to balance on a static bicycle. After deriving the proper controller for the
Chapter 7. Conclusions and Remarks
71
static case (regulator problem), the next step is to explore the full dynamics of the vehicle
and track a trajectory (servo problem). The faster the speed of the vehicle is, the less
balancing torque will be required from the gyroscopes. Consider the bicycle case again.
(ii) Water-travelling: As we have stated in chapter 1, this project is part of the ongoing NEWT
project. Thus, after having guaranteed the stability of the vehicle on land, we can focus on
the water travelling. The vehicle shall be slightly modified to fulfil certain requirements for
this case. For instance a propeller should be installed to convert the rotational motion into
thrust and so enable the water mode of the concept vehicle. One might think that it is easier
to stabilize the vehicle on water. However, there are other parameters that should be taken
into consideration such as the waves and their periodicity.
(iii) Large-scale Vehicle: Having met all the above specifications, the next step is to study and
investigate gyro-based stabilizing controllers for a large scale prototype.
(iv) Gyroscopic Stabilization Applications: In this project we have focused on the transportation
perspective. However, there are several applications waiting for the gyro-stabilizers to be
exploited. Take for instance the gyroscopic camera stabilizers. Two gyroscopes can be used
to control the horizontal and vertical orientation of the camera. Self balancing robots can
also use gyro-stabilizers to hold a reference orientation.
72
7.3. Future Research
Bibliography
[1] Temitope Akinlua, Dmitry Dryga, Adam Gunnet, Jon Montague, and Elobuike Oji. Gyroscopic stabilization of unstable vehicles. 2007.
[2] Karl J. Åström and Tore Hägglund. PID Controllers: Theory, Design, and Tuning. Instrument
Society of America, Research Triangle Park, North Carolina, 1995.
[3] Benzos A.V., Formalky A.M., Gurfinkel E.V., Jicharev D.N., Lensky A.V., Sativsky K.V., and
Tchesalin L.S. Control of autonomous motion of two-wheel bicycle with gyroscopic stabilization. Proceedings of the 1998 IEEE International Conference on Robotics and Automation,
pages 2670–2675, 1998.
[4] G.K. Batchelor. An Introduction to Fluid Dynamics. Cambridge University Press, 1967.
[5] L. Brennan. Means for Imparting Stability to Unstable Bodies. US Patent No. 796893, 1905.
[6] A.E. Bryson. Control of Spacecraft and Aircraft. Princeton University Press, 1994.
[7] H. Cousins. The stability of gyroscopic signle track vehicles. Engineering, pages 678–681,
711–712, 781–784, 1913.
[8] J.M. Gallaspy. Gyroscopic stabilization of an unmanned bicycle. M.S. thesis, Electrical Engineering Department, Auburn University, AL, 2000.
[9] Torkel Glad and Lennart Ljung. Modeling of Dynamic Systems. Prentice Hall, Englewood
Cliffs, 1994.
[10] Torkel Glad and Lennart Ljung. Control Theory: Multivariable and Nonlinear Methods. Taylor
& Francis, 2000.
[11] P. Jorgensen. Gyroscopic stabilization of unstable dynamical systems. Research and Participants, Land, Air, and Space Robotics Laboratory Texas A&M University, College Station, TX
Aerospace Department, 2012.
[12] Dean Karnopp. Tilt control for gyro-stabilized two-wheeled vehicles. Vehicle System Dynamics, 37(2):145–156, 2002.
[13] Hassan K Khalil. Nonlinear Systems. Prentice Hall, Upper Saddle river, 2nd edition, 2002.
[14] Daniel Kee Young Kim, Kevin Bretney, and Andrew L. Tsang. Gyroscopic stabilized vehicle.
Patent No:8706390, Issued date: 22 April 2014.
[15] John G. Proakis and Dimitris G. Manolakis. Digital Signal Processing. Pearson Higher Education, 1996.
[16] A. Rao. Dynamics of Particles and Rigid Bodies: A Systematic Approach. Cambridge University Press, 2006.
[17] Karl J. Åström and Björn Wittenmark. Computer-Controlled Systems: Theory and Design.
Prentice Hall, 1984.
73
74
Bibliography
[18] P. Shilovsky. Gyroscope. US Patent No. 1,137,234, 1915.
[19] P. Shilovsky. The Gyroscope: its Practical Construction and Application. London, E. and
F.N. Spon; New York, Spon and Chamberlain, 1924.
[20] Jean-Jacques E. Slotine and Weiping Li. Applied Nonlinear Control. Englewood Cliffs, NJ:
Prentice-Hall, 1991.
[21] Stephen C. Spry and Anouck R. Girard. Gyroscopic stabilization of unstable vehicles: Configurations, dynamics, and control. 2008.
[22] S. Suntharasantic and M. Wongsaisuwan. Piecewise affine model and control of bicycle by
gyroscopic stabilization. Proceedings of ECTI-CON, pages 549–552, 2011.
[23] Denhard WG. Wrigley W., Hollister WM. Gyroscopic theory, design and instrumentation.
Cambridge: The MIT Press, 1969.
[24] Harun Yetkin, Simon Kalouche, Gegory Colvin, Keith Redmill, and Umit Ozguner. Gyroscopic
stabilization of unmanned bicycle. Proceedings of American Control Conference, 2014.
Appendix A
Hardware
In this project we used the Futaba servo motors as the first type of actuators, shown in Figure
A.1, to produce the stabilizing torques required to balance the vehicle in its equilibrium. The only
drawback is the high power consumption. For more details on the technical characteristics see
Futaba BLS172SV.
Figure A.1: Futaba Servo Motor.
From the technical characteristics one can find that at 7.4V this servo yields back a Tst =
37kgcm2 ≈ 3.63N m stall torque and a 0.11sec/60o ≈ 9.5rad/s no-load maximum speed. Some
basic torque-speed curve knowledge is required. We consider that on the operating point the servo
can yield back at maximum 5rad/s. This is just a draft approximation which can be used in the
analysis made in this project. Therefore, we can consider that the maximum precession rate of
the gyroscopes will be |α̇|max ≈ 5rad/s. For safety reasons the boundary values of the precession
angles are chosen to be ±45o .
The Netzer DS-37 encoders (Figure A.2) attached on the the opposite side of the DC motors
measure the angular velocity of the gyroscopes. For more details on the technical characteristics
of the encoders see Netzer DS 37.
75
76
Figure A.2: The Netzer DS-37 Encoder.
Moreover, a 14.8 battery (Figure A.3) is placed on the vehicle’s base in order to achieve autonomy.
For more details on the technical characteristics of the battery see Graupner.
Figure A.3: Graupner Battery.
The motor we used to control the spinning velocity of the gyroscopic flywheels is the MT3506 from
T-MOTORS series seen in Figure A.4. For the sake of completeness we provide a table with some
characteristics (see Figure A.5). Since we work with a 14.8V battery, we only focus on the lower
half of this table. As we typically run on half throttle, it seems reasonable to choose Ω ≈ 4000rpm
as our spinning velocity. This motor works as our second actuator. We first feed the DC motor
with a certain rpm value via a PWM signal. The DC motor accelerates the speed until the flywheel
reaches the desired velocity Ω. Then, the flywheel spins with velocity Ω. Given that we consider Ω
a constant parameter of our system, we have modified our C++ so there will be some delay before
the main-loop starts. It was found that four seconds were enough for the flywheels to reach the
desired spinning velocity. For more technical characteristics one could see MT3506.
Appendix A. Hardware
77
Figure A.4: The MT3506-Motor.
Figure A.5: MT3506-Table.
Finally the CPU we will base are calculations on will be an ArduPilot board presented in Figure A.6.
The particular board is massively used for applications related to control of vehicles, helicopters,
planes, ships, robots, etc.. For more details on the technical characteristics of the ArduPilot see
78
ArduPilot.
Figure A.6: The Ardupilot.
Appendix B
Simulink Models
CONTINUOUS GLOVER
0
ROLL REF
..
...
num(s)
den(s)
num(s)
den(s)
Glover
W1
num(s)
den(s)
.
-K-
plant
Manual Switch3
-K-
G4
DISCRETE GLOVER
-K- deg2rad
2
Step
Gain7
roll angle
x' = Ax+Bu
y = Cx+Du
State-Space
num(z)
den(z)
num(z)
den(z)
In1 Out1
Glover_d
W1d
DAC
2
Group 1
Voltage
Signal Builder1
Figure B.1: Glover Mc-Farlane Controller - Simulink model.
rad2deg
0
u[k]
-K-
y[k]
Roll Ref
Gain6 roll angle (yellow)
precession angle (purple)
Discretized Linearized Vehicle
contr1
My Kalman Filter
K_dlqr* u
xhat[k]
Discrete LQR
Controller
u[k]
yhat[k]
L[k+1]
y[k]
Discrete Stationary
Kalman-Filter
Figure B.2: LQG Controller - Simulink model.
79
80
1
Discrete
Transfer Fcn
Phi
xhat[k|k]
A_full_d* u
1
xhat[k]
xhat[k+1|k+1]
1
z
xhat[k+1|k]
B_full_d* u
u[k]
Gamma
L(k+1)*(y[k+1]-Hxhat[k+1|k+1])
H
D_full_d* u
C_full_d* u
Matrix
Multiply Matrix Multiply
D
2
3
yhat[k]
L[k+1]
y[k]-yhat[k+1]
2
Constant
y[k]
L_Kalman
L(k+1)
Figure B.3: Kalman Filter - Simulink model.
roll angle (yellow)
precession angle (purple)
1500
Precession Angle (rad)
Horizontal Position
Precession Rate (rad/s)
PWM limits
Complete SERVO SYSTEM
1
-K-
Gain4
PWM
DAC
rad2deg
0.5
0.5
Gain5
precession rate
roll angle
In1 Out1
PWM
precession angle
...
Precession Angle (rad)
3000
disturbance
Desired Position (pwm)
roll rate
Precession Rate (rad/s)
Mirrored signal
Vehicle Dynamics
Complete SERVO SYSTEM1
Transport
Delay
2
RollRate
Fd
Disturbance1
Product1
Transport
Delay1
1
Aver.PrecAngle
Group 1
Signal 1
Signal Builder1
0.7
Gain6
Figure B.4: KTH Vehicle - Simulink model.
Appendix B. Simulink Models
sin
u2
Trigonometric
Function
Math
Function
cos
u2
Trigonometric
Function1
Math
Function1
81
k3
Gain2
k1
Gain1
Add
k4+k7
Constant
1
2*Omega*I_G33
precession rate
VEHICLE DYNAMICS
Gain4
2
sin
2*k5
precession angle
Trigonometric
Function2
Product6
Divide
Gain3
Product4
cos
Add2
cosinus
sin
Product5
k6
sin5
Constant2
g
Gain7
d1
3
Product2
1
s
1
s
Integrator
Integrator1
1
roll angle
Add3
Constant1
2
disturbance
cos1
Add1
Divide1
Product3
cos
0
Product1
cos
Gain5
Constant3
.
g
Trigonometric
Function4
Constant4
k_asymmetry
Constant5
ASYMMETRY INTRODUCED
Figure B.5: Vehicle Dynamics - Simulink model.
servo_dist
roll rate
82