Institutionen för systemteknik Department of Electrical Engineering three-axis camera gimbal

Institutionen för systemteknik Department of Electrical Engineering three-axis camera gimbal
Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Model-based automatic tuning and control of a
three-axis camera gimbal
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Henric Edlund
LiTH-ISY-EX–15/4853–SE
Linköping 2015
Department of Electrical Engineering
Linköpings universitet
SE-581 83 Linköping, Sweden
Linköpings tekniska högskola
Linköpings universitet
581 83 Linköping
Model-based automatic tuning and control of a
three-axis camera gimbal
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Henric Edlund
LiTH-ISY-EX–15/4853–SE
Handledare:
Daniel Simon
isy, Linköpings universitet
Mårten Svanfeldt
Intuitive Aerial
Examinator:
Martin Enqvist
isy, Linköpings universitet
Linköping, 11 juni 2015
Avdelning, Institution
Division, Department
Datum
Date
Automatic control
Department of Electrical Engineering
SE-581 83 Linköping
2015-06-11
Språk
Language
Rapporttyp
Report category
ISBN
Svenska/Swedish
Licentiatavhandling
ISRN
Engelska/English
Examensarbete
C-uppsats
D-uppsats
—
LiTH-ISY-EX–15/4853–SE
Serietitel och serienummer
Title of series, numbering
Övrig rapport
ISSN
—
URL för elektronisk version
http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-XXXXX
Titel
Title
Modellbaserad automatisk inställning och reglering av en treaxlig kameragimbal
Författare
Author
Henric Edlund
Model-based automatic tuning and control of a three-axis camera gimbal
Sammanfattning
Abstract
En gimbal är en pivoterande anordning som frikopplar rörelser hos en plattform från dess
last. Lasten är i detta fall en filmkamera som måste vara frikopplad från yttre rörelser för
att kunna spela in videomaterial utan oönskade rörelser. En utmaning med en kameragimbal är att den kan användas med många olika kombinationer av kameror och objektiv, som
påverkar systemets dynamik. Detta innebär att systemet måste ställas in på nytt varje gång
en ny kamerauppsättning ska användas. Då de flesta inom filmbranschen är obekanta med
reglerteknik är denna justering ganska så problematisk.
Detta examensarbete undersöker vilka möjligheter som finns för att göra en automatisk
inställning av ett reglersystem. Detta görs genom att excitera systemet och samla in data.
Denna data används sedan för att beräkna en modell för systemet. Modellen används sedan
för att reglera systemet och på så sätt behöver inte systemet justeras manuellt.
Som grund används en fysikalisk modell av systemet framtagen med hjälp av Lagrangeekvationen. Denna modell innehåller okända parametrar som tröghetsmoment, tyngdpunktslägen och friktionskonstanter. För att beräkna dessa parametrar används metoder
från systemidentifiering. Ett centralt problem vid systemidentifiering är hur systemet ska
exciteras för att få fram så mycket information som möjligt om systemets dynamik. Detta
problem angrips genom att formulera ett optimeringsproblem som används för att ta fram
optimala trajektorior.
Den identifierade modellen används sedan för att reglera gimbalen. Olika metoder för
att göra detta diskuteras. Genom att använda metoden återkopplingslinjärisering kan man
kompensera för effekter som modelleras. Detta gör att systemet blir oberoende av modellparameterarna och dessutom linjärt och frikopplat. För att styra det nya linjära systemet
används en pid-regulator. Effekten av modellfel i återkopplingslinjäriseringen analyseras
och för att garantera robusthet av det slutna systemet används metoden Lyapunov redesign.
Slutligen presenteras experimentella resultat. Modellkvaliteten hos den skattade modellen undersöks. Sedan används modellen för att reglera gimbalen och referensföljning testas. Resultaten visar att det finns problem med den skattade modellens prestanda.
Nyckelord
Keywords
system identification, feedback linearisation, camera gimbal
Sammanfattning
En gimbal är en pivoterande anordning som frikopplar rörelser hos en plattform
från dess last. Lasten är i detta fall en filmkamera som måste vara frikopplad
från yttre rörelser för att kunna spela in videomaterial utan oönskade rörelser.
En utmaning med en kameragimbal är att den kan användas med många olika
kombinationer av kameror och objektiv, som påverkar systemets dynamik. Detta
innebär att systemet måste ställas in på nytt varje gång en ny kamerauppsättning
ska användas. Då de flesta inom filmbranschen är obekanta med reglerteknik är
denna justering ganska så problematisk.
Detta examensarbete undersöker vilka möjligheter som finns för att göra en
automatisk inställning av ett reglersystem. Detta görs genom att excitera systemet och samla in data. Denna data används sedan för att beräkna en modell för
systemet. Modellen används sedan för att reglera systemet och på så sätt behöver
inte systemet justeras manuellt.
Som grund används en fysikalisk modell av systemet framtagen med hjälp
av Lagrangeekvationen. Denna modell innehåller okända parametrar som tröghetsmoment, tyngdpunktslägen och friktionskonstanter. För att beräkna dessa
parametrar används metoder från systemidentifiering. Ett centralt problem vid
systemidentifiering är hur systemet ska exciteras för att få fram så mycket information som möjligt om systemets dynamik. Detta problem angrips genom att
formulera ett optimeringsproblem som används för att ta fram optimala trajektorior.
Den identifierade modellen används sedan för att reglera gimbalen. Olika metoder för att göra detta diskuteras. Genom att använda metoden återkopplingslinjärisering kan man kompensera för effekter som modelleras. Detta gör att systemet blir oberoende av modellparameterarna och dessutom linjärt och frikopplat.
För att styra det nya linjära systemet används en pid-regulator. Effekten av modellfel i återkopplingslinjäriseringen analyseras och för att garantera robusthet
av det slutna systemet används metoden Lyapunov redesign.
Slutligen presenteras experimentella resultat. Modellkvaliteten hos den skattade modellen undersöks. Sedan används modellen för att reglera gimbalen och
referensföljning testas. Resultaten visar att det finns problem med den skattade
modellens prestanda.
iii
Abstract
A gimbal is a pivoted device that decouples movements of a platform from its payload. The payload is a camera which must be stabilized to capture video without
motion disturbances. A challenge with this type of gimbal is that a wide span of
cameras with different sizes and weights can be used. The change of camera has
significant effect on the dynamics of the gimbal and therefore the control system
must be re-tuned. This tuning is inconvenient, especially for someone without
knowledge of control engineering.
This thesis reviews suitable methods to perform an automatic controller tuning directly on the gimbal’s hardware. This tuning starts by exciting the system
and then using data to estimate a model. This model is then used to control the
gimbal, thus removing the need for manual tuning of the system.
The foundation of this thesis is a physical model of the gimbal, derived through the Lagrange equation. The physical model has undetermined parameters
such as inertias, centre of gravity and friction constants. System identification
is used to determine these parameters. A problem discussed is how the system
should be excited in order to achieve data with as much information as possible
about the dynamics. This problem is approached by formulating an optimization
problem that can be used find suitable trajectories.
The identified model is then used to control the gimbal. Different methods for
model based-control are discussed. By using a method called feedback linearisation all of the parameter-dependant dynamics of the gimbal can be compensated
for. Apart from being independent of model parameters the new outer system
is also decoupled and linear. A pid controller is used for feedback control of
the outer system. The uncertainty of the feedback linearisation is analysed to
find the effects of model errors. To assure robustness of the closed loop system a
Lyapunov redesign controller is used to compensate for these model errors.
Some experimental results are also presented. The quality of the estimated
model is evaluated. Additionally, the reference tracking performance of the control system is tested and results reveal issues with the estimated model’s performance.
v
Acknowledgments
I want to thank my supervisor Mårten Svanfeldt at Intuitive Aerial AB for his
valuable contributions and for helping me sorting out various practical issues. I
also want to thank my supervisor Daniel Simon and examiner Martin Enqvist for
providing helpful feedback and support during this thesis work.
Linköping, June 2015
Henric Edlund
vii
Contents
Notation
1 Introduction
1.1 Background .
1.2 Purpose . . . .
1.3 Thesis scope .
1.4 Approach . . .
1.5 Related work .
1.6 Thesis outline
xiii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
1
1
2
2
2
3
2 System overview
2.1 Introduction . . . . .
2.2 Gimbal construction
2.3 Actuators . . . . . . .
2.4 Sensors . . . . . . . .
2.5 Gimbal balancing . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
5
5
5
6
3 Modelling
3.1 Introduction . . . . . . . . . . . . . . . . .
3.2 Mechanical multibody system theory . . .
3.2.1 Kinematics . . . . . . . . . . . . . .
3.2.2 Differential kinematics . . . . . . .
3.2.3 Dynamics . . . . . . . . . . . . . .
3.3 Assumptions for the gimbal . . . . . . . .
3.4 Gimbal kinematics . . . . . . . . . . . . .
3.4.1 Coordinate representation . . . . .
3.4.2 Denavit-Hartenberg convention . .
3.5 Gimbal differential kinematics . . . . . . .
3.5.1 Derivation of the Jacobian . . . . .
3.5.2 Jacobian for the gimbal system . .
3.6 Gimbal dynamics . . . . . . . . . . . . . .
3.6.1 Energy of the gimbal . . . . . . . .
3.6.2 Lagrange equations for the gimbal
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
9
10
10
11
11
13
13
14
14
16
17
17
18
18
20
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
ix
x
Contents
3.6.3 Actuator modelling . . . . . . . . .
3.6.4 Friction modelling . . . . . . . . .
3.7 The resulting model for the gimbal system
3.7.1 Complete model . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
21
22
22
23
4 System identification
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 System identification theory . . . . . . . . . . . . . . . .
4.2.1 Linear regression model . . . . . . . . . . . . . .
4.2.2 Identification methods . . . . . . . . . . . . . . .
4.2.3 Identifiability and informative experiments . . .
4.2.4 Method for determining identifiable parameters
4.2.5 Model structure for identification . . . . . . . . .
4.2.6 Validation . . . . . . . . . . . . . . . . . . . . . .
4.3 Regression model for the gimbal . . . . . . . . . . . . . .
4.3.1 Base parameters for the gimbal . . . . . . . . . .
4.4 Experiment design . . . . . . . . . . . . . . . . . . . . . .
4.4.1 Open loop versus closed loop identification . . .
4.4.2 Optimal trajectories . . . . . . . . . . . . . . . . .
4.4.3 Optimization of trajectories . . . . . . . . . . . .
4.5 Identification for the gimbal . . . . . . . . . . . . . . . .
4.5.1 Gimbal measurement equations . . . . . . . . . .
4.5.2 Gimbal measurement filtering . . . . . . . . . . .
4.5.3 Gimbal predictor . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
25
25
26
26
26
28
29
30
31
31
32
33
33
34
36
37
37
38
38
5 Control design
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . .
5.1.1 Model based control . . . . . . . . . . . . . .
5.1.2 Proposed controller . . . . . . . . . . . . . . .
5.2 Control theory . . . . . . . . . . . . . . . . . . . . . .
5.2.1 Feedback linearisation . . . . . . . . . . . . .
5.2.2 Lyapunov theory . . . . . . . . . . . . . . . .
5.3 Feedback linearisation of the gimbal . . . . . . . . .
5.4 Outer loop control . . . . . . . . . . . . . . . . . . . .
5.4.1 pid controller . . . . . . . . . . . . . . . . . .
5.4.2 Uncertainty analysis of feedback linearisation
5.4.3 Robust controller . . . . . . . . . . . . . . . .
5.5 Controller tuning . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
39
39
40
41
41
43
45
46
46
47
47
51
6 Experimental results
6.1 Introduction . . . . . . . . . .
6.2 Experimental setup . . . . . .
6.3 Identification results . . . . .
6.3.1 Raw measurements . .
6.3.2 Data processing . . . .
6.3.3 Estimated parameters .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
53
53
53
53
54
57
59
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
xi
Contents
6.3.4 Validation . . . . . . . . . . . .
6.3.5 Conclusions . . . . . . . . . . .
6.4 Controller results . . . . . . . . . . . .
6.4.1 Simultaneous axes step tracking
6.4.2 Separate axis step tracking . . .
6.4.3 Conclusions . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
60
65
65
66
69
71
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
73
73
73
74
74
A Gimbal model
A.1 Components of the D matrix . . . . . . . . . . . . . . . . . . . . . .
A.2 Components of the C matrix . . . . . . . . . . . . . . . . . . . . . .
A.3 Gimbal regressor matrix . . . . . . . . . . . . . . . . . . . . . . . .
77
77
77
79
Bibliography
81
7 Conclusions and future work
7.1 Results . . . . . . . . . .
7.2 Discussion . . . . . . . .
7.3 Conclusions . . . . . . .
7.4 Future work . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Notation
Notation
Symbol
j
Ri
j
Ti
ri/j
j
ωi
Γ
q
Explanation
Rotation of frame i expressed in frame j.
Homogenous transformation, rotates and translates
frame i expressed in frame j.
Vector originating in point i ending in j
Angular velocity of body i expressed in frame j
The model error vector of the feedback linearisation
Gimbal axis angle vector, contains axis angles q1 , q2
and q3
Abbrevations
Symbol
ia
uav
dh
imu
ahrs
Explanation
Intuitive Aerial
Unmanned aerial vehicle
Denavit-Hartenberg convention
Inertial measurement unit
Attitude and heading reference system. Uses the imu
to compute attitude and heading measurements.
xiii
1
Introduction
1.1
Background
This thesis was performed at the company Intuitive Aerial (ia), which is specialized at producing flying camera rigs for professional aerial photography. Their
product consists of a multi-rotor platform and a gimbal that carries the camera.
The gimbal is a pivoted device that allows the camera to rotate freely around
its three axes. Since the gimbal is attached to a moving platform it is crucial to
isolate the camera from the movements of the platform. It is also desirable to
control the camera’s line of sight. This is accomplished by using electric motors
and inertial sensors. The gimbal is constructed to handle cameras weighing up
to 9 kilograms. Therefore, cameras with a wide span in weight and inertial properties can be used with the gimbal. This poses a challenge for the control system
since its performance is dependant on the dynamics of the whole system. The
current control system uses pid controllers and requires cumbersome tuning for
good performance. Another difficulty with the current system is the balancing
of the gimbal. In order to have good controller performance the system must be
carefully balanced so that the centre of mass is aligned with the rotational centre
of the gimbal.
1.2
Purpose
The purpose of this thesis is to overcome the above mentioned issues by developing an automatic online tuning of the control system. This is achieved by deriving
a mathematical model of the gimbal’s dynamics and then using methods from
system identification to find the parameters of the model. Finally, this identified
model can be used with a model-based control method which does not require
the user to perform any tuning. Additionally, this type of controller can improve
1
2
1
Introduction
the overall performance, compared to a pid-based system.
1.3
Thesis scope
The system uses sensors such as gyroscopes, accelerometers, magnetometers and
angular sensors at the joint axes. The processing and fusion of these signals will
not be covered in this thesis. Instead, the solutions developed by ia will be used.
A fundamental part of the control system is the input preprocessing which manipulates the camera operator’s control inputs to smooth reference trajectories
for the gimbal. This system will not be considered in this thesis.
To focus on the theoretical work some practicalities, such as the actual online
tuning, are not implemented on the hardware. It is instead performed offline
and the parameters are then programmed in the gimbal’s hardware. Relations
for transforming sensor and reference signals into the signals required by the
model have been derived but not implemented. Instead the gimbal is mounted
to a rigid platform which reduces the complexity of sensor transformations for
identification and control.
1.4
Approach
The base of this thesis is a physical model derived with methods from multi-body
mechanics. This physical model has unknown parameters that describe the dynamic properties of the camera and the gimbal. To find estimates of these parameters, methods from system identification have been used. The performance
of the controller is then evaluated on the gimbal. For ease of use the whole automatic tuning procedure should be handled by the gimbal system. This means
that all data collection, data preprocessing, estimation and calculations related
to controller synthesis should be possible to run on the gimbal’s hardware.
1.5
Related work
Previous work on gimbals is not very common, especially system identification.
Johansson [2012] is a previous master thesis on a three-axis gimbal performed
at ia, where a model for the gimbal is derived and a pid controller is used. In
Skoglar [2002], a model is derived and then identification and control methods
are evaluated through simulations. Li et al. [2013] covers system identification
of a two-axis gimbal. It results in a non-linear, demanding optimization problem
which may not be suitable for this thesis. In Sangveraphunsiri and Malithong
[2009] a robust feedback linearisation controller is evaluated as well as a sliding
mode controller. The sliding mode controller is also evaluated in Shtessel [1994].
Kwon et al. [2007] evaluates a decentralized and linear controller for a gimbal
system.
The structure of a gimbal closely resembles a robot. The field of robotics has
several established approaches for modelling, identification and control. There-
1.6
Thesis outline
3
fore, many of the methods that have been used for robotics should be appropriate
to apply to the gimbal.
1.6
Thesis outline
First a system overview is given in Chapter 2. This chapter is then followed by the
three main parts, modeling in Chapter 3, identification in Chapter 4 and control
in Chapter 5. Each main part contains a theory part going over the fundamental
theory which is then applied to the gimbal. In Chapter 6, the experimental results are presented and in Chapter 7 conclusions and a discussion concludes this
thesis.
2
System overview
2.1
Introduction
This chapter’s purpose is to give a short overview of how the gimbal works. It
briefly covers construction, sensors and actuators of the gimbal.
2.2
Gimbal construction
The mechanical structure of the gimbal consists of four parts. On the top there
is the head that connects the gimbal to the multirotor platform. The connection
consists of damper springs to remove vibrations. Connected to the head is the
spine of the gimbal, which is also connected to the legs, see Figure 2.1. Then
there is the fork and finally the cradle, where the camera is mounted. The body
of the gimbal is built of carbon fibre and aluminium.
2.3
Actuators
Each axis of the gimbal is driven by an electric motor. The motors are brushless
direct drive motors with high torque output. The motors are controlled by a
motor control system which uses angular sensors to measure the angle of the
motor. The motors are torque controlled which means that the controller takes a
reference torque as input signal.
2.4
Sensors
The gimbal has two imus, one situated at the spine and one at the cradle. The
imus’ computer uses an attitude and heading reference system (ahrs) algorithm
5
6
2
Head
System overview
Yaw motor
Upper imu
Spine
Tilt motor
Roll motor
Camera mount
Fork
Cradle
Lower imu
Figure 2.1: A schematic of the gimbal where the parts of the gimbal are presented. The positions of actuators and sensors are pointed out. Illustration
reproduced by courtesy of Intuitive Aerial
to obtain both attitude angle and angular velocity. The gimbal is also equipped
with angular sensors at each joint. These are used in the motor controllers but
are also available for use in the control unit.
2.5
Gimbal balancing
The purpose of balancing the gimbal is to adjust the positioning of the camera
such that the whole system’s centre of mass is aligned with the gimbal’s rotational
axes. If a perfect balancing is achieved, gravity will not interfere with the gim-
2.5
Gimbal balancing
7
bal’s dynamics thus making it easier to control. If the gimbal is not balanced the
motors must compensate for static moments induced by gravity. This degrades
the performance of the pid regulators. A poor balancing may result in gravity
moments higher than the motors’ maximum output torque making it impossible
to control the gimbal. For instance, a gimbal without a camera mounted is so
badly balanced that it becomes difficult for the motors to move the gimbal.
3
Modelling
3.1
Introduction
For system identification and model-based control methods it can be advantageous to have a physical model of the gimbal. Since the gimbal is a mechanical
system, methods used in mechanics should be a good starting point. There are
many more or less systematic modelling approaches available in engineering mechanics which can be used for derivation of a mathematical model describing the
motion of the parts in the gimbal.
A camera gimbal closely resembles an industrial robot, which has been studied comprehensively. The literature suggests two different modelling approaches.
One is the Lagrange method and the other is the Newton-Euler method [Siciliano
et al., 2009, Ch. 7]. The principal difference is that the Lagrange method is based
on energy balances in the system and Newton-Euler’s method is based on balance
laws of forces and torques for the system.
Before any of these two methods can be used some basic mechanical relations
must be derived. These are kinematics and differential kinematics. Kinematics describes the geometrical relations of the system. Differential kinematics is
based on these relations and describes the mapping between joint velocities and
link velocities. To determine the kinematical relations it is convenient to assign
a coordinate system to each body of the gimbal. The coordinate assignments
are made systematically using a convention called Denavit-Hartenberg [Siciliano
et al., 2009, Ch. 2]. The derivation of the gimbal model is based on general n-link
robot approach presented in Siciliano et al. [2009].
This approach for modelling a gimbal has been tried before in Johansson
[2012]. The particular model deduced in this thesis had the problem of being
difficult to work with due to its size. The main reason for this was the coordinate
frame assignment which was mechanically correct but not made in the easiest
9
10
3
Modelling
way. In this thesis, a different coordinate system assignment is used which results in more simple dynamic equations.
3.2
Mechanical multibody system theory
The three fundamental parts of mechanical modelling are kinematics, differential
kinematics and dynamics, as was mentioned in the introduction. We will now
briefly go over the theoretical background of these.
3.2.1
Kinematics
To be able to model a mechanical system it is essential to describe the geometry
of the system. Kinematics offers us a tool to describe this geometry. A comprehensive discussion on kinematics can be found in Siciliano et al. [2009, Ch. 2]. The
geometry can be divided into rotations and translations that describe the bodies
of the system. To represent the rotations, rotation matrices are useful. A rotation matrix relates a coordinate between two frames that have a relative rotation.
The rotation in Figure 3.1 is a rotation around the z0 axis by the angle θ and the
resulting rotation matrix is

