Download Sistema de Control de UAVs con aplicaciones a vuelo en formación

Tesis de grado de Ingenierı́a Electrónica:
Sistema de Control de UAVs con
aplicaciones a vuelo en formación
Author:
Claus M. Rosito
[email protected]
Director:
Dr. Ing. Juan I. Giribet
in the
Grupo de Procesamiento de Señales, Identificación y Control
Facultad de Ingenierı́a, Universidad de Buenos Aires
May 7, 2013
Abstract
In this work we consider the attitude control problem. We present a robust, almost
global, non linear attitude controller based on Lyapunov functions. It is shown to be robust against unmodeled perturbations induced by aerodynamic or random perturbations,
and against perturbations induced by noisy or faulty attitude and angular measurements
used to compute the control action. Then, bounds on the maximum sample time for
the discrete time (digital) implementation of the controller are found using Sample and
Zero Hold analysis. The bounds and the minimum attainable error are found to depend
on the choice of gains for the controller. This relationship is explored and bounds on the
sample time are found in terms of the bounds used for robustization. From this relationship, gains to achieve the desired error and sample time performance can be found.
Simulations are carried out to demonstrate the results on a quadrotor UAV. From a
systems engineering point of view, these results allow for higher level requirements in
attitude and angular velocity to be translated into lower level requirements on the digital
controller and the navigation subsystem as timing and error performance requirements
respectively.
El resultado acerca de la robustez del controlador y las cotas de
error en función de las cotas de las mediciones se lo dedico a
Cynthia.
El resultado acerca de las cotas en el tiempo de muestreo del
controlador se lo dedico a mis viejos, mi hermano y mi abuela.
El resultado acerca de la relación entre error y tiempo de muestreo
y su uso para la selección de las ganancias se lo dedico a mis
compañeros.
ii
Acknowledgements
Al Dr. Ing. Juan I. Giribet por su apoyo y también por la libertad que me brindó durante
el desarrollo de esta tesis. Gracias por la gran calidad como profesional y como persona.
A los jurados, Dr. Ing. Rafael Garcı́a, Dr. Ing. Alejandro Ghersin y Dr. Ing. José Luis
Mancilla-Aguilar, por hacerse el tiempo de leer y juzgar este trabajo.
Al Grupo de Sistemas Embebidos, su director Ariel Lutenberg, y el subgrupo del quadrotor, Alan Kharsansky, Ezequiel Espósito, Federico Roasio y Daniel Schermuk, por darme
la oportunidad de entrar en el tema de los cuadricópteros, viniendo yo de un background
totalmente diferente.
A los profesores y colegas que he tenido a lo largo de la carrera, y en particular a los que
con su pasión me incitaron a explorar el mundo de la matemática y sus rincones más
fascinantes, al Dr. Aberle, Marta Anaya, Sebastián Grynberg y Daniel Prelat.
A Cynthia por ser una persona maravillosa, por todas las cosas lindas que vivimos juntos
y por todas las que vendrán.
A mi familia por el apoyo incondicional.
A los compañeros con los que compartı́ tantas aventuras en el camino.
A Mariano, Cecilia y Martı́n por siempre estar ahı́.
iii
Contents
Abstract
i
Acknowledgements
iii
List of Figures
vii
List of Tables
viii
Symbols
ix
1 Introduction
1.1 Motivation & Challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2 Tools
2.1 Mathematical Tools . . . . . . . . . . . . . . . . . . . . . . .
2.1.1 Group . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.2 Subgroup . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.3 Lie Group . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.4 Group Homorphism . . . . . . . . . . . . . . . . . . .
2.1.5 Lie Group Isomorphism . . . . . . . . . . . . . . . . .
2.1.6 One Parameter Subgroup . . . . . . . . . . . . . . . .
2.1.7 Tangent Space . . . . . . . . . . . . . . . . . . . . . .
2.1.8 Left Action . . . . . . . . . . . . . . . . . . . . . . . .
2.1.9 Left Invariance of Functions and Vector Fields . . . .
2.1.10 Left Trivialization . . . . . . . . . . . . . . . . . . . .
2.1.11 Left Trivialized Derivative . . . . . . . . . . . . . . . .
2.1.12 Lie Algebra . . . . . . . . . . . . . . . . . . . . . . . .
2.1.13 Lie Algebra Isomorphism . . . . . . . . . . . . . . . .
2.1.14 The Lie Algebra of a Lie Group . . . . . . . . . . . . .
2.1.15 The group GL(n, R) . . . . . . . . . . . . . . . . . . .
2.1.16 Matrix Lie Group . . . . . . . . . . . . . . . . . . . .
2.1.17 Matrix One Parameter Subgroup Generator . . . . . .
2.1.18 Matrix Lie Algebra . . . . . . . . . . . . . . . . . . . .
2.1.19 The Special Orthogonal group of dimension 3, SO(3) .
iv
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
4
5
8
8
8
9
10
10
10
11
11
11
12
12
13
13
13
14
14
14
14
15
15
Contents
2.2
2.3
2.4
v
2.1.20 The so(3) Matrix Lie Algebra and The “Hat Map” . . . . .
2.1.21 Some properties of the “Hat Map” . . . . . . . . . . . . . .
2.1.22 Analytic Property of so(3) . . . . . . . . . . . . . . . . . . .
2.1.23 Rodrigues Formula in so(3) . . . . . . . . . . . . . . . . . .
2.1.24 Rotation Angle between two elements of SO(3) . . . . . . .
2.1.25 Decomposition of the Difference Formula . . . . . . . . . .
2.1.26 Euler Angles as Exponential Coordinates . . . . . . . . . .
2.1.27 The Special Euclidean Group SE(3) and its algebra . . . .
2.1.28 Time Derivative of Elements of SO(3) . . . . . . . . . . . .
Rigid Body Kinematics and Dynamics . . . . . . . . . . . . . . . .
2.2.1 Configuration . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.2 Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.3 Attitude . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.4 Configuration Space and Configuration for the Rigid Body
2.2.5 Dynamical Properties of the Rigid Body . . . . . . . . . . .
2.2.6 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.7 Translational Kinematics & Dynamics . . . . . . . . . . . .
2.2.8 Rotational Kinematics . . . . . . . . . . . . . . . . . . . . .
2.2.9 Rotational Dynamics . . . . . . . . . . . . . . . . . . . . . .
Matrix Norms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.1 Induced Norms . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.2 Matrix 2-norm . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.3 Frobenius Norm . . . . . . . . . . . . . . . . . . . . . . . .
2.3.4 Bounds on Vectors multiplied by Matrices . . . . . . . . . .
Stability of Dynamical Systems . . . . . . . . . . . . . . . . . . . .
2.4.1 Lyapunov Stability . . . . . . . . . . . . . . . . . . . . . . .
3 Attitude Control
3.1 Attitude Representations . . . . . . . . . . . . . . . . .
3.2 Rigid Body Dynamics . . . . . . . . . . . . . . . . . . .
3.3 The Attitude Control Problem . . . . . . . . . . . . . .
3.4 Attitude Control Laws in SO(3) . . . . . . . . . . . . .
3.4.1 Attitude Error Dynamics . . . . . . . . . . . . .
3.4.2 Error Tracking Function . . . . . . . . . . . . . .
3.4.3 Attitude Error Vector . . . . . . . . . . . . . . .
3.4.4 Angular Velocity Error Vector . . . . . . . . . .
3.4.5 Attitude Error Dynamics . . . . . . . . . . . . .
3.4.6 Attitude Control Law . . . . . . . . . . . . . . .
3.4.7 Lyapunov Function for the Attitude Control Law
3.4.8 Summary of the results on Attitude Control . . .
3.4.9 Units . . . . . . . . . . . . . . . . . . . . . . . .
4 Robust Attitude Control
4.1 Aerodynamic Perturbations . . . . . . . . . . .
4.2 Perturbations due to Measurement Inaccuracies
4.3 Robustization . . . . . . . . . . . . . . . . . . .
4.4 Perturbed Errors Vectors . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
16
17
18
19
20
20
21
22
23
23
23
23
24
25
25
26
26
27
27
27
27
28
28
28
29
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
30
30
32
32
33
33
33
34
35
37
38
39
43
43
.
.
.
.
45
45
46
47
47
.
.
.
.
.
.
.
.
.
.
.
.
Contents
4.5
4.6
vi
Robustization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5 Bounds on the sample time
5.1 SZH Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2 Bound on the Maximum Sample Time . . . . . . . . . . . . . . . . . . .
5.3 SZH Results for Asymptotically Stable Systems . . . . . . . . . . . . . .
5.3.1 Bound on the Sample Time for the Attitude Controller in SO(3)
5.3.2 Bound on the Sample Time for the Robust Attitude Control Law
in SO(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
55
56
57
59
60
. 63
6 Controller Design
67
7 Results and Discussion
7.1 UAV model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Choice of Error Measurement Function . . . . . . . . . . . . . . . . . . . .
7.2.1 A note on the bound on the sample time . . . . . . . . . . . . . .
7.3 Effect of the gains on the error and sample time . . . . . . . . . . . . . .
7.4 Angular velocity and acceleration bounds derived from different flight modes
7.4.1 “Super acrobatic” flight mode . . . . . . . . . . . . . . . . . . . . .
7.4.2 “Acrobatic” flight mode . . . . . . . . . . . . . . . . . . . . . . . .
7.4.3 “Stable” flight mode . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.4 Comparison between different flight modes . . . . . . . . . . . . .
7.5 Simulation Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6.1 Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6.2 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . .
70
70
71
72
72
74
75
76
77
78
79
81
81
84
8 Conclusion
88
8.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
Bibliography
90
List of Figures
1.1
Example of a quadrotor UAV. For reference, the distance between rotor
axis is 450mm in this model. . . . . . . . . . . . . . . . . . . . . . . . . .
2
2.1
2.2
Overview of Mathematical Tools . . . . . . . . . . . . . . . . . . . . . . . 9
Attitude as a Linear Transformation . . . . . . . . . . . . . . . . . . . . . 24
3.1
3.2
The Double Cover Problem . . . . . . . . . . . . . . . . . . . . . . . . . . 31
The Attitude Error Function and the Attitude Error Vector . . . . . . . . 35
4.1
Error Ellipse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.1
5.2
SZH Implementation is not Whole System Discretization . . . . . . . . . . 57
Robust Step Time Bound: Too conservative . . . . . . . . . . . . . . . . . 66
7.1
7.2
7.3
7.4
7.5
7.6
7.7
7.8
7.9
7.10
7.11
7.12
7.13
7.14
7.15
7.16
Error Measurement Function e(ε) . . . . . . . . . . . . . . . . . .
Error and Max Time Step vs. Gains . . . . . . . . . . . . . . . .
Error Ellipses for Selected Gains . . . . . . . . . . . . . . . . . .
Gains and Max Time Step for the “super acrobatic” flight mode
Gains and Max Time Step for the “acrobatic” flight mode . . . .
Gains and Max Time Step for the “stable” flight mode . . . . . .
Max Time Step for different flight modes . . . . . . . . . . . . .
Simulation Setup . . . . . . . . . . . . . . . . . . . . . . . . . . .
Simulation Visualization . . . . . . . . . . . . . . . . . . . . . . .
Simulation Results for Stabilization . . . . . . . . . . . . . . . . .
Random Gains vs. Best Gains . . . . . . . . . . . . . . . . . . . .
Simulation: Trajectory Tracking . . . . . . . . . . . . . . . . . .
Simulation: Trajectory Tracking (detail) . . . . . . . . . . . . . .
Simulation: Trajectory Tracking (detail) . . . . . . . . . . . . . .
Trajectory Tracking Error . . . . . . . . . . . . . . . . . . . . . .
Force Commanded to Opposing Rotors to perform the Flip . . .
vii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
72
73
74
76
77
78
79
80
80
82
83
84
85
86
86
87
List of Tables
3.1
Attitude Error and its Dynamics . . . . . . . . . . . . . . . . . . . . . . . 38
4.1
Bounds used for Robustization . . . . . . . . . . . . . . . . . . . . . . . . 54
5.1
5.2
Time Step Bound and required Constants . . . . . . . . . . . . . . . . . . 63
Constants required for the Robust Time Step Bound . . . . . . . . . . . . 66
7.1
7.2
7.3
7.4
Parameters used for Numerical Results . . . . . . . . .
Parameters for “super acrobatic” flight mode . . . . .
Summary of the parameters used for “acrobatic” flight
Parameters for “super acrobatic” flight mode . . . . .
viii
. . . .
. . . .
mode.
. . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
71
76
77
77
Symbols
c
Cross Gain
N ·m·s
eR
Attitude Error Vector
R3 vector, unitless
eΩ
Angular Velocity Error Vector
R3 vector, rad/s
J
Inertia Matrix
R3x3 matrix, kg · m2
kR
Attitude Error Gain
N ·m
kΩ
Angular Velocity Error Gain
N ·m·s
R
Attitude
R3x3 matrix, unitless
Rd
Attitude Command
R3x3 matrix, unitless
TSM AX
Maximum Time Step
ms
V
Lyapunov Function Candidate
J
¯
Measured Value of units of δ
Bound on ∆
units of ∆
Perturbation Vector associated with R3 vector, units of λm ()
Maximum Eigenvalue of Matrix units of λM ()
Minimum Eigenvalue of Matrix units of Ω
Angular Velocity
R3 vector, rad/s
Ωd
Angular Velocity Command
R3 vector, rad/s
ix
Chapter 1
Introduction
Unmanned Aerial Vehicles are aerial vehicles that can fly autonomously without the need
of a pilot or human controller. In the past, most UAVs were aircrafts usually smaller
than commercial manned airplanes but flying by the same principles of lift generated
by aerodynamic profiles. Vertical lift UAVs like autonomous helicopters or multi-rotors
were not so widespread mainly because of the high requirements in computation power,
sensors, and propulsion associated with their autonomous flight.
With the advancement of different fields of technology, the last decade has seen an unprecedented development and adoption of UAVs, mainly of the vertical lift type. New
sensor technologies like MEMs allow for small, low power, high precision inertial measurement units. Global positioning systems like GPS allow for longer mission range.
New processor technologies allow for fast, low power computation and vision-based navigation. Miniaturization of RF technologies allow for lower power, longer range communications. New material science applications allow for smaller, faster, lighter and more
powerful motor technologies like brushless motors. New battery technologies like LiPO
allow for low weight, high current, high capacity batteries.
Integration of all these new technologies with advanced control algorithms made possible
a new era in UAV technology where the new family of quadrotor or multi-rotor aircraft
entered the scene previously dominated by airplanes and helicopters. Multi-rotor UAVs
are vertical lift UAVs with several rotors spinning around parallel axes at variable speed
each, usually arranged in one or two planes.
Applications include a wide and growing range of fields including artistic performances,
photography and video production, precision agriculture, geographic information systems, and remote sensing just to name a few. Remarkable examples are the search and
rescue monitoring mission after the Fukushima nuclear incident [1], search and rescue in
1
Chapter 1. Introduction
2
the wilderness [2], precision 3D mapping [3], and its use by biologists to study the flight
patterns of bats [4].
Further applications may be found for swarms or teams of UAVs in formation flying [5],
where precise coordination between several UAVs can lead to improvements in items
such as area of coverage, mission reliability and small load transportation.
The most common multi-rotor UAV is the quadrotor [6], with 4 rotors attached to
a cross-shaped airframe. In general, the size of these machines does not exceed half
meter in diameter, and its mass is usually less than 1kg. For this reduced size they
are referred to as micro-UAVs although smaller form factors named nano-UAVs are
becoming popular. An example is shown in Figure 1.1.
Figure 1.1: Example of a quadrotor UAV. For reference, the distance between rotor
axis is 450mm in this model.
Rotors are arranged in two counterrotating pairs and each rotor can be controlled separately. This is done to achieve zero angular momentum transfer with the air when
hovering, i.e. to keep the UAV with constant yaw. Control is achieved by differentially
varying the speeds of the rotors, and thus the thrust they produce, creating an imbalance
in force between opposing sides of the craft. In this way rolling and pitching movements
can be performed. A difference between the counter-rotating rotors speeds makes the
rotors produce a yawing torque. Total thrust is achieved by simultaneously varying the
speed of the four rotors.
With these commands alone the craft cannot directly produce thrust in the plane of
the rotors, so, in order to move in a direction contained in the current plane of the
rotors it must first reorient itself. This is due to the fact that it is underactuated, it
is a system with 4 control actions moving with the 6 degrees of freedom of rigid body
Chapter 1. Introduction
3
rotation and translation. To follow a trajectory it must orient itself in a way such that
the combination of total thrust and torque allows the UAV to move along the trajectory.
For example, if the UAV is hovering and is commanded to move to the right, it will have
to slightly incline itself so the total thrust actually pushes the UAV to the right.
This results in position control strongly relying on attitude control. Although the system is underactuated, its rotational part is fully actuated, as the rotors can achieve
torque in three perpendicular axes. Since the rotational dynamics is decoupled from the
translational dynamics, the rotational part of the problem can be studied on its own.
Then, position control can be implemented using the underlying attitude controller to
orient the craft. By a combination of attitude control and thrust control, trajectories
can be followed.
The powerful motors, their location on the airframe and the relatively small size of the
craft make the dynamics of the system very fast. Stable flight can only be achieved
when the thrusts of the rotors are carefully computed in a feedback loop that includes
attitude and angular velocity sensor readings. The control law of this loop is a solution
to the attitude control problem.
The attitude control problem refers to the problem of applying torques to the craft,
seen as a rigid body, such that it follows a certain attitude or orientation over time.
The physics of the problem is the physics of rigid body rotations and it involves highly
nonlinear dynamics. Several solutions to the attitude control problem for quadrotor
UAVs have been proposed. Linear control laws often in the forms of PIDs showing
extreme simplicity have been used [7], but the range of operation conditions is limited.
Solutions of increasing complexity like neural networks [8], feedback linearization and
backstepping [9] and sliding modes [10] solutions have been proposed. These solutions
increase the range of operation in terms of allowable attitudes and perturbation rejection,
but as they depend on non-global or unique representations of the rotations of the rigid
body, they do not provide global convergence properties, that is, there are attitudes
outside the operation range for which the craft may be out of control and unable to
stabilize itself.
Lee [11] provided a robust global control law which is able to reject unmodeled perturbations while attaining almost global convergence properties for the attitude control
problem. This is achieved by treating the physics of the problem in a natural geometric way instead of relying on representations or linearizations thereof, which induce
singularities and artifacts.
To actually implement this control law, the control system must read the data from the
sensors, compute the control action (usually using a digital computer or microprocessor)
Chapter 1. Introduction
4
and pass the command to the rotors. Although the discrete implementation of continuous control laws has been extensively studied in the literature (for example see [12]),
and some studies have been carried out regarding attitude control and sample time [13],
no results for the particular case of almost global attitude control could be found in
the literature. Likewise, the influence of measurement errors in attitude and angular
velocity on the performance of the controller could not be found in the literature. At
last, this control law depends on gains for which no criteria on their selection could be
found.
The lack of these results limits some part of the requirements analysis for the design of
UAVs, and it also relegates several design decisions to experience-based design and trial
and error design.
1.1
Motivation & Challenges
In this work we address three sets of questions.
Firstly, to stabilize the craft the control algorithms use data provided by the navigation
subsystems. These subsystems read the raw data from the sensors and filter them
to obtain an estimate of the attitude and angular velocity. Sensor measurements are
often faulty and noisy, and may contain systematic bias. All these factors influence the
performance of the controller. The first set of questions is: Given a bound on the errors
provided by the estimation algorithm, how can we design a controller that tries to reject
them? How do errors in the estimation of the state of the craft translate to errors in
its attitude? How well can the controller track a commanded attitude when fed with an
estimation of the state of the craft which is not accurate?
Control-theoretic results for continuous time system can be obtained based on energy-like
considerations, like Lyapunov analysis. They are elegant and intuitive. The controller
we derived as the answer to the first set of questions is guaranteed to work in continuous
time. The second set of questions we will address relates to the actual implementation
of the controller in a digital system. Digital systems work in discrete time whereas
the physical systems they are controlling evolve in continuous time. How fast must we
update the control action for the system to conserve its stability properties?
Lastly, both questions give raise to a third set of questions: How is the time step related
to the error in tracking? How can we design our controller, such that when fed with
noisy and biased state estimations, it still produces the smallest possible error for the
given noisy estimation with the largest possible time step?
Chapter 1. Introduction
1.2
5
Outline
An introduction to the mathematical and physical tools used in this work is presented
in section 2. Emphasis is placed on the group structure and algebraic properties of rigid
body physics. The natural representation of rigid body attitude is the special orthogonal
group SO(3) and by focusing on this group and its related entities, geometric, coordinatefree expressions are introduced to describe its kinematics and dynamics. As coordinate
charts are used to describe this natural representation, they provide no extra benefits,
and indeed they induce artifacts and singularities. A relationship between SO(3) and
Euler angles is made explicit in section 2.1.26 because of its widespread use and because
it is a standard way to describe static attitudes. The kinematic and dynamic evolution
of the rigid body are introduced in section 2.2. A short introduction to matrix norms is
also presented in section 2.3, as they will be intensively used in later sections to bound
functions and vectors depending on matrices.
The attitude control problem is addressed in section 3. Firstly, the attitude representation is discussed in terms of the tools from chapter 2 and the choice of SO(3) as the
attitude representation and the working space for controller design is justified. This
proves to have many advantages over methods that depend on the coordinates used to
describe the underlying physics. Controllers depending on these coordinate charts may
have some practical advantages like simpler computational implementation, but they
sacrifice either globality, continuity or both. An attitude control law is introduced in
section 3.4. This controller is obtained in the same geometric, coordinate-free way as
the kinematics and dynamics are described. By making use of the tools introduced in
section 2 and Lyapunov analysis this controller is shown to be almost global and to
exhibit exponential convergence.
A robust attitude controller is proposed in section 4.5. It adds a robustization term
to the controller described in section 3 which exhibits almost global exponential convergence properties. The robust controller assumes it is working with measured or
estimated values of attitude and angular velocity. Given bounds on the error attainable
by these measurements, bounds on any unmodeled perturbation and on angular velocity, its command, and the time derivative of its command, the proposed controller can
guarantee a bound on the error in attitude and angular velocity it ultimately produces.
This bound has the shape of an ellipse in the plane spanned by the magnitude of the
attitude and angular velocity errors. The controller proves to be almost global and to
exhibit exponential convergence until it reaches this ellipse. From there on, convergence
is replaced by simple convergence to the ellipse. This controller has three free gains.
Chapter 1. Introduction
6
The sample time required for the discrete time implementation of the controller is studied
in section 5. The system is not discretized as a whole, as in other sample time analysis,
but only the controller action is modeled as discrete using its SZH implementation, while
the rest of the system is left to evolve in continuous time. Other analyses rely on the
discretization of the whole system and introduce ad hoc hypothesis and error bounds
by doing so, which are not of a physical nature, but are only an artifact of the way of
describing the system. On the other hand, the SZH implementation of the controller
is in fact a very accurate model of the real situation, where physical systems evolve in
continuous time, while the controller implemented in a digital computer updates the
control action at a fixed interval. The convergence properties of the controller are used
to obtain a bound on the maximum sample time. Bounds are found for the non-robust
version of the controller as well as for its robust version. By the way it is obtained, it
is a conservative bound. But it is a strong result because it is found analytically and
the model is a very good model of reality. The bound on the sample time depends
on the physical properties of the rigid body, namely its moment of inertia, and on the
gains of the controller. For the robust controller, the bound also depends on the bounds
used for the robustization. The results for the robust version are found to be extremely
conservative by the nature of the robustization term. Nevertheless, they are also found
analytically and provide a good starting point for controller design.
Next, the problem of controller design was addressed. The choice of the gains for the
controller is loosely restricted so that exponential convergence is achieved. This choice
is refined in section 6. The shape of the ellipse of convergence depends on the gains,
as does the bound on the sample time. Since both magnitudes depend on the gains,
they can be interrelated to find gains that obtain the minimum possible error in the
maximum possible sample time. The solution of this minimax problem is proposed as
the best choice of gains, and is affected by the precise way the errors on attitude and
angular velocity are weighted in an error measurement function, so that the best choice
of gains will vary according to the application requirements.
Lastly, the results from previous sections are studied numerically in section 7 and tested
against a UAV model by means of simulations in section 7.6. The UAV under study is
first described, then bounds for the robustization are obtained based on the measurement
errors and the physical properties of the UAV. Additional bounds are derived from the
constraints imposed on the angular velocity, its command and the time derivative of
its command by different flight modes. With these bounds, the shape of the error
measurement function and the bound on the sample time are explored. Then, the
simulation setup is described. Controller performance with respect to controller sample
time is shown by simulating the stabilization from a random attitude with different
controller time steps. The results show that the bounds found analytically are very
Chapter 1. Introduction
7
conservative and larger time steps can safely be used. But when used in conjunction
with the results of controller design, they provide a useful tool to choose the gains
and thus design the controller to perform as good as possible with respect to the error
measurement function. The trajectory tracking capabilities are also shown by simulation.
Lastly, the conclusion and future work are discussed in section 8.
Chapter 2
Tools
In this chapter we present the main results regarding the attitude and position kinematics
and dynamics for a rigid body evolving in the full SE(3) configuration space. We follow
the geometric approach presented by Jurdevic [14], Lewis and Bullo [15], and Lee [16].
[17]. We introduce the required mathematical tools we will be using. Then, we develop
the basics of attitude kinematics and dynamics in a geometric, coordinate free way.
2.1
Mathematical Tools
Figure 2.1 shows an overview of the different mathematical tools presented and their
relationship.
2.1.1
Group
Given a set G, and a binary operation ?, such that for any two elements x, y ∈ G it
assigns to them another element in G,
?:G×G→G
(x, y) → x ? y
the pairing (G, ?) is called a Group if
(1) ? is associative: x ? (y ? z) = (x ? y) ? z ∀x, y, z ∈ G
(2) ? has an identity element e ∈ G: x ? e = x ∀x ∈ G.
(3) every element x has an inverse element x−1 , i.e. x ? x−1 = x−1 ? x = e.
8
Chapter 2. Tools
9
Figure 2.1: Overview of the main mathematical tools presented in this section, physical quantities and their relationship.
Usually, the group is written just G if the operation is implicit from the context.
An example of a group is the integers with the addition operation. The natural numbers
lack the identify element for the addition operation, so they provide an example of what
is not a group.
2.1.2
Subgroup
Given a Group (G, ?), if a subset H ⊂ G and the operation ? also form a Group, this
Group is called a Subgroup of (G, ?) and is denoted by (H, ?).
An example of a subgroup of the integers is the set of even integers (with zero considered
even) with the same addition operation, since adding two even integers yields an even
integer. However, the set of odd integers provide an example of what is not a subgroup,
since adding two odd integers yields an even integer. Although the resulting even integer
is in the group of integers, it is not in the set of odd integers, so odd integers with the
Chapter 2. Tools
10
usual addition operation do not form a group and are not a subgroup of the integer
group.
2.1.3
Lie Group
A Lie Group is a Group which is also a manifold and for which the group operation and
its inverse are smooth.
An example of a Lie Group is the real numbers with the addition operation. A counterexample is the integers with the addition operation, because of it lacks smoothness.
2.1.4
Group Homorphism
Given two groups, (G, ?) and (H, •), a map ρ : G → H is called a Group Homorphism, if
applying the ρ map to the ? operation of two elements of G is the same that applying the
• operation to the images in H of these elements through ρ, i.e. ρ (x ? y) = ρ (x) • ρ (y)
∀x, y ∈ G.
An example of a homomorphism is the identification of a number with its parity, and
the sum with the parity product. Take the group (Z, +), the group (H, ?) with H =
{even, odd}, and the ? operation defined as commutative and verifying even ? odd = odd,
even ? even = even and odd ? odd = even. The map ρ(n) taking values even if n is even
and odd if n is odd is a group homomorphism, and it allows the abstraction of the parity
property of integers under summation.
Another example is the map that identifies real numbers with angles of rotation in the
plane. The composition of two rotations is a rotation. Composing a rotation by an angle
α with a rotation by an angle β results in a rotation of angle α + β.
The restriction of some elements of a group to create a subgroup is also an example of
a homomorphism.
2.1.5
Lie Group Isomorphism
A Lie Group Isomorphism is a homomorphism between Lie Groups that is smooth and
bijective. Examples will be given in what follows.
An example is the exponential map, which is an isomorphism between the group of the
real numbers with the addition as group operation and the positive real numbers with
the product as group operation.
Chapter 2. Tools
2.1.6
11
One Parameter Subgroup
A One Parameter Subgroup of a Lie Group G is the image of a smooth group homomorphism ρ : R → G, denoted by t → g(t).
As this map smoothly assigns elements in G to values in R, it may be seen as a curve
in G.
2.1.7
Tangent Space
Due to the smoothness properties, through an element g ∈ G pass infinitely many
continuous differentiable curves γ(t), defined from an open real interval t ∈ (−r, r) to
G. The equivalence class of all curves such that γ(0) = g deffines a linear space which
is called the Tangent Space of G at g, and is denoted by Tg G.
The derivatives of the curves γ evaluated at 0 give the structure of this linear space, and
some of these derivatives may be chosen to construct a basis for this linear space. The
dimension of this linear space is the same as the dimension of the group.
For example, for a point a on the sphere, the tangent space is the space of vectors that
are tangent to the sphere when pinned at point a.
2.1.8
Left Action
Given a Lie Group (G, ?), the Left Action of g ∈ G is the map Lg defined by
Lg : G → G
h→g?h
An example of left action is found by identifying the points of a sphere with rotations.
Each element is a point that may represent a rotation starting at the north pole and
ending at said point. The left action of an element on the points of the sphere is to
rotate all the points by the rotation it represents.
The map Lg inherits its properties of the group action, so it is a smooth map with smooth
inverse Lg−1 . For g, the tangent map Te Lg : Te G → Tg G is a natural isomorphism
between the tangent space at g and the tangent space at the identity element e.
For example, every vector tangent to the sphere at a point a (i.e. belonging to the tangent
space at a ∈ G), when left acted with the rotation represented by a− 1, is transformed
Chapter 2. Tools
12
in an unique way such that it becomes tangent to the sphere when pinned at the north
pole.
For two elements of two different tangent spaces, α ∈ Ta G and β ∈ Tb G, comparison
and algebraic operations are not possible at first glance because they lie in different
spaces. But with the identifications Te La and Te Lb , both can be treated as elements of
the tangent space at identity, and engage in comparison and algebraic operations. For
this to happen the tangent spaces must preserve its shape when left trivialized, which
will be seen next.
2.1.9
Left Invariance of Functions and Vector Fields
A function φ : G → R is said to be Left Invariant if f (h) = f (g ? h). Likewise, a vector
field χ : G → Rn is said to be Left Invariant if χ (g ? h) = Th Lg (χ (h)). This implies
that χ (g) = Te Lg (χ (e)), meaning that Left Invariant functions and vector fields are
determined by their values at the identity element.
Extending the previous example, because of the properties of rotations, the length of
the vector tangent at a is the same as the length of the vector rotated to the north
pole. This means that the length (seen as a function) is left invariant with respect to
rotations.
A Left Invariant Vector Field taking the value α at the identity is denoted by αL , i.e. it
is defined as αL : g → Te Lg (α).
To allow for comparison or algebraic operations between elements of different tangent
spaces and their derivatives, Left Trivialization and Left Trivialization of the Derivatives
are introduced.
2.1.10
Left Trivialization
The smooth map αg : T G → G × Te G defined by αg → g, Te Lg−1 α is called the
Left Trivialization of α at g, and is a natural identification between the tangent space
at g and the tangent space at identity, i.e. for the tangent vector γg = (g, γ) ∈ T G,
its relationship to (g, α) ∈ G × Te G may be written as (g, γ)T G → (g, α)G×Te G , with
γ = Te Lg (α) = gα.
So for the previous example, by specifying the point a of the sphere at which the vector
tangent to the sphere is to be pinned, the vector specified at the north pole suffices to
uniquely determine the vector pinned at a for any choice of a.
Chapter 2. Tools
2.1.11
13
Left Trivialized Derivative
For a function Ψ : SO(3) → R its Left Trivialized derivative at g ∈ SO(3) denoted by
Dg Ψ : SO(3) → T ∗ G is defined by
Dg Ψ(g) · γ =
d
|t=0 Ψ (h(t)) · γ
dt
(2.1)
where h(t) is a One Parameter Subgroup such that h(0) = g and γ = gα with α ∈ Te G
is an element of the tangent space γ ∈ Tg G called a variation of g.
Extending the previous example, a function that assigns real numbers to every point
of the sphere can be defined. The variation of this function over the sphere can be
studied by means of its left trivialized derivative. For example if the function measures
the smallest distance on the sphere between the north pole and point a, this derivative
will be a vector showing in which direction the distance increases the most. It will be
represented by left trivialization as a vector pinned at the north pole, and to actually
find the direction it must be left acted by a. As stated previously, trivialization allows
the comparison of value the derivative at different points in an unique way. For this
particular function, the result for each point of the sphere a will be a vector pinned at
the north pole, of unit length, and tangent to the geodesic that connects the north pole
with a.
2.1.12
Lie Algebra
A Lie Algebra denoted (V, [·, ·]V ) is the algebra of a vector space V over R, together
with a bilinear operation [·, ·] : V × V → V called bracket which satisfies
(1) anticommutativity: [α, β] = − [β, α], ∀α, β ∈ V
(2) the Jacobi identity: [α, [β, γ]] + [β, [γ, α]] + [γ, [α, β]] = 0.
2.1.13
Lie Algebra Isomorphism
Given two Lie Algebras (V, [·, ·]V ) and (W, [·, ·]W ) , a map ρ : V → W is a Lie Algebra
Isomorphism if it satisfies
(1) ρ ([α, β]V ) = [ρ (α) , ρ (β)]W ∀α, β ∈ V
(2) linearity: ρ (α + β) = (α) + (β)
Chapter 2. Tools
14
(3) bijectivity
When there exists one such isomorphism, both algebras are said to be isomorphic, denoted by V ' W . As it is a linear map, it is smooth. If the bijectivity condition is not
met, it is called a homomorphism instead.
2.1.14
The Lie Algebra of a Lie Group
The Lie Algebra g of a Lie Group G is identified with the tangent space at the identity
g ' Te G by defining the bracket [·, ·] : g × g → g given by [α, β] = [αL , βL ]e . It assigns a
bracket operation to the Left Invariant Vector Fields on G.
2.1.15
The group GL(n, R)
The group GL(n, R) consists of the square n × n matrices of real numbers.
2.1.16
Matrix Lie Group
A Matrix Lie Group is a subgroup of GL(n, R).
2.1.17
Matrix One Parameter Subgroup Generator
Given a Matrix Lie Group G, the matrix A ∈ Rn×n is a Matrix One Parameter Subgroup
Generator if for t ∈ R the map t → exp(tA) takes values on G. The matrix exponential
is defined as the solution of the differential equation
d
exp(tA) = Aexp(tA)
dt
The power series
exp(B) =
∞
X
Bk
k=0
k!
(2.2)
(2.3)
verifies equation (2.2) with B = At. Power series will be studied in section 2.1.22.
From the properties of the exponential map it is shown that the set of Matrix One
Parameter Subgroup Generators is a subspace of Rn×n which is closed under the matrix
commutator defined by
[A, B] = A · B − B · A
Chapter 2. Tools
15
with the dot representing the usual matrix product. This induces a Lie Algebra structure
for the generators of G. Since the set of the Matrix One Parameter Subgroup Generators
of the group G is a vector space of dimension n, it is identified with the tangent space
at identity of G, Te G. The set of Matrix One Parameter Subgroup Generators of the
group GL(n, R) is denoted by gl(n, R).
2.1.18
Matrix Lie Algebra
A Matrix Lie Algebra is a subspace of Rn×n closed under the bracket defined by matrix
commutator [α, β] = α · β − β · α, with the dot representing the usual matrix product.
It is the algebraic structure of the tangent space at identity of a Matrix Lie Group, and
since for Left Invariant Vector Fields the tangent space at identity is isomorphic to the
tangent space at an element g, it is the algebraic structure of the tangent spaces of G.
The Matrix Lie Algebra is a subalgebra of gl(n, R).
2.1.19
The Special Orthogonal group of dimension 3, SO(3)
The Special Orthogonal group of dimension 3, denoted SO(3), will be used to describe
the attitude. It is defined as the set of proper orthogonal 3 × 3 real matrices
SO(3) = {R ∈ R3×3 | RT · R = I, det (R) = 1}
(2.4)
Left actions of SO(3) on itself or on elements of R3 are identified with rotations, since it
describes the set of shape preserving (isometric) transformations in R3 which leave the
origin fixed. The configuration space for the rotations of a rigid body is identified with
SO(3). The velocity phase space of the free rigid body is the tangent space T SO(3).
For a matrix to be in SO(3) it must satisfy
R−1 = RT
2.1.20
(2.5)
The so(3) Matrix Lie Algebra and The “Hat Map”
The Matrix Lie Algebra of skew symmetric 3 × 3 real matrices so(3) is defined as
so(3) = {S ∈ R3×3 | S + S T = 0}
(2.6)
Chapter 2. Tools
16
Matrices S ∈ so(3) are identified by S = ŝ with vectors s ∈ R3 by constructing a Lie
Algebra Isomorphism (s, ×) → (ŝ, [·, ·]) identifying the bracket in so(3) with the usual
vector cross product in R3 . This isomorphism is called the “hat map”

s1


0
−s3



 → ŝ =  s3
s=
s
2



s3
−s2
0
s1
s2


−s1 

0
(2.7)
ˇ A canonical basis for the set
and its inverse is called the inverse hat map denoted by .
of One Parameter Subgroup Generators from SO(3) is given by

0 0
0




ê1 = 
 0 0 −1 
0 1 0

0
0 1




ê2 = 
 0 0 0 
−1 0 0

0 −1 0

ê3 = 
 1
0
0
0


0 

0
(2.8)
and they form a basis of so(3). They may be interpreted as infinitesimal rotations
about each of the coordinate axes. Furthermore, the map s = (s1 , s2 , s3 )T → ŝ can be
expressed as
ŝ = s1 ê1 + s2 ê2 + s2 ê3
(2.9)
The inverse map ŝ → (s1 , s2 , s3 )T , which maps elements from so(3) back into R3 , written
ˇ : so(3) → R3 , is given by
as sk = −εijk ŝij
(2.10)
with εijk the Levi-Civita symbol, taking values +1 if ijk is an even permutation of (123),
−1 if ijk is an odd permutation of (123) and 0 otherwise. A double summation over i
and j is implied. This inverse map is included here for completeness and it will only
be used in the numerical implementation and simulations, but for the proofs in what
follows we will use properties of the hat map to relate elements in so(3) to elements of
R3 because it is more elegant and intuitive to do so instead of twiddling with indices
and coordinates.
2.1.21
Some properties of the “Hat Map”
Several properties of the hat map can be readily proved by direct computation.
ŝ2 = ssT − ksk2 I
(2.11)
Chapter 2. Tools
17
ŝ2k+1 = (−1)k ksk2k ŝ
(2.12)
Note that the usual matrix product of two elements of so(3) is usually not in so(3). The
closedness is with respect to the bracket operation, not matrix product.
The hat map may also be interpreted as the linear operator associated to the cross
product since
x̂y = x × y
(2.13)
and by this property, x generates the null space of x̂,
x̂x = 0
(2.14)
1
T
= −xT A − AT ˇ
trace (Ax̂) = trace x̂ A − A
2
(2.15)
Also, for the trace
and for the sum of looks-like-transposed terms
(Ax)ˆ= x̂
T
1
1
trace (A) I − A +
trace (A) I − A
x̂
2
2
(2.16)
and if R ∈ SO(3)
Rx̂RT = (Rx)ˆ
2.1.22
(2.17)
Analytic Property of so(3)
Equations (2.11) and (2.12) give a closed form to compute any positive integer power of
an element of so(3), and this allows us to build power series, in particular Taylor series.
Negative powers do not exist since the elements of so(3) are singular matrices. As all
positive integer powers of so(3) elements exist, the convergence of the formula will be
linked to the convergence of the series for the real variable case.
From this, the sine and cosine can be defined as
sin(x̂) =
∞
X
x̂2k+1
(2k + 1)!
(2.18)
∞
X
x̂2k
(2k)!
(2.19)
k=0
cos(x̂) =
k=0
They satisfy
Chapter 2. Tools
18
sin(x̂) =
exp(x̂) − exp(−x̂)
2
(2.20)
cos(x̂) =
exp(x̂) + exp(−x̂)
2
(2.21)
Odd functions such that f (α) = −f (−α) turn out to have power series in terms of so(3)
elements which also lie in so(3), whereas even functions such that f (α) = f (−α) have
power series representations which are symmetric, f (x̂) = (f (x̂))T . A function neither
even nor odd will be a combination of a symmetric and antisymmetric part, given by
their even and odd subseries respectively.
In particular, sin(x̂) ∈ so(3) and cos(x̂) is symmetric.
2.1.23
Rodrigues Formula in so(3)
The series expansion of the exponential was given in section 2.1.17 without an explicit
way to compute the powers of the matrix x̂ ∈ so(3) as
exp(x̂) =
∞
X
x̂k
k=0
(2.22)
k!
Applying formulas (2.11) and (2.12), the matrix exponential can be shown to be
exp (ŝ) = I +
sin (ksk)
1 − cos (ksk) 2
ŝ +
ŝ
ksk
ksk2
(2.23)
which can be related to the Rodrigues Formula in R3 , giving the rotation of an angle
θ about an axis n, identifying n with n =
double cross product with n × n× =
ŝ2
ksk2
s
ksk ,
the cross product with n× =
ŝ
ksk ,
the
and the angle of rotation θ with θ = ksk
This map verifies
exp (−ŝ) = (exp (ŝ))T
(2.24)
exp (−ŝ) = (exp (ŝ))−1
(2.25)
and
so (exp (ŝ))T = (exp (ŝ))−1 , which means that (exp (ŝ)) ∈ SO(3).
Chapter 2. Tools
19
This establishes a Group Isomorphism between elements of so(3) and SO(3). Every
element of SO(3) can be written as the exponential of an element in so(3). The inverse
of the exponential map is the logarithm map, which is not unique. A closed formula like
equation (2.23) exists for the logarithm.
2.1.24
Rotation Angle between two elements of SO(3)
The previous results imply that the elements of so(3) may be regarded in two different
ways. The first way will be seen in 2.1.28 and interprets them as instantaneous angular
velocities. The second way is to think of them as just a parametrization of SO(3), in
which an element X = exp(x̂) is the result of rotating the identity element by an angle
kxk around the axis defined by x where x ∈ R3 .
Furthermore, an element X ∈ SO(3) can be rotated to another one Y by the left action
of Y X T because of the orthogonality property, Y X T · X = Y . This can be conceived as
a rotation about some axis n ∈ R3 with an angle kθk.
To find n and θ, we note that for this rotation the axis remains fixed, so Y X T n = n.
T
Additionally, the inverse rotation also leaves n fixed, so Y X T n = n. Subtracting
T
both equations yields Y X T − Y X T n = 0, so the null space of the parenthesis
term contains n.
Recalling equation (2.14), the so(3) element corresponding to the rotation axis is found
from n̂n = 0 so the term in parentheses is proportional to n̂. To take into account the
rotation angle θ, the exponential from (2.23) must be used. By rearranging terms, the
so(3) element identified with the rotation from X to Y can be written as
ŝ =
1
XT Y − Y T X
2
where the angle is given by sin(θ) = ksk and the direction n =
(2.26)
s
ksk .
The relationship to
the sine in the norm can be obtained by recalling the fact that R = exp(x̂). By equation
(2.18) this can be related to a sine in so(3) as
ŝ = sin(x̂)
from where the previous identities arise.
(2.27)
Chapter 2. Tools
2.1.25
20
Decomposition of the Difference Formula
An additional property involving elements of so(3) and SO(3) is the decomposition
of the difference of two elements R1 , R2 ∈ SO(3) obtained by left and right pre and
post-multiplication of the square roots of R1 and R2 given by
R1 − R2 =
p p p T p T p p
R1
R1 R2 − R1
R2
R2
(2.28)
Note that the difference itself is not an element of SO(3), but each square root is, and
√
R corresponds to the rotation by half the angle represented by R around the same
proper axis of rotation. The term between brackets may be further identified by equation
(2.26) with an element of so(3) corresponding to a sine, so
R1 − R2 =
ŝ =
p
R1
p
R1 · ŝ ·
p
R2
p T p Tp ˆ
R2 − R1
R2 = sin(ξ)
(2.29)
(2.30)
for some ξˆ ∈ so(3).
2.1.26
Euler Angles as Exponential Coordinates
The group SO(3) may be parametrized according to a decomposition of a rotation matrix
R as 3 successive rotations around the three coordinate axes. By specifying an order of
3 successive rotations and the angle of each rotation, a 3-parameter representation of R
is obtained. The default order of rotations is Z → Y → X, where the angles are called
yaw ψ, pitch θ and roll φ respectively, but other orders of rotations may be specified.
The map Euler : (−π, π] × − π2 , π2 × (−π, π] → SO(3) given by
(ψ, θ, φ) → exp (ψê3 ) exp (θê2 ) exp (φê1 )
(2.31)
with closed form

c(θ) c(ψ)

R'
 −c(θ) s(ψ)
−s(θ)
−c(φ) s(ψ) + c(ψ) s(θ) s(φ)
c(φ) c(ψ) + s(θ) s(φ) s(ψ)
c(θ) s(φ)
s(φ) s(ψ) − c(φ) c(ψ) s(θ)


−c(ψ) s(φ) + c(φ) s(θ) s(ψ) 

c(θ) c(φ)
(2.32)
(where sin and cos were abbreviated by their initials) is a way of describing elements of
SO(3) in terms of only three parameters. As it is built upon three exponentials and the
Chapter 2. Tools
21
identity element is covered by this map, it gives a coordinate chart for a neighborhood of
the identity element of SO(3). The pitfall is that this map is not bijective, as the subset
{R ∈ SO(3) | |R31 | = 1} cannot be expressed in terms of these parameters. Excluding
these elements, the inverse map is given by
(ψ, θ, φ) =
q
2 + R2
,
Atan(R
,
R
)
Atan(R21 , R11 ), Atan −R31 , R11
32
33
21
(2.33)
which is defined for R ∈ SO(3) − {R ∈ SO(3) | |R31 | = 1} and the map is a bijection.
Other rotation sequences define the maps differently, but it is a topological result that
3 parameters are not enough to parametrize the whole of SO(3). In fact, the minimum
number of parameters to uniquely represent SO(3) is 5, as will be seen later.
For example, if the restriction is dismissed in this case and the intervals for (ψ, θ, φ) are
taken as closed, (ψ, θ, φ) = (− π2 , π2 , π2 ) and (φ, ψ, θ) = (0, − π2 , 0) correspond to the same
value of R. Additionally, the dynamics derived of the Euler Angles representation present
singularities which are not present in SO(3) but are an artifact of this representation.
In spite of these disadvantages, Euler Angles are still a simple way of specifying static
orientations when no dynamics are implied, as, for example, initial and ending conditions.
2.1.27
The Special Euclidean Group SE(3) and its algebra
As SO(3) alone will be used to describe the attitude, it is useful to specify the position
and the attitude in a single element. This is done by combining SO(3) with vectors of
R3 in a single entity called the Special Euclidean Group of dimension 3.
SE(3) = {g ∈ R
4×4
|g=
R r
0
!
,
1
R ∈ SO(3),
r ∈ R3 }
(2.34)
and its associated Lie Algebra is an extension of so(3) to so(3) × R3 given by
se(3) = {α ∈ R
4×4
|α=
S v
0
0
!
,
S ∈ so(3),
v ∈ R3 }
(2.35)
Chapter 2. Tools
22
As elements of SE(3) will be used to specify attitude and position, elements of se(3)
will be used to specify velocity and angular velocity. The action of an element of the
group SE(3) on R3 , given by (R, r) : R3 → R3 is a rotation followed by a translation
(R, r)x → Rx + r
(2.36)
Likewise, the action of an element of the group SE(3) on SE(3) is also a rotation followed
by a translation, defined by
(R, r)(A, x) → (RA, Rx + r)
(2.37)
(R, r)−1 → (RT , −RT r)
(2.38)
and the inverse is given by
Also, given two vectors α = (ω, v), they may be identified with elements of se(3) by
means of the hat map, defined in this case as
α̂ =
2.1.28
ŵ v
0
!
0
(2.39)
Time Derivative of Elements of SO(3)
An important result about the time derivative of X ∈ SO(3) is that Ẋ can be written
as the product
Ẋ = XY
(2.40)
with Y ∈ so(3). Since X T X = I is constant, its time derivative
(X T Ẋ)T + X T Ẋ = 0
(2.41)
vanishes. Calling Y = X T Ẋ, it can be seen that X takes values in SO(3) only if Y
satisfies
YT +Y =0
(2.42)
Chapter 2. Tools
23
that is to say Y takes values in so(3). With the identification of so(3) as the tangent
space of SO(3), equation (2.40) is called kinematic equation. Further physical insight
into this naming will be gained in the next section.
2.2
Rigid Body Kinematics and Dynamics
The rigid body will be considered as a distribution of matter in space, which is free
to move around and rotate, i.e. it has no constraints on its motions, and in which the
relative position of every mass point in the body with respect to every other mass point
in the body remains constant over time.
2.2.1
Configuration
The configuration of a rigid body is a compact subset B ⊂ R3 with smooth or piecewise
smooth boundary called the shape of the rigid body, acting as the support of a Borel
measure µ, called the mass density or mass distribution of the rigid body [15].
2.2.2
Orientation
Given a configuration defined arbitrarily to be the reference configuration, an orientation
is a smooth map from any other configuration to the reference configuration. This map
may be time-varying, describing the evolution of the orientation in time, and because of
the rigidity condition it must be bijective and shape preserving (isometric).
2.2.3
Attitude
The attitude is the orientation of a rigid body relative to a reference configuration. It
may be described by a linear transformation between the reference frame, taken to be
an inertial frame fixed in space with axes {~e1 , ~e2 , ~e3 }, and a body fixed frame {~b1 , ~b2 , ~b3 },
that follows its orientation, assuming both frames are right-handed (See Figure 2.2).
Besides describing the orientation of the body in space, this linear transformation also
becomes a natural tool for describing the dynamics of the system in terms of bodyreferenced coordinates. It has two main advantages: The incremental sensor readings
are referenced to this moving frame; and the inertia tensor, intervening in the dynamics,
is constant when described relative to the body-fixed frame (assuming the mass distribution of the rigid body remains constant as required by the rigidity condition), which
is not the case for the space fixed frame.
Chapter 2. Tools
24
R
Figure 2.2: Attitude as the linear transformation (represented by the matrix R) from
a reference orientation represented by axes e1 , e2 , e3 (left) to the actual orientation of
the rigid body with axes b1 , b2 , b3 (right). The origin is usually located at the center
of mass for convenience.
The columns of the matrix R describing the transformation are the axis of the space
frame in the attitude orientation with coordinates in the reference orientation

| | |


~ ~ ~ 
R=
 b1 b2 b3 
| | |

2.2.4
Configuration Space and Configuration for the Rigid Body
The configuration space of the rigid body is identified with SE(3), the R3 part will be
identified with its translational part and the SO(3) part will be identified with its rotational part, the attitude description. As the rotational dynamics are far more complex
than the translational dynamics, the rotational part will be studied on its own, and the
configuration space will be said to be SO(3). The velocity phase space will take place
in the tangent space T SE(3), and as stated in section 2.1.27, it will be identified with
se(3) (or so(3) if the rotational part is considered alone).
A configuration of the rigid body will be denoted by (R, r) ∈ SE(3), where R is the
attitude and r is the position of the center of mass with respect to the space fixed
frame. The velocity and angular velocity are denoted by (Ω, v) ∈ se(3). As they are
isomorphic, Ω and its hat map Ω× = Ω̂ will be used interchangeably to avoid cluttering
the equations.
R and Ω components are in body-referenced coordinates, while r and v are in space-fixed
referenced coordinates.
Chapter 2. Tools
2.2.5
25
Dynamical Properties of the Rigid Body
The rigid body may be characterized by three properties. Its mass, its center of mass
and its inertia tensor. They are derived from the measure µ and the set B introduced
in subsection 2.2.1.
The total mass of the rigid body is denoted by m and is defined by
m = µ(B)
(2.43)
The center of mass is defined as
1
r=
m
Z
xdµ
(2.44)
B
In what follows, the body fixed axis {~b1 , ~b2 , ~b3 } will be pinned to the center of mass. By
doing so, the resulting equations take a simpler form.
The total mass m and the center of mass r will play an important role in the translational
dynamics, but will not affect the rotational dynamics, which will be governed by the
moment of inertia defined as a linear map L̂ : so(3) → so? (3),
Z
(x − r) × Ω̂(x − r)dµ
L̂(Ω̂) =
(2.45)
B
When using the body-referenced frame, the matrix representation of this map can be
written as
L(Ω) = JΩ
(2.46)
with J ∈ R3x3 called the inertia matrix. For non-degenerate rigid bodies (i.e. such that
their support B is not a line or a point) it holds that J > 0.
2.2.6
Kinetic Energy
The kinetic energy of the rigid body B with mass distribution µ at time t is given by
the sum of the kinetic energy of each point of the rigid body, that is
Z
KE(t) =
B
1
kr(t) + Ṙ(t)xk2 dµ
2
(2.47)
The Kinetic Energy can be decomposed into rotational and translational kinetic energies because if r(t) describes the position of the center of mass, the cross-term when
developing the square vanishes. So the separate energies are given by
1
KEtran (t) = kṙ(t)k2
2
(2.48)
Chapter 2. Tools
26
and
Z
KErot (t) =
B
1
kṘ(t)xk2 dµ
2
(2.49)
and it holds that
KE(t) = KErot (t) + KEtrans (t)
(2.50)
With these terms, we can directly construct the Lagrangian, as we are not working with
potential energy associated to the rigid body. Later on, the effects of gravity will be
accounted for as external forces.
2.2.7
Translational Kinematics & Dynamics
The translational dynamics derived from equation (2.48) can be seen as described by
Newton’s equations, with m the mass of the rigid body and f the total external force
applied to the rigid body
2.2.8
ṙ = v
(2.51)
v̇ = mv + f
(2.52)
Rotational Kinematics
The rotational kinematic equation is somewhat more involved, but can be derived from
geometrical grounds. For each basis vector ~bi rotating with angular velocity Ω, its time
derivative is a vector perpendicular both to ~bi and Ω, and its magnitude will be that
of Ω
~b˙ i = Ω × ~bi
(2.53)
Since the columns of R are the vectors ~bi , the following equation must hold for R
Ṙ = RΩ̂
Equation (2.54) is called the rotational kinematic equation.
(2.54)
Chapter 2. Tools
2.2.9
27
Rotational Dynamics
From equation (2.49), the rotational kinetic energy can be obtained in terms of the
moment of inertia, equation (2.45), as
KErot (t) =< L(Ω(t)), Ω(t) >
(2.55)
With this kinetic energy the Lagrangian can be constructed and Euler-Lagrange equations of motion can be derived with variational methods in SO(3). They are derived
as
Ṙ = RΩ̂
(2.56)
J Ω̇ = −Ω̂JΩ
(2.57)
These equations describe the rotation of a free rigid body. Equation (2.54) is can also
be derived by variational methods in SO(3). Additionally, the Lagrange-D’Alambert
principle must be used to introduce external forces (as torques lying in so∗ (3)), which
will be crucial to control the rigid body, so equation (2.57) must be modified to allow
for these external forces. By calling the external torque M , the equation becomes
J Ω̇ = −Ω̂JΩ + M
(2.58)
This equation is also called Euler’s second law, and together with the translational
dynamics equations, they are called Newton-Euler equations.
2.3
Matrix Norms
2.3.1
Induced Norms
A norm k · kp of Rn induces a norm on Rn×n by
kAkp = sup
kAxkp
kxkp
(2.59)
where x ∈ Rn and A ∈ Rn×n .
2.3.2
Matrix 2-norm
The matrix 2-norm is induced by the vector 2-norm and it can be shown that
kAk2 = λM (A)
(2.60)
Chapter 2. Tools
2.3.3
28
Frobenius Norm
The Frobenius Norm of a matrix A ∈ Rn×n is defined by
kAkF =
q
tr (AT A)
(2.61)
The Frobenius norm is equivalent to the matrix k · k2 norm although it is not induced
by the vector k · k2 norm. In particular, for x̂ ∈ so(3) we have the useful equivalence
between the matrix and vector norm
1
√ kx̂kF = kxk2
2
2.3.4
(2.62)
Bounds on Vectors multiplied by Matrices
The norm of a vector times a matrix is bounded by the largest eigenvalue of the matrix,
since the vector 2-norm induces the matrix 2-norm. The following relationship holds
kAxk ≤ |λM |kxk
(2.63)
Also, if A > 0, that is to say, A is positive definite, then
kAxk ≥ λm kxk
(2.64)
Here the norm is the standard vector 2-norm, since all the normed quantities are vectors.
2.4
Stability of Dynamical Systems
A dynamical system whose state is given by x, evolving over time according to
ẋ = f (x)
(2.65)
is said to have an equilibrium point at x = xeq if f (xeq ) = 0. By defining a new state
variable, x0 = x − xeq , equation (2.65) is unaltered. The equilibrium point for the system
in terms x0 is now the origin, so the following definitions will be given in terms of the
origin without losing generality.
The origin is said to be a stable equilibrium [18], if for each > 0 there exists δ() > 0
such that the evolution of the system starting at a distance less than δ of the origin
Chapter 2. Tools
29
remains within distance of the origin, i.e.
kx(0)k < δ ⇒ kx(t)k < ∀t ≥ 0
(2.66)
The equilibrium is said to be asymptotically stable, if it is stable and there exists a δ,
such that
kx(0)k < δ ⇒ lim x(t) = 0
t→∞
(2.67)
and it is said to be unstable if it is not stable.
If the system verifies
kx(t)k ≤ kx(t0 )ke−γ(t−t0 )
(2.68)
for t > t0 > 0 and kx(t0 )k < c, then it is said to be exponentially stable.
2.4.1
Lyapunov Stability
If the origin is an equilibrium point for the system (2.65) and there exists a continuously
differentiable function V defined on D a neighborhood of the origin, such that
V(x) ≥ 0
∀x ∈ D,
V (0) = 0 ⇔ x = 0
V̇(x) ≤ 0 ∀x ∈ D
(2.69)
(2.70)
then the origin is stable. Additionally, if the inequality in equation (2.70) holds for
x 6= 0, then the origin is asymptotically stable.
If there exist α1 (r) and α2 (r) and α3 (r), strictly increasing real functions vanishing at
r = 0, such that
α1 (kxk) ≤ V(x) ≤ α2 (kxk)
(2.71)
V̇(x) ≤ −α3 (kxk)
(2.72)
then the origin is asymptotically stable. If the functions αi can be chosen so that
αi (r) = ci r2 then the origin is exponentially stable.
A converse theorem exists stating that if the origin is exponentially stable, then a function V satisfying equations (2.71) and (2.72) exists.
Chapter 3
Attitude Control
In this chapter we start by reviewing some common ways to represent the attitude to
justify our choice of attitude representation. Following that, we develop the control laws
using this representation. In this chapter, we follow the work of Lee [17].
To construct the control law we will define errors associated with the difference between
the actual and desired states of the system. The stabilization will be obtained from
these errors, combining them in ways that make the closed loop system drive the errors
towards their smallest attainable values, so the study of the error dynamics is carried
out in detail. Over these dynamics, the control is built using Lyapunov methods to
assure it converges to the minimum error state.
3.1
Attitude Representations
An Attitude Representation is a parametrized description of the attitude. An extensive
list of different attitude representations is presented in [19] . The representation is said
to be global, if every possible attitude may be represented in terms of the parameters.
It is said to be unique, if for each attitude there is a single combination of parameters
which represent it.
The number of parameters required to represent the attitude depends on the kind of
representation. If the representation isn’t required to be global and unique, 3 parameters
are enough, and Euler Angles can be used, as stated in section 2.1.26. If we want the
representation to be at least global or unique, a higher count of parameters must be
used.
The minimum number of parameters required to uniquely and globally represent attitude, as proved by Hopf [20] and cited by Stuelpnagel [21] is 5. By taking the first 2
30
Chapter 3. Control
31
columns of the rotation matrix R, a 6-dimensional representation can then be further
reduced by projection to a 5-dimensional representation.
But this projective representation is seldom used as the advantage of reducing the number of parameters is hindered by the complexity of the resulting kinematic and dynamic
equations.
Another 5-parameter representation is the Unit Quaternion (also called Euler-Rodrigues
Parameters) representation, in which the attitude is represented by 4 parameters plus
the sign. This approach is widely used because of the resultingsimple equations and
the computational efficiency, but the sign must be accounted for separately, breaking
smoothness. Using only the 4-parameter unit quaternion without taking into account
the sign may lead to unexpected behavior such as unwinding, in which the command to
go to a nearby attitude produces an unexpected whole turn instead of a small twist (see
Figure 3.1). This leads to the fact that although the global dynamics of the parametrized
attitude may have, when controlled, closed loop global or almost global stability, it
doesn’t necessarily imply that the physical rigid body evolving in SO(3) also has these
closed loop properties [22].
Figure 3.1: To illustrate the problem of using unit quaternions to represent SO(3)
we can get intuition from a similar problem [23], representing the circle with the edge
of a Moebius strip. Points on the circle are labeled a, b, . . . , z. Points on the edge of
the strip a0 , b0 , . . . , z 0 , a00 , . . . , z 00 can be used to represent points on the circle, the map
being “remove the primes”. But for each point in the circle there are two points on the
edge, so it’s not unique. Unwinding arises because the command to go from z to a may
be interpreted as go from z 0 to a00 , go from z 00 to a0 (short movements), or also as go
from z 0 to a0 or from z 00 to a00 (an extra turn around the circle needed).
Chapter 3. Control
32
By using the full 3x3 attitude matrix R ∈ SO(3) instead of a representation of it, all these
problems are avoided. The downside is an increase in the number of parameters, since
the 9 elements of the matrix must be taken into account. But the resulting kinematic
and dynamic equations are simple and intuitive, and allow for Lyapunov methods of
stability analysis and control.
3.2
Rigid Body Dynamics
The configuration of the rigid body having mass m and inertia moment with respect to
its center of mass and in body fixed coordinates J will be denoted by (R, r) ∈ SE(3)
where R is the attitude and r is the position of the center of mass with respect to the
space fixed frame. The angular velocity and velocity are denoted by (Ω, v) ∈ se(3), with
Ω in body-referenced coordinates.
We review the kinematics and dynamics results from previous sections as a starting
point to obtain the control laws with the terms for control force f and control torque
M . The perturbations regarding applied force ∆f and torque ∆M have been written
separately to better analyze their effects. The equations (2.51), (2.54) and (2.57) take
the form
ṙ = v
(3.1)
v̇ = mv + f + ∆f
(3.2)
Ṙ = RΩ̂
(3.3)
J Ω̇ = −Ω̂JΩ + M + ∆M
(3.4)
As we are focusing on attitude control and since rotational and translational dynamics
are decoupled, we will focus our study on the last pair of equations.
3.3
The Attitude Control Problem
The problem of Attitude Control is the problem of applying the correct control torque
M to the rigid body so that it attains a desired Attitude over time.
Which torque is the correct torque is the essence of the problem, since according to
different constraints on the allowable attitudes itself, on the kinds of perturbations of
the attitude, on how this torque is applied, on its magnitude, on the allowed bound
Chapter 3. Control
33
on the error of the attitude with respect to the desired attitude, on the error in the
knowledge of the actual attitude for which the torque is computed, etc. will lead to
different definitions of which is the correct torque; the solution to the Attitude Control
Problem is not unique.
3.4
Attitude Control Laws in SO(3)
Taeyoung Lee et al. [24], [17] developed several almost global control laws for the
attitude control of a quadrotor UAV. In what follows, variables subscripted by d will be
the reference variables indicating the commanded or desired value of that variable, so
for example Rd (t) is the desired value of the attitude at time t. The reference variables
must satisfy the kinematic equations.
3.4.1
Attitude Error Dynamics
An important step in the design of any control system is the design of an error function
that vanishes if the system is in the desired reference state and increases as it deviates
from it. To be able to compare the value of the error function for different states of the
system and reference states, this function must take values on an ordered set; R is the
most usual choice.
3.4.2
Error Tracking Function
An error tracking function for the attitude as defined by [15] must be a map Ψ : SO(3)×
SO(3) → R, such that Ψ(R, Rd ) = 0 ⇔ R = Rd , it is symmetric in R and Rd , Ψ(R, Rd ) =
Ψ(Rd , R), its differential with respect to any of its arguments vanishes at R = Rd and
its Hessian with respect to any of its arguments is positive definite at R = Rd .
The simplest error function which satisfies all these properties is the squared Frobenius
Norm of the difference
1
Ψ(R, Rd ) = kR − Rd k2F
2
(3.5)
where the positive proportionality constant has been arbitrarily chosen, and since the
trace is linear and trace(A) = trace(AT ), the error function may also be written as
1
Ψ(R, Rd ) = trace I − RdT R
2
(3.6)
which vanishes when R = Rd . The symmetry requirement is easy to verify, as equation
(3.5) is invariant against permutation of R and Rd .
Chapter 3. Control
34
The squared norm form may be related to the potential of a harmonic oscillator V (x) =
1
2
2 kxk .
This potential drives the point x towards the origin with a force f = −∇V .
Likewise, to find the error function derivative, called the attitude vector error, Ψ must
be differentiated. But as the domain of Ψ is SO(3)×SO(3), the left trivialized derivative
introduced in 2.1.11 must be used.
3.4.3
Attitude Error Vector
For elements of SO(3), this derivative gives values in so∗ (3), so the inverse hat map will
be used to identify it with values of R3 . As the system is meant to be driven towards Rd ,
the derivative is taken with respect to R and is defined by the map eR : SO(3)×SO(3) →
R3 given by
êR = DR Ψ (R, Rd )
(3.7)
which by simple computation is found to be
eR =
1
RdT R − RT Rd ˇ
2
(3.8)
From equation (2.26), the error vector eR has the direction of the axis of rotation that
takes R to Rd . Its magnitude is the sine of the angle of rotation θ around this axis (see
Figure 3.2). With this geometric interpretation of the error vector (and due to the fact
that trace(A) = trace(AT )), equation (3.6) can be written as
RdT R + RT Rd
1
1
ˆ
= trace I − cos(ξ)
Ψ(R, Rd ) = trace I −
2
2
2
(3.9)
for some ξˆ ∈ so(3) such that kξk = θ. It can be shown that
ˆ = 1 + 2cos(kξk)
trace(cos(ξ))
so
Ψ(R, Rd ) = 1 − cos (θ)
(3.10)
Likewise, for the vector error function, from the definition of the sine in equation (2.18)
it can be shown in the same way that
keR k = sin (θ)
(3.11)
Chapter 3. Control
35
Figure 3.2: Geometric interpretation of the attitude error function and the attitude
error vector. For this representation, one dimension has been omitted from SO(3) and
it is shown embedded in R3 as the unit sphere, with the north pole identified with
the desired attitude Rd . The sine of the angle between R and Rd is sin(θ) = keR k.
The distance in R3 along Rd between Rd and the projection of R onto Rd is the error
function Ψ. Ψ = 0 when R = Rd , and increases as R deviates from Rd . At the equator
it takes the value Ψ = 1 and towards the south pole it approaches Ψ = 2. The error
vector is perpendicular to both R and Rd and belongs to the tangent space of R, but
was pinned to the center of the sphere for clarity.
3.4.4
Angular Velocity Error Vector
The angular velocity vector eΩ compares Ω with Ωd , but as they lie in different tangent
spaces to R and Rd respectively, Ωd is left trivialized to R to make the comparison
meaningful. It depends not only on the value of Ω and Ωd but also on the attitude R
and the attitude command Rd . It is defined as
eΩ = Ω − RT Rd Ωd
(3.12)
Chapter 3. Control
Figure 3.3: Geometric interpretation of the angular velocity error vector. The same
representation restrictions apply as in Figure 3.2, but here the tangent spaces at Rd
and R have been sketched in three dimensions. The desired angular velocity Ωd is a
vector in the tangent space at Rd , TRd G, whereas the current angular velocity belongs
to the tangent space at R, TR G. For the comparison to be carried out, Ωd must be
taken to TR G. This is done by the left action of Rd taking it to the tangent space of the
identity element and then left acting again with RT to take it to TR G. The composition
of these two actions is the same as the RT Rd action. The intermediate step has been
omitted for clarity. The comparison can then be carried out subtracting both terms,
as Ω and RT Rd Ωd belong to the same linear space, which is also where the errors to
be fed back later belong. The attitude error vector eR depicted at the origin of Figure
3.2 also belongs to this space.
36
Chapter 3. Control
3.4.5
37
Attitude Error Dynamics
With these definitions, the key element in obtaining the attitude error dynamics is the
time derivative of the relative rotation RdT R, given by
d
RdT R = RdT R êΩ
dt
(3.13)
This equation can be interpreted by identifying it with a kinematic equation (section
2.1.28), where the relative orientation matrix is itself a matrix of SO(3) rotating with
an angular velocity given by the angular velocity error.
The time derivative of the attitude error function Ψ is found by first noting that the
trace is a linear operator, so
d
1
Ψ (R, Rd ) = − trace
dt
2
d
RdT R
dt
1
= − trace RdT RêΩ
2
(3.14)
and by using equation (2.15),
d
Ψ (R, Rd ) = eR eΩ
dt
(3.15)
The time derivative of the vector error function is
ėR =
1
d
(skew (R, Rd )ˇ) =
RdT RêΩ − êΩ RT Rd ˇ
dt
2
(3.16)
which by properties (2.15) and (2.16), using the auxiliary function
E (X) =
1
(trace (X) I − X)
2
(3.17)
can be reduced to
ėR = E RT Rd eΩ
(3.18)
which predicts the way eR will increase (or decrease) given the error in angular velocity
and the relative attitude between current attitude and commanded attitude. By using
an auxiliary variable αd , which is the acceleration produced by the attitude command
Ωd on the attitude
αd = −Ω̂RT Rd Ωd + RT Rd Ω̇d
(3.19)
the time derivative of the angular velocity error vector can be found by using properties
and substituting with the kinematic equation to be
ėΩ = J −1 −Ω̂JΩ + M + ∆M − αd
(3.20)
Chapter 3. Control
38
Errors
Attitude Error Function
Ψ(R, Rd ) = 12 trace I − RdT R)
Attitude Error Vector
eR = skew (R, Rd )ˇ
Angular Velocity Error Vector
eΩ = Ω − RT Rd Ωd
Auxiliary Functions
1
2
Error Matrix
E RT Rd =
Angular Acceleration Command
αd = −Ω̂RT Rd Ωd + RT Rd Ω̇d
trace RdT R I − RT Rd
Time Derivatives ...
of the Attitude Error Function
Ψ̇ = eR eΩ
of the Attitude Error Vector
ėR = E (R, Rd ) eΩ
ėΩ = J −1 −Ω̂JΩ + M + ∆M − αd
of the Angular Velocity Error Vector
Table 3.1: Summary of the Attitude Error and its Dynamics.
So the time derivative of the angular velocity error vector consists of two terms, the first
one includes the dynamics of the free rotation and the control torque, while the second
term is the acceleration generated by the attitude command.
3.4.6
Attitude Control Law
With the previous definitions of Attitude Error and its Dynamics, a law for the control
torque can be obtained to drive R towards Rd , i.e. make the attitude track a reference
attitude. The law has two free parameters (also called gains) kR , kΩ ∈ R and is given by
M = −kR eR − kΩ eΩ + Ω̂JΩ + Jαd
(3.21)
For the following initial conditions
Ψ (R(0), Rd (0)) < 2
keΩ (0)k2 <
2
kR (2 − Ψ (R(0), Rd (0))) < 2
λM (J)
(3.22)
(3.23)
and assuming that there is no perturbation, ∆M = 0 and kR and kΩ are both positive,
this control law will be proven to exponentially stabilize the attitude around Rd .
Chapter 3. Control
3.4.7
39
Lyapunov Function for the Attitude Control Law
To prove these convergence properties, a Lyapunov function candidate is constructed as
1
V = eΩ · JeΩ + kR Ψ(R, Rd ) + ceΩ · eR
2
(3.24)
where c is a positive parameter introduced for convenience. It can be seen as a cross
gain involving both eR and eΩ .
From an energy perspective point of view, the first term represents the kinetic energy of
the current attitude with respect to the desired attitude, the second term its potential
energy and the last term its angular momentum. In the case when the current attitude
coincides with the desired attitude all these terms vanish.
Also for convenience, a subset of SO(3) is considered for the stability analysis, denoted
by Lψ , it is the set of attitudes with error function Ψ smaller than ψ, denoted by
Lψ = {R ∈ SO(3) | Ψ(R, Rd ) < ψ}
(3.25)
The analysis will be initially restricted to Lψ , and the extension to the whole SO(3)
will be found out to be trivial. At R = −Rd some of the following arguments cease to
hold, despite the fact that the system remains stable. To avoid these problems, we take
as an assumption that ψ < 2, so that the error function is assumed to be bounded by
Ψ(R, Rd ) ≤ ψ < 2.
By using the attitude error dynamics, its time derivative is
V̇ = eΩ · J ėΩ + kR eR · eΩ + ceΩ · ėR + cėΩ · eR
(3.26)
By equation (3.16) the relationship between eΩ and ėR is
kėR k ≤ kE(RT Rd )k2 · keΩ k
(3.27)
The norm of E(RT Rd ) is found by replacing RT Rd by exp(x̂), and evaluating its eigenvalues from the Rodrigues formula, equation (2.23), and using the bound on the matrix
norm from equation (2.63) it is found to be kE(X)k ≤ 1 for any X ∈ SO(3). So
kėR k ≤ keΩ k
(3.28)
By noting that the Euler equation (2.58) can be combined with the control torque
equation (3.21) to yield
J ėΩ = −kR eR − kΩ eΩ
(3.29)
Chapter 3. Control
40
we can replace equation (3.28) into equation (3.26) to begin to look for a bound on V̇.
We obtain
V̇ ≤ eTΩ J ėΩ + kR eTR eΩ + ckeΩ k2 + ceTR ėΩ
(3.30)
V̇ ≤ kR eTR eΩ + ckeΩ k2 + ceTR + eTΩ J ėΩ
(3.31)
Rearranging the terms
V̇ ≤ kR eTR eΩ + ckeΩ k2 + ceTR J −1 J ėΩ + eTΩ J ėΩ
V̇ ≤ kR eTR eΩ + ckeΩ k2 + cJ −1 eR + eΩ
T
(3.32)
J ėΩ
(3.33)
(−kR eR − kΩ eΩ )
(3.34)
Replacing the last term with equation (3.29)
V̇ ≤ kR eTR eΩ + ckeΩ k2 + cJ −1 eR + eΩ
T
V̇ ≤ −ckR eTR J −1 eR + (c − kΩ ) keΩ k2 − ckΩ eTR J −1 eΩ
(3.35)
Now, by noting that for a matrix A > 0 it holds that
1
kxk2 ≤ xT A−1 x
λM (A)
it follows that the first term is bounded by
−ckR eTR J −1 eR ≤ −
ckR
keR k2
λM (J)
and by noting that it also holds that
±xT A−1 y ≤
1
kxkkyk
λm (A)
the last term is bounded by
−ckΩ eTR J −1 eΩ ≤
ckΩ
keR kkeΩ k
λm (J)
We can now bound equation (3.35) using only the gains, the smallest and largest eigenvalues of the inertia matrix J and the norm of the error vector and angular velocity
error vector as
V̇ ≤ −
ckR
ckΩ
keR k2 − (kΩ − c) keΩ k2 +
keR kkeΩ k
λM (J)
λm (J)
(3.36)
Calling
z=
keR k
keΩ k
!
(3.37)
Chapter 3. Control
41

