force control for robotic manipulators with - circle

FORCE CONTROL FOR ROBOTIC MANIPULATORS

WITH STRUCTURALLY FLEXIBLE LINKS

By

Douglas John Latornell

B.Sc. ions. (Mechanical Engineering) Queen’s University

M.Sc. (Mechanical Engineering) Queen’s University

A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF

THE REQUIREMENTS FOR THE DEGREE OF

DOCTOR OF PHILOSOPHY

in

THE FACULTY OF GRADUATE STUDIES

DEPARTMENT OF MECHANICAL ENGINEERING

We accept this thesis as conforming

to the required standard

THE UNIVERSITY OF BRITISH COLUMBIA

1992

© Douglas John Latornell, 14 August 1992

In presenting this thesis in partial fulfilment of the requirements for an advanced

degree at the University of British Columbia, I agree that the Library shall make it

freely available for reference and study. I further agree that permission for extensive

copying of this thesis for scholarly purposes may be granted by the head of my

department or by his or her representatives. It is understood that copying or

publication of this thesis for financial gain shall not be allowed without my written

permission.

________________________________

Department of / iç7

The University of British ColumbiaVancouver, Canada

Date aki /92

DE-6 (2/88)

Abstract

This thesis reports on the development of strategies for the control of contact forces exerted

by a structurally flexible robotic manipulator on surfaces in its working environment. The

controller is based on a multivariable, explicitly adaptive, long range predictive control

algorithm. A static equilibrium bias term which is particularly applicable to the contact

force control problem has been incorporated into the control algorithm cost function. A

general formulation for the discrete time domain characteristic polynomial of the closed

loop system has been derived and shown useful in tuning the controller.

Kinematic and dynamic models of a robotic manipulator with structurally flexible links

interacting with its working environment are derived. These models include inertia and

damping effects in the contact dynamics in addition to the contact stiffness employed in

most previous work. Linear analyses of the dynamic models for a variety of manipulator

configurations reveal that the controlled variable, the contact force, is dominated by different

open ioop modes of the system depending on the effective stiffness of the contacting surfaces.

This result has important implications for the selection of the controller parameters.

The performance of the controller has been evaluated using computer simulation. A

special purpose simulation program, TWOFLEX, which includes the dynamics models of the

manipulator and the environment as well as the control algorithm was developed during

the research. The configurations investigated using the simulation include a single flexible

manipulator link, two link manipulators with both rigid and flexible links, and a two link

prototype model of the Mobile Servicing System (MS S) manipulator for the proposed Space

Station, Freedom. The results show that the controller can be tuned to provide fast contact

force step responses with minimal overshoot and zero steady-state error. The problem of

11

maintaining control through the discontinuous situation of unexpectedly making contact

with a surface is addressed with the introduction of a contact control logic level in the

control hierarchy.

111

Abstract

List of Tables

Table of Contents

11

vi”

List of Figures

Nomenclature

Acknowledgements

1 Introduction

1.1 Introduction

1.2 Motivation

1.3 Survey of Related Research

1.4 Contributions of the Thesis

1.5 Outline of the Thesis . .

x

xvi

xxv

2 Manipulator Modelling

2.1 Introduction

2.2 Literature Review

2.3 Kinematics

2.3.1 Manipulator Configuration

2.3.2 Flexible Link Model .

2.3.3 Contact Model

14

14

15

19

19

25

29

1

1

2

3

6

9

Model .

iv

2.4 Derivation of Equations of Motion 32

2.5 Contact Force Model 48

2.5.1 Hertz Contact Model 52

2.6 Voltage Controlled DC Electric Motor Actuator Model 54

2.7 Summary 58

3 Control Algorithm 59

3.1 Introduction 59

3.2 Review of SISO GPC Algorithm 62

3.2.1 Control Law Structure and Closed Loop Characteristics 66

3.3 Multivariable Predictive Control Algorithm 69

3.3.1 Control Law Structure and Closed Loop Characteristics 76

3.4 Static Equilibrium Bias Term 79

3.5 Control Algorithm Parameters and Tuning 83

3.6 Parameter Estimation 86

3.7 Adaptive Controller Implementation 90

3.8 Stability and Convergence 92

3.9 Summary 93

4 Dynamics and Control Simulation — TWOFLEX 95

4.1 Introduction 95

4.2 General Description of the TWOFLEX Program 96

4.3 Integration of Equations of Motion 99

4.4 Discrete Time Control Algorithm 102

4.5 Validation 106

4.6 Summary 109

v

111

• • • . 111

• • • . 114

• • 114

• • • . 121

• • • • 135

• 135

• • 136

• • • . 137

• • 141

• • • . 151

• . . • 160

• . . . 163

• . . . 166

• • . . 169

• . . . 174

• . . . 175

• . • . 197

• . . . 199

202

• . . . 217

• . . . 218

• . . . 224

6 Summary of Major Results

6.1 Recommendations for Further Research

5 Force Control Algorithm Performance and Evaluation

5.1 Introduction

5.2 Linear Analysis

5.2.1 Rigid Link

5.2.2 Flexible Link

5.3 Force Control for a Single Flexible Link

5.3.1 Link Model Parameters

5.3.2 Controller Model

5.3.3 Soft Contact Case (keff 3.lkN/m)

5.3.4 Hertz Contact Model Case (kff 5MN/m)

5.3.5 keff = 62kN/m Contact Stiffness Case .

5.3.6 Summary

5.4 Making and Breaking Contact

5.4.1 Stop Motion Strategy

5.4.2 Force Setpoint Modification Strategy .

5.5 Force Control for Two Link Manipulators

5.5.1 Linear Analysis


5.5.3 Force Control for Two Rigid Links . .

5.5.4 Force Control for Two Flexible Links . .

5.5.5 Summary

5.6 Force Control for DC Motor Actuated Space Station Manipulator

5.7 Summary of Force Control Performance

228

231

vi

Bibliography

Appendices

235

242

A Equations of Motion for Various Manipulator Configurations

A.1 Planar Two Link Flexible Manipulator in Free Motion

A.2 Planar Two Rigid Link Manipulator in Contact

A.3 Planar Two Link Rigid Manipulator in Free Motion

A.4 One Flexible Link in Contact

A.5 One Flexible Link in Free Motion

A.6 One Rigid Link in Contact

A.7 DC Motor Actuated Single Flexible Link in Free Motion

242

242

245

247

248

249

249

250

B Cost Function Expansions

B.1 Multivariable Predictive Control Algorithm

B.2 Static Equilibrium Bias Term

C Static Equilibrium Function Derivation 253

251

• 251

• 252

vii

List of Tables

4.1 Configurations and initial conditions for validation of inertia matrix and right

hand side vector terms 107

4.2 Configurations and initial conditions for free response simulation validation

runs 107

5.1 Mode shape function integral factor values for a single flexible link with one

structural mode 123

5.2 Natural frequencies and mode shape function integral factor values for single

flexible link run 136

5.3 Material properties and Hertz model parameters for steel on aluminium contact 142

5.4 Stabilising controller horizons and closed ioop characteristics for steel on

aluminium contact (keff = 5.0037MN/m) for a single flexible link with one

structural mode 146


aluminium contact (keff = 61.554kN/m) with a single flexible link with one

structural mode 153


aluminium contact (keff = 61.554kN/m) with a single flexible link with three

structural modes 160

5.7 Parameter values for two rigid link manipulator controller model 200

5.8 Stabilising controller horizons and closed ioop characteristics for a struc

turally rigid two link manipulator contacting a keff = kN/m surface 202

viii

5.9 Parameter values for two flexible link manipulator controller model 204

5.10 Stabilising controller horizons and closed ioop characteristics for a two flexible

link (one structural mode each) manipulator contacting a keff = lOkN/m

surface 208

5.11 Physical parameter values for a two link MSS model with DC motor actuators.219

ix

List of Figures

2.1 Geometry of a two link flexible manipulator in contact with a deformable

surface 19

2.2 Coordinate frames for a two link flexible manipulator in contact with a de

formable surface 21

2.3 Free body diagram of a differential beam element 26

2.4 Axial foreshortening of a differential beam element 29

2.5 Manipulator—environment model for derivation of contact force expressions 49

2.6 Free body diagrams for the x direction contact region mass 50

2.7 Schematic of direct current motor actuator system 55

3.1 SISO GPC closed loop system block diagram 68

3.2 Comparison of EFRA and U/D RLS parameter estimation algorithms. . 89

3.3 Control strategy block diagram 91

4.1 Flowchart of the overall structure of the TWOFLEX dynamics and control sim

ulation program 97

4.2 Flowchart of the calculation of the generalised acceleration vector 102

4.3 Flowchart of the discrete time control algorithm 103

4.4 Comparison of joint angle responses for free response validation 108

4.5 Comparison of modal amplifier responses for free response validation 110

5.1 Topics discussed in Chapter 5 112

5.2 Single rigid link in contact with a deformable environment 114

x

5.3 Variation of the natural frequencies with effective contact stiffness for a single

rigid link/environment system 117

5.4 Single flexible link in contact with a deformable environment 121

5.5 Variation of the natural frequencies with effective contact stiffness for a single

flexible link with one structural mode 124

5.6 Single flexible link in contact with a deformable environment approximated

by a steady state contact dynamics model 125

5.7 Variation of the natural mode components of the contact force with effective

contact stiffness for a single flexible link with one structural mode 127

5.8 Variation with effective contact stiffness of the relative excitation of the nat

ural modes of a single flexible link with one structural mode in contact. . . 129

5.9 Variation with effective contact stiffness of system natural frequencies for a

soft link (El1 = 40N . m2) 130

5.10 Variation with effective contact stiffness of system natural frequencies for a

flexible link with four structural modes 132

5.11 Variation of the natural mode components of the contact force with effec

tive contact stiffness for a single flexible link (El1 = 200N . m2) with four

structural modes 133

5.12 Variation with effective contact stiffness of the relative excitation of the nat

ural modes of a single flexible link (El1 200N . m) with four structural

modes 134

5.13 Variation of natural frequencies of a single flexible link (El1 = 574N m2,

two structural modes) in contact 139

5.14 Contact force response of a single flexible link on a soft surface (k5 = ke =

6200N/m) 140

xi

5.15 Variation of natural frequencies of a single flexible link (with one structural

mode) in contact 145

5.16 Comparison of results from simulations using full contact dynamics and

steady state approximation 147

5.17 Force control step response for a single flexible link (with one structural

mode), steel on aluminium contact (keff = 5MN/m) with N2 = 20, N = 2,

= 0, and Age = 0 148

5.18 Force control step responses for a single flexible link (with one structural

mode), steel on aluminium contact (keff = 5MN/m) with 0 Age 0.4. . . 150

5.19 Experimental contact stiffness estimate for steel on aluminium 152

5.20 Force control step response for a single mode flexible link (with one structural

mode), steel on aluminium contact (kff = 61.554kN/m) with N2 = 10,

N=3,A=0,A8=0 154

5.21 Force control step response for a single mode flexible link (with one structural

mode), steel on aluminium contact (kff = 61.554kN/m) with N2 = 5, N.

4, A = 0, A = 0 155


mode), steel on aluminium contact (keff = 61.554kN/m) with N2 = 5, N

3, A = 0, Age 0 156


mode), steel on aluminium contact (keff = 61.554kN/m) with N2 = 7, N =

4,Ac0,A8e0 158

5.24 Variation of the natural frequencies of a single flexible link (El1 = 574N m2

with three structural modes) in contact 159

xii

5.25 Force control step response for single flexible link (with three structural

modes), steel on aluminium contact (keff = 61.554kN/m), with N2 = 8,

N, = 4, = 0, )ise = 0 161

5.26 Block diagram of the overall control structure 165

5.27 Joint angle and contact force responses for a single flexible link colliding with

a 6200N/m surface 167

5.28 Simple 1 degree of freedom model of a single link in contact with an environ

ment 170

5.29 Contact force response for a single flexible link making contact with a 6200N/m

surface at 0.536m/s 172

5.30 Contact force response of a single flexible link colliding with a 6200N/m

surface with force setpoint modification 173

5.31 Two rigid link manipulator in contact with a deformable environment . . . 176

5.32 Variation of the natural frequencies with effective contact stiffness for a two

rigid link manipulator in contact 178

5.33 and 2 mode shapes for various values of for a two rigid link manipulator.183

5.34 Variation of contact force components in natural coordinate space with O

with keff = 61.554kN/m 185

5.35 Relative excitation of the natural modes of a two link rigid manipulator by

the control inputs 187

5.36 Variation of the natural frequencies with effective contact stiffness of a two

link manipulator with identical, flexible (El = 574N m2, one structural

mode each) links in contact with an isotropic (kxeff kyeff) environment. . . 191

5.37 Variation of the natural frequencies with effective contact stiffness for a two

flexible link manipulator (El = 574N m2, one structural mode each) with

= 1.Om and L2 = 2.Om 193

xlii

5.38 Variation of the natural mode components of the contact forces with effective

contact stiffness for a two flexible link manipulator 195

5.39 Variation of the relative excitation of the natural modes with effective contact

stiffness for a two flexible link manipulator 196

5.40 Force control step responses for a structurally rigid two link manipulator

contacting a keff = lOkN/m surface with N2 = 5, Nu = 2, c = 0 203

5.41 Variation of the natural frequencies for a two link manipulator with identical

flexible links assuming steady state contact dynamics 206

5.42 Force control step responses for linear dynamics simulation of a two flexible


surface with N2 20, N = 2, 0 209

5.43 Force control step responses for linear dynamics simulation of a two flexible


surfacewithN2=8,N=4,)=0 211

5.44 Force control step responses showing output interactions for linear dynamics

simulation of a two flexible link (one structural mode each) manipulator

contacting a keff = lOkN/m surface with N2 = 20, N = 2, 0 212

5.45 Force control step responses showing output interactions for linear dynamics

simulation of a two flexible link (one structural mode each) manipulator

contacting a keff = lOkN/m surface with N2 = 8, N = 4, = 0 213

5.46 Force control step responses for nonlinear dynamics simulation of a two flexi

ble link (one structural mode each) manipulator contacting a keff = lOkN/m

surface with N2 = 20, N = 2, , = 0 215

5.47 Variation of the modal natural frequencies for DC motor actuated two link

MSS model 220

xiv

5.48 Force control responses for linear dynamics simulation of a two flexible link

Space Station MSS contacting a keff = 2OkN/m surface 222

5.49 Force control responses for nonlinear dynamics simulation of a two flexible

link Space Station MSS contacting a keff = 2OkN/m surface 223

xv

Nomenclature

Roman letters

A(q1) discrete time input/output model denominator polynomial of degree ma

a radius of circular contact area in the Hertz contact model

B(q’) discrete time input/output model numerator polynomial of degree mb

Bk(q’) discrete time input/output model numerator polynomial relating kth input toith output in a discrete time multivariable system model

ba equivalent viscous damping coefficient for DC motor armature frictional damping

bex equivalent viscous damping factor for contact damping of the environmentsurface in the x direction

bey equivalent viscous damping factor for contact damping of the environmentsurface in the y direction

bk, equivalent viscous damping factor for structural damping of mode i of link k

b3 equivalent viscous damping factor for contact damping of the manipulator tipsurface in the x direction

b3 equivalent viscous damping factor for contact damping of the manipulator tipsurface in the y direction

bek equivalent viscous damping factor for friction in joint k

c, vector of generalised coordinate components for linearised model of F

c, vector of generalised coordinate components for linearised model of F

c1 cosine of joint angle 1 (Ox)

c2 cosine of joint angle 2 (&2)

ca2 cosine of [w’10(t) + 2(t)]

Clai2 cosine of [61(t) + w0(t) + 2(t)]

xvi

D,(t) homogeneous transformation matrix to describe the structural deformation oflink 1 relative to coordinate frame

D2(t) homogeneous transformation matrix to describe the structural deformation oflink 2 relative to coordinate frame F2

D inverse of effective planar strain modulus in the Hertz contact model

d(t) disturbance term for ith output in a multivariable system model

dm, an element of mass on link 1

dm2 an element of mass on link 2

E matrix whose columns are the eigenvectors of a linear system

E(q’) a backward shift polynomial which satisfies the Diophantine identity

Ek modulus of elasticity of the material of surface k in the Hertz contact model

Elk fiexural rigidity of link k in bending about its joint axis

ê vector of optimal predictions of future output errors for the ith output of amultivariable system

ek eigenvector corresponding to the kth natural frequency of a linear system

F applied force in the Hertz contact model

F3(q’) a backward shift polynomial which satisfies the Diophantine identity

Fk peak contact force during collision transient

a direction component of contact force, relative to and projected onto F°

F y direction component of contact force, relative to and projected onto F°

F° fixed world coordinate frame with mutually perpendicular unit direction vectors; i, i and i

F’ coordinate frame for link 1 with mutually perpendicular unit direction vectors;i, i and i

F2 coordinate frame for link 2 with mutually perpendicular unit direction vectors;i, i and i

FC coordinate frame at the location of initial contact with mutually perpendicularunit direction vectors; i, i and i

xvii

f vector of q’ polynomial operators

f vector of free response components of the ith predicted output from time stept + 1 to t + N2; subscript is dropped in SISO case

f(t + j) free response part of (t + jt)

f natural frequency in Hz

fd damped natural frequency in Hz

G’ N2 x N2 matrix of G polynomial coefficients

G2k N2 x N matrix of G:jk polynomial coefficients obtained by truncating the leftmost N2

— N columns from Gk; subscripts dropped in SISO case

C, polynomial product of E(q1)and B(q’)

g gravity field vector

g1 vector of q’ polynomial operators; subscripts dropped in SISO case

H(s) n0 x n, matrix of transfer functions in the Laplace domain

Ho,,(t) homogeneous transformation matrix between coordinate frames F1 and F°

H0,2(t) homogeneous transformation matrix between coordinate frames F2 and F°

H1,2(t) homogeneous transformation matrix from coordinate frame F2 to frame F’

H’,2(t) rigid body component of coordinate transformation from frame F2 to frameF’

H2,(t) homogeneous transformation matrix between frames FC and F2

H2k(s) Laplace domain transfer function relating the kth input to the ith output; theik element of matrix H(s)

I identity matrix

‘ok rigid body component of the moment of inertia of link k about joint axis k

7-a DC motor armature inertia

‘mki mth inertia integral for the ith admissible mode shape function of link k

i mode index for link 1 in dynamics model;output index for multivariable system model

xviii

a DC motor armature current

J(s) Laplace transformed matrix of left hand side terms in a system of linearisedequations of motion

j ij minor of matrix J(s)

J1 quadratic cost function for SISO CPC algorithm

J2 cost function for multivariable predictive control algorithm

J3 overall cost function for multivariable predictive control algorithm

Jae static equilibrium bias cost function

j mode index for link 2 in dynamics model;time step index in predictive control algorithm

Kepj DC motor back emf coefficient

KT DC motor torque coefficient

k link index in dynamics model;input index for multivariable system model;surface index for Hertz contact model

kff effective contact stiffness

ke stiffness of the environment surface in the x direction

key stiffness of the environment surface in the y direction

k stiffness of the manipulator tip surface in the z direction

k3 stiffness of the manipulator tip surface in the y direction

L a unit lower triangular matrix

L1 length of link 1

L2 length of link 2

La DC motor armature inductance

£ Lagrangian; £ = T — V

Mk total mass of link k

xix

m total contact mass; sum of m3 and me

me mass of contact region of environment

m3 mass of contact region of manipulator tip

N2 prediction horizon in predictive control algorithm

Ng actuator gear ratio

N control horizon in predictive control algorithm

n2 number of inputs in multivariable system model

k number of admissible functions included in the structural model of link k

n0 number of outputs in multivariable system model

ma degree of discrete time input/output model denominator polynomial

nb degree of discrete time input/output model numerator polynomial

P covariance matrix in a recursive least squares (RLS) parameter estimationalgorithm

Q a generalised force

q vector of q’ polynomial operators

q vector of q’ polynomial operators

q a generalised coordinate

q’ backward shift operator; q’x(t) — 1)

R effective radius of curvature of the surfaces in the Hertz contact model

Ra DC motor armature resistance

Rk radius of curvature of surface k in the Hertz contact model

? total energy dissipation function

r(t) position vector, relative to and projected onto F°, for an element of mass onlink 1

r(t) position vector, relative to and projected onto F°, for an element of mass onlink 2

xx

r(t) position vector, relative to and projected onto F°, for the total contact mass

r(t) position vector, relative to and projected onto .F’, for an element of mass onlink 1

r(t) position vector, relative to and projected onto F2, for an element of mass onlink 2

r(t) position vector, relative to and projected onto FC, for the total contact mass

s the Laplace operator

s1 sine of joint angle 1 (6)

2 sine of joint angle 2 (82)

sine of [w’10(t) + 82(t)]

1a2 sine of [&1(t) + w() + 82(t)]

T total kinetic energy

time

U(s) n x 1 vector of Laplace transformed inputs to a multivariable system

Uk(s) Laplace transform of kth input in a multivariable system model

Uk vector of kth control inputs for N2 time steps into the future

11k vector of static equilibrium values for kth input for N2 time steps into thefuture

Uk vector of increments for kth input from time step t to t + N2 — 1; subscript isdropped in SISO case

Uk(t) time series of values of the kth input in a discrete time multivariable systemmodel; subscript dropped in SISO case

Luk(t) increment for kth control input at time step t; subscript dropped in SISO case

u value of the kth control input at the current time step

Vk volume of material moved due to the deflection of surface k in the Hertz contactmodel

V total potential energy

xxi

v(t) velocity vector, relative to and projected onto F°, for an element of mass onlink 1

v(t) velocity vector, relative to and projected onto F°, for an element of mass onlink 2

Va DC motor armature voltage

vb DC motor back emf voltage

w2 vector of future setpoint levels for ith output from time step t + 1 to t + N2;subscript dropped in SISO case

wk(xk, t) displacement of the actual centreline from the nominal centreline of the undeformed link k

wk0 total deflection of the tip of link k due to bending

xk distance measured along the nominal centreline of link k

x direction coordinate of the point of initial contact, relative to and projectedonto F°

x direction component of manipulator tip position, relative to and projectedonto F°

x direction component of manipulator tip velocity, relative to and projectedonto F°

Y(s) n0 x 1 vector of Laplace transformed outputs of a multivariable system

Y(s) Laplace transform of ith input in a multivariable system model

5r vector of optimal predictions for ith output from time step t + 1 to t + N2;subscript dropped for SISO case

y2(t) time series of values of the ith output in a discrete time multivariable systemmodel; subscript dropped in SISO case

(t + j t) optimal prediction of y(t -f- j) given the information available at time step t;subscript dropped in SISO case

Yw y direction coordinate of the point of initial contact, relative to and projectedonto F°

Ytip y direction component of manipulator tip position, relative to and projectedonto F°

xxii

thj y direction component of manipulator tip velocity, relative to and projectedonto F°

Greek letters

a1 local, instantaneous slope of link 1 at its tip; a1 w’10

a2 local, instantaneous slope of link 2 at its tip; a2 w0 =

ae parameter estimation gain adjustment constant in the exponential forgettingand resetting algorithm (EFRA)

/3e constant directly related to the minimum eigenvalue of the covariance matrixin the exponential forgetting and resetting algorithm (EFRA)

I3ki solution of the cantilever beam frequency equation for the ith mode of link k

A differencing operator; 1—

S total deformation of contacting surfaces in Hertz contact mode

8e constant inversely related to the maximum eigenvalue of the covariance matrixin the exponential forgetting and resetting algorithm (EFRA)

5k deflection of surface k in the Hertz contact model

e x direction generalised coordinate for contact dynamics model

e y direction generalised coordinate for contact dynamics model

damping ratio

71k kth natural coordinate of a system

‘9 vector of parameter estimates in a recursive least squares (RLS) parameterestimation algorithm

8 joint angle for link 1

82 joint angle for link 2

8 nominal value of joint angle 1

O nominal value of joint angle 2

8a DC motor armature speed

8 joint angle rate at the instant of contact

xxiii

X predictive controller input increment weighting factor

exponential forgetting factor constant in the exponential forgetting and resetting algorithm (EFRA)

‘se static equilibrium bias term weighting factor in predictive control algorithm

a unit vector

11k Poisson’s ratio of the material of surface k in the Hertz contact model

axial foreshortening of a beam due to bending

d axial foreshortening of a differential beam element due to bending

(t) time series of uncorrelated random noise values

pk mass per unit length of link k

r1 torque applied to link 1 by the actuator at joint 1

r2 torque applied to link 2 by the actuator at joint 2

-rL load torque applied to the DC motor

Tm DC motor armature torque

c/k regressor vector of past input and output data values for recursive least squares(RLS) parameter estimation algorithm

qfi ith admissible mode shape function for link 1

q52j jth admissible mode shape function for link 2

q5 slope of ith admissible mode shape function for link 1; dqi/dxi

c4 slope of jth admissible mode shape function for link 2; dq23/dx2

ith modal amplifier for link 1

/2j jth modal amplifier for link 2

bli first time derivative of ith modal amplifier for link 1; d12/dt

1’2j first time derivative jth modal amplifier for link 2; di/’1/dt

b1 second time derivative of ith modal amplifier for link 1;d2’e/..’11/dt2

b2 second time derivative jth modal amplifier for link 2;d2b11/dt2

w natural frequency in rad/s

xxiv

Acknowledgements

I wish to thank my research supervisor, Dr. Dale Cherchas, for his many contributions to

the research presented in this thesis. From the initial stages of selecting the topic through

the final proofreading he was always available with a ready ear and thoughtful suggestions.

The input of the supervisor committee, Dr. Peter Lawrence, Dr. Vinod Modi and

Dr. Stan Hutton, during the early stages of the research and their comments which helped

to improve the finished thesis are gratefully acknowledged. I also wish to express my ap

preciation to Dr. Ray Gosine, Dr. Chris Ma and Dr. Clarence de Silva for their helpful

comments during and following my oral examination and to Dr. Andrew Goldenberg of the

University of Toronto, the external examiner, for his careful reading of my thesis and his

insightful comments.

Funding for the project was provided by the Natural Sciences and Engineering Research

Council (NSERC) in the form of a Post Graduate Scholarship granted to the author and an

Operating Grant (OP 00004682) to Dale Cherchas. The British Columbia Advanced System

Institute (ASI) also provided funds for scholarships and research equipment. The University

of British Columbia Department of Mechanical Engineering supported the author through

teaching assistantships.

The work described in this thesis relied heavily on computing facilities and resources.

Without the assistance and cooperation of Alan Steeves and Gerry Rohling of the Depart

ment of Mechanical Engineering and Tom Nicol of University Computing Services many

aspects of the research and the thesis preparation would have been much more time con

suming, if not impossible.

xxv

The experience of doing doctoral research and writing this thesis would have been very

much more trying were it not for many people not directly involved with the work. I

thank my parents for their ongoing support and encouragement of all of my endeavours and

my brother for his occasional, and usually humorous, reminders of the wide and different

world out there. I also thank my friends in the department, in particular, Alan Spence,

Sam and Anat Kotzev, Harry Mah and Afzal Suleman, for many fruitful discussions and

commiserations.

The music of Johannes Brahms, Franz Liszt and George Thorogood helped to smooth

some of the rough spots in the writing of the thesis and CBC Stereo provided almost

constant accompaniment.

Finally, I express my deepest thanks to my wife, Susan. She has always been there to

listen, especially when no one else would or could, and she has endured the worst of me over

the past few years. Through all of this she has been a constant source of encouragement.

xxvi

Chapter 1

Introduction

1.1 Introduction

This thesis discusses research centred on the development of strategies for the control of

the contact forces exerted by a structurally flexible robotic manipulator on surfaces in

its working environment. The ability to sense and control the forces exerted by a robotic

manipulator on its environment has, for some time, been recognised as a necessary and ben

eficial extension of robotic system capabilities. Force sensing and control offers improved

performance of tasks which involve contact of the end—effector with the environment by

providing robustness to variations in location and shape of the objects being manipulated.

One example of such improved performance is the detection and alleviation of jamming

parts during insertion tasks in assembly operations. In addition, force control makes pos

sible tasks which are extremely difficult, if not impossible, under purely positional control

such as deburring of stamped metal parts, grinding of welded joints and polishing oper

ations on objects with complex surface shapes. Structural flexibility in the manipulator

links presents particular challenges in the context of force control because of the complex

dynamics which are introduced between the actuators at the manipulator joints and the

force sensor at the manipulator wrist or tip. These challenges have been met in this study

with the development of a multivariable predictive control algorithm. The algorithm can be

readily implemented as an explicit adaptive controller which provides the controller with the

ability to compensate for the time varying, nonlinear components of the system dynamics.

1

Chapter 1. Introduction 2

1.2 Motivation

This research was motivated both by the academic challenges of understanding and con

trolling a complex dynamic system and by the need for high performance force control

capabilities in robotic manipulation. A particular application which has provided inspira

tion and direction for this work is the identified need [1] for contact force control in the

development of autonomous and telerobotic manipulator systems for the proposed Space

Station, Freedom.

In academic terms, the problem is of interest because the dynamics of even a rigid

manipulator with several links in free motion are strongly coupled and highly nonlinear.

The addition of structural dynamics in the links and interaction between the manipulator

tip and a working surface increases the number of coupled degrees of freedom in the system

and introduces time varying components into the dynamics. Specifying the modulation of

the contact forces as the control objective and using the torques generated by actuators

at the manipulator joints as the inputs to achieve this objective poses a difficult control

problem because of presence of complex dynamics between the actuators and the sensor.

Furthermore, the control problem is inherently discontinuous because the contact force is a

positive indefinite function of the system state, that is, if the manipulator is not in contact,

or loses contact, with the environment the contact force is, by definition, zero and therefore

provides no information about the system state. Thus a force control algorithm which

provides stable, acceptably shaped closed ioop responses must be augmented with a control

strategy to provide smooth, stable, predictable performance in the absence of contact and

during the transitions of making and breaking contact.

The importance of compliance, associated with either the manipulator or the environ

ment, in achieving stable force control with existing industrial robots and controllers is well


established [2]. Manipulators with structurally flexible links or flexible joints naturally pro

vide compliance which is distributed over the entire manipulator structure. This distributed

compliance is helpful in achieving high performance force control by absorbing the impulse

which occurs when contact is made. Large initial contact forces are currently avoided in

most robotic force control operations by approaching the environment at very low speed.

Clearly, to take advantage of this shock absorption ability, the compliant nature of the links

must be included in the controller design.

Space based manipulators with structurally flexible links will, in the near future, be

required to operate under conditions where force control is essential. The Space Shuttle Re

mote Manipulator System (SSRMS or Canadarm) and the Mobile Servicing System (MSS)

and Special Purpose Dextrous Manipulator (SPDM) are all examples of this type of manip

ulator. Among the tasks which have been proposed for these manipulators are the assembly

of the structural elements of the Space Station Freedom, maintenance and cleaning activ

ities on the exterior of the station, removing and installing thermal protection coverings,

and servicing of other spacecraft. All of these tasks clearly require control of contact forces

in the presence of significant structural flexibility of the manipulator. As the capabilities of

these space based systems are proven, new, light-weight earth based manipulators designed

for high speed motion can also be expected to exhibit structural flexibility which will need

to be accounted for in the design of both force and motion controllers.

1.3 Survey of Related Research

The control of the contact forces exerted by robotic manipulators on their working envi

ronments has been an active field of research since the 1970’s. Among the earliest reports

in the literature on this subject is the work of Whitney [3]. Though terminology has, in


some cases, changed, most of the issues identified by Whitney remain important in cur

rent research. The distinction made by Whitney between gross motions (large movements

of the end-effector from one point to another in the workspace) and fine motions (small

motions during which the possibility or certainty of contact with the environment exists)

reflects the fundamentally discontinuous nature of the overall force and motion control of

manipulators. A particular problem which arises from this discontinuity is that of unex

pected collisions between the end-effector and the environment. In the course of analysing

a linear force feedback control law Whitney highlights the issue of force controller stability

and reveals the intimate relationship between controller performance and compliance in the

manipulator/sensor/environment system.

The problem of simultaneously combining force and motion control was a focus of early

research. The methodologies for solving this problem generally focus on the use of feedback

to associate a particular physical characteristic (e.g., stiffness [4], damping [3], impedance

[5—7]) with the movements of the manipulator in each direction of an appropriately selected

coordinate frame. Fundamental to all of these methods are the needs to select a coordinate

frame appropriate to the force/motion task being undertaken and to specify the task in terms

of desired force/motion trajectories in that frame. This specification of the manipulator

motion subject to constraints imposed by the task geometry was referred to as compliant

motion by Mason [8].

Many force/motion control methodologies implicitly combine the task specification in

Cartesian space and the control algorithm (see Hogan [5—7], Wu [9], Kazerooni, et al.

[10, 11]). On the other hand, the hybrid position/force control methodology proposed by

Raibert and Craig [12] is primarily a control system framework for Cartesian compliant

motion into which servo level force and position control algorithms appropriate to particular

manipulator configurations can be incorporated (see, for example Salisbury and Craig [13]).

All of these methods employ the manipulator Jacobian to relate the joint space to the


Cartesian task space. Indeed, it has been shown by An, et al. [14] that, unless carefully

structured, hybrid position/force control can be subject to a kinematic instability due to the

matrix inversion of the Jacobian in the feedback path. The more general operational space

formulation of Khatib [lSj also provides such a framework for non-Cartesian task spaces

and kinematically redundant manipulators. Another distinction, cited by An and Hollerbach

[16], between the hybrid position/force method and the operational space formulation is the

fact that the manipulator dynamics model is included in the latter.

In summarising more than a decade of progress in force control Whitney [2j notes that

the areas of stability and strategy have been well treated but that most papers contain

“no model of the origin of the contact forces” and “deal with [contact force] control only

in passing”. Particularly neglected areas of research cited by Whitney include the problem

of collisions, the effects of manipulator compliance and the general theory of task space

constraint specification. Uchiyama [17] offers a similar list of strengths and weaknesses in

the field.

All of the work cited above, and indeed the vast majority of the work to date in robotic

manipulator contact force control, has been focused on completely rigid manipulators. This

is hardly surprising given the complexity and difficulty of the rigid manipulator problem and

the many fruitful areas for research which were available without considering the additional

complexity of dynamics due to flexibility in the joints and links of the manipulator. How

ever, driven by the desire for lighter weight, higher performance industrial manipulators and

proposed teleoperated and autonomous applications of large, space based manipulators, at

tention has been turned to non-rigid manipulators. An increasingly large body of literature

exists which deals with the modelling of joint and link flexibility as well as the problem of

tip position control. Comparatively recently, the problems posed in the context of contact

force control by manipulators with significant flexibility in their joint assemblies [18, 19] and

structurally flexible links [20--25] have begun to be addressed. Given the acknowledged link


between physical compliance in the manipulator/sensor/environment system and the perfor

mance of force control systems, the possibility exists that manipulators with flexibility may

provide higher performance contact force control than totally rigid manipulators, provided

that the additional dynamics introduced by the flexibility can be adequately compensated

for in the controller design.

Research into force control with flexible link manipulators is still in its early stages with

the result that, as in the early stages of rigid manipulator force control, attention is being

focused on stability issues (see Chiou and Shahinpoor [21,23]), fundamental understanding

of the system dynamics (see Li [26]), feasible servo level control algorithms (see Pfeiffer

[24], Latornell and Cherchas [22, 27]) and control strategies (see Pfeiffer [24], Tzes and

Yurkovich [28], Matsuno, et al. [25]). The focus of the research reported in this thesis is the

development and application of control algorithms for flexible link manipulator force control

and the first steps in the integration of that control algorithm into an overall force/motion

control strategy.

It is to be expected that, as these fundamental issues are more fully explored and as

flexible link research manipulators with more than one or two links are constructed, flexible

link manipulator contact force control research will draw from and merge with the main

stream of force control research. Indeed the beginnings of this merger is already evident in

the work of Matsuno, et al. [25] which employs the hybrid position/force control framework

for a flexible link manipulator configuration.

1.4 Contributions of the Thesis

The primary contribution of the research reported in this thesis is the application of a

predictive control algorithm in the context of an overall control strategy for the control of the

contact forces exerted by a structurally flexible manipulator on surfaces in its environment.


The models and techniques used in the course of the research which lead to the achievement

of this goal have several important features.

Modelling: Kinematic and dynamic models of a robotic manipulator with structurally

flexible links interacting with surfaces in its working environment are derived. In contrast to

the simple linear spring model for contact dynamics used in much of the research reported

in the literature, the model derived here includes the effects of the local inertia and damping

as well as the stiffness of the contacting regions of the manipulator tip and the environment

surface. Analysis shows that a pure stiffness model is sufficient when the mass of the

contacting regions is small, but when the mass of the surface regions deflected due to contact

is comparable to the inertias of the links the additional dynamics in the environment model

become important. The kinematics and dynamics models are derived in a form which can

be readily transformed to less complicated configurations for rigid links and free motion

(general motion in the absence of contact).

Control Algorithm: A general formulation has been obtained for the characteristic poly.

nomial of a closed loop system controlled by the Generalised Predictive Control (GPC)

algorithm of Clarke, et al. [29]. The formulation is in discrete time transfer function form

applicable to both the single input, single output form of the algorithm presented by Clarke,

et al. and its multivariable extension. The result is shown to be useful in tuning the control

law, in particular in selecting the algorithm parameters (horizons and weighting factors) to

produce fast step response rise times and minimal overshoots from among the large set of

parameter values which provide stable control for a given system configuration.

A static equilibrium bias term extension has been derived for the predictive control al

gorithm. It is applicable to both the single and multivariable version of the algorithm. A

term which weights the deviation of the control inputs from the levels required for static


equilibrium is added to the control algorithm cost function. This addition results in a pro

portional action feedforward term in the control law which generally reduces step response

rise times. The static equilibrium bias term is particularly applicable to manipulator con

tact force control because there exists a clear static equilibrium relationship between the

joint torques and the contact force components.

Contact control logic is introduced to deal with the problem of the discontinuity which

exists in combined force and motion control when contact is made or broken. The contact

control logic is implemented at a level just above the joint servo level control laws in the

manipulator control hierarchy. Two operational strategies for the contact control logic

which seek to provide smooth control through unexpected contact events are demonstrated.

Linear Analysis: Linear analyses of a variety of configurations of manipulators in con

tact with environment surfaces provide insight into the fundamental dynamics of the sys

tem. The analyses use a combination of analytic and numerical techniques to shown that

the contact force is dominated by different open ioop modes of the system depending on

the effective contact stiffness of the surfaces. The transfer of dominance among the modes

follows an orderly progression as the effective contact stiffness is increased in a given ma

nipulator/environment configuration. In contrast, the control inputs are shown to provide

excitation to all of the open loop modes. The implications of these results for the imple

mentation of a discrete time predictive force controller are that the controller model should

include all modes of the manipulator dynamics but the sampling rate can be selected on

the basis of mode which makes the dominant contribution to the contact force.

Simulation: A general purpose computer simulation program, called TWOFLEX, which

includes the dynamics model and control algorithms was developed. The dynamics model

in the program can be configured to represent either a single link or a two link planar


manipulator with revoliite joints. The links can be rigid or flexible or, in the two link

case, a combination. The presence or absence of contact between the manipulator tip and

the environment surface is determined by monitoring the relative positions of the tip and

the environment. Contact can be made and broken repeatedly and the dynamics model

is modified accordingly on each change of the contact status. The program was used to

explore the dynamics characteristics of the system and to evaluate the performance of the

force control algorithm and the overall force and motion control strategy.

Force Control Performance Evaluation: Simulations results from the TWOFLEX pro

gram demonstrate the performance of the explicitly adaptive force and motion control strat

egy based on the predictive control algorithm for the following configurations and situations:

• manipulators with one and two flexible links in contact with environment surfaces

with effective contact stiffnesses varying over a wide range;

• prototype model of the two DC electric motor driven, 7.5m long, main links of the

Mobile Servicing System (MSS) for the proposed Space Station, Freedom; and

• use of contact control logic to maintain control through the discontinuous situation of

unexpectedly making contact with an environment surface.

1.5 Outline of the Thesis

As an overview, the remainder of the thesis is divided into chapters which discuss the

details of the manipulator and environment models, the derivation of the control algorithm,

the structure the computer simulation program TWOFLEX used to investigate the algorithm

performance, the analysis of the system and the performance of the control algorithm applied

to various manipulator configurations, and a statement of the major results of the research

and recommendations for future work in this area. Following the final chapter is a detailed


list of references. The thesis concludes with appendices which provide the mathematical

details of the equations of motion for several manipulator configurations, the expansion

and minimisation of the control algorithm cost functions, and the derivation of the static

equilibrium bias function.

Chapter 2 presents the detailed mathematical models of the manipulator, environment

and actuator systems. It begins with a brief review of the published literature concerning

the modelling of the kinematics, structural characteristics and dynamics of manipulators

with flexible links as well as the modelling of contact force generation and direct current

electric motor actuators. In section 2.3 a kinematic representation of the manipulator and

environment system based on a modified form of Denavit—Hartenberg homogeneous trans

formation matrices [30,31] is developed. The modification allows the kinematic effects due

to the link deflections to be incorporated in the homogeneous transformation matrix form.

The chapter continues with the derivation, in section 2.4, of the kinetic and potential energy

and equivalent viscous dissipation functions, and ultimately the equations of motion for a

two link planar manipulator with flexible links in contact with a deformable environment.

The general structure of the equations of motion is discussed and their simplification to

various subset configurations of interest is described. The equations resulting from these

simplifications are included in Appendix A. Section 2.5 presents expressions which relate

the contact force components to the generalised coordinates of the dynamics model. The

section also includes a discussion of the parameters of the contact dynamics model on the

basis of the material properties of the contacting surfaces. The chapter concludes with the

derivation of a model for a voltage controlled DC electric motor actuator and a discussion

of the coupling of the actuator model to the dynamic model of the manipulator.

In Chapter 3 the discrete time multivariable predictive control algorithm which lies at

the heart of the force control strategy is derived. Following an introductory discussion of

the particular challenges involved in force control for flexible link manipulators, section 3.2


reviews the Ceneralised Predictive Control (GPC) algorithm [29, 32, 33] from which many

of the ideas used in the multivariable algorithm are drawn. The section also includes

a derivation of the approximate closed ioop characteristics of a generic GPC controlled

system. Section 3.3 presents the derivation of a generic multivariable predictive control

algorithm. This derivation is supported by Appendix B in which the detailed expansion

and minimisation of the controller cost functions appear. For specific applications such as

force control, in which a clearly defined static equilibrium state exists, a new, augmented

form of the algorithm is derived by including a static equilibrium bias term (with details

in Appendices B and C) in the cost function, as described in section 3.4. This term adds

a proportional feedforward element to the control law. In section 3.5 the horizon and

weighting factor parameters of the control algorithm are discussed in terms of their roles in

the tuning of the controller. The chapter concludes with three sections in which parameter

estimation algorithms are reviewed, the implementation of the force control algorithm in

the context of an overall adaptive force and motion control strategy is presented, and and

the stability and convergence characteristics of the algorithm are discussed.

The computer program TWOFLEX is described in Chapter 4. This program is a simulation

of the continuous time dynamics of the planar two flexible link manipulator and environ

ment system dynamics, the discrete time multivariable predictive control algorithm and the

overall force and motion control strategy. It was developed in the course of the research

to provide a facility for testing and evaluating the performance of the control algorithm as

well as for the study of the open ioop dynamics of the system. The chapter is divided into

sections in which a general description of the structure of TWOFLEX is given (section 4.2),

specific details of the integration of the equations of motion (section 4.3) and the imple

mentation of the discrete time control algorithm (section 4.4) are discussed. A complete

listing of the fully documented program (approximately 18,200 lines of FORTRAN) as well as

a guide to its use is available as a UBC—CAMROL report [34]. The chapter concludes with


the results of a set of validation tests in which the homogeneous response of the manipu

lator dynamics model in the program was compared with that from a program developed

by Mah [35] which simulates an orbiting platform and manipulator system modelled as a

chain of flexible links connected by flexible joints.

Chapter 5 presents the results of linear analyses of the dynamics of several manipulator

and environment configurations, discusses the implications of those analyses for successful

force control, and presents a collection of simulation results which evaluate the performance

of the control algorithms under a variety of circumstances. In section 5.2 linear analysis

of a single link in contact with a deformable environment reveals that the domination of

the system dynamics by particular modes changes in an orderly fashion as the effective

contact stiffness that exists between the manipulator tip and the environment increases.

The important implications for force control of this changing modal structure are discussed.

The performance of the adaptive predictive control algorithm applied to force control is

demonstrated in section 5.3 for the case of a single flexible link. Contact with three surfaces

with widely differing effective contact stiffnesses is examined and it is shown how the results

of the linear analysis and the approximate closed ioop characteristics calculated from the

control law are applied to select high performance controllers with fast rise time and negli

gible overshoot. The problem of dealing with the discontinuous nature of the relationship

that exists between the contact force and the system state when contact is made or bro

ken is addressed in section 5.4. A new control level, called the contact control logic level,

which supervises the servo level predictive control algorithm is introduced. Two operational

strategies for the contact control logic are proposed and the overall system performance is

examined. In section 5.5 the analysis and performance evaluation of the multivariable force

controller for a two link manipulator contacting an environment surface is presented. The

linear analysis techniques applied in the single link case are employed and reveal expected

extensions of characteristics from that case as well as additional complexities which depend


on the configuration of the manipulator, in particular the joint angles. The results of this

analysis, in conjunction with the approximate closed ioop characteristics calculated from

the rnultivariable control law are used to select controller parameters which provide fast

rise times and minimal overshoots. The controller performance is demonstrated for both

rigid and flexible link manipulators. The effect of interaction between the contact forces

responses and the influence of the highly nonlinear manipulator dynamics on the controller

performance are also discused. A final application example is presented in section 5.6 in

which force control of a prototype model of two links of the Space Station Freedom Mobile

Servicing System (MSS) manipulator is demonstrated. The long, massive links of the MSS

are driven by voltage controller DC electric motor actuators which introduce additional dy

namics as well as saturation type nonlinearities. Portions of the work presented in Chapter 5

have already been published 122,27].

The thesis concludes with a summary of the major results and a statement of the original

contributions of this research in Chapter 6. Several recommendations for directions in future

research in this area are also included.

Chapter 2

Manipulator Modelling

2.1 Introduction

The manipulator model, which forms the basis for the computer simulation program TWOFLEX

described in Chapter 4 and the simulation study results presented in Chapter 5 is developed

in this chapter. The model is that of a planar two link manipulator with structurally flexible

links which can make contact with a deformable environment. The model consists of four

parts:

1. the manipulator configuration model, that is, the kinematics and dynamics of the

serial chain of links;

2. the link model, that is, the structural kinematics and dynamics of each link as a

flexible body;

3. the contact model, that is, the kinematics and dynamics of the regions of the manipu

lator tip and the environment that make contact. It is the interaction of these regions

that gives rise to the contact force which is the quantity to be controlled; and,

4. the actuator model, that is, the kinematics and dynamics of system which transform

the control signals into driving torques applied to the links.

This chapter begins with a review of the literature on the modelling of the kinematics and

dynamics of structurally flexible manipulators, contact dynamics, and DC motor actuator

systems. In section 2.3 the kinematic models of a planar two link manipulator with flexible

14

Chapter 2. Manipulator Modelling 15

links and a deformable environment are presented. Following, in section 2.4, is the derivation

of the equations of motion of the system and a discussion of the general structure of those

equations and the nature of the coupling among them. Section 2.5 presents the derivation of

the expressions for the contact force in terms of the generalised coordinates of the dynamics

model as well as a discussion of the selection of numerical values for the parameters of the

contact model. A model of an armature voltage controlled DC electric motor to be used as

an actuator is derived in section 2.6 and the coupling of the motor model to the equations

of motion is discussed.

2.2 Literature Review

Kinematics: The use of Denavit—Hartenberg [30] homogeneous transformation matrices

to describe the kinematics of robotic manipulators with rigid links is well established [36—38].

The extension of the Denavit—Hartenberg concept to describe the additional kinematics

due to structural flexibility of manipulator links was made by Book [39]. This approach is

attractive in that it is a natural extension of a widely used method.

A homogeneous transformation matrix approach, slightly different in form from that of

Book [39], is used in section 2.3.1 to obtain the position and velocity vectors for elements

of mass of the manipulator links and in section 2.3.3 for the contact region masses.

An alternative approach to the kinematic analysis of flexible manipulators has very

recently been proposed by Chang and Hamilton [40]. The concept of their Equivalent Rigid

Link System (ERLS) is to separate the rigid body and the structural dynamics of the

manipulator such that the global motion is described by a large rigid body motion with a

superimposed small motion due to the structural flexibility.

Structural Dynamics: The consideration of the structural flexibility in the links of a

robotic manipulator introduces a challenging analytical problem, namely, the links can no


longer be viewed as lumped masses whose motions are described by the motions of their

centres of mass; instead the links must be viewed as distributed parameter systems. As a

result, the motions are explicitly dependent on spatial as well as temporal variables and must

be described in terms of partial differential equations. Closed form solutions for systems

other than those with very simple geometry and uniform distributions of mass and stiffness

are rare and, as a result, approximate solution methods, generally involving discretisation

of the spatial coordinates, are widely used. Two major types of spatial discretisation are

available [41]: expansion of the solution into a finite series of given functions and the

lumped parameter approach. Series expansion methods are more widely used than the

lumped parameter approach and are divided by Meirovitch [41] into two classes, Rayleigh—

Ritz type methods and weighted residual methods. The majority of the literature on flexible

link manipulator modelling employs either the method of assumed modes [23, 42—45] or the

finite element method [46—48], both of which are Rayleigh—Ritz type methods. The use of

series expansion discretisation methods raises the issues of selection of appropriate functions

for use in the series and of the number of terms required in the series [43,49]. For highly

flexible systems consisting of many bodies, such as flexible spacecraft like the Space Station

Freedom, the finite element method is often used and an important issue is the selection of

appropriate subsets of modes to make computational solution of the system feasible [50—52].

In section 2.3.2 the mode shape functions for a cantilever beam are derived in closed

form. These functions are employed as admissible functions in the method of assumed modes

in the derivation of the equations of motion for the planar two flexible link manipulator in

section 2.4.

Equations of Motion: Early analyses of the dynamics of manipulators with structurally

flexible links, such as that by Hughes [53], were largely inspired by the development of the

teleoperated Space Shuttle Remote Manipulator System (SSRMS), or Canadarm. Analyses


of more general flexible robotic manipulators, such as that of Kelly and Huston [54], often

drew on the techniques developed for the analysis of flexible space structures [55,56].

A seminal work is that of Book [31], which employs homogeneous transformation ma

trices to describe both joint and link deflection kinematics and uses Lagrange’s equation

to derive the equation of motion. The analysis assumes that the link deflections are small,

such that the links are linearly elastic, and employs the method of assumed modes. An al

ternative approach to the modelling of link flexibility, which is briefly discussed by Book, is

the introduction of “imaginary” joints into a strictly rigid link dynamics model such as the

Newton—Euler formulation of Walker and Orin [57]. Book concludes that the computational

complexity of the Lagrangian approach is significantly greater than that of an equivalent

Newton—Euler formulation. The work of Silver [58], however, suggests that this need not

necessarily be so. In any case, the Lagrangian formulation provides better physical insight

into the interaction of structural and rigid body dynamics.

The majority of recent analyses of flexible manipulator dynamics employ energy methods

to derive the equations of motion [23, 42—45, 48, 49, 59, 601. Those that use a Newton—

Euler approach [46, 47,61] generally apply a lumped parameter discretisation to the spatial

coordinates of the link structural dynamics.

Dynamic analyses of flexible link manipulators making contact with their working en

vironment are rare in the literature. Most researchers [21, 23, 24, 26] model the contact

force as arising due to the compression of a simple linear spring connecting the manipulator

tip to the environment, thus lumping the contact dynamics into a single element. In the

course of the present research a more complete development of the dynamics of a single

flexible link contacting a deformable environment has been achieved and reported in the

literature [22,27]. That model includes a dynamic model of the areas of contact between

the manipulator tip and the environment surface. The model considers the stiffnesses of


the contacting surfaces, energy dissipation within the materials and the inertia of the sur

face material in the regions surrounding the point of contact on each surface. Section 2.4

presents an expanded version of that development. The equations of motion for a planar

two flexible link manipulator in contact with a deformable environment are derived using

Lagrangian dynamics.

Contact Force Model: In general a contact force model should include impact dynamics,

inertia effects, energy dissipation and elastic deformation. Whitney [2] notes that “control

analyses of arms in contact with an environment are rare” and that most papers “contain

no model of the origin of the contact forces themselves. Those that do are restricted to

modelling force as arising from deformation of some elastic elements.”

The literature on the solid mechanics of contact is extensive [62], however, the vast

majority of these analyses assumes that the contact is quasi-static. Analyses of dynamic

effects are generally concerned with the generation and propagation of stress waves in the

contacting bodies. For contact which does not involve plastic deformation of the surface

Johnson [62] shows that the dynamic response of an elastic half-space can be modelled

with good approximation by an elastic spring in parallel with a viscous damper, the latter

representing the energy dissipation of the stress waves. The theory of elasticity, in particular

the results derived by Hertz for the deformation of spherical surfaces in contact [63, 64],

provides some insight into the relationship between contact forces and surface deflections

under quasi-static conditions.

The contact model included in the dynamic analysis of section 2.4 uses the parallel spring

and damper model as well as a surface mass. Results from the theory of elasticity are used

in section 2.5.1 to provide estimates of the numerical values of the model coefficients based

on physical material properties.


Figure 2.1: Geometry of a two link flexible manipulator in contact with a deformable surface.

Actuator Models: The armature voltage controlled direct current electric motor actua

tor system discussed in section 2.6 is widely used and accepted models are readily available

both in the general literature [65] and for specific application to robotics [14].

2.3 Kinematics

2.3.1 Manipulator Configuration Model

Figure 2.1 shows a schematic representation of a planar two link manipulator with struc

turally flexible links in contact with a deformable environment. The geometry of the system

Fy

Fx

y w1(x1,t)

x


is described in terms of the four coordinate frames shown in Figure 2.2. Frame F° is the

fixed, world coordinate frame with the joint of link 1 located at its origin. Frame F’ is

fixed at the tip of link 1, Frame F2 is fixed at the tip of link 2 and frame FD is fixed at the

point of contact between the surfaces of link 2 and the environment. The orientation of the

nominal centreline of link 1 with respect to the world frame is given by the angle 19-, mea

sured counter-clockwise from the i axis of F°. Likewise, 82, measured counter-clockwise

from the i axis of F1, gives the orientation of the nominal centreline of link 2 with respect

to F’.

The links are modelled as elastic beams clamped to rotary actuators at the joint ends.

The elastic deformations of the links are given by wk(xk, t), the displacement of actual

(curved) centreline of the link from the nominal (straight) centreline of the undeformed link.

The subscript k indicates the link number. The distance along the link, Xk, is measured

along the nominal centreline relative to F’.

The geometric relationships among the coordinate frames can be compactly described

using homogeneous transformation matrices [30]. The transformation between frames F’

and F° is

c1 —s 0 L,c,

s, c1 0 L1s1H0,,(t) = (2.1)

0010

0001

where L, is the length of link 1, .s,= sin81(t) and c1= cos8i(t). Similarly, the transforma

tion,

C2 2 0 L2c2

H,2(t) =

L2s2(2.2)

000 1

Chapter 2. Manipulator Modelling

01, •0

Ii

21

Figure 2.2: Coordinate frames for a two link flexible manipulator in contact with a deform-

C

2ii

02)

2

T//

w2(x2,t) ,“

5C

‘2I

/

/

•012

Ii

w1 ( x1, t

1

- /

FO

able surface.


where L2 is the length of link 2, s2= sin02(t) and c2= cos &2(t), accounts for the rigid body

components of rotation and translation between frames F2 and F’.

An additional rotation and translation due to the structural deformation of link 1 must

also be considered. Based on ideas introduced by Book [31], the structural deformation of

link 1 relative to the undeformed centreline is described by the transformation matrix,

cosa, —sina1 0 0

sina1 cosa1 0 w10D,(t) = (2.3)

0 0 10

0 0 01

8w,0where to10 = wi(0, t) and a = to,0 =

(Ix,

that is, a1 is the local, instantaneous slope of link 1 at its tip. Premultiplying H2(t) by

D1(t), we obtain the homogeneous transformation matrix H1,2(t) which accounts for both

the rigid and flexible body effects in the geometric description of link 2 relative to and

projected on to F’:

Ca12 a12 0

8a 2 Ca 2 0 L2sa 2 + W10IT12(t) = D,(t) 111,2(t)

= 1 1 1(2.4)

0 0 1 0

0 0 0 1

where a2= sin [w’10(t) + 2(t)] and caj2= cos [w0(t) +02(t)]. Finally, premultiplication

of H,,2(t) by H0,1(t) gives H0,2(t) which describes the geometry of link 2 relative to and

projected on to the world coordinate frame F°:

C12 31a22 0 L,c1 — w,0s, + L2CIa12

5ia22 C1a12 0 L,s1 + W10C1 + L231a22H0,2(t) = H0,1(t)H,,2() = , (2.5)

0 0 1 0

0 0 0 1


where 1a2= sin[1(t) + w’0(t) + 82(t)j and ci12 cos [81(t) + w0(t) +02(t)j.

The structural deformation of link 2 introduces additional rotation and translation which

is described by

cos2 —sin2 0 0

sin c2 cos c2 0 w20D2(t) (2.6)

0 0 10

0 0 01

8w2where w20 = w2(0,t) and c2 = w20 =

Ux2

The position, relative to and projected on to F1, of an element of mass, dm1, located a

distance —x1 from the tip of link 1 is

—xl

w1(—x1 t)r(t)= ‘ . (2.7)

0

1

Premultiplying r(t) by H0,1(t) gives the position, relative to and projected on to F°, of

drn1:

(L1 — xi)ci — w1s1

r(t)= (L1 x1)s1 +w1c1

(2.8)

1

Likewise, the position, relative to and projected on to F2, of an element of mass, dm2,


located a distance —a2 from the tip of link 2 is

w2(—a2,t)r(t) (2.9)

0

1

and premultiplication by H0,2(t) gives the position, relative to and projected on to F°, of

dm2:

L1c1— 11)131 + (L2 — x2)ci12 —

L1s1 + W1C1 + (L2 2)31a22 + W2C1cii2(2.10)

1

Differentiating r(t) and r(t) with respect to time yields the velocities, v(t) and v(t),

of the elemental masses dm1 and dm2, relative to and projected on to .F°:

— [(L1—

x) s1 + wici] Oi —

v(t) = r(t)[(L1 — x1) Ci

—:‘‘

6 + W1C1

(2.11)

0

and

— (Lisi + w10c1)Si — iosi — (L2 — x2) 122 (ê1 + + 82)

(ô1 + + 82) — i2S1a22

v(t) = -f-r(t)(Lici — w10s1)Si — zij10c1 + (L2 — x2)c12 (ô1 + w + 82)

dt+222 (& + W + &2) — th2Clai2

0

0

(2.12)


In these expressions dotted quantities denote derivatives with respect to t and primed

quantities denote derivatives with respect to x.

The position vectors, r(t) and r(t), and the velocity vectors, v(t) and v(t), are used in

section 2.4 to form the kinetic energy, potential energy and dissipation function expressions

for the manipulator.

2.3.2 Flexible Link Model

Structurally flexible manipulator links are distributed parameter systems and thus have an

infinite number of degrees of freedom and must be modelled by partial differential equations

with both temporal and spatial independent variables. The inherent difficulty of solving

partial differential equations with space dependent coefficients, particularly in satisfying

boundary conditions, makes the use of approximate solution methods attractive, if not

essential.

For this research the method of assumed modes [41] is used. That is, the spatial co

ordinate is discretised into a series expansion of admissible functions each multiplied by a

generalised coordinate. The mode shape functions of a cantilever beam are employed as the

set of admissible functions. For the case of an Euler-Bernoulli beam, in which the effects

of rotary inertia and shear deformation are neglected, the mode shape functions are readily

derived in closed form [66]. While it is clearly evident that the boundary conditions for each

member of a chain of flexible manipulator links connected by torque producing actuators are

much more complex than those for a cantilever beam, it should be noted that the method of

assumed modes permits the use of any set of admissible functions; the number of terms in

the series determines the accuracy of the approximation. In particular the issue of whether

a pinned—free or a cantilever (fixed—free) beam model provides the more appropriate set of

admissible functions has been resolved both experimentally [43] and analytically [49]. The

accuracy of the predicted dynamic response, with a given number of included modes, is


Figure 2.3: Free body diagram of aEuler—Bernoulli beam bending equation.

found to be greater when cantilever beam mode shape functions are employed than when

those for a pinned—free beam are used.

Force and moment balances on a differential element of a beam (see Figure 2.3) lead to

the beam equation for an Euler—Bernoulli beam [66j:

ôw(xi)+

(EI(x)0’t)) 0, (2.13)

where p(x) is the mass distribution along the length of the beam and EI(x) is the distri

bution of flexural rigidity. This partial differential equation is separable. Letting w(x, t) =

4(x)?/(t), the partial derivatives can be expressed as:

= qb and

and the beam equation rewritten as

-

= (EI”)”(2.14)

z

F (xt)

M

dSS + — dx

dx

w(x,t)

_1_ dx

dMM + — dx

dx

differential beam element for the derivation of the

ô2w(x,t) —

8x2 —

Setting each side of equation (2.14) equal to a separation constant, w2, yields

=0 (2.15)


and

(EJqY’)” — w2pq = 0. (2.16)

Equation (2.15), subject to a pair of initial conditions, describes the temporal variation

of the shape of the beam while equation (2.16), combined with a set of four boundary

conditions, describes the spatial variations. It is this latter equation that provides the

mode shape functions which will be used as admissible functions in modelling the flexible

link dynamics.

Assuming p and El to be constant along the length of the beam, equation (2.16) is

rewritten as

— = 0, (2.17)

where=

(2.18)

Equation (2.17) is a linear homogeneous ordinary differential equation with constant coef

ficients for which the solution is

4(z) = Asinh/3x + B cosh3x + Csin/3x + D cos!3x. (2.19)

The boundary conditions which facilitate evaluation of the arbitrary constants in equa

tion (2.19) are, for a cantilever beam of length L, zero displacement and slope at the fixed

end and zero shear and moment at the free end; in mathematical terms

8ww=o = 0

or

EI = 0 EI4 = 0, (2.20)

= 0 = 0

‘7H=L — 0 IL = 0. (2.21)


Application of these boundary conditions to equation (2.19) yields the frequency equation:

cos5L coshBL + 1 = 0, (2.22)

for which there are an infinite number of solutions, {i3, i = 1,. . . , oo}, each of which is

related to the natural frequency, w, of one of the modes of the beam by equation (2.18).

The corresponding mode shape function for the ith mode is

q(x) = cosh,Bx — cos/3x—

(sinh,82x— sinf3x) (2.23)

In this form the mode shape functions are normalised such that

JL

= L (2.24)

and they are orthogonal, that is,

L

f 4j(x)j(x) dx -0, i j. (2.25)

These mode shape functions are used as admissible functions for the method of assumed

modes which is employed in the derivation of the equations of motion in section 2.4.

The slope of the mode shape function for the ith mode is

4(x) = = _,i [sinix + sin jx+ (:: (cos/3x — cosh/3ix)]

(2.26)

these slope expressions are used to calculate = w0, the local, instantaneous slopes of

the links at their tips which appear in the structural deformation transformation matrices

given by equations (2.3) and (2.6).

A final aspect of the kinematics of a flexible beam that must be noted is the fact that as

the beam deflects, its length, measured along the nominal (undeformed) centerline decreases.

The magnitude of this axial foreshortening can be calculated by considering an element of

the deflected beam as shown in Figure 2.4. The decrease in length of the beam element is


Rewriting the cosine function

2.3.3 Contact Model

dx In

cos cos ( qSbJds \i=1 j

(2.29)

(2.30)

1

Figure 2.4: Axial foreshortening of a differential beam element.

d = ds — dx. From the geometry of the situation we see that

(2.27)

(2.28)

that is, the cosine of the local, instantaneous slope. Thus

d= [1

cos

and so= jL

[i_ cos ds.

in this integral as a series expansion,\2 /n

L ()=

j1 — 1— Z=121 —

214! — ds,

we see that the is O(’].). Since the elastic deformations of the links, which are proportional

to are assumed to be small the foreshortening of the links will be neglected.

A manipulator making and breaking contact with its environment must be viewed not as a

single dynamic system but rather as two closely related systems. At the instant of contact


a kinematic constraint, such that the position, velocity and acceleration of the contacting

surfaces are identical, is enforced on the system for the duration of the contact. The contact

force arises from the interaction of the contacting surfaces. Conversely, on breaking contact

the kinematic constraint is removed, the surfaces move independently and the contact force

is, by definition, zero.

In order to predict the contact force which will arise between the manipulator tip and

the environment surface a contact force model is required. The model used in the present

research is based on results reported by Johnson [62] which show that the dynamic response

of an elastic half-space can be modelled with good approximation by a linear elastic spring

in parallel with a viscous damper, the latter representing the energy dissipated by the

stress waves radiating through the material surrounding the region of contact. This model

neglects reflection of these stress waves. As indicated in the inset of Figure 2.1, this model is

adopted by representing the contacting surfaces of the link and the environment as masses

on damped linear elastic foundations. The contact coordinate frame, .Fc, is located at the

point of initial contact between the manipulator tip and the environment. Relative to and

projected onto the world frame, F°, the origin of .Fc is located at Xw+Yw.

The contact masses, m and me, represent the inertia of the contact regions of the

manipulator tip and the environment, respectively. These masses should not be confused

with the total mass of those bodies. It is the compression of these regions that gives rise

to the contact force. The deformation of the surfaces during contact is represented by the

motion, ei+Ei of the total contact mass m (the sum of the contact region masses m

and me).

The contact coordinate frame, Fc, is chosen with the same orientation as the fixed world

frame, F°. This is tantamount to assuming that the contact force is measured in world coor

dinates, a valid assumption since such transformations are easily and routinely performed by

the hardware and software associated with wrist force sensors. The transformation between


fC and .F2 is thus simply a rotation of — [81(t) + w’0(t) + 82(t) +w0(L)]:

C1i2a2 81a12a2 0 0

H2(t)= 81cj22 C1a12a2 0 0

(2.31)o 0 10

0 0 01

and hence the transformation between FC and F° is a simple translation, obtained by

premultiplying H2,(t) by D2(t) and Ho,2(t):

1 0 0 L1c1 — w10s1 +L2C1a12 W2oSlai2

0 1 0 L1s1 + w10c1 + L2s112 + w20c1a12H0,2(t)D2(t)H2,(t) = . (2.32)

001 0

000 1

The position of the contact mass, m, relative to and projected on to F’, is

= U(2.33)

0

1

Premultiplying r(t) by H0,(t) gives the position vector, relative to and projected onto LF°:

L1c1 — w10s1 + L2C1a12—

W2081a12+ E2

L1s1 + wiOc + L281a12+ W20C112 + Eyr(t) = Ho,(t)r(t) = . (2.34)

0

1

The velocity vector, v(t) for the contact mass, relative to and projected onto F°, is obtained


by differentiating r(t) with respect to t:

— (Lisi + w10c1)Oi —— (L2s1a12 + wloclaj2) + +

W2o81ai2 + E

v(t) = r(t)= (Lici — w10s1)01 +i10c1 + (L2C1a12 — WioSiai2)(é1 + Wio + 02)

+‘th2oClcxi2 + y

0

0

(2.35)

The position vector r(t) and velocity vector v(t) are used to derive the kinetic en

ergy, potential energy and dissipation function expressions for the contact dynamics in

section 2.4.

2.4 Derivation of Equations of Motion

Given the position and velocity vectors for the elemental link masses and the the contact

region mass (derived in section 2.3), the equations of motion for the manipulator and envi

ronment system are derived using the well known energy method of Lagrangian dynamics.

As noted in section 2.2 this approach to the derivation of equations of motion for robotic

manipulators is widely used [23,31,48,60]. It has the advantage that the resulting equations

are in a form well suited to numerical integration for the purposes of simulation studies.

Also, in the case of structurally flexible manipulators, the problem of accounting for the

movement of the centres of mass of the bodies as they deform is avoided. The structure of

the resulting equations of motion gives clear insight into the interaction of the rigid body,

structural and contact dynamics.

During the early stages of the research the equations of motion for several simple con

figurations of the manipulator and environment system, such as a single flexible link with


unidirectional contact, were derived manually [22]. The equations for the two flexible link

system, presented below, were derived with the assistance of the Maple V algebraic com

putation system [67]. Maple V proved invaluable in helping to avoid transcription and

other mechanical errors which often arise in the course of manual algebraic manipulations.

Furthermore, Maple V includes a rudimentary FORTRAN code generator, which was used

to translate (with minimal errors) the equations of motion into source code which served

as starting points for the simulation subroutines which calculate the terms of the inertia

matrix and the right hand sides of the equations. As well, the J.TEX code generator in

Maple V was used to assist in the preparation of the equations of motion for typesetting in

this document.

The total kinetic energy, 7, of the system is

= 2 JMv dm1 +

1M2v dm2 + mcvT v, (2.36)

where Mk is the total mass of link k. The first two terms are the kinetic energies of the

links and the last term is the kinetic energy of the contact region. Expanding the vector

dot products in equation (2.36) and evaluating the integrals over the link masses (assuming

constant distributions of mass over the lengths of the links), the total kinetic energy is

rewritten as:

7- = Io1o+1Iiiiiioi

+ ‘21j (b +e)

+M2 (L + + [102+fl2]

(ô1 + + é2)2 + iitb

+I22j + [M2 (LiL2ci2i+ L2WloSai2&1 +L2c121o) + h12’2i

— (L1Sa12Ô1— + 10s2) I42i2i] (i + w10 + 62) +M2L18110


+ (LiCai2i+ WloSal2ô+ai2io) 14221 + ( + é + + ti)

4m [(L + + (L + t4) (a1 + +

—m [(Lisi + wi0ci) Ô1 + (L2si12 + W2oClai2) (ô1 + W10 + 02)] x

+m [(Lici — w10s1)01 + (L2C1a12—w20s112)(1 + 11o + 02)]

—me, (tiii0si + W2o31a12) + m (zhi0ci + tb2ocIaj2) Ey

+m [— (L18a12— wlocaj2) w20Ô1 + (L2c12— W205a12)tub] (ô1 + W10 + 02)

+m [(LiL2ai2+L2Wioai2)01 + L2tui2o] (o1 + t140 + 02)

+m [(L1Ca12+ wbosai2)01tv20 + W10W20Ca12+L181tu10] . (2.37)

The assumption of constant mass distribution in the links is made for simplicity; any

mass distribution function, pk(xk), may be chosen and the integrals evaluated in closed

form or numerically, as necessary. The summations in equation (2.37) are the result of the

application of the method of assumed modes [41] to discretise the spatial variations of the

link motions. The upper limits of summation, k, are the number of admissible functions

included in the structural model of link k. The subscripted I factors are integrals of the

admissible functions. They are discussed in detail below, following the statement of the

system potential energy.

The link kinetic energy terms in equation (2.37) are in agreement with an appropriate

subset of the kinetic energy terms for a model of an orbitting space platform based mobile

flexible manipulator reported by Chan [681. Since the kinematic representation used by

Chan is markedly different from that described in section 2.3.1 the agreement of kinetic

energy expressions serves to validate the method used here.

The total potential energy, 1), of the system is

1) = — J gT r dm1— J gT r dm2

M1


1 1 1 2+>T3ii’tb1+

2+k8(Lici — W1031 + L2C1a12 — W2oS1c2 — xw — E)

+k8(Ljsi + W10C1 + L231a12+ W2oClai2 Yw— )2

2 j. 2m

where g is the gravity field vector. The first two terms are the gravitational potential

energies of the links. They are followed by the elastic potential energies of the links due

to their structural stiffnesses. The last four terms are the elastic potential energies of the

contacting surfaces of the manipulator tip and the environment with the point of initial

contact located at xi + The zero condition for the springs used to represent the

contact stiffness is that of zero force.

The bk factors in equation (2.37) represent the rigid body components of the link

moments of inertia about the joint axis:

1 2‘ok = (Lk — Xk) dmk. (2.39)

J Mk

The other subscripted I factors in equations (2.37) and (2.38) represent integrals which

involve the set of admissible functions used as assumed modes to model the link structural

dynamics. Given a set of admissible functions, these integrals evaluate to constants in

the equations of motion. Employing the cantilever beam mode shape functions derived in

section 2.3.2 these integrals are evaluated, for mode i of link k, as follows:

‘iki= J (Lk

—

xk) qki(xk) dmk = 2-- (2.40)I3k

where 13k1 is the solution of the cantilever beam frequency equation (equation (2.22)) for

the ith mode of link k and pk is the mass per unit length of link k,

‘2ki= J k(Xk)2dmk = pkLk = Mk, (2.41)

Mk


13ki

JLk

Elk(82)2

dxk = EIkLk MkWL, (2.42)

where Elk is the fiexural rigidity of link k in bending about its joint axis, and,

I pk (sinh/3k2Lk smI3kLk N‘4ki = 4’k(xk)dmk = 2— i (2.43)JMk /3kz \cosh/3kJk+cos/3k:LkJ

All of the I factors are retained as general constants in this derivation and in the subse

quent simulations to facilitate the use of any arbitrary set of admissible functions and link

dimensions.

Assuming that the energy dissipation in the system can be modelled using equivalent

viscous damping, the total dissipation function, R., for the system is

= +

1 2+ b12’4b1 +

[(Lisi + wi0ci)Ôi + (L2ia12 + W2oC1a12)(ê1 + o + 2)

+W10S1+ W2oSlai2 + J+b8 [(Lici — wi0s1)1+ (L2ci12 —w20s112)(o + w + 82)

+w10c1+ W20C2— Ej

1 1ezEx

where the subscripted b terms are equivalent viscous damping coefficients. The total dissi

pation function is somewhat analogous to the total potential energy function. The first two

terms represent the energy dissipated due to friction in the link joints. They are followed by

terms for losses in the links due to structural damping. The last four terms are the energy

dissipated in the materials of the contacting surfaces of the manipulator tip and the envi

ronment. The dissipation of energy in the links and the contact regions is due to hysterisis

associated with cyclic stressing of the materials. The selection of damping coefficient values


is somewhat complicated by the fact that the energy dissipation is dependent on a number

of factors including temperature, stressing frequency and stress level. Lazan [69] provides

a collection of experimentally determined loss factors which are the ratio of the energy

dissipated per cycle to the maximum strain energy in the material. Assuming harmonic ex

citation these data can be used to estimate damping coefficient values of lumped parameter

systems following the method described by Meirovitch [70]. However, for more general situ

ations Gaylord and Gaylord [71] recommend that the damping in most structural members

be assumed to be a few percent of critical.

Using equations (2.37) and (2.38) to form the Lagrangian for the system, C’ T — 1), the

equations of motion are given by Lagrange’s equation, augmented with a disspation term:

d (ôL’ &C oR.o) = — —

+ Q (2.45)

where q is one of the system variables,

1, 02, ,i = 1,... ,ni}, ,j = 1,... ,n}, e,

selected as generalised coordinates and Q is the corresponding generalised force. The com

plete set of equations of motion consists of 4 + ni + n2 strongly coupled, nonlinear, second

order, ordinary differential equations. The general structure of each of the equations is

Q + bj + kT + g(q) + N(q, 4), (2.46)

where I = I(q) is a vector of inertia terms, j= j(t) is a vector of the second time derivatives

of the generalised coordinates (generalised accelerations), Q is a generalised force, b = b(q)

is a vector of dissipation coefficients,=

4(t) is a vector of the first time derivatives of

the generalised coordinates (generalised velocities), k = k(q) is a vector of stiffness terms

(which are generally weakly nonlinear functions of the generalised coordinates due to the

geometry of the system), q q(t) is a vector of the generalised coordinates, g(q) is the


gravity term and N(q, ) is a collection of centripetal and Coriolis terms which involve

products of the generalised velocities.

For the O generalised coordinate the generalised force is T1, the torque applied to joint 1,

and, after simplification and collection of only second time derivative terms on the left hand

side, equation (2.45) yields the equation of motion which will be referred to as the rigid

body equation for link 1:

[i + 102 + M2 (L + w0 +L1L2c12 +L2ioai2)

722

+I2iz”,b1j+I22i,b2— 2 (Liat2 — WioCaj2)142jI’2j

+m (L +w0+L + wo — 2 (LlW2oSai2 — WIOW20Ca12 L1L2Ca12 L2W10Sa12))j i

+ I11i11i

+ [M2 (L1 +L2c12)— I:: + m (L1 + L2c12

—2oSa2)] L

+ 1o2 + -M2L2(L1Ca12+ WloSai2) + 122jI’2j — (L18a12— wlocai2)

j=1 j=1

+m (L + w0 + LiL2ca12 — L1W2OSa22+ L2wlosai2 + W1OW2QCa12)]

+ [102 +M2L2(Lic12+W10Sa12)+I22j — (Lia12

+m (L + w0 +L1L2Ca12 —L1w20s12 + L2wlosaj2 + WloW2oCai2)] 02

+ + m [c12Li +s12w10 + L2] 2jo2j + [Liai2 + wlosa12] Ij/7

—m [Lisi + w10c1 + L2s112 + W2OC1a12]E

+m [Lici — wiosi +L2c1a12 — W2081a12]Ey

= T1 —

+b35 (L1s1 + W10C1 + L251a12+ W2oClai2) [(Lisi + w10c1 + L2Sai2 + W2oClai2) 6


(L231a12+w20c12)62 — W10S1 —— (L2S1a12+w20c112)

— x]

—b3 (Lici — W10S1 + L2C1a12 — W2031a12) [(Lici — wiosi + L2C1a12 — W2081a12)8i

+ (L2C1a2 — W2OS1a12)62 + mc + tii2c112 + (L2c1a12— W2oSlai2) o— y]

+k (Lisi + W10C1 + L2S1a12 + W2oClai2) (Lici — wi0si +L2c112— W2081a12— —

e)

—k8 (Lci — w10s1 + L2c112— W2Q81a12)(Lisi + w10c1 + L2s1a12 + W2oC1ci2 Yw — E)

+gx (MiLsi + C1 + M2L1S1+ M2w10c1+ M2L2Siai2+

—gy (_M1L1c1— i I41j1j + M2L1c1 — M2w10s1+ M2L2Ciai2— 812 I42j2j

i=1 j=1

+ { [ML2(L13a12 — W1OCa12) + 2 (Lic12 + WloSai2) 142j’2j

+2mg (LiL2s12 +L1W20Ca12—L2w10c12 + WloW2oSai2)] 82

+1I2 [L2 (Lis12— W1OCa12)4c — (L28a12+ 2wi0)thi0j

2I21b11 — 2122 b2b2

+2 [(Llcai2 +w10s12)Wo — Cak2Wlo] ‘42j23 + 2 (L13a12— w10c12) ‘42j2j

—2mg [(w10 + L28a12 + W2OCa12)11)10 — (L1sa12 — W1OCa12 — w20)w20j

—2m0[(L1L2s12 + Liw2ocai2+ W1OW20Sa12— L2W10Ca12)11401} i+ [M2L2(L1Sa12 — wlocai2) + (L1ca12+ hl1o3ai2)I42j2j

+m0(L1L2Sa12+L1w20c12 +w10w20s2— L2ioCai2)]

+ [M2L2(Lisai2—

c12wi0)11i0_ + 2(Lisai2 —

+2 (L1Ca12+ 5ai2t’1o) lL I42j1’2j + 2m0 (L13a12 — W10Ca12— w20)12o

j=1


+2m (LiL2ai2— wloL2caj2 + L1W2OCa12+w10w20s12)io] 2

+ [M2L2(Liai2 WloCai2)+(LlCai2+W1o3ai2)I42j2j

+m(L1L2s12 —w10L2c2+L1w20ca12 + WloW2oSai2)] Zi

+2 [(L1Sa12— wIocai2)I42j2j — I22j2j2j +2mc(Llsai2 — WloCai2 —w2o)b2o]

(2.47)

Similarly, for link 2 with the generalised coordinate 2 and the corresponding joint torque,

r2, the rigid body equation is:

[102 + M2L2(L1Ca12+w10s12)+ I22j’ — (L13a12—

+m (L + wL +L1L2c12 — L1W2oSa2 +L2W10Sa12+ WloW2oCal2)1 Oi

+ [M2L::ai2— + mc — Sa12W20)]

+ [102 + I22j + m (L

+ [102 + I22j + m (L + w0)] 2 + I12j2j +

—m [L2si12 +w20c112}ë + m [L2ci12 —w20si2]ë,

=r2—b0202

—b3 (L281a12+ W2øClai2) [(Lisi + W10C1 + L251a12+ w2oclai2) &1

+ (L251a12+ W2OCIa12)02 + +l0sl + i2os1ai2 + (L251a12+ W2oClai2)1140]

b8 (L2C1a12— W2051a12) [(Lici — wiosi + L2C1a12 — w2oslai2) &i

— (L2ci12 — w20s1a12)82 — é +(10c1 + th2oClai2 + (L2ci12 —w20s12)

+k8(L2s112 + w2ocIai2) (Lici — w10s1 + L2c1a12 W2o51cx12—


—k3 (L2C1a12- w2oslai2) (Lisi + w10c1 +L2s112 + W2oCicj2 Yw —Ey)

1 fl2

+gx (M2L2sii2+ C1ai2I422) — gy (M2L2iai2— 81ai2I422)

+ [M2L2(WloCai2 — L1Sa12) — (L1ca12+ W1oSai2)I42j2j

—m (LjL2sai2+ Liw2ocaj2 WioL2Cai2+ wlow2oSai2)] è

— [M2L2ai2io+ 2Cai2ioI42j2j +

+2m [(L2Sa12+w20c12)io +w2olb2o]] 8

— + 2mcw2o2o](+ &). (248)

The time variation of the ith mode of the shape of link 1 is given by the generalised co

ordinate i& for which the corresponding generalised force is zero. Applying equation (2.45)

yields the deflection equation for link 1:

+ [M2 (L1 + L2Cai2)—

3ai21oI42j’2j + m(Li + L2ca12 — W208a12)]

+ [102 + M2L2(Lic12 + Wioa12)+ I22j — (Lis12 —

+m (L + wL +L1L2Ca12 — LlW2oSat2 +L2w10s2+ W1OW2OCa12)] io] i

+I21j1j + {M210 + (M2L2ci2 Sai2I42i)2)

+rnc [qizo + (L2Ca12— W203a12)14]} 412oh/’1z

+ { (M2L2Cai2—

Sai2I422i) 1io + (102 + I22))


+m [(L2Ca12 —w20s12) + ( +)] }

+ { [cai: — 142j2j + m (L2ca12— 1112o3a12)] &jo

+ ‘o2 + 122ij + rn (L + w)] iQ} 2

fl2

-f-jo + cai2cliwI42b2+ [m1.(L2q10 + ca12qiio)]3=1 j=1 j=1

—rn [1n81 + (L2s1a12+ W2OC1a12)4cJ a, + rn [4i0ci + (+L2ciai2— w2081a12)1jo] E

= —b1J.1—

—b3(10s1 + L2S1a2j0+ W2oC1ai2j0)[(Lisi + wci + L251a12+ w2oclai2) 01

+ (w2oclai2 + L251a12)02 + e + W1o81 + W2031a12+ (L231a12+w20ci12)tb]

—b3(10c1 + L2C1aL2iO —w20s1120)[(Lici — W10S1 + L2C1a12 —w20s112)Ô1

+ (L2ci12— W2o31a12)02 — Ey + 1ol + W20C12 + (L2ci12 —w20s112)

+k8 + L2S1ai26jO + W2oC1ai294o) (Liei — wi0si + L2C1a12 W2o81a12— — e)

—k (410c1+ L2c1a12ç/4j0— W2oS1ai24j0)(Lisi + w10c1 + L2s112 + w20c1a12 — E)

+gxslI4li —

+ {1211 + [M2 (L2Sai2 + 10) + cai2 142j’2j + m (w10 + L2s12 +w20c12)] 1jO

— [M2L2(Llsai2 — W1QCa12) + (Llcai2 + l111o5ai2)I42i2i] io

—m(L1L2Sa12 +L1W20Ca12 —L2wiocai2+ WloW2oSai2)

+ { [M2L212 + 2c12 I42j2j + 2m (L2s12 +:12)]

1io02

+2 M2L2ai2ti4o + a2 I42j&2j + Cai2tL4o I42jlI’2j:1=1 j=1


+m (Sai22o + (L2sai2 + w2ocaj2)

2[f2L2ai2io+ I22j2j2 + Cai2WloI42j)2j

+m(w20b20+ (L2Sa12+ W2ocai2)1o)] io}

+ [M2L2ai2+ Cai2I42j2j+ mc(L23a12+ W2ocai2)]

+2 { [M2L2Sai2toiio + Sai2I42j2j+ Cai2W1oI42jJ2j

+mc(L2sai2o+w2oC12W10+ 1jo — + mcw2oti2o) io} 82

+ [M2L2Sai2+ Ca12 ‘42j2j + m (L2Sa12+ W2oCai2)) 1jo

+2 [(Sai2I422 + mcsai2zi2o) iio — + mcw2oui2o) io] to (2.49)

Likewise, the deflection equation from the jth mode of link 2 is obtained from (2.45) using

the generalised coordinate b2i:

[112j + (L1CaI2 + Sai2Wlo)142j + m (Lic12 + w10s12 + L2) 2jo1

+ [Cai2142 + mc1220] &jo1i + [112j + mL22501 oij

+[112j + mL220j82 + ‘22j2j + m20 2jo2j — +mc2joclai2ëv

= —b2/’2—

b8x2joS1ai2 [(Lisi + wioci +L2s112 +w20ci12)8 + (L2s12 +w20ci12)82

+6rW1o81 + W2051cz12+ (L251cz12+ w2oc1a12)i4o}

bsy2joC1a2 [(Lici — ws1 + L2c1a12— w2oslai2) Oi + (L2Ca12 W2o51a12)82

6!,W1OC1+ W2oClcxi2 + (L2C1a12 W2o51a12)th0J


+ksx42joS1ai2 (Lici — w10s1 +L2c112 ——

—

—ksyq52jociai2(Lisi + WioC + L2S1a12+ W2oClai2— Yw — e1)

+gs1aj2I42j— 9yC1ai2142j

+ [I22j’2j — (L18a12— tUloCai2)142j + m (wlocai2 — L1Sa12 + w20)q52jo] 0

+2 {I22&2 + mw2o20102— ai2142j1o + I22b2iW0— m (sa12thio — w20t140)b20}i

+[122j’c/-’2j + mcw2oq2jo] (th, + & + 2i4o02) (2.50)

The generalised coordinate for the x direction component (in the world frame) of the

contact mass deflection is e and the x direction contact equation is:

—m [Lisi + L2S1a12+ W10C1 + W2C112]01 — ms1 1jo1j

—m [L2s112 + w20c1a12] —m [L231a12+ W2OC1a12I02

—ms112 2jo2j + mE

bexx — kexEx

—b [(Lisi + wioc1 + L2S1a12+ W2OC1a12)Oi + (L231a12+ w20c1a12)02

+E + W10S1 + W2oSlcxi2 + (W2oClai2 + L281a12)ti4ol

+ksx (L1c1 — w10s1 + L2c1a12—— — Ex)

+m (Lici — w10s1 + L2c1a12 W2031a12)0 + m(L2ci12— W2051a12)02

+m(L2ci12 —w20s112)tb + 2m?i20c112?d40

+2m {(L2c112— W2051a12)02 + 10c1 + zb20c12 + (L2ci12

— w2031a12)4] ôi

+2m [(L2cm12 — W2081a12)l)1o + W2oC1ci2j 02. (2.51)

Finally, the y direction contact equation, obtained with the generalised coordinate es,, is:

ii [Lici — w10s1 + L2c1a12 W2o31a121 1 + mc1 1jo1i


+m [L2C1a12— W2o31a12] — m [L2Clai2 — W2oSi2]02

+mc112 2jo2 + mE

= beyEy — keyEy

+b [(Lici — w10s1 +L2c112 — W2QS1a2) Si + (L2ci12 —w20s112)02

+ iJ-10c1 + w20c1a12+ (L2c1a12—w20s112)Wi0]

+k3 (Lisi + wi0ci +L2s112 + W2oCiaj2— Yw c11)

+m (Lisi +L2s112 + w10c1 + w2oclai2) 0 + m (L251a12+ w2ociai2) 02

+m (L231a12+ W2oClai2) ti4 + 2mctb2osiai2ti4o

+2mg [(L251a12+ lD2oC1ai2) 02 + WioSi + W2oS112+ (L251a12+ W2oCiai2) oj 6

+2m [(L281a12+ W2oClai2) Wlo + W2oSlai2] 62. (2.52)

Several steps have been taken in an effort to ensure the correctness of the derivation of

equations (2.47) through (2.52). As noted previously, the kinetic energy expression (equa

tion (2.37)), from which most of the terms in the equations of motion arise, is in agreement

with that derived independently by Chan [68]. Furthermore, Chan’s derivation is in agree

ment with two other independent derivations for related systems carried out by Mah [35]

and Ng [72]. The application of Lagrange’s equation to the energy expressions to obtain

the equations of motion and the subsequent simplification and manipulation of those equa

tions was done using the Maple V algebraic computation program. This method protects

against errors which are common in manual algebraic manipulation such as transcription

errors, sign errors and misapplication of the chain rule of differentiation. Finally, as will

be discussed in section 4.5, the FORTRAN coded form of the equations of motion have been

validated against the code developed by Mah [35] both by comparison of calculated values

Chapter 2 Manipulator Modelling 46

for the terms in the inertia matrix and on the right hand side, and by comparison of ho

mogeneous responses for a variety of initial conditions. Mah’s program has been similarly

validated against programs developed independently by Chan [681 and Ng [72].

With reference to the generic equation of motion, (2.46), it is evident that, when contact

exists, equations (2.47) through (2.52) describe a system of 4 + ni + n2 damped, coupled,

nonlinear oscillators. The oscillators are forced by the joint torques in the rigid body

equations which act to change the joint angle generalised coordinates.

The coupling among the oscillators ensures that an excitation of any of the generalised

coordinates will be transmitted to all of the other generalised coordinates. The inertial

component of the coupling is evident in the fact that the inertia terms on the left hand side

of each equation are a weighted sum of all of the generalised accelerations. Considering the

complete set of equations, the weighting terms can be written as a symmetric generalised

inertia matrix. The terms in that matrix can be divided into two groups; those which are

inherent in the dynamics of a serially connected chain of flexible links and those which

arise due to the existence of contact between the manipulator tip and an environment

surface. The same division is true of the nonlinear right hand side terms, N(q, 4), which

are functions of the generalised coordinates and velocities. On the other hand the coupling

in the dissipation and stiffness terms on the right hand side exists solely due to contact.

As will be shown in section 2.5, the contact force exerted by the manipulator on the

environment can be expressed as a function of the generalised coordinates. Thus, joint

torque inputs will tend to excite oscillations of the generalised coordinates which will be

expressed as oscillations of the contact force. The goal of force control is to calculate and

apply joint torques which will cause the contact force to quickly follow changes in the desired

contact force level.

In the absence of contact the system becomes a set of ni +n2 damped, coupled, nonlinear

oscillators with an additional two rotational degrees of freedom. The joint torques act to


produce rotations of the link bodies about the joints and the inertial and nonlinear cou

pling ensures that oscillations of the generalised coordinates associated with the structural

dynamics are excited.

Equations (2.47) through (2.52) can be transformed to the equations of motion for

several other configurations which have been investigated in the course of the research.

Setting each element of the set of coefficients {m, bex, b3, b3, kex, k3, k8} to

zero and dropping the generalised coordinates E and ey and equations (2.51) and (2.52)

yields the 2 + ni + n2 equations of motion for a planar two link flexible manipulator in free

motion. The resulting equations are shown in section A.l of Appendix A. These equations

govern the motion of the manipulator when it is not in contact with the environment.

Note that by dropping the generalised coordinates and e in the absence of contact, the

motions after contact of the contacting surfaces are neglected. By implication, when contact

is broken, the contacting surfaces effectively come immediately to rest without residual

deflection. This assumption is reasonable considering that the masses and deflections of the

contacting surfaces are small and that neither plastic deformation of the surfaces nor gross

motions of the environment are included in the model.

Starting from equations (2.47) through (2.52), the equations of motion for a planar

two link manipulator with rigid links contacting a deformable environment are obtained

by setting each of the coefficients {I1k, ‘2ki, JJ3ki, ‘4ki, bk, i 1,. . . , rik, k 1, 2} to zero,

dropping the generalised sets of coordinates {‘,bi2,i 1,... ,m1} and {b23,j = 1,... ,n2} and

dropping equations (2.49) and (2.50). The resulting four equations of motion are shown

in section A.2. Further operation on this resulting set of equations, namely setting each

of the coefficients {m, bex, bey, b3, kex, key, k8x, ksy} to zero, dropping the generalised

coordinates e,, and E and dropping equations (A.7) and (A.8) yields the two equations of

motion for a planar, rigid, two link manipulator in free motion, shown in section A.3.


Finally, the 2 + n1 equations of motion for a single flexible link in contact (section A.4)

and the 1 + n1 for the same link in free motion (section A.5) are arrived at by setting

the coefficients {IO2,I12,I22,I32,I42, b2, b€, b3, kex, k3,i = 1,... ,n2} to zero, dropping

the generalised coordinates 62, {b2,i = 1,. . . ,n2} and e, and dispensing with equations

(2.48), (2.50) and (2.51) in the former case and by additionally setting the coefficients

{m, key, k3} to zero, dropping the generalised coordinate e, and dispensing with

equation (A.13) in the latter.

2.5 Contact Force Model

Figure 2.5 shows the geometry of the two flexible link manipulator and deformable environ

ment system with the link deflections and the environment model much exaggerated in scale

(compared to Figure 2.1). The contact force arises from the local compression of the regions

surrounding the point of contact between the link (or end-effector) tip and the environment

surfaces. The local dynamics of the contacting surfaces are represented, as recommended

by Johnson [62], by a contact region mass on a damped, linearly elastic foundation for each

surface. The deflection of the combined contact mass from its location at the instant when

contact is made represents the local compression of the surfaces. Note that, while the local

compression of the manipulator tip surface in general has a component along the axis of the

last link, the links are assumed to be stiff in that direction and the effects of global axial

compression of the links are ignored.

The expressions for the contact force components as a function of the generalised coor

dinates of the model will now be derived by considering the geometry of the system and

the forces acting on the contact region masses. These expressions are necessary to the sim

ulation studies in order to permit the contact force between the link tip and environment


y€

I

x

Figure 2.5: Manipulator—environment model geometry for derivation of contact force expressions.

Ijkev

sym= m+ me

yw

b5

xw ox

Chapter 2. Manipulator Modelling

mpc

50

k5(e— ô—x)

b5(—x1)

kex(E ô X)

bex’x

b5(— *tp)

(a) Total contact mass

Figure 2.6: Free body diagrams for the x direction contact region mass.

surface masses to be calculated from the results of the integration of the equations of mo

tion. Furthermore, they are used, in simplified form, to devise the initial controller model

coefficients for the force control algorithm in Chapter 5.

Based on the free body diagram shown in Figure 2.6a a force balance for the total contact

mass, m = m3 + me, in the x direction yields

më = —k9 (e1, — —— ( — Sx

— w) — b3 (r — — bexE. (2.53)

Considering each of the surfaces separately, with the x direction contact force, F, acting

equally but in opposite directions on each contact mass as shown in the free body diagrams,

k5 (E— — xtjp

F F

(b) Manipulator tip contact mass

kex(Eôx)<w)

b ex

(c) Environment contact mass


a force balance on m3 (Figure 2.5b) gives

— F — k3 (e — — x) — b3 ( — = m3 (2.54)

and a force balance on me (Figure 2.5c) gives

F — kex (e — — x)— = meêYx. (2.55)

Solving equation (2.53) for ë and substituting the results into equation (2.55), noting that

=— 8x —

x, yields:

F = (kexx+ bexx)+ x.. x)j. (2.56)

A similar procedure applied in the y direction yields the expression for that component of

the contact force, F:

= (keyEy + beyy) + [ksy (Ytip — Yw — e) + b (th —ay)]. (2.57)

‘rn

Expressions for the world frame manipulator tip position (x2 and y) and velocity (i1

and !‘tip) components which appear in equations (2.56) and (2.57) were derived as part of

the kinematic analysis in section 2.3 (equations (2.34) and (2.35)).

When the tip of the manipulator is just contacting (but exerting no force on) the en

vironment the following geometric relationship exists among the link lengths, joint angles

and the location of the environment:

= L1 cos i9 + L2 cos ( + 8) = L1c + L2c2

and Yw = L1 5111 8 + L2 sin (8’ + 6) = L1s + L2s2, (2.58)

where O and O are the particular values of the joint angles at which contact occurs.


2.5.1 Hertz Contact Model

The values of the contact region masses, m5 and me, stiffnesses, kex, key, and k5, and

damping coefficients, bex, bey, b3 and b3, in this lumped parameter contact model must be

related in some way to the material properties of the surfaces in contact. For instance, an

aluminium end-effector in contact with a steel assembly jig will have much higher stiffness

values and lower mass values than a rubber cover tool contacting a wooden surface. The

solution for the deformation of contacting spherical surfaces, derived in 1882 by Hertz,

provides some insight into selecting contact mass and stiffness values based on the physical

properties of the materials in contact. It must be noted that the Hertz theory is based on

the assumptions that the contact occurs between surfaces of non-conforming shape and is

quasi-static. As a result, it can only be used as a guide in the selection of coefficient values

for the more complex, dynamic contact case being modelled here.

One result of Hertz’s analysis is that the decrease in distance, 8, between the centres of

two spheres held in contact by a force F is [63]

2 1 1 1_v2 1—vaS = F3 {D2 ( + where D 1

+ 2 (2.59)

is the inverse of the effective planar strain modulus of the materials, v1 and v2 are the

Poisson’s ratios of the materials, E1 and E2 are the moduli of elasticity and R1 and R2

are the radii of curvature of the spheres. Equation (2.59) can easily be expressed as a

force/compression relationship:

F = KS, (2.60)

where(R8) R1R2

K= and R=D

In this expression R is the effective radius of curvature, that is, the contact between surfaces


of curvature R1 and R2 is equivalent the contact between a surface of curvature R and a

flat surface [64].

Note that, unlike in Hooke’s Law for a linear spring, the stiffness in equation (2.60) is a

function of the square root of the compression, that is, the stiffness of the material increases

the more it is compressed. Given the restrictive assumptions under which equation (2.59)

was derived and the fact that in elastic contacts between hard materials the surface deflec

tions must be small, the contact stiffness coefficient values used in the present research were

obtained by approximating equation (2.60) by its tangent at an appropriate nominal level

of contact force. Details of the calculation of the coefficients for specific combinations of

materials are included in Chapter 5.

It follows from the Hertz theory [63] that the area of contact between spherical surfaces

is a circle of radius

a (FDR). (2.61)

On the basis of this, a first order estimate of the volumes of material moved by the defor

mation of the surfaces is that they are equal to the segments of a sphere of radius R and

heights S and 62, that is,

= 7rS (R— -6) and V2 7r6 (R — s2) . (2.62)

The deflections of the individual surfaces, 6 and 62, sum to the total deflection given by

equation (2.59) and are in inverse proportion to the planar strain moduli of the surface

materials. These volumes, combined with the densities of the contacting materials, give

the mass of material on each surface that is involved in the contact deformation and thus

provide a basis for the selection of values for the contact region masses, m5 and me.


2.6 Voltage Controlled DC Electric Motor Actuator Model

In large, flexible manipulator systems such as the Space Shuttle Remote Manipulator Sys

tem (SSRMS), or Canadarm, and the proposed Mobile Servicing System (MSS) and Special

Purpose Dextrous Manipulator (SPDM) for the Space Station Freedom, the links are driven

by direct current electric motors at each joint. A typical DC motor actuator system, con

trolled by armature voltage, consists of:

• a signal amplifier which raises the control input voltage signal to a level suitable to

drive the motor armature;

• a current amplifier which produces the current through the motor armature;

• the motor itself with its inherent armature resistance and inductance, rotor inertia

and mechanical as well as electrical damping; and

• a gear box which raises the motor torque to the level required to drive the link.

Figure 2.7a shows a block diagram of such a typical DC motor actuator system.

The mathematical model of an armature voltage controlled DC motor is as follows [65j.

With reference to the armature circuit diagram in Figure 2.7b, the sum of voltage drops

around the armature circuit is:

Va = Raja + Laja + Vb, (2.63)

where Va is the armature voltage, derived from the control input signal, Ra is the electrical

resistance of the armature windings, a is the current through the armature windings and

La is the inductance of the windings. The back emf, vb, a feedback voltage proportional to

the armature speed, 0a, is a result of the rotation of the motor armature in the magnetic

field induced in the stator windings;

Vb KemfOa, (2.64)


(b) Armature circuit

Figure 2.7: Schematic of direct current motor actuator system.

(a) Block diagram

+


where Ke is the back emf coefficient. Equation (2.63) can be rewritten as an ordinary dif

ferential equation for the armature current in terms of the motor parameters, the armature

speed and the applied armature voltage, the latter being the forcing function:

Laja + Raja Va — Ke,6a. (2.65)

Equation (2.65) constitutes the electrical model of the motor. Mechanically, the motor

is represented by an armature inertia, ‘a a frictional damping, ba, proportional to the

armature speed, and the load torque, TL, at the motor side of the gear box. A torque

balance for the motor yields:

1aa Tm — TL — baOa.

The torque produced by the motor, Tm, iS proportional to the current in the armature

windings, or

Tm = Kra, (2.66)

where K.,. is the motor torque coefficient. Thus, the mechanical model of the motor as

viewed from the motor side of the gear box is

TL = K.rja — Ia&a — baa. (2.67)

The gear box increases the torque by a factor Ng, the gear ratio, at the expense of decreasing

the shaft speed by the same factor such that

= N9TL and = jIa.

Thus, when viewed from the link side of the gear box at joint k the mechanical equation of

the motor is

Tk N9Krja — Ng2 (2ak + bask) . (2.68)

This actuator model is coupled to the rigid body equation of motion for the link being

driven by adding the inertia, NIaOk, and damping, Nba8k, terms from equation (2.68)


to the corresponding terms in the rigid body equation and by using the geared up motor

torque, NgKTia, as the joint torque, Tk. In order to calculate this joint torque the differential

equation for the armature current (2.65) must be solved simultaneously with the equations

of motion since it involves the armature speed which is proportional to the joint rate.

Section A.7 shows an example of how the coupling is implemented for the equations of

motion for a single flexible link in free motion driven by a DC motor.

This linear model of an armature voltage controlled DC motor actuator is widely used

and accepted [65]. The signal and current amplifiers are components of such an actuator

system that are often overlooked in modelling. They are important not only in that they

are part of physical actuator systems but also because they are the source of the dominate

nonlinear feature of the actuator system, saturation. The voltage drop which can be applied

across the motor armature is limited as is the current which the motor may draw. Figure 2.7a

shows the amplifiers as linear devices with limits and they are modelled in the TWOFLEX code

in much the same way; if the calculated values of voltage or current exceed the saturation

limits the values are clamped at the limits. The actuator model neglects features of the

motor dynamics such as non-viscous friction and torsional flexibility in the shaft and gear

head. These features are ignored in order to focus on the control issues which arise due to

the structural dynamics of the manipulator links.

Section 5.6 presents results of several simulations of maneuvers of a prototype Space

Station Mobile Servicing System (MSS) arm modelled as a two flexible link manipulator

with DC motor actuation.


2.7 Summary

This chapter has presented the derivation of models for the kinematics and dynamics of

a planar two link manipulator with flexible links making contact with a deformable en

vironment. Also derived are expressions which relate the contact force to the generalised

coordinates of the dynamics model and relationships between the physical properties of the

materials in contact and the parameters of the contact dynamics model. The derivation

of the contact dynamics model is the primary contribution of the chapter in that, unlike

the models used in previously published robotic force control research, the model includes

the effects of inertia and damping in the local region surrounding the point of contact, in

addition to the contact stiffness. Results from the theory of elasticity, based on the work of

Hertz, indicate that the linear contact dynamics model used here is an approximation to the

more complex situation in which the contact stiffness and the mass of the region deflected

during contact vary nonlinearly with the contact force. However, the Hertz model is also

limited by its tacit assumptions of non-conforming surface shapes and quasi-static contact.

Chapter 3

Control Algorithm

3.1 Introduction

This chapter presents derivations and discussions of the various parts of the overall force

control strategy that is the focus of the research. At the heart of the control strategy is a

multivariable, receding horizon, adaptive, long range predictive control algorithm. Devel

oped with the primary goal of providing stable, accurate tracking control of desired levels

of contact forces between the manipulator tip and its environment, the algorithm can also

be used to control the gross motions of the manipulator when it is not in contact.

In the early stages of the research several controller designs were investigated before long

range predictive control was selected. The complexity of the nonlinear, coupled dynamics

of a flexible manipulator and environment system make conventional PID controller and

compensator designs impractical. Some of the difficulties with conventional designs can be

overcome with the use of gain scheduling but to do so requires very complete knowledge

of the plant dynamics over the operating range, including the time scales of characteristics

which are likely to necessitate gain changes. State feedback techniques are better suited

to complex dynamic systems. A pole placement tracking controller was designed for force

control of a single flexible link and was successful with the significant exception of its

inability to remove steady-state errors from the response. A similar deficiency was found to

limit the use of linear quadratic optimal state feedback. A scheme to use an adaptive inverse

59

Chapter 3. Control Algorithm 60

dynamics model of the system to provide state feedforward for the removal of steady-state

offsets was devised but the robustness of such a scheme is questionable.

The long range predictive control design that is developed in this chapter has the ad

vantage of possessing inherent integral action to yield a steady-state error free response.

Setpoint tracking is included directly in the formulation, the resulting control law is op

timal in the sense of minimising the sum of squares of the difference between the future

setpoint levels and the predicted output levels, and the algorithm is easily implemented in

an adaptive, digital controller form. Predictive control is well suited to deal with systems

exhibiting an abundance of lightly damped modes and non-minimum phase characteristics

as is the case in flexible manipulator force control. The inclusion of the expected future

effects of these characteristics in the controller cost function results in a control law which

produces more effective compensation than one based solely on instantaneous sensor data.

The Generalized Predictive Control (GPC) algorithm of Clarke, et al. [29] incorporates

many of the beneficial features of earlier predictive algorithms. It is a relatively mature im

plementation of the concepts of predictive control which has sparked a great deal of interest

and ongoing study of its properties and potential applications.

The dynamics of a flexible link manipulator in contact with deformable environment

present several challenges to the design of a force control algorithm. The dynamic behaviour

is highly coupled, nonlinear, stiff, discontinuous, and nonminimum phase. To deal with the

strong coupling, the predictive controller design is based on a multivariable model that

includes all of the open ioop system modes and their interactions. Nonlinearities arising

from the system dynamics are dealt with by making the controller adaptive, that is, an

instantaneous linear model of the system is used and the coefficients of the model are

updated regularly, on-line, to reflect the changes in the system due to its nonlinear nature.

The open loop system is, in most configurations of practical importance, stiff, in both

physical and mathematical senses. The natural frequencies of the open loop modes can easily


span a range of several orders of magnitude in the case of a highly flexible manipulator in

contact with a metallic surface. This characteristic of the system causes difficulties in the

numerical integration of the equations of motion, as will be discussed in Chapter 4. It also

leads to the coefficients of the instantaneous linear model, on which the controller is based,

being sensitive to small changes in the system configuration. As in the case of nonlinearities,

this difficulty is overcome by using an adaptive control approach.

Contact force control is a discontinuous problem due to the fact that the contact force

can, by definition, only take on either positive or negative values (depending on the orien

tation of the reference frame in which it is measured). When the manipulator tip looses

contact with the environment surface the contact force is zero and the error signal available

to the controller is independent of the manipulator state. While it is possible that, even

with such a degenerate error signal, the controller will successfully return the manipulator

tip to the environment surface and continue stable control of the contact force, it is more

likely that the system will become unstable with manipulator tip repeatedly bouncing on

and off the surface. To avoid this situation the controller is designed to switch from a force

control mode to a motion control mode when the contact is unexpectedly broken. The

objective of the controller is then to return the manipulator tip to the surface and switch

back to force control mode. The switching of modes between force and motion control is

governed by a contact control logic block at a level above the force and motion controllers in

the manipulator control hierarchy. Two operational strategies for the contact control logic

are discussed and demonstrated in Chapter 5.

The structural flexibility of the manipulator links makes the contact force response of

the system nonminimum phase. Physically, a nonminimum phase system is one for which a

positive step input produces a response which is initially negative and subsequently reverses

sign. In free motion, the tip of a flexible link subjected to a step torque input oscillates about

the path that would be described by the tip if the link were rigid. Due to the inertia of the


link, the tip initially lags and the oscillation is initially negative. This characteristic carries

over into the contact force response when the link is acting on a surface. Mathematically, a

nonminimum phase system is characterised by the presence of unstable zeros in the transfer

function numerator. Predictive control is well suited to dealing with nonminimum phase

systems because the prediction horizon can be extended to include the nonminimum phase

portion of the response and it can thus be compensated for in the calculation of the control

inputs.

Section 3.2 presents a review of the single input, single output (SISO), Generalised Pre

dictive Control (GPC) algorithm of Clarke, et al. [29,32], a discussion of the control law

structure and a derivation of the closed ioop system characteristics. The development of

a multivariable predictive control algorithm based on GPC is presented in section 3.3. A

new extension of the algorithm with a static equilibrium bias term that takes advantage of

available a priori knowledge of the dynamics of force controlled manipulators is presented in

section 3.4. This extension is an original contribution of this thesis. The parameters of the

predictive control algorithm and their effects on the shaping of the closed ioop response are

discussed in section 3.5. A discussion of parameter estimation, focussing on the Exponential

Forgetting and Resetting Algorithm (EFRA) is presented in section 3.6, and a description

of the structure and logic of the complete adaptive multivariable force/motion controller in

section 3.7. The chapter concludes with a discussion of stability and convergence charac

teristics of the adaptive, predictive control algorithm (section 3.8).

3.2 Review of SISO GPC Algorithm

This section briefly reviews a nonadaptive, single input, single output (SISO) long range

predictive control algorithm, known as the Generalized Predictive Control (GPC) algorithm,

developed by Clarke, et al. [29,32]. This discrete time algorithm yields a control law with


inherent integral action and robustness to inaccuracies in both model order and time delay.

The algorithm is based on prediction of the plant output for a series of future time steps.

The predictions are expressed in a form which allows a solution for an optimal (in the sense

of minimisation of a quadratic cost function) set of future control inputs that will minimise

the error between the predicted plant output and the desired setpoint. This procedure is

conducted in a receding horizon context, that is, a new set of control inputs are calculated

at each time step and only the first member of the set is used. Receding horizon predictive

control predates GPC in the process control literature but, as the name suggests, the GPC

algorithm is a generalised form of the method. Indeed, it has been shown that, depending

on the choice of parameters, the GPC algorithm results in control laws identical to a variety

of well known algorithms [73—75].

The plant to be controlled is modelled in the discrete time domain by an input/output

representation of the form

A(q’)y(t) = B(q’)u(t — 1) + , (3.1)

where A(q’) and B(q1)are polynomials, of degree na and nb respectively, in the backward

shift operator q’:

A(q1) = 1 +a1q’ +a2q2 + . .. + anaq,

B(q1) = b0 + b1q’ + b2q2 + ... +

where q1x(t) = x(t — 1),

y(t) is the time series of plant output values and the plant input time series is u(t). The

last term in equation (3.1) is a disturbance term consisting of (t), an uncorrelated ran

dom sequence and L 1— q’, the differencing operator. This model assumes that the

disturbances to which the plant is subject can be modelled as steps of random magnitude


occurring at random times [76]. Using the Diophantine identity [77],

1 = E(q1)A(q’)z + qF(q’), (3.2)

the plant model can be manipulated to yield a j—step ahead predictor of the plant output:

y(t + j) = Fj(q’)y(t) +E(q1)B(q1)L\u(t + j — 1) + E(q1)(t + j). (3.3)

The polynomials E3(q’) and F3(q1)in equation (3.2) are completely defined given A(q’)

and j, and are unique provided that [78]:

deg(E,) j and deg(F,) <deg(A).

The coefficients of E+1 and F3+1 are calculated from E, and F, with minimal computational

effort using simple recursion formulae [29]. The predicted output y(t + j) is the sum of

filtered time series of the output, the input and the disturbances. Given the fact that the

degree of E3 is, at most, j — 1 and the assumed random nature of (t), the contribution

to y(t + j) of the disturbance sequence is not only unknown but unknowable. Hence, the

optimal predictor of y(t + j) given the information available at time step t is

(t + jt) = F,y(t) + Gu(t + j — 1), (3.4)

where G3= E3B = gjo+gjlq1+...+gj(nb+j_flq(flb+I 1)• The optimal prediction (t+j It)

can be written as a term, f(t + j), which consists entirely of past inputs and outputs (in

effect, the free response of the system) summed with terms due to the future control input

increments which are yet to be determined.

Defining N2 as the prediction horizon and considering predictions at each of the next

N2 steps into the future the following vectors are formed:

= + 1 t) (t +2 It) ... (t + N2It)]:

(3.5)

[ut u(t + 1) ... u(t + N2 — 1)] , and (3.6)

r = [f(t+1) f(t+2) ... f(t+N2)]T,

(3.7)


where

f(t + 1) = Fi(q’)y(t) + (Gi(q’) gb) Au(t)

f(t + 2) F2(q1)y(t) + q (G2(q’) g2lq — g2o) Au(t)

f(t + N2) = FN2(q’)y(t) + qN2 (GN2(q’)— gN2(N2-1)q’ — ... — gN2o) 1u(t).

(3.8)

The set of N2 prediction equations of the form of equation (3.4) are written in matrix form

as

5’ = G’ü + f, (3.9)

where G’ is an N2 x N2 lower triangular matrix whose elements are the coefficients of the G3

polynomials from equation (3.4) corresponding to the yet to be determined future inputs.

A quadratic cost function, J1, is defined:

J1 = (5’_w)P(5’_w)+AuTu

= (3.10)

The vector

[wt + 1) w(t + 2) ... w(t + N2) (3.11)

is a sequence of future setpoints, hence, the first term of this cost function is simply the sum

of squares of the errors between the predicted plant output and the desired setpoint at each

of the next N2 steps into the future. The second term is the sum of squares of the future

control input increments (i.e., the incremental control energy), weighted by a constant, ).

Minimising J1 with no constraints on future inputs yields the control law

u = (c’’ + AI)’ G1T(w— f) (3.12)


which will track the specified setpoint sequence with zero steady state error. Note, from

equation (3.6), that the first element of ii is Liu(t) which, given the receding horizon method

of applying the control law, is the only input increment which will be used.

This control law yields an offset free response but is subject to certain restrictions and

deficiencies, not the least of which is the computational load of inverting the N2 x N2 matrix

(G’ TGI + at each time step, as is required in an adaptive implementation due to the

updating of the plant model from which G’ is derived. To improve the robustness of the

algorithm, and at the same time reduce its computational load, the concept of a control

horizon [79] is introduced [29]; that is, after some number of time steps, N< N2, the control

input level is held constant. N is the control horizon. Imposition of this condition results

in the vector ii being truncated from N2 to N elements and the left most N2 — N columns

of the matrix G’ being truncated to yield the N2 x N matrix G. The resulting control law

has the same form as (3.12) but with G substituted for G’:

u (G’G + i)’ GT (w — f). (3.13)

This control law is less computationally intensive than equation (3.12) because it requires the

inversion of an N x N matrix instead of an N2 x N2 one and N is generally chosen much

smaller than N2. Furthermore, the structure of the non-triangular matrix G guarantees

that GTG will be nonsingular. It is the inclusion in the GPC algorithm of the control

horizon concept that is at the heart of its ability to control difficult plants like flexible

link manipulators which exhibit nonminimum phase characteristics and lightly damped or

poorly modelled modes.

3.2.1 Control Law Structure and Closed Loop Characteristics

While equation (3.13) gives the control input increments in a form suitable for implementa

tion in a digital controller or simulation, insight into the underlying structure of the control


law and the closed loop characteristics of the controlled system can be obtained by further

manipulation of equation (3.13). Firstly, note that the quantity (GTG + \I)1

GT is sim

ply an N x N2 matrix of scalars, in effect a gain matrix which we will designate as K for

convenience, and hence the control law becomes

üz=K(w—f). (3.14)

From equation (3.8) we note that the vector f is the predicted free responses for the

next N2 time steps and that these predictions are composed of contributions from the past

members of the output time series, y(t), and the time series of input increments, Au(t),

known at the present time step. Defining the following vectors of polynomial operators in

q’:

Fi(q’) Gi(q)— 910

F2(q) q (G2(q) g2lq — 920)and g= , (3.15)

FN2(q) qN2l (GN2(q)— gN2(N2—1)q’ — ... — gNo)

the vector f can be written as f = fy(t) + Au(t) in which each term is a vector of

polynomials in q operating on a sampled time series. in order to write ii and w in

the same form we define the vectors

1 q

q q2q,= and (3.16)

qNul qN2

such that ii = qLu(t) and w = qw(t). Thus the control law can be rewritten as

qL\u(t) = K (qww(t)—

y(t) — Au(t)) (3.17)


or

y

(q + Kg) xu(t) = Kqw(t) — Kfy(t).

This is a set of N difference equations of the familiar [78] form

R(q’)/Xu(t) = T(q’)w(t) — S(q’)y(t).

(3.18)

(3.19)

Combining this result with the difference equation (3.1) which describes the open ioop

dynamics we see that the block diagram of the closed loop system is as shown in Figure 3.1.

The overall closed loop transfer function is obtained by combining equations (3.18) and

(3.1) so as to eliminate u(t):

[(qu + Kg) LA + KfBq’] y(t) = KqBq’w(t). (3.20)

Due to the structure of q all but the first of the N difference equations in this expression

are non-causal. However the receding horizon implementation of the control law implies

that the closed loop system will have the characteristics given by the first (causal) equation,

which can be written as

[R\A + SBq’] y(t) = TBq’w(t). (3.21)

The characteristic polynomial of the closed loop system is the bracketed quantity on

the left hand side. The closed ioop poles in the discrete time domain are obtained by

w

Figure 3.1: SISO GPC closed loop system block diagram


finding the roots of this polynomial and the closed ioop characteristics (natural frequencies,

damping ratios, time constants) of the system in the continuous time domain can be found

by using well known [80] relationships between the discrete and continuous time domains.

The closed ioop characteristics which result from these calculations are an approximation

of those exhibited by the long term response of the system due to the fact that the control

law is implemented in an explicitly adaptive, receding horizon context. Nonetheless, it

will be shown in Chapter 5 that these approximate characteristics are useful in selecting

the controller parameters (horizons and weighting factors) which yield the most desirable

response from among the set of parameters which stabilise the system.

The foregoing formulation of the control law structure and the resulting expression for

the approximate characteristic polynomial of the closed loop system is the result of extension

and formalisation of the numerical example in an appendix of Clarke, e al. [29]. It differs in

both form and intent from those in the literature [32,33,81]. Other authors have generally

expressed the closed ioop structure of the system in a state space representation, in contrast

to the transfer function form of equation (3.21). Their objective in so doing has been the

investigation of asymptotic stability conditions, comparison to other algorithms, etc. The

goal of the analysis presented here is to provide as direct a means as possible of assessing

the effects of the controller parameters (horizons and weighting factors) on the closed loop

performance of a given system. While the relationship among the parameters and the poles

of equation (3.21) is admittedly nontrivial, the numerical calculation and assessment of

those poles for a wide range of parameter values is straightforward.

3.3 Multivariable Predictive Control Algorithm

The algorithm derived in the preceding section provides excellent force and motion control

for a single manipulator link (as will be demonstrated in Chapter 5). Such a system involves


a direct relationship between one input (the joint torque) and one output (the joint angle

or contact force). For a multiple link manipulator with torque producing actuators at

each joint a more complex situation exists in that several output quantities (contact force

components, joint angles, tip position or velocity components, etc.) can, in principle, be

controlled and each input contributes to each output. While the situation sometimes exists

in which the coupling among the various inputs and outputs is weak and the system can be

controlled by multiple single input, single output feedback ioops, this is generally not so in

the case of manipulators with flexible links. Successful control of these systems requires a

control algorithm in which the effect of all inputs on each of the outputs is accounted for.

This is referred to as multivariable or multiple input, multiple output (MIMO) control.

The primary reference papers for the GPC algorithm (Clarke, et cii [29, 32]) make no

mention of the extension of the algorithm to the multivariable case. It was not until the

appearance of the Clarke and Mohtadi paper [33] over two years later that my attention

was drawn to the multivariable derivation presented in Shah, et al. [82]. Thus, the generic

multivariable predictive control algorithm presented below was derived independently of,

but has a similar structure to, that which has appeared in the literature. In the course of

that derivation it became evident that the extension of the algorithm with a new, static

equilibrium bias term (derived in section 3.4) would provide performance improvements

in manipulator contact force control applications. The formulation of the discrete time

domain closed loop characteristic polynomial derived in section 3.2.1 and extended to the

multivariable case in section 3.3.1 differs from the formulations of the closed loop structure

previously reported in the literature [32, 33,81]. The formulation derived here is shown to

be particularly useful for tuning the control algorithm in specific applications.

For the purpose of deriving a multivariable predictive control algorithm based on the

GPC algorithm reviewed in section 3.2, consider first a general multivariable system in the


continuous time domain, described by

Y(s) = H(s) U(s), (3.22)

where Y(s) is an n0xl vector of outputs, U(s) is an n1xl vector of inputs and H(s) is an

no x n matrix of transfer functions, all transformed to the Laplace domain. Equation (3.22)

can be written as a system of n0 linear algebraic equations:

Yj(s) = Hk(s)Uk(s), i = 1,... ,n0. (3.23)

Each of the H2k(s) terms is a transfer function in the form of a ratio of polynomials in

the Laplace operator, s. Supposing that these transfer functions can be transformed from

the Laplace (continuous time) domain to the Z (discrete time) domain, the system can be

described by a collection of difference equations in the backward shift operator, q1:

A(q’) y(t) = Bk(q’)uk(t — 1), i = 1,.. . ,n0, (3.24)

where y(t) is the time series of values of the ith output, Uk(t), is the time series of values

of the kth input, and where A(q’) and Bk(q) are polynomials in q:

A(q’) = 1 +a1q’ +a2q2 + . . . + anaq,

Bk(q’) = b0 + bIk1q’ + bk2q2+ ... + bkflq

Note that in inertially coupled systems like robotic manipulators the denominators of all

of the H2k(s) transfer functions are identical and thus, upon transformation to the discrete

time domain, only a single A(q’) polynomial is required. If disturbance terms, d(t),

modelled as randomly occurring steps of random amplitude:

d(t)= t),

where A = 1— q1, (3.25)

and 4(t) is a random, uncorrelated sequence, are added to equation (3.24) the system model

is analogous to the single input, single output CARIMA (Controlled AutoRegressive and

Integrated Moving Average) model [76] used in section 3.2.


From this point the derivation of the optimal output prediction equations is very similar

to that described in section 3.2, except that we are now considering each element of a set of

output equations, {y, i = 1,... , n0}, that depends on several inputs, {uk, k = 1,. . .

rather than a relationship between a single input/output pair. With this in mind, the major

results for the multivariable derivation can be stated as follows:

• Diophantine identities:

1 = E(q’) A(q1)+ q3 F(q’), i = 1,.. . , n0, (3.26)

• Optimal j-step ahead output predictors:

th(t + jt) = F(q’)y(t) + G(q’)(t + j — 1), i 1,. . . ‘no, (3.27)

where Gk(q’) = E2(q’) Bk(q’) and Uk(t + j — 1) tUk(t + j — 1).

Note that for each of the j steps in the future (t + j t) is composed of a series of filtered

past outputs and input increments which are known at time step t and a series of future

input increments which are yet to be determined, that is:

(t + 1 It) Fi(q1)y(t) + [G(q’)— g1k0j Uk(t) +gk0k(t),

i—

(t + 2 It) = F2(q’)y(t) + [G(q)— g2k,q — gi2] qük(t)

+ (gi1q’ + g2k0) Uk(t + 1), i 1,. . . ‘no,

etc. (3.28)

At this point a control horizon, N, analogous to that defined in section 3.2 is introduced,

that is, it is assumed that after some number of time steps, N < N2, the control inputs will

be held constant. Introduction of this control horizon has similar benefits to those observed


in the SISO case, namely, the computational complexity of the algorithm is significantly

reduced and the robustness is improved. The optimal output prediction equations for N2

steps into the future, subject to an N step control horizon, can be written in matrix form

as:

= +, i = 1,... ,n0, (3.29)

where Sri=

+ 1 t) (t + 2 t)...

+ N]T

Uk= { u(t) u(t + 1) ... u(t + N — 1)]

9ilko 0 ... 0

912k1 912k0 . . . 0G3k = . . . , and

gN2k(N_l) g2N2k(N2) . . . g2N2k0

Fi(q1)y1(t)+ [G(q’)— gllk0lUk(t)

F2(q’)y1(t)+ [G,(q) — g,.’ciq1 —g,0]qLUk(t)

f1= k=1

FN2(q’)y(t) + [G1k(q’)— g1N2k(1)q__1) —...

— giN2ko} q_)Auk(t)

Note that each element of f. consists only of contributions from the past (i.e. known) inputs

and outputs, filtered by coefficients derived from the known system model. As a result, the

elements of f1 can be viewed as predictions of the free response of the ith output if no further

control action were applied.

Defining

= [w + 1) w1(t + 2) ... w1(t + N2)], i = 1,... ,n0, (3.30)


as the set of vectors of setpoints for each of the outputs for the next N2 time steps, the

vector, ê, of optimal predictions of the output errors is:

e. =

i= 1,...,n0. (3.31)

The cost function, J2, which will be minimised to yield the set of optimal control input

increments is:

+ ‘c > U ii. (3.32)

Minimisation of this cost function will minimise the sum of squares of the optimal predictions

of the output errors. At the same time, the energy of the control input increments is

minimised to an extent that depends of the value of ). The control energy is represented

by the second term in equation (3.32) (the sum of squares of the control input increments).

The expansion of equation (3.32) is shown in section B.1. Partial differentiation of that

result with respect to Urn, an arbitrary member of the set of control input increment vectors,

gives:

= 2 > G Gk Uk + .icUm + C (f, — w)]. (3.33)Urn i=lk=1

The set of control input increments, {Um,m 1,.. . ,n}, that minimises J2 is given by:

0J2—=O m=1,...,n2. (3.34)GUm

Substituting into this expression from equation (3.33) and rearranging yields:

m=1,...,n (3.35)i=1 k—_i

This is a system of linear, algebraic equations which must be solved simultaneously to yield


the desired control input increments. Written in matrix form the system is:

G G1 ... G1 G11 G12 ... ii

+G G2 ... G2 G21 G22 ... G2 U2

G G’ . . . G1 Gn2 . . . Un

riT rT‘-11 ‘‘21 W1

— ii

G12 G ... G’2 w2—f2(3.36)

T ç‘1n 2n1 W,.0

—

The matrix of Gk terms can be considered as a composite matrix (that is a matrix, each of

whose elements are matrices), G, with the result that the control law can be written more

compactly as

ui w1—f1

U2 —1 w2—f2= (I+GTG) GT

:(3.37)

w0 — f,

which is recognizably analogous to the SISO control law, equation (3.13).

Each of the Urn vectors in equation (3.35) is N. x 1 and the w and f2 vectors are N2 >< 1,

thus the vector of Urn vectors on the left hand side of equation (3.36) is x 1 and the

vector of (w, — f1) vectors on the right hand side is n0N2 x 1. Correspondingly, each of

the Gzrn matrices in equation (3.35) is N2 x N and thus the product matrices G G2k

are N x N symmetric matrices. Hence, the left hand side matrix in equation (3.36) is an

n0N x n2N matrix and the right hand side matrix is n0N x n0N2. These dimensions of

the left hand side matrix imply that a unique set of control inputs exists only if the number

of inputs and outputs are equal. If the number of outputs exceeds the number of inputs the


system is underspecified and not all the the control objectives can be satisfied. With more

inputs than outputs the system is overspecified and additional criteria must be introduced

to obtain a unique set of controls. In the case of robotic control this means that the number

of force, torque or motion components that can be controlled is equal to the number of

actuators available.

3.3.1 Control Law Structure and Closed Loop Characteristics

The method used in section 3.2.1 to investigate the structure of the SISO GPO control law

can be extended, using composite matrices, to explore the structure of the multivariable

control law and the characteristics of the resulting closed ioop system. Replacing the com

posite matrix + GTG)1

GT in equation (3.37) by K, the control law can be written

in a form analogous to equation (3.14):

ü1 w1

=K: — :

(3.38)

w,

where the gain matrix K is an nN x m0N2 matrix of scalars (or, equivalently, an n x n0

composite matrix of N x N2 gain matrices).

The f vectors can be written as vectors of polynomials in q operating on sampled time

series by employing the vector f defined in equation (3.15) and extending the definition of

from the same equation to

I_v I, —1\‘-ii.q i Yzklo

—1 —1q (Gk2(q ) gk21q — gik2o)= (3.39)

qN2l (GjkN2(q_1)— — . . .

— gikN2o)


Thus

1 0 0 yi(t) Au1(t)

= 0 0 y(t) + Lu(t). (3.40)

fN2 0 0 yn(t) n1 g2 Lu(t)

Likewise, employing the vectors q and q, defined in equation (3.16), the vectors of iiks

and ws are written in the form of polynomials in q’ operating on sampled time series:

ii q 0 0 zui(t)

112 = 0 q, •.• 0 Au2(t)(3.41)

0 0 q.

and

q 0 0 wi(t)

W2 = 0 q 0 W2(t)(3.42)

w,, 0 0 q

Using these expressions the control law can be rewritten as

ui(t) wi(t) yi(t)

U2(t) W2(t) y2(t)(quI+KG)z =KqI KfI . (343)

u(t) w0(t)

This is a set of n1N difference equations.


With the difference equations (3.24) which describe the open loop dynamics of the system

expressed in matrix operator form:

yi(t) u(t)

A(q’)y2(t)

= B(q’)q1u2(t)

(344)

y(t) u(t)

where

B11(q1) B12(q’) . . . B1(q’)

B21(q1) B22(q1) ... B2(q)

Bi(q1)B02(q1) ... B0(q’)

the vector of y2(t)s can be eliminated to yield

ui(t) wi(t)

w2(t)(qui + KG) AA + KfIBq’]

:= AKqI

:(3.45)

u(t) w0(t)

This expression is a closed ioop transfer function relationship between the setpoints and the

control inputs. Manipulation of equations (3.43) and (3.44) to arrive at the more conven

tional closed ioop transfer function representation relating the setpoints to the outputs is

problematic because it involves the inversion of the B matrix of q’ polynomial operators.

Fortunately, because the inputs and outputs are linearly related by the control law, this

difficulty affects only the eigenvectors of the closed ioop representation; the eigenvalues of

the matrix on the left hand side of equation (3.45) are the poles of the closed ioop system.

The calculation of the approximate, continuous time domain, closed ioop characteristics

from these poles is shown in Chapter 5 to be a useful method of tuning the multivariable

control law.


3.4 Static Equilibrium Bias Term

Generic, linear input/output models are used to describe the systems on which the deriva

tions of the SISO GPC algorithm (section 3.2) and its multivariable counterpart (section 3.3)

are based. While this structure results in a control algorithm that is applicable to many

diverse systems, it is desirable to customise the control algorithm for a particular system

by using additional knowledge about that system and thereby improve the control. In the

case of control of contact forces between a robotic manipulator and its environment the

dynamic model of the system can be used to form a relationship between the desired force

levels (setpoints) and the actuator input levels required to maintain those forces when the

system is in static equilibrium. This information is introduced into the control algorithm by

augmenting the cost function with a term that weights the sum of squares of the deviation

of the actual total control input levels from the calculated static equilibrium levels. This

term is referred to as a static equilibrium bias term and its addition to both the SISO and

multivariable predictive control algorithms is an original contribution of this thesis.

The static equilibrium bias term acts as a proportional feedforward term in the control

law. This term has the beneficial effect of reducing the step response rise time but this

improvement comes at the expense of introducing a small amount of overshoot. Simulation

results discussed in section 5.3.4 illustrate these characteristics.

The function which maps the set of setpoint levels, {w, i = 1,. . . , n0}, at a given time

step onto the corresponding set of static equilibrium control inputs, {Ük, k = 1,. . . ,n}, is

obtained from the equations of motion of the system and the contact force model. Details

of the necessary manipulations of equations (2.47) through (2.52), (2.56) and (2.57) to

obtain the mapping function for the case of a two flexible link manipulator are shown in

Appendix C.


Defining vectors, analogous to the vector of future setpoints (equation (3.30)), for the

actual control inputs, Uk, and the calculated static equilibrium inputs, Uk,

= { u(t+ 1) uk( + 2) Uk(t + Nu)] k = 1,... ,n (3.46)

and

= [Ük( + 1) Uk(t + 2) tLk(t + N) k 1,. . . ,n, (3.47)

the static equilibrium bias term for the cost function is the sum of squares of differences

between Uk and

- T -

Jse >Z(uk_tlk) (Uk —Uk). (3.48)

This term is weighted by a factor, ,X, and added to the predicted output error and incre

mental control energy cost function terms given by equations (3.32) to yield the overall cost

function

J2 + .X8eJse

=+ cÜÜk + \se(Uk — uk)T (Uk Uk). (3.49)

The vector of control inputs, Uk, is formed from the (previously defined) vector of control

input increments, Uk, by adding the future increments to the current input value, u= Uk(t).

Defining i as a unit vector and L as a unit lower triangular matrix,

1 100...0

1 110...0IL= . and L=

1 1l1...1

Uk can be written as:

Uk = u + LUk. (3.50)


Substituting equation (3.50) into equation (3.49) yields:

J3 = : eej + \c Ü U + se (u H- Lfik Uk) + Lük — Uk). (3.51)

Since J3 is a linear combination of J2 and Jse the principle of superposition allows the results

of the expansion and minimisation of 18e to be added to the results given by equation (3.35)

to obtain the overall control law. Expanding Jse and differentiating with respect to Urn (an

arbitrary member of the set of control input increment vectors, see section B.2 for details)

gives:

(3.52)

The set of control input increments, {Um,m = 1,... ,n}, that minimises Jse is given by:

OJse--—=0, m=1,...,m. (3.53)oum

Applied to equation (3.52), this condition yields:

LTLIIm LT (m —uji.), rn= 1,...,n2. (3.54)

Adding this set of equations, weighted by .se, to (3.35) gives

>GnGikUk +AcUm+seLTLUm = >G (w _fj)+AeLT (Urn _UjL),irzl kr1 jzl

m=1,...,n (3.55)

an augmented system of linear algebraic equations which can be solved for {Um,m =

1,.. . ,n2}. These equations can be written in matrix form similar to equation (3.36):

G G’1 ... G11 G12 ... G11

G G ... G2 G21 G22 ... G2

... G02


LTL 0

0 LTL+.\3e

0

0

L

U1 -

U2

Un1 -

— fi

— f2

— fTl0

— 0U1 — U1,i

— 0U2 — U2L

Wi

W2

wna

0 0 ... L’

‘T rITLW11 ‘21

GT iT CIT12 ‘22 n02

-‘iT CIT T“1n1 ‘2n non

)iseLT 0 ... 0

0 AseLT... 0

0 0 ... .X3eLT

+ (3.56)

iin—

u1p

This linear system has the same dimensions as equation (3.36). The matrix LT is N x

N and the vector of static equilibrium biases on the right hand side is n2N x 1. The

requirement of an equal number of inputs and outputs for a unique solution also holds.

With the matrix of Gk terms represented by a composite matrix; G, equation (3.56)

can be rewritten as

w1—f1

w2—f2= K1 (3.57)

w0 — f — u,i -


where—1

LTL 0 ... 0

o LTL... 0K1 = )I + GTG +

: : :GT (3.58)

o o ... LTL

and

—1LTL 0 ... 0 LT 0 ... 0

o LTL... 0 0 LT ... o

K2A3e AcI+GTG+Ase . . . .

o o ... LTL 0 0 ... LT

(3.59)

Comparing this result to equation (3.37) it is evident that the control laws differ slightly in

the structure of the inverted quantity in K and K1. The more significant difference is the

addition of the second (K2) term in equation (3.57). This term make a contribution to the

control input which is proportional to the difference between the expected static equilibrium

inputs (u) and the current levels (u). The magnitude of the contribution is governed by

the value of ?. Thus, the term produces a proportional feedforward action in the control

law.

3.5 Control Algorithm Parameters and Tuning

The multivariable predictive control algorithm has four tuning parameters:

1. N2, the prediction horizon;

2. N, the control horizon;

3. ), the input increment weighting factor; and,


‘ae, the static equilibrium bias term weighting factor.

The prediction horizon specifies the number of time steps into the future for which

predicted system outputs will be calculated and included in the control law. Clarke, et

al. [29] recommend that N2 be set comparable to the rise time of the open ioop system.

The prediction horizon should also be far enough in the future to include the negative-going

part of a nonminimum phase open ioop response. Too short a prediction horizon causes

the controller to work with insufficient information and the control is likely to be poor or

even unstable. Increasing N2 causes the poles of the closed ioop system to move toward the

locations of the open loop poles.

The control horizon is the number of time steps in the future after which the control

input level will be held constant, that is, all future increments set to zero. This horizon

has significant effect on the computational load of the algorithm since the left hand side

matrix of equation (3.56) (which must be inverted) has dimensions n0N x n2N. Clarke,

et al. [29] recommend that N,. be greater than or equal to the number of unstable or poorly

damped zeros in the open ioop system. In the multivariable form of the algorithm the

control horizon should be at least equal to the number of inputs to the system. Increasing

N,. generally increases the magnitude and rate of change of the control input signals. To

a certain extent this increased activity leads to higher performance control but too large

a value for N,. may result in excessive ringing of the control signals and significant high

frequency components in the step response transients.

The input increment weighting factor governs the contribution to the cost function of

the sum of squares of control input increments term, that is, the incremental control energy.

With ) set to zero this term has no effect on the cost function or the control law. The

use of a nonzero has been found to introduce a pole on the negative real axis into the

continuous time representation of the closed loop system. This pole moves toward the origin


as is increased. In the context of contact force control this additional pole may have a

stabilising effect for the case of a rigid link manipulator but its interaction with the poles

due to other modes in the flexible link case generally results in a slower step response.

The static equilibrium bias term weighting factor controls the extent to which that term

influences the control law. A value of zero for Xse results in no static equilibrium bias.

Increasing Ase causes the control inputs to tend toward the calculated static equilibrium

values. Thus the static equilibrium bias term can be viewed as a proportional feedforward

term in the control law with a gain related to )se. The fact that the contribution of this

term to the overall control input signal is larger when the output is far from the setpoint

than when it is near means that the response will generally be faster as ) is increased.

However, as the results in Chapter 5 show, this performance improvement comes at the

expense of introducing overshoot into the response.

Clarke, et al. [29] include one additional parameter in the GPC algorithm: the delay

horizon, N1. This horizon serves to reduce the computational load of the algorithm in

situations where there is a delay between the application of a control input increment and

its effect appearing on system outputs. Such situations are common in process control

applications but in the case of robotic force control there is no delay, provided that the

control law calculations can be completed with a single time step, and N1 is set to 1.

In principle, a different prediction horizon value, N22, could be used for each output in

the multivariable algorithm. Similarly, independent values for N, X, and Ase for each input

could also be specified. The resulting algorithm is substantially more complex to implement

and no clear advantage can be seen in using multiple horizon and weighting factor values.


3.6 Parameter Estimation

As was noted at the beginning of this chapter the inherently stiff nature of the dynamics of

a manipulator in contact with its environment as well as the strong coupling and nonlinear

dynamic effects present a significant challenge in the design of a controller for contact

forces. This challenge is met, in part, by implementing the control algorithm in an explicitly

adaptive context, that is, the values of the coefficients of the polynomials in the controller

model (equation (3.24)) are estimated on-line from the time series of calculated control

inputs and sensed contact force outputs. This parameter estimation is accomplished by a

recursive least squares (RLS) algorithm.

The basic RLS algorithm [77] is described by a pair of update equations, one for the

vector of parameter estimates,9 and the other for the covariance matrix, P:

+ 1 k(Yk — ‘k-i) (3.60)

1) —

rki ((‘k ‘I’kIE k — ‘ k—i

— 1 .iT 0 > U,

I + Yj E k—i ‘Pk

where cbk is the regressor vector of input and output data values and yk is the value of

the system output at time step k. This algorithm has optimal convergence and stability

properties when the parameters to be estimated are time invariant. Unfortunately, the basic

algorithm is unsuitable for tracking time varying parameters because its gain converges to

zero. A large number of modified RLS algorithm have been developed to overcome this and

other limitations.

In the early stages of the research the U/D RLS algorithm of Bierman [83] was employed.

The U/D RLS algorithm factorises the covariance matrix into the product of an upper tri

angular and a diagonal matrix (U/D factorisation) and thereby ensures that the covariance

matrix will always be positive definite. Tracking of time varying parameters is facilitated

by use of an exponential forgetting factor to weight older data less heavily than new. The


effect of this weighting is to establish a nonzero lower bound on the minimum eigenvalue

of the covariance matrix and thereby prevent the occurrence of zero elements in the gain

term of the parameter update equation (3.60). Unfortunately, exponential forgetting can,

in the absence of sufficient excitation in the input data, lead to covariance wind-up; the

unbounded growth of the eigenvalues of the covariance matrix which leads to corresponding

growth of the elements in the gain term of equation (3.60).

All of the adaptive control results presented in Chapter 5 employ a newer modified RLS

algorithm developed, by Salgado, et al. [841, known as EFRA which stands for Exponential

Forgetting and Resetting Algorithm. As well as using the exponential data weighting nec

essary to track time varying parameters the EFRA algorithm also incorporates exponential

resetting of the covariance matrix in such a way as to give new data unprejudiced treatment.

Furthermore, the algorithm provides a means of establishing bounds on the maximum and

minimum eigenvalues of the covariance matrix, thus avoiding covariance wind-up. The

update equations for EFRA are:

=e +LePklbk

(Yk-

(3.62)

=— ePk_1kPk_1

+e1 SeP_i, (3.63)l+41Pk—1çbk

where ce, /3e, 8e, and ) are constants. Salgado, et al. [84] give the following guidelines for

the values of these constants:

• adjusts the gain of the least squares algorithm; typically e E [0.1, 0.5];

• /3e is a small constant which is directly related to the minimum eigenvalue of P;

typically/3e [0,0.01];

• 6e is a small constant that is inversely related to the maximum eigenvalue of P;

typically Se E [0, 0.011;


Xe is the usual exponential forgetting factor; typically A, E [0.9,0.99].

Figure 3.2 illustrates the superior performance of EFRA compared to the U/D RLS

algorithm. The system being identified is described by a simple moving average:

Yk auk + buk_l, where Uk = [i + sgn (sin k)], (3.64)

that is, Uk is a square wave centred about with a period of 100 time steps. The parameter

values change in time according to the following schedule:

k a b

[1,789] 0.1 0.2

[790,999] 0.2 0.4

[1000, 1489] 0.4 0.8.

The vector of parameter estimates is

=

T

with initial value 90= [ a b

and the regressor vector is

cIJk—_[Uk Uk_lj.

The constants for EFRA are

= 0.95, ae = 0.5, 13e = 0.005, Se = 0.005,

and for the U/D RLS algorithm the forgetting factor is ?u/d 0.95.

Figure 3.2 shows the evolution with time of the parameter values calculated by EFRA

(Figure 3.2a) and the U/D RLS algorithm (Figure 3.2b). Comparing the EFRA and U/D

RLS results, the most striking difference is the presence of spikes in the estimates calculated

by U/D RLS. These spikes are very poor estimates of the actual parameter values. Using

Chapter 3. Control Algorithm

3.5

3.0

2.5

0 2.0ci)i.5E4-.Cl)w .5

0ci)

.5

-2.0

-2.5

-3.00

89

.8

.7

U)C).

00 100 200 300 400 500 600 700 800 900 1000 1100 1200

Time Step

(a) EFRA parameter estimates

100 200 300 400 500 600 700

Time Step800 900 1000 1100 1200

(b) U/D RLS parameter estimates

Figure 3.2: Comparison of EFRA and U/D RLS parameter estimation algorithms.


those values in a controller model leads to inaccurate calculation of the required control

inputs which, in turn, yields poor control and perhaps even instability.

Finally, it should be noted that the update equations (3.62) and (3.63) for EFRA are

equally applicable to single input, single output systems as above and to multivariable

systems of the form of equation (3.24). For a multivariable system the vector of parameter

estimates and the regressor vector are augmented to include the coefficients and data from

each of the inputs:

= [ a1 ... anai b10 ... b1b1 ... bno ...

]T

(3.65)

and

ik= { Yk ... Yk—na Ulk ... U1(k_nbi) ... ...

(3.66)

At each time step the estimation algorithm must be executed once for each system output.

3.7 Adaptive Controller Implementation

The overall control strategy implementation is shown in Figure 3.3. This strategy allows

control of both the contact forces and the gross motions of the manipulator when it is not

in contact. In the latter case the joint angles or velocities are taken as the system outputs.

Due to the discontinuous nature of the system dynamics, the transfer function models used

in these two modes of control are necessarily distinct and of different order. The force and

motion models are identified and maintained in parallel so that switching between control

modes can be accomplished smoothly. The contact control logic block controls selection

of the appropriate (force or motion) control law, based on data from the force sensor and

joint encoders and the trajectory planner, as well as controlling whether or not the transfer

function models are updated by the parameter estimation blocks. The force and motion


Figure 3.3: Control strategy block diagram.


setpoint programs are assumed to be provided by the higher levels of the control hierarchy

which include the operator, a task planner and a trajectory generator.

With the parameter estimation blocks enabled, the controller is explicitly adaptive. At

each time step the coefficients of the transfer function model currently in use are estimated

using the Exponential Forgetting and Resetting Algorithm (EFRA) [84]. The estimated

coefficients are assumed to be instantaneously exact and the set of control input increments

for that time step is calculated. At the next time step a new set of coefficients is estimated

and used to calculate a new set of control input increments.

The performance of the overall control strategy in situations which necessitate switching

between force and motion control is discussed in section 5.4.

3.8 Stability and Convergence

Stability and convergence are important and difficult issues in the design of adaptive con

trollers. For a given set of values for the model coefficients the stability of the control

algorithm part of an adaptive controller can be considered. In the case of the CPC al

gorithm it has been shown [33] that, with a finite prediction horizon (N2), the resulting

closed—loop control is stable provided that:

1. the open—loop system model is stabilizable and detectable,

2. the delay (N1) and control (Nu) horizon are selected with respect to the number of

system model states (n) such that N = N1 n and N2 — N1 n — 1, and

3. the control weighting factor () is small (i.e. = —* 0).

It should be noted that this is only a sufficient (but not necessary) condition for which

stability of the algorithm can be proven. The GPC algorithm is demonstrably stable for

sets of parameters (N1,N2,Nu, and ?) other than those which satisfy the above conditions.


Given this result, the issue of stability of the adaptive GPO controller becomes one

of whether or not the system identification algorithm provides model coefficients which

describe a stabilizable and detectable open—loop system. The Exponential Forgetting and

Resetting system identification algorithm (EFRA) [84] discussed in section 3.6 is of the

recursive least squares (RLS) family. The asymptotic properties of this type of algorithm

are well established [85]. However, those asymptotic properties are concerned with the “best

possible predictor” for the system which the algorithm can determine. The relationship

between that predictor and the stabilizable and detectable model of the open—loop system

which is required for stability of the adaptive GPO controller is dependent on such factors

as the number of model coefficients and the system input.

In light of these factors, it is evident that an analysis of the dynamics of the system

to be controlled is necessary to select an appropriate number of model coefficients and to

select initial values for those coefficients which satisfy the stabilizability and detectability

requirements of the adaptive GPO algorithm. Augmentation of that analysis with a series

of identification experiments on the system would most certainly be beneficial.

39 Summary

This chapter presents the development of a manipulator contact force control strategy based

on an explicitly adaptive implementation of a predictive control algorithm. The control

algorithm is based on the Generalised Predictive Control (GPO) algorithm of Clarke, et

at. [29]. This form of predictive control algorithm was selected over other control methods

because it incorporates the following desirable features:

• inherent integral action;

• setpoint tracking;


• ease of implementation in explicitly adaptive, digital form, and;

• inclusion of non-minimum phase and lightly damped mode characteristics in predictor

model.

The primary contributions of this chapter are the extension of the generic predictive control

algorithm with a new static equilibrium bias term that allows knowledge of the system

dynamics to be employed to improve the controller performance, development of a new

formulation of the closed loop characteristic polynomial which is directly useful in controller

tuning, and introduction of a contact control logic level in the control hierarchy to deal with

the discontinuity involved in making and breaking contact between the manipulator tip and

the environment surface.

Chapter 4

Dynamics and Control Simulation — TWOFLEX

4.1 Introduction

The simulation computer program TWOFLEX incorporates the dynamic model of the pla

nar two flexible link manipulator and deformable environment derived in Chapter 2 and

the adaptive single- and multi-variable long range predictive control algorithms derived in

Chapter 3. The code is written in FORTRAN. It was originally developed on a VaxStation

3200 under the VMS operating system and subsequently ported to a Sun SparcStation

under SunOS. Both versions of the program use double precision arithmetic throughout.

Results of the runs are stored in text files as tabulated time series of related groups of vari

ables and as plots of the evolution of selected variables with time. The dynamics models

of the links, actuator, and contact regions are coded in such a way that the program can

easily simulate subsystems of the two flexible link manipulator system (e.g. a single rigid

or flexible link or two rigid links). Likewise, the controller structure to be simulated (i.e.

SISO or multivariable, adaptive or constant parameter, force, motion or combined control,

etc.) is also selectable. The particular configuration of dynamics model and controller to

be used for a given run is specified by a set of parameter values which are read from a data

file each time TWOFLEX commences execution.

This chapter begins with a description of the overall structure of the TWOFLEX program in

section 4.2. Section 4.3 presents the details of the numerical integration of the equations of

motion for the manipulator/environment system. The description of the program concludes

95

Chapter 4. Dynamics and Control Simulation — TWOFLEX 96

in section 4.4 with a discussion of the routines that simulate the discrete time control algo

rithm. The structure of these various parts of the program are illustrated with flowcharts.

A complete listing of the documented source code for the TWOFLEX program and the associ

ated subroutine libraries (approximately 18,200 lines of FORTRAN code in total) is available

in the UBC CAMROL (Computer Aided Manufacturing and Robotics Laboratory) Report

number R.92—2 [34]. Section 4.5 describes several tests which were conducted to validate

the operation of the program by comparison to a simulation of a multibody space station

based flexible manipulator developed by Mah [35]. The chapter concludes with a summary

of the features of the TWOFLEX code.

4.2 General Description of the TWOFLEX Program

A flowchart of the overall structure of the TWOFLEX program is shown in Figure 4.1. The

steps in the code execution, as represented in Figure 4.1, are described in the following

paragraphs.

The program begins with the reading and processing of data from the input files. These

data specify the configuration of the system for the run, the initial conditions, and, nu

merous logical flags to select various options in the code. The main input file for a typical

run consists of about 50 lines of data. The input routines use free formatting and permit

comments to be interspersed with the data by placing them on lines beginning with an *

character. Auxiliary input files contain the setpoint information for the controller. Process

ing of the input data includes calculation of various quantities needed in the simulation.

In particular, values of the mode shape functions and the corresponding inertia integral

factors, ‘mkt, for the equations of motion are calculated at this point.

Output initialisation involves opening of, and writing headings to, the various files which

will receive output data as the simulation proceeds. A summary of the configuration for


Figure 4.1: Flowchart of the overall structure of the TWOFLEX dynamics and control simulation program.

False


the run is written out at this point, both to serve as a reference when reviewing stored run

results and to provide a check on the validity of the input data.

Next, the initial state of contact is checked. If the initial conditions are such that the

apparent separation between the tip of the manipulator and the surface of the environment

is negative, contact exists and the components of the contact force are calculated. In the

case of positive separation, contact does not exist and the contact force is zero.

Following these initialisation steps, the main loop of the simulation begins. This loop

is controlled by the value of the integration time variable, t2, and proceeds in steps of dt.

The value of dt, is generally selected to be quite small since the main loop includes the

integration of the equations of motion which simulates the response of a continuous time

system. Within the main ioop, the integration time, is checked to determine if it is modulo

dtd, the discrete time step. Only if that condition is true are the discrete time control

algorithm routines executed.

The discrete time control algorithm consists of the contact control logic, the parameter

estimation algorithm and the control algorithm itself, which calculates the input signals to

be applied to the actuators. Section 4.4 describes these parts of the program in detail.

Flags in the input data file can be set to disable the execution of the control algorithm

and instead apply one of a set of open loop input signals to the system. The available open

loop input signals include impulse, step, ramp, square wave, sine wave and white noise.

These signals are useful in observing the forced, but uncontrolled, response of the system.

Regardless of the source of the input signal, the main loop continues with the integra

tion of the equations of motion over the time interval dt. The DGEAR routine from the

International Mathematical and Statistical Library (IMSL) [86] is used for the integration.

A discussion of the features of DGEAR and the details of the form of the equations which are

actually integrated is included in section 4.3.


After completion of the integration time step, the new state of the system is checked to

determine if the contact state has changed. If the manipulator tip is in contact with the

environment surface the components of the contact force are calculated. The new state of

the system and a collection of data calculated from it are written as text to the time series

output data files. Some of those values are also stored for later plotting.

Finally, the system state is checked to determine if any of a set of limits on quantities

such as the manipulator joint angles have been surpassed. This limit checking allows runs in

which the system state “blows up” to be halted before the entire duration of the integration

has been calculated while preserving, for later analysis, the evolution of the response up to

the time at which the run is terminated.

If the time specified for the end of the integration has not been reached the main loop

is repeated, after incrementing the integration time, t, by dt2. When the loop has been

completed for the last time the final results of the simulation are written to the output data

files and plotted.

4.3 Integration of Equations of Motion

The equations of motion for the manipulator/environment system are integrated numerically

using the IMSL routine DGEAR to yield the approximate continuous time response of the

system. The DGEAPL routine is based on the work of Gear [87]. It is a backward differencing

method, meaning that it uses backward difference formulae to approximate derivatives.

The algorithm incorporates adaptive step size selection which allows optimal performance

to be achieved by subdividing the interval over which the integration is being performed as

necessitated by the characteristics of the equations, without user intervention. The DGEAR

routine was selected for two reasons:

1. it is specifically designed for integration of stiff systems of differential equations, and,


2. it is well suited for integrations from which results are required at frequent, small

increments of the independent variable.

A system of differential equations is said to be stiff if there are two or more very different

scales of the independent variable which simultaneously effect the dependent variables [88].

A more mathematically rigorous definition [86] is that the system of first order ordinary

differential equations given as

y’=f(x,y1,y2,...,y) (4.1)

ofis stiff if some of the eigenvalues of the Jacobian matrix of partial deLivatives, are

large and negative. As was noted in Chapter 3, the system of differential equations (2.47)

through (2.52) is stiff because the natural frequencies of the system can easily span several

orders of magnitude and thus the generalised coordinates are composed of variations with

several, widely varying time scales.

The simulation uses a small time step, dt1, in the main integration ioop in order to

adequately reflect the continuous time nature of the dynamics in contrast to the discrete time

control algorithm. Small time steps are also required to adequately resolve high frequency

components of the response in graphical presentation of the simulation results. Finally,

and most importantly, small integration time steps are necessary to properly detect the

making and breaking of contact between the manipulator tip and the environment. If too

large a time step is used the manipulator will appear to penetrate the environment during

the time step in which contact is made and an erroneously large transient contact force will

result. The maximum permissible time step is dependent on the stiffnesses of the contacting

surfaces.


The DGEAR routine integrates systems of first order, ordinary differential equations.

Equations (2.47) through (2.52) are a system of second order ordinary differential equa

tions of the form:

Jj=b(q,4). (4.2)

Such a system is easily transformed to first order by the substitution:

q1 = q and q2 = 4 (4.3)

which yields,

I 0 41 q2. (4.4)

0 J q b(q1,q2)

In this representation, J is the generalised inertia matrix, q1 is the vector of generalised

coordinates, 4i = q is the vector of generalised velocities, 42 is the vector of generalised

accelerations and b (q1,q2) is the vector of values of the right hand sides of the equations

of motion. The matrices, I and 0, are identity and zero matrices, respectively. The state

vector for the system is [q1q2]T•

The DGEAR routine calls a user provided routine which must return the first time deriva

tive of the state vector. The generalised velocity part of that vector is simply copied from

the most recently calculated system state and the generalised acceleration part is calculated

by the steps shown in the flowchart in Figure 4.2. Since the dynamics of the DC motor

actuators are represented by first order, ordinary differential equations the state variables

from those models are simply concatenated onto the end of the state vector. Likewise, right

hand sides of the actuator model equations are concatenated onto the right hand side of

equations (4.4) for simultaneous solution with the equations of motion.


Calculate InertiaMatrix

1Calculate Right Hand

L Side Vector

1Add actuator couplingterms to inertia matrix

and RHS vector

‘I;Invert Inertia

Matrix

Multiply RHS vectorby Inertia Matrix Inverse

to yield GeneralisedAcceleration Vector

Figure 4.2: Flowchart of the calculation of the generalised acceleration vector.

4.4 Discrete Time Control Algorithm

The control algorithm described in Chapter 3 is implemented in a series of subroutines which

are executed only when the integration ioop time, t, is a whole multiple of the discrete time

step, dtd, (that is, t mod dtd = 0). Figure 4.3 shows a flowchart of the steps in the control

algorithm.

The first step in the control algorithm is the contact control logic block. This part of the

algorithm takes, as inputs, signals from the manipulator force sensor and joint encoders,

information from the trajectory planner and instructions from the operator. These data

are analysed and used to generate the sequences of future setpoints, to determine which

control mode (force or motion) should be used, and to decide whether or not the parameter

estimation algorithm should be activated to update the controller model coefficients. The

primary purpose of the contact control logic is to govern the actions of the control algorithm

Chapter 4. Dynamics and Control Simulation TWOFLEX 103

hitialise DiophantineIdentity Polynomials

for Recursion Formulae

Figure 4.3: Flowchart of the discrete time control algorithm.


during the time period immediately before and after a change in the contact state. This

function is particularly important when the manipulator unexpectedly comes into contact

with the environment, in which case the impact velocity may be large and result in contact

force transients which the predictive control algorithm cannot immediately compensate for.

Some examples of the categories of programs that the contact control logic can execute are:

• assuming the trajectory planner has an accurate world model of the manipulator and

its surroundings, the speed of approach to a surface and the time at which the control

should be switched between motion to force modes can be specified and the contact

control logic generates appropriate setpoint sequences and switching signals;

• in unknown or uncertain surroundings the contact control logic switches the control

mode from motion to force when a nonzero force signal is sensed and from force to

motion when the signal goes to zero;

• when contact is sensed by the force sensor the contact control logic switches to force

control mode and briefly modifies the contact force setpoint sequence from that pro

vided by the trajectory planner to a form suitable to compensate for the contact force

transient; the setpoint modification is based on an heuristic rule.

Contact control logic programs are discussed in more detail, with examples of the resulting

closed ioop responses in Chapter 5.

If the contact control logic program specifies that the control should be adaptive in

the current time step the coefficients of the controller model are updated using one of the

parameter estimation algorithms discussed in section 3.6. Both the U/D RLS algorithm

and EFRA are implemented in the TWOFLEX code. The algorithm to be used is selected by

a flag set in the input data file. The algorithm parameters are also included in the data

read at the beginning of the run.


The actual calculation of the control input signal increments begins with the initialisation

of the polynomial coefficients in the Diophantine identity recursion formulae. With those

coefficients and the coefficients from the controller model, the free response vectors, f1, for

one time step into the future are calculated.

Following the initialisation step, the Diophantine identity polynomial coefficients and

free response vectors are calculated for each of the future time steps up to the prediction

horizon, N2.

The final step in the predictive part of the algorithm is the calculation of the future

static equilibrium input sequences from the specified future setpoint sequences. This step

is only applicable in the case of force control since a direct relationship is available between

desired contact forces and the static equilibrium input levels that would be required to

produce them.

For computational efficiency the control horizon is checked before beginning to calculate

the control input increments. If N = 1 the calculation can be quickly performed as a scalar

operation in the case of a SISO system or by Cramer’s rule for a two input, two output

MIMO system. For N > 1, the future step response matrices, G1k, and their transposes

are formed and the left hand side matrix and right hand side vector terms in equation (3.56)

are constructed. The system of linear algebraic equations described by these terms is solved

for the sequences of future control input increments. Finally, the control input increments

for the next time step are added to the control input signals.

The entire discrete time control algorithm described above is embedded in a general

input calculation subroutine in the TWOFLEX code. That routine uses flags set in the input

data file to select the type of input to be used, either the control algorithm or the one of

the open loop inputs described in section 4.2. The input calculation routine also applies

the appropriate actuator saturation limits to the inputs.


4.5 Validation

The simulation of the unforced manipulator dynamics was validated by comparison with the

results of a program developed by Mah [35]. Mah’s program simulates the dynamics of an

orbiting platform and manipulator system modelled as a chain of flexible links connected by

flexible joints. It has been validated [89] against two other simulation programs for related

systems independently developed by Chan [68] and Ng [72]. Two types of validation test

were carried out:

1. the calculated inertia matrices and right hand side vectors for the equations of motion

with specified initial conditions were compared; and,

2. the evolution with time of the values of the generalised coordinates were compared

for simulations of the free response of the system from various initial states.

The calculated initial values of the elements of the inertia matrices and right hand side

vectors were compared for the seven cases listed in Table 4.1. The links were modelled as

1.Om long, 2.0cm diameter aluminium rods in all cases. For each of these cases the initial

inertia matrix and right hand side vector were in agreement to at least three significant

digits. The agreement was least good for terms which depend on the integrals of the

flexible link mode shape functions. This is due to the fact that Mah calculates those

integrals numerically whereas the analytical expression for those integrals for a cantilever

beam are included in the TWOFLEX code.

Comparisons of free response simulation results were made for the three cases listed in

Table 4.2. In the first two cases the links were modeled as 1.Om long, 2.0cm diameter,

aluminium rods. In case 3 the second link was 1.4m long. There is no damping in the

system. Good agreement was obtained in all three cases. Figures 4.4 shows the response of

the joint angle generalised coordinates, 1 and 02, for the first 0.5s of case 2. The results


Initial ConditionsManipulator Structure 8- 62 62 /11 2/’21 ,1’21

Case Link 1 Link 2 [0] [0] [rad/sj [rad/s] [—j [—] [1/sj [1/s]1 Flexible — 0 — 0 0 — 0 —

(1 mode)2 Flexible — 0 0 — LO 0.1 —

(1 mode)3 Rigid Rigid 30 30 0.1 0.1 — — —4 Flexible Flexible 0 0 0 0 0 0 0 0

(1 mode) (1 mode)5 Flexible Rigid 0 0 0 0 0 0 0 0

(1 mode)6 Rigid Flexible 0 0 0 0 0 0 0 0

(1 mode)7 Flexible Flexible 0 0 0 0 1.0 1.0 0.1 0.1

(1 mode) (1 mode)

Table 4.1: Configurations and initial conditions for validation of inertia matrix and righthand side vector terms.

Initial ConditionsManipulator Structure 6 62 2 /‘11 21

- ‘/ii /‘21Case Link 1 Link 2 [0] [] [rad/s] [rad/s] [—] [—] [1/s] [1/s]

1 Rigid Flexible 20 -25 0 0 — 0 — 0.1(1 mode)

2 Flexible Flexible -20 -75 0 0 0.05 -0.1 0 0(1 mode) (1 mode)

3 Flexible Flexible 35 35 0 0 -0.05 0.1 0 0(1 mode) (1 mode)

Table 4.2: Configurations and initial conditions for free response simulation validation runs.

Chapter 4. Dynamics and Control Simulation — TWOFLEX

-70

-75

-80

-85

-90

-95

-100

-105

-110

Time [s]

108

0 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50

Time [s]

(a) 81 response

-4

-6

-8

-10

-12

U)2. -14

-16

-18

-20

-22

U)U)a)L.

a)V

c’J

.50

(b) 82 response

Figure 4.4: Comparison of joint angle responses between TWOFLEX and Mah [89] for freeresponse validation.

0 .05 .10 .15 .20 .25 .30 .35 .40 .45


from the TWOFLEX program and those from Mah [89] are initially coincident and differ by

only a small amounts after 0.5s (5000 integration time steps). The responses of the modal

amplifier generalised coordinates, and ‘çb21, for case 2 are shown in Figure 4.5 and

illustrate the same excellent agreement.

4.6 Summary

The TWOFLEX program is a versatile simulation of the dynamics and control of a planar,

two flexible link manipulator and a deformable environment. The code is modular, ex

tensively documented and includes optionally compiled code to generate debugging output

from almost every subroutine. The integration of the continuous time equations of motion

and the calculation of the discrete time control algorithm are conceptually and structurally

separated in the code, making it a valuable tool for the investigation of both the open and

closed ioop behaviour of the system.

Chapter 4. Dynamics and Control Simulation — TWQFLEX

.07

• 06

• 05

• 04

.03

• 02

.01

0

- .01

- .02-

. 03

- .04-

. 05-

. 06

- .07

- .080

Time [s]

(a) b11 response

110

.12

.10

08

• 06

04

• 02

0

-. 02

—

. 04

-

. 06

-. 08

-.10

-.120

TWOFLEX----j

Time [s]

.05 .10 .15 .20 .25 .30 .35 .40 .45 .50

c’J

.50

(b) 21 response

Figure 4.5: Comparison of modal amplifier responses between TWOFLEX and Mah [89] forfree response validation.

.05 .10 .15 .20 .25 .30 .35 .40 .45

Chapter 5

Force Control Algorithm Performance and Evaluation

5.1 Introduction

This chapter presents results and discussion of the analysis of the dynamics of flexible link

manipulator force control systems and the results of numerous runs of the TWOFLEX dynam

ics and control simulation program. Analysis of the eigenstructure of the system reveals

orderly changes in the dominant natural mode with increasing effective contact stiffness.

These changes have important implications for the selection of the controller parameters.

The simulation results demonstrate the capabilities and evaluate the performance of the

adaptive, multivariable, long range predictive control algorithm developed in Chapter 3.

The results are organised roughly in order of increasing complexity of both the manipula

tor/environment system and the maneuvers which are simulated. This organisation seeks

to emphasise the basic performance of the control algorithm before introducing such com

plications as making and breaking contact, highly nonlinear dynamics, and actuator effects.

An alternative view of the contents of this chapter is shown in Figure 5.1. The topics which

will be addressed can be broadly classified as:

• linear analysis of various manipulator configurations in contact with their working

environments;

• force control step response results for various manipulator configurations; and

• the problem of maintaining control when contact is made or broken.

111

Chapter 5. Force Control Algorithm Performance and Evaluation

I Stop MotionStrategy(5.4.1)

Making andBreaking Contact

Force SetpointModification Strategy

(5.4.2)

112

Figure 5.1: Topics discussed in Chapter 5. Section numbers in which the discussion of eachtopic appears are shown in parenthesis.

Chapter 5. Force Control Algorithm Performance and Evaluation 113

Figure 5.1 indicates the sections in which each of these topics are discussed for each of the

manipulator configurations considered.

The chapter begins with an analysis of the eigenvalues and eigenvectors of the equations

of motion for a single link/environment system. Analytical approximations to the trends

observed in the numerical evaluation of the eigenvalues are developed and the implications

of changes in the modal structure of the system for contact force control are discussed. The

performance of the controller for force control of a single flexible link is discussed in sec

tion 5.3. The calculation, from the physical parameters of a particular link and environment,

of the various parameters and coefficients required by the TWOFLEX program is presented

as well as the derivation of the structure and initial coefficient values of the discrete time

transfer function model for the control algorithm. Simulated step and ramp response results

are shown to demonstrate the performance of the controller. The effects of the controller

horizons and weighting factors in shaping the closed ioop response are also discussed in the

course of Section 5.3. The discontinuous nature of the manipulator/environment system

dynamics is considered in section 5.4 with a discussion of the problems involved in main

taining control during the making and breaking of contact. The use of contact control logic

to overcome those problems is discussed along with the presentation of simulation results

for two different contact control logic strategies. The performance of the multivariable form

of the control algorithm is demonstrated by considering force control for a two link manip

ulators in section 5.5; both rigid link and flexible link manipulators are discussed. Results

of simulations of two links of a prototype model of the DC motor driven Space Station

Mobile Servicing System (MSS) manipulator are presented and discussed in sections 5.6.

The chapter concludes with a summary of the force control algorithm performance. Some

of the work presented in sections 5.3 and 5.4 has already been published [22,27].


Figure 5.2: Single rigid link in contact with a deformable environment.

5.2 Linear Analysis

This section presents a linear analysis of the dynamics of a single link in contact with

a deformable environment. Particular emphasis is placed on the interaction between the

structural stiffness of the manipulator link and the stiffnesses of the contacting regions of the

link tip and the environment. Using analytical and numerical analysis of the eigenvalues and

eigenvectors of systems of linearised equations of motion it is shown that, for a given link,

this interaction results in distinct changes in the modal structure of the link/environment

system as the effective contact stiffness increases. The implications of these modal changes

for contact force control are discussed.

5.2.1 Rigid Link

Consider first a single rigid link which is the limiting case of infinite structural stiffness. The

equations of motion for the system shown in Figure 5.2 are given in section A.6. Simplifying

those equations by neglecting damping, gravitational effects and the small coupling inertia

L1

1k

/;


terms between the link and contact mass allows them to be linearised about the nominal

state 6 = 0 and written as:

10181 = ui(t) — k3L1(L1e1 — (5.1)

mcfy = —k3 (E — L61) keyEy, (5.2)

where the generalised coordinates {i, e} are now perturbations about the nominal state.

For the sake of generality the joint torque, r1, has been replaced by a general control input,

ui(t).

To study the eigenstructure of the system these equations are transformed by applying

the Laplace transform and rewriting in matrix form:

1J(s) = ui(s), (5.3)

0

where

= 101s2 + k5L1 —k3L1. (5.4)

—kL1 ms2 + (k3 + k)

The characteristic equation of the system is given by setting the determinant of J(s) to

zero:

=I01ms4+ [I01(k3+ key) + mck8yL] 2 + ksykeyL = 0. (5.5)

The natural frequencies of the system (eigenvalues of J(s)) are given by the roots of equa

tion (5.5). We will first investigate the variation of the natural frequencies with contact

stiffness numerically by considering a 1.Om long, 2.0cm diameter aluminium rod treated

as a rigid link. Given the density of aluminium of 2690kg/rn3 such a link has a mass of

0.8451kg and a moment of inertia about the joint axis of

101 = M1(d)2

+ = 0.2817kg m2. (5.6)


The contact region masses, m3 and me, are taken as 0.19 each. Figure 5.3 shows the

variation of the natural frequencies of the system with effective contact stiffness ranging

from 50N/m to 5MN/m. The effective contact stiffness is defined as

keffksy+key

(5.7)

The data for this plot was generated by numerical calculation of the eigenvalues of J(s)

for various values of k3 and key. The plot clearly indicates that the natural modes of the

system are well separated in frequency and both frequencies increase in proportion to the

square root of the contact stiffness. These observation can be confirmed analytically using

equation (5.5). Substituting (jw) for s,

I0im(jw)4+ [Ioi(ksy + key) + mkL} (jw)2 + ksykeyL = 0, (5.8)

gives a quadratic equation of the form

ax2+bx+c=0, (5.9)

the roots of which are the squares of the natural frequencies of the system. The coefficients

of equation (5.8) are such that b2 >> 4ac and so the quadratic formula can be approximated

using the binomial expansion to yield:

2 iF 12(sw) = — —b+ b .—4ac)

- :

b+b(14ac)]

1 / 1 4ac 1 4ac2=-

— or (5.10)


11111111 I I 1111111 I I 1111111 11111111 I I 1111111 I 111114

iø-

1/2•

1 ksy+key•

2[ms+me -

iø

N

>%C-)C

-a)

:

1/2

ioeI I iii I I liii I i i I i I

10_2 101 i00 101 102 iø i04

Effective Contact Stiffness [kN/m]

Figure &3: Variation of the natural frequencies with effective contact stiffness for a singlerigid link/environment system.


Note that for the case shown in Figure 5.3, and indeed for most cases of interest, m <<101,

therefore,

2 ksy + keyand

keL(5.11)

m 101

These results confirm the correlation of the natural frequencies with the square root of

the contact stiffness that is shown in Figure 5.3. Also note that the natural frequencies are

inversely proportional to the square root of the link moment of inertia and the contact region

mass. In what follows we consider a given contact mass and link inertia and concentrate on

the effect of changing contact stiffness on the system.

Based on the approximate natural frequencies given by equations (5.11) the modal eigen

vectors of J(s) are approximated by

E [ei e2]1

(5.12)

1 (1_)L

For surfaces of comparable stiffness, such as is the case in metal on metal contact, the ratio

of key to k3 is of order 1. Combining this condition with the assumption that rn,, <<

we see that the first element of the e1 eigenvector is very much smaller than the second.

This result is confirmed by the numerically determined eigenvectors for the case shown in

Figure 5.3 with key = k3. The eigenvectors, normalised to unit magnitude, are found to be

independent of the effective contact stiffness:

—1.0640 x iO 0.8943E = { e1 e2 j . (5.13)

1.0 0.4472

The relative magnitudes of the elements of the eigenvectors indicate the contribution of

each of the generalised coordinates to the natural modes, that is:

[:1 E (5.14)


It is evident that the mode, which corresponds to w1, the higher of the two natural

frequencies, is composed almost entirely of the e, generalised coordinate. Physically, this

composition means that the high frequency mode is the oscillation of the contact region

mass, m, between the virtually stationary link tip and the environment body. The other

mode is the oscillation of the link about its joint. This mode includes an e component

because the contact region mass, m, moves in phase with the link tip. Because m <<101

the energy of the 2 mode is concentrated in the link motion. The ratio of the components

of this mode is determined by the contact stiffnesses. In equation (5.13) the ratio is 2 to 1

because = key for that case.

Control of the system is effected by the relationship of the natural modes to the contact

force and the control input. The contact force exerted by the link tip on the environment

is given by equation (2.57). For a single rigid link with damping effects neglected it can be

linearised to:

T

!!!&k L1= mc

. (5.15)1( j.

—m8 —

Noting that the contact force is expressed as a linear combination of the generalised coor

dinates, the matrix ET can be used as a linear transformation to obtain an expression for

the contact force in terms of the natural modes:

= (ETcy)T[1]

L1 1Li_1(i+)

mk,,, —

. (5.16)

L1 + (1— ) (:::

—

1) L1 ?72


For comparably stiff surfaces we see that the contact force is composed almost entirely of

the 2 natural coordinate, that is, oniy the low frequency mode contributes significantly to

the contact force. The numerical values confirm this observation:

1

F —5.3201 x iO=

. (5.17)0.4472

The ET transformation can be used to show the relative excitation of the natural modes

due to a step change in the control input. Noting that the control input, u1(t), acts on the

8i generalised coordinate, the relative excitation of the natural modes is given by:

ET1

ui(t)0

—1.0640(5.18)

0.8943

This result implies that the control input excites almost exclusively the 2 coordinate.

To summarise, analysis of the eigenvalues and eigenvectors of a linearised, undamped

model of an infinitely stiff link in contact with the environment under the assumptions that

the contact region mass is small compared to the moment of inertia of the link (m <<I)

and that the contacting surfaces are comparable (k k, and m3 me) reveals that:

1. the natural frequencies of the system are widely separated in frequency and, for a

given link and surface, are proportional to the square root of the effective contact

stiffness;

2. the high frequency mode is the vibration of the surfaces of the link tip and the envi

ronment in the region of contact;

3. the contact force is almost entirely due to the low frequency mode, and;


Wi1:110

Figure 5.4: Single flexible link in contact with a deformable environment.

4. the control input primarily excites the low frequency mode.

The implications of these results for control of the contact force, using the discrete time

predictive control algorithm developed in Chapter 3, are that the high frequency mode can

be ignored and the sampling rate selected to satisfy the Nyquist criterion for w2. Because

the control input is directly coupled to the contact force output through the w2 mode the

system falls in the class of systems described by Clarke, et al [29] as “easy” to control.

For such systems a prediction horizon of N2 10 and a control horizon of N 1 are

recommended to obtain satisfactory control. As noted at the outset the rigid link case is

primarily of interest for comparison as the limiting case of infinite link stiffness.

5.2.2 Flexible Link

The equations of motion for a single flexible link in contact (see Figure 5.4) are given in

section A.4. Modelling the link with a single structural mode and neglecting damping,

101 1111 1211 1311k

Ty


gravity and link to contact mass inertial coupling the equations of motion, linearised about

{ ; è i4 0, are:

+ = ui(t) — k3L1(L101 +q110b11-

(5.19)

111161 +I211b11 = I3111I’11 —k3110 (L161 +q110b11 — (5.20)

më = —k3 (e — L1e91 —q110,bii)— keyE!,. (5.21)

The joint torque, r1(t), has been replaced with a general control input, u1(t), and the

generalised coordinated {i, e,} are now perturbations about the nominal state.

As in section 5.2.1 we will begin by considering the natural frequencies of the system.

In preparation, the Laplace transform is appled to the linearised equations of motion and

they are rewritten in matrix form:

1

J(s) bii(s) = 0 u1(s), (5.22)

0

where

Is2 + k8L1 1s2 +k3L1cb11, —k8L1

J(s) = Lj11s2 +k3Liqii0 1211s2 + 1311 +k5çb10 —k5b110 . (5.23)

—k3L1 —k54110 m.s2 + k3 + key

The derivation of analytic expressions for the eigenvalues of J(s) is rather involved. More

immediate insight can be gained by solving for the eigenvalues numerically and considering

the data with the results of section 5.2.1 as a guide.

Considering the same 1.Om long, 2.0cm diameter aluminium rod used in section 5.2.1, the

physical parameters of the link model must now include quantities related to the structural

characteristics of the link. The modulus of elasticity for aluminium is 7.31 x 10’°N/m2


Mode Mode ShapeNumber Function Integrals

(i) Ifl ‘21z ‘31i 4’iiO

1 0.4807 0.8451 7096.0 2.0

Table 5.1: Mode shape function integral factor values for the first mode of a 1.Om long,2.0cm diameter, aluminium cantilever beam.

which, combined with the link dimensions, gives a flexural rigidity in transverse bending of:

El1 = Ed = 574.024N m. (5.24)

The first mode of a cantilever beam is used as the admissible mode shape function for the

link structure. Assuming uniformly distributed mass and rigidity, the values of the mode

shape function integral factors are shown in Table 5.1. These values are calculated, using

closed form expressions for the integrals, from the physical parameters of the link in the

input processing step of the TWOFLEX program.

Figure 5.5 shows the natural frequencies for the flexible (one mode) link (in contact)

with effective contact stiffnesses varying from 50N/m to 5MN/m. The lower of the two

natural frequencies for the same link, modelled with infinite structural stiffness, is shown

as a dashed line for reference. As in the case of the rigid link the highest natural frequency

is very closely approximated by

2 = k3 + key(5.25)

The intermediate frequency, w2, is independent of the contact stiffness for small values

and proportional to the square root for large ones. Conversely, w3, the frequency of the

slowest mode, is indistinguishable from the rigid link frequency for small contact stiffnesses

and constant for large values. The isolation of the w1 mode, which is the vibration of the

contact region, from the other two modes suggests the consideration of a simplified model

of the system, in which the contact dynamics are replaced by a steady state approximation

(see Figure 5.6). Neglecting the contact region mass, m, and replacing the contact region


ncr 310>%0C0)

0•0)U

CuI

1 102z

Figure 5.5: Variation of natural frequencies with effective contact stiffness for a single flexiblelink with one structural mode (El1 = 574N m2). Lower frequency for an equivalent rigidlink is shown dashed.

124

1 4

I 11111111 I I Ilillif I 11111111 I 11111111 I I 1111111 I 111111:

iModeFlexible Link

— — - EquivalentjinJ

keff=

kN/m

10’

ioio_2 10_i

I’I I II 11111 I I II

100 101 jØ2

Effective Contact Stiffness [kN/mj

111111 I I IlIlill I 1111111


Wi1 (p110

stiffnesses, k3 and key, with the corresponding effective contact stiffness, keff, yields the

following equations of motion:

:loi8i + Iiiiii = ui(t) — keffL (L181 +&10b11), (5.26)

+I211b11 = I3iii1 keffci1o (L11 + 110b11). (5.27)

This reduced system can be used to explain the asymptotic behaviour of the lower two

natural frequencies shown in Figure 5.5.

The characteristic equation of this system is

(IOiI2i. — z2) (w)4

+ [IiI311 + keff (IoiqS10 +I211L — 2IiiiLiqiio)] (jw)2 +I3likeffL = 0. (5.28)

101 1111 1211 1311k eff

Figure 5.6: Single flexible link in contact with a deformable environment approximated bya steady state contact dynamics model.

/

Note that the coefficient of (jw)2 is dominated by the ‘oi’3n term for small effective contact

stiffnesses and by keff +1211L1 — 21111L14110)for large values of keff. With this in

mind, the binomial expansion approximation of the quadratic formula used in section 5.2.1

reveals that, for a given link, the lower two natural frequencies approach, for small values

of kff— ‘o1’311

—

— T2211 -‘-iii

and= keffL

101

(5.29)


and for large keff:

2 — keff (I0iq10 + T211L —2I111L1qS110)

— 1011211

and

‘T r22

__________________________________

— (Io+I211L—2I111L1110)

These results predict very closely the values of w2 and w3 shown in Figure 5.5 at the extremes

of effective contact stiffness. The transition from the frequencies given by equations (5.29)

to those given by equations (5.30) occurs when the magnitudes of the two terms in the

coefficient of (jw)2 in equation (5.28) are approximately equal. Empirically, the values of

keff over which the transition occurs fall in the range

1011311 1011311

+I211L —2I111L1110keff < 10i +I211L —2I111L1110

(5.31)

For the case shown in Figure 5.5 this result implies that the transition is centred at keff

40.7kN/m.

The changes in the modal structure of the system as the contact stiffness increases

are further illuminated by considering the eigenvectors. Unlike the rigid link case, the

normalised eigenvectors of J(s) are not independent of the contact stiffness. For the purposes

of control the most important effects of this dependence on contact stiffness are variations in

the contribution of the natural modes to the contact force and the excitation of the natural

modes by the control input. The contact force, derived from equation (2.57) by neglecting

the damping and link 2 terms and linearising is:

= [ mL1 mello (m5— me) ] Ii (5.32)

Using the matrix of eigenvectors, ET, to transform the components of the contact force ex

pression from generalised coordinate space to natural coordinate space produces the results


.45 I I I I 111111 I

.40

.35 -

.30 -

.25

a) -a)0 -

g.15

.r3O -

I...

0E05Zo

0

0

05 -

-.10

-

. 20102 101 10 101 102 iø


Figure 5.7: Variation of the natural mode components of the contact force with effectivecontact stiffness for a single flexible link with one structural mode. Contact force components are normalised by k8.

I 1111111 I 1111111 I I huh uiiul I I 11111


shown in Figure 5.7. These data are for the 1.Om long, 2.0cm aluminium link modelled

with one flexible mode and were obtained by calculating the eigenvectors of J(s) and per

forming the transformation on the contact force components numerically. The figure shows

that at low values of contact stiffness the contact force is composed of components of the

72 and natural modes, with dominating. As the effective contact stiffness increases

the contribution of 773 disappears and 772 dominates. The contribution of m to the contact

force is negligible. In terms of control this implies that as the contact stiffness increases

the contact force changes from being similar to that which would arise were the link rigid

to being dominated by the structural characteristics of the link. In particular this means

that the required sampling rate is governed by w3 at low contact stiffness but by w2 at high

values.

To complete the picture we consider the effect of the control input on the excitation of

the natural modes. Figure 5.8 shows the components of the natural coordinates that are

the result of the numerical transformation from generalised coordinate to natural coordinate

space of the input vector, [1 0 01T from equation (5.22). In contrast to the contribution of

the natural modes to the contact force, the excitation of the 772 and 773 modes by the control

input remains relatively constant over the range of contact stiffnesses, with 773 receiving the

most excitation. The 771 mode undergoes negligible excitation. For contact force control

this result means that, without making any contribution the the controlled variable at high

contact stiffness, the 773 mode remains important as the primary route of input of control

energy into the system.

All of the results presented above are for a specific link structural stiffness. Reducing the

flexural rigidity of the link from 574.024N m2 to 40.ON m2 produces the results shown

in Figure 5.9. The pattern of the variation of natural frequencies with contact stiffness is

similar but it is shifted to the right and downward. That is, the horizontal asymptotes of w2

and w3 occur at lower frequencies and the transition from w3 dominance to w2 dominance


1.0 i I I 1111111 I 11111111 I IIIIII I I IIIIiii

113

.8

.7

.60

0oxW .5::

.4

.3 -

.2 -

- 11

0 I huuH1 I uhuHIl I I hII I I 11111., I I

1W2 101 101 102 i& i04


Figure 5.8: Variation with effective contact stiffness of the relative excitation of the naturalmodes of a single flexible link with one structural mode in contact.


•i::r i’ -

>‘C)Ca)Cci)I-.

LL

(1

D(I,z

130

1 5

1 4

I I 1111111 I I IIIIIIj I I 1111111 I I 1111111 I I 11111!

_____

1 ModeFlexible Link

EquivalentRigid Link

1

k9ff = 2.8 kN/m

101

1ø 11111111 11111111 i 11111111 111111111

10_2 10_i i00 101 102 iø


Figure 5.9: Variation with effective contact stiffness of system natural frequencies for a softlink (El1 = 40N . m2). Lower frequency for an equivalent rigid link is shown dashed.


occurs at lower effective contact stiffness (keff 2.8kN/m).

The modelling of the link structure using a single admissible mode shape function con

veniently permits analytical approximation of the asymptotic behaviour of the natural fre

quencies. However, additional modes may be required in the model, depending on the degree

of accuracy with which the continuum dynamics of the link structure must be represented.

Figure 5.10 shows the natural frequencies varying with effective contact stiffness for a link

with its structural characteristics modelled by the first four modes of a cantilever beam

with a flexural rigidity of El1 = 200N m. As in the previous cases the highest frequency

mode, associated with the vibration of the surfaces in the contact region, is proportional to

the square root of keff and well separated from the other modes. For iow contact stiffnesses

the lowest frequency mode is proportional to the square root of keff and the system is domi

nated by rigid link-like behaviour. With increasing keff the slowest mode becomes constant

and each of the higher frequency modes successively passes through a transition from one

constant frequency to a higher one. During the transitions each of the modal frequencies

becomes proportional to k!ff and dominates the system. This progression of dominate modes

is clearly shown in Figure 5.11 in which the variation with effective contact stiffness of the

natural mode components of the contact force are plotted. While the modal composition of

the contact force changes markedly with increasing keff the excitation of the natural modes

by the control input remains roughly constant as shown in Figure 5.12.

In terms of contact force control the results of this linear analysis imply that, at a given

effective contact stiffness, a flexible link/environment system in which the link structure is

modelled by several structural modes is analogous to a system with one structural mode for

the purposes of selecting an appropriate controller sampling interval because the contact

force is dominated by one of the modes. On the other hand, since the control input supplies

energy to all but the highest frequency mode, the controller model and parameters must be

specified with due attention to the true effective order of the system.

z>.C)C0D

0

U

D(‘3z

i itiiiil

iø


Figure 5.10: Variation with effective contact stiffness of system natural frequencies fora flexible link with four structural modes (El1 = 200N . ma). Lowest frequency for anequivalent rigid link is shown dashed.


LI IIIIII I I 1111111 I 11111111 I 11111111 I 1111111 I I

_____

1 ModeFlexible Link

— — — EquivalentRigid Link

1 ô

1

1 4

1

101

1 ø I .::

iw 10=1 10 101 102


cl)

-D 0o LL

ct

—oco

ci)—

,-

OEzoC)

133

40

35

30

.25

.20

15

10

05

0

- .05

-.10

-.15

- .201 10_I i00 101 102 i03

Effective Contact Stiffness [kN/m]1 4

Figure 5.11: Variation of the natural mode components of the contact force with effectivecontact stiffness for a single flexible link (El1 = 200N . m2) with four structural modes.Contact force components are normalised by k3.


1.0 I 11111111 I I 1111111 I I 1111111 I I IIIIII I I 1111111 I I 111111

.8- 14

.7

.60w •.

ooxh0)

a)4

.3

.2

0 I liii. I I 1111111 I I Ijilill IIInI

1Ø_2 10_i

Effective Contact Stiffness [kN/mj

Figure 5.12: Variation with effective contact stiffness of the relative excitation of the naturalmodes of a single flexible link (El1 = 200N . rn2) with four structural modes.


5.3 Force Control for a Single Flexible Link

The first demonstration of the performance of the adaptive, long range predictive force

control algorithm is for the case of a single flexible link in contact with a deformable en

vironment surface. The physical parameters of the particular system that was simulated

are given in the following sections along with the calculation from those parameters of the

coefficient and parameter values used in the TWOFLEX program.

5.3.1 Link Model Parameters

The link is modelled as an aluminium rod 2.0cm in diameter and 1.Om long. From these

dimensions the volume of the link is:

V1 = dL1 = 3.1415 x 104m3. (5.33)

Given the density of aluminium of 2690kg/rn3,the mass per unit length (p1) of the link is

0.8451kg/rn. The moment of inertia about the joint axis is:

= M1(d)2

+ = 0.2817kg m2. (5.34)

Finally, the modulus of elasticity for aluminium is 7.31 x 1010N/m2which, combined with

the link dimensions, gives a flexural rigidity in transverse bending of:

El1 = Ed = 574.024N m2. (5.35)

The gravitational field is assumed to be perpendicular to the plane of the link motion and

therefore has no effect.

The structural dynamics of the link are modelled by using the mode shapes of a cantilever

beam as admissible functions. Based on the uniformly distributed mass and rigidity values

calculated above the natural frequencies of the beam and the values of the mode shape


Mode Natural Mode_Shape Function IntegralsNumber Frequency ‘31i

(i) (Hz) (Ivg.m2) (kg.m2) (N.m2) (m)1 14.6 0.4807 0.8451 7096 2.02 91.4 7.6707 x 10—2 0.8451 2.7870 x 1O -2.03 255.9 2.7395 x 10—2 0.8451 2.1850 x 106 2.04 501.5 1.3980 x 10—2 0.8451 8.3907 x 106 -2.05 829.0 8.4569 x i0 0.8451 2.2929 x io 2.0

Table 5.2: Natural frequencies and mode shape function integral factor values for the firstfive modes of a 1.Om long, 2.0cm diameter, aluminium cantilever beam

function integral factors for the equations of motion for the first five modes are listed in

Table 5.2. All of these data were calculated from the physical parameters of the link in the

input processing step of the TWOFLEX program.


While the dynamic simulation of a single flexible link in contact is based on the equations

of motion (A.11) through (A.13), the force control algorithm requires a linear model in the

form of a discrete time domain transfer function. The equations of motion can be linearised

about a nominal zero state, transformed into algebraic equations using the Laplace operator

and written in matrix form, as demonstrated in section 5.2.2, to obtain a matrix equation

of the form:

1

0

?,b12(S) 0J(s) (5.36)

0

eu(s) 0


Likewise, the contact force equation can be linearised and expressed in the form:

9i(s)

L’1i(s)

F = ci2(3)

(5.37)

/‘in (s)

e,(s)

These expressions are combined to yield the continuous time transfer function for the system:

JH

F(s) 1cT

—

5 38Ui(S) - J(• (. )

(—1)’+ J1

where is the ij minor of the n x n matrix J(s).

The discrete time transfer function which constitutes the controller model is obtained

from equation (5.38) by substitution of numerical values and transformation to the discrete

time domain using the selected sampling rate and assuming zero-order hold sampling. The

specific controller models for various cases are given in the following sections.

5.3.3 Soft Contact Case (keff = 3.lkN/m)

We begin by considering the performance of the control algorithm in the case of relatively

soft (k3 ke = 6200N/m) contacting surfaces on the link tip and the environment. The

contact region masses on each surface are taken to be m5 = me = 1.0g. The link structure is

modelled using the first two cantilever beam mode shapes as admissible functions. Portions

of this work have already been published [22,27].


Controller Model

With k ke = 6200N/m, m5 = me = 1.0g, n1 = 2 and the values given in Table 5.2

equation (5.38) yields the continuous time transfer function

Fr(s) 0.1944s6 + 1.0486 x 106s4— 9.6132 x lOis2 + 7.6021 x l0

u(s) — 1.8731 x 10_6s8 + 18.933s6 + 4.6472 x 1Os + 8.0802 x 1012s2 + 7.6021 x 1016

(5.39)

which has open loop natural frequencies of 15.81Hz, 66.29Hz, 303.5Hz and 409.0Hz. Fig

ure 5.13 shows the plot of natural frequencies versus effective contact stiffness for this

system with keff 3100N/m marked. The relationships among the modal natural frequen

cies shown in Figure 5.13 are more complicated than those seen in the similar figures in

section 5.2. This increased complexity is due to the fact that the contact region mass of 2.Og

is an appreciable fraction of the link moment of inertia (0.2817kg . in2). With this in mind,

we still expect the system dynamics to be dominated by the 15.81Hz and 66.29Hz modes

and use a sampling interval of 4ms which satisfies the Nyquist criterion for the 66.29Hz

mode. The two higher frequency modes are neglected because keff is such that they are

in the range in which the 303.5Hz mode frequency is independent of keff and the 409.0Hz

mode frequency is proportional to kff; this would not be the case if the effective contact

stiffness were significantly lower or the contact region mass appreciably higher. While the

choice of 250Hz (h = 4ms) sampling implies that the two highest frequency modes will be

aliased, the controller performs relatively well owing to the relatively low excitation of these

modes.

With a sampling interval of 4ms and zero order hold sampling equation (5.39) is trans

formed to give the discrete time transfer function controller model:

F(q’)

u(q)

—14.7 — 2.38q + 12.8q2 + 5.36(q3 + q4) + 12.8q5 — 2.38q6 — 14.7q7

1 — 0.3lq1 + O.84q2— 0.46q3— 0.059q4 — O.46q + 0.84q6— 0.31q7+q_8


iø. I 1111111 I 1111111 I 1111111 I 1111111 I I IIIIII

iø

kif 3 1 kN/m

iø>.C.)Ca,D

D 2.10z

101

iOøI I iiiiiI I I iiiiiil I iIiiIiiII I I iiiiiil IIIIiiI

102 101 101 102 i03 i04


Figure 5.13: Variation of natural frequencies of a single flexible link (El1 574N m, twostructural modes) in contact.


30

25

‘20a)C.)

015

olO

5

0

Time [s]

Figure 5.14: Contact force response of a single flexible link on a soft surface(k8 = ke = 6200N/m).

(5.40)

Step Response

Figure 5.14 shows a series of ramp and step responses from the closed loop system. The

link is initially stationary and just touching the environment surface, such that the contact

force is zero. The parameters for the predictive control algorithm are a delay horizon (N1)

of 1 time step, a prediction horizon (N2) of 20 time steps, a control horizon (Nu) of 12 time

steps, a control increment weighting factor (Ar) of 1.0 and a static equilibrium bias term

weighting factor se) of 0. The controller time step is 4.Oms. The controller is adaptive,

with the model coefficients being updated at every time step.


In Figure 5.14 the desired contact force setpoint is represented by the dashed line and

the contact force response of the system calculated by the simulation is the solid line.

Initially, the setpoint is ramped at 50N/s and the response is seen to track very well, with

the exception of some minor perturbations during the first lOOms which are caused by

the system identification algorithm making fairly rapid changes in the model coefficients.

Following the ramp is a series of steps and dwells of various magnitudes and durations.

The calculated force response to these steps is very quick however, there is generally some

overshoot and oscillation about the new setpoint level. The magnitude of the overshoots

and oscillations decreases with subsequent steps as the system model is refined by the

identification algorithm. The inherent integral action of the control law ensures that there

is no steady state error. The overall controller performance illustrated by Figure 5.14 and

the other results presented in this chapter is superior to that demonstrated by the other

control methods (see section 3.1) which were investigated.

One characteristic of the predictive controller that must be noted is its inherent tendency

to anticipate changes in the setpoint level. This is a result of the control law being based on

the minimisation of not only the current force error but also all of the predicted future errors

up to the prediction horizon. This characteristic is most clearly observed in the deviation

from the setpoint which appears at approximately 1.30s, after a period of quiescence. Also,

during the lOOms dwell at iON which begins at O.85s the almost immediate anticipation

of the next setpoint change by the controller is, at least in part, responsible for the system

never coming to rest at iON.

5.34 Hertz Contact Model Case (keff 5MN/m)

Next we consider very much stiffer contact regions on the link tip and the environment.

The parameters of the contact dynamics model are derived from the Hertz contact model

described in section 2.5. The link structural dynamics are modelled by a single cantilever


Radius of Curvature [rn] 0.002 1000Elastic Modulus [N/rn2] 2.1 x 1011 7.1 x 1010

Poisson’s Ratio 0.300 0.334Density [kg/rn3] 7849 2690Eff. Planar Strain Modulus [N/rn2] 7.9145 x 1010

Eff. Radius of Curvature [m] 0.002Total Contact Deflection [m] 1.9986 x 10_6

Eff. Contact Stiffness [N/rn] 5.0037 x 106

Contact Radius [m] 6.3222 x iOSurface Deflection [m] 5.1405 x iO 1.4845 x 106

Linear Contact Stiffness [N/rn] 1.9453 x i0 6.7365 x 106

Estimated Contact Mass [kg] 1.3031 x 10_il 3.7235 x lO

Table 5.3: Material properties and Hertz model parameters for steel on aluminium contact.

beam mode.

Contact Model Parameters

The contact between the link tip and the environment is modelled as a 4mm diameter steel

bearing attached to the link tip and making contact with a flat aluminium plate. The

physical properties of the contacting materials are summarised at the top of Table 5.3.

From these data the effective planar strain modulus and radius of curvature are calculated.

Assuming a nominal contact force level of iON, equation (2.59) gives a total deflection of

the contacting surfaces of 1.9986 x 106m. The deflections of the individual surfaces are

inversely proportional to their planar strain moduli. A tangent to the force deflection curve

given by equation (2.59) at iON yields estimates of the contact stiffnesses of the surfaces,

modelled as linear springs. Finally, equations (2.61) and (2.62) give the radius of the contact

region and estimates of the mass of material on each surface moved during contact. Tile

results of these calculations for the case of a steel bearing on an flat aluminium surface are

listed in the lower portion of Table 5.3. These calculated contact mass and stiffness values

Steel AluminiumBearing Plate


are used for the variables m3, me, k3, and in the equations of motion.

Controller Model

Using the contact model parameter values calculated above and those for the link dynamics

with one structural mode calculated in section 5.3.1, equation (5.38) yields a model of the

open loop system with natural frequencies of 60.42Hz, 945.7Hz, and 114.9MHz. The latter

frequency corresponds to the mode dominated by the vibration of the contact region mass

between the stationary environment surface and link tip. In contrast to the situation in

section 5.3.3 the contact region mass is very small compared to the link moment of inertia.

Though no experimental data was sought, the analytical results developed in section 5.2.2

suggest that, under the condition of large contact stiffness and small contact mass (compared

to the link moment of inertia) the contact region mass vibration mode is neither significantly

excited by the control input nor does it contribute appreciably to the contact force and can

thus be ignored in the controller model. Hence, by introducing the effective contact stiffness,

keff and dropping the y direction contact equation we arrive at the linearised equations

of motion with a steady state approximation of the contact dynamics given by equations

(5.26) and (5.27). Under these assumptions the contact force model given by equation (2.57)

reduces to:

F = c , where

c = kff { L1 (5.41)

and continuous time domain transfer function relating the control input, u(s), to the contact

force, Fr(s), is:

F(s) — 1u(s)

— J(5)I—


keff (1211L1— Ii&i) 52 + keffI3llL1

5 42

where

(1011211 — I2) 4

+ [1011311 + keff +I211L —2I111L1110)}2

+keffI3llL. (5.43)

Substituting numerical values into this expression gives:

Fr(s) — —5.8193 x 105s2 + 3.5506 X 10u(s) — 6.9922 x 10s + 2.4768 x 105s2 + 3.5506 x 1010’ ( . )

Figure 5.15 shows the variation of the natural frequencies with effective contact stiffness for

the system described by equation (5.44). At keff = 5.0037MN/m the natural frequencies

are 60.42Hz and 945.7Hz and the latter frequency is proportional to kff.

A sampling frequency of 2857Hz (h 0.35ms) satisfies the Nyquist criterion for the

945.7Hz mode and zero order hold sampling yields the discrete time transfer function:

F(q’) — —2.294 + 2.303 (q + q2) — 2.294q3 —1 (5 45)u(q’) — 1 —2.043q’ + 2.105q2 —2.043q3+

q4

Equation (5.45) provides the initial coefficient values for the force controller model. The co

efficient values are updated at each time step by the EFRA system identification algorithm.

The coefficients are also used to tune the controller by determining the closed ioop charac

teristics of the system for various controller horizons and weighting factors, as described in

section 3.2.1.

Step Response

The 114.9MHz mode in the dynamics mode of the system indicates that the equations of

motion are very stiff. As a result the integration algorithm time step must be very small


10 —

102 101 j2 iøEffective Contact Stiffness [kN/m]

Figure 5.15: Variation of natural frequencies of a single flexible link (with one structuralmode) in contact.

145

k =5.0037MN/moff

I I II

ff=61.55N/rn:

>‘0

a)Dc,.

U(0L..D

z

I I iiiiiil I I iiiiiil I I 111111 I I I 1111111 I I I IIIII


N2 N Overdamped Mode Underdamped ModeDominant Pole fd [Hz] C

18 1 -45.21 857.5 0.30420 2 -96.76 1429 0.35919 3 -84.38 1429 0.56221 4 -99.13 1429 0.239

Table 5.4: Stabilising controller horizons and closed loop characteristics for steel on aluminium contact (keff = 5.0037MN/m) for a single flexible link with one structural mode.fd = damped natural frequency and (= damping ratio.

and long run times are expected. Figure 5.16 shows the initial 12.5ms of a step response

which required 50.3 hours of CPU time to calculate on a Sun SparcStation 2. Also shown is

the response calculated using a steady state approximation of the contact dynamics which

required only 23.2s of CPU time. The results are almost indistinguishable and hence it is

concluded that, under conditions when the contact dynamics mode is well separated from

the other modes of the system, the contact dynamics can be approximated by their steady

state response with substantial savings in computation time and negligible difference in the

computed results.

Using equation (5.45) and the expression for the closed loop transfer function derived

in section 3.2.1, four combinations of prediction horizon (N2) and control horizon (Ne)

which yield stable control were identified. They are listed, with corresponding approximate

continuous time domain closed loop characteristics, in Table 5.4. In all four cases the closed

loop is characterised by one overdamped mode (a pair of poles on the negative real axis)

and one underdamped mode (a complex conjugate pair of poles in the left half plane).

The natural frequency and damping ratio of the underdamped mode are such that its

oscillation disappear long before the response, dominated by the smaller magnitude pole

of the overdamped mode, reaches the setpoint. Figure 5.17 shows the step response of the

adaptive predictive force controller with N2 = 20, N,, = 2, ), = 0, and .Xse = 0. The

response shown starts from a stable 5N contact force level in order to focus on the step


I I I I I I

3.2

______

Steady State3.0 - Approximation

2.8FuliContact

2.6:I

2.4 -

2.2 -

2.0 -

ci) -

C.)

C -

1.6 -

8 1.4 -

1.2 -

1.0 -

.8 -

.4 -

.2 -

0 ‘ I I I I I0 .001 .002 .003 .004 .005 .006 .007 .008 .009 .010 .011 .012 .013

Time [SI

Figure 5.16: Comparison of results from simulations using full contact dynamics and steadystate approximation.


22

20

18

16

14

z12

1000

8

6

4

2

00 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32

Time [s]

Figure 5.17: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (ken = 5MN/m) with N2 = 20, N = 2, ) = 0, and ‘se 0.


response performance of the controller and avoid the complications due to the discontinuity

in the system dynamics which occurs at ON as contact is initiated. Control through the

discontinuity is discussed in section 5.4.

The N2 = 20 and N = 2 combination of horizons offers the best compromise of rise time

and control signal activity. The rise time is related to the magnitude of the dominate closed

loop pole. The closer that pole is to the origin (in the continuous time domain), the longer

the rise time. The rise times achieved with N = 1 and N 3 are both longer than that

with N = 2 and, while the rise time with N = 4 is less than with N 2, the control input

signal becomes excessively active and subject to ringing. In all cases prediction horizons

less than those in Table 5.4 yield an unstable closed ioop and increasing N2 causes the slow

mode to become slightly underdamped, introducing overshoot into the response.

Increasing the control increment weighting factor, , from zero has negligible effect on

the response because the closed ioop poles introduced by are generally close to the origin

in the discrete time domain or they interact with the poles of the fast mode; in either case

the slow mode remains dominant.

Figure 5.18 shows superimposed step response simulations for ‘\se ranging from 0 to

0.4. As .kse is increased the rise time decreases but at the expense of increasing overshoot,

which may be undesirable. It is evident from these observations that the static equilibrium

bias term in the control law acts as a proportional feedforward term in which the gain is

proportional to se

The inherent tendency of the predictive controller to anticipate changes in the setpoint

level is apparent in the step responses shown. With a 20 step prediction horizon and

O.35ms sampling interval the controller begins to respond to setpoint changes 7ms before

they occur. The initial sign of the response to a step change in setpoint is opposite to that

of the step due to the nonminimum phase nature of the system dynamics.


zC)0U

0

0C)

Time [s]

Figure 5.18: Force control step responses for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 5MN/m) with 0 A3e 0.4.

150

18

16

14

12

10

8

6

4

2

00 .01 .02 .03 .04 .05 .06 .07 .08 .09


5.3.5 ke = 62kN/m Contact Stiffness Case

The sampling rate of nearly 3kHz required to obtain the results shown in section 5.3.4

means that implementation of the controller would require very high performance hardware

and actuators. The high sampling rate is required because the effective contact stiffness

of 5MN/m is very much greater than the 7096N/m modal stiffness of the link and thus

the dynamics are dominated by the high frequency mode whose natural frequency is pro

portional to k!ff as can be seen in Figure 5.15. Inclusion of additional structural modes in

the dynamics and controller models results in a dominant mode with even higher natural

frequency and thus higher sampling requirements. However, it must be recalled that the

Hertz contact model used to determine the contact stiffnesses and masses is based strictly

on the local deformations of the surface during quasi-static contact. An, et al. [141 used a

parameter estimation algorithm to determine experimentally the effective contact stiffness

of a steel bearing on an aluminium surface and found it to be 61554N/m. Unfortunately

sensor limitations prevented the determination of effective mass and damping coefficients

from their experimental data. It can be surmised that, while the local effective contact

stiffness of a pair of surfaces may approach that given by the Hertz model, more global

deformation of the bodies results in much lower actual effective contact stiffnesses. This

proposition is somewhat supported by the presence of an initial spike in the data collected

by An, et al. (reproduced as Figure 5.19) which suggests that the contact stiffness upon

initial contact is much greater than that which prevails once contact is well established.

Controller Model

To obtain an effective contact stiffness of 61554N/m the values in Table 5.3 were scaled

to maintain relative stiffnesses of the steel and aluminium surfaces with the result that

the steel surface stiffness becomes k3 2.3930 >< 105N/m and that of the aluminium k =


IC 800007000060000soooo Final Value: 61554 N/rn40000300002000010000

00.0 0.2 0.4 0.6 0.8 1.0

Estimated Stiffness [or Aluminum Surface (N/rn)

Figure 5.19: Experimental contact stiffness estimate for steel on aluminium. Reproducedfrom An, et al. [14]

8.2870 x 104N/m. As can be seen in Figure 5.15, these values imply that, in contrast to

the situation at higher effective contact stiffness, the system characteristics are composed of

contributions from both modes. The natural frequencies of the open ioop modes are 50.65Hz

and 125.0Hz, thus sampling at 384.6Hz (h 2.6ms) satisfies the Nyquist criterion. The

resulting discrete time transfer function model for the controller is:

F(q1) — —2.144 + 3.084 (q1 + q2) — 2.144q3 —1 (5 46)u(q1) 1 — 0.4456q1+ 0.7709q2 —0.4456q3+q_4

Step Response

The approximate closed ioop poles, calculated from the control law and the linearised system

dynamics, show that for N,. = 1 and N,. = 2 all values for the prediction horizon result

in both of the closed ioop modes being uriderdamped. With a 3 step control horizon the

approximate closed ioop poles show that for certain prediction horizons the closed ioop

system has one overdamped mode and one underdamped mode, otherwise both modes are

underdamped. With N,. = 4 there is always one overdamped and one underdamped closed

loop mode. Table 5.5 lists the approximate closed ioop characteristics of the system for

several combinations of prediction and control horizon.


N2 N Overdamped Mode Underdamped ModeDominant Pole fd [Hz] C

5 3 -181.3 192.3 0.2007 3 -99.07 192.3 0.3218 3 -58.56 192.3 0.342

10 3 -190.1 192.3 0.4115 4 -183.7 192.3 0.1426 4 -171.4 192.3 0.1667 4 -79.75 192.3 0.2898 4 -44.51 192.3 0.3019 4 -80.90 192.3 0.312

10 4 -75.68 192.3 0.314

Table 5.5: Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with one structural mode.fd damped natural frequency and C = damping ratio.

The dominant pole locations and damping ratios listed in Table 5.5 suggest that the

N2 10, N = 3 controller will provide the fastest rise time combined with the least 192.3Hz

disturbance. As Figure 5.20 shows, this is indeed the case, however this performance comes

at the expense of the controller adjusting the system well in advance (26ms) of setpoint

changes because of the 10 step prediction horizon. This potentially undersirable anticipation

effect is markedly reduced with N2 5 and N, = 4 without much increase in rise time but

the response exhibits slight overshoots due to the low damping of the 192.3Hz mode (see

Figure 5.21). The next best set of controller horizons is N2 = 5, Ne,, = 3 which produces the

response shown in Figure 5.22. The rise time of this response is slightly greater than that of

the two previous ones but the 192.3Hz mode is sufficiently well damped to avoid overshoot

and the setpoint change anticipation time is limited to l3ms. These results emphasise the

need for simulations in the tuning of the controller due to the approximate nature of the

closed ioop characteristics calculated based on linearised models, and to the fact that those

characteristics reveal nothing about the relative excitation levels of the closed loop modes.


20

18

16

14

z12C)0U

100C)

8

6

4

2

Time [s]

Figure 5.20: Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (keff = 61.554kN/m) with N2 10, N = 3, = 0,‘se =

0 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32


22

20

18

16

14z00

IL 12C)

C0

10

8

6

4

20 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32

Time [s]

Figure 5.21: Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (keff 61.554kN/m) with N2 5, N = 4, = 0,‘se 0.


20

18

16

14

z12

C.)

0U

C.)Cu

1000

8

6

4

2

156

0 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32

Time [s]

Figure 5.22: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (keff = 61.554kN/m) with N2 5, N = 3, \ = 0, ‘\se 0.


Table 5.5 indicates a potential difficulty with the 3 step control horizon in that the

closed ioop system does not always have an overdamped mode. This implies a possible

lack of robustness in that the effects of unmodelled dynamics or sensor noise may cause

the estimated controller model coefficients to change such that controller horizons which

initially provided an overdamped closed loop mode would no longer do so. For this reason

the 4 step control horizon is preferable. Figure 5.23 shows step responses for the controller

with N2 = 7 and N = 4. The responses are free of overshoot and steady state offset with

a rise time of 24ms.

It can be seen from Table 5.5 that the closed ioop characteristics do not vary mono-

tonically with the controller horizons. This is due to the fact that the horizons are integer

multiples of the sampling interval, h, and this results in an aliasing effect.

We now consider the effect of additional structural modes in the link model. Figure 5.24

shows the natural frequencies of the linearised equations of motion for the single link system

with the link structure modelled by the first three mode shapes of a cantilever beam. At

keff = 61.554kN/m the open ioop natural frequencies are 37.49Hz, 113.5Hz, 231.5Hz and

664.9Hz. It is evident from Figure 5.24 that the frequencies of the latter two open loop

modes which arise due to the additional admissible functions are independent of keff

Under these circumstances the results of section 5.2.2 imply that the additional modes

make minimal contribution to the contact force and thus the sampling rate need not be

increased to resolve them. On the other hand, the control input channels energy into all of

the modes and so they should be included in the controller model. The result of theses two

conclusions is the controller model

F,(q1) —

ui(q’) —

—0.41 — 1.43q’ + 2.56q2 + 3.08(q3+ q4) + 2.56q5 — 1.43q6 — 0.41q71 + 0.79q’ + l.50q2 + 0.21q3 + O.64q4+ O.21q5+ 1.50q6 + 0.79q7+

q_8

(5.47)


20

18

16

14

12zci,

F:6

4

2

00 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32

Time [s]

Figure 5.23: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (keff = 61.554kN/m) with N2 = 7, N1, = 4, A, = = 0


I 11111111 I I

159

I I I I liii I I I I I 1111 I I I I I I 1——I-—I—1—i•

k = 61 554 kN/m

-icr

>‘()Ca,zwU

(15I.

D

z

108

1

iø

iø

1

11

1 ø

-+--------

I iiiiiil I I 1111111 I I iiiliiil I iiiiiil 1 111111

iø iø 101 102 iø


Figure 5.24: Variation of the natural frequencies of a single flexible link (El1 = 574N mwith three structural modes) in contact.


Overdamped Mode 192.3Hz Mode Underdamped ModesN2 N Dominant Pole C fd [Hz] C f [Hz] C6 4 -217.5 0.517 105.5 0 192.3 0.3077 4 -157.8 0.584 105.3 0 149.4 0.2018 4 -105.8 0.624 105.3 0 145.0 0.1229 4 -81.56 0.622 105.3 0 151.5 0.140

10 4 -53.99 0.638 105.3 0 143.5 0.125

Table 5.6: Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with three structural modes.fd = damped natural frequency and C = damping ratio.

in which the two high frequency modes are aliased to 153.1Hz and 104.3Hz due to the

relatively low sampling rate. The aliasing of these modes presents little difficulty because

of their minimal excitation.

Table 5.6 lists the approximate closed loop characteristics calculated from the linearised

model of the system for several prediction horizon values with a control horizon of N = 4.

As in the previous cases, the closed loop response is composed of an overdamped mode, the

smallest magnitude pole of which defines the rise time, and an underdamped mode with a

natural frequency of one half the sampling frequency. In addition, the aliased high frequency

modes appear as lightly damped modes in the closed loop. Simulated step responses show

that, as was observed in the case of the one mode link, the fast rise times produced by short

prediction horizons (N2 = 6, 7) are accompanied by overshoot because the underdamped

modes do not die out sufficiently before the response reaches the set point level. Figure 5.25

shows the response with N2 8 which is free of overshoot.

5.3.6 Summary

In summary, this section has illustrated the calculation of the parameters of the link, contact

and controller models based on the physical dimensions and material properties of particular

physical systems. The link model was based on a 1.Om long, 2.0cm diameter aluminium


22

28

18

16

14

z12

.100

C.)

8

6

4

2

80 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32

Time [SI

Figure 5.25: Force control step response for single flexible link (with three structural modes),steel on aluminium contact (keff 61.554kN/m), with N2 8, N = 4, = 0, . = 0.


rod with its structural dynamics modelled using the mode shape functions of a cantilever

beam. For the case of relatively soft (kff = 3100N/m) contacting surfaces, with excitation

of the contact dynamics due to the contact mass being significant compared to the link

moment of inertia, simulation results demonstrate stable control with fast rise time and

minor overshoot.

Contact model parameters calculated from the Hertz contact model (derived in sec

tion 2.5), based on a 4.0mm diameter steel bearing as an end effector on the link contacting

a planar aluminium surface, resulted in very stiff contacting surfaces (keff 5.0037MN/m).

The calculation of the closed ioop response of this system was found to be very time consum

ing due to the mathematically stiff structure of the equations of motion, however a steady

state approximation of the contact dynamics portion of the model was shown to substan

tially reduce the computation time without significantly affecting the results. Approximate

closed loop characteristics calculated from the controller model and the linearised equations

of motion were successfully used to tune the controller for the case of a one mode flexible

link. However, control of this system requires 2857hz sampling and the effective contact

stiffness is so much greater than the link structural stiffness that, for any reasonable number

of admissible functions used to model the link structure, the dynamics are dominated by

the highest frequency mode and an even higher sampling rate is necessary.

Experimental results from An, et al. [14] suggest that the effective contact stiffness of

5.0037MN/m given by the Hertz contact model is an extreme upper bound and that a more

reasonable value is keff = 61.554kN/m. At this level of effective contact stiffness the system

was found to be dominated by the two modes with the lowest natural frequencies, as the

results of the linear analysis in section 5.2.2 suggest should be the case. The approximate

closed loop characteristics, calculated from the control law and the linearised system model,

are used to select stable, robust controller parameters for the case of the link structure

modelled by a single admissible function.


Finally it has been shown that when additional admissible functions are included in

the link structure model the number of modes in the controller model should be increased

but, due to the domination of the system dynamics by the lower frequency modes, the

sampling rate need not be increased. These results establish the value of the plots of the

variation in modal natural frequencies with effective contact stiffness (obtained from linear

analysis) in determining the number of significant modes and required sampling rate for

specific configurations.

5.4 Making and Breaking Contact

The circumstances surrounding the making and breaking of contact between the manipula

tor tip and the environment surface pose a difficult control problem. When contact exists

the contact force provides a measure of the state of the entire manipulator and environment

system as is exemplified by the expressions for the contact force components in terms of

the generalised coordinates that were derived in section 2.5. On the other hand, when the

manipulator tip is not in contact with the environment surface the contact force is, by def

inition, zero and therefore the controlled variable provides no information about the state

of the system. Thus the contact force is a positive indefinite function of the system state

which exhibits a discontinuity at zero.

This lack of information in the absence of contact presents two difficulties for the force

control algorithm. Firstly, the control input levels which the force control algorithm calcu

lates in attempting to make or regain contact will not reflect the position or velocity of the

manipulator tip with respect to the surface. In most cases this will result in the tip even

tually arriving at the surface with a substantial contact velocity as the controller attempts

to remove the error between the force setpoint level and the zero contact force. Without

special actions the force controller is likely to have difficult controlling the force transient


which results from a non-zero contact velocity. The second difficulty arises in the operation

of the system identification algorithm used in the adaptive controller because the algorithm

updates the model coefficients on the basis of the difference between the actual and pre

dicted output with the tacit assumption that the model and the system being identified are

continuous through zero.

In order to overcome these difficulties it is necessary to control the motion of the manip

ulator when it is not in contact as well as the forces it exerts on the surface when it is. The

predictive control algorithm derived in Chapter 3 can be readily applied to the task of con

trolling the manipulator joint angles simply by providing it with a transfer function model

which relates the joint angles to the control inputs. To successfully use the combination of

force and motion controllers to operate through the discontinuity when contact is made or

broken a level of control above the basic predictive control algorithm is introduced. This

higher level control is referred to as the Contact Control Logic. Like the circumstances it

is intended to deal with, it is discontinuous and, to some extent, heuristic in nature. On

the basis of sensor inputs and data supplied by the operator and the trajectory generator

the contact control logic generates the setpoint programs for the predictive control algo

rithm, selects between force and motion control algorithms and chooses whether or not to

allow system identification to proceed. The relationship of the contact control logic to the

manipulator, sensors and other elements of the control structure is shown in Figure 5.26.

The contact events which must be dealt with by the contact control logic can be broadly

classified as expected (or intentional) and unexpected. An example of an intentional contact

event is the situation where the operator or task planner accurately knows the location of

the surface to be contacted. Under these circumstances motion control can be used to

move the manipulator tip to a location adjacent to the surface and to reduce its approach

velocity sufficiently to avoid a violent collision and accompanying high level of transient

contact force. From this state a smooth transition to force control can be made. The role


Figure 5.26: Block diagram of the overall control structure.


of the contact control logic in this situation is to switch between motion and force control

at the appropriate time as directed by the operator or task planner.

In contrast, an example of an unexpected contact event is the situation when, due to

imperfect knowledge of the manipulator’s working environment, it collides with a surface.

If the collision is due to an object being unexpectedly in the path of manipulator motion

the contact control logic must take measures to minimise the collision force and bring the

manipulator to rest, pending corrective action from the operator or task planner. On the

other hand, if the collision is due to inaccurate knowledge of the location of a surface on

which the manipulator was intended to exert force the role of the contact control logic is

to switch to force control mode, minimise the force transient that results from the collision

and reach the desired force level setpoint. This latter case is the focus of the remainder

of this section. Two operational strategies for the contact control logic which seek to

achieve sustained, controlled contact force control following an unexpected contact event

are proposed and their performance is examined by way of simulations. The analysis, results

and discussion presented in this section have already been published [27].

5.4.1 Stop Motion Strategy

The first, relatively simple, strategy is referred to as the stop motion strategy. The contact

control logic is programmed such that the control mode is switched from motion to force

when a non-zero contact force is sensed and motion control is resumed if contact is broken.

In addition, when contact occurs the motion control setpoint is changed so that when the

motion controller resumes operation, rather than continuing the with motion that that

was being executed prior to contact, the joint angles at which contact was made will be

maintained.

Figure 5.27 shows the results of a simulation in which this strategy is used. A single

link manipulator is used, modelled on a 1Mm long, 2.0cm diameter aluminium rod with the


20

i:; 150Co 100

(b) Contact Force Response

167

Figure 5.27: Joint angle and contact force responses for a single flexible link colliding with a6200N/m surface. Control mode switching is based on sensed contact force and the motioncontrol setpoint is modified due to the contact event.

(a) Joint Angle Response

30

Time [sJ


structural dynamics represent by the first two modes of the rod considered as a cantilever

beam. The link is initially in motion with constant joint velocity toward a surface with

a contact stiffness of 6200N/m. The predictive control algorithm parameters are a delay

horizon (N1) of 1 time step, a prediction horizon (N2) of 40 time steps, a control horizon

(Na) of 12 time steps and a control increment weighting factor () of 1.0. The controller

time step is 4.Oms. The motion controller is adaptive but the model coefficients for the force

controller are held constant. A non-adaptive (i.e. fixed model coefficients) force controller

has been used in order to focus attention of the issues surrounding the transition from

motion to force control. The problems that arise when adaptive force control is used during

this transition period are discussed below.

The simulated joint angle and contact force responses (solid lines) and corresponding

setpoints (dashed lines) are shown in Figures 5.27a and 5.27b, respectively. The motion

controller shows good tracking of the joint angle ramp during the initial part of the maneu

ver. The desired angular velocity of the joint is 0.5rad/s which gives a nominal velocity at

the link tip of O.5m/s (neglecting the vibrational motion of the flexible link). After 0.257s

the link tip contacts the environment surface with a velocity of 0.536m/s. At this point

the control mode is switched from motion to force due to the sensing of a non-zero contact

force and the force controller attempts to bring the system to rest at the specified contact

force setpoint of 8.ON. Upon switching control modes the motion control setpoint for future

use is modified to hold the joint angle stationary at 7.56°, the joint angle at which contact

was detected. Due to the non-zero velocity of the link tip at the instant of contact the

contact force rises rapidly to approximately 16N and, before the force controller can have

any significant effect on the system the link rebounds from the surface and motion control

is resumed. This cycle is repeated twice more with force control being established on the

third contact.


After sustained contact has been achieved an adaptive force controller (i.e. one for which

the model coefficients are updated using a system identification algorithm at each time step)

is the correct choice. However a non-adaptive controller (i.e. fixed model coefficients) is

required during the transition period while the tip is bouncing on the environment surface.

During this period the brief intervals of rapidly changing force levels interspersed with

periods of zero force cause the system identification algorithm to produce wildly inaccurate

estimates of the model coefficients. Using these coefficients in the control law can result in a

loss of control. A solution to this problem is to delay the start of force model identification

until sustained contact is assured. A delay of 25 time steps is adequate for the case shown

in Figure 5.27. However, this approach requires that some means be devised to decide

that sustained contact has been achieved; a difficult proposition considering the number of

factors on which the duration of the transition period (number of bounces) depends.

In addition to the difficulties involved in using adaptive force control during the transi

tion period, the strategy of bouncing on the surface until force control can be achieved is, for

most applications, not acceptable. Furthermore, it has been observed in other simulation

runs that the number of bounces can be much greater than the three seen in Figure 5.27b,

depending on the dynamic characteristics of the link and the surface. Worst of all, there

is, apparently, no guarantee that stable force control will in fact be achieved at all using

this strategy. While various refinements of the motion control strategy during the bounces

might produce better results, observation of the contact force transient which occurs during

the first bounce suggests another course of action.

5.4.2 Force Setpoint Modification Strategy

Comparison of the contact force transient during the first period of contact in Figure 5.27b

with the corresponding transient for a collision between the link and the environment at the

same velocity during which no attempt is made to control the contact force shows very clear


Figure 5.28: Simple 1 degree of freedom model of a single link in contact with an environment.

similarity between the transients. In both cases the transient closely resembles the positive

portion of a sinusoid, the period of which correlates closely with the natural frequency of

the first mode of the open loop system. Based on this observation it is apparent that the

force controller has almost no effect on the contact force level immediately following contact

and that the response of the system at that time is entirely due to the open ioop dynamics,

in particular the velocity of the link at the instant of contact.

This suggestion is confirmed by consideration of the simple single degree of freedom

model of the link and contact dynamics shown in Figure 5.28. The equation of motion of

this system without forcing is

I8 + kL28 = 0 (5.48)

Given initial conditions at t = 0 of 8 = 0 and & =&, the particular solution of equation (5.48)

iskL2

8(t) = —sinwt, wherew2 =

10k

/

Li.)(5.49)


In this model the contact force is the force generated by the compression of the spring, that

is

F(t) = kLO(t)

kL8sinwt

w

= Fksinwt, (5.50)

where Fk=

With Ic set to the value of the effective stiffness of the link and environment surfaces

(considered in series) and 10 set equal to the rigid body moment of inertia of the link

about the joint axis, equation (5.50) gives a very good prediction of the initial contact force

transient shown in Figure 5.27b.

Note that the contact force transient described by equation (5.50) is entirely a result of

the initial condition velocity, which correlates with to the velocity with which the link tip

contacts the surface. The classical form of transfer function model on which the predictive

force control algorithm is based, however, assumes quiescent initial conditions (that is, the

system is stationary at the instant of contact). Thus, a fundamental incompatibility exists

between the system for which control is desired and the model on which the controller is

based.

Viewing the situation in terms of energy, the force controller expects the system to have

zero energy when it begins operation. The system however has non-zero energy, in the

amount of the kinetic energy of the link at the instant of contact. If stable force control is

to be achieved without letting the link bounce off the surface the controller must overcome

the initial energy of the system. This is confirmed by simulations similar to those shown

in Figure 5.27, in which higher contact force setpoints are specified. In those simulations

the initial portion of the contact force transient is similar to the corresponding uncontrolled

collision transient, however the force controller (which is more energetic by virtue of the


z0C.)0I]4.-C-)(U4.-

0C-)

172

.0 .1 .2 .3

25

20

15

10

5

0

Time [s]

Figure 5.29: Contact force response for a single flexible link making contact with a 6200N/msurface at 0.536m/s.

higher setpoint it is seeking to achieve) overcomes the initial energy of the system and brings

the system to rest at the desired contact force level. Figure 5.29 shows the contact force

response with a setpoint of 20N. As a consequence of these observations a force setpoint

modification strategy is proposed for the contact control logic.

Differentiating equation (5.50) with respect to time and solving for k at a time immedi

ately after contact when t is small gives

1(t)k —v—-. (5.51)

L6

This result, combined with Fk = /J6 and w2=

from equations (5.50) and (5.49),

make it possible to use the joint encoder data just prior to the instant of contact and the

force sensor data immediately following contact to estimate the peak force and the duration


20

15

zci)C)

10

C0

C)

5

0

Time [sj

Figure 5.30: Contact force response of a single flexible link colliding with a 6200N/m surfacewith force setpoint modification.

() of the contact that will occur if the force controller does not act. The peak force is

effectively a measure of the initial energy of the system which the controller must overcome.

In order to do this the specified setpoint is compared to the estimated peak force. If the

setpoint is found to be less than the estimated peak the former is modified to the estimated

peak force level for a period of time equal to predicted duration of the uncontrolled collision

contact transient. This enables the controller to exert the energy necessary to overcome the

initial kinetic energy of the link.

Figure 5.30 shows the contact force response for the same conditions as that shown in

Figure 5.27 but with contact transient prediction and contact force setpoint modification

included in the contact control logic. Contact occurs at 0.257s with a link tip velocity of

.0 .1 .2 .3


0.536m/s. At 0.260s the force controller estimates the effective stiffness of the contactingsurfaces to be 3041N/m (compared to the actual value of 3100N/m) and predicts a peak

force of 15.69N and an uncontrolled contact duration of 30.2ms. Based on these predictionsthe contact force setpoint is raised to 15.69N for 30.2ms and a stable 8.ON contact force

level is achieved without the link leaving the surface as happened in Figure 5.27b. The initial

overshoot in Figure 5.30 is largely due to the initial contact velocity of the link and cannotbe removed by a controller which has no provision for non-zero initial conditions. In anycase, the overshoot is significantly less that that which eventually occurs in Figure 5.27b.

The model, based on equation (5.50), used to predict the peak contact force and cor

responding uncontrolled contact duration is a very simple one. The necessary calculations

require a total of 2 additions, 5 multiplications and 2 square root operations. (Two ad

ditions and one multiplication can be eliminated from this total if the force rate can be

obtained from the force sensor.) As such, the setpoint modification can easily be put into

effect within one or two time steps after contact is sensed. Furthermore, it has been noted

in simulation results that a lag of one or two sampling intervals between the instant of

contact and the completion of the prediction calculation (and application of the setpoint

modification) does not adversely effect the control.

5.5 Force Control for Two Link Manipulators

Consideration of a manipulator with two or more links greatly increases the complexity of

the analysis and control problems. In this section force control for planar manipulators with

two links is addressed. Both rigid link and flexible link manipulators are considered. The

two joint torques available as control inputs for a two link manipulator permit control of

two world frame components of contact force.


In this what follows, similar analytical techniques to those used in section 5.2 are em

ployed to gain insight into the modal structure of the system and the results are applied

to the selection of appropriate controller parameters. Because of the strongly coupled na

ture of the system the multivariable control algorithm is necessary. The derivation of an

appropriate multivariable controller model from the linearised equations of motion is pre

sented. Simulated step responses are used to evaluate the performance of the closed loop

force control for both rigid link and flexible link manipulators.

5.5.1 Linear Analysis

In this section a linear analysis of a two link manipulator in contact with a deformable en

vironment is presented. The analysis follows the same general approach that was employed

for a single link system in section 5.2.

Rigid Links

We begin by considering the case of rigid links which is the limiting case of infinite link

stiffness. As well, the characteristics of rigid link manipulators are of practical interest in

their own right since they typify the vast majority of existing industrial manipulators. Fig

ure 5.31 shows the configuration and important parameters for a manipulator with two rigid

links exerting forces on the environment in the r and y world frame coordinate directions.

The equations of motion for this system are given in section A.2. Neglecting damping,

gravitational effects and inertia coupling between the links and the contact region mass,

replacing the joint torques {Tj(t),r2(t)} with the general control inputs {ui(t),u2(t)} and

linearising about the nominal state

{ B2 Ex Lii x y ] = [ 0 ... 0 j


key

k m5+ me

w2(x2,t)

102 L2 M2

y w1(x1,t)

U2

ILM01 1

x

I

Figure 5.31: Two rigid link manipulator in contact with a deformable environment.


gives

[101 + 102 + M2 (L +L1L2C) + m (L + L + 2L1L2c)] 1

+ [102 + M2L1L2c + m (L +L1L2c)]

—m [L1.s + L2s2j + m [Lic + L2c2]

= u1(t)— k5 (List + L2s2)[(Ljs + L2s2)Oi + L2s202+ j

—k (Lic + L2c2)[(Lic + L2c2)0 + L2c202—

(5.52)

[I2 + M2L1L2c+ m (L +L1L2c)] i + [102 + mL22] 62

—mL2s2ë+ mL2c2ë

= u2(t)—k3L2s2[(List + L2s2)0 + L2s202+ e]

—k8L2c2[(Lic + L2c2)8 + L2c202— (5.53)

m(Lis + L2s2)8i + mL2s262— më = —k5 [(List + L2s2)0 + L2s282+ e] — kexEx

(5.54)

m (Lic + L2c2)01 + mL2c262+ mi = —k8 [(Lic + L2c2)0 + L2c202—

—

(5.55)

where the generalised coordinates {0, 02, e, e} are now perturbations about the nominal

state.

Figure 5.32 shows the numerically calculated eigenvalues of the system with the following

parameters: 0 = O’, = 90°, 101 = 102 = 0.2817kg m2, M2 = 0.8451kg, L1 = L2 = 1.Om,

and = kex = k8 key. The eigenvalues are shown for the range of effective contact

stiffness, keff, from 50N/m to 5MN/m. As was seen in Figure 5.3, the upper frequency

line in Figure 5.32 is well separated from the other frequencies. That line is actually two

superimposed lines representing the natural frequencies of the contact mass vibration modes


1 4

‘Ic’

>‘C)Ca)

a)U-

- 1

z

10’

101 —

1


Figure 5.32: Variation of the natural frequencies with effective contact stiffness for a twolink manipulator with identical, rigid links in contact with an isotropic (kxeff = kyeff) environment.

1 6 I 11111111 I I 1111111 I IIIIII I IIIIIIj I I

113

1/2 1/2

_L rksx÷kexl L ksy+key3 2t Lm+ mej 4 2r [m÷ me

178

14

1 ø

111

.1

101 1 3


( and 7)4) in the x and y coordinate directions. These lines are closely approximated by

lk+k 2 1 k+kf =

— sX exand f =

— sj ey(5.56)

27r m5 + me 2K m3 + me

They are coincident because k = kex k811 = key for the case shown.

For most cases of practical interest the contact mass is small compared to the other

inertias in the system with the result that the high frequency modes associated with its

local vibration have minimal effect on the rest of the system. As was done in section 5.2.2,

we now introduce a steady state approximation for the contact dynamics and focus the

analysis on the remaining modes. Neglecting the contact region mass, m, and replacing

the directional contact stiffnesses, k8 and kex, k8 and key, with the corresponding effective

contact stiffnesses, kff and kyeff, allows the linearised equations of motion to be reduced

to:

[101 + 102 + M2 (L + LiL2c)] ë1 + [i + M2LiL2c]2

= ui(t) — kxeff (List + L2s2)[(List + L2s2) +L2s2e2]

kyeff (Lic + L2c2)[(Lic + L2c2)8 +L2c282] (5.57)

[102 + M2LiL2cj +

= u2(t) — kffL2s2[(List + L2s2)6i +L2s282j

kyeffL2C2[(Lic + L2c2)&i +L2c282]. (5.58)

After a great deal of algebraic manipulation the characteristic equation for this system

can be written as

+ M2L) 102 — MLLc2] (jw)4

+ [[I +102 + M2 (L + LiL2c)j (k11ff + zks2) L

+102 [kyeff (L + L + 2L1L2c4) + Ak (List +L2s2)2]


— (2102L2+M2LJ) [kyeff (Lic + L2) + txk (List + L2s2)S2]} (jw)2

+kxeffkyeffLL.32= 0, (559)

where Lik = kxeff — k1jeff. The structure of equation (5.59) provides important insights into

the physical behaviour of the system. Firstly, note that the substitution, kxeff kyeff + 1k,

used to arrive at equation (5.59) separates the overall effects of the contact stiffness from

those due to a difference in the contact stiffnesses in the x and y directions. In particular, this

substitution reveals that the nominal joint angle of link 1, 8, affects the natural frequencies

oniy when kff kyeff. Secondly, in the case of exact alignment of the links (8 00 or

180°), the (jw)° term disappears. This circumstance results in one of the natural frequencies

of the system being zero which implies that the two components of contact force cannot

be simultaneously controlled because the manipulator lacks sufficient degrees of freedom.

This is an analogous situation to the familiar degenerate condition which arises in motion

control when planar links are exactly aligned.

Equation (5.59) can be solved to yield an expression which closely approximates the

natural frequencies of the and 2 modes. For the cases typified by Figure 5.32 (identical

links and keff kyeff keff) we have:

(,w)2

= 2 (z + M2L2)— MLc2 [_ (i + M2L2s2) + (i + ML4s2) 2]. (5.60)

While this expression is somewhat more complex than those obtained in the case of a single

link (equation (5.11)), it displays the came general structure. The natural frequencies

are directly proportional to kff and L and inversely proportional to the square root of a

combination of inertias. This expression also reveals that the frequencies of the and

72 modes vary between bounds as & varies modulo 90°. At 0 = 00 the manipulator

configuration is degenerate and the frequency of ij attains its minimum value, namely zero,

while the frequency of ?)2 is at its maximum. At O = 90° the frequencies are in the ratio


of 1 to 2 and the frequency of is at its maximum and that of i2 is at its minimum. The

variation if the frequency of i is much larger than that of 72.

Returning our attention to the model which includes the full contact dynamics, we now

consider the eigenvectors of the system and, in particular, the information they provide

about the contributions of the natural modes to the contact force components and the exci

tation of those natural modes by the control inputs. As in the case of a single rigid link, the

eigenvectors, normalised to unit magnitude, are found to be independent of effective contact

stiffness for the case where kxeff = kyeff. With the parameter values used in Figure 5.32 the

eigenvectors are

E [ei e2 e3 e4]

—0.667 —1.286 x 1O_12 —2.962 x iO’ —2.662 x 1O_6

0.667 0.894 1.065 x i0 2.662 x 106(5.61)

8.087 x 10_13 —0.147 1 4.311 x 10u1

0.333 —6.433 x iO’ 1.070 x lO 1

With these numerical values the transformation between the generalised coordinates and

the natural modes of the system,

‘1)1

=ET , (5.62)

174

clearly shows the isolation of the and modes (high frequency vibration of the contact

region mass) from the and 772 modes. The energy of the 772 mode is concentrated in

the oscillation of link 2 about its joint. The h mode includes oscillations of both links

about their joints in equal proportions of opposite sign. This particular arrangement of the

contributions of the 8 and 62 generalised coordinates to the 77i and 772 natural coordinates


is due to the nominal joint angles, t = 00 and t9 = 90°. The effect on the 7h and 12

eigenvectors of changing the nominal joint angle of the second link is shown in Figure 5.33.

The relatively small contribution of the 8 generalised coordinate to the 2 mode decreases

as increases, as shown in Figure 5.33b. This trend indicates that the majority of the

energy associated with the ‘]2 mode is in the form of oscillations of link 2 about its joint.

Figure 5.33a shows that the i mode combines contributions from 6 and 2 with opposite

signs, reflecting the coupling between the links. The ratio of the components is determined

by the lengths and inertias of the links. In the case shown the links are identical and the

ratio approaches -2 to 1 as the links are aligned and -1 to 1 as they become perpendicular.

The linearised contact force model equations, obtained from equations (2.56) and (2.57)

by neglecting damping and structural flexibility terms, are:

F c2

rhereEs

Eu

= [ - (List + L2s2) -L2s2 (±: - i) o] (5.63)

and

F , where

= k5- [ (Lic + L2c2) L2c2 0 (:: - i) j. (5.64)

\\ \\\\

\I\

)r.

3r..

)

\‘‘

IIIi

iiii

II

\\\\\

c•-

jo

—

\\\\

CC

CC

C

.1

•I

I•

II

•I

I•

cc

Gen

eral

ised

Coo

rdin

ate

Com

pone

nts

ofN

atur

alM

ode

&

Gen

eral

ised

Coo

rdin

ate

Com

pone

nts

ofN

atur

alM

ode

II

II

—

0; CD cz C CD CD

C C0 C

I I1 I I


Transforming these expression into natural coordinate space we find that, with the eigen

vectors given in equation (5.61),

0 77i

F = —0.4472 772(5.65)

0 773

—5,3248 x 10—6 77

andT

0.3333 77i

F —6.4319 x 1013 772= (5.66)

—1.3312 x 10-6 77

—1.4810 x 10_17 77

These results show that the contact forces, F and F, are composed almost entirely of the 772

and 771 modes, respectively, that is, the high frequency vibrations of the contact region mass

contribute minimally to the contact forces. As would be expected from the above discussion

of the natural frequencies, the contact force components in natural coordinate space are

independent of & when Lk = 0. The effect of varying O in the range from 00 to 90° is

shown in Figure 5.34 where the values of the 7i and 712 components of F and F are plotted

for identical links and equal directional contact stiffnesses with keff = 61.554kN/m. When

& is small Figure 5.34b shows that F is dominated by the 772 mode, reflecting the fact that

a motion about joint 2 results primarily in a force in the y direction. For intermediate values

of i9 the motions of both links contribute to F and it is therefore composed of components

of both 77i and 772 At O = 90° the 71i mode dominates F because, in that configuration,

only motion about joint 1 can produce a force in the y direction. The degenerate state

of the manipulator which occurs when the links are aligned is reflected by the zero values

of both the ‘i1 and 72 components of F shown in Figure 5.34b. As éL increases the 772

component increases in magnitude to a maximum at 0 = 90°, in which configuration only


0

- .05

-.10- 0CLI..

-.15

- 25= a) j2

- .300EZ 8 - .35

- .40

45I I I •

0 10 20 30 40 50 60 70 80 90

o [degrees]

(a) i and 12 components of F

.35

.30

.25

.20

0u .15 /.10

.05

0a) .-

- 05

- 10- 0.0 E -.15Zo

0 - .20

- .25-

. 30

—.35 .• I • • I •

0 10 20 30 40 50 60 70 80 90e [degrees]

(b) and ]2 components of F

Figure 5.34: Variation of the components of F and F in natural coordinate space with 6for identical rigid links with equal directional contact stiffnesses, keff = 61.554kN/m.


motion about joint 2 can produce a force in the x direction. The i1 component of F is due

to the coupling between the links.

Finally, we consider the relative excitation of the natural modes by the control inputs,

u1 and u2, by using ET to transform the input vectors, [1 0 0 01T and [0 1 0 01T

from generalised to natural coordinate space. As in the single link case, the excitation of

the modes corresponding to the local vibrations of the contact region mass are found to be

negligible when that mass is small compared to the link inertias. The excitations of the

other ( and 1)2) modes are shown in Figure 5.35 for values of éL between 00 and 90°. The

geometric relationships which were discussed above in the context of the contributions of

the 8i and 82 generalised coordinates to the natural modes are reflected in the data shown

in Figure 5.35. The u1 input primarily excites the mode, particularly as the links become

perpendicular. On the other hand, u2, excited both the and the 1)2 modes when the

links are nearly aligned, with the excitation of 1h decreasing somewhat as the links become

perpendicular. The u2 input excites both modes, regardless of the configuration of the

manipulator, because an input at joint 2 not only drives link 2 but also produces a reaction

force on the tip of link 1.

In summary, linear analysis of the contact case for a two link manipulator with infinitely

stiff links under the assumption that the contact region mass is small compared to the link

inertias reveals that:

1. the natural frequencies are proportional to k for a given set of link lengths and

inertias;

2. the natural frequencies of the contact region mass vibration modes (i and ij) are

widely separated from those due to the links oscillating about their joints (‘ii and 1)2);

3. the natural frequencies of the i and 1)2 modes are independent of 6 when kseff = kyeff

and, for a given keff, are bounded above and below as 8 varies modulo 90°;


0

- .05

-.10

-.15

.94

.92

.90

.88

.86

.84

.82. x

lii .80LQ)

.78

.76

.74

72

70

.68

.650

Figure 5.35: Relative excitation of thefor a two link rigid manipulator in contact.

187

Caa) .;

o0

. xLu

4-.

U)

11

- .20

- .25

- .30

- .35

10 20 30 40 50

o [degrees]

(a) Joint 1 input, u1

60 70 80 90

10 20 30 40 50 60 70 80

o [degrees]

(b) Joint 2 input, u2

90

7li and 72 modes by the control inputs, u1 and u2,


4. the contact forces, F and F, are composed primarily of the and 2 natural modes

in proportions which depend on the nominal manipulator geometry; and

5. the control inputs excite only the and 2 modes.

For the purposes of contact force control using a discrete time algorithm these results imply

that the high frequency modes can be ignored and the sampling interval selected to satisfy

the Nyquist criterion for the 2 mode frequency when 8 = 00. Employing the multivariable

form of the predictive control algorithm developed in Chapter 3 with a controller model that

properly includes the effects of coupling (as discussed in section 5.5.2) allows the system to

be easily controlled. The presence of two undamped open loop modes means that a control

horizon of N = 2 is needed. The performance of the force control algorithm for this case

is demonstrated in section 5.5.3.

Flexible Links

The equations of motion for a two flexible link manipulator making contact with a deform-

able environment were derived in Chapter 2 (equations (2.47) through (2.52)). Applying

the steady state approximation for the contact dynamics, linearising about the nominal

manipulator joint angles O = i9 and 62 = 6 (with all other state variables nominally zero),

neglecting damping and gravitational effects, and considering only one flexible mode for

each link, the equations of motion are simplified to:

[r + 102 + M2 (L +L1L2c)] i

+ + M2 (L1 + L2c) &lo + (102 +M2LiL2c)o] ii

+ (102 + M2LiL2c]2 + [1121 +L1cI421]21

— [kff (List +L2s2)2+ kyeff (Lic H- L2c2)2] 81

— [kxeff (s4ii0 +L2s2410)(L1s+ L2s2)+ k,eff (cq.ii0 +L2c2q10)(Lic + L2c2)]‘b


— [keffL2S2(List + L2s2)+ kyeffL2C2(Lic + L2c2)]82

— [kxeffs2q2lo (List + L2s2)+ kyeffc2qS2lo(Lic + L2c2)] ‘j&21

(5.67)

[im + M2 (L1 + L2c) 11o + (102 + M2L1L2C2)11o] i

*1”+ [I21 + M2 (11O +L2c2&10) + +M2L2c&io) 11

+ [Io21o +M2L2c;iio] 2 + [I12j10+cI42iii0j21

=— {kxeff (sii0 +L2s2q10)(List + L2s2)+ kff (cq110 +L2c2b10)(Lic + L2c2)j 01

— {kxeff (Sii +L2s210)2+ kyeff (c110 +L2c210)2+ 1311] ii

— [kxeff (q11o +L2s2q10)L2s2 + kyeff (cq5iio +L2c2410)L2c2]02

— [kseff (sqiio +L2s2410)S127210 + Icyeff (cqii0 +L2c2410)c2qS210]b21

(5.68)

[102 + M2LiL2c;] i + [1oio +M2L2ciio] ii + I& +

= u2(t) — [kxeffL2S2(List + L2s2)+ kyeffL2C2(Lic + L2c2)jOi

[keffL2S2(sqSii0 +L2s2q10)+k1ieffL2C12(c41i0+L2c24410)jiI)ii

— ceff128l2+ kyeffLC] 02 — [kxeffL2s2s2q!2lo+ kyeffL2C2C242lo]l/’21 (5.69)[k 2*2

[1121 + LicI42i1i + [I1211 +cI421qS110]bi + 112102 + I22121

— [kxeffS24)21o (List + L2s2)+ k,effC22lo(Lic + L2c2)j 81

—[keffs2qS2lo +L2s2q410)+ kyeac2q2io(cq110 +L2c2qS10)1b11

[kxeffs2q21oL2s2+ kyeffCl24)210L2C12]02

*2 i2*2 i2— [kxeffsi2c2io+ kyeffc12q210 + 1321j I’21. (5.70)


Even in this greatly simplified form an algebraic analysis of the system is difficult to ac

complish as well as to gain insight from. However, the insights obtained through the single

link analysis Section 5.2 and the two rigid link analysis above provide guidance as to the

expected characteristics of the system. These characteristics will be explored numerically.

Figure 5.36 shows the variation in natural frequencies with effective contact stiffness for

a manipulator with identical links, each with a single structural mode, in contact with a

surface having identical contact stiffnesses in the x and y directions. The nominal manip

ulator joint angles are = 00 and 6 = 900. The characteristic pattern of the frequency

lines for the four modes ( through i) with the lowest natural frequencies is recognizably

analogous to the single flexible link case. The pattern implies the same type of transition

seen in the single flexible case, namely domination of the system dynamics at low contact

stiffness by low frequency, rigid link modes (iii and 2) giving way to domination by the

higher frequency, structural modes (m and i) at high stiffnesses. The lines for the 1i and

772 mode natural frequencies are asymptotic to the frequency lines for the corresponding two

rigid link case when the contact stiffness is small. As the effective contact stiffness increases

these frequencies both become independent of keff. The link structural modes each intro

duce a mode (773 and i) whose natural frequency at low contact stiffnesses is independent

of keff while at high stiffnesses the frequency is proportional to kff. The lines for the highest

frequency modes ( and 776) are superimposed in Figure 5.36 because the contact surfaces

are isotropic (keff = kyeff). These modes are the local vibrations of the contact region mass

in the x and y directions.

Other characteristics of the two flexible link system that follow the trends observed in

the earlier linear analyses are:

1. the natural frequencies of the four low frequencies modes are independent of O when

kxeff = kyeff and, for a given keff, they are bounded above and below as O varies


1 6

N

>C.)

ci)D

I

1 ø

1ø_2 iø


Figure 5.36: Variation of the natural frequencies with effective contact stiffness of a twolink manipulator with identical, flexible (El = 574N m2, one structural mode each) linksin contact with an isotropic (kzeff = kyeff) environment.

1 g_1 1 1gi

1 1


modulo 900; furthermore, the frequency of the ‘h mode varies over a much larger

range and in opposition to the frequencies of the other modes;

2. additional admissible functions in the link models introduce additional intermediate

frequency modes, each of which dominates the dynamics over a specific range of keff

in a manner analogous that for the single link case shown in Figure 5.10;

3. in the range of contact stiffnesses where they are independent of keff, the natural

frequencies of all but the two highest frequency modes are proportional to the flexural

rigidities of the links;

4. the centres of the ranges of effective contact stiffness values in which the transitions

among dominant modes occur are also proportional to the flexural rigidities of the

links in the same fashion as illustrated in Figure 5.9.

There are two features of particular interest in Figure 5.36 which are not explained

by the linear analyses of the simpler configurations. Firstly, the fact that the and 772

frequency lines both asymptotically approach the same frequency for large values of keff.

This coincidence of frequencies is due to the links having identical lengths and inertias.

Secondly, the curvatures of the and 773 mode frequency lines in the transition range of keff

are somewhat sharper than the curvatures of the 772 and mode lines. This characteristic is

a result of there being, in reality, two overlapping transition ranges. The overlap is also due

to the links being identical. Figure 5.37 shows the situation when link 2 is twice as long and

twice as massive as link 1. The pattern of transfer of dominance from one mode to another

is greatly complicated by the different link lengths but the established characteristics are

still recognisable.

The variation with keff of the contributions of the natural modes to the contact forces

illustrates the affect on control of the transfer of dominance among the various modes. The

Nz>C)Ca)

a)U

(L..

z

I I IIIITT I I 1111111 I I 1111111 I I 1111111 ,1

Ti

Ti


i05

14

iø

1

101

101


Figure 5.37: Variation of the natural frequencies with effective contact stiffness for a twoflexible link manipulator (El = 574N m, one structural mode each) with L1 = 1.Om and

= 2.Om.

Ti2

111

10_i ig

111i1111 I i i

101 1


linearised contact force model equations for the case of two flexible links, each with a single

structural mode are

‘I’ii

02F = c , where

21

Ey

me= k8— [ — (List + L2s2) — (sii +L2s210) —L2s2 (‘ — i) o]

(5.71)

and

01

02where

I’21

Ex

E!I

= k3y [ (Lic + L2c2) +L2c210) L2c2 Ci21io o (rn— i) ].

(5.72)

Figure 5.38 shows the results of the transformation of equations (5.71) and (5.72) from

generalised to natural coordinate space with the same parameter values as were used in

Figure 5.36. The transfer of dominance from the 2 to the 7)4 mode in F and from the i

to the 773 mode in F is shown clearly in Figure 5.36. The i and 7)6 modes make minimal

contributions to F and F.

The relative excitation of the natural modes by the control inputs is shown in Figure 5.39.

As was found in the linear analysis of the single flexible link, we see that the control inputs


.20

.15

.10

.05

0

- .05

-.10

-.15

- .20

- .25

- .30

- .35

- .40

- .45101 102


(a) F components

1 4

111

a)

.z 0o LL

Dc:4.. 0

a)

CO

ZoC-)

• 35

• 30a)

D 0Ow

.20

.15c’oZ_ .10

0a) +-Oc= a)c’cco 0L.

z - .05()

-.10

-.1510_2 102 i04


(b) F components

Figure 5.38: Variation of the natural mode components of the contact forces with effectivecontact stiffness for a two flexible link manipulator. F components are normalised by k3and F components by k3.

2 4

13

1O 101


1.: ..:.*..... .**

L U) .44-.

U)Q .2

.1 1121411516

0

10_i i00 101 102 i03 i04


(a) Joint 1 input, u1

1.0 I I I

.8

12 14

iø_2 10_i iø 101 102 iø


(b) Joint 2 input, u2

Figure 5.39: Variation of the relative excitation of the natural modes with effective contactstiffness for a two flexible link manipulator.


excite all but the high frequency modes across the entire range of keff. For the particular

configuration of identical links with O = 900 the input at joint 1 excites modes and 773,

those in which the energy is mainly associated with the link 1 generalised coordinates, 8

and ‘çb. Due to the coupling of the links the u2 input excites all of the first four modes but

the 772 and 774 modes are excited to a greater degree than the i and 173 modes.


In order to control the contact forces exerted in the x and y directions by the tip of a

two link manipulator a multivariable control algorithm must be used. The model on which

such a control algorithm is based accounts for the effects of both of the inputs, u1 and u2,

on each of the outputs, F, and F. Attempts to control the system using a pair of single

input, single output loops generally fail because of the highly coupled nature of the system

dynamics.

The multivariable predictive control algorithm derived in Chapter 3 requires a linear

model of the system in the form of discrete time transfer functions relating both of the

inputs to each of the outputs. With the equations of motion linearised about a nominal

state, transformation using the Laplace operator allows the system to be described by a


matrix equation of the form:

8(s) 1 0

0 0

0 0

02(5) 0 1 ui(S)J(s) . (5.73)

b2i(s) 0 0 ui(s)

i1’2n2(8) 0 0

er(s) 0 0

0 0

The linearised forms of the contact force expressions are:

b11(s)

I1ni (s)

Fr(s) c 0(s)

Fr(s) c b2i(s)

i/)2n2(s)

e( s)

with c and as defined in equations (5.71) and (5.72). Combining these expressions

gives the following four transfer function relationships among the contact forces and control


inputs:rTf \ TrS) CJ1 rS1

— CJ(n1+2)575

ui(s)— J(s)l’ U2(S)

—

and,,j \ T i-i, ‘ Tr,Is) — C ii rs) — Cy J(ni+2)

5 76ui(s) J(3)I’ U2(S) —

where

T / i\(ni+3) 1‘11 i1)

— Ii2I . (_i)’) J(+2)2ii = and J(n+2) =

(—i)(’+”) J1I (_i)(fh+71) J(12)

and J, is the ij minor of the n x n matrix J(s). Note that the transfer functions share a

common denominator, IJ(s). For the case of rigid links the b generalised coordinates are

neglected and the number of admissible functions used to model structural dynamics of the

links, ri1 and n2, are, of course, set to zero.

5.5.3 Force Control for Two Rigid Links

We begin our examination of the performance of the multivariable predictive force control

algorithm with the case of a planar two link manipulator with rigid links. As noted in

section 5.5.1, this case is of theoretical interest in the context of this thesis because it

represents the limiting situation of infinitely link stiffnesses. It is also of practical interest

because most existing industrial manipulators have rigid links.

For this test case the link models are based on the same physical model of a 2.0cm

diameter, 1.Om long aluminium rod that was used throughout section 5.3. The nominal

joint angles are t9 00 and 6 = 300 and the tip of the manipulator is in contact with an

isotropic environment having an effective contact stiffness of keff lOkN/m. The motion

of the manipulator is in the plane perpendicular to the gravitational field.


Link DynamicsLink (k) 1 26(°) 0 30Lk(m) 1.0 1.0Mk(kg) 0.8451 0.845110k(kg m2) 0.2817 0.2817

Contact DynamicsDirection x yk3(kN/m) 20 20ke(kN/m) 20 20keff(kN/m) 10 10

Table 5.7: Parameter values for two rigid link manipulator controller model.

Controller Model

Using the values listed in Table 5.7 for the parameters of the link and contact dynamics

models the continuous time transfer functions given by equations (5.75) and (5.76) evaluate

to:F’(s) — 1830s2 + 0.4330 x 108

ui(s) — 0.l835s + 77472 + 0.2500 x 108’

Fr(s) — _7464s2— 0.9330 x 108

5 78u2(s) — 0.1835s + 77472 + 0.2500 x 108’

F(s) — _352.1s2 + 0.2500 x 108

ui(s) — 0.1835s + 7747s2 + 0.2500 x 10’

andF(s) — 645152

— 0.2500 x 1085 80

u2(s) — 0.1835s + 7747s2 + 0.2500 x 108

Recalling the results of the linear analysis, for a small contact region mass the dynamics

of the system are dominated by the first two natural modes (see Figure 5.32). For an effective

contact stiffness of keff = 10k N/rn the natural frequencies of those modes are 9.4Hz and

31.3Hz. A sampling frequency of 100Hz (h = lOms) satisfies the Nyquist criterion for those

modes. Assuming zero order hold sampling, the continuous time domain transfer functions


can be transformed to the following discrete time controller model:

F(q1) — 0.4298 + 0.3909(q1 + q2) + 0.4298q3 1 5 81ui(q1) — 1 — 0.8861q’ + 0.7200q — 0.8861q3+

q_4 ‘

F(q1) — —1.589 — 0.1793(q1+ q2) — 1.589q3 1 5 82u2(q1) — 1 — O.8861q’ + 0.7200q2— 0.8861q3+

q_4 ‘

_______

— —1.698 x 10—2 + 0.4908(q’ + q2) — 1.698 x 102q35 83

ui(q’) — 1 — 0.8861q’ + 0.7200q2— O.886lq3+ q4q

andF(q1) — 1.165 — 1.639(q1 + q2) + 1.165q3 —‘ 5 84u2(q’) — 1 — 0.886lq’ + O.7200q2— 0.886lq3+

q_4

These polynomials provide the initial model coefficient values for the multivariable adaptive

predictive force control algorithm. They are updated at each time step by a multivariable

form of the EFRA system identification algorithm. The coefficients are also used in con

junction with the closed loop characteristic polynomial formulation derived in section 3.3.1

to tune the control algorithm and analyse its performance by calculating the approximate

closed loop characteristics.

Step Response

The linear analysis in section 5.5.1 suggests that force control for a structurally rigid two

link manipulator using the multivariable predictive control algorithm should be easily ac

complished using a sampling interval sufficient to resolve the 72 mode and a control horizon

of N = 2 time steps. The results presented in this section confirm this prediction.

The approximate closed loop characteristics calculated from the closed loop character

istic polynomial formulation derived in section 3.3.1 show that virtually any combination

of prediction and control horizon values yield stable closed loop performance. However, as

the data in Table 5.8 indicate, a control horizon of N = 2 time steps provides the best

combination of fast rise time and minimal overshoot because the closed loop response is


Overdamped Mode Underdamped ModesN2 N Dominant Pole fd [Hz] c fd [Hz]3 1 — 7.40 0.678 33.29 0.0885 1 — 7.20 0.389 32.37 0.0428 1 — 7.23 0.090 32.04 0.041

10 1 — 7.35 0.069 32.00 0.035T 2 -134.7 35.14 0.095 — —

5 2 -88.30 35.35 0.077 — —

6 2 -60.37 35.06 0.075 — —

8 2 -26.93 34.32 0.077 — —

10 2 - 9.43 33.91 0.077 — —

5 3 — 40.68 0.032 50.00 0.2348 3 — 39.76 0.002 50.00 0.4735 4 — 39.96 0.019 50.00 0.0928 4 — 39.77 0.005 50.00 0.188

Table 5.8: Stabilising controller horizons and closed loop characteristics for a structurallyrigid two link manipulator contacting a keff = kN/m surface. Id = damped natural frequency and damping ratio.

composed of one overdamped and one underdamped mode. For all other control horizon

values both of the closed ioop modes are underdamped. Figure 5.40 shows the contact force

step responses in the world frame x and y directions and the control inputs (joint torques)

for the case of a 2 time step control horizon and a prediction horizon of N2 5. As can

be seen, this combination of controller horizons yields a fast, offset free step response with

negligible overshoot. Reducing the prediction horizon to N2 = 4 moves the dominant pole

of the overdamped mode farther to the left on the real axis, thereby producing a somewhat

faster rise time but this is accompanied by a slight overshoot due to the 35Hz mode. On

the other hand, increasing the prediction horizon only results in slower rise times.

5.5.4 Force Control for Two Flexible Links

To demonstrate the performance of the multivariable predictive control algorithm for the

case of flexible links we will examine the case of a two link manipulator with identical links


(a) direction contact force (b) y direction contact force

14

13

12

11

10

E9z

01.—aC05-

4

3

2

0 .05 .10 .15 .20 .25 .30 .35 .40

Time [s]

(c) Joint torques

Figure 5.40: Force control step responses for a structurally rigid two link manipulatorcontacting a keff = lOkN/m surface with N2 = 5, N = 2, ) = 0.

11

10

9

8

C04

C)

3

2— Response

Setpoint

11

10

9

8

7

6

5

4

3

2

ci)C)0

cicciC00

0 .05 .10 .15 .20 .25 .30 .35 .40 “0 .05 .0 .15 .20 .25 .30 .35 .40Time [s] Time [SI

— ResponseSetpoint

— Joint 1Joint 2


in which the link structural dynamics are modelled by one admissible function per link.

The link models are based on the same physical model of a 2.0cm diameter, l.Om long

aluminium rod that was used throughout section 5.3. The nominal joint angles are 19 = 00

and 6 = 30° and the tip of the manipulator is in contact with an environment having an

isotropic environment with a effective contact stiffness of keff = lOkN/m. The motion of

the manipulator is in the plane perpendicular to the gravitational field.

Controller Model

Table 5.9 lists the numerical values of the link and contact dynamics model parameter for

the system. Using these values the transfer functions given by equations (5.75) and (5.76)

evaluate to:

Fr(s) —36.49s6 — 1.56 x 106s4 + 1.95 x 10h1s2 + 2.18 x iO’ui(s)

— IJ(s)I

Link Dynamics

204

Link (k) 1 28(°) 0 30Lk(m) 1.0 1.0Mk(kg) 0.8451 0.84517J0k(kg.m2) 0.2817 0.2817Tlkl(kg m2) 0.4807 0.480712k1(kg.m2) 0.8451 0.845113k1(N m) 7096 7096I4k(kg.m) 0.6617 0.6617cIk1o(m) 2.0 2.0ç1410(m/m) 2.753 2.753

Contact DynamicsDirection x yk(kN/m) 20 20ke(kN/m) 20 20keff(kN/m) 10 10

Table 5.9: Parameter values for two flexible link manipulator controller model

(5.85)


Fr(s) —

_289.3s6— 6.67 x — 2.95 x 10’2s2 — 1.18 x 1016

5 86U2(S)

-

Fr(s) —

_3.26s6 + 3.47 x i0s — 2.96 x 1010s2 + 1.26 x 1O’5 87

ui(s)-

andF(s) — _29.85s6

— 3.60 x 106.s4 + 2.40 x 10u1s2— 1.26 x 1O’

(5 88)u2(s)

where

= (1.39 x 104s8+ 88.05s6 + 1.36 x 107s4 + 4.08 x 101182 + 1.26 x 1015). (5.89)

Figure 5.41 shows the natural frequencies of the first four modes of the system as a

function of keff with keff = lOkN/m marked. At this effective contact stiffness the frequencies

of the 7h and 772 modes are in the region of approximate proportionality to kff and therefore

make significant contributions to the dynamics. The mode frequency is independent

of keff and the 774 mode frequency is just entering the transition range. The open ioop

natural frequencies of the modes are 9.39Hz, 29.7Hz, 67.5Hz and 102Hz. Sampling at

250Hz (h = 4ms) to satisfy the Nyquist criterion for the mode results in the following

discrete time transfer function models for the controller:

F —1 — Bi(q1)ui +B2(q1)u2 —i(q

— A —‘ q (.

and

FBi(q)ui +B2(q’)u2 —1

Y A —i q ,

where

A(q’) = 1— 1.48q’+0.67q2—0.34q3+0.54q4—0.34q5+0.67q6—1.48q7+q8,(5.92)

Bi(q’) = —0.81 +3.76q’ —5.20q2+2.46(q3+q4)—5.20q5+3.76q6—0.81q7, (5.93)

B2(q1) —9.47+18.5q’—17.0q2+6.79(q3+q4)—17.0q5+18.5q6—9.47q7,(5.94)


iø

1OkN/rn.

101

2//

iOø I I iiiil I iiiiiil I hull1 uuuuuul lulihul I 1111111

10_2 i00 101 102 iø


Figure 5.41: Variation of the natural frequencies for a two link manipulator with identicalflexible (El = 574N m2, one structural mode each) links assuming steady state contactdynamics and an isotropic environment.


—0.047q’ — 0.87q2 + 0.70(q3+ q4) — 0.87q5+ 0.34q6— 0.047q7, (5.95)

and

B2(q’) = —0.78+2.91q’ —2.61q2+0.36(q3+q4)—2.61q5+2.91q6—0.78q7. (5.96)

These polynomials provide the initial model coefficient values for the multivariable adaptive

predictive force control algorithm. They are updated at each time step by a multivariable

form of the EFRA system identification algorithm. The coefficients are also used as part of

the technique developed in section 3.3.1 to analyse the performance of the control algorithm

by calculating the approximate closed loop characteristics.

Step Response

The results plots presented in the first part of this section are from a simulation using

the linearised manipulator dynamics. This is done in order to minimise the mismatch be

tween the controller model and the system dynamics so that attention can be focussed on

the performance of the control algorithm. Later in the section results from the nonlinear

dynamics simulation are shown. These results demonstrate the role that the system identi

fication algorithm plays in the adaptive controller, namely, compensating for the presence

of unmodelled, nonlinear components of the manipulator dynamics.

Examination of the approximate closed loop poles, calculated from the control law and

the linearised system dynamics, shows that with N = 1 all prediction horizons which give

stable control result in all of the closed loop modes being underdamped. The resulting

response therefore exhibits undesirable overshoot and oscillations about the setpoints. This

poor control is not surprising in light of the fact that there are two dominant modes in

the system. Allowing the controller only one time step in which to minimise the predicted

error does not provide enough freedom to deal with two modes. Better control is obtained

with N = 2, that is, the control horizon equal to the number of dominant open loop


Overdamped Mode Underdamped ModesN2 iç Dominant Pole Id [Hz] C fd [Hz] C fd [Hz] C9 2 -91.87 23.20 0.675 32.74 0.222 101.2 0.05515 2 -103.6 12.43 0.889 36.51 0.174 98.99 0.05218 2 -39.86 30.91 0.447 35.51 0.297 100.1 0.06020 2 -25.58 31.96 0.187 39.02 0.423 99.63 0.04422 2 -17.55 30.89 0.165 44.43 0.351 99.44 0.0486 3 -41.98 37.15 0.040 89.77 0.045 125.0 0.290

25 3 -49.95 35.63 0.008 99.62 0.002 125.0 0.8478 4 -92.27 35.16 0.010 96.12 0.012 125.0 0.34210 4 -78.50 35.57 0.029 95.24 0.014 125.0 0.39112 4 -65.09 35.78 0.016 96.50 0.010 125.0 0.466

Table 5.10: Stabilising controller horizons and closed ioop characteristics for a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/m surface. Id =damped natural frequency and C = damping ratio.

modes. The closed ioop response is composed of one overdamped and three underdamped

modes. The top part of Table 5.10 shows the approximate continuous time closed ioop

characteristics for various values of N2. In this case the closed loop performance is largely

governed by the time constant of the overdamped mode and the damping ratio of the

lowest frequency underdamped mode. With a 2 step control horizon, N2 = 20 provides the

response with the fastest rise time and no overshoot, as can be seen in Figures 5.42a and

5.42b. A shorter prediction horizon causes the rise time to be such that the oscillations of

the underdamped modes do not completely damp out before the setpoint is reached and

the response overshoots. Longer prediction horizons result in unnecessarily slow responses.

With a control horizon of 3 time steps the closed system generally has four under

damped modes, but for selected prediction horizons (see Table 5.10) the response has one

overdamped mode and three underdamped ones. In the case of four underdamped modes

one of the modes is always of very low frequency and has a relatively high damping ratio.

While the step response with these characteristics will, by definition, always exhibit over

shoot, if the damping ratio of the lowest frequency mode is sufficiently high the overshoot


12

F:

2

30

28

26

24

22

18a)D 160

14

12

-, 10

8

6

4

2

0

(a) a direction contact force

(c) Joint torques

(b) y direction contact force

(d) Link tip deflections

Figure 5.42: Force control step responses for linear dynamics simulation of a two flexible link(one structural mode each) manipulator contacting a keff = l0kN/m surface with N2 = 20,N = 2, 0.

14

209

F’a)C-)

0U

C0

C-)

0


.05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60

Time [s]

ResponseSetpoint

0 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60

Time [s]

.05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60

Time tsl


will be negligible. As noted in section 5.3.5 the possibility of one of the closed ioop modes

changing from being overdamped to being underdamped may be cause for concern regarding

the robustness of the control law.

With N = 4 the controller has a control horizon equal to the total number of modes

in the open loop system. Table 5.10 indicates that the lowest frequency closed ioop mode

which dominates the response when N = 2 has been completely removed and a light

damped 125Hz mode (half the sampling frequency) has appeared. The dominant pole of

the overdamped mode is generally more negative than is the case with Nu < 4, indicating

that the rise time is decreased. Figure 5.43 shows the step responses for the case of N2 = 8

and N = 4. The faster response of the N 4 controller compared to the N = 2 is

the result of a much more active control input signal as can be seen by comparing Figures

5.42c and 5.43c. Increasing the prediction horizon from 8 time steps generally increases the

damping of the closed ioop modes, thereby reducing the ripple in the response but only at

the expense of increased rise times.

The step responses examined so far have concentrated on the case of simultaneous

changes in the x and y direction contact force setpoints. Another important aspect of the

closed ioop performance, given the strongly coupled nature of the system, is the interaction

of the outputs. This aspect is illustrated by commanding a step change in one output while

holding the other setpoint constant. Figure 5.44 shows the responses under these conditions

that results from controller horizons of N2 = 20 and N = 2 and Figure 5.45 shows the

corresponding results with N2 = 8 and N = 4. These figures show that the disturbance

of one component of the contact force which occurs in response to a step change in the

setpoint of the other component is of much larger magnitude and longer duration when the

N = 2 controller is used rather than N = 4. The difference in duration of the disturbance

is due in part to the anticipation characteristic of predictive control and results from the

difference in prediction horizons. The difference in magnitude is due to the difference in


zci)C)0

LL

C)CuC0

C.)

32

30

28

26

24

18D216

1214

C0

—, 10

8

6

4

2

0

(a) direction contact force

(c) Joint torques



Figure 5.43: Force control step responses for linear dynamics simulation of a two flexible link(one structural mode each) manipulator contacting a keff = lOkN/m surface with N2 = 8,N = 4, ) = 0.

211

16

14

12

10

8

6

4

2— Response

Setpoint

ci)C)0

C)(8C00

IL

.05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55.

Time [s]


35 .40 .45 .50 .55

Time [s]

0 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60

Time [s] Time [s]


2’

C)0U

C0

C.)

212

Figure 5.44: Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting akeff = lOkN/m surface with N2 = 20, N 2, 0.

16

14

12

10

8

6

4

2

0

ResponseSetpoint

ci)C)0

LlC)cciC0

C.)

II

.1 .2 .3 .4 .5

Time [s].6 .7 .8 .9 1.0


0 .1 .2 .3

— Response- Setpoint

.4 .5 .6 .7 .8 .9 1.0

Time [s]


34

32

30

28

26

24

E 22

-201)D 180

16

14CO 12-3

10

a6

4

2

0

Time [s] Time [s]

(c) Joint torques (d) Link tip deflections


20

18

16

14G)o 0

12LI

10

Eo 0o 0

6

4

2

25

200

0-)

Figure 5.45: Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting akeff = lOkN/m surface with N2 = 8, N 4, 0.

213

16

14

12

10

8

6

4

2

0

Response-. Setpoint

1’

I .1 .2 .3 .4 .5 .6 .7 .8 .9 1.0

Time [s]


— Response- Setpoint

40

35

.1 .2 .3 .4 .5 .6 .7 .8 .9 1.0

Time [s]


30

10

5

.1 .2 .3 .4 .5 .6

Time [s]

(c) Joint torques

.1 .2 .3 .4 .5 .6 .7 .8 .9 1.0Time [s]



the lowest closed ioop natural frequencies and the degree to which they are excited. The

disturbance of the y direction contact force due to a step change in the x direction is less

significant than when the situation is reversed but the trends of magnitude and duration of

the disturbance are comparable. The generally better performance in the y direction shown

in all of the above figures is due primarily to the 8 0°, 0 30° configuration of the

manipulator.

All of the preceding simulation results in this section are based on a simulation of the

linearised equations of motion. Under those circumstances the controller model and the

system dynamics are exactly matched. When the full, nonlinear dynamics of the system

are considered the situation is somewhat more complicated. The nonlinearities represent,

from the point of view of the controller, unmodelled dynamics. The controller is able to

cope with the unmodelled dynamics due to its explicitly adaptive implementation, that

is, at every time step the system identification algorithm calculates a new estimate of the

coefficients of the controller model. These coefficients are presumed to accurately represent

the instantaneous system dynamics in a linear form and are used to calculate the control

input for the next time step. Figure 5.46 shows a series of step responses from the nonlinear

dynamics simulation for the same manipulator and environment configuration used above.

The controller has a prediction horizon of N2 = 20 time steps and a control horizon of N = 2

steps. During the early part of the response the mismatch between the controller and system

dynamics results in strong excitation of the high frequency modes. The high frequency

structural vibrations of the links are evident in the plot of the local defiections of the link

tips shown in Figure 5.46d. These vibrations are also evident in the plots of the contact force

components (Figures 5.46a and 5.46b). In order to control the high frequency modes the

control inputs are forced to switch rapidly over relatively large amplitudes (Figure 5.46c).

While this high frequency activity results in a relatively poor response with a great deal of

overshoot it also provides rich excitation for the system identification algorithm with the


a) Q)o 0o o

U

(aC Co 0o C)

Figure 5.46: Force control step responses for nonlinear dynamics simulation of a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/m surface withN2=2O,N=2,)=O.

215

11


20

18

16

14

12

10

8

6

4

2

0

45

40

35

0 .2 .4 .6 .8 1.0 1.2 1.4 1,6 1.8

Time [SI

(a) a direction contact force

0 .2 .4 .6


.8 1.0 1.2 1.4 1.6 1.8

Time Is]


3Ø

za) 250’

1S 20

0—, 15

10

Time [s]

(c) Joint torques (d) Link tip deflections


result that after 0.8s (200 time steps) the controller model coefficients have been sufficiently

well adjusted to compensate for the unmodelled nonlinear dynamics and the subsequent step

responses are much improved.

It is not surprising that the effect of the unmodelled nonlinear dynamics is much more

pronounced in the case of the two link manipulator than was observed in the single link

results when the structure of the equations of motion for the two cases are compared. The

number of centripetal and Coriolis terms in the two link equations of motion far exceeds

those in the single link equations. In particular the two link equations have a large number of

terms which represent interactions between the joint angle and modal amplifier generalised

coordinates both for a given link and between one link and the other.

The results shown in Figure 5.46 demonstrate the limitations of the application of a

strictly linear controller to a highly nonlinear system. Several possibilities for improvement

of the closed loop performance can be considered. The simplest is filtering of the high fre

quency component of the response which would reduce the amplitude of the disturbances

but would likely also increase the time required for the identification algorithm to arrive

at a suitable model. A more fruitful approach might be to model the disturbances due to

the nonlinear unmodelled dynamics as coloured noise and include the noise model in the

controller model [32]. In either case the effect of the nonlinear dynamics can be substan

tially reduced by specifying setpoint programs which would produce less excitation of the

structural dynamics modes that the step changes shown in Figure 5.46. Ramps or higher

order polynomials would provide smoother transitions between setpoint levels and therefore

reduce the level of excitation. Reformulation of the predictive control algorithm on the

basis of a nonlinear model (for example [90]) could also be considered.


5.5.5 Summary

In summary, this section has presented an analysis of force controlled two link manipulators

with rigid or flexible and an evaluation of the performance of the multivariable, adaptive,

predictive control algorithm applied to the control of world frame contact force components

exerted by the manipulator tip on environment surfaces.

Linear analysis of the case of infinitely stiff links reveal the expected extension of char

acteristics which were identified in the single link case while at the same time illustrating

the added complexities of the two link system. The modal frequencies are found to vary

with k!ff for a given set of link lengths and inertias and the two highest frequency modes

are due to the local vibrations of the contact region mass. The frequencies are independent

of when the contact is isotropic (kxeff = kyeff) and are bounded above by the values for

é1 = 900. The contributions of the natural modes to the contact force components are also

found to vary with the nominal manipulator geometry. With the addition of modes due to

the structural flexibility of the links, linear analysis reveals a transfer of dominance among

the modes similar to that seen in the single link case, but with the expected additional

dependence on joint angles.

A transfer function model of the system which relates the world frame x and y direction

components of the contact force to the joint inputs was derived on the basis of the linearised

equations of motion. Specific parameter values were selected for a manipulator with two

identical links in contact with a relatively stiff (keff = lOkN/m) isotropic (k =k) environ

ment. The links were based on the physical model of 1.Om long, 2.0cm diameter aluminium

rods. Cases in which the link are assumed rigid as well as having their structural dynamic

modelled using the first mode shape function of the rods considered as a cantilever beams

have been addressed.


As in the single link case, the approximate closed ioop characteristics calculated from the

control law were found to be very useful in selecting controller horizons which produced the

fastest rise times and least overshoots from among the large set of horizons which stabilise

the system.

5.6 Force Control for DC Motor Actuated Space Station Manipulator

As a final demonstration of the performance of the multivariable predictive force control

algorithm we will examine the case of a simple prototype model of the two long links of the

proposed Mobile Servicing System (MSS) for the Space Station Freedom. These massive,

long, slender links are driven by geared direct current motors at their joints. The physical

parameter values for the system are listed in Table 5.11. The top part of the table shows

the link model parameter values which are based largely on those used by Chan [68]. The

MSS manipulator operates in a microgravity environment in which the gravitational field

is approximately balanced by the orbital centripetal acceleration and thus the gravitational

loading of the links is neglected in the model. The center section of the table shows the

parameters used to model the DC motors. Since the motor parameters are not publically

available the values shown are estimates based on assumptions of a maximum motor power

of 400W and a maximum torque delivery of 1500N m. Due to the mass of the links, the

natural frequencies of the cantilever beam modes used as admissible functions to model the

link structural dynamics are much lower than in the previously considered cases. In fact

the frequency of the first mode is only about 0.3Hz. Even when the tip of the second link

is in contact with a surface with very high effective contact stiffness the natural frequencies

are all less than 30Hz (see Figure 5.47).

The controller model structure is similar to that derived in section 5.5.2. The control

inputs are the input voltages to the motors. These voltages are amplified to produce the


Link DynamicsLink (k) 1 2

(°) 0 30Lk(m) 7.5 7.5Mk(kg) 1600 1600IOk(kg m2) 3.0 x i0 3.0 x iOIlkl(kg . m) 6826 682612k1(kg.m) 1600 160013k1(N.m) 6388 6388141C1(kg.m) 1253 12534k1o(m) 2.0 2.0qYklO(m/m) 0.3671 0.3671

Actuator DynamicsJoint (k) 1 2Iak(kg m2) 0.5 0.5Ngk () 50. 50.Rak(Q) 1.0 1.0Kk(N . rn/A) 1.5 1.5Kem(V s/rad) 0.5 0.5K(V/V) 4.0 4.02akmax(A) 20. 20.Vznkmax(V) 5.0 5.0

Contact DynamicsDirection x yk(kN/m) 40 40ke(kN/m) 40 40keff(kN/m) 20 20

Table 5.11: Physical parameter values for a two link MSS model with DC motor actuators.


210 I I 1111111 I I 1111111 I 11111111 11111111 I I 1111111

101

1 0

D- I

z I

10-

I

11,

1_2 i ii I I I ii I I I I I ii I I I I II I

10_i 1& 102 iø


Figure 5.47: Variation of the modal natural frequencies for DC motor actuated two linkMSS model.


armature circuit voltages which in turn produce the armature currents to which the torques

supplied by the motors are proportional. Thus, the discrete time transfer function models

for the controller are of the form:

—1 Bi(q1)vi +B2(q1)v2 _.iF(q ) = A(q1)q (5.97)

and

F— Bi(q1)vi +B2(q’)v2 —1 5 98Y— A(q1)

q

With the manipulator in contact with a surface such that the effective contact stiffness is

2OkN/m, the open loop natural frequencies of the first four modes of the system are 0.30Hz,

0.90Hz, 1.5Hz and 2.4Hz. As can be seen in Figure 5.47, the mode is, at keff = 2OkN/m,

in transition between being independent of lCeff and being proportional to kff and therefore

contributes significantly to the contact force. To satisfy the Nyquist criterion for the

mode a sampling frequency of 7.7Hz (h = l3ms) is used.

Figure 5.48 shows the contact force responses from the linearised dynamics model of the

MSS controlled by the adaptive, multivariable predictive control algorithm with N2 = 14,

N = 2, k = 0 and .Xse = 0. Note that the time scale of the figures in this section is much

longer than that of the previously shown results owing to the much greater mass of the

MSS links compared to aluminium rod links used in the previous cases. The y direction

contact force setpoint in Figure 5.48 consists of a smooth transition between force levels.

The transition is constructed by matching exponential functions of the form et— 1 and

1 —

et at the midpoint of the transition. The direction goal is to maintain a constant

force level. Figure 5.49 shows the responses of the full nonlinear dynamics model with

the same setpoints and controller parameters. These results illustrate the improved per

formance for the nonlinear case (compared to Figure 5.46) which is obtained by using a

smooth setpoint transition as opposed to a step change. The relatively small magnitude of

the level change also contributes to the improved performance. Both of these factors result


5.5

5.0

4.5

4.0

a)0

C00

3.0

2.5

2.0

1.5

1 .0

.5

0>

0.

z

0U

0(aC00

EEC0C)

a)

0.I

C-J

222

Figure 5.48: Force control responses for linear dynamics simulation of a two flexible linkSpace Station MSS contacting a keff = 2OkN/m surface.

.4.5

4.0

/7I1/

3.0

2.5

2.0

1.5

1 .0

— Response.5 .

..---. Setpoint

0.1,1 1,1. .1, I. .1.1.1.

0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s]


r— Response

2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s]

(a) n direction contact force

- 060 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30

Time [s]

(c) Motor input voltages

2 4 6 8 10 12 14 16 18 20 22 24 26 28 30

Time [s]



5.5

5.0

4.5

3.50

Li_

C)a)C0

C.)

2.5

2.0

1.5

1.0

.5

.35

.30

.25

.20

.15(a

.10>

.05

C— 0

-. 05

- .10

- .15

-. 20

223

(c) Motor input voltages (d) Link tip deflections

Figure 5.49: Force control responses for nonlinear dynamics simulation of a two flexible linkSpace Station MSS contacting a keff = 2OkN/m surface.

5.0

4.5

4.0

3.5

3.0

2.5

2.0

a)0

0U

00

.

... .

— Response r— Response----- Setpoint .5

. L- Setpoint

.1 .1. .1.1.. .1.1.1. 0 .1. .1.1. .1.1.

0 2 4 6 8 10 12 14 16 18 20 22 24 28 28 30 0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s] Time [s]

( a) z direction contact force (b) y direction contact force

4 • 1.5

— Joint 1 1 I 1 .0 — — —... link 1

--- Joint 2 ii L- -

- link 2

aE. -.5C.2 -1.0

.,j I’.. —.

0 2 4 6 8 10121416182022242628

Time [s]30 14 16 18 20 22 24 26 28 30

Time [s]


in less excitation of the nonlinear dynamics of the system which are not included in the con

troller model. While the improvement in the system performance is encouraging, additional

filtering and/or reformulation of the control algorithm, as discussed in section 5.5.4, would

be necessary to meet the expected operational performance specifications for the MSS.

5.7 Summary of Force Control Performance

The simulation results in this chapter have demonstrated the viability of the use of a

multivariable, explicitly adaptive, long range predictive control algorithm to control the

contact forces exerted by manipulators on surfaces in their working environment. The

following configurations were considered:

• a single flexible link with its structural dynamics modelled by one, two and three ad

missible mode shape function in contact with surfaces with effective contact stiffnesses

ranging from 3.1kN/n to 5MN/m;

• the same link, initially under motion control, making unexpected contact at an impact

velocity of approximately O.5m/s with a surface;

• a planar, two link manipulator with rigid links in contact with an isotropic effective

contact stiffness of lOkN/m;

• the same two link manipulator with the structural flexibility of the link modelled by

one admissible mode shape function for each link, also making contact with a surface

such that the isotropic effective contact stiffness is keff = lOkN/m; and;

• a prototype model of the two long links of the DC motor actuated Mobile Servicing

System (MSS) manipulator for the proposed Freedom Space Station, with its tip in

contact with a keff = 2OkN/m surface.


The single flexible link step response results demonstrate that with a controller model of

appropriate order and a sampling rate selected to resolve the dominant mode, as indicated

by the shape of a frequency versus effective contact stiffness plot, there are generally a wide

range of control algorithm horizon and weighting factor values for which the closed ioop

will be stable. The inherent integral action of the control algorithm guarantees that the

response will not have a steady-state offset. A new formulation for the discrete time domain

closed ioop characteristic polynomial has been used to select control algorithm horizons and

weighting factors to satisfy additional performance requirements such as minimal overshoot

and rise time. The inclusion of a static equilibrium bias term in the controller has been

shown to decrease rise time at the expense of introducing a slight amount of overshoot into

the response. The importance of selecting a reasonably accurate initial controller model

implies that knowledge of the structural dynamics of the manipulator and the contact

characteristics of the surfaces likely to be encountered in the workspace is necessary to

ensure successful control.

The problem of maintaining control while unexpectedly making contact with a surface

is a challenging one because the complex dynamics of the system are compounded by the

presence of a discontinuity. To cope with this problem a contact control logic level has

been introduced into the manipulator control hierarchy. Two operational strategies for the

contact control logic have been demonstrated and shown to allow control to be maintained

through contact events, albeit with some unavoidable contact force transients. Of par

ticular interest is the fact that under the setpoint modification strategy there is a direct

relationship between the impact velocity and the magnitude of the initial force transient.

This relationship allows manipulator motions, in particular tip velocities, to be planned on

the basis of the transient contact force levels to which nearby surfaces can be acceptably

subjected, should a collision occur.


The step response results for the case of a two link manipulator with a rigid structure

show that the controller has the potential to provide excellent force control performance for

this large subset of existing industrial manipulators. In this context, the predictive control

algorithm could be readily incorporated into a combined force and motion control structure,

such as the hybrid methodology of Raibert and Craig [12]. This case is also of interest as

the limiting situation of infinite structural stiffness because it offers validation of some of

the fundamental conclusions of the linear analysis developed in this chapter.

With the inclusion of structural flexibility the dynamics of the two link manipulator

become markedly more complex and nonlinear. The results demonstrate that the insight

gained from the linear analysis of the system and the calculation of the roots of the close

loop characteristic polynomial offer guidance in this case, as in the less complex cases, in

the construction of the controller model and the selection of control algorithm horizons and

weighting factors to provide the most desirable response. The simulations also show that

the strongly coupled structure of the manipulator dynamics, combined with the optimal (in

the sense of minimising a cost function based on predicted output errors) form of the control

law, make total isolation of the controlled quantities impossible. That is, the response to a

step change commanded for one component of contact force will result in some disturbance

of the other component.

The highly nonlinear nature of the two flexible link manipulator dynamics presents some

difficulty for the inherently linear control algorithm. The unmodelled, nonlinear dynamics

are compensated for, to some extent, by the adaptive structure of the controller but it has

been found that it is possible to excite the unmodelled dynamics to such an extent that

stability is lost. Several potential remedies to this problem are possible including filtering of

the contact force signals, using smooth setpoint transitions instead of steps, inclusion of a

coloured noise model in the control algorithm formulation, and rederivation of the predictive

control algorithm on the basis of a nonlinear model.


rpIle force control simulations for the Space Station Mobile Servicing System (MSS) ma

nipulator model demonstrate the effectiveness of the controller with a manipulator having

radically different dynamic parameters from those considered previously. The model in

cludes voltage controlled DC electric motor actuators which drive the joints through gear

boxes. The force control results show that the use of smooth setpoint transitions attenuates,

to some extent, the difficulties caused by unmodelled, nonlinear dynamics.

Chapter 6

Summary of Major Results

This thesis has developed and demonstrated an overall strategy for the control of contact

forces exerted by a structurally flexible manipulator on surfaces in its working environ

ment. The control strategy is centered on an explicitly adaptive implementation of a pre

dictive control algorithm based on the Generalised Predictive Control algorithm of Clarke,

et al. [29]. The thesis also presents analyses of the linearised equations governing the manip

ulator/environment system in several configurations, using a combination of analytic and

numerical techniques. The results of these analyses provide insight into the fundamental

dynamics which dominate the force controlled system. These insights are used to guide the

selection of the appropriate controller model and parameters for particular configurations.

The major contributions of the research reported in this thesis are:

• Kinematic and dynamic models of flexible link manipulator interacting with surfaces

in its working environment has been developed. The model includes the effects of

local inertia, energy dissipation and surface stiffnesses of the contacting surfaces of

the manipulator tip and the environment. Analysis shows that when the mass of the

surface regions deflected due to contact is comparable to the inertias of the links the

inertia effects in the contact model become important.

• Linear analyses of various configurations of manipulators in contact with environment

surfaces have been carried out using a combination of analytic and numerical tech

niques. The results of these analyses provide new insights, not apparently available

228

Chapter 6. Summary of Major Results 229

in the literature, into the fundamental dynamics of the system using established lin

ear analysis methods. In particular, they reveal that the contact force response is

dominated by different open loop modes, depending on the effective stiffness of the

contacting surfaces and that the transfer of dominance among the modes follows an

orderly progression from lower to higher frequency modes as the effective contact stiff

ness is increased in a given configuration. The analyses also show that the control

inputs tend to provide excitation to all but the very high frequency contact vibration

modes. The implications of these two results for the design of discrete time domain

contact force controllers are that the loop must be closed at a sampling frequency suf

ficient to resolve the dominant mode in the contact force response and the controller

model must include the dynamics of all of the excited modes. The linear analyses

provide guidance in determination of the dominant and significantly excited modes

for a particular manipulator configuration.

• A new general formulation for the closed loop characteristic polynomial of a system

controlled by the Ceneralised Predictive Control (GPC) algorithm has been obtained

in the discrete time domain. The formulation is applicable to both the original single

input, single output form of the algorithm and to its multivariable extension. Cal

culation of the approximate closed loop characteristics of the system from this result

has been shown to be useful in tuning the control law for particular configurations.

• The predictive control algorithm has been extended with a static equilibrium bias

term in the cost function. This extension is applicable to both SISO and multivariable

forms of the algorithm. The addition of this new term augments the feedback control

law with a proportional action feedforward component. The static equilibrium bias

term is particularly applicable in the contact force control application because of the


easily defined static equilibrium relationship between joint torques and contact force

components.

• Contact control logic is introduced as a first step in the integration of the predictive

control algorithm into an overall force/motion control strategy. In the form presented,

the primary function of the contact control logic is to deal with the discontinuity

involved in making and breaking contact and collisions between the manipulator tip

and the environment surface due to unexpected contact events.

• Detailed simulation results for one and two flexible link manipulators have been pre

sented. The results demonstrate that the predictive control algorithm can be tuned

to provide fast step responses with minimal overshoot for a wide variety of manipu

lator configurations and contact stiffnesses. The importance of the effective contact

stiffness in the dynamic structure of the system indicates the need for some a priori

knowledge of the surfaces a given manipulator is likely to contact during particular

operations.

• Two operational strategies for the contact control logic have been demonstrated in

the situation of a collision due to an unexpected contact event. The setpoint modifi

cation strategy was found to be superior to the stop motion strategy, which tends to

induce bouncing on the surface. In the setpoint modification strategy joint and force

sensor data around the instant of contact are used to predict the characteristics of

the expected force transient and the contact force setpoint is then briefly increased to

allow the controller to supply more energy to overcome the unexpected force transient

due to the kinetic energy transferred from the link to the surface.

• The explicitly adaptive implementation of the predictive force control algorithm using

the Exponential Forgetting and Resetting Algorithm (EFRA) for system identification


has been found well capable of compensating for unmodelled structural modes in the

single link case. The unmodelled nonlinear dynamics in the two link case present a

greater difficulty which significantly limits the performance of the controller.

6.1 Recommendations for Further Research

The following directions for future research stemming from the results of this thesis are

recommended:

• integration of the predictive control algorithm for force and motion control of flexible

link manipulators into the framework for simultaneous force and motion control such

as the hybrid position/force control framework of Raibert and Craig [12];

• extension of the contact control logic concept to deal with a wider range of making

and breaking contact events; this may be a suitable application for fuzzy control;

• improvement of the force control algorithm in situations where significant unmodelled

nonlinear dynamics exist; possible actions are simple filtering, inclusion of a coloured

noise model in the controller model, selection of setpoint programs less likely to excite

the nonlinear dynamics, and reformulation of the predictive controller on the bassis

of a nonlinear model.

Implementation Issues

Early in the course of the research force control experiments were conducted in which a

version of the damping algorithm introduced by Whitney [3] was implemented on a Puma

560 robot equipped with a JR3 force sensor. Edge following and peg—in—hole operations were

successfully demonstrated. Unfortunately, the closed architecture of the Puma controller,

its relatively slow ioop closing time (‘-. 28ms), and the unsuitability of the interpreted Val


II controller programming language for extensive mathematical computations precluded

the implementation of a more complex predictive force control algorithm.

The experiments with the Puma 560 robot and the JR3 force sensor demonstrated

two characteristics of the force sensor which can pose difficulties. Firstly, under some

circumstances the raw sensor signal was found to be quite noisy. The JR3 sensor firmware

includes digital filtering capabilities which allowed a much cleaner signal to be obtained

without appreciable loss of accuracy for the operations undertaken. The second problem was

the tendency of the sensor to measure inertial forces when the manipulator was executing

large motions. This problem was overcome by only activating the force control algorithm

when contact was imminent and by establishing a deadband around zero force in the control

algorithm. With more powerful computational hardware this problem could perhaps be

reduced by calculating an estimate of the inertia force at the sensor and applying it to the

sensor signal as a bias.

The analytical and simulation results presented in this thesis serve to establish the viabil

ity of the adaptive, multivariable predictive algorithm and contact control logic for contact

force control applications. Several considerations which arise from these results will have to

be addressed in the course of future experimental and eventual practical implementations.

The selection of the appropriate sampling rate for the discrete time controller has been

discussed throughout the simulation results sections. It has been shown that the necessary

sampling rate as well as the structure of the controller model and the initial values of the

coefficients of that model are closely related to the dynamic characteristics of the manipu

lator links and the effective stiffness and inertia of the contact region that exists between

the manipulator tip and the environment. Knowledge of the link dynamics can be obtained

by spectral analysis of their structural vibrations. The contact characteristics which are re

quired are the effective values for the particular end-effector in contact with the particular

environment surface. These values will account for the effects of all sources of compliance


which are relevant to the contact operation. In addition to the local characteristics of the

contacting surfaces, sources of compliance might include the presence of discrete compliance

devices such as a Remote Compliance Centre (RCC) at the manipulator wrist, force sensor

compliance, manipulator joint compliance, global deformation of the environment surface,

etc. A procedure such as that used by An, et al. [14] to identify the effective contact stiffness

using the manipulator sensors would seem to be the best available approach to obtaining

values for the effective contact characteristics.

The need to incorporate the characteristics of particular end-effector and environment

surfaces in the controller model suggests that, for operation in a relatively structured

workspace, a “model scheduling” strategy might improve the performance of the controller

in comparison to depending entirely on the system identification algorithm to adjust the

model coefficients when a different end-effector is used or a different surface contacted. Such

model scheduling could easily be incorporated in the contact control logic.

The computational complexity of the control and identification algorithms governs the

type and speed of processing hardware required for successful implementation. The calcu

lations required in the contact control logic and the predictive part of the control algorithm

are relatively simple. However, the calculation of the control input increments from the

results of the predictor calculations is somewhat more taxing because it involves the cal

culation of a matrix pseudo-inverse, the order of which increases with the control horizon

(Na) and the number of inputs and outputs. The identification algorithm must be executed

independently for each system output at each time step and thus lends itself to parallel

processing. The versions of the control and identification algorithms coded in TWOFLEX

execute at approximately 20% to 50% of the speed required for real time operation on a

SparcStation 2. The routines are coded in FORTRAN and were written with emphasis on

making them understandable and maintainable, not necessarily time efficient. Rewriting of


the routines in a language and form intended for real time operation should allow successful

implementation using existing hardware.

Bibliography

[1] I. Winton. Breathing life into the SPDM. in Proceedings of 6th CASI Conference:Symposium on Space Station. Canadian Aeronautics and Space Institute, November1989.

[2] D.E. Whitney. Historical perspective and state of the art in robot force control. International Journal of Robotics Research, 6(1):3—14, 1987.

[3] D.E. Whitney. Force feedback control of manipulator fine motions. ASME Journal ofDynamic Systems, Measurement and Control, 98:91—97, June 1977.

[41 J.K. Salisbury. Active stiffness control of a manipulator in cartesian coordinates. InProceedings of the 19th IEEE Conference on Decision and Control, pages 95—100, December 1980.

[5] N. Hogan. Impedance control: An approach to manipulation. Part I — Theory. ASMEJournal of Dynamic Systems, Measurement and Control, 107:1—7, March 1985.

[6] N. Hogan. Impedance control: An approach to manipulation. Part II — Implementation. ASME Journal of Dynamic Systems, Measurement and Control, 107:8—16, March1985.

[7] N. Hogan. Impedance control: An approach to manipulation. Part III — Applications.ASME Journal of Dynamic Systems, Measurement and Control, 107:17—24, March1985.

[8] M.T. Mason. Compliance and force control for computer controlled manipulators.IEEE Tranactions on Systems, Man and Cybernetics, SMC-11:418—432, June 1981.

[9] C. Wu. Compliance control of a robot manipulator based on joint torque servo. International Journal of Robotics Research, 4(3):55—71, 1985.

[10] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipulators, Part I: The fundamental concepts of compliant motion. IEEE Journal of Roboticsand Automation, RA-2:83—92, 1986.

[11] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipulators, Part II: Design method. IEEE Journal of Robotics and Automation, RA-2:93—105,1986.

235

Bibliography 236

[12] M.H. Raibert and J.J.. Craig. Hybrid position/force control of manipulators. ASMEJournal of Dynamic Systems, Measurement and Control, 102:126—133, June 1981.

[13] J.K. Salisbury and J.J. Craig. Articulated hands: Force control and kinematic issues.International Journal of Robotics Research, 1(1):4—17, 1982.

[14] C.H. An, C.G. Atkenson, and J.M. Hollerbach. Model-Based Control of a Robot Manipulator. The MIT Press Series in Artificial Intelligence. The MIT Press, Cambridge,Massachusetts, 1988.

[15] 0. Khatib. A unified approach for motion and force control of robot manipulators:The operational space formulation. IEEE Journal of Robotics and Automation, RA3:43—53, 1987.

[16] C.H. An and J.M. Hollerbach. The role of dynamic models in cartesian force controlof manipulators. International Journal of Robotics Research, 8(4):51—72, August 1989.

[171 Uchiyama:1989. Control of robot arms. JSME International Journal, Series III,32(1):1—9, 1989.

[18] M.W. Spong. Force feedback control of flexible joint manipulators. In Proceedings ofthe ASME Winter Annual Meeting, pages 177—183, 1987.

[19] M.W. Spong. On the force control porblem for flexible joint manipulators. IEEETransactions on Automatic Control, 34(1):107—111, January 1989.

[20] S.W. Tilley, RH. Cannon, Jr., and R. Kraft. End point force control of a very flexiblemanipulator with a fast end effector. In Proceedings of the ASME Winter AnnualMeeting, 1986.

[21] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a one—link force—controlled flexible manipulator. Journal of Robotic Systems, 5(5):443—451, 1988.

[22] D.J. Latornell and D.B. Cherchas. Force control for a flexible manipulator link usinggeneralized predictive control. In M. Jamshidi and M. Saif, editors, Robotics andManufacturing — Recent Trends in Research, Education and Applications, volume 3,pages 569—576, New York, 1990. ASME Press.

[23] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a two—link force—controlled flexible manipulator. ASME Journal of Dynamic Systems, Measurementand Control, 112:661—666, December 1990.

[24] F.G. Pfeiffer. Path and force control of elastic manipulators. In Proceedings of theIEEE Conference on Decision and Control, pages 514—519, 1990.

Bibliography 237

[25] F. Matsuno, Y. Sakawa, and T. Asano. Quasi-static hybrid position/force controlof a flexible manipulator. In Proceedings of the IEEE Conference on Robotics andAutomation, pages 2838—2843, April 1991.

[26] D. Li. Tip—contact force control of one—link flexible manipulator: An inherent performance limitation. In Proceedings of the American Control Conference, pages 697—701,1990.

[27] D.J. Latornell and Cherchas D.B. Force and motion control of a single flexible manipulator link. Robotics é4 Computer—Integrated Manufacturing, 9(2):87—99, April 1992.

[28] A.P. Tzes and S. Yurkovich. Flexible—link manipulator force control. In Proceedingsof the American Control Conference, pages 194—199, 1990.

[29] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control — Part I.The basic algorithm. Automatica, 23(2):137—148, 1987.

[30] J. Denavit and R.S. Hartenberg. A kinematic notation for lower-pair mechanisms basedon matrices. ASME Journal of Applied Mechanics, pages 215—221, June 1955.

[31] W.J. Book. Recursive lagrangian dynamics of flexible manipulator arms. InternationalJournal of Robotics Research, 3(3):87—101, 1984.

[32] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control — Part II.Extensions and interpretations. Automatica, 23(2): 149—i 60, 1987.

[33] D.W. Clarke and C. Mohtadi. Properties of generalized predictive control. Automatica,25(6):859—875, 1989.

[34] D.J. Latornell. Source listings for the TWOFLEX program — A dynamics and controlsimulation for structurally flexible robotics manipulators making contact with theirenvironment. Report CAMROL R.92-2, University of British Columbia, Departmentof Mechanical Engineering, Computer Aided Manufacturing and Robotics Laboratory(CAMROL), September 1992.

[35] H.W. Mah. On the Nonlinear Dynamics of a Space Platform Based Mobile Manipulator. PhD thesis, Department of Mechanical Engineering, University of BritishColumbia, October 1992.

[36] R.P. Paul. Robotic Manipulators: Mathematics, Programming and Control. The MITPress Series in Artificial Intelligence. The MIT Press, Cambridge, Massachusetts, 1981.

[37] K.S. Fu, R.C. Gonzolez, and C.S.G. Lee. Robotics: Control, Sensing, Vision, andIntelligence. McGraw-Hill Inc., New York, NY, 1987.

Bibliography 238

[38] J.J. Craig. Introduction to Robotics. Addison-Wesley, Reading, Massachusetts, 1986.

[39] W.J. Book. Analysis of massless elastic chains with servo controlled joints. ASMEJournal of Dynamic Systems, Measurement and Control, 110(3):187—192, 1979.

[40] Liang-Wey Chang and J.F. Hamilton. The kinematics of robotic manipulators withflexible links using and equivalent rigid link system (ERLS) model. ASME Journal ofDynamic Systems, Measurement and Control, 113:48—53, March 1991.

[41] L. Meirovitch. Computational Methods in Structural Dynamics. Sijthoff and Noordhoff,Alphen aan den Rijn, The Netherlands, 1980.

[42] N.G. Chalhoub and A.G. Ulsoy. Dynamic simulation of a leadscrew driven flexiblerobot arm and controller. ASME Journal of Dynamic Systems, Measurement andControl, 108:119—126, June 1986.

[43] G.G. Hastings and W.J. Book. A linear dynamic model for flexible robotic manipulators. IEEE Control Systems Magazine, pages 61—64, February 1987.

[44] M. Benati and A. Morro. Dynamics of chain of flexible links. ASME Journal ofDynamic Systems, Measurement and Control, 110:410—415, December 1988.

[451 E. Barbieri and U. Ozguner. Unconstrained and constrained mode expansions for aflexible slewing link. ASME Journal of Dynamic Systems, Measurement and Control,110:416—421, December 1988.

[46] G. Naganathan and A.H. Soni. Coupling effects of kinematics and flexibility in manipulators. International Journal of Robotics Research, 6(1):75—84, 1987.

[47] J.D. Lee and B.-L. Wang. Dynamic equations for a two—link flexible robot arm. Computers and Structures, 29(3):469—477, 1988.

[48] Liang-Wey Chang and J.F. Hamilton. Dynamics of robotic manipulators with flexible links. ASME Journal of Dynamic Systems, Measurement and Control, 113:54—59,March 1991.

[49] S. Cetinkunt and Wen-Lung Yu. Closed—loop behaviour of a feedback—controlled flexible arm: A comparative study. International Journal of Robotics Research, 10(3):263—275, June 1991.

[50] P.A. Blelloch and K.S. Carney. Modal selection in structural dynamics. In Proceedingsof the 7th International Modal Analysis Conference, pages 742—749, January 1989.

[51] J.T. Spanos and W.S. Tsuha. Selection of component modes for the simulation offlexible multibody spacecraft. In Proceedings of AAS/AIAA Astrodynamics SpecialistConference, August 1989.

Bibliography 239

[52] P.A. Blelloch and KS. Carney. Selection of component modes. American Institute ofAeronautics and Astronautics, 90(AIAA-90-1201-CP):105—1 12, 1990.

[53] P.c. Hughes. Dynamics of a flexible manipulator arm for the space shuttle. In Proceedings of AAS/AIAA Astrodynamics Specialist Conference, Jackson Hole, Wyoming,September 1977.

[54] F.A. Kelly and R.L. Huston. Modelling of flexibility effects in robot arms. In Proceedings of the 1983 Joint Automatic Control Conference, 1983.

[55] P.C. Hughes. Dynamics of a chain of flexible bodies. Journal of Astronautical Sciences,27(4):359—380, 1979.

[56] V.J. Modi and A.M. Ibrahim. Dynamics of the orbiter based flexible members. InProceedings of the 14th International Symposium on Space and Technology, Tokyo,May 1984.

[57] M.W. Walker and D.E. Orin. Efficient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104(3):205—211, September 1982.

[58] W.M. Silver. On the equivalence of Lagrangian and Newton—Euler dynamics for manipulators. In Joint Automatic Control Conference, 1983. Paper TA-2A.

[59] K.H. Low and M. Vidyasagar. Dynamic study of flexible manipulators with open andclosed chain mechanisms. In Proceedings of the 1987 ASME Winter Annual Meeting,volume 6, pages 277—286. American Society of Mechanical Engineers, December 1987.

[60] K.H. Low and M. Vidyasagar. A Lagrangian formulation of the dynamic model forflexible manipulator systems. ASME Journal of Dynamic Systems, Measurement andControl, 110(2):175—181, June 1988.

[61] Y. Huang and C.S.G. Lee. Generalization of Newton—Euler formulation of dynamicequations to nonrigid manipulators. ASME Journal of Dynamic Systems, Measurementand Control, 110:308—315, September 1988.

[62] K.L. Johnson. Contact Mechanics. Cambridge University Press, Cambridge, UK, 1985.

[63] L.D. Landau and E.M. Lifshitz. Theory of Elasticity, volume 3 of Course of TheoreticalPhysics. Pergamon Press, 1959.

[64] K.L. Johnson. One hundred years of Hertz contact. Proceedings of the Institution ofMechanical Engineers, 196:363—378, 1982.

Bibliography 240

[65] C.W. de Silva. Control Sensors and Actuators. Prentice-Hall, Inc., Englewood Cliffs,NJ, 1989.

[66] R.L. Bisplinghoff, H. Ashley, and R.L. Halfman. Aeroelasticity. Assison-Wesley, Reading, Massachusetts, 1955.

[67] B.W. Char, K.O. Geddes, 0.11. Gonnet, M.B. Monagan, and S.M. Watt. Maple User’sGuide. Waterloo, Ontario, 5th edition, March 1988.

[68] J.K.W. Chan. Dynamics and control of an orbiting space platform based mobile flexiblemanipulator. Master’s thesis, Department of Mechanical Engineering, University ofBritish Columbia, April 1990.

[69] B.J. Lazan. Damping of Materials and Members in Structural Mechanics. PergamonPress, 1968.

[70] L. Meirovitch. Elements of Vibration Analysis. McGraw-Hill, 2nd edition, 1986.

[71] E.H. Gaylord and C.N. Gaylord. Structural Engineering Handbook. McGraw-Hill, 3rdedition, 1990.

[72] C.-K.A. Ng. Dynamics and Control of Orbiting Flexible Systems: A Formulation withApplications. PhD thesis, Department of Mechanical Engineering, University of BritishColumbia, April 1992.

[73] D.W. Clarke and P.J. Gawthrop. Self-tuning controller. lEE Proceedings, Part D,122:929—934, 1975.

[74] V. Peterka. Predictor-based self-tuning control. Automatica, 20(1):39—50, 1984.

[75] B.E. Ydstie. Extended horizon adaptive control. In Proceedings of the 9th TriennialIFAC World Congress, Budapest, 1984.

[76] P.S. Tuffs and D.W. Clarke. Self-tuning control of offset: A unified approach. lEEProceedings, Part D, 132:100—110, 1985.

[77] G.C. Goodwin and K.S. Sin. Adaptive Filtering Prediction and Control. Prentice-Hall,Inc., Englewood Cliffs, NJ, 1984.

[78] K.J. Aström and B. Wittenmark. Computer Controlled Systems Theory and Design.Prentice-Hall, Inc., Englewood Cliffs, NJ, 1984.

[79] R.C. Cutler and B.L. Ramaker. Dynamic matrix control — A computer control algorithm. In Proceedings of the 1980 Joint Automatic Control Conference, volume 1, SanFrancisco, 1980.

Bibliography 241

[80] K. Ogata. Discrete—Time Control Systems. Prentice-Hall, Inc., Englewood Cliffs, NJ,1987.

[81] A.-L. Elshafel, G. Dumont, and A. Elnaggar. Perturbation analysis of GPC with one-step control horizon. Automatica, 27(4):725—728, 1991.

[82] S.L. Shah, C. Mohtadi, and D.W. Clarke. Multivariable adaptive control without aprior knowledge of the delay matrix. System and Control Letters, 9:295—306, 1987.

[83] G.J. Bierman. Factorization Methods for Discrete Sequential Estimation. AcademicPress, 1977.

[84] M.E. Salgado, G.C. Goodwin, and R.H. Middleton. Modified least squares algorithmincorporating exponential resetting and forgetting. International Journal of Control,47(2):477—491, 1988.

[85] L. Ljung and T. Soderstrom. Theory and Practice of Recursive Identification. TheMIT Press Series in Signal Processing, Optimization, and Control. The MIT Press,Cambridge, Massachusetts, 1983.

[86] International Mathematical and Inc. Statistical Libraries. IMSL Library: ReferenceManual. Houston, Texas, 1980.

[87] W.C. Gear. Numerical Initial Value Problems in Ordinary Differential Equations.Prentice-Hall, Inc., Englewood Cliffs, NJ, 1971.

[88] W.H. Press, B.P. Flannery, S.A. Teukoisky, and W.T. Vetterling. Numerical Recipes.Cambridge University Press, Cambridge, UK, 1986.

[89] H.W. Mah. Private Communication, July 1991.

[90] S. Chen and S.A. Billings. Representations of non-linear systems: The NARMAXmodel. International Journal of Control, 49(3):1013—1032, 1989.

Appendix A

Equations of Motion for Various Manipulator Configurations

This appendix contains the equations of motion for several other configurations of manip

ulators which were studied during the course of the research. These equations were arrived

at by transforming equations (2.47) through (2.52), derived in section 2.4, as noted in each

section.

A.1 Planar Two Link Flexible Manipulator in Free Motion

Setting each element of the set of coefficients {m, be, bex, b6,b3, kex, key, k, k3} to zero

and dropping the generalised coordinates e2, and , and equations (2.51) and (2.52) yields

the 2+n1+n2 equations of motion for a planar two link flexible manipulator in free motion.

Rigid body equation for link 1:

+ 102 + M2 (L + w0 +L1L2c12 +L2ioai2)

+ ‘21ii + I22j — 2 (Lis12— wiocai2) I42)2i] i

+ ‘11j1j + [M2 (L1 + L2Ca2)— ::12

I42i2]

fli

+ 102 + -JvI2L2(L1ca12 + Wioai2)+ :2-225b2— (Liai2 wioca12)I42çb23 14 c’jz1 j1

+ [102 + M2L2(Lic12 +W10Sa12)+I22j — (Lis12 —w10c12)EI422]

+ I12j/’2j + [Liai2 + W10Sa12j

3=1 3=1

242

Appendix A. Equations of Motion for Various Manipulator Configurations 243

= — b8181

+gx (MiLisi + ci + M2L1s1+ M2w10c1+ M2L28ia12 +

—gy (MiLici— si + M2Lici — M2w10s1+ M2L2ciai2— 8iai2I422)

+ { [2 (L18a12 — WloCai2) + 2(Lic12 + a2+412 [L2 (Lis12

— wlocai2) zL4() — (L2Sa12+ 2w10)Wio]

—

2I—

2I22jb2j2j

+2 [(L1Ca12+ WioSai2) Wo— Cai2Wio] I’ + 2 (Lis12

— wiocai2) I42i2}

+ [M2L2(Liai2 W10Ca12)+ (Lic::2 + W103a12) I422]

+ PV12L2(L1Sa12 — cai2wlo) z0 — 2 + 2 (L15a12—c12wio)

j=i

+2 (L1c12 + Sai2Wlo)

fl2

I42J2J] 82

+ [12L2(Lia12 — Wi:Ca12)+ (Lic:12 + wio&ai2)I42525]

+2 (Lia12 — wi0c12) I42j’J2j—I22j’/J2j’/)2j ti4. (A.1)

j=i 5=1

Deflection equation for ith mode of link 1:

+ [ (L1 + L2Cai2)—

sl2 iOI42525] &jo + [102

+M2L2(Lic12 + WloSai2) — (L13a2—

w1ocai2)I42525] i

+I21iii + [M210 + (M2L2ai2 S2 I42525)i

1jo1i


+ [(M2L2c2— a2 I42i?2) + + I22I)5)]

+[(:2L2l2

— aj2 1422) &jo + (102 + I22J) io]

I2b2+ Ca12lo

3=1 2=1

= —b1b1— I31iI’1i

+gs1I4j — 9yC1I4li

+ {121i1i + [M2 (L2ai2+ 10) + Cai2I422]iio

— [M2L2(Ljs12 —w10c12)+ (Liai2 + wiosai2)I42)2]0}

+ ML2Sa12 + 2Cai2 I42j)2j 1o02

1. 3=1

+2 [M2L2ai2o+ 8a12 I42j’2j + ai2 iio

2 [M2L2ai2i:+I22j2j2 + io} Ôi

+ -M2L2sai2+ Cc12>142ççb2 qi08j=1

+2 { [M2L2ai2o&io + Sai2I42j2j + 1io

—

‘22j2 0 + M2L2s12 + a2 142j2 10W

j=1 ) j=1

+2—

.(A.2)


[102 + M2L2(Lic12 + WioSai2) + — (Lisa12 — wiocai2)I42i2]


+ [ZM2L2Cai2— a2

2

I42:b2i], [ +I22b]

+ 102 + 122jh1123 2 + 112j,12jj=1 j=1

= b6202

+gx (M2L2iai2+ C1ai2I422)— gy (M2L2Ciai2— S1ai2I422)

+ [M2L2(wioca2 Liai2) (Licai2 + Wio8ai2)I422]è

— M2L2Sai2i)io + 2caj2zbio I42j’1/’2j + 2 6j=1 2=1

+ 2). (A.3)

Deflection equation for jth mode of link 2:

[112j + (L1Ca12+s12w,0)I42]i

+ca12142j 1o1i + 112j jo1i + ‘12j2 +122j2j

= —b2’çb2 —I32’b2

+gxsla12I42j— gyClai2I42j + [I22ib2i— (Lis12 —w,0c12)I42]6

+2 {I22j&2j2 —s12I423w,o +I22b2W0} +I22b2(ti4 + O + 2ti40&2). (A.4)

A.2 Planar Two Rigid Link Manipulator in Contact

Setting each of the coefficients {I1ki,I2k,I3ki,I4k:, bk2,i = 1,... ,k, k = 1, 2} to zero, drop

ping the generalised coordinates 1,... ,n,} and {i/2j,j = 1,... ,n2} and dropping

equations (2.49) and (2.50) yields the equations of motion for a two rigid link manipulator

in contact with a deformable environment.



[i + 102 + M2 (L +L1L2c2)+ m (L + L + 2L1L2c2)]ë1

+ [102 + M2L2L1c2+ m (L +L1L2c2)]ë2

—m [Lisi + L2s12]ë + mi,, [L1c1 + L2c12]

= —

+b3 (Lisi + L2s12) [(Lisi + L2s12)a1 — L2s122—

—b3 (Lici + L2c12) [(Liei + L2c12)i +L2c1202 — éj+k8 (Lisi + L2s12)(Lici + L2c12 — — Err)

—k9, (Lici + L2c12)(Lisi + L2s12— Yw E)

+gx (MiLiSi + M2L1s1+ M2L2si2)— g1, (MiLici + M2L1c1+ M2L2ci2)

+L1L2s2[(M2+ mc) 6 + (M2 + 2m) (A.5)


[102 + M2L2L1c2+ m (L +L1L2c2)] öi + [I + mcL] 2

—mL2si2ë+ mL2c12ë

= T2 —

—b3L2s12 [(Lisi + L2s12)Ô1 +L2s12&2+

—b3L2ci2[(Lici + L2c12)01 — L2c1262—

+k8L2s12(Lici + L2c12—

—

—k8L2ci2(Lisi + L2s12- 71w —

+g-M2L2si2— g-M2L2ci2— (M2L2+ mc) . (A.6)

x direction contact equation:

—m [Lisi + L2s12]8i — rncL2s122 + mE


= — kexEx

—b8 [(Lisi + L2s12)&i +L2s1262+ + k5 (Lici + L2c12—

—

+m [Liciô + L2c2 (o + 9 + 28)]. (A.7)

y direction contact equation:

m [Lici + L2c12]8 — mL2c1262+

= beyEy — keyEy

+b8 [(Lici + L2c12)i9 +L2c1202—

+ k (Lisi + L2s12— Yw — cy)

+m [LisiÔ + L2s12 (ô + 8 + 2O12)] . (A.8)

A.3 Planar Two Link Rigid Manipulator in Free Motion

Further operation on the set of equations in section A.2, namely setting each of the coef

ficients {m, bex, bey, bx, b3, kex, key, k8} to zero, dropping the generalised coordinates

e and e, and dropping equations (A.7) and (A.8) yields the 2 equations of motion for a

planar rigid link manipulator in free motion.


+ 102 + M2 (L +L1L2c2)]1 + {z + M2L2Lic2]82

= Ti — b9181 +M2L1L2s2[9 + 8182]

+gx (MiLiSi + M2L1s1+ M2L2si2)— gy (MiLiCi + M2L1c1+ M2L2ci2) (A.9)


[102 + M2L2LiC2]81 + 10282 T2 — b8282 + M2L2(gxsl2 — gyc12) — M2L2. (A.lo)


A.4 One Flexible Link in Contact

The 2 + n1 equations of motion for a single flexible link in contact are arrived at by setting

the coefficients {I2,1i2i, 122:, ‘32i, 142:, b2, bex, b8, kex, k5, i 1, . . . , n2} to zero, dropping

the generalised coordinates &2, {b23,j = 1,... ,n} and e and dispensing with equations

(2.48), (2.50) and (2.51).

Rigid body equation for link:

+ m (L +0)] O + + mCLi 1o1j + m [Lici — wi0si] ë

= — b811

—b3 (Lici— WioSi) [(Lici — w10s1)8i +i0ci

—k3 (Lici — wi0si) (Lisi + w10c1— 71w —

+9x (MiLisi -t- Cl z1:4lbl) (M1L1C1—i )—2 (I2liiili + mcwioio) 6. (A.11)

Deflection equation for ith mode of link:

[liii + mLiioj + ‘2l:li + iioii +m10c1

= —bi1/)1—

I3iil/’li

—b8110ci [(Lici—w10s1)81— + WioCi] —k8110c1(Lisi + W10C1

— 71w —

+ (gxSi — gyci) ‘41i + (I21b1+ mcwioqijo) 6 (A.12)


m [Liei — w10s1]9i + mc1 1jo1j + më

= beyy — keyEy

+b8 [(Lici — W1031)8i —é + W10C1] + k8 (Lisi + W10C1

— 71w —

+m (Lisi + W10C1)8 + 2mzbi0si8i (A.13)


A.5 One Flexible Link in Free Motion

The 1 + n1 equations for the a single flexible link in free motion are obtained from the

equations in section A.4 by setting the coefficients {m, bey, bsy, key, k3} to zero, dropping

the generalised coordinate E and dispensing with equation (A.13)


+

= T1— b911 + g (MiLisi + C I411) — gy (MiLici— 1 14ii)

—2I21,b1b1&1. (A.14)

Deflection equation for ith mode of link:

111191 +121j’Ii = —b12’11 —I31b1.+ (gs1 — gci)I4i,+ I21ibn8 (A.15)

A.6 One Rigid Link in Contact

From the equations in section A.4, the 2 equations of motion for a single rigid link in contact

are obtained by setting the coefficients{1111,1211,1311,i= 1,. ,ni} to zero, dropping the

generalised coordinates {&11,i = 1,... ,m}, and dispensing with equation (A.12).


+ mLj 8 + mL1c1ë

= T1 — b8181 —b8yLici (L1c1Ô1—

—k3L1c1(Lisi Yw—

e)

+gx (MiLisi)— gy (MiLici) — 2mwi0i09i. (A.16)


mL1ci81+ më

= beyy — keyEy + b8 (LiciÔi—

+ k3y (Lisi— Yw —

e) + mLisi8. (A.17)


A.7 DC Motor Actuated Single Flexible Link in Free Motion

This DC motor actuator model is coupled to the rigid body equation of motion for the

link being driven by adding the inertia, NIak, and damping, Ng2bak, terms from equa

tion (2.68) to the corresponding terms in the rigid body equation and by using the geared

up motor torque, N9KTja, as the joint torque, ‘rk. In order to calculate this joint torque the

differential equation for the armature current (2.65) must be solved simultaneously with the

equations of motion since it involves the armature speed which is proportional to the joint

rate. For example, the equations of motion for a single flexible link in free motion driven

by a DC motor are as follows.


[101 + N1I01]ö + I11i1i

NgiKriiai — bO1 + g (MiLiSi + C1 1 1i1i) — gy (MiLici— 3i I411)

—2>I2ibibi28i. (AJ8)

Deflection equation for link:

Iii +I21b1.= —b1211 I31b1.+ (gzsi — gci)I4i.+I21b1 (A.19)

Armature current equation for motor:

Laijai + KernfNgll = Kv1 — Raijai (A.20)

Appendix B

Cost Function Expansions

This appendix contains the details of the algebraic expansion and differentiation of the cost

functions defined for the control algorithms developed in Chapter 3.

B.1 Multivariable Predictive Control Algorithm

For the multivariable algorithm the cost function is the sum of squares of the optimal

predicted output errors, ê, plus the sum of squares of the control input increments, Uk, the

latter weighted by a positive factor, ):

J = ê”êi + XcZU’Uk. (B.1)

Expanding the first term using equation (3.31):

n T fl

G2kUk+fw1 G2kUk+fw +cU11k

kz1 k1 k1

mt

(,G1kuk) +(f_w)T +(f—w) +.AU’Ukk=1 k=1 k=1

n, /n \ fn fn= (>U’G) (Gkuk) +2(UG) (f—w1)

i=1 kzrl krl k1

+(f_w)T (f-w)] +cilUk.

=

=1

(B.2)

251

Appendix B. Cost Function Expansions 252

Differentiating with respect to Urn, an arbitrary member of the set of control input increment

vectors gives:

= [G Gim]+2GL(fi_Wi)+2?icUk

no ni no

= 2 G Gk Uk + .\cUm + G (f,— w) . (B.3)

=1 k—_i

B.2 Static Equilibrium Bias Term

The static equilibrium bias term is the sum of squares of the differences between the actual

control inputs, Uk, and the calculated static equilibrium inputs, ük:

T -

Jse = >Z (ujc — Uk) (Uk — Uk). (B.4)k=1

Substituting for Uk from equation (3.50):

ft

0 - T-

Jse = (iL + Luk - Uk) (ukp + Lu,.— Uk)

kzrl

=

(uj + uLT— u) + LUk Uk)

=

+ (u)2 ‘ + Ü Uj — 2ujtT Uk]. (B.5)

Differentiating with respect to Urn, an arbitrary member of the set of control input increment

vectors yields:

= 2 [LT L Uk + uLT — LT u]

= 2 [LT L Uk + LT (uj— ü)] (B.6)

Appendix C

Static Equilibrium Function Derivation

This appendix presents the details of the algebraic manipulations of the equations of motion

and contact force model for a two flexible link manipulator in contact with a deformable

environment to yield the function that maps the set of desired contact force levels (setpoints)

onto the corresponding set of static equilibrium torques (control inputs). This function is

used to obtain the static equilibrium bias term for the control algorithm cost function that

is derived in section 3.4.

The static equilibrium relationships for the system are obtained by setting to zero all

of the time derivatives in the equations of motion ((2.47) through (2.52)). From the rigid

body equations this yields, for link 1:

o = T1

+k8(L1s1 + w10c1 +L2s112 + w2oclai2) (Lici — wiosi + L2c1a12 — W2o81a12—

—

—k3 (Liei — wiosi +L2c112 — W2031a12)(Lisi + wioci + L251a12+ W2oClai2— Yw

—

(C.1)

and, for link 2:

o = r2 + k (L2si12 + W2oClai2) (Lici — W10S1 + L2C1a12 W2o51a12—

Xw

—k5 (L2c1a12— W2081a12)(L1s1 + W10C1 + L251a12+ W2OC1a12— 71w — en). (C.2)

The deflection equations for the ith mode of link 1 and the jth mode of link 2 yield:

253

Appendix C. Static Equilibrium Function Derivation 254

o—

+k8(q10s1 +L2s112q0+ w2oc1aj2q50)(Lici — Wiosi + L2Clai2 — W2o91a12—

—

—k5(q1joci +L2ci1260— W2oS1ai2q.o) (Lisi + Wioi + L281a12+ W2oClai2

— Yw —

(C.3)

and

o = I321b2+ks3çb2jos1a12(Lici — W1o.1 + L2C1a12— W2oSlcx2 — — e)

ksy42joC1ai2 (Lisi + wioci + L231a12+ W2OC1c112— Yw

—

Ey). (C.4)

Finally, the static equilibrium relationships obtained from the x and y direction contact

equations are:

o = —k6e+ k8 (Lici — w10s1 + L2C1a12— W2oS1c2 — Xw — Ex) (C.5)

and

o = keyEy + (Lisi + W10C1 + L2S1a12+ W2oCh12— Yw — eu). (C.6)

Setting the time derivatives in the contact force model (equations (2.56) and (2.57)) to

zero yields:rn3 me

F kezEx + k8 — — (C.7)

and

F!, = keyEy + k3(Ytip — Yw en), (C.8)Tfl::

where the world frame tip position components are given by

L1c1 — w10s1 + L2C1a12 — W2o51a12 (C.9)

and

Ytip L1s1 + W10C1 + L2S1cz12+ W2oClai2. (C.1O)


The static equilibrium defiections of the contact region mass are obtained by substituting

equations (0.9) and (0.10) into equations (0.5) and (C.6) and solving for e and e:

=(xj

— x) (0.11)

and

ksy+key(Ytip — yw). (0.12)

With these results e and Ey can be eliminated from the contact force model to give

F = kxeff (xt — x) (0.13)

and

F = kyeff (Ytip yw). (0.14)

Substituting (C.11) through (0.14) into (0.1) and (0.2) and solving for r1 and r2:

T1 = F (Lici — wi0si + L2C1a12 — W2oS1a2) — F (L1s1 + wc1 + L231a12+ w2oc1a2)

(0.15)

and

= F (L2C1a12— w2081a12) — F, (L2S1a12+ W2OC1a12). (0.16)

Similar substitutions into (0.3) and (0.4) yield the following expressions for the static

equilibrium modal amplifiers:

F [4)i3i + (L231a12+ w2jocla12)44] — F [4i0ci + (L2C1a12+w20si12)44cJ(0.17)

and

I31/)1i Fxq2jos1a12— Fyb2jocia12. (0.18)

These expressions are not explicit relationships between F and F and ib1 and b2 because

of complications which arise from the link 1 tip slope terms (c and Equations (0.17)


and (C.18) could, in principle, be solved by simultaneous iteration to give static equilibrium

values of bh and /‘ for given contact forces. However, assuming the tip deflection of link 1 is

small compared to its length a will be small and the slope terms can be neglected, resulting

in the following approximate expressions for the static equilibrium modal deflections:

1j (F10si —F0c1) (C.19)131i

and

b2—-—(F20s12 — Fb20c12). (C.20)‘32j

The same approximation applied to equations (C.15) and (C.16) gives:

F (Lid — i0’1s1+ L2c12 + 2io28i2)

— F (Lisi—

0ici + L2s12 + (C.21)

and

T2 F (L2di2— 2

2O2s12) — F (L2s12 + . (C.22)

These final four equations provide a close approximation to the desired relationship between

the contact force level setpoints and the joint torques required to maintain them under static

equilibrium conditions. Note that in the case of a single link the complications due to the

link tip slope terms do not arise and exact expressions can be obtained.

force control for robotic manipulators with - circle

Documents