cos θ

R01 =  sin θ

0
y1
− sin θ
cos θ
0

0
0

1
(3.1)
y0
x1
θ
z0 , z 1
x0
Figure 3.1: Two coordinate frames related by a rotation matrix.
A useful property of rotation matrices is their orthogonality property. This
makes it possible to invert a rotation matrix by transposing it.
To have a complete description of the relation between two coordinate systems the translation must be included. This is simply done by defining the vector
d0/1 between the origins of the systems.
3.2
11
Mechanical multibody system theory
Let us look at how rotations and translations can be used to describe points
in the frames. Let p0 and p1 be the coordinate of a point p in frame 0 and 1,
respectively. By using the previously established rotation and translation we can
write
p0 = d0/1 + R01 p1
(3.2)
A more compact way to represent coordinates in different frames is to use homogeneous transformations. The homegenous coordinate for an arbitrary point
p is denoted as
" #
p
p̃ =
(3.3)
1
The complete coordinate transformation with both the rotation and translation
combined can be written as
" 0
#
R1 d10
0
T1 =
(3.4)
0
1
The coordinate transformation in (3.2) can now be written as
p̃0 = T10 p̃1
(3.5)
Coordinate transformations between a sequence of n frames can be written as
p̃0 =
n
Y
Tkk−1 p̃n
(3.6)
k=1
This is called a kinematic chain and is useful for describing a system’s geometry.
3.2.2
Differential kinematics
We would also like to have a description of the velocities between different frames.
By using rotation matrices, velocity vectors can be related to other frames as
1
0
ω1/0
= R01 ω1/0
(3.7)
Furthermore, once velocities are expressed in the same frame they can be added.
These properties can be used to determine a relation between the velocities of
the bodies in a system. This relation is called a Jacobian and will be determined
specifically for the gimbal in Section 3.5. Differential kinematics is treated in
Siciliano et al. [2009, Ch. 3].
3.2.3
Dynamics
The goal of dynamics is to find the differential equations that determine the relation between inputs and system states. One method to determine the dynamics
of a system is the Lagrange method. It is based on knowledge of a system’s total
energy in the form of a function known as the Lagrangian which is defined as
L=T −U
(3.8)
12
3
Modelling
where T denotes the total kinetic energy and U is the total potential energy.
By using the Lagrange equation [Siciliano et al., 2009]
!
d ∂L
∂L
−
=Q
(3.9)
dt ∂q̇
∂q
where Q denotes the external force and q is the system state, the dynamics of the
system is obtained.
Let us consider a simple example to illustrate this method
3.1 Example: Pendulum
ℓ
θ
p
Figure 3.2: A rigid pendulum seen from the side.
We want to model the pendulum in Figure 3.2. The pendulum has mass m and
inertia I. Its centre of gravity is located at p, at the distance ` from the pivot
point, which is fixated in an inertial frame. The expressions of both kinetic and
potential energy must be determined first. Since the pendulum can be modelled
as a rigid link, one contribution is the rotational energy, the other is the centre of
mass translational energy. Hence the total kinetic energy ca be written
T =
1 2 1
I θ̇ + mṗ2
2
2
(3.10)
The potential energy is
U = mg`(1 − cos(θ))
(3.11)
To use the Lagrange method we must choose a generalized coordinate, and a
simple choice is θ. Then ṗ must be expressed as a function of θ̇. By using the
differential kinematics relation
ṗ = ` θ̇
(3.12)
3.3
Assumptions for the gimbal
13
we can form the Lagrangian as
1 2 1 2 2
I θ̇ + m` θ̇ − mg`(1 − cos(θ))
2
2
By using the Lagrange equation we have
L=
(I + m` 2 )θ̈ + mgl sin(θ) = 0
(3.13)
(3.14)
If we would have used the Newton-Euler method instead, we would then have
had to look at the forces and torques of the system to obtain the equations of
motion. In this case, there is not much difference in complexity of the modelling
procedures of these two methods. But as the system complexity is increased it
is generally easier to compute the energy of the system rather than to calculate
the torques and forces in the system. Therefore the Lagrange method has a clear
advantage. A complete description of both the Lagrange method and the NewtonEuler method can be found in Siciliano et al. [2009, Ch. 7].
3.3
Assumptions for the gimbal
A fundamental dilemma when modelling a physical system is the trade-off between simplicity and model accuracy. We want the model to be simple enough
to be used with established methods, yet it is crucial that the model can describe
the mechanical behaviour well enough. In order to achieve a balanced trade-off
some assumptions are made for the gimbal.
For certain loads and velocities, the links of the system will start to deform
elastically and this causes oscillations in the system. This oscillative behaviour
is assumed to be insignificant compared to the overall dynamic behaviour of the
gimbal. When doing the calibration it will be important not to excite these flexibilities for a good model estimation. Furthermore, the controller must be so
robust that these flexibilities can be handled. The main reason we do not want to
model these flexibilities is the increase in model complexity.
When modelling friction there are many different aspects to consider. To obtain a useful model many of these effects are neglected.
Since the gimbal will be attached to a multi-rotor platform some assumptions
must be made about the interaction between these two. It is assumed that the
movement of the gimbal does not affect the multi-rotor platform’s movement.
The movement of the multi-rotor platform, however, affects the gimbal and cannot be neglected.
3.4
Gimbal kinematics
The geometry of the gimbal can be seen as the result of a sequence of rotations
and (constant) translations between a fixed base and the camera mounted in the
gimbal.
14
3.4.1
3
Modelling
Coordinate representation
When working with transformation matrices one has to consider how they are
represented. A commonly used coordinate representation is Euler angles; it uses
three parameters to describe a rotation. A drawback is the prevalence of singular points. For the natural rotation order of the gimbal these singularities are
located at ± 90◦ in the roll axis. These points will never be reached in any operating scenario as they lie outside of the gimbal’s workspace, hence this causes no
problem.
An alternative to Euler angles is quaternions. They consist of a non-minimal
representation with 4 parameters and have no singularities. One the downside
they lead to more complex expressions and are not as intuitive to work with.
Since there is no benefit with using quaternions in this case we chose to work
with Euler angles. A more comprehensive treatment of these coordinate representations can be found in Siciliano et al. [2009, Ch. 2]
3.4.2
Denavit-Hartenberg convention
The Denavit-Hartenberg (dh) convention is a procedure to assign coordinate
frames to the links of a robot. The gimbal consists of 4 bodies and thus 4 coordinate systems are needed for a complete description of the geometry of the
system. The structure can be seen in Figure 3.3. Body (0) is the base of the gimbal which is attached to the multirotor platform. Body (1) is called the spine and
connects body (0) to body (2), which is called the fork. The fork then connects
to body (3) which is called the cradle, where the camera is mounted. The DH
convention is described in Siciliano et al. [2009, Ch. 2] and can be used for a general link structure. The gimbal’s structure results in some simplifications when
applying the DH convention, therefore a modified version is used.
Modified Denavit-Hartenberg Algoritm
1. Find the joint axes and set the corresponding z0 , .., zn axes.
2. Choose frame 0 by locating the origin along the z0 -axis. Axes x0 and y0 can
be chosen arbitrarily as long as a right-handed frame is achieved.
Go through Steps 3 to 5 for i = 1, ..., n − 1
3. Locate origin 0i at the intersection of zi and zi−1
4. Assign axis xi along the common normal to axes zi and zi−1 with direction
from joint i to joint i + 1.
5. Choose axis yi to obtain a right-handed frame.
Final steps
6. Choose the last frame n. Align zn with zn−1 and choose xn according to step
4 and yn to obtain a right-handed system.
7. Compute the transformation matrices for each part of the system.
3.4
15
Gimbal kinematics
Denavit-Hartenberg applied to the gimbal
In Figure 3.3 a schematic view of the gimbal is presented with the coordinate
systems from the dh algorithm. The rotational angles are represented by θ1 , θ2
and θ3 and the axis of rotation coincides with body fixed coordinate axis z0 , z1
and z2 .
Y0
Y0
X0
X0
Z0
X1
Z0
Z1
Y1
Y3 Z 2
X1
X 3 Y2 Z 1
Z2
Y3
Y2
X2
Z 3 X 2 Y1
Figure 3.3: The gimbal with coordinate systems seen from the side.
X3
Z3
Figure 3.4: Side view of the
gimbal where coordinate systems
with overlapping origins have been
moved for a clearer view.
Transformation matrices for the gimbal
Now we want to derive the transformation matrices between the coordinate systems defined above. We start with frame 0 and go step by step until frame 3 is
reached. The rotation matrix that describes frame 1 relative to frame 0 is


− sin θ1 0 cos θ1 


R01 =  cos θ1 0 sin θ1 
(3.15)


0
1
0
The translation vector is
 
0
 
d10 = 0
 
h
And finally the transformation matrix is obtained as
" 0
#
R1 d10
0
T1 =
0
1
(3.16)
(3.17)
16
3
The rotation matrix that describes frame 2 relative to frame 1 is


− sin θ2 0 cos θ2 


R12 =  cos θ2 0 sin θ2 


0
1
0
Modelling
(3.18)
The origins of the two frames coincide, thus
 
0
 
d21 = 0
 
0
(3.19)
Furthermore, the transformation matrix becomes
" 1
#
R2 d21
1
T2 =
0
1
(3.20)
The rotation matrix of frame 3 relative to frame 2 is


− sin θ3 0 cos θ3 


R23 =  cos θ3 0 sin θ3 


0
1
0
(3.21)
Again the origin of the two frames coincides, and
 
0
 
d32 = 0
 
0
"
T32
R2
= 3
0
d32
1
(3.22)
#
(3.23)
Finally, the transformation between frame 0 and fram 3 is

cθ1 cθ3 − sθ1 sθ2 sθ3
c s + cθ sθ sθ
0
1
2
3
T3 =  θ3 θ1
−cθ2 sθ3

0
−cθ2 sθ1
c θ1 c θ2
sθ2
0
cθ1 sθ3 + cθ3 sθ1 sθ2
s θ1 s θ3 − c θ1 c θ3 s θ2
c θ2 c θ3
0

0
0

h

1
(3.24)
where cθi = cos(θi ) and sθi = sin(θi ).
3.5
Gimbal differential kinematics
To determine the equations of motion of the gimbal it is necessary to know how
joint velocities map to velocities at the links of the gimbal. This mapping is called
the Jacobian and it is a linear mapping of joint velocities to link velocities.
3.5
17
Gimbal differential kinematics
3.5.1
Derivation of the Jacobian
As mentioned was earlier, the gimbal motion consists of only rotations since all
rotation axes intersect in a common point. Thus, when deriving the differential
kinematics the parts concerning translational velocity are left out. This derivation can be found in Siciliano et al. [2009, Ch. 3].
The rotation Jacobian is formulated as
j
j
ωi = Ji θ̇
(3.25)
j
and is a mapping between the joint velocity θ̇ and the body velocity ωi . The vector
j.
j
ωi
denotes the angular velocity of body i, represented in coordinate system
For a single joint it can be seen that
i−1
= θ̇i zi−1
ωi/i−1
(3.26)
where zi−1 is the previous joint’s rotational axis. The vector zi−1 can now be
represented in the base frame by using the kinematic chain (3.6) as
zi−1 =
i
Y
Rkk−1 z0
(3.27)
k=1
Since the rotational axes are represented in the same frame, the angular velocities
can be added. For each joint k, the corresponding column of the Jacobian becomes



