-
Mobile Manipulator Control
António Nunes Henriques
Thesis to obtain the Master of Science Degree in
Mechanical Engineering
Supervisors: Prof. Paulo Jorge Coelho Ramalho OliveiraProf.
Carlos Baptista Cardeira
Examination Committee
Chairperson: Prof. Paulo Rui Alves FernandesSupervisor: Prof.
Carlos Baptista Cardeira
Member of the Committee: Prof. Jorge Manuel Mateus Martins
June, 2017
-
ii
-
Dedicated to my family and friends...
iii
-
iv
-
Acknowledgments
In this section, I would like to acknowledge all my lab mates,
for all the support they have given me
throughout the writing of this dissertation. In particular, I
would like to thank my friends Francisco Fer-
nandes, Tobias Pereira, Miguel Roque and Guilherme Leite for
their endless patience towards me and
their unwavering willingness to help whenever needed.
Moreover, I would like express my sincere gratitude to all the
people at Instituto Superior Técnico who
were a part of my academic journey. Namely, I would like to
thank my supervisors Prof. Paulo Oliveira
and Prof. Carlos Cardeira for setting such high standards and
constantly pushing me to do better.
Finally, I would like to thank my family and friends for their
unconditional support, and for keeping
sane throughout the writing of this dissertation.
v
-
vi
-
Resumo
A presente dissertação centra-se no desenvolvimento do
protótipo de uma impressora 3D móvel, cujo
objetivo é realizar impressões com área ilimitada. Esta é
composta por um braço robótico montado na
plataforma de um robô diferencial, sendo que numa fase
posterior do projeto a cabeça da impressora
será instalada na extremidade do braço. Como em qualquer outra
impressão 3D, após a criação de
um modelo virtual da peça pretendida em software CAD será
gerada uma trajetória única para esse
objecto especı́fico, tendo em conta as caracterı́sticas da
impressora. Se seguida com precisão pelo
end-effector do sistema, permitirá que a cabeça da impressora
deposite as várias camadas de material
da maneira pretendida, levando à criação da peça desejada. O
principal foco desta tese é precisamente
o seguimento desta trajetória. Pretende-se que este processo
decorra com elevada precisão, de modo
a que os objetos criados se aproximem o mais possı́vel do seu
modelo virtual. Serão apresentados
vários estudos realizados em diversas áreas da Engenharia,
tais como a Robótica, Controlo e Visão
Computacional, e que têm em comum o objectivo fundamental de
fazer o robô mover-se da forma
pretendida. No final, serão apresentados os resultados da
implementação do algoritmo desenvolvido.
Palavras-chave: Impressão 3D , Controlo, Manipulador Móvel,
Visão Computacional, Sis-temas Não-holonómicos
vii
-
viii
-
Abstract
The present thesis describes the progresses towards the
development of a mobile 3D printer’s prototype,
whose goal is to be able to print with unlimited area. The
printer is composed of a robotic arm, mounted
on top of the platform of a differential drive robot.
Posteriorly, the head of the 3D printer will be installed
on the tip of the arm. As in any other 3D printing, after the
creation of a virtual model of the desired
part in CAD software a unique trajectory will be generated for
that specific object, taking the printer’s
specifications into consideration. If followed with enough
accuracy and precision by the system’s end-
effector, it will allow the head of the printer to deposit the
numerous layers of material as pretended,
leading to the creation of the desired item. The main focus of
this dissertation is the tracking of this
trajectory. It is intended that this process develops with
extreme precision, so as to ensure the created
objects are as close to their virtual models as possible.
Several studies conducted in diverse areas
such as Robotics, Control and Computer Vision will be presented,
and whose goal when put together
is to make the robot move in the desired way. In the end, the
results of the developed algorithm’s
implementation will be presented.
Keywords: 3D printing, Control, Mobile Manipulator, Computer
Vision, Nonholonomic Systems
ix
-
x
-
Contents
Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . v
Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . vii
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . ix
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . xiv
List of Figures . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . xv
1 Introduction 1
1.1 Brief History . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . 1
1.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 2
1.3 Topic Overview . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 3
1.4 State of the Art . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 3
1.4.1 Mobile Manipulator . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 3
1.4.2 Robotic Arm . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 4
1.4.3 Experimental Setup . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . 5
1.5 Objectives . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 5
1.6 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 6
2 Denavit-Hartenberg Parameters 7
2.1 Mobile Platform . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 7
2.2 Manipulator . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 10
3 Direct Kinematics 12
3.1 Operational and Joint Coordinates . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 12
3.2 Direct Analysis . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 14
3.3 Transformation Matrices . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 14
3.4 Validation . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 15
4 Jacobian and Redundancy Analysis 16
4.1 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 16
4.2 Redundancy . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 17
4.3 Constraints . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 17
xi
-
5 Inverse Kinematics 21
5.1 Algorithm - Theoretical Model . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 21
5.1.1 Inversion of the Jacobian . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 21
5.1.2 Model . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 22
5.1.3 Closed Loop . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 23
5.2 Validation . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 25
5.2.1 Results . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 26
6 Dynamics 30
6.1 Newton-Euler Formulation . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 30
6.1.1 Mobile Platform . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 33
6.1.2 Manipulator . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 35
7 Control Algorithm 36
7.1 Inverse Dynamics Control . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 36
7.1.1 Validation . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 37
7.2 Non-linear Model Based Predictive Control . . . . . . . . .
. . . . . . . . . . . . . . . . . 39
7.2.1 Validation . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 43
8 Motor Controllers 47
8.1 Motors’ Dynamics . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 47
8.2 Parameter Estimation . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 49
8.3 Structure of the Algorithm . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 50
9 Implementation 51
9.1 Specifications . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 51
9.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 53
9.2.1 Analysis . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 54
10 Positioning 55
10.1 Parameter Estimation . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 55
10.2 Positioning . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 57
10.3 Image Processing . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 61
10.4 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 62
10.5 Computational Efficiency . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 63
10.5.1 Context . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 63
10.5.2 UDP Communication . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . 63
11 Nonholonomic Mobile Platform Control 64
11.1 Context . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 64
11.2 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 65
11.2.1 Tracking Control of the Mobile Platform . . . . . . . . .
. . . . . . . . . . . . . . . 65
xii
-
12 Results 67
12.1 Simulation Details . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 67
12.2 Simulation Results . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 68
12.3 Analysis . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . 72
13 Conclusions 74
13.1 Achievements . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 74
13.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 74
Bibliography 76
A Block Diagrams - Simulink 79
A.1 Simulink - Mobile Platform Control . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 79
A.2 Simulink - Nonlinear-linear Model Based Predictive Control .
. . . . . . . . . . . . . . . . 80
A.3 Simulink - Inverse Kinematics . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 81
xiii
-
List of Tables
2.1 Denavit-Hartenberg parameters - Platform 1 . . . . . . . . .
. . . . . . . . . . . . . . . . 8
2.2 Denavit-Hartenberg parameters - Platform 2 . . . . . . . . .
. . . . . . . . . . . . . . . . 9
2.3 Denavit-Hartenberg parameters - Robotic Arm . . . . . . . .
. . . . . . . . . . . . . . . . 10
2.4 Denavit-Hartenberg parameters - Mobile Manipulator . . . . .
. . . . . . . . . . . . . . . 11
5.1 Reference Trajectory - Inverse Kinematics. . . . . . . . . .
. . . . . . . . . . . . . . . . . . 25
6.1 Denavit-Hartenberg parameters - Platform Dynamic Model . . .
. . . . . . . . . . . . . . 33
8.1 EMG30 Motor - Dynamic Model. . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 49
10.1 Focal Length Estimation. . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 57
xiv
-
List of Figures
1.1 Mobile Platform - Rasteirinho . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 4
1.2 Robotic Arm . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 4
1.3 Final Assembly . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 5
2.1 Base of the Manipulator Location . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 8
2.2 Diagram of the Robotic Arm . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 10
3.1 Transformation between two consecutive reference frames and
corresponding Denavit-
Hartenberg parameters [17] . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 14
5.1 Inverse Kinematics: Block Diagram . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 24
5.2 Error Associated with Position Operational Coordinates . . .
. . . . . . . . . . . . . . . . 27
5.3 Error Associated with the Unrestricted Euler Angle . . . . .
. . . . . . . . . . . . . . . . . 28
5.4 x: Reference vs Obtained Trajectory . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 28
5.5 Prismatic Joints Velocity . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 29
5.6 Revolute Joints Velocity . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 29
7.1 Block Diagram: Inverse Dynamics Control . . . . . . . . . .
. . . . . . . . . . . . . . . . . 37
7.2 Position Errors: Prismatic Joints . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 38
7.3 Position Errors: Revolute Joints . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 39
7.4 Position Errors: Prismatic Joints . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 43
7.5 Position Errors: Revolute Joints . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 44
7.6 Position Errors: Prismatic Joints . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 45
7.7 Position Errors: Revolute Joints . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 45
9.1 Prismatic Joints Errors . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 53
9.2 Revolute Joints Errors . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 53
10.1 Camera Positioning . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 55
10.2 Coordinate Systems [21] . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 56
10.3 Point in Stereo Cameras [22] . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 58
10.4 Processed Images of Both Cameras . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 61
10.5 Kalman Filter: Filtered and Unfiltered Comparison . . . . .
. . . . . . . . . . . . . . . . . 62
xv
-
11.1 Driving Platform Coordinate System [23] . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 65
12.1 Manipulator’s Prismatic Joints Errors . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 68
12.2 Manipulator’s Revolute Joints Errors . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 68
12.3 Evolution of xq with time . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 69
12.4 Error associated with xq . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 69
12.5 Evolution of yq with time . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 70
12.6 Error associated with yq . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 70
12.7 Evolution of θq with time . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 71
12.8 Error associated with θq . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . 71
A.1 Simulink - Mobile Platform Control . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 79
A.2 Simulink - Nonlinear-linear Model Based Predictive Control .
. . . . . . . . . . . . . . . . 80
A.3 Simulink - Inverse Kinematics . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 81
xvi
-
Chapter 1
Introduction
Considered by many to be one of the backbones of a 4th
Industrial Revolution ([1], [2] to quote a few),
3D printing is a rapidly growing technology that is becoming
more commonplace by the day. It can be
described as an additive manufacturing process, meaning that
three-dimensional objects are created
through the deposit of numerous layers of material in order to
conceive as accurately as possible a
previously created digital model. Naturally, the recent upsurge
of this technology has a direct correlation
with its many advantages, some of which are presented:
• It allows the creation of increasingly complex parts
• It makes the customization of each individual item more
feasible
• It eases and speeds up the prototyping phases
• Its flexibility allows small businesses and individuals to
have the means to produce their own
components, contradicting the established presumption that this
is a privilege only within the reach
of manufacturing industries
Nowadays, the impact of 3D printing reaches the most diverse
areas of our society, such as economy
[3], medicine ([4], [5] and [6]), construction [7] or even
gastronomy [8].
1.1 Brief History
A brief description of 3D printing history is described in [9].
The development of this technology was
boosted by the emergence of several computer-aided methods for
additive fabrication in the late 1970s.
Up to this point, this type of manufacturing had almost no
relevance in the industrial context, except in
the production of microchips.
In 1984, the first patent regarding 3D printing technology was
issued. It belonged to Charles Hull,
who is the creator of stereolitography (STL). Basically, this
process consisted in the hardening of liquid
1
-
polymers, through ultra-violet light. He was able to conceive
the first ever 3D printed object: a small
5cm tall cup, that took months to build. He is also the founder
of 3D Systems, the company that in 1992
built the first ever 3D printer, based on the previously
mentioned STL process. In the 1980s, some other
important additive manufacturing methods are worth mentioning,
such as:
• Laminated Object Manufacturing (LOM), applied mainly by
Helisys (USA), Solido3D (Israel) and
Kira (Japan)
• Selective Laser Sintering (SLS), developed in the University
of Texas
• Fused Deposition Modelling (FDM), developed by C.S.Crump
Up to the early 2000s, 3D printers’ use was almost irrelevant in
the industrial sector, mainly due to
how expensive the machines were. The spreading of this
technology started to take place in 2005, when
a project called Rep Rap (Replicating Rapid Prototyping) [10]
was launched at the University of Bath,
led by Adrian Bowyer. The goal of the project was to create a 3D
printer able to print out most its parts,
allowing individuals to build their own low-cost 3D printers
without the huge price limitations that existed
before. The software was open-source and the electronics were
based on an Arduino platform. This
pioneer project led to the appearance of many similar
initiatives throughout the decade, of which the
MakerBot stands out. Ultimately, all of these projects together
boosted the evolution of 3D printing into
the widespread technology that it is today.
Naturally, all this scientific development led to more complex
engineering challenges. Nowadays,
some of the projects regarding 3D printing technology involve
building entire houses [11] or zero-gravity
3D printing [12], for example. The European Space Agency (ESA)
is inclusively carrying out a project to
test the feasibility of building a lunar base with a 3D printer
using only local materials [13].
1.2 Motivation
Despite the recent emergence of 3D printing around the world,
there are still considerable limitations
associated with this technology. Arguably one of the most
significant ones is that the printing area of a
regular 3D printer is somewhat limited, which restricts the size
of the components to be built. For the
time being, there are already solutions to deal with this
problem, but they mostly involve building mas-
sive 3D printers, capable of conceiving components of
substantial size. Obviously this is an expensive
solution and one with completely prohibitive prices to the
majority of people. When it comes to having an
affordable and accessible way to build a 3D printer with
unlimited printing area, there are still no feasible
solutions available.
2
-
1.3 Topic Overview
Having in mind the aforementioned background, this thesis aims
to make a contribution towards solv-
ing the problem of building a small-scale, low-cost 3D printer
with unlimited printing area. It is the
sequence of a project previously initiated by Tobias Pereira and
Rui Vasconcelos, and whose work can
be found in [14]. A more detailed description of their
contribution can be found in the State of the Art
section. Most of the hardware part of the project is already
done, apart from a few changes that will
be addressed when relevant (essentially the installation of a
pair of stereo cameras and the addition of
counterweights to balance the robot). The main focus of this
dissertation will be the software part of the
project, namely all the issues related to Robotics, Control and
Computer Vision that are necessary for
the robot’s end-effector to follow a predefined trajectory.
Having in mind that the end-effector of the robot
will eventually be the printing head, it is of the utmost
importance that this trajectory is followed with as
much precision as possible.
1.4 State of the Art
In the beginning of this section, the results of an extensive
research aiming to find existing solutions
to the small-scale mobile 3D printer are presented. In a later
stage, a brief description of the structure
of the studied robot is presented.
As expected, it was challenging to find existing work on this
particular field of study. However, a very
similar robot to the studied one was developed by industrial
engineers working at NEXT (Núcleo de
Experimentação Tridimensional) and LIFE (Laboratório de
Interfaces Fı́sicas Experimentais), both aca-
demic laboratories belonging to PUC-Rio (Pontifı́cia
Universidade Católica do Rio de Janeiro). Although
it was not possible to find very specific details regarding
their project, one major difference stands out
when comparing both robots: theirs uses omni-directional wheels,
while the robot considered in this the-
sis is composed of a robotic arm mounted on top of a
differential-drive platform. Information and videos
regarding this project can be found in [15] and [16].
The mobile manipulator can be divided into two different parts:
the mobile platform and the robotic
arm.
1.4.1 Mobile Manipulator
The platform (also know as Rasteirinho) is a differential-drive
robot. This means that it is composed
of two wheels, whose wheel axis is the same. Due to the way that
they are attached to the platform, it
is not possible for the wheels to turn. Consequently, if both
wheels have the same velocity in the same
direction a purely linear movement will occur. In contrast,
considering the case where both wheels have
the same velocity but in opposite directions, a purely
rotational movement will occur. Naturally, if the
3
-
wheels have different velocities a combination of linear and
rotational movements will take place. Each
of the wheels is connected to a EMG30 motor and both the motors
are controlled by a MD-25 board.
Figure 1.1: Mobile Platform - Rasteirinho
1.4.2 Robotic Arm
The robotic arm is composed of a revolute joint, followed by two
prismatic ones. A system similar to
the worm and wheel was set up in the mobile platform, but with a
plastic gear and a threaded rod with
equivalent pitch instead of the custom made metallic set. A
EMG30 motor was again chosen to be the
actuator of this joint, and was attached to the rod. This whole
arrangement forms the revolute joint of
the system. Both of the ensuing prismatic joints are assembled
using the same system: a spindle with
one EMG30 motor at one end and a bearing at the other. Similarly
to the mobile platform, all the motors
actuating these joints are controlled by MD-25 boards. In this
case, one board controls the revolute joint
and the other controls the prismatic ones.
Figure 1.2: Robotic Arm
4
-
1.4.3 Experimental Setup
All the computational work necessary to ensure that the
end-effector follows the desired trajectory
(computation of the trajectory, control, among others) will be
executed in a computer. The computer will
communicate via USB with an Arduino board, fixed to the platform
of the Rasteirinho. The role of this
board is simply to make a connection between all of the MD-25
boards attached to the motors and the
computer. The board receives the inputs to provide to the motors
and then reads the sensors’ values
(encoders) that will be sent to the computer to generate new
inputs for the motors. In a later stage of
the project, the encoders corresponding to the platform’s wheels
will not be used. Two cameras will be
installed in the mobile platform, and will communicate via USB
directly with the computer to provide the
platform’s position feedback signals. The whole system will be
powered by a transformer, that provides
current directly to one of the MD-25 boards. In turn, this board
is connected to a breadboard that will
provide power to all the other boards (including the
Arduino).
Figure 1.3: Final Assembly
1.5 Objectives
This thesis aims to make a significant contribution towards
having the robot’s end-effector follow a
preassigned trajectory, so as to posteriorly allow the printer
to be functional. It aims to develop a valid
control law that makes the first mentioned goal possible and it
aims to change the robot’s positioning
system, which at this point has a few limitations later to be
discussed. It also aims to develop both a
valid kinematic and dynamic model for the mobile manipulator,
resulting in an increase of the precision
of tasks like the computation of an optimal trajectory or the
results of the control law.
5
-
1.6 Thesis Outline
This thesis is structured as follows. Chapters 2 and 3 describe
the steps taken to estimate the printer’s
Denavit-Hartenberg parameters, followed by the computation of
its direct kinematics model. Afterwards
an analysis of the robot’s redundancy and Jacobian are made, in
a chapter where the robot’s nonholo-
nomic constraint is also discussed. In chapter 5 the Inverse
Kinematics problem is tackled (as well as the
singularities one), followed by an estimation of the system’s
dynamic model in chapter 6. The ensuing
section compares the performance of different control
algorithms, whereas in chapter 8 the modelling of
the motors’ dynamic is examined. In chapter 9, the results of
the application of the obtained control law
in the real robot using encoders are presented. Thereafter, an
analysis on the new positioning system
of the robot using two cameras is made (chapter 10), as well as
an analysis of the implications that this
new system has on the algorithm’s computational efficiency
(chapter 11). In chapter 12 a new control
algorithm for the mobile platform is presented, followed by a
chapter where the results of tracking a
trajectory using the camera system are presented. Ultimately, a
final analysis regarding the obtained
results is made.
6
-
Chapter 2
Denavit-Hartenberg Parameters
The first item to consider is the definition of the robot’s
Denavit-Hartenberg parameters. This convention
defines a method to determine the relative position and
orientation of two consecutive link reference
frames. The parameters obtained will later be used to compute
the robot’s Direct Kinematics. The four
parameters associated with this convention can be defined as the
following:
• d: Distance covered in the direction of the Z axis of the
initial reference frame
• θ: Angle around the Z axis of the initial reference frame
• a: Distance covered in the direction of the X axis obtained
after the two first transformations
• α: Angle around the X axis obtained after all the above
mentioned transformations
2.1 Mobile Platform
The definition of the Denavit-Hartenberg parameters can be
divided into two sections: the mobile plat-
form and the robotic arm. Regarding the first, an initial fixed
reference frame will be considered. Its Z
axis is perpendicular to the axis that passes through both wheel
centres, intersecting it while pointing
up. Its X axis is also perpendicular to the wheel axis, but in a
plane parallel to the plane formed by the
ground. The Y axis belongs to this same plane, but parallel to
the wheel axis. It is important to note that
these axis are fixed, even if the platform’s position
changes.
The platform’s parameters will be chosen so as to meet the
following requirement: The frame corre-
sponding to the beginning of the manipulator (located at its
base) will have its Y axis parallel to the axis
that passes through the wheel centres (parallel to the axis of
the horizontal prismatic joint in its initial
position). This will allow us to posteriorly define the
parameters of the manipulator with greater ease.
7
-
A total of 8 parameters will be considered regarding the mobile
platform. The transformations that
the initial reference frame undergoes until it reaches its final
pose are illustrated in the following image:
dbase
12
Z8
Y8
X8
Z0
X0
Y0
0
Figure 2.1: Base of the Manipulator Location
The five first parameter sets define the transformation from
point 0 to point 1 in image 2.1, and are
represented in the following table:
Table 2.1: Denavit-Hartenberg parameters - Platform 1
Link di θi ai αi
1 0 0 0 −π2
2 yq −π2 0 −π2
3 xq −π2 0 −π2
4 0 −π2 0 0
5 hplat 0 0 0
The first four transformations ensure that the origin of the
reference frame follows the mobile platform
when it moves along the originalX and Y axis. These parameters
also guarantee that there is no rotation
of the original reference frame. The fifth line moves the origin
of the frame from the ground plane to the
mobile platform plane.
8
-
It is important to point out that number of lines in the
estimated Denavit-Hartenberg parameters is su-
perior to the number of links. Had the conventional
Denavit-Hartenberg approach been taken, this would
not happen. However, and taking into account the fact that we
are dealing with a mobile manipulator, the
choice of including the additional links was made so as to
facilitate the definition of the platform’s final
reference frame. Considering the alternative approach taken,
several tests were performed to validate
the obtained parameters. These results will be presented later
in this report.
Although the aforementioned situation has no effect in the final
result of the Direct Kinematics com-
putation, it does imply a few changes in the application of the
Newton-Euler method to estimate the
robot’s dynamic. This issue will be addressed later on the
report.
Still taking only the mobile platform into consideration, the
following parameters are added:
Table 2.2: Denavit-Hartenberg parameters - Platform 2
Link di θi ai αi
6 0 θq − α 0 π2
7 dbase 0 0 −π2
8 0 −π + α 0 0
In order to explain the above defined parameters, it is
necessary to be aware of the fact that the ma-
nipulator’s base centre is not coincident with the centre of the
wheels’ axis. To move the reference frame
from the latter to the former (point 1 to point 2 in image 2.1),
a rotation by a fixed angle α is required,
followed by a translation of module dbase from the centre of the
wheel axis to the base of the manipulator.
Each of the individual transformations originated by the
different lines of the added parameters can
be summarized the following way:
• 1st: Rotation by θq, followed by a set of rotations that make
the Z axis point to the base of the
manipulator
• 2nd: Translation of the frame to the base of the manipulator,
followed by a rotation the makes the
Z axis point up
• 3rd: Alignment of the Y axis with the axis of the wheels
(becoming the two axis parallel)
9
-
2.2 Manipulator
The manipulator consists of one revolute joint and two prismatic
ones. Adopting the convention defined
in [17], two cubes will be used to represent the prismatic
joints and a cylinder will be used to represent
the revolute one. A diagram of the robotic arm is presented:
rotq
rq
zq
rotq
Figure 2.2: Diagram of the Robotic Arm
The corresponding Denavit-Hartenberg parameters are defined
as:
Table 2.3: Denavit-Hartenberg parameters - Robotic Arm
Link di θi ai αi
9 0 rotq 0 0
10 zq 0 0 −π2
11 rq 0 0 −π2
12 ee 0 0 0
Taking into account the configuration of the manipulator, the Z
axis of the final reference frame is
always pointing downwards (in the opposite direction of the
original one).
10
-
Putting all of the aforementioned parameters together, the
complete Denavit-Hartenberg definition of
the studied mobile manipulator becomes:
Table 2.4: Denavit-Hartenberg parameters - Mobile
Manipulator
Link di θi ai αi
1 0 0 0 −π2
2 yq −π2 0 −π2
3 xq −π2 0 −π2
4 0 −π2 0 0
5 hplat 0 0 0
6 0 θq − α 0 π2
7 dbase 0 0 −π2
8 0 −π + α 0 0
9 0 rotq 0 0
10 zq 0 0 −π2
11 rq 0 0 −π2
12 ee 0 0 0
As mentioned before, the definition of these parameters is
extremely important to compute the robot’s
Direct Kinematics. This issue will be approached in the
following section.
11
-
Chapter 3
Direct Kinematics
3.1 Operational and Joint Coordinates
In order to make the printing possible, the robot’s end-effector
has to follow accurately the desired tra-
jectory. This trajectory is expressed in terms of the system’s
operational coordinates ζi. Consequently,
it becomes necessary to convert these coordinates into the
corresponding generalized ones, that is, the
corresponding joint coordinates. This process is denominated
Inverse Kinematics.
However, for this operation to be possible, the opposite process
must be defined first. It is named
Direct Kinematics and consists on the expression of the robot’s
given generalized coordinates of the
end-effector (q) in terms its operational coordinates (ζ). The
following generalized coordinates are con-
sidered:
xq
yq
θq
rotq
zq
rq
=
q1
q2
q3
q4
q5
q6
= q (3.1)
Where x and y are the distances covered by the mobile platform
and θ is the angle between the
perpendicular to the wheel axis and the X axis of the world
reference frame. These coordinates can
be divided into two groups: the ones referring to the mobile
platform and the ones referring to the
manipulator.
xq
yq
θq
=q1
q2
q3
= qprotq
zq
rq
=q4
q5
q6
= qm (3.2)
12
-
That is,
qpqm
= q (3.3)
The operational coordinates are defined the following way:
x
y
z
e1
e2
e3
=
ζ1
ζ2
ζ3
ζ4
ζ5
ζ6
= ζ (3.4)
To emphasize the difference between generalized coordinates (ζ)
and the operational ones (q), all of
the variables attributed to joint coordinates are assigned the
index q. For example, x, y and z represent
the operational coordinates that characterize the end-effector’s
position, whereas xq and yq represent
the position of the central point of the wheel axis of the
Rasteirinho and zq represents the position of the
robot’s vertical joint.
The last three parameters of the operational coordinates are
defined as a ZY X set of Euler angles.
Having in mind the manipulator’s configuration, the Z axis of
the final reference frame will always be
pointing in the opposite direction of the original Z axis (that
is, downwards). The final Y axis will be
parallel as the axis corresponding to the rq prismatic joint,
but with opposite direction. Therefore, ζ5 will
be constant and equal to 0, while ζ6 will also be constant but
equal to 180o.
The Direct Kinematics of the mobile manipulator can be
calculated in one the following two ways:
• Direct Analysis of the robot’s configuration
• Multiplication of the several Transformations Matrices
obtained through the Denavit-Hartenberg
Parameters
Both methods will be applied. If correct, the same results
should be obtained with both methods.
13
-
3.2 Direct Analysis
Through the direct analysis of the mobile manipulator, the
following equations were obtained:
ζ1 = xq + dbase ∗ cos(−α−π
2+ θq) + rq ∗ cos(−
π
2+ θq + rotq)
ζ2 = yq + dbase ∗ sin(−α−π
2+ θq) + rq ∗ sin(−
π
2+ θq + rotq)
ζ3 = zq + hplat+ ee
ζ4 = −π + θq + rotq
ζ5 = 0
ζ6 = π
(3.5)
3.3 Transformation Matrices
Each of the lines that represent the Denavit-Hartenberg
parameters has a corresponding transformation
matrix. This matrix contains both the Rotation matrix and the
position vector between the reference
frames of two consecutive joints. These matrices are obtained
through the following relation:
Ai−1i (qi) =
cos(θi) −sin(θi) ∗ cos(αi) sin(θi) ∗ sin(αi) ai ∗ cos(θi)
sin(θi) cos(θi) ∗ cos(αi) −cos(θi) ∗ sin(αi) ai ∗ sin(θi)
0 sin(αi) cos(αi) di
0 0 0 1
(3.6)
Where i is the corresponding link. The described transformation
between the reference frames of
two consecutive joints is illustrated in the following
image:
Figure 3.1: Transformation between two consecutive reference
frames and corresponding Denavit-Hartenberg parameters [17]
14
-
The direct kinematics of the robot can be obtained by
multiplying the several transformation matrices:
T 0n(q) = A01(q1)A
12(q2)...A
n−1n (qn) (3.7)
After the application of both methods, several tests were made
to confirm the validity of the model.
One of those examples is exposed in the following section.
3.4 Validation
As mentioned before, the validity of the model can be confirmed
if both methods have the same results.
In order to test this, the joint coordinate values of the robot
in a specific configuration were measured.
The following values were obtained: xq = 0mm, yq = 0mm, θq = 0o,
rotq = 0o, zq = 150mm, rq =
200mm, α = −52.11o, dbase = 123.4196mm, hplat = 70mm and ee =
0mm. Replacing these numbers
in the direct kinematics function obtained as described in 3.6
and 3.7, the end-effector’s position vector
(Pee) and Rotation Matrix (Ree) are the following:
Ree =
−1 0 0
0 1 0
0 0 −1
Pee =−97.4017
−275.7978
145
(3.8)
The same position vector was obtained when using the direct
analysis equations. After converting
the calculated Euler angles to the corresponding Rotation
matrix, it was possible to verify that the results
were also the same regarding the reference frame’s orientation.
These results were all corroborated by
human made measures. Similar conclusions were drawn for any of
the values’ sets tested.
The equations obtained in the direct analysis section are also
useful to make the calculation of the
Jacobian easier. This topic will be explored in the following
chapter.
15
-
Chapter 4
Jacobian and Redundancy Analysis
4.1 Jacobian
The Jacobian is a matrix of paramount importance in the
definition of the system’s inverse kinematics,
because it defines the contribution of the various joint
velocities to each of the operational velocities.
It is formed by the first order derivatives of the direct
kinematics’ equations exposed in the preceding
chapter, more specifically in 3.5.
To summarize:
ζ̇ = J(q) ∗ q̇ (4.1)
Jij =δfiδqj
(4.2)
After the application of the respective derivatives, the
following Jacobian matrix is obtained:
J(q) =
1 0 dbase ∗ sin(α− θq + π2 )− rq ∗ sin(θq + rotq −π2 ) −rq ∗
sin(θq + rotq −
π2 ) 0 cos(θq + rotq −
π2 )
0 1 dbase ∗ cos(α− θq + π2 ) + rq ∗ cos(θq + rotq −π2 ) rq ∗
cos(θq + rotq −
π2 ) 0 sin(θq + rotq −
π2 )
0 0 0 0 1 0
0 0 1 1 0 0
0 0 0 0 0 0
0 0 0 0 0 0
(4.3)
The last two lines are composed of zeros only due to the fact
that both ζ5 and ζ6 are constant.
16
-
4.2 Redundancy
Much of the contents of this and of the ensuing section
(Constraints) are based on the work presented
in [18]. The maximum rank of the previously presented Jacobian
is defined as M(q). In this case,
M(q) = 4. The number of existing joints (xq, yq, θq, rotq, zq,
rq) is defined as Nj and is equal to 6.
Therefore, the number of existing joints is bigger than the
number of operational velocities available,
which leads to each operational velocities’ vector (ζ) having
more than 1 unique correspondent joint
velocities’ vector (q). Formally, we can classify a robot as
redundant if Nj is bigger than M , being
R = Nj −M the redundancy degree. If the rank of the Jacobian is
lower than its maximum value for a
given configuration q, we designate that point as a kinematic
singularity. The study of these singularities
is determinant to the performance of the robot, because they can
lead to any of the listed problems:
• Reduced Mobility in a given configuration
• Infinite Solutions for the Inverse Kinematics problem
• Small velocities in the proximity of singularities can
increase drastically to values considered too
high
The methods used to deal with these issues will be presented in
the Inverse Kinematics chapter.
4.3 Constraints
A constraint is defined as holonomic when it is formed by a
restriction that depends only on position
variables (and time in some cases). Specifically, an holonomic
constraint is one that can be expressed
as:
hi(q) = 0 i = 1, ..., k < Nj (4.4)
Analysing the Rasteirinho’s configuration, we verify that there
is a restriction that keeps it from moving
in the direction of the axis that passes through both wheel
centres (in the Y axis direction). It is named
the ”rolling without slipping condition”. It is a non-holonomic
constraint and as such doesn’t imply loss of
accessibility in the Rasteirinho’s configuration space. This
restriction can be represented by the following
equation:
ẋq ∗ sin(θq)− ẏq ∗ cos(θq) = 0 (4.5)
Expressing this constraint in its Pfaffian form:
Ap(qp) ∗ q̇p = 0 (4.6)
17
-
Where the index p indicates that only the mobile platform
coordinates are being considered. Ap and
q̇p are defined as:
Ap(qp) =[sin(θq) cos(θq) 0
]q̇p =
ẋq
ẏq
θ̇q
(4.7)
The mobility index of the mobile platform is equal to 3
(considering the 3 × 1 dimension of q̇p) and it
is subjected to one non-holonomic constraint. This leads to the
Rasteirinho only having two admissible
generalized velocities (3− 1), η1 and η2 (also called
quasi-velocities).
ηp and q̇p are related the following way:
q̇p = Sp(qp) ∗ ηp ηp = STp (qp) ∗ q̇p (4.8)
STp (qp) is defined as:
STp (qp) =
cos(θq) sin(θq) 00 0 1
(4.9)
The vectors forming the lines of STp are both basis of the null
space of the matrix associated with the
constraint Ap.
The quasi-velocities ηp can be defined as:
ηp =
vqθ̇q
(4.10)
Where vq represents the linear velocity of the Rasteirinho and
θ̇q its angular speed.
Summarizing:
ẋq
ẏq
θ̇q
=cos(θq) 0
sin(θq) 0
0 1
∗ vqθ̇q
(4.11)
This concept can extended to the whole mobile platform plus
manipulator system. Considering no
additional constraints, the constraint matrix A would
become:
A =[Ap(qp) 01×Nj
](4.12)
18
-
And the S(q) would become:
S(q) =
Sp(qp) 0Nj×Nj0Nj×Nj INj
(4.13)
Where INj is an identity matrix of order 3.
The vector of quasi-velocities can be defined as:
η =
ηpq̇m
(4.14)
As stated before, the term q̇m represents the joint velocities
of the manipulator. Similarly to the way
q̇p was defined, q̇ is expressed by the equation:
q̇ = S(q) ∗ η (4.15)
As a result:
ẋq
ẏq
θ̇q
˙rotq
żq
ṙq
=
cos(θq) 0 0 0 0
sin(θq) 0 0 0 0
0 1 0 0 0
0 0 1 0 0
0 0 0 1 0
0 0 0 0 1
∗
vq
θq
˙rotq
żq
ṙq
(4.16)
It is possible to verify that the S matrix defines a functional
relationship between the generalized
coordinates velocities and the quasi-velocities. As such, it is
also possible to define a Jacobian matrix
that relates the quasi-velocities with the operational
coordinates velocities:
Jqv(q) = J(q) ∗ S(q) (4.17)
Which allows us to obtain:
ζ̇ = Jqv(q) ∗ η (4.18)
19
-
The final Jqv matrix can then be defined as:
Jqv(q) =
cos(θq) dbase ∗ sin(α− θq + π2 )− rq ∗ sin(θq + rotq −π2 ) −rq ∗
sin(θq + rotq −
π2 ) 0 cos(θq + rotq −
π2 )
sin(θq) dbase ∗ cos(α− θq + π2 ) + rq ∗ cos(θq + rotq −π2 ) rq ∗
cos(θq + rotq −
π2 ) 0 sin(θq + rotq −
π2 )
0 0 0 1 0
0 1 1 0 0
0 0 0 0 0
0 0 0 0 0
(4.19)
The previously discussed concepts of redundancy and
singularities continue to be valid after this
transformation. However, the redundancy degree R is now equal to
1.
20
-
Chapter 5
Inverse Kinematics
The goal of this section is to define an Inverse Kinematics
algorithm, whose purpose is to specify any
trajectory expressed in terms of the end-effector’s ζ
coordinates (and their respective velocities) in the
corresponding joint positions, velocities and accelerations. A
closed-loop model aiming to minimize the
error between the obtained and the desired end-effector’s
trajectory is proposed, based on the model
presented in [17].
5.1 Algorithm - Theoretical Model
5.1.1 Inversion of the Jacobian
In the first place, the inverse of the Jacobian Matrix needs to
be defined. Being a non square matrix, a
direct inversion is not possible. Alternatively, we can define
its Right Pseudoinverse as:
J−1r = JT (JJT )−1 (5.1)
This solution locally minimizes the norm of the joint
velocities. However, in the neighbourhood of
singularities, the solutions obtained by multiplying this matrix
by the operational velocities might not be
viable (too high). To work around this problem, we add an
additional term to the equation. By doing so,
the Damped Least Squares Inverse is defined:
J−1 = JT (JJT + k2 ∗ I)−1 (5.2)
Naturally, this equation can be applied to both the original
Jacobian J(q) and the quasi-velocities
one Jqv(q). The k factor will be selected experimentally taking
into consideration the correct balance
between the pros and cons of the algorithm, explained in the
ensuing paragraph.
21
-
As mentioned, this method has its advantages, but also a few
disadvantages. It allows a smoother
evolution of the obtained velocities, especially in the
neighbourhood of singularities (the bigger the gain
k, the smoother it gets). However, the increase of k also
increases the error between the desired and
the computed velocities. In the case of the studied robot
specifically, the impact of this disadvantage
was not significant. After numerous simulation runs with
different constant gains, an appropriate balance
between the smoothness of the obtained velocities and the
correspondent error was achieved. It is im-
portant to mention that usually the gain k is not considered
constant, but rather a significantly small value
that increases in the neighbourhood of singularities. Naturally,
had this variable gain been considered
the obtained trajectory would be closer to the desired one.
However, and taking into account the already
satisfactory results obtained with a constant gain, the variable
gain approach was not considered. The
results will be exposed posteriorly, in section 5.2.1.
5.1.2 Model
Having calculated the Jacobian’s Damped Least Squares Inverse
(solving the singularities problem), it
is now possible to address the inverse kinematics problem.
The mobile manipulator is redundant (R = 1), so to each set of
end-effector’s velocities, positions
and accelerations there is an infinite number of possible
solutions in terms of joint coordinates. The so-
lution is to approach this problem as a Linear Programming one.
This means that the goal is to minimize
a certain objective function subjected to linear restrictions.
To do so, the Lagrange Multipliers method is
applied.
Firstly, the objective function is defined:
χ′(q̇) =1
2∗ (q̇ − q̇0)T (q̇ − q̇0) (5.3)
The imposed restriction is:
q̇ = J−1(q)ζ̇r (5.4)
Where ζ̇r represents the resulting velocity of the end-effector.
Therefore, the intention is to minimize
the norm of q̇ − q̇0 without violating the mentioned
restriction. q̇0 will be chosen so as to satisfy an
additional constraint, whose definition will be later
approached. Adding the Lagrange multiplier term to
the objective function, the following Lagrange function is
obtained:
Γ ′(q̇, λ) =1
2∗ (q̇ − q̇0)T (q̇ − q̇0) + λT (ζ̇r − Jq̇) (5.5)
22
-
As previously mentioned, the goal is to minimize the objective
function χ′. If a certain (q̇, q̇0) point is
an extremity of the original problem, then there is a λ value
for which a corresponding (q̇, q̇0, λ) point is a
stationary point of Γ . In other words, this is a point for
which the partial derivatives of Γ are 0. In order
to find it, the following conditions are defined:
(δΓ ′
δq̇
)T= 0
(δΓ ′
δλ
)T= 0 (5.6)
They are applied to the Lagrange function, and after both
resulting equations are putt together and
developed, the result is:
q̇ = J−1(q)ζ̇r + (I − J−1J)q̇0 (5.7)
5.1.3 Closed Loop
Considering ζ̇d the desired velocity, the error is defined
as:
ė = ζ̇d − ζ̇r (5.8)
Having in mind the direct kinematics equation:
ζ̇r = J(q)q̇ (5.9)
The error then becomes:
ė = ζ̇d − J(q)q̇ (5.10)
Using equation 5.7, the obtained solutions would eventually
diverge from the desired ones. Consid-
ering this, a few changes are made to the joint velocity vector
5.4, that becomes:
q̇ = J−1(q)(ζ̇d +KP e) (5.11)
With KP being a positive definite matrix.
23
-
Solving equations 5.10 and 5.11 for q̇ and putting them
together, the following system is obtained:
ė+KP e = 0 (5.12)
Which means that the error converges to zero, proving that the
above system is asymptotically stable.
The demonstration presented in the model section for the
redundant mobile manipulator applied to this
new joint accelerations restriction (5.11) then leads to final
inverse kinematics algorithm:
q̇ = J−1(q)(ζ̇d +KP e) + (I − J−1J)q̇0 (5.13)
However, and after several tests were conducted, it was possible
to verify that the results obtained
when the second term ((I−J−1J)q̈0) of equation 5.13 was not
considered were very satisfactory. Having
this in mind, and considering the problems that arose during the
implementation of this term (mainly due
to the difficulties faced in stabilizing the error), a choice
was made not to consider it. It is important to
emphasize the fact that this term does not render the system
unstable, and that it was only dropped for
ease of implementation while taking into account the already
satisfactory obtained results.
A block diagram of the final inverse kinematics scheme is
presented:
Dir_Kin
Figure 5.1: Inverse Kinematics: Block Diagram
It is possible to verify in the image that both the previously
defined quasi-velocities’ Jacobian and the
S matrix are used. This issue will be approached in the
validation section.
24
-
5.2 Validation
In order to test the validity of the model, a reference
trajectory lasting 50 seconds was created. It is
important to emphasize the idea that this trajectory will only
be used to test the validity of the Inverse
Kinematics algorithm. This happens due to the fact that the
robot’s limitations were not taken into ac-
count during its creation (issues like joint limits or the
motors’ saturation, for example).
As explained previously, ζ5 and ζ6 will be kept constant at 0
and π, respectively. Throughout the
simulation, there will be periods of time (t) in which the
remaining reference coordinates will be kept
constant and others in which they will not. Every time one of
these coordinates is time-varying, the
functions describing this variation are cubic polynomial
equations:
q(t) = a3 ∗ t3 + a2 ∗ t2 + a1 ∗ t+ a0 (5.14)
With the respective velocities being:
q̇(t) = 3 ∗ a3 ∗ t2 + 2 ∗ a2 ∗ t+ a1 (5.15)
The desired initial and final values (of both time and position)
chosen to build these time-dependant
equations are presented in the table:
Table 5.1: Reference Trajectory - Inverse Kinematics.
Reference ζ t = 0 t = 30 t = 40 t = 50
ζ1 −97.4 −97.4 2.6 2.6
ζ2 −275.8 −275.8 −275.8 −275.8
ζ3 145 200 200 200
ζ4 −π −π −π −π
Where the shown values are presented in millimetres and radians,
according to the case.
25
-
Although the initial values might seem somewhat random at first,
it is necessary to take into con-
sideration that the values attributed to the joint coordinates
must be coherent with the ones attributed
to the operational coordinates. In order to guarantee this, an
initial set of joint coordinates was se-
lected. Afterwards, the previously explained direct kinematics
algorithm was applied to this set of joints,
resulting in the initial operational coordinates values. These
are presented in the previous table, and
are obtained by applying the direct kinematics algorithm to the
following set of initial generalized co-
ordinates: xq = 0mm, yq = 0mm, θq = 0o, rotq = 0o, zq = 150mm,
rq = 200mm, with α = 52.11o,
dbase = 123.4196mm, hplat = 70mm and ee = 75mm.
Two more aspects are worth mentioning regarding this algorithm.
The first is that the quasi-velocities’
Jacobian defined in 5.16 is used. By doing so, we obtain the
mobile platform’s movement in terms of
linear and angular velocity (instead of ẋq,ẏq and θ̇q), which
will facilitate the implementation of the trajec-
tory in the real robot.
The second aspect worth mentioning is the calculation of the
acceleration reference values, neces-
sary to execute the control law to be later discussed. Being an
offline simulation, none of the problems
regarding the use of derivatives in real applications arise
(namely the occurrence of sudden disturbances
that can cause the derivative value to burst). Therefore, the
acceleration values are obtained through
the differentiation of the computed velocity values.
5.2.1 Results
Although the obtained results were quite satisfactory, one
problem arose. The low complexity of the
robot’s configuration made it necessary to control 4 operational
coordinates having only 5 available
joints. The resulting low manoeuvrability of the robot made it
impossible to achieve insignificant errors in
both the position and the orientation coordinates. In other
words, the reduction of the error in a specific
operational coordinate had to be done at the expense of the
error in some other coordinate.
Considering the function of the robot (3D printing), it becomes
obvious that as long as the end-
effector’s position is the correct one (x, y and z) in each time
step the printing will be done correctly,
regardless of its orientation. For this reason, the feedback
gain (defined in KP ) regarding the orientation
error was set to 0. This allowed for a quite significant
reduction of the position errors.
26
-
After testing, the following gain parameters were chosen. KP is
the gain used in the inverse kine-
matics algorithm defined in 5.13 and k is the damping factor
used in the computation of the inverse of
the Jacobian (5.2).
KP =
2 0 0 0 0 0
0 2 0 0 0 0
0 0 2 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
k = 0.01 (5.16)
Where k is the coefficient used to smooth the velocities’
transition near singularities in the definition
of the jacobian’s inverse.
Using these parameters, the following results were obtained:
0 10 20 30 40 50 60
Simulation Time
-2
0
2
4
6
8
10
12
14
Err
or in
mm
×10-5Error associated w\Position Operational Coordinates
xyz
Figure 5.2: Error Associated with Position Operational
Coordinates
27
-
0 10 20 30 40 50 60
Simulation Time
-0.35
-0.3
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
Err
or in
Rad
ians
Error associated w\ the Unrestrained Euler Angle
euler1
Figure 5.3: Error Associated with the Unrestricted Euler
Angle
The remaining Euler angles are fixed, so naturally no
information regarding these variables is pre-
sented. It is important to reinforce the idea that only the
end-effector’s position is important to make
the printing precise and accurate, regardless of the
end-effector’s orientation. As such, and even if it
meant having a bigger orientation error (as can be seen in 5.3),
only the position errors were taken into
account. Having this in mind, the obtained results are very
satisfactory, seeing that the errors associated
with the position operational coordinates have an order of
magnitude of 10−4mm. As expected, both the
position and the orientation errors stabilize. In order to
better visualize the obtained results, both the
reference and the obtained trajectories regarding the x
coordinate are presented:
Figure 5.4: x: Reference vs Obtained Trajectory
28
-
At last, the obtained quasi-velocities are presented, with the
intent of showing their smooth evolution
throughout time:
0 10 20 30 40 50 60
Simulation Time
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3V
eloc
ity in
mm
/sPrismatic Joints Velocity
lin velz velr vel
Figure 5.5: Prismatic Joints Velocity
0 10 20 30 40 50 60
Simulation Time
-0.01
0
0.01
0.02
0.03
0.04
0.05
Vel
ocity
in r
ad/s
Revolute Joints Velocity
ang velrot vel
Figure 5.6: Revolute Joints Velocity
29
-
Chapter 6
Dynamics
6.1 Newton-Euler Formulation
In order to estimate the robot’s dynamic model the Newton-Euler
method was applied. This recursive
algorithm was chosen due to its computational efficiency. It is
based on a balance of the acting forces
and moments in each of the robot’s links and it can be divided
in two stages. The first consists of a
forward recursion from the first joint to the end-effector, that
calculates all the velocities and accelerations
corresponding to each of the joints. In a second phase, a
backward recursion takes place (from the end-
effector to the first joint). This recursion determines all the
forces and torques the joints are subjected
to. The following nomenclature is introduced:
• wii is the angular velocity of the link
• ẇii it the angular acceleration of the link
• p̈ii the linear acceleration of the origin of frame i
• p̈iCi is the linear acceleration of the centre of mass i
• ẇi−1mi is the angular acceleration of the rotors
• The Ri−1i matrices are the rotation matrices associated with
link i
• f ii is the force applied by link i− 1 on link i
• µii is the moment applied by link i− 1 on link i
• τi is the generalized force applied in joint i, resulting from
the sum of either the projection of f iior µii along the joint axis
with the rotor inertia torque
30
-
The above mentioned recursions are performed based on the
following calculations:
Velocities and Accelerations (Forward Recursion):
wii =
Ri−1 Ti w
i−1i−1 Considering a Prismatic Joint
Ri−1 Ti (wi−1i−1 + ϑ̇iz0) Considering a Revolute Joint
(6.1)
ẇii =
Ri−1 Ti ẇ
i−1i−1 Considering a Prismatic Joint
Ri−1 Ti (ẇi−1i−1 + ϑ̈iz0 + ϑ̇iw
i−1i−1 × z0) Considering a Revolute Joint
(6.2)
p̈ii =
Ri−1 Ti (p̈i−1i−1 + d̈iz0) + 2ḋiw
ii ×Ri−1 Ti z0
Considering a Prismatic Joint+ ẇii × rii−1,i + wii × (wii ×
rii−1,i)
Ri−1 Ti p̈i−1i−1 + ẇ
ii × rii−1,i
Considering a Revolute Joint+ wii × (wii × rii−1,i)
(6.3)
p̈iCi = p̈ii + ẇ
ii × rii,Ci + w
ii × (wii × rii,Ci) (6.4)
ẇi−1mi = ẇi−1i−1 + kriq̈iz
i−1mi
+ kriq̇iwi−1i−1 × zi−1mi (6.5)
Forces and Torques (Backward Recursion):
f ii = Rii+1f
i+1i+1 +mip̈
iCi
(6.6)
µii = −f ii × (rii−1,i + rii,Ci) +Rii+1µ
i+1i+1 +R
ii+1f
i+1i+1 × rii,Ci (6.7)
+ Ii
iẇii + w
ii × (I
i
iwii) + kr,i+1q̈i+1Imi+1z
imi+1
+ kr,i+1q̇i+1Imi+1wii × zimi+1
τi =
f i Ti Ri−1 Ti z0 + kriImiẇ
i−1 Tmi
zi−1miConsidering a Prismatic Joint
+ Fviḋi + Fsisgn(ḋi)
µi Ti Ri−1 Ti z0 + kriImiẇ
i−1 Tmi
zi−1miConsidering a Revolute Joint
+ Fviϑ̇i + Fsisgn(ϑ̇i)
(6.8)
31
-
Where p represents the reference frame position vector in terms
of operational coordinates, w is
the angles’ vector in terms of operational coordinates and d and
ϑ represent the joint coordinates q
corresponding to prismatic and revolute joints, respectively.
The forces obtained using the Newton-Euler
method represent the right side of the following equation, that
defines the dynamic of the system:
B(q)q̈ + C(q, q̇)q̇ + Fvq̇ + Fssgn(q̇) + g(q) = τ − JT (q)he
(6.9)
Where the following nomenclature is adopted:
• B(q) is the Inertia Matrix
• C(q, q̇) is the matrix containing both the centripetal forces
and the Coriolis ones
• g(q) is the Gravitational forces vector
• τ is the actuating torques and forces vector
• Fv represents the viscous friction forces
• Fs represents the Coulomb friction forces
• JT (q)he represents the forces and torques applied by the
end-effector in the surrounding envi-
ronment
Disregarding the effects of the viscous friction, the Coulomb
friction and the forces and torques
applied by the end-effector in the surrounding environment, we
can simplify the dynamic of the system:
B(q)q̈ + C(q, q̇)q̇ + g(q) = τ (6.10)
After calculating the forces (left side of the equation), the
matrices B(q),C(q, q̇) and the vector g(q)
are determined:
• g(q) is obtained by replacing all the joint velocities and
accelerations by 0 in the final forces vector
• B(q) is obtained by differentiating each of lines of the final
forces vector with respect to the
various joint accelerations
• C(q, q̇) multiplied by the velocities vector is obtained by
replacing all the joint accelerations by 0
in the final forces vector and subtracting g(q)
The calculations exposed in the next section do not take into
account the dynamic of the motors
existing in the robot.
32
-
6.1.1 Mobile Platform
As mentioned before, there is not a direct correspondence
between the number of joints and the number
of sets (lines) of Denavit-Hartenberg parameters. This happens
due to the fact that the studied system
is a mobile manipulator, and several approximations had to be
made in order to compute the direct kine-
matics with as much precision as possible.
To work around this problem, the following methodology was
adopted:
• In the forward recursion to calculate the velocities and
accelerations of each point, we consider
each line of the Denavit-Hartenberg parameters matrix to be a
joint. This step is extremely impor-
tant, because even though some of these ”joints” are fictional
and not considered in the backward
recursion to calculate the forces they are fundamental to obtain
the velocities and accelerations
with the desired precision.
• When dealing with fictional joints, the corresponding
generalized velocities and accelerations
(ḋ,d̈,ϑ̇ and ϑ̈) are considered 0.
• In the backward recursion to calculate the forces and torques,
all the masses and inertia matrices
associated with fictional joints are considered 0.
• In the end, select only the lines of the final forces vector
corresponding to real joints.
Having in mind the constraints indicated in the Inverse
Kinematics section, it becomes of the utmost
importance to express the dynamics of the system in terms of
linear and angular acceleration, instead of
doing it in terms of ẍq, ÿq and θ̈q. To do so, a few changes
were made to the Denavit-Hartenberg matrix
of the robot’s parameters, that become the following:
Table 6.1: Denavit-Hartenberg parameters - Platform Dynamic
Model
Link di θi ai αi
1 hlpat π2 0 0
2 0 θq 0π2
3 d π2 0 −π2 − α
4 dbase −π2 0 −π2
5 0 −π − α 0 0
33
-
In is important to reinforce the idea that these parameters are
used only during the estimation of
the mobile platform’s dynamic, whereas in the inverse kinematics
computation and in the control run
the original parameters are used. In this case, only two real
joints are considered (the remaining are
fictional, and the before mentioned procedure to deal with this
issue is applied). The term d represents
the travelled distance of the Rasteirinho. The current sequence
of fictional joints has the same goal as
the sequence presented in the Denavit-Hartenberg parameters
chapter, but adapted so as to fit the new
parameter d. The vectors were defined the following way:
P 10,1 =
−hplat
0
0
P 11,C1 =
0
0
0
P 21,2 =
0
0
0
P 22,C2 =
0
0
0
P 32,3 =
0
0
0
P 33,C3 =
0
0
0
P 43,4 =
0
−dbase
0
P 44,C4 =
0
0
0
P 54,5 =
0
0
0
P 55,C5 =
0
0
0
(6.11)
The rotation matrices are the ones corresponding to each of the
lines of the Denavit-Hartenberg
parameters. As expected the result was the following:
g ∗mRastθ̈q ∗ IRastd̈ ∗mRast
0
0
(6.12)
The first line was predictable, because the approximations made
imply that the Rasteirinho’s centre
of mass moves along with the reference frame from the ground
level to the platform’s height. This joint is
fictional and shall not be considered. The second and the third
lines are the only ones corresponding to
real joints, and therefore are the only to be selected. They
correspond to a revolute and a prismatic joint,
respectively. As expected, these results correspond to Newton’s
second law and to the torque formula.
The remaining lines correspond to fictional joints and are
therefore ignored.
34
-
6.1.2 Manipulator
The manipulator consists of three joints: one revolute followed
by two prismatic ones. The parameters
for the three joints are defined as follows:
3rd Joint - Revolute:
P 65,6 =
0
0
0
P 66,C6 =
0
0
dcm1
(6.13)
4th Joint - Prismatic:
P 76,7 =
0
−zq0
P 77,C7 =
0
zq − dcm20
(6.14)
5th Joint - Prismatic:
P 87,8 =
0
−rq0
P 88,C8 =
0
rq − dcm30
(6.15)
Where dcm1 , dcm2 and dcm3 represent the distances from the
final reference frame of the joints to
their mass centres (3rd,4th and 5th joints respectively). The
rotation matrices used are the ones corre-
sponding to the 9th, 10th and 11th lines of the original
Denavit-Hartenberg parameters matrix.
Considering only five joints (two replicating the movement of
the platform plus three forming the
manipulator), all the obtained matrices and vectors (B(q), C(q,
q̇) and g(q)) have the desired dimensions.
Several tests were made with various inputs (random sets of
generalized coordinates and velocities) and
in every one of them the Inertia Matrix B(q) came out symmetric,
which indicates a correct application
of the method. Further validation can only be achieved during
the actual control run.
35
-
Chapter 7
Control Algorithm
7.1 Inverse Dynamics Control
The first control method applied to the previously obtained
dynamic model is named Inverse Dynamics
Control, and is detailed in [17]. It is a form of centralized
control, where the whole dynamic of the system
is described by a single block, instead of considering each
joint an independent system (decentralized
control). As such, and being the system formed by Nj joints, the
block describing it should have Nj
entry torques and output Nj generalized coordinates and
velocities. This type of control allows for a
more efficient handling of the errors regarding a reference
trajectory caused by external disturbances e
by non-linear terms existing in the dynamic of a system.
The system’s dynamic model can be described as:
B(q)q̈ + n(q, q̇) = u (7.1)
Where
n(q, q̇) = C(q, q̇)q̇ + F q̇ + g(q) (7.2)
The goal is to find an input vector u (containing the desired
torques) that is able to establish a linear
functional relationship between the inputs and the outputs of
the mobile manipulator block. This is done
through a non-linear feedback of the state of the system.
More specifically, two feedback loops are present in this
technique:
• An inner feedback loop, whose aim is to be a direct
compensation of the non-linear terms of
the system. By doing so, it establishes a linear functional
relationship between the inputs and the
outputs of the manipulator block
• An outer feedback loop, whose aim is to stabilize the system
using position and velocity errors
36
-
Establishing:
q̈d +KD ˙̃q +KP q̃ (7.3)
Where the index d indicates the desired value and the tilde
indicates the error. This vector is the
outer control loop, that multiplies by the inertia matrix.
Adding to this multiplication the inner control loop,
we obtain the u torque vector we wish to apply to the
manipulator. The block diagram of the system is
presented:
Figure 7.1: Block Diagram: Inverse Dynamics Control
Both gain matrices are defined as diagonal with only positive
entries.
7.1.1 Validation
To test this control type, a simple trajectory was created where
all the considered joints velocities are
constant. The desired coordinates therefore have a linear
evolution in time. It is important to mention
that these trajectories do not take into consideration
limitations like joint limits or each joint’s maximum
speed. There are only used to test if the control scheme’s
computed torques lead to an acceptable
evolution of the error.
37
-
The defined trajectories (dq, θq, rotq, zq and rq respectively)
are:
45
30∗ t 30
30∗ t 50
30∗ t 10
30∗ t 5
30∗ t (7.4)
t represents the simulation time, that was defined as being 30
seconds. The gain matrices entries
were chosen based on testing, and were selected as follows:
KP =
2000 0 0 0 0
0 32 0 0 0
0 0 23 0 0
0 0 0 28 0
0 0 0 0 10
(7.5)
KD =
139 0 0 0 0
0 122 0 0 0
0 0 2.8 0 0
0 0 0 10 0
0 0 0 0 3.2
(7.6)
The position errors obtained with these parameters were the
following:
0 5 10 15 20 25 30
Simulation Time
-0.02
0
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Err
or in
mm
Error associated with Prismatic Joints
dzr
Figure 7.2: Position Errors: Prismatic Joints
38
-
0 5 10 15 20 25 30
Simulation Time
-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04E
rror
in R
adia
ns
Error associated with Revolute Joints
thetarot
Figure 7.3: Position Errors: Revolute Joints
As predicted, the position errors stabilize. The initial
overshoot was also expected, considering that
in the beginning of the simulation the actual velocities are
zero, while the desired ones are all positive
constants. Regardless of this, we can observe an acceptable
evolution of the errors throughout the
simulation time.
7.2 Non-linear Model Based Predictive Control
The control method presented in this section was developed by
Adel Merabet and Jason Gu, and can
be found in [19]. As previously mentioned, the dynamic of the
system is defined as:
B(q)q̈ + C(q, q̇)q̇ + g(q) = u (7.7)
There are several sources of uncertainties that can have an
influence on the calculation of the various
parameters in this equation. Of these uncertainties, the
following stand out: modelling errors, unknown
forces applied to the robot, computation errors or the disregard
of several terms during the application
of the Newton-Euler method. Having this issues in mind, the
equation describing the dynamic of the
system undergoes a few changes:
(B0(q) + ∆B)q̈ + (C0(q, q̇) + ∆C)q̇ + g0(q) + ∆g + Fr = u+ b
(7.8)
39
-
Where each of the matrices and vectors obtained in the
Newton-Euler method are now formed by
their nominal values plus the associated uncertainties. The
external disturbances (b) and the friction
forces (Fr) are also added. Simplifying:
B0(q)q̈ + C0(q, q̇)q̇ + g0(q) = u+ δ(q̈, q̇, q, b) (7.9)
Where the last term represents the uncertainties.
Defining:
X =[X1 X2
]T=[q q̇
]T(7.10)
We are able represent to represent the system in its state space
format:
Ẋ = f(X) + p(X)u+ p(x)δ (7.11)
Y = h(X) = CX (7.12)
With the following constituent elements:
f(X) =
X2−B0(X1)−1(C0(X1, X2)X2 + g0(X1))
p(X) = 0Nj×NjB0(X1)
−1
C =[INj×Nj 0Nj×Nj
](7.13)
Putting all of these elements together, it is possible to define
the system containing the outputs and
all of their correspondent derivatives:
Y (t) =
Y (t)
Ẏ (t)
Ÿ (t)
=
X1
X2
−B0(X1)−1(C0(X1, X2)X2 + g0(X1))
+
0Nj×1
0Nj×1
B0(X1)−1u(t)
(7.14)
40
-
The objective is to define a control law u(t) to be applied in
real time. It is a predictive control method,
which means that the control law u(t) designed to follow a
reference trajectory Yd is calculated based on
the predicted output of the system in the ensuing time step: Y
(t+ τ). The term τ designates the sample
time. This is made through the minimization of a cost function,
that depends on the predicted error in
t+ τ :
κ = f(eY (t+ τ)) (7.15)
Through a Taylor expansion using Lie derivatives, it is possible
to predict the output Y of the system
in any given instant using the position, velocity and
acceleration values of the previous instant:
Y (t+ τ) = Y (t) + τ ∗ Ẏ (t) + τ2
2!Ÿ (t) (7.16)
Based on this expansion and on the previously mentioned Y (t)
system, the predictive model is
defined as:
Y (t+ τ) = T (τ)Y (t) (7.17)
With
T (τ) =[INj×Nj τ ∗ INj×Nj τ
2
2∗ INj×Nj
](7.18)
Assuming the reference trajectory is already available in each
time step, the predicted error to be
used in the cost function is:
eY (t+ τ) = Y (t+ τ)− Yd(t+ τ) = T (τ)(Y (t)− Yd(t)) (7.19)
At this point, it is already possible to specify the cost
function. It is based on the quadratic form of
the error in t+ τ :
κ =1
2
∫ τr0
(Y (t+ τ)− Yd(t+ τ))T (Y (t+ τ)− Yd(t+ τ))dτ (7.20)
And it can be simplified as:
1
2(Y (t)− Yd(t))TΠ(Y (t)− Yd(t)) (7.21)
41
-
Being
Π =
∫ τr0
T (τ)TT (τ)dτ =
τr ∗ INj×Nj
τ2r2 ∗ INj×Nj
τ3r6 ∗ INj×Nj
τ2r2 ∗ INj×Nj
τ3r3 ∗ INj×Nj
τ4r8 ∗ INj×Nj
τ3r6 ∗ INj×Nj
τ4r8 ∗ INj×Nj
τ5r20 ∗ INj×Nj
= Π1 Π2
ΠT2 Π3
(7.22)
Differentiating and matching to zero, the optimal control law is
obtained, that after a few changes has
the form:
u(t) = −B0(X1){[
Π−13 ΠT2 INj×Nj
](M(t)− Yd(t))
}(7.23)
With
M(t) =
Y (t)
Ẏ (t)
−B0(X1)−1(C0(X1, X2)X2 + g0(X1))
(7.24)
[Π−13 Π
T2 INj×Nj
]=[ 103τ 2r∗ INj×Nj
5
2τr∗ INj×Nj INj×Nj
](7.25)
And the gain matrices being defined as:
K1 =10
3τ 2r∗ INj×Nj K2 =
5
2τr∗ INj×Nj (7.26)
As previously mentioned, the robot is subjected to
uncertainties, whose characteristics are unknown.
As a way to improve the robustness of the applied algorithm, an
extra term to the control law is added.
Basically, it is a way to compensate those unknown uncertainties
in the original equation, reconstructing
the torque vector u(t) into the following:
u(t) = −B0(X1){K1(Y − Yd) +K2(Ẏ − Ẏd)−B−10 (C(X1, X2)X2 +
g(X1))− Ÿd
}− δest(t) (7.27)
42
-
The uncertainties derivative can be estimated by:
δ̇est = L B−10 (X1)(δ − δest)
= −L B−10 (X1)δest + L(Ÿ +B−10 (X1)C0(X1, X2)Ẏ +B
−10 (X1)g0(X1)−B
−10 (X1)u)
(7.28)
Substituting the term u in this equation by the original control
law (without the uncertainties):
δ̇est(t) = L(ëY (t) +K2ėY (t) +K1eY (t)) (7.29)
That integrated leads to:
δest(t) = L
(ėY (t) +K2eY (t) +K1
∫eY (t)dt
)(7.30)
Substituting this estimation of the uncertainties in equation
7.27, we obtain the final control law.
7.2.1 Validation
Applying the defined method to same trajectory created in the
Inverse Dynamics Control section (linear
evolution of the generalized coordinates with time), the
following results were obtained:
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Simulation Time
-0.02
0
0.02
0.04
0.06
0.08
0.1
0.12
Err
or in
mm
Error associated with Prismatic Joints
dzr
Figure 7.4: Position Errors: Prismatic Joints
43
-
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
Simulation Time
-2
0
2
4
6
8
10
12
14
16
Err
or in
Rad
ians
×10-3 Error associated with Revolute Joints
thetarot
Figure 7.5: Position Errors: Revolute Joints
In order to facilitate the viewing of the obtained errors, only
the first five seconds of the simulation are
presented when prismatic joints are considered. In the case of
revolute joints, only the first two seconds
are shown. In both graphics, it is possible to verify that the
error stabilizes. As in the previous control
scheme, there is an initial overshoot caused by a difference
between the desired and the actual veloc-
ities in the beginning of the simulation. However, the magnitude
of this initial overshoot is considerably
smaller using the second control law. Besides the error
converges faster to zero and with no oscillations.
Considering the better results obtained, the Non-linear Model
Based Predictive Control is chosen for
further testing (both in simulations and on the actual
robot).
As mentioned before, all the trajectories created with 3−D
printing purposes will initially be expressed
in the end-effector’s operational coordinates. Afterwards, the
previously explained Inverse Kinematics
algorithm will be applied to those same reference trajectories,
expressing them in terms of the sys-
tem’s generalized coordinates. Only then will the control scheme
be employed, using the various joint
coordinates (and velocities) values for feedback. In order to
completely simulate one of these cycles,
the reference trajectories obtained in the Inverse Kinematics
chapter (whose velocities are illustrated in
figures 5.5 and 5.6) were tested, using the selected control
scheme.
44
-
The following results were obtained:
0 10 20 30 40 50 60-5
-4
-3
-2
-1
0
1
2
3E
rror
in m
m×10-4 Error associated with Prismatic Joints
dzr
Figure 7.6: Position Errors: Prismatic Joints
0 10 20 30 40 50 60
Simulation Time
-0.5
0
0.5
1
1.5
2
2.5
Err
or in
Rad
ians
×10-4 Error associated with Revolute Joints
thetarot
Figure 7.7: Position Errors: Revolute Joints
45
-
As in the previous case, we can verify that the error
stabilizes. It is also possible to notice peaks in
the errors obtained in seconds 30 and 40 of the simulation.
These were expected, given that the obtained
acceleration values are not continuous in those points. Despite
the error being considerably bigger in
the proximity of these points than in the rest of the
simulation, it is still considered irrelevant due to its
low magnitude.
It is important to mention that being a simulation, the matrices
used to apply the control law (B(q),
C(q, q̇) and g(q)) were the same as the ones used to simulate
the mobile platform system. Obviously,
when testing in the real robot begins this will not be verified.
The real dynamic model of the robot can
not be estimated with absolute accuracy due to several factors
(unidentified uncertainties, for example)
and consequently the matrices used by the control law will not
correspond exactly to the desired ones.
Taking this into consideration, the errors obtained when testing
in the real system begins are expected
to increase. Despite that, the overall obtained results are
satisfactory.
46
-
Chapter 8
Motor Controllers
8.1 Motors’ Dynamics
The five motors incorporated in the robot are all driven by
MD-25 boards. These drivers already come
with integrated speed control, achieved through the encoders’
feedback of each motor. Consequently,
when a certain set of velocities is intended, instead of
directly controlling the matching voltages a 1
byte value that corresponds to the desired velocity should be
provided to the board (out of 256 possible
values).
The selected control law calculates the torque to apply to the
various joints in each time step. There-
fore, it becomes necessary to find the motor velocities that
correspond to each of those torques. The
MD-25 board will also be important considering it reads the
encoders of the motors, fundamental in the
estimation of the robot’s position and velocities.
Kr is defined as the matrix that relates the angular velocities
of each motor with the respective joint
velocities:
Kri q̇i = ϑ̇mi (8.1)
Naturally, when a joint is prismatic its corresponding value in
Kr is a dimensional quantity. This
matrix is also fundamental to define the functional relationship
between the torques applied to a joint
and the ones provided by the motors:
τm = K−1r τ (8.2)
47
-
The influence of inductance in the motors’ dynamics will be
ignored in an initial stage. Therefore, the
equation describing it is:
K−1r τ = Ktia
va = Raia +Kvϑ̇m
(8.3)
Where Kt is the matrix containing the torque constants, Kv the
matrix containing the voltage con-
stants and R the matrix containing the resistances of the
motors. Since all motors are the same, they
all have the same torque and v