W =
ckR
λM (J)
Ω
− 2λck
m (J)
Ω
− 2λck
m (J)
kΩ − c


(3.38)
the terms can be rearranged to the matrix inequality
V̇ ≤ −z T W z
(3.39)
Recalling equations (3.10), (3.11), restriction Ψ ≤ ψ and using the trigonometric relationship sin2 (γ) = (1 + cos(γ)) (1 − cos(γ)), the error vector function can be used to
bound from above and from below the error function Ψ since
keR k2 = sin2 (θ) = (1 + cos(θ)) Ψ(R, Rd )
(3.40)
keR k2 = (2 − Ψ(R, Rd )) Ψ(R, Rd )
(3.41)
1
1
keR k2 ≤ Ψ(R, Rd ) ≤
keR k2
2
2−ψ
(3.42)
so solving for Ψ(R, Rd )
This bound is then used to bound the Lyapunov function V from below as
1
1
kR keR k2 − ckeΩ kkeR k + λm (J)keΩ k2 ≤ V
2
2
(3.43)
and from above as
V≤
2kR
1
keR k2 + ckeΩ kkeR k + λM (J)keΩ k2
2−ψ
2
(3.44)
Both bounds can be put together in matrix form by calling
M1 =
kR
−c
!
(3.45)
−c λm (J)
and
M2 =
bkR
c
c
λM (J)
!
(3.46)
then using z as defined in equation (3.37), and recalling equation (3.39), these results
can be put in the usual Lyapunov form as
z T M1 z ≤ V ≤ z T M2 z
(3.47)
V̇ ≤ −z T W z
(3.48)
Chapter 3. Control
42
For this to mean exponential stability as defined in section 2.4.1, the matrices W , M1
and M2 must be positive definite. As the matrices are 2 × 2, the eigenvalues can be
easily obtained. Since they are symmetric matrices, if the eigenvalues are positive the
matrices are positive definite. This is achieved if for a given inertia tensor J the gains
kR , kΩ and c are chosen such that they verify
c < min{kΩ ,
p
4kΩ kR λ2m (J)
,
kR λm (J)}
2 λ (J) + 4k λ (J)2
kΩ
M
R m
(3.49)
When this is the case, equations (3.47) and (3.48) can be written as
By calling β =
λm (W )
λM (M2 ) ,
λm (M1 )kzk2 ≤ V ≤ λM (M2 )kzk2
(3.50)
V̇ ≤ −λm (W )kzk2
(3.51)
the relationship between V and its time derivative is
V̇ ≤ −βV
(3.52)
V(t) ≤ V(0)e−βt
(3.53)
so an explicit bound for V(t) is
meaning that the attitude error function is bounded by a decreasing exponential. This
guarantees that the attitude error function converges to zero as the vector with the norm
of the attitude error vector and attitude angular velocity vector kzk → 0 when t → ∞.
This also implies that the set Lψ to which we restricted our analysis (defined in equation
(3.25)) is positively invariant for any ψ < 2. Note that ψ can get arbitrarily close to 2.
It can be shown that the submanifold defined by the points in which Ψ(R, Rd ) = 2 is
of zero measure and unstable. Therefore, the stabilization is said to be almost global.
Moreover, this is the best result regarding globality for continuous control laws in SO(3),
as continuous global stabilization cannot be achieved to topological constraints [25].
To design a Controller, only the maximum and minimum eigenvectors of the inertia
tensor J need to be known, and the parameters kR , kΩ and c are selected according
to the restriction imposed by equation (3.49). If the inertia tensor J is not known, the
quote on its maximum and minimum eigenvalues may be chosen to give some headroom,
or an adaptive algorithm to estimate J online may be used, as in [17].
Chapter 3. Control
3.4.8
43
Summary of the results on Attitude Control
We briefly summarize the results of this section for clearer reference.
Given the kinematic and dynamic equations
Ṙ = RΩ̂
J Ω̇ = Ω̂JΩ + M
(3.54a)
(3.54b)
and the command Rd (t) satisfying Ṙd = Rd Ω̂d , the errors
1
Ψ(R, Rd ) = trace I − RdT R
2
êR = DR Ψ (R, Rd )
(3.55b)
eΩ = Ω − RT Rd Ωd
(3.55c)
(3.55a)
together with the feedback law
M = −kR eR − kΩ eΩ + Ω̂JΩ + Jαd
(3.56)
reduce the closed loop error dynamics to
ėR = E (R, Rd ) eΩ
(3.57a)
ėΩ = −kR J −1 eR − kΩ J −1 eΩ
(3.57b)
and the Lyapunov function defined as
1
V = eΩ · JeΩ + kR Ψ(R, Rd ) + ceΩ · eR
2
(3.58)
λm (M1 )kzk2 ≤ V ≤ λM (M2 )kzk2
(3.59)
V̇ ≤ −λm (W )kzk2
(3.60)
satisfies
with z = (keR k
3.4.9
keΩ k)T
Units
A note of warning should be raised about units. From its definition, eR is unitless while
eΩ should have units of angular velocity. When combining them in the control law
equation (3.56) or in the Lyapunov equation (3.58) and its time derivative, they always
mix with constants kR and kΩ to comply with the resulting units.
Chapter 3. Control
44
But when evaluating kzk we are adding a unitless number to a number with units
of angular velocity. It is assumed that the right conversion factor is included in the
definition of the norm of either one of the quantities being added to avoid any trouble.
This could be further expanded into a weighted sum but this path is avoided because it
would hinder the elegance of the equations.
Chapter 4
Robust Attitude Control
In this chapter, we study the stability of the attitude control law with respect to perturbations, and guarantee stability if certain conditions on the system and perturbations
are met. Perturbations arise from several causes, but the two types of perturbations we
will take into consideration are aerodynamic perturbations, acting as additional, uncontrolled torques, and perturbations due to inaccurate, faulty or noisy measurements of
the attitude and the angular velocity.
4.1
Aerodynamic Perturbations
The analysis we have been carrying out so far is agnostic of the actuators, assuming
they apply exactly the torque provided by equation (3.21). But for the aircraft under
study, this torque is created by several rotating and counter-rotating rotors, which are
difficult to model precisely.
Other sources of aerodynamic perturbations in the rotors are the change in wind speed
due to the craft’s vertical motion, with the effect that the same blade rotation produces
different lift in different situations, and the tip flapping effect due to the craft’s horizontal
motion, when the tip of the blade oscillates due to differential force created by the blade’s
relative airspeed being different when moving against or along the wind speed [26], [27].
Drag is also dependent on the motion of the craft and is also treated as a perturbation.
Although we are not concerned with the position of the craft in this analysis, it may
still act differentially on different parts of the craft creating torque.
To account for the inaccuracies of the aerodynamic model of the rotors, and also for
other unmodeled torques acting on the aircraft such as the effect of wind gusts, or any
45
Chapter 3. Control
46
other torque, the term ∆M has been added to equation (3.4). In the following analysis,
the only assumption we will make about this unmodeled torque is that it is bounded by
δR , a known constant such that
k∆M k ≤ δR
(4.1)
Obtaining this bound is no easy task. In section 4.5 we will see that with all other
parameters fixed, the bound on the minimum attainable error in attitude and angular
velocity is linear in δR .
4.2
Perturbations due to Measurement Inaccuracies
Another source of perturbations is the fact that to actually obtain the required stabilizing
torque of equation (3.21) we need the actual attitude and angular velocity of the aircraft.
But this knowledge is never precise, and usually these quantities are known up to a small
measurement error. Factors such as noise and drift present in the data used to calculate
the torque appear as perturbations in the control dynamics.
These factors are usually reduced by filtering, like [28] and [29]. Filtering and estimation
is in itself an area of active research. We call them “measured quantities” and talk of
“the measurement error” in the broad sense of these concepts. But despite the method
used for estimating the angular velocity and attitude, a residual error always remains.
Given the measured attitude R̄ and angular velocity Ω̄, their relationship to the actual
attitude R and angular velocity Ω are given by the attitude measurement misalignment
term z and the angular velocity measurement error χ (preserving the structure of SO(3)
and so(3)) respectively as
R̄ = R · z
(4.2)
Ω̄ = Ω + χ
(4.3)
The following bounds are assumed to be satisfied by the estimation algorithm, with θZ
the angle of rotation between R̄ and R about the proper rotation axis
θZ ≤ κM AX
(4.4)
kχk ≤ χM AX
(4.5)
In the following section we will guarantee robust stability of the attitude when given
a bound for the knowledge of both the angular velocity and the attitude, provided the
angular velocity and its command are bounded.
Chapter 3. Control
4.3
47
Robustization
To study stability we will construct the stabilizing torque using the measured values and
propagate the measurement inaccuracies to the actual torque, where they are composed
with the unmodeled perturbations. We will see how these terms affect the Lyapunov
function and its time derivative.
To perform this analysis we need bounds on the allowed angular velocities and their
command. These bounds will be the subject of a compromise in almost the same way
as the bound of the unmodeled torques, to be studied in a subsequent section. Note
that regarding the rotational dynamics, only the angular velocity will be required to
be bounded, meaning that the attitude is unrestricted as in the previous unperturbed
analysis so this analysis will hold almost globally, i.e. for any attitude and any attitude
command satisfying the kinematic restriction (2.40). Subscripting the bounds with a
M AX term, these bounds are
4.4
kΩk ≤ ΩM AX
(4.6)
kΩd k ≤ ΩdM AX
(4.7)
kΩ̇d k ≤ Ω̇dM AX
(4.8)
Perturbed Errors Vectors
To construct the control torque we rely on the measurements in order to build the error
vectors. The measured quantities will be denoted by a bar, so x̄ means the measured
value of x. These vectors are obtained by solving for R and Ω in equations (4.2) and (4.3)
and replacing them in equations (3.8) and (3.20). In this way, we obtain the perturbed
error vectors ēR and ēΩ , and also the perturbed error tracking function Ψ̄ which will be
useful later, and we express them in terms of the real error vectors and error tracking
function as
1
Ψ̄ = Ψ + trace RdT R (z − I)
2
ēR = eR + skew RdT R (z − I) ˇ
ēΩ = eΩ − RdT R (z − I)
T
Ωd + χ
ˆ R̄R Ω + R̄R Ω̇
ᾱd = −Ω̄
d d
d d
(4.9)
(4.10)
(4.11)
(4.12)
Except for the last equation, we managed to express the measured error vectors in
terms of the real error vectors and the error terms. This will prove to be useful for the
Lyapunov analysis.
Chapter 3. Control
48
With these measured error vectors, we may construct the control torque by replacing
the actual values by the measured values in the control law of equation (3.21). This
control law will also contain a term for the robustization, denoted by µ̄, which was not
present in the previous control law. The control law in terms of the measurements is
given by
ˆ J Ω̄ + J ᾱ + µ̄
M̄ = −kR ēR − kΩ ēΩ + Ω̄
d
(4.13)
The relationship with the exact torque can be expressed by introducing a fictitious
torque due only to the propagation of the measurement errors, ∆E .
∆E = M̄ − M
(4.14)
As the system remains the same and we are only changing the control law to accommodate the fact that we are feeding back the measured values and not the real ones,
we can reuse the Lyapunov function of equation (3.24). This has proved to be a valid
Lyapunov function and it does not depend on the control input.
On the other hand, its time derivative, equation (3.26), which we rewrite here for clarity,
depends on the control input to be negative definite.
V̇ = eΩ · J ėΩ + kR eR · eΩ + ceΩ · ėR + cėΩ · eR
(4.15)
The only way the control torque enters the Lyapunov time derivative is through the time
derivative of the angular velocity error vector, ėΩ as stated in equation (3.20). We also
rewrite it here for clarity, with the acting torque M replaced by the composition of the
exactly stabilizing torque given by the control law equation (3.21) and the propagated
error vector M̄ = M + ∆E
ėΩ = J −1 −Ω̂JΩ + (M + ∆E ) + ∆M − αd
(4.16)
From this equation, it is clear that the propagated error vector plays the same role
as the perturbation torque ∆M , because this is the only place it enters the equations.
From this point of view ∆M and ∆E have the same effect on the time derivative of the
Lyapunov function.
Some properties will come in handy to write down the term ∆E . By using the property
of the decomposition of the difference of SO(3) elements, equation (2.29) we can write
(z − I) as
(z − I) =
with ξˆ =
√
z−
√
z
T
.
√
z ξˆ
(4.17)
Chapter 3. Control
49
By property (2.18) and using the fact that kRk = 1 ∀R ∈ SO(3) we obtain the norm of
(z − I) as
k (z − I) k = 2sin
θZ
2
(4.18)
Likewise, by using equation (3.17), equation (4.10) may be written as
√ ēR = eR + E RdT R z ξ
(4.19)
Lastly, we will also be using the property given by equation (2.16).
With these properties ∆E takes the form
√ ∆E = − kR E RdT R z ξ
√
T
− kΩ I − J Ω̂ ξˆ z + Jχz T RT Rd Ωd
√ T
− J ξˆ z RT Rd Ω̇d
+ Ω̂ (2J − trace (J) I) + J Ω̂ − kΩ I χ
(4.20)
From this, it can be seen that what is different about ∆M and ∆E is their internal
structure, one of which is by now assumed unknown but bounded, and the other one is
known and is made explicit in equation (4.20). This internal structure will not be used
for robustization in this work; it will be used just to obtain a bound on its magnitude.
But in future works this may prove an interesting lead to follow.
A bound can be placed on ∆E as
θZ
kR
∆E ≤2sin
2
θZ
+2sin
kΩd k (kΩ + λM (J) kΩk) + λM (J) χM AX
2
θZ
λM (J) kΩ̇d k
+2sin
2
(4.21)
+ (2kΩkλM (J) + kΩ ) kχk
Using bounds (4.4), (4.5), (4.6), (4.7) and (4.8) we can find a bound δE for ∆E which
depends on the previous bounds and is given by
δE = 2sin
κ
M AX
kR + kΩ ΩdM AX + λM (J) ΩM AX ΩdM AX + Ω̇dM AX
2
+ (λM (J) (ΩM AX + ΩdM AX ) + kΩ ) χM AX
(4.22)
so
k∆E k ≤ δE
(4.23)
Chapter 3. Control
50
This bound condensates the bound for the attitude and angular velocity knowledge,
equations (4.4) and (4.5), the bound for the angular velocity (4.6) and the bounds for
the commanded acceleration (4.7) and (4.8).
An auxiliary error eA playing a key role in the robustization will be introduced as
eA = eΩ + cJ −1 eR
(4.24)
and its measured reconstructed counterpart is given by
ēA = ēΩ + cJ −1 ēR
(4.25)
which, replacing by the values for ēR and ēΩ , gives
√ T
√ ēA = eA + κ̂ z RT Rd Ωd + χ + cJ −1 E RT R z κ
(4.26)
and it will give rise to yet another bound, the last one for now, as
a = 2sin
κ
M AX
2
ΩdM AX +
c
λm (J)
χM AX
(4.27)
so that
kēA − eA k ≤ a
4.5
(4.28)
Robustization
Now, we will return to the time derivative of the Lyapunov function and make use of
equation (4.16) to make explicit the impact of the unmodeled torque perturbations and
the error propagation vector. The control torque constructed in terms of the measured
quantities cannot exactly match and cancel the errors, and this mismatch is expressed
in the second term of the following equation, letting the first terms be the consequence
of the same reduction as in equation (3.35) with z and W defined in equations (3.37)
and (3.38).
V̇ ≤ −z T W z + eA · (∆M + ∆E + µ̄)
(4.29)
From this it is clear that the only way we can maintain the time derivative of the
Lyapunov function negative definite is by adequately constructing the robustization
term µ̄ in terms of the available measurements.
Following the work by Lee [11], we perform a fixed-sigma modification. In the usual
leakage or fixed-sigma modification, as in [30], the robustization term has only one free
parameter R > 0 determining the bound, but we will see that the bound a defined
Chapter 3. Control
51
in equation (4.28) takes its role in the robustization performed here, so the bound will
depend on a. We define the robustization as
µ̄ = −
(δR + δE )
ēA
kēA k + a
(4.30)
With this robustization term, we can now study the last term of equation (4.29); our
objective is to prove that it is bounded. For this purpose, we will use the bounds from
equations (4.1) and (4.23) in the last term of equation (4.29), which is expanded to
eA · (∆M + ∆E + µ̄) =∆M · eA
+∆E · eA
(4.31)
(δR + δE )
−
(eA + (ēA − eA )) · eA
kēA k + a
so
eA · (∆M + ∆E + µ̄) ≤keA k · δR
+keA k · δE
(δR + δE )
keA k2
kēA k + a
(δR + δE )
−
(ēA − eA ) · eA
kēA k + a
−
(4.32)
and noting that −akeA k ≤ (ēA − eA ) · eA ≤ akeA k and (keA k − a) ≤ kēA k ≤ (keA k + a),
eA · (∆M + ∆E + µ̄) ≤ (δR + δE ) keA k
(δR + δE )
keA k2
keA k + a + a
(δR + δE )
+
keA ka
keA k + a − a
−
(4.33)
which by combining the first term of the right hand side into the first fraction reduces
to
eA · (∆M + ∆E + µ̄) ≤2a (δR + δE )
keA k
keA k + 2a
(4.34)
+ (δR + δE ) a
and the fraction of the first term of the right hand side is always smaller than the unit,
so the bound takes the form
eA · (∆M + ∆E + µ̄) ≤ 3 (δR + δE ) a
(4.35)
Chapter 3. Control
52
and the time derivative of the Lyapunov equation becomes bounded by
V̇ ≤ −z T W z + 3 (δR + δE ) a
(4.36)
z T W z ≥ 3 (δR + δE ) a
(4.37)
It will be negative as long as
which, recalling the definition of z = [keR k
keΩ k]T , corresponds to the outside of
an ellipse in the plane spanned by keR k and keΩ k. With this robustization term the
errors converge no longer to the origin of this plane, as in the unperturbed case, but
to the ellipse itself. The term corresponding to the product of the semi-axes of the
ellipse depends on the unmodeled perturbations bound δR and on the measurement
error bounds through δE and a.
It is interesting to note that the dependence of this last term on the measurement errors
is second order, because it is the product of δE with a, which also depends on the error
bounds. This also holds for the bound on the commanded angular velocity.
Figure 4.1: Representation of ellipse ε. The robust closed loop system converges to
this ellipse. Outside the ellipse (green shaded area) the time derivative of the Lyapunov
function is negative. The behavior inside the ellipse (orange shaded area) is unkown.
Also marked is the major semi axis sM .
Since we will be referring this ellipse later on, we will define it as the set ε which depends
on the gains as
ε(kR , kΩ , c) = z = (x, y)T ∈ R2 | z T W z = 3 (δR + δE ) a
(4.38)
Chapter 3. Control
53
Note that, although this ellipse looks like the ellipses used to represent knowledge as
mean and covariance in Gaussian Filters, the meaning is different in this case. The four
quadrants are included in the depiction (Figure 4.1) to make it more intuitive, but only
the first quadrant has a real meaning, as the equation is quadratic in the norm of the
errors. Besides that, the behavior of the errors inside the ellipse is unknown and no
claim about the probability of the state being on the inside is made. The only thing
the proposed control can guarantee is that the state will converge asymptotically to the
ellipse.
As the Lyapunov function remains unaltered by the robustization term, for any ψ <
2 the set Lψ defined in equation (3.25) remains positively invariant and the system
exponentially converges towards the origin like in the previous section until the ellipse
described by equation (4.37) is reached.
Afterwards, as the time derivative of the Lyapunov function is no longer negative, the
conditions for global exponential stability of the origin are no longer met. Inside the
ellipse the control action may no longer be able to avoid the effects of the perturbations,
and the behavior is unknown.
Depending on the instantaneous values of the perturbations, the system may randomly
walk inside the ellipse and, in favorable situations, it may approach the origin, but this
cannot be guaranteed. It may even be possible for the state to reach the ellipse again at a
later time, to bounce back in or slide along its surface, but it will never get further away
from the origin. This matches the definition of simple convergence, so the convergence
is replaced by simple convergence to the ellipse.
4.6
Summary
We summarize the bounds in Table 4.1. With these bounds, the auxiliary vector ēA
constructed from the measured values of attitude and angular velocity
ēA = ēΩ + cJ −1 ēR
is used to build a robustization term
µ̄ = −
(δR + δE )
ēA
kēA k + a
which, when added to the stabilizing torque (constructed also in terms of measured values
of attitude and angular velocity), assures that the errors in eR and eΩ will converge to
Chapter 3. Control
54
Bound
Evaluates to
ΩM AX
Max. angular velocity allowed by the system.
ΩdM AX
Max. commanded angular velocity.
Ω̇dM AX
Max. commanded angular acceleration.
κM AX
Max. error in attitude measurement.
χM AX
Max. error in angular velocity measurement.
δR
δE
a
Max. aerodynamic or unmodeled perturbation torque.
κ
M AX
2sin
kR + kΩ ΩdM AX + λM (J) ΩM AX ΩdM AX + Ω̇dM AX
2
+ (λM (J) (ΩM AX + ΩdM AX ) + kΩ ) χM AX
2sin κM2AX ΩdM AX + λmc(J) χM AX
Table 4.1: Summary of the bounds the system must satisfy to robustly converge to
the ellipse ε.
the ellipse
ε(kR , kΩ , c2 ) = z = (x, y)T ∈ R2 | z T W z = 3 (δR + δE ) a
with W given by