zk−1 k ≤ i
Jk = 
(3.28)

0 otherwise
and
3.5.2
h
Jn0 = Jk
Jk+1
...
i
for each k = 1, ..., n
(3.29)
Jacobian for the gimbal system
Using the briefly presented theory above, the Jacobian is
h
i h
i
J30 = z2 z1 z0 = R01 R12 z0 R01 z0 z0
where
h
z0 = 0
0
iT
1
(3.30)
(3.31)
Note that the Jacobian is represented in the base frame. Then the Jacobian for
each individual link becomes




0 0 0
0 cos θ1 0




J10 = 0 0 0 , J20 = 0 sin θ1 0
(3.32)




1 0 0
1
0
0


0 cos θ1 − cos θ2 sin θ1 


and J30 = 0 sin θ1 cos θ1 cos θ2 
(3.33)


1
0
sin θ2
18
3.6
3
Modelling
Gimbal dynamics
The goal of the dynamic equations is to obtain a model that describes how the
joint states depend on the actuators. As was mentioned in the introduction, there
are two well-regarded methods that can be used; the Lagrange method and the
Newton-Euler method. These methods are based on different principles and result in different types of expressions for the dynamics. The Lagrange method
results in what is known as the inverse dynamics form. This means that it gives
the actuator torque as a function of the joint variables and their differentials. This
form can easily be written as a state-space model.
The Newton-Euler method results in a recursive formulation of the system’s
dynamics. It is possible rewrite either method’s result to the same form as the
other. Since the Lagrange method requires a bit less work and gives the model on
the desired form directly, it is chosen. The dynamics have been derived based on
Siciliano et al. [2009, Ch. 7]. The Lagrange method is independent of the chosen
coordinate frames, but it depends on joint variables qi , which must be chosen.
The natural choice is the joint angles θi , thus qi = θi . To use the Lagrange method
we need to find expressions for the energy of the gimbal.
3.6.1
Energy of the gimbal
Kinetic energy
The total kinetic energy of the system is
T =
n
X
Ti
(3.34)
k=1
where
1
Ti =
2
Z
Vi
ṗTi ṗi ρdV
(3.35)
is the energy for link i and ṗi is the velocity of volume element dV for link i. Let
us define the vector
ri/c = pi − pc
(3.36)
where pc is the rotational centre of the gimbal. The relation that describes the
velocities of two points in a rigid body as it rotates is
ṗi = ṗc + S(ωi )ri/c
where S is a skew-symmetric matrix. By using this relation we have
Z
1
Ti =
(ṗ + S(ωii )rc/i )T (ṗc + S(ωii )rc/i )ρdV
2 Vi c
(3.37)
(3.38)
3.6
19
Gimbal dynamics
In Section 3.5 it was determined that ṗc = 0 for all joints and therefore that the
energy can be expressed as
Z
1
Ti =
(S(ωii )ri/c )T (S(ωii )ri/c )ρdV
2 Vi
Z
1
=
(−S(ri/c )ωii )T (−S(ri/c )ωii )ρdV
(3.39)
2 Vi
!
Z
1
1
= (ωii )T
S(ri/c )T S(ri/c )ρdV ωii = (ωii )T Ii ωii
2
2
Vi
where we have defined the matrix
R
R
R 2

2
− rix riz ρdV 
 (riy + riz )ρdV R− rix riy ρdV
R


2
2
...
(rix
+ riz
)ρdV R− riy riz ρdV 
Ii = 


2
2
...
...
(rix
+ riy
)ρdV 


Iixx −Iixy −Iixz 

Iiyy −Iiyz 
=  . . .


...
...
Izz
(3.40)
(3.41)
Note that the matrix is symmetric and that the ". . ." elements are obtained from
symmetry. Now the kinetic energy is expressed as a function of link velocity in
the link’s own coordinate system. To express the kinetic energy as a function of
joint variables two steps must be taken. First the velocity of each link must be
expressed in the base frame. By using
ωii = (R0i )T ωi0
(3.42)
the expression
1 0T 0
(ω ) Ri Ii (R0i )T ωi0
(3.43)
2 i
for the kinetic energy is obtained. The next step is to express the kinetic energy
as a function of the joint variables. By using the Jacobian, ωi0 can be replaced by
joint variables through the following expression
Ti =
ωi0 = Ji (q)q̇
(3.44)
The total kinetic energy becomes
n
T =
n
1
1 XX
dij q̇i q̇j = q̇T D(q)q̇
2
2
(3.45)
i=1 j=1
where
D(q) =
n
X
Ji (q)T R0i Ii (R0i )T Ji (q)
i=1
and D(q) is symmetric and positive definite [Siciliano et al., 2009, Ch. 7].
(3.46)
20
3
Modelling
Potential energy
The potential energy is calculated for each link as
U=
n
X
(3.47)
Ui
i=1
where
Z
Ui = −
g0T pi,c ρdV = −mi g0T rci/0
Vi
(3.48)
and rci/0 is the vector between the centre of gravity for link i and the base frame.
Let us introduce
 
`ix 
 
rci/i = `iy 
(3.49)
 
`iz
Then
rci/0 = Ti0 rci/i
(3.50)
where Ti0 is the homogeneous transformation between frame i and 0. It is determined by using the kinematic chain described by (3.6).
3.6.2
Lagrange equations for the gimbal
When the kinetic and potential energy have been computed the Lagrangian becomes
L(q, q̇) = T (q, q̇) − U (q)
(3.51)
By considering the differentiations of L according to (3.9) we get
n
∂L X
=
dij q̇j
∂q̇i
(3.52)
j=1
Note that U (q) does not depend on q̇. Then we have
! X
n
n
n
n X
n
X
X
X
∂dij (q)
d ∂L
d
=
dij q̈i +
dij (q)q̇j =
dij q̈i +
q̇ q̇
dt ∂q̇i
dt
∂qk j k
j=1
j=1
j=1
and finally
n
(3.53)
j=1 k=1
n
∂L
1 X X ∂djk (q)
∂U
q̇j q̇k −
=
∂qi
2
∂qi
∂qi
j=1 k=1
|{z}
(3.54)
gi
By putting it all together, we get
n
X
j=1
dij q̈i +
n X
n
X
j=1 k=1
hijk q̇j q̇k + gi (q) = Qi
for i = 1, ..., n
(3.55)
3.6
21
Gimbal dynamics
where
hijk =
∂dij
∂qk
and
cij =
n
X
−
1 ∂djk
2 ∂qi
hijk q̇k
(3.56)
(3.57)
k=1
To summarize, (3.55) can be written on compact matrix form as
D(q)q̈ + C(q, q̇)q̇ + G(q) = Q
(3.58)
Physical interpretation
The Lagrange method may not be as physically intuitive as, for instance, NewtonEuler’s equations. The following description gives a physical interpretation of
what the model represents.
The matrix D(q), with elements dij , describes inertial effects of the gimbal.
When i = j the element describes the inertia of that particular axis. When i 6= j
the element describes inertial effect on axis i by axis j.
Next we have the matrix C(q, q̇), with elements cij . According to (3.57), these
elements can be written as hijk . The element hijj describes the centrifugal effects induced on joint i by joint j. The elements hijk represent the Coriolis effect
induced on joint i by joints j and k.
Finally, the element gi in the vector G(q) describes the moment induced by
gravity on link i.
3.6.3
Actuator modelling
The actuators consist of brushless motors. The torque τ is proportional to the motor’s current, according to τ = Ki i. The motor’s torque is not linear but saturated
at some τ max . Furthermore, the current in a brushless electric motor is described
by
d
(3.59)
L i + Ri + V − Vemf = 0
dt
where L is the inductance and R is the resistance of the motor. Vemf = Kb ω is the
counter electromotive force which is proportional the velocity. The inductance
of the motor is omitted by letting L = 0, thus making the motor model static.
Furthermore, since the motor is directly coupled to the gimbal axis, without a
gearbox, the rotational speed of the motor is going to be very low. The term Vemf
is thus insignificant and removed from the equations. The torque then becomes

KV


V < Vmax
 Ri
(3.60)
τ=

τ max V > Vmax
The actuator system is considered as a separate system and is therefore not included explicitly in the gimbal model. Therefore we consider the gimbal system
input to be the actuator torque.
22
3.6.4
3
Modelling
Friction modelling
Friction is difficult to model and therefore some friction effects are omitted. By
using a very simplified model, which only accounts for static and viscous friction,
the complexity can be reduced. This model is expressed as
Fi = Fis sign(q̇i ) + Fiv q˙i
(3.61)
where Fis is the static friction parameter and Fiv is the viscous friction parameter.
This model is linear with respect to the parameters which is useful when estimating friction together with the rest of the dynamics. A drawback with this model
is that it is only valid for velocities over a certain threshold.
3.7
The resulting model for the gimbal system
So far, the equations have just been derived from theory with only a few assumptions made about the gimbal system. The results from kinematics, differential
kinematics and dynamics will now be put together to get the model for the gimbal.
To obtain a more usable model, the inertia matrices are assumed to be diagonal. This simplification may not be needed for identification and control, as the
number of terms in the model should not be a problem for the hardware of the
gimbal. This will be tested and discussed in detail later. Note that some elements
of the inertia matrix are in fact zero due to geometric symmetries. Along with the
model equations the parameters of the model will be presented as well.
Model parameters
In Table 3.1 the parameters of the model are summarized. Note that some previously defined parameters have no effect due to the restriction of motion. For
instance body 1 only has one degree of freedom and therefore only one inertial
parameter is needed. Furthermore, the motion of body 1 is not affected by gravity
and therefore no gravity parameters are needed to describe it’s motion. Body 2
has two degrees of freedom are therefore two inertial parameters are needed as
well as two gravity parameters.
Table 3.1: Table with the model’s parameters.
D I1yy I2xx I2yy I2zz I3xx I3yy I3zz
C I1yy I2xx I2yy I2zz I3xx I3yy I3zz
G `2mx `2mz `3mx `3my `3mz
F F1s F1v F2s F2v F3s F3v
Calculating D
The matrix D is obtained from (3.46) as
D(q) = J1T R01 I1 (R01 )T J1 + J2T R02 I2 (R02 )T J2 + J3T R03 I3 (R03 )T J3
(3.62)
3.7
The resulting model for the gimbal system
23
All elements of the matrix are presented in Appendix A.
Calculating C
The matrix C(q, q̇) is obtained from D(q), by using (3.56) and then (3.57). All
elements of the matrix are presented in Appendix A.
Calculating G
By using the relations from Section 3.6.1, G (q) is obtained. The notation is simplified by letting one variable, l∗m∗ , describe the product of length, mass and gravity.
Each element describes the gravity effect och the corresponding gimbal axis.
The first element of G is
g1 = 0
(3.63)
Furthermore, for the second axis we have the following expression
g2 = l3my cos(q2 ) + l2mz cos(q2 ) − l2mx sin(q2 ) −
l3mz cos(q3 ) sin(q2 ) + l3mx sin(q2 ) sin(q3 )
(3.64)
Finally, for the third axis
g3 = − cos(q2 ) (l3mx cos(q3 ) + l3mz sin(q3 ))
(3.65)
Calculating Q
The modelled non-conservative torques are actuator torque and friction torque,
respectively
Q = τ − F (q̇)
(3.66)
with the first element
Q1 = τ1 − F1s sign(q̇1 ) − F1v q̇1
(3.67)
Furthermore, the second element is
Q2 = τ2 − F2s sign(q̇2 ) − F2v q̇2
(3.68)
Q3 = τ3 − F3s sign(q̇3 ) − F3v q̇3
(3.69)
and the third element is
3.7.1
Complete model
Finally, the complete model has the form
D(q)q̈ + C(q, q̇)q̇ + G(q) + F (q̇) = τ
(3.70)
This structure is called inverse dynamic form since it gives a static relation between the dynamic variables q, q̇ and q̈ and the torque τ. This structure will be
used in both identification and control.
24
3
Modelling
It is interesting to be able to simulate the gimbal model. For simulation we
want to know how a torque input affects the motion of the gimbal. This can be
done by writing the model on state-space form. By solving the equation for q̈ we
get
(3.71)
q̈ = D(q)−1 (τ − C(q, q̇)q̇ − G(q) − F (q̇))
and by introducing the state vectors
xθ = q
(3.72)
xω = q̇
(3.73)
the dynamic equations can be written on state-space form as
#
" # "
xω
ẋθ
=
ẋω
D(xθ )−1 τ − C(xθ , xω )xω − G(xθ ) − F (xω )
(3.74)
4
System identification
4.1
Introduction
The physical model derived in Chapter 3 has parameters which depend on the
gimbal and the mounted camera. We want our model to describe the actual system and therefore we must find these parameter values. The accuracy of the
model will largely depend on how accurately we can determine these parameters.
The performance and robustness of model based controllers will also depend on
the quality of the parameters.
One way of determining some of the required parameters is to use a cadmodel of the gimbal to find the centre of gravity and inertia for the gimbal’s
parts. Parameters as friction cannot be found by this method and the inertia of
the camera varies depending on what camera is used. Therefore, we cannot rely
on this method, but it can still be used to find reference values for the gimbalrelated parameters.
Another method is to use system identification to find these parameters. The
basis of system identification is to observe the input/output behaviour and use
this data to estimate models. Estimates of the parameters can be found by minimizing the difference between the model output and the measured output, and
this is called the prediction-error method. System identification has three fundamental aspects that must be considered.
The first aspect is to consider is the structure of the model. This concerns both
the input/output dynamics and also the modelling of disturbances in the system.
When a model structure has been chosen we must consider how to compute estimates for this particular structure. Depending on properties such as linearity
and disturbance modelling different estimation methods must be used.
Finally, we must consider how we acquire and use the data for estimation. For
example, what sensors should be used, and how should the data be preprocessed.
25
26
4
System identification
A major question is how the system should be excited to acquire data with as
much information as possible of the system’s dynamics. This area has been studied extensively for robots and there exist a few books. One such book is Khalil
and Dombre [2002] where an approach is presented for identification of robots. A
good property with the model structure called inverse dynamics form, obtained
in Chapter 3, is the possibility to rewrite the model as a linear regression. Before
the regression model can be used to compute estimated parameters we must find
out which parameters can be identified.
4.2
System identification theory
Here, the theory for general regression models as well as a method for determining the identifiability of regression models will be presented. For a complete
review of regression theory, see Ljung [1987].
4.2.1
Linear regression model
A linear regression is a model structure which is linear with respect to its parameters and can be written
y(t) = ϕ T (t)θ + e(t)
(4.1)
where, in the particular case of an arx model,
h
ϕ(t) = −y(t − 1) . . . y(t − na ) u(t)
...
iT
u(t − nb )
(4.2)
is the regression vector, y is the output measurement, u is the input and θ is the
parameter. The parameter na is the number of poles and nb is the number of
zeros.
4.2.2
Identification methods
The prediction
ŷ (t|θ) = ϕ T (t)θ
(4.3)
of y(t) makes it possible to define the prediction error
(t, θ) = y(t) − ŷ (t|θ)
(4.4)
Finding the θ that results in the smallest possible prediction error is a common
estimation objective. One approach is the least squares method.
Least squares
The prediction error can be used to define an optimization criterion for estimating θ. One such criterion is the sum of squared prediction errors
N
1 X1 2
VN (θ) =
(t, θ)
N
2
t=1
(4.5)
4.2
27
System identification theory
and by minimizing this sum with respect to θ, we get the parameter estimate
θ̂N = arg min VN
θ
(4.6)
We thus have the least squares method. The criterion in (4.6) can be minimized
analytically by substituting (4.3) and (4.4) into (4.5), which gives
VN (θ) =
N
1 X1
(y(t) − ŷ (t|θ))2
N
2
t=1
N
1 X1
=
y (t)2 − 2θ T ϕ(t)y(t) + θ T ϕ ϕ T θ
N
2
(4.7)
t=1
By identifying
fN =
RN =
N
1 X
ϕ(t)y(t)
N
(4.8)
1
N
(4.9)
t=1
N
X
ϕ(t)ϕ T (t)
t=1
(4.7) can now be written as
VN (θ) =
N
1 X1
−1
T
−1
y(t)2 − fNT R−1
N fN + (θ − RN fN ) RN (θ − RN fN )
N
2
(4.10)
t=1
The first two terms are independent of θ and will not affect the minimum. Since
RN is assumed positive semi-definite, the last term will always be positive and
the minimum is found at
θ̂N = R−1
(4.11)
N fN
The covariance of the estimate is calculated as
PN
−1
N

