B.A.G. Genuit

DC 2010.008

Bilateral teleoperation using

non-similar robots

Traineeship report

Coach(es): dr. A. Rodríguez Angelesdr. C.A. Cruz Villar

Supervisor: prof. dr. H. Nijmeijer

Eindhoven University of TechnologyDepartment of Mechanical EngineeringDynamics and Control Technology Group

Eindhoven, January 2010

Typeset by the author using LATEX2ε.

This document is best appreciated in digital form (to enjoy hyperlinks) or, second best, printedwith a color printer. When no such printer is available, a hard copy will on request happily besupplied by the author.

For requesting a (digital or hard) copy of this document or the corresponding digital files, makingcomments or remarks, please send an e-mail to the author at bartgenuit@gmail.com.

matlab, Simulink and Real-Time Workshop are trademarks of The Mathworks, Inc., registeredin the U.S. and other countries.

This work (all text and figures unless noted otherwise, see last line) is licensed underthe Creative Commons Attribution 3.0 Netherlands License. To view a copy of thislicense, visit http://creativecommons.org/licenses/by/3.0/nl/ or send a let-ter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California,

94105, USA. Attribution under the license should be to: Eindhoven University of Technology, cin-

vestav and B.A.G. Genuit. Explicitly excluded from the license are figure 1.1 by Peter Hokayemand Mark Spong, figures 2.1 and A.1 by R. Cortes-Martinez and figure 2.2 by D. Muro-Maldonado.

c© 2010, Eindhoven University of Technology. Some rights reserved.

ii

Preface

This report is the result of an internship of three months at the Mechatronics department of theCentro de Investigación y de Estudios Avanzados del Instituto Politécnico Nacional (cinvestav),located in Mexico City, Mexico. dr. Alejandro Rodríguez Angeles and dr. C.A. Cruz Villarare the coaches at cinvestav. The author and his supervisor prof.dr. Henk Nijmeijer are withthe department of Mechanical Engineering at Eindhoven University of Technology (TU/e), theNetherlands.

iii

iv Preface

Abstract

Teleoperation is a research field within control technology with many interesting applications, suchas remotely controlled underwater vehicles or precise micro-invasive surgery. The term ‘teleop-eration’ stands for ‘operation at a distance’. Since the 1950’s, teleoperation systems have beenextensively studied, both in theory and in practice. However, a gap between theory and widespreadapplication still remains to date.

Most existing solutions are based on analytical expressions. This makes it possible to findsolutions mathematically (using Lyapunov theory for example) but tuning the model parametersproves troublesome, which makes it hard to apply at industry level. This research is targetedat designing a teleoperation control scheme based on frf’s (frequency response functions), i.e.measurements, rather than analytical expressions. This more practical approach helps bridgingthe gap from the other side.

The platform used in this research consists of two non-similar robots, a master and a slave.On this platform, a bilateral (i.e. two-way) teleoperation control scheme based on pid controllersis implemented that makes the slave track the position of the master, and makes the mastermimic the force encountered by the slave. In this way, the operator can feel this force on theslave tip via the master. The control scheme is tested using simulations before implementation.Because at implementation time force sensors were unavailable, the encountered force is modeledusing an environment model. Two types of environment models are implemented, an impedance(mass-spring-damper) model and a holonomic (rigid) model.

Simulations and experiments show that the control scheme works well, the tracking errors(position and force) are within acceptable levels. However, finding suitable controller parameterscan be tiresome because of the chosen approach and the nature of the system (hybrid, non-linearand highly inter-coupled).

v

vi Abstract

Samenvatting

Teleoperatie is een onderzoeksgebied binnen de regeltechniek met vele interessante toepassingen,zoals micro-invasieve chirurgie en afstandbestuurbare onderwaterrobots. De term ‘teleoperatie’komt van het Grieks en betekent ‘operatie op afstand’. Vanaf de 50’er jaren wordt dit typesystemen uitgebreid onderzocht, zowel in theorie als praktijk. Wijdverbreide toepassing is echternog niet in zicht.

De meeste huidige oplossingen zijn gebaseerd op analytische vergelijkingen. Hiermee is het mo-gelijk algebraïsche oplossingen te vinden (bijvoorbeeld door middel van de theoriën van Lyapunov),maar het afstellen van de modelparameters is vaak lastig. Hierdoor vinden deze oplossingen vaakgeen navolging in de industrie. Het doel van dit onderzoek is het ontwerpen van een van eenregelschema gebaseerd op frf’s (frequentieresponsfuncties), metingen dus, in plaats van analytis-che vergelijkingen. Deze praktische inslag helpt vanaf de andere kant de kloof tussen theorie enpraktijk te dichten.

Het platform wat in dit onderzoek wordt gebruikt bestaat uit twee niet-gelijksoortige robots,één zogeheten master en een slave. Op dit platform wordt een bilateraal (tweezijdig) regelschemageïmplementeerd gebaseerd op pid-type regelaars. Dit zorgt ervoor dat de slave de positie vande master volgt, en de master de kracht imiteert die de slave ondergaat. Hierdoor kan de op-erator die de master bedient, de kracht op de punt van de slave voelen. Het regelschema wordtgetest door middel van simulaties voordat het wordt geïmplementeerd op het platform. Omdat erten tijde van dit onderzoek geen krachtsensoren beschikbaar waren, wordt gebruik gemaakt vanomgevingsmodellen die de kracht simuleren. Twee typen modellen worden geïmplementeerd, eenimpedantie (massa-veer-demper) model en een holonomisch (star) model.

Experimenten tonen aan dat het regelschema goed functioneert, de volgfouten (zowel krachtals positie) zijn acceptabel. Het vinden van geschikte regelaarparameters is wel tijdrovend, ditkomt door de gekozen aanpak in combinatie met de aard van het systeem (hybride, niet linear enmet veel interconnecties).

vii

viii Samenvatting

Resumen

Dentro de control automático, teleoperación es un área de investigación de gran interes. Tieneaplicaciones tales como cirugía microinvasiva precisa y submarinos del mar profundo por controlremoto. El término ‘teleoperación’ proviene del griego, y significa ‘operación a distancia’. Estostipos de sistemas son sujetos de investigación extensa desde los años ’50, tanto en teoría comopráctica. Sin embargo, todavía persiste una distancia entre la teoría y la práctica.

Hoy día, muchas soluciones están basadas en ecuaciónes algebraicas. Con éstas, es posiblederivar soluciones de manera teorica (por ejemplo con las teorías Lyapunov), pero a menudo lasintonización de los parámetros de los modelos es difícil. En consecuencia, estos métodos noson tan utilizables en industria. Esta investigación está basada en frf’s (funciones respuesta enfrecuencia), es decir mediciones. Por esta vía, la práctica se acerca a la teoría desde el otro lado.

La plataforma usada consta de dos robots no-similares, uno maestro y uno esclavo. En estaplataforma se implementa un esquema de control de teleoperación bilateral, basado en contro-ladores de tipo pid. Este esquema obliga al esclavo a seguir la posición del maestro y al maestroa imitar la fuerza sentida por la punta del esclavo. De esta manera, el operador puede sentir estafuerza por medio del maestro. Por medio de simulaciones, el esquema es probado antes de serimplementado en la plataforma. Dado que los sensores de fuerza no están disponibles al momentode esta investigación, modelos del ambiente son usados para simular las fuerzas. Dos tipos demodelos son implementados, uno de tipo impedancia (masa-resorte-amortiguador) y uno de tipoholonomico (rígido).

Tanto los simulaciones como los experimentos muestran que el esquema funciona bien, loserrores de seguir posición y fuerza son tolerables. Sin embargo, la sintonización de los parámetrosadecuados de controladores lleva mucho tiempo, debido al método elegido combinado con la clasedel sistema (híbrido, no-lineal e interconectado).

ix

x Resumen

Nomenclature

Symbol Units Comments

Romanae [-] Direction vector of environment planeai⋆ [m] Denavit-Hartenberg (dh) length parameter 1,3

be [Ns/m] Damping of impedance environment modelc⋆ [-] Shorthand for cos(⋆), see section 3.3di⋆ [m] Denavit-Hartenberg (dh) length parameter 1,3

d⋆ [m] Temporary variable to calculate inverse kinematics 1

D⋆ [m] Temporary variable to calculate inverse kinematics 1

ef⋆ [N] Force error 2

ep⋆ [m] Position error 2

eq,f⋆ [Nm] Force error in robot coordinates 2

eq,p⋆ [rad] Position error in robot coordinates 2

fe [N] Force reflected by environment modelf

d[N] (Master) force desired by operator

fm

[N] Force exerted by master on environment model

fs

[N] Force exerted by slave on environment model

f⋆

[N] Force on robot tip

H⋆ [-] Transfer function matrix of robot 1

J⋆ [-] Robot Jacobian 1

Jϕ⋆ [-] Constraint Jacobian 1

ke [N/m] Stiffness of impedance environment modelkD⋆ [-] pid-controller derivative parameters 2

kI⋆ [-] pid-controller integral parameters 2

kP⋆ [-] pid-controller proportional parameters 2

li⋆ [m] Link length 1,3

me [kg] Mass of impedance environment modelpe [-] Position of environment planep⋆ [m] Temporary variable to calculate inverse kinematics 1

qs,d

[rad] Desired link angles of slave robot

q⋆

[rad] Generalized robot coordinates (link angles) 1

Q [-] Decoupling matrix, see section 3.4rz,m [m] Projected radius of master tip on xy–planes⋆ [-] Shorthand for sin(⋆), see section 3.3x0 [m] Starting position of robots in x–directionxe [m] Cartesian position of environment modelx⋆ [m] Tip cartesian coordinate in x–direction 1

xd [m] (Master) cartesian coordinates desired by operatorxm [m] Master tip cartesian coordinatesxs [m] Slave tip cartesian coordinates

xi

Symbol Units Comments

y0 [m] Starting position of robots in y–directionym,0 [m] Master offset in y–directiony⋆ [m] Tip cartesian coordinate in y–direction 1

z⋆ [m] Tip cartesian coordinate in z–direction 1

Greekαi⋆ [rad] Denavit-Hartenberg (dh) angle parameter 1,3

β⋆ [rad] Temporary variable to calculate inverse kinematics 1

γ⋆ [rad] Temporary variable to calculate inverse kinematics 1

ε [m] Boundary of holonomic environment modelζe [-] Dimensionless ‘damping’ of holonomic environment modelη⋆ [rad] Temporary variable to calculate inverse kinematics 1

θi⋆ [rad] Link angle 1,3

λ [-] Lagrange multiplierρ [-] Measure of singularity; ratio of smallest and biggest singular valueτ c⋆ [Nm] Controller torque 2

τ f⋆ [Nm] Torque on robot motors due to a force on the robot tip 2

τ⋆ [Nm] Torque on robot links 2

φ⋆ [rad] Temporary variable to calculate inverse kinematics 1

ϕ⋆ [m] Constraint equationψ⋆ [rad] Orientation of end effectorωe [rad/s] ‘Eigenfrequency’ of holonomic environment model

Acronymscinvestav Centro de Investigación y de Estudios Avanzados del ipn, Méxicocnc Computer numerical control, an automated type of machiningdh Denavit-Hartenbergdof Degree of freedomfrf Frequency response functionipn Instituto Politécnico Nacional, Méxicojsom Joint-space orthogonalization method, see section 3.4matlab Software package, [1]mimo Multiple input, multiple outputpid Proportional-integral-derivative (type of controller)rov Remotely operated vehiclertai Real-Time Application Interface, extension to Linux kernel, [2]rtw Real-Time Workshop, a module of Simulink.TU/e Eindhoven University of Technology

1The subscript ⋆ denotes m (for master) or s (for slave)2The subscript ⋆ denotes the type, see section 3.23The subscript i denotes the (dh) link number

xii

Contents

Preface iii

Abstract v

Samenvatting vii

Resumen ix

Nomenclature xi

1 Introduction 1

2 Models 3