W =
ckR
λM (J)
Ω
− 2λck
m (J)
Ω
− 2λck
m (J)
kΩ − c


Chapter 5
Bounds on the sample time
The results from previous sections allow us to robustly stabilize the rigid body under
random perturbations and uncertainties in attitude and angular velocity measurements,
and we obtained a control law which is ready for implementation.
In this section we study the requirements for the discrete time implementation of said
control law. Two different paths can be followed to study the discretization. The whole
system can be discretized, obtaining an equivalent system with discrete time equations,
or we can analyze the closed loop dynamics of the continuous time model, but with the
feedback law updated at discrete time points and held constant between them. The
latter is followed in this section and follows the work of Mancilla-Aguilar et al. [31], [32]
and is referred to as “Static Feedback Law via Sampling and Zero-order Hold” or SZH
analysis.
The discretization of the whole system often takes the form of an approximation, and
bounds need to be placed to measure how good the approximation is and thus what
order of magnitude of errors to expect. In [33] such a procedure is described step by
step, and in [34] an extensive review of methods is presented.
The SZH analysis is a better model of reality because it makes almost no approximations, it just describes the fact that the dynamics of the system evolve in continuous
time but the control action is updated at discrete points in time. This is the case in
most modern control situations, where the controller is implemented using digital and
embedded systems, intertwining computers and microprocessors.
55
Chapter 3. Control
5.1
56
SZH Analysis
The following analysis takes as hypothesis that the control action is updated at fixed
intervals given by the sample rate (also called time step), and held constant during those
intervals. A finer analysis would need to take into account this step-like output and filter
it with a model of the step response of the actuators, which here are assumed to be ideal.
Although it is the the only approximation in this analysis, the difference between the
step-like and the actual response of the actuators can be included in the term δR in the
robust version of the controller, diluting the approximation meaning.
If we assume that the actuators are fast compared with the sample rate, and that the
bounds given in the previous sections are abided, the following analysis is a very good
approximation, and we are modeling with much finer detail than in the alternate whole
system discretization.
At first, we study the time bound for the unperturbed system, i.e. the robustization
term is not taken into account. Then, we will see what changes are required to also take
it into consideration.
In [32], Mancilla-Aguilar gives conditions for existence, maximality and uniqueness of
solutions for this kind of systems. The existence of a maximum time step is also proven
for the discrete update law. We rewrite the main results first, and then specialize them
for the system we are studying.
Given a system
ẋ(t) = f (x(t), u(t))
(5.1)
such that the origin is globally asymptotically stable when fed back with the control law
u(t) = γ (x(t))
(5.2)
by using the function 1k (t) defined as the characteristic function of the interval starting
at k and of length 1,

 1 if k ≤ t < (k + 1)
