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

Dedicated to my family and friends...

iii

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 Té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

Resumo

A presente dissertação centra-se no desenvolvimento do protótipo de uma impressora 3D móvel, cujo

objetivo é realizar impressões com área ilimitada. Esta é composta por um braço robótico montado na

plataforma de um robô diferencial, sendo que numa fase posterior do projeto a cabeça da impressora

será instalada na extremidade do braço. Como em qualquer outra impressão 3D, após a criação de

um modelo virtual da peça pretendida em software CAD será gerada uma trajetória única para esse

objecto especı́fico, tendo em conta as caracterı́sticas da impressora. Se seguida com precisão pelo

end-effector do sistema, permitirá que a cabeça da impressora deposite as várias camadas de material

da maneira pretendida, levando à criação da peça desejada. O principal foco desta tese é precisamente

o seguimento desta trajetória. Pretende-se que este processo decorra com elevada precisão, de modo

a que os objetos criados se aproximem o mais possı́vel do seu modelo virtual. Serão apresentados

vários estudos realizados em diversas áreas da Engenharia, tais como a Robótica, Controlo e Visão

Computacional, e que têm em comum o objectivo fundamental de fazer o robô mover-se da forma

pretendida. No final, serão apresentados os resultados da implementação do algoritmo desenvolvido.

Palavras-chave: Impressão 3D , Controlo, Manipulador Móvel, Visão Computacional, Sis-temas Não-holonómicos

vii

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

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





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 (Núcleo de

Experimentação Tridimensional) and LIFE (Laboratório de Interfaces Fı́sicas Experimentais), both aca-

demic laboratories belonging to PUC-Rio (Pontifı́cia Universidade Cató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:

ẋq ∗ sin(θq)− ẏ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 =

ẋq

ẏ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:

ẋq

ẏ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:

ẋq

ẏq

θ̇q

˙rotq

żq

ṙ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

żq

ṙ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:

ė = ζ̇d − ζ̇r (5.8)

Having in mind the direct kinematics equation:

ζ̇r = J(q)q̇ (5.9)

The error then becomes:

ė = ζ̇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:

ė+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 ẋq,ẏ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

• ẇ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

• ẇ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)

ẇii =

Ri−1 Ti ẇ

i−1i−1 Considering a Prismatic Joint

Ri−1 Ti (ẇ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) + 2ḋiw

ii ×Ri−1 Ti z0

Considering a Prismatic Joint+ ẇii × rii−1,i + wii × (wii × rii−1,i)

Ri−1 Ti p̈i−1i−1 + ẇ

ii × rii−1,i

Considering a Revolute Joint+ wii × (wii × rii−1,i)

(6.3)

p̈iCi = p̈ii + ẇ

ii × rii,Ci + w

ii × (wii × rii,Ci) (6.4)

ẇi−1mi = ẇ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

iẇ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 + kriImiẇ

i−1 Tmi

zi−1miConsidering a Prismatic Joint

+ Fviḋi + Fsisgn(ḋi)

µi Ti Ri−1 Ti z0 + kriImiẇ

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̇,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 ẍq, ÿ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:

Ẋ = 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)

Ẏ (t)

Ÿ (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) + τ ∗ Ẏ (t) + τ2

2!Ÿ (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)

Ẏ (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(Ẏ − Ẏd)−B−10 (C(X1, X2)X2 + g(X1))− Ÿ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(Ÿ +B−10 (X1)C0(X1, X2)Ẏ +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(ëY (t) +K2ėY (t) +K1eY (t)) (7.29)

That integrated leads to:

δest(t) = L

(ė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


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


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


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


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

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

Documents

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