2.1 Robot kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.1 Master kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2 Slave kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2 Robot dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.1 Master dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.2 Slave dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.3 Environment models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.4 Human operator model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.5 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3 Controller scheme 11

3.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.3 Jacobian matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.4 Joint-space orthogonalization method (JSOM) . . . . . . . . . . . . . . . . . . . . 153.5 Diagnostics and safety checks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.6 Initial setpoint optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4 Results 17

4.1 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5 Conclusions and recommendations 25

5.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255.2 Recommendations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

Bibliography 27

Acknowledgements 29

xiii

xiv Contents

A Derivations 31

A.1 Master inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.2 Slave inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

B Controller parameters 35

B.1 Simulation parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35B.1.1 Impedance environment model . . . . . . . . . . . . . . . . . . . . . . . . . 35B.1.2 Holonomic environment model . . . . . . . . . . . . . . . . . . . . . . . . . 36

B.2 Experiment parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36B.2.1 Impedance environment model . . . . . . . . . . . . . . . . . . . . . . . . . 36B.2.2 Holonomic environment model . . . . . . . . . . . . . . . . . . . . . . . . . 37

C MATLAB and Simulink code 39

C.1 Simulink library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39C.2 General control scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59C.3 Initialization code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59C.4 Visualization code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64C.5 Initial setpoint optimization code . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

Chapter 1

Introduction

Background From deep sea exploration using remote operated vehicles (rov’s) to micro-invasivesurgery, teleoperation is a field of study with many interesting applications. Teleoperation basi-cally means ’operation at a distance’, such as displayed in figure 1.1. This is an example of bilateralteleoperation (as opposed to unilateral), meaning that the information flows both ways. In figure 1the process is displayed once more, in schematic form. The human operator controls the slaverobot at a distance by means of the master robot, which not only forwards the movements of theoperator but also feeds back the forces acting on the slave’s tip. Clearly, this concept can be quiteuseful, for example by putting people out of harm’s way.

Figure 1.1: Teleoperation, copyright: Peter Hokayem and Mark Spong [9]

This is why, over the past 60 years, a lot of effort has gone into understanding the problemsinvolved and the building of theoretic frameworks (for a good comprehensive survey, see [9]). Also,multiple different control strategies have been applied to this field, such as passivity-based control([8], for example) and sliding mode control [10]. However, most methods use (simplified) analyticmodels derived from the design drawings using Lagrange or Newton-Euler methods, rather thanmeasurement data from the actual setup. Admittedly, this makes it easier performing mathemati-

1

2 Chapter 1. Introduction

H u m a n M a s t e r

Comm

S l a v e E n v i r o n m e n t

xd xm xm xs

fs

fs

fs

fm

Figure 1.2: Schematic of bilateral teleoperation

cal manipulations, but the process is more sensitive to modeling errors due to machining errors orunknown motor inertia, for example. Besides that, most research focusses on using similar robotsfor master and slave, since this simplifies the controller schemes. The physical setup used in thisresearch consists of non-similar robots.

Note that the environment force inhibits hybrid1 behavior. It is one-sided, so a ‘free’ anda ‘contact’ state of the manipulator can be distinguished. Furthermore, while the dynamicalmodes of the robots might be initially decoupled, the (position-force-position) path through theenvironment inevitably yields coupling between the modes, as the environment force is distributedover all links (depending on the orientation). The various feedback loops via controllers also addto the intercoupling of the subsystems. The setup can thus be classified as a hybrid, intercoupled,non-linear system.

Previous work was done by Zutven, [16] on the same physical setup as used here. This wasbased on frf (frequency response function) data and control theory of linear systems. One of therobots, the master, was substituted by a simulation. It has been the basis for the current work,which shares a similar approach.

Approach The focus of this particular research is on using frf-based models to develop andimplement a control scheme for bilateral teleoperation on a real-life setup composed of two non-similar robots. Since force sensors are not (yet) available, the forces will be obtained by usingenvironment models, instead of real environments and sensors. In short, the objective can beformulated as follows:

To develop, implement and test a bilateral teleoperation control scheme on a real-lifesetup of non-similar robots, using frf-based robot models, and environment modelsinstead of force sensors.

Because of the relatively short time available (3 months), the main concern is getting the setupto work in the first place. To speed up development, simplicity is a major part of the designphilosophy. Also, the code for simulation models and controller schemes is developed in a modularway, and using a version-control system (a Simulink user library to be specific). This way, pieces ofcode can be re-used in multiple models while ensuring changes such as bug fixes are propagated toall instances. matlab and Simulink [1] are used to design and run simulation and experimentationcode.

Outline Chapter 2 discusses the used models for the robots and their environment. Using thesemodels, the control scheme can be developed, which is discussed in chapter 3. In chapter 4,the results of implementing this scheme in simulations and on the real-life setup are discussed.Also, some difficulties and remarks are discussed. Finally, conclusions and recommendations arestated in chapter 5. The appendices consist of derivations (appendix A), controller parameters(appendix B) and the full Simulink code (appendix C).

1Hybrid system: a dynamic system that exhibits both continuous and discrete dynamic behavior.

Chapter 2

Models

As usual in engineering, models provide the starting point of a solution. In this case, models ofthe dynamics and kinematics of both robots are needed. Furthermore, the environment has to bemodeled for lack of a force sensor. In this chapter, these models are discussed.

2.1 Robot kinematics

The layouts and kinematics of both robots have already been described by [7] (master) and [12](slave), and are summarized in [16]. They are repeated here for completeness and to clearly defineparameters and variables. The complete derivations of the inverse kinematics are included inappendix A.

2.1.1 Master kinematics

l1m [m] l2m [m] l3m [m]0.113 0.250 0.260

Table 2.1: Dimensions of master robot, seen in fig. 2.1

The forward kinematics can be easily derived from figure 2.1 as follows:

xm = − (l2mcos(θ2m) + l3mcos(θ3m)) sin(θ1m) (2.1)

ym = l1s + l2s + l3s − (l1m + l2msin(θ2m) + l3msin(θ3m)) (2.2)

zm = (l2mcos(θ2m) + l3mcos(θ3m)) cos(θ1m). (2.3)

The corresponding dh-parameters are shown in table 2.2, but note that this implies a differentcoordinate frame (xDH = xm, yDH = zm, zDH = −ym).

i αi [rad] ai [m] θi [rad] di [m]1 π/2 0 θ∗1m −ym,0

2 0 l2m θ∗2m 03 0 l3m −θ∗2m + θ∗3m 0

Table 2.2: Denavit-Hartenberg parameters (convention of [15]) of master robot

3

4 Chapter 2. Models

Figure 2.1: The layout of the master robot, copyright: R. Cortes-Martinez [7]

The inverse kinematics are a bit tougher, but have already been derived in [7], for a differentcoordinate frame. The complete derivation is included in appendix A.1, with the following result:

ym,0 = l1s + l2s + l3s − l1m (2.4)

rz,m =√

x2m + z2

m (2.5)

Dm =r2z,m + (ym,0 − ym)2 − l22m − l23m

2l2ml3m

(2.6)

θ1m = −atan2

(

xm

zm

)

(2.7)

ψm = ±atan2

(

√

1 −D2m

Dm

)

(2.8)

θ2m = atan2

(

ym,0 − ym

rz,m

)

+ atan2

(

l3msin(ψm)

l2m + l3mcos(ψm)

)

(2.9)

θ3m = θ2m − ψm, (2.10)

where atan2(y

x

)

denotes the four-quadrant inverse tangent (corresponding to atan(

yx

)

on the

right half plane) as used in matlab, among others. The offset ym,0 is a result of a shift in theorigin of the coordinate frame, such as to make the master– and slave frames exactly overlap, whichsimplifies position synchronization. The measured link lengths of the master robot are found intable 2.1.

2.1 Robot kinematics 5

2.1.2 Slave kinematics

Figure 2.2: The layout of the slave robot, copyright: D. Muro-Maldonado [12]

l1s [m] l2s [m] l3s [m]0.175 0.130 0.143

Table 2.3: Dimensions of slave robot, seen in fig. 2.2

The forward kinematics of the slave are straightforward:

xs = l1msin(θ1s) + l2msin(θ1s + θ2s) + l3msin(θ1s + θ2s + θ3s) (2.11)

ys = l1mcos(θ1s) + l2mcos(θ1s + θ2s) + l3mcos(θ1s + θ2s + θ3s), (2.12)

since the slave is a planar robot but has three links, it is redundant in position. Its dh-parametersare shown in table 2.4.

The inverse kinematics have been derived using part of the derivation of [16], and using [15](see appendix A.2 for the complete derivation):

l2sx = xs − l3ssin(ψs) (2.13)

l2sy = ys − l3scos(ψs) (2.14)

Ds =l22sx + l22sy − l21s − l22s

2l1sl2s

(2.15)

θ2s = ±atan2

(

√

1 −D2s

Ds

)

(2.16)

θ1s = atan2

(

l2sx

l2sy

)

− atan2

(

l2ssin(θ2s)

l1s + l2scos(θ2s)

)

(2.17)

θ3s = ψs − θ1s − θ2s. (2.18)

The dimensions of the slave robot are found in table 2.3. Note that angle ψs is denoted in figure 2.2as ‘φm’.

6 Chapter 2. Models

i αi [rad] ai [m] θi [rad] di [m]1 0 l1s θ∗1s 02 0 l2s θ∗2s 03 0 l3s θ∗3s 0

Table 2.4: Denavit-Hartenberg parameters (convention of [15]) of slave robot

2.2 Robot dynamics

The dynamics as used in simulations have been taken from [16], where frf-measurements werecarried out and second order models fitted to the data. All under the assumption of the dynamicsbeing decoupled, because the robots move sufficiently slow.

2.2.1 Master dynamics

The dynamics of the master have been measured using the motor torques τm as inputs and thelink angles q

mas outputs. Second order models have been fitted to the frf’s. The resulting open

loop transfer matrix of the master:

Hm =

40.77

s2 + 112.5s+ 167900 0

01390

s2 + 55.22s+ 121400

0 01876

s2 + 129.1s+ 65870

(2.19)

A Bode plot of this frf is displayed in figure 2.3.

2.2.2 Slave dynamics

The slave robot was equipped with velocity controllers (hardware) at the time of the work of vanZutven ([16]).1 Hence, the inputs were the desired link velocities q

s,dand the link angles q

swere

used as outputs. Second order models were fitted and result in the open loop transfer matrix ofthe slave:

Hs =

132

s2 + 154.2s+ 20100 0

0148.8

s2 + 160.8s+ 15880

0 0136.7

s2 + 152.6s+ 668

(2.20)

A Bode plot of this frf is displayed in figure 2.4.

1During this work (between simulations and experiments) the hardware controllers of the slave are replaced bytorque controllers to enable feedback of the environment torques.

2.2 Robot dynamics 7

−150

−100

−50

0M

agni

tude

(dB

)

100

101

102

103

104

−180

−135

−90

−45

0

Pha

se (

deg)

Bode Diagram

Frequency (rad/sec)

Hm1Hm2Hm3

Figure 2.3: Bode plot of the master (model) dynamics

−120

−100

−80

−60

−40

−20

0

Mag

nitu

de (

dB)

10−1

100

101

102

103

104

−180

−135

−90

−45

0

Pha

se (

deg)

Bode Diagram

Frequency (rad/sec)

Hs1Hs2Hs3

Figure 2.4: Bode plot of the slave (model) dynamics

8 Chapter 2. Models

fe,−x

y

me

beke

(a) Impedance model

fe,−x

y

(b) Holonomic model

Figure 2.5: Environment models

2.3 Environment models

As mentioned in the introduction, force sensors are unfortunately unavailable for use in this project.Hence, environment models (or ‘virtual environments’) are used to approximate forces that wouldotherwise be encountered and measured in real robot-environment interaction. This also impliesthe assumption that the environment is known. The general constraint equation of a single, planarenvironment is:

ϕ = aTe x− pe, (2.21)