1k (t) =
 0 otherwise
(5.3)
we define the SZH implementation of the feedback law γ with sample time Ts as
γSZH,Ts (t) =
∞
X
k=0
γ (x(kTs )) 1k
t
Ts
(5.4)
Chapter 3. Control
57
In [32], proof is given that for a system like (5.1) for which a continuous time feedback
law γ(x) exists which renders the closed loop system locally or globally asymptotically
stable, there exists a maximum sampling period TsM AX which guarantees that the system
will remain stable when fed back with the SZH implementation of γ with Ts ≤ TsM AX . In
Figure 5.1, the relationship between γ and its SZH implementation γSZH,Ts is illustrated.
Figure 5.1: The SZH implementation does not try to discretize the whole system,
but allows the state to evolve in continuous time while updating the control action
at discrete time points. The blue continuous time system is rendered assymptotically
stable by the control law u = γ(x). Its SZH implementation γSZH,Ts uses the same
law as γ to compute the control action at every time step, but as the resulting control
action is not the same, the system evolves slightly differently while maintaining the
asymptotical convergence property (if some conditions hold) as shown in section 5.3.
5.2
Bound on the Maximum Sample Time
The result stated in the previous paragraph is subject to the initial conditions being
within a given distance from the origin, and the stability depends on this distance.
Furthermore, in Theorem 3.1 of [32], an explicit formula for the bound on the maximum
sampling period is given. We replay a part of the proof of said theorem to bring the
equation here.
Chapter 3. Control
58
We assume that there exists a proper Lyapunov function V for the system (5.1), such
that when fed back with γ it holds that V (x(t + Ts )) < V (x(t)) and the hypothesis of
the theorem hold. We will work in the level set Lξ = {x ∈ Rn | V(x) ≤ ξ}, and stability
will be reached for the level set L so that V (x(t + Ts )) < if V (x(t)) < .
Now, we define some auxiliary functions to obtain the bound on TsM AX .
Define P (r) as
P (r) = sup{η ∈ R : kyk ≤ η ∧ x = V −1 (r) ⇒ ∇V (f (x, γ(x)) + y) < 0}
(5.5)
so P (r) measures how far we may push the control away from the stabilizing action
before turning V̇ positive.
Define θ (x, x0 ) as
θ x, x0 = f x, γ(x0 ) − f (x, γ( x ))
(5.6)
so θ measures the deviation in the time derivative of the state when presented with a
control action computed at a state other than the present state.
From this, define a Lifschitz constant with respect to x − x0 , calling it L such that
kθ x, x0 k ≤ Lkx − x0 k
(5.7)
We may bound said deviation with L and the distance between the current state and
the state at which the control action is computed.
Finally, define G(r) as the maximum magnitude of the time derivative of the state when
fed back with the stabilizing action γ in the level set of V for all future states
G(r) = sup{kg(x)k
∀x ∈ Lr }
(5.8)
Then, the bound for the maximum sample time is given by
P (r)
TsM AX = sup δ | δeLδ ≤
LG(r)
∀0 < r < ξ 0
(5.9)
with R0 = max {V(x) | kxk ≤ R} and 0 such that L0 ⊂ {x | kxk < }. This restriction
actually implies that the set {x | kxk ≤ ξ} is included in Lξ0 , so equation (5.9) holds for
x in the range ≤ kxk ≤ ξ.
The result of Theorem 3.1 in [32] is that the system (5.1), when fed back with the
SZH implementation of the stabilizing feedback law γ with sample time Ts < TsM AX , if
started with initial conditions x0 , such that x0 ∈ Lξ , then the states evolving from x0
Chapter 3. Control
59
are defined for every discrete time step kTs , the state evaluated at these sample times
enter the level set L , and given any real number ν > 0, there exists a value of k0 such
that the state evaluated at these sample times enters the level set L+ν and remains
there for every time step kTs ≥ k0 Ts .
These results describe the evolution of the system at the time points described by the
time steps. Although no stronger results on stabilization can be obtained from here with
those assumptions alone, if the additional condition
lim inf
r→0
P (r)
>0
G(r)
(5.10)
is met, further stability results follow.
5.3
SZH Results for Asymptotically Stable Systems
In particular, if condition (5.10) holds, and if the origin is locally asymptotically stable
for the system (5.1), its SZH implementation given by
ẋ(t) = f (x(t), γSZH,Ts (t))
(5.11)
with Ts < TsM AX is locally asymptotically stable as proven by Theorem 4.1 in [32].
This is a strong theoretical result proving the intuitive fact that the discretization of
an asymptotically stable system remains asymptotically stable, if the sample time is
well chosen. Additionally, an important practical result is that it provides a bound for
choosing the sample time.
Equation (5.9) can be put in a more convenient form if the system (5.1) has local
exponential stability. If the Lyapunov function verifies the following conditions for constants c1 − c4 :
c1 kxk2 ≤ V(x) ≤c2 kxk2
(5.12a)
V̇(x) ≤ −c3 kxk2
(5.12b)
k∇V(x)k ≤c4 kxk
(5.12c)
and if we can find a Lifschitz constant T for the continuous closed loop state time
derivative defined by
kf (x, γ(x)) k ≤ T kxk
(5.13)
Chapter 3. Control
60
then P (r) defined in equation (5.5) verifies
√
c3 c1 √
P (r) ≥
r
c2 c4
(5.14)
and G(r) defined in equation (5.8) verifies
T √
G(r) ≤ √
r
c2
(5.15)
so a bound like (5.9) can be written as
Lδ
TsM AX = sup δ | δe
5.3.1
1 c3
≤
LT c4
r
c1
c2
(5.16)
Bound on the Sample Time for the Attitude Controller in SO(3)
Now, we will specialize these results for the system studied in the previous sections. We
recall that z was defined as z = (keR k
keΩ k)T . In this section we will also use an
augmented vector x with the errors put together, x = (eR
eΩ )T .
The error dynamics
ėR
ėΩ
!
=
!
E (R, Rd ) eΩ
−αd − J −1 Ω̂JΩ + J −1 M (eR , eΩ )
(5.17)
have the form
ẋ = f (x, γ(x))
(5.18)
with γ(x) given by
γ(x) =
0
!
J −1 M (eR , eΩ )
(5.19)
The errors may not take any random value but are limited by the bounds on the allowed
angular velocities from section 4.3, by the fact that keR k ≤ 1, and by
keΩ k ≤ kΩ − RT Rd Ωd k ≤ kΩk + kRT Rd Ωd k ≤ kΩk + kΩd k
(5.20)
xM AX = 1 + ΩM AX + ΩdM AX
(5.21)
so by calling
the restriction on the errors translates to the restriction on x as
kxk ≤ xM AX
(5.22)
Chapter 3. Control
61
so the bound on the sample time we will find in this section applies to the set LxM AX .
In what follows, we will obtain the constants L, T and c1 − c4 to plug in equation (5.16).
The constant T defined in equation (5.13) is found by bounding the closed loop error
time derivative
f (x, γ(x)) =
!
E (R, Rd ) eΩ
(5.23)
−kR J −1 eR − kΩ J −1 eΩ
leaving the bound in terms of the errors so
1
1
kf (x, γ(x)) k ≤ kR
keΩ k
keR k + 1 + kΩ
λm (J)
λm (J)
from which we can read T by noting that kxk =
(5.24)
p
keR k + keΩ k and writing
kf (x, γ(x)) k ≤ T kxk
so that we can evaluate T from
s
T =
kR
λm (J)
2
+ 1+
(5.25)
kΩ
λm (J)
2
(5.26)
The same comments as in section 3.4.9 about units apply.
Next, we look for L
0
0
f x, γ(x ) − f (x, γ(x)) =
!
J −1 (M (e0R , e0Ω ) − M (eR , eΩ ))
(5.27)
so
kf x, γ(x0 ) − f (x, γ(x)) k ≤
1
k − kR e0R − eR − kΩ e0Ω − eΩ k
λm (J)
(5.28)
which can be written as
1
1
kR ke0R − eR k +
kΩ ke0Ω − eΩ k
λm (J)
λm (J)
≤
or
1
≤
(kR ,
λm (J)
kΩ )
!
ke0R − eR k
ke0Ω − eΩ k
which can be written as
kf x, γ(x0 ) − f (x, γ(x)) k ≤ Lkx0 − xk
(5.29)
Chapter 3. Control
62
if L is given by
L=
1
λm (J)
q
2 + k2
kR
Ω
(5.30)
Next, we need the constants c1 − c4 . The constants c1 and c2 defined in equation (5.12a)
come from the bounds on the Lyapunov function. Since kxk = kzk, c1 can be obtained
from matrix (3.45) by looking at its smallest eigenvalue which is given by
c1 = λm (M1 ) =
kR λm (J)
+
−
2
2
q
4c2 + (kR − λm (J))2
(5.31)
2
and is guaranteed to be positive because of the choice of c in equation (3.49).
The constant c2 is found in the same way from M2 as
c2 = λM (M2 ) =
bkR λM (J)
+
+
2
2
q
4c2 + (bkR − λM (J))2
2
(5.32)
The constant c3 is obtained from matrix W as
r
c3 = λm (W ) =
ckR
kΩ − c
+
−
2λM (J)
2
ckR
λM
+ c − kΩ
2
+
ckΩ
λm (J)
2
2
(5.33)
At last, to find c4 we may write the gradient in terms of eR and eΩ as
∇eR V = ceTΩ
(5.34)
∇eΩ V = eTΩ J + ceTR
(5.35)
and
, so, in term of the errors, the gradient becomes
(∇eR V, ∇eΩ V) = ceTΩ , ceTR + eTΩ J
(5.36)
k∇Vk ≤ ckeR k + (c + λM (J)) keΩ k
(5.37)
and its norm bounded by
Coming back to our bound,
with c4 given by
k∇Vk ≤ c4 kxk
(5.38)
q
c4 = c2 + (c + λm (J))2
(5.39)
Chapter 3. Control
Constant
L
T
c1
c2
c3
c4
TsM AX
63
Evaluates to
q
1
2 + k2
kR
Ω
λ
(J)
m
r
2 2
kR
kΩ
+
1
+
λm (J)
λm (J)
q
4c2 + (kR − λm (J))2
kR λm (J)
+
−
2
2
2
q
2
4c + (bkR − λM (J))2
bkR λM (J)
+
+
2
2
2
r
2 2
ckR
ckΩ
+
c
−
k
+
Ω
λM
λm (J)
ckR
kΩ − c
+
−
2λM (J)
2
2
q
c2 + (c + λm (J))2
r 1 c3 c1
Lδ
sup δ | δe ≤
LT c4 c2
Table 5.1: Summary of the required constants to compute the bound on the maximum
sample time TsM AX , and the formula for the bound itself.
With these results, summarized in Table 5.1, we have all the constants to find the bound
for the maximum sample time using equation (5.16).
5.3.2
Bound on the Sample Time for the Robust Attitude Control Law
in SO(3)
In the preceding section, we left aside the robust control law obtained previously. Now
we will study how it is affected by the robustization term, and what modifications should
be carried out to include its effects in the bound for the sample time.
The asymptotic stability assumed for the bound in terms of the constants no longer
holds because of the ellipse of (4.37) lying at the center of the region under analysis.
We recall that inside the ellipse we cannot guarantee that the time derivative of the
Lyapunov function is negative.
But when standing outside, we can analyze the behavior pretending that everything is
fine inside the ellipse and knowing that it will only apply until the ellipse is reached.
Constants c1 and c2 and c4 are not affected by the robustization term since they only
depend on the Lyapunov function, and it was stated that the Lyapunov function for the
system is not affected by the control input, only its time derivative is.
Chapter 3. Control
64
Constant c3 is changed; now it no longer holds that it is bound by a purely quadratic
term because when the system enters the ellipse the bound no longer holds. But equation
(4.36) has the form
V̇(x) ≤ −akxk2 + b
(5.40)
and we can use another parabola which passes through the origin and intersects the
parabola of equation (5.40) at kxk = xmin and is always below the original parabola
from there on. This value of xmin must be chosen so that the values of x lie outside the
ellipse, in the region where V̇ < 0, so
s
xmin >
3a(δR + δE )
λm (W )
(5.41)
We call the new constant associated with this new parabola c03 and we obtained it from
equating both parabolas
−c03 x2min = −c3 x2min + b
and it takes the value
c03 = c3 −
3 (δR + δE ) a
x2min
(5.42)
For the constant L it is more difficult to obtain a closed value for the effect of the
robustization, but we can analyze a worst case scenario. To equation (5.28) we must
add the term pertaining to µ̄ as defined in equation (4.30) so it becomes
¯ 0 ) − f x, γ(x)
¯ k ≤
kf x, γ(x
1
k − kR e0R − eR − kΩ e0Ω − eΩ + µ̄0 − µ̄k (5.43)
λm (J)
with µ̄0 indicating that it is evaluated at the primed error vectors, while the unprimed
µ̄ is evaluated at the unprimed error vectors.
A worst case is when the vector ēA = ēR + cJ −1 ēΩ is collinear with ėΩ and with their
primed versions are also collinear at the same time, but with opposite directions. This
is not actually possible, but it serves the purpose of finding a worst case bound.
In such a situation L should be replaced by L0 which is obtained by subtracting the
robustization term from the non-robust control action.
Chapter 3. Control
65
kf (x, γ̄(x)) − f x, γ̄(x0 ) k ≤
0
1
k − kR e0R − eR − kΩ e0Ω − eΩ
λm (J)
!
−
(δR + δE ) e0R + cJ −1 e0Ω + ∆A
kēA k + a
!
+
(δR + δE ) eR + cJ −1 eΩ + ∆A
kēA k + a
(5.44)
k
kR
(δR + δE )
k e0R − eR k
+
λm (J)
aλm (J)
kΩ
(δR + δE )
+
k e0Ω − eΩ k
+
2
λm (J)
aλm (J)
kf (x, γ̄(x)) − f x, γ̄(x ) k ≤
(5.45)
We have assumed that the perturbation is the same at x and at x0 , as it is an instantaneous value, so ∆A = ē0A − e0a = ēA − eA . If this assumption is not made, the difference
in the time derivative of the state due to the control being evaluated at different states
x0 and x would not be Lifschitz in the difference x0 −x and there would not be a Lifschitz
constant L. We can define L0 as
s
(δR + δE ) 2
(δR + δE ) 2
1
0
kR +
+ kΩ +
L =
λm (J)
a
aλm (J)
(5.46)
so
kf (x, γ̄(x)) − f x, γ̄(x0 ) k ≤ L0 kx0 − xk
(5.47)
The constant T is found in a similar way as L to take a new value for the robust control,
T 0 , given by
1
T0 =
λm (J)
s
(δR + δE )
kR +
a
2
(δR + δE )
+ λm (J) + kΩ +
aλm (J)
2
(5.48)
The results from the preceding section hold only for xmin ≤ kxk < xM AX and with c3 ,
T and L replaced c03 , L0 and T 0 respectively. If xmin is chosen too close to
1
λm (W ) ,
it will
yield too small a bound for the time step. And the values for L0 and T 0 will also further
reduce the bound on the time step. Additionally, the structure of the stabilizing term
µ itself makes it extremely conservative to bound it by its Lifschitz constant since, as
seen in Figure 5.2, when keA k > a it tends to a constant value.
Chapter 3. Control
66
Figure 5.2: The bound by a Lifschitz constant on the robustization term µ makes the
resulting bound extremely conservative, as the actual function being bounded tends to
a constant value.
We remark that the bound is only a sufficient condition for the system to be stable, but
it is not required. It can be viewed as a conservative bound. In other words, values of
the time step larger than the bound will probably still render the system stable, just
that we cannot guarantee it from these equations. Stability inside kxk ≤ xmin also is
probably achieved for the time steps provided by this bound, and larger ones as well. In
Table 5.2 we summarize the results of this section.
Constant
L0
T0
c03
Evaluates to
1
λm (J)
s
s
(δR + δE )
kR +
a
2
(δR + δE )
+ kΩ +
aλm (J)
2
(δR + δE ) 2
(δR + δE ) 2
+ λm (J) + kΩ +
kR +
a
aλm (J)
r
2 2
ckR
ckΩ
+ λm
λM + c − k Ω
(J)
kΩ − c
3 (δR + δE ) a
ckR
+
−
−
2λM (J)
2
2
x2min
1
λm (J)
Table 5.2: Summary of the modifications on the constants of Table 5.1 for the time
bound for the robust version of the control law.
Chapter 6
Controller Design
In previous sections, we found control laws that track an arbitrary attitude command
subject to some restrictions on its maximum angular speed and the magnitude of the
perturbations it is subject to, arising from actual perturbations on the torque experienced by the system or due to uncertainties in the measurements of the attitude and
the angular velocity.
The resulting controller depends on 3 parameters, namely kR , kΩ and c. Note that c
must be chosen such that it obeys restriction (3.49). These act as the gain of the control
law, and are subject to all the trade-offs associated with controller gains. In particular,
for a given system and given bounds on the perturbations, the bound on the sample
time depends only on these gains, and the minimum attainable error also depends only
on these gains.
We will regard them as functions of the gains and obtain best values for the gains in
terms of the desired performance.
We start by defining the minimum attainable error as some kind of measure of the ellipse
of equation (4.37). We recall that that equation is a bound on the minimum error the
system can attain and we cannot guarantee that the tracking error will take smaller
values than this bound. Taking the bounds on the perturbations as fixed, this ellipse
depends only on the gains. So we define a map κ : R3 → R
κ (kR , kΩ , c) = µ (ε (kR , kΩ , c))
(6.1)
where µ implies some measurement function that allows us to compare ellipses and order
them under some criteria. One such function may be µ(ε) = sM its major semi axis
length, or µ(ε) = A(ε) its area.
67
Chapter 3. Control
68
We will also define the sample time associated with a choice of gains, τ : R3 → R as the
bound from previous section
τ (kR , kΩ , c) = TsM AX
associated
with
kR , kΩ , c
(6.2)
We also define the best choice of the gains as the one which simultaneously minimizes
the error and maximizes the sample time.
The set of all the combinations of (kR , kΩ , c) that give sample times larger than a given
value t will be called Θ(t)
Θ(t) = (ζ, ξ, ς) ∈ R3 | τ (ζ, ξ, ς) ≥ t
(6.3)
and the set of all the combinations of (kR , kΩ , c) that give minimum attainable errors
smaller than e will be called Ξ(e)
Ξ(e) = (ζ, ξ, ς) ∈ R3 | κ (ζ, ξ, ς) ≤ e
(6.4)
This sets Θ and Ξ need not be defined for every value of (kR (e), kΩ (e), c(e)), the first
restriction on the gains being to the values where these two sets are defined.
For every pair (t, e) the values of (kR , kΩ , c) ∈ Θ(t) ∩ Ξ(e) will give implementations of
the controller with sample time larger or equal than t and minimum attainable error
smaller than or equal to e. To check if a given implementation satisfies requirements
on sample time and minimum attainable error it suffices to check if it is included in the
intersection.
But this relationship can be further explored if some conditions hold. We can find a
bound on the sample time in terms of the minimum attainable error alone, and give the
values of the gains (kR , kΩ , c) that will induce such performance. It can also be used the
other way round, to obtain the error and the gains from the bound on the sample time,
although we think this is less useful for practical controller design.
Therefore, we will parametrize the time in terms of the error e as
t(e) = max {τ ∈ R | Θ(τ) ∩ Ξ(e) 6= ∅}
(6.5)
and the best gains for this error will be given by choosing a triple (kR (e), kΩ (e), c(e))
such that
(kR (e), kΩ (e), c(e)) ∈ Θ (t(e)) ∩ Ξ(e)
(6.6)
Chapter 3. Control
69
If there is a single element in the intersection we can obtain unique values for the gains
and the bound on the sample time. This will be the best controller we can obtain using
the analysis of the present work.
In this section, we found a way to choose the gains for the controller such that they
verify error constraints and we have also seen that this choice of gains determines the
bound on the maximum sample time that renders the discrete implementation of the
system stable. In the next chapter, we will present some examples and numerical results
for these examples.
Chapter 7
Results and Discussion
In this chapter, we present some examples and numerical results for these examples. We
start by presenting the model of the UAV used for the numerical examples, we study
the controller design with the tradeoffs of selecting the gains, and we then use these
results to simulate the UAV and its performance under different flight modes. Finally,
we contrast the results.
7.1
UAV model
As stated in section 2.2.5, the main property determining the rotational dynamics of a
rigid body is its moment of inertia. It is represented by a positive definite matrix and we
only need to know its minimum and maximum eigenvalues to implement the controller.
For the UAV under analysis, we chose the values λm (J) = 0.015 and λM (J) = 0.025,
which represent a typical medium sized quadrotor such as [35]. All physical values will
be given in SI units, so the moment of inertia is in kg · m2 .
The measurement accuracy for attitude and angular velocity will be given by the typical
performance of navigation algorithms. We will assume that the maximum errors the
navigation system provides will be values κM AX = 0.7◦ for attitude and χM AX = 0.3◦ /s
for angular velocity. It is important to note that, although when coming from navigation
algorithms, these values have a probabilistic character meaning 2σ deviation, i.e. that
(usually) 95% of the estimated values are within that range of the true value, we are
taking these as an absolute bound.
To better match the results from navigation algorithms, if a 6σ bound could be provided,
it would more faithfully fit the bounds used in the robustization than the usual 2σ
bounds. Probabilistic analyses of the robustization are not easy to perform because of
70
Chapter 4. Results and Discussion
71
the involved formulas and the difficulty of Montecarlo simulations with so many free
parameters.
The bound on aerodynamic and unmodeled perturbation torque was set to an order of
magnitude smaller than the typical δE value found for this system to better understand
the results regarding the perturbations due to measurement inaccuracies and the effect
of sample time on stability instead of focusing on simple unstructured perturbations.
We summarize these values in Table 7.1.
Parameter
λm (J)
λm (J)
δR
κM AX
χM AX
Description
Minimum eigenvalue of inertia tensor
Maximum eigenvalue of inertia tensor
Bound on unmodeled perturbations
Bound on attitude error
Bound on angular velocity error
Value
0.015
0.025
δE /10
0.7
0.3
Unit
kg · m2
kg · m2
Nm
◦
◦ /s
Table 7.1: Summary of the parameters used for numerical results.
7.2
Choice of Error Measurement Function
For all the results in this section, we used as an error measurement function, equation
(6.1), half the diagonal of the rectangle bounding the ellipse, as shown in Figure 7.1,
which is equivalent to choosing k1 = 1 and k2 = 1s in the error measurement function
q
e = µ(ε) = k1 2x + k2 2y
(7.1)
where
x = arcsin(max({x | (x, y) ∈ ε}))
(7.2)
y = max({y | (x, y) ∈ ε})
are the extremal horizontal and vertical points of the ellipse respectively. Note that x
is passed through an arcsine to recover the angle, as keR k is unitless or has units of sine
of angle. The error e has units of angle. This choice puts attitude error and angular
velocity error on equal footing.
Other choices would result in other performance results according to these choices. For
example, for some applications the angular velocity must be precisely controlled and the
attitude may not be so important; the error function may then be chosen to give more
weight to the angular velocity error.
Chapter 4. Results and Discussion
72
Figure 7.1: Definition of the error measurement function e.
7.2.1
A note on the bound on the sample time
The bound on the sample time obtained using the bounds for the robust attitude controller described in section 5.3.2 yield very small values. This is due to the fact that the
robustization term has almost unit magnitude far from the ellipse, and is being bounded
by its behavior near the ellipse with a linear term. This results in the bound being
fair for small values of attitude error and angular velocity error, but very restrictive for
larger values.
Abiding to the note at the end of that section, we use the bound on the sample time for
the non-robust controller as an approximation. We think it is a very good approximation,
as the magnitude of the error is relatively large, and it is not that much important if it
is not so good an approximation for smaller values of the error because they are already
small.
7.3
Effect of the gains on the error and sample time
Before studying the relationship between minimum attainable error and maximum step
time, we will study how the error varies with respect to the choice of the gains by means
of a numerical example. We will use the bounds on angular velocities according to the
“acrobatic” flight mode, Table 7.3.
The minimum attainable error and maximum sample time depend on the three gains
(kR , kΩ , c), but to gain an insight into how it is affected by these gains we will limit our
study to a plane of constant gain c, spanned by kR and kΩ .
Chapter 4. Results and Discussion
73
In Figure 7.2, we overlay a contour plot of the error with the level curves of the sample
time. P0 represents the best choice of gains. Ellipses for the marked points are shown
in Figure 7.3, where we can see the tradeoff being made. If we try to increase the time
step (from 10ms to 12ms in the figure), the resulting error also increases. If we try to
lower the error, moving towards P2 , we are requiring a smaller time step.
Additionally, the tradeoff in error is clearly seen in Figure 7.3, where a higher value of
kΩ , like the value of P1 , yields less error in angular velocity with a correlated increase
in attitude error, as the ellipse is more oblong and closer to the keR k axis.
On the other hand, a higher value of kR , like for P2 would result in smaller attitude error
at the expense of larger angular velocity error. The ellipse for the best choice of gains,
corresponding to P0 , is a compromise between these situations. By choosing different
values of k1 and k2 in the error measurement function, equation (7.1), we can alter the
compromise solution if needed.
Error and Max Time Step
slice of c=0.002
−3
x 10
°
.0
18
16
.0°
0°
14.
16
°
12.0
14
°
10.0
12
s
ms
6.0m
6.0°
4.0
6
8.0ms
8
8.0°
10.0ms
kΩ
P1
12.0ms
14.0ms
10
5.0°
2.
0m
s
P0
4
P2
2
0.01
0.02
0.03
0.04
0.05
kR
0.06
0.07
0.08
0.09
Figure 7.2: A plane of constant c with the overlay of minimum attainable error,
labeled in degrees (color) and maximum sample time labeled in ms (level curves).
Points labeled Pi are detailed in Figure 7.3. P0 corresponds to the best choice of gains.
0.1
Chapter 4. Results and Discussion
74
Error Ellipses for selected points
6
4
|| eΩ || (deg/s)
2
0
−2
−4
P0: ||eR||<4.9°, ||eΩ||<2.6°
P : ||e ||<7.5°, ||e ||<1.4°
1
R
Ω
P2: ||eR||<4.3°, ||eΩ||<6.6°
−6
−8
−6
−4
−2
0
|| eR || (deg)
2
4
6
8
Figure 7.3: Error ellipses corresponding to points selected in Figure 7.2. The red rectangle in the center represents the bound on the attitude measurement error κM AX and
on the angular velocity measurement error χM AX . Ellipse marked as P0 corresponds
to the best choice of gains. The other two show the tradeoff between attitude error and
angular velocity error. It should be recalled that only the first quadrant of the plot
makes sense, the other quadrants are left for clarity, and that this is not a knowledge
ellipse, the errors will converge to these ellipses, but the behavior inside is unknown.
7.4
Angular velocity and acceleration bounds derived from
different flight modes
The constraints on the angular velocity and its command will be defined by the type
of flight we are subjecting our UAV to. The time derivative of the angular velocity will
be limited by the thrust of the rotors and the inertia tensor of the UAV, and it is of
common sense that the command on this angular velocity does not exceed this value. If
we call FM AX the maximum thrust a rotor may exert, the maximum torque MM AX it
will generate will be
MM AX = k · l · FM AX
where k is a constant depending on the geometry of the UAV, and l is the average arm
length at which the rotors are placed with respect to the center of mass. For example,
Chapter 4. Results and Discussion
75
for a quadrotor UAV the maximum torque is exerted when two adjacent rotors are at
√
full throttle while the other two are stopped. In that case k = 2.
Then, by Newton’s law
M = J Ω̇
so Ω̇M AX is bounded as
Ω̇M AX ≤
k · l · FM AX
λM (J)
(7.3)
where we are bounding with the maximum eigenvalue of J to allow for the fact that we
want to be sure that no matter what attitude the UAV has, the angular acceleration
should not be greater than Ω̇M AX .
For the UAV under study, we can estimate FM AX = 5N and l = 0.25m so we obtain
approximately Ω̇M AX ≤ 3000◦ /s2 . But for the commanded angular velocity, we need
only reflect our command, not the physical limit. As the obtained performance depends
on the bounds on angular velocity, its command and the time derivative of its command,
we can see that for different flight modes, i.e. trajectories with different bounds, different
bounds on the performance are obtained. We will study a “super acrobatic” flight mode,
with higher values of ΩM AX , ΩdM AX and Ω̇dM AX , an “acrobatic” mode with smaller
values and a “stable” mode with conservative values.
7.4.1
“Super acrobatic” flight mode
In “super acrobatic” flight mode, we choose Ω̇dM AX =
Ω̇M AX
.
2
In this mode we want the
UAV to perform a full flip starting at a hovering condition and ending at a hovering
condition in the shortest time possible. From a simple back of the envelope calculation,
we can find that the maximum angular velocity required to accomplish this maneuver
in minimum time is half the product of Ω̇M AX and the desired time. We can perform
such a flip in 1s if we choose ΩM AX = 750, ΩdM AX = 750 and Ω̇dM AX = 1500. The time
to perform a flip assumes that the limit on the maximum angular velocity is due only
to the maximum value it reaches during the acceleration phase and is given by
s
tf lip =
4 · 360◦
Ω̇dM AX
(7.4)
Gains and sample time vs error are plotted in Figure 7.4. Values used for this mode are
summarized in Table 7.2.
Chapter 4. Results and Discussion
76
Error vs Max Time Step
mode: super acrobatic
0.2
30
kR
kΩ
0.1833
27.5
c2
0.1667
25
22.5
0.1333
20
0.1167
17.5
0.1
15
0.0833
12.5
0.0667
10
0.05
7.5
0.0333
Max Time Step τ (ms)
Gains kR, kΩ, c2
0.15
5
0.0167
2.5
Max step time τ (ms)
0
0
7.5
15
22.5
30
error (deg)
37.5
45
0
60
52.5
Figure 7.4: Gains and Max Sample Time for the “super acrobatic” flight mode. The
ripples are due to the grid of the search algorithm used and should be smoothed with
a finer pitched grid.
Parameter
ΩM AX
ΩdM AX
Ω̇dM AX
Description
Maximum allowed angular velocity
Maximum commanded angular velocity
Maximum commanded angular acceleration
Value
750
750
1500
Unit
◦ /s
◦ /s
◦ /s2
Table 7.2: Summary of the parameters used for “super acrobatic” flight mode.
7.4.2
“Acrobatic” flight mode
In “acrobatic” flight mode, we choose more moderate values in exchange for extended
times needed to perform the maneuvers. The minimum time to perform the same flip
under the restriction on the maximum angular velocity is obtained by letting the angular
velocity increase at the rate given by Ω̇dM AX = 180 until it reaches ΩdM AX , letting the
angular velocity fixed for a while and then decreasing it at the same rate, and is given
by
tf lip =
Ωd
360◦
+ M AX
ΩdM AX
Ω̇dM AX
(7.5)
The time required to perform the flip restricted to ΩM AX = 120, ΩdM AX = 120 and
Ω̇dM AX = 180 is now extended to approximately 3.5s. Gains and sample time vs error
are plotted in Figure 7.5. Values used for this mode are summarized in Table 7.3.
Chapter 4. Results and Discussion
77
Error vs Max Time Step
mode: acrobatic
0.2
30
kR
kΩ
0.1833
27.5
c2
0.1667
25
22.5
0.1333
20
0.1167
17.5
0.1
15
0.0833
12.5
0.0667
10
0.05
7.5
0.0333
Max Time Step τ (ms)
Gains kR, kΩ, c2
0.15
5
0.0167
2.5
Max step time τ (ms)
0
0
2.5
5
7.5
10
error (deg)
12.5
15
0
20
17.5
Figure 7.5: Gains and Max Sample Time for the “acrobatic” flight mode. Note the
different horizontal scale with respect to the previous plot.
Parameter
ΩM AX
ΩdM AX
Ω̇dM AX
Description
Maximum allowed angular velocity
Maximum commanded angular velocity
Maximum commanded angular acceleration
Value
120
120
180
Unit
◦ /s
◦ /s
◦ /s2
Table 7.3: Summary of the parameters used for “acrobatic” flight mode.
7.4.3
“Stable” flight mode
In “stable” mode, we may be using the attitude to produce horizontal movements while
hovering, but we are not so much interested in doing flips. We chose ΩM AX = 15,
ΩdM AX = 15 and Ω̇dM AX = 60 for this mode. Note that provided the UAV is high
enough above the ground it will still be able to perform the flip, the only problem is it
will take much longer to complete (almost 25s with these values, equivalent to 3000m
in free fall!)
Parameter
ΩM AX
ΩdM AX
Ω̇dM AX
Description
Maximum allowed angular velocity
Maximum commanded angular velocity
Maximum commanded angular acceleration
Value
15
15
60
Unit
◦ /s
◦ /s
◦ /s2
Table 7.4: Summary of the parameters used for “stable” flight mode.
Chapter 4. Results and Discussion
78
Error vs Max Time Step
mode: stable
0.2
30
kR
kΩ
0.1833
27.5
c2
0.1667
25
22.5
0.1333
20
0.1167
17.5
0.1
15
0.0833
12.5
0.0667
10
0.05
7.5
0.0333
Max Time Step τ (ms)
Gains kR, kΩ, c2
0.15
5
0.0167
2.5
Max step time τ (ms)
0
0
1
2
3
4
error (deg)
5
6
7
8
0
Figure 7.6: Gains and Max Time Step for the “stable” flight mode.
7.4.4
Comparison between different flight modes
In Figure 7.7, we analyze the effect of the angular velocity bounds on the error vs. time
performance.
Chapter 4. Results and Discussion
79
Max Time Step vs Error for different flight modes
30
Max Time Step (ms)
25
20
15
10
5
0
stable
acro
s. acro
0
10
20
30
error (deg)
40
50
60
Figure 7.7: Comparison between the maximum time step vs. error for different flight
modes. In red the “stable” mode, in green the “acrobatic” mode, in blue the “super
acrobatic” mode.
7.5
Simulation Setup
To simulate the system under different conditions a standard control simulation setup
was used, depicted in Figure 7.8. The blocks are as follows: (a) is the reference used for
the simulation. We can use fixed trajectories as a source for the commanded attitude
and angular velocities or let the reference be given by the joystick input, a simple switch
allows us to choose the source. The controller block (b) implements the robust controller
developed in this work. It takes as input the estimated or measured angular velocity
and the attitude matrix, and outputs the 4 forces for each of the motors. Block (c)
implements the model of the quadrotor itself, including the 6DOF integration. No
aerodynamic effects are taken into account, although the function calls to do so are
there. It outputs its state and auxiliary variables to evaluate its performance. Block
(d) implements the navigation. For this work, it just reads the state provided by the
quadrotor and adds random noise to account for the perturbations in the measurement
process. It outputs noisy attitude and angular velocity measurements. For future work
this may be replaced by Kalman filters or any other estimation algorithm. Block (e)
serves as the bridge to FlightGear flight simulator, used for visualization purposes only.
Chapter 4. Results and Discussion
80
The controller in block (b) also implements a PID altitude control which can be switched
off and the 6DOF integrator (c) can also suppress position integration to focus only on
the attitude control problem. Altitude control comes in handy for visualization and to
get a real feel of the craft’s performance, but the simulator is not designed to test this
feature.
?
modelo2_diagrama_NF
a
c
Command
[roll, pitch, yaw, height]
b
Command
a
command
Switch
Axes
joyinput
Buttons
Point of View
Joystick Input
state
position
attitude
Control [f1 f2 f3 f4]
control=[f1 f2 f3 f4]
aux1
aux2
Omega
state
errors
R
Quadrotor Controller
Quadrotor
e
position
attitude
FlightGear
(Visualization)
simout2
To Workspace1
simout1
To Workspace
d
R_est
Omega_est
state
Quadrotor Navigation
Figure 7.8: A standard control simulation setup was used. The blocks (a)-(e) are
described in section 7.5.
Figure 7.9: Visualization of the quadrotor being simulated, using FlightGear. Rotors
are depicted as solid colored discs to improve visibility.
Chapter 4. Results and Discussion
7.6
81
Simulations
Next, we will see how the controller performs in simulations. We will firstly study how
it stabilizes a random initial condition to a nominal attitude. Then, we will see its
performance tracking a flip described in the preceding section.
7.6.1
Stabilization
The first analysis will be carried out in “stable” flight mode (as described previously),
and the objective is to see how the robust controller stabilizes from an initial condition
towards nominal attitude.
To model the effect of the sample time on the convergence properties of the systems,
the simulation was run with a time step of 0.5ms using a variable step Runge-Kutta
integrator. This small time step was chosen to represent the continuous time solution
and will be used as a reference to compare larger time steps against. The controller
was configured as a SZH implementation of the continuous time robust controller from
section 4.5, and the controller time step was parametrically increased during different
simulation runs.
The simulation was started with the craft in a “perturbed” state consisting of Euler
angles (0◦ , 0◦ , 45◦ ) (similar to the attitude seen in Figure 7.9) and 20◦ /s pitching angular
velocity. The commanded attitude is the nominal attitude, i.e. Rd = I.
For time steps Ts = 1ms, Ts = 10ms,Ts = 20ms and Ts = 30ms the gains for each
run were selected according to the curves of Figure 7.6. For larger time steps, the
values of the gains were left at the values obtained for Ts = 30ms as the curve goes no
further. But several trial runs were made with different gains, and convergence results
only worsened. The convergence results are shown as an overview in Figure 7.10, with
the ellipses of error also depicted. Some special cases are illustrated in more detail in
subsequent figures.
The error bounds found theoretically in previous section proved to be very conservative,
as much better behavior was observed than predicted. The main reason behind this is
the fact that the commanded attitude was static but the error ellipses correspond to
non-zero bounds on angular velocity and acceleration.
Another influence may be the way the random perturbations were chosen. The bounds
apply to any perturbation that abides by them. But for these simulations, zero-mean
random variables were used as perturbations, so it may be the case that a perturbation
in the form of a constant bias gives different results.
Chapter 4. Results and Discussion
82
Stabilization, Ts=1ms
25
25
25
20
20
20
15
15
15
10
|| eΩ || (deg/s)
30
10
5
0
0
0
−5
−5
−5
−5
0
5
10
15
20
|| eR || (deg)
25
30
35
40
−10
−10
45
5
−5
0
5
10
(a)
25
30
35
40
−10
−10
45
20
20
20
15
15
15
10
|| eΩ || (deg/s)
25
10
5
0
0
0
−5
−5
−5
5
10
15
20
|| eR || (deg)
25
30
35
40
−10
−10
45
−5
0
5
10
15
20
|| eR || (deg)
25
30
35
40
−10
−10
45
Stabilization, Ts=75ms
25
20
20
15
15
15
|| eΩ || (deg/s)
25
20
|| eΩ || (deg/s)
25
10
5
0
0
0
−5
−5
−5
15
20
|| eR || (deg)
25
30
35
40
−10
−10
45
−5
0
5
10
15
20
|| eR || (deg)
25
30
35
40
−10
−10
45
Stabilization, Ts=200ms
20
15
15
15
|| eΩ || (deg/s)
25
20
|| eΩ || (deg/s)
25
20
10
5
0
0
0
−5
−5
−5
15
20
|| eR || (deg)
(j)
25
30
35
40
45
−10
−10
15
20
|| eR || (deg)
25
30
35
40
45
0
5
10
15
20
|| eR || (deg)
25
30
35
40
45
30
35
40
45
10
5
10
10
Stabilization, Ts=1000ms
25
5
−5
Stabilization, Ts=500ms
30
0
45
(i)
30
−5
5
(h)
30
−10
−10
40
5
(g)
10
35
10
5
10
30
Stabilization, Ts=150ms
30
5
0
Stabilization, Ts=100ms
30
0
25
(f)
30
−5
−5
(e)
10
15
20
|| eR || (deg)
5
(d)
−10
−10
10
10
5
0
5
Stabilization, Ts=50ms
30
25
−5
0
Stabilization, Ts=30ms
30
25
−10
−10
−5
(c)
30
|| eΩ || (deg/s)
|| eΩ || (deg/s)
15
20
|| eR || (deg)
(b)
Stabilization, Ts=20ms
|| eΩ || (deg/s)
10
5
−10
−10
|| eΩ || (deg/s)
Stabilization, Ts=10ms
30
|| eΩ || (deg/s)
|| eΩ || (deg/s)
Stabilization, Ts=0.5ms
30
5
−5
0
5
10
15
20
|| eR || (deg)
(k)
25
30
35
40
45
−10
−10
−5
0
5
10
15
20
|| eR || (deg)
25
(l)
Figure 7.10: Simulation results for stabilization. Sample times: (a) 0.5ms, (b) 1ms,
(c) 10ms, (d) 20ms, (e) 30ms, (f) 50ms, (g) 75ms, (h) 100ms, (i) 150ms, (j) 200ms,
(k) 500ms, (l) 1000ms. Ellipses correspond to the error bounds for 1ms, 10ms, 20ms
and 30ms.
Chapter 4. Results and Discussion
83
Stabilization, Ts=100ms
30
25
20
|| eΩ || (deg/s)
15
10
5
0
−5
−10
−10
−5
0
5
10
15
20
|| eR || (deg)
25
30
35
40
45
(a)
Stabilization, Ts=100ms, random gains
30
25
20
|| eΩ || (deg/s)
15
10
5
0
−5
−10
−10
−5
0
5
10
15
20
|| eR || (deg)
25
30
35
40
45
(b)
Figure 7.11: For sample time Ts = 100ms, the choice of best gains still renders
the system stable, while a random or sub-optimal choice of gains shows a degraded
behavior.
In Figure 7.11 (a), the results for Ts = 100ms are plotted again in a more detailed
view. In Figure 7.11 (b) the same sample time for the controller is used but the gains
were chosen in a sub-optimal way, showing degraded performance. Even after trying to
improve the performance adjusting the gains by trial and error, no better results than
those of Figure 7.11 (a) could be obtained.
Chapter 4. Results and Discussion
7.6.2
84
Trajectory Tracking
In this section, we present some simulations that reflect the trajectory tracking performance of the proposed controller. As this controller is of the almost global kind, it is
suitable for acrobatic maneuvers on the whole space of rotation matrices. In particular,
we will show the capability of the controller to perform flips as described in section 7.4.
super acrobatic mode, Ts=20ms
390
360
330
300
270
pitch (deg)
240
210
180
150
120
90
60
30
0
0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
t (s)
Figure 7.12: Trajectory command (black) and actual trajectory (red) for flip and
backflip maneuver. Details highlited in green are presented in figures 7.13 and 7.14
Gains were selected from Figure 7.4 according to the 20ms error bound. The controller
step time was set at 20ms also. A trajectory demanding a flip and a backflip at a maximum acceleration of 1500◦ /s2 was passed as reference. The UAV was subject to the
same perturbations as before, but the initial condition was very close to nominal attitude. The commanded trajectory as well as the actual trajectory followed by the UAV
are presented in Figure 7.12, with selected areas zoomed in figures 7.13 and 7.14. Visualizations of the simulation are made available online (http://youtu.be/WUM7wxO1uRY
and http://youtu.be/YUNBwQVy1Bw).
Errors obtained are presented in Figure 7.15. They were within the bounds given by
the choice of the gains during the whole simulation. The largest error values were found
during the steep uphill and downhill sections of the curve. During those intervals the
angular acceleration was increasing the angular velocity of the craft up to the 180◦ mark,
and then decreasing it until the resting position was reached, both at the maximum rate
of 1500◦ /s2 . The maximum error is attained also during these periods.
Chapter 4. Results and Discussion
85
super acrobatic mode, Ts=20ms
180
175
pitch (deg)
170
165
160
155
150
3.45
3.455
3.46
3.465
3.47
3.475
t (s)
3.48
3.485
3.49
3.495
3.5
Figure 7.13: Trajectory command (black) and actual trajectory (red) for a flip and
backflip maneuver. During the steep angular acceleration phase, the tracking error is
at its largest.
Finally, the control command is shown in Figure 7.16. This is the force commanded to
the rotors with the common bias to all four rotors removed (this bias is used to obtain
vertical force to counteract gravity and to achieve altitude control, but has not been part
of this study so it has been removed for clarity). As the forces are exerted on opposing
sides of the craft, the net torque applied is proportional to the difference in the forces.
The time range plotted corresponds to the first flip. For the first section of the plot,
the force imbalance causes the torque to increase the angular velocity. For the second
section of the plot, the force imbalance is reversed to reduce angular velocity. The effect
of the controller step time is clearly seen in the plot, as the forces are piecewise constant
between control updates.
Chapter 4. Results and Discussion
86
super acrobatic mode, T =20ms
s
375
370
365
pitch (deg)
360
355
350
345
340
335
330
3.8
4
4.2
4.4
4.6
4.8
t (s)
Figure 7.14: Trajectory command (black) and actual trajectory (red) for a flip and
backflip maneuver. After finishing the flip the error is still small, in part due to the
shape of the angular acceleration command over time.
super acrobatic mode, Ts=20ms
14
12
error (deg)
10
8
6
4
2
0
0
5
10
15
t (s)
Figure 7.15: Errors are at its largest during the steep angular acceleration phases,
they remain moderately small afterwards.
Chapter 4. Results and Discussion
87
Force commanded to rotors 1 and 3
1.25
1
0.75
0.5
Force (N)
0.25
0
−0.25
−0.5
−0.75
ref
F1
−1
−1.25
2.5
F3
2.6
2.7
2.8
2.9
3
3.1
3.2
3.3
3.4
t (s)
3.5
3.6
3.7
3.8
3.9
4
4.1
4.2
4.3
Figure 7.16: Force commanded to opposing rotors to perform the flip. The time
range corresponds to the flip maenuver starting at t = 3s and ending at approximately
t = 4s, shown in black for reference and not to scale. The other pair of rotors has been
removed for clarity. The bias common to the 4 motors to achieve altitude control has
also been removed. The step-like shape of the curves is due to the controller being a
SZH implementation of a continuous controller.
Chapter 8
Conclusion
8.1
Conclusions
A geometric, coordinate-free, almost global controller for the attitude control problem
was studied. This controller was made robust against random bounded perturbations
and also against perturbations induced by noisy or faulty attitude and angular velocity
measurements or estimates.
Bounds on the sample time for the digital implementation of the non-robust version as
well as for the robust version of the controller were found. These bounds depend on the
gains of the controller and on the inertia moment of the rigid body (and on the bounds
used for the robustization in the robust version of the controller).
As both the bounds found for the robust controller and the bound on the sample time
depend on the gains, a relationship between bounds in perturbations and bounds on
sample time can be found. From this relationship, gains can be obtained that minimize
the error while maximizing the sample time, thus giving a practical way to choose the
gains, i.e. design the controller.
Simulations show that the bounds on the sample time are very conservative; nonetheless
they are a good tool to design the controller, as gains chosen using other criteria perform
worse for the same controller sample time.
Although the simulations were carried out using a quadrotor UAV model, the results
are valid for the general rigid body attitude control problem.
From a systems engineering point of view, these results allow for higher level requirements in attitude and angular velocity to be translated into lower level requirements
88
Chapter 6. Conclusion
89
on the digital controller and the navigation subsystem as timing and error performance
requirements respectively.
8.2
Future Work
While developing the robust controller, the errors induced by the measurements were
propagated to the error in attitude and angular velocity in the form of a bound used for
the fixed-σ robustization. The internal structure of this error propagation vector can be
used to improve the robust properties of the controller, instead of just using a bound.
In the design of the controller only the exponential convergence properties were taken
into account while performing the Lyapunov analysis. By also taking into account the
effects of the different parameters of the controller in the bound on the sample time, the
controller may be constructed from the ground up to obtain good sample time bounds.
This can be done in the geometrical framework provided by the SO(3) structure and its
so(3) algebra.
The different flight modes yield different bounds on the attitude and angular velocities,
which in turn end up giving different choices of the gains for the controller. By adjusting
these gains online according to the trajectory to be tracked, tighter bounds can be found
and better gains can be chosen. But switching gains on the fly may lead to instabilities
caused by step responses. An analysis of the effects of changing the values of the gains
on the fly may prove useful to reduce the attitude error and angular velocity errors
achieved by the controller. Such an analysis may be carried out in a switched gains
framework or the gains may be included in the Lyapunov analysis.
The same applies to the bounds on the errors provided by the measurement or estimation
algorithms. It is common for navigation algorithms to provide an online estimate of its
performance. If the algorithm can do so, this can be used to further reduce the errors
on attitude and angular velocity the controller may yield.
Bibliography
[1] R.R. Murphy. Human-robot interaction in the wild: Land, marine, and aerial
robots at fukushima and sendai. In RO-MAN, 2011 IEEE, pages xx–xx, 2011. doi:
10.1109/ROMAN.2011.6005214.
[2] M. Goodrich L. Lin, M. Roscheck and B. Morse. Supporting wilderness search and
rescue with integrated intelligence: autonomy and information at the right time
and the right place. In Proc. 24th AAAI Conf. Artificial Intelligence, Atlanta, GA,
2010.
[3] K. Schmid, H. Hirschmuller, A. Domel, I. Grixa, M. Suppa, and G. Hirzinger. View
planning for multi-view stereo 3d reconstruction using an autonomous multicopter.
Journal of Intelligent & Robotic Systems, 65(1):309–323, Jan 2012.
[4] Zhaodan Kong, Kayhan Özcimder, Nathan Fuller, Alison Greco, Diane Theriault,
Zheng Wu, Thomas Kunz, Margrit Betke, and John Baillieul. Optical Flow Sensing
and the Inverse Perception Problem for Flying Bats. arXiv.org, cs.SY, mar 2013.
[5] Matthew Turpin, Nathan Michael, and Vijay Kumar. Trajectory design and control
for aggressive formation flight with quadrotors. Autonomous Robots, 33(1-2):143–
156, 2012.
[6] R. Mahony, V. Kumar, and P. Corke. Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor. Robotics Automation Magazine, IEEE, 19(3):20–32,
2012.
[7] A. Tayebi and S. McGilvray. Attitude stabilization of a vtol quadrotor aircraft.
Control Systems Technology, IEEE Transactions on, 14(3):562–571, 2006.
[8] M.O. Efe. Neural network assisted computationally simple pi d control of a quadrotor uav. Industrial Informatics, IEEE Transactions on, 7(2):354–361, 2011.
[9] S. Bouabdallah and R. Siegwart. Backstepping and sliding-mode techniques applied
to an indoor micro quadrotor. In Robotics and Automation, 2005. ICRA 2005.
Proceedings of the 2005 IEEE International Conference on, pages 2247–2252, 2005.
90
Bibliography
91
[10] M.O. Efe. Robust low altitude behavior control of a quadrotor rotorcraft through
sliding modes. In Mediterranean Conference on Control Automation, 2007. MED
’07., pages 1–6, 2007.
[11] T. Fernando, J. Chandiramani, Taeyoung Lee, and H. Gutierrez. Robust adaptive
geometric tracking controls on so(3) with an application to the attitude dynamics
of a quadrotor uav. In Decision and Control and European Control Conference
(CDC-ECC), 2011 50th IEEE Conference on, pages 7380–7385, 2011.
[12] K.J. Åström and B. Wittenmark. Computer controlled systems: theory and design.
Prentice-Hall information and system sciences series. Prentice-Hall, 1984.
[13] A. Tchernychev and T. Alt. Stability analysis of sampled-data system with application to control of international space station. In Control Applications, 2000.
Proceedings of the 2000 IEEE International Conference on, pages 820–825, 2000.
[14] Velimir Jurdjevic. Geometric Control Theory. Cambridge University Press, dec
1996.
[15] F Bullo and A Lewis. Geometric Control of Simple Mechanical Systems: Modeling,
Analysis, and Design for Simple Mechanical Control Systems:. Springer-Verlag New
York, LLC, nov 2004.
[16] Taeyoung Lee, M Leok, and N H McClamroch.
Control of complex maneu-
vers for a quadrotor UAV using geometric methods on SE (3). Arxiv preprint
arXiv:1003.2005, 2010.
[17] Taeyoung Lee. Robust Global Exponential Attitude Tracking Controls on SO(3).
arXiv.org, math.OC, sep 2012.
[18] H.K. Khalil. Nonlinear systems. Macmillan Pub. Co., 1992.
[19] M D Shuster. A Survey of Attitude Representations. The Journal of the Astronautical Sciences, 41:439–517, oct 1993.
[20] H Hopf and B Eckmann. Collected Papers - Gesammelte Abhandlungen. Springer,
2001.
[21] John Stuelpnagel. On the Parametrization of the Three-Dimensional Rotation
Group. SIAM Review, 6:1–9, oct 1964.
[22] N A Chaturvedi, A K Sanyal, and N H McClamroch. Rigid-Body Attitude Control.
Control Systems, IEEE, 31(3):30–51, 2011.
[23] John Baez. Week 141. This Week’s Finds in Mathematical Physics, October 26,
1999.
Bibliography
[24] Taeyoung Lee.
92
Robust adaptive geometric tracking controls on SO (3) with
an application to the attitude dynamics of a quadrotor UAV.
Arxiv preprint
arXiv:1108.6031, 2011.
[25] Sanjay P Bhat and Dennis S Bernstein. A topological obstruction to continuous
global stabilization of rotational motion and the unwinding phenomenon. Systems
& Control Letters, 39(1):63–70, 2000.
[26] Paul Pounds, Robert Mahony, and Peter Corke. Modelling and Control of a QuadRotor Robot. In Australasian Conference on Robotics and Automation 2006, Auckland, New Zealand, 2006.
[27] G. D. Padfield. Helicopter Flight Dynamics. Balckwell Publishing, 2008.
[28] Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin. Nonlinear Complementary
Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control,
53(5):1203–1218, dec 2008.
[29] S Bonnabel, P Martin, and P Rouchon. Non-Linear Symmetry-Preserving Observers on Lie Groups. IEEE Transactions on Automatic Control, 54(7):1709–1713,
aug 2009.
[30] Petros Ioannou, P A Ioannou, and Jing Sun. Robust Adaptive Control. Prentice
Hall PTR, har/dskt edition, sep 1995.
[31] J L Mancilla-Aguilar. Estabilidad y estabilización de sistemas de control a datos
muestreados. PhD thesis, Facultad de Ciencias Exactas y Naturales, Universidad
de Buenos Aires, 2001.
[32] J L Mancilla-Aguilar, R A Garcia, and M I American Control Conference 1997
Proceedings of the 1997 Troparevsky. Digital implementation of static feedback
laws via SZH. In American Control Conference, 1997. Proceedings of the 1997,
1997.
[33] Lars Grüne, Karl Worthmann, and Dragan Nešić. Continuous-time controller redesign for digital implementation: a trajectory based approach. Automatica, 44(1):
225–232, 2008.
[34] T. Chen and B.A. Francis. Optimal sampled-data control systems. Communications
and control engineering series. Springer, 1995.
[35] Ezequiel Espósito, Alan Kharsansky, Ariel Lutenberg, Federico Roasio, Claus Rosito, and Daniel Schermuk. Diseño e Implementación de un Cuadricóptero de Vuelo
Autónomo. In Congreso Argentino de Sistemas Embebidos, Buenos Aires, Argentina, 2012.