Mobile Manipulator ControlMobile Manipulator Control António Nunes Henriques Thesis to obtain the...

of 97 /97
Mobile Manipulator Control António Nunes Henriques Thesis to obtain the Master of Science Degree in Mechanical Engineering Supervisors: Prof. Paulo Jorge Coelho Ramalho Oliveira Prof. Carlos Baptista Cardeira Examination Committee Chairperson: Prof. Paulo Rui Alves Fernandes Supervisor: Prof. Carlos Baptista Cardeira Member of the Committee: Prof. Jorge Manuel Mateus Martins June, 2017

Transcript of Mobile Manipulator ControlMobile Manipulator Control António Nunes Henriques Thesis to obtain the...

  • 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