Post on 27-Jan-2021
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