Dynamics Analysis of Flexible-Link Cooperating M anipulators

by

Qiao Sun
B.Sc.. Shanghai Jiao Tong I’niversity, 1982
M.Sc.. Shanghai Jiao Tong Cniversity. 1986

A Dissertation Subm itted in Partial Fulfillment of the

Requirements for the Degree of
D o c t o r o f P h i l o s o p h y

in the
D epartm ent of Mechanical Fngineering.

We accept this dissertation as conforming

to the required staiulard

Dr. M. Xahon. Co-Supervisor (Dept, of . Me c j i . F.ng.j

Dr. I. Shar||^ Co;;Super\^sor (Dept, of Meeh. F.ug.

Dr. R. P. Todhorodeski. Departm ental .Membtu-( Dept. of Mech. Frig. i

/-
" Dr. B. Tabarrok. D epartm ental Memlier ( D ept. of .Mech. F.ng.j

Dr. W. S. I.u. Outside M ember (Dept, of Flee. Fug.j

D r / V Y.(y). Lu h.^f eternaT yien iber ( Clemson Fiiiversity)

0 Ql.VO SCN. 1!)!)6

1 ttiversilv of \ ictoria

.All rights reserved. This dissertation may not be reproduced in whole or in p art. l>y
photocopy O f o ther means, without the permissioti of the autlior.
Supervisor; Drs. M. Nahon and I. Sharf

A b str a c t
Cooperative operation of multiple manipulators has been increasingly used in indus­
trial automation, outer space and hazardous terrestrial applications. Moreover, the

requirement for increased speeds of operation and light-weight design of robot m a ­

nipulators has made stru ctu ral flexibility a dominant factor in the design and control
of cooperating m anipulator systems.
When multiple m anipulators act cooperatively on an object, a closed-loop chain
stru c tu re is formed. R edundant actuation is one of the inherent characteristics of
such systems. Determining actuator torques necessary to achieve a prescribed object
motion is known as the inverse dynamics process. Due to the presence of the redun­
dant actuators, inverse dynamics torques for cooperating m an ip u lato r systems admit
an infinite number of solutions.
Consideration of flexibility in the links of manipulators, particu larly relevant in
space applications, not only complicates the dynamics modeling of th e system, but
also introduces instability in the inverse dynamics solution. In this study, a dynamics

model is derived for a flexiblc-link cooperating m anipulator system and the inverse
dynamics procedure for such a system is investigated. In particular, th e latter is di­
vided into two subproblems — the force distribution problem and the inverse flynam-
ics problem for serial flexible-link manipulators. The approach chosen to the force

distribution problem is to formulate it as a linearly constrained local optimization

problem. Several objectives particularly relevant to flexible-link cooperating m an ipu ­
lators are proposed. These include minimum strain energy, m inim um weighted norm
of elastic accelerations and optimal load sharing schemes. T he result ing algorithms
are shown to be effective in reducing the vibration of the system and stabilizing the
inverse dvnamics solution.
 Il l

A stability analysis of the internal dynamics of the inverse dynamics system is also

performed by using linearization. Agreement in the behavior of the inverse dynamics

system is d em onstrated between directly solving the nonlinear dynamics equations
with optimal force distribution and calculating I he eigenvalues of the plant matrix of

the linearized system. .A stability approach to the force distribution problem is then
proposed which ensures stable behavior of the internal dynam ics system under the
condition th a t the num ber of elastic coordinates of the system is less than or equal
to the total num ber of redundant actuators.

Examiners:

Dr. M. Nahon. Co-.Supervisor (Dept, of .Mech. Rug.)

Dr. I. Sharf. C o % i p e r v i ^ (Dept, of Mech. Eng.)

R. P. Podhorodeski, D epartmental Member ( Dept, of Mech. Eng.

Dr. B. Tabarrok. D epartmental .Member (Dept, of Mech. Eng.)

Dr. VV. S. Ru. Outside Memlier (Dept, of Elee. Eng.|

Dr. J / V ■S.'Liih. External Member (Clenison University)

 IV

T able o f C on ten ts

A b stra ct ii

T able o f C ontents iv

List o f T ables vii

List o f F igu res viii

A ck n ow led gem en ts x

D ed ica tio n xi

N o m en cla tu re xii

1 In tro d u ctio n 1
1.1 B a c k g r o u n d ............................................................................................................ I
1 . 1 .1 System D e s c rip tio n ................................................................................. -1
1 . 1 .2 Introduction to Some P r o b l e m s ......................................................... 5
1.2 Research O bjectives.............................................................................................. 11
1.3 Review of Previous W o r k .................................................................................... 12
1.3.1 Dynamics of Constrained Multibody Systems .............................. 13
1.3.2 Work on Rigid-Link Cooperating M anipulators .......................... 14
1.3.3 Inverse Dynamics of the Flexible-Link M a n ip u la to r s ................... 1-5
1.4 Outline of the T h e s i s ........................................................................................... 16

2 D y n a m ics M odeling 18
2.1 I n tr o d u c tio n ............................................................................................................ IS
2.2 Kinematics ............................................................................................................ 19
2.2.1 Kinematics of a Serial M a n i p u l a t o r .................................................. 22
2.2.2 Kinematics of the O b j e c t ....................................................................... 28
2.3 Formulation of the System D y n a m i c s ............................................................ 30
T A B L E OF C O N T E N T S v

2.3.1 Dynamics of A Serial Flexible-Link M a n i p u l a t o r ......................... 31

2.3.2 Dynamics of the Object ...................................................................... 36

3 Inverse D y n a m ics o f Serial F lexib le-L in k M an ip u lators 37

3.1 I n tr o d u c ti o n ........................................................................................................... 37
3.2 Inverse Dynamics of a Single Flexible L i n k ................................................. 40
3.2.1 S tate Variable D e s c r i p t i o n ................................................................... 41
3.2.2 Transfer Function R e p r e s e n t a t i o n ..................................................... 48
3.2.3 Stability Criteria for the Inverse Dynamics S o l u t i o n .................. 49
3.3 Introducing Some Solutions .............................................................................. .34
3.3.1 Park and .Asada’s Torque Transmission System ........................ .5-5
3.3.2 Bayo’s Non-causal Solution .............................................................. .5 .5
3.4 Inverse Dynamics Solution for Serial-Chain Flexible M anipulator . . . 58
3.4.1 Solutions for the Non-collocated Joint A c t u a t i o n .......................... 60
3.4.2 Solutions for the Collocated Tip A c t u a t i o n ..................................... 68
3.4.3 Validation of the Solution by Energy C h e c k .................................. 69

4 Inverse D y n a m ics of M ultip le C o o p era tin g M a n ip u la to rs 73

4.1 Underdeterminacy of the P r o b l e m .................................................................... 74
4.2 .Actuation Redundancy and Controllable internal f o r c e s .......................... 78
4.2.1 Degrees of Actuation R e d u n d a n c y .................................................... 78
4.2.2 Degrees of Freedom in Choosing Grasping W r e n c h e s ................... 78
4.2.3 Degrees of Freedom of Internal F o r c e ............................................... 79
4.3 Inequality C o n s t r a i n t s ........................................................................................ 84
4.4 C om putational .A lgorithm s................................................................................. 85
4.5 Validation of the Numerical Calculation ..................................................... 87
4.6 Examples of Cooperating M a n i p u l a t o r s ......................................................... 89
4.6.1 Planar D u a l - A r m ................................................................................... 89
4.6.2 3D D u a l - A r m ......................................................................................... 92
4.7 Example 4 . 2 ............................................................................................................ 95

5 O p tim iza tio n Approach 104

5.1 I n tr o d u c ti o n ............................................................................................................ 104
5.2 Formulation of the P r o b l e m .............................................................................. 106
5.3 Linear and Quadratic P r o g r a m m in g ............................................................... 109
5.3.1 Linear P r o g r a m m i n g ............................................................................ 109
5.3.2 Q uadratic P r o g r a m m i n g .................................................................... 110
5.4 Objective F u n c tio n s.............................................................................................. 112
5.4.1 Minimizing the Internal F o rc e s .......................................................... 113
5.4.2 O ptim al Load S h a r i n g ........................................................................ 116
 T A B L E OF C O N T E N T S vi

5.4.3 Minimizing the Norm of th e Elastic A c c e le r a t io n s ...................... 125

5.4.4 Minimizing the Strain Energy Stored in the Flexible Links . . 129
5.4.5 Combination of Various O b j e c t i v e s .................................................. 133

6 S ta b ility A pproach 141

6 .1 Description of Internal D y n a m i c s .................................................................. 142
6.2 Properties of the Internal D y n a m i c s .............................................................. 147
6.2.1 C onstant Kee and Dee . ^ ................................... 148
6.2.2 Configuration Dependent M g,, Mge, and Mgg ............................. 148
6.2.3 Nonlinear Inertial T e r m s ...................................................................... 149
6.3 Linearization ....................................................................................................... 150
6.4 Stability Issue R e v i s i t e d ................................................................................... 1.54
6.4.1 Stability of Linear Tim e-Invariant S y s t e m s ................................... 154
6.4.2 Configuration Dependent E ig e n v a lu e s .............................................. 156
6.4.3 Changing the Linear Plant of System B by Force Distribution
S c h e m e s .................................................................................................... L58
6.5 Stability .A p p r o a c h ............................................................................................. 165
6.5.1 Changing the Plant M atrix ............................................................... 167

7 C on clu sion s 172

R eferen ces 177

A In terb o d y M atrices and P ro jectio n M atrices 184

A .l Rotation M a tr ic e s ................................................................................................. 184
.A. 2 Matrix R n + i . n ........................................................................................................ 185
A.3 Interbody T ra n s fo rm a tio n s ................................................................................ 186
.A 4 Generalized Transformation M atrix for Elastic B o d i e s ............................ 186
A.5 Projection M a t r i x ................................................................................................. 187
.A. 6 Global Transformation M a t r i c e s ..................................................................... 187

B D efin itio n s and T heorem s of O p tim iza tio n 189

B.l General Form of Optimization P r o b l e m ....................................................... 189
B.2 O ptim ality C o n d i t i o n s ...................................................................................... 191
B.3 Extended O ptim ality C o n d itio n s ..................................................................... 195
B.4 Convex F u n c t i o n s ................................................................................................. 197
 vu

L ist o f T ables

3.1 Inertia Properties of the Planar .A .r m ............................................................ 66

5.1 M aximum Torque of the M o t o r s ...................................................................... 122

5.2 Maximum Value of Torques in Figure 5 . 4 ..................................................... 122
5.3 Coefficients of the Object T r a j e c t o r y ........................................................... 131

6.1 Stable Eigenvalues of A . 4 ................................................................................... 156

6.2 Stable Eigenvalues of A c ................................................................................... 157
6.3 Eigenvalues of A with Minimum Norm of Torque S c h e m e ....................... 163
6.4 Eigenvalues for Various Optim ization S c h e m e s .......................................... 164
 Vl I l

L ist o f F igu res

1 .1 T h e Mobile Servicing System ......................................................................... 3

1 .2 C oordinated Multiple Manipulators Handling a Common O b je ct . . . 5

2 .1 Tree Structure of M ultiple-M anipulator/O bject S y s t e m ........................ 20

2.2 Coordinate Frames for M ultiple-manipulator System ............................ 21
2.3 Spatial Velocity between Two Successive B o d i e s ...................................... 25
2.4 A Serial Chain of Flexible M u l t i b o d i e s ....................................................... 26

3.1 Single Flexible L i n k ............................................................................................ 42

3.2 Value of Square Root Term ............................................................................ 52
3.3 Single Link .Arm with Transmission M e c h a n i s m ...................................... 56
3.4 C om putational .Algorithm for Inverse Dynamics Solution of a Serial
Flexible A r m ....................................................................................................... 63
3.5 .A Planar Arm Tracing a 1/4 C i r c l e .............................................................. 64
3.6 G eometric Dimensions of the Flexible L i n k ................................................ 65
3.7 Solution for Example 3 . 1 .................................................................................. 67
3.8 Solution for Example 3 . 2 .................................................................................. 70
3.9 Energy Drift for Example 3.1 72

4.1 Decomposition of Grasping W r e n c h e s .......................................................... 82

4.2 Inverse Dynamics Algorithm for Multiple-Arm System ........................ 88
4.3 Flexible-Link Cooperating Manipulator Test Bed ..................................... 90
4.4 Planar D ual-arm /O bject System ................................................................. 91
4.5 .Architecture of the 6 -DOF . A r m .................................................................... 93
4.6 3D Dual-.Arm/Object S y s t e m ........................................................................ 94
4.7 Function o[t) Used for Defining a Line in 3D S p a c e ............................... 95
4.8 Grasping W r e n c h e s ............................................................................................ 98
4.9 .Actuator Torques of the Right A r m ............................................................. 99
4.10 Rigid C o o rd in a te s ............................................................................................... 100
4.11 Elastic Coordinates of the First Flexible L i n k ......................................... 101
4.12 Elastic Coordinates of the Second Flexible L i n k ..................................... 102
L I S T O F FI G U R E S ix

4.13 Error in the Energy B a l a n c e ............................................................................ 103

5.1 Minimal Internal Forces ................................................................................... 117

5.2 Comparison of M inim um Norm of Torques and M inimum Internal Forces 121
5.3 Comparison of M inimum Weighted and Unweighted Norm of Torques 123
5.4 Shifting Loads on Flexible Links towards the E n d - e f f e c to r s ................. 124
5.5 Singularity of the Elastic .Jacobian M a t r i x ................................................. 127
5.6 Results with Minimal Norm of Elastic A c c e le ra tio n s ............................... 128
5.7 End-Point Deflections with Minimum Strain Energy S c h e m e ............. 132
5.8 Comparison of the Strain Energy ( . J ) ........................................................... 133
5.9 In-Plane Deflections of the First Flexible L i n k ......................................... 134
5.10 In-Plane Deflections of the Second Flexible L i n k ...................................... 134
5.11 A ctuator Torques of the Flexible Arm in Case 1 ..................................... 137
5.12 A ctu ato r Torques of the Rigid Arm in Case 1 138
5.13 T ip Deflections of the Flexible Links in Case 1 ......................................... 138
5.14 Results of Case 2 ................................................................................................ 139

6.1 Eigenvalues of System . 4 .................................................................................. 156

6.2 Eigenvalues of System C .................................................................................. 158
6.3 Eigenvalues of System B .................................................................................. 159
6.4 .\c tu a to r Torques via Stability .A p p ro a c h ................................................... 170
6.5 Stable Elastic Motion via Stability A p p r o a c h ............................................ 171

B .l Example of Inactive C o n s t r a i n t s .................................................................... 192

 A c k n o w le d g e m e n ts

I would like to thank my supervisors, Dr. M. Nahon and Dr. I. Sharf for th e ir
guidance, patience as well as their financial support thoughout the course of my
study. I would also like to thank my advisory com m ittee members for th e ir advice
and assistance.

I wish to acknowledge the Departm ent of Mechanical Engineering which provided

the atm osphere and resources to make my study possible. My thanks also go to th e
professors and graduate students of the departm ent for many of the interesting and

helpful discussions which clarified my understanding of the subject m atter.

Finally, my special thanks are due to my family for their understanding, e n c o u r­
agement, and endurance.
 XI

To my motherland
and my parents
 XI1

N om en cla tu re

c, position of the z'th contact point on the object relative to the mass
centre of th e object expressed in the fixed fram e

i m anipulator index in the a rm /o b je c t system

P num ber of manipulators in the system
7- ! world coordinate frame

Tn link coordinate frame

n body index in a single arm
-V number of bodies in a single arm

J- E end-effector coordinate frame

7- B object coordinate frame

M total num ber of joint degrees of freedom of a single arm
.S’ total num ber of elastic coordinates of a single a rm
K total num ber of generalized coordinates of a single arm
column vector of joint coordinates of a single a rm
column vector of elastic coordinates of a single arm
T ’n+i.n inter body transformation m atrix from body n to body n + 1 for rigid
degrees of freedom
s„ number of elastic coordinates of body n
<S„+i „ interbody transformation matrix for elastic degrees of freedom
NOMENCLATURE xiii

Bn body n

On origin of
Vn spatial velocity of Bn expressed in T n
v„ absolute linear velocity of 0 „
m„ degrees of freedom of joint n
Vn projection m atrix for joint n
uin absolute angular velocity of Bn

C[,E 6 x 6 rotation m atrix from end-effector to fixed frame

C[,E 3 X 3 rotation m atrix from end-effector to fixed frame
Jr rigid Jacobian m atrix

Jg elastic Jacobian m atrix

7~a augm ented interbody transform ation m atrix
global projection m atrix for a single arm

<Sv global interbody transform ation matrix for elastic degrees of freedom
of a single arm
rg position vector of O g expressed in the inertial frame
(a . d. 7 ) angles defining the orientation of the object

Cg rotation m atrix from jF g to the inertial frame

vg absolute linear velocity of O g
wg absolute angular velocity of B g

J?g cross-product operation m atrix of wg

r, position vector of the zth
V, spatial velocity of the ith
Mn.rr 6 x 6 rlgid-body mass m atrix
NOMENCLATURE xiv

M„,re mass m atrix of Bn associated with equations for rigid motion and elastic
coordinates

M „ ,„ mass m atrix of associated with equations for elastic motion and

elastic coordinates

D„,ee dam ping m atrix of

Kn.ee stifFncss m atrix of Bn
fnT.r generalized total force on associated with rigid coordinates
fnT.e generalized total force on Bn associated with elastic coordinates
fn i,r generalized nonlinear inertial force on S „ associated with rigid
coordinates

fn[.e generalized nonlinear inertial force on Bn associated with elastic

coordinates

M rr global mass m atrix of a single arm associated with global equations for
rigid motion and rigid coordinates
Mre global mass m atrix of a single arm associated with global equations for
rigid motion and elastic coordinates
Mee global mass m atrix of a single arm associated with global equations for
elastic motion and elastic coordinates
Dee global dam ping matrix for a single arm
Kee global stiffness m atrix for a single arm
T column vector of the actuator torques of a single arm
f[ ^ global generalized nonlinear inertial force of a single arm associated
with rigid coordinates
f[ ^ global generalized nonlinear inertial force of a single arm associated
with elastic coordinates
NOMENCLATURE xv

fext.r global generalized external force on a single arm associated with rigid
coordinates

fext,e global generalized external force on a single arm associated with elastic
coordinates

fw,r global generalized force resulting from the tip wrenches on a single arm
associated with rigid coordinates

fw.c global generalized force resulting from the tip wrenches on a single arm
associated with elastic coordinates
w tip wrench applied to the object by a single arm
f force components of the tip wrench applied to the object by a single
arm

n moment components of the tip wrench applied to the object by a single

arm

fiv.w tip wrench applied to the arm expressed in ^ , v

fs.w .r generalized force on due to tip wrench associated with rigid

coordinates

ÎN.w.e generalized force on due to tip wrench associated with elastic

coordinates

^ m atrix of shape functions associated with elastic displacement in S,v

© m atrix of shape functions associated with elastic rotation displacement

in iBjv
f,r,w.r assembled generalized force due to tip wrench on a single arm associated
with rigid coordinates
NOMENCLATURE xvi

assembled generalized forces due to tip wrench on a single arm associ­

ated with elastic coordinates

Ig second m om ent of inertia of the object about its mass center

mg mass of the object
n, net external m om ent applied to the object
fe net external force applied to the object
g acceleration due to gravity

Ce position vector from the object’s mass centre to th e po int of application

of fg
a width of a rectangular beam
h height of a rectangular beam

/ length of a beam
m mass of a beam

El bending stiffness of a flexible beam

EA axial stiffness of a flexible beam
(oTp, tjp) coordinates of a point on a planar flexible beam
u vector of elastic displacements

( Xp//
row vector of shape functions for elastic displacement in th e K direction
//(xp,f) elastic displacement in the fV direction of body fram e
v{t) transverse displacement at a nodal point
v'{t) slope at a nodal point
Cl first moment of mass of a single flexible beam
cross-product operation matrix of Ci
J 1 second moment of inertia of a beam
p density of a beam
NOMENCLATURE xvii

y output vector

9i angular displacement of the joint of a single beam

■s Laplace variable

q column vector of the generalized coordinates

Q (s ) Laplace transform of the generalized coordinates
T( s] Laplace transform of actu ato r torque for a single beam
y (s) Laplace transform of the o u tp u t

H{s) transfer function of the forward system

H( s ) transfer function of the inverse system
X vector of states

{(x.t) function on the right-hand side of the first-order differential equation

/ time variable
L acceleration period
if terminal time
~ power
[V work
E{t ) energy drift

) percentage error in the energy balance

^E.RMS root mean square value of the energy balance error

t ' inverse dynamics torques to generate a prescribed tip motion in the
absence of tip wrenches
q'^ inverse dynamics solution for rigid accelerations corresponding to a
prescribed tip motion in the absence of tip wrenches
q'^ inverse dynamics solution for elastic accelerations corresponding to a
prescribed tip motion in the absence of tip wrenches
NOMENCLATURE xviii

jf matrix representing the influence of tip wrenches on the inverse d y n a m ­

ics torques

m atrix representing the influence of tip wrenches on the inverse d y n a m ­

ics solution for rigid accelerations
J3 matrix representing the influence of tip wrenches on the inverse d y n a m ­
ics solution for elastic accelerations

Tr column vector of a ctu a to r torques for a multi-arm system

Wr column vector of tip wrenches for a multi-arm system
AI matrix of contact force m ap

ric number of degrees of freedom of a closed-loop system

ria number of redu nd an t actuators
R(A. i ) range space of A ,
/V(Ai) null space of A 1

z null space variables

column vector containing the m axim um values each a ctu a to r can
generate
A- 2 coefflcienl m atrix on the left-hand side of the inequality constraints of

optimization problem
b -2 right-hand side of the inequality constraints of optimization problem
X vector of design variables for the optimization problem
C damping ratio
/(x ) objective function
W weighting m atrix
coefficient m atrix of the linear part of a quadratic objective function
NOMENCLATURE xix

A column vector of Lagrange multipliers

X* o p tim u m solution
x'*’ resultant components of the grasping wrenches
x“ internal components of the grasping wrenches
//(x ) objective function for minimum internal force scheme
/r(x ) objective function for minimum weighted norm of a ctu a to r torque
scheme

/g (x ) objective function for minimum weighted norm of elastic acceleration

scheme
/t/-(x) objective function for minimum strain energy scheme

U strain energy stored in the multi-arm system

hv nonlinear inertial terms in the internal dynamics of system B

nonlinear inertial terms in the internal dynamics of system C

Mee coefficient matrix of the second derivative term in the internal dynamics
equations of system B
Me, coefficient m atrix of the second derivative term in the internal dynamics
equations of system C
y vector of state variable of the internal dynamics
y equilibrium states
6y variation of the state

A .4 plant m atrix of the linearized model for system d

Ag plant m atrix of the linearized model for system B
Ac plant m atrix of the linearized model for system C
,\j 7 th eigenvalue of the plant matrix of a linearized system

aj, fth modal frequency

1 identity m atrix
0 zero m atrix
(•) time derivative
C h a p ter 1

In tro d u ctio n

1.1 B ackground

Robotic systems are increasingly used in industrial autom ation, deep-sea exploration,
outer space and hazardous terrestrial applications. Common requirements for robot
manipulators to be applied in these situations are autonomy and dexterity. Moreover,

in order to meet the challenges of rapidly expanded applications, higher operating

speeds and productivity, larger payload-carrying capacity, increased positioning ac­
curacy and improved system reliability have become more desirable. Many endeavors
have been m ade to enhance the versatility of potential applications and upgrade the
performance of robot manipulators toward these goals. .Among them , coordinated
operation of m ultiple manipulators has been recognized as an im portant technicjue
which offers several advantages over operating a single m anipulator in the workspace.
These are:

• improved dexterity and manipulability of the system because of the superior

capability to assemble sophisticated equipment and handle flexible objects:

 C H A P T E R I. [NTRODUCTION 2

• higher pay load-carrying capability achieved by sharing th e loads within m ultiple

m anipulators in the system;

• increased productivity which is evident in assembly tasks where m anipulators

may insert parts into the sam e object at different locations and o perate in
parallel.

• enhanced reliability due to the fact th a t redundant m anipulators are norm ally
involved so th a t alternative strategies can be adopted by the system if a com­
ponent failure is detected.

Figure 1 .1 illustrates the proposed Mobile Servicing System designed at SPAR Aerospace
Ltd.. in which th e Special Purpose Dextrous M anipulator (SPD M ) is attach ed to the

Space Station R em ote Manipulator System (SSRMS). T h e SPD M consists of two

m anipulator arm s which work cooperatively to assemble the space station, repair
satellites and perform a variety of m aintenance tasks on the space station. T h e coor­
dinated operation of multiple m anipulators gives the potential for improved quality
of performance of robot systems. However, it has been realized th a t the traditional
design of both mechanical devices and robot controllers based on the assum ption of
rigid bodies limits the system performance to a certain level. For example, present
generation of serial industrial m anipulators are restricted to a load-carrying capacity
of only Ô — 1 0 % of their weight by the requirement of rigidity to ensure a satisfactory
performance. T h e same ratio of payload to weight is retained in cooperating m an ip­
ulators system . Limitations also exist in the speed of motion of single m anipulators
and therefore the multiple m anipulator systems. When the system is op eratin g be­
yond these limits, the assumption of rigid links is no longer justified for the dynam ics
model and th e controller design. .As well, lightweight and energy efficient design of
m anipulators is preferable in many situations, most notably, in space applications
 C H A P T E R 1. IN T R O D U C T IO N

S p a c e Station
R em ote Manipulator System
(SSRMS)

Special P urpose
Oexterous Manipulator
(SPOM)

%
Mobile R em ote
Servicer B ase S y ste m
(MBS)
E lectrical/
E lectronic
C ontrol
EVA Work Station E quipm ent

Flight Telerobotic
S ystem (FTS)
Interface
Payload/ORU
A ccom odations
Support
A ssem bly
(PSA)

Mobile T ransporter
(MT) S p a c e Station Truss

MBS to MT Interface

Figure 1. 1: The Mobile Servicing System

 C H A P T E R 1. I N T R O D U C T I O N 4

where links of the m anipulators are designed to be long (due to the ineffectiveness of

the ground-based wheels and legs) and slender (since the micro-gravity environment
of space allows the gravity loads th a t dominate the design of ground-based mechanical
structures to be neglected) [1 ]. Structural flexibility therefore becomes a dom inant

factor in modeling and controlling space-based manipulators. Motivated by these

facts, this thesis focuses on various aspects of the dynamics analysis of a coordinated
m ultiple m anipulator system with flexible links.

1.1.1 S y ste m D escrip tio n

.A. system of multiple manipulators could be very complicated in terms of its kine­
m atic stru ctu re and dynam ic features. However, we do not intend to confront all the
problems th a t might stem from considering the most general model. Instead, we still
need to specify the system with some assumptions in order to highlight the essential

characteristics of the system. Figure 1 .2 represents a system of P robotic arms han­

dling a common object. In particular, each manipulator is assumed to be composed
of elastic bodies connected by rigid joints. This is to simulate a situation where link
flexibility of the m anipulators is a dominant factor as for space applications. The

commonly handled object is assumed to be rigid. The manipulator are assumed to be

driven by jo in t-m o u n te d actuators. The serial arms are not kinematically redundant
th a t is. the num ber of degrees of freedom of each serial manipulator is equal to the
dimension of the task space. Rigid grasping which allows no relative motion between
the object and the m anipulator end-effectors is also assumed.

Usually, each m anipulator is capable of performing certain tasks in its own workspace.
However, multiple manipulators become constrained kinematically and dynamically
when they form closed loops through a commonly grasped object. When this hap­
pens, the total num ber of degrees of freedom of the system decreases whereas the total
 C H A P T E R 1. I N T R O D U C T I O N

arm P

arm /
arm /

Figure 1.2: Coordinated Multiple Manipulators Handling a C om m on O bject

num ber of actuators remains unchanged and the system is said to be red u n d a n tly ac­
tuated. We also note th at general motion of the system in 3-D space is allowed while
the links may undergo bending, extension and torsion.

1.1.2 In tro d u ctio n to Som e P ro b lem s

Interest in developing coordinated multiple m anipulator systems arise from realizing

th a t some tasks which may be difficult or even impossible for a single m a n ipu lator to
accomplish can be performed by two or more m anipulators operating cooperatively.
Hence, a system of coordinated multiple manipulators is normally composed of several
 C H A P T E R I. INTRODUCTION 6

existing serial manipulators. It is the way th a t manipulators o perate makes it a topic

of special interest.

Extensive studies have been carried out for rigid cooperating m an ip u lato rs. These
are prim arily related to motion planning, coordinated control and application software
[2]. On the other hand, studies of flexible-link manipulators are still in th e design,

simulation and experimental stages. When considering the problem of a coordinated

m ultiple m anipulator system while including structural flexibility effects, fu nd am en tal
issues related to the kinematics and dynamics modeling of the system a n d its control

need to be re-investigated. Dynamics analysis is thus an im p ortan t consideration and

is expected to yield answers to problems ranging from mechanical design to control
algorithm development. Since the research issues related to the m ultiple m a n ip u la to r
system s are mainly of coordinated control type, we are particularly interested in
performing the inverse dynam ics analysis concerned with the driving forces a n d /o r
torques for a given trajectory of the reference body — the commonly grasped object.
.A,lthough this study is directed primarily toward control applications, it also provides
useful information for the design engineer with concerns on the control aspects.

D y n a m ics M od ellin g

Form ulating a concise description of the system dynamics is the first problem we
face. Although much research has been conducted on both rigid and flexible m u lti­
body. constrained and unconstrained systems, a concise but effective d ynam ics model
suitable for the inverse dynamics analysis of a flexible-link cooperating m a n ip u la to r
system has not been formulated. On the other hand, in order to analyze and pre­
dict the system behavior precisely, an accurate dynamics model is especially useful.

However, there always exists a tradeoff between model accuracy and co m p u ta tio n al
efficiency. From the standpoint of system control, it is satisfactory th a t th e dynam ics
 C H A P T E R I. INTRODUCTION 7

model be sim ple to calculate and be reasonably accurate. In this study, finite element
m etho d is used to model th e elastic links of the manipulators. There is no limitation

on the n u m b er of elem ents used to discretize the flexible beams. However, in the
simulations carried out to illustrate the performance of inverse dynamics calculation,

flexible links are modeled by one or two beam elements as it has been shown by many
researchers th a t the first two modes of vibration are most significant [-3 ].

P la n n in g

In coordinated multiple m anipulator systems, two or more manipulators share the

same workspace and act in a cooperative manner. Coordination should be ensured

both kinem atically {motion planning and tracking) and in terms of forces {internal
force control). Motion planning, in this case, differs from what is usually discussed in
single kinem atically redundant or non-redundant manipulators. Emphasis is placed

on coordination within the system rather than optim ality associated with individ­
ual m anipulators. This involves avoiding collisions with other manipulators in the

workspace, determ ining the best set of contact locations on the object for each m a­
nipulator [4], e.g., vectors c,. {i = 1___, P) in Figure 1.2. and defining a suitable
trajecto ry for th e object which minimizes the excitation of the system elastic modes
[5]. It is also necessary th a t the trajectory of the object lie in the common workspace
of all the m anipulators.

Once the motion is pre-planned for the object, manipulators are required to act
in a way to achieve the desired motion of the object. It is im portant to realize that
redundant actu atio n is an inherent characteristic, once the multiple manipulators
contact the common object. By definition, actuation redundancy allows for an infinite

set of actuation strategies. This may be beneficial since actuators may share the load
evenly so th a t th e overall load carrying capability of the system is enhanced. However.
 C H A P T E R 1. I N T R O D U C T I O N 8

w ithout force coordination, actuators may act against each other. In the worst case,
th e m anipulators may crush or tear the object and even cause task failure.

R eso lu tio n o f A ctu ation R ed u n d an cy

B etter understanding of actuation redundancy promises better utilization of th e re­

d u n d a n t actuators. Indeed, this redundancy results in an indeterminacy of the grasp­
ing forces and moments (grasping wrenches [6 . 7]) for carrying the object over a
prescribed trajectory. N akam ura [8 ] proposed to decompose the total generalized
force acting on the object into two orthogonal subsets, the resultant force and the
internal force. The resultant force lies in the range space of the grasp m atrix [4] of the

system and contributes to the motion of the object. It can be uniquely determ ined
for a given object trajectory. By contrast, the internal force lies in the null space of
the grasp m atrix and produces a squeezing or tearing effect on the object. It is not

completely defined by the dynamics equations for the object. Therefore, the term
force control in the context of multiple m anipulator systems specifically refers to the
control of internal forces. It is necessary that the internal forces be controlled to avoid

excessive values even if the prim ary task is simply to track a predefined motion of the
object.

T he above leads naturally to the lorce distribution problem [9. 10. I I . 12]. It
is pu t forward for determining the set of grasping wrenches to be applied to the
object so as to achieve a predefined object motion and ensure satisfactory internal
forces. To define the grasping wrenches, the force balance equations, i.e.. th e d y n a m ­
ics equations of the object should be solved. ,-\s mentioned earlier, these equations are
underdeterm ined. Extra constraints need to be imposed to give a unic|ue solution.
Evidently, optimization techniques can be used to find the best solution. M ath e ­
m atical descriptions of the optimization problem need to be formulated to define a
C H A P T E R 1. I N T R O D U C T I O N 9

solution which minimizes a certain objective function while satisfying some equality
and inequality constraints [13]. T he force balance equations of the object co n stitu te
the prim ary constraints. They represent a set of linear equality constraints. D epend­
ing on the requirements of the particular task and the system under consideration,

inequality constraints are optional. One practical concern with the cooperating m a ­
nipulator systems is the limited capacities of the actuators. W hen these are included
as constraints, dynamics equations of the manipulators are used to relate th e a c tu a to r
forces/torques to the grasping wrenches [14] so that linear inequality constraints in
terms of the grasping wrenches are formulated.

Com pared with setting up the constraint equations, formulating the objective
function is less straightforward. Firstly, it depends on the type of optim ization tech­
nique to be adopted. Common choices for the objective functions are linear and

quadratic functions of design variables (e.g.. grasping wrenches). Secondly, interests

in obtaining optimal quantities are usually many and their priorities may be different
for various systems. .Among the many possibilities, commonly implemented ones are

the minimal internal forces [15]. minimal power consum ption [16, 17] and optim al
load distribution within the actuators [10. 17, 12], .Attempts have also been m ade to
employ a weighted linear combination of the many objectives to obtain a combined
optimal performance [13], W hen the system exhibits structural flexibility, intuition

suggests th a t minimizing the system vibration may be critical for upgrading the per­
formance of the system [IS, 19], However, it is not so obvious how this objective
should be formulated and how it can be related to other criteria already used in the
rigid case.
 C H A P T E R I. INTRODUCTION 10

S ta b ility o f th e Inverse D ynam ics Solu tion

To clarify th e above concerns, a detailed investigation of the problems arising in the

inverse dynam ics solution of flexible-link m anipulators is necessary. Previous research
shows th a t th e inherent difficulty with the inverse dynamics solution for flexible-link

serial m a n ip ulators is its instability [20, 21]. This instability results from th e fact
th a t the jo in t-m o u n ted actuators and the required end-effector motion are coupled
by s tru c tu ra lly flexible links. Hence, there is a time delay for the end-effector to
react in response to a given joint actuation. Therefore, the causal solution which

determ ines th e joint actu ato r torques according to the instantaneous motion of the
end-effector exhibits high magnitude and frequency. This is likely to excite th e system
vibration an d result in unbounded values of dynamics variables. .Accordingly, Bayo
[2 1 ] developed a noncausal inverse dynamics solution for the joint actuator torques
which s ta r ts earlier and ends later than does the tip motion. This solution is shown

to be unique and stable [21]. However, the noncausal solution requires solving the
problem in th e frequency domain. This in turn results in extensive com putations for

the transfo rm atio n of the dynamics model, trajectory information and the solution
between th e frequency domain and the tim e domain. It is also necessary th a t the

com plete tim e history of the desired motion of the end-effector be known before one

starts solving th e problem, which is not always possible. Therefore, it would still be
desirable to find a causal solution with the stability issue resolved.

It hcLS been suggested by several other researchers th a t the stability of the inverse
dynam ics solution can be guaranteed if actuator forces/torques are applied at the
location where the trajectory is specified [22]. Very interestingly, we noted th a t
when m ultiple m anipulators act cooperatively on a common object, wrenches applied
by each a rm to the commonly handled object can be viewed as active forces and

torques applied to the other arms. This thought is supported by the fact th a t m any
 C H A P T E R 1. I N T R O D U C T I O N 11

possible grasping wrenches exist to achieve th e given object motion due to the presence

of th e redundant actuators. If the grasping wrenches could be chosen arbitrarily,

the cooperating manipulators could be viewed as driven by the tip wrenches. This
scenario accomplishes a collocation of th e actuation and the end-effector motion,

which ensures a stable dynamic behavior of the system. In reality however, the
grasping wrenches have to satisfy the object dynamics, but the internal forces can
be chosen freely. We therefore propose as a goal of the force distribution scheme to
ensure stable dynamics of the system by appropriate selection of internal forces.

A nother objective is to reduce the system vibration, by which, we mean reducing

both the m agnitude and the frequency of the responses of significant elastic modes.
Minimizing the strain energy stored in the elastic members of the system effectively
achieves minimization of elastic deformations. Minimizing the tip deflection may

be appropriate for improving the tip trajecto ry tracking accuracy. Minimizing the
elastic accelerations helps to smooth the elastic motion and therefore reduces its
frequencies. It is worthwhile to experim ent with these choices and their combinations
as it helps achieve an effective force distribution scheme for flexible-link cooperating
manipulators.

To sum m arize the above discussion, this thesis primarily endeavors to answer two
questions: I) how does the link flexibility influence the force distribution problem in
coordinated multiple manipulator systems? 2 ) how will the inverse dynamics solution
for flexible-link m anipulator system be affected by the redundant actuation of the
system?
C H A P T E R 1. [NTRODUCTION 12

1.2 R e s e a r c h O b je c tiv e s

In light of the above questions, the principal objectives of th e proposed work are
summarized as follows:

1. A general and accurate model needs to be formulated to describe the dynam ics
of the system. T h e model will allow three dimensional large rigid-body m otion
and small superimposed link deflection. The model will be 'accu rate' in the
sense th a t all the nonlinear terms due to coupling between rigid and elastic
motions will be included.

2 . Based on the dynamics model, the inverse dynamics problem of serial flexible-
link manipulators will first be investigated to gain insight into the various as­
pects of the inverse dynamics solution arising from relaxing the assum ption of
structural rigidity.

3. Formulate the force distribution problem for flexible-link m ultiple m an ip u lato r

systems. In particular, optimization techniques will be employed to find the
best solution for a variety of criteria.

4. Investigate the relationship between actuation redundancy and the instability

aspect of the inverse dynamics problem, and the possibility of improving the
dynamics behavior of the inverse dynamics system by varying the internal forces.

5. Implement numerical simulations on the proposed force distribution scheme

and the inverse dynamics solution. For purposes of mechanical design, these
simulations are carried out on examples with various arrangem ents of flexible
and rigid links. Some design guidelines are to be obtained in this respect.
 C H A P T E R 1. [NTRODUCTION 13

1.3 R e v ie w o f P r e v io u s W ork

In view of the aforem entioned research objectives, in this section, a detailed literature
review is presented on the related topics.

1.3.1 D y n a m ic s o f C on strain ed M u ltib o d y S y ste m s

■A. system of coordinated multiple manipulators handling a com m on object forms a

constrained m ulti-body system. The fundamental theory of form ulating the d yn am ­
ics of constrained m ulti-body systems is well established [23, 24, 25. 26]. Lagrange
multipliers are com m only used to append the constraint equations to the Lagrange
equations for the dynam ics equations of the system. A nother way to deal with con­
straints is to insert th e constraints into the Lagrange equations by means of a penalty

formulation [25).

T he solution to the inverse dynamics problem for a closed-chain rigid-body system

was first introduced by Luh and Zheng [26]. They proposed th e 'virtual c u t'm e th o d
which involves ‘cutting" each kinematic loop in the system at an unactu ated joint to
produce a kinem atic chain with a tree structure. Using the inverse dynamics algo­

rithm for a serial chain and with an explicit calculation of Lagrange multipliers, the
forces/torques required at th e a ctuated joints can be found. Based on the sam e idea of
"virtual cut'. N akam ura and Ghodoussi [27] derived the torques applied at the active
joints by projecting the generalized torque vector of the unconstrained tree-structure
system. This was accomplished with a linear map incorporating the .Jacobian of the
passive joints with respect to the active joints. The procedure elim inated the neces­
sity of calculating Lagrange multipliers. These methods, however, have only been
used in the context of systems without redundant actuation.

Relatively little work has been done in closed-loop systems with structurally flex­
 C H A P T E R 1. [NTRODUCTION 14

ible members. Kim and Haug [28] derived a recursive formulation for the simulation
dynamics of a closed-loop Hexible-multibody system. C ut joint constraint equations

and the associated Lagrange multipliers are introduced to represent c u t joint reaction
forces and torques. Bayo et al. [29] extended Bayo’s inverse dynam ics algorithm for

serial chain flexible m anipulators to the closed-chain case. T h e m e th o d is not suitable

for a general inverse dynam ics analysis in th a t it assumes th a t all th e joints follow a
nominal predeterm ined trajectory.

1.3.2 W ork on R igid -L in k C o o p era tin g M a n ip u la to rs

.A. coordinated m ultiple m anipulator system belongs to a special class of closed-loop

mechanisms, in which redundant actuation is a prominent characteristic. The same

framework encompasses multi-fingered mechanical hands and multi-legged locomotion

vehicles.

Among the m any issues relating to the cooperative m anipulation by robotic arms,
the force distribution problem has received considerable a ttentio n. T h e presence of

redundant actuators in a system makes it possible to use optim ization techniques to

actively distribute the load in the system [30]. Orin and Oh [13] proposed to d e te r­
mine the joint torques while minimizing the energy consum ption and the m axim um
normal tip reaction force for a legged locomotion system. T h e ir m eth od involved

solving a linear program m ing problem to minimize an objective function comprised

of a weighted sum of joint torques, joint rates and task space reaction forces. This
was done while satisfying equality constraints (force balance ecpiations) and inequal­
ity constraints (m ax im um joint torque capacities and tip reaction force constraints).
This approach, while appealing, proved to have the disadvantage of excessive co m pu ­
tational expense [31].
 C H A P T E R I. [NTRODUCTION 15

Since uniqueness of the solution calculated with linear program m ing is not en­
sured, discontinuity of the solution can be observed for small changes in the con­
stra in ts [14]. By contrast, quadratic programming yields a globally (in the design
space) unique solution and proves to be more efficient. It has thus been used to m ini­

mize the internal forces [15], the strain energy in th e object [32] and the power losses
[33, 9, 17]. Taking into account the different capacities of the arms for carrying the
loads, an optimal force distribution can be achieved by assigning different weights to
th e design variables in the objective function [10]. Noticing th a t only the objective

function is non-linear, Luh and Zheng [16] applied the approxim ate linear program ­
m ing m ethod to solve the non-linear programming problem. O ther researchers have

investigated the use of the 'p-ri^^rn approach' to solve the constrained optim ization
problem via unconstrained optimization techniques for multiple arm m anipulators
[ 12].

1 .3 .3 Inverse D y n a m ics o f th e F lexib le-L in k M an ip u lators

.A.S alluded to in the previous section, the major problem with the inverse dynamics
solution for serial flexible-link manipulators is th a t it may be unstable. To deal
w ith this problem, approaches can be classified into two categories: the non-causal
solution and the causal solution. The former approach was proposed by Bayo [21].
Kwon and Book [34] later improved this approach by reducing the com putational

burden involved in the computationally intensive Fourier transformations. T he causal

m etho ds have been pioneered by .-\sada and Park [35. 22]. and they have solved the
problem in the time domain. The stability of the solution is guaranteed by means
of a torque transmission mechanism to accomplish a collocated construction of the

sensors and actuators [2 2 ].

not her difficulty with the inverse dynamics solution for flexible-body systems
 C H A P T E R 1. I N T R O D U C T I O N 16

arises because of the coupling effects between the rigid-body motions and the elastic
deformations. In contrast to the rigid inverse dynamics solution, the nominal joint
m otion cannot be determined solely by solving the inverse kinematics equations for
the prescribed end-effector motion. Asada et al. [35] proposed to use a ‘v irtu al link
coordinate system ’ to decouple the kinematics from the dynamics of a flexible-link
m anipulator. They also assumed that all the joints follow a nominal predeterm ined
trajectory. C hang and Hamilton [36] also calculated the causal solution by using
an ‘equivalent rigid link system ’ to describe the rigid kinematics of the system . Xi
and Fenton [37] derived a causal solution by solving the kinematics and dynam ics
equations simultaneously and integrating them in the time domain. However, most
of these studies leave the stability issue unaddressed.

1 .4 O u tlin e o f th e T h e sis

T h e kinematics and the dynamics of a system of multiple flexible-link m anipulators

to handle a common rigid object are modeled in C hapter 2. As a result, fundam ental
equations for the system are composed of two sets: that of the m anipulators and

th a t of the common object. Constraint forces and torques between the m anip u la­
tor end-effector and the object are explicitly included in the dynamics equations in
view of their significance to the force distribution problem and actuation redundancy
resolution. Following Hughes’ procedure [38], derivations of the kinematics relations
between the end-effector and the variables representing the degrees of freedom of a
serial flexible-link manipulator are detailed. In particular, rigid and elastic .Jacobians
are formulated which map the configuration space velocities into the Cartesian space
end-effector velocities. .Also, the transpose of the aforementioned .Jacobians m ap the
C artesian tip wrenches into configuration space generalized forces.
 C H A P T E R 1. I N T R O D U C T I O N 17

In C h ap ter 3, the inverse dynam ics problem is investigated for a serial flexible-link
m anipulator. We explore analytically the stability issue of the inverse dynam ics sys­

tem driven kinematically by the prescribed end-effector trajectory. Both s ta te space

description and the input-output relation for a single flexible link case are established
and the latter is used to determ ine the stability status of the inverse dynamics system .
The causal inverse dynamics solution for serial flexible-link manipulators is described
and written with the view of showing the influence of the tip wrenches on th e solution
of the prescribed tip trajectory and the effect of the tip wrenches.

T h e inverse dynamics solutions for the multiple flexible-link m anipulators are dis­
cussed in chapter 4. In particular, redundancy in choosing the grasping wrenches and
in applying the actuator torques required in handling the object along a prescribed
trajectory are identified. Moreover, the degree of redundancy in determ ining the

grasping wrenches and in calculating the actuation torques are shown to be the same
as the num ber of components of the internal forces. .As a result, the inverse dynam ics
algorithm for multiple flexible arm system is presented in which tip wrenches arc
chosen to be the design variables.

In chapter 5, optimization techniques are discussed and used to solve the force
distribution problem. .After comparing the merits of using the linear and quadratic-
programming techniques, the latter is adopted. Objective functions are discussed with
an emphasis on tackling the particular problems of flexible-link cooperating m a n ip u ­
lators, th a t is, stabilizing and reducing the system vibration. Numerical exam ples are
carried out for each objective function. Comparisons of the inverse dynam ics solution
by using different objective functions are also made.

T he issues related to stability of the internal dynamics of the inverse dynam ics
system for a cooperating flexible-link m anipulator are further discussed in C hap ter
6 . These are particularly relevant to the inverse dynamics control of flexible ma-
C H A P T E R 1. INTRODUCTION 18

nipulators. Linearization of the internal dynamics equations are perform ed around

equilibrium points. Dynamics behavior of the original nonlinear system can be pre­
dicted by examining the behavior of the linearized system . Accordingly, a stability
approach to the force distribution problem is proposed which, under certain condi­
tions, ensures a stable dynam ic behavior of the causal inverse dynam ics systems.

Finally the conclusions are drawn in C hapter 7 together with recom m endations
for future work.
 19

C h a p te r 2

D y n a m ic s M od elin g

2 .1 I n tr o d u c tio n

A system composed of multiple flexible-link coordinated manipulators and an object

falls into the class of flexible m u ltih o d y systems. In particular, it contains one or more
kinem atic loops when the multiple arms are handling the common object. Standard

m ethods for deriving the equations of motion of a closed-loop multibody system

include two basic steps [2.3]. First, a virtual cut is made at one or more hinges
to yield a system with a tree structure. Obviously, this can be done in different
ways. O ne way is to virtually cut the closed loops in such a way that a reduced
system is produced. In the reduced system, only holonomie constraints remains. This
means th a t places of virtual cuts should at least include all those where nonholonomic
constraints are introduced. Dynamics equations are then derived for the reduced

system for which modeling techniques have been well developed. In the second step,
both kinem atic constraints and the internal force relations at the places of virtual cut
are re-introduced. In this way. the original system with closed loops is recovered.
 C H A P T E R 2. D Y N A M I C S M O D E L IN G 20

For a system of m ultiple cooperating manipulators, each of the m anipulators is

considered to be a serial chain. One or more closed loops are formed when the serial
m an ipu lators contact the object. Holonomie a n d /o r nonholonomic constraints are
introduced where the object is grasped depending on the type of contact between
th e m an ip u lato r end-effectors and the object. Rigid grasping, for example, allows no
relative motion between the gripper and the object and therefore imposes holonomie
constraints, whereas th e point contact in multi-fingered mechanical hands introduces
both holonomie and nonholonomic constraints. Besides, if the motion of the object

is pre-defined, the system is divided by the object into kinematically independent

sub-systems. T h e reduced system defined earlier can then be obtained by virtually
c u ttin g the system at the grasp locations. For a system of P manipulators with a rigid

grasp, the m inim um num ber of virtual cuts is P — I. However, in order to facilitate
the analysis of the constraint forces and torques between the end-effector and the
object (grasping wrenches applied by the manipulators), we make virtual cuts at all
the places of grasp to allow us to explicitly include all the grasping wrenches in the

equations of motion (F ig u re 2 .1). Therefore, the reduced system is composed of P

m anipulators and the object. Modeling techniques developed for serial fiexiblc-link
m anipulators are immediately applicable. Some modifications are needed to treat

external end-effector wrenches. .\s well, we need to express the equations of motion
in a form which is suitable for the force analysis of multiple m anipulator systems
which will be discussed in chapter 4.

2 .2 K in e m a tic s

While form ulating the dynamics equations is the ultim ate goal of this chapter, e sta b ­
lishing the kinematics relations is an essential step. To start, we first define several
 C H A P T E R 2. D Y N A M I C S M O D E L I N G 21

-n

-n -n

Figure 2.1: Tree Structure of M ultiple-M anipuiator/O bject System

coordinate frames with respect to which the kinematic quantities (position, velocity
and acceleration) are expressed. Each of the frames is a three dimensional orthogonal
coordinate frame (F ig u re 2.2).

W o r ld c o o r d i n a t e f r a m e !Fi is a frame Hxed in inertial space. It represents a

unique reference for all the moving bodies in the system. Its origin is d en o ted
by 0 [ and the three coordinate axes by X i , Y t and Z i - For convenience, we
use {O /, X I, Y I. Z i] to represent the origin and three axes of the c o o rd inate
svstem.
 C H A P T E R 2. D YN AM IC S MODELING 99

arm i Object

Figure 2.2: Coordinate Frames for Multi pic-manipulator System

L i n k c o o r d i n a t e f r a m e J-n is a frame attached to the link of the m a n ip u la to r with

its origin located at the proximal end of the link. For link n (n = 0, I. • • • . N,)
of m anipulator i (i = the link frame is denoted by a n d the
origin and axes by ( 0 „,, K„,. In particular. X n , is o rien ted in the
direction along the line connecting the proximal and distal ends of th e link in
its undeformed state.

E n d -effec to r c o o rd in a te fra m e is similar to the link frame except th a t it is

attached to the end-effector of the manipulator. Its origin and axes are den oted

by { O s t , X e ,- YEl, Z e i ]-

O b j e c t c o o r d i n a t e f r a m e !F b is attached to the commonly handled o b je c t w ith

its origin located at the mass center of the object. Its origin a n d axes are
 C H A P T E R 2. D Y N A M I C S M O D E L IN G 23

denoted by { O g , X g , Y g , Z g } .

We also need to introduce the following definitions which will be used repeatedly
in this thesis.

G e n e r a l i z e d c o o r d i n a t e s For a rigid body system, generalized coordinates are a set

of independent variables Çri, Çr2 , ' -, ÇrM which completely define the location
and orientation of each rigid body in the system. For a deformable multibody

system, the generalized coordinates are composed of Çri,?r 2 , " , <7rM and the

set of tim e-dependent variables (?ei, ?e2 , • • •, <?e5 used to approxim ately model
the elastic deform ation of the flexible bodies in the system . T he total number
of generalized coordinates is K = M 4 - S.

C o n f i g u r a t i o n s p a c e is a K dim ensional space in w hich gen eralized coordinates

</ri, <7r2, • • • . 9rM T 9 e i,< 7 e 2 , ' '' . Correspond to a particular p oint.

For a multiple-chain system, bodies or joints must be identified by two indices,

namely the index i for th e individual chain and the index n for the particular body

within the chain. Generalized coordinates used to model each body of the system
requires the third index. This becomes cumbersome. For this reason, we will omit
th e chain index i when we investigate a single serial m anipulator and will omit body
index n when we stud y the assembled multiple-chain system. In the latter case, each
serial chain is treated as a whole.

2.2.1 K in em a tics o f a Serial M an ip ulator

As mentioned in the previous chapter, throughout this work, it is assumed th at the

desired trajectory of the object has been pre-planned. This desired trajectory can be
C H A P T E R 2. D Y N A M I C S M O D E L IN G 24

determined either by off-line path planning o r by on-line trajectory generation. In

any case, advance knowledge of the desired o b ject trajectory is required, if not for
the whole task period, at least for a small interval of future time. In this situation,
the multiple manipulators are kinematically independent of each other. Due to the

rigid grasp, prescribing the object trajectory is equivalent to prescribing the motion
of each end-effector. Therefore, the kinem atics relations established in this section
are those relating the Cartesian space end-effector motion to the configuration space
generalized motion.

The complexity of deriving the kinematics relations for an interconnected flexible

multibody system stems from the coupling effects between bodies and those between
the rigid motion and the elastic oscillation. Moving frames such as the link coordi­
nate frames introduced above, are usually used to account for the rigid (gross) motion,

with respect to which the elastic deformations are expressed. Several ways of setting
up these moving frames have been proposed. Book [39] attached frames to the prox­
imal end of every link and extended Denavit and H artenberg’s 4 x 4 homogeneous

transformation m atrix notation to describe th e kinematics of flexible links connected

l)v rotary joints. Hughes and Sincarsin [3S| also defined the moving frames attached
to each individual body in its rigid configuration. Interbody transformation matrices
7”E (for rigid degrees of freedom) and <Se elastic degrees of freedom
where is the num ber of elastic coordinates for body n) are used to perform transfor­

mations of spatial velocities between two interconnected bodies. T heir method allows
for general interjoint motion, that is, arbitrary interjoint displacements and rotations.

Chang and Hamilton [36] used an equivalent rigid link system to represent the large
motion of the arm . The small motion of the sy stem consists of both small rigid-body
motion and elastic deformation. Asada ct al. [35] and Bayo et al. [29] assigned mov­
ing frames to each body with one of the axes coinciding with the direction from the
C H A P T E R 2. D YN AM IC S MODELING 25

proxim al joint to the distal joint of the link in the deformed configuration. In this

way, rigid motion can be decoupled from the elastic deformation by assum ing th at
all joints follow their nominal trajectories obtained from the rigid inverse kinematics
solution.

To establish an expression for the end-effector motion in terms of th e motion in

configuration space, a direct method would be to derive equations for the end-effector
position and orientation in terms of the configuration space generalized coordinates.
For the purpose of inverse dynamics analysis, we are particularly interested in the

inverse kinem atics which determ ine the generalized coordinates corresponding to the
end-effector motion. Using the kinematics expressions at the coordinate level (posi­
tion) has some disadvantages. One is th a t only for some specially configured rigid
m anipulators there exists a closed form formulation of the inverse kinematics solu­
tion. T h e other is that even if a closed-form inverse kinematics equations can be
o btained for some particular manipulators, they are nonlinear functions of the gen­
eralized coordinates. The generalized coordinates in the inverse kinematics problem

a d m it multiple solutions. For these reasons, the differential kinematics relations which
describe the differential changes of the position and orientation of the end-effector re­
sulting from the differential changes of the generalized coordinates are advantageous.
T hese are linear algebraic equations. For rigid non-redundant m anipulators, a unique

solution to the inverse kinematics problem is guaranteed if the system is not at a

singular configuration.

For the present study, we adopt the notational scheme and definition of body
coordinate frames proposed by Hughes and Sincarsin [38. 40]. Small changes are
m ad e to some symbols in order to incorporate traditional robotics notation. .As
described in reference [38], the velocity kinematics between successive bodies and
(F ig u re 2.3) is:
 C H A P T E R 2. D Y N A M I C S M O D E L IN G 26

n+2

r»+ l

A
o,

Figure 2.3: Spatial Velocity between Two Successive Bodies

■ün+i — '7 * n + l . n W n + « ^ n + l . n 9 n , e + ”^ ^ + 1 9 n + I , r ( ‘- - 1 )

where a spatial vector Vn<E R® is used to define a combination of the absolute linear
and angular velocities of a moving frame expressed in its own frame. T h a t is.

Vn
v„. = ( 2 .2 )

Vectors R^" and R'""+‘ in eq. (2.1) are referred to as generalized ve­
locities associated with th e elastic and rigid degrees of freedom, respectively. We
also use .s„ to denote th e num ber of elastic coordinates used to model the flexible
body S n and m„ for the num ber of degrees of freedom of joint 0 „ . T h e transfor­
m ation matrices T'n+i.n'ë. and «S„+i.,iG as well as the projection matrix
7^„+ i G arc defined in .Appendix A. Conceptually, eq. (2.1) means th a t the
sp atial velocity of 'n the chain is the sum m ation of three effects: the first due to
the velocity of 0 „, the second due to the elîistic deformation of and th e last due
to the relative motion at th e joint On+i- Referring to F ig u re 2.4. we use eq. ( 2 . 1 ) to
define the generalized velocity of 13e in terms of variables of th e previous body. t3s-
 C H A P T E R 2. D YN AM IC S MODELING 27

S.v

#
Be

Figure 2.4: .A Serial Chain of Flexible Multibodies

Since it is always possible to assume that the end-effector is rigidly attach ed to the
last link of the chain, the spatial velocity of !Fe is

V E = 'T E ,^ fV i\r + S e ,.\- i p w e ( - .- 1 )

Using eq. (2.1) again to substitute for u.v into the above equation, we obtain

V E = T £ .,v { T + <S.V„V-1 Ç ,V -l.f + "P.V Q n . t } + ^ E . N g.V.e (--f )

T h e sam e procedure can be applied to replace Uw-i and so on until the base body of
the chain is reached. In th at case, we obtain

ve = T'E.o'PoqQ^ + T e . i V x ^ + . . . + T e .n V \ r

T T ' £ .I« S i,0 Ço.e T T E : 2 ^ 2 A 9 l.e + • • • + ^ E . . \ ' ^ . \ . X - l Ç ,V -l.e + ^ E , X

Introducing vectors q^ and q , which combine the rigid and elastic coordinates re-
C H A P T E R 2. D Y N A M IC S M O D E L IN G 28

spectively for all bodies in the chain as

<70.r

9 l.r
<7r = 9, = (2 .6 )

^ N .r

the quantity V e can be written in a matrix-vector form as:

T
— '^ E .o 'P o T ' e .i ' P i ■■■ T e .s ' P n 9 o .r 9nr ••• ^

r 7"
+ ■■■ 'T'e .N ^N.N -I S e ,N 9 o ,e 9ue “ •

Since usually the desired end-effector motion is prescribed with respect to a fixed
reference frame, we need to transform v e from IF e into the inertial fram e IFi by
pre-multiplying it by the generalized rotation m atrix C T h i s generalized

rotation m atrix is defined as C/.£ = d i ag{ C e e ■G e e } and is the rotation

m atrix of th e end-effector frame relative to the inertial frame. To tre a t th e multiple
bodies in a chain as a single entity, wc now use v to denote the spatial end-effector
velocity expressed in the inertial frame. Finally, the velocity kinem atics equation

(2.7) can be written more compactly as:

V = J r Çr + Je ( 2 .6 )

where J r and are referred to as the rigid and elastic .Jacobians. T hey are defined
bv

Jr = C e e 'T'a 'P z (2.9)

Jr = CeeTaSzS'e_s ( 2. 10)

where

T a = T E.Ü T ea ■ • T e .s (2 . 11)

^ E .S — 0 0 ••• 0 S E.s ( 2 . 12)

 C H A P T E R 2. D Y N A M IC S MODELING 29

with and «Ss as defined in Appendix A.

2 .2 .2 K in e m a tics o f th e O b ject

S p ecify th e O b jec t M otion

As mentioned earlier, throughout this work, we assume th a t the motion of the object
is prescribed; th a t is, the position of O g and orientation oI b are known as a
function of tim e. One way to define this trajectory is by specifying, in the inertial

frame, the position vector rg of O g and the orientation m atrix C g € of !F b -

•Another more com pact representation is by a column m a trix of six components:

re
a
reit) = (2.13)
j

where (a . 3. -<] can be Euler angles or roll-pitch-yaw angles. By knowing (q. 3. 7 )

as a function of tim e, the orientation m atrix C g can be obtained [41] as a function of

time. Since we have decided to consider differential kinematics as a means of defining

the motion of the system in its configuration space corresponding to the C artesian
space object motion, the prescribed motion of the object m ust also be specified at the
velocity a n d /o r acceleration level. .Assuming the velocity level kinematics, the spatial
velocity of B e is given as:
vg
Vb = 12.14]
a-’ g

where both v g and w g are expressed in the inertial frame. While it is clear th a t
differentiating rg in eq. (2.13) with respect to time gives Vg. differentiating (a. 3. 7 )
 C H A P T E R 2. D Y N A M I C S M O D E L I N G 30

however, does not yield Wg. To o b ta in th e spatial velocity in eq. (2.14) from eq. (2.13),
we need to consider the following

C g = f?^C g (2.15)

from which it follows

f? ^ = C g C ^ (2.16)

In the above, m atrix f2g represents a cross-product operation of Wg = [u;^ ujy ui-Y
and is defined by
0----- --- U l , Ü jy

UJ. 0 —Ur (2.17

— u jy L u 'x 0

and C g can be obtained from ( à . /?. 7 ). .A.Iso notice th a t in eq. (2.16), th e orthogo­
nality of the orientation m atrix C g has been used, th a t is, Cg^ = C g .

D e f in e t h e E n d -e fF e c to r M o t i o n s

Once the object motion is defined, motions of the m anipulator end-effectors are com ­
pletely determ ined for the case of a rigid grasp as assumed here. .At each grasping
location, we have the following constraint equations:

r, = rg -f c, ( 2. 18)

'l.E i = C g Cg.g, (2.19)

Referring to Figure 2.2. c, is the vector connecting O g to O g, expressed in th e inertial

frame. In the above, Cg.g, represents the orientation of !Fgi relative to J-g- T h e
spatial velocity of the ith m an ip u lato r end-effector is then given by

Vg + Wg X c,
V, = ( 2.20 )
UJg
 C H A P T E R 2. D Y N A M IC S MODELING 31

Differentiating eq. (2.20) yields the desired acceleration for the zth end-effector:

Vg + Wg X Ci -f Wg X (wg X c.)
Vi = ( 2 .21 )
Wg

which will be used as the prescribed trajectory for the m an ipu lato r end-effectors when
solving th e inverse dynamics problem for each chain.

2 .3 F o r m u la tio n o f t h e S y ste m D y n a m ic s

Difficulties in formulating the dynamics of a flexible m ultibody system lie in th e fact

th a t the constituent bodies are deformable and the motions between them are cou­
pled. T h e complexity of the equations increases rapidly as the number of the bodies

increases. W hen reviewing the literature, it becomes evident that both analytical
m ethods, such as Lagrange's equations [39. 42, 43] and H am ilton’s Principle [20],
and vectorial formulations, such as the .N'ewton-Euler equations [38, 44, 21], have
been used extensively to derive the dynamics equations for flexible m ultibody sys­

tems. By assum ing th at the flexible bodies undergo small deflections (compared with

the dimensions of the link), linear theory of elasticity is usually employed. In fact.
Euler-Bernoulli beam theory is commonly adopted to represent m anipulator links.

T he dynam ics of a flexible body can be written relative to a moving frame which m ay
represent a rigid configuration of the body. .A continuous flexible body is typically dis­
cretized an d approxim ated by a finite number of degrees of freedom. .Assumed modes,
finite elem ent m ethod or lumped parameters (masses and springs) are commonly used
for this purpose.
 C H A P T E R 2. D Y N A M I C S M O D E L IN G 32

2.3.1 D y n a m ics o f A Serial F lexib le-L in k M an ip u lator

Considering the tree structure of the reduced system, dynamics equations need to be
formulated for the P seriai-chain manipulators and for th e object. In this section, we

study a single serial-chain flexible-link manipulator. T h e constraint forces between

the m anipulator end-effector and the rigid object are considered as external forces and
torques acting a t the end-effector. Following the procedures introduced by Hughes
and Si nears in [38, 40], the Newton-Euler formulation is first applied to each body
of the chain to describe the individual body dynamics. T he finite element m ethod

is used to derive the equations of motion for elastic coordinates relative to the rigid
configuration. As a result, the motion equations for an elastic body are:

+ h/In.re Çn.e ~ f nT.r T fn i.r (2.22)

^ n . r e ’^ " T ?n,e "h ^^n.ee Qn.e T Kn.ee Qn.e ~ f nT.e T f nl.e (2.23)

where M„,rrG and Mn.eeG are the mass matrices while

Dn.ee€ and Kn.eeG are the damping and stiffness matrices of Bn re­
spectively; column matrices /n/,r and /n/.e are the nonlinear inertial forces whereas

fnT,r and /nT.e represent the total forces (constraint and external) on Bn- As before,
the subscript r stands for rigid, while e stands for elastic.

By including the interbody geometric constraints and interbody force relations,

dynamics equations for all the bodies in the chain are then assembled to yield a global

formulation of the chain dynamics [40]. The resulting global equations can be w ritten
in the following form:

M rr q,. + M rf 9e = + / / . r + /ext.r + f (2.24)

q,. + Mee + Dee 9 e + K^e Çg = //,e + /exi.e + f w,e: (‘2-25)

Assuming th a t the manipulator is not kinematically redundant, the total num ber of
independent joint variables in is 6 while the total num ber of elastic coordinates
 C H A P T E R 2. D YN AM IC S MODELING 33

in is denoted by S = In eqs. (2.24) and (2.25), M^rG R^'^^

and M re€ R ^^^ are mass matrices associated with the rigid and elastic coordinates
of the chain and the coupling between them ; D ^ G R ^ ^ ^ is the d am ping m atrix and
KeeG R^'^^ is the stiffness matrix; r G R® consists of driving forces a n d /o r torques at

the joints; vectors f^xt^r fext,e represent the effect of the external forces applied
to the m anipulator not including those due to the object, whereas and are
the generalized forces generated in reaction to the grasping wrench applied to the
object. Finally, vectors f [ and f [ ^ denote the non-linear inertial forces.

Eqs. (2.24) and (2.25) differ from those given in reference [40] in th a t the general­
ized forces due to the external forces (other than the driving forces) are separated into
two parts: those due to the tip wrenches and those due to the o th er external forces
e.g., the gravity forces. This is because for a multi-arm system , we are particularly

interested in the effect of grasping wrenches. Therefore, vectors f , ^ ,. and ^ are

new terms whereas calculations of all coefficient matrices and o th er generalized ex­
ternal and inertial force terms are described in references [38, 40]. It is thus required
to derive the analytical forms of and ^ in eqs. (2.24) and (2.25).

Let w represent the grasping wrench applied to the object by the m anipulator,
expressed in the inertial frame and given by

f
w = (2.26)
n

Following the procedure used to assemble the chain (global) dynamics from the body
dynamics equations [38. 40], we first obtain the generalized force term s appearing
in the body dynamics equations of the last body in the chain. , to which the
end-effector is rigidly attached. We s ta rt by expressing the tip wrenches applied by
 C H A P T E R 2. D YN A M IC S MODELING 34

the object, —w , in the body frame as

f/V.tu
f N .w — (2.27)

and th e generalized force terms appearing in the body d yn am ics equations of Byv are
[38]:

^ N .w 1 0 ^ N .w
f N . w .t — =
^ N , E ^ N . w T H /v.iii " ^N .E 1 n /v .w

= ^ E , J V f N ,w (2.28)

/ /V .u ,.e •■= + © ^ ( r A ^ .£ :) n /v .u ,

f/V.m
(2.29)
nyv.u,

In eq. (2.28), is the m atrix form representing th e cross product of vector

r ,v £, a position vector from Ojv to O g. The m atrix ^$ ^ 6 contains the elas­
tic displacement shape functions satisfying cantilevered b o u n d a ry conditions a t the
proximal end of th e link and © € is defined by

(2.30)

Substituting from eq. (2.27) and making use of the definitions for the transform ation
matrices T and «S, eqs. (2.28) and (2.29) become:

f.W w .r = w E C E .n o = - T \ y C E .lW (2.31)

f N .w .e = - ^ I e .n C s .e C e .IW = - S e ^j v C e .IW (2.32)

Then, the generalized force terms in th e global dynamics equations due to th e grasping
wrench are calculated from those in the body dynamics equ ation s of S,v by

f W.T — f Z.W.T (2.33)

fw .e = + /s . (2.34)
C H A P T E R 2. D Y N A M I C S M O D E L IN G 35

where fv^^j,r and ^ are defined as:

0 0

:
f^ ,w ,r — f E ,u /,e (2.35)
0 0

f N ,w ,r _ f N , w ,e

Following Hughes and Sincarsin’s notation, suffix “E ” is used to symbolize assembled

q uantities of all th e bodies in the chain. Definitions of 'T ^, «Sr and are given in
A ppendix A. O th e r th an th a t, the “assembled” (chain) matrix is obtained by placing

th e corresponding body matrices along the diagonal while the assembled vector is
formed by aligning the body vectors in sequence.

Expanding the m atrix multiplications in eqs. (2.33) and (2.34) and m aking use of
eqs. (2.9)-(2.I2). we obtain:

1 T 10
0

v j
f w .r — 1
0

N f iV.iL'.r
1

f iW.w.r — f UJ

v l v l
= —'P z ^'TJ\C e .i w = - J j w (2..3G)
 C H A P T E R 2. D Y N A M I C S M O D E L IN G 36

Similarly,

0 0 ... Q 1 T fo 'J 'T

0
^ NO

cT ...
0 C>2 i 1 T N \ 0

tif.e - 0 1

cT n-T 0
•^ N ,N -l ^ N .N -X

0
f n . w .t

c T tj - T
0 • ^ 1 0 -^ N l 0

oT
0 0
‘^ 2 1 -^ N 2

+ : = : f
f n .w .t N .w .e

cT
0 0
^ N . N - 2 -^ N , N - 1

cT
f N .w .e • ^ N ,N -\ 1

o r /rT
N l 0

oT tj- T
^ 2 1 N 2 0

: '^ ^ .N ^ E .I 'W —

oT t^ -T
* ^ N . N - 2 -^ N . . N - 1 0

cT
< ^ N .N -1 1

T.W
= - { S s: ^ T \ C E ,I + S '^ j ^ ) w = -J :2.3T)

Eqs. (2.36), (2.37) and (2.26) represent closed-form relations between the generalized
forces and and the grasping wrench w . Substituting these into eqs. ( 2 .2 -1 )
and (2.25), the global form of the dynamics equations of each flexible-link m anipulator
can be written as:

M rr = "^ + f I.r + ~ ^ ( 2 -3 8 )

Rt + -f- Def Ç, - fI + f ext.e ~ (2.39)

It is not surprising to note that the .Jacobian matrices which map the rigid and
elastic rates from configuration space to the Cartesian space also relate the generalized
C H A P T E R 2. D Y N A M I C S MODELING 37

external forces in the Cartesian space to those in the configuration space.

Equations (2.38) and (2.39) represent a compact form of th e dynam ics equations
for a seriai-chain fle.xible-Iink manipulator interacting with the object through the
grasping wrench w . However, calculation of the mass m atrices, the stiffness matrices
and non-linear inertial force terms are quite complicated. T h e present work is based
on a model which includes all the nonlinear inertial term s arising from the coupling
between the rigid and elastic motions. Geometric stiffening effects are also accounted
for in calculating the stiffness matrix. The reader is referred to D am aren and Sharf

[45] for a detailed description of the model.

2.3.2 D y n a m ics o f th e O bject

In comparison to the dynamics modeling of a flexible-body system , such as a flexible-

link m anipulator, the derivation of the equations of motion for a rigid body is quite
straightforward. By applying Newton's Second Law and E uler's equation to the
object, we have

M
^ f, = m e ( v s - f g) - f„ (2.40)
1=1
.V/
^ ( n , -f c, -< f,) = IflWB -f <
jJ b X Ig w g — (n^ -r c, x f ,) (2.41 )
1=1

where all the vectors are expressed in the inertial frame. T h e scalar q u a n tity mg is

the mass of the rigid object, the matrix Ig is the inertia m atrix of the ob ject about its
mass center and g represents the gravity effect. Eqs.(2.40) an d (2.41) also quantify
the dynamic coupling between the individual manipulators since th ey contain the tip
forces f, and moments n, which represent the wrenches applied by each m anipulator.
These equations constitute the primary constraints on the force d istrib u tio n problem
as will be discussed later in the thesis.
 38

C h a p ter 3

In v erse D y n a m ics o f Serial

F lex ib le-L in k M an ip u lators

3 .1 I n tr o d u c tio n

In th e last chapter, the equations of motion for a multiple-arm system were formu­
lated. Given the external forces, solving these differential equations to o btain the
m otion of the system is known as the forward dynamics. In contrast, inverse d y n a m ­

ics entails the calculation of the driving forces a n d /o r torques needed to achieve a
desired motion. More generally, the inverse dynam ics solution can be used in force
analysis, th a t is. the study of both driving forces and constraint forces at th e joints of
a m ultibody system for a specified motion of <i reference body. For robot m anip u la­
tors, this reference body is normally the end-effector. For a multiple m a n ip u la to r and
o bject system , achieving a satisfactory motion of the object is a com m on concern.

T h e solution of the inverse dynamics has a num ber of im po rtan t applications. It

allows the determ ination of the forces to which a multibody system is sub jected in
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 39

the dynam ics and kinematics sim ulation problems. Force analysis is also a necessary

procedure in the design of a mechanical system. Probably the most prom inent appli­

cation of the inverse dynamics solution is in the design of advanced control schemes
used for controlling multibody systems. In feed-forward compensation, th e a ctu ator

torques are calculated to track a prescribed end-effector motion [46, 47]. In feedback
control, the inverse dynamics solution provides an inversion of the system dynamics
[48, 49, .50]. This gives the control specialist a powerful tool with which he or she can
design stable and robust control laws for the motion control of m ultibody systems.

A lthough solutions to the inverse dynamics problem have been well developed
for rigid m ultibody systems [51. 26], these become substantially more difficult when
stru ctu ral flexibility in the links is considered. Normally, solving the inverse dynam ics
problem requires knowledge of the motion of the system, th a t is, not only the pre­

scribed motion of the end-effector but also the corresponding desired motion in the
configuration space of the m anipulator. Then, solving the inverse dynamics problem
is equivalent to evaluating a set of linear algebraic equations where th e generalized

coordinates and their first and second derivatives are known as a function of tim e and

the a ctu ato r torques must be determ ined. The .Newton-Euler method has been used
and proven to be com putationally efficient for solving the inverse dynamics problem
in the rigid case. Nevertheless, with the same definition of the inverse dynam ics prob­
lem, obtaining a solution for the flexible multibody system is not so straightforward.

One difficulty is that the desired motion in the configuration space cannot be
d eterm ined from the prescribed motion of the end-effector because of the unknown
deflections of the flexible components. These deflections are, in turn, dependent on
the external forces applied to the system including the a ctu ato r torques, which are also
unknown. It would seem, at this point, th a t there are more unknowns th a n equations
(dynamics equations) in the inverse dynamics problem for flexible-link manipulators.
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 40

To avoid this problem, we could use nominal joint trajectories obtained from the rigid

inverse kinematics solution so th a t the desired joint variables are known when solving
the dynamics equations [29, 35, 52, 53]. A more general m ethod is to include the
acceleration kinematics equations when solving the inverse dynamics equations [37]

so th a t no assum ption is made regarding the desired m otion in the configuration space.
W ith this m ethod, the inverse kinematics and inverse dynam ics problems for flexible-
link manipulators become coupled and the solutions for both the a ctu a to r torques
and configuration space motions are obtained. In any case, the inverse dynamics

solution for flexible-link multibody system differs from th a t for the rigid case in th a t
it requires integration of the differential equations to produce the configuration space
trajectory.

.\ not her difficulty with obtaining inverse dynamics solution for flexible-body sys­

tems stems directly from the numerical integration of the derivatives of state variables.
Solutions of a set of differential equations may or m ay not be stable. It has been shown
th at the inverse dynamics solution for serial flexible-link m anipulators with zero ini­
tial conditions is unstable [21, 22. 52, 54]. In this context, instability implies th a t the

dynamics variables do not remain bounded as t — oo. Calculating th e feed-forward

torques therefore becomes meaningless. As a result, obtaining a stable solution is a
key issue in using an inverse dynamics technique to control flexible-link manipulators.
Many studies of this issue have been carried out for the past few years. Bayo and his
co-workers have developed a so called ’non-causal’ solution which has been shown to
be unique and stable. Kwon and Book reduced th e com putational burden of Bayo's

method by dividing the solution into a causal and anti-causal parts and integrating
them separately. Realizing that an unstable inverse system implies a nonminimum
phase forward system which is further caused by the non-collocation of actuation and
end-effector motion. Asada et al. used a torque transmission mechanism to design a
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 41

collocated system [22]. Wang etal. proposed a delayed ad aptive inverse m ethod which

provides a stable and close approximation to the inverse dynamics solution [5 5 ].

In this chapter, we focus our attention on the inverse dynamics solution for serial
flexible-link m anipulators. We begin this study with a single link case for which an
analytical formulation of the system dynamics is obtained. Both a state variables

description and an input-output description are established and the latter is used for
analyzing the stability of the inverse dynamics solution. Based on the analysis of the

single link arm and comparison of causal vs. non-causal solutions, we choose to use

the causal solutions for the inverse dynamics analysis of flexible m ulti-body system.

Finally, the causal inverse dynamics solutions are formulated for serial flexible-link
manipulators.

3 ,2 I n v e r se D y n a m ic s o f a S in g le F le x ib le L in k

T he study of a single flexible-link manipulator is always a good start. It allows us

to obtain an analytic formulation of the system dynamics without undue difficulty.
Besides, the model obtained is a set of linear time-invariant second-order differen­

tial equations. This in turn makes it easier to analyze the system behavior, and in
particular, the system stability.

T he stability of a system governed by a set of differential equations can be analyzed

by directly looking at the time evolution of the system sta te variables. It can also
be analyzed by studying the input-output relations th a t assigns to each input [56].
These two m ethods give different insights into how the system works and may lead
to solution of some of the problems from different angles. For an inverse flexible-link
dynam ics system , the o u tp u t driving forces are calculated in response to the input
prescribed trajectory of the end-effector. A natural question to consider is whether
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 42

or not the o u tp u t driving forces have similar characteristics to those of th e input,

say, th e boundedness. This motivates us to consider in p u t-o u tp u t descriptions to

investigate th e stability issue.

Before proceeding, we need to formulate the dynamics equations of a single flexible-

link system for which input-output relations are then established. In m any cases,

inp u t-o u tp u t descriptions can also be obtained by direct m easurem ents when deriving
the dynamics equations is too complicated. For the present study, we will s ta r t with
deriving closed-form dynamics equations since this leads to analytical in p u t-o u tp u t

relations. It also helps to understand th e procedure of formulating the dynam ics

model using the m ethod proposed by Hughes and Sincarsin.

3 .2.1 S ta te V ariable D esc rip tio n

F ig u re .3.1 shows a single flexible link driven by a torque Ti at the hub while th e oth er
end of the link is free. The link is assumed to move in a horizontal plane a nd its joint

position is specified by The mass, length, and flexural rigidity of the link are m.
/ and E l respectively. .A, uniform cross section (a x k) of the link over the length is
assumed.

Referring to th e same figure. X i — Y) and .Vi — Vj represent the axes of a fixed

and a moving (body) frame. It is assumed that the height of the beam is much
g reater than the w idth i.e., a «C h. so th a t the vibration of the beam is constrained
to the horizontal plane. Also, the shear deformation and the rotary inertia, effects are
ignored for simplicity. .An arbitrary point P on the link can be located in th e moving
frame by [Xp, ijp). For simplicity, the deflection of the flexible link is modeled by a
single planar b e am element with two nodal degrees of freedom. .A cubic polynomial
shape function is adopted to express the transverse deflection of the beam which
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 43

m . I. E l

Yi

0.0
X,

Figure 3.1: Single Flexible Link

satisfies the cantilevered boundary condition. By using the shape function m atrix
elastic displacement u of any point on the link can be calculated from the nodal
displacement by:

U= (-FI)

For the present case, the shape function m atrix is [.57|:

0 0 0
= * ( f) (3.2)

0 0 0

where ^ = x / l Ç. [Q, I] represents the location of a point on the link relative to the

joint. By defining the shape function m atrix ^ as in eq. (3.2). th e deflection of the
link takes place only in the Vj direction of the body frame and it can be calculated
as:
v( 0
(:j.3)
v' ( 0

where (v,v') are the time-dependent elastic coordinates consisting of th e transverse

displacement and the slope at the tip of the link. The corresponding cantilevered
 C H A P T E R 3. S E R IA L -C H A IN IN V E R S E D Y N A M IC S 44

elem ent stiffness m atrix is [57]

El 12 - 6/
K ,„ (3.4)
P - 6 / 4/2

Following the derivation of the dynam ics equations as described in reference [40],
we calculate the mass matrices of the flexible link. The mass m atrix associated with
the unconstrained rigid degrees of freedom is given by

ml —
h^l.rr — (3.3)
cf J,

In th e above, c f is the cross product o p erato r of Ci, the first m om ent of mass. The
second m om ent of mass is denoted by J ; It should be noticed th a t these quantities

are calculated in the body frame, th a t is. th e moving frame (Ox, X \ . Y i ) attached
to th e joint of the link. In the present case, these are:

1 0 0 0
2 Irx 0 0
Cl = m 0 0 0 _nU (3.6)
Cl = J. = 0 lyy 0
0 0 ml 0
2 0 0 Ex

and

Irx = + =')pdV [yy = J ^ { x - + :-)pdV = J ^ ( . t - + ij-)pdV (3.7)

T h e mass matrices associated with the elastic coordinates and the coupling between
the rigid and elastic coordinates are defined by

-^rn l
M (3.6)
-^ m / 105
m
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 45

/ Tldm " i/o 0 0

Ml.re = (3.9]
f r^^dm 0 0

0 0

. 20^^ ~2Ô^
where r denotes the coordinates of point P in the link frame. Since the link is only

allowed to rotate in the horizontal plane a b o ut the Z\ axis, the projection m a trix is
thus defined as:

0 0 0 0 0 I (3.10)

T he spatial velocity of the body frame is,

- -
0 0
Vi
Vi = Vl = 0 UJl = 0 (3.11
UJi
0 01

.A. vector defined in the inertial frame will be expressed in the moving frame by pre
multiplying it by
cos^i sin 0 , 0

C l/= — sin t?i cos <?i 0 (3.12)

0 0 I
and the inter-bodv transform ation m atrix is calculated bv

Cl/ - C l / rfi Cl/ 0

T i/ = = (3.13)
0 Cl/ 0 Cl/

in which r/i = 0 has been employed. The nonlinear inertial forces are given by

— ( mv 1
/i/.r — (3.14)
—(w(^ J i Wi +c(^u;j^Vi)

/i/.r = Ÿ ^(w (=V i + w ( 'w ( 'r ) d m = 0 0 (3.15)

C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 46

As a convention in th e formulation proposed by Hughes and Sincarsin, quantities

denoted with a body index, namely, 1 in the above are used to form ulate the body
dynamics equations. For a single link, there seems to be no difference between the
body dynamics and th e global dynamics except th a t quantities are expressed in dif­

ferent frames. This is not really true with Hughes' and Sincarsin’s m ethod. Indeed,
in their method, equations describing the body dynamics are w ritte n in terms of the
derivative of the spatial velocity of the body frame whereas th e global dynamics for­

mulation is expressed in term s of the generalized coordinates and th e ir derivatives in

the configuration space. .Another notable difference is th a t in th e global formulation,
constraint forces are absent while this is not the case in the body dynam ics equations.

To obtain the global dynamics formulation of the single link in the configura­

tion space, the assembled projection and inter-body transform ation matrices and the
assembled mass, stiffness and inertial terms are as follows:

'Pz^'Pl Tr = «Sv = OfixJ

= 0 T~S' = 0 = 0
(;3.I6)
M v rr = M i.rr Mv.re = M j.r. Mv = Mi.ec

K ^.ee = K i.ee /s./.r ~ /l./r f'Z.I.e ~ fl.Ie

By including eq. (3.16). th e global mass, stiffness and the inertial forces are [40]:

Mrr = V ^ ' ^ T . ' ^ M s . r r T . V z = (3.17)

Mre = "P z^^S^(Mr.re + M.Z.rr'T v«Sv ) = ' P j Mi.re (3. IS)

M r e = r ^ M r . r e + M r . r e T " - f r ^ M r . r r T ' - f M ^ . e e

= Mi.ee (3.19)

//.r = - M s .rrffsC P z O 'l + 9e) + ^ . ( 5 . ?e + ))}

= '^ r/i./.r = 0 (3.20)

//.e = y . ; . r - Mv.rr[rv(T>v<?'i + S v ç j -f T . ( S . + T^O,}]}

 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 47

+{ g j + T e(5 e + 'P s^ i)]}

— / l./.e = 0 (3.21)

I^ee — I^S.ee — Kl.ee (.3.22)

Therefore, the global description of the dynamics of the single flexible link is obtained
as

M r r Qr + M re = T (3.23)

Mre ?r T Mg, gg + Kgg g^ = 0 (3.24)

or, in a more com pact form

M g+ K g = Br (3.25)

w here

^ml 01

M = 9 = V

v'
(3.26)
0 0 0 I
K = ÿ- 0 12 —61 B = 0 r = [t \

0 - 61 \P 0

A complete description of the system dynamics should include information de­

scribing the input, the output and the state variables. Usually, actu atio n efforts
which are used to drive the system are considered as the input and motions th a t
are m easurable in response to the input are called the ou tp u t. As a dynamics prob­
lem, definitions for input and output could be different. A dynam ics simulation can
be driven' kinematically by the motion of the system while th e solution for the ac­
tu a to r torques can be considered as tlie ou tp u t of the system. This is the inverse
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 48

dynamics problem which we are interested in looking at. However, we will retain the

conventional definition for input and ou tp u t for now since the inverse system can be
investigated by simply reversing the input-output description of the forward system .
Therefore, r in eq. (3.25) is considered as the input to the system and the motion

of a point on the link as the o utp ut. T he o u tpu t trajectory of P can be defined by
th e angle Op measured from the fixed X / axis to the line connecting the joint with P
(F ig u re 3 .1). It is also the sum m ation of the rigid joint displacement and th a t due to
th e elastic displacement, the latter can be calculated by including eq. (3.3):

Op{t)^e^{t)A = g ,(() + e € [0 ,l] (3.27)

Xp iq

For reasons which will be mentioned shortly when discussing the properties of the
transfer function, we define the o u tp u t y as the angular acceleration of point P:

y = Op = C q (3.28)

C = € (3.29)

It is evident from eq. (3.27) that the inverse kinematics problem for obtaining the joint

motion 0 , corresponding to the specified motion. Op. of a point located other th a n at

th e joint cannot be solved due to the unknown deflection of the link. Therefore, the

inverse dynamics problem cannot be decoupled from the inverse kinematics solution
for even a single flexible-link arm. .As mentioned earlier, this certainly complicates
the inverse dynamics solution for flexible-link manipulators.

In the above, eqs. (3.25) to (3.28) represent a linear time-invariant system, which
is probably the simplest model one can assemble for a flexible-link m anipulator. T h e
advantage of obtaining such a model will be proven in subsequent sections where the
inp ut-o utp ut relation is established and. based on this relationship, the input-output
stabilities are investigated.
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 49

3.2.2 Transfer F u n ction R e p r e se n ta tio n

The method of Laplace transform can be used for solving linear differential equations.
It substitutes a relatively easily solved linear algebraic equations for the more difficult
differential equations. The transfer function of a linear system is defined as the ratio
of the Laplace transform of the o u tp u t variable to th e Laplace transform of the input
variable, with all initial conditions assumed to be zero. T h e zero initial conditions
imply th a t the system is assumed to be relaxed or at rest before an input is applied.

Indeed, for an input-output relation to be valid for investigating key properties of

a system, it is necessary that the system be initially relaxed and th a t the ou tp u t

be excited solely and uniquely by the input applied thereafter. Since all physically
possible signals have a Laplace transform , the existence of the Laplace transform is
also assumed for the present analysis.

The transfer function of the single flexible arm is obtained from ecjs. (3.25) and
(3.28). rewritten with zero initial conditions as follows:

M s-Q (.s) -f KQ(.s) = BT(.s) (3.30)

V (.s) = C s 'Q ( s ) (3.31 )

where Q(.s) is the Laplace transform of the generalized coordinates q while T(.s) and
V’(s) are those of the input r and the o u tp u t Op. In the present case, we have a single
input and a single output. Therefore. T{s) and V' (s) are scalar functions of .s. the

Laplace variable. Combining eq. (3.30) with eq. (3.31) and solving for V’(s) in terms
of T(.s). we obtain the transfer function //(.s) from input 7’(s) to the output } (.s) as:

K(.sj
f/(.s) = = ./^C (M s- + K ) - ‘B (3.321
T{s)

■Substituting cq. (3.26) into the above, and expanding, we derive the input-output
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 50

description as

^ (0.21^2 _ 0.3^ + 0 .1 + (88.2^2 _ 2 2 6 .8 ^ + 122.4)m E /s2 + 1 5 1 2 E /2

(-0 .3 3 m /2 + C , ) m ‘^l^.s* + (-3 9 0 .6 m /2 + I 2 2 4 / „ ) m P E / s 2 + 1 5 I 2 0 / „ E P

(3.33)
This is a rational transfer function. We now introduce several im p o rtan t definitions
th a t are relevant to transfer functions.

D e f i n i t i o n 3.1 (Proper Function)

A rational function H{s) is said to be prop er if H{oo) is a finite (zero or nonzero)

constant. H{s) is said to be stric tly proper if f/(o o ) = 0 .

D e f i n i t i o n 3 .2 (Poles)

Poles of a proper transfer function H( s ) are num bers (real or complex) which
give |/f ( s ) | = 00 .

D e f i n i t i o n 3 .3 (Zeros)

Zeros of a proper transfer function H( s) are num bers(real or complex) which

yield H( s ) = 0.

It can be shown that f l {s) defined by eq. (3.33) is proper since //(o o ) equals a
constant.

3 .2.3 S ta b ility C riteria for th e In verse D y n a m ic s S o lu tio n

There are many notions and definitions of ‘stability" which are used for defining the
properties of dynamic systems. .\ system th a t is stable according to one definition of
stability may be unstable under some o ther definitions. Depending on the application,
ensuring a ‘stab le’ performance under certain definitions is more appropriate. For
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 51

example, an asym ptotically stable system will retu rn to an equilibrium s ta te no m a tte r

how far away the system was initially from the equilibrium state. By contrast, the
concept of Lyapunov stab ility is only concerned with th e behavior of a system when
its initial state is near an equilibrium state. A B IB O (bounded input and bounded

o utp ut) stable system dem onstrates a bounded o u tp u t as long as the inp ut is bounded.
These are commonly used concepts in system analysis for control purposes.

For an inverse dynamics system, our prim ary concern is whether th e system is
BIBO stable or not. This is mainly due to the application of the inverse dynamics

solution. For example, when performing force analysis for mechanism design, we are
interested in determining the m axim um value of the forces needed to be tra n sm itte d
to the system and the m ax im u m constraint forces on each component in response to
a variety of required motions of the system. Boundedness of these forces is one of the

m ajor concerns. On the other hand, when the inverse dynamics solution is used as
part of the control scheme, boundedness of the a ctu a to r torques is essential.

Again, it is of great advantage to use a transfer function to describe the input-

o utp ut relations of a linear time-invariant system. In term s of the transfer function,

the stability criteria are set up by the following theorems [58]:

T h eorem 3.1
A relaxed single-variable system th a t is described by a proper rational function
H{s) is BIBO stable if and only if all the poles of H{s] are in the open left-half
■s plane or, equivalently, all the poles have negative real parts.

T h eo rem 3.2
If all the poles have nonpositive real parts, but one or more have a zero real

part, the system will be BIBO stable if and only if all the poles having zero
real parts are simple roots (as opposed to m ultiple roots) of the characteristic
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 52

equation.

For analyzing the inverse system, we need to switch th e input with the ou tp u t
th a t was defined in Section 3.2.2. The input to the inverse dynam ics system is now
the desired m otion of point P , 9p, while the o u tp u t is the corresponding joint actuato r
torques r. Consequently, the transfer function for the inverse dynam ics system, H{.s),
can be obtained from th a t of the forward system by:

H( s) = l / H{ s ) (3.34)

.Also, H{-s) is a proper rational function since H{oo) ^ 0. This is the reason why we
define the o u tp u t of the forward system to be the acceleration of point P on the link.
Otherwise, the order of the numerator polynomial would be higher than th a t of the
denom inator polynomial and H{s] would not be proper.

If a proper rational transfer function H{s) is irreducible, th a t is. there is no non­

trivial common factor in its numerator and denominator, the poles of the transfer
function are known to be the roots of the characteristic ecjuation of H{s) obtained
by setting the denom inator of f/(s), or equivalently, the n u m erato r of H{s). equal to
zero. Thus, in o u r case, we have:

(0.21(^ - 0.3( + 0.1 )m-.s ' + (88.2(^ - 226,8( + 122.4)m E/s^ + 1 5 l2 (P f)^ = 0 (3.35)

In the above, the scalar quantity is a known location of point P on the link relative
to the joint. It can also be interpreted as the relative location of the o u tp u t to the
input. We define

P= (3.36)

and sub stitu te it into eq. (3.35) to yield

(0.21^- - 0.3( -f- 0 . 1 ) / + ( 8 8 .2 / - 226.8( + 122.4)p -f 1512 = 0 (3.37)

C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 53

200

180

160

140

_ 120

S 100

80

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9

x/1

Figure 3.2: Value of Square Root Term

The two roots of eq. (3.37) are obtained as

-4 41 ^- + 1134( - 612 ± 3y3(T203(" - 37044(3 + 66444(2 _ 49728^ + 13312)

P i ,2 =
2 .1(2 - + 1

(3.38)

Figure (3.2) depicts the value of the square root term in ec(. (3.38) as a function of
( € [0, I]. It is shown th a t this term is always a real positive number. Hence, p from
eq. (3.38) is always a real (positive or negative) number. It can be predicted th a t the
poles of the transfer function, calculated from

•^1.2 — ± ( ---- )2 Vm (3.39)

m

•>■3.4 = ± ( — )^ \/m (3.40)

m

can either be a real num ber or a pure im aginary number depending on the value of
( (= .r//). We now consider the following Ccises:
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 54

1. f = 0

This is the case where the joint motion is specified, th a t is, in p u t a n d o u tp u t

are collocated at th e joint. The poles of the transfer function of th e inverse
system are: ± 3 4 . ± 3 . 5 3 ^ ^ ^ . According to Theorem 3.2, th e sy ste m is
BIBO stable since all the poles have zero real parts and th e y a re sim p le roots
of the characteristic equation.

2 . f= l
This is the case where the tip trajectory is specified and th e a c tu a to r is located
at the joint. This is th e most general definition of the inverse d y n a m ic s problem.
T he poles of f l {s) are: ± 9 . 9 7 ^ ^ , ± 3 8 . 9 9 ^ / ^ . According to T h e o re m s 3 . 1 and

3.2, we can conclude th a t the inverse dynamics system is u n s ta b le since there

are poles located in the right-half s-plane.

3. 0 < f < l
When the location of point P is moved from the joint towards th e tip of th e link,
values of pi o change from being both negative to both positive, which means
th a t the poles of / / ( s ) move from the imaginary axis to th e o p e n righ t-half of
the s-plane as ^ increases from 0 to 1. .A critical position where positive roots of
eq. (3.37) start to a p p e a r is found to be at = 0.529; here, th e s ta b ility s ta tu s
of the inverse dynam ics model changes from being stable to u n stab le.

From the above analysis, we conclude that stability of the inverse d y n a m ic s model
of a single flexible link described by eqs. (3.25) and (3.28) is directly affected by the
location of specified trajectory relative to the actuator. For th e p resent m odel, a
critical position exists where a change in the stability characteristics of th e sy stem is
observed. Indeed, for different approximate models, this critical position varies. W ith
a finite element discretization of the elastic link, the critical position moves towards
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 55

the a c tu a to r as the num ber of elements used to model the link is increased. Based on
this, we predict that with an exact (continuous) model of the link, a stable inverse
dynam ics system will only be obtained if the a c tu a to r and the specified trajecto ry
are collocated.

T he o th e r meaning of unstable poles of the transfer function of the inverse system

is th a t the forward system is nonm inim um phase. For a linear system, if th e transfer
function has zeros in the open right-half s-plane, the system is said to be nonm inim um
phase. In practice, the nonminimum phcise property often arises in a system where

there is a tim e delay between the input and the o u tp u t (measured by th e sensors). Ev­
idently, when applying a torque at the joint of a flexible link, the tip of the link starts
to respond with a time delay. Assuming instantaneous response of th e tip motion,
the causal inverse dynamics solution calculates th e joint actuator torques according
to the desired tip motion at the same time instant. One can predict this torque to
be of high m agnitude and frequency in order to overcome the tim e delay. Such an
input signal is likely to excite the elastic vibration and eventually cause unbounded

a ctu a to r torques. This physical explanation agrees with the conclusion given by the
above analysis th a t collocation between the input and the o utp ut guarantees a stable
behavior of the inverse dynamics system.

3 .3 I n tr o d u c in g S o m e S o lu tio n s

Several m ethods have been published in the literature to generate stable solutions to
the inverse dynamics problem for flexible-link serial manipulators. These m ethods
can be classified into two main categories: the causal solution and th e non-causal

solution. T h e first group was introduced by Asada and his research group whereas
the non-causal solution was first proposed by Bayo. We now briefly discuss both of
 C H A P T E R 3. S E R I A L - C H A [ N I N V E R S E D Y N A M I C S 56

these m ethods.

3.3.1 P ark and A sa d a ’s Torque T ran sm ission S y ste m

.A. system is said to be causal or non-anticipatory if th e o u tp u t of the system at tim e t

does not depend on input applied after time t, but depends only on the input applied
before and at tim e t. Otherwise, the system is said to be non-causal.

M otivated by the work of Spector and Flashner [54] who calculated zero locations
of th e transfer function of the forward system as a function of the sensor location
for tracking control of flexible arms. Park and A sada also investigated th e transfer
function for a single-link arm [22]. They assumed th a t actuation efforts could be

applied at an arbitrary point on the link. Therefore, they obtained the transfer
function for an input being the actu ato r torque and an o u tp u t as the tip trajectory.
.A critical position c was also found (F igure3.3) which separates stable and unstable
regions. They concluded that actuation efforts should be applied as close as possible
to the tip. They also pointed out th a t “as long as th e actuator torque is applied at
the joint and the endpoint is the control o u tp u t, the system is nonm inim um phase,
regardless of how the stiffness and mass of the arm are distributed". In consequence,
they proposed to design a torque transmission m echanism to transmit the torques
applied by the actuator located at the joint to a point within the stable region.
Figure 3.3 shows the basic idea of the mechanism design.

3 .3 .2 B a y o ’s N on -cau sal S olu tion

In the previous section, we derived the input-output representation of a single flexible-

link robot by applying Laplace transformation to the state variable equations. .Ac­
cordingly. we obtained a transfer function / /( s ) which constitutes the inpu t-o u tpu t
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 0(

Outer Hub
(connected to motor shaft)
Joint A xis
Torque Application Point P

tip mass m

Figure 3.3: Single Link .Arm with Transmission Mechanism

relation in the frequency domain. T he inverse Laplace transform of the transfer func­
tion is known as the impulse-response H{t). The tim e dom ain o u tp u t y(t) can be
expressed by using the convolution of the impulse-response with the input u(t) as:

v(t) = [ H (t — X ) u ( \ ) d \ . (3.41
Jo

In fact, application of this m ethod implicitly assumes that the system is causal. Sim ­

ilarly, the fact th a t direct integration of the dynam ics equations in the tim e dom ain
only requires the past and the present information about the system also implies th a t
the system is treated as causal.

Because of the existing tim e lag between the joint actuation and the tip motion.
Bayo suggested that the actu ato r should act before the endpoint motion and continue
after it has been completed. Considering the a ctu a to r torques as the o u tp u t of the
inverse dynam ics solution and the desired endpoint motion as the input, this means
th a t the o u tp u t is anticipatory. Therefore, the inverse dynamics system for flexible-
link m anipulators is non-causal and direct integration of motion equations in th e tim e
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 58

dom ain is not applicable.

A single flexible link is again taken as an example to o b ta in th e non-causal solution

proposed by Bayo and his co-workers. The method s tarts by form ulating the dynamics
equations for the single link arm [59]:

M v + C v -h K v = T (.3.42)

Following B ayo’s notation, in the above, M , C and K are th e mass, dam ping and

stiffness m atrices. T h e column m atrix v = [Oh, xi, X2 , , x„, y]^ is composed of the
hub rotation angle Oh, n elastic coordinates x i, , x„ for m odeling the flexible link
using the finite elem ent method, and the tip displacement y. T h e la tte r is known as
the desired trajectory. T he column m atrix T = [r, 0, , 0]^ contains the unknown
torque to be applied at the hub. Next, the Fourier transform of eq. (3.42) is obtained
for a particular frequency u-' as:

(M -f T -)v{u,') = T(ct.’) (3.43)

tu: u:^

where (•) denotes the Fourier transform of a variable. T h e above represents a linear

system of n -f 2 complex equations, the solution of which can be w ritten as:

v(uu) = (M 4- — - ^ ) - ‘t( a - ) = G(w)T(ü,-) (3.44)

tu: u,"

From the last equation in eq. (3.44) we have:

ÿ(u.') = G',,+2 .i(u,')T(w) (3.45)

where T[u:) is the Fourier transform of the actuator torque r. Since ÿ{uj) represents
th e prescribed tip trajectory, the actuator torque is calculated in th e frequency dom ain

as
f(vu') = ff(w)ÿ(ù.) (3.46)
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 59

where H{ uj) = Letting h{t) denote the inverse Fourier transform of H{ u) ,

the time dom ain solution r( t ) is then given by the convolution integral

r{t) = f h{t — X)y{X)d\ (3.47)

J — OO

For a typical tra je c to ry defined over a tim e interval [ti, f?], this reduces to:

r(t) = r hit - X)y(X)dX (.3.48)

Jti

T he difference between the above equation and eq. (3.41) is readily seen. By using

eq. (3.48), the noncausality of the solution is captured. T h e existence and uniqueness
of the non-causal solution is proved by Moulin etal. [60]. However, we point out th a t

this m ethod requires advance knowledge of the prescribed tip trajectory for the whole

task period. Also, as com m ented by Bayo and Moulin [59], their solution requires

heavy co m p utation al efforts since it involves the fast Fourier transform, solution of
linear systems of complex equations, and the inverse fast Fourier transform.

3 .4 I n v e r s e D y n a m ic s S o lu tio n for S e r ia l-C h a in

F le x ib le M a n ip u la to r

.Although we were able to formulate and obtain analytical solutions for the inverse
dynamics equations of a single flexible-link arm, it is. in general, impossible to do so
for a general multiple-link arm. It is well known that the dynamics equations for a
multiple-link m an ip u lato r are time-variant and nonlinear. Analysis of the behavior
of such a system is very complicated and the method we used for a single flexible-link
arm is no longer valid. However, preliminary conclusions can be readily drawn for the
inverse dynam ics system of a multiple flexible-link arm based on the previous analysis

of the single-link arm . These are presented below.

 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 60

N o n -ca u sa lity o f th e Inverse D yn am ics S y stem

According to the definition of the inverse dynamics problem for a multiple-
link m anipulator, joint actuator torques are to be d eterm in ed to generate a
prescribed motion of the remotely-located end-effector. T im e delays when the
end-effector starts to respond to the joint actuation efforts exist as long as at
least one of the links is considered to be flexible. It can be further predicted
that in order to achieve a desired motion of the end-effector, the jo int actuation
should s ta rt earlier than the specified end-effector trajectory and continue after
the input trajecto ry has been completed. This means th a t the inverse dynamics
solution for the joint actuation efforts depends on not only the past but also
the future information of the input and therefore implies non-causality of the

system.

C ollocation o f th e Inp ut w ith O utput Im proves S y ste m B eh a v io r

Since the non-causality of the inverse dynamics system is caused by the tim e
delay between the input actuation and the outp ut response, collocation of the
input with the o u tp u t eliminates this tim e delay and thereby makes the system
causal. It can be predicted that if the a ctuator efforts are to be applied at the

same place as the specified trajectory, solution of the inverse dynam ics problem
would have better behavior than th a t of the conventional (noncollocated) case.

From the previous analysis of a single flexible-link arm, we know th a t the inverse
dynamics solution in which we seek a joint a ctuator torque to achieve a prescribed
tip motion is generally unstable. To avoid the instability of the solution, one way
is to collocate the actuators at the tip of the manipulator and obtain a causal so­
lution. Another way is to transform the system equations into frequency domain
and solve for a non-causal solution. Both methods have been shown to be effective
with experimental implementations [22, 21, 29]. .A non-causal solution ensures s ta ­
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 61

ble solution but is computationally less efficient than a causal solution. However,

a causal solution, due to the instability, is not always applicable unless torques are
tra n sm itte d to a point close to the tip. Very interestingly, we notice th a t when m ul­
tiple manipulators are handling a common object, wrenches applied by each arm to
the object can be viewed as active forces and torques on the o th e r arms. This can
be well illustrated by a two-arm system composed of one flexible and one rigid arm .
.Assuming both arms are away from the configuration singularities, the rigid arm can
be used to manipulate both the object and the flexible arm (through the rigid grasp
between the flexible arm and the object). In this situation, a ctu a tio n wrenches are
applied at the tip of the flexible arm through the rigid object by th e rigid arm. This
scenario implies a collocation between the actuation and the end-effector motion of
the flexible arm. Motivated by this observation, we propose to im plem ent the causal
inverse dynamics solution while seeking out a way of utilizing re d u n d a n t actuation to
stabilize the solution.

.\n o th e r important observation is that the behavior of the nonlinear inverse d y ­

namics system may depend critically on the input trajectory assigned to the system,
in contrast to the single flexible-link case (linear system) where th e input-ou tp ut re­
lations can be described, for example, by the transfer function, independently of the
input. For the present study, we will use smooth acceleration profiles to describe
trajectories of the object, starting and ending from rest.

3.4.1 S olution s for th e N on -collocated Join t A c tu a tio n

.-\s mentioned earlier, the inverse dynamics and the inverse kinem atics problems are
coupled. Differentiating the velocity kinematics equation ( 2 .8 ). we obtain the accel­
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 62

eration kinematics relation:

Û= Jr (3.49)

We repeat here the dynamics equations (2.24) and (2.25) from C hapter 2 for serial
m anipulators in the absence of the external tip wrenches as:

M r r Qr + M r , = T f j ^ + (3-50)

MJe 9r + Me. Q, + Dee Qe + Kee<7. = f[.e + fext.e (3.51)

These are linear algebraic equations in terms of unknown variables q^ and r . .At
every tim e instant, the mass matrices, the stiffness m atrix and the non-linear inertial
and generalized external forces are calculated from the known states. Solving for q^.
q^. and r yields;

T = - M ^ J r ' â ) 4- M r r J r ' â - (3.52)

= J ; ' [ ( l -h (3.53)

9. = M - M - M ^ r 'â - t- f ) (3.54)

wher e
â — Û —J r ^ r — JeQe f — f/.ert.e ~ K . . Ç. ~ D .. 9 .
f / .c x « .r = f f.r A feit.r ft.ert.e= f [ , ^ A /.x t.t (3.55)
Mr. = Mr. - M rrJr'J. M .. = M .. -

In the above, we assume M . . is invertible. Therefore, eq. (3.52) gives the inverse
dynamics solution for actuator torques r . To forward the solution in time, integration

of q^. q^ and q. is required to calculate the initial states for the next tim e instant.
In fact, we can define a set of state variables to be

® = [<7r- 9 .T 9r (3-56)
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 63

T h en , the equations of motion in s tate space form can be w ritten as first-order dif­
ferential equations:

x{t) = I{x, t ) (3.57)

with known initial states as:

1(0) = ( ,.( 0 ) , <,,(0). ,,( 0 ) . i,,(0)l’ (3.58)

where q^{Q) represents the static deformation of the links at the beginning of the
task. The right-hand side of eq. (3.57) is

(3.59)

Eq. (3.57) can be integrated numerically in tim e and in this way. a time history
of both a ctu a to r torques and the state variables are obtained which constitute the
inverse dynamics solution for a serial chain flexible-body system . Figure (3.4 ) contains
the flow chart of the algorithm for calculating the inverse dynam ics solution of a single
serial flexible arm . Through the following example, we will illustrate the solution of
inverse dynamics problem by using the proposed algorithm.

E x a m p le 3.1

F ig u re 3.5 shows a 3-DOF planar serial flexible arm. It is composed of three links
with the first (counted from the base) link flexible. The lengths of the links are 0.5/??.
0.5r?? and 0.1??? respectively. The geometric dimensions of the flexible link are shown
in F ig u re 3.6. and the inertia properties of the arm are sum m arized in Table 3.1. In

the table, we o m itted columns for /j,y. 1^: and since in the present example, these
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 64

Start

luitialization
9r(0)- 9 ,(0 ), 9r(0). 9 ,(0 )

from cq.(3.51)

from cq .(3.-32) Output

I •— t - lit from c(p(3.33)

Stop

Figure 3.4: C om putational .\lgorithm for Inverse Dynamics Solution of a Serial Flex­
ible ,A.rm
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 6.5

Rigid Links

t= 0

Prescribed Tip Trajectory

Flexible Link

/7#7

Figure 3.5: .A. Planar Arm Tracing a 1/4 Circle

C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 66

a = G.3Ô

h = 38 11) n i

I = 300 n im

Figure ;i.G: Geometric Dimensions of the Flexible Link

values are zeros for all the members. VVe model the flexible link by using two planar

beam elements with a total of four degrees of freedom to allow benfling in the plane
of motion. 1 he tip of the arm is refpiired to m<jve alone a circular arc during time
/ “ ]fj. I ft. It is also required that the tip of the last link point to the center ot the
circle during th<‘ inuvr. I sing a sinusoidal acceleration profile, the anele ot rotation
around the center of the circle is defined as.

h 1 .
o = —( t ----- sin(.x,-/1 ). •d.boi

the desired tip trajectory ( 1- igure.'Lô) is then determ ined by:

Vj- = —(/l/;sin(_-/ )sino — ( ^ r ( 1 —c<jsi_'/ ) I'coso oLid I

/ f'

V,, = —(/[/jsitU^-/)coso — ( ^ r ( I —cosl..^'/ ! psitio I ;L(i'J 1

 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 67

Table 3.1: Inertia Properties of the Planar Arm

Mass First Moment o f Inertia Second M oment of Inertia

.Members m Cy c.- Cr lyy hz
(tg ) (kg — m) (kg - m) (kg - m) (kg - mr) (kg - m- ) (kg - m2)
Elastic Link 0.47.3 0.109 0 0 9.55 X 10-5 3.65 X 1 0 -- 3.64 X 10-2
Rigid Link 0.809 0.214 0 0 4.0 X IQ-4 8.36 X 10-2 8.37 X 10-2
End-effector 0.300 0.015 0 0 2.46 X 10-4 5.02 X 10-2 1.00 X 10-3
Base Rotor 11.550 0 0.084 0 5.61 X 1 0 -- 7.60 X 10-2 5.61 X 10-2
Elbow Motor 14.000 0 0 558 0 7.33 X 1 0 -- 6.05 X 10-2 7.33 X 10-2
W rist Motor 6.500 0 0.289 0 2.66 X 1 0 -- 1.04 X 10-2 2.66 X 10-2

COSO —sino 0

■I.E = sino COSO 0 (3.63)

0 0 I

where

Of = ~/4 h = 0f/tf = 2~ / 1f cl = 0.5

■As we are aware of. the actuator torques that we are going to obtain through the
causal inverse dynamics algorithm (F ig u re3.4) may be unbounded. Depending on

the properties of the input, the behavior of the inverse dynamics system may vary
significantly. For a desired tip trajectory as defined in eqs. (3.61)-(3.63). a larger
value of terminal time i f implies a slower maneuver. Letting t f = 100 sec and setting
the absolute tolerance used for numerical integration to e = 1 0 “*
’. the numerical
integration is carried out with an off-the-shelf routine LSODE. Results for the actuator
torques and the tip deformations of the flexible link are shown in F ig ure3.7. VVe
observe from Figure3.7 that the maximum value of the transverse tip deflection is
less than 0 .0 0 2 1 % of the length of the link because the maneuver is slow enough to
allow the arm to behave as 'rigid'. However, if we reduce the duration of the maneuver
to 90 sec. the actuator torques become infinite shortly after the maneuver starts. We
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 68

0.015 0.015

0.01

2 0.005, 2 0.005

too
H- 50
f(s)
-0.005

-0.015 -0.015

Base Joint

0.0025 0.00004

0002

0 0015
1.00002

:t).ooo5

100
-0 0005
-0 .0000 !

00000:
0 0015

^0025

E lb o w Joint

0 00001

too
Î0000025

-0.000005

10000075

W rist Joint

Figure 3.7: Solution for Example 3.1

C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 69

therefore conclude that the unboundedness of the dynam ic variables prohibits the
application of causal solutions to the inverse dynamics problem for serial flexible-link
manipulators.

3.4.2 S olution s for th e C ollocated T ip A c tu a tio n

As a comparison to the previous example, we now propose a hyp oth etical situation.
.Assuming a set of driving wrenches w to be applied at the tip of the a rm . we calculate
the time profiles of these wrenches necessary to achieve the sam e tip tra je cto ry used
in the previous example. Since the tip wrenches are collocated at th e tip of th e arm.
we expect the behavior of the inverse dynamics system to be improved. Combining
the dynamics equations (3.23)and (3.24), in the absence of joint torques r . together
with the acceleration kinematics of eq. (3.49), the driving tip wrenches as well as the
rigid and elastic accelerations are calculated, at every tim e instan t, according to:

W = - M^Jr'â) + MrrJr'â -
- R , M 7/ ( J 7 ‘â + J [ J r " 'f ; . „ ,. .) ] (3.64)

= J ; '[ ( i + J , M - 'M j ; j ; ') â - (3.65)

~q, = M ; ' ( - M ^ j ; ' â -i- f) f (3.66)

where
= M l -

.A similar computational algorithm can be derived by simply replacing r by w in

Figure 3.4.

E x a m p l e 3.2

Using equations (3.6I)-(3.63) to describe the tip trajectory, we consider a faster

maneuver by reducing the terminal time to 1 sec and to 0.5 sec. T he resulting solutions
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 70

for the tip driving wrenches and the deflections at the tip of th e flexible link are

shown in Figure 3.8. T h e left column in the figures corresponds to the maneuver
with t f = I sec while th e right column displays the results for t f = 0.5 sec. Higher
driving wrenches an d tip deflections are observed with the increeise of the speed of

the maneuver. C om pared with example 3.1, bounded solutions are obtained when
the arm is driven by tip wrenches for a much faster maneuver. Certainly, this means
th a t tip driven system s have much better behavior than the joint a ctu a te d systems.

3.4.3 V a lid a tio n o f th e S olu tion by E nergy C heck

One direct way of verifying the correctness of the inverse dynam ics solution is to
apply the forward dy na m ic s procedure by inputing the actu ato r torques obtained as
solutions of the inverse dynamics problem to the same dynamics equations under the
sam e initial states. If the ou tp u t of the forward dynamics simulation gives a response
of the system close to th a t of the input to the inverse dynamics procedure, the inverse

dynamics solutions can be considered to be trustable.

The other way which is also effective and probably more efficient is to keep track of
the error in the energy balance between the input and output of the inverse dynamics
procedure. W ithout using a separate procedure each time after the inverse dynamics
solution is obtained, this check is done within the inverse dynam ics calculations. By
o u tp u ttin g the profile of th e energy drift together with the solution of the a ctu ato r
torques and generalized coordinates, the inverse dynamics solutions are thus verified.
In case when the only external forces applied to the m anipulator are the a ctuator
lorc[ues, the input energy IT is obtained by integrating the power - which is given at
every time instant by

~ = Tq^, i r = f zdt (3.67)

Jo
C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 71
fy (N ) Cy(N)
Oj (N-m) (N-m)
400 1500

300
1000

200

500
100

05 0.75 0.2
•100
-500

•200

-1000
-300

-too

Tip Wrenches Tip Wrenches

£
I £
§
j^ '
05

0 5
Η
(.2 5 0.75 0.3 ' 0 .5
0,5 l(S>

•10

•15

-20

0.015 0075

001 0.05

0.025

(.25 0.5 0.75

/{S)
0 025

•0 . 0 1 0.05

■0075

Figure 3.S: Solution for Example 3.2

C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 72

Part of th e above energy is balanced by the dissipative influence such as the modal
dam ping and this can be calculated by

1 .
^ L 9 (3.68)

Assuming the system starts from rest, the error in the energy balance can be calcu­
lated from:

E( t) = {T{t) -f V{t)) - { W - W,) (3.69)

where T ( t ) and V(t) are the kinetic and potential energies of the system. T h e kinetic
energy at any time instant is defined by

T= ^q^M q (3.70)

while the potential energy, if in the micro-gravity environment, is th a t due to the

elcistic deformation of the links:

V = -q^KeNq. (3.71)

Not only can the error in the energy balance be used to verify the inverse dynam ics
calculation, it can also be used to assess the validity of the dynam ics model [1-5].
Furthermore, in the case of unbounded solution, a large error in the energy balance
must result since unbounded solution implies unstable system vibrations.

It is expected that the absolute value of the energy drift is related to the m axim um
total energy of the system during the maneuver. It is therefore natural th a t we
measure percentage error in the energy balance by comparing the error with the
m axim um energy of the system:

The root mean square value of the energy balance error is then obtained as:

eE.RMS = \/ (3.7:1)
 C H A P T E R 3. S E R I A L - C H A I N I N V E R S E D Y N A M I C S 73

0.2

0 .1 5

*c
OO 0 .0 5

25 ICO
f (S )
-0 .0 5

•O.l

-0 .1 5

- 0.2

Figure 3.9: Energy Drift for Example 3.1

where I\ is the total number of points at which the output solution is saved.

Although a sufficiently small error in the energy balance indicates a necessary

but not a sufficient condition for a correct inverse dynamics solution, it proves to be
efficient in debugging and verifying the inverse dynamics codes. Upon testing the
codes with different mechanical set-ups and various maneuvers, the calculation has
been proven to be trustable. Still, the energy drift is considered to be part of the
inverse dynamics solution and serves as a means of deciding w hether the solution is
well behaved.

F ig u re 3.9 illustrates the error in the energy balance between the input and the
o u tp u t for example 3.1. The root mean square value of the percentage energy balance
error is 0.07% while the peak value of the system energy is 0.0033 .1.
 74

C h a p ter 4

In verse D y n a m ics o f M u ltip le

C o o p era tin g M a n ip u la to rs

In the preceding chapter, we discussed the inverse dynamics solution for a single serial-
chain fle.xible-link manipulator. In particular, we focused on finding th e actu a to r
torques required to achieve a desired motion of the end-effector. It was found th a t the
a ctu ato r torques needed to produce a prescribed motion of a non-collocated reference
body may be unbounded (at this point, we assume that the driving torques are applied
at each joint of the manipulator). Along the sam e line of thought, we define the
inverse dynamics problem for multiple cooperating manipulators as: given a motion
of the object, find the set of actuato r torques for all the arms which will generate
the desired object motion. In this chapter, we will first show that this problem is
underspecified meaning that the num ber of equations necessary to solve the problem
is less than the number of unknown a ctu ato r torques. Then, we will discuss in more
detail the relationship between the actuation redundancy and the degrees of freedom
in choosing the internal forces. Finally, we propose a computational algorithm for

calculating the causal inverse dynamics solution for a system of multiple roo peratin g
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 75

m anipulators, based on which a numerical exam ple is given.

4 .1 U n d e r d e te r m in a c y o f t h e P r o b le m

Based on the discussion in chapter 3, the following work concentrates on finding a

causal inverse dynamics solution. This implies th a t direct integration of the dynamics
equations with known initial states of the system (positions and velocities) will be car­
ried out in time domain, for a predefined motion of th e object, .\lthough it was found
th a t the causal inverse dynamics solution for a c tu a to r torques may be unbounded for
a serial flexible-link m anipulator, we will investigate the possibilities of utilizing the
redundant actuation to improve the dynamics behavior of the multiple-arm system.

Repeating, from C hapter 2. the dynamics equations (2.38) and (2.39) for each of
the decoupled serial chains are:

M rrÇ r + = T -f , - J^U 7 (1.1 I

-f 4- D „ q, + K ,, ç, = , - Jjw (1.2)

la the above. / ^.nd ^ have been used to denote the sum m ation of non­
linear inertial forces and the generalized external forces (excluding the tip wrenchesi
which originally appeared in eqs. (2.38) and (2.39). If we include the acceleration
kinematics relations for each serial chain, eq. (3.49). and solve together with the above
equations for q^. q^ and r . we find:

T = (f “ ' â ) -(- M r r J r *â —f / 4- ( 4.-1 |

~q. = f} + J[u ; (4.4)

= J ; ' {( 1 4 - J;' )â - + Jfw (4.5)

 C H A P T E R 4. I N V E R S E D Y N A M I C S OF MU LT IP L E A R M S 76

where

J[ = Jf (4.6)

Com paring with the inverse dynamics formulation for the noncollocated joint actua­
tion system as described in eqs. (.3.52)-(.3.54), eqs. (4..3)-(4..5) can be w ritten cls

T = t' + JJw (4.7)

9r = q'r+Jjw (4.8)

Qe = (4.9)

where r ' , g' and g' are as defined in eqs. (3.ô2)-(3.ô4). These quantities represent

the inverse dynamics solution in the absence of tip wrenches acting between the end-
effector and the object.

In the case of a rigid grasp, a given object motion automatically defines the motion
of the m anipulator end-effectors. Therefore, t ' . g(. and g' can be determ ined, at,
every tim e instant, from the desired motion of the object regardless of the grasping

wrenches applied at the end-effectors. The last terms in eqs. (4.7)-(4.9) then represent
the effects of the tip wrenches on the solution. In the following, we use E to denote
assembled quantities for P manipulators, for example.

Tr = col{ri T 2 •■■rp} (4.10)

Jfj, = d iag {jf, J^, •• • J [p } (4.11)

It should be pointed out that although "S' was used in C hapter 2 to symbolize assem­
bled quantities over bodies in a single chain, it disappears once the chain dynamics
equations have been obtained and each manipulator is treated as a whole. When

dealing with multiple arms, E ' is used to denote ‘global* quantities for the multiple
arms. Thus, assembling eq. (4.7)-(4.9) for all manipulators, we have

Tv = r ( . -i- 14.12)
 C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S iI

<7rv (4.13)

Qez = (4 .1 4 )

T h e above relations represent the dynamics of the m anipulators constrained by each

other through the object. The grasping wrenches have to satisfy the dynamics
equations of the object which are given by eqs. (2.40) and (2.41). These are rewritten
here as:

A ; Wr = bi (4 .1 5 )

where

1 0 1 0 ... 1 0
Ai = (4.16)
cf 1 c| 1 ... c ; 1

mg(vB + g) - fe
bi = (4.17)
I g w a + w g X Ig w g - (n^ + c, x f,]

In the above, the m atrix represents the cross-product operator of the relative

contact location vector c,. In the literature, matrix A i has been called the contact
force m ap [11] or the grasp m atrix [4j. Itcan also be w ritten as

Ai = [<p( Cl ) ip( Cy) ■■■y ( cp)] G (4.18)

with (pi c,) defined by

0
(4.10)
1
VVe note that ip( c , ) is invertible and moreover, p ^( c,) = y>(—c,). The invertability
of (pi c, ) ensures th a t A i is full rank and A i A f € is always invertible.

VVe solve the inverse dynamics problem by evaluating eqs. (4.12)-(4.1.5) for r ^ .

Çrr And at every tim e instant. .Assuming is invertible, the constraint tip
wrenches W z can be further eliminated from equations (4 .12)-(4.15) by using

tiJj- = Jj-^^(rj- - r ; ) (4.20)

 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 78

and substituting it into eqs. (4.13)-(4.15). T h en , th e solution of th e inverse dynam ics

problem for the closed-loop multiple arm system can be obtained by solving th e linear
svstem:
A iJ jJ 0 0 bi + A[ Tr
1 0 = - JT,. j r J (4.21)

0 1 , - JÎE-’ r p ;
T h e right hand side of the above is dependent on the prescribed trajecto ry and the
present states of the system. The kinematic constraints at the points of contact have
been included in generating the free m otion accelerations and [II]. Hence,
eq. (4.21) accounts for all the necessary constraints imposed on the system when the
m ultiple serial chains are closed through grasping the common object. To confirm
th a t th e coefficient m atrix in eq. (4.21) is not rank deficient, we only need to m ake
sure th a t the first block entry is full rank. This is apparent since

-T \
rank( Ai J ,j^ ) = m in(rank(A i ). rank(J(^^ )) = m in( 6 . 6 P) 14.22)

VVe have introduced in C hapter 2 the symbol T.S', to denote the total nu m ber of
elastic coordinates in the assembled system. T hen, the num ber of unknown variables
in eq. (4.21) is 6 P + 6P + while the num ber of equations given by (4.21) is
6 4- 6 P 4- ES,. It becomes clear that once P > 1, i.e.. the system consists of more
than one manipulator grasping the object, the num ber of equations used to solve the
inverse dynamics problem is less than the n um b er of unknowns. Therefore, the inverse
dynam ics problem for a multiple arm system is underdetermined. The degree of this
underdeterm inacy is equal to the difference between the row and column dimensions
of the coefficient matrix in eq. (4.21). th at is. 6 ( P — 1 ). In o th e r words, a unique
solution of the inverse dynamics problem can only be obtained if additional 6 ( P — 1)
constraints are imposed on the system.
 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 79

4 .2 A c tu a tio n R e d u n d a n c y an d C o n tr o lla b le in ­

te r n a l fo r ce s

4.2.1 D eg rees o f A ctu a tio n R ed u n d an cy

Imposing the closed loop kinematics constraints results in a loss of degrees of freedom
of the multiple arms. For a rigid grasp, closing each loop imposes six holonomie
constraints on the system, thereby reducing the total degrees of freedom of the system
by six. When all P arms are grasping the common object, they form P — I loops and
therefore, the num ber of degrees of freedom of the system is

rî, = 6 P - 6 (P-I) = 6 (4.2:3)

.-Vgain. it is assumed th at the manipulators are not kinematically redundant. Since

the number of actuators driving the system is unaffected by the loop closure, the
number of redundant actuators is therefore

n,, = 6 P - = 6 ( P — I) (4.24)

This number is called the degree of actuation redundancy and it is equal to the total
number of arm configuration degrees of freedom lost when the object is grasped. We
shall show that the degree of actuation redundancy is also equal to the num ber of
controllable internal forces that exist in the system.

4.2.2 D eg rees o f Freedom in C h oosin g G rasp ing W renches

Recall that in deriving the inverse dynamics equations (4.21). we elim inated the
constraint wrenches acting between the m anipulator end-effectors and the object.

This is a logical step to take if only the driving wrenches are of interest in the inverse
C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 80

dynamics analysis. However, it is not appropriate in th e case of multiple arm systems.

T he reasons are twofold: first, if the manipulators are handling a fragile object, the
constraint wrenches applied at the contact points m ust be kept as small as possible.
Thus, the constraint wrenches should be part of the o u tp u t when analyzing the inverse

dynamics solution. Secondly, because of the actu atio n redundancy, it is natural to

expect that the constraint wrenches can be altered by applying different actuator
torques. This can be an im portant factor for stabilizing the inverse dynamics solution,
as suggested by our previous analysis of the serial fle.xible-link inverse dynamics.
Therefore, we wish to have the arm tip constraint wrenches as part of the inverse
dynamics solution.

.Another interpretation of eqs. (4.T2)-(4.14) is th a t these equations can be con­

sidered as the inverse dynamics solution for P decoupled manipulators with known
external tip wrenches. The solution is for a given set of tip wrenches. Since the ob­
ject dynamics equations (4.15) constitute the prim ary constraints on the tip wrenches,
they must be solved to yield the grasping wrenches required to produce the desired
motion of the object. It is only necessary for the m anipulators to supply forces and

moments at their end-effectors which, when pre-multi plied by the contact force map
give the right hand side of eq. (4.15). The redundancy of the grasping wrenches can
be readily observed since only six forces and m om ents are required to handle the
object, while the number of the components of the grasping wrenches is 6P.

4 .2 .3 D egrees o f Freedom o f In tern a l Force

Nakamura [8 ] proposed to decompose the contact forces and moments (the grasping
wrenches) into resultant ones and internal ones. T h e resultant forces are those which
contribute to the motion of the object while the internal forces represent th e squeezing
effect on the object. Physically, internal forces do not contribute to the work needed
C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 81

to move the object through a certain distance. We will use this fact to determine
these forces in the analysis below.

Let the manipulators apply the wrenches to, {i = 1. • • •. P ) to the object and
assume the virtual displacements at the contact points are 6 r . {i = I. ■■■. P) Ç R®.
The total virtual work done by the grasping wrenches is

SW = Y^6t Jw , (4.2.5)
1=1

From the kinematics relations, we know that is related to the set of independent
virtual displacements of the object S r ^ = [Svg where 6(f) is the orientation
virtual displacement. This is:

6r, = ip{ Ci)SrB (4.26)

Then. eq. (4.25) can be rewritten as

p p

6\-V = c , ) 6 t b ) ^ w , = ( ^ y( c U w . ' j ^ S r B = é r ^ A i W ^ ' 4.27)

1=1 1=1

For an arb itrary 6 rg. the sufficient and necessary condition for the virtual work to
vanish is:

AitOr = 0 (4.28 )

.Accordingly, the internal forces must lie in the null space of A i denoted by ,V( Ai ).
Introducing A ^ to denote the pseudoinverse of A i . the resultant wrenches are those

which lie in the orthogonal complement of iV(Ai). th a t is. the range space of A^
denoted as We have shown that Ai depends only on the relative position
of the contact points on the object and is always full rank which ecjuals the row
dimension ‘A A i . The pseudoinverse of A, can therefore be calculated by

A+ = A [ ( A , A [ ) - ‘t (4.29)
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T IP L E A R M S 82

T he decom position of the grasping wrenches satisfying eq. (4.15) can now be w rit­
ten as:

=A fb i-f A fz (4.30)

In th e above, the first term on the right hand side represents the projection in the
range space of A ^ while the columns of A'^ constitute a basis for the null space of
A ;. The vector z € R_dimX(At) jg a rb itrary and it represents the freedom in choosing

the internal forces. The dimension of .V(Ai) is determined from

d i m / ? ( A i ) = r a n k ( A i) = 6
=> dim :V (A i) = 6 ( P — I) (4.31 )
dim /?( A i ) + dim iV(Ai ) = 6P

T he dimension of the null space of A] represents the degree of redundancy in d e ter­

mining the grasping wrenches. It also represents the degrees of freedom in controlling
the internal forces.

In concluding the above, we have shown that the degree of actuation redundancy
equals to the degree of redundancy in determining the grasping wrenches which fur­
ther represents the degrees of freedom in controlling the internal forces. Hence, in
the context of m ultiple cooperating manipulator systems, resolution of the actuation
redundancy, force distribution and internal force control are commonly used te rm i­
nology. all referring to the same concept of resolving the redundancy in determining
the inverse dynam ics solution.

E x a m p l e 4.1 - Decomposition of the Grasping Wrenches

Figure 4.1 shows an example where two planar wrenches are applied to manipulate
an object in a vertical plane. planar wrench is composed of force components in the
A / and V/ directions and a moment about the Z/ axis, that is. w = [ / , fy n.]^. The

assembled colum n m atrix of the grasping wrenches is = [ /i- fi y n ;. f j r fiy ■

 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T IP L E A R M S S3

mg
K

O: X,
X
Z:

Figure 4.1: Decomposition of Grasping Wrenches

.\ssuming that the center of mass is at the mid-point of the line segment connecting
the two contact points, we have Ci = [—a 0 Oj^andco = [a 0 0]^. In this case, the
m atrix of contact force m ap (grasp matrix) is:

I 0 0 1 0 0

A i = 0 I 0 0 1 0 4.321

0 —a I 0 Ü 1

If the object is m aintained in static equilibrium, the right hand side in eq. (4.1Ô)
contains only the external force which is the weight of the object:

= 0 —{ —mg) 0 ( 1.33)
 C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 84

T h e pseudoinverse and m atrix representation of the null-space of A i are;

I 0 0 0 - 1 0

0 I -a(o : + 1 )"' - 1 0 0

0 0 (o :-F l)-' -2 a 0 - 1
Af = (4 .3 4 )
1 0 0 0 I 0

0 1 a {a^ + I) ” ‘ 1 0 0

0 0 (a f+ l)-' 0 0 I

Therefore, to keep the object static, the resultant forces are:

A ^bi — ^ 0 mg 0 0 mg 0 ( 4 .3 .3 )

which shows that only forces in the Y direction are necessary. T he internal forces
are:
A fz = '-1 -l-X
{'lazi + C3 -I -3

wliere z,, z-, and Z3 can be chosen freely. It is thus apparent th a t aside from the
resultant forces, there is an infinite number of internal forces which can be applied
to the object without affecting its motion. From eq. (4.36). the internal forces at the

two grasping points have to be equal and opposite in the X and Y directions while
the internal moments at the two points must satisfy:

Hi; — 2a fly — U2 ~ — —2 a f 2 y — n?: ( 4 .31

.A.S a result, eqs. (4.35) and (4.36) are the decomposed orthogonal com ponents of the

grasping wrenches — the resultant and the internal forces, respectively.

C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 85

4 .3 I n e q u a lity C o n str a in ts

Until now, we have only mentioned the prim ary constraints in solving the inverse
dynamics problem for m ultiple cooperating m anipulators, th a t is, th e dynamics equa­
tions of the system. Indeed, since the inverse dynamics system satisfying only the
prim ary constraints is underdetermined, it is possible and even desirable th a t some
secondary constraints be taken into consideration in finding an op tim al solution. Most
commonly considered are th e limited actuator capacities which impose inequality con­
straints on the solution for t ^. These can be compactly w ritten as:

|T s| < (4..3S)

where is a column m atrix containing the maxim um values of the torques that
each actu ator of all the arm s can generate.

Eq. (4.38) can be expanded into two linear inequalities:

(4.39)

T r > - r ' ”" (4.40)

These ecjuations can be written in terms of the contact wrenches by substituting

ec|. (4.12) into eqs. (4.39) and (4.40) to yield

(4.41)

— 4- (4.42)

The assembled inequality constraints for the multiple m anipulator system are then

A jW r > b j (4.43)
 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 86

with
0 0 0 r
J n
_rUm _
- J u 0 0 0
—r Urn
0 J 12 0 0
- r ‘^rn _
0 TT 0 0
—J l 2
A -2 = 62 = —T lim + To (4.44)
0 0 0

0 0 0
- r ' r - r'p
0 0 0 jjp
—r ,/im
+ Tp
yr -
0 0 0 —J l p

O th e r constraints, for example, the m ax im um normal at the co n tac t points re­

quired to prevent dam age to the object and the m axim um force and m o m en t capabil­
ities of the joints also impose inequality constraints on the solution. These, however,
will not be discussed in detail here.

4 .4 C o m p u ta tio n a l A lg o r ith m s

Sum m arizing the above analysis, we have shown th a t the inverse d ynam ics solution for
a m ultiple arm system is not unique due to the actuation redundancy. T he existence
of this redundancy complicates the solution procedure. One approach would be to
set the torques for the redundant actuators to zero, once they ap p ea r in the system,
and find a unique solution for the rest of the dynam ics variables. However, a better
solution might be to optim ally use this redundancy to improve the d yn am ics behavior
of the system. To accomplish this, optimization techniques a n d /o r o th e r methods can
he used to obtain the inverse dynamics solution.

.-\s alluded to in the previous section, we include explicitly the grasping wrenches
in the inverse dynamics equations from which Tv, and are obtained.
 C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 87

These equations can be written compactly as

1 0 0 -J^ -^2

0 1 0 —

(4.45)
0 0 1 —

0 0 0 Ai . .
bi

T hey are a set of linear algebraic equations in the unknown variables

w ^ . In particular, the prescribed object trajectory and thereby th a t of the m anipu­
lator end-effectors is included in the right-hand sides of these equations. Coefficient
matrices on the left-hand side of the equations are configuration dependent.

T he strategy for resolving the actuation redundancy can be applied to the set
of design variables x (here, x = [rv Wv]^) while satisfying the constraint
equations (4.45) and perhaps (4.43) if required. Selection of the design variables
among these dynamics variables is indeed arbitrary. For example, we can also define
the design variable to be x = [r^- In this case, the prim ary constraints imposed

on X are the first and the last equations in (4.45):

1 ^2 ' ^t2L (4.46)

0 A, b,

Once and have been determined, the variables q^^ and can be obtained
uniquely from the second and third equations in (4.45).

T he difference in the solution procedures resulting when we choose different sets

of design variables can be readily seen now. .Although they lead to identical solutions
if the same redundancy resolution scheme is applied, the com putational cost may be
significantly different. For example, if an optimization procedure is used to minimize
a certain cost function, the computational expense depends largely on the num ber of
design variables and the num ber of constraint equations. In fact, an even more com­

pact formulation is obtained by defining the design variables to be only the grasping
C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 88

wrenches. One reason for this choice is that the grasping wrenches are particularly
im p o rtan t in the performance of the inverse dynamics system. .A.Iso, none of the other
three variables can be determ ined without knowing the value of . And finally, any
cost function formulated in term s of the other variables, namely, r ^ . and q^^

can be-easily written in term s of because of the linear relationship between these
variables and the grasping wrenches. Therefore, the num ber of the design variables
in our formulation is 6P and the primary constraints on the redundancy resolution
are reduced to:

AiX = hi (4.47)

Once the grasping wrenches have been determined, actuator torques and rigid and
elastic accelerations can be obtained from the algebraic equations (4.r2)-(4.14 ).

In conclusion, a flowchart of the algorithm for solving the inverse dynamics prob­
lem for a multiple-arm system is proposed and shown in F ig ure4.2. It can be seen
from the flowchart that the solution to the inverse dynamics problem for multiple co­
operating flexible arm systems is composed of two parts: the redundancy resolution
and the inverse dynamics solution for serial flexible arms.

4 .5 V a lid a tio n o f t h e N u m e r ic a l C a lc u la tio n

.A.gain, the percentage error in the energy balance and the root mean square value of
the energy drift are used to validate the inverse dynamics solution. The work done
by the actuator torques of a multiple-arm system is thus given by

H ' = - dt (4.481
Jo
The dissipative work function becomes

^ (4.49)
C H A P T E R 4. I N V E R S E D Y N A M I C S OF MU LT IPL E A R M S 89

Inpiii
üfl.

Out ptit

Figure 4.2: Inverse Dynamics .\lgorithm for M ultiple-.\rm System

The kinetic energy th a t the system possesses at every tim e instant should include

that of the rigid object. It is therefore calculated by:

I
^ + Vgmgvg 4-w g l g w g ) ( 4.5(11

The potential energy of the system due to the elastic deformation of the links is:

I
(4..51 i

.Assuming the system starts from rest, the error in the energy balance at every tim e
instant is of th e sam e form as introduced in eq. (.3.69):

£ (0 = (T (0 + V ( f ) ) - ( H ' - H ; )

The percentage error and the root mean square error, respectively, are calculated
according to eqs. (3.72) and (3.73).
 I

CHAPTER 90

4 .6 E> S - t o -?

I '•la o - * which
1 i, : 1l : ' planar
simulations c
1.
and a 3D flf mg r =. oulator

system, we a. '.-I. f.h "•e used

to illustrate 9 'V ■ ‘lat the

present anah ^ cti p.! - a em s.

4 .6 .1 P I

The physical or test

bed construct S-i-' ' ictoria

a
[61]. .As show a ' r glass

top table wh' ) links

V -
with three r< 1 • other

by harmonic- ithms

for flexible st tl- ' 'thed.

e
For the prese . ve use

direct-drive i placed

at the joints. ■^■1. k r 1 - ihirfl

joim of each 1

Special de < ange-

able. Therefc 9 '- "xible

s-
and one rigid ^ : e!û iffer-

eut mechanic; « 1 and

on the difficu fit-i; .i ■up is

I
9

I
 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T IP L E A R M S 90

4 .6 E x a m p le s o f C o o p e r a tin g M a n ip u la to r s

We now describe two prototypes of flexible-link cooperating m an ip ulato rs for which

simulations of the inverse dynamics algorithm are carried out. These include a planar
and a 3D flexible two-arm and object systems. For each cooperating m anipulator
system, we also present trajectories for the specified object m aneuvers th a t are used
to illustrate the inverse dynamics algorithm. It should be noted, however, th at the

present analysis and the proposed algorithm are not limited to d u a l-a rm systems.

4.6.1 P lan ar D u al-A rm

The physical model of this prototype is the Hexible-Unk cooperating m anipulator test

bed constructed in th e Subspace Robotics Laboratory at the University of Victoria

[61]. .A.S shown in Figure 4.3, both arms are supported by air bearings on a planar glass
top table which eliminates the effect of gravity. Each arm is composed of two links
with three revolute joints. One arm is driven by direct-drive m otors while the other
by harmonic-dri ve motors. Validation of the dynamics models and control algorithms
for flexible serial and cooperating manipulators is the main purpose of this testbed.
For the present analysis, since actuator dynamics is not our m a jo r concern, we use

direct-drive motors to actuate both arms and model the motors as rigid bodies placed
at the joints. To sim ulate a general planar motion, we attach a rigid link to the third
joint of each arm to represent the end-effector.

Special design of the arms allows the first two links of each arm to be interchange­
able. Therefore, each arm can be assembled to have both links flexible, or one flexible
and one rigid link, or both rigid links. This allows us to investigate th e effect of differ­
ent mechanical compositions of the arms on the dynamics behavior of the system and
on the difficulty of controlling it. The notation used to describe a p articu lar setup is
 C l l A P T E R !. I S V E R S E D Y S A M I C S O F M C L T f P L E A R M S ‘M

[•i i ini*- l.'{; I’ l«‘x i l » i < ‘- [ . i i i k ( ' u o p i ’i v i i i t m ; M a i i i p u k t t o r [i-'i Hi-ti

! i\ I I >1111 ! I l l t i l l - l i n k ' I n n i l t I n - I n ' i u t i lie Idt a n i t . i l i n i u “ l i t i n - • li > ji ■! i . i >. i l n ' :

l i n k I I I I I n •r i i i l i i . n i l I . \ \ !■ u s i - ■ !' ’ I l ) i l i ' i n i i i ‘.1 t l i ' x i l > l r l i n k . t i n l II ■. i f .1 fiu ii| lini-.. !■

I I I 1p | i I I l\ ( ) l\ I I i l i ' l i i i i ' ' .1 ' I ' t u p w i l l : li I ' I I \ v i ) l i n k ' ■ >1 1h n !i , 1 1 1 1 1 ' . i - l l i - \ i ‘ i | . ■

I ' l i f u f i ' l . l i l l u ' i f. ' iii' > I In- ; i n l i i i i ' i - i i i n - I l f I In- p k i n a r i | n , i ! . n : ! . . , m i . >11i m i , y

I In- i i n- i I i.i p f i i p i T ' n ' <>1 r . n ' l i > u n i p n i n ii! u t ' h r a n i i l i a. ' I n ' i ' f ' I i d w i i in l . i l i l i ' ■!.

I I n ' 1 >11 |i 'I I I I I l l ' l l |i '! I 'I I i ' .1 I l u l l I a In I II I i i i i i n I >a f u i I li i l i r l n l l n ' a . i i n i p i 1 i p i 'i t n ' :

l . i ' i n i ’ li. /: 1 .11 III

\|.i>>. ni: I 1.1 l\u

. Mi i i i i i ' i i i III i n n I i a . / : I . I \ i i iir

I n i ' n i < i n ! | i i i ! a i i I f ' .111- i i ' i | n n r i l i n r n i . i i i ' i I n ' . i h i i n i n u i n i m : . r ' l . n i i ;i ' ii'n'i-; ,

inn" I l i n m u l l ' Ml" 1 K i r k w i ' i ' i n '<'r<anU. I In'ii'Ini'i'. I In' p> I ' i i ii ai >>1 i I n ' 1:i . i " > , n i .

Ill 11 n ' I ill j c r I fi 'II in I I I ' i i i i i ' l a i i i . ' i i i m n i i l a l . n n n ' l r i a i n ai p n i l i l r ■' i i ' i '> I i n | n'i n I m 1 •
 C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 92

Yi

<Pb . Xi

Arm-1 Arm-2

nW n

Figure 4.4: Planar D u a l-a rm /O b je ct System

sm ooth rotation of the object starting and ending at rest:

^•s = - ( ^ ) ’ sin(TrO 0 < / < 2 sec (4.52)

Integrating the above twice yields the prescribed angular velocity and displacement
for the object:

u-'B = ——( 1 — c o s (7 t / ) ) (4.5-Î )

T 7T 1
Ob = j - j(f - -sm(7r<)) (4.54)

T he origin O i of the inertial coordinate frame X [ — V) is located at the mass center

of the object. The corresponding desired trajectory of the m anipulator end-effectors
can be determined from the prescribed object trajectory by

viT = d (sin -I- cos Ob ) Vjr = d ( - sin Ob ^ ' b - cos Os -^'b )

Viÿ = d ( —cos Os d/'B + sin Ob Wg ) < viy = d (cos Ob d.’B —sin Ob ^ 's )
u.'i- = u/’B u,'2 ; - Jjb
(4.Ô5)
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 93

Each flexible link is modeled with two planar beam elements with two degrees
of freedom per node, representing the transverse displacement and th e slope of the
beam . Therefore, each flexible link has four elastic degrees of freedom since the links
are cantilevered at th e joints.

4 .6 .2 3D D u a l-A rm

Figure 4.-5 shows a drawing of a serial 3D flexible arm . T he forearm and upper arm
(the two shaded links in the figure) are considered to be flexible and have the following

properties:

1. Geometric Properties:
Length: 1.0 m
Cross Section: 18mm x 18mm

2. Stiffness Properties:
E l: 603.17 i\-m^
EA: 2.23 X 10' N

3. Modal Damping Ratio:

(: 0.005%

4. Inertia Properties:
Mass: 0.878 Kg
Cf : 0.439 Kg-m

<y- 0

r-: 0

7rr: 4.74 X 10“ '’ Kg-m"

lyy-. 0.293 Kg-m*
L:'. 0.293 Kg-m"
C H A P T E R 4. I N V E R S E D Y N A M I C S OF MULTIPLE A R M S 94

< 7 rl

Figure 4.5; .Architecture of the 6 -DO F .Arm

We consider another 3D arm with the same architecture except that the flexible
links are replaced by rigid ones. This rigid arm will cooperate with the flexible arm.
The object is th e same one as introduced earlier for the planar arm (F igured. 6 ). Each
flexible link is modeled using two 3D beam elements. Bending deflections in the .V —V
plane and A" — Z plane of the moving frames fixed to the links (with .V axis along
the length a nd Z axis of rotation) are referred to as the in-plane and out-of-plane
bendings respectively. Therefore, each of the flexible links has eight elastic degrees of
freedom.

The m aneu ver specified for the two arms is to carry the object with its mass center
moving along a linear path without changing its orientation in the 3D workspace. The
linear part is defined by the following vector ec[uation in the fixed frame:

I = X y 4- z 4.56)

with X. y . z being the displacements in each of the unit directions. Let x . y . z be

the m agnitudes of displacements of the object center of mass along the three axes
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T IP L E A R M S 95

^ I.

Figure 4.6: 3D Dual-.A.rm/Object System

respectively and expressed by:

X = j:( 0 ) + a 0 (F) y = y(Q) + z = :{0) + (4.5T)

where a. 3 and 7 are some constants and o ( 0 is a function of tim e with continuous
first and second derivatives. VVe define the trajectory to be composed of three seg­
ments: the accelerating part, the constant speed part and the decelerating part. The
time periods for accelerating and decelerating segments are identical and are denoted
by ta, while t j represents the terminal time. We use h to denote the maximum value
of o{t). and accordingly define the second derivative of o{t) as:

h sin p 0 < / < (,

o{t) = la < t t f ~ ta 1.58)
h sin t f — t.i < t < tf

Figure 4.7 shows 0 , 0 and d for = 2 and = 0.4.

C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 96

2.5

0.5

-1

-2

- 2,5
0.4 0.6 0.8 1.4 1.6 1.8

Figure 4.7: Function 0 {t) Used for Defining a Line in 3D Space

4 .7 E x a m p le 4.2

.As an example of calculating the inverse dynamics solution by th e proposed algorithm ,

we use the planar dual-arm (FFR-O-RRR) to dem onstrate the performance of the
numerical simulation. Figure4.4 shows the initial configuration of the system. In
configuration space, it is defined by the following joint angles:

<7^(0) = [qJi qj^] = [90° - 135° 90° 90° 45° 90°]

The system starts from rest, and no initial deformations are present, therefore.

—0 —0 —0

The dynamics equations (4.15) of the grasped object are specified by:

I 0 0 I 0 0 0

A, = 0 I 0 0 I 0 bi = 0 4.591

j sin Ob - ' cos Ob I — sin Ob j cos Ob 1 I^-B

C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U LT IP L E A R M S 97

where / denotes the length and / is the m om ent of inertia of th e a lu m inu m bar. The
design variables are defined as:

X = w ^ = [ w \ w l V = [JlT f l y n u f 2r f j y " 2=]^

In the present case, the num ber of degrees of freedom of the system is three while the
system has six actuators in total. Once the two arms are grasping the object, three of
the actuators become redundant. Equivalently, this redundancy implies arbitrariness

in determining the grasping wrenches w ^ . .According to the proposed algorithm , we

need to resolve this redundancy first and then find the corresponding unique solution
to the inverse dynamics problem for each of the two manipulators.

Let us suppose th a t we want all th e actuators of arm-1 be tu rn e d off. th a t is:

' 11 — ~ 1 2 — ~13 — 0 (4.60)

From th e above, the first three equations in eq. (4.12) then define the additional
constraints imposed on the design variables x as:

{Jul. 0 -
"1 1 - "u ~ 'll
Wi
{Jf. }2 0 — ” 12 — ~12 — ~ "l i (4.61
W 2
0 “ 13 - “ l3 - “l3

where { jfi} , represents the I'th row of m atrix of arm 1 and r ' can be calculated
from the known end-effector trajectory at every time instant. If th e coefficient m atrix
of eq. (4.61 ) is not rank deficient, this equation imposes three constraints on w ^ .

Together with the dynamics of the object characterized by (4.Ô9). these equations
completely determ ine the value of x. Once x is obtained, the solutions for
and q^^ are uniquely determined from eqs. (4.12)-(4.14).

The numerical results for this exam ple are shown in Figures 4.8-4.12. In Figure 4.S.
the left column displays the tip wrenches of the left arm (arm - 1 ) while the right column
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 98

shows those of the right arm . The force components of the wrenches are equal and

opposite since no translational movement of the object mass center is required. It

is also observed th at the right arm exerts a larger moment in ro tatin g the object
because we choose to set the actuator torques of the left arm to zero. Figures 4.9
and 4.10 illustrate the a ctu a to r torques generated by the right arm and the joint
angle profiles (rigid coordinates) for both arms. From Figures4 .II and 4.12 where
elastic coordinate profiles are shown, we observe th a t the first flexible link shows
larger deformation than the second flexible link. T he energy drift for this example
is also shown in F igu re4.13, the m aximum energy is {T + V)peak = T.46 .1 and the
root mean square value of the error is c e .r m s = 3.04 x 10~‘‘%. In this example, the
relative tolerance used for numerical integration was set to e = 10“ *^. VVe consider
that the results of numerical simulation are realistic and the inverse dynamics solution
is stable.
C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 99

40 40

5 20 20

0.5 0.5
-10 10 M S)

•20

30 -30

-<0 -*0
-50 -50

25

20

15

10 Z

0
0 5
MS) f ( S»

•10 -10

J5

20 -20

z
I

0 5
f ( S>
-10

•20

30

Figure 4.S: Grasping Wrenches

C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T I P L E A R M S 100

40

0.5
-10
r(s)

:o
30

-*o

40

05
/(S)

:o

Ad

40

MS)
:n

Figure 1.9: .Ac tu ato r Torques of the Right .Arm

C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 101

150 ISO

100 •ÿ too

OJ

r(s)
150

100

100 50

- 150

100 100

25

-50 50

-75

-too -too

Figure 4.10; Rigid Coordinates

C H A P T E R 4. I N V E R S E D Y N A M I C S OF M U L T IP L E A R M S 102

0.05 0.0005

0.04 0.0004

0.03

0.02 10.0002

0.01 50.0001

-0.01 -0.0001 r(s)

-0.02 •0 .0002 ,

•0.03

-0.04 -0.0004

■0.05 -0.0005

0.002

s. 0.001
.50.0005

0.5

-0.0005
f (S)
•0 001

•0.0015

Figure 4.11: Elastic Coordinates of the First Flexible Link

C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 103

0.0004

0.02
§0.0002
0.01
.50.0001

-0.0002

002
-0.0003

-0.03 -0.0004

0.25 0.0015

_ 0.2
0.001
S 0.15

1 0 1
:o.ooo5
I 0.05

1
u:
0 / \
-0 05
0.5 \ 1
/
/ f(S )
2 0.5 1.5
r (s)
4.1

-0.15
-0 001

-0.2

■0.25 -0.0015

Figure 4.12; Elastic Coordinates of the Second Flexible Link

C H A P T E R 4. I N V E R S E D Y N A M I C S O F M U L T I P L E A R M S 104

0.0004

05
f (S)

Figure 4.13; Error in the Energy Balance

 105

C h a p te r 5

O p tim iz a tio n A p p ro a ch

5 .1 I n tr o d u c t io n

T h e concept of optim ization has been widely used as a principle to analyze m any com­
plex decision or allocation problems. It offers a means for obtaining the ‘best' solution
to c ertain m ath em atically defined problems. Using this concept, one approaches a

complex decision m aking problem by focusing a tten tion on a single objective designed
to q uantify the outcom e or the quality of th e decision. This objective is minimized (or
m axim ized, depending on the formulation) subject to certain constraints th at may
lim it the selection of the variables involved in the problem.

Form ulation of the inverse dynamics problem for a flexible-link cooperating m anip­
u la to r system shows th a t this problem is underdeterm ined, meaning that an infinite
n u m b e r of solutions exist which generate the sam e object motion. We dem onstrated

in th e proposed algorithm (F ig u re 4.2) th a t resolving the actuation redundancy or

d e term in in g force distribution is an essential step which requires a decision making
procedure. Different sets of solutions for the actuator torques, although generating
 C H A P T E R 5. O P T I M I Z A T I O N A P P R O A C H 106

th e sam e object motion, can have drastically different effects on the system d ynam ­

ics. Investigating th e perform ance of the various inverse dynamics solutions is thus
an im p o rta n t process which leads to a “b e tte r” or even the “best” solution among all
th e possible ones. O ptim ization techniques provide a powerful m ath em atical tool for
achieving this goal. This ch ap ter is devoted to the resolution of actuation redundancy
via an optimization approach.

As a designer of the optim ization problem, our goal is to formulate a simple

b u t physically meaningful and reasonably accurate m athem atical model. This model
m ust include most of the significant factors affecting the complex decision on the force
d istribu tio n within the system . T h e design procedure includes the following steps:

1. Choose the set of variables which constitute a design space. These variables are
term ed the design variables:

2. Determine all the prim ary a n d /o r secondary constraints which restrict the de­

sign variables;

3. Formulate a m a th em atical statem ent of the problem in terms of the design

variables. This sta te m e n t must include the objective necessary for evaluating

the quality of the decision:

4. Solve the optim ization problem with the available resources to obtain a satis­
factory solution.

It should be noted th a t these steps should not be treated independently. For example,
when choosing the design variables, one should take into account the complexity of
th e constraints and the formulation of objectives. .Most importantly, one needs to
ascertain th a t it is possible to obtain a solution for a particular problem formulation
as well as predict how fast it can be calculated. Good decisions at the design stage
 C H A P T E R 5. OPTIMIZATION APPROACH 107

rely on b o th the knowledge and the experience of the designer. In m any instances, the

solution of a complex problem cannot be directly treated in its entirety, b u t instead

m ust be decomposed into subproblems — each subproblem being represented by a

particu lar objective and having constraints that are imposed to restrict its scope.

5 .2 F o r m u la tio n o f th e P r o b le m

Beginning with the first step of the design process, we propose to choose th e set

of grasping wrenches w-r as the design variables. This is a natural choice since
m any of th e quantities we want to minimize can be written as a function of grasp­
ing wrenches. As well, these wrenches are primarily constrained by linear equality
constraints which we expect to be solved more easily. Considering also th e possible
secondary constraints, for instance, the limited capacities of the actu a to rs as given
in eq s.(4 .4 I) and (4.42). it is not difficult to see th at the optim ization problem we

are facing can be described as a linearly constrained optimization problem , th a t is.

the constraint equations are composed of a set of linear equality a n d /o r inequality
equations. In particular, the problem can be mathem atically stated as
ndn /(x ) (ô.lû)

Subject to AiX = bi (5.16)

and AiX > bo (5.1c)

where / ( x ) is the objective function: vector x contains the design variables: (5.16)
represents the equality constraints and (5.1c) the inequality constraints. Form ula­
tion of a practical optim ization problem always involves a tradeoff betw een building
a m a th e m atica l model sufficiently complex to capture the im portant features of the
physical system and building a model that is tractable. We can identify a model,
w hether tractab le or not. by the following characteristics:
C H A P T E R 5. OPTIMIZATION APPROACH 108

T y p e o f P r o b lem
In the most general Ccise, both objective function an d the constraint equations
m ay be nonlinear functions of x. Linear functions, by virtue of their simplic­
ity, are often selected as an easy way to start. Indeed, linearity of the con­

s traint equations, as defined for the present study, is most appealing. Linearly
constrained problems can be handled through a com bination of an elimination
m eth od and an active set method [62]. The simplest cases of linearly constrained
problems are when the objective function is either linear or quadratic. These

are referred to, respectively, as linear program m ing or quadratic program m ing

problems. M ethods to handle these simple cases are well-developed and will
be m entioned in more detail in Section 5.3. From this standpoint, the reason
for excluding the state variables from the design variables is obvious in our
case since they are subject to the constraints described by nonlinear differential
equations.

P ro b lem S ize
One obvious measure of the complexity of an optim ization problem is its size,

m easured in term s of the number of unknown variables and the num ber of con­
straints. From this viewpoint, the merits of choosing the grasping wrenches
as the design variables for the present study can be readily seen. In particular,
the nu m b er of unknown variables is 6P (P is the n um b er of m anipulators) while

the num ber of equality constraints is 6 . If. say. the elastic coordinates are
included in the design variables because reducing the structural vibration is one
of our m ajor interests, both the number of unknown variables and the num ber
of constraint equations will be increased by (i = I ...... P). Besides, these
variables are subject to nonlinear differential equations. To avoid these compli­
cations. we retain tOy. as the design variables but a tte m p t to reduce the system
 C H A P T E R 5. OPTIM IZATION AP P R O A C H 109

vibration by actively choosing the value of grasping wrenches.

C onvergence P ro p e rtie s

Most of th e algorithms designed to solve large optim ization problems are iter­
ative in nature. Typically, when seeking the solution to an the optimization
problem, an initial vector Xq is selected a n d the algorithm generates a sequence
of ever-improving points in th e design space, Xi - - - , x t ••• that approaches a
solution point x". As in any interactive scheme, the convergence properties are
of p rim ary concern. In particular, one needs to consider the property of global
convergence and th e rate of local convergence. .A globally convergent algorithm

will generate a sequence th a t converges to a solution point even when initi­

ated far from the solution point. W ith o u t an acceptable rate of convergence,
however, the algorithm m ay require an excessive am ount of time to reduce th e
error to a designated tolerance. These seemingly algorithm-related features can
som etim es be affected by the inherent stru c tu re of the model. For example, if
the objective function possesses certain properties, such as strict convexity, any
algorithm which is locally convergent can be used to find the global solution

since a strict convex objective function guarantees a unique optimal solution.

In addition, for a given m ethod, the sequence converges to the solution more
rapidly for a quadratic than for a linear objective function.

For the present analysis, it is particularly im p o rtan t th a t the model we build be

simple and not require expensive com putation. T he reasons are two-fold; first, redun­
dancy resolution, for which an optim ization problem is to be formulated, constitutes
only one step in the inverse dynamics algorithm . For a multiple flexible-link m anip­
ulator system , this algorithm is fairly complex already due to the required numerical
integration of the s ta te variables. Further complication of the solution procedure
should be avoided. Secondly, if the solution is to be used for control purposes and
 CHAPTERS. OPTIMIZATION A P P R O A C H 110

control efforts are intended to suppress the system vibration, then a higher bandw idth

of the controller will be required, thus necessitating a small step size for calculating
the torques. Before proceeding to complete th e formulation of the optimization prob­
lem, we would like to state some fundam ental theorems used to solve optim ization
problems since they influence our decision on how to formulate the optim ization p ro b­
lem.

5 .3 L in ea r a n d Q u a d r a tic P r o g r a m m in g

Fundam ental definitions and theorems relevant to the formulation and solution of the
optim ization problem are given in Appendix B. In this section, we focus on introducing
the linear a nd quadratic programming techniques.

5.3.1 L inear P rogram m ing

The simplest type of constrained optimization problem is obtained when the objective
function / ( x ) and the constraint equations are linear functions of x. The resulting
problem is known as a linear program m ing (LP) problem. T h e term ‘program m ing’
here is synonymous with ‘optim ization’. .A linear programming problem can be ex­
pressed in the standard form as;

nun /( x ) = c^x

subject to AiX = bi (5--)

and X > 0

Normally, we consider the case where the m atrix A i € is not rank deficient and
m < n. Then, the system A , x = bi is underdeterm ined and n — rn variables in x
remain free to be determined. Verv naturallv. we can trv to eliminate m variables
 CHAPTERS. OPTIMIZATION APPROACH 111

by using the equality constraints and determ ine the remaining n — m variables which

give a m inim um value of / ( x ) . Since the objective function is linear, it does not have
a curvature which can give rise to a minimizing point. It is thus im p o rtan t to realize
th a t th e inequality constraints x > 0 are accessary in linear program m ing in order
to define boundaries of the feasible region.

A solution of the stan dard LP problem always exists at a particular extrem e point
or vertex of the feasible region, with at least n — m variables having zero value, and
th e remaining variables uniquely determ ined by AiX = b i and taking non-negative
values. The main difficulty in linear programming is then to find which of the n —m
variables should be set to zero at the solution and this is where the various methods
for solving the LP problem differ from each other.

Because of the semi-positive definiteness of the Hessian m atrix (.A.ppendix B),

linear program m ing problems have non-unique solutions. It has also been shown that
for continuous time-varying constraints AiX = b ,, linear program m ing m ay generate
discontinuous solutions [63]. This is particularly relevant to the present study since
our equality constraints are dynamics equations of the object with A[ and b , changing
continuously with its desired trajectory.

5 .3 .2 Q uadratic P rogram m in g

.A.not her im portant type of objective function is th a t of a quadratic form. It can be

w ritten as:
/(x ) = c ^ x -f ^ x ^ W x (5.3)

where W can always be defined as a symmetric matrix. Minimizing such an objective

function subject to linear constraints is referred to as quadratic program m ing (QP).

Q u ad ratic programming differs from linear programming in th a t it is possible

 CHAPTERS. OPTIMIZATION APPROACH 112

to formulate meaningful problems w ithout the inequality constraints. As shown in

the subsequent sections, this will be th e basic s tru c tu re we adopt to solve the force
optim ization problem. If the problem contains only equality constraints A iX = bi
and A i is full rank, the solution can be obtained by directly using the Lagrange
first-order conditions (Appendix B) to yield:

W x -l-A fA -f-c = 0 (5.4)

A i x — bi = 0 (5.5)

If in the above, th e m atrix W is positive definite, it can be proven th a t the matrix

W A[
(5.6)
Ai 0

is nonsingular [62] and therefore, a unique solution for x" and A ' exists which satisfies
the Lagrange first-order necessary conditions (.Appendix B). Indeed, since W is the
Hessian of the objective function, a positive definite W implies a strict convex objec­

tive function (see .Appendix B) which ensures th a t this unique solution is exactly the
unique global m inim um solution to the problem.

Proceeding to solve the Lagrange conditions, from eq. (5.4) we have

x = - W ~ ‘A [ A - W - ‘c (5.7)

Substituting for x in eq. (5.7) into eq. (5.5) and solving for A, we obtain

A" = - ( A i W - ' A [ ) - ‘( A i W - ‘c + b ,) (5.S)

and therefore,

X- = W - ' A [ ( A , W - ' A [ ) - ' ( A , W - ' c - f - b , ) - W - ‘c (5.9)

The above representation is a useful analytical form and will be used inthe develop­
ments inthe next chapter. However, from the numerical efficiency point of view, the
solution may be b e tte r calculated by some other method.
 C H A P T E R 5. OPTIM IZATION APPROACH 113

G eneral quadratic program m ing with inequality constraints is almost always solved

with a n active set m ethod [62]. Positive definiteness of m atrix W again simplifies
the solution procedure. It is worth mentioning th a t both linear and q u a d ra tic pro­
g ra m m in g can be solved in a finite num ber of steps which dem onstrates well-behaved
convergence of these problems.

5 .4 O b je c tiv e F u n c tio n s

Upon com paring qu ad ratic with linear programming formulations, we chose to adopt
the form er for solving the force distribution problem. We also chose to consider only
the p rim a ry constraints, th a t is. the linear equality constraints in order to capture
and highlight the most im p o rtan t features of the force distribution for fle.xible-link
cooperating m anipulators. Hence, the resulting optim ization problem takes the fol­
lowing form:
nun c ^ x H- y X ^ W x (5 . 1 0 a)

Subject to AiX = bi (3.106)

where (5.10 6 ) comprises the dynamics equations of the object. As was shown in chap­
ter 4. th e m atrix A iG is always full rank which means th a t the constraints are
linearly independent. T he last but ver\" im portant task which remains is to define
suitable quadratic objective functions which will represent criteria for a b e tte r force
distribu tio n. We will first consider some of the criteria proposed for rigid-link systems.
e.g.. th e m inim um internal forces. However, emphasis is placed on im plem enting new

criteria which are particularly relevant to a system consisting of fle.xible-link m anip ­

ulators. Here, the u ltim ate goal is to improve the dynamics behavior and reduce
the sy stem vibration. Finally, objective functions which combine m any different sub­

objectives are proposed and implemented.

 C H A P T E R 5. OPTIMIZATION APPROACH 114

5 .4 .1 M in im izin g th e Internal Forces

T h e vector of design variables, x, in eq. (5.10) only includes the wrenches applied to
th e grasped object. As a result, the force distribution determined by the solution to
the optim ization problem eq. (5.10) may or may not take into account the dynamics

of th e m anipulators, depending on the objective function. In a situation where m a­

nipulators operate at slow speeds a n d /o r carry out delicate tasks, optim ality criteria
can be formulated which describe how well the object is handled. This is particularly
relevant for a multi-fingered mechanical hand grasping an object. In this case, the

perform ance criteria may include any one of the following; (i) the contact condition
between the fingers and the object to ensure a positive normal contact force [64]; (ii)
th e o bject sta b ility which ensures that the object remain in a static equilibrium when
its position is perturbed [65]: (iii) the contact sta b ility which guarantees th a t the
o b ject remains grasped when disturbed by external forces [8 ] and (iv) the m in im u m

internal forces criterion which reduces the potential damage to the object [15]. Inter­
nal forces play an im portant role in achieving all these objectives. For cooperating

m an ip u lato rs with a rigid grasp of the object, the aforementioned contact condition
is not needed because the manipulators can apply arbitrary wrenches. .As well, the
o b je ct and contact stability are ensured by the rigid grasp. However, it is desirable
to minim ize the internal forces because excessive internal forces may damage a fragile
o b ject and impose high loads on the system.

.As discussed in chapter 4. internal forces are the components of the grasping
wrenches in the null space of A ,. These forces do not contribute to the motion of the
o b ject but produce a squeezing or tearing effect. Decomposing the grasping wrenches
applied to the object into resultant and internal components, denoted by x"*" and x "
respectively, we have
X = x"*" + .x~ = A j ' b i - I - A f z (5.11 )
 C H A P T E R . 5. O P T I M I Z A T I O N A P P R O A C H 115

where th e first term x+ (= Aj^’b i) does not vary in the design space. T h e vector of
internal forces x~ must satisfy:

A ix " = 0 (5.12)

Therefore, minimizing the am ount of internal forces can be described by minimizing

the following quadratic objective function:

//(x -) = ^ x - V (.5.13)

Expanding the above by using eq. (5.11), we obtain:

A (x") = ^ ( x - x + )^ (x - x+)

= ^(x ^x - 2 x^x'^ + x ’^'^x'*') (5.14)

We now recall that the pseudoinverse of a full rank m atrix A ; maybe calculated by
A ^ = A f ( A i A f ) ~ ‘. We can therefore rewrite the second term in eq. (5.14) as:

x^x+ = x ' ^ A [ ( A , A [ ) - ^ b i = b [ ( A i A [ ) - ^ b , (5.15)

which is independent on the design variable x. Hence, the only term in eq. (5.14)
which is a function of x is the first te rm ^ x ^ x . Similarly, the constraint equations
(5.12) are rewritten as follows:

A iX " = A i ( x — x"*’) = 0

=> A i x = A ;X ^ = A i A f ( A i A f )“ ‘bi = bi (5.16)

which leads to the equations of tiie object dynamics. Therefore, the problem of
minimizing the internal forces for a m ultiple cooperating manipulator system can be
stated as:
I T
min X
^ (Ô.1T)
subject to A iX = bi
 CHAPTERS. O P T I M I Z A T I O N .A P P R O A C H 116

Similar formulations to the above were derived by N akam ura [8 ] to determ ine a
m inim um internal force while satisfying the static frictional inequality constraints or
the contact stability condition. He also proposed a more physically accurate criterion

[32] which minimizes the strain energy stored in the object with the goal of minimizing
th e possible dam age to the object. This is performed by first modeling the object as
a rigid body covered with a massless elastic layer. Strain energy is then formulated
as a quadratic function of contact wrenches. N akam ura showed th a t the minim um

strain energy criterion is equivalent to the m inim um internal force criterion when the
stiffness of the elastic layer is the same in all directions a t all contact points.

To illustrate the performance of th e minimal internal force scheme (5.17), we

im plem ent the inverse dynamics algorithm (F ig u re d .2) com bined with a strategy for
minimizing the internal forces. Since this scheme does not take m anipulator dynamics
into consideration, solution of the serial flexible-link inverse dynamics will be unstable.
Therefore, the procedure for minimizing the internal forces will be implemented only

for a rigid multiple-arm system. However, as will be shown later, this scheme can
be combined with other criteria to produce a useful solution for flexible m anipulator
systems.

E x a m p l e 5.1 — Minimum Internal Forces

T he sam e configuration of dual arms and object as shown in Figured.! and the
desired object trajectory as defined in eqs. (4.52)-(4.54) are used. The numerical

solution is obtained by implementing the m inim um internal force scheme. In this

exam ple, the two identical arms are considered to be rigid.

C om paring eqs. (5.17) and (5.10). we observe th a t minimizing internal forces is

equivalent to setting W = 1 and c = 0 in eq. (5.10). Obviously, the Hessian m atrix
W is positive definite and therefore a unique solution exists to th e force optim ization
 CHAPTERS. OPTIMIZATION AP PROAC H 117

problem . T h e results for the left arm are displayed in F ig u re 5 . 1 . It shows th e profile of

the grasping wrenches compared with those obtained by the m ethod used in exam ple
4.2 where th e base motor torque and th e elbow motor torque of the left arm were
set to zero. It can be seen from the figure that grasping wrenches which give the
m in im u m in tern al forces are smaller in magnitude than those obtained by th e o th er
scheme. T h e sam e trend can be observed in the solution of but not a n d T2 since
in the n o n -m in im u m internal force scheme, the torques at the base and elbow joints
were forced to zero.

5 .4 .2 O p tim a l Load Sharing

T ask S p ace S h a rin g

O ne of th e m erits of a multiple-arm system is that the loads can be shared am ong the
m anip ulato rs. Considering each robot arm as a unit which contributes to carrying

the o b ject, it is n a tu ra l to expect that arm s with high load carrying capacities should
c o n trib u te m ore th a n those with low capacities. .-\n optim al load distribution within
the arm s can be achieved by assigning different weights to the diagonal block of the

m a trix W . In particular, numbers in the ith block are chosen in inverse proportion to
the load c arry ing capacity of the ith arm, which in turn is quantified by the m a x im u m
allowable load it can carry. Each block of W thus represents a relative 'w eight' of
wrenches applied by one arm relative to those of the other arms and accordingly,

m a trix W is usually called a weighting m atrix. When all manipulators are identical,
the weighting m a trix W is set equal to an identity matrix.

In ad d itio n , elem ents in each 6 x 6 diagonal blocks of W represent the relative

weights of th e components constituting each wrench. To effectively utilize the m e ­
chanical a d v an ta g e provided by multiple arm control, it has been suggested [ 1 0 ] th a t
C H A P T E R 5. OPTIMIZATION APPROACH 118

. m in . in L f o r c e s . n o n - m i n . i n t. f o r c e s . m i n . in L f o r c e s . n o n - m in . inL f o r c e s

60

40

Ij
r(s)
•20
-10

-15

20

-25

T,| ( N - m )

. m i n . i n t. f o r c e s . n o n - m in . in t. f o r c e s . m i n . InL f o r c e s . n o n - m in . in L f o r c e s

•10

f„(N ) t ,2 (N-m )

. m in . in t. f o r c e s . n o n - m in . in t. fo rc e s _ m in . in t. f o r c e s . n o n - m in . in t. fo rc e s

75

0.5 IJ

-to
-7.5

-10 15

n „ (N-m ) T,, ( N -m )

Figure 5.1: Minimal Internal Forces

 C H A P T E R 5. OPTIMIZATION A P PR O A C H 119

mom ents required to handle the object should be attained prim arily through th e cross

product terms c, x f, in the grasp m atrix A i of eq. (2.41) ra th e r than the directly
applied moments n, . To enforce a large penalty on the use of n, in achieving a desired
net moment c, x f,- + n,-, we set

W = j_
w. (5.18)

where the scalar w, is the maximum load the ith arm can carry and A:, > 1. The
selection of A; m ust also take into consideration th e location of th e it h contact point on
the object. In other words, those end-effectors grasping the object closer to its mass

center should be required to supply less m om ent n,. A similar a rgu m en t applies to the
components of the grasping forces. .At a certain posture (m a n ip u la to r configuration),
a manipulator is more effective in applying, say, a force in the .V-direction th a n in
other directions. Thus, in general, the weighting m atrix W can be chosen as diagonal
with different weights for each nonzero entry.

C o n fig u ra tio n S p ace S h arin g

.As can be expected, the above load sharing scheme does not ensure optimal use of
the actuators. It is desirable that loads on the actuators be d istribu ted such th a t
motors with a higher capacity support more load than those with smaller capacities.
Taking a quadratic function

I
/r(r) = - t ^ W tTs (5.19)

as the objective function, an optimal load distribution in the configuration space

can be achieved by placing different weights on the diagonal com ponents of W r .
 C H A P T E R 5. OPTIMIZATION APPROACH 120

Analogously to the task space sharing scheme, these weights are selected to be the re­

ciprocals of the m ax im u m torques that the corresponding actu ators can generate. For
flexible-link m anipulators, minimizing and actively distributing th e a c tu a to r torques
is particularly im p ortan t. T h e analysis of the inverse dynamics of serial flexible-link
m anipulators has shown th a t collocation of the actuation with the end-effector ensures
a stable inverse dynamics solution. It has also been shown th a t in an ap proxim ate
situation when the elastic links are modeled with finite num bers of elastic modes,
a critical point on the arm exists which partitions the stable and unstable regions

[22, 6 6 ]. A stable region lies between the arm tip and the critical point. A useful
inference is th a t the a ctu a to r efforts should be shifted as much as possible towards the
end-effectors in order to approxim ate a collocated case which promises b e tte r inverse
dynamics behavior. To implement this idea, we suggest th a t minimizing th e norm of
the torques required at the joints located prior to the proximal end of th e most distal
flexible link of a serial arm will shift the actuation efforts toward the end-effectors.
.A.ri improved dynamics behavior of the inverse system would then be expected.

To incorporate objective (Ô.19) into the optimization problem (5.10), we need to

include the following relations:

^r — -f- J I r tUV*

Note th a t is known from the m anipulator states. Substituting the above, eq. (5.19)
can be w ritten as:

/r(x ) = -h (5.20)

T he first term is independent of the design variables, and therefore does not affect
the solution of the optim ization problem. Eq. (5.19) can therefore be reduced to:

/ r ( x ) = c ^ x -f ;^x’^ W r x (5.21)
 CHAPTERS. O PTIM IZATIO N APPROACH 121

where

ct = J ij- W r r 'j, W r = J i^ W rJ^ (5.22)

It remains to be verified w hether W r , the Hessian m atrix of the objective function

in eq. (5.21) is positive-definite. Referring to eq. (5.22) and recalling th a t W r is chosen
to be positive-definite, th e sam e property will be retained for W r as long as J j s is
nonsingular [67]. Since J i v is a block diagonal m atrix of J ^ ( i = 1, . . . . P) , is
nonsingular if and only if all Jj- have full rank. For a serial m an ip u lato r with its
end-effector in contact w ith the environment or a moving object, singularity of
implies th a t the m a n ip u la to r cannot generate an arbitrary tip wrench. For a rigid
m anipulator, this hap p en s w hen the manipulator is a t a kinematic singularity.

E x a m p l e 5.2 — M in im u m N orm of Torques

T h e same rigid d u a l-a rm and object system as used in the previous example is
employed to show th e num erical performance of the optim al load sharing scheme.
First, we minimize th e Euclidian norm of actu ato r torques by setting.

/ r ( r ) = ^ W r = 1

T h e actu a to r torques ob tain ed with the present scheme are com pared with those
o btained by the m in im u m internal forces scheme in example 5.1. Results for the

two arm s are similar a nd those for the left arm are shown in F ig u re 5.2. It is seen
from the figure th a t th e schem e of minimum norm of torques generates more evenly
d istrib uted loads on th e a c tu a to rs while the minim um internal force schem e generates
more evenly distrib uted tip forces and torques.

Next, considering the a c tu a to r capacities given in Table 5.1. the weighting matrix
in eq. (5.19) is chosen to be

W r = d ia g { ^ , - i. jL ) ,,5,23)
 C H A P T E R 5. OPTIM IZATION APPROACH 122

m in . n o r m o f t o r a u e s ___ . m in . in L f o r c e s m in n o r m o f t o r a t iM m tn tnr

60 25

/ \ 20
40
/ \ 15
/ \
10
20
/ \ 5

0 / \ 0
\ oV 1 6j : I / [-5 2
-5 • / ns)
-20 ^ ^
•10

•15
AO
■20

60 -25

t*u(N) T ,i (N-m)

m m . n o r m o f to r q u e s ______ m in . in L f o r c e s m in . n o r m o f rom iif*« m tn mr

10 5

75 / \
/ \ 3
5
/ \

2.5
I

0 0
*‘ "T T 5......... I \ / 15 2 V /y .5 \ / \ \ 1 -5 / h
-I
-2 J V /

-2 \ / /
5
•3
• \ /
•7.5
-4

-10 5

(N) t ,2 (N -m )

. m in . n o rm o f to rq u e s .............. m in . im . f o r c e s . m in . n o rm o f to rq u e s m in . m t. fo rc e s

6 75

2S

O
0.5

•2.5

n,, (N -m ) (N -m )

Figure -5.2: Comparison of .Minimum Norm of Torques and M inim um Internal Forces
 C H A P T E R 5. OPTIMIZATION APPROACH 123

Table 5.1: Maximum Torque of the Motors

Motors Bcise Elbow Wrist

(N-m) 150 40 9.8

Table 5.2: Maximum Value of Torques in Figure 5.4

Motors Base Elbow Wrist

(|T,|)max (N-m) 13.5 3.14 1.37
9.0 7.9 13.9

C om paring the torques obtained by using this minimum weighted norm scheme with
those of th e Euclidian norm scheme implemented above, the results in Figure 5.3 show
the loads shifting from motors having lower capacities to those with higher capacities.
T h e torque required at the base and elbow motors increased while the torque at the
wrist m oto r decreased when minimizing the weighted norm of the actuator torques.

Also, the m axim um values of the torques at each joint (Table 5.2) are all close to
10% of the respective maximum torque values that the actuators can generate. This
d em o nstrates th a t the actuators contribute according to their abilities.

E x a m p l e 5 .3 — Shifting Torques Towards the End-effectors

Special care needs to be taken when manipulators are composed of flexible links.

N umerical experim ents conducted with the above weighted-norm or Euclidian norm
of torques scheme produce unbounded solutions for joint torques during the maneuver
of rotating a rigid object for 90° in 2 seconds. Since a flexible serial arm driven by tip
wrenches has a b e tte r dynamics behavior, we a ttem p t to shift the actuator loads from
those before the proximal end of the last flexible link to those beyond its distal end. In
o th er words, we propose to minimize the actuator torques at the joints located before
and including the last flexible link of the arms. In this example, we consider the first
C H A P T E R 5. OPTIM IZATION APPROACH 124

. Weighted Norm . Unweighted Norm -W eighted Norm . Unweighted Norm

60

40

20

05.

-20

-*0 -to

n. (N) ti, (N-m)

. Weighted Norm . Unweighted Norm . Weighted Norm . Unweighted Norm

7-5 3

2.5

o
0.5

-10 ■4

r,y(N) Ti: ( N - m )

. Weighted Norm Unweighted Norm . Weighted Norm Unweighted Norm

7.5

0.5 1-5
1(H)
•2-5

n,, (N-m) Tn (N-m)

Figure 5.3: Comparison of .Minimum Weighted and Unweighted Norm of Torques

CHAPTERS. O PTIM IZATION APPROACH 125

a n d second links of th e left arm to be flexible. Since the system has three redundant

a ctu a to rs, in add itio n to minimizing the base and elbow a c tu a to r torques, we also
choose to minimize the wrist torques in order to better approxim ate the situation
w here the flexible a rm is driven by the tip wrench.

t 2 j (N-m)
0.75 40

OJ

0.15

05 n_5
J(S) -10
f(S)

-0.25

•20

-0.75 -JO

Right Arm Torques

(a) Solution for Torques

0 4

0 2
o .i

005

0.5 0.5

-O.l

-0 2

-0 3
<) 2

-0 4 •0 25

R rst R ex. Link Second R ex . Link

(b) Tip Transverse Displacements of the Flexible Links

Figure 5.4: Shifting Loads on Flexible Links towards th e End-effectors

 C H A P T E R 5. OPTIMIZATION APPROACH 126

Therefore, the weighting m atrix is set to:

W r = diag{1.0, 1.0, 1.0, 10-'°, 10-'°, 10-'°} (.5.24)

Note th a t the last three elements in the above are chosen to ensure the positive­
definiteness of the m atrix W r - The solution for the torques at bo th arm s and the
elastic coordinates of the left arm are shown in Figure 5.4.

5 .4 .3 M in im izin g th e N orm o f th e E lastic A c c e le r a tio n s

.A.lthough extensive studies of force optimization schemes have been carried out for
rigid cooperating manipulators, relatively little research exists which considers the
dynam ics of the elastic components in the system. Pfeiffer et al. [6 8 ] calculated the

contact forces for a walking stick insect (a biological model for a multi-legged walking
machine) while minimizing the deformation energy stored in the legs. In particular,
bending loads on the outer, thin leg segments were minimized based on a static
relation between the deformation and the loads applied to the leg segments.

It is interesting to note that one of the characteristics of the flexible-link inverse

dynam ics solution is th a t the generalized coordinates may be significantly affected by
the wrenches applied at the end-effectors of the manipulators. By contrast, in the
rigid-link case, a different choice of tip wrenches only affects joint torques since the

generalized coordinates (joint variables) are predetermined by the desired motion of

the grasped object. .Mthough this fact complicates the inverse dynam ics problem, it
can be used to advantage. It allows us to actively choose the a c tu a to r torques and
hence, the wrenches which produce the least excitation of the system vibrations, VVe
therefore set as our goal for a flexible multi-arm system to find a set of wrenches
which minimizes the elastic vibration in the flexible links.
 C H A P T E R 5. O P T I M I Z A T I O N A P P R O A C H 127

Observing that the m a n ip u la to r dynamics equations are linear in elastic accelera­

tions, we propose to minimize a weighted norm of the elastic accelerations. As in the
CcLse of minimizing a weighted norm of the actu ato r torques, the objective function is
w ritten as:

(5.25)

Su bstitu ting the elastic accelerations

into the objective function (5.25), we obtain

/E (x ) = c | x + (5.26)

where

e g = J3V W e W e = J3e W eJ^ , (5.27)

Once again, the positive-dehniteness of m atrix W e can be ensured if the matrix

W e is positive-definite and has full rank [67]. The matrix has full rank
equaling to its row dimension provided th a t J 3 , (f = I. . . . . P) are not rank deficient.

Since as defined in eq. (4.6). and we assume the existence of M ~ |,

has full rank as long as is of full rank. The rank deficiency of corresponds
to a situation where some of th e elastic degrees of freedom cannot be affected by
any tip wrench. Recalling th a t is th e elastic .Jacobian matrix which maps config­
uration space elcistic rates into a corresponding workspace end-effector velocity. Je,
becomes rank deficient when the elastic motion of the ith m anipulator only generates
end-effector motion in a subspace of the Cartesian space. Figure 5.5 provides an il­
lustration of a planar two-link arm in a rigid singular configuration (.\'i and .\C are
aligned). .Assuming th a t only bending is modeled for the links, any nonzero elastic
rates of the arm will only produce tip velocity in one direction, as shown by V£. In
this case, J . is rank deficient.
 C H A P T E R 5. O PTIMIZATION A P P R O A C H 128

Figure 5.5: Singularity of the Elastic Jacobian Matrix

E x a m p le 5.4

-A.S in the previous example, the left arm is considered to have two flexible links
while the right arm has rigid links only. Since each flexible link is m odeled with
two planar beam elements in bending, the total num ber of elastic coordinates is

S. We implement the scheme of m inim um norm of elastic accelerations with an

identity weighting matrix W g . Figure 5.6 shows the resulting profile of th e transverse
displacements at the midpoint and the tip of each flexible link, as well as the strain
energy of the system. .Although increasing elastic deformations in the elastic links

are observed in the figure, they still a p pear to be bounded within the specified time
of the maneuver. This behavior is characteristic of a short time stable system [69].
T h e short time stability deals with determ ining w hether a system response lies within
specified bounds over specified intervals of tim e when the inputs are w ithin specified

bounds. In investigating the inverse dynam ics solution, this concept is very useful
since th e system behavior within specified tim e intervals is our main concern.
C H A P T E R 5. OPTIMIZATION APPROACH 129

. m id - p o in t d i s . ( m m ) e n d -p o in t d is . (m m )

0.4

OJ
0.2

0.1

-0.1 f(S )

■0 .2

-0.3

-0.4

First Flex. Link

.m id -p o in t d is. (m m ) e n d -p o in t d is. (m m )

0.75

0 25

-0.25

S e c o n d Flex . L ink

= 0.005

0.5

F igure 5.6: Results with M inim al Norm of Elastic .Accelerations

 CHAPTERS. OPTIMIZATION APPROACH 130

5.4.4 M in im iz in g th e S tra in E nergy S to red in th e F lex ib le

L ink s

If we accept t h a t minimizing elastic accelerations smoothes the profile of elastic dis­

placements, th e n minimizing elastic displacements should decrease the magnitude of

deformations. For this purpose, it would be desirable to express the objective in term s
of the elastic coordinates th a t is,

f i Qe ) = Qe (5-28)

Since is linearly related to the design variables x, double integration of x over

the time history is required to determ ine the effects of x on q^. In other words, the
elastic deform ation of a m anipulator at time t depends not only on the present value
but also on th e history of its end-effector wrenches. Ideally, the force distribution

problem should be solved to produce the minimum vibration excitation in the system
over the com plete trajectory, i.e., globally. However, as mentioned in the beginning
of the thesis, in th e present work, we have focused on the development of local force
optimization schemes because of their simpler formulation, their lighter computational
requirements, a n d the fact th a t they can be implemented without knowing the object
trajectory apriori.

Since the solution to the optim ization problem must be carried out numerically,

we discretize tim e into intervals and approximate q^ at a tim e instant by its

second-order Taylor expansion at tim e

gt % -k (Ô.2 Ü)

where A t = tk — and the superscript k denotes quantities evaluated at In

formulating a q u a d ra tic objective function of the form (5.28), we note that using
identical weights on all the elastic coordinates may be unacceptable. This is partly
 C H A P T E R 5. OPTIMIZATION APPROACH 131

due to the fact that these coordinates are likely to represent different quantities and

hence have different dimensions. For instance, some components of represent linear
displacement measured in millimeters, while others represent angular displacem ent
measured in radians. Also, the importance of each elastic coordinate will be very
different for links with different stiffnesses.

Given th at the strain energy can be used as a measure of the elastic deform ation,
we could set as our goal to minimize the strain energy in the flexible links. T h is can

be achieved with the following objective

f u i 9e) —^ ^ (5.30)

Substituting eq. (5.29) into the above, the strain energy at tim e 1^, , in term s of
the design variables x at tim e becomes:

== { ( )2 9:;. + / u 4-

+ ^ ( A 0 ‘ X ^ J3 v ; K e e v J j v X +17( 9ev • (5.51)

In the above, the m atrix is of dimension 6 P x T he expression for

represents an approximation for the strain energy at tk in terms of the variables
evaluated at the previous time instant. Thus, minimizing the value on the right-hand
side of eq. (5.31) am ounts to minimizing the strain energy at the next tim e instant.
T h e last term in the above equation, g{ q^. 9 ') , is independent of x and its detailed
form is omitted asit has no influence on the solution. T he resulting objective function
can be expressed in the form (5.10a) with the following definitions:

èu = J 3 v K . e v ( ^ ( A G ' 9 : v + A ^ g . s + gev) (5-32)

Wf- = (5.33)

Once again, since Kggr is a positive definite matrix, will be positive-definite as

long as Jag has full rank.

 C H A P T E R 5. OPTIMIZATION APPROAC H 132

Table 5.3: Coefficients of the Object Trajectory

i f (sec) ta (sec) a 7 h (m)

1. 0 0.5 - 1. 0 0.5 1. 0 1. 0

E x a m p l e 5.5 — M inimum Strain Energy

We implement the minim um strain energy scheme on the 3D dual-arm and object
system first introduced in section 4.5.2. The forearm and the upper a rm of th e left
a rm are considered to be flexible while the right arm is rigid. For this exam ple, the

object trajectory is defined with the trajectory constants given in Table 5.3. T he
resulting maneuver has no constant speed segment and the total m otion in the V'

direction is smaller than in the other directions. T he weighting m atrix is invariant

with tim e and is defined as:

Wt; = diaglK^ell. KeeI2}

with the two diagonal blocks given by:

115809-5 0.0 0.0 0.0 —57904.8 0.0 0.0 14476.2

0.0 115809.5 0.0 0.0 0.0 - 5 7 9 0 4 .8 - 1 4 4 7 6 .2 0.0

0.0 0.0 9650.8 0.0 0.0 14476.2 2412.7 0.0

0.0 0.0 0.0 9650.8 -1 4 4 7 6 .2 0.0 0.0 2412.7
K ^gli —

-5 7 9 0 4 .8 0.0 0.0 -1 4 4 7 6 .2 57904.8 0.0 0.0 -1 4 4 7 6 .2

0.0 -5 7 9 0 4 .8 14476.2 0.0 0.0 57904.8 14476.2 0.0

0.0 -1 4 4 7 6 .2 2412.7 0.0 0.0 14476.2 4825.4 0.0

14476.2 0.0 0.0 2412.7 -1 4 4 7 6 .2 0.0 0.0 4825.4

Numerical results were generated with A t = 0 .0 0 0 2 (reasons for this will be given
after the analysis of the stability of the inverse system). Solutions for the transverse
displacem ent and the slopes at the end-point of both flexible links are shown in
C H A P T E R 5. O PTIM IZATION APPROACH 133

End-Poim Vy (mm) End-Point 0y(rad) End-Point (mm) End-Point 9j(rad)

0.00004

0.00003

0.00002

0.00001

0.25 0.75

-0.00001

■0.00003

-0.00004,

First Flex. Link First Flex. Link

End-Pomi v„ (mm) End-Poim 8^(rad) End-Point imm> End-Poim 0,(rad)

000075

0 0(X)25

0 25 0.75

-0.00025

-0.00075

Second Flex. Link Second Flex. Link

Figure 5.7: E nd-Point Deflections w ith M inim um S train Energy Scheme

 C H A P T E R 5. OPTIMIZATION APPROACH 134

Min. Strain Energy Min. Norm H as. Acc

0.025

0.015

Figure 5.8: Comparison of the Strain Energy (J)

F ig u re 5.7. It is observed that in-plane deflections (v^) are much larger than th e out-
of-plane deflections (v,). since faster motion in the X f direction is required for the
flexible links th a n in the Z/ direction. .-\s a result of minimizing the strain energy,
the strain energy stored in the flexible links is kept to less th a n 3 x I0“ ‘‘ Joule. W hen

compared to the results obtained using the minimum Euclidian norm of the elastic
acceleration scheme (F ig ure5.8, 5.9 and 5.10), it is apparent th a t the m inim um strain
energy scheme is effective in reducing both strain energy and the elastic deformations.

5 .4 .5 C o m b in a tio n o f V arious O b jectiv es

One can take a weighted combination of the aforementioned individual objectives

to form a new objective function. This objective function represents a combined
criterion which is particularly relevant to optimal performance of the system. T he
choice of the relative weights on each component of the objective depends largely on
the particular m anipulator system and numerical experiments are required to "tune"
C H A P T E R 5. OPTIMIZATION APPROACH 135

. min. strain energy . min. norm elas. acc. _ min. strain energy . min. norm elas. acc.

0.05 0.05

0.04 0.04

0.03 0.03

0 .0 2 , 0.02

0 .0 1 0 .0 1

0.25 0.75 0.25 :oj 0.7$.

-0 .0 1 r(s) •O.Ol rfs)

- 0.02 -0.02

-0.03

-0.04

0.05 -0.05

Mid-Point v„ End-Point v_

Figure 5.9: In-Plane Deflections of the First Flexible Link

. min. strain energy . min. norm elas. acc. . min. strain energy . min. norm elas. acc.

:.5

0.5
05

0
7 0 .5 0.75 0.25 0.5 0 75
f(s) •0.5

1.5 •2.5

M id-Point Vy End-Point Vy

Figure 5.10: In-Plane Deflections of the Second Flexible Link

 C H A P T E R 5. OPTIMIZATION APPROACH 136

the weights. T h e resultant objective function can be formulated as:

/(x ) = affix) + b frir) + c /g ( qf} + d Ju{ (5.34)

where th e weights a, 6, c, d are positive numbers, some of which can be set to zero.
T he term s in eq. (5.34) represent component objectives of m inim um internal forces,
op tim al load sharing, m inim um elastic acceleration a nd m inim um strain energy. Since
each individual te rm is a strictly convex function, the combined objective function is

also strictly convex and it is characterized by:

c = bcT + c c E + d c u (5.35)

W = a W [ + b' Wr + cW e + <TWu (5.36)

The definitions for each te rm in the above have been given in Sections 5.4.1 to 5.4.4.
We now present some examples to illustrate the performance of the combined objec­
tive function scheme.

E x a m p l e 5.6 — C om bined Objective

To dem onstrate th e performance with the combined objectives, we consider two cases:

C ase 1 : /(x ) = + rfo -f -^ 3 +U

.\ccording to eq. (5.34). the relative weights on each of the four objectives are: a = 0.
6 = I. c = 0 and d = 1. T h e weighting matrices for the combined objective function
are:

Wr = d ia g { W r ,. W r . } (5.37)

Wu = d i a g { K „ n Keen} (5.3S)
 C H A P T E R 5. OPTIMIZATION AP PR O A C H 137

where

W n = diag{ 2 , 2 , 2 , 1 0 "*°}

W t2 = 1 0 ~‘° 5x6

It can be seen from F ig u re 5 .I I th a t the torques driving the flexible a rm are kept
small in m agnitude since the norm of torques at th e first three joints is m inim ized. T h e
torques for th e rigid arm are shown in Figure5.12. T h e tip transverse deflections (in­

plane an d out-of-plane) for both links are shown in Figure 5.13. Larger deflections in
the out-of-plane direction occur because the end-elFectors are required to move faster
in th e X f — Z f plane than in the X f — Yf plane.

C ase 2 — / ( x ) = 10"®(jx^x) -t- U

VVe have mentioned in the first exam ple (Example 5 . 1) th a t minimum intern al force

schem e by itself is not suitable for solving the force distribution problem for flexible-
link cooperating manipulators. However, sometimes it can be combined w ith other
schemes to yield a compromise performance between lower internal forces a n d other

objectives. To illustrate this idea, we propose case 2 which sets a = I0~'^. T h e tim e
histories of the tip deflections at each flexible link and th e strain energy for this case
are shown in Figure5.14. W ithout the second term in the objective function, th e m in ­
im u m internal force scheme almost always generates unbounded solution. However,

com paring these results with those obtained in exam ple 5.5 when using th e m inim u m
strain energy scheme, the dynam ic response of the system is dram atically different
for the two schemes, even though only a small change to the objective function was
made.

Based on the large num ber of numerical simulations performed with two-arm
(planar and 3D) cooperating manipulators (only representative results are shown
C H A P T E R 5. OPTIMIZATION APPROAC H 138

0.001 0.0006

0.00075:

^.0005
Z0.0002
%00025
0
0.25 0.75
f(S )
-0.00025
■0.0002

-0.0005

-0.0004
-0.00075

-0.001 [ -0.0006

000 05 0.03

ooocM:
0.0003;

=00002
0.01
CO.ÜOOI
0.15 0.75
f (s)

0.0005 002

£
Z 0.01

05 0.75 0.25 0.75

-0.01

-0 02

Figure Ô.11: A ctuator Torques of the Flexible Arm in Case 1

C H A P T E R 5. OPTIMIZATION AP P R O A C H 139

. t24 <N-m) • ^25 (N-m)

T26(N-m)
6

20 4

0
0.23 0.75 0.25 0.75

-ID 2

•20 -4

-30 -6

Figure 5.12: ,A.ctuator Torques of the Rigid A rm in Case 1

In-plane (m m ) O ut-of-plane (mm ) fn-plone (m m ) O ut-of-plane (m m )

0.04 0.7 5

0.03
0.5

002

0.25
0.01

0.25 0.75 0.25 0.75

-0.01
-0.25

-0.75

First R ex. Link Second R ex . Link

Figure 5.13: Tip Deflections of th e Flexible Links in Case 1

C H A P T E R 5. OPTIMIZATION A P P R O A C H 140

. In-planc (mm) . Ouc-of-plane (mm)

0.75
f (S)
•0.5

First Flex. Link

.In-plane (mm) . O ut-of-planc (m m )

40

0.75

Second Flex. Link

Oft

»
0.4

0 2

0 O 25 0.5 0.75
r(s)

Figure -5.14: Results of Case 2

C H A P T E R . 5. O PTIM IZATION APPROACH 141

in the thesis), th e following conclusions which are particularly relevant to the force
optim ization issue for flexible cooperating manipulators m ay be drawn:

• Minimizing th e strain energy is one of the better objectives to use for flexible

cooperating m anipulators. Strain energy is a more physically meaningful mea­

sure than th e elastic accelerations. Furthermore, in m any cases, the minimum
strain energy scheme provides a bounded solution while the other schemes do
not.

• Shifting loads from the joint actuators before the last flexible link to beyond the
distal end of th e last flexible link of the arm helps to obtain a bounded inverse
dvnamics solution.

To obtain a bounded solution, either the minimum strain energy or the load
shifting scheme should be used in combination with other objectives.

• Systems with different mechanical setups in terms of the location of the flexible
links behave differently with the same force optimization scheme.

• For a system with only one flexible link, better behavior is obtained when the
flexible link is connected to the base. This can be explained with the same
argum ent as th a t used to design the load shifting scheme. W hen the flexible
link is closer to th e base, there is a b e tte r chance th a t the load can be shifted
to beyond its distal end and therefore, the system more closely approxim ates a
collocated situation.
 142

C h ap ter 6

S ta b ility A p p ro a ch

One of the important applications of the inverse dynamics solution is for trajectory
tracking control of nonlinear dynamics systems. The well-known co m pu te d torque
m eth od [70]. also known as inverse dynamics control is basically designed to achieve
feedback linearization of a nonlinear plant. .A. prerequisite for this method to be

applicable in controlling a robot m anipulator is th a t a bounded solution for the inverse

dynamics torques be obtained during the specified time period, which is usually the
period over which the reference trajectory is defined. The computed torque technique
has been successfully employed to control rigid manipulator systems. For a serial
flexible-link manipulator, however, we have observed that the nonlinear inversion
of input-output map does not exist because of the nonminimum phase zeros of the
input-output map. Hence, the inverse dynamics torcjues are no longer guaranteed to
be bounded.

In this chapter, further discussion on the stability of the inverse dynamics system
for flexible manipulator system is performed. In particular, we aim at obtaining a
stable inverse dynamics solution for flexible-link cooperating manipulator system with
an emphasis on its control applications.
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 143

6.1 D e s c r ip tio n o f In tern a l D y n a m ic s

In the context of control, it is known that the invertability of the in p u t-o u tp u t m ap is

closely related to the stability of the zero dynamics, also called the internal dynamics
The definition of the zero dynamics is as follows:

D e fin itio n 6 .1 (Zero Dynamics) [56]

If we divide the system states into the observable and unobservable parts, the
zero dynamics represents the dynamics of the unobservable states when: I ) the

input is set equal to zero, and 2 ) the outpu t is constrained to be identically

zero.

.4s mentioned earlier, the instability of the causal inverse dynam ics solution for
flexible-link manipulators is due to the inherent nonminimum phase property of the
system. Indeed, according to its definition, the nonminimum phase linear system is
that which has zeros of the input-output transfer function in the right half of s-plane
whereas a nonminimum phase nonlinear system means that the system has unstable
zero dynamics. Therefore, stabilizing the internal dynamics (zero dynam ics) is related
to changing a nonminimum phase system into a minim um phase system for which
stable causal inverse dynamics solutions can be obtained.

In tip trajectory tracking control of a flexible manipulator, the o u tp u t equations

are those which define the tip position and orientation, or their first and second
derivatives since the latter are easier to formulate. T he unobservable sta te variables
are the coordinates and their rates which define the elastic motion. Therefore, the
internal dynamics is described by the elastic equations of motion with the joint torcjues
(input) and the tip velocity (output) both set to zero. It is found th a t the design
of a stable closed-loop control scheme is critically dependent on the stability of the
internal elastic dynamics [56. 71. 72. 73].
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 144

T h e importance of the elastic response can also be observed when considering

th e inverse dynamics solution for flexible manipulators. In the inverse dynamics
system , th e tip motion is predefined (this is also true in multiple-arm systems with
rigid grasp). The rigid states (the joint coordinates and speeds, and of
the T th manipulator are dependent on the elastic states (elastic coordinates and
speeds, and through the kinematics relations. The behavior of the inverse
dynam ics solution, comprising rigid states and a ctu a to r torques, is determ ined by the
elastic motion. In other words, if the equations describing the evolution of elastic

coordinates produce a stable trajectory in the tim e domain, the inverse dynamics
solution for actuator torques is ensured to be stable as well. To analyze the stability
of th e internal dynamics, we first present its state-space description. We assume in the
following analysis that there are no external forces acting on the m anipulator system
o th er than the actuator efforts. This is done in order to exclude the dependency of
the system behavior on the external forces.

It can be found in the literature that some researches have focused on modifying
the o u tp u t of the system in order to realize a minimum phase property. .Joint-based
control [71] accomplishes collocation of the input and ou tpu t at the joint. Reflected
tip position is used to allow precise tracking of a close-to-equivalent tip position [74].
Torque transmission mechanism is also used to transfer the actuation efforts to the
tip so th a t the input is transferred to be collocated with the o utp ut [22]. For the
m ultiple arm system, we are interested in investigating the possibility of stabilizing
the internal dynamics by actively choosing the tip wrenches. For the purpose of
analyzing the inherent properties of various systems, we propose the following three
typical cases:

S y ste m ,4: C ollocation at th e Joints

CHAPTERS. STABrU TY APPROACH 145

In this case, actuators are located at the joints and th e joint variables (displace­

ments. velocities and accelerations) are predefined as functions of time. This is not
impractical and in fact, most of th e existing inverse dynam ics control schemes for
serial flexible m anipulators adopt such a scheme. Not only is joint collocation simple
to implement since the actuators and sensors can be easily set up at the joints of the
manipulator, bu t more significantly, it results in a stable internal dynamics system.
Therefore, a stable closed-loop behavior can be obtained by designing a robust linear
PD controller [71. 53]. The principal lim itation is th a t the resulting controller only
ensures tracking of a prescribed joint motion. For tip tra je cto ry tracking control, a
modification is m ade to allow precise tracking of th e nominal joint trajectory while
the dynamics equations describing th e elastic motion are w ritten with respect to the

moving frames attached to the virtual links [35. 29]. Some researchers have tried to
control the motion of some virtual point (not a real point on th e manipulator) the
coordinates of which can be expressed as a linear com bination of the joint variables.
For example, the reflected tip motion [74] has been used so th a t a linear combination
of the joint motions is predefined and controlled [75. 66 ].

To put in place this situation in a m ultiple-arm system , we consider th a t the m a ­

nipulators are decoupled to follow their own joint trajectories w ithout interfering with
each other. This is imposed because, for a m ultiple-arm system , the joint coordinates
can only be freely specified if the m anipulators are not constrained by each other,
th a t is. when there are no constraint wrenches acting at th e tip of each manipulator.
This seems artificial since the m ultiple arm s are not arranged to cooperate on the
common task. However, we do this prim arily to gain an understanding on why the
internal dynamics is stable in this case.

To investigate the inherent properties of such a system with joint motion specified,
we consider the dynamics equation for the elastic motion, eq. (4.2). We assign suffix
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 146

S to each m atrix and vector variable to represent a m ultiple-arm system, and set

Çrr = 0 since it is the output of the forward collocated system. T h e internal dynamics
is then described by:

T ~ f I.e^, (6-1)

We point out the difference between the nonlinear forcing te rm in the above equation.

and th a t in eq. (4.2), The latter includes th e effect of external forces

on the manipulators. It is evident th a t if eq. ( 6 . 1 ) gives a stable trajectory for
then the inverse dynamics torques calculated from the algebraic equation (4.1).
will be bounded.

S y ste m B: N on collocation o f th e Joint A ctuation and T ip T rajectory

.\s implied by tip trajectory tracking, the joint variables in this system are not
explicitly prescribed. Instead, the desired output is a nonlinear function of both
rigid and elastic coordinates. For rigid manipulators, tip tracking control can be
implemented by tracking the prescribed joint trajectories since the joint variables
are uniquely defined by the tip trajectory (assuming the m anipulators are not kine­

matically redundant). However, this is not the case for flexible-link m anipulators
because of the unknown elastic variables appearing in the kinematics equations. In a
multiple-arm system, the reference trajectory is prescribed for the object. W ith the
assumption of a rigid grasp, this is equivalent to defining the reference trajecto ry for
the respective end-effectors of the arms. To derive the equations of internal dynamics
for system B. we set to zero. In chapter 4. we derived the solution for the elastic
accelerations which is represented by eq. (4.9). Expanding this equation by substi­
tuting the expressions for and from eq. (3.54) and eq. (4.6). respectively, the

governing equation for the elastic motion becomes:

9eV T D e e v T K e c v ~ T J ç ^ î ü v (6.21
CHAPTERS. S T A B IL IT Y APPROACH 147

where

/iv = col{/ii, hp} (6.3)

MeeE = d ia g { M „ i,---.M „ p } ( 6 .4 )

hi = + je. g« ) (6.5)

Mee. = M „, - (6.6)

i = 1 , 2 , • • •, P

Eq. (6.2) represents the internal dynamics for the system when the actuators and the
specified object tra je cto ry are noncollocated.

S y s te m C: C o llo ca tio n at th e Tip

This again represents a hypothetical situation where the manipulators are driven
by wrenches at the tips. The motivation for investigating such a system is due to
the successful design of a stable PD controller by Park and .Asada [22] for a one-link
flexible arm with the actu a to r torque transm itted to the tip of the arm. It is implied
th a t the internal dynam ics of such a system is stable. To derive the internal dynamics
equations for system C. we simply set = 0 in eq. (4.12) to nullify the actuation at
the joints. Solving the resulting equations for the "actuation” at the tip i.e.. the tip
wrenches w ^ . we obtain:

t -T /
^ r — J 1V

= ( G. T)

T d rr (M rrv ~ j r “ j g r Çgr ) ~ f f.rr )

Upon substituting the above into the elastic dynamics equation (4.14) and setting
to zero, we obtain:

r Qcf T D ecs 9 e r T K e er ~ 9 s' (6.6)

C H A P T E R 6. S T A B I L I T Y A P P R O A C H 148

where

(6.9)

= /l£ — — Mrrj; J r v ( Çrv; + j e r 9er )) (6.10)

In the above. Mgg^ is sym m etric which can be shown by substituting the expressions
for Mggv and M^er into eq. (6.9) to yield:

Mggr = Mee^ - Mggj- + (6 . 1 1 )

and noting that Mrrr; and Mggv are both sym m etric in nature. .\s a result, eq. ( 6 .8 )
describes the internal dynamics of system C in which the m anipulators are driven by
th e wrenches collocated with the specified trajectory at the tip of the end-effectors.

6 .2 P r o p e r tie s o f t h e I n te r n a l D y n a m ic s

Since closed-form solutions to the internal dynamics equations (6.1). (6.2) or ( 6 .8 )

do not exist, we shall analyze the behavior of the internal dynamics through an
approximation method, in particular, the linearization method in the neighbourhood
of equilibrium points of the nominal configurations. To develop an effective linearised
model of the internal dynamics, it is necessary to identify the properties of the internal
dynam ics equations by reviewing the procedure for deriving these equations.

For notational convenience, we now drop the suffix E used to denote the assembled
quantities for the multiple m anipulator system and focus our discussion on a single
"decoupled" manipulator. VVe note th a t equations (6.1). (6.2). and ( 6 .8 ) represent a
set of second-order ordinary differential equations for the elastic coordinates q^.
C H A P T E R 6. STABILITY APPROACH 149

6.2.1 C on stan t Kee an d Dge

VVe have observed that when m ultiple manipulators are in contact with a com m on
object, they form several closed-loop chains. The multi-arm mechanism as a whole

is stiffened because of the constraints th a t manipulators impose on each o ther. It is

therefore reasonable to expect smaller elastic deformations than in the situation where
m anipulators are decoupled to follow the same end-effector trajectory. A lthough the
present stu d y of the inverse dynam ics solution for flexible m anipulators is not limited
to a particular dynamics model, to render the stability analysis more tractab le, we use

linear theory of elasticity to derive b od y mass, stiffness and modal dam pin g m atrices.
Therefore, these body matrices are assumed to be constant.

Furtherm ore, according to the assembly procedure of the global dynam ics ec;na­
tions for a serial-chain m anipulator [40], the stiffness and dam ping m atrices in the
global dynam ics equations are formed by placing the body matrices along th e diago­
nals. Therefore, the global stiffness and damping matrices are constant atul depend
only on the material and geometric properties of the links.

6.2.2 C onfiguration D e p e n d e n t Mgg, and Mgg

VVe consider the coefficient matrices of the acceleration terms in the internal dynam ics
equations for the foregoing three systems and repeat the expressions for M,.-. !VL,.
and as.

Mee = 4- (6.12)

M e. = Mee " , 6.13)

Mee = % - (6.14)
C H A P T E R 6. STABILITY APPROACH 150

Note th a t in eq. (6.14), vve m ade use of the symmetry of the m atrix Mee. The suffix S
appearing in eq. ( 6 . 1 2 ) denotes the assembled quantities over all the bodies in a single
serial chain. T h e dependence of these matrices on the configuration variables ç,. and
Ç, is through the coordinate transform ation matrices and and th e .Jacobian
matrices Jr and J,. .A.lthough generally, these are functions of both q^. and in an
approxim ate situation, they can be evaluated at a nominal rigid configuration while
ignoring th e effects of elastic deformations. For the same reason th a t a closed-loop
multiple-arm system is stiffer than the individual serial m anipulators, we ignore the
variation of the body transformation matrices on the body deformations and focus
on analyzing the dominant factors governing the internal dynamics. Therefore, the
coefficient matrices are assumed to be dependent on rigid configurations only.

6.2.3 N o n lin ea r In ertia l T erm s

In the equations of internal dynamics, the right hand sides contain the nonlinear
inertial forces and in the case of system B. another term of tip wrenches. For the
nonlinear inertial forces in eqs. (6.3) and (6.10). we repeat the following expressions
for calculating and ^ (40):

/f.r = - M . . . 4 t v ( P s q . + 5 s q , ) + T s ( 5 v q , - f Pq,)]}(6.15)

+ /v ./.e - M r . r e [ T v C p T g ^ -f S ^ q ^ ) -f T v ( < S v ç ^ -f " P g j ] (6. 16 )

In the above, f ^ j , . and f ^ j ^ . are composed of the body nonlinear inertial forces
which in turn include only the rigid motion nonlinearities, that is. terms of 0 (|| q,.|i'|.
With the earlier assumptions, it is not difficult to see that the nonlinear inertial forces
f [ g . h and g contain terms that are 0 ( || gJI^) and 0{\\ g,.|||| Çp||). T he coefficients
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 1Ô1

of these terms are, again, generally functions of th e variables and but are

approximated to be dependent only on the rigid coordinates q^.

It is also worth repeating here that in system B . the tip wrenches appear in
the internal dynamics equations. Keeping in mind th a t we have some degrees of
freedom in choosing these wrenches, the potential benefit is th at these wrenches may
be actively chosen to stabilize the internal dynamics which is the key objective of this
chapter.

6 .3 L in e a r iz a tio n

T he main purpose of analyzing the internal dynamics for the three different systems
described in section 6.2 is to predict whether the internal dynamics possesses a stable
behavior. For the multiple-arm system, we are also aiming to find a force distribution
scheme which, based on the stability analysis, will ensure improved behavior of the
system. However, as shown in section 6.2. the internal dynamics equations are non­
linear even though we made several approximations in deriving them . The analytical
solution of the internal dynamics equations is impossible to obtain. In view of the

much greater simplicity and tractability of linear, as opposed to nonlinear equations,

it is desirable to make use of the linear approximation. T he behavior of the solution
to the original nonlinear equations about a nominal configuration can be investigated
by considering the linearized model.

Reinstating the use of suffix T for a multiple-arm system, we now define a vector
of state variables of the internal dynamics as:

y = [ y iy 2F = k .v (G .iti
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 152

It is not difficult to rewrite equations ( 6 . 1 ), (6.2), and ( 6 .8 ) in th e form:

jr = f(y ) (6.18)

For instance, in system A. the function f(y ) on the right hand side of eq. ( 6 . IS) is

V2
f.4(y) = (6.19)
—Mjçj,[(Kee£Î/j -i- Dgeî;î/ 2 ) "h //,e v ]

We emphasize th a t f( y ) is evaluated at a nominal rigidconfiguration with state

variable satisfying the kinematics relations in the absence of elastic deformations

and = 0 . Eq. (6.18) also represents an autonomous system since tim e variable t
does not appear explicitly in the function f.

D e fin itio n 6.2 (Equilibrium Point)

Suppose an autonom ous system is described by the vector differential equation

ÿ = f(y ) ( 6 .2 0 )

y is said to be an equilibrium point of the system if

f(y) = 0 ( 6 .2 1 )

It can be shown th a t y = (q^^. q^^) = (0. 0) is one of the equilibrium points of the
internal dynamics equations eqs. (6.1).( 6 .2) and ( 6 .8 ). In other cases, for example,
where gravity effects have not been dropped, q^^ may not be zero but would represent
the static deformation of the flexible structure. .Analyzing the system behavior in the

neighbourhood of equilibrium points is particularly im portant in the situation where

dam ping out the system vibration at an operating point is of specific interest.

The local stability properties of equilibrium point can be obtained by linearisation.

We therefore linearize the internal dynamics equations around the equilibrium point
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 153

( 9 ev- 9 ev) = ( 0 , 0 ) by taking variations of the variables from the nominal values and
lettin g

"b ( 6 . 22 )

9ej: "b (6.23)

"b (6.24)

tt7v + (6.25)

We will use eq. (6.2) which is th e internal dynam ics equation for system B to illustrate
th e derivation of the linearized model. E xpanding eq.(6.2) into a Taylor series, for
small variations of the state variables as in eqs. (6.22)-(6.25), the original nonlinear
m odel can be approximated by the first-order expansion about the nominal state. For

th e sake of simplicity, the circumflex for denoting nominal values is o m itte d in the
following. Thus, we have:

+ (D.ev + + K„v<î)<7ev = (6.26)

dhv
where we observe that ■ = 0 since h^- contains nonlinear coupling terms of

0{\\ 7rvllll lev'll) which vanish when evaluated at = 0. Once again, all the co­
efficient matrices in eq. (6.26) are calculated at th e nominal states. The resultant
linearized model of eq.( 6 .2 ) can be w ritten as:

0 0
-f- bw. (6.27)
M -L K - M -eev^ees
ID

In the same manner, the linearized model of system .4. that is. the case of joint
collocation is described bv:

69=2 a ? ,.
(6.2S)
6 9 ,2
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 154

Finally for system C where the input and o u tp u t are collocated at the tip. eq. ( 6 .8 )
is approxim ated by:

0 I
(6.29)
- M " ‘j ,Deev

For systems .4 and C, the linearized model can be w ritten compactly as:

8y = A 6 y (6.30)

It can be seen from eq. (6.27) which is the actual case for multiple flexible arm s, that
the tip wrenches influence the value of the s ta te variables. The linearized model for
system B is accordingly given by

6y = A S y + (6.31)

where
0
B = (6.321
iT

T h e coefficient matrices A in the three different cases denoted by A,.i. A g and A r

respectively are:

0
A-..1 = (6.33)

0
As = (6.3(1

0 I
Ac = (6.3.5)

T he differences between these are in the ‘‘mass" m atrix which premultiplies the stiff­

ness and damping matrices, that is. the matrices . Meej and . For system
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 1Ô5

B. since we have some freedom in choosing the tip wrenches and in particular, if
they are chosen to be a function of the state variables and then it is pos­
sible to change the inherent behavior of the linearized internal dynamics around the
equilibrium points.

6 .4 S ta b ility I s s u e R e v is ite d

We stated in chapter 3 th a t in considering the inverse dynamics solution, we were

particularly interested in obtaining a bounded solution when a bounded input is
applied. From a practical point of view, it is perhaps more useful to require th a t the
solution be bounded over a specified finite time interval. In terms of the behavior
of the internal dynamics of the inverse dynamics system about an equilibrium point,
what is more relevant is whether the elastic variables will approach zero as time
goes to infinity. .A,t this point, it is necessary to review some definitions and related
stability theorems.

6.4.1 S ta b ility o f Linear T im e-Invariant S y stem s

D e f in itio n 6.2 (Stable Equilibrium)

.A.n equilibrium point y is stable if. given any e > 0 . there exists <5 > 0 such th at
|y ( 0 ) —ÿ| < 6 implies ly(/) — y\ < e for all t > 0 .

The physical interpretation of a stable equilibrium point is that a trajectory described

by eq. (G. 18) can be made to remain within an arbitrarily small region about ÿ by
starting sufficiently close to it.

D e f in itio n 6.3 (Asymptotically Stable Equilibrium)

For a stable equilibrium point y . if there exists A > 0 such th a t |y(0) —y | < A
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 156

implies th a t y{t) y as t oo, then th e equilibrium point y is said to be

asym ptotically stable.

Evidently, asym ptotic stability requires th a t a tra je cto ry starting in a sufficiently

close region of th e equilibrium point not only rem ain close enough to y , but th at it
also approaches y asymptotically.

If the original nonlinear system (6.18) is app rox im ated by a time-invariant lin­
ear system at the equilibrium point, it is well-known th a t the local stability of the
equilibrium point y of th e given nonlinear system can be examined by checking the
eigenvalues of th e plant m atrix A. The system is unstable if A has at least one eigen­
value with a positive real part and it is asym ptotically stable if all th e eigenvalues
of A have negative real parts. If some of the eigenvalues are purely imaginary and
the others have negative real parts, then the linearization technique is inconclusive.
In this case, the stability of the equilibrium is d eterm ined by the higher order terms
that are neglected in the linearization process.

For end-point positioning control, the dynam ics model is required at the desired
terminal rigid configuration. In many trajectory tracking control algorithm s the dy ­
namics model of th e system must be evaluated at several nominal rigid configurations.
In this case, linear time-invariant equations approxim ating the nonlinear equations
are obtained at the equilibrium ÿ = T hese applications justify the analysis
of the linearized internal dynamics of the inverse dynam ics problem a n d we therefore
concentrate on calculating the eigenvalues of the plant m atrix A. It should be real­
ized that since the linear model is configuration dependent, ideally, we should check
the complete workspace of the manipulator for th e stability status at th e equilibrium
points. However, because of certain properties of th e linearised system , there exist
certain patterns of the eigenvalues of the plant m a trix , which will be discussed shortly.
 C H A P T E R 6. S T A B I L I T Y A P P R O A C H 157

Table 6 .1; Stable Eigenvalues of A .4

Eigenvalues Ai ,2 -^3.4 >^5,6 ^7,8

C= o ±.3032.45; ± 842.25; ± 2 0 .5 4 ; ± 8 .7 2 ;
C = 0.5% -1 2 .9 ± 3 0 3 2 .4 3 ; - 3 .1 ± 8 4 2 .2 4 ; -0 .0 0 3 6 ± 20.54; - 0 . 0 5 ± 8 .7 2 ;

. 1 (Imag) 3 (Imag) 5 (Imag) 7 (Imag)

2 (Imag) 4 (Imag) 6 (Imag) . . 8 (Imag)
4000 40

3000

2000

1000

0
0.5
.( (s )
-1000 •10
-2000 -20

-3000 -30

-4000 -JO

Figure 6.1: Eigenvalues of System .4

6 .4 .2 C onfiguration D ep en d en t E igen valu es

We now present some sample results obtained for th e planar dual-arm architecture
used in the previous chapter (F R R -O -R R F). As before, the trajectory specified for the
end-effectors is to rotate the object about its mass center through 90° in 2 seconds.
For systems .4 and C. the two arms are not carrying the object but are required
to follow the same trajectory as if they were handling a common o b ject. Since the
flexible link in each arm is modeled with two planar beam elem ents and hence 4
elastic degrees of freedom in total, the number of eigenvalues of the plant matrices is
S. The eigenvalues of the undam ped system are calculated along th e tra je cto ry and
shown for systems A, C and B in Figure 6 .1-6.3 respectively.
 C H A P T E R 6. S T A B IL IT Y APPROACH 158

Table 6.2: Stable Eigenvalues of A c

Eigenvalues ■^1,2 -^3,4 4s ,6 -^7,8

C= o ±5275.49; ±2162.64; ±582.75; ±77.11;
C = 0.5% -.321.0 ± 5753.35; -58.5 ±2162.84; -5 .5 ± 582.81; -0 .0 5 ± 77.11;

For sy stem A. where the actuators and the specified trajectories are collocated
at the joints, th e eigenvalues of the undamped system are purely imaginary since in
eq. (6.28), th e mass and stiffness matrices are sym m etric and positive definite. Indeed,
the eigenvalues A of the plant matrix are related to the natural frequencies of the arm
by:
Xj = — f = I. • • •. S' j = {2i — I). 2i (6.36)

where u,', > 0 is the ith modal frequency and S is the total number of elastic co­
ordinates used to model the flexible links of the arm. It is also known that when
dam ping is included, the eigenvalues are shifted to the left half of the complex plane
since the d am p ing m atrix is always positive definite. Table 6.1 compares the
eigenvalues of the plant m atrix A 4 at the initial configuration, with and without
dam ping. We therefore conclude that system .4 is always stable since this is ensured
by the aforem entioned properties of the system matrices. Figu re 6 .1 also shows that
the eigenvalues vary little with the rigid configurations.

F ig u re 6.2 illustrates the eigenvalues for plant C (eq. (6.29)) — a system where
actuators and the specified trajectories are collocated at the tip of the end-effector.
Interestingly, we have verified numerically that m atrix M^pr possesses similar proper­
ties to th e mass m atrix M ppr. in particular, it is also symmetric and positive definite.
T he sam e conclusion as for plant .4 can be drawn for the system with collocation
at the tip: th e nonlinear equations of motion in the region of equilibrium points are
stable (See also Table 6.2).
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 159

1 (Imag) 3 (Imag) _ 5 (Imag) 7 (Imag)

2 (Imag) . . . 4 (Imag) . 6 (Imag) . . . 8 (Imag)
6000 600

4000 400

2000 200

0 0
0.5 I 1.5 2 0.5 1 1.5 2
f(s) ....................... i ( s )
-2000 -200

-4000 -400

-6000 -600

Figure 6 .2 : Eigenvalues of System C

Figure 6.3 illustrates the eigenvalues of the plant m atrix A g . The linearized model
for system B is characterized by A g if the tip wrenches are independent of the
elastic state variables (q^^. since then the second term on the right hand side

of eq. (6.27) vanishes. From Figure6.3. we conclude that if the tip wrenches aren't
chosen to vary with the elastic state variables, the internal dynamics at equilibrium
points are unstable because of the eigenvalues in the right half s-plane. We also note
from Figures 6.3 and 6 .2 th a t the eigenvalues of the corresponding linearised systems
are nearly constant with the rigid configuration.

6 .4 .3 C han gin g th e Linear P lan t o f S y stem B by Force D is­

trib u tio n S ch em es

In chapter 5. we discussed and demonstrated the performance of various optim ization

schemes used for force distribution to determine the tip wrenches. .As it turns out.
some of the schemes are not effective for flexible cooperating manipulator systems.
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 160

. 1 (Real) 3 (Real) 5 (Real)

.2 (Real) . . . 4 (Real) 6 (Real)
4000 750

3000
500

2000

250
1000

0
Ij OJ
-1000
t(s) M s)
-250

-2000

•500
-3000

-4000 -750

Figure 6.3: Eigenvalues of System B

For example, a scheme which only considers minimizing the internal forces produces

unbounded inverse dynamics solutions. We have also shown th a t some o th e r schemes

designed to account for the dynam ics variables of the m anipulators, yield bounded
solutions. This motivated us to consider the following question. W hat is the connec­
tion between the behavior of the inverse dynamics solution and the stability of the
linearized system? To look for th e answer, we consider how the tip wrenches produced
by different optimization schemes affect the plant m atrix of the linear system . Let
the tip wrenches be a function of y . and therefore

at": = (6.37)

Substituting the above into eq. (6.31). the linearized model for a real situ atio n of
multiple-arm and object system is characterized by the following plant m atrix:

A =- A s -h B- i 6.38 )

In chapter .5. the tip wrenches are obtained as the o ptim al solution of the linearly-
constrained quadratic optimization problem. To find out how the tip wrenches
C H A P T E R 6. STABILITY APPROACH 161

are related to th e internal state variables y , we repeat eq, (Ô.9 ).

ti;‘ = W - ‘ a [ ( A i W - ' A f ) - ' ( A i W ~ ' c + b j - W ‘c

where A i is the grasp matrix. Since m atrix A i is not related to th e elastic variables
of th e m anipulators, the dependence of id* on y can only be due to th e weighting
m atrix W and th e column m atrix c. In the m inim um internal force scheme. W is
an identity m a trix while c is a zero vector. From eq. (6.38), th e plant m a trix in this
case is equal to A g which has previously been d e m o n strated to be unstable, thereby
explaining why this approach yields unbounded solutions. It is evident th a t in order
to improve the behavior of the inverse dynamics system , th e tip wrenches m ust be
chosen to be functions of the elastic variables.

Let us now consider the optimal load sharing scheme for which the objective
function is characterized by (eq. (-5.22)):

CT = J i v W r - r ', Wr =

where 'Wj- is a constant matrix. By ignoring the dependence of on as was

done in deriving the linear internal dynamics equations, the partial derivative of
with respect to y is obtained as

= ( W T - 'A [ ( A ,W T - 'A [ ) - 'A .- l ) W r - '$ :

= A w ^ (6.39)

Eq. (3.-52) for calculating for a multiple-arm system can be rew ritten as:

V = —MrsvM7ç‘^ (K eer -f Ççv ) + gv

where includes the nonlinear terms of 0 (||g,.^||^). and 0 ( ||?gr II") As well as the
coupling term s of (9(||g,.^ ^evID- Taking the partial derivative of with respect to
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 162

y , eq. (6.39) becomes:

g y ' — -A-VV (6.40)

Substituting the above into eq. (6.27), the plant m atrix of the noncollocated multiple-
arm inverse system becomes:

A = As + B ^

0
= As + A ^ / J l r W^T-M «ev

~(1 + AwJlv WxMrer )Mgg^Kger ~ (1 + A»’Jl£ WjMrev )M^^V Dfc£

(6.41:

The above equation can be written com pactly a.s

A = Ar As (6.42)

where
^ 1 0
Ar = (6.43)
0 1 + J Ah' Jiv; W r M r t v
T h e plant m atrix in eq. (6.42) represents th a t for the linearized model of the internal
dynamics when optim al load sharing scheme is applied to determine the grasping
wrenches within the system.

.As shown in Example 5.3. we successfully implemented a torque shifting scheme

for the dual-arm system with one arm flexible and the other rigid. This scheme
minimizes the norm of actuator torques of the flexible arm. The idea underlying the
torque shifting scheme is that it emulates a collocated situation where the flexible
arm is driven by the tip wrenches (applied through the object by the rigid arm).
While we were unable to establish analytically a relationship between the eigenvalues
CHAPTERS. STA B IL ITY APPROACH 163

of the plant m atrix in eq. (6.42) for this scheme and for the collocated system A c .
numerical experiments revealed some interesting results. One interesting observation
was that the eigenvalues of the two plants were identical. However, based on this
special case, we cannot draw a general conclusion th a t one can predict the behaviour
of the nonlinear system from the eigenvalues of the linear plant m atrix.

Table 6.3 lists the eigenvalues of the plant m atrix of eq. (6.42) calculated for two
load sharing schemes and different mechanical setups of the flexible dual-arm system.
Since the eigenvalues do not change significantly along the trajectory, we include

here sample calculations at the beginning of the trajectory only. From Table 6.3.
we conclude th at minimizing t ^ T i is effective for one flexible and one rigid arm
but becomes ineffective if both arms are flexible. However, minimizing gives
unstable eigenvalues for all three setups.

In the same way. we can formulate the plant matrix which corresponds to the
optimization scheme which minimizes the norm of the elastic accelerations. Towards
this end. we first express w ' as;
<y

- An (6.44)

Then, the corresponding plant m atrix is obtained as

A = AgAg (6.4-5)

where
1 0
Ag : (6.46)
0 1 -f A u Jav W g

•Similarly, the scheme of minimum strain energy yields a plant m atrix

A = A fA g (6.47)
 C H A P T E R 6. S T A B I L I T Y A P P R O A C H 164

Table 6.3: Eigenvalues of A with M inimum Norm of Torque Scheme

Mechanical Setup
Optim ization F R R -O -R R R R FR -O -R R R F R R -O -R R F
Scheme A rsxs A rs x s A t 16x16
±3958.27
±2039.26
±37.52.49; ±5734.24; ±710.94
±2161.64; ±2165.48; ±582.74
min t J r .
±•582.74; ±•595.77; -3 .1 X 1 0 -'- ±2161.64;
±77.11; ±124.84; - 2 .1 X 10-9 ±5732.49;
±77.11;
±64.39;
±3958.0
2326.1 ±2211.9;
2741.0 ±13.52.4; 1680.3 ±2351.0; -232 6.1 ±2211.9;
-2741.0 ±13.52.4; -1680.3 ±2351.0; ±2059.6
min
±489.6 ±383.0 ±710.8
±64..5; ±49.6; ±229.1
±57.7;
±64.3;
 C H A P T E R 6. S T A B I L I T Y ' A P P R O A C H 165

Table 6.4: Eigenvalues for Various O ptim ization Schemes

Objective Mechanical Setup

Function F R R -O -R R R R FR -O -R R R FR R -O -R R F F F R -O -R R R
56.5 1178.1 0 .1 4.0 X 10-8
U ( A t = 0.0002) 0.77 59.8 0 .1 5.7 X 10-1°
V* t *• y *•
ry T s + q iq . 0.25 0 .1 2 1.96 0 .6
r i Ti -f- if 1.7 X 10-8 0 .0 1424.1 ± 1994.1; 1.5 X 10-'

with
^ 1 0
(6.48)
0 1 + A w J3vK „v

We note in the above, th a t the plant m atrix A in eq. (6.47) depends on the value
of A /. Recall that A i is used in the minimum strain energy scheme to approxim ate
elastic coordinates in term s of velocities and accelerations at the previous step. For

a be tte r approxim ation, one would expect th a t A i should be as small as possible

within the requirements of computational efficiency. However, from eq. (6.48) we

observe th a t as A i becomes very small, the m atrix A c approaches identity and the
plant m atrix A reduces to A g which in turn is unstable. Indeed, this phenomenon
was verified numerically when we implemented the scheme of m inim um strain energy
— decreasing A i below a certain value would result in unbounded solutions. Based
on our experience with the simulations, it is found A i = 0.0002 sec is a suitable value
in most of the cases.

T he eigenvalues of the plant matrices in eq. (6.45) and eq. (6.47) have been calcu­
lated for various mechanical setups and for several of the optim ization schemes with
which inverse dynamics solutions were a ttem p ted . In Table 6.4. we present for each
case only the eigenvalue which has the largest real part. We shall adopt the term
almost stable [75] to refer to the eigenvalues with a very small positive real part. In
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 166

this case, the internal dynamics is said to be very lightly unstable and a linear stab i­

lizer can still be applied to obtain a stable closed-loop behavior [75]. From Table 6.4.
we observe th a t minimizing a combined objective of th e total strain energy and the
norm of the actuator torques of the left arm ( r [ r i -f 6 ') gives stable or almost stable

behavior of the linearized system except for the setup where both arm s have th e base
link flexible. This is predictable since minimizing th e torque in one arm results in
a heavier burden on the other arm which also has a flexible link. Very likely, the
elastic vibrations are excited first in the arm whose actu ato rs are required to supply

the m ajority of the torques which the system needs. Although our previous findings
indicated th a t minimizing the norm of the actuator torques alone (min. r ^ r ^ ) was
not feasible, the results in Table 6.4 show th at minimizing the combination of

and produces an almost stable linearized internal dynamics. Furtherm ore,

minimizing the strain energy H has b e tte r performance than minimizing the norm of
elastic accelerations This is in complete accordance with our conclusions in
chapter 5.

6.5 S ta b ility A p p ro a ch

The stability analysis based on the eigenvalues of the plant matrices of the linearised

models shows some agreement with the conclusions of other researchers and those
drawn from the numerical results in chapter 5. These can be sum m arized as follows:

• Collocation at either the joints (system .4) or at the tip (system C) of a serial

flexible manipulator ensures a stable behavior of th e nonlinear internal dynam ics

at the equilibrium points.

• N'oncollocation of the joint actuators with the specified tip trajectory (system
B) results in unstable behavior of the internal dynamics.
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 167

For cooperating flexible manipulators, stability properties of the system can

be modified by optimizing the force distribution. This essentially allows us to

choose the relationship between the tip wrenches and the internal s ta te y .

• Depending on the optimization scheme used, the force distribution solution may
produce a stable or unstable behavior of the system. Agreement in stability
characteristics has been dem onstrated between nonlinear simulation and the
eigenvalues of the plant matrices of the linearized system.

• Various mechanical arrangements of flexible and rigid links produce responses

with different stability characteristics. Again agreement has been d e m o n strated
between directly solving the nonlinear equations and calculating the eigenvalues
of the linear plant matrix. For a manipulator with only one flexible link, b etter
behavior is obtained when this link is located closer to the base.

• .Although the internal dynamics is defined as the dynamics of the internal states
when both input and output are set to zero, the stability of the internal d y ­
namics around equilibrium still allows us to predict the behavior of the inverse
dynamics system along a nominal trajectory. Once again, this is su p p o rte d by
the agreement we obtained between the inverse dynamics solution via o p tim iz a ­
tion approach and the stability analysis of the corresponding linear system .

Motivated by the above, we set out to find a force distribution scheme which
would allow the tip wrench to be dependent on the states so as to give a stable
plant m atrix of the linearised system.
CHAPTERS. STABILITY APPROACH 168

6.5.1 C hanging th e P la n t M atrix

Recall from chapter 4, th a t the grasping wrenches can be calculated ais

tܣ = A ^ b i + A f z (6.49)

where the arbitrarily chosen vector z in the null space of A , represents the degrees
of freedom in determining the grasping wrenches. Based on the stability analysis, it
is apparent that it would be desirable to let the tip wrenches vary in a way which
forces the plant m atrix of the m a n ip u la to r system to be equal to the plant m atrix of
the system with tip collocation. This requires th at we set:

A = A c = Ag + (6.50)

From eq. (6.49), the partial derivative of tUr with respect to y should satisfy

^ = A -;"§ = A r K (6.511

where K is an arbitrary m atrix of dimension 6 P x W ith this approach, we

essentially a ttem p t to vary the internal force in the multiple arm system in accordance

with the internal state variables y. Substituting eq. (6.51) into eq. (6.50). we have

Ac = Ag 4- B A'^ K = A g 4 - B 'K (6.52)

Solving for K from the above and replacing A g and A c with eq. (6.34) and eq. (6.35)
respectively leads to:

K = (J^A -^)+ G (6.53)

wit 11
G = - ( M -^ - M - ^ )K ,.r - ( M -^ - M - ^ ) D ,e V

Substituting eq.(6.53) back into eq.(6.49) gives the following solution for w ^ :

= Ai*'bi 4- A -'(Jj^A )^ )'*‘G y (6.55 )

CHAPTERS. ST A B IL IT Y APPROACH 169

with y = [gjj.

W hether the above grasping wrenches will guarantee a s tab le solution of the in­
verse dynamics depends on the properties of m atrix . Here. is of dimension

X 6 P where recall 5, denotes the to tal num ber of elastic coordinates of arm
i. T he dimension of is 6 P x dim jV (A i). It is also known th a t dim A'( A; ). the
dimension of the null space of A ; is equal to the order of the a c tu a tio n redundancy
of the system. Therefore, the rank of JJ^A','' must satisfy the inequality below:

ran/:( J jj. Af* ) < dim A'( A ; )) (6.36)

.Assuming, as is usually the case, th a t A{^ is not rank deficient, we can define the
following cases:

• = dim -V( A i )
In this case. ( A'^ A'^ )"^ and eq.(6.33) has a unique solution for
tüj- which satisfies the requirements of both the prim ary task (handling the
common object) and secondary task (stabilizing the elastic motions).

• Tfl,.S’, < d im .V (A i)
When the order of actuation redundancy is greater than th e num b er of elastic
coordinates in the system, there is an infinite num ber of choices for K which
will produce the plant m atrix A equal to A c- The e x tra degrees of freedom in
choosing K can be further used to accomplish o ther tasks, such as. minimizing
the actuator torques or the elastic deformations.

• T/Li-h’. > d im .V (A i)
This is the case when the degree of a ctuation redundancy is insufficient to alter
the plant m atrix of the linearized model exactly as desired. Instead. eq.( 6 .3 3 )
will produce a close (in the least-square sense) approxim ation of A to A c- This,

however, does not guarantee a stable behavior of the elastic motion.

 C H A P T E R 6. S T A B I L I T Y A P P R O A C H 170

E x a m p le 6 .1 — Varying th e plant matrix

VVe now implement th e force distribution scheme of eq. (6-55) to solve the inverse
dynamics problem. We consider the planar dual-arm example (R FR -O -R R R ), first

introduced in section 4.5.1 to demonstrate its performance. The elastic link is modeled
with one beam element which allows considering stretching and bending deformations.
This gives a total of 3 elastic coordinates. The sam e trajectory profile as described
in section 4.5.1 (rotating the object for 90° in 2 seconds) is employed to specify the
object trajectory. .A.s mentioned earlier, the degrees of actuation redundancy for a
planar dual-arm is 3 which is the same as the total number of elastic coordinates.
From the aforementioned conditions, it is concluded th a t there exists a unique solution
for grasping wrenches which will generate the plant m atrix of the linearized elastic
motion identical to th a t of a collocated system C. The solutions for actuator torques,
the elastic coordinates and the tip moment are shown in Figures 6.4 and 6.5. From
Figure 6.4. we observe th a t in the present case, the actuators of the rigid arm (right

column in the figure) contribute more to moving the object th at those of the flexible
arm . The sam e conclusion can be made by comparing (5a) with (5b) in Figure6.5 for
the tip m om ent. This means that in order for the joint actuated flexible arm to have
the same behavior as th a t of the tip collocated system, driving forces for controlling
th e flexible arm must be shifted towards the end-effector of the flexible arm. Plot
(5f) of figure 6.5 shows the actual trajectory of the object generated by calculated
a ctu a to r torques.
C H A P T E R 6. S T A B I L I T Y A P P R O A C H 171

0.5 IJ 0.5
r(s) M S)
• 10

.10 -20

• 15 -30

(4a) (4d)

0
0 5 0.5
/(S)

-10

•2

-20
3

(4b) (4e)

0-5

0.5 05
r(s)
•0 5 -10

•20

30

(4c) (4 0

Figure 6.4: A ctuator Torques via Stability Approach

C H A P T E R 6. S T A B I L I T Y A P P R O A C H 172

Ks)
10

•20

•30

(5a) (5b)

0015

3
0.01

H 0.005
I
0
05
/(S)

0005

•2
•0 01
3

(5 0 (5d)

0 015

40
001

& 0,005

0 5 0.5
• 10

0005
■20

•30

Mi
■0015 •50

(5e) (5 0

Figure 6.5: Stable Elastic .Motion via S tab ility .Approach

 173

C h a p ter 7

C on clu sion s

T he present work has focused on investigating the inverse dynamics solutions for
m ultiple cooperating manipulators with flexible links. .Although extensive studies
have been carried out in the past on the rigid cooperating manipulators, little has
been done to account for the s tru c tu ra l flexibility in the m anipulators. Including link

flexibility in the analysis is not a trivial extension of the case for a rigid m u ltip le arm
system. In fact, the key issues which motivated this study were the com plications
introduced by the structural flexibility in the inverse dynamics problem and th e ways
in which redundant actuation could be used to improve the dynamics behavior of the
system.

We began by modeling the dynam ics of the system. These dynamics are com posed
of two parts: that of the m anipulators and that of the commonly grasped o b je ct. The
dynam ics equations of the m anipulators are formulated in terms of the generalized
coordinates for every serial chain while the coupling effects between the serial arm s
are represented by the grasping wrenches applied to the object. The rigid a nd elastic
.Jacobian matrices are derived which transform the Cartesian space tip wrenches into
the generalized forces associated with the generalized coordinates.
C H A P T E R T. C O N C L U S I O N S 174

To gain a thorough understanding of the inverse dynamics problem in the presence

of link flexibility, the analytical approach to the inverse dynamics problem of a flexible
single-link arm was formulated. Both the state space description and the in p u t-o u tp u t
relation are derived for a flexible beam. T he latter was used to identify th e issue of

causality and the resulting the instability of the causal inverse dynamics solution.
Correlation between the instability and the collocation of th e input and o u tp u t was
dem onstrated by expressing the dependency of the nonm inim um phase zeros of the
forward system on the location of th e prescribed trajectory relative to the joint. Upon

comparing the advantages and disadvantages of the noncausal and causal solutions,
and in view of the potential of a multiple arm system to improve the causal behavior,
the causal solution for the inverse dynamics of the multiple flexible arm system was
adopted in this study.

In chapter 4. the inverse dynamics solution is formulated for a m ultiple flexible

arm system. In particular, the inverse dynamics torques as well as the rigid and elastic
accelerations are found to be a combination of those required to achieve a prescribed
object trajectory and those associated with the required tip wrenches, which in turn
are determ ined by a force distribution scheme. The underdeterm inacy of the inverse
dynamics problem is revealed by comparing the number of unknowns with th e num ber
of equations used to obtain th e solution. It is also dem onstrated, for kinematically
non-redundant manipulators rigidly grasping the object, th a t the degree of redundant
actuation is equal to the degree of freedom in choosing the tip wrenches as well as
the dimension of the internal force vector.

Therefore, the inverse dynam ics problem which determines the a ctu a to r torques
necessary to realize a desired motion of the object in a multiple arm system adm its
an infinite num ber of solutions. Unlike the rigid case, different solutions affect the
dynamics behaviors of the arm s due to the flexibility of the arms. T he problem is
C H A P T E R 7. C O NC LU SIO N S 175

then formulated as a linearly constrained optim ization problem by choosing the tip

wrenches as the design variables. Q uadratic program m ing is employed to solve the
resulting optimization problem in light of its simplicity, uniqueness of the optim al so­
lution and computational efficiency. Besides the objectives commonly used for rigid
cooperating manipulators, special a tten tio n is given to improving the dynamics be­
havior of the flexible manipulator system , in particular, to reducing and stabilizing
the system vibrations. We illustrate with various numerical examples th a t the mini­
mum strain energy, minimum weighted norm of elastic accelerations and the optimal
load sharing schemes are effective in achieving these goals.

Stability of the internal dynam ics of a inverse dynamics system is then investigated
in chapter 6 . The internal dynam ics is described by the equations of elastic motion
in the absence of both input (tip trajecto ry) and the o u tp u t (joint actuato r torques).

Linearization was used to a p proxim ate the state space trajectories around equilibrium
points. In many cases, the stability status of the nonlinear internal dynamics in
the neighbourhood of equilibrium points can be determ ined by the eigenvalues of
the plant m atrix of the linearized system. In fact, agreement in the behavior of
the inverse dynamics system was obtained between directly solving the nonlinear
dynamics equations and by calculating the eigenvalues of the linear plant matrix.
The stability analysis helped to explain why some of the optimization schemes are
not suitable for flexible multiple-arm systems, for example, minimizing the internal
force alone cannot be used as the objective of the optim ization problem. In practice,
calculating the eigenvalues of th e plant m atrix of the linearized internal dynamics
allows us to predict the behavior of th e nonlinear inverse dynamics system in response
to various force distribution schemes.

In view of the application of th e inverse dynamics solution in control of nonlinear

dynamics systems, a stability approach was proposed to actively distribute grasping
C H A P T E R 7. C O NC LUS ION S 176

wrenches in such a way th at the plant matrix of the linearized internal dynam ics is
stable. T h e idea here is that tip wrenches should vary in accordance with the sta te s of
the internal dynamics. In the case where the degree of actuation redundancy is equal
to or larger th an the total number of elastic coordinates, stable internal dynam ics can
be ensured by the stability approach. Numerical examples were used to illu strate the
performance of the stability approach.

D irection s for Future Work

It is known th a t for a rigid cooperating m anipulator system , actuation redundancy

resolution is formulated as a local optimization problem. The solution for forces and
moments at / = /i does not affect the solution at f By contrast, introducing
link flexibility causes a drastic change in the characteristics of the force o ptim ization
problem. Because of the different behaviors of the internal dynamics in response to
different sets of forces and moments (the grasping wrenches), the actuation redu n­
dancy resolution is no longer local in nature. Determ ination of the grasping wrenches
a.t t = ti influences the solution of force optimization problem at t > f T h e r e f o r e ,
force optim ization problem for flexible cooperating m anipulator systems should be
further investigated by formulating it as a global optim ization problem, th a t is. with
a cost function optimized over the complete trajectory of the object.

Possible selections of the cost function would be the elastic deflection of the system
over the whole trajectory which would serve as a criterion for minimum excitation
of the system vibration. It can also be viewed as a means of avoiding u nstable in­
verse dynamics solution. It would also be interesting to compare the global force
optimization approach with Bayo's noncausal inverse dynamics solution and to de­
term ine if there is any correlation between the two approaches. From the practical
standpoint, it would be significant to determine if there exists a local o p tim izatio n
 C H A P T E R 7. CONCLUSIONS 177

approach which approximates the solution to the global optim ization problem in or­
der to combine the merits of the global solution with the com putational simplicity of
the local optim ization.

To d em o nstrate the significance of the proposed inverse dynamics solution for

flexible-link cooperating m anipulator systems, it would also be worthwhile to extend

the present work to the control of a flexible-link cooperating m anipulator system.

Especially relevant is the application of the stability approach presented in chapter 6 .
Performance of this approach can be better illustrated by implementing it together
with a robust controller which guarantees th at th e states of the system stay in a
region close to the equilibrium point. Further developm ent of this approach would
also consider cases where the degree of actuation red un d an cy is less than the num ber
of elastic coordinates.

.'Vnother extension of the present work is to investigate cases where joint flexibil­
ity is significant. .Justification for considering the stru c tu ra l flexibility in either the
m anipulator links or the joints or both lies in the relevant applications. For instance,
in space applications, link flexibility is dominant because of the requirement for long
and slender m anipulator structure. By contrast, in most industrial m anipulators,
joint flexibility is more prominent because of the elastic transmission components
and because the links of industrial manipulators tend to be heavy and stiff. However,
when m anipulators are required to move at fast speeds, both joint and link flexibil­

ities could significantly affect the performance of th e system . .A.It hough control of
cooperating manipulators with flexible joints has been tackled by some researchers
[76. 77]. consideration of both joint and link flexibility in the m anipulators has been
left un ad dressed.
 178

R eferen ces

[1] W. J. Book. Controlled motion in an elastic world. Journal o f D ynamic Systems.

Measurement, and Control, 115:252-261. 1993.

[2] A. .J. Koivo and G. .A. Bekey. Report of workshop on coordinated multiple robot
m anipulators: Planning, control, and applications. I E E E Journal o f Robotics
and Automation. 4(l):91-93, February 1988.

[3] A. R. Fraser and R. VV. Daniel. Perturbation Techniques f o r Flexible manipula­

tors. Kluwer .Academic Publishers. 1991.

[4] Ping Hsu. Control of multi-manipulator systems - tra je c to ry tracking, load

distribution, internal force control, and decentralized architecture. In Proceedings
o f IE E E Inttem ational Conjerence on Robotics and .Automation, volume 2. pages
1234-1239. 1989.

[5] 0 . .Al-.Jarrah. Y. F. Zheng, and K-Y. Yi. Efficient tra je cto ry planning for two
m anipulators to deform flexible materials with experim ents. In Proceedings 199.5
IE E E International Conference on Robotics and A utom ation, volume 1 . pages
312-317. Nagoya, .Aichi. Japan. May 1995.

[6 ] M. T. Mason and J. K. Salisbury. Robot Hands and the Mechanics o f .Manipula­

tion. MIT Press. 1985.

[7] Zexiang Li. Ping Hsu. and Shankar Sas try. Grasping and coordinated m an ip­
ulation by a multifingered robot hand. The International Journal o f Robotics
Research. 8(4):33-49. 1989.

[S] \ . Nakamura. .Advanced Robotics: Redundancy and Optimization. .Addison-

VV’elsley Publishing Company. 1991.

[9] Y. F. Zheng and J. V. S. Luh. Optimal load distribution for two industrial
robots handling a single object. In Proceedings o f the 198S IE E E International
Conference on Robotics and Automation, pages 344-349. Philadelphia. Pa.. 1988.
REFERENCES 179

[10] T. E. Alberts and D. I. Soloway. Force control of a m ulti-arm robot system.

In Proceedings o f the 1988 IE E E International Conference on Robotics and A u ­
tomation. pages 1490-1496, Philadelphia, Pa, 1988.

[ 1 1 ] K. Kreutz and A. Lokshin. Load balancing and closed chain m ultiple arm control.
In Proceedings o f the 1988 American Control Conference, pages 2148-2155, 1988.

[12] VV. S. Lu and Q. H. M. Meng. An improved load distribution scheme for coor­
dinating m anipulators. In Proceedings 1993 IE E E International Conference on
Robotics and Automation, pages 523-528, Atlanta, Georgia, May 1993.

[13] D. E. Orin and S. Y. Oh. Control of force distribution in robotic mechanisms

containing closed kinematic chains. Journal o f Dynamic System s, Measurement,
and Control. 1G3(2):134-141, 1981.

[14] M. Nahon and .1. Angeles. Real-time force optimization in parallel kinematic
chains under inequality constraints. IE E E Transactions on Robotics and A u ­
tomation. S(4);439-450. 1992.

[15] V. Nakamura. K. .Nagai. and T. Yoshikawa. Mechanics of coordinative m anip u­

lation by multiple robotic mechanisms. In Proceedings 1987 I E E E International
Conference on Robotics and Automation, pages 991-998. Raleigh. 1987.

[16] .1. Y. S. Luh and Y. F. Zheng. Load distribution between two coordinating
robots by nonlinear programming. In Proceedings o f the 1988 Am erican Control
Conference, pages 479-482, Atlanta. Georgia. June 1988.

[17] M. .Nahon and J.Angeles. Minimization of power losses in cooperating m anip­

ulators. Journal o f Dynamic Systems. Measurement, and Control. 114:213-219.
1992.

[IS] Q. Sun. M. .Nahon. and I. Sharf. Force optimization in m ulti-arm ed m anipulators

with flexible links. In Intelligent Automation and Soft Computing, pages 183-187.
Hawaii. August 1994.

[19] Q. Sun. 1. Sharf. and M. .Nahon. .A stable solution for the inverse dynamics
problem of Hexible-link cooperating manipulators. In Proceedings o f the loth
Canadian Congress o f Applied Mechanics, volume 1. pages 798-799. Victoria.
B.C., .May 1995.

[20] R. H. Cannon and E. Schmitz. Initial experiments on the end-point control of a

flexible one-link robot. The International Journal o f Robotics Research. 3(3):62-
75. 1984.
REFERENCES 180

[2 1 ] E. Bayo. A finite-element approach to control the end-point motion of a single­

link flexible robot. Journal o f Robotic Systems, 4:63-75, 1987.

[22] J.-H. Park and H. Asada. Dynamic analysis of noncollocated flexible arms and
design of torque transmission mechanisms. Journal o f D ynam ic Systems. Mea­
surement. and Control, 116:201-207, 1994.

[23] J. W ittenburg. D ynam ics o f System s o f Rigid Bodies. B. G. Teubner S tu ttg art.
1977.

[24] .A. A. Shabana. D ynamics o f Multibody Systems. John Wiley & Sons. 1989.

[25] E. Bayo. J. G. de Jalon, and M. Serna. A modified lagrangian formulation for

the dynamic analysis of constrained mechanical systems. C om pu ter Methods in
.Applied Mechanics and Engineering, 71:183-195, 1988.

[26] J. Y. S. Luh and Y. F. Zheng. C om putation of input generalized forces for

robots with closed-kinematic chain mechanisms. IE E E Journal o f Robotics and
.Automation. RA-a(2):95-103, Jun e 1985.

[27] Y. Nakamura and M. Ghodoussi. Dynamics computation of closed-link robot

mechanisms with nonredundant and redundant actuators. IE E E Transactions
on Robotics and .Automation. 5(3):294-302. June 1989.

[28] S. S. Kim and E. J. Haug. .\ recursive formulation for flexible multi body d y ­
namics, part ii: Closed-loop system. Computer Methods in .Applied .Mechanics
and Engineering. 74:251-269, 1989.

[29] E. Bayo, P. Papadopoulos, J. Stubbe. and M. .A.. Serna. Inverse dynamics and
kinematics of multi-link elastic robots: .An iterative frequency dom ain approach.
The International Journal o f Robotics Research. S(6):49-62. 1989.

[30] R. J. Williams and .A. Seireg. Interactive modeling and analysis of open or closed
loop dynamic systems with redundant actuators. Journal o f .Mechanical Design.
101:407-416, J u ly 1979.

[31] S. Hayati. Hybrid position/force control of multi-arm cooperating robots. In

Proceedings 1986 IE E E International Conference on Robotics and .Automation.
pages 82-89. San Fransisco, .April 1986.

[32] Y. Nakamura. Minimizing object strain energy for coordination of multiple

robotic m anipulators. In Proceedings o f the 1988 American Control Conference.
pages 499-504. 1988.
REFERENCES 181

[33] C. D. Kopf. Two-a rm hybrid position/force control with dynam ic com pensa­
tion. In Proceedings o f the Second International Symposium on Robotics and
Manufacturing, pages 371-381, A lbuquerque, NM, 1988.

[34] Dong-Soo Kwon and Wayne J. Book. .A time-domain inverse dynam ic track ­
ing control of a single-link flexible m anip ulator. Journal o f D ynam ic Systems.
Measurement, and Control, 116:193-200, J u n e 1994.

[35] H. Asada, Z.-D. Ma, and H. Tokum aru. Inverse dynamics of flexible robot arms:
Modeling and co m putation for tra je cto ry control. Journal o f D ynam ic Systems.
Measurement, and Control, 112:177-185, J u n e 1990.

[36] Liang-Wey C hang and J. F. Hamilton. T h e kinematics of robotic m anipulators

with flexible links using an equivalent rigid link system (ERLS) model. Journal
o f Dynamic Systems. Measurement, and Control, 113:48-52. March 1991.

[37] F. Xi and R. G. Fenton. A sequential integration method for inverse dynam ic

analysis of flexible link m anipulators. Proceedings 1993 IE E E International Con­
ference on Robotics and .Automation. 3:473-478. May 1993.

[38] P. C. Hughes and 0 . B. Sincarsin. Dynam ics of an elastic m ultibody chain:

Part .A - bodv motion equations. D ynam ics and Stability o f Systems. 4(3 &'
4):209-226. 1989.

[39] W. J. Book. Recursive lagrangian dynam ics of flexible m anip ulator arms. The
International Journal o f Robotics Research, 3(3):87-101. 1984.

[40] P. C. Hughes and G. B. Sincarsin. D ynamics of an elastic m ultibody chain: Part

B - global dynamics. Dynamics and Stability o f Systems, 4(3 & 4):227-244. 1989.

[41] T. Yoshikawa. Foundation o f Robotics — Analysis and Control. Corona P u b ­

lishing Co. Ltd.. 1988.

[12] W. Sunada and S. Dubowsky. T he application of finite element m ethods to the

dynamic analysis of flexible spatial and co-planar linkage systems. Journal o f
Mechanical Design. 103:643-651. .July 1981.

[43] D. Wang and .VI. Vidyasagar. Modeling of a 5-bar-linkage m an ipu lator with one
flexible link. In Proceedings 1988 IE E E International Conference on Robotic.-^
and .Automation, volume 1. pages 21-26. 1988.

[44] .A. .A. Shabana. Dynamics of flexible bodies using generalized newton-euler eq u a­
tions. Journal o f Dynamic Systems. .Measurement, and Control. 112(3):496-503.
1990.
REFERENCES 182

[45] C. Damaren and I. Sharf. Simulation of flexible-link m anip u lato rs with inertial
and geometric nonlinearities. Journal o f Dynamic Systems, Measurement, and
Control, 117:74-87. March 1995.

[46] B. Gebler. Feed-forward control, strategy for an industrial robot w ith elastic links
and joints. In Proceedings 1987 IE E E International Conference on Robotics and
Automation, pages 923-928, Raleigh, NC., April 1987.

[47] C.-H. Menq and .J. Chen. Precision tracking control of discrete tim e
nonminimum-phase systems. In Proceedings o f the 1992 A m erica n Control Con­
ference, pages 1097-1101, Chicago, II, .June 1992.

[48] S. N. Singh and .A. A. Schy. Decomposition and state variable feedback control of
elastic robotic systems. In Proceedings o f the 1985 American C ontrol Conference.
pages 375-380, Boston, MA, .June 1985.

[49] D. Wang and M. Vidyasagar. Control of a class of m an ipu lato rs with a single
flexible link — part I: Feedback linearization. Journal o f D y n a m ic Systems.
.Measurement, and Control, 113:655-661. December 1991.

[50] D. Chen. An iterative solution to stable inversion of nonm inim um phase systems.
In Proceedings o f the 1993 .American Control Conference, pages 2960-2964. San
Francisco. CA. .June 1993.

[51] \ . Stepanenko and M. Vukovratovic. Dynamics of a rticulated open-chain active

mechanisms. .Mathematical Biosciences. 28:137-170. 1976.

[52] D. S. Kwon and VV. .J. Book. An inverse dynamic m e th o d yielding flexible
manipulator state trajectories. In Proceedings o f .American C ontrol Conference.
volume 1 . pages 186-193. 1990.

[53] VV. Gawronski. C.-H. C. Ih, and S. .J. Wang. On dynamics a n d control of m u lti­
link flexible manipulators. Journal o f Dynamic Systems. M ea su rem ent. and Con­
trol. 117:134-142. .June 1995.

[54] V. .A. Spector and H. Spector. Modeling and design implications of noncollocated
control in flexible systems. Journal o f Dynamic Systems. .Measurement, and
Control. 112:186-193. 1990.

[55] D. S. Wang, G.-B. Vang, and M. Donath. Non-collocated flexible beam motion
control based on a delayed adaptive inverse method. In Proceedings o f the 1993
.American Control Conference, pages 552-559. San Francisco. C.A. .June 1993.

[56] M. V idyasagar. Nonlinear Systems Analysis. Simon and Schuster. 1993.

REFERENCES 183

[ô7] L. Meirovitch. Elements o f vibration analysis. McGraw-Hill. 1986.

[58] C.-T. Chea. Linear System Theory and Design. Holt, R inehart and Winston.
1984.

[59] E. Bayo and H. Moulin. An efficient com putation of the inverse dynamics of flex­
ible manipulators in the time domain. In Proceedings 1989 IE E E International
Conference on Robotics and .Automation, pages 710-715, Scottsdale, AZ. 1989.

[60] H. C. Moulin, Eduardo Bayo, and Bradley Paden. Existence and uniqueness of
solutions of the inverse dynamics of multilink flexible arms: Convergence of a
numerical scheme. .Journal o f Robobic Systems, 10( 1):73-102. 1993.

[61] M. Nahon, C. Damaren, A. Bergen, and .J. Gonçalves. .\ test facility for m ulti­
arm ed space-baaed manipulators. Canadian Aeronautic and Space .Journal, pages
391-400, 1995.

[62] D. G. Luenberger. Linear and Nonlinear Programming. .Addison-Wesley P ub­

lishing Company, 1984.

[63] .VI. Nahon. Optimization o f Force Distribution in Redundantly-.Actuated Robotic

System. PhD thesis. VIcGill University. Montreal. Canada. February 1991.

[64] .]. K. Salisbury and I. I. Craig. Articulated hands: Force control and kinematic
issues. The International .Journal o f Robotics Research. I(I):4 -17 . 1982.

[65] H. Hanafusa and H. .Asada. Stable prehension by a robot hand with elastic
fingers. In Proceedings o f the 7th International Symposium on Industrial Robots.
pages 361-368. Tokyo, October 1977.

[6 6 ] W. Yim and S. N. Singh. Inverse force and motion control of constrained elastic
robots. .Journal o f Dynamic Systems. Measurement, and Control. 117:374-383.
Septem ber 1995.

[67] G. Strang. Linear .Algebra and its .Applications. .Academic Press Inc.. 1976.

[6 8 ] F. Pfeiffer. H.-.J. Weidemann. and P. Danowski. Dynamics of the walking stick

insect. In Proceedings o f the 1990 IE E E International Conference on R o b o t i c s
and .Automation, volume 3. pages 1458-1463. Cincinnati. Ohio. .May 1990.

[69] H. D .Angelo. Linear Time-Varying Systems: Analysis and Synthesis. .Allyu and
Bacon Inc.. 1970.
REFERENCES 184

[70] M. VV. Spong and M. Vidyasagar. Robot Dynamics and Control. New York:
VViley, 1989.

[71] A. De Luca and B. Siciliano. Joint-based control of a nonlinear model of a flexible

arm. In Proceedings o f the 1988 American Control Conference, pages 935-940.
.Atlanta, GA, Ju ne 1988.

[72] J. Z. Xia and C.-H. Menq. Real tim e estim ation of elastic deformation for end­
point tracking control of flexible two-link manipulators. .Journal o f Dynamic
Systems, Measurement, and Control, 115:385-393. Septem ber 1993.

[73] M. VV. Vandegrift, F. L. Lewis, and S. Q. Zhu. Flexible-link robot arm control
by a feedback linearization/singular perturbation approach. .Journal o f Robotic
Systems, ll(7):591-603, 1994.

[74] D. W'ang and M. Vidyassagar. Passive control of a stiff flexible link. The Inter­
national .Journal o f Robotics Research. 11(6):572-578. 1992.

[75] S. K. Madhavan and S. N. Singh. Inverse trajectory control and zero dynamic
sensitivity of an elastic manipulator. The International .Journal o f Robotic.^ and
Automation, 6(4):179-192. 1991.

[76] K. P. Jankowski, H. .A. ElMaraghy, and VV. H. ElMaraghy. Dynamic coordination

of multiple robot arms with flexible joints. The International Journal o f Robotics
Research. 12(6):505-528. 1993.

I I V.-R. Hu adn A. A. Goldenberg. .An approach to motion and force control of

coordinated robot arm s in the presence of joint flexibility. In IR IS Workshop on
Dynamics and Control o f Flexible Manipulators, Vancouver. BC, .April 1993.
 185

A p p e n d ix A

In te rb o d y M a trices an d
P r o je c tio n M a trices

In this appendix, we introduce various sets of matrices which are used to relate the

geometric properties of the bodies in a chain to one another and to describe the chain
kinematics.

A .l R o ta tio n M a tr ic e s

We associate with each body frame (u = 0. I V) a corresponding vectrix

— a column of three dextral. orthogonal unit vectors. W'e denote by F„ the vectrix
corresponding to the frame J~n-

Components of any vector in any fram e can be transformed into the components

in any other frame by means of the ro tation m atrix C„.m- defined as follows:

C „ ,m = F „ F ^ ( n . m = 0 . 1........... .V) (A .l)

A P P E N D I X A. I N T E R B O D Y M A T R I C E S A N D P R O J E C T I O N M A T R I C E S 186

T his rotation matrix is orthonormal so that

'^n,m — ^71,m ~ '^m.n (A.2)

It is also convenient to define the following 6 x 6 rotation matrix:

0
Cn,m — (A.3)
0 Cr

A .2 M a tr ix R^+i.n

il is helpful in the dynamics analysis of m ultibody systems to define the following

6 x 6 matrices:

1 -r,n . n + I
R-n+I.n — (n = 0. I , \ j A.4)

We also define

R n.n+l — (n = 0. I . \ ( A.Ô)

T h e transpose and inverse of R^+t,» are respectively

pT
1 0
^n+l.n A. 6 )
‘' n . n + l 1

and
p -I ^ ‘' n + l . n
^n+l,n (A .7)
0 1
A P P E N D I X A. I N T E R B O D Y M A T R I C E S A N D P R O J E C T I O N M A T R I C E S 187

A. 3 I n te r b o d y T ra n sfo rm a tio n s

T he interb o dy transform ation matrix is defined aa

^ n + l ,n — ^ n + l.n a n + l,n (A. 8 )

Also, it is not difficult to show that

'J 'n ,n + l — ^ n , n + l l ^ . n + l (A.9)

and
n—I _ 'j- (A.IO)
ft+l.n — n,n+l

Moreover, for m > n + 1. the following is also true

T'm .n T~jri.m —l T ' m —l , m —2 ' ' ' T ”n+ 2 ,n + I 'A .l

and

A .4 G e n e r a liz e d T ra n sfo rm a tio n M a tr ix for E la s­

t ic B o d ie s

T he generalized transform ation matrix for elaatic body is used to determ ine the effect
of the elastic deformation on the spatial velocity at the joint. It is defined as:

• ^ r i + I . n — ^ n + I ,n>—Ti+1 .n (A. 13)

where
r’n.n+l
“ n+I,n — : a . i 4)
©(rn.n + l'
A P P E N D I X A. I N T E R B O D Y M A T R I C E S A N D P R O J E C T I O N M A T R I C E S 188

In the above, '9?(rn,n+i) is th e shape function m a trix at r^.n+i associated with the
elastic deform ation and © is defined by:

(A.15)

A .5 P r o je c t io n M a tr ix

A projection m atrix is used to define the geometric constraints imposed at each joint.

Columns span the subspace of allowable interbody m otion. Therefore, the dimension
of V n is 6 X TTin whcrc m„ ( < 6 ) is the number of allowable degrees of freedom.

In a typical case of robotics application, the following is commonly used:

T
0 0 0 0 0 1 (A. 16)

The above implies that only one rotational degree of freedom about the third axis is
allowed.

A .6 G lo b a l T r a n sfo r m a tio n M a tr ic e s

The global transformation matrices for a m ultibody chain are defined as follows:

1 0 0 •• 0

Tuo 1 0 •• 0

T = T ,.o T 2.1 1 •• 0 ( A . 17)

^ .v .o T~ S. 2 • 1
A P P E N D I X A . I N T E R B O D Y M A T R IC E S A N D P R O J E C T IO N M A T R I C E S 189

and
0 0 0 0

«^1.0 0 0 0

«5^ = 0 S 2.1 • ■■ 0 0 (A.IS)

0 0 ... 0
 190

A p p e n d ix B

D efin itio n s a n d T h eorem s o f

O p tim iza tio n

B .l G e n e r a l F o rm o f O p tim iz a tio n P r o b le m

VVc now introduce some basic definitions and fundamental theorems used in solving
the constrained optimization problems. .A. general form of such a problem is given as
[62 ):

rmn /(x ) x € R ’‘ ( B .la)

Subject to hi {x) = 0 ; = (B .lb )

and gj {x) < 0 _/ = 1. - . p (B .lc)

where rn < n and functions / . h, (i = 1. • ■■.m ) and ffj (j = I. - ■■.p) are continuous,
and are usually assumed to possess continuous second partial derivatives. For the
objective function, we define a gradient vector V / € R " of the objective function /
A P P E N D I X B. D E F IN IT I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 191

at x:
df/dxi

dfldx2
V/ = (B.2)

and the Hessian m a trix of f :

& ^ f l & ^ x i d ' ^ f I d x i d x 2 ■ ■ ■ d ^ f / d x i d x n

d^f / dx2ÔXi d'^fl&^X2 ••• d^f / dX2dXn

H ( /) = (B.3)

d^ffdXridXi d'^f/dXndx2 • • • d ^ f/d ^ X n

C onstraints on the design variables separate the design space into feasible and infea­
sible regions which are defined below.

D e f in itio n B . l (Feasible Point and Feasible Region)

If any point x' satisfies all the constraints in (B .lb ) and ( B.lc). it is said to be
a feasible point. The set of all such points is referred to as the feasible region
and is denoted by Q.

.A.n inequality constraint can be characterized according to the following definition;

D e f i n i t i o n B .2 (.Active and Inactive Constraint)

.A constraint gj (x) < 0 is said to be active at a feasible point x if ijj{x) = 0 and
inactive at x if gj(x) < 0. By convention, any equality constraint h,{x) = 0 is
active at any feasible point.

In the investigation of the general problem (B .l). we distinguish two kinds of

solution points: local mininnim points and global m in im u m points.
A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S OF O P T I M I Z A T I O N 192

D e f i n i t i o n B .3 (Local Minimum Point)

A point X* 6 n is said to be a local m in im u m point of / over Q if there is an
5 > 0 such th a t / ( x ) > /( x * ) for all x 6 within a distance e of x" (th a t
is, X € n and |x — x*| < e). If / ( x ) > / ( x ‘ ) for all x € fl, x 7^ x ‘ , within a

distance c of x “, then x ‘ is said to be a strict local m in im u m point of / over Q.

D e f i n i t i o n B .4 (Global Minimum Point)

A point x ' Ç Q is said to be a global m in im u m point of / over Q if / ( x ) > / ( x ' )
for all X G n . If / ( x ) > /(x * ) for all x G H, x 7 ^ x “. then x* is said to be a
strict global m in im u m point of / over Q.

In formulating and attacking problem ( B .l) we are, by definition, seeking a global

m inim um point of / over the feasible region Q. An ideal situation is when a strict
global m inim um point exists which means th e minimum point is unique over the feasi­
ble region. In reality, however, searching for the minimum point by comparison of the
values of nearby points is all that is possible when using a convergent stepwise proce­
dure. Therefore, the optimality conditions which will be introduced in the following

section are all related to local minima. Global minimum solutions can only be found
if the problem possesses certain convexity properties which essentially guarantee that
any local m inim um is a global minimum.

B .2 O p tim a lity C o n d itio n s

In studying the properties of a local m inim um point, it is clear that only the active
constraints need to be considered. This is illustrated in Figure B .l where the local
properties satisfied by the solution x" do not depend on the inactive constraints po and
Ç3 . This observation suggests that optim ality conditions can be derived by considering
A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S OF O P T I M I Z A T I O N 193

th e equality constraints alone and subsequently making additions to account for the

selection of th e active inequality constraints. We begin with the following definition.

</3(X) = 0

32(X) = 0 j ,y,(x) = 0

Figure B .l: Example of Inactive Constraints

D e f i n i t i o n B .5 (Regular Point)
.A. point x ' satisfying the constraint (B .lb ) is said to be a regular point of the

constraint if the gradient vectors V /ii( x ’ ). Xhni x' ) . Vhmi' x") are linearly
independent.

For a problem constrained by linear equality equations of the form in (-5.16). the above
condition for a regular point is equivalent to the matrix A i having a full rank. From
th e simplest optim ization problem — the unconstrained one-dimensional op tim ization
— we know th a t a local minimum point requires zero slope and positive curvature. We
would also expect similar optimality conditions for the constrained multi-dimensional
problems.

T h e o re m B .l (Lagrange First-Order Necessary Conditions)

Let X be a local extrem um point (a minimum or maximum) of / subject to the

A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 194

constraint ( B . 16) w ritten compactly as h (x ) = 0 with h = col{/ii, 62, 6 ^}-

Assume further th a t x ' is a regular point of these constraints, then there exists
A = coI{Ai, A2 . . . . Am} such that

V / ( x ' ) + V h (x -)A = 0 (B.4)

where the elements of A are called the Lagrange multipliers.

In the above, the gradient of h is evaluated at x" according to

dhildxi dh2/dxi dhm/ dxi

dhi(dx2 dhildx2 dhjn(dx2

V h ( x ') = (B.5)

dhifdxn dh2/dXn ••• dkm/OXn

In the case that (B .lb ) is a set of linear equations, that is.

h (x ) = AiX — bi = 0 B.G)

the gradient matrix composed of V 6 , is then a constant matrix:

Vh = A [ ; b .7)

Therefore, the first-order necessary condition of eq. (B.4) becomes

V /(x -) = - A [ A : b .s )

This means that the gradient vector of / must lie in the range space of A [ . or
ecjuivalently. V f ( x ' ) is orthogonal to the null space of A ;.

The information supplied by the first-order necessary condition is not sufficient

to determine whether or not the extremum point is a local m in im um since eq. (B.4)
will be satisfied by both the minima and m axim a in the feasible region. Therefore,

we introduce a sufficient condition for obtaining a strict local m inim um point:

A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 195

T h e o r e m B .2 (Second-Order Sufficiency Conditions)

Suppose there is a point x* satisfying h(x*) = 0, and R'" such th a t

V /(x*)-h Vh(x*)A = 0 (B.9)

Suppose also th a t the m atrix L(x') = H ( / ( x ' ) ) -f H (h (x ‘ ))A is positive definite

on M = {y : Vh(x*)y = 0}, th a t is, for y € M, y ^ 0.we have y ^ L (x ')y > 0.
Then x* is a strict local minimum of / subject to h(x) = 0 .

In the above theorem, the Hessian m atrix of h is calculated as:

m
H (h (x " )) = ^ H ( A X x ' ) ) (B.IO)
;=i

T h e second term in th e m atrix L represents the following expansion [62]:

m
H(h)A = ^ AjH(/i_,(x’ )) (B.II)

Again, in the case when h is linear in x. H(h) = 0. Hence, the condition for the
positive definiteness of L(x") on M reduces to:

y ^ H (/(x -))y > 0 (B.12)

The significance of introducing Lagrange multipliers can be observed from Theo­

rem B .l which reduces a minimization (or maximization) problem subject to equality
constraints to a constraint-free stationary value problem. The Lagrange multipliers
are also im portant in determining how much a cost function / varies with a small
variation in the constraint equations. Therefore, the Lagrange multipliers are also
interpreted as a marginal price [62].
A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 196

B .3 E x t e n d e d O p tim a lity C o n d itio n s

We consider now the problems of th e form (B .l), th a t is, an optimization problem

subject to bo th equality and inequality constraints. We start by extending the defi­
nition B..5 of a regular point as follows:

D e fin itio n B . 6 (Regular Point Extended)

Let x ' be a point satisfying the constraints

h (x -) = 0 g ( x ') < 0 (B.I.3)

and let J be the set of indices j for which (x" ) = 0. Then x ' is said to be a
regular point of the constraints (B.13) if the gradient vectors V /z d x '). Vg^(x").
\ < I < m . j Ç: J are linearly independent.

We note th a t according to the definition for active constraints given in definition B. 2 .

a point x ' is a regular point if the gradients of the active constraints are linearly
independent. For a linearly constrained problem, if we collect all active constraints
as columns of a single m atrix, the condition for a regular point of these constraints
is th a t the resulting m atrix has a full rank. For the extended first-order necessary
conditions, the following theorem is given.

T h e o r e m B .3 (Kuhn-Tucker Conditions)
Let x" be a local m inim um point for the problem as defined in e q .( B .l) and
suppose x" is a regular point for the constraints. Then there is a vector A t R
and a vector /fE R '' with ^ > 0 such that

V / ( x ' ) - F V h ( x ‘ )A -1- V g ( x ') / i = 0 (B .U )

fi^ g (x -) = 0 (B.I5)
A P P E N D I X B. D E F IN IT I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 197

We note th a t, since ^ > 0 and g ( x ') < 0 , eq. (B.15) is equivalent to the statem e n t
th a t a com ponent of fi may be nonzero only if the corresponding constraint is active.
Similarly, the second-order sufficient conditions are extended for the situation with
inequality constraints as follows:

T h e o r e m B .4 (Extended Second-Order Sufficiency Conditions)

Let / . g. h have continuous second partial derivatives. Sufficient conditions for
a point x ' satisfying (B .lb ) and (B .lc ) to be a strict relative minimum of the
problem (B .l) is th a t there exist A ç R ”* and fxE R^, such that

fi> 0 (B.16)

At^g(x’ ) = 0 (B.17)

V / ( x ') - f - V h ( x * ) A - f V g ( x ' ) ^ = 0 (B.IS)

and the Hessian matrix

L(x*) = H ( / ( x ‘ )) + H ( h ( x ‘ ))A 4 - H ( g ( x ') ) ^ (B.I9)

is positive definite on the subspace

-V/' = {y : V h ( x ‘ )y = 0 . V g ^ (x ')y = 0 for all j G •/}

where

J = { j : g j ( x ' ) = 0,fij > 0 }

Once again, for a linearly constrained problem, the condition for the positiveness of
the Hessian m atrix L (x ') reduces to H ( / ( x “)) being positive definite on the subspace
M' . In sum m ary, we note that the inequalities are treated by determining which of
them are active at a solution. .A.n active inequality constraint is then treated just like
an equality constraint, except th a t its associated Lagrange multiplier can never be
negative.
A P P E N D I X B. D E F IN IT I O N S A N D T H E O R E M S OF O P T I M I Z A T I O N 198

B .4 C o n v e x F u n c tio n s

Based on the above optimality theorems, a constrained optim ization problem can be
approached by solving directly the Lagrange first-order necessary conditions. T h e
second-order sufficient conditions are then used to determ ine w hether or not point
x ‘ satisfying the Lagrange first-order conditions is a local minim um . This procedure

is known as the Lagrange method. In some cases, if the objective function takes a
special form, the second step can be om itted. An ideal situation arises for a particular
objective function, when a solution of the Lagrange first-order necessary conditions
is guaranteed to produce a unique global minimum point in the design space. Such
objective function is known as a convex function as defined below.

D e f in itio n B . 6 (Convex Set)

A set Q in R " is said to be convex if for every Xi. X2 G D and every real num ber
o. 0 < a < I. the point a x , -f ( 1 — a)Xj € 0.

W ith this definition, it is trivial to show that the design space R " is always convex
since any point on the line connecting two other points in R " is also a m em ber of R "

D e f in itio n B .7 (Convex Function)

.A function / defined on a convex set Q is said to be convex if for every x ,. x> <E
O and every a . 0 < q < I. the following holds

/(q X i -I- (I - a ) x 2 ) < a / ( x i ) -I- ( I - a ) / ( x , ) ( B.20)

The function / is said to be strictly convex if for every a . 0 < a < 1. and
X, ^ x>. the following holds

/ ( a x i -h (I - q ) x 2 ) < a / ( x i ) 4- ( I - a ) / ( x 2 ) (B.21)
A P P E N D I X B. D E F I N I T I O N S A N D T H E O R E M S O F O P T I M I Z A T I O N 199

In addition, convex functions can be combined to yield new convex functions. For a

function which is twice continuously differentiable, an alternative characterization of

convexity can be sta te d in terms of the function’s Hessian m atrix: a positive semidef-
inite Hessian implies a convex function, while a positive definite Hessian implies a

strictly convex function.