EXPERIMENTAL INVESTIGATION OF STIFFNESS CONTROL

FOR A ROBOTIC MANIPULATOR

Neil Tischler

A thesis submitted in conformity with the requirements

for the degree o f Master of Applied Science

Graduate Department of Mechanical and Industrial Engineering

University of Toronto

O Copyright by Neil Tischler 2000

National Library 1*1 of Canada Bibliothèque nationale du Canada

Acquisitions and Acquisitions et Bibliographie Services services bibliographiques

395 Wellington Stræt 395. rue Wdlingîon OItawaON K1AON4 Ot(awaON K1A OPJ4 Canada canada

The author has granted a non- exclusive liceiice allowing the National Library of Canada to reproduce, loan, distribute or sel copies of this thesis in microform, paper or electronic formats.

The author retains ownership of the copyright in this thesis. Neither the thesis nor substantial extracts from it may be printed or otheMrise reproduced without the author's permission.

L'auteur a accordé une licence non exclusive permettant à la Bibliothèque nationale du Canada de reproduire, prêter, distribuer ou vendre des copies de cette thèse sous la fome de microfiche/film, de reproduction sur papier ou sur format électronique.

L'auteur conserve la propriété du droit d'auteur qui protège cette thèse. Ni la thèse ni des extraits substantiels de celle-ci ne doivent être imprimés ou autrement reproduits sans son autorisation.

Experimental Investigation of S tiffness Control

for a Robotic Manipulator

Masters of Applied Science

Neil Alfred Tischler

Department of Mechanical and Industrial Engineering

University of Toronto

ABSTRACT

hpedance control is a suategy for controlling both the position of a robotic

manipulator, and the forces it exerts on its environment. Stiffness control is the simplest

fonn of irnpedance control. An approach to stiffness control, using force feedback. is

implemented on a 2 degree-of-freedom manipulator. The two joints are actuated by

bmshless d.c. motors connected to a hannonic drive gear. A wrist mounted forcehorque

sensor provides the measured forces and torques at the sarne rate as the control cycle: 500

Hertz. Experimental results demonstrate the use of force feedback for overcorning

complicated friction and dynamic effects for geared manipulators. However. the effect of

joint cornpliance lirnits the ability of the controller to realize the desired Cartesian

stiffness.

The inner loop based approach to force control is implemented for cornparison with

stiffness control. It relies on the position servo to reject disturbances. rather than on force

feedback. However, filtering lirnits the bandwidth of the outer force control loop.

Thus two practical approaches to force control have been demonstrate4 which are

suitable for implementation on manipulators using harmonic drive gears.

ACKNOwLEDGnmNTS

1 would like to thank my supervisor, Prof. A. A. Goldenberg for the opponunity to

conduct my research in the Robotics and Automation Laboratory. His influence and

support were key elements of my research.

1 would also like to thank Pawel Kuzan, Jacek Wiercienski, and Nenad Kircanski for

their practical advice and assistance, and Dr. M.R. Emami for his help dunng my work at

RAL.

This research was made possible through funding from the Ontario Ministry of

Education and Training through the Ontario Graduate Scholarship Program.

1 would like to recognize the help and influence of the following people with whom 1

had the pleasure of working in the Robotics and Automation Laboratory: David Pitts.

Lin Sheng, Wael Melek, Ms. Liz Montoro, Patrick, Frank, Fabien, Edmir. and Mona.

I thank my fiiends and family for their patience and support.

Finally 1 want to give full acknowledgment to Jesus Chnst, my personal Lord and

Savior. His love and grace endures despite my weaknesses and failures, and gives me the

strength to continue the struggle.

TABLE OF CONTENTS

ABSTRACT ....................... ......... ....................................................................................................... II

ACKNOWLEDGMENTS .. .............................................. ................................................................. III

..................................................................................................................... TABLE OF CONTENTS IV

LIST OF TABLES .............................................................................................................................. .VI1

LIST OF FIGURES ........................................................................................................................... VIII

LIST OF SYMBOLS .............................................................................................................................. X

CHAPTER 1 LNTRODUCTION ...................................................................................................... 1

1 - 1 PROBLEM STATEMENT AND MOTIVATION ..................................................................................... I

1 -2 RESEARCH OBJECTIVES ............................ .. ................................................................................ 3

1.3 COMR~BUTIONS ............................................................................................................................ 3

1.4 BACKGROUND ............................................................................................................................. 5

1 .Q. 1 Theoretical Backgroiind for inipedance Conrrol ......................................... .. .......... 5

1.4.2 Selection of Desired Impedarice ............................................................................................ S

1.4.3 Force Control rcsing Impedance Conirol .............................................................................. Y

................................................................................................ 1.4.4 Rigid Body Dytantic Mode1 / O

1.4.5 The Eflects of Harwtonic Drive Gears ................................................................................. 13

................................................................................................................... 1.5 LITERATURE REVIEW 14

1.5.1 Harmonic Drive Gears: Torque Conrrol ............................................................................. 14

1.5.2 Modelling Harmonic Drive Gears. ..................................................................................... 16

1-53 Srimess Conrrol using Force Feedback .............................................................................. 17

1.5.4 Contact Instability ................................................................................................................ /Y

1.5.5 lnner Loop Based Force Control ........................................................................................ 20

1.5.6 Summory .......................................................................................................................... 23

........................................................................................ 2.1 INTRODUCTION TO ï f i E IRIS FACILiTY 25

............................................................................................................. 2.1. 1 General Description 25

....................................................................... 2.1.2 Overview of Changes to the IRIS Hardware 27

2.1.3 Overview of Changes ro IRIS Software ........................................................................ 25

2.2 FORCE SENSOR INTEGRATION ..................................................................................................... 37

..................................................................................... 2-2- 1 Sensor Descriprion and Connecrion 37

2.2.2 Force Sensor Sran-up Proceditre .................................................................................... 38

.................................................................................................... 2.2.3 Parallel Port Connecrion 39

2.2.4 Trarrsformarion ro End Eflecror Frurrre ....................... ... ................................................... J O

2.2.5 Law Pass Filtering .............................................................................................................. 41

...................................................................................................................................... 2.3 MODEL 4 3

2.3.1 Nominal Paramerers ............................................................................................................ 43

.................................................................................................................. 2.3.2 Contrai Model 48

................................................................................................................ 2.3.3 Joint Conipliance 5 1

- - ................................................................................................................... 2.3.4 Joint Velociries -73

................................................................................................... 2.3.5 Open b o p Srep Response 56

CHAPTER 3 STIFFNESS CONTROL RESULTS ......................................................................... 67

3.1 THE STTT;MESS CONTROLLER .................................................................................................... 67

.............................................................................................................. 3 -2 EXPERMENTAL RESULTS 71

3.2.1 Intposing the Desired Stifiess ........................................... ... ......-......... 71

3.2.2 Discussion of Resulrs: Desired Srifi ess ............................................................................. 79

.................................................................................. 3.2.3 Force Conrrol wirh Srimess Conrrol 8 /

3.3.7 Discussion of Results: Force Conrrol .................................................................................. 57

..................................................................................................................................... 3.3 SUMMARY 88

CHAPTER 4 INNER LOOP BASED FORCE CONTROL ............................................................. 89

4.1 THE INNER LOOP BASED CONTROLLER .......................................................................................... 89

4.1. J Carresian Straight Line Trajecrory Scheme ......................................................................... 91

4.1.2 inverse Kinematics ............................................................................................................. 92

4.2 EXPERIMENTAL RESULTS .................. ... ......................................................................................... 94

4.3 SUMMARY ................................................................................................................................... 101

CHAPTER 5 CONCLUSIONS & RECOMMENDATIONS .................................................... 102

REFERENCES ................ ........................................................... ................................................ 106

APPENDIX A: NEWTON EULER DYNAiMICS ........................................................................... I l l

APPENDIX B: IRIS PROGRAM CONNECT STRUCTURE .......................................... 117

APPENDIX C: SAMPLE OF DATA FILE ..................................................................................... 1 3

vii

LIST OF TABLES TABLE 2.1 LIST OF CHANGES TO IRIS FACILiTY ........................................................................................... 27

TABLE 2.2 LIST OF FLJNCTIONS ADDED TO THE IRIS SORWARE ........................................................... 35

TABLE 2.3 BRUSHLESS DRIVE AMPLIFIER .................................................................................................... 45

TABLE 2.4 BRUSHLESS DC MOTORS .......................................................................................................... 46

TABLE 2.5 HARMON~C DR~VE GEARS ........................................................................................................... 47

TABLE 2.6 L[NK P A R A M ~ R S .................................................................................................................... 48

TABLE 2.7 BRUSHLESS DRIVE AMPLFER CURRENT LMITER SETTINGS ........................................................ 5-1

TABLE 2.8 F R I ~ I O N EFFECiS ...................................................................................................................... 57

TABLE 3.1 SUMMARY OF DATA RLES ........................................................................................................... 71

viii

LIST OF FIGURES ................................................................. FIGURE 1.1 TW'O DEGREE-OF-FREEDOM P W A R MANIPULATOR 10

FIGURE 1.2 HARMONIC DRIVE GEAR COMPONEhTS ...................................................................................... 14

FIGURE 2.1 P r c r u ~ ~ OF THE IRIS SYSTEM ............................................................................................ 25

FIGURE 2.2 THE TORQUE SENSOR CONNEClïON TO THE IRIS FACILCTY ...................................................... 2s

FIGURE 2 3 Fm SENSOR CONNECTlON TO THE IRIS FACILTTY ..................................................................... 30

FIGURE 2.4 EXPLODED ViEW OF THE CIRCULAR SPLINE ASSEMBLY ............................................................ 32

FIGURE 2.5 EXPLODED VEW OF THE TORQUE SENSOR ASSEMBLY .............................................................. 37

FIGURE 2.6 THE END EFFECTOR ROUER . .................................... .............................................................. 34

FIGURE 2.7 THE PARALEL PORT CONNECTION .................... .. .................................................................. 39

FIGURE 2.8 TRANSFORMATION OF FORCES FROM SENSOR FRAME TO END EFFECTOR FRAXIE ....................... 10

FIGURE 2.9 THE BODE DIAGRAM FOR THE LOW PASS FILTER ................................................................... 43

FIGURE 2.10 ACTUATOR MODE L. .................................................................. ... ............................................ 44)

FIGURE 2.1 1 THE COMPLIANCE OF ARM TWO JOINT 3 ................................................................................ 5 2

FIGURE 2.12 THE COMPLWNCE OF ARM TWO JOINT 4 ............................. ., ................................................... 37

FIGURE 2.13 JOINT 3 MOTOR CURRENT STEP RESPONSE ............................................................................... 39

FIGURE 2.14 JONT 3 P o s r r l o ~ s ................................................................................................................... 60

FIGURE 2.15 JOINT 3 ESTMATED VELOCTTES .............................................................................................. 60

FIGURE 2.16 JOWT 3 MWSURED TORQLIES ................................................................................................... 61

FIGURE 2.17 JOINT 4 MOTOR CURREhT S E P RESPONSE ................................................................................ 62

FIGURE 2.18 JOINT 4 MEASURED POSITIONS ................................................................................................. 63

FIGURE 2.19 JOINT 4 v ~ m ï ï m ................................................................................................................. 63

FIGURE 2.20 JOLW 4 TORQUES ..................................................................................................................... 64

FIGURE 3.1 B UXK DIAGRAM OF THE STIFFh'ESS CONTROLLER ................................................................... 67

FIGURE 3.2 THE APPARATUS USED FOR STIFMESS CONTROL EXPERMWTS ............................................. 73

FIGURE 3.3 CONTACT w m A STIFF ENVIRONMENT ..................................................................................... 73

FIGURE 3.4 JOCNT 3 RESPONSE - RIGlD ENVIRONMENT ...................................... ,... ........................................ 75

.................................................. . ....................... FIGURE 3.5 JOINT 4 RESPONSE RIGID ENVIRONMENT .. 76

FIGURE 3.6 CARTESLAN Posrno~s - RIGD ENVIRONMENT ......................................................................... 77

T ........................................ .................... FIGURE 3.7 S T ~ E S S ERROR AND J Fsf - RIGID ENVIRONMENT .. 78

FIGURE 3.8 END EFFECTOR FORCES - RlGID ENVIRONMENT ........................................................................ 79

.............................................................................................. FIGURE 3.9 J o r n 3 RESPONSE - BlAS FORCE 82

............................................................................................. FIGWRE 3.10 JOINT 4 RE~PONSE - B ws FORCE 83

....................................................................................... FIGURE 3.1 1 CARTESIAN P~!jrrlON - B IAS FORCE 84

............................................................................................. FIGURE 3.12 S ~ E S S ERROR - BIAS FORCE 85

.................................................................................... FIGURE 3.13 END EFFECTOR FORCES - BUS FORCE 86

....................... FIGURE 4.1 B ~ K DIAGRAM OF INNER LUOP BASED FORCE CONTROL .. ............................ 89

............................................................................. FIGURE 4.2 THE 2 DEGREE-OF-FREEDOM MANIPU WTOR 93

....................................................... FIGURE 4.3 J O L I 3 W P O N S E . INNER LOOP BASED FORCE CONTROL 95

............................................................................................... FIGURE 4.4 JOINT 4 RESPONSE - iNNER LOOP '35

FIGURE 4.5 THE DESIRED AND MEASURED FORCES - INNER LOOP ...................... .. ................................ 96

FIGURE 4.6 CARTESIAN POSITION - [NNER LOOP ......................................................................................... 97

FIGURE 4.7 Posrrro~ OFFSETS FROM THE OUTER FORCE CONTROLLER .................................................... 98

FIGURE 4.8 FILTERED FORCE IN THE Y -DIRECTION ........ .... ................................................................... 99

FIGURE 4.9 END EFFECTOR FORCES - [NNER ~ 3 0 ~ .................................................................................... 1

LIST OF SYMBOLS

acceleration of gravity

torques due to the centrifugai, Corriolis, and gravity effects

commanded torques ( 1.16)

vector of joint torques ( 1.3)

bias torques (J*F~)

disturbance torques

disturbance torques due to friction

static friction compensation term

joint angle for the ith joint

equivalent torques for rneasured force ( J~F,,,)

motor sbaft angle

motor torque

position offset increments

normalized acceleration (4.4)

distance to travel in the x and y directions

desired Cartesian damping matrix

static friction coefficient

Coulomb friction terms for positive and negative joint velocities

stiffness or torque error ( 1.16)

servo error defined as the desired less actual joint angle qd-q

vector of extemal forces and torques

complex amplitude of the force

bias forces

filtered forces in the x and y-directions respectively

measured forces

moment of inertia of the ith Iink about its center of mass

motor current

manipulator Jacobian

integer

motor torque sensitivity constant

motor back electro-motive force constant

desired Cartesian stiffness matrix

brushless drive amplifier gain

derivative gain matrix

distance frorn the ith joint to the center of m a s of the ith link

length of the ith link

mass matrix of the rnanipulator

desired Cartesian inertia matrix

mass of the ith link

gear ratio of the ith joint

actual joint space position

desired joint space position

motor resistance

xii

parameter for trajectory generation

sin(@)

sin(@ +@)

time

sampling period

blend time

desired duration of motion

unit step function at tirne t

control output

complex amplitude of the velocity

input voltage to motor amplifier

viscous friction terrns for both positive and negative joint velocities.

motor supply voltage for pwm signal

Cartesian position and orientation

scalar variable or distance in the X-direction

desired Cartesian position

Cartesian position error x -xd

final x and y position for a Cartesian straight line trajectory

unfiltered and filtered discrete variable at time Tz

initial x and y position for a Cartesian straight line trajectory

Cartesian positon offsets

distance in Y-direction

complex amplitude of the impedance

CHAPTER 1 INTRODUCTION

"Of the making of many books there is no end,

and much strrdy wearies the body. " Ecclesiastes 12:f 2

1.1 PROBLEM STATEMENT AND MOTIVATION

Industrial robotic manipulators are predominantly position controlled. There are

numerous tasks involving contact with the environment for which it would be desirable to

control not only the position of the manipulator, but also the forces it exerts on the

environment. Such tasks include assembly and grinding tasks.

The Remote Center of Cornpliance (RCC) is a passive device that limits end effector

forces that can arise due to contact. Jarnrning can occur during assembly due to

limitations both in the accuracy of part positioning and manipulator accuracy. The RCC

also has some particular advantages when assembling parts, since it is possible to place

the center of compliance at the tip of the part being inserted. This reduces problems with

j arnming and wedging.

Active approaches to contrd the end effector forces promise more flexibility than ri

passive device. However, they also present particularly difficult challenges to the control

engineer. The field has been actively investigated for over two decades.

Two different approaches stand out from early work on force control: hybrid

force/position control, and impedance control. Hybrid force control simply breaks the

control task into position controlled directions and force controlled directions.

Impedance control defines a dynamic relationship between forces and position mors.

The task of an impedance controiler is to replace the manipulator's impedance with the

desired impedance using feedback of measured positions, measured forces, and a mode1

of the manipulator dynamics. Stiffhess control is the simplest form of impedance control.

Theoreticall y s tiffness control can be implemented without force feedbac k, and wi t hou t a

dynamic model, simply by changing the manipulaior's position servo gains.

More recently, inner loop based force control methods have k e n introduced. For the

inner loop approach, the force controller adds position offsets to the commmded

trajectory of the "inner" position servo. One advantage of this approach is that it can

easily be implemented using comrnercially avaiiable industrial manipulators.

Harmonic drive gears are popular for industrial manipulators. However. because the

dynamic behavior of such gears is c~rnpl icated~ impiementation of impedance control

faces unique challenges. Impedance control theory normdly assumes that a rigid body

dynamic model of the manipulator is available. The behavior of the harrnonic drive gears

complicates the use of dynamic models for implementing impedance control. hence ü

robust aitemative is needed. The fundamental assumption that joint torques are

controlled is not valid for geared manipulators. Thus even stiffness control. which does

not require a dynamic model, is difficult to impiement.

In addition to conuolling the impedance of the end effector, it may be desirable to

apply a normal force on a workpiece. For example, a desired normal force may be added

when it is desired to maintain contact between a grinding tool and a workpiece, in spite of

positioning errors. A practical approach to impedance control is needed: suitable for

industrial manipulators, and able to apply a desired normal force.

1.2 RESEARCH OBJECTIVES

The objective of this thesis is to investigate practical issues for the implementation and

the performance of impedance control, for manipulators using h m o n i c drive gears. The

use of impedance control to apply a desired normal force is also investigated.

An approach to stiffness control using force feedback is implemented. Stiffness

control does not require a dynamic model, but the ability to control joint torques is

required. Aithough theoreticaliy force feedback is unnecessary for irnplementing

stiffness control, force feedback is used to compensate for static friction and the

difficulties in controlling the joint torques for geared manipulators.

The inner loop based approach to force control is an intuitive scheme for explicit force

control. It simply uses the measured forces to change the commanded trajectory. which

is tracked by an inner position servo. It was implemented for cornparison with the

stiffness control approach.

The experimental apparatus consists of a plana 2 degree-of-freedom manipulator.

The joints used are part of the CRIS facility, a modular reconfigurable robotic facility

consisting of two arms with 4 joints each. The performance of each üpproach is

evaluated expenmentally.

1.3 CONTRIBUTIONS

The outcome of this research is an experirnental verifkation of a practical approach to

stiffness control using force feedback. The advantages of this approach, and the

limitations of the use of a dynarnic model for implementing stiffness control, are

discussed and demonsuated. Both the stiffness control approach and the inner Joop

approach for force control are suitable for drive systems using harmonic drive gears.

which are popular for industrial manipulators.

The main contributions of this research are:

The integration of the forcdtorque (Fm) sensor with the IRIS facility: The

IOW level digital communication program allows the acquisition of the force data

at a rate of 500 Hz. This allows the IRIS facility to be used for testing and

evaluating a wide range of force control strategies.

The implementation of stihess control using force feedback is demonstrated:

This approach overcomes the effects of static friction and the difficulties in

controlling the joint toque for manipulators using harmonic drive geürs.

Feedback of forces makes stiffness control Iess sensitive to disturbances and plant

parameter variations. However. the effects of hardware limitations o n stiffness

control, such as joint compliance, are aiso discussed.

Stiffness control is used to apply a desired normal force on the workpiece: An

approach that does not require knowledge of the dynamic behavior of the

environment or of the contact point is demonstrated experimentally. The results

iIlustrate the difficulties of trying to control both the impedance of a rnanipulator

and the force it applies on a workpiece.

The inner loop based approach to force control is also demonstrated: This

approach has advantages when combining position control w ith ex plici t force

control. Tt provides a simple and intuitive alternative to the stiffness control

approach.

1.4 BACKGROUND

1.4.1 Theoretical Background for Impedance Conîtol

Ln Hogan's (1985) seminai paper he presents the theory of impedance control. which

imposes a dynamic relationship between positioning errors and end effector forces.

Impedance control is a general approach to both motion and force control that seeks to

make the manipulator behave like a mas-spring-dashpot system in response to external

forces. Pure force control has low impedance. while position control has very hizh

impedance and seeks to reject al1 position errors. The inspiration for impedance control

cornes from the way humans change the impedance of their m s when controlling both

the position of, and the forces exerted by their hands. The irnpedance of the human a m

can be changed by changing the orientation of the a m , by CO-activating antagonist

muscle pairs, and by using polyarticular muscles (muscles that generate torques about

more than one joint). The impedance control proposed by Hogan is achieved through

feedback control of position and forces.

Impedance control is implemented using a model of the manipulator dynamics to

cancel al1 disturbances except those caused by extemal forces. If the model based

compensation is perfect, then position errors will only be due to extemal forces. By

controlling the relationship between joint torques and position errors the external force is

also controlled.

The definition of impedance is based on the "mas-inductance" analogy which

parallels electrical circuits. The impedance, Z(s), is defined as the complex amplitude of

the force (F(s) the through variable), over the complex amplitude of the velocity (V(s)

the across variable).

The inverse definition is sometimes used and is based on the "mas-capacitance

analogy".

The desired irnpedance specifies a dynamic relationship between Cartesian errors and

extemal forces. For the general manipulator the desired impedance is given by

(Yoshikawa, 1990: 2 18-22 1)

M,X+B,(X-i,)+K,(x-x,)= F ( 1.2)

where x represents the Cartesian position and orientation of the end effector.

xd is the desired Cartesian position and orientation, and

Bdr and & are 6x6 matrixes of the desired inertia. damping and stiffness.

The task of an impedance controller is to apply the joint torques necessary to cancel

the non-linear rnanipulator dynarnics and impose the desired impedance. The control law

can be formulated based on the dynamic equation of the manipulator with an external

force,

M(q)q + h, = r + JT(q)F

where q is the vector of joint angles,

M(q) is the mass matrix of the rnanipulator,

h (q, q) includes the centrifugai, Corriolis, and gravity effects,

.r is the vector of joint torques,

J(q) is the manipulator Jacobian ( x = J(q)q ), and

F is an external load.

One significant factor neglected in the mode1 (1.3) is the joint friction. which is

difficult to mode1 and is often treated as a disturbance.

In order to obtain the desired dynamic behavior (1.2) the required joint torques are

where x, = x is the difference between the measured and desired trajectories. and

x(q) is calculated using the fonvard kinernatics of the manipuliitor.

Norrnally this control law is formulated using the measured force, rather than the

acceleration

Since the inverse Jacobian is used, the control Law (1 -4b) is not valid at singularities.

If it is acceptable to have the actual rnanipulator inertia, which changes with position.

then we can set h& equal to M(q). This approach is called a "Cartesian damping and

stiffness controller". The desired relationship then becomes,

B,(x-Xd)+Kd(x-x,) =F.

Then equation (l.4a) reduces to

T = h N ( q r q ) - ~ ( q ) ~ - ' ( q ) k q ) q - J ~ ( ~ ) ( B & + K ~ x , ) . ( 1.6)

By replacing the desired Cartesian damping (Bd) with joint space damping (K,) one

can simpliQ (1.6) further to give stiffness control. The desired relationship between

forces and Cartesian errors can then be expressed by

Kd(xd -x)=-F. ( 1.7)

Craig (equation 1 1.22, 1989: 385) gives the following control law for stiffness control

= ~ ~ ( q ) ~ , ~ ( q ) e , + ~ , é , (1 -8)

where K, is a constant derivative gain matrix,

eq is the servo error defined as the desired less the actual joint angle (qd-q).

The stiffness control law presented in equation (1.8) is nothing more than a

proponicnai derivative controller that Mght be used to control the Cartesian position of

the robot. This scheme modifies the position gains in a joint-based servo system such

that the end-effector appears to have the desired stiffness dong the Cartesian degrees of

freedom. The first term on the right hand side of (1.8) is an "expression for how joint

torques should be generated as a function of srna11 changes in joint angles te,) in order

that the manipulator end-effector behaves as a Cartesian spring with six degrees of

freedom" (Craig, 1989: 385). The position and orientation errors in task space (x,) are

assurned to be smail, allowing them to be represented by a simplified expression

xiJ(q)e,. The second term on the right hand side of (1.8) is added to increase the

damping.

This thesis will focus on stiffness control, which is the simplest of the three levels of

impedance control. Each level differs only in the desired dynamic relationship: full

impedance control(1.2), damping and stiffness control (1 S), or stiffness control ( 1.7).

1.4.2 Seleciion of D e s i d Impedance

Assuming that the required mode1 is available and c m be implemented in real time

control, it becomes necessary io choose an appropriate impedance for a given task.

Hogan (1985) discusses one possible approach to select a suitable impedance for

interaction with environments having different dynamics. If the dynamic behavior of the

environment is known, then a suitable impedance could be chosen either to minirnize the

forces and position errors, or to maximize power transmission.

Kazerooni et ai. (1986) presents an approach to select a suitable target impedance. that

takes into account the limitations of the dynamic model. These limitations include both

uncertain ty in the model parameters, and robustness to high- frequenc y unrnodeled

dynamics. This approach should ensure a stable system that achieves the target

impedance for a limited frequency range. The approach was implemented using a planar

linear positioning table. The system is able to achieve a desired stiffness and damping

when in contact with a ngid surface. The desired force system bandwidth was 4 Hz.

1.4.3 Force Conîrol using Impedance Control

There are two simple approaches to apply a desired end effector force while usins

impedance control. One approach is to adjust the desired trajectory such that it passes

just far enough beyond the point of contact that the desired steady state force is applied.

This approach requires an estimate of the environment sti ffness (which includes the

cornpliance of the manipulator), and the precise location of the contact point. A second

approach is to simply add a bias force. The steady state force can be made to match the

bias force (regardless of the positioning errors) by setting the desired stiffness to zero in

the direction of the desired force. The second approach does not require knowledge of

the environment stiffness and is insensitive to the contact point.

Goldenberg (1988) proposes an approach to force control using irnpedance control that

uses a mode1 of the manipulator dynamics, and does not require force measurement.

However, this thesis demonstrates the advantages of using measured forces for

implementing impedance control on geared manipulators.

1.4.4 Rigid Body Dynamic Model

The general f o m of the manipulator dynamics (1.3) is now applied specifically to a

two degree-of-freedom manipulator, since this is the type of manipulator that will be used

to test force control schemes in this thesis. The dynamic equations for a two

degree-of-freedom planar manipulator, sketched in Figure 1.1, are presented here.

Figure 1.1 Two degree-of-freedom planar rnanipulator.

The variables shown in Figure 1.1 are defined as,

8, the joint angle for the ith joint,

mi is the mass of the ith link,

1, is the moment of inertia of the ith link about its center of m a s .

1, is the length of the ith link,

1,; is the distance from the ith joint to the center of mass of the ith link. and

g is the acceleration of gravity,

Si represents sin(@), and

Sg represents sin(@ +9) (the same convention is used for cosine).

The closed form of the dynamic equations of motion can be derived using either the

Lagrangisui approach or the Newton-Euler approach. The detailed derivation using the

Newton-Euler approach is presented in Appendix A. The result is the same as that

obtained by the Lagrangian approach (Yoshikawa, 1990: 88-90). The closed form

dynarnic equations are then

2 where M, , = m,l,, + Il +m2 (1,' + la,' + 24fg2C2) + I I . (1.10)

M12 = M , , = rn2(lRL2 +l1lp.C2) + L -. (1.1 1)

ML2 = m21,22 + L. (1.12)

- hIl2 - hl12 =-h2,, =-)n211112S27 (1.13)

g, =m,&C, + m2g(l,C, + 42C12). (1.14)

gl = m&2c129 (1.15)

Mi; are elements from the manipulator mass mauix, and

h,p are elements of the velocity tem.

Because of high gear ratios (Ni) even a small motor inertia (JM) can cause significant

dynamic loads. To include the motor inertia an equivalent inertia is added to the link

inertia (Lin and Yae, 1991). Then the manipulator mass matrix terms (1.10) and ( 1.12)

become

- M~~ -mzlp,' + I~ + N ~ ~ J , ~ ~ . (1 .129

The Jacobian for this manipulator, which maps joint angular velocities to Cartesian

velocities of the end effector, is given by (equation 5.66, Craig, L989)

and the rate of change of the Jacobian is

The mode1 presented above does not include fnction effects. The disturbance torque

due to friction (9) c m be modeled with an asymrnetnc mode1 of Coulomb and viscous

friction,

where ch, cp, are Coulomb friction terms for positive and negative joint velocities. and

vfi, vy,, are viscous fnction terms for both positive and negative velocities.

Once suitable parameters have k e n determined, the dynamic model (1 -9) is relatively

easy to implement. The model is a simplification of the actual system and has several

limitations. It neglects high frequency dynamics, such as actuator dynamics or flexibility

of the joints. In addition, it is difficult to accurately determine the model parameters. For

example, the link inertial properties c m be estimated based on the link geometry. but a

more accurate approach is to measure the inertia of the link itself. which requires

disassembly of the manipulator.

For manipulators with more joints the model complexity increases. Another limitation

to modeling is the computation time. The control laws presented above are continuous.

In contrat, control implernentation using digital computers is discrete. If the sampling

rate is sufficiently high then it is reasonable to use this continuous model with a discrete

sarnpling system (Craig, 1995: 341). However, as the model complexity increases. the

time required to compute the control law also increases, limiting the sampling rate.

Because of the difficulty and complexity of model-based control. other high gain

robust schemes or adaptive conuol schemes have been developed to try to meet the

objectives of robot control. These objectives include insensitivity to model parameter

uncertainties and payload variations, and fast on-line computation of control (Hsirt et al..

1988).

1.4.5 The Effects of Harmonie Drive Gears

Some problems with dynamic models specific to manipulators using h m o n i c drive

gears are now presented. A h m o n i c drive is a compact, high-torque, high-ratio. in-line

gear mechanism consisting of three main parts (shown in Figure 1.2): an elliptical wave

generator, a non-rigid flexspline, and a rigid circular spiine. Some advantages of the

harmonic drive which make it attractive for robotic joints are its lightweight compact

design, zero backlash and back drivability. However, harmonic drive systems also

exhibit nonlinear stiffness, resonance vibration, and friction nonlinearities. In contras.

direct drive manipulators connect the output shaft of the motor directly to the output Iink

and thus rninimize some undesirable transmission effects (such compliance, and

backlash) which are difficult to model. Fairly good control of joint torque is possible

with a direct drive manipulator.

Figure 1.2 Hannonic drive gear components.

1.5 LITERATURE REVIEW

1.5.1 Hannonic Drive Gears: Torque Control

For geared manipulators controlling the joint torque is difficult. The traditional rigid

body robot dynarnic models are reviewed critically by Good et al. (1985). They show

that the assumption that the actuator c m be modeled as a pure torque source is

inappropriate for an electro-mechanical drive consisting of a brushless dc-motor with a

harmonic drive. Such drives are cornmon in indusvia1 manipulators and are also used in

the IRIS joints. Good et al. (1985) emphasizes the importance of using redistic modeis

that accurately describe the dynamic behaviour of electro-mechanical drives for control

design. Some important non-linear effects include: stiffening spnng characteristics of

the harmonic drive, friction effects, and current limiters in the motor control loops.

As can easily be seen from the irnpedance control laws (1.4. 1.6. 1.8), the assumption

that one can conuol the torque applied to the link is fundamental to the theory of

impedance control. This assumption is aiso made for many computed torque control

schemes which seek to decouple the dynamics of the robot links through nonlinear

control. Good et al. (1985) are not the only researchers who note the inadequacy of this

assumption. In fact, Lawrence et al. (1989) use a torque sensor in the third joint of a

PUMA 500 manipulator to close a torque servo around the motor and transmission

system in order to compensate for disturbance torques in the transmission. Hashimoto et

al. (1991), Hashimoto et ai. (1992), and Baines et al. (1995), use a torque sensing system

which is integrated into the harmonic drive itself to provide local joint torque control.

Then computed torque control is implemented on the local joint-[orque-loop controlled

robot.

Hogan et al. ( 199 1) presents some theoretical considerations for the use of joint torque

feedback for implementing impedance control on a manipulator that uses h m o n i c drive

gears.

1 S.2 Modeilhg Hannonic Drive Gears

Modelling of the harmonic drive transmission system has been investigated in the

literature, and proves to be a challenging task. Such modelling generally requires the use

of special experimental facilities to identiQ suitable model parameters. In addition. the

friction effects of the harmonic drive are extremely complex and difficult to model. In

this research, a detailed mode1 of the harmonic drive will not be used, but a few such

investigations are reviewed here since they are important for understanding the behaviour

of the harmonic drive.

Nye and Kraml (1991) present an experimental investigation of the problem of

harmonic drive gear error. Gear error is the difference between the theoretical and

measured angular position of the input and output of a gear reducer. The concentricity

and perpendicularity of the harmonic drive components has a major effect on gear error.

Kircanski and Goldenberg (1997) present an experirnental study on the friction and

stiffness effects of harmonic drives for a single IRIS joint. Careful experiments are used

to develop a rnodel and identify suitable parameters. A wedping effect during

constrained high torque experirnents is also reported. This behavior may not be common

for al1 harmonic drive joints, but rather reflects o n the design of the RIS joints.

Tuttle (1992) presents a thorough experïmental investigation into the behavior of

harmonic drive actuated joints, and presents several approaches for modelling their

be havior.

Thus, in order to implement impedance control on the IRIS facility the typical

challenges for modeling (parameter uncenainty, unmodeled dynamics). as well as the

particular difficulties of modeling for an electro-mechanical actuator consisting of a dc-

motor with a harmonic drive. must be overcome. The reliability of the mode1 is more

critical for impedance control than for computed torque control schemes. Computed

torque control schemes use outer proportional and derivative (PD) loops to cancel joint

errors (Craig, 1989: 339), and compensate for the limitations of the model. However. in

impedance control this approach would not allow external forces to be accommodated.

Thus impedance control is not well equipped to d e d with changes in the plant

parameters, or disturbances. One alternative to using a dynamic model is to use force

feedback to ensure that the desired impedance is achieved.

1.53 Stiffness Contrul using Force Feedback

According to impedance control theory, force feedback is only required in order to

change the apparent inertia of the manipulator (Kazerooni, 1986). However. there is a

better way to use force feedback. Salisbury (1980) proposed a "Stiffness Controller"

which is able to exhibit a Cartesian stiffness in response to external forces. Salisbury

uses force feedback to distinguish between the disturbance torques due to external forces.

= can and those due to other effects. Thus disturbances like friction and dynamic coupIin,

be rejected using a suitable feedback controller (rather than a model). while disturbances

due to the externai forces are accomrnodated according to the desired Cartesian stiffness.

Salisbury implemented stiffness control using end effector force feedback on the six

degree-of-freedom Scheinman-Stanford arm (which uses reducing gears). Salisbury then

demonstrated the use of stiffness control for an assembly task.

The stiffness control Iaw (1.8) does not contain measured forces. However. force

feedback allows a stiffness error (or torque error e.J to be defined. The stiffness error is

the difference between the commanded torque (r,consists of the desired stiffness and a

bias torque) and the measured joint torques (based on the measured end effector forces).

e, =T, -,JT F,,, = J ~ K ~ J ~ , +tr -J'F,,, , (1.16)

where Fb is a bias force,

TI, is the equivalent bias torque (rb = J ~ F ~ ) , and

Fm is the measured force.

If the stiffness error is regulated, using a suitable cornpensator. then the desired

stiffness will be imposed on the manipulator.

Feedback of measured forces has several advantages. It eliminates the need for a

dynamic model for the rnanipulator. It also compensates for the difficulty in controlling

the joint torque for dc-motors with harmonic drive gears. Feeding back measured forces

in this way is a practicai approach to implementing stiffness control. For example. Lu

and Goldenberg (1993) use force feedback to implement stiffness and darnping control.

and full irnpedance control, on a two degree-of-freedom direct drive manipulator. Force

feedback is used to define the system error, and a robust sliding mode scheme ensures

that the system error is regulated.

There are some practical limitations to using force feedback, because of its delay and

low-pass dynamics. Measured forces typically inciude high frequency noise and require

sorne low-pass filtering (Salisbury, 1980; 97). The force sensor also has some sensor

dynamics. These effects add some delay between the measured forces and the actual

applied forces. Thus the performance of force feedback for canceling sudden disturbance

torques is limited.

1.5.4 Contact Instability

The performance of force control strategies in contact with a stiff environment is

sometimes unsatisfactory. Contact instability describes the behaviour of the force

conuolled manipulator which exhibits pathological instability when in contact with a

rigid environment. Several investigators have concluded that passive compliance is

always needed in stiffness and impedance control for steady state force accuracy and fast

execution (Qian and Schutter, 1992a). Force feedback c m excite high frequency

unmodeled dynamics, and damping in the system may be insufficient when the contact is

stiff, leading to instability.

Because of the compliance of the IRIS joints, even contact with a rigid environment

will not result in a high contact stiffness. Nevertheless, the effects of friction and sensor

delay on the stability of force control are relevant to the IRIS facility. Some background

on contact instability is presented here to provide a better understanding of this

phenornenon.

Qian and De Schutter (1992a. 1992b) discuss the role of darnping and filtering on the

stability of discrete time implemented robot force control. Salisbury and Townsend

(1987) discuss the effect of Coulomb friction and stiction on force control. The effect of

static friction (stiction) can be to cause a limit cycle in the applied force. Increasing gain

in the integral force compensator leads to a domain of unstable limit cycles.

Manda1 and Payandeh (1994) present some results for stiffness control using a direct

drive manipulator. Some difficulties in obtaining the required damping when in contact

with a stiff environment are presented. The joint velocity term may not provide sufficient

darnping when in contact with a stiff environment. For stiff environments the oscillations

are fast. the calculated velocity may lag its ideal value, and damping may be applied out

of phase with the tnie velwity. if the change in position is small then the resolution of

the optical encoders will also limit the use of velocity for damping. in addition, the rate

of change of the force signal cmnot be used for damping because of signal noise.

Filtenng and estimation techniques can be used to provide damping using force signals in

spite of noise.

Surdilovic ( 1997) presents an investigation of the contact transition stabilit y of

impedance control. This approach determines the necessary damping for stability. but

does not discuss the practical problems in achieving it.

impedance control theory does not provide a convenient frarnework for addressing the

practical limitations of the system, or interpreting their effects with respect to contact

stability. The inner loop approach is a more intuitive approach to force control that is

better suited to addressing such issues.

1.5.5 Inner b o p Based Force Control

For inner loop based force control, position offsets from an outer force control loop

are simply added to the commanded trajectory which is tracked by the inner position

servo (refer to Figure 4.1). It can easily be applied to industriai manipulators that accept

position offsets, and the force loop c m be run at a lower rate than the joint servo. The

high-gain position feedback loop rejects Coulomb friction and other disturbances (like

dynarnic coupling). Thus a dynamic model is unnecessary.

The use of a dynamic model for direct force control methods (like impedance control

or hybrid positiodforce control) has limitations due to the difficulties in modelling

friction and the complicated transmission behavior of the harmonic drive. The force

feedback loop has delay and low-pass dynamics which make it inappropriate for

disturbance rejection. Thus the inner loop approach has an advantage over stiffness

control, since it relies on the joint position servo to cancel the disturbances rather than the

force feedback.

One of the earliest examples of the i ~ e r loop approach is given by Stepien et al.

(1987). An inner loop based force controller is developed to allow deburring of

rnachined and cast parts. The controller is applied to a General Electric P50 five-axis

robot. A six-axis forcehorque sensor is mounted at the end effector and uses a PiD

compensator to generate the position offsets needed to control forces. The manipulator

was able to move dong a surface at tangentid speeds frorn 1 to 9 cmls while maintaining

normal forces in the range from 1 to 15 N.

The inner loop approach has k e n successfully applied to industrial manipulators for

both cornpliant motion and cooperative manipulation by robot arms. Freund and Pesara

(1998) present an inner loop force controller that is designed to provide high bandwidth.

and smooth position corrections. Their force controller is also able to realize impedance

control.

Volpe and Khosla (1993) compare inner loop based force controllers with explicit

force control (using the hybrid control approach) and conclude that the position based

approach has real disadvantages. However, their analysis and experimental validation is

based on the six degree-of-freedom CMU DD Arm II, a direct drive manipulator. In the

system exarnined by Volpe and Khosla the manipulator has a much higher stiffness then

the environment. This suggests that the suitability of the inner loop based approach

depends on the type of manipulator, as well as the environment with which it should

interact. Direct force control methods may be more suitable for direct drive

manipulators, while the inner loop approach is well suited to industrial manipulators with

high gear reduction and flexible joints.

Despite the advantages of the inner loop approach, the environment dynamics still

play an important part in the performance and stability of the force controller. In general.

it is possible to tune the outer force control loop to perform well when the manipulator is

in contact with a speci fic environment (perhaps descnbed by some stiffness). However.

the performance of this controller may not be acceptable when the manipulator is in

contact with a softer o r harder surface. Numerous authors, like Tarokh and Bailey ( 1997 )

and Tin and Kiong (1997), have used fuzzy logic controllers in the outer force control

loop, and present different methods for adapting the controller in response to the

environment stiffness.

The inner loop approach will exhibit some physical impedance when in contact with a

given environment. By adjusting the force feedback gains it is possible to change this

impedance. However, assuming that the uttirnate objective is to control the end effector

forces, then there is little advantage to adding control of impedance to the inner loop

approach. Still, several authors have investigated the use of the inner loop based

approach for implementing impedance control. Carignan and Smith (1994) present

simuiation results for 2 approaches to inner loop based stiffness control. Pelletier and

Doyon (1994) present an investigation of the tracking performance (tracking the nominal

irnpedance) of several position-based impedance controllers. Heinrichs and Sepehri

(1999) apply inner loop based irnpedance control to a manipulator using hydraulic

actuators, and note some limitations due to finite positioning accuracy.

1.5.6 Summary

This Section presents a brief review of literature related to impedance control. The

concept and the basic theory of impedance control was first presented. Then some

guidelines for selecting the desired impedance and the major implementation issues were

discussed. A simple rigid body model for the two degree of freedom planar manipulator

was presented. Modeling issues, significant to impedance control, and some specific

issues for modeling when using harmonic drive gears, were described.

Impedance control is sensitive to variations in the plant parameters and disturbances.

Since a perfect model of the manipulator dynamics is not available. alternative

approaches to obtain the desired impedance are presented. One alternative is to use force

feedback to define the system error, which can then be regulated using a suitabIe

compensator. Force feedback has limitations because of the delay and low pass dynamics

of the measured force signal.

The issue of contact instability is also reviewed. This type of instability is related to

the physical limitations of both force and position sensors, and the difficulties in

providing sufficient damping when in contact with a sti ff environment. Friction forces

dso affect the stability of the force controller. Contact stability problems can be

hardware specific, so care must be used when considering the literature on this subject.

Finally the inner loop approach to force control is presented. This more recent

approach to force control has been applied successfully to industnal manipulators and

provides a frarnework to address practical issues such as delay in the force signal, and

environment dynamics. This approach has an advantage over the proposed stiffness

controller since disturbances due to friction and dynamic coupling are cancelled using the

position servo, rather than by force feedback. However, the inner loop approach is better

suited to explicit force control than to impedance conuol.

1.6 ORGANIZATION OF THE THESIS

This thesis will focus on Salisbury's stiffness control approach that uses force

feedback to define the stiffness error. This investigation demonstrates the advantages of

iising force feedback especially for geared manipulators. Then a simple approach to

combine force control with irnpedance control will be applied to the stiffness controller.

and its performance compared with the inner loop approach.

In Chapter 1 the purpose of the thesis is explained. Some background information for

irnpedance control and a literature review are presented. The literature survey addresses

contact stability, harrnonic drive gears, and inner loop based force control.

Chapter 2 describes the experirnental setup, and provides a brief introduction to the

IRIS facility. The changes to the system hardware and software are described. and the

integration of the F/T sensor is briefly documented.

Chapter 3 describes the implernentation of stiffness control using force feedback.

Experimental results are presented for stiffness control in contact with both a stiff and a

rigid environment. Then stiffness control is used to apply a desired normal force.

Chapter 4 describes the implementation of innerloop based force control.

Experimental results show the performance of this approach: when unconstrained. during

contact transition, and in response to a step change in bias force.

Finally Chapter 5 presents the conclusions and recommendations of this thesis.

CHAPTER 2 EXPERIMENTAL SET-UP

" Whaf is nvisted cannot be straightened;

What is lacking cannot be counted. " Ecclesiastes 1: 15

2.1 INTRODUCTION TO THE IIUS FACILITY

2.1.2 General Descn@tion

The RIS Grasping and Manipulation Facility (Figure 2.1) was devefoped about 7

years ago at the Robotics and Automation Laboratory, Department of Mechanical and

Industrial Engineering, University of Toronto. It is a moduiar, reconfigurabie robotic

system consisting of two rnanipulator arms with four rotary joints each. Each joint is

driven by a d.c. brushless motor connected to the output link through a harmonic drive

gear, and is instrumented with both position and torque sensors.

Figure 2.1 Picture of the RIS system.

The IRIS Facility is described succinctly by R. Hui et al. (1993). T o sumrnarize. the

cornputing architecture consists of two parts: HOST and RISC. The host computer runs

on a real time OS (AMX 386 Multitasking executive by KADAK) and handles al1 the

data acquisition, communication and display functions. The RISC CO-processor provides

the high computation speeds needed to compute the trajectory, kinematics and the control

law.

During every control cycle, the host computer collects the sensor readings, and passes

this data to the RISC. At the same time the RISC returns the control output which was

computed based on the previous set of sensor data. Finally the host cornputer updates the

voltage sent to the motor amplifiers (which is proportional to the control output).

The A M . real-time system is a multitasking system that ailows several activities to

operate simultaneously. The main tasks are initiated at regular intemals using timers.

The rnicroprocessor is managed so that tasks of higher priority are executed first. Real

time control on the IRIS system is achieved by using a timer to initiate the control cycle.

The clock tick rate of the AMX systern timer is set at 1 kHz. Timers are also used to stan

the data acquisition (by storing data to a buffer), and to update the user screen display at

regula. time intervals. The rate of the control cycle was reduced from 1 kHz to 500 Hz

by setting the timer interval for the control task to 2 ticks. AMX provides a

Configuration Builder prograrn (AMBLD-EXE) which is full screen interactive me thod

of preparing and editing an AMX system definition module. It was used to change the

timer interval and generate the new AMX configuration file.

General users of the IRIS facility require only a basic understanding of the software.

The user only needs to make modifications to certain files in order to define a control

algorithm, the desired trajectory points, and the measurements to be acquired in the data

file. A more in depth understanding of the host software was necessary to integrate the

FIT sensor.

In the remainder of this section, the main changes made to the IRIS hardware and

software will be summarized. Section 2.2 describes the integration of the Fm sensor in

detail. FinalIy Section 2.3 presents a closer look at the system, and identifies some

sources of non-linearity that make modeiing difficult.

2.1.2 Overview of Changes to the IRIS Hardware

The mechanical changes made to the IRIS facility dunng this project are summarized

in Table 2.1, and each change is descnbed bnefly below.

Table 2.1 List of changes to iRiS facility.

1

l 2 3

Calibration of Torque Sensors

Integration of F/T sensor 1 1

Modifications to Joint 3

4

! Acquisition of current monitor signal

Addition of an end-effector roller i

1. The torque sensor signai is amplified in an external signai conditioning circuit.

Since the torque sensor (or loadcell) works Iike a Wheatstone bridge. the torque is

proportional to the milivoit (mV) output divided by the supply voltage (V). A voltage

proportionai to the milivolt per volt ( m V N ) signal from the torque sensor is sent to the

analogue to digitai converter (ADC), mode1 AT-MIO- 16F-5 manufactured by National

Instruments. This connection is illustrated in Figure 2.2. In order to scale the measured

voltage back to wiits of torque, both the cdibration data for the sensor and the

amplification of the signal conditioning circuit must be known.

1

coMbrrlQh)\N6

Hosr pc

Figure 2.2 The torque sensor connection to the RIS facility.

Mode1 TRT-500 Transducer Techniques loadcells, serial numbers 107036 and 88 134

respectively, are used in joints 3 and 4 of arm two. Each has a Capacity of 56.5 N-m (500

in-lbs). The torque sensors are provided with a certificate of calibration, which can be

traced to the National Institute of Standards and Technology (NET) standard. The full

scale output for the torque sensors is roughly 2.0 mV/V (typical excitation is 10V). Since

no documentation was available for the signai conditioning circuit (built at RAL). the

amplification had to be determined empirically.

A voltage divider was constnicted for this task, having a bridge resistance similar to

the torque sensor (350 Ohms). This voltage divider can be connected to the signal

amplification circuit, replacing the torque sensor (Figure 2.2). By adjüsting the

potentiometer in the voltage divider circuit, the full range (E20mV) of signals expected

from the torque sensor can be generated. The amplification of the signal conditioning

circuit can be determined by measuring the m V N signal (mV output over the excitation

voltage) and recording the voltage received at the ADC using the RIS interface. Then.

based on the certificate of calibration, the proper scaling factor for the ADC digital value

to give the joint torque can be determined. The scaling was verified by applying standard

weights to the arm and comparing the measured torques with the expected values.

2. The connection of the F/T sensor is illustrated in Figure 2.3. Since the sensor

hardware had already been purchased and was available in the lab, this step consisted

primarily of writing the low level code to communicate with the Fm controller. and

commissioning the system. This task was difficult because force polling functions do not

work reliably when trying to communicate using both the serial port and the parallel port.

This problem was eventually overcome by requesting a continuous Stream of data frorn

the FR controller. In hindsight. a better approach would be to use the 5532-DMA digital

communication card and the parallel port utilities provided by ATI. Section 3.2 gives a

detailed description of the F/T sensor integration.

Figure 2 3 F/T Sensor connection to the IRIS facility.

3. Vibrations and high torques in the IRIS joints c m eventudly lead to the loosening

of threaded fasteners. Periodic maintenance is required to tighten threaded fasteners, to

elirninate any mechanical looseness or play. However, when typical maintenance failed

to elirninate mechanical play in joint 3, a closer look was taken at the design of the joint.

Two potential sources of play were determined with the help of Jacek Wiercienski

( 1999), and changes were made in order to eliminate the problem. Figure 2.4 shows the

two pins (PINS) added to secure the Circular Spline to the output link (through the

BOTTOM PART). Figure 2.5 shows the screw that was added to secure the torque sensor

rnounting plate (TORQUE PLATE) to the shaft that rnounts on the flexspline (SHAFTI ).

These steps are relevant to force control, since force feedback cm excite vibrations.

which are aggravated by any looseness or play in the joint.

4. The brushless drive amplifiers for the motors are equiped with a current monitor

chat provides a voltage output proportional to the motor current (ihl). In order to

determine the current limiter settings for joints 3 and 4, the current monitor voltages were

acquired by connecting the current monitor outputs to channels O and 1 of the ADC.

These channels are nonnally used for torque sensor readings from joints 1 and 2 of a m

one. Acquiring the motor current also allowed the relationship between control output

and motor current to be detennined (2.2), (2.3), and (2.4). To wire this connection an

extra section of ribbon cable was added between the ADC and the torque sensor signal

conditioning circuit such that the connection to the current monitors can easily be

removed, and the torque sensors reconnected.

NOT TO SCALE

Figure 2.4 Exploded view of the Circular Spline Assembly.

TORGUE SENSOR

TOROUE

4 PINS

P L A T E

NOT TO SCALE

Figure 2.5 Exploded view of the Torque Sensor Assembly.

5. The end effector roller, shown in Figure 2.6. was added to reduce the affect o f

tangentid fiction forces during contact tasks. Surface friction forces can change rapidly

and the stick slip nature of these forces can lead to limit cycles when using the stiffness

controller. The end effector roller mounts onto the hollow shaft that is connected to the

F/T sensor. The fork is made from mild steel, while the roller is made from copper.

NOT TO SCALE

Figure 2.6 The end effector roller.

2.1.3 Ovemiew of Changes ta IRlS Softwore

Table 2.2 lists the major changes made to the IRIS software for this project. Changes

were made to the HOST software in order to integrate the F/T sensor. Two new motion

functions were added on the RISC side for stiffness control and inner loop based force

control. A bnef description of each change or function is given below. An updated

Iisting of the program connect structure, showing al1 the IRIS functions and their

interconnection, is included in Appendix B.

Table 2.2 List of functions added to the IRIS software.

7

HOST j

I

1

r-para( reads a sarnple of force data 1 i I

1 init-ATI-read( )

I IUSC

initializes the PIO-12 for communication with the FR sensor

1 motion using inner loop based force control

l

1

EXTERNAL PROGRAM

stiffj( ) motion using stiffness control I I

Two Iow-level digital communication functions were added to iris-1ib.c. The function

int init-ATZ-re~d(void) is used to initialize the PIO-12 and clears the AT1 output buffer.

The function int read-ATl(short* &tu) reads one 16 bit word of data from the parallel

port and generates the appropriate handshaking signals for communication with the F/T

controller.

l for start-up and testing communication with the force sensor

Changes were made to the functions in the file seri0.c (Appendix B) to allow the use

of cornrn port 2 for senal communication with the F/T sensor at a Baud rate of 38.400.

The function send-string( ) was added to send commands to the F/T controller over the

serial port. A few lines of code were also added to the file h-wt.c to start force data

acquisition and allow bias forces to be set (to zero the readings) during IRIS system

initiaiization.

A brief surnmary of the two motion functions added to RISC is provided here. a more

detailed description is provided in Chapters 3 and 4. Stiffness control is implemented in

the function int stiffjfloa& ts, pslO, p s l l , ps12, ps13, ps20, ps21, ps22, ps23, irtt hold).

A 5'h order polynomial is used to generate the desired trajectory between the initial

position (or the final position of any preceding motion comrnand) and the desired final

position. The joint errors, veloçity estimate, and measured forces are used to implement

stiffness control. The variables pslO and p s l l are used to allow smooth changes in the

desired stiffness, the variables ps12 and ps13 allow smooth changes in the bias force.

The gains for the PI stiffness error compensator can be changed by editing the function

itself in r-1iba.c.

The inner loop based controller is implemented in the function in& line ~Cf loat fJ; xJ

yS, psZ2, int ps13), which uses linear functions with parabolic blends to generate the

desired Cartesian trajectory. The Cartesian trajectory is transforrned into a joint space

trajectory using the inverse kinematics. The outer force control loop adds Cartesian

offsets to the desired trajectory. Joint PID controllers are used to track the joint space

trajectory. The variable psl3 allows the outer force control loop to be disabled, and ps12

is used to provide smooih changes in the bias force. The estimate of the environment

stiffness cm be changed by editing the function itself.

2.2 FORCE SENSOR ICNTEGRATION

2.2.1 Sensor Description and Connecîion

The wrist mounted force sensor, mode1 m/30/100, serial number Fï3102-3F, is

manufactured by AT1 Industrial Automation (refer to AT1 manual, 1993). It is rated for a

maximum force of 133.4 N (30 lbs) and maximum torque of 1 1.3 N-m ( 100 inelbs). The

transducer is c o ~ e c t e d to a F/T. controller (refer to Figure 2.3). The F R controller is set

in fast communication mode, and the intemal update frequency is set to 1200 Hz (from

700 Hz) using a startup rnacro. The force controller rnonitors the analogue strain gauge

readings from the transducer. It cornputes the vector of forces and torques acting on the

sensor from the strain readings, and transfers this data through the parallei port (binary

mode). The parailel port of the Fm controller is connected to the PIO-12 digital

communication card, manufactured by Keithley MetraE3yte Corp. (re fer to manual

Keithley, 1992), which is installed on the host computer. Serial communication is uscd

to send comrnands to the F/T sensor. Nickalls and Ramasubramanian (1995) give a good

description of the registers of the Universal Asynchronous Receiver Transrnitter (UART).

used for serial communication. The host computer controls the communication with the

F/T controller. Each control cycle (Le. every 2ms) the host checks that new data is

available from the FIT controller. Then the PIO-12 is used to read the binary data, and to

give the appropnate handshaking signals to the FR controller. This transfer of data

takes about 0.4 miliseconds. The data consists of seven 16 bit binary values: the error

flag, three forces, and three torques.

2.2.2 Force Sensor Sfart-up Procedure

During the IRIS system initialization (before entering the real time kemel). the query

Stream (qs) command is sent to the F/T controller over corn port 2. The qs command

initiates a continuous Stream of data from the Fm controller to the parallel pon. The data

is updated as fast as it is read, within the limits of the intemal update rate of the F/T

controller. The user can also set bias levels for the forces and torques during the

initialization. Before launching the IRIS software. communication with the F/T system

should be tested using the independent C program called para5.c.

2.2.3 Parallel Port Connection

The pin assignments for the ATI Parallel Interface and the PIO- 12 are shown in Figure

2.7. The 16 output pins of the AT1 Parallel interface arc: connected to Ports A and B of

the PIO-12. Port C of the PIO-12 is used for handshaking signais. PCO and PC 1 are used

as input lines and are connected to !STB and !ACK respectively. PC4 and PC5 are used

as outputs connected to the !OBF and B F lines. The input pins on the AT1 Parallel

Interface are connected to ground (pin 2 1 of the PIO- 12), except for three lines which are

connected to +5 V (pin 20 of the PIO-12). Thus the AN fast polling command is hard

wired into the connection, and is always ready to be sent to the Ffï controller.

PI042

Figure 2.7 The p d l e l port connection.

Fff Controller

2.2.4 Transformation to End Effector Frame

In the RIS software the F/T readings are available for acquisition to the data file and

for use by the RISC CO-processor. The raw F/T readings are given with respect to the

XYZ frame of the sensor. These forces are then transformed to give the forces and

torques in the end effector (or hand) frame. The end effector frarne is related to the

sensor frame by a rotation of 23" about the sensor Z a i s , and a translation of 92 mm

(3.622 in.) dong the sensor Z axis. The transformation of forces to the end effector

frame is done in the the scale-sensors( ) functions on both the host and RISC sides (in

r-1ibs.c and hctr1.c).

Figure 2.8 Transformation of forces from sensor frame to end effector frame.

2.2.5 LQW Pass Filtering

During free motion the wnst mounted Fiï sensor measures the acceleration of the

mass in the robot "hand". Force measurements are noisy and the force sensor is sensitive

to vibrations dunng motion. A second order Butterworth low p a s filter is used to

remove noise and high frequency vibrations from the force sensor signal, The difference

equation for a second-order low-pass Butterworth digital filter with unity gain has the

forrn

y, =b,xk + b2x&, +b'3~~-2 -a?yk-l - a3 Y k - l *

where y is the filtered variable,

x is the unfiltered variable,

xk is the value of x at time tk,

xkml is the value of x at tk-1,

t k = kT is the current time,

T = tk-tk-1 is the constant sampling interval, and

k is an integer.

The coefficients a; and bi can be designed based on the desired cut off frequency using

the Matlab comrnand: [b,a] = butter(N,Wn). For a second order filter N = 2, and the cut-

off frequency Wn must be between O and 1, with 1 corresponding to the Nyquist

frequency (or half the sample rate). Given a sampling rate of 500 Hz, the coefficients for

a filter with a cut-off frequency of 5 Hz (3 1.4 rads) are determined by setting Wn = 0.02.

The resulting coefficients are b=[0.000944 0.00 1889 0.0009441, and a=[- 1.9 1 1 2

0.91501. This filter was impiemented in the scale-sensors( ) function in r-1ibs.c in the

IRIS software. The time delay caused by this filter is roughiy 0.1 seconds (see Figure

4.8).

The frequency response for this discrete filter was generated using Matlab and is

shown in Figure 2.9 beiow. The Nyquist frequency for the system is 250 Hz ( 1570.8

rad/s). The command bode(sys, 1: 1570) generates the frequency response of the range of

frequencies from 1 to 1570 rad/s. The discrete system is defined using the command

sys=tf(b,a,Ts), where Ts is the sarnpling penod (0.002 s).

Bode Diagrams

Frequency (rad/sec)

Figure 2.9 The Bode diagram for the low pass filter.

2.3 MODEL

The dynarnic behavior of the joint actuators is examined in this section. A brief

description of the system components and their nominal parameters is presented.

followed by the motor mode1 and a description of the experiments done to determine

some important mode1 parameters.

This investigation shoutd provide a better understanding of the relationship between

the control output (ur2) and the joint torque. This relationship is relevant to impedance

control, since impedance control theory assumes that joint torques are controtled.

2.3.1 Nominal Parameters

The control output ur2 is scaled and then sent to the digital to analogue converter

(DAC board) which sends a voltage (Vont) to the motor amplifiers. The DAC board uses

a 12 bit vaiue and has a range o f f lOV. The relationship between ur2 and the vo

- 10 v ~ ~ c j - 2047 39.35 -CU-2, for joint 3, and (2 .2 )

- 10 VDAC, - - (-68.0272) - ~ 2 , for joint 4. (2 .3)

2047

Physicaily the control output corresponds to a voltage; however, in this report it will

be treated as both a variable and a unit. The scaling factors appear to have been chosen

to make the control output roughly equal to the stalled motor current (see Table 2.7)

The brushless drive amplifiers are manufactured by Kollmorgen (refer to document

SBA Series, Operating Instructions: Servo Amplifier for brushless nzotors, Gali 1 Mot ion

Control Inc.). Al1 the joints for the IRIS facility use the sarne type of amplifier, and its

nominal parameters are sumrnarized in Table 2.3. They are set in current mode. and thus

supply a current to the motors proportional to the input voltage ( V D ~ ~ ) . The current

control bandwidth is 2 kHz. The amplifier supplies a pulse width modulated (PWM)

voltage to the motor. The PWM frequency is 20 kHz. A constant gain (K,) relates rnotor

current to voltage for the Iinear range of the amplifier:

i~ = V m c - (2.4)

Severai additionai factors affect the rnotor current: the motor back electro-motive

force (back emf). and the amplifier current lirniters. At high velocities the back emf

limits the current according to

i, = (Vs - ) 1 R (2 .5 )

where V, is the amplifier output voltage (24 V),

KBmfF is the motor back EMF constant,

R is the motor resistance, and

&,,, is the motor velocity.

The amplifier output current

The current lim?ers protect the

by changing

lirniters is an

two resistors on the motor

important non-linear effect.

limited using peak and continuous current limiters.

from overheating, and the settings c m be adjusted

amplifier board. The behavior of the current

The continuous current limiter settings for a m

two joints 3 and 4 were estimated experimentdly by acquiring

bmshless drive amplifier current monitor.

Table 2.3 Brushless Drive Amplifier.

the voltage outputs of the

(Kollmorgen) BCL-02806-A0 I

Pulse Width Modulation (PWM) frequency [kHz]

Supply Current [A]

Supply Voltage [VI min 24 max 32

Continuous 6 Peak 10

36.Y

- -

Output Voltage V' [VI min 16 max 28

Output Current RMS ihf [A] Continuous 6 Peak 10

Output Power [W] Continuous 144 Peak 240 I

Current rnonitor output [V/A] I I 1

Current loop bandwidth [kHz]

*meas11 red A m 2 Joint1 November 1999

The motors are brushless dc-rnotors, models RBE- 1202 and RBE-0070 1 for joints 3

and 4 respectively. Table 2.4 shows the nominal motor parameters. Three Hall effect

sensors are used to control commutation.

The rnotor torque sensitivity (Kr) relates the rnotor torque to the motor current:

z , ~ = KtiL, . (2 .6)

Table 2.4 Brushfess DC Motors.

RBE- 1202 RBE-0070 1

1 Maximum continuous stall torque [N-m] 0.250

Maximum continuous stall Cunent [A]

1 Peak Torque [Nam] fl5%

Maximum Continuous Output Power

2.7

1.12

1 Peak Current [A] f 15%

i 1.7 I

1

1 Back EMF K e E M ~ [V/(rad/s)] + 10%

10.9

Torque sensitivity Kr [hT-m/A] + 10%

I 6.5 I

O. 102

Weight [g]

0.059 1

3.5 I l

I

1

Inductance L [mH]

No load Speed [RPM]

Inertia [kg m']

Resistance R [ohm @ 2S°C] f 12% 2.2

1.3

2150

1 2 . 0 ~ 1 o - ~

1.7

3750

1-4x10" i

The motor shaft is connected to the wave generator of the harmonic drive gertr. Model

CSF-50-2A-GR size 20 hannonic drives manufactured by HD Systems Inc are used for

both joints (refer to "CSF Series prodirct and designer's gliide: No. CSF-1-1/96 HD

Systems hc., 1996). The flexspline is fixed to the input link through the torque sensor.

The ouput link is connected to the circular

another important non-linear effect.

The stiffness of the hannonic drive is

1 Rated torque at 2000 RPM [N-m] !

Table 2.5 Harrnonic Drive Gears

1 ~ a x i r n u m input speed [RPM]

Reduction ratio N (with CS as output)

S ti ffness [N-rnhad] K1 (0-7 N-m)

Model CSF-50-2A-GR size 20 (ratio 50)

5 1

K3 (over 25 Nom) 1

23000 I

Approximate Input Inertia [kg cm']

Weight [kg]

1 No-load back driving torque [Nm] I 4.0

O. 193 t I i

0.28 1 I

I

No-load starting torque [N-m] 0.065 1 i

The link from joint 3 to joint 4 is made from a hollow aluminum tube. The F/T sensor

is mounted on joint 4. Table 2.6 shows the link parameters. The inertia values were

estimated based on the link geometry (Emami, 1997).

Table 2.6 Link Parameters.

Joint 3 Joint 4 l

I I

Length 1 [ml

Distance to COG 1, [ml

0.250

Mass rn [kg]

I I i *without the end effector roller

f

0.202 (O. 180y 1 i

O. 142

2.3.2 Control Model

Figure 2-10 shows a sketch representing one possible physical model of the actuator

system. The same form could be used for both joints, although the load inertia for joint 3

0.05 i 2.8 10

2

0.002 1 !

1

will change with the position of joint 4. This model is presented only to provide some

Moment of inertia I [kg m']

physical understanding for the relationship between the control output (trr-2) and the

motor torque, and to point out some of the major non-linear effects. Identification of the

structure and the parameters for a model of the internal dynamics of the joint is beyond

the scope of this research.

0.603 (0.503)*

0.04

4

Motor Amplifier r""""""".

L

P" r"""""""'

Harmonic Drive J L

Figure 2.10 Actuator model.

The scaled control output ur2 is converted into the voltage VDAc (3.1) (3.2) which

drives the motor amplifier. The amplifier sends a PWM voltage to the brushless dc-

motor resulting in current b. NorrnalIy this current is proportional to the input voltage

(2.4), but it can saturate either due to the limited supply voltage and motor back emf

(2.3), or the effects of the current limiters (see Table 2.7). The motor converts the current

to torque (2.6). The interna1 joint dynamics are complicated but could be modeled as ri

system with inertia, damping, and disturbance torques on either side of a non-linear

spring and reducing gear. The disturbance torque (rd) could include the static and

Coulomb friction effects, the effect of payioad, extemal forces, and the dynamic coupling

from joint 4 to joint 3. Link inertia Jt is constant for joint 4 but changes for joint 3. The

damping term BL is the overall damping factor due to the viscous friction in the bearings

and the harmonic drive gear.

The main challenge to developing a model is to deal with the behavior of the harrnonic

drive gear. Another challenge is in the limitated accuracy of the nominal parameters.

For example, the nominal motor resistance R bas error bounds of f 12%. Non-lineÿr

effects include the saturation of the motor current, static friction. and the non-tinear

stiffness of the harmonic drive gear.

This investigation is not concemed with developing a model for the joint dynamics.

However, before implementing stiffness conuol two sets of open loop experiments are

performed to provide a better understanding of the relationship between the control

output and the motor current.

2.3.3 Joint Cornpliance

The compliance of the joints will play an important role in the performance of

stiffness control. The purpose of this section is to present the experirnental results for a

simple joint compliance test. For the IRIS system, the joint angles are rneasured using

Hewlett-Packard HEDS-5000 (500 CPR) incremental optical encoders mounted on the

motor shafts. The actual output shaft angle differs from the expected angle (based on the

gear ratio and the motor shaft angle) because of the compliance of the joint. Although

the output shaft angle is not rneasured, the joint compliance can be tested simpiy by

holding the output shaft in a fixed position, while changing the motor torque. Data is

acquired using the IRIS interface. A similar test was performed by Kircanski ( 1993) and

his results for arm one joint 4 are included for cornparison within "{ }" braces. Kircansky

has shown that most of the compliance occurs in the harmonic drive gear assembly. The

harrnonic drive gear behaves as a stiffening spring with hysteresis. The following tests

sirnpl y demonstrate the average stiffness, and the maximum de flection expected under

static conditions using full motor torque.

joint 3

-101 1 t 1 I 1 1 I

O 2 4 6 8 10 12 14 time [s]

Figure 2.11 The cornpliance of arrn two Joint 3.

Figures 2.1 1 and 2.12 show the deflection of the motor shaft as motor torque

increases. The top graph shows the measured motor angle, while the output shaft angle is

fixed at 90". The bottom graph shows the control output rrr2, the measured current

(crcrr). and the rneasured joint torque (tq2). The maximum deflection of the motor shafts

at full stalled motor torque are: 106.1 O (equivalent to 106.1/5 1=2.0S0 link angle) for 26

N-m torque for joint 3, and 18.0" (0.353" link angle) at a torque of 8 N.m for joint 4.

Note that the measured joint torque and motor angle for joint 3 are not changing

smoothly with the motor torque. This is due to sticking or wedging of the harmonic

drive. Also note that the current is proportionai to the ramped control output unril

reaching the level of the current limiter. Saturation occurs at a control output of 5.5 and

3.2 [rrr2], with peak currents of 4.5A and 4.4A, for joints 3 and 4 respectively.

O

1 I 1 I 1 1 1 1 II

2 4 6 8 10 12 14 16 18 time [s]

Figure 2.12 The cornpliance of arm two Joint 4.

The range of motor torque available for joint 4 is lower than for joint 3. and

deflections for joint 4 are also Iower. The effect of the current limiter is clearly seen.

The joint torque only begins to increase after the motor current reaches a value of roughly

1.5 A. This is because of the no-load starting torque of the harmonic drive unit. Note

that the joint torque stops increasing before the peak current is reached. This may be dur

to a wedging problem with the harmonic drive gear, similar to that observed for joint 3.

The continuous current limiter settings, and the amplifier gain (KT) c m be deterrnined

from the compliance test data. Table 2.7 sumrnarizes the results. Note the difference

between the torques predicted using the nominal torque sensitivity and the measured joint

torques (refer to the bottom two rows of Table 2.7). Note that the units of the control

output are not scaled to give the static torque in N-m.

Table 2.7 Brushless drive amplifier current limiter settings.

Peak Stalled Current [A] 1 4.5

Joint 3 Joint 4

Max Continuous Stalled current [A]

Max Stalled Motor Torque [ N m ]

ut-2 proportional to static torque [N.mllrr2]

4.4

constant from ur2 to curr [A/~rr2]

3.8

0.54

average sti ffness Ka,, [Ndrad ]

0.25

0.82

1 1

1.20

amplifier factor Ks [ A N ]

predicted motor torque N Kr i~ [N-ml

4.2

Max measured torque rmDs [N-ml

4.1

t 22.3

26

13.0

8

2.3.4 Joint Velocities

The joint velocities are computed using a discrete approximation, based on the current

position of the motor shaft, and the position 10 miliseconds earlier. For a digital

implementation the discrete values of measured position from a position sensing device

are available at each sampling interval. The simplest digital approximation of

differentiation is to use Euler's method (Franklin, 1998). To exactly know the derivative

at a given time t requires knowledge of both the past and future values of .r. For a finite

time interval T the following relationship will be approximately true

where T = t k - q ~ (the sample interval),

tk=kT (for a constant sample interval),

k is an integer,

x(k) is the value of x at t k , and

x(k-1) is the value of x at tk -1 .

This equation is known as the backward rectangular version of Euler's method.

The discrete nature of the optical encoder position measurement introduces noise into

the velocity approximation. The velocity signal is also sensitive to mechanical vibrations

of the joint. Noise can be removed using an appropriate filter. Filtering in real time

introduces unwanted delay; however, dunng postprocessing it is possible to filter without

adding delay. Thus reasonable estimates of the joint velocities and accelerations can be

determined dwing postprocessing.

The discretization error of the optical encoder depends on the resolution of the encoder

as well as the gear ratio of the harmonic drive. Al1 the joints of the IRIS facility are

equipped with Hewlett-Packard HEDS-5000 optical encoders with a resolution of 500

cycles per revolution (CPR). Thus the angular resolution on motor shaft angle is

360°1500 = 0.72". The gear ratio for the two joints used in this expriment is 1 5 1. thus

resolution on output shaft angle is e, = 360°/(500*51) = M.0141176471°. The

maximum error in the velocity, based on a time period of 10 miliseconds (or 5 samples).

is 2*em&(5*T) = 0.0493 radians/second (2.82"/s). This estimate of velocity reduces

noise, since it is obtained by taking the average velocity over several sarnpling intervais.

It is used in the RIS software without further filtering. To obtain a reasonable estimate

of the velocity error, the estimated velocity should be compared to the average desired

velocity over five sampling intervals.

2.3.5 Open b o p Step Response

The maximum velocity and the overall friction coefficients can be determined from

open loop tests. in this experirnent the output link is not fixed but free to move. and a

step change in control output (ur2) is applied to the joint. The motor amplifier then

forces the motor current to be proportional to the input voltage (3.3). After a brief period

of acceleration the joint reaches an equilibrium velocity where friction [orques balance

rnotor torque. The effect of the motor back ernf on the maximum motor current is seen

from the experimental results, and the maximum joint velocity is identified.

Sources of friction include bearing friction and friction in the harmonic drive. The

harmonic drive seems to dominate friction effects. Note that improper rissembly of the

hannonic drive can lead to the dedoidal condition, which causes high friction Ioads. In

the dedoidal condition, meshing of the teeth between the Flexspline and Circular Spline is

eccentric rather than syrnmetric. The nominal static friction behavior of the harmonic

drive is described by the no-load starting torque of 0.û65 N-m and the no-load back

drîving torque 4 N-m (Table 2.5). The no-load starting torque can be compared with the

Coulomb friction estimates in Table 2.8. The friction torque values in Table 2.8 are

based on the measured motor current and the nominal torque sensitivity.

Table 2.8 Friction effets.

Maximum joint velocity [rad/s]

Percent of no load speed

Coulomb friction [N-ml

Assuming that friction behavior can be described by constant Coulomb and viscous

friction coefficients, two data points can be used to estimate the coefficients for joint

Coulomb and viscous friction. These values are shown in Table 2.8, and the results

obtained by Kircanski (1993) using arm one joint 4 are included for cornparison.

Joint 3

3.66

83%

Joint 4 I 1 1

3.79 { 4 7 1 ] * 1

49% (61%)

3.25 1a.0627

0.798 { 1 .O57 }

0.75- 1 .O0

Viscous friction [N.m/(rad/s)]

ur2 to overcome static friction

2.315 1 1 { 2.03 )

*{Kircanski, 1993: 106)

1.15

0.45-0.5

The friction loads can be estimated using two open loop responses, by assuming the

relationship between motor torque and friction is described by

rM = K r i M =û+beM, (2.8)

or in tems of control output (ur2) using

where a is the Coulomb friction term, and

b is the viscous friction tenn.

It is convenient to express a and b in tems of the control output (ur2). Then usinp

(2.9) and two steady state velocities from the open loop responses for joint 3 one obtains

which can be solved to give a = 0.42 [w2] and b = 0.00446 [rtr2/("/s)].

Sirnilarly for joint 4

results in a = 0.737 [ur2] and b = 0.00526 [~2/("/s)].

For joint 3 the range of control output (ur2) step inputs examined is from 0.5 to 3.0

Cm-21. A control output above 1.25 [ur2] is high enough to reach the maximum joint

velocity, while control outputs below 0.45 [ur2] are unable to overcome the no-load

starting torque of the harmonic drive. The joint position response [degrees] to these

currents are shown in Figure 2.14, while the velocity response [degrees/second] is shown

in Figure 2.15. The steady state velocities are 18"/s, 68"/s, 130°/s and the maximum

velocity is 210°/s.

Figure 2.13 f oint 3 motor current step response.

From the response for a convol output of 2.0 and 3.0 [ w 2 ] it is clear that the

micximum motor current is limited to roughly 1.0 A when the motor is running at its

maximum velocity. This is due to the back emf effect of the motor, the current limiters

do not affect the free motion response for the range of control outputs examined. Below

this threshold (1.0 A or 1.25 ur2) the motor amplifier is able to maintain the proportional

relationship between control output and motor current.

1 I I 1 1 L

20 40 60 80 1 O0 120 time [s]

Figure 2.14 Joint 3 positions.

20 40 60 80 1 O0 120 140 time [SI

Figure 2.15 Joint 3 estimated velocities.

time [s]

Figure 2.16 Joint 3 measured torques.

The rneasured torques jump up briefly as the link accelerates. Once the steady state

velocity is reached the torque drops nearly to zero. This shows that the viscous drag on

the link side of the torque sensor is fairly low. Thus most of the viscous damping is

within the hannonic drive assembly and the motor bearings.

The range of step inputs examined for joint 4 is from 1.0 to 4.0 [iw2]. An input of 1.8

[rtrZ] would be high enough to reach the maximum joint velocity, while inputs below

0.75 [ur2] are unable to overcome static friction. The positions [O] are shown in Figure

2.18 and the estimated velocities ["/SI are shown in Figure 2.19. The maximum velocity

for joint 4 is 2 17 "/S.

-0.5 1 1 a 1 I I 1 1 O 20 40 60 80 100 120 140

time [s]

Figure 2.17 Joint 4 motor current step response.

The current lirnit is approximately 2.3 A at maximum velocity for joint 4. A control

output ur2 = 1.0 results in a current of 1.2 A, which agrees with the relationship between

control output and current found in the cornpliance test.

The maximum measured motor current is only 2.3. while the current limit is 3.4f1.5 A

at a maximum velocity of 217"/s, based on the nominal parameters and (2.5). The values

agree within the confidence interval defined using the uncertainty of the nominal

parameters, but with so much uncertainty in the nominal parameters (back ernf constants

and resistance are shown in Table 2.4) it is difficult to accurately predict the motor

current.

60 80 time [s ]

Figure 2.18 Joint 4 measured positions.

Figure 2.19 Joint 4 vekities.

O 20 40 60 80 1 O 0 1 2 0 140 time [s]

Figure 2.20 Joint 4 torques.

The Coulomb friction for joint 4 is high. Since the link mass and inertia for joint 4 is

lower (compared to joint 3), and the maximum motor torque for joint 4 is also lower.

friction loads dominate the torque response shown in Figure 2.20. In contrast. the joint 3

torque response clearly shows an initial acceleration stage.

2.4 SUMMARY

This Chapter describes the IRIS faciiity. and the changes made in order to use it for

testing force control suategies. Integration of the FFI' sensor was a key step towards

implementing and evaluating the performance of different force control schemes. A

sarnpling rate of 500Hz was achieved for FR data by using the parallel port for fast

communication.

The non-linear behavior of the motor amplifiers, and the torque transmission

characteristics of the harmonic drive, have a significant effect on the relationship between

control output and the joint torque. The open loop response of the joints. both for free

and constrained motion, is presented. The results are used to estimate the average joint

compliance and friction parameters.

From the open loop tests the behavior of the joints is better understood. For example.

for joint 3 the following observations can be made:

urZ3 < 0.5 results in no joint torque, no joint velocity because of the no-load starting

torque of the hmonic drive gear.

FREE MOTION

0.5 < < 1.25 the steady state velocity is proportional to the control output.

1.25 < ur23 the initial acceleration varies with ur& but the same maximum steady

state velocity (210°/s) is eventually reached. The joint torque is high durinp

acceleration but drops down low once a steady velocity is reached.

CONSTRAINED (for slowly changing ur&)

0.5 < < 3.2 the static torque is proportional to the control output.

3.2 < urz3 the static torque is limited to 25 N-m by the action of the current limiter.

There are two significant non-linear effects: the brushless drive amplifier current

limiters, and the stiffness of the harmonic drive gear. Friction has a significant effect on

the dynamic behavior of the joint. An unusual stick-slip behavior of the harmonic drive

w i ~ observed for constrained motion.

For linear time invariant systems there are severai standard techniques to ensure

stabiiity. For non-linear systems, stabiiity can be tested either through simulation or by

operating the actual system (Phillips and Harbor, 1996: 209-10). Developing or

identifying the parameters of a suitable non-linear mode1 for this system is beyond the

scope of this research. Hence the stability of the stiffness control scheme wiil be

demonstrated through actuai tests on the IRIS facility.

CHAPTER 3 STIFFNESS CONTROL RESULTS

"Mensch denkt, Gott lenkt"

3.1 THE STIFFNESS CONTROLLER

The purpose of this chapter is to demonstrate the use of force feedback to implement

stiffness control. In Section 1.3.2 the advantages of using force feedback for

implementing stiffness control were discussed. Theoretically force feedback is not

necessay for implementing stiffness control. Stiffness control experiments without force

feedback have shown poor sensitivity to extemal forces, because of static friction and the

no-load back driving torque of the harrnonic drive (refer to Table 2.5). The eifect of

static friction on the external forces can not be compensated without force feedback.

For practical applications force feedback can be used to define the system error and

impose the desired behaviour despite static friction and the di fficulty in controIling joint

torques for manipulators using harmonic drive gears. Figure 3.1 shows a block diagram

of the stiffness controller using force feedback.

I T ~ S form F m Scnsor to joinls * Dynrimics

Figure 3.1 Block diagram of the stiffness controller.

The stiffness controller uses a proportional integral (PI) compensator to regulate the

stiffness error (or torque error e (1.16)). The desired relationship between external

forces and Cartesian erron is defined according to (1.7). Both a dead band and limiter

are used with the integral controller. The dead band is included to avoid iimit cycles

caused by static friction, while the limiter reduces the influence of transient impact forces

(Salisbury, 1980). When the desired stiffness is low. including a static friction

compensation term improves uajectory tracking. Force feedback compensates for the

complicated relationship between the control output and the torque applied to the joint.

which is especially relevant when applied to manipulators with geared drives. However.

the force sensor has some dynamics or delay, and thus the perf'orrnance of force feedback

is subject to some limitations, especialiy for compensating sudden friction or dynamic

disturbance torques.

The torque error, defined by equation 1.16, is written for each joint

eT3 = ~,,(J,, 'K, + J ~ , ~ K ~ , . ) + ~ ~ ~ ( J ~ , J , ~ K ~ + J21J22KJ, If 5D3 .. and (3.11

eT4 = eq3 ( + J,, J ~ K & ) + eq*(~IZ'K<li + J ~ ~ ~ K ~ ~ ) + 'b4 - 5m4 7 (3.2)

where Ji2 is the rem (lst row, 2nd colurnn) frorn the manipulator Iacobian (1.13).

e,], e,q are the joint position errors for joints 3 and 4 respectively,

Kdr, Km. are the desired stiffness in the X and Y directions respectively,

' b 3 FbiusX + J 2 1 FbiusY and

z = J12FbiosX + JI FbiUIY are equivalent joint torques for the bias force, and

rpd, Tm4 are the joint torques based on the measured forces.

Then the control law used to compute the control output (see 2.3. 2.4. and 2.5) for

stiffness control is

irr2, = e , , ( ~ , , * ~ + J ~ , ~ K , ) + ~ , , ( J ~ ~ j l Z K , + J 2 t 1 2 2 K h ) + K , q 3 +r,, + K,er3 + j ~ [ e , ~ (3.3)

2 11~2, = e q 3 ( J I I J l 2 K B + J ~ , J ~ K ~ ) + ~ ~ ~ ( J ~ ~ ~ K ~ + J12 K A ) + K F q 4 +Th4 + Klell + j ~ ~ e ~ ~ (3-4)

where the integral term is computed subject to the deadband and limit on e,. and

ut-& and u& are the control outputs for joints 3 or 4 respectively.

The expressions in (3.3) and (3.4) only make sense if the control output is scaled to be

roughly equivalent to the joint torque. An appropriate scaling factor. based on the

cornpliance test data (Table 2.7). must therefore be added to the stiffness and bias torque

terms.

During free motion the measured forces are nearly zero (provided force required to

accelerate the mass of the robot 'hand' is low). Then the effect of the force feedback

during free motion is to drive the joint position errors to zero. It is necessary to add a

static friction compensation term ( %rion) to reduce trajectory tracking errors. especiaily

when the desired stiffness is low. Conservative values for the static friction coefficients.

based on the results shown in Table 2.8, are 0.4 and 0.8 [tir21 for joints 3 and 4

respectively. Then the static fnction terms added to (3.3) and (3.4) are given by.

2 fric<iun = Cf ~gn(ild ) (3.5

where Cf is the static fnction coefficient (symrnetric).

The stiffness control algorithm is implernented by adding a new motion function.

called s t i f f j ( ), to the IRIS system. It uses the joint angles. joint velocity estimates. and

force measurements to implement the stiffness conuoller shown in Figure 3.1. The user

specifies the duration of motion and the final position when calling the function. The

bias force and desired stiffness is also specified when calling the functions. and any

change from the previous settings will be made smoothly over the duration of motion.

The desired trajectory (qc2) is generated using the same 5" order polynomial scheme

(Craig. 1989: 237-8) that is used in other IRIS motion functions. The trajectory is

updated at the same rate as the control cycle. The trajectory is computed brtsed on the

desired time of motion, with zero initial and final velocity and acceleration. Then the

position, velocity, and acceleration at tirne t is given by

4 B(r) =O,, +a,r3 +o,r +as[ 5

3 where a, = 10(Bf -0, ) I r f ,

9 is the desired time of motion,

9 is the final position, and

8, is the initiai position.

In order to ensure smooth changes in error, the final position of the preceding motion

comrnand is used as the initial position for the next leg in the trajectory. Previous motion

functions in the IRIS software assumed that joint errors are low, and did not provide

continuity in joint error. Such an assumption is certainly not vaiid for force control.

3.2 EXPERIMENTAL RESULTS

The first experiment shows the performance of the stiffness controller in contact with

a very stiff environment. Then the use of stiffness control to apply a desired normal force

on the environment is demonstrated.

3.2.2 Imposing the Desiired Stiffness

The purpose of this expenment is to test the ability of the stiffness controller to

achieve the desired Cartesian stiffness. The apparatus, shown in Figure 3.3. consists of

the two link planar manipulator formed using arm two joints 3 and 4 of the IRIS facility.

The links are free to move in the horizontal plane, thus gravity compensation is

unnecessary. The environment is provided by an aluminum angle iron which is bolted to

the mounting surface of the other IRIS arm (arm one). A piece of aluminum sheet metal

is fixed to the angle iron and acts as a cantilever beam, providing a stiff surface. while the

angle iron itself provides an essentially rigid contact. Although no surface is truly rigid.

the deflections of the angle iron are negligible when compared to the visible deflections

of the flexible metal plate or the compliance of the manipulator. The end effector roller

was used. since it reduces the contact friction forces in the X-direction (tangential to the

plate).

The procedure consists of the following steps: initialize the force sensor. start the

RIS interface, run the planned trajectory and collect data. Postprocessing is performed

using Matlab.

For the first experiment, the manipulator is comrnanded to move between two points

(Figure 3.2). The commanded trajectory moves from point A (-30°, 75") to point B (-20".

75") in 2 seconds, holds that position for 1 second, and moves back to point A. As the

manipulator rnoves towards point B it contacts the environment (see Figure 3.2). Joint

positions, velocities, control output, motor curent, measured joint torque and measured

end effector forces are

summarizes al1 the data

included in Appendix C.

collected at 100 Hz using the IRIS interface. Table 3.!

files of experimental results. A sample from a data file is

Table 3.1 Surnrnary of data files.

n3- 1 .dat 1 Cornpliance test

Data file

n34.dat 1 Cornpliance test

step3*.dat l Open loop step response -joint 3

Description Postprocessing

stablel-dat I Stiffness control - stiff environment

I

jurnp2.dat 1 Stiffness control - rigid environment

step4* .dat

Inner loop based force control i n29.m

Open loop step response -joint 4

d 13-4.dat

stepfig-m

Stiffness control - bias force d13.m

Figure 3.2 The apparatus used for stiffness control experiments.

Figure 3.3 Contact with a stiff environment.

The desired stiffness has been set to Ke&,=2.0 N/rnrn, and the velocity damping

term is K,*.3. The proportional and integral gains for the stiffness error regulator are set

at Klp=O.l, and &=0.01. The time history of position (92) and velocity (il@) for joints 3

and 4 of arm 2 are shown in the top two graphs of Figures 3.4 and 3.5. The desired

trajectory (qc2) is also shown. The Tirne history for control output (ur2). the measured

torque (tq2) and measured motor current (curr2) are shown in the bottom graphs.

Contact occurs at roughiy t = 0.65 seconds as c m be seen from the measured torques (and

forces Figure 3.8). Joint three reaches an angle of -21.3, instead of the desired mgle of

-20 because of the contact forces.

-2 I 1 I 1 I 1 1

O O. 5 1 1.5 2 2.5 3 3.5 time [SI

Figure 3.4 Joint 3 Response - ngid environment.

Ln Figure 3.4 the current drops to 0.1 A after time t = 1.25 S. Thus the motor torque

required to maintain the measured 2.7 N-m joint torque is practically zero. The control

output ur2=0.38, which is less than the no-load starting torque. This clearly demonstrates

the ability of the no-load back driving torque to support an extemal force.

Figure 3.5 Joint 4 Response - rigid environment.

The initial joint 4 position error during motion is not corrected until after contact is

made. Thus the change in joint angle should be viewed as a response to the X-force. not

a correction to the tracking error. Steady state positioning errors are not surprising

considering static friction torques and the lack of an integral term for the position servo.

The final desired trajectory point (point B), the final point of the measured trajectory.

and the point when contact is first made with the angle iron are shown in Figure 3.6.

Figure 3.6 Cartesian Positions - rigid environment.

0.082

0.08

0.078

c.

E Y

0.076

0.074

0.072

The displacement of the environment is very Iow, and the actual position of the end

effector does not move beyond the stiff surface (Y = 0.074 m). Assuming that the

unfiltered force signal provides a good indication of the instant of contact and that the

environment is rigid, then the difference between the measured position and the actual

position due to the joint cornpliance is roughly 2 mm. Then the actual stiffness is

7.3N/6.05mm = 1.21 N/mm, which is much less then the desired stiffness (2.0 N/mm).

The actual stiffness will always be less than the desired stiffness, even when the

compensator drives the stiffness errors to zero, because the joint cornpliance affects the

measured angle of the IRIS joints.

- rn E I 1 I I I

measured final desired position <0.3508,0.080> - \

- final measured position ~0.35 16,0.0765>

-

- - contact, t = 0.68 4.3528,0.0739>

- I 1 1 1 1 I

0.346 0.348 O. 35 0.352 0.354 0.356 0.358 x (ml

Figure 3.7 Stiffness Error and J ~ F ~ ~ - rigid environment.

O I 1 1

-1 - I'-1 - -

Stiffness errors Iess than M.1 are not eliminated because of the deadband on the

€ z -2

-3

integral term. This deadband prevents limit cycles due to sensor delay and contact

- 0

0

- 3-

- \\

t 1 1 1 1

friction. The measured torque for joint 3 (Figure 3.9) is 2.8 N-m, while the predicted

O 0.5 1 1.5 2 2.5 3 time [s]

torque based on the external force is 2.65 Nom. For joint 4 the measured torque is 0.6

N m while the external force based torque is 1.07 N-m.

During free motion the stiffness errors fiuctuate periodically. The desired trajectory

stops at point B, at time t = 1.0 seconds, and the stiffness errors settle about 0.25 seconds

thereafter.

Forces in base frame 2, r r 1 1 1

-7 - ? '~*usFw~.c-*-~--i=

-8 1 I I L

O 0.5 1 1.5 2 2.5 3 time [s]

Figure 3.8 End Effector Forces - tigid environment.

3.2.2 Discussion of Results: Desired Stiffness

The stiffness controller behaved in a stable fashion, and stiffness error was well

regulated while in contact with the environment. The performance of the stiffness

controller for trajectory tracking dunng free motion is affected by the choice of desired

stiffness: tracking errors for free motion are lower when the desired stiffness is high.

Although the desired stiffness is imposed between the measured position errors and

the measured forces, the rneasured joint angles are separated from the actual joint angles

by the joint compliance, as demonstrated in Section 2.3. Thus the actual end effector

Cartesian position differs from the position based on the measured joint angles and the

fonvard kinematics. Because of joint compliance the actual stiffness is less than the

desired stiffness, even though the stiffness error is well regulated by the PI controller.

The average joint stiffness values Ka,, (refer to Table 2.7) cm be used to estimate the

manipulator Cartesian stiffness Kx and the Cartesian deflections aX due an external

force F. The estimate of the Cartesian deflections due to joint flexibility is given by

ax = J ( K , ~ ) - ~ J ~ F = ( K . ) - ' F . (3 -9)

At tirne t = 2.0 s the externai forces on the manipulator are F = r1.28 - 7 . 3 4 1 ~

Newtons. Using average joint stiffness values of 716 N-&rad for joint 3. and 1375

Nadrad for joint 4 gives the foiiowing estimate of the manipulator Cartesian stiffness

Then using (3.9) the Cartesian deflections of the end effector are 0.4 mm and -1.41

mm in the X and Y directions respectively. This estimate is quite crude, just as an

average iinear stiffness is a poor representation of the non-linear relationship between

defiections and joint torques (refer to Figure 2.1 1). In comparison. the deflection in the Y

direction based on the assumption that the environment is rigid is about -2 mm (see

Figure 3.1 1). Note that the desired stiffness in the Y direction is higher than the

estirnated manipulator stiffness ( 1.55 N/mrn) in the Y direction.

Due to joint compliance the physical meaning of the desired stiffness is lost. In

addition, the physical significance of Cartesian damping, and the Cartesian inertia (based

on measured joint angles) would also be subject to the joint compliance and interna1 joint

dynamics. Thus, if the stiffness controller were to be extended to stiffness and damping

control, or full impedance control, it's ability to impose the desired impedance would be

limited by the joint compliance and the intemal joint dynamics.

The ngid contact demonstrates an important consequence of joint compliance: the

desired stiffness is not physically realized. This problem could be solved either by

changing the mechanical design of the joint to reduce joint cornpliance. or by measuring

the joint angles directly.

3.2.3 Force Conîrol with Stiiness Control

This next expriment demonstrates the use of stiffness control to simultaneously apply

a desired normal force on the environment. A desired normal force can be applied by

adding a bias force in one Cartesian direction and setting the desired stiffness in that

direction to zero (refer to Section 1.3.1). Fn this mannec, the desired normal force can be

applied, despite the fact that the desired stiffness is not physically realized. Rather than

setting the desired stiffness to zero, it was set to a low value to show the effect of position

errors on the steady state force.

The desired stiffness is set to 2.0 N/mm in the X-direction. and only O. 1 N/mm in the

Y-direction. With such a low stiffness it is necessary to include the static friction

compensation terrns to reduce tracking errors d u h g free motion. The static friction

coefficients (3.10) were set to Cf3 = 0.3, and Cf4 = 0.8 [w2] (refer to Table 2.13). The

proportional gain for the stiffness error wîs set to 0.10, and the integral gain was lowered

to 0.002.

time [s]

Figure 3.9 Joint 3 Response - bias force.

Contact is made at roughly t = 2.9s by moving the environment such that the end

effector roller just touches the metal plate. Then between time t = 6.00s and time t =

6.50s the bias force in the Y-direction is rarnped from 0.0 to 10.0 N. The F7 key is

pressed at time t = 8.5 s, which forces the control outputs (and motor currents) to zero.

Figure 3.10 Joint 4 Response - bias force.

The measured positions, and the measured forces are forced to obey the desired

stiffness. Figure 3.1 1 shows that when the bias force is added. the measured position

moves beyond the desired position. This is possible even when in contact with a rigid

environment, because of the joint cornpliance.

Figure 3.11 Cartesian Position - bias force.

The desired stiffness is pulling the rnanipulator back towards the desired position.

Given the Y position error of 3.86 mm, a spring with a stiffness of 0.1 N/mm would

apply a force of 0.386 N in the negative Y-direction. Thus we expect the force in the Y-

direction to only reach 10.0-0.39 = 9.7 1 N, not the 10.0 N comrnanded by the bias force.

By making the desired stiffness in the Y-direction lower, the effect of the stiffness on the

steady state force can be reduced. Pure force control can be applied in a particular

direction by making the desired stiffness in that direction zero.

Figure 3.12 Stiffness Error - bias force.

1

0 1

-1 E = - 2 -

-3

4

Note that the stiffness errors settle to zero (-0.04 N-m) at tirne t = 8.0 seconds, within

I 1 1 1 I 1 1 I a

O ts3 ts4

- - v- z 9 0 - - - -

I 1 1 1 1

about 2.0 seconds from the start of the ramp change to the bias force.

O 1 2 3 4 5 6 7 8 9 1 O

i / l - #

time [s]

After t = 8.0 seconds the joint torques predicted using measured forces are 3.3 and 0.9

N-m, while the measured joint torques from Figure 3.9 and 3.10 are 3.9 N m . and 0.78

N m for joints 3 and 4 respectively.

Forces in base frame 2 1 t 1 t I t 1 1 I

time [s]

Figure 3.13 End Effector Forces - bias force.

The measured Y-force, after time t = 8.0 seconds, is roughly 9.7N, which agrees with

the expected value based on the desired stiffness and the Cartesian errors. The rise time

for the measured force is roughly 0.27 seconds. However, the Y-force only starts to

change at time t = 6.15 seconds, which is 0.15 seconds after the s t m of the ramp in the

bias force. A similar delay c m be seen between the time when the control output begins

to change, and when the torque sensor measurement (an analogue signal) increases in the

bottom graph of Figure 3.14. Thus it can be concluded that most of the 0.15 second delay

is due to the interna1 dynamics of the joint, not the sensor dynamics. The peak Y-force is

10.58 N, hence the overshoot is 5.8%.

Note that when the motor torques are zero (after time t = 8.5 seconds) the rnanipulator

stili applies a Y-force of 6.5 N, because of joint static friction and the no-load back

driving torque of the harmonic dive gears.

3.3.7 Discussion of Resu Us: Force Conîtol

The addition of a bias force can be used to apply a desired normal force on the

environment using the stiffness controller. In order to eliminate the influence of the

Cartesian position errors on the steady state force, the desired stiffness in the direction of

force control can be set to zero. The result would be similar to hybnd force/position

control: providing pure force control in one direction, and stiffness control in other

directions.

In this test, the bias force was applied using a ramp with a duration of 0.5 seconds.

The response of the system shows some initial delay due to the interna1 joint dynamics.

and then catches up to the bias force. There is some overshoot, and then the effect of the

desired stiffness and position errors is imposed. Thus the final steady state force is

slightly Iess than the bias force. The settling time for the force is roughly 1.8 seconds

from the start of the rarnp input. Rather than a ramp change in the bias force. a third

order polynomial, with zero initial and final rate of change, can be used to ensure smooth

changes in the bias force. This approach was implemented in the stiffness control

function by ailowing a new bias force to be specified in the function call.

It is extremely important that the desired stiffness be changed before contacting the

environment. Once external forces are applied to the end effector, any change in desired

stiffness can result in sudden motion of the end effector, because of the delay in the force

measurement.

3.3 SUMMARY

These results demonstrate the successful use of force feedback for the implementation

of stiffness control. The use a proportional integral (PI) regulator of stiffness error does

ensure that the desired Cartesian stiffness is applied between the measured position and

the desired trajectory. However, although the stiffness error is well regulated. the actual

physical stiffness of the manipulator is less than the desired stiffness. This problem could

be minimized by reducing joint cornpliance, or by measunng the link angle directly.

Suitable gains for the stiffness controller were obtained through iterative heuristic

procedures, based on a good understanding of the system. Acceptable gains should not

result in lirnit cycles in the applied force, nor should they excite vibrations in the joint.

The results demonstrate that the stiffness control structure is stable when in contact with

both stiff and rigid environments, for certain gains, under certain operating conditions.

For low desired stiffness it is necessary to compensate for the static joint friction in

order to obtain acceptable trajectory tracking results dunng free motion.

The addition of a bias force allows the stiffness controiler to apply a desired normal

force. UnIess both the environment and the manipulator are very stiff, it is necessary to

lower the desired stiffness in the direction of the bias force, to reduce the influence of

position errors (and the desired stiffness) on the steady state force. However, the desired

stiffness should not be changed after making contact with the environment.

One advantage of this stiffness control approach is that it does not require filtering of

the measured forces. This is in contrast to the inner loop approach, as will be seen in the

next Chapter.

CHAPTER 4 INNER LOOP BASED FORCE CONTROL

"und rver von euch and der Spitze stehen will,

sol1 sich allen unterordnen. " Mattharts 20~27

4.1 THE INNER LOOP BASED CONTROLLER

The inner loop approach is a popular and successful approach for implementing force

control on industrial manipulators. Rather than relying on force feedback, the inner

position servo is used to reject disturbances. Although the inner loop approach has k e n

used to control the impedance of manipulators (refer to Section 1.55). the objective of

this chapter is to demonstrate its use for explicit force control.

The 'inner loop' controller is the position servo of the RIS joints. It consists of a

simple P D position controlfer that tracks the commanded joint trajectories. The desired

trajectory is generated using a Cartesian straight line trajectory generation scheme. and

then the manipulator inverse kinematics are used to generate the joint space trajectories.

The outer force control loop cornputes offsets to the desired trajectory based on the

desired bias force and the measured end effector forces.

Dynamics & filtcr

I 1 L------------

Figure 4.1 Block diagram of inner loop based force control.

For a two degree-of-freedom rnanipulator the x and y components of the Cartesian

position offset increments are given by

fm f mu AX =- , and & = - . A., A.,,

The Cartesian stiffness terms (k,, k,') in (4.1) should be an estimate of the

environment stiffness that includes the cornpliance of the manipulator itself. For this

experiment an arbitrary estimate of the environment stiffness of 1M) N/mm is used.

Using a high estimate of environment stiffness ensures that position offsets change

slowly. The position offsets for discrete sampling instant k is given by

Using the sum of incremental offsets ensures that the position offset changes

srnoothly. One can also add a corresponding increment to the commanded velocity

(Freund et al., 1998). The cornmanded Cartesian trajectory consists of the sum of the

position commanded by the trajectory generator and the position offset from the force

controller.

The measured forces must be filtered to remove noise before being used to generate

position offsets, since rapidly changing position offsets can excite vibrations in the joint.

A second order Butterworth filter with a cutoff frequency of 5 Hz is used to filter the

measured forces (refer to Section 2.2.5).

During free motion the estimated environment stiffness limi ts the maximum veloci ty

contribution frorn the force control loop. With a high stiffness (Kp=IOO Nhrim) the

maximum velocity contribution of the force conuol loop is 50 mm/s for a force error of

A better approach to limiting the maximum velocity contribution of the outer force

ioop during free motion is to impose a limit on AX when forces are below a certain level.

An absolute limit c m be imposed on the maximum value of LLY (based on the

manipulator Jacobian), so that it does not exceed the maximum joint velocities (refer to

Section 2 Table 2.13).

4.1.1 Cattesian Sîtaight Line Trajectory Scheme

In order to simpliEy the interaction of the manipulator with objects located in the

workspace, and to incorporate the Cartesian position offsets in response to measured

forces, a simple Cartesian straight line generation scheme was implemented. This

Cartesian straight line trajectory generation scheme is based on linear functions w i th

parabolic blends (Craig, 1989: 238-40). The pararnetric equations of a line brised on the

start and end points are given by

where s is a function of time,

x,, and y, are the initial Cartesian coordinates,

xf and y~ are the final coordinates.

Ln order to determine the blend time the norrnalized acceleration (acc) of the

parameter s is computed based on the desired linear acceleration (in this case 0.9 d s ' )

from

l i ~ ~ c c acc = ,

2 - a,- +a,.

Then the blend time (r6) is given by

fb =[il-[ ,/T-4.(IEC 2 - ÛCC 1. where tf is the desired duration for the motion.

Thus a point moving dong the line will accelerate frorn time t = O up till time r = t,,

with a constant acceleration of 0.9 m/s2. From time t = tb till time r, - rb the point moves

at constant velocity. Finally the point decelerates to zero velocity at t/. Thus the desired

trajectory at time t is generated by solving for s using equation (4.6) and substituting this

back into equation (4.3) for x and y,

where u(t - tb) is a unit step function at time t-tb, and

~ ( t - 9 +tb) is a unit step function at time t-fj+tb.

4.1.2 In verse Kinematics

The joint angles required to reach a given Cartesian point (assuming this point lies

within the manipulator workspace) are give by the inverse kinematics. For a two

degree-of-freedom manipulator, shown in Figure 4.2, the inverse kinematics are (Craig.

where the positive value is used for sz in equation (4.7b), and

ATAN2 is an arctangent function which uses the sign of numerator and

denorninator to determine the correct quadrant for the angle.

Position offsets from the force control loop (4.2) can simply be added to the desired

Cartesian position (4.3). Then the Cartesian position commands are transformed into

joint space and passed to the individual joint proportional, integral and derivative (PID)

controllers.

Figure 4.2 The 2 degree-of-freedom manipulator.

4.2 EXPERIMENTAL RESULTS

The purpose of this experiment is to demonstrate the use of the inner loop approach to

apply a desired normal force on the environment. The apparatus consists of the sarne

planar manipulator used in the stiffness control experiments, in contact with the same

flexible aluminum pIate. The end effector roller was not used for this experiment. For

the inner loop based pure force controt, the manipulator is rnoved to a point close to the

environment, and commanded to hold this point. Then a bias force of -5 N in the

Y-direction is commanded. Force compensation in the X-direction is disabled during this

experiment, thus forces in the X-direction are rejected by the P D controller. The

rnanipulator moves towards the environment, contacts it, and applies the desired force.

After a couple seconds the desired force is changed to -10 N. Thus we see the 5 N step

response of the outer force control loop while in contact with a stiff environment. Finaily

the desired force is changed back to -5 N.

The time history of the measured variables for Joints 3 and 4 are shown using the

same format as for Figures 3.4 and 3.5. Measured torque for joint 4 was temporarily

unavailable dunng this experiment and is not included in Figure 4.4. Note the difference

between measured and desired position for joint 4 with P D control. The errors are high

during free motion because of static friction (or the harmonic drive no-load starting

torque). Note that the changes in commanded joint angles (qc2) are due entireiy to the

outer force control Ioop, there is no contribution from the trajectory generator for this

experiment.

-5 ' I 1 I I I I O 1 2 3 4 5 6 7 8 9 1 O

time [s]

Figure 4 3 Joint 3 Response - inner loop based force control.

Figure 4.4 Joint 4 Response - inner loop.

The manipulator contacts the metal plate at t=0.6 seconds. The disturbance at t=2.7

seconds is due to the integral terni for joint 4 which suddenIy overcomes static friction.

The bias force is changed frorn -5 to -10 N at t=3.55 seconds, and back down to -5 N at

t=6.03 seconds.

The desired and measured forces in the X and Y directions are shown in Figure 4.5.

The desired X-force is not shown, since force compensation in the X-direction has been

disabled. Measured forces in the X-direction Cfbx) are quite high, since the end effector

roller was not used for this experiment. The filtered Y-force @y) cm be compared to the

unfiltered Y-force shown in Figure 4.9.

5 I I I I I I 1 I I

time [s]

O

2

0 - 5

-2

4 .

Figure 4 J The Desired and Measured Forces - inner loop.

.A--F

fdesy --..

I

K I I 1 1 I I I 1

7-j 1 1 ; 'a.-.. \,-...-."~ - d

-. t t : t I % f--:p,~4.1-->: - !../---- : -

+---- y-. 1 1 1 1 t I 1 f I

O 1 2 3 4 5 6 7 8 9 10

The measured and desired trajectory in the X-Y plane is shown in Figure 4.6. Contact

occurs at roughly (0.39m,O.O3m). The measured motion beyond this point is due to the

cornpliance of the joint, and to the deflection of the metal plate. The desired trajectory

reaches (0.390m,O.O37m) at it's highest point.

There is a l m . Cartesian position error in response to an external force of 10 N. This

is a limitation of the P D joint servo. In this investigation it was assumed that a suitable

position servo was available, and the RIS joint P D servo gains were not adjusted.

Unlike stiffness control, dl position errors should be rejected by the inner position servo.

since the desired trajectory is being adjusted in response to the forces.

meas ured desired

Figure 4.6 Cartesian Position - inner loop.

The Y offsets from the outer force conuol loop are shown in Figure 4.7. The initiaf

offset is quite high since the manipulator was moved brtck by hand while the environment

was secured in the manipulator workspace. The X offsets are al1 zero.

-0.035 I I I I 1 I 1 1 1 I

O 1 2 3 4 5 6 7 8 9 10 time [s]

Figure 4.7 Position Offsets from the Outer Force Controllet.

The effect of filtering is shown in Figure 4.8. The delay between rneasured and

filtered forces is around 0.1 S. The filter used is a second order Buttenvorth filter with a

cut off frequency of 5 Hz (refer to Section 2.2.5).

2 3 4 5 6 7 time [s]

Figure 4.8 Filtered Force in the Y-direction.

Measured forces (transformed to the X-Y Coordinate frarne) are shown in Figure 4.9.

The force in the X-direction is due to a static friction reaction force tangent to the plate.

The desired force is achieved. The response of the system shows some overshoot but the

settling time is less than 1 second.

-

O 1 2 3 4 5 6 7 8 9 10 time [s]

Figure 4.9 End Effector Forces - inner loop.

4.3 SUMMARY

The inner loop approach is able to apply the desired normal force on the metal plate.

even though an arbitrary estimate of the environment stiffness is used. The compliance

of the joints prcvents estimation of the environment stiffness (based on the measured

forces and positions). The PID gains of the joint position servo play an important role in

the performance of this approach. The performance of the inner loop based approach to

force control is lirnited by the performance of the inner position servos as a Cartesian

positioning system. Since the maximum position offsets from the force control loop are

limited, the behavior of the inner loop approach is stable even when out of contact with

the environment. When the desired force is set to zero this controller behaves in a

manner similar to pure darnping control.

In order to ensure smooth Cartesian trajectory offsets, measured forces are filtered.

and the sum of incremental position offsets is used. Srnooth trajectory offsets 'are

essential to avoid exciting interna1 joint vibrations of the IRIS joints. However. the delay

of the filtered forces can lead to problems with lirnit cycles, especially in response to

stick slip friction effects of the contact between the end effector and the environment.

Filtering severely limits the bandwidth of the outer force control loop.

It may be possible to use the inner loop approach not only for controlling forces

explicitly, but also to impose a desired end effector stiffness. This would only be

reasonable if the desired stiffness were much less than the manipulator compliance. and if

the complex amplitude of the desired impedance lies within the bandwidth of the inner

loop based force controller.

CHAPTER 5 CONCLUSIONS & RECOMMENDATIONS

"Das Intum ist vie1 leichter zu erkennen,

als die Wahrheit zu finden" Johann Wolfgang von Goethe ( 1749- 1832)

5.1 CONCLUSIONS

This thesis began by investigating impedance control, and its use for force control.

The difficulty in controlling joint torques for geared manipulators led to the use of force

feedback. Force feedback was used to implement stiffness control, which is the simplest

form of impedance control. The measured forces at the end effector can be used to define

the system error (stiffness or torque error), which is then regulated using a proportional

integral compensator. This approach was implemented using a two degree-of-freedorn

manipulator constructed using two joints from the IRIS modular reconfigurable robotic

faciiity. These joints use harmonic drive gears, whose dynarnic behavior is complicated

and difficult to model. Force feedback is shown (by expenments) to be an effective

approach for implementing stiffness control.

The control scheme succeeds in enforcing the desired stiffness between the measured

end effector position and the measured forces; however, due to the effect of joint

compliance, the desired stiffness is not physicdly redized. Unless the manipulator

Cartesian stiffness is much greater than the desired stiffness, the physical meaning of the

desired stiffness is lost. The effects of joint compliance o n the performance of the

stiffness controller could be eliminated by measuring the joint angles directly (rather than

measuring the motor shaft angle).

Force feedback has advantages over the use of a dynamic model; it is robust to plant

parameter changes and disturbances, and it permits good sensitivity to end effector forces

in spite of the no-load back driving torque of the harmonic drive gear. Even a perfect

dynarnic mode1 would not dlow compensation for this static friction effect.

In order to measure the end effector forces, a wrist mounted forcehorque sensor was

integrated into the IRIS facility. By communicating over the FIT controller parailel port.

force data is transferred at a sampling rate of SOOHz. Thus the force data is acquired at

the sarne rate as the manipulator control cycle.

The use of force feedback for implementing stiffness control is demonstrated

experimentaily. Experimental results are presented for contact with both a stiff and a

rigid environment. The stability of the stiffness controller with force feedback is not

guaranteed. The effects of static friction (in the joints and at the contact point) and sensor

delay c m lead to limit cycles. The stiffness error compensator gains must be selrcted to

avoid limit cycles, and to avoid exciting intemal vibrations within the joint. Mechanical

changes had to be made for joint 3 to reduce mechanical looseness. which aggravates

vibrations. Delay in the measured force signal (due to sensor dynamics. discrete

sarnpling, or low pass filtering) has a negative effect on the stability and bandwidth of the

stiffness controller.

Stiffness control can also be used to apply a desired normal force on the environment.

A simple approach that adds a bias force and sets the desired stiffness to zero (in the

direction of the bias force) is demonstrated. This approach does not require knowledge

of the environment stiffness and is not sensitive to the point of contact or commanded

trajectory.

The inner loop based approach to force control was also implemented for cornparison

with stiffness control. Inner loop based approach has numerous advantages when

implementing force control with industrial manipulators. It relies on the joint position

servo to reject dynamic disturbances and friction forces. unlike stiffness control. which

relies on force feedback to cancel these effects. However, inner loop based control

requires smooth position offsets, and as a result the measured forces were filtered.

Filtering introduces an undesirable delay in the force control loop. The inner loop

approach is a simpler and more intuitive approach than stiffness control. It is also easier

to implement.

5.2 RECOMMENDATIONS

These experimental results only demonstrate the feasibility of two approaches to force

control: stiffness control, and inner loop based force control. Further investigations

could be made to improve performance. In this implementation the singular

configuration had to be avoided, because of the use of the manipulator Jacobian. An

accepted approach to deal with singular configurations could be added to the controller.

to allow the use of the complete manipulator workspace. Both the stiffness controller and

inner loop based controller could be extended to a manipulator with six degrees-of-

freedom.

For inner loop based force control, an industrial rnanipulator may provide better

performance than that which is achieved using the IRIS facility. Both the cornpliance of

the IRIS joints, and the quality of the position servo, limit the use of the IRIS facility for

inner loop based force control. It would be interesting to investigate, using an industrial

manipulator, whether a fuzzy logic PI controller in the outer force control loop has any

advantages over the conventional approach. It would be equally interesting to investigate

the performance of an inner loop based implementation of impedance control on such a

faciiity.

A user friendly programming language that incorporates bias force or desired stiffness

specifications would have to be developed to provide a complete solution to the robot

force control problem. To assess the feasibility of stiffness control or inner loop based

force control, the cost and reliability must be compared against the traditional passive

approaches to force control, such as the Remote Center of Cornpliance (RCC) device.

The IRIS joints are equipped with torque sensors, which were not used for

implernenting stiffness or inner loop based control. It would be interesting to investigate

what advantages or disadvantages there are to using joint torques rather then end effector

forces. The joint torque sensors could also be used to implement an inner joint torque

controller to be used either with computed torque schemes, or for implementing mode1

based impedance control.

REFERENCES

ATI, installation and Operations Manual for FIT: Intelligent nzrtlti-avis forcdtorqur

serzsor system, Manual PN96 10-05- 100 1-08, Assurance Technologies Inc.

Baines, P.J., Mills, J.K., "Feedback Linearized Joint Torque Control of a Geared. DC

Motor Driven Industrial Robot", IEEE Int. Con$ on Robotics and Arrtontcrtion, pp.

3 129-3 136, 1995.

Carignon. C.R., Smith, J. A., "Manipulator irnpedance accurnq itz position-bmed

irnpedance control implernentations", IEEE Int. Conf. on Robotics and Automation.

vol. 2, pp. 1216-1221, 1994.

Craig, J.J., htroduction to Robotics Meclratronics and Control, znd Ed., Addison- Wesle y.

N.Y., 1989.

Emami, M . R., Systematic Methodology of Friz.~- Logic Modelirzg arzd Corltrol crtid

Application to Robotics, Ph.D. Thesis, Department of Mechanical and Industrial

Engineering, University of Toronto, 1997.

Franklin, G.F., Powell, J.D., Workrnan, M.L., Digital control of dyrzcirtric sptertrs.

Addison-Wesley Pub. CO., N. Y ., 1998.

Freund, E., Pesara, J, "High-bandwidth force and irnpedance control for industria1

robots", Robotica, vol. 16, pp. 75-87, 1998.

Goldenberg, A.A., "hplementation of Force and Irnpedance Control in Robot

Manipulators", Proc. 1988 IEEE Int. Con$ on Robotics and A~itornation. pp. 1626-

1632, 1988.

Good, M.C., Sweet, L.M., Strobel, K.L., "Dynamic Models for Control System Design of

Integrated Robot and Drive S ystems", Journal of Dynamic Systems. Merrsurrrtrmt

and Control, vol. 107, pp. 53-58, 1985.

Hashimoto, M., Kiyosawa, Y., Hirabayashi, H., Paul. R.P., "A joint sensing technique for

Robots with hannonic drives", IEEE Int. Con$ on Robotics and A u t o ~ ~ z ~ ~ f i o ~ i . pp.

1034- 1039, 199 1.

Hashimoto, M., Koreyeda, K., Shimono, T., Tanaka, H., Kiyosawa, Y., Hirabayashi. H..

"Experimental study of torque control using harmonic drive built-in torque sensors".

IEEE Int. Con$ on Robotics and Automation, pp. 2026-2030, 1992.

Heinrichs, B., Sepehri, N., "A limitation of position based Nnpedartce coiztrol in stcrtic

force regdation: theory and experiments", IEEE ht. Conf. on Robotics and

Automation, vol. 3, pp. 2 165-2 170, 1999.

Hogan, N., "hpedance Control: An Approach to Manipulation: P m 1 - Theory. Part 1

- Irnpiementation, Part 3 - Application", Journal of rnnarnic Systerrts. Metrs~trei?ir~ir

and Control, vol. 107, pp. 1-24, 1985.

Hogan, N., Rasolee, A., Andary, J., "Impedance control of robots with harmonic drive

s ys tems", Proc. 1991 American Conrrol Con ference, pp. 398-402, 1 99 1 .

Hsia, T.C., Lasky, T.A., Guo, Z.Y., "Robust independent robot joint control: design and

experimentation", lEEE Int. Con$ on Robotics and A~ttontatiorz, pp. 1329- 1334.

1988.

Hui, R., Kircanski, N., Goldenberg, A., Zhou, C., Kuzan, P., Wiercienski, J., Gershon, D..

Sinha, P., "Desigh of the IRIS Facility - A modular, reconfigurable and expandabie

robot test b e d , Proc.1993 IEEE Int. Con$ on Robotics and Automation, 1993.

Kazerooni, H., Shencan , T.B., Houpt, P. K., "Robust cornpliant motion for manipulators.

Part 1 : The fundamental concepts of cornpliant motion, Part 2: Design Method".

IEEE Journal of Robotics and Automation, vol. RA-2. no 2, pp. 83-92. pp.93- 105.

1986.

Keithley, PIO-12 Parallel Digital Interface Board User's Guide, rev. B. no. 2J42J.

Keithley MetraByte Corp., 199 1.

Kircans ki, N .M., IRIS Grusping and Manipulation Facility: perfonncincr eryrilurrtiori.

rnodeling and parameter estimation, RAL interna1 Report. 1993.

Kircanski, N.M., Goldenberg, A.A., "An experimental study of nonlinear stiffness.

hysteresis, and friction effects in robot joints with harmonic drives and torque

sensors", Int. J. of Robotics Researclz, vol. 16, no. 2. pp. 2 11-239, 1997.

Lin, S.-T., Huang, A-K., "Position-based fuzzy force control for dual industrial robots".

Journal of Intelligent and Robotic Systenis: Theory ami Applicatiom. vol. 19. no. 1.

pp.393-409, 1997.

Lin, T., Yae, H.K., "The effects of harmonic drive gears on robot dynarnics". Adva~zces iri

Design Automation ASME, Design Engineering Division, vol. 32. part 2. pp. 5 15-

522, 1991.

Lu, 2.. S liding Mode-Based Impedunce Control and Force Regrrlatiorz . Ph. D. Thes is.

Department of Mechanical Engineering, University of Toronto. 1993.

Mandal, N., Payandeh, S., "Force control strategies for cornpliant and stiff contact: an

expenmental study", Proc. IEEE [nt. Conf on Systems, Mun und Cybernetics, vol. 2.

pp. 1285-1290, 1994.

Nickfalls, R.W.D., Ramasubramanian, R., Interfacing the IBM-PC ro Medicd

Eqriipmertt: The art of serial commurtication, Cambridge University Press.

Cambridge, 1995.

Nye, T.W., Krami, R.P., "Harmonie drive gear error: characterization and compensation

for precision pointing and tracking", Proc. of the 25"' Aerospacr Mecltattistns

Symposium, pp. 237-252, 199 1.

Peletier, M., Doyon, M., "On the implementation and performance of impedance control

on position controlled robots", IEEE Int. Con$ on Robotics artd Aritorrratiort pl-2. pp .

1228-1233, 1994.

Pfeffer, L.E., Khatib, O.. Hake, J., "Joint Torque Sensory Feedback in the Control of a

PUMA Manipulatoi', IEEE Transactions on Robotics and Automation, vol. 5. No. 4.

pp. 4 18-425, 1989.

Phillips, C.L., Harbor, R.D., Feedback Control Systems. 3" Ed., Prentice-Hall. New

Jersey, 1996.

Qian, H.P., Schutter, J.De., "The role of damping and low pass filtering in the stability of

discrete time implemented robot force conuol", Proc. 1992 Int. Cor$ ort Robaric~.

and Automation, pp. 1368- 1373, l992a.

Qian, H.P., Schutter, J.De., "Indroducing active linear and nonlinear damping to enable

stable high gain force control in case of stiff contact", Proc. 1992 Irtr. Colt$ or1

Roborics and Automation, pp. 1374- 1379, 1992b.

Salisbury, I.K., "Active stiffness control of a manipulator in cartesian coordinates". IEEE

1980 Con$ on Decision and Control, vol. 1, pp. 95- 100, 1980.

Salisbury, J.K., Townsend, W.T., "The effect of Coulomb friction and stiction on force

control", IEEE Int. Con$ on Robotics and Automation, pp. 883-889, 1987.

Stepien, T.M., Sweet, L.M., Good, M.C., Tomizuka, M., "Control of TooINorkpiece

Contact Force with Application to Robotic Debumng", IEEE Joumcrl of Robotics

and Automation, vol. RA-3, no. 1, pp. 7-18, 1987.

Surdiiovic, D.T., "Contact Transition Stability in the Impedance Control", froc- IEEE

Int. Con$ on Robotics and Aittornation, pp. 847-852, 1997.

Tarokh, M., Bailey, S., "Adaptive fùuy force control of manipulators with unknown

environment parameten", Journal of Robotic Systems, vol. 14. no. 5. pp 34 1-353.

1997.

Tuttle, T.D., Undestanding and rnodeling the behavior of a Iiarn~onic drive getrr

Oence transmission, M-Sc-Thesis; also Technical Report 1365, MIT Arti ficial InteHi,

Laboratory, 1 992.

Volpe. R-, Khosla, P., "A theoretical and experimental investigation of explici t force

control strategies for manipulaton", IEEE Trans. on Aiiromaric Co~trol. vol. 38.

no. 1 1, pp. 1 634- 1650, 1993.

Wiercienski, J., Employee at Engineering Services Incorporated (ESI) and original

designer of the IRIS joints, reference to conversation at the Robotics and Automation

Laboratory, September 1, 1999.

Yoshikawa, T., Foundations of Robotics: Analysis and Control, MIT Press. 1990.

APPENDIX A: NEWTON EULER DYNAMICS

APPENDIX A: NEWTON-EULER DYNAMICS

Given: a 2 degree-of-freedom (d.0.f.) planar manipulator.

Find: the closed form of the manipulator dynamic equations.

Solution:

using equations 6.45-6.53 Craig (1989: 200)

Outward iterations: i: O + 1

i+l -i+l wP,+, - , R'W, '+t21+l,

i+l ";+, - -i+l - , R ~ ; ~ , + ~ + ~ R ~ ~ , . ~ t $ + ~ i+iï?i+l +ëiTl i+ l&~l ,

i+l - i+i L*,+~ = , ~ ( ' i i , x ' ~ , , ~ +'ir., x(' W , . X ~ ~ + ~ )+'Y, ),

i+l - -i+l,;, Xi+l 'C i-1- 14 p +'+' w;,~ x ( ' + ~ ~ + ~ xi+l P )+'+' CiTi 9

i+l isl FI+l = m;+i vci+l -

r + l -Ci+I ;+i b;. , +i+1 xCi+l 14 N'cl - ' i+t 1-1 "'i+l li+i lCpj+l 7

Inward iterations: i: 2 + 1

ff,.=,+[~l+lfi+i 2~;.

' 1 1 , = ' N , + ~ + [ R ~ + ~ ~ ~ + ~ + i ~ c i x ' ~ , + ' ~ + , x , ~ ~ i ' l f,+, . 1 f ' ^ ri = n, '2,.

This formulation assumes the links can be treated as ngid bodies, and that the center of

mass (COM) and the inertia tensor for each link are known.

For the 2 d.0.f. manipulator the locations of the center of gravity for each link is given by

and the location of the link moving frames are

The rotation matrix from frame 2 into frame 1 is given by

Outward Iterations

Link O

The outward iterations begin from the base and move outwards to the end effector. To

include the effect of gravity an upwards acceleration g is applied to the base frame.

O >% =O ,+, = 0

Link 1

The angular velocity and acceleration for link 1 is simply

The acceleration for the center of mass (COM) of link 1 is

L

the force on the COM is then

the moment about the COM is

Link 2

Repeating the outwards iterations (Al-A5) for link 2 gives

Inwards Iterations

Solving (A6) and (A7) for forces and moments at frame 2 (assuming there are no extemal

forces) gives

' f?='~~,

Link I

Comments:

The symbolic equations (A9) and (A10) can be used to compute the joint torques

corresponding to any motion. The closed form expressions are likely the most

computationally efficient formulation of the dynamics, and are manageable for the simple

two-link manipulator. The dynarnics can be expressed in matrix form, in state space

(equation 6.59, Craig, 1989: 203), configuration space (equation 6.65, Craig. 1989: 206).

or in the Cartesian space (equation 6.98, Craig, 1989: 2 12) forms.

Note that the expression for joint torques do not include the effects of friction forces.

which have a significant effect on the motion of the manipulator.

If the dynarnic equations are used to implement a computed torque scheme. the ability of

the actuators to provide joint torques must be considered. The actuator dynamics for

geared manipulators can play a significant role in the dynarnic behavior of the

manipulator. For example, the flexibility of harmonic drive gears gives rise to

resonances. These affects are difficult to model, and careful experiments are required to

ensure that the model gives a reasonable approximation of the actuai dynarnic behavior of

the manipulator (Good et al., 1985).

References:

Criag, J.J., Introduction to Robotics, Mecliatronics. and Control. znd Ed.. Addison-

Wesley, N.Y., 1989.

Good. MC., Sweet, L.M., Strobel, K.L., "Dynarnic models for control system design of

integrated robot and drive systems", Journal of Dynarnic Systenis. Mecrsurenlrrti atzd

Control, vol. 107, pp. 53-58, 1985.

I l 7

APPENDIX B: IRIS PROGRAM CONNECT STRUCTURE

APPENOIX B: PROGUAM CONNECT STRUCTURE

HOST COMPUTER

1 ptdtsv 1

h l A h l B h l C

rrscrnn mernap mernad

invk-cpp r

h l 01

h l 32 cleanup h l 33 p - part h91, h94 disabled due to changes in h9

getpartc h l 31

[hl 34 p - getsererr 1

hl01

h l 3 convps

makepat h

\

h111-7

APPENDIX B: PROGRAM CONNECT STRUCTURE

763 sys-defauit-paramts 767 show-long-to-window 765 show~unsiqned~to~window

intial-menu

76C ass-data

76F setcursor 76G stringinput +

h6 l stringfofloat 76J string-to-unsiqned 16K read-arm-paramts 16L sting-to-long 96M clreol 16N sysinit h2F,hZt,h2E,h2M, h2C,h2H,hZN

dacvscm

h37

- - m 1 h31 1 ptscm h32 1 CU rsor386

h7

h3B h3C

dacscrrnsg tickscm

h3D h3E

ghscm timerror

APPENDIX 6: PROGRAM CONNECT STRUCTURE

czwrite czmake czdrbox caitle

h7B h7C

czscf b czschwr

h7E h7F

rernote

h7D czvideo scrÏnd dosfn A

h7G biosf nd k

A2, h 1 23

L

h-ctrl.c h2- h21

h29 h2A h2B

contrl

%=pute joint-velocities scale-outputs write-actuators

APPENDIX 8: PROGRAM CONNECT STRUCTURE

I h l l l 1 initgraph 1

h-r-1oad.c h8l h82

\h l 14 1 getmaxy 1

h81 yarcinit setuplPCBlock

hl 12 h l 13

outte setcolor

h68

ciosegrap h getrnaxx

[h l19 1 line 1

h42 1 send-dac h43 1 kbc hec k

1 stbreak 1

h7F h-keybrd.~ h41

1h45 1 send-par 1

h4 ptkeyb

h46 test-quit h47 save-point b

APPENDIX B: PROGRAM CONNECT STRUCTURE

RlSC COMPUTER

. r73 1 move

r-main-c r 1 main

misin eter r58

r74 r75 r76

l r7C Imove stem I

sinep sinef move-han

acobian 1

'acobian2 inverse

r53 6 4 r55

6 2 r33 r34

ram

para torque DH1

r5E im rSF line i

- -

tac 1 mu~tipiy I

APPENDIX 8: PROGRAM CONNECT STRUCTURE

RlSC COMPUTER

1 r49 1 end-draw 1

r4 1 r42

transfer start r31 ,r7A

APPENDIX C: SAMPLE OF DATA FILE