X
T

= λ0 
ϕ(t)ϕ (t)
(4.12)
t=1
where λ0 is the variance of the noise term e(t). This variance can be estimated
with
N
X
1
λ0 =
(t, θ̂N )2
(4.13)
N − nθ
t=1
where N is the number of samples and nθ is the length of θ.
28
4
System identification
Matrix formulation
The summations can be substituted with matrices for a more compact representation. The measurements can be written as


 y(1) 
 . 
YN =  .. 
(4.14)


y(N )
and the regression matrix as
ΦN
The estimate becomes
 T

 ϕ (1) 


=  ... 
 T

ϕ (N )
θ̂N = (ΦNT ΦN )−1 ΦNT YN
(4.15)
(4.16)
Multivariable case
The multivariable case is obtained by a slight modification of the one-dimensional
case. The measurement is


 y1 (t)


y(t) =  ... 
(4.17)


yn (t)
and the regression matrix is
 T 
ϕ1 (t)


T
ϕ (t) =  ... 
 T 
ϕn (t)
(4.18)
for both the matrix and summation forms. The target function becomes
N
1 X1
VN (θ) =
k(t, θ)k2
N
2
(4.19)
t=1
4.2.3
Identifiability and informative experiments
Identifiability concerns which parameters of a model that can be estimated uniquely. When doing analysis of identifiability it is important to separate between
identifiability of the model structure and the excitation of the system. The input
of the system may not affect some dynamics which makes the associated parameters practically unidentifiable.
To study identifiability, the regressor ϕ is used. For each row j of the regressor,
ϕj , there is an associated parameter θj . By including different sets of parameters
4.2
29
System identification theory
with corresponding regressor rows we can form ΦNT ΦN , which can be used to
determine identifiability by calculating its rank.
By removing all unidentifiable parameters and replacing all linearly dependent parameters with new parameters we can obtain a fully identifiable set, called
base parameters. Note that the parameter introduced to replace a linear combination has no physical meaning. Since the goal often is to achieve a predictive
model the loss of physical interpretation may not be restrictive.
4.2.4
Method for determining identifiable parameters
Finding the identifiable parameters can be done both analytically and numerically. Finding unidentifiable parameters is a simple procedure. By going through
each regressor row ϕj , with respective parameter θj , if all columns of row ϕj
is zero the corresponding parameter θj will not affect the model output and is
therefore not identifiable.
The other parameter types are not as easy to find directly in the regressor.
Finding linear combinations of identifiable parameter and independently identifiable parameters is cumbersome analytically, especially for a large models. Therefore a numerical method is preferred.
Numerical method for determining base parameters
A numerical method is presented in Khalil and Dombre [2002]. By using N random values for the regressor inputs and then placing them in the regressor matrix
(4.15), Φ with size (N , nθ ), θ has size (nθ , 1). The rank, denoted b, of Φ is the
number of identifiable parameters. Φ can be split into two parts, one which contains the independent columns and one with dependent columns. Φp = (Φi , Φd )
where Φi contains the linear independent columns and Φd contains the linear
dependent columns. The corresponding is done for splitting θ into θi and θd .
Since Φi spans Φd we can write
Φd = Φi β
(4.20)
where β has dimension (b, nθ − b) and determines the linear combinations. We
can now express the regressor model as
YN = Φi θbase + e
(4.21)
θbase = θi + βθd
(4.22)
where
is the new parameter vector that will be used. Note that θbase is ambiguous, since
it depends on the order of the columns in Φd and Φi . We now have to determine
Φi , Φd and β. By using a qr factorization of Φ we get
"
#
R(c,c)
Φ = Q(n,n)
(4.23)
0(n−c,c)
30
4
System identification
where Q is an orthogonal matrix and R is an upper triangular matrix. By studying the diagonal of R, the elements equal to zero indicates linearly dependant
columns of Φ, which are put intp Φd . The remaining independent columns are
put into Φi . What is left to determine is β. By defining
h
i
Φp = Φi Φd
(4.24)
and using a qr factorization of
h
Φp = Qi
we have
and from (4.20) we have
"
i R
i
Qd
0
Rd
0
#
(4.25)
Φd = Φi R−1
i Rd
(4.26)
β = R−1
i Rd
(4.27)
For reference, an alternative numerical method for finding base parameters is
found in Gautier [1991]. It uses a svd factorisation instead of a qr factorization. Additionally, a method for doing this analysis analytically can be found in
Gautier and Khalil [1990].
4.2.5
Model structure for identification
In the modelling chapter, two model structures were presented, the inverse dynamics form and state space form. The inverse dynamics form can be written as
a regression model and the state-space model results in a greybox model.
Regression model
Since the physical model (3.70) is linear with respect to its parameters, which
were presented in Table 3.1, it can be formulated as a regression. The regressions
has dynamic variables q, q̇ and q̈ as inputs and torque τ as output. The linearity
property reduces the complexity of the estimation. The regression model has a
simple noise and it is assumed that the noise is white. If this requirement is not
fulfilled there might be a bias in the estimate.
Grey-box model
The grey-box model is based on the state-space model (3.74) which is nonlinear
with respect to its parameters. It has torque τ as input and dynamic variables q,
q̇ and q̈ as outputs. The nonlinearity property makes estimation more difficult
as a more complex and computationally demanding method must be used. One
advantage with grey-box modelling is the capability of handling complex noise
models. The dynamics of the noise model, which may be non-linear, can be added
directly to the state-space model. The resulting model will then have white noise
as input.
4.3
31
Regression model for the gimbal
4.2.6
Validation
One commonly used validation method is to use the model for simulation of
the system response for a particular input signal. If a model can describe the
behaviour of the system well it can be considered being a good model. This is
done by using cross validation. It means that the model is estimated on one
data sequence and then validated against another sequence. Preferably this data
sequence is collected using a different trajectory than the one that is used to estimate the parameters, since this reduces the risk for overmodeling.
It is desired to have a measure of how good a model is for simulation, and one
such measure is called model fit and is defined as
!
kŷ − yk
(4.28)
Mf it = 100 1 −
ky − ȳk
where ȳ is the mean of y. Fit is the percentage of the output that is described by
the model.
4.3
Regression model for the gimbal
The regression approach is well proven in the mentioned literature, it is also less
complicated than the grey-box model approach and its computational requirements seems appropriate for the hardware platform. The approach with regression model structure is therefore used.
The physical model is linear with respect to its parameters so it can be written
on a regression form
τ = ϕ T (q, q̇, q̈) θ
(4.29)
where θ contains all the parameters of the gimbal. τ is the torque from the motors
and ϕ is the regression vector. The parameter vector for body i is
h
θi = Iixx
Iiyy
Iizz
`imx
`imy
`imz
Fis
Fiv
iT
Then θi for each body can be combined to form
 
θ1 
 
θ = θ2 
 
θ3
The regression matrix has the following form

···
ϕ1,1 ϕ1,2 ϕ1,3

···
0
ϕ2,p+1
ϕ T (q, q̇, q̈) =  0
 0
0
···
0
(4.30)
(4.31)
···
···
ϕ3,2p+1
···
···
···

ϕ1,3p 

ϕ2,3p 
ϕ3,3p 
(4.32)
It has three rows, one for each axis of the gimbal.
Since the model of the gimbal is on an inverse form the measurements of
the gimbal becomes inputs to the model and the input of the gimbal becomes
32
4
System identification
the output. The regressor model does not handle input noise and therefore the
measurements of the gimbal must have low noise levels in order to avoid bias in
the estimates. It is therefore important to use accurate sensors and appropriate
preprocessing of the measurements.
4.3.1
Base parameters for the gimbal
By using the theory of identifiability we can find the base parameters of the gimbal. By forming the regressor matrix and doing a quick inspection the unidentifiable parameters can be found by looking for zero columns. This yields that the
parameters that are unidentifiable are
Su = {I1xx , I1zz , `1mx , `1my , `1mz , `2my }
(4.33)
These parameters could easily be found by looking at how the motion of the gimbal is constrained. For instance since the spine of the gimbal has just one axis of
rotation, the inertial parameters for the other two axes will not have any effect on
the gimbal. Let us use the numerical method to find one set of base parameters.
One vector of identifiable parameters is


 I1yy 
 F1s 


 F 
 1v 
 I 
 2xx 


 I2yy 


`2mx 


 `2mz 


θi =  F2s 
 F 
 2v 
 I 
 3xx 
 I 
 3yy 


`3mx 


 `3mz 


 F3s 


F3v
and the matrix of linear combinations is
The vector of unidentifiable parameters is


 I2zz 


θd =  I3zz 


`3my

0
 1
 0
0


0
 0

−1 1

 0
1

 0
0

 0
0


0
β =  0
 0
0

 0 −1


0
 0

0
 0
 0
0


 0
0

0
0

0

0

0

0
0

0

1
0

0
0

0

0
0

0

0
4.4
33
Experiment design
Then, the base parameter vector is
θbase


 I1yy + I2zz 


F1s






F
1v


I2xx − I2zz + I3zz 


 I2yy + I3zz 




`2mx




 `3my + `2mz 



= 
F2s


F2v




 I3xx − I3zz 




I3yy




`3mx