where the vector ae specifies the direction, and pe the position of the environment. In this report,the environment is chosen to be in x–direction, at x = xe, so aT

e = [1 0 0] and pe = xe. Twodifferent types of environment models are used; impedance (mass-spring-damper) models (see also[15]) and holonomic (rigid) models (see [11]). Both types are displayed in figure 2.5.

Impedance model In figure 2.5a, a robot end effector is shown in contact with an impedancetype of environment with mass me, damping be and stiffness ke at position xe. The tip can freelymove in y–direction, the equations of motion in x–direction are:

fe =

{

mex+ bex+ ke(x − xe) , x− xe > 00 , x− xe ≤ 0

(2.22)

Holonomic model In figure 2.5b, the same effector is shown in contact with a holonomic typeof environment at position xe. The tip can freely move in y–direction. In x–direction, the tipwould, upon contact, not be able to move any further, losing one degree of freedom. At the sametime, the environment force fe appears following Newton’s third law: action equals minus reaction.The environment can be regarded as ‘infinitely stiff’, which poses problems to numerical solversand makes straightforward calculation of the force (like in the case above) impossible. In our case,we would like the environment (constraint) force to be calculated from the position x, and relaxthe demands on the numerical solver, which is why a more intricate approach is used.

2.4 Human operator model 9

−x,−ϕ

2ε

ϕ = 0, λ = 0

Figure 2.6: Schematic of holonomic constraint equation

Recall the constraint equation (2.21):

ϕ = x− xe, (2.23)

in other words, ϕ = 0 at contact. Now consider figure 2.6, where the constraint equation issketched. In order to relax the demands on the numerical solver, we define a small boundary εaround the environment. Inside this boundary, we approximate the holonomic constraint by apid-controller with setpoint ϕ = 0:

λ = kDϕ+ kPϕ+ kI

∫

ϕ, (2.24)

λ = ϕ+ 2ζeωeϕ+ ω2eϕ. (2.25)

where ζe, ωe are parameters we can tune. The sought-after environment force fe (or λ) is straight-forwardly calculated:

fe =

∫ t

0

λdt ,−ε < ϕ < ε

0 , elsewhere.(2.26)

The strict holonomic behavior is now replaced by a more relaxed variant, basically a pid-controller,working within a boundary layer.

Comparison of models From the above derivations, it is immediately evident that the impedancetype of environment model is far more simple than its holonomic counterpart. Furthermore, itpresents less problems to numerical solvers. However, it might not be representative for sometypes of real environments, which after all is the goal of modeling.

2.4 Human operator model

A model of the human operator is needed for simulation purposes. While some interesting literatureis available on modeling humans in teleoperation systems (such as [13]), the focus of this workwas on (fast) implementation. Therefore, the human is modeled as a pid-type controller, which isdiscussed in general in section 3.2. Moreover, the controller gains are chosen in a pragmatic way,such as to yield stability in simulations rather than accurately model human behavior.

2.5 Assumptions

During the development of the models discussed in this chapter, some assumptions are made. Thissection is aimed at giving a comprehensive summary of implicit and explicit assumptions made:

• The parameters (such as mass, damping, etc.) of the robots are stationary.

10 Chapter 2. Models

• By using cnc machining, the parameters of the actual robots can be well approximated bythe design variables.

• The encoders used to measure link angles q⋆

are sufficiently accurate.

• The robot dynamics of master and slave can be approximated using linear, second ordermodels.

• When using sufficiently slow trajectories, the robot dynamics are decoupled.

• The location, orientation and parameters of the environment are known.

• The used types of environment models (impedance and holonomic) do well resemble realenvironment behavior.

• The human operator can be approximated by a pid-controller.

• There is only a small, constant delay in the communication between the two robots.

Chapter 3

Controller scheme

To control the movement as well as exerted force of master and slave robot, controllers are needed.As mentioned in the introduction, simplicity was a key design objective throughout this work. Forthis reason, all controllers used are of the pid-type. Gravity compensation is used on both robotsto lower the needed (integral) control effort. This chapter will first give a general overview ofthe overall controller scheme and the definition of its main ingredient, the pid-controller. Next,Jacobian matrices are discussed, which are used to translate forces to torques. In this work, usehas been made of the joint-space orthogonalization method (jsom) (further reference: [11]) todecouple motions parallel and perpendicular to the environment. This gives more flexibility intuning the controllers. This is discussed in chapter 3.4. Finally, a number of measures takento ensure some safety for the robots themselves and their surroundings during experiments, arediscussed in chapter 3.5. Also, these measures provide diagnostics to speed up error tracking andcan be used to test feasibility of trajectories in simulation. The implementation of this controllerscheme in Simulink code are found in appendix C for further reference.

3.1 Overview

A simplified overview of the complete controller scheme is displayed in figure 3.1. At the right side,the slave robot is depicted, along with the environment model and slave controller. To the left,the master robot is depicted, along with its environment model, master controller and (human)operator. As mentioned in the introduction, forces are acquired through environment models,instead of real environments and sensors. The slave controller has the task of controlling bothposition and force of the slave robot, with respect to the master position and force of the masterenvironment model.

The master robot is controlled by both the operator and the master controller. The latter hasthe function of controlling the master such that the exerted force on the environment model mimicsthe force on the slave environment model, in effect establishing the force feedback. The operatorcontrols the position and force of the master against their desired trajectories. In simulation,the operator is modeled as two lookup-tables of (Bezier) trajectories and pid-controllers on bothposition and force. Note that two environment models are needed (on both master and slave side),since they replace the force sensors that would otherwise be used. Also note that the controllerscheme used here is of the ‘three channel’ form. Position and force at the master side are fed to theslave, the force of the slave side are fed back to the master. It can be regarded as a ‘four-channel’control in the sense of chapter 2 of [5], without the velocity feedback from the slave side.

A more comprehensive overview is shown in figure 3.2. In this figure, the Jacobian transfor-mations and forward and inverse kinematics incorporated in the controllers are shown. Also, theposition/force decoupling (jsom) at the slave side is displayed. The working of these buildingblocks will be discussed in this chapter.

11

12 Chapter 3. Controller scheme

Mastercontroller

Operator

Environmentmodel

Masterrobot Slave

controller

Environmentmodel

Slaverobot q

s

qm

fs

fm

τ co

τcm

τ cs

Figure 3.1: Simplified overview of controller scheme

3.1

Overv

iew13

Slaveposition

PID

SlaveforcePID

SlaveJacobian

Masterforward

kinematics

Operator

Environmentmodel

Masterrobot

MasterJacobian

SlaveQ matrix(JSOM)

Slaveinverse

kinematics

MasterforcePID

Environmentmodel

Slaverobot q

s

qm

qs,dxm

fs

fm

τm τ s

τcfm + τ fm

τ fs

τco

τ cps

τ cfs

τ cs

Figure 3.2: Comprehensive overview of controller scheme

14 Chapter 3. Controller scheme

3.2 PID-controller

The generic pid-controller is defined throughout this report in the following way:

τc⋆ =

(

kTP⋆ + kT

I⋆

1

s+ kT

D⋆s

)

eq,⋆, (3.1)

where τ c⋆ denotes the controller torque, kP⋆, kI⋆ and kD⋆ are the controller parameters, eq,⋆

denotes the error and the subscript ⋆ stands for the specific controller referred to, for exampleτ cpo refers to the controller torque of the position control (p) of the operator (o). All controllerparameters are found in appendix B.

While the controllers themselves act on errors in robot coordinates (q, τ), the errors expressedin cartesian coordinates (x, f) are more comprehensible to us humans. Therefore a distinctionis made throughout this report between eq,⋆, denoting the error in robot coordinates, and e⋆,denoting the same error but expressed in cartesian coordinates. Where possible, the latter variantwill be used for comprehensibility.

The errors of the operator’s controllers are defined as epo = xd − xm and efo = fd− f

m. The

force error of the master controller is defined as efm = fm− f

s. The errors of the slave controller

are defined as eps = xm−xs and efs = efm = fm−f

s. Note that the force errors fed to the master

and slave controller are the same.

3.3 Jacobian matrices

As mentioned previously, three sets of coordinates are used: master robot coordinates, slave robotcoordinates and cartesian coordinates. Transformations are needed to translate cartesian forcesto corresponding robot torques for use in the controllers. A common way to do this is using theJacobian matrices or ‘Jacobians’ of the robots [15]:

JTm =

(

δxm

δqm

)T

=

−(l2mc2m + l3mc3m)c1m 0 −(l2mc2m + l3mc3m)s1m

l2ms2ms1m −l2mc2m −l2ms2mc1m

l3ms3ms1m −l3mc3m −l3ms3mc1m

, (3.2)

JTs =

(

δxs

δqs

)T

=

l1sc1s + l2sc1s+2s + l3sc1s+2s+3s −l1ss1s − l2ss1s+2s − l3ss1s+2s+3s 0l2sc1s+2s + l3sc1s+2s+3s −l2ss1s+2s − l3ss1s+2s+3s 0

l3sc1s+2s+3s −l3ss1s+2s+3s 0

,

(3.3)

where s⋆ and c⋆ are short for sin(θ⋆) and cos(θ⋆), respectively, and θ1s+2s = θ1s + θ2s, etc. Withthese matrices, the transformations are defined as follows:

xm = Jm(qm

)qm, (3.4)

xs = Js(qs)q

s. (3.5)

These relations lead to the following (equivalent) torques resulting from forces fs

on the tips ofthe robots:

τ fm = JTm(q

m)f

m, (3.6)

τ fs = JTs (q

s)f

s. (3.7)

Also useful is the Jacobian of the constraint Jϕ, which translates the Lagrange multiplier λ of theconstraint to robot coordinates. Using the definition of Jϕ it can be easily derived from the robotJacobian and (2.21):

Jϕ⋆ ,δϕ⋆

δq⋆

=δϕ⋆

δx⋆

δx⋆

δq⋆

= aTe J⋆. (3.8)

3.4 Joint-space orthogonalization method (JSOM) 15

So in our case, where aTe = [1 0 0] (environment in x–direction), the Jacobians of the constraint

for master and slave robot are

JTϕm =

(

δxm

δqm

)T

=

−(l2mc2m + l3mc3m)c1m

l2ms2ms1m

l3ms3ms1m

, (3.9)

and

JTϕs =

(

δxs

δqs

)T

=

l1sc1s + l2sc1s+2s + l3sc1s+2s+3s

l2sc1s+2s + l3sc1s+2s+3s

l3sc1s+2s+3s

. (3.10)

3.4 Joint-space orthogonalization method (JSOM)

When controlling constrained robot manipulators (such as one constrained by a holonomic en-vironment), it is often useful to decouple the position and force control. This way, the positioncan be controlled independently of the force. The joint-space orthogonalization method [11] (alsoknown as ‘hybrid position/force control’) can be used to decouple the degrees of freedom (dof’s)at the manipulator tip into those perpendicular to the environment (force controlled) and thoseparallel to the environment (position controlled). This orthogonalization is done using a mappingin joint-space, hence the name. The mapping matrix is given as ([11]):

Q = I − JTϕ (JϕJ

Tϕ )−1Jϕ, (3.11)

with I the identity matrix and Jϕ the Jacobian of the constraint.

Now, the joint control torques τ c can be decomposed into two parts:

τc = Qτcp + (I −Q)τ cf (3.12)

The first part of the equation consists of the torques that only affect the parallel degrees of freedom,the second part consists of torques that affect the perpendicular (constrained) dof’s.

3.5 Diagnostics and safety checks

Because the workspaces of the robots are bounded, as are the angles and torques, not all tra-jectories are feasible. On top of that, robots can enter singular configurations, in which controlis no longer guaranteed in all directions. Therefore, it is necessary to implement safety checksand diagnostics, to quickly find out what is going wrong and to be able to check trajectories insimulations beforehand.

Two types of diagnostic checks on the desired trajectories are developed; checks on workspacefeasibility and checks on non-singularity. The checks on workspace feasibility are passed when thedesired angles are within range. The checks on non-singularity compute the ratio between thesmallest and biggest singular value ρ(J) of the Jacobian and compare this against a threshold. Ahigh ratio indicates the robot is possibly close to a singular configuration.

The safety checks consist of end-of-stroke sensors on the actual robots, which will terminateexecution of the current program, resulting in zero torque of the robot motors. An alternativewould be to hold the robot in the current position to prevent ‘falling down’ of the robot arm,possibly damaging the robot or its environment. However, in the case of an instable controller(as could well be in experimental trials), this would not solve the problem of the robot moving(uncontrolled) out of bounds, possibly damaging the environment, itself or both.

16 Chapter 3. Controller scheme

3.6 Initial setpoint optimization

Like their angular movement, the torque of the robot motors is also constrained. Because theselimitations have proven problematic in earlier experiments on the slave robot, and are likewiseexpected here, measurements are taken. An optimization algorithm is used to minimize theexpected torque on the slave motors due to gravity and environment interaction. The optimizationprocedure is sketched in figure 3.3. The slave robot is depicted together with the environment.The constraints on this optimization problem are the feasible workspace, and a desired trajectory‘dy’ of 10 cm along the environment (in y–direction). The free variable is the starting point (x0, y0,shown as question marks) of the trajectory.

As noted in section 2.3 the environment is assumed to be known. Because the third linkof the slave robot is chosen perpendicular to the environment1 the total workspace (gray solidsemicircle) is reduced to a smaller one (gray dashed semicircle), when considering only the firsttwo links. Furthermore, the reduced workspace is shifted the length of the third link to the left(indicated ‘dx’) because of this choice. Finally, to allow for a trajectory along the environment,the reduced workspace is shifted up (indicated ‘dy’) to create the optimization constraint on thestarting position (indicated black dashed semicircle).

Intuitively, the torque is expected to be minimum when the torques of gravity and environmentare in opposite direction, (partly) canceling out. Using this style of reasoning, an initial guess wasmade during the first experiments by moving the robot manually to a starting position. Calculationof the cartesian coordinates from these angles afterwards yields approximately (0.10, 0.13). Theoptimum found by using the matlab algorithm fmincon, (0.11, 0.14) closely matches this initialguess, giving it a more solid basis. The details of the optimization routine are found in appendix C.

d y

d x

o p t . c o n s t r a i n t

?

?

s l a v e

e n v i r o n m e n t

Figure 3.3: Schematic of initial setpoint optimization

1This is possible since the slave is redundant, as mentioned in section 2.1. Hence, φs =π

2.

Chapter 4

Results

In this chapter, the results of this project are discussed. First, the results of simulations arediscussed in section 4.1. More important however are the results of the experiments, discussed insection 4.2. The matlab and Simulink code used for simulations and experiments are found inappendix C. Probably most important are the lessons learned from these efforts, as summarizedin section 4.3.

A common way to assess performance of a controller is to look at the associated error. Insimulations, the human operator is substituted by pid-controllers on both position and force, withrespect to a reference trajectory. Details are discussed in section 3.1. This gives rise to positionerror epo and force error efo. However, in the final application stage a human operator will bepresent, so the focus lies on errors eps and efs. The latter two errors are used here as performanceindicators.

4.1 Simulations

Two types of simulations are carried out, one with the impedance environment model, the otherwith the holonomic environment model, both as described in section 2.3. The used simulinkmodels are found in appendix C. The controller parameters are found by trial and error, insome instances led by theory but mainly on intuition. The resulting parameters are found inappendix B. The main results are displayed in figure 4.1 (impedance) and figure 4.2 (holonomic),and are summarized in table 4.1.

In figure 4.1a the position trajectory is displayed (blue lines). The robot tip first moves tothe environment which is placed in x–direction, touches it at t ≈ 4.7 s (dotted black line), thentraverses some 10 cm along the environment. During this contact, a desired force should beapplied, which is displayed in figure 4.1b (blue line). It builds up to 8 N, holds this for some time,drops to 4 N, holds this, and drops back to zero. As can be seen, all errors are within reasonablerange, that is, small enough for useful application. However, these are just simulations, so theirresults can only be considered as some indication of feasibility of the control scheme.

In a more detailed look, the following can be noted. In figure 4.1a the error of the masterin x–direction (the red line with respect to the blue line) could be expected, because with theimpedance environment model, the reflected force is proportional to its deflection. So in orderto obtain the desired force, the controller should deviate from the desired position. The mastererror in y–direction however, is not to be expected at all. Given the fact that movement in thisdirection is not constrained by any environment, this error is expected to be close to zero. Butbecause the force of the environment is translated to torques on all robot motors, it works as adisturbance in all directions.

The same figure for the holonomic model, figure 4.2a, shows that the master error in x–directionis close to zero, because of the high stiffness of the holonomic environment model. This stiffnessalso explains the peak in figure 4.2b at t ≈ 4 s, where the robots first touch the environment.

17

18 Chapter 4. Results

Environment eps,max, 10−4 [m] efs,max [N]Impedance 7.8 0.8Holonomic 2.7 0.3

Table 4.1: Summary of maximum errors in simulations

Environment eps,max, 10−3 [m] efs,max [N]Impedance 4.5 1.1Holonomic 6.6 1.0

Table 4.2: Summary of maximum errors in experiments

4.2 Experiments

Similar to the simulations, two types of experiments are carried out, one with either type ofenvironment model. Contrary to the simulations, the operator is human (the author himself),instead of a set of pid-controllers and trajectory generators. The used simulink models are foundin appendix C. The controller parameters are found in the same manner as with the simulations,using those of simulations as a starting guess. The resulting parameters are found in appendix B.The main results are displayed in figure 4.3 (impedance) and figure 4.4 (holonomic), and aresummarized in table 4.2. Note that the mentioned cartesian positions (and errors) are not thereal positions at the robot tip, since they are calculated from the angular encoder measurementsusing forward kinematics. However, the positions are accurate within approximately 1 · 10−3 m(excluding dynamic effects).

In figures 4.3a and 4.4a the position trajectory as dictated by the operator is displayed asthe set of blue lines. The desired trajectory is similar to that of the simulated operator; moveto the environment, move over the environment in one direction and back. The operator triedto maintain a constant force while moving over the environment, as is visible from figures 4.3band 4.4b, this is not easy. Note that the accuracy of the human force control according to theseexperiments, lies somewhere in the order of 1 N.

As expected, the errors summarized in table 4.2 are higher than in simulation. However, theyare still within reasonable bounds for application in practice.1 Like predicted in simulation, usageof a holonomic environment model leads to peaks in the force and the errors at first contact becauseof the high stiffness.

4.3 Discussion

More often than not, reports and literature only give an overview of the successes of a particularresearch. The hard way to get there, with its dead ends and pitfalls, is often omitted. Admittedly,success should be the goal from start to finish, since it proves an excellent motivator, but afterwardsthe focus can and should be widened. Furthermore, no author should be ashamed to tell aboutthe failures and difficulties encountered along the way. This information is often just as importantto future readers. This small section has the goal of sharing some of the less fancy (but the moreinteresting) experiences, considerations and pitfalls with the reader.

For starters, getting the simulations to work proved quite harder than expected. The solversinitially got stuck in numerical problems because the switched environments induce chattering

1Note that the only difference between practice and these experiments would be the use of force sensors insteadof environment models, as explained in section 2.3. The influence of this last step on the final results is quitedifficult to predict, but would intuitively be beneficial since problems with numerical solving of the models woulddisappear.

4.3

Discu

ssion

19

0 5 10 15 20 25 30−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

time [s]

posi

tion

[m]

xyz

(a) Tip position trajectories

0 5 10 15 20 25 30−8

−7

−6

−5

−4

−3

−2

−1

0

1

time [s]

forc

e [N

]

FdmFmFs

(b) Force trajectories

Figure 4.1: Impedance model simulation results

20

Chapte

r4.R

esults

0 5 10 15 20 25 30−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

time [s]

posi

tion

[m]

xyz

(a) Tip position trajectories

0 5 10 15 20 25 30−10

−8

−6

−4

−2

0

2

time [s]

forc

e [N

]

FdmFmFs

(b) Force trajectories

Figure 4.2: Holonomic model simulation results

4.3

Discu

ssion

21

10 15 20 25 30 35−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

time [s]

posi

tion

[m]

xyz

(a) Tip position trajectories

10 15 20 25 30 35−6

−5

−4

−3

−2

−1

0

time [s]

forc

e [N

]

FmFs

(b) Force trajectories

Figure 4.3: Impedance model experiment results

22

Chapte

r4.R

esults

10 15 20 25 30 35−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

time [s]

posi

tion

[m]

xyz

(a) Tip position trajectories

10 15 20 25 30 35−8

−7

−6

−5

−4

−3

−2

−1

0

1

time [s]

forc

e [N

]

FmFs

(b) Force trajectories

Figure 4.4: Holonomic model experiment results

4.3 Discussion 23

behavior. The solution proved to be bounding the time step of the solver from below, so as to notallow for the solver to lose itself in this Zeno2-like behavior.

Because of the nature of the system (a set of highly coupled, hybrid3, non-linear subsystems)finding feasible controller parameters within the total parameter space can be tiresome. For exam-ple, when a problem such as high vibration occurs throughout the system, it is often impossible topinpoint the source. A badly adjusted position controller could lead to (initially small) vibrationsof the robot tip, which lead to the same type of vibrations in the force, getting fed back into multi-ple systems, multiplied by controllers and spread (by Jacobians) into all degrees of freedom. Thisall happens instantaneously in the whole system, making it impossible to time-track or deduce itback to the source. Leaving as only usable methods some intuition and the widely renowned T&E(trial and error) method.

The procedure could be sped up if some accurate initial guess could be made, based on availableknowledge of the system. Based on the type of system, one could think of mimo

4-theory-basedcontroller synthesis (as in [14]). However, the currently available theory is based on linear systems,and while the robots themselves can (within certain boundaries) quite well be approximated lin-early, the interaction with the environments cannot. Another possible approach might be to usethe analytic equations of the robots and controllers to mathematically derive an initial guess forcontroller parameters. The downside to this approach is that the real properties (such as mass-and stiffness matrices) of the robots might be different from the ones calculated from the designsketches. Estimating these properties from experiments on the real robots would tackle this prob-lem, but proves troublesome to date. Clearly, the ideal toolbox for teleoperation control engineerswould include theory combining mimo synthesis with analytic robot equations and reliable toolsfor parameter estimation.

2Zeno behavior: infinitely many mode switches in a finite length time interval.3Hybrid system: a dynamic system that exhibits both continuous and discrete dynamic behavior.4Multiple input, multiple output

24 Chapter 4. Results

Chapter 5

Conclusions and recommendations

5.1 Conclusions

In the preceding chapters a bilateral teleoperation control scheme is developed, building furtherupon the works of van Zutven [16], Cortes-Martinez [7] and Muro-Maldonado [12]. The schemeis implemented in simulations and experiments, only one last step (implementing force sensorsinstead of environment models) away from a total real-life setup. Suitable controller parametersare found and resulting data have been analyzed.

The results of experiments, summarized in table 4.2, indicate that the setup is functional. Theposition errors are acceptable given the dimensions of the robots. The force errors seem a bit highbut are still within the same order as the ‘human control performance’ as shortly mentioned insection 4.2. Furthermore, the controller performance was limited by instability issues occurring athigher gains. An intuitive explanation for this is that an initially small error grows because of ahigh total feedback gain, as discussed in section 4.3.

As mentioned in section 4.3, finding a feasible parameter subspace can be time-consuming.This is mainly because a comprehensive understanding of this type of system is lacking, making ithard to know which way to ‘turn the knobs’. Also, the complexity and high intercoupling of thesystem makes it hard, if not impossible, to backtrack and isolate the source of occurring problems.

5.2 Recommendations

Implementing force sensors, one on the slave side and one on the master side, would probablybe a great improvement. This way, the real-life setup is ‘complete’ and more advanced controlschemes can be developed and put to the test. Furthermore, the need for environment models inexperiments disappears, hopefully taking away some associated numerical issues as well.

A systematic and comprehensive approach, based on analytic robot models, mimo theory, hy-brid system theory, linear and non-linear system theory, would be preferable to the non-systematicapproach used here. Hereto, the different theories should be bundled and put into a more applica-ble framework. This framework does not need to be all-embracing, but could be split in a part usedto find an accurate first guess for the controller structure and parameters. And a part to find thedirection of a better solution if the current does not suffice. Furthermore, the tools should be asstandardized as possible, so a totally new implementation would require only minor adjustmentsand little academic skill.

Because the development of such a framework could take years, smaller steps can be takenin the meantime. For example, linear mimo theory (H∞) can be used for controller synthesis ina systematic way, when using the linear models of the robots and neglecting the hybrid natureof the environment contact (by synthesizing two controllers, free/contact). Also, hybrid systemtheory could be used to design a controller that deals easier with switching between the twomodes. However, these approaches have originally been developed for linear systems, so whether

25

26 Chapter 5. Conclusions and recommendations

they are robust against the non-linear nature of the position-environment-force feedback loop (viaJacobians) or the highly non-linear holonomic type of environments remains to be seen.

Bibliography

[1] matlab and Simulink, mathematical software from The Mathworks, Inc.

[2] rtai, the Real-Time Application Interface, an extension to the Linux kernel.

[3] Comedi, a control and measurement device interface for Linux.

[4] Sensoray, Co. Inc, manufacturer of embedded electronics.

[5] G.A.V. Christiansson, Hard master, soft slave haptic teleoperation, Ph.D. thesis, Delft Uni-versity of Technology, Oct. 2007.

[6] P.I. Corke, A robotics toolbox for MATLAB , IEEE Robotics and Automation Magazine 3

(1996), no. 1, 24–32, toolbox itself is available at http://www.petercorke.com/robot/.

[7] R. Cortes-Martinez, Diseño y construcción de un sistema de teleoperación maestro esclavo nosimilar, Master’s thesis, cinvestav, México, October 2007.

[8] Henrik Flemmer, Control design and performance analysis of force reflective teleoperators - apassivity based approach, Ph.D. thesis, KTH Sweden, Machine Design, 2004.

[9] Peter F. Hokayem and Mark W. Spong, Bilateral teleoperation: An historical survey, Auto-matica 42 (2006), no. 12, 2035–2057, doi:DOI:10.1016/j.automatica.2006.06.027.

[10] T.C. Kuo and Y.J. Huang, A sliding mode pid-controller design for robot manipulators, Com-putational Intelligence in Robotics and Automation, 2005. CIRA 2005. Proceedings. 2005IEEE International Symposium on (2005), 625–629, doi:10.1109/CIRA.2005.1554346.

[11] Yun-Hui Liu, V. Parra-Vega, and S. Arimoto, Decentralized cooperation control: joint-space approaches for holonomic cooperation, vol. 3, Apr 1996, pp. 2420–2425 vol.3,doi:10.1109/ROBOT.1996.506526.

[12] D. Muro-Maldonado, Control optimo de posición de manipuladores redundantes, Master’sthesis, cinvestav, México, November 2007.

[13] T.B. Sheridan, Space teleoperation through time delay: review and prognosis, Robotics andAutomation, IEEE Transactions on 9 (1993), no. 5, 592–606, doi:10.1109/70.258052.

[14] S. Skogestad and I. Postlethwaite, Multivariable Feedback Control : Analysis and design,second ed., John Wiley & Sons, 2007.

[15] Mark W. Spong and M. Vidyasagar, Robot Dynamics and Control, John Wiley & Sons, 1989.

[16] P.W.M. van Zutven, Design of a bilateral position/force master slave teleoperation systemwith nonidentical robots , Internship report DCT 2008.118, TU Eindhoven, September 2008.

27

28 Bibliography

Acknowledgements

Looking back at this project and my time in Mexico, there are some people who deserve specialcredit for their contribution. First of all, I would like to extend my sincere gratitude to AlejandroAngeles and Carlos Cruz, who have been my coaches during this project and have provided invalu-able support and criticism. Furthermore to Henk Nijmeijer for helping me arrange and preparethe internship and for his good advice in due time. To Stefan Lichiardopol for his critical reviewof this report. To Humiko Hernández for showing me around the real-time setup. Annel, Carlos,Efrain, Flavio, Jaime, Julio, Luis, Mario, Memo, Ricardo, Siddharta, and the other students andstaff at cinvestav, many thanks for helping with various problems, your friendship and teachingme Mexican Spanish. Thanks to cinvestav and TU/e for their financial support. To café Pepein Xico, Veracruz for growing the excellent coffee which supported me while writing this report.To Alexander, Quint and Victoria for spell-checking this report. Thanks to Mariska and Arianafor travelling with me around Mexico. To the country itself, for being a very interesting, warmand diverse place to spend your time. And last, but certainly not least, to my family and friends,reading my weblog, keeping in touch and supporting me mentally, both abroad and back homewriting this report.

–Bart

29

30 Acknowledgements

Appendix A

Derivations

A.1 Master inverse kinematics

Figure A.1: The master robot, copy of figure 2.1.

The angles and dimensions of the master robot are depicted in figures A.1 and A.2. From this,

31

32 Chapter A. Derivations

ym

xm

ym,0

θ2m

θ3m

ηm

βm

pm

dm

rz,m

lml2m

l3m

γm

ψm

Figure A.2: The master robot, when θ1m = −π/2

the inverse kinematics (using [7] and [15]) can be derived as:

ym,0 = l1s + l2s + l3s − l1m (A.1)

rz,m =√

x2m + z2

m (A.2)

lm =√

r2z,m + (ym,0 − ym)2 (A.3)

θ1m = −atan2

(

xm

zm

)

, (A.4)

where atan2(y

x) is the four-quadrant inverse tangent function, as implemented in matlab, among

others. Using the Law of Cosines:

− cos(βm) =r2z,m + (ym,0 − ym)2 − l22m − l23m

2l2ml3m

:= Dm = cos(ψm) (A.5)

ψm = ±atan2

(

√

1 −D2m

Dm

)

, (A.6)

A.2 Slave inverse kinematics 33

using some trigonometry:

sin(ηm) =pm

lm=l3msin(ψm)

lm(A.7)

cos(ηm) =l2m + dm

lm=l2m + l3mcos(ψm)

lm(A.8)

ηm = atan2

(

l3msin(ψm)

l2m + l3mcos(ψm)

)

(A.9)

θ2m = γm + ηm = atan2

(

ym,0 − ym

rz,m

)

+ atan2

(

l3msin(ψm)

l2m + l3mcos(ψm)

)

, (A.10)

and the last one to finish it:

θ3m = θ2m − ψm. (A.11)

Note that this derivation differs a bit from that of [15] in the signs of ψm. Here, the positive ψm

corresponds to an upper elbow configuration.

A.2 Slave inverse kinematics

ys

xs

θ1s

θ2s θ3s

ηs

βs

ps

ds

ls

l1s

l2s

l3s

l3sx

−l3sy

ψs

φs

γs

Figure A.3: The slave robot

The slave robot angles and dimensions are depicted in figure A.3. From this, the inversekinematics (from [15], and similar to A.1) follows stepwise as:

φs = ψs −π

2(A.12)

l3sx = l3scos(φs) = l3ssin(ψs) (A.13)

l3sy = −l3ssin(φs) = l3scos(ψs) (A.14)

l2sx = xs − l3sx = xs − l3ssin(ψs) (A.15)

l2sy = ys − l3sy = ys − l3scos(ψs) (A.16)

ls =√

l22sx + l22sy, (A.17)

34 Chapter A. Derivations

now, from the Law of Cosines follows:

− cos(βs) =l2s − l21s − l22s

2l1sl2s

:= Ds = cos(θ2s) (A.18)

θ2s = ±atan2

(

√

1 −D2s

Ds

)

, (A.19)

and the other angle can be found using some more trigonometry:

sin(ηs) =ps

ls=l2ssin(θ2s)

ls(A.20)

cos(ηs) =l1s + ds

ls=l1s + l2scos(θ2s)

ls(A.21)

ηs = atan2

(

l2ssin(θ2s)

l1s + l2scos(θ2s)

)

(A.22)

θ1s = γs − ηs = atan2

(

l2sx

l2sy

)

− atan2

(

l2ssin(η2s)

l1s + l2scos(θ2s)

)

, (A.23)

which leaves the last angle, which is defined by the orientation of the end effector, ψs:

θ3s = ψs − θ1s − θ2s. (A.24)

Here, a positive θ2s corresponds to an upper elbow configuration.

Appendix B

Controller parameters

Since four different types of control scheme are built (simulation/experiment, impedance/holonomicenvironment models), four sets of controller parameters are discussed here. First, the parametersof simulation schemes will be stated, second those of experiments. The convention of section 3.2is used for the notations. Since the master controller is not needed in simulations (no disturbancein master model), these parameters are omitted.

B.1 Simulation parameters

B.1.1 Impedance environment model

i kPps kIps kDps kPfs kIfs kDfs

1 50 10 2.02 50 10 2.0 1 0.5 0.0013 30 6 1.2

Table B.1: Slave controller parameters (sim, imp)

i kPpo kIpo kDpo kPfo kIfo kDfo

1 5000 500 1.02 500 200 1.0 80 200 0.13 500 100 0.1

Table B.2: Operator controller parameters (sim, imp)

35

36 Chapter B. Controller parameters

B.1.2 Holonomic environment model

i kPps kIps kDps kPfs kIfs kDfs

1 150 30 6.02 150 30 6.0 32 16 0.043 90 18 3.6

Table B.3: Slave controller parameters (sim, hol)

i kPpo kIpo kDpo kPfo kIfo kDfo

1 5000 500 1.02 500 200 1.0 20 80 0.0013 50 10 0.1

Table B.4: Operator controller parameters (sim, hol)

B.2 Experiment parameters

B.2.1 Impedance environment model

i kPfm kIfm kDfm

12 0.1 0.01 03

Table B.5: Master controller parameters (exp, imp)

i kPps kIps kDps kPfs kIfs kDfs

1 50 10 2.02 50 10 2.0 8 1.5 03 30 6 1.2

Table B.6: Slave controller parameters (exp, imp)

i kPpo kIpo kDpo kPfo kIfo kDfo

1 - - -2 10 1 0.002 2 1.5 0.13 8 1 0.002

Table B.7: Operator controller parameters (exp, imp)

B.2 Experiment parameters 37

B.2.2 Holonomic environment model

i kPfm kIfm kDfm

12 0.1 0.01 03

Table B.8: Master controller parameters (exp, hol)

i kPps kIps kDps kPfs kIfs kDfs

1 50 10 2.02 50 10 2.0 8.0 3.5 0.23 30 6 1.2

Table B.9: Slave controller parameters (exp, hol)

i kPpo kIpo kDpo kPfo kIfo kDfo

1 - - -2 6 2 0.02 4.5 2.0 0.023 6 2 0.02

Table B.10: Operator controller parameters (exp, hol)

38 Chapter B. Controller parameters

Appendix C

MATLAB and Simulink code

The controller scheme developed in chapter 3 must be translated to code in order to implement iton the real-life setup. For this, matlab Simulink [1] has been chosen as computing language for itssimplicity and intuitive use. The code is parsed to C-code and compiled into an executable usingrtai [2] (Real-Time Application Interface) and Simulink rtw (Real-Time Workshop). Comedi [3](linux control and measurement device interface) is used for communication with the Sensoray [4]model 626 analog and digital I/O card.

To increase manageability of the code, a user-defined library is used, which is discussed insection C.1. Next, the general layout of the simulation– and experimentation blockschemes isdiscussed in section C.2. The code used for loading parameters, gains etcetera at initialization isdiscussed in section C.3. In section C.5, the code used for the optimization routine of section 3.6is discussed.

C.1 Simulink library

The library is displayed in figure C.1. It consists of all blocks that are frequently used in thisproject and are not part of the standard Simulink blockset (such as integrator, gain, etc.). Itscontents are discussed here block by block, roughly top to bottom, left to right. Where blocksdisplay a ‘link’ icon in their lower left corner, such as the Master Jac mult block in figure C.2,they are instances of a library block.

Master model Figure C.2 on page 44. This block contains the dynamical systems from sec-tion 2.2, a master Jacobian multiplication block and an initial setpoint.

Slave model Figure C.3 on page 44. This block contains the dynamical systems from section 2.2,a slave Jacobian multiplication block and an initial setpoint.

Master Jac mult Figure C.4 on page 44. Takes the Jacobian from Master Jacobian, transposesand multiplies (from left) with second input.

Slave Jac mult Figure C.5 on page 45. Takes the Jacobian from Slave Jacobian, transposesand multiplies (from left) with second input.

Master Jacobian Figure C.6 on page 45. Computes the Jacobian as in section 3.3.

Slave Jacobian Figure C.7 on page 46. Computes the Jacobian as in section 3.3.

39

40

Chapte

rC

.M

AT

LA

Band

Sim

ulink

code

Library of simulation /experimentation blocks used for bilateral teleoperation project . Bart Genuit , July 2009

env2cart

xe x

cart2env

x xe

Virtual env imp

x Fe

Virtual env hol

x Fe

Traj gen Bezier Sim

xdm

Fd

Traj gen Bezier

xdm

Fd

Slave out

Ts

Slave model

Tcs

Fs

qs

Slave limit switches

Slave inv kin

xs

phis

ext?

qs1

qs2 Slave init

qs

Slave in

qs

Slave grav

qs Tg

Slave forw kin

qs

xs

phis

Slave controller

qs

qds

Fs

Fds

Tcs

Slave cont PID

eps Tps

Slave Jacobian

qs Js

Slave Jac mult

qs

F

T

SWC

qs check

SJC

qs check

Q mult slave

qs

T

Fs

Tq

Q mult master

qm

T

Fm

Tq

Operator Sim

qm

Fm

To

xdm

Fdm

Operator

qm

Fm

To

xdm

Fdm

Master out

Tm

Master model

To

Tcm

Fm

qm

Master limit switches

interrupts

Master inv kin

xm

qm1

qm2

Master init cascade

trig

qm

out

off _pos

stop

Master init

qm

off _pos

Ti

done

Master in

qm

Master grav

qm Tg

Master forw kin

qm xm

Master force contr

qm

Fm

Fdm

Tcm

Master digital outputs

Master Jacobian

qm Jm

Master Jac mult

qm

F

T

MWC

qm check

MJC

qm check

Figure C.1: Overview of Simulink library

C.1 Simulink library 41

Q mult master Figure C.8 on page 46. Takes the Jacobian from Master Jacobian, multiplieswith aT

e to get Jϕm, computes Qm according to (3.11). Switches between jsom mode and normalmode depending on force (whether contact with environment or not).

Q mult slave Figure C.9 on page 47. Takes the Jacobian from Slave Jacobian, multiplieswith aT

e to get Jϕs, computes Qs according to (3.11). Switches between jsom mode and normalmode depending on force (whether contact with environment or not).

Master grav Figure C.10 on page 47. Computes the gravity vector of the master for compen-sation.

Slave grav Figure C.11 on page 47. Computes the gravity vector of the master for compensa-tion.

Env model imp Figure C.12 on page 48. Translates from cartesian coordinates to environmentcoordinates, calculates the reflected force (2.22) and translates back to cartesian force.

Env model hol Figure C.13 on page 48. Translates from cartesian coordinates to environmentcoordinates, calculates the reflected force (2.26) and translates back to cartesian force.

cart2env Figure C.14 on page 48. Translates from cartesian coordinates to environment coor-dinates.

env2cart Figure C.14 on page 48. Translates from environment coordinates to cartesian coor-dinates.

Master forw kin Figure C.16 on page 49. Calculates the master forward kinematics (2.1).

Slave forw kin Figure C.17 on page 49. Calculates the slave forward kinematics (2.11).

Master inv kin Figure C.18 on page 50. Calculates the master inverse kinematics (2.4), upper-and lower elbow configuration.

Slave inv kin Figure C.17 on page 49. Calculates the slave inverse kinematics (2.13), upper-and lower elbow configuration, using an internal ‘farthest reach’ algorithm or external signal forthe end effector orientation.

MJC Figure C.20 on page 52. Master Jacobian check. Calculates ρ (Jm) and compares this againsta threshold to detect singularity.

SJC Figure C.21 on page 52. Slave Jacobian check. Calculates ρ (Js) (only the nonzero part)and compares this against a threshold to detect singularity.

MWC Figure C.22 on page 52. Master workspace check. Checks whether the desired angles arewithin the restrictions.

SWC Figure C.23 on page 52. Slave workspace check. Checks whether the desired angles arewithin the restrictions.

Master force contr Figure C.24 on page 53. pid control on cartesian force, with a Master

Jac mult block to translate to torques.

42 Chapter C. MATLAB and Simulink code

Slave controller Figure C.25 on page 53. pid control on both slave angles as cartesian force,with a Q mult slave block to implement jsom and a Master Jac mult block to translate force totorques. The pQs gain is used as a switch on jsom, set in the initialization m-file. The time-delayon the force can also be set in this file.

Master init Figure C.26 on page 54. Master initialization procedure, see also Master init

cascades. The setpoint is a ramp on all angles, with a ‘cascade’ that holds an angle once itslimit switch is reached. When all limits are reached, an ‘offset position’ is calculated to resetthe current angles to their now-known values. Next, the master is moved in the direction of itshome position (qm,0), when an angle reaches its home, it is held. When all angles are in home,the ‘done’ signal will be given for another controller to take over. A soft pid controller controlsthe motions. Because the third generalized coordinate is specified as the difference between thesecond and third encoder, some translations between absolute angles and generalized coordinateshave to be made back and forth.

Master init cascade Figure C.27 on page 55. See Master init for explanation.

Slave init Figure C.28 on page 56. Since the slave is not calibrated but put in its zero positionmanually, initialization is far simpeler. The slave moves in 5 seconds from zero to qs, 0, followinga fifth order polynomial, after which the output is held.

Operator Sim Figure C.29 on page 56 Operator model for simulations. Consists of a Traj gen

Bezier sim block to generate trajectories, master inverse kinematics to obtain desired angles, andpid control on position and force. Also, the same jsom setup as in the slave controller has beenimplemented, which can be switched using gain pQm. Force control can also be switched with pFo,for diagnostic purposes. An env2cart block is used to translate the scalar desired force to the 3Dcartesian vector.

Operator Figure C.30 on page 57 Operator model for experiment. Same as Operator Sim, butis enabled only after initialization.

Traj gen Bezier sim Figure C.31 on page 57. Generates the desired trajectories of positionand force for simulations.

Traj gen Bezier Figure C.32 on page 58. Generates the desired trajectories of position andforce for experiments. Because the time of first contact is uncertain in experiments, the trajectorygenerator is started (within the operator) by a pulse, the time is conditioned to start from zero atthis event.

Platform-specific blocks The remaining blocks are dependent on platform-specific rtai/Comeditarget links. They are the communication link between controller and motors, encoders, and limitswitches. Since describing them in detail is not significant to this report, only a short descriptionis included here.

Master in The input of the master to the I/O card.

Master out The output to the master from the I/O card.

Slave in The input of the slave to the I/O card.

Slave out The output to the slave from the I/O card.

C.1 Simulink library 43

Master limit switches The signals from the master limit switches at the limits of the permis-sible angles. Also used for calibration.

Slave limit switches The signals from the slave limit switches at the limits of the permissibleangles are checked to kill the motors in case of emergency.

Master digital outputs Some extra signals to the master motor controllers.

44 Chapter C. MATLAB and Simulink code

qm

1

link 3

1876

den(s)

link 2

1390

den(s)

link 1

40.77

den(s)

Master Jac mult

qm

F

T

qm0

Fm

3

Tcm

2

To

1

Figure C.2: The master model

qs

1

link 3

136 .7

den(s)

link 2

148 .8

den(s)

link 1

132

den(s)

Slave Jac mult

qs

F

T

qs0

Fs

2

Tcs

1

Figure C.3: The slave model

T

1Transpose

uT

Product

MatrixMultiplyMaster Jacobian

qm Jm

F

2

qm

1

Figure C.4: The master Jacobian multiplication

C.1 Simulink library 45

T

1Transpose

uT

Slave Jacobian

qs Js

Product

MatrixMultiply

F

2

qs

1

Figure C.5: The slave Jacobian multiplication

Jm

1

Reshape

Reshape

J33

f(u)

J32

f(u)

J31

f(u)

J23

f(u)

J22

f(u)

J21

0

J13

f(u)

J12

f(u)

J11

f(u)

qm

1

Figure C.6: The master Jacobian

46 Chapter C. MATLAB and Simulink code

Js

1

Reshape

Reshape

J33

0

J32

0

J31

0

J23

f(u)

J22

f(u)

J21

f(u)

J13

f(u)

J12

f(u)

J11

f(u)

qs

1

Figure C.7: The slave Jacobian

Tq

1

Switch

Pseudoinverse

Pseudoinverse

(SVD)

A X

Product 2

MatrixMultiply

Product 1

MatrixMultiply

Product

MatrixMultiply

Master Jacobian

qm Jm

LogicalOperator

OR

Identity Matrix

eye(3)

Data Type Conversion

int 8

Constant

a_env

CompareTo Zero

~= 0

Fm

3

T

2

qm

1

Figure C.8: The master Q-matrix multiplication

C.1 Simulink library 47

Tq

1

Switch

Slave Jacobian

qs Js

Pseudoinverse

Pseudoinverse

(SVD)

A X

Product 2

MatrixMultiply

Product 1

MatrixMultiply

Product

MatrixMultiply

LogicalOperator

OR

Identity Matrix

eye(3)

Data Type Conversion

int 8

Constant

a_env

CompareTo Zero

~= 0

Fs

3

T

2

qs

1

Figure C.9: The slave Q-matrix multiplication

Tg

1

g3

f(u)

g2

f(u)

g1

0

qm

1

Figure C.10: The master gravity compensation

Tg

1

g3

f(u)

g2

f(u)

g1

f(u)

Constant

[pi /2 0 0 ]

qs

1

Figure C.11: The slave gravity compensation

48 Chapter C. MATLAB and Simulink code

impedance equation

Fe

1

b_env

env2cart

xe x

cart2env

x xe

s

a.s+1

s

a.s+1

Saturation

ke

be

me

x

1

Figure C.12: The impedance environment model

holonomic equation

Fe

1epse

b_envenv2cart

xe x

cart2env

x xe

s

a.s+1

s

a.s+1

Switch

Integrator

1s

Ground

ome ^2

2*psie*ome

x

1

Figure C.13: The holonomic environment model

change to env coordinates

xe

1

a_env

Product 1

MatrixMultiply

x

1

Figure C.14: The translation from cartesian to environment coordinates

C.1 Simulink library 49

change to cartesian coordinates

x

1

a_env

Product 1

MatrixMultiply

MathFunction

uT

xe

1

Figure C.15: The translation from environment to cartesian coordinates

xm

1

z

f(u)

y

f(u)

x

f(u)

qm

1

Figure C.16: The master forward kinematics

phis

2

xs

1

y

f(u)

x

f(u)

Constant

0

Add

qs

1

Figure C.17: The slave forward kinematics

50

Chapte

rC

.M

AT

LA

Band

Sim

ulink

code

qm2

2

qm1

1

−L2m^2−L3m^2

y0

L1s+L2s+L3s−L1m

atan 2

cos

sin

atan 2

atan 2

q3

atan 2

cos

sin

atan 2

q13

q12q10

q1

atan 2

−u

−u

sqrt|u|2

|u|2

|u|2

sqrt

|u|2

|u|2

[rz]

[zm]

[ym]

[q1]

[z1]

[psi]

[xm]

L3m

L3m

L3m

L3m

[z1]

[rz]

[z1]

[rz]

[ym]

[z1]

[psi]

[rz]

[psi]

[psi]

[xm]

[psi]

[zm]

[q1][xm]

[zm]

L2m

2*L2m*L3m

L2m

1xm

1

Figure C.18: The master inverse kinematics

C.1

Sim

ulin

klib

rary

51

qs2

2

qs1

1

atan 2

cos

sin

atan 2

atan 2

cos

sin

−u

−u

atan 2

atan 2

cos

sin

Switch

Selector (x+y)

Phi

x,y phi

sqrt|u|2

|u|2 sqrt

|u|2

|u|2

[L]

[Ly]

[Lx]

[phi ]

[y]

[x]

[t2]

L2s

L2s

L2s

L2sL3s

L3s

[t2]

[t2]

[L]

[Ly]

[Lx]

[Ly]

[Lx]

[y]

[t2]

[t2]

[Ly]

[Lx]

[phi ]

[phi ]

[x]

[phi ]

Divide

Data Type Conversion

int 8

2*L1s*L2s

L1s

L1s

1−L1s 2−L2s 2

ext?

3

phis

2

xs

1

Figure C.19: The slave inverse kinematics

52 Chapter C. MATLAB and Simulink code

check

1

Terminator 1

Terminator

Singular ValueDecomposition

A = U * S * V ’

A

U

S

VSelector

Master Jacobian

qm Jm

Divide CompareTo Constant

>= 0.1

qm

1

Figure C.20: The master Jacobian check

check

1

Terminator 1

Terminator

Slave Jacobian

qs Js

Singular ValueDecomposition

A = U * S * V ’

A

U

S

VSelector

Product

MatrixMultiply

Divide

Constant

[2x3]

CompareTo Constant

>= 0.15

qs

1

Figure C.21: The slave Jacobian check

check

1

LogicalOperator

AND

Gain

180 /pi

<= −16

>= −160

>= 0

<= 145

>= −8

<= 180

qm

1

Figure C.22: The master workspace check

C.1 Simulink library 53

check

1

LogicalOperator

AND

Gain

180 /pi

<= 150

>= −150

>= −68

<= 150

>= −150

<= 68

qs

1

Figure C.23: The slave workspace check

Tcm

1

PID

PID

PID

Master Jac mult

qm

F

T

Fdm

3

Fm

2

qm

1

Figure C.24: The master force controller

54 Chapter C. MATLAB and Simulink code

Tcs

1

err_f_slave.mat

err_q_slave.mat

TransportDelay

Slave Jac mult

qs

F

T

Q mult slave

qs

T

Fs

Tq

PID

PID

PID

PID

PID

PID−1

pQs

Gain

1

Demux

Fds

4

Fs

3

qds

2

qs

1

Figure C.25: The slave controller

control the difference of the last two angles

done

3

Ti

2

off_pos

1

PID

PID

PID

Master limit switches

interrupts

Master init cascades

trig

qm

out

off _pos

stop

Demux

Demux

qm

1

Figure C.26: The master initialization

C.1

Sim

ulin

klib

rary

55

offset and set the home position

back−home triggers

switching criterium

end−of−workspace triggers

movement back to home

back−home cascade

setting initial position (way to switches)

switching criterium

end−of−workspace cascade

stop

3

off_pos

2

out

1

qm0

−[0 90 /5 −110/5]*pi /180

master_offset

Unit Delay

1/z

In1Out1Out2

In1Out1Out2

In1Out1Out2

In1 Out1

In1Out1

In1Out1

In1 Out1

In1Out1Out2

In1Out1Out2

In1Out1Out2

Switch 8

Switch 7

Switch 6

Switch 5

Switch 4

Switch 3

Switch 2

Switch 1

Switch

Ramp

NOR

LogicalOperator 2

NOTNOTNOR

Integrator

1sxo

Ground

Demux

DemuxDemux

Demuxdouble

false

false

>= 0

<= 0

<= 0

>= 0.5

qm

2

trig

1

Figure C.27: The master initialization cascade

56 Chapter C. MATLAB and Simulink code

qs

1hold 1

In1 out

delta

qs0

Switch 5

Product

Polynomial

P(u)O(P) = ???

Goto

[T]

From

[T]

Digital Clock

12:34Compare

To Constant

>= 5

0

[0 0 0 ]

Figure C.28: The slave initialization

PID control

PID control

Trajectory generation

Fdm

3

xdm

2

To

1

env2cart

xe x

tau_opp_for.mat

err_f_master.mat

qdo .mat

tau_opp .mat

err_q_master.mat

tau_opp_pos.mat

Traj gen Bezier Sim

xdm

FdQ mult master

qm

T

Fm

Tq

PID

PID

PID

PID

Master inv kin

xm

qm1

qm2

Master Jac mult

qm

F

T pFo

−1 −1

pQm

Demux

Fm

2

qm

1

Figure C.29: The operator for use in simulations

C.1 Simulink library 57

PID control

PID control

Trajectory generation

Fdm

3

xdm

2

To

1

env2cart

xe x

tau_opp_for.mat

err_f_master.mat

qdo .mat

tau_opp .mat

err_q_master.mat

tau_opp_pos.mat

Traj gen Bezier

xdm

FdQ mult master

qm

T

Fm

Tq

PID

PID

PID

PID

PID

PID

Master inv kin

xm

qm1

qm2

Master Jac mult

qm

F

T pFo

−1 −1

pQm

Demux

Demux

Enable

Fm

2

qm

1

Figure C.30: The operator for use in experiments

Fd

2

xdm

1

Lookup Table z

Lookup Table y

Lookup Table x

Lookup Table F

Digital Clock

12:34

Figure C.31: The Bezier trajectory generator for simulations

58 Chapter C. MATLAB and Simulink code

time conditioning

Fd

2

xdm

1

hold

In1 out

Rate Limiter

Lookup Table z

Lookup Table y

Lookup Table x

Lookup Table F

IC

[0]

Digital Clock

12:34

Figure C.32: The Bezier trajectory generator for experiments

C.2 General control scheme 59

C.2 General control scheme

From the blocks discussed in section C.1, several different control schemes can be built. Since theschemes used in this project are rather similar, only the most elaborate is discussed here. Theothers can be derived from this one by the reader. The scheme discussed is displayed in figure C.33.It is the one for an experiment, using real robots for master and slave, but a simulated operator, andimpedance environment models. Note that the results of this type are not discussed in chapter 4,because they are less interesting. However, the scheme is the most extensive.

When compared to figure 3.1, one can recognize the general form. The master and slavedynamic models are replaced with the in- and outputs of the robots and Jacobian multiplicationblocks. Blocks are added to handle the slave limit switches and master motor controllers. Gravitycompensation is added, and forward and inverse kinematic blocks are outside of the controllerblocks. Furthermore, initialization blocks are added, and switches that act on the ‘done’ signal toswitch from a steady setpoint to the current position and activate the operator.

To transfer this scheme into a scheme with a human operator, the operator block is simplyremoved. Because of the gravity compensation and friction in the master motors, the masterwill stand still after initialization, ready for operation. To change to a holonomic environmentscheme, we just replace the environment models. To change to variant for simulation, the operatoris replaced by its simulation version, and initialization, switches, gravity compensation, limitswitches and digital outputs are removed. The inputs and outputs of master and slave andJacobian multiplication blocks are replaced by the master and slave dynamical model blocks.

C.3 Initialization code

All parameters of master, slave, environment and controllers are listed in a matlab script,parameters.m. The first part lists all the general parameters, needed for every type of con-trol scheme. The other parts list the control parameters from appendix B, different for every typeof control scheme. The initialization uses a function, bezier.m to calculate the Bezier trajectories.

Script parameters.m:

%% Parameters internship Mexico

% This is a file with the parameters used for the simulations and

% experiments during an internship on bilateral teleoperation.

% Bart Genuit, July 2009

clear all;

% Some constants

g = 9.81; %[m/s^2] Gravity constant

a = 1e-2; %[s] Approximate differentiation time-constant for env. models

% Position and orientation of environment plane

a_env = [1 0 0]; % Selector vector of environment position (a*x <= b) |a| = 1

b_env = 0.11; % Bounding scalar for environment position

% Parameters of impedance model

me = 0; %[kg] (unit) Mass of virtual environment

be = 1; %[Ns/m] Damping of virtual environment

ke = 1000; %[N/m] Stiffness of virtual environment

% Parameters of holonomic model

epse = 4e-3; %[m] Size of boundary layer

60

Chapte

rC

.M

AT

LA

Band

Sim

ulink

code

Virtual env imp 1

xFe

Virtual env imp

x Fe

Switch 2

Switch 1

Switch

Slave out

Ts

Slave limit switches

Slave inv kin

xs

phis

ext?

qs1

qs2

Slave init

qs

Slave in

qs

Slave grav

qsTg

Slave forw kin

qs

xs

phis

Slave controller (PID)

qs

qds

Fs

Fds

Tcs

Slave Jac mult

qs

F

T

Operator

qm

Fm

To

xdm

FdmMaster out

Tm

Master init

qm

off _pos

Ti

done

Master in

qm

Master grav

qm Tg

Master forw kin

qm xm

Master force contr

qm

Fm

Fdm

Tcm

Master digital outputs

Master Jac mult

qm

F

T

Goto

[A]

From

[A]

true

pi /2

qm0

Figure C.33: Example of a total control scheme

C.3 Initialization code 61

psie = 3; %[-] Damping parameter of virtual environment

ome = 100; %[rad/s] Eigenfrequency of virtual environment

L1m=.113;L2m=.250;L3m=.260; %[m] Link lengths Master

L1s=.175;L2s=.130;L3s=.14257; %[m] Link lengths Slave

Lxc1m=.010;Lxc2m=.124;Lxc3m=.108;Lxc4m=.1072;D1=0.030; %[m] Centers of mass Master

Lxc1s=.144;Lxc2s=.105;Lxc3s=.054; %[m] Centers of mass Slave

m1s=.670;m2s=.530;m3s=.138; %[kg] Link masses Slave

m1m=.0092;m2m=.0714;m3m=.06874 + .03;m4m=.1305; %[kg] Link masses Master

%[rad] Position of master end-of-workspace switches w.r.t. zero position

master_offset = [90 143 -21]*pi/180;

xm0 = [0.09 0.14 0]; %[m], Starting position of master

qm0 = [1.5708 2.3173 0.0425]; %[rad] q0 master (upper elbow)

qs0 = [-1.1692 2.1568 0.5833]; %[rad] q0 slave (lower elbow, psi=pi/2)

xm_1 = [b_env xm0(2) 0]; %[m], Intermed. position of master (first contact)

xm_end = [b_env xm0(2)+10e-2 0]; %[m], End position of master

%Parameters of polynomial trajectories initialization

p_traj_master = [6e-5 -1.5e-3 1e-2 0 0 0];

p_traj_slave = [1.92e-3 -2.4e-2 8e-2 0 0 0];

xm_traj_x = [0 xm0(1);

bezier(2000,[1 xm0(1);2 xm0(1);4 xm_1(1);5 xm_1(1)]);

bezier(2000,[6 xm_1(1);7 xm_1(1);23 xm_end(1);24 xm_end(1)]);

25 xm_end(1)]; %Parameters of Bezier trajectory

xm_traj_y = [0 xm0(2);

bezier(2000,[1 xm0(2);2 xm0(2);4 xm_1(2);5 xm_1(2)]);

bezier(2000,[6 xm_1(2);7 xm_1(2);23 xm_end(2);24 xm_end(2)]);

25 xm_end(2)]; %Parameters of Bezier trajectory

xm_traj_z = [0 xm0(3);

bezier(2000,[1 xm0(3);2 xm0(3);4 xm_1(3);5 xm_1(3)]);

bezier(2000,[6 xm_1(3);7 xm_1(3);23 xm_end(3);24 xm_end(3)]);

25 xm_end(3)]; %Parameters of Bezier trajectory

F1 = 8; %[N] Force during first part

F2 = 4; %[N] Force during second part

Fedm = [0 0;

bezier(2000,[8 0;9 0;9 F1;10 F1]);

bezier(2000,[12 F1;13 F1;13 F2;15 F2]);

bezier(2000,[20 F2;21 F2;21 0;22 0]);

30 0]; %Force trajectory

%% Controller parameters impedance model (sim)

pQs = 0; % Don’t use JSOM decoupling in slave

pQm = 0; % Don’t use JSOM decoupling in master

pEo = 0; % Don’t use edge resetting of integrators in operator

pFo = 1; % Use force control in operator

kN = 100/2/pi; % Gain of derivative divisor (approx) in PID-controllers

Fdelay = 0; %[s] Delay for Fds in slave controller

% Operator (master) position controller (PID)

kpPo = [5000 500 500];

62 Chapter C. MATLAB and Simulink code

kpIo = [500 200 100];

kpDo = [1 1 .1];

% Operator (master) force controller (PID)

kfPo = 80;

kfIo = 200;

kfDo = .1;

% Master force controller (PID)

kfPm = 0;

kfIm = 0;

kfDm = 0;

% Slave position controller (PID)

kpPs = [50 50 30];

kpIs = [10 10 6];

kpDs = [2 2 1.2];

%Slave force controller (PID)

kfPs = 1;

kfIs = .5;

kfDs = 1e-3;

%% Controller parameters holonomic model (sim)

pQs = 0; % Use JSOM decoupling in slave

pQm = 0; % Don’t use JSOM decoupling in master

pFo = 1; % Use force control in operator

Fdelay = 1e-3; %[s] Delay for Fds in slave controller

% Operator (master) position controller (PID)

kpPo = [5000 500 50];

kpIo = [500 200 10];

kpDo = [1 1 .1];

% Operator (master) force controller (PID)

kfPo = 20;

kfIo = 80;

kfDo = 0.001;

% Master force controller (PID)

kfPm = 0;

kfIm = 0;

kfDm = 0;

% Slave position controller (PID)

kpPs = [150 150 90];

kpIs = [30 30 18];

kpDs = [6 6 3.6];

%Slave force controller (PID)

kfPs = 32;

kfIs = 16;

kfDs = .04;

%% Controller parameters impedance model (exp)

pQs = 0; % Don’t use JSOM decoupling

pQm = 0; % Don’t use JSOM decoupling

pFo = 1; % Use force control in operator

Fdelay = 0; %[s] Delay for Fds in slave controller

% Operator (master) position controller (PID)

kpPo = [0 10 8];

kpIo = [0 1 1];

kpDo = [0 .002 .002];

C.3 Initialization code 63

% Operator (master) force controller (PID)

kfPo = 2;

kfIo = 1.5;

kfDo = .1;

% Master force controller (PID)

kfPm = .1;

kfIm = .01;

kfDm = 0;

% Slave position controller (PID)

kpPs = [50 50 30];

kpIs = [10 10 6];

kpDs = [2 2 1.2];

%Slave force controller (PID)

kfPs = 8;

kfIs = 1.5;

kfDs = 0;

%% Controller parameters holonomic model (exp)

pQs = 1; % Use JSOM decoupling in slave

pQm = 1; % Use JSOM decoupling in master

pFo = 1; % Use force control in operator

Fdelay = 2e-3; %[s] Delay for Fds in slave controller

% Operator (master) position controller (PID)

kpPo = [0 6 6];

kpIo = [0 2 2];

kpDo = [0 .02 .02];

% Operator (master) force controller (PID)

kfPo = 4.5;

kfIo = 2;

kfDo = .02;

% Master force controller (PID)

kfPm = .1;

kfIm = .01;

kfDm = 0;

% Slave position controller (PID)

kpPs = [50 50 30];

kpIs = [10 10 6];

kpDs = [2 2 1.2];

%Slave force controller (PID)

kfPs = 8.0;

kfIs = 3.5;

kfDs = 0.2;

%EOF

Function bezier.m:

% Function to obtain a bezier curve of n points through control points P

function C = bezier(n,P)

d = size(P,1)-1;

C = zeros(n+1,2);

for i = 0:d

64 Chapter C. MATLAB and Simulink code

j = 1;

for t = 0:1/n:1

B = factorial(d)/((factorial(i)*factorial(d-i)))*t^i*(1-t)^(d-i);

C(j,:) = C(j,:) + P(i+1,:)*B;

j = j + 1;

end

end

%EOF

C.4 Visualization code

To visualize the robots and their movements, the robotics toolbox 7.1 [6] is used. The scriptshow.m is used to define the robots using their dh-parameters and link lengths, the second partof the script is a loop to make an animation of the robot movements of data from simulations orexperiments.

Script show.m:

%% Script for visualization (using robotic toolbox 7.1)

% Slave robot

L1 = link([0 L1s 0 0 0], ’standard’);

L2 = link([0 L2s 0 0 0], ’standard’);

L3 = link([0 L3s 0 0 0], ’standard’);

slave = robot({L1 L2 L3},’Slave’);

L4 = link([pi/2 0 0 -(L1s+L2s+L3s-L1m)], ’standard’);

L5 = link([0 L2m 0 0 ], ’standard’);

L6 = link([0 L3m 0 0 ], ’standard’);

master = robot({L4 L5 L6},’Master’);

%% Loop visualization

% Warning: Master lives in different coordinate frame!

% xm = x, ym = -z, zm = y.

qs1 = pi/2-qs.signals.values(:,1);

qs2 = -qs.signals.values(:,2);

qs3 = -qs.signals.values(:,3);

qm1 = pi/2-qm.signals.values(:,1);

qm2 = qm.signals.values(:,2);

qm3 = -qm.signals.values(:,2)+qm.signals.values(:,3);

N = 100; %number of frames

close all

figure(1)

view(180,90) % Set view master

figure(2)

view(-180,0) % Set view slave

Qs = zeros(N,3);

Qm = zeros(N,3);

C.5 Initial setpoint optimization code 65

time = qs.time;

t_array = linspace(time(1),time(end),N); %normalized vector

% build matrices with angle-data

for m = 1:N

k = find(time >= t_array(m),1); %normalize time (to discrete timesteps)

Qs(m,:) = [qs1(k) qs2(k) qs3(k)];

Qm(m,:) = [qm1(k) qm2(k) qm3(k)];

end

figure(1)

plot(slave,Qs,50,’workspace’,[-0.5 0.5 -0.5 0.5 -1 1],’erase’,...

’noshadow’, ’nobase’);

figure(2)

plot(master,Qm,100,’workspace’,[-0.5 0.5 -0.5 0.5 -0.5 0.5],’erase’,...

’noshadow’, ’nobase’);

%EOF

C.5 Initial setpoint optimization code

The optimization procedure described in section 3.6 to find a feasible environment position andstarting point is carried out in matlab. The code consists of three files. The main file optim.m

contains the options, call to the optimization algorithm and visualization (using the script ofsection C.4). The constraints function optim_con.m describes the semi-circle of figure 3.3. Lastly,the objective function optim_obj.mwhich adds up the torque due to gravity and the environment,in 20 points along the trajectory, and calculates the absolute maximum of this as the optimizationobjective.

Script optim.m

% Main function for optimization of slave initial position. The optimization

% minimizes the estimated combined torque of gravity and experienced force from

% the environment.

figure(1);

close(1);

options = optimset(’fmincon’);

options = optimset(options,’Display’,’iter’,’MaxFunEvals’,500);

[x, fval, exitflag, output, lamba] = fmincon(@(x)obj_grav(x,Fedm),...

xm0,[],[],[],[],[-inf .1 0],[inf inf 0],@(x)con_grav(x),options);

q = zeros(1,3);

% Inverse kinematics

phi = pi/2;

L2sx = x(1) - L3s*sin(phi);

L2sy = x(2) - L3s*cos(phi);

D = (L2sx^2 + L2sy^2 - L1s^2 - L2s^2)/(2*L1s*L2s);

q(2) = atan2(sqrt(1 - D^2),D);

q(1) = atan2(L2sx,L2sy) - atan2(L2s*sin(q(2)),L1s+L2s*cos(q(2)));

q(3) = phi - q(1) - q(2);

figure(1)

66 Chapter C. MATLAB and Simulink code

view(180,90)

plot(slave,[pi/2 0 0]-q,50,’workspace’,[-0.5 0.5 -0.5 0.5 -1 1],...

’erase’, ’noshadow’, ’nobase’);

%EOF

Function optim_con.m

function [c, ceq] = optim_con(x)

% (non-linear) Constraint function for optimization of slave initial

% position. Because of slave geometry, desired trajectory and environment

% position, constraint is a semi-circle, displaced in x and y.

% Constants

L1s = .175;L2s = .130;L3s = .14257; %[m] Link lengths Slave

c = x(2) - (sqrt((L1s+L2s-.02)^2-(x(1)-L3s).^2)-.1);

ceq = 0;

%EOF

Function optim_obj.m

function f = optim_obj(x0,Fedm)

% Objective function to find the optimum position for the slave robot,

% where the estimated combined torque of gravity and experienced force from

% the environment is minimal.

echo off;

% Constants

g = 9.81; %[m/s^2] Gravity constant

L1s = .175;L2s = .130;L3s = .14257; %[m] Link lengths Slave

Lxc1s = .144;Lxc2s = .105;Lxc3s = .054; %[m] Centers of mass Slave

m1s = .670;m2s = .530;m3s = .138; %[kg] Link masses Slave

% Move along trajectory of dy cm along the y-axis from initial position (x)

dy = .2; %[m] distance of trajectory along virtual environment

F = -4; %[N] (max) force along trajectory

y_arr = linspace(x0(2),x0(2)+dy-1e-4,20);

x = x0(1);

p = zeros(1,length(y_arr));

for k = 1:length(y_arr)

y = y_arr(k); % Pick (next) y along trajectory

q = zeros(1,3);

% Inverse kinematics

phi = pi/2;

L2sx = x - L3s*sin(phi);

L2sy = y - L3s*cos(phi);

D = (L2sx^2 + L2sy^2 - L1s^2 - L2s^2)/(2*L1s*L2s);

q(2) = atan2(sqrt(abs(1 - D^2)),D);

q(1) = atan2(L2sx,L2sy) - atan2(L2s*sin(q(2)),L1s+L2s*cos(q(2)));

q(3) = phi - q(1) - q(2);

C.5 Initial setpoint optimization code 67

% Gravity functions

u = q + [pi/2 0 0];

G(1) = m1s*g*cos(u(1))*Lxc1s+m2s*g*cos(u(1))*L1s+m2s*g*cos(u(1)+u(2))...

*Lxc2s+m3s*g*cos(u(1))*L1s+m3s*g*cos(u(1)+u(2))*L2s+...

m3s*g*cos(u(1)+u(2)+u(3))*Lxc3s;

G(2) = m2s*g*cos(u(1)+u(2))*Lxc2s+m3s*g*cos(u(1)+u(2))*L2s+...

m3s*g*cos(u(1)+u(2)+u(3))*Lxc3s;

G(3) = m3s*g*cos(u(1)+u(2)+u(3))*Lxc3s;

% Jacobian functions

u = q;

J11 = L1s*cos(u(1))+L2s*cos(u(1)+u(2))+L3s*cos(u(1)+u(2)+u(3));

J12 = L2s*cos(u(1)+u(2))+L3s*cos(u(1)+u(2)+u(3));

J13 = L3s*cos(u(1)+u(2)+u(3));

J_phi = [J11; J12; J13];

T = J_phi’*F;

% Updating objective function

p(k) = max(abs(T+G));

end

f = max(p);

%EOF