`3mz




F3s


F3v
(4.34)
Note that there are several valid combinations of parameters that can form the
base parameters, since the ordering of the regression matrix columns is not ambiguous.
4.4
Experiment design
An important part of system identification is how the input signal of the system
should be used to excite the system. The excitation is either performed with a
direct input signal or a reference trajectory, through a feedback controller. The
excitation should provide as much information as possible in as short time as possible. It should also adhere to constraints on position, velocity and acceleration
of the gimbal. One approach that has been successful in the field of robotics is to
use optimization to determine reference trajectories so the system is excited for
maximum information. This approach is used in Swevers et al. [2007].
4.4.1
Open loop versus closed loop identification
When designing an identification experiment one has to choose how the system
is operated during the actual experiment. The proposed method for generating
optimized trajectories results in a reference consisting of trajectories for the gimbal. The most straight-forward approach would be to use a simple controller for
making the gimbal follow these trajectories during the experiment, thus resulting
in a closed-loop system. With open loop control the system input is manipulated
directly. This makes it difficult to use the generated trajectories.
34
4.4.2
4
System identification
Optimal trajectories
The goal is to formulate an optimization problem to find suitable trajectories for
identification. A general optimization problem has the form
minimize
ft (x)
subject to
fc (x) ≤ 0
x
(4.35)
We must determine the scalar target function ft and the constraint function fc
and also the optimization parameter x.
The first question to answer is what we should to minimize. The goal of the
whole identification procedure is to find as good model/parameter estimates as
possible. The quality of the estimates can be determined by the covariance matrix
P , assuming there is no bias. The lower the variance, the better the estimate is.
From regression theory we know that the covariance can be calculated from the
regressor matrix only, without any knowledge of the actual parameters. Therefore we can find the covariance of the estimates for an arbitrary trajectory before
the experiment is performed. This means we can form a target function which
can be used to evaluate a trajectory in our optimization problem. Since P is a
matrix and ft must be scalar the covariance cannot be used as an optimization
target directly. A suggested approach is to use a scalar measure of the covariance.
There exist many variants of these measures and two of them will be presented
here.
The first measure, proposed in Khalil and Dombre [2002], uses an svd decomposition of P −1 , resulting in a vector σ containing all singular values. The target
function is then
max(σ )
1
+ λ2
(4.36)
ft,svd = λ1
min(σ )
min(σ )
The parameters λ1 and λ2 are weighing factors to adjust the importance of the
two terms. The first term is the condition number of the covariance matrix. The
condition number measures how well-conditioned a matrix is. The second term
ensures that the matrix is non-singular.
The other target function, proposed in Swevers et al. [2007] is
ft,det = log(det(P ))
(4.37)
ft,det = det(P )
(4.38)
This is essentially the same as
which is called D-optimality [Ljung, 1987] and can be geometrically interpreted
since the determinant can be seen as a measure of the size of the parameter uncertainty region.
The constraints of the optimization problems are physical constraints on orientation, velocity and acceleration of the gimbal. The constraints are important
to have information of the physical limitations included in the optimization. Too
large constraints can cause the gimbal to reach torque saturations and excite unwanted dynamics such as flexibilities.
4.4
35
Experiment design
What is left to determine is the trajectory parametrisation. The trajectory q
can be seen as a function of x, meaning there is some mapping q = Ψ(x) which
defines how the optimization parameters influences the trajectory. A general, extreme case is to let Ψ = x resulting in an infinite dimensional case where x is the
actual trajectory. For obvious reasons this is not a feasible choice of parametrization. A commonly used parametrisation is to use a finite Fourier series. A fourier
series is defined as
qt (t) =
N
X
ak sin(kωt) − bk cos(kωt)
(4.39)
ak kω cos(kωt) + bk kω sin(kωt)
(4.40)
−ak (kω)2 sin(kωt) + bk (kω)2 cos(kωt)
(4.41)
k=1
and its first derivative
q̇t (t) =
N
X
k=1
and second derivative
q̈t (t) =
N
X
k=1
By choosing ω and N we can decide the frequencies of the trajectory. This reduces
the optimization problem to finding constants ak and bk . This results in the
following parameter vector for the optimization
h
i
x = a1 b1 a2 b2 · · · aN bN
(4.42)
By discretizing the trajectories, qt (t), q̇t (t) and q̈t (t), through sampling with the
frequency fs we can form the matrix


 ϕbase (qt (t1 ), q̇t (t1 ), q̈t (t1 )) 


..

Φt = 
.


ϕbase qt (tNd ), q̇t (tNd ), q̈t (tNd )
(4.43)
where tk is the sample times and Nd is the number of samples. The covariance
matrix becomes
−1
P = λ0 ΦtT Φt
(4.44)
Since λ0 does not affect the optimization and we can choose λ0 = 1.
To solve the optimization problem a heuristic search algorithm, called genetic
algorithm, has been used. A genetic algorithm can be used to solve nonconvex optimization problems. It mimics attributes of natural selection and uses a population of individuals. These are then evaluated and new generations of individuals
are generated until the algorithm converges.
36
4.4.3
4
System identification
Optimization of trajectories
Before we can start the optimization we must determine the parameters of the
trajectories, consisting of ω and N . The parameter ω is the base frequency of the
signal and determines the period of the excitation. The parameter N determines
the number of optimization variables, the larger N the more computationally demanding the optimization becomes. The parameter N also determines the highest frequency of the signal which is the product N ω. Here, the period time, T , is
chosen to 10 seconds resulting in f = 0.1. N is chosen to 5, resulting in a highest
frequency component of 0.5 Hz.
The sample frequency, fs , is chosen to 15 Hz. We choose the number of data
points to cover one whole period of the signal, therefore Nd = fs T . A higher
sampling frequency results in more data points and increases computation times.
The constraints of the optimization is the maximum value for angle, velocity and
acceleration of each axis. In order to guarantee that a large angular span is covered a constraint on minimal maximum angle is added. We then have the constraint function as


 max(|q1 |) − c11 
− max(|q1 |) + c12 


 max(|q̇1 |) − c13 


 max(|q̈ |) − c 

1
14 
 max(|q |) − c 

2
21 


− max(|q2 |) + c22 


fc = 
(4.45)
 max(|q̇2 |) − c23 


 max(|q̈2 |) − c24 


 max(|q3 |) − c31 


− max(|q3 |) + c32 


 max(|q̇3 |) − c33 


max(|q̈3 |) − c34
where cij is an element of the matrix C. The idea is to adjust the constraints such
that the trajectory provides as much information as possible of the system while
not exciting unwanted dynamics. During testing it was noted that very rapid
accelerations and de-accelerations resulted in bad data. Therefore the constraints
were tuned to achieve a trajectory that worked well for estimation. The following
constraint matrix was used


π
 π π 3 π − 1


π 
(4.46)
C =  π4 π π3

5 
π
π
π 
π
2
3
3
Now we have established everything needed to run the actual optimization. The
measure ft,svd was first tested. This measure resulted in solutions that never
fully utilized the domain given by the constraints. This could be a result of the
tuning of λ1 and λ2 . When ft,det was tested, it immediately yielded solutions
that were close to the constraints. Since optimization of ft,det was successful it
was considered unnecessary to try to get the tuning right for ft,svd to work. In the
thesis Olsson [2005] the same target functions were evaluated and the conclusion
4.5
37
Identification for the gimbal
was also that ft,svd never utilized the domain given by the constraints. It was also
concluded that estimates identified with ft,det outperformed ft,svd in validation
tests. Therefore we chose to do the optimization with ft,det . The optimized joint
trajectories are presented in Figure 4.1.
Joint trajectories
2.5
q1
q2
q3
2
1.5
Rad
1
0.5
0
−0.5
−1
−1.5
0
1
2
3
4
5
t
6
7
8
9
10
Figure 4.1: Optimized trajectory angles.
4.5
Identification for the gimbal
We have the regression τ = ϕbase (q, q̇, q̈) θbase , where we need to measure or estimate the torque τ, joint angle q, joint velocity q̇ and joint acceleration q̈.
4.5.1
Gimbal measurement equations
The torque measurement is
ytorque (tk ) = τ(tk ) + et (tk )
(4.47)
τ̂(tk ) = ytorque (tk )
(4.48)
and the torque estimate is
where ytorque is the torque reference to the motor controller. Note that this is a
reference signal that uses the actuator model described in Chapter 3.
By assuming that the gimbal base is at rest and aligned horizontally, the joint
angle measurement is
yorient. (tk ) = q(tk ) + eo (tk )
(4.49)
38
4
System identification
and the joint angle estimate is
q̂(tk ) = yorient. (tk )
(4.50)
where yorient. is the orientation of from the ahrs algorithm of the imu.
With the same assumption as for the joint angles, the joint velocity measurement can be determined with the Jacobian as
yvel. (tk ) = J33 (q(t))q̇(tk ) + ev (tk )
(4.51)
and the joint velocity estimate
−1
ˆ k ) = J 3 (q(t))
q̇(t
yvel. (tk )
3
(4.52)
where yvel. is the angular velocity from the gyro of the imu.
Since joint acceleration is not measured (the accelerometer measures lateral
accelerations) we must estimate the joint acceleration. We do this by differentiating the joint velocity. The most simple differentiation approximation is the
Euler backward method. This results in the following expression for the joint
acceleration
1
(q̇(tk ) − q̇(tk−1 ))
q̈(tk ) =
(4.53)
Ts
where Ts is the sample period.
4.5.2
Gimbal measurement filtering
Since we have chosen the regression model type, only the regression output noise
is modelled. It is therefore important to remove as much noise as possible from
the joint measurements through filtering to avoid bias in the estimates. Since the
filtering should be possible to do online a simple approach with a discrete time
Butterworth filter might suffice.
If the Butterworth filter is not good enough a more advanced filter can be
used. A more advanced filter is the Savitzky-Golay filter [Savitzky and Golay,
1964] which uses polynomials to smooth data with little distortion.
One approach that would eliminate the need for filtering is to add these noises
to the model. This would lead to a more complex errors-in-variables grey-box
model which could not be treated as a regression.
The filtering of the joint measurements results in qf ilt. (tk ), q̇f ilt. (tk ) and
q̈f ilt. (tk ).
4.5.3
Gimbal predictor
We can now form the prediction error as
(tk , θbase ) = ytorque (tk ) − ϕbase qf ilt. (tk ), q̇f ilt. (tk ), q̈f ilt. (tk ) θbase
With the theory of least squares estimation, θbase can be estimated.
(4.54)
5
Control design
5.1
Introduction
The goal of a control system is to manipulate the behaviour of a system to achieve
a certain goal. The manipulation is achieved in the form of a controller that
uses measurements from the system and external references to compute a control
signal. For the gimbal we want a controller that can hold the camera completely
still during motion disturbances. The orientation of the gimbal is manipulated
through reference signals to the controller.
The currently used control system for the gimbal is a decentralized pid controller. This means that each axis is controlled independently from the other axes
and the coupling effects between axes are treated as disturbances. Furthermore,
the pid controller is not based on a model, but instead relies on tuning parameters. When the pid controller is properly tuned it is often possible to achieve
good performance, but the tuning may be time consuming. This is especially a
problem for a camera gimbal which can be used with different types of cameras.
By using a model-based, multi-variable control method a centralized controller can be determined. The model that was derived in Chapter 3 and identified in Chapter 4 will here be used as a foundation for the control system.
5.1.1
Model based control
During modelling some assumptions and simplifications were made for the model. Since these omitted effects will not be predicted by the model it is important
to study the controller’s robustness properties. There exist a handful of methods
that may be suitable for gimbal control. These will be discussed to find which
one is the most suitable. Since the gimbal is a nonlinear system there exist two
approaches. The first is to linearise the model and use a linear control design
method, the other is to use a nonlinear method.
39
40
5
Control design
The model can be linearised by performing a first order Taylor expansion. The
linearisation is an approximation of the system in the proximity of the linearisation point. The linear model can be used to design a linear control system. To
linearise the gimbal several linearisation points must be used. Controllers designed for each linearisation point can be combined using gain scheduling.
Passivity-based control is a non-linear method and takes advantage of the passivity of the system. It does not depend on exact cancellation of non-linearities
and is therefore more robust to model errors. Passivity-based control is described
in Spong and Hutchinson [2005].
Sliding mode control is also a non-linear method and relies on forcing the
system into a sliding manifold. When this manifold is reached the controller
handles uncertainties and disturbances with good performance. For a complete
review of sliding mode control, see Khalil [2002].
Feedback linearisation is a non-linear method which uses the model to compensate for non-linear and coupling effects in the system. The feedback linearisation uses the state variables of the system as measurements and results in a new,
linear and decoupled, system with a new input. This new system is identical
for all axes and all camera configurations and is controlled by an outer loop controller. Since the outer loop controller controls a linear and decoupled system a
linear control design method may be used. The compensated system will also be
independent of the camera setup, therefore the outer loop can be kept the same.
This is a clear advantage over the other discussed control methods. This method
is evaluated in Sangveraphunsiri and Malithong [2009] for a two axis gimbal with
promising results.
One complication with model-based control is how measurements and reference signals are handled. The reference direction for the camera and its position
and velocity are given in its own coordinate frame. The model-based controller
needs measurements and references in its joint space. Therefore the kinematics
relations derived earlier must be used to transform measurements and references
for the camera into the corresponding joint space measurements and references.
5.1.2
Proposed controller
Of the discussed control methods, feedback linearisation is chosen to design the
gimbal controller. The use of feedback linearisation requires an outer loop controller to control the feedback-linearised system. A common controller is the pid
controller. It is not necessarily model based, and instead relies on tuning parameters, often tuned manually by a trial and error approach. For its simplicity we
choose the pid as our outer loop controller.
The feedback linearisation is dependant on the quality of the model that is
used. Due to errors in parameters and effects omitted from the model, there
will be a certain error in the feedback linearisation. It is therefore important to
guarantee the robustness of the closed loop system. Therefore a controller which
guarantees robustness is introduced in the outer loop.
To summarize, the structure of the control system can be seen in Figure 5.1.
5.2
41
Control theory
q d q̇ d q̈ d
+
ėq
Outer loop controller
Inner nonlinear compensation loop
Robust controller
+
−
+
+
eq
v
Inverse dynamics
u
q
Gimbal
+
q̇
pid controller
−
Figure 5.1: An overview of the proposed control structure. The inner loop,
which is the same as the feedback linearisation controller, uses the inverse
dynamics model to compensate for nonlinearities in the gimbal and results
in a linear and decoupled system. The outer loop consists of a traditional pid
controller together with a robust controller which ensures the controller’s
robustness to model errors. q d , q̇ d and q̈ d are reference inputs to the outer
loop controller.
5.2
5.2.1
Control theory
Feedback linearisation
The theory of feedback linearisation is presented in Glad and Ljung [2003]. A
more complete presentation can be found in Khalil [2002]. The principle of exact
linearisation is to use the nonlinear model to cancel out all nonlinearities and
coupling effects between signals to obtain a linear and decoupled system. We
consider non-linear systems on state-space form that are affine in the control
signal and can be written
ẋ = f (x) + g(x)u
(5.1)
y = h(x)
The goal is to obtain a feedback controller
u = α(x) + β(x)v
(5.2)
together with the variable transformation
z = T (x)
(5.3)
that results in a linear system without coupling on the form
ż = Az + Bv
This is illustrated with the example below.
(5.4)
42
5
Control design
5.1 Example
Consider the system
ẋ1 = a sin(x2 )
(5.5)
−x12
(5.6)
ẋ2 =
+u
By making the variable change
z1 = x1
(5.7)
z2 = a sin(x2 )
(5.8)
and applying the control
u = x12 +
1
v
a cos(x2 )
(5.9)
the result is a double integrator
ż1 = z2
(5.10)
ż2 = v
(5.11)
Input-output linearisation
A common property of the system (5.1) is that y does not depend on u explicitly.
To find the relation between u and y we must differentiate y until u appears.
Differentiation once gives
ẏ = hx (x)ẋ = hx (x)(f (x) + g(x)u)
(5.12)
and if hx g 6= 0 we have found a relation between y and u. On the other hand,
assume this is not case and hx g = 0. Then we can continue to differentiate ẏ until
u appears. To simplify notation of these repeated derivations we introduce the
Lie derivative. The Lie derivative of h with respect to f is written Lf h(x) where
L f = f1
∂
∂
+ ... + fn
∂x1
∂xn
(5.13)
The relative degree of a nonlinear system can be defined using Lie derivatives
[Khalil, 2002].
5.2 Definition. The relative degree v is the smallest integer v such as
Lg Lv−1
f h 6= 0
(5.14)
Furthermore, if the above statement holds for all x it is called strong relative
degree.
5.2
43
Control theory
Let us assume we have a system with strong relative degree v yielding
y (v) = Lvf h + uLg Lv−1
f h.
(5.15)
We can then use the feedback
u=
1
(r − Lvf h)
Lg Lv−1
h
f
(5.16)
The resulting linear system is
y (v) = r
(5.17)
Let us express this system in new coordinates with the transform z = T (x) where


 
 T1   h 
 ..   .. 
 .   . 


 T  Lv−1 h
 v   f

(5.18)

=
Tv+1   φ1 

 

 ..   . 
 .   .. 

 

Tn
φn−v
where Tv+1 up to Tn is arbitrary. This results in the system
ż1 = z2
..
.
żv = ξ(z) + uη(z)
żv+1 = Φ1 (z, u)
..
.
żn = Φn−v (z, u)
where Φ1 (z, u) to Φn−v (z, u) are nonlinear functions. Therefore we have nonlinear
dynamics in the system which is not observable in the output. This is called zero
dynamics and may be problematic if it is unstable.
When the number of states n is equal to the relative degree it can easily be
seen that there is no zero dynamics. Therefore it is desirable to have a relative
degree that is equal to the number of states.
5.2.2
Lyapunov theory
The theory presented here, as well as proofs for the theorems, can be found in
Khalil [2002]. Lyapunov theory gives us a method that can be used to analyse
stability of systems. Note that the conditions given by Lyapunov theory are sufficient and not necessary. Consider the system
ẋ = f (x)
(5.19)
44
5
Control design
An equilibrium point of this system is a point x̄ for which f (x̄) = 0. For convenience we assume x̄ = 0. We can now define the stability of an equilibrium point
as in the following definition.
5.3 Definition. An equilibrium point x̄ = 0 is
• stable, if for each > 0 exists δ() > 0 such that
kx(0)k < δ ⇒ kx(t)k < ∀t ≥ 0
(5.20)
• unstable if it is not stable
• asymptotically stable if it is stable and δ can be such that
kx(0)k < δ ⇒ lim x(t) = 0
t→∞
(5.21)
The following theorem is called Lyapunov’s stability theorem.
5.4 Theorem. Let x̄ = 0 be an equilibrium point for (5.19) and D ⊂ Rn be a domain containing x̄ = 0. Let V : D → R be a continuously differentiable function
such that
V (0) = 0 and
V (x) > 0 in D - {0}
V̇ (x) ≤ 0 in D
(5.22)
(5.23)
Then x̄ = 0 is stable. Furthermore if
V̇ (x) < 0 in D - {0}
(5.24)
then x̄ = 0 is asymptotically stable.
A continuously differentiable function V (x) that satisfies (5.22) and (5.23) is
called a Lyapunov function. If V (x) satisfies (5.22) it is said to be positive definite.
If V̇ (x) satisfies (5.23) it is said to be negative semidefinite.
What makes this theorem useful is that stability can be determined without
actually solving (5.19). The challenge with this method is to find the Lyapunov
function. Note also that we require the initial state to be bounded for asymptotic
stability. The theorem below, known as Barbashin-Krasovskii theorem, expands
the result of the previous theorem to be valid globally.
5.5 Theorem. Let x̄ = 0 be an equilibrium point for (5.19) and let V : Rn → R
be a continuously differentiable function such that
V (0) = 0 and
V (x) > 0 ∀x 6= 0
kxk → ∞ ⇒ V (x) → ∞
V̇ (x) < 0 ∀x 6= 0
(5.25)
(5.26)
(5.27)
Then x̄ = 0 is globally asymptotically stable.
A function that satisfies (5.26) is said to be radially unbounded and expands
the local stability result into a global result.
5.3
5.3
45
Feedback linearisation of the gimbal
Feedback linearisation of the gimbal
The feedback linearisation controller can be obtained directly from the inverse
dynamic form of the model, without using any theory from feedback linearisation. Let us verify this by considering the state space model of the gimbal
" # "
#
xω
ẋθ
(5.28)
=
ẋω
D(xθ )−1 τ − C(xθ , xω )xω − G(xθ ) − F (xω )
Choosing
τ = D(xθ )v + C(xθ , xω )xω + G(xθ ) + F (xω )
(5.29)
ẋθ = xω
ẋω = v
(5.30)
yields the system
(5.31)
which is a double integrator, for each gimbal axis. We can see that (5.29) is the inverse dynamics form (3.70) with ẋω = v. To study the presence of zero dynamics
we can assume xθ is our output. By differentiating our output twice we have
ẍθ = D(xθ )−1 τ − C(xθ , xω )xω − G(xθ ) − F (xω )
(5.32)
and τ has appeared. Since D(xθ )−1 is positive definite [Siciliano et al., 2009, Ch.
7] we have strong relative degree v = 2. Since we have two states per axis there is
no zero dynamics.
This controller type results in an inner loop that makes the gimbal linear. This
makes it possible to use a linear method to control the new system given by (5.34).
Note that the physical system does not become fully linear when this controller
is applied since there is a torque saturation in the motors which is not included
in the model. If the motor saturation levels are known, we can use the model to
determine the limits on the outer loop control signal v.
Let us see how the feedback linearised controller can be obtained directly
from the inverse dynamics form (3.70)
D(q)q̈ + C(q, q̇)q̇ + G(q) + F (q̇) = τ
(5.33)
q̈ = v
(5.34)
and by replacing
we get the controller in (5.29), expressed in joint variables q, q̇ and q̈. To simplify
the expression for the controller we can define
N (q, q̇) = C(q, q̇)q̇ + G(q) + F (q̇)
(5.35)
τ = D(q)v + N (q, q̇)
(5.36)
and get
46
5.4
5
Control design
Outer loop control
The outer loop controls the feedback linearised system through v. This system is
decoupled and linear and therefore a decentralized and linear controller can be
used. The inner system that the outer loop controls is always the same regardless
of camera, if model errors are disregarded. This means that the outer loop is designed independently of what camera is mounted in the gimbal. A first approach
for control of the outer loop is to use a pid controller.
The outer loop is also responsible for the robustness properties of the closed
loop system. Therefore, in addition to the pid controller a robust controller is
added to the outer loop.
5.4.1
PID
controller
We can write a pid controller with feed-forward of the reference acceleration as
d
d
Zt
d
v = q̈ + Kp (q − q) + Kd (q̇ − q̇) + Ki
(qd − q)dt
(5.37)
0
where Kp , Kd and Ki are diagonal matrices on the form Kx = diag{kx1 , ..., kxn }.
Let us define the error
eq = qd − q
(5.38)
Then (5.37) together with (5.34) can be written as a homogenous third order differential equation
Zt
ëq + Kp eq + Kd ėq + Ki eq dt = 0
(5.39)
0
If Ki = 0 we have a second order system. A general scalar second order system
can be written on canonical form as
ë + 2ζω0 ė + ω02 e = 0
(5.40)
where ω0 represents the time scaling of the system and ζ is the relative damping of the system. A value ζ < 1 results in an under-damped system and it will
therefore be oscillative. Similarly, ζ > 1 results in an over-damped system, which
essentially means that the damping restricts the tracking performance of the system. If ζ = 1 we have a critically damped system, and it results in error dynamics
free of oscillations.
The relations between the scalar pid tuning parameters and the canonical
form parameters are
q
Kp
(5.41)
K
ζ = pd
2 Kp
(5.42)
ω0 =
and
5.4
47
Outer loop control
To obtain a critical damping we get the following condition between Kp and Kd
q
Kd = 2 Kp
(5.43)
This simplifies the tuning of the outer loop.
5.4.2
Uncertainty analysis of feedback linearisation
The performance of feedback linearisation is highly dependant on model accuracy. Therefore we must study how model errors affect our control system. To do
so we must analyse the uncertainty of the feedback linearisation. Siciliano et al.
[2009] suggest the following method to analyse the uncertainty of the feedback
linearisation. Let us consider the feedback linearisation controller
τ = D̂(q)v + N̂ (q, q̇)
(5.44)
where D̂(q) and N̂ (q, q̇) denote the nominal model. This indicates that the exact
model of the system cannot be determined practically, mainly due to unmodelled
dynamics and errors in model parameters. We can now form the error model as
D̃ = D̂ − D
(5.45)
Ñ = N̂ − N
(5.46)
Let us see what happens when (5.44) is used as input to our system (3.70), with
the compact representation (5.35). In this case, we get
D(q)q̈ + N (q, q̇) = D̂(q)v + N̂ (q, q̇)
⇒ q̈ = D(q)−1 D̂(q)v + N̂ (q, q̇) − N (q, q̇)
⇒ q̈ = v − D(q)−1 D̃(q)v + Ñ (q, q̇)
(5.48)
q̈ = v − Γ
(5.50)
Then, the result is
with the nonlinear deviation from the linear system as
Γ = D(q)−1 D̃(q)v + Ñ (q, q̇)
(5.47)
(5.49)
(5.51)
The term Γ contains information regarding the error introduced by feedback linearisation. We can use this to design a robust controller that can handle model
errors.
5.4.3
Robust controller
To design the robust controller we are interested in the error dynamics of the
system. The error dynamics of this system can obtained by rewriting (5.50) as
q̈d − q̈ = q̈d − v + Γ
| {z }
ëq
(5.52)
48
5
Control design
Let us write the system on state-space form by introducing the state variables
" #
eq
ξ=
(5.53)
ėq
Then (5.52) is
ξ̇ = Aξ + B q̈d − v + Γ
where
"
0
A=
0
I
0
#
and
(5.54)
" #
0
B=
I
(5.55)
Note that Γ depends on q, but not explicitly on ξ.
Let us consider the pid controller with acceleration feed-forward.
Zt
d
v = q̈ + Kp eq + Kd ėq + Ki
eq dt + w
(5.56)
0
The variable w will be designed to guarantee robustness. This controller can be
added to the state-space model by augmenting the state vector as
 
eq 
 
ξ̃ = ėq 
(5.57)
 
ζ
where ζ is the integrator’s state. By inserting (5.56) in (5.54) we get the new
state-space model
ξ̃˙ = Ãξ̃ + B̃ (Γ − w)
(5.58)
where

 0

à = −Kp

I
I
−Kd
0

0 
−Ki 

0
and
 
0
 
B̃ =  I 
 
0
(5.59)
This system describes the error dynamics of the closed loop system. We want to
guarantee that this error always converges to zero, independent of the error term
Γ. By using Lyapunov’s second method in Spong and Hutchinson [2005] (also
known as Lyapunov redesign in Khalil [2002] ) we can design a controller that
gives a signal w that guarantees error convergence. Let us consider the Lyapunov
function
V (ξ̃) = ξ̃ T Q ξ̃ > 0 ∀ξ̃ 6= 0
(5.60)
which is radially unbounded and Q is a positive definite matrix. By differentiating V we get
T
V̇ (ξ̃) = ξ̃˙ Q ξ̃ + ξ̃ T Q ξ̃˙
T
= Ãξ̃ + B̃ (Γ − w) Q ξ̃ + ξ̃ T Q Ãξ̃ + B̃ (Γ − w)
= ξ̃ T ÃT Q + Q Ã ξ̃ + 2ξ̃ T Q B̃ (Γ − w)
(5.61)
5.4
49
Outer loop control
Let us consider the Lyapunov equation
ÃT Q + Q Ã = −P
(5.62)
if P is a positive definite matrix and à has negative eigenvalues then Q will be
positive definite as well. Therefore we can use the Lyapunov equation to obtain
V̇ (ξ̃) = −ξ̃ T P ξ̃ + 2ξ̃ T Q B̃ (Γ − w)
(5.63)
Since P is positive definite we have
− ξ̃ T P ξ̃ < 0
(5.64)
and therefore the Lyapunov condition will always be fulfilled for that term. By
defining a new variable
z = B̃ T Q ξ̃
(5.65)
the second term of (5.63) can be written as
2z T (Γ − w)
A choice of w is
w=
ρ
z
kzk
(5.66)
ρ>0
(5.67)
where ρ is an arbitrary function. By using Cauchy-Schwarz inequality this results
in
ρ
zT Γ −
z T z = z T Γ − ρkzk≤ kzk kΓk−ρkzk= kzk(kΓk−ρ)
(5.68)
kzk |{z}
kzk2
If we can choose ρ such that ρ ≥ kΓk it is assured that V̇ < 0 is fulfilled.
Next we must assure that such a ρ can be found. By making the following
assumptions we can determine that kΓk is bounded. Note that these assumptions
do not depend on the choice of vector norm and matrix norm.
5.6 Assumption. The desired acceleration is bounded
supkq̈d k< Qm < ∞
∀q̈d
(5.69)
t>0
This assumption is reasonable since it is always important to control the gimbal smoothly. For instance, it is never desired to have a step in gimbal position.
5.7 Assumption. The following matrix norm is bounded
kD(q)−1 D̃(q)k = α < 1
for all bounded inertias and all joint angles q.
(5.70)
50
5
Control design
It can be determined that kD(q)−1 k can be bounded as
0 < Dm ≤ kD(q)−1 k≤ DM < ∞
(5.71)
where Dm and DM are lower and upper bounds respectively. The existence of the
lower bound is motivated by the fact the that D(q) has limited inertias and that
the gimbal only has rotational joints and therefore terms depending on q will be
bounded by trigonometrical functions. The upper bound is motivated by the fact
that D(q) is positive definite, which we know from Chapter 3.
Furthermore, a choice for D̂ always exists such that
kD(q)−1 D̃(q)k≤ α ≤ 1 ∀q
(5.72)
is satisfied. Let us examine the choice of nominal model as
D̂ =
2
I
Dm + DM
(5.73)
By using the reverse triangle inequality together with Cauchy-Schwarz inequality
kD(q)−1 D̃(q)k= kD(q)−1 D̂(q) − Ik≤
2
D − Dm
≤ DM
−1= M
=α<1
Dm + DM
DM + Dm
(5.74)
(5.75)
This is not a proof but it shows that a rough approximation still fulfils the
condition.
5.8 Assumption. The matrix Ñ can be bounded by
kÑ (q, q̇)k≤ Φ < ∞
∀q, q̇
(5.76)
Since the gimbal only has rotational joints, terms depending on q will be
bounded by trigonometrical functions. The derivative q̇ will have direct influence, but it can be safely assumed that the velocities are bounded for all physically possible situations.
Let us go back to (5.51). By overestimating the norm of Γ with the CauchySchwarz inequality and the triangle inequality we have
kΓk≤ kD(q)−1 D̃(q)k kq̈d k+kKk kξ̃k+kwk + kD(q)−1 k kÑ (q, q̇)q̇k
(5.77)
where

Kp

K =  0

0
0
Kd
0

0 

0 

Ki
(5.78)
By using the results of the assumptions, (5.69), (5.70), (5.71), (5.76) and kwk= ρ
we have
kΓk≤ α QM + kKk kξ̃k+ρ + DM Φ
(5.79)
5.5
51
Controller tuning
Next we want to find ρ, by defining
ρ=
1 (αQM + αkKk kξ̃k+DM Φ
1−α
(5.80)
It follows that ρ > kΓk is fulfilled. Unfortunately, the parameters of this choice of
ρ are difficult to determine. By defining
Kρ1 =
αQM + DM Φ
1−α
(5.81)
αkKk
1−α
(5.82)
and
Kρ2 =
the following is obtained
ρ = Kρ1 kξ̃k+Kρ2
(5.83)
With this ρ, affinity in kξ̃k is preserved. The new parameters Kρ1 and Kρ2 are
used as tuning parameters for the controller.
So far we have concluded that a specific choice of w guarantees error convergence towards zero. One problem with this controller is that it may cause
high-frequent vibrations in system. These vibrations are called chattering and
are caused by the discontinuity in the subspace z = 0. One approach to reduce
chattering is to approximate the controller with a continuous variant as
 ρ


 kzk z kzk≥ wc = 
(5.84)

ρz
kzk< by increasing the chattering is reduced.
5.5
Controller tuning
The outer loop controller has parameters that must be manually tuned. Since the
robust controller depends on the tuning of the pid controller we start with tuning
of the latter.
Since all axes of the feedback linearised system are identical they can have the
same tuning. By using the condition for critical damping we only need to find
Kp , which determines how fast the closed loop systems converges towards the
reference trajectories. For practical reasons such as time delays and unmodelled
dynamics, Kp must be limited so it does not cause oscillations in the system. By
using the condition for critical damping (5.43), we get


0 
25 0


Kp =  0 25 0  ,


0
0 25

10

Kd =  0

0
0
10
0

0 
0  ,

10

0

Ki = 0

0
0
0
0

0
0

0
52
5
Control design
Now when the tuning of the pid controllers is determined we know the closed
loop system dynamics from (5.59) and therefore the robust controller can be synthesised. The robust controller depends on the parameters P , Kρ1 , Kρ2 and . By
chosing P as a unit matrix, which is positive definite, we can use (5.62) to obtain

0
0
0.0200
1.5000
 0
1.5000
0
0


 0
0
1.5000
0
Q = 
0
0
0.0520
0.0200

0.0200
0
0
 0

0
0
0.0200
0
0
0.0200
0
0
0.0520
0

0 
0 

0.0200

0 

0 

0.0520
(5.85)
The tuning parameters in ρ were selected as Kρ1 = 0 and Kρ2 = 5. However,
the suitability of these parameter choices from a robustness perspective has not
been investigated due to time limitations. The last parameter is tuned to make
chattering indiscernible, which occurred at = 0.004.
6
Experimental results
6.1
Introduction
In this chapter the experimental results for both identification and control are
presented. The experiment was carried out with two test cases, where one is a
Red Epic film camera. The other is a stack of metal sheets, screwed together.
They have similar weight but different inertial properties.
6.2
Experimental setup
For testing purposes, the gimbal is mounted rigidly to a wooden plank which
is fasten between two tables. This allows for testing of both identification and
control. Due to time limitations, the control system is never tested with the head
of the gimbal being free. However, since the main purpose is to test the modelbased control approach this is not restrictive. The experiment setup can be seen
in Figure 6.1.
6.3
Identification results
To perform identification, data for estimation and validation is needed. The estimation data was collected under closed loop using the trajectories in Section 4.4.3.
A cascaded pid controller for each axis was used. The outer controller uses axes
angle references. The inner controller uses an angular velocity reference to generate the output torque. The parameters of the controllers are presented in Tables
6.1 and 6.2.
The estimation data was collected at 125 Hz, during one minute, and was
repeated three times for each test case.
53
54
6
Experimental results
Figure 6.1: The gimbal mounted in the test rig with the payload of Case one.
To obtain validation data, the gimbal was controlled manually under closed
loop with the same pid controller as for the estimation data with a radio controller. To obtain suitable data for cross validation, the gimbal was operated in a
different manner compared to the optimized trajectories.
6.3.1
Raw measurements
In Section 4.5 it was determined which signals that are needed for identification.
All measurements are presented in the xyz coordinate system of the imu, where
x is aligned with axis 2, y is aligned with axis 3, z is aligned with axis 1. Here,
one period of the gimbal measurements from the first data set of Case one is
presented. In Figure 6.2 the torque input is shown. The orientation measurement
from the imu is shown in Figure 6.3. Finally, in Figure 6.4 the angular velocities
are shown.
6.3
55
Identification results
Table 6.1: Outer loop pid controller parameters used for identification experiments.
Kp Ki Kd
pid axis
Axis 1
8
0
0
Axis 2
8
0
0
Axis 3
8
0
0
Table 6.2: Inner loop pid controller parameters used for identification experiments.
Kp Ki
Kd
pid axis
Axis 1
1.8 0.001 0.1
Axis 2
2.0 0.01
0
Axis 3
1.5 0.02
0.1
Motor torque
0.4
x
y
z
0.3
0.2
Nm
0.1
0
−0.1
−0.2
−0.3
0
2
4
6
8
10
Time (s)
Figure 6.2: One period of motor torque data from Case one.
56
6
Experimental results
Orientation
3
x
y
z
2.5
2
Rad
1.5
1
0.5
0
−0.5
−1
−1.5
0
2
4
6
8
10
Time (s)
Figure 6.3: One period of the imu zxy Euler angle measurements of Case
one.
Angular velocity
1.5
x
y
z
1
Rad/s
0.5
0
−0.5
−1
−1.5
0
2
4
6
8
10
Time (s)
Figure 6.4: One period of the angular velocity measurements from the imu
of Case one.
6.3
57
Identification results
6.3.2
Data processing
From Section 4.5 we know how to convert our measurements to the joint variables, and how to filter them. Note that the measurement equation transforms
from xyz axes of the imu to joint axes 1,2 and 3.
The orientation measurement can be converted directly to joint measurements.
Since the ahrs algorithm output is already filtered, no additional filtering of the
joint angle measurements is needed.
The joint velocities are obtained from the measurement equation presented
in Section 4.5. The velocity measurement is then filtered with a Savitsky-Golay
filter. The level of smoothing is controlled by the polynomial degree. The lower
polynomial degree, the more smoothed the data becomes. We choose to filter the
velocity measurement with a filter of polynomial order of 75. In Figure 6.5 the filtered velocity measurement is presented. By using the differentiation expression
(4.53), we obtain the the angular joint acceleration. To reduce noise it has been
filtered with a Savitsky-Golay filter with a polynomial order of 75. In Figure 6.6
the filtered angular acceleration measurements are presented.
Joint velocity
1.5
axis 1
axis 2
axis 3
1
Rad/s
0.5
0
−0.5
−1
0
2
4
6
8
10
Time (s)
Figure 6.5: One period of the filtered joint velocity measurements.
58
6
Experimental results
Joint acceleration
2.5
axis 1
axis 2
axis 3
2
1.5
Rad/s
2
1
0.5
0
−0.5
−1
−1.5
−2
0
2
4
6
8
10
Time (s)
Figure 6.6: One period of the filtered acceleration measurements.
6.3
59
Identification results
6.3.3
Estimated parameters
By using the least squares method presented in Chapter 4 and the predictor
(4.54) on the processed data the estimation is performed. The least squares procedure results in the parameter estimate θ̂. The standard deviation σ̂ , is obtained
through the covariance (4.12). The estimation results for Case one and Case two
can be seen in Tables 6.3 and 6.4.
Table 6.3: The estimated parameters and standard deviation of Case one.
Case one
Parameter
I1yy + I2zz
F1s
F1v
I2xx − I2zz + I3zz
I2yy + I3zz
`2mx
`3my + `2mz
F2s
F2v
I3xx − I3zz
I3yy
`3mx
`3mz
F3s
F3v
Dataset 1
θ̂
σ̂
0.13034
0.00207
0.05323
0.00061
0.10300
0.00134
0.04839
0.00223
0.04860
0.00050
-0.17279 0.00128
0.03119
0.00032
0.00301
0.00059
0.09334
0.00122
-0.02653 0.00047
0.03599
0.00037
0.01534
0.00041
-0.04585 0.00062
0.00872
0.00065
0.05642
0.00095
Dataset 2
θ̂
σ̂
0.14324
0.00258
0.05080
0.00071
0.10915
0.00157
0.02578
0.00275
0.05126
0.00060
-0.17711 0.00154
0.02775
0.00038
0.00297
0.00070
0.09496
0.00146
-0.03123 0.00056
0.03474
0.00045
0.01392
0.00049
-0.04882 0.00074
0.00866
0.00077
0.05706
0.00114
Dataset 3
θ̂
σ̂
0.12393
0.00278
0.03822
0.00079
0.12466
0.00174
0.04699
0.00299
0.05368
0.00069
-0.18466 0.00175
0.03267
0.00043
0.00375
0.00080
0.09192
0.00168
-0.03314 0.00063
0.03418
0.00050
0.01483
0.00056
-0.04544 0.00084
0.01146
0.00089
0.05387
0.00130
Table 6.4: The estimated parameters and standard deviation of Case two.
Case two
Parameter
I1yy + I2zz
F1s
F1v
I2xx − I2zz + I3zz
I2yy + I3zz
`2mx
`3my + `2mz
F2s
F2v
I3xx − I3zz
I3yy
`3mx
`3mz
F3s
F3v
Dataset 1
θ̂
σ̂
0.14036
0.00273
0.05749
0.00072
0.10214
0.00163
0.00532
0.00290
0.01635
0.00062
-0.12214 0.00154
0.11296
0.00038
0.00295
0.00069
0.06966
0.00146
0.01150
0.00055
0.00543
0.00046
-0.05163 0.00049
-0.06806 0.00075
0.00969
0.00077
0.05046
0.00119
Dataset 2
θ̂
σ̂
0.13876
0.00243
0.05480
0.00067
0.09848
0.00150
0.01342
0.00259
0.01817
0.00059
-0.12627 0.00144
0.11309
0.00036
0.00410
0.00066
0.06657
0.00138
0.00871
0.00052
0.00586
0.00043
-0.05404 0.00046
-0.06642 0.00071
0.01140
0.00072
0.04830
0.00110
Dataset 3
θ̂
σ̂
0.13814
0.00192
0.05522
0.00056
0.10094
0.00123
0.00472
0.00206
0.01972
0.00049
-0.13422 0.00120
0.11584
0.00030
0.00411
0.00055
0.06740
0.00115
0.00837
0.00044
0.00666
0.00035
-0.05390 0.00038
-0.06463 0.00058
0.01242
0.00061
0.04705
0.00092
60
6.3.4
6
Experimental results
Validation
The goal of the validation is to determine how good the estimated model is. Some
of the parameters have a physical interpretation that we can use to validate the
result. When using the least squares method the parameter variance can be estimated. This gives us a quality measure of the model.
The most important validation is to test the model’s predictive performance.
In this case we want to use our measurements and estimates of angle, angular velocity and angular acceleration to compute the resulting torque with the inverse
dynamics. This is called predictive validation. Another validation method is to
simulate the model with the model input. To use simulation, the inverse dynamics model is written on state space form. By using the estimated parameters and
the system inputs, the resulting trajectories can be simulated and then be compared with the measured trajectories. Since the control method that is used relies
on the inverse dynamics, it is interesting to determine how well this model structure can predict the torque. We therefore choose to use predictive validation.
A new dataset is used, where data is collected with different trajectories. This
reduces the possibility that the model is conformed to the precise data. The estimation data was collected with the optimized trajectories. For validation, the
gimbal has been controlled manually in a more or less random pattern. Since the
trajectory for the estimation and the trajectory for validation differ substantially
we have good data to evaluate the model.
Physical validation
Since many of the model’s parameters have a physical meaning a first step to
validate the result is to check their reasonableness. The sign of a single parameter
is good start to see if the model is reasonable. The inertia terms must by definition
be positive, and as can be seen in Table 6.3, all inertial parameters and sums of
inertial parameters are positive. The difference I3xx − I3zz is negative which is
reasonable considering the shape of the payload. All friction related parameters
should be positive for a physical model and as can be seen in Table 6.3, all are
positive.
The parameters can also be validated by comparing parameters in terms of
quantity and relate this to the structure of the gimbal and the camera. Some
parameters should be the same, regardless what is mounted in the gimbal. Therefore, these parameters can be evaluated by comparing the two data sets.
There is a cad model of the gimbal which has most of the components in
the gimbal modelled. The cad program can calculate the inertial parameters
and centre of mass for the gimbal which can then be used to validate the results.
Unfortunately, this was never done due to time limitations.
6.3
61
Identification results
Predictive validation of Case one
To test the predictive ability the validation data is used. A combined estimate is
made by taking the mean of the three estimates of each case and the result for all
axes are presented in the Figures 6.7, 6.8 and 6.9. There are two problematic areas
in this model. One is at near stationary conditions, at low and almost constant
torque. This can be seen in Figure 6.8, around 15 up to 23 seconds. The other
is at the high frequency region, where the torque spikes are. Apart from these
regions the model performs well.
Predictive validation of Case two
By combining the three estimates with a mean, as for Case one, we get the resuling cross validation results in Figures 6.10, 6.11 and 6.12. As for Case one, we
have the same problematic areas at near constant torque and at high frequencies
as were noticed for Case one.
Axis 1
1.5
predicted
measured
1
Nm
0.5
0
−0.5
−1
−1.5
0
5
10
15
20
Time (s)
25
30
35
Figure 6.7: Predicted and measured torque on axis 1 for Case one.
62
6
Experimental results
Axis 2
0.3
predicted
measured
0.2
0.1
Nm
0
−0.1
−0.2
−0.3
−0.4
0
5
10
15
20
Time (s)
25
30
35
Figure 6.8: Predicted and measured torque on axis 2 for Case one.
Axis 3
0.2
predicted
measured
0.15
0.1
0.05
Nm
0
−0.05
−0.1
−0.15
−0.2
−0.25
0
5
10
15
20
Time (s)
25
30
35
Figure 6.9: Predicted and measured torque on axis 3 for Case one.
63
Identification results
Axis 1
1.5
predicted
measured
1
Nm
0.5
0
−0.5
−1
−1.5
0
10
20
30
Time (s)
40
50
60
Figure 6.10: Predicted and measured torque on axis 1 for Case two.
Axis 2
0.5
predicted
measured
0.4
0.3
0.2
Nm
6.3
0.1
0
−0.1
−0.2
0
10
20
30
Time (s)
40
50
60
Figure 6.11: Predicted and measured torque on axis 2 for Case two.
64
6
Experimental results
Axis 3
0.25
predicted
measured
0.2
0.15
0.1
Nm
0.05
0
−0.05
−0.1
−0.15
−0.2
0
10
20
30
Time (s)
40
50
60
Figure 6.12: Predicted and measured torque on axis 3 for Case two.
Model fit evaluation
As a complement to the graphical validation the measure called model fit (4.28),
is presented for Case one in Table 6.5 and for Case two in Table 6.6.
Table 6.5: Model fit in percent for all three validation data sets of Case one.
fit for axis 1 fit for axis 2 fit for axis 3
validation set one
55.3410
41.4288
48.9292
validation set two
52.4655
38.5085
46.1770
validation set three 63.1761
39.5104
54.4607
Table 6.6: Model fit in percent for all three validation data sets of Case two.
fit for axis 1 fit for axis 2 fit for axis 3
validation set one
61.5789
44.2966
50.5940
validation set two
57.2505
37.0220
56.9815
validation set three 57.9559
31.4153
29.5770
The fit for the validation data is rather low. This can be explained by the problems that were observed in the predictive validation. For instance, it could be
seen that the model did not perform well near stationary conditions. Since the
validation data sets contains stationary parts these will accumulate, thus worsening the model fit.
6.4
Controller results
6.3.5
65
Conclusions
The validation figures and the model fit presented in Section 6.3.4 indicate that
identified model has some issues. Some of these issues can be attributed to the
model structure.
At low axis velocities the friction is highly nonlinear and this could be problematic for the model since it only accounts for viscous friction and static friction.
When the model was derived the flexibilities of the system were neglected. When
working with the gimbal it could be observed that the dynamics of the gimbal is
heavily dependent on the flexibilities. This have likely caused some of the performance issues in the estimation.
To simplify, the motor control, which is responsible for making sure that the
torque output that is commanded from the control system is the same that is actually outputted from the motor, was not a part of the thesis. In Section 3.6.3 the
model used in the motor controller is presented. This is a simple linear model
with a saturation. The model excludes inductance and the emf. The exclusion
of inductance makes the motor model static. This could be an explanation to
issues observed in the high frequency region, where torque spikes could be observed. The inductance resists the increase of current in the motor windings and
thus limits how fast the torque can be varied in the motor. This results in a low
pass effect of the actual torque from the motor. The emf is proportional to the
axis speed and will therefore cause the torque output to be inaccurate when the
gimbal is moving.
There are nonlinear effects in the motor which are not considered in the model
presented in Section 3.6.3. One nonlinear effect is cogging that exists in all electric motors. Cogging is the force between the rotor magnets and the stator iron
cores. It results in a non-linear force that varies with motor axis angle. Cogging
is problematic at low torque since the torque produced by cogging is a large part
of the total output. This leads to inaccuracies in the torque measurement. The
conclusion to draw from this is that the motor controller may not be accurate
enough to be used for torque measurements for identification.
6.4
Controller results
The most important objective of the controller is to handle disturbances induced
by the movement of the helicopter. When testing disturbance rejection manually
the gimbal can be thrown around while measuring how well the camera’s direction is held. If the gimbal is not tested with the same disturbance it is difficult
to compare the performance of different controllers. Since the objective is to test
feedback linearisation we can simplify the testing and choose to test only the
reference tracking performance of the controller
The interesting point is to see if the feedback linearisation results in a closedloop system that is independent of the camera. This is tested by testing all axes
simultaneously with the same excitation but different cameras. The feedback
linearisation also offers decoupling, and this can be tested by exciting only one
axis and see how the other axes are affected.
66
6
Experimental results
When evaluating the controller reference tracking performance, a smoothed
step response test will be used. One way to produce a smoothed step response
test is to limit the acceleration of the step reference signal. The acceleration limit
results in a trapeze-formed velocity curve which in turn results in a smooth step
response. This trajectory reference resembles how the gimbal is moving when it
is steered by the camera operator.
The parameters were estimated offline and then used to determine the controller, which was tested online on the gimbal. The tuning presented in Section 5.5 was used for all tests.
6.4.1
Simultaneous axes step tracking
The goal of the simultaneous axes step tracking test is to evaluate how well the
feedback linearisation can compensate for modelled effects. As was mentioned in
Section 5.1.1, the feedback linearisation should result in identical systems for all
axes and it is therefore interesting to look at the outer loop output, which should
be the same for all axes and cases.
Case one
In Figure 6.13 simultaneous trajectory tracking for all three axes is presented. A
slight delay in the gimbal angle compared to its reference can be observed. This
is because the acceleration feed-forward is not used. In Figure 6.14 the torque
and outer loop control signal are shown, which should be identical for all axis. It
can be seen that the outer loop output is similar for all axes during the moving
phases of the test. When the gimbal is stationary there is a difference at about 3
to 5 seconds between axis 1 and the other axes. During the last stationary part at
about 8 seconds there is a large difference between axis 2 and the other axes.
Case two
The results for the simultaneous trajectory tracking test of Case two can be seen
in Figure 6.15. Axis 1 and two are very similar to Case one but axis 3 has an
oscillation. The torque and outer loop control signal are shown in Figure 6.16. By
observing the torque it can be seen that the gimbal is not perfectly balanced in
the roll and tilt axis. This imbalance can also be seen in the parameters regarding
the centre of mass of the camera. In the outer loop control signal the oscillations
in the tilt axis are prominent. As previously stated the outer loop control signal
should be the same for all axes if the feedback linearisation is working correctly.
This is not the case here and there are differences between all axes.
6.4
67
Controller results
Axis 1
Axis 2
1.5
V. ref.
V.
A. ref.
A.
V. ref.
V.
A. ref.
A.
1
0
0.5
0
0
−0.5
−0.5
−0.5
−1
−1
−1
−1.5
0
5
Time (s)
10
V. ref.
V.
A. ref.
A.
1
0.5
Angle/Velcity
0.5
Angle/Velcity
1.5
Angle/Velcity
1
Axis 3
1.5
−1.5
0
5
Time (s)
10
−1.5
0
5
Time (s)
10
Figure 6.13: Case one tracking test for all axes. "V. ref." is the reference
velocity and "V" is the measured velocity. "A. ref." is the reference angle and
"A" is the measured angle.
Torque
1
Axis 1
Axis 2
Axis 3
Nm
0.5
0
−0.5
−1
0
2
4
6
8
10
Time (s)
v
5
Axis 1
Axis 2
Axis 3
0
−5
−10
0
2
4
6
8
10
Time (s)
Figure 6.14: Case one tracking test for all axes. The upper part shows the
torque and the lower shows the outer loop control signal.
68
6
Axis 1
Axis 2
1.5
V. ref.
V.
A. ref.
A.
1
0
0.5
0
0
−0.5
−0.5
−0.5
−1
−1
−1
−1.5
0
5
Time (s)
10
V. ref.
V.
A. ref.
A.
1
0.5
Angle/Velcity
0.5
Angle/Velcity
1.5
Angle/Velcity
1
Axis 3
1.5
V. ref.
V.
A. ref.
A.
Experimental results
−1.5
0
5
Time (s)
10
−1.5
0
5
Time (s)
10
Figure 6.15: Case two tracking test for all axes. "V. ref." is the reference
velocity and "V" is the measured velocity. "A. ref." is the reference angle and
"A" is the measured angle.
Torque
1
Axis 1
Axis 2
Axis 3
Nm
0.5
0
−0.5
−1
0
2
4
6
8
10
Time (s)
v
15
Axis 1
Axis 2
Axis 3
10
5
0
−5
0
2
4
6
8
10
Time (s)
Figure 6.16: Case two tracking for all axes. The upper part shows the torque
and the lower shows the outer loop control signal.
6.4
69
Controller results
6.4.2
Separate axis step tracking
To test the decoupling performance of the controller we excite only one axis and
investigate how the others are affected. The axis that influences the other two the
most should be axis 1, since it moves the whole system. Therefore we choose to
use this axis to test the coupling effects.
In Figures 6.17 and 6.19 the resulting trajectories for Case one and Case two
are presented. The disturbances on axis 2 and axis 3 seem to be negligible for
both cases.
In Figures 6.18 and 6.20 the torque and outer loop control signal for Case one
and Case two are presented. It seems like the outer loop controllers for axis 2
and axis 3 do not need to work, which indicates that the cross coupling effects
are compensated for by the feedback linearisation.
A constant offset in reference velocity and outer loop control signal can be
observed on axis 2 for Case one. The offset may be a result of the model’s poor
performance near stationary conditions that causes the outer loop controller to
compensate for the model error.
Axis 1
Axis 2
1.5
V. ref.
V.
A. ref.
A.
1
V. ref.
V.
A. ref.
A.
1
0.5
Angle/Velcity
Angle/Velcity
0.5
1.5
0
0
0
−0.5
−0.5
−0.5
−1
−1
−1
−1.5
0
5
Time (s)
10
−1.5
0
V. ref.
V.
A. ref.
A.
0.5
Angle/Velcity
1
Axis 3
1.5
5
Time (s)
10
−1.5
0
5
Time (s)
10
Figure 6.17: Case one separate axis test. "V. ref." is the reference velocity and
"V" is the measured velocity. "A. ref." is the reference angle and "A" is the
measured angle.
70
6
Experimental results
Torque
1
Axis 1
Axis 2
Axis 3
Nm
0.5
0
−0.5
−1
0
2
4
6
8
10
Time (s)
v
5
Axis 1
Axis 2
Axis 3
0
−5
−10
0
2
4
6
8
10
Time (s)
Figure 6.18: Case one separate axis test. The upper part shows the torque
and the lower shows the outer loop control signal.
Axis 1
Axis 2
1.5
V. ref.
V.
A. ref.
A.
1
V. ref.
V.
A. ref.
A.
1
0.5
Angle/Velcity
Angle/Velcity
0.5
1.5
0
0
0
−0.5
−0.5
−0.5
−1
−1
−1
−1.5
0
5
Time (s)
10
−1.5
0
V. ref.
V.
A. ref.
A.
0.5
Angle/Velcity
1
Axis 3
1.5
5
Time (s)
10
−1.5
0
5
Time (s)
10
Figure 6.19: Case two separate axis test. "V. ref." is the reference velocity
and "V" is the measured velocity. "A. ref." is the reference angle and "A" is the
measured angle.
6.4
71
Controller results
Torque
1
Axis 1
Axis 2
Axis 3
Nm
0.5
0
−0.5
−1
0
2
4
6
8
10
Time (s)
v
15
Axis 1
Axis 2
Axis 3
10
5
0
−5
0
2
4
6
8
10
Time (s)
Figure 6.20: Case two separate axis test. The upper part shows the torque
and the lower shows the outer loop control signal.
6.4.3
Conclusions
The goal of the simultaneous axes step tracking was to evaluate the feedback linearisation controller, and to see if it results in identical systems for all axes and
test cases. Therefore the outer loop control signal should be identical for both test
cases. As can be seen in Figures 6.14 and 6.16, there are some differences in the
outer loop control signal. An oscillation was present in axis 3 for Case two. The
probable cause of this oscillation is that the estimated inertial parameter for that
axis was inaccurate. This results in a low system gain and thus an oscillation occurs. This indicates that the optimized trajectory should have higher acceleration
in axis 3 and thus increase the information in the measurement data regarding
the inertial parameter of that axis.
During the model validation it was noted that model has some problematic
areas, mainly near static conditions and in high frequency regions. The outer
loop control signal differs at the static regions of the trapeze step for all axes and
cases, which is consistent with the previous mentioned problems with the model.
During the non-stationary phase of the trapeze step, the outer loop control signal
is more consistent between all cases and axes, which was also noted in the model
validation tests.
The separate axis step tracking tested the decoupling performance of the controller. Since the axes which are not excited are not affected by the excited axis
in Case one it seems like the decoupling of the feedback linearisation is working
when the model is accurate enough.
7
Conclusions and future work
7.1
Results
The goal of this thesis was to investigate the possibility of a self-tuning controller
based on system identification and model-based control methods.
In this thesis, a method to identify parameters of a physical model was used.
The identifiability of the gimbal model has been evaluated. Furthermore, it has
been investigated how the gimbal should be excited to obtain informative data for
identification. Finally, the parameters have been identified from data collected
during experiments.
The control method called feedback linearisation makes it possible to compensate for all modelled camera dependent dynamics so that the system will not
require tuning. Furthermore, the robustness of the closed loop system has been
investigated and a method called Lyapunov redesign has been used to guarantee
tracking error stability. The controller has then been implemented and tested on
the gimbal’s hardware.
For the time span of this thesis all initial goals were not possible to reach
and re-prioritizations had to be done. The system was supposed to be fully implemented to the gimbal’s hardware. To reduce the time to implement the functionality only the critical system parts were implemented on the gimbal. The
controller, the data processing and estimation were performed offline.
7.2
Discussion
There have been some problems with inconsistent results during this thesis work.
A test could be performed twice with varied results. During identification some
parameter estimates often become unreasonable, either the size or the sign. This
becomes problematic for inertial parameters as they determine the gain for the
73
74
7
Conclusions and future work
outer loop controller. If an inertial parameter becomes too small the gain will be
too low and the gimbal will oscillate or even become unstable.
The usual work flow when doing system identification is to start with data
from experiments and intuitive knowledge of the system before doing modelling
and system identification. Due to delays in the project all theory work had to
be done before actually working with the system. This meant that it was very
difficult to go back and do changes when something was not working as it was
supposed to.
The most interesting part of the modelling is the assumptions that were made
for the gimbal, mainly the trade-off between model complexity and model accuracy. One assumption that was made was to consider the gimbal as being rigid
and thus not accounting for any flexibilities at all. Since the gimbal is constructed
with strict limitations to weight it is difficult to make it rigid. After working some
time with the gimbal it became obvious that these flexibilities could not be discounted for since they have such a large influence on the gimbal dynamics.
7.3
Conclusions
The two primary conclusions that can be drawn from the discussion above is that
the flexibilities should be included when modelling a gimbal. The other conclusion concerns the motor controller, which was discussed in Section 6.3.5. The
torque measurements obtained from the motor controller seems to lack accuracy
and may therefore cause issues when relying on these measurements for system
identification. These factors can probably contribute to the issues with parameter
estimation and controller performance experienced during the thesis.
7.4
Future work
It would be interesting to look at a different modelling approach where flexibilities are included and the cross-coupling effects of the gimbal are omitted. This
could lead to a model with similar complexity but better simulation capabilities.
A more complex friction model could probably improve the model for low velocities.
The accuracy of the motor controller needs to be increased. Some method to
evaluate the accuracy should be developed. The motor modelling has not been
a part of this thesis and the model was provided by ia. The motors in the gimbal have been modelled with a linear approximation and a saturation. By using a more advanced model it may be possible to improve torque measurements.
Having better torque measurements could potentially improve the identification
results and subsequently the controller performance.
Appendix
A
Gimbal model
A.1
Components of the D matrix

d11

D(q) = d21

d31
d12
d22
d32

d13 
d23 

d33
(A.1)
d11 = I1yy + I2xx cos(q2 )2 + I3yy sin(q2 )2 + I2zz sin(q2 )2
d12
d13
+ I3zz cos(q2 )2 cos(q3 )2 + I3xx cos(q2 )2 sin(q3 )2
= − cos(q2 ) cos(q3 ) sin(q3 ) (I3xx − I3zz )
= I3yy sin(q2 )
d21 = − cos(q2 ) cos(q3 ) sin(q3 ) (I3xx − I3zz )
I
cos(2 q3 ) I3zz cos(2 q3 )
I
I3xx
+ I2yy + 3zz + 3xx
−
2
2
2
2
=0
= I3yy sin(q2 )
d22 =
d23
d31
d32 = 0
d33 = I3yy
A.2
Components of the C matrix

c11

C(q, q̇) = c21

c31
77
c12
c22
c32

c13 
c23 

c33
(A.2)
78
A
Gimbal model
c11 = q̇3 2 I3xx cos(q2 )2 cos(q3 ) sin(q3 ) − 2 I3zz cos(q2 )2 cos(q3 ) sin(q3 )
− q̇2 2 I3zz cos(q2 ) sin(q2 ) cos(q3 )2 + 2 I3xx cos(q2 ) sin(q2 ) sin(q3 )2
− q̇2 2 I2xx cos(q2 ) sin(q2 ) − 2 I3yy cos(q2 ) sin(q2 ) − 2 I2zz cos(q2 ) sin(q2 )
1
(q̇ sin(2 q3 ) sin(q2 ) − 2 q̇3 cos(2 q3 ) cos(q2 )) (I3xx − I3zz )
2 2
= I3yy q̇2 cos(q2 )
c12 =
c13
I2xx q̇1 sin(2 q2 ) I3xx q̇1 sin(2 q2 ) I3yy q̇1 sin(2 q2 ) I2zz q̇1 sin(2 q2 )
+
−
−
2
4
2
2
I3zz q̇1 sin(2 q2 ) I3yy q̇3 cos(q2 )
−
− I3xx q̇3 cos(2 q3 ) cos(q2 )
+
4
2
I q̇ sin(2 q3 ) sin(q2 )
+ I3zz q̇3 cos(2 q3 ) cos(q2 ) + 3xx 2
4
I3zz q̇2 sin(2 q3 ) sin(q2 ) I3xx q̇1 cos(2 q3 ) sin(2 q2 )
−
−
4
4
I3zz q̇1 cos(2 q3 ) sin(2 q2 )
+
4
1
= − sin(2 q3 ) (I3xx − I3zz ) (4 q̇3 + q̇1 sin(q2 ))
4
I3yy q̇1 cos(q2 )
= −
2
1
= cos(q2 ) 2 I3yy q̇2 + I3xx q̇2 cos(2 q3 ) − I3zz q̇2 cos(2 q3 )
2
1
+ cos(q2 ) (−I3xx q̇1 sin(2 q3 ) cos(q2 ) + I3zz q̇1 sin(2 q3 ) cos(q2 ))
2
1
= (I3xx − I3zz ) (q̇2 sin(2 q3 ) + q̇1 cos(2 q3 ) cos(q2 ))
2
=0
c21 =
c22
c23
c31
c32
c33
A.3
Gimbal regressor matrix
A.3
79
Gimbal regressor matrix
y11 = q̈1
y12 = tanh(20 q̇1 )
y13 = q̇1
y14 = q̈1 cos(q2 )2 − 2 q̇1 q̇2 cos(q2 ) sin(q2 )
y15 = y16 = y17 = y18 = y19 = 0
y110 = q̇1 2 q̇3 cos(q2 )2 cos(q3 ) sin(q3 ) − 2 q̇2 cos(q2 ) sin(q2 ) sin(q3 )2
q̇2 2 q̇3 cos(q2 ) 2 cos(q3 )2 − 1 − 2 q̇2 cos(q3 ) sin(q2 ) sin(q3 )
−
2
+ q̈1 cos(q2 )2 sin(q3 )2 − q̈2 cos(q2 ) cos(q3 ) sin(q3 )
y111 = q̈1 sin(q2 )2 + q̈3 sin(q2 ) + q̇2 q̇3 cos(q2 ) + 2 q̇1 q̇2 cos(q2 ) sin(q2 )
y112 = y113 = y114 = y115 = y21 = y22 = y23 = 0
q̇12 sin(2 q2 )
2
= q̈2
= − sin(q2 )
y24 =
y25
y26
y27 = cos(q2 )
y28 = tanh(20 q̇2 )
y29 = q̇2
y210 = q̈2 cos(q3 )2 + q̇1 q̇3 cos(q2 ) + q̇12 cos(q2 ) sin(q2 ) − 2 q̇2 q̇3 cos(q3 ) sin(q3 )
− q̇12 cos(q2 ) cos(q3 )2 sin(q2 ) − 2 q̇1 q̇3 cos(q2 ) cos(q3 )2
− q̈1 cos(q2 ) cos(q3 ) sin(q3 )
sin(2 q2 ) q̇12
− 1 q̇3 cos(q2 ) q̇1
2
= sin(q2 ) sin(q3 )
= − cos(q3 ) sin(q2 )
y211 = −
y212
y213
y214 = y215 = y31 = y32 = y33 = y34 = y35 = y36 = y37 = y38 = y39 = 0
sin(2 q3 ) q̇12 cos(q2 )2
sin(2 q3 ) q̇22
+ cos(2 q3 ) q̇1 q̇2 cos(q2 ) +
2
2
= q̈3 + q̈1 sin(q2 ) + q̇1 q̇2 cos(q2 )
= − cos(q2 ) cos(q3 )
y310 = −
y311
y312
y313 = − cos(q2 ) sin(q3 )
y314 = tanh(20 q̇3 )
y315 = q̇3
80
A
Gimbal model
Bibliography
M. Gautier and W. Khalil. Direct calculation of minimum set of inertial parameters of serial robots. Robotics and Automation, IEEE Transactions on, 6(3):
368–373, 1990. Cited on page 30.
Maxime Gautier. Numerical calculation of the base inertial parameters of robots.
Journal of Robotic Systems, 8(4):485–506, 1991. Cited on page 30.
T. Glad and L. Ljung. Reglerteori: flervariabla och olinjära metoder. Studentlitteratur, 2003. ISBN 9789144030036. URL http://books.google.ie/
books?id=3sTsMAAACAAJ. Cited on page 41.
Jakob Johansson. Modelling and control of an advanced camera gimbal. Master’s thesis, Linköping University, Department of Electrical Engineering, 2012.
LiTH-ISY-EX–12/4644–SE. Cited on pages 2 and 9.
H.K. Khalil. Nonlinear Systems. Prentice Hall PTR, 2002. ISBN 9780130673893.
URL http://books.google.se/books?id=t_d1QgAACAAJ. Cited on
pages 40, 41, 42, 43, and 48.
W. Khalil and E. Dombre. Modeling, Identification and Control of Robots. HPS,
2002. ISBN 9781903996133. URL http://books.google.se/books?id=
WB_E0WsVCnUC. Cited on pages 26, 29, and 34.
Young Shin Kwon, Hong Yeon Hwang, and Yun Seok Choi. Stabilization loop
design on direct drive gimbaled platform with low stiffness and heavy inertia.
In Proceedings of the International Conference on Control, Automation and
Systems, ICCAS’07, pages 320–325, Seoul, Korea, October 2007. Cited on page
2.
Zhi-qiang Li, Zhi-yong Zhang, Qing-kun Zhou, and Da-peng Fan. Parameter
identification of inertially stabilized platforms using current command design.
Journal of Central South University, 20:342–353, 2013. Cited on page 2.
L. Ljung. System Identification: Theory for the User. Prentice-Hall Information and System Sciences Series. Englewood Cliffs, New Jersey, 1987.
ISBN 9780138816407. URL http://books.google.se/books?id=M_
hQAAAAMAAJ. Cited on pages 26 and 34.
81
82
Bibliography
Rasmus Olsson. Identifiering av stelkroppsmodell för industrirobot. Master’s thesis, Linköping University, Department of Electrical Engineering, 2005. LITHISY-EX–05/3625–SE. Cited on page 36.
V. Sangveraphunsiri and K. Malithong. Robust inverse dynamics and sliding
mode control for inertial stabilization systems. Asian International Journal of
Science and Technology in Production and Manufacturing Engineering, 2(4):
33–45, 2009. Cited on pages 2 and 40.
Abraham Savitzky and Marcel JE Golay. Smoothing and differentiation of data by
simplified least squares procedures. Analytical Chemistry, 36(8):1627–1639,
1964. Cited on page 38.
Yuri B. Shtessel. Sliding mode stabilization of three axis inertial platform. In
Proceedings of the 26th Southeastern Symposium on System Theory, pages
54–58, Athens, OH, USA, March 1994. IEEE. Cited on page 2.
B. Siciliano, L. Sciavicco, and L. Villani. Robotics: Modelling, Planning and
Control. Advanced Textbooks in Control and Signal Processing. Springer,
2009. ISBN 9781846286414. URL http://books.google.se/books?id=
jPCAFmE-logC. Cited on pages 9, 10, 11, 12, 13, 14, 17, 18, 19, 45, and 47.
Per Skoglar. Modelling and control of IR/EO-gimbal for UAV surveillance applications. Master’s thesis, Linköping University, Department of Electrical Engineering, 2002. LiTH-ISY-EX-02/3258-SE. Cited on page 2.
M.W. Spong and S. Hutchinson.
Robot Modeling and Control.
Wiley,
2005. ISBN 9780471649908. URL http://books.google.se/books?id=
wGapQAAACAAJ. Cited on pages 40 and 48.
Jan Swevers, Walter Verdonck, and Joris De Schutter. Dynamic model identification for industrial robots. IEEE, Control Systems, 27(5):58–71, 2007. Cited on
pages 33 and 34.
Upphovsrätt
Detta dokument hålls tillgängligt på Internet — eller dess framtida ersättare —
under 25 år från publiceringsdatum under förutsättning att inga extraordinära
omständigheter uppstår.
Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner,
skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid
en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av
dokumentet kräver upphovsmannens medgivande. För att garantera äktheten,
säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ
art.
Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman
i den omfattning som god sed kräver vid användning av dokumentet på ovan
beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan
form eller i sådant sammanhang som är kränkande för upphovsmannens litterära
eller konstnärliga anseende eller egenart.
För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/
Copyright
The publishers will keep this document online on the Internet — or its possible replacement — for a period of 25 years from the date of publication barring
exceptional circumstances.
The online availability of the document implies a permanent permission for
anyone to read, to download, to print out single copies for his/her own use and
to use it unchanged for any non-commercial research and educational purpose.
Subsequent transfers of copyright cannot revoke this permission. All other uses
of the document are conditional on the consent of the copyright owner. The
publisher has taken technical and administrative measures to assure authenticity,
security and accessibility.
According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected
against infringement.
For additional information about the Linköping University Electronic Press
and its procedures for publication and for assurance of document integrity, please
refer to its www home page: http://www.ep.liu.se/
© Henric Edlund
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertising