Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Force control for robotic manipulators with structurally flexible links Latornell, Douglas John 1992

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-ubc_1992_fall_latornell_douglas_john.pdf [ 5.82MB ]
Metadata
JSON: 831-1.0081044.json
JSON-LD: 831-1.0081044-ld.json
RDF/XML (Pretty): 831-1.0081044-rdf.xml
RDF/JSON: 831-1.0081044-rdf.json
Turtle: 831-1.0081044-turtle.txt
N-Triples: 831-1.0081044-rdf-ntriples.txt
Original Record: 831-1.0081044-source.json
Full Text
831-1.0081044-fulltext.txt
Citation
831-1.0081044.ris

Full Text

FORCE CONTROL FOR ROBOTIC MANIPULATORS WITH STRUCTURALLY FLEXIBLE LINKS By Douglas John Latornell B.Sc. ions. (Mechanical Engineering) Queen’s University M.Sc. (Mechanical Engineering) Queen’s University  A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY  in THE FACULTY OF GRADUATE STUDIES DEPARTMENT OF MECHANICAL ENGINEERING  We accept this thesis as conforming to the required standard  THE UNIVERSITY OF BRITISH COLUMBIA  1992  ©  Douglas John Latornell, 14 August 1992  In  presenting  this  thesis  in  partial  fulfilment  degree at the University of British Columbia, freely available for reference and study.  of  the  requirements for an  I agree that the Library shall make it  I further agree that permission for extensive  copying of this thesis for scholarly purposes may be granted department  or  by  his  or  her  advanced  representatives.  It  is  by the head of my  understood  that  copying  or  publication of this thesis for financial gain shall not be allowed without my written permission.  Department of  /  The University of British Columbia Vancouver, Canada  Date  DE-6 (2/88)  aki /92  iç7  Abstract  This thesis reports on the development of strategies for the control of contact forces exerted by a structurally flexible robotic manipulator on surfaces in its working environment. The controller is based on a multivariable, explicitly adaptive, long range predictive control algorithm. A static equilibrium bias term which is particularly applicable to the contact force control problem has been incorporated into the control algorithm cost function. A general formulation for the discrete time domain characteristic polynomial of the closed loop system has been derived and shown useful in tuning the controller. Kinematic and dynamic models of a robotic manipulator with structurally flexible links interacting with its working environment are derived. These models include inertia and damping effects in the contact dynamics in addition to the contact stiffness employed in most previous work. Linear analyses of the dynamic models for a variety of manipulator configurations reveal that the controlled variable, the contact force, is dominated by different open ioop modes of the system depending on the effective stiffness of the contacting surfaces. This result has important implications for the selection of the controller parameters. The performance of the controller has been evaluated using computer simulation. A special purpose simulation program, TWOFLEX, which includes the dynamics models of the manipulator and the environment as well as the control algorithm was developed during the research. The configurations investigated using the simulation include a single flexible manipulator link, two link manipulators with both rigid and flexible links, and a two link prototype model of the Mobile Servicing System (MS S) manipulator for the proposed Space Station, Freedom. The results show that the controller can be tuned to provide fast contact force step responses with minimal overshoot and zero steady-state error. The problem of  11  maintaining control through the discontinuous situation of unexpectedly making contact with a surface is addressed with the introduction of a contact control logic level in the control hierarchy.  111  Table of Contents  Abstract  11  List of Tables  vi”  List of Figures  x  Nomenclature  xvi  Acknowledgements  xxv  1  2  Introduction  1  1.1  Introduction  1  1.2  Motivation  2  1.3  Survey of Related Research  3  1.4  Contributions of the Thesis  6  1.5  Outline of the Thesis  9  .  .  Manipulator Modelling  14  2.1  Introduction  14  2.2  Literature Review  15  2.3  Kinematics  19  2.3.1  Manipulator Configuration Model  2.3.2  Flexible Link Model  2.3.3  Contact Model  .  19 25  .  29  iv  3  2.4  Derivation of Equations of Motion  32  2.5  Contact Force Model  48  2.5.1  52  2.6  Voltage Controlled DC Electric Motor Actuator Model  54  2.7  Summary  58  Control Algorithm  59  3.1  Introduction  59  3.2  Review of SISO GPC Algorithm  62  3.2.1  66  3.3  4  Hertz Contact Model  Control Law Structure and Closed Loop Characteristics  Multivariable Predictive Control Algorithm  69  3.3.1  76  Control Law Structure and Closed Loop Characteristics  3.4  Static Equilibrium Bias Term  79  3.5  Control Algorithm Parameters and Tuning  83  3.6  Parameter Estimation  86  3.7  Adaptive Controller Implementation  90  3.8  Stability and Convergence  92  3.9  Summary  93  Dynamics and Control Simulation  —  TWOFLEX  95  4.1  Introduction  95  4.2  General Description of the TWOFLEX Program  96  4.3  Integration of Equations of Motion  99  4.4  Discrete Time Control Algorithm  102  4.5  Validation  106  4.6  Summary  109  v  5  Force Control Algorithm Performance and Evaluation  5.1  Introduction  5.2  Linear Analysis  5.3  5.4  5.5  6  5.2.1  Rigid Link  5.2.2  Flexible Link  •  •  •  .  •  •  •  .  •  Force Control for a Single Flexible Link 5.3.1  Link Model Parameters  5.3.2  Controller Model  5.3.3  Soft Contact Case (keff  5.3.4  Hertz Contact Model Case (kff  5.3.5  keff = 62kN/m Contact Stiffness Case  5.3.6  Summary  3.lkN/m) 5MN/m) .  Making and Breaking Contact 5.4.1  Stop Motion Strategy  5.4.2  Force Setpoint Modification Strategy  .  Force Control for Two Link Manipulators 5.5.1  Linear Analysis  5.5.2  Controller Model  5.5.3  Force Control for Two Rigid Links  5.5.4  Force Control for Two Flexible Links  5.5.5  Summary  .  .  .  •  •  •  .  •  •  •  •  5.7  Summary of Force Control Performance  Summary of Major Results  114  121 135  135 136  •  •  •  •  •  •  •  •  •  .  •  .  .  •  •  .  .  .  •  .  .  .  •  •  .  .  •  .  .  .  •  .  .  .  •  .  •  .  •  .  .  .  •  .  137  141 151 160 163 166  169 174 175 197 199 202  .  Force Control for DC Motor Actuated Space Station Manipulator  111  114  •  •  5.6  6.1  111  •  .  .  .  •  .  .  .  •  .  .  .  217 218 224  228  Recommendations for Further Research  vi  231  Bibliography  235  Appendices  242  A Equations of Motion for Various Manipulator Configurations  242  A.1 Planar Two Link Flexible Manipulator in Free Motion  242  A.2 Planar Two Rigid Link Manipulator in Contact  245  A.3 Planar Two Link Rigid Manipulator in Free Motion  247  A.4 One Flexible Link in Contact  248  A.5 One Flexible Link in Free Motion  249  A.6 One Rigid Link in Contact  249  A.7 DC Motor Actuated Single Flexible Link in Free Motion  250  B Cost Function Expansions  251  B.1 Multivariable Predictive Control Algorithm  • 251  B.2 Static Equilibrium Bias Term  • 252  C Static Equilibrium Function Derivation  vii  253  List of Tables  4.1  Configurations and initial conditions for validation of inertia matrix and right hand side vector terms  4.2  107  Configurations and initial conditions for free response simulation validation runs  5.1  107  Mode shape function integral factor values for a single flexible link with one structural mode  5.2  123  Natural frequencies and mode shape function integral factor values for single flexible link run  136  5.3  Material properties and Hertz model parameters for steel on aluminium contact 142  5.4  Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff  =  5.0037MN/m) for a single flexible link with one 146  structural mode 5.5  Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with one 153  structural mode 5.6  Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with three structural modes  160  5.7  Parameter values for two rigid link manipulator controller model  200  5.8  Stabilising controller horizons and closed ioop characteristics for a struc turally rigid two link manipulator contacting a keff = kN/m surface  viii  202  5.9  Parameter values for two flexible link manipulator controller model  204  5.10 Stabilising controller horizons and closed ioop characteristics for a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface  208  5.11 Physical parameter values for a two link MSS model with DC motor actuators.219  ix  List of Figures  2.1  Geometry of a two link flexible manipulator in contact with a deformable surface  2.2  19  Coordinate frames for a two link flexible manipulator in contact with a de formable surface  21  2.3  Free body diagram of a differential beam element  26  2.4  Axial foreshortening of a differential beam element  29  2.5  Manipulator—environment model for derivation of contact force expressions  49  2.6  Free body diagrams for the x direction contact region mass  50  2.7  Schematic of direct current motor actuator system  55  3.1  SISO GPC closed loop system block diagram  68  3.2  Comparison of EFRA and U/D RLS parameter estimation algorithms.  3.3  Control strategy block diagram  4.1  Flowchart of the overall structure of the TWOFLEX dynamics and control sim  .  89 91  ulation program  97  4.2  Flowchart of the calculation of the generalised acceleration vector  102  4.3  Flowchart of the discrete time control algorithm  103  4.4  Comparison of joint angle responses for free response validation  108  4.5  Comparison of modal amplifier responses for free response validation  110  5.1  Topics discussed in Chapter 5  112  5.2  Single rigid link in contact with a deformable environment  114  x  5.3  Variation of the natural frequencies with effective contact stiffness for a single rigid link/environment system  117  5.4  Single flexible link in contact with a deformable environment  121  5.5  Variation of the natural frequencies with effective contact stiffness for a single flexible link with one structural mode  5.6  124  Single flexible link in contact with a deformable environment approximated by a steady state contact dynamics model  5.7  125  Variation of the natural mode components of the contact force with effective contact stiffness for a single flexible link with one structural mode  5.8  Variation with effective contact stiffness of the relative excitation of the nat ural modes of a single flexible link with one structural mode in contact.  5.9  127  .  .  129  Variation with effective contact stiffness of system natural frequencies for a soft link (El 1 = 40N m ) 2  130  .  5.10 Variation with effective contact stiffness of system natural frequencies for a flexible link with four structural modes  132  5.11 Variation of the natural mode components of the contact force with effec tive contact stiffness for a single flexible link (El 1 = 200N m ) with four 2 .  structural modes  133  5.12 Variation with effective contact stiffness of the relative excitation of the nat 1 ural modes of a single flexible link (El  200N m) with four structural .  modes  134  1 = 574N m , 2 5.13 Variation of natural frequencies of a single flexible link (El two structural modes) in contact  139  (k = ke = 5.14 Contact force response of a single flexible link on a soft surface 5 140  6200N/m)  xi  5.15 Variation of natural frequencies of a single flexible link (with one structural mode) in contact  145  5.16 Comparison of results from simulations using full contact dynamics and steady state approximation  147  5.17 Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (keff  =  5MN/m) with N 2 = 20, N = 2,  = 0, and Age = 0  148  5.18 Force control step responses for a single flexible link (with one structural mode), steel on aluminium contact (keff = 5MN/m) with 0  0.4.  Age  .  .  5.19 Experimental contact stiffness estimate for steel on aluminium  150 152  5.20 Force control step response for a single mode flexible link (with one structural mode), steel on aluminium contact (kff = 61.554kN/m) with N 2 = 10, 154  =0 8 N=3,A=0,A 5.21 Force control step response for a single mode flexible link (with one structural mode), steel on aluminium contact (kff = 61.554kN/m) with N 2  =  5, N.  4, A = 0, A = 0  155  5.22 Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (keff = 61.554kN/m) with N 2 = 5, N 3, A = 0, Age  0  156  5.23 Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (keff  =  61.554kN/m) with N 2  =  7, N  =  158  e0 8 4,Ac0,A 5.24 Variation of the natural frequencies of a single flexible link (El 1 = 574N m 2 with three structural modes) in contact  xii  159  5.25 Force control step response for single flexible link (with three structural modes), steel on aluminium contact (keff = 61.554kN/m), with N 2 = 8, N, = 4,  = 0,  )ise  = 0  161  5.26 Block diagram of the overall control structure  165  5.27 Joint angle and contact force responses for a single flexible link colliding with a 6200N/m surface  167  5.28 Simple 1 degree of freedom model of a single link in contact with an environ ment  170  5.29 Contact force response for a single flexible link making contact with a 6200N/m surface at 0.536m/s  172  5.30 Contact force response of a single flexible link colliding with a 6200N/m surface with force setpoint modification  173  5.31 Two rigid link manipulator in contact with a deformable environment  .  .  176  .  5.32 Variation of the natural frequencies with effective contact stiffness for a two rigid link manipulator in contact 5.33  and  2  178  mode shapes for various values of  for a two rigid link manipulator.183  5.34 Variation of contact force components in natural coordinate space with O with keff = 61.554kN/m  185  5.35 Relative excitation of the natural modes of a two link rigid manipulator by the control inputs  187  5.36 Variation of the natural frequencies with effective contact stiffness of a two link manipulator with identical, flexible (El = 574N m , one structural 2 mode each) links in contact with an isotropic (kxeff  kyeff) environment.  .  .  191  5.37 Variation of the natural frequencies with effective contact stiffness for a two , one structural mode each) with 2 flexible link manipulator (El = 574N m  2 = 2.Om = 1.Om and L  193 xlii  5.38 Variation of the natural mode components of the contact forces with effective contact stiffness for a two flexible link manipulator  195  5.39 Variation of the relative excitation of the natural modes with effective contact stiffness for a two flexible link manipulator  196  5.40 Force control step responses for a structurally rigid two link manipulator contacting a keff = lOkN/m surface with N 2 = 5, Nu = 2,  c  = 0  203  5.41 Variation of the natural frequencies for a two link manipulator with identical flexible links assuming steady state contact dynamics  206  5.42 Force control step responses for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2  20, N = 2,  0  209  5.43 Force control step responses for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m 211  =8,N=4,)=0 2 surfacewithN 5.44 Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2 = 20, N = 2,  0  212  5.45 Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2 = 8, N = 4,  = 0  213  5.46 Force control step responses for nonlinear dynamics simulation of a two flexi ble link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2 = 20, N = 2,  ,  = 0  215  5.47 Variation of the modal natural frequencies for DC motor actuated two link MSS model  220  xiv  5.48 Force control responses for linear dynamics simulation of a two flexible link Space Station MSS contacting a keff = 2OkN/m surface  222  5.49 Force control responses for nonlinear dynamics simulation of a two flexible link Space Station MSS contacting a keff  xv  =  2OkN/m surface  223  Nomenclature  Roman letters ) 1 A(q  a B(q’)  discrete time input/output model denominator polynomial of degree ma radius of circular contact area in the Hertz contact model discrete time input/output model numerator polynomial of degree mb  Bk(q’)  discrete time input/output model numerator polynomial relating kth input to ith output in a discrete time multivariable system model  ba  equivalent viscous damping coefficient for DC motor armature frictional damp ing  bex  equivalent viscous damping factor for contact damping of the environment surface in the x direction  bey  equivalent viscous damping factor for contact damping of the environment surface in the y direction  bk,  equivalent viscous damping factor for structural damping of mode i of link k  3 b  equivalent viscous damping factor for contact damping of the manipulator tip surface in the x direction  3 b  equivalent viscous damping factor for contact damping of the manipulator tip surface in the y direction  bek  equivalent viscous damping factor for friction in joint k  c,  vector of generalised coordinate components for linearised model of F  c,  vector of generalised coordinate components for linearised model of F  1 c  cosine of joint angle 1 (Ox)  2 c  cosine of joint angle 2  2 ca Clai2  (&2)  cosine of [w’ (t) + 2 10 (t)] cosine of 1 [6 ( (t)] t) + 0 w ( t) + 2 xvi  D,(t)  homogeneous transformation matrix to describe the structural deformation of link 1 relative to coordinate frame  D ( 2 t)  homogeneous transformation matrix to describe the structural deformation of link 2 relative to coordinate frame F 2  D  inverse of effective planar strain modulus in the Hertz contact model  d(t)  disturbance term for ith output in a multivariable system model  dm,  an element of mass on link 1  2 dm  an element of mass on link 2  E E(q’) Ek Elk  matrix whose columns are the eigenvectors of a linear system a backward shift polynomial which satisfies the Diophantine identity modulus of elasticity of the material of surface k in the Hertz contact model fiexural rigidity of link k in bending about its joint axis  ê  vector of optimal predictions of future output errors for the ith output of a multivariable system  ek  eigenvector corresponding to the kth natural frequency of a linear system  F (q’) 3 F Fk  applied force in the Hertz contact model a backward shift polynomial which satisfies the Diophantine identity peak contact force during collision transient a direction component of contact force, relative to and projected onto F°  F  y direction component of contact force, relative to and projected onto F°  F°  fixed world coordinate frame with mutually perpendicular unit direction vec tors; i, i and i  F’  coordinate frame for link 1 with mutually perpendicular unit direction vectors; i, i and i  2 F  coordinate frame for link 2 with mutually perpendicular unit direction vectors; i, i and i  FC  coordinate frame at the location of initial contact with mutually perpendicular unit direction vectors; i, i and i xvii  f(t +  f  vector of q’ polynomial operators  f  vector of free response components of the ith predicted output from time step t + 1 to t + N ; subscript is dropped in SISO case 2  j)  free response part of (t + jt)  f fd  G’ k 2 G  natural frequency in Hz damped natural frequency in Hz 2 x N N 2 matrix of G polynomial coefficients N x N matrix of G:jk polynomial coefficients obtained by truncating the left 2 most N 2 N columns from Gk; subscripts dropped in SISO case —  C, g 1 g H(s)  polynomial product of E(q ) and B(q’) 1 gravity field vector vector of q’ polynomial operators; subscripts dropped in SISO case 0 x n, matrix of transfer functions in the Laplace domain n  Ho,,(t)  homogeneous transformation matrix between coordinate frames F 1 and F°  H ( 2 , 0 t)  homogeneous transformation matrix between coordinate frames F 2 and F°  H ( 2 , 1 t)  homogeneous transformation matrix from coordinate frame F 2 to frame F’  H’, ( 2 t)  rigid body component of coordinate transformation from frame F 2 to frame F’  H , 2 (t)  homogeneous transformation matrix between frames FC and F 2  H k 2 (s)  Laplace domain transfer function relating the kth input to the ith output; the ik element of matrix H(s)  I ‘ok -a 7 ‘mki  i  identity matrix rigid body component of the moment of inertia of link k about joint axis k DC motor armature inertia mth inertia integral for the ith admissible mode shape function of link k mode index for link 1 in dynamics model; output index for multivariable system model xviii  a J(s)  j  DC motor armature current Laplace transformed matrix of left hand side terms in a system of linearised equations of motion ij minor of matrix J(s)  1 J  quadratic cost function for SISO CPC algorithm  2 J  cost function for multivariable predictive control algorithm  3 J  overall cost function for multivariable predictive control algorithm  Jae  j Kepj KT k  static equilibrium bias cost function mode index for link 2 in dynamics model; time step index in predictive control algorithm DC motor back emf coefficient DC motor torque coefficient link index in dynamics model; input index for multivariable system model; surface index for Hertz contact model  kff  effective contact stiffness  ke  stiffness of the environment surface in the x direction  key  stiffness of the environment surface in the y direction  k  stiffness of the manipulator tip surface in the z direction  3 k  stiffness of the manipulator tip surface in the y direction  L  a unit lower triangular matrix  1 L  length of link 1  2 L  length of link 2  La  DC motor armature inductance  £  Lagrangian; £ = T  Mk  total mass of link k  —  V  xix  m  total contact mass; sum of m 3 and me  me  mass of contact region of environment  3 m  mass of contact region of manipulator tip  2 N  prediction horizon in predictive control algorithm  Ng  actuator gear ratio  N  control horizon in predictive control algorithm  2 n  number of inputs in multivariable system model  k  number of admissible functions included in the structural model of link k  0 n  number of outputs in multivariable system model  ma  degree of discrete time input/output model denominator polynomial  nb  degree of discrete time input/output model numerator polynomial  P  covariance matrix in a recursive least squares (RLS) parameter estimation algorithm  Q  a generalised force  q  vector of q’ polynomial operators  q  vector of q’ polynomial operators  q q’ R  a generalised coordinate backward shift operator; q’x(t)  —  1)  effective radius of curvature of the surfaces in the Hertz contact model  Ra  DC motor armature resistance  Rk  radius of curvature of surface k in the Hertz contact model  ?  total energy dissipation function  r(t)  position vector, relative to and projected onto F°, for an element of mass on link 1  r(t)  position vector, relative to and projected onto F°, for an element of mass on link 2 xx  r(t)  position vector, relative to and projected onto F°, for the total contact mass  r(t)  position vector, relative to and projected onto .F’, for an element of mass on link 1  r(t)  position vector, relative to and projected onto F , for an element of mass on 2 link 2  r(t)  position vector, relative to and projected onto  s  sine of joint angle 1 (6)  2  sine of joint angle 2 sine of [w’ (t) 10  T  for the total contact mass  the Laplace operator  1 s  1a2  FC,  (82)  (t)] 2 +8  sine of [& (t) + w() 1  (t)] 2 +8  total kinetic energy time  U(s)  n x 1 vector of Laplace transformed inputs to a multivariable system  Uk(s)  Laplace transform of kth input in a multivariable system model  Uk k 11  Uk  vector of kth control inputs for N 2 time steps into the future vector of static equilibrium values for kth input for N 2 time steps into the future vector of increments for kth input from time step t to t + N 2 dropped in SISO case  —  1; subscript is  Uk(t)  time series of values of the kth input in a discrete time multivariable system model; subscript dropped in SISO case  Luk(t)  increment for kth control input at time step t; subscript dropped in SISO case  u  value of the kth control input at the current time step  Vk  volume of material moved due to the deflection of surface k in the Hertz contact model  V  total potential energy xxi  v(t)  velocity vector, relative to and projected onto F°, for an element of mass on link 1  v(t)  velocity vector, relative to and projected onto F°, for an element of mass on link 2  Va  DC motor armature voltage  vb  DC motor back emf voltage  2 w  vector of future setpoint levels for ith output from time step t + 1 to t + N ; 2 subscript dropped in SISO case  t)  displacement of the actual centreline from the nominal centreline of the unde formed link k  wk(xk,  0 wk  xk  total deflection of the tip of link k due to bending distance measured along the nominal centreline of link k x direction coordinate of the point of initial contact, relative to and projected onto F° x direction component of manipulator tip position, relative to and projected onto F° x direction component of manipulator tip velocity, relative to and projected onto F°  Y(s)  0 x 1 vector of Laplace transformed outputs of a multivariable system n  Y(s)  Laplace transform of ith input in a multivariable system model  5r  vector of optimal predictions for ith output from time step t + 1 to t + N ; 2 subscript dropped for SISO case  (t) 2 y  time series of values of the ith output in a discrete time multivariable system model; subscript dropped in SISO case  (t + j t)  optimal prediction of y(t -f- j) given the information available at time step t; subscript dropped in SISO case  Yw  y direction coordinate of the point of initial contact, relative to and projected onto F°  Ytip  y direction component of manipulator tip position, relative to and projected onto F° xxii  thj  y direction component of manipulator tip velocity, relative to and projected onto F°  Greek letters 1 a  local, instantaneous slope of link 1 at its tip; a 1  10 w’  2 a  local, instantaneous slope of link 2 at its tip; a 2  0 w  ae  parameter estimation gain adjustment constant in the exponential forgetting and resetting algorithm (EFRA)  e 3 /  ki 3 I  A S e 8  =  constant directly related to the minimum eigenvalue of the covariance matrix in the exponential forgetting and resetting algorithm (EFRA) solution of the cantilever beam frequency equation for the ith mode of link k differencing operator; 1  —  total deformation of contacting surfaces in Hertz contact mode constant inversely related to the maximum eigenvalue of the covariance matrix in the exponential forgetting and resetting algorithm (EFRA)  k 5  deflection of surface k in the Hertz contact model  e  x direction generalised coordinate for contact dynamics model  e  y direction generalised coordinate for contact dynamics model damping ratio  1k 7  kth natural coordinate of a system  ‘9  vector of parameter estimates in a recursive least squares (RLS) parameter estimation algorithm  8  joint angle for link 1  82  joint angle for link 2  8  nominal value of joint angle 1  O  nominal value of joint angle 2  a 8  DC motor armature speed  8  joint angle rate at the instant of contact xxiii  X  predictive controller input increment weighting factor exponential forgetting factor constant in the exponential forgetting and reset ting algorithm (EFRA)  ‘se  static equilibrium bias term weighting factor in predictive control algorithm a unit vector  k 11  Poisson’s ratio of the material of surface k in the Hertz contact model axial foreshortening of a beam due to bending  d  axial foreshortening of a differential beam element due to bending  (t)  time series of uncorrelated random noise values  pk  mass per unit length of link k  1 r  torque applied to link 1 by the actuator at joint 1  2 r  torque applied to link 2 by the actuator at joint 2  -rL  load torque applied to the DC motor  Tm  DC motor armature torque  c/k  regressor vector of past input and output data values for recursive least squares (RLS) parameter estimation algorithm  qfi  ith admissible mode shape function for link 1  2j 5 q  jth admissible mode shape function for link 2  5 q  slope of ith admissible mode shape function for link 1; dqi/dxi  c4  2 /dx 23 slope of jth admissible mode shape function for link 2; dq ith modal amplifier for link 1  /2j  jth modal amplifier for link 2  bli  12 d first time derivative of ith modal amplifier for link 1; /dt  1’2j  first time derivative jth modal amplifier for link 2; di/’ /dt 1  1 b  ’e/..’ d 2 /dt second time derivative of ith modal amplifier for link 1; 11  2 b  11 b d 2 second time derivative jth modal amplifier for link 2; /dt  w  natural frequency in rad/s xxiv  Acknowledgements  I wish to thank my research supervisor, Dr. Dale Cherchas, for his many contributions to the research presented in this thesis. From the initial stages of selecting the topic through the final proofreading he was always available with a ready ear and thoughtful suggestions. The input of the supervisor committee, Dr. Peter Lawrence, Dr. Vinod Modi and Dr. Stan Hutton, during the early stages of the research and their comments which helped to improve the finished thesis are gratefully acknowledged. I also wish to express my ap preciation to Dr. Ray Gosine, Dr. Chris Ma and Dr. Clarence de Silva for their helpful comments during and following my oral examination and to Dr. Andrew Goldenberg of the University of Toronto, the external examiner, for his careful reading of my thesis and his insightful comments. Funding for the project was provided by the Natural Sciences and Engineering Research Council (NSERC) in the form of a Post Graduate Scholarship granted to the author and an Operating Grant (OP 00004682) to Dale Cherchas. The British Columbia Advanced System Institute (ASI) also provided funds for scholarships and research equipment. The University of British Columbia Department of Mechanical Engineering supported the author through teaching assistantships. The work described in this thesis relied heavily on computing facilities and resources. Without the assistance and cooperation of Alan Steeves and Gerry Rohling of the Depart ment of Mechanical Engineering and Tom Nicol of University Computing Services many aspects of the research and the thesis preparation would have been much more time con suming, if not impossible.  xxv  The experience of doing doctoral research and writing this thesis would have been very much more trying were it not for many people not directly involved with the work.  I  thank my parents for their ongoing support and encouragement of all of my endeavours and my brother for his occasional, and usually humorous, reminders of the wide and different world out there. I also thank my friends in the department, in particular, Alan Spence, Sam and Anat Kotzev, Harry Mah and Afzal Suleman, for many fruitful discussions and commiserations. The music of Johannes Brahms, Franz Liszt and George Thorogood helped to smooth some of the rough spots in the writing of the thesis and CBC Stereo provided almost constant accompaniment. Finally, I express my deepest thanks to my wife, Susan. She has always been there to listen, especially when no one else would or could, and she has endured the worst of me over the past few years. Through all of this she has been a constant source of encouragement.  xxvi  Chapter 1  Introduction  1.1  Introduction  This thesis discusses research centred on the development of strategies for the control of the contact forces exerted by a structurally flexible robotic manipulator on surfaces in its working environment. The ability to sense and control the forces exerted by a robotic manipulator on its environment has, for some time, been recognised as a necessary and ben eficial extension of robotic system capabilities. Force sensing and control offers improved performance of tasks which involve contact of the end—effector with the environment by providing robustness to variations in location and shape of the objects being manipulated. One example of such improved performance is the detection and alleviation of jamming parts during insertion tasks in assembly operations. In addition, force control makes pos sible tasks which are extremely difficult, if not impossible, under purely positional control such as deburring of stamped metal parts, grinding of welded joints and polishing oper ations on objects with complex surface shapes. Structural flexibility in the manipulator links presents particular challenges in the context of force control because of the complex dynamics which are introduced between the actuators at the manipulator joints and the force sensor at the manipulator wrist or tip. These challenges have been met in this study with the development of a multivariable predictive control algorithm. The algorithm can be readily implemented as an explicit adaptive controller which provides the controller with the ability to compensate for the time varying, nonlinear components of the system dynamics.  1  Chapter 1. Introduction  1.2  2  Motivation  This research was motivated both by the academic challenges of understanding and con trolling a complex dynamic system and by the need for high performance force control capabilities in robotic manipulation. A particular application which has provided inspira tion and direction for this work is the identified need [1] for contact force control in the development of autonomous and telerobotic manipulator systems for the proposed Space Station, Freedom. In academic terms, the problem is of interest because the dynamics of even a rigid manipulator with several links in free motion are strongly coupled and highly nonlinear. The addition of structural dynamics in the links and interaction between the manipulator tip and a working surface increases the number of coupled degrees of freedom in the system and introduces time varying components into the dynamics. Specifying the modulation of the contact forces as the control objective and using the torques generated by actuators at the manipulator joints as the inputs to achieve this objective poses a difficult control problem because of presence of complex dynamics between the actuators and the sensor. Furthermore, the control problem is inherently discontinuous because the contact force is a positive indefinite function of the system state, that is, if the manipulator is not in contact, or loses contact, with the environment the contact force is, by definition, zero and therefore provides no information about the system state. Thus a force control algorithm which provides stable, acceptably shaped closed ioop responses must be augmented with a control strategy to provide smooth, stable, predictable performance in the absence of contact and during the transitions of making and breaking contact. The importance of compliance, associated with either the manipulator or the environ ment, in achieving stable force control with existing industrial robots and controllers is well  Chapter 1. Introduction  3  established [2]. Manipulators with structurally flexible links or flexible joints naturally pro vide compliance which is distributed over the entire manipulator structure. This distributed compliance is helpful in achieving high performance force control by absorbing the impulse which occurs when contact is made. Large initial contact forces are currently avoided in most robotic force control operations by approaching the environment at very low speed. Clearly, to take advantage of this shock absorption ability, the compliant nature of the links must be included in the controller design. Space based manipulators with structurally flexible links will, in the near future, be required to operate under conditions where force control is essential. The Space Shuttle Re mote Manipulator System (SSRMS or Canadarm) and the Mobile Servicing System (MSS) and Special Purpose Dextrous Manipulator (SPDM) are all examples of this type of manip ulator. Among the tasks which have been proposed for these manipulators are the assembly of the structural elements of the Space Station Freedom, maintenance and cleaning activ ities on the exterior of the station, removing and installing thermal protection coverings, and servicing of other spacecraft. All of these tasks clearly require control of contact forces in the presence of significant structural flexibility of the manipulator. As the capabilities of these space based systems are proven, new, light-weight earth based manipulators designed for high speed motion can also be expected to exhibit structural flexibility which will need to be accounted for in the design of both force and motion controllers.  1.3  Survey of Related Research  The control of the contact forces exerted by robotic manipulators on their working envi ronments has been an active field of research since the 1970’s. Among the earliest reports in the literature on this subject is the work of Whitney [3]. Though terminology has, in  Chapter 1. Introduction  4  some cases, changed, most of the issues identified by Whitney remain important in cur rent research. The distinction made by Whitney between gross motions (large movements of the end-effector from one point to another in the workspace) and fine motions (small motions during which the possibility or certainty of contact with the environment exists) reflects the fundamentally discontinuous nature of the overall force and motion control of manipulators. A particular problem which arises from this discontinuity is that of unex pected collisions between the end-effector and the environment. In the course of analysing a linear force feedback control law Whitney highlights the issue of force controller stability and reveals the intimate relationship between controller performance and compliance in the manipulator/sensor/environment system. The problem of simultaneously combining force and motion control was a focus of early research. The methodologies for solving this problem generally focus on the use of feedback to associate a particular physical characteristic (e.g., stiffness [4], damping [3], impedance [5—7]) with the movements of the manipulator in each direction of an appropriately selected coordinate frame. Fundamental to all of these methods are the needs to select a coordinate frame appropriate to the force/motion task being undertaken and to specify the task in terms of desired force/motion trajectories in that frame. This specification of the manipulator motion subject to constraints imposed by the task geometry was referred to as compliant motion by Mason [8]. Many force/motion control methodologies implicitly combine the task specification in Cartesian space and the control algorithm (see Hogan [5—7], Wu [9], Kazerooni, et al. [10, 11]). On the other hand, the hybrid position/force control methodology proposed by Raibert and Craig [12] is primarily a control system framework for Cartesian compliant motion into which servo level force and position control algorithms appropriate to particular manipulator configurations can be incorporated (see, for example Salisbury and Craig [13]). All of these methods employ the manipulator Jacobian to relate the joint space to the  Chapter 1. Introduction  5  Cartesian task space. Indeed, it has been shown by An, et al. [14] that, unless carefully structured, hybrid position/force control can be subject to a kinematic instability due to the matrix inversion of the Jacobian in the feedback path. The more general operational space formulation of Khatib [lSj also provides such a framework for non-Cartesian task spaces and kinematically redundant manipulators. Another distinction, cited by An and Hollerbach [16], between the hybrid position/force method and the operational space formulation is the fact that the manipulator dynamics model is included in the latter. In summarising more than a decade of progress in force control Whitney [2j notes that the areas of stability and strategy have been well treated but that most papers contain “no model of the origin of the contact forces” and “deal with [contact force] control only in passing”. Particularly neglected areas of research cited by Whitney include the problem of collisions, the effects of manipulator compliance and the general theory of task space constraint specification. Uchiyama [17] offers a similar list of strengths and weaknesses in the field. All of the work cited above, and indeed the vast majority of the work to date in robotic manipulator contact force control, has been focused on completely rigid manipulators. This is hardly surprising given the complexity and difficulty of the rigid manipulator problem and the many fruitful areas for research which were available without considering the additional complexity of dynamics due to flexibility in the joints and links of the manipulator. How ever, driven by the desire for lighter weight, higher performance industrial manipulators and proposed teleoperated and autonomous applications of large, space based manipulators, at tention has been turned to non-rigid manipulators. An increasingly large body of literature exists which deals with the modelling of joint and link flexibility as well as the problem of tip position control. Comparatively recently, the problems posed in the context of contact force control by manipulators with significant flexibility in their joint assemblies [18, 19] and structurally flexible links [20--25] have begun to be addressed. Given the acknowledged link  Chapter 1. Introduction  6  between physical compliance in the manipulator/sensor/environment system and the perfor mance of force control systems, the possibility exists that manipulators with flexibility may provide higher performance contact force control than totally rigid manipulators, provided that the additional dynamics introduced by the flexibility can be adequately compensated for in the controller design. Research into force control with flexible link manipulators is still in its early stages with the result that, as in the early stages of rigid manipulator force control, attention is being focused on stability issues (see Chiou and Shahinpoor [21,23]), fundamental understanding of the system dynamics (see Li [26]), feasible servo level control algorithms (see Pfeiffer [24], Latornell and Cherchas [22, 27]) and control strategies (see Pfeiffer [24], Tzes and Yurkovich [28], Matsuno, et al. [25]). The focus of the research reported in this thesis is the development and application of control algorithms for flexible link manipulator force control and the first steps in the integration of that control algorithm into an overall force/motion control strategy. It is to be expected that, as these fundamental issues are more fully explored and as flexible link research manipulators with more than one or two links are constructed, flexible link manipulator contact force control research will draw from and merge with the main stream of force control research. Indeed the beginnings of this merger is already evident in the work of Matsuno, et al. [25] which employs the hybrid position/force control framework for a flexible link manipulator configuration.  1.4  Contributions of the Thesis  The primary contribution of the research reported in this thesis is the application of a predictive control algorithm in the context of an overall control strategy for the control of the contact forces exerted by a structurally flexible manipulator on surfaces in its environment.  Chapter 1. Introduction  7  The models and techniques used in the course of the research which lead to the achievement of this goal have several important features. Modelling:  Kinematic and dynamic models of a robotic manipulator with structurally  flexible links interacting with surfaces in its working environment are derived. In contrast to the simple linear spring model for contact dynamics used in much of the research reported in the literature, the model derived here includes the effects of the local inertia and damping as well as the stiffness of the contacting regions of the manipulator tip and the environment surface.  Analysis shows that a pure stiffness model is sufficient when the mass of the  contacting regions is small, but when the mass of the surface regions deflected due to contact is comparable to the inertias of the links the additional dynamics in the environment model become important. The kinematics and dynamics models are derived in a form which can be readily transformed to less complicated configurations for rigid links and free motion (general motion in the absence of contact). Control Algorithm:  A general formulation has been obtained for the characteristic poly.  nomial of a closed loop system controlled by the Generalised Predictive Control (GPC) algorithm of Clarke, et al. [29]. The formulation is in discrete time transfer function form applicable to both the single input, single output form of the algorithm presented by Clarke, et al. and its multivariable extension. The result is shown to be useful in tuning the control law, in particular in selecting the algorithm parameters (horizons and weighting factors) to produce fast step response rise times and minimal overshoots from among the large set of parameter values which provide stable control for a given system configuration. A static equilibrium bias term extension has been derived for the predictive control al gorithm. It is applicable to both the single and multivariable version of the algorithm. A term which weights the deviation of the control inputs from the levels required for static  Chapter 1. Introduction  8  equilibrium is added to the control algorithm cost function. This addition results in a pro portional action feedforward term in the control law which generally reduces step response rise times. The static equilibrium bias term is particularly applicable to manipulator con tact force control because there exists a clear static equilibrium relationship between the joint torques and the contact force components. Contact control logic is introduced to deal with the problem of the discontinuity which exists in combined force and motion control when contact is made or broken. The contact control logic is implemented at a level just above the joint servo level control laws in the manipulator control hierarchy. Two operational strategies for the contact control logic which seek to provide smooth control through unexpected contact events are demonstrated. Linear Analysis:  Linear analyses of a variety of configurations of manipulators in con  tact with environment surfaces provide insight into the fundamental dynamics of the sys tem. The analyses use a combination of analytic and numerical techniques to shown that the contact force is dominated by different open ioop modes of the system depending on the effective contact stiffness of the surfaces. The transfer of dominance among the modes follows an orderly progression as the effective contact stiffness is increased in a given ma nipulator/environment configuration. In contrast, the control inputs are shown to provide excitation to all of the open loop modes. The implications of these results for the imple mentation of a discrete time predictive force controller are that the controller model should include all modes of the manipulator dynamics but the sampling rate can be selected on the basis of mode which makes the dominant contribution to the contact force. Simulation:  A general purpose computer simulation program, called TWOFLEX, which  includes the dynamics model and control algorithms was developed. The dynamics model in the program can be configured to represent either a single link or a two link planar  Chapter 1. Introduction  9  manipulator with revoliite joints. The links can be rigid or flexible or, in the two link case, a combination. The presence or absence of contact between the manipulator tip and the environment surface is determined by monitoring the relative positions of the tip and the environment. Contact can be made and broken repeatedly and the dynamics model is modified accordingly on each change of the contact status. The program was used to explore the dynamics characteristics of the system and to evaluate the performance of the force control algorithm and the overall force and motion control strategy. Force Control Performance Evaluation:  Simulations results from the TWOFLEX pro  gram demonstrate the performance of the explicitly adaptive force and motion control strat egy based on the predictive control algorithm for the following configurations and situations: • manipulators with one and two flexible links in contact with environment surfaces with effective contact stiffnesses varying over a wide range; • prototype model of the two DC electric motor driven, 7.5m long, main links of the Mobile Servicing System (MSS) for the proposed Space Station, Freedom; and • use of contact control logic to maintain control through the discontinuous situation of unexpectedly making contact with an environment surface.  1.5  Outline of the Thesis  As an overview, the remainder of the thesis is divided into chapters which discuss the details of the manipulator and environment models, the derivation of the control algorithm, the structure the computer simulation program TWOFLEX used to investigate the algorithm performance, the analysis of the system and the performance of the control algorithm applied to various manipulator configurations, and a statement of the major results of the research and recommendations for future work in this area. Following the final chapter is a detailed  Chapter 1. Introduction  10  list of references. The thesis concludes with appendices which provide the mathematical details of the equations of motion for several manipulator configurations, the expansion and minimisation of the control algorithm cost functions, and the derivation of the static equilibrium bias function. Chapter 2 presents the detailed mathematical models of the manipulator, environment and actuator systems. It begins with a brief review of the published literature concerning the modelling of the kinematics, structural characteristics and dynamics of manipulators with flexible links as well as the modelling of contact force generation and direct current electric motor actuators. In section 2.3 a kinematic representation of the manipulator and environment system based on a modified form of Denavit—Hartenberg homogeneous trans formation matrices [30,31] is developed. The modification allows the kinematic effects due to the link deflections to be incorporated in the homogeneous transformation matrix form. The chapter continues with the derivation, in section 2.4, of the kinetic and potential energy and equivalent viscous dissipation functions, and ultimately the equations of motion for a two link planar manipulator with flexible links in contact with a deformable environment. The general structure of the equations of motion is discussed and their simplification to various subset configurations of interest is described. The equations resulting from these simplifications are included in Appendix A. Section 2.5 presents expressions which relate the contact force components to the generalised coordinates of the dynamics model. The section also includes a discussion of the parameters of the contact dynamics model on the basis of the material properties of the contacting surfaces. The chapter concludes with the derivation of a model for a voltage controlled DC electric motor actuator and a discussion of the coupling of the actuator model to the dynamic model of the manipulator. In Chapter 3 the discrete time multivariable predictive control algorithm which lies at the heart of the force control strategy is derived. Following an introductory discussion of the particular challenges involved in force control for flexible link manipulators, section 3.2  Chapter 1. Introduction  11  reviews the Ceneralised Predictive Control (GPC) algorithm [29, 32, 33] from which many of the ideas used in the multivariable algorithm are drawn.  The section also includes  a derivation of the approximate closed ioop characteristics of a generic GPC controlled system.  Section 3.3 presents the derivation of a generic multivariable predictive control  algorithm. This derivation is supported by Appendix B in which the detailed expansion and minimisation of the controller cost functions appear. For specific applications such as force control, in which a clearly defined static equilibrium state exists, a new, augmented form of the algorithm is derived by including a static equilibrium bias term (with details in Appendices B and C) in the cost function, as described in section 3.4. This term adds a proportional feedforward element to the control law.  In section 3.5 the horizon and  weighting factor parameters of the control algorithm are discussed in terms of their roles in the tuning of the controller. The chapter concludes with three sections in which parameter estimation algorithms are reviewed, the implementation of the force control algorithm in the context of an overall adaptive force and motion control strategy is presented, and and the stability and convergence characteristics of the algorithm are discussed. The computer program TWOFLEX is described in Chapter 4. This program is a simulation of the continuous time dynamics of the planar two flexible link manipulator and environ ment system dynamics, the discrete time multivariable predictive control algorithm and the overall force and motion control strategy. It was developed in the course of the research to provide a facility for testing and evaluating the performance of the control algorithm as well as for the study of the open ioop dynamics of the system. The chapter is divided into sections in which a general description of the structure of TWOFLEX is given (section 4.2),  specific details of the integration of the equations of motion (section 4.3) and the imple mentation of the discrete time control algorithm (section 4.4) are discussed. A complete listing of the fully documented program (approximately 18,200 lines of FORTRAN) as well as a guide to its use is available as a UBC—CAMROL report [34]. The chapter concludes with  Chapter 1. Introduction  12  the results of a set of validation tests in which the homogeneous response of the manipu lator dynamics model in the program was compared with that from a program developed by Mah [35] which simulates an orbiting platform and manipulator system modelled as a chain of flexible links connected by flexible joints. Chapter 5 presents the results of linear analyses of the dynamics of several manipulator and environment configurations, discusses the implications of those analyses for successful force control, and presents a collection of simulation results which evaluate the performance of the control algorithms under a variety of circumstances. In section 5.2 linear analysis of a single link in contact with a deformable environment reveals that the domination of the system dynamics by particular modes changes in an orderly fashion as the effective contact stiffness that exists between the manipulator tip and the environment increases. The important implications for force control of this changing modal structure are discussed. The performance of the adaptive predictive control algorithm applied to force control is demonstrated in section 5.3 for the case of a single flexible link. Contact with three surfaces with widely differing effective contact stiffnesses is examined and it is shown how the results of the linear analysis and the approximate closed ioop characteristics calculated from the control law are applied to select high performance controllers with fast rise time and negli gible overshoot. The problem of dealing with the discontinuous nature of the relationship that exists between the contact force and the system state when contact is made or bro ken is addressed in section 5.4. A new control level, called the contact control logic level, which supervises the servo level predictive control algorithm is introduced. Two operational strategies for the contact control logic are proposed and the overall system performance is examined. In section 5.5 the analysis and performance evaluation of the multivariable force controller for a two link manipulator contacting an environment surface is presented. The linear analysis techniques applied in the single link case are employed and reveal expected extensions of characteristics from that case as well as additional complexities which depend  Chapter 1. Introduction  13  on the configuration of the manipulator, in particular the joint angles. The results of this analysis, in conjunction with the approximate closed ioop characteristics calculated from the rnultivariable control law are used to select controller parameters which provide fast rise times and minimal overshoots. The controller performance is demonstrated for both rigid and flexible link manipulators. The effect of interaction between the contact forces responses and the influence of the highly nonlinear manipulator dynamics on the controller performance are also discused. A final application example is presented in section 5.6 in which force control of a prototype model of two links of the Space Station Freedom Mobile Servicing System (MSS) manipulator is demonstrated. The long, massive links of the MSS are driven by voltage controller DC electric motor actuators which introduce additional dy namics as well as saturation type nonlinearities. Portions of the work presented in Chapter 5 have already been published 122,27]. The thesis concludes with a summary of the major results and a statement of the original contributions of this research in Chapter 6. Several recommendations for directions in future research in this area are also included.  Chapter 2  Manipulator Modelling  2.1  Introduction  The manipulator model, which forms the basis for the computer simulation program TWOFLEX described in Chapter 4 and the simulation study results presented in Chapter 5 is developed in this chapter. The model is that of a planar two link manipulator with structurally flexible links which can make contact with a deformable environment. The model consists of four parts: 1. the manipulator configuration model, that is, the kinematics and dynamics of the serial chain of links; 2. the link model, that is, the structural kinematics and dynamics of each link as a flexible body; 3. the contact model, that is, the kinematics and dynamics of the regions of the manipu lator tip and the environment that make contact. It is the interaction of these regions that gives rise to the contact force which is the quantity to be controlled; and, 4. the actuator model, that is, the kinematics and dynamics of system which transform the control signals into driving torques applied to the links. This chapter begins with a review of the literature on the modelling of the kinematics and dynamics of structurally flexible manipulators, contact dynamics, and DC motor actuator systems. In section 2.3 the kinematic models of a planar two link manipulator with flexible 14  Chapter 2. Manipulator Modelling  15  links and a deformable environment are presented. Following, in section 2.4, is the derivation of the equations of motion of the system and a discussion of the general structure of those equations and the nature of the coupling among them. Section 2.5 presents the derivation of the expressions for the contact force in terms of the generalised coordinates of the dynamics model as well as a discussion of the selection of numerical values for the parameters of the contact model. A model of an armature voltage controlled DC electric motor to be used as an actuator is derived in section 2.6 and the coupling of the motor model to the equations of motion is discussed.  2.2  Literature Review  Kinematics:  The use of Denavit—Hartenberg [30] homogeneous transformation matrices  to describe the kinematics of robotic manipulators with rigid links is well established [36—38]. The extension of the Denavit—Hartenberg concept to describe the additional kinematics due to structural flexibility of manipulator links was made by Book [39]. This approach is attractive in that it is a natural extension of a widely used method. A homogeneous transformation matrix approach, slightly different in form from that of Book [39], is used in section 2.3.1 to obtain the position and velocity vectors for elements of mass of the manipulator links and in section 2.3.3 for the contact region masses. An alternative approach to the kinematic analysis of flexible manipulators has very recently been proposed by Chang and Hamilton [40]. The concept of their Equivalent Rigid Link System (ERLS) is to separate the rigid body and the structural dynamics of the manipulator such that the global motion is described by a large rigid body motion with a superimposed small motion due to the structural flexibility. Structural Dynamics:  The consideration of the structural flexibility in the links of a  robotic manipulator introduces a challenging analytical problem, namely, the links can no  Chapter 2. Manipulator Modelling  16  longer be viewed as lumped masses whose motions are described by the motions of their centres of mass; instead the links must be viewed as distributed parameter systems. As a result, the motions are explicitly dependent on spatial as well as temporal variables and must be described in terms of partial differential equations. Closed form solutions for systems other than those with very simple geometry and uniform distributions of mass and stiffness are rare and, as a result, approximate solution methods, generally involving discretisation of the spatial coordinates, are widely used. Two major types of spatial discretisation are available [41]: expansion of the solution into a finite series of given functions and the lumped parameter approach.  Series expansion methods are more widely used than the  lumped parameter approach and are divided by Meirovitch [41] into two classes, Rayleigh— Ritz type methods and weighted residual methods. The majority of the literature on flexible link manipulator modelling employs either the method of assumed modes [23, 42—45] or the finite element method [46—48], both of which are Rayleigh—Ritz type methods. The use of series expansion discretisation methods raises the issues of selection of appropriate functions for use in the series and of the number of terms required in the series [43,49]. For highly flexible systems consisting of many bodies, such as flexible spacecraft like the Space Station Freedom, the finite element method is often used and an important issue is the selection of appropriate subsets of modes to make computational solution of the system feasible [50—52]. In section 2.3.2 the mode shape functions for a cantilever beam are derived in closed form. These functions are employed as admissible functions in the method of assumed modes in the derivation of the equations of motion for the planar two flexible link manipulator in section 2.4. Equations of Motion:  Early analyses of the dynamics of manipulators with structurally  flexible links, such as that by Hughes [53], were largely inspired by the development of the teleoperated Space Shuttle Remote Manipulator System (SSRMS), or Canadarm. Analyses  Chapter 2. Manipulator Modelling  17  of more general flexible robotic manipulators, such as that of Kelly and Huston [54], often drew on the techniques developed for the analysis of flexible space structures [55,56]. A seminal work is that of Book [31], which employs homogeneous transformation ma trices to describe both joint and link deflection kinematics and uses Lagrange’s equation to derive the equation of motion. The analysis assumes that the link deflections are small, such that the links are linearly elastic, and employs the method of assumed modes. An al ternative approach to the modelling of link flexibility, which is briefly discussed by Book, is the introduction of “imaginary” joints into a strictly rigid link dynamics model such as the Newton—Euler formulation of Walker and Orin [57]. Book concludes that the computational complexity of the Lagrangian approach is significantly greater than that of an equivalent Newton—Euler formulation. The work of Silver [58], however, suggests that this need not necessarily be so. In any case, the Lagrangian formulation provides better physical insight into the interaction of structural and rigid body dynamics. The majority of recent analyses of flexible manipulator dynamics employ energy methods to derive the equations of motion [23, 42—45, 48, 49, 59, 601.  Those that use a Newton—  Euler approach [46, 47,61] generally apply a lumped parameter discretisation to the spatial coordinates of the link structural dynamics. Dynamic analyses of flexible link manipulators making contact with their working en vironment are rare in the literature. Most researchers [21, 23, 24, 26] model the contact force as arising due to the compression of a simple linear spring connecting the manipulator tip to the environment, thus lumping the contact dynamics into a single element. In the course of the present research a more complete development of the dynamics of a single flexible link contacting a deformable environment has been achieved and reported in the literature [22,27]. That model includes a dynamic model of the areas of contact between the manipulator tip and the environment surface. The model considers the stiffnesses of  Chapter 2. Manipulator Modelling  18  the contacting surfaces, energy dissipation within the materials and the inertia of the sur face material in the regions surrounding the point of contact on each surface. Section 2.4 presents an expanded version of that development. The equations of motion for a planar two flexible link manipulator in contact with a deformable environment are derived using Lagrangian dynamics. Contact Force Model:  In general a contact force model should include impact dynamics,  inertia effects, energy dissipation and elastic deformation. Whitney [2] notes that “control analyses of arms in contact with an environment are rare” and that most papers “contain no model of the origin of the contact forces themselves. Those that do are restricted to modelling force as arising from deformation of some elastic elements.” The literature on the solid mechanics of contact is extensive [62], however, the vast majority of these analyses assumes that the contact is quasi-static. Analyses of dynamic effects are generally concerned with the generation and propagation of stress waves in the contacting bodies. For contact which does not involve plastic deformation of the surface Johnson [62] shows that the dynamic response of an elastic half-space can be modelled with good approximation by an elastic spring in parallel with a viscous damper, the latter representing the energy dissipation of the stress waves. The theory of elasticity, in particular the results derived by Hertz for the deformation of spherical surfaces in contact [63, 64], provides some insight into the relationship between contact forces and surface deflections under quasi-static conditions. The contact model included in the dynamic analysis of section 2.4 uses the parallel spring and damper model as well as a surface mass. Results from the theory of elasticity are used in section 2.5.1 to provide estimates of the numerical values of the model coefficients based on physical material properties.  Chapter 2. Manipulator Modelling  19  F  y  Fx  y  (t) 1 w , x  x  Figure 2.1: Geometry of a two link flexible manipulator in contact with a deformable surface. Actuator Models:  The armature voltage controlled direct current electric motor actua  tor system discussed in section 2.6 is widely used and accepted models are readily available both in the general literature [65] and for specific application to robotics [14]. 2.3 2.3.1  Kinematics Manipulator Configuration Model  Figure 2.1 shows a schematic representation of a planar two link manipulator with struc turally flexible links in contact with a deformable environment. The geometry of the system  Chapter 2. Manipulator Modelling  20  is described in terms of the four coordinate frames shown in Figure 2.2. Frame F° is the fixed, world coordinate frame with the joint of link 1 located at its origin. Frame F’ is fixed at the tip of link 1, Frame F 2 is fixed at the tip of link 2 and frame FD is fixed at the point of contact between the surfaces of link 2 and the environment. The orientation of the nominal centreline of link 1 with respect to the world frame is given by the angle 19-, mea sured counter-clockwise from the i axis of F°. Likewise, 82, measured counter-clockwise from the i axis of F , gives the orientation of the nominal centreline of link 2 with respect 1 to F’. The links are modelled as elastic beams clamped to rotary actuators at the joint ends. The elastic deformations of the links are given by wk(xk, t), the displacement of actual (curved) centreline of the link from the nominal (straight) centreline of the undeformed link. The subscript k indicates the link number. The distance along the link,  Xk,  is measured  along the nominal centreline relative to F’. The geometric relationships among the coordinate frames can be compactly described using homogeneous transformation matrices [30]. The transformation between frames F’ and F° is 1 c ,H 0 ,(t)  s,  —s 0 L,c, 1 c  0 L s 1  =  (2.1)  0010 0001 where L, is the length of link 1, .s,= sin 8 (t) and c 1 = cos8i(t). Similarly, the transforma 1 tion, 2 C  H, ( 2 t)  2  0 L c 2 s 2 L  =  000  1  (2.2)  Chapter 2. Manipulator Modelling  21  C  2  ii 2  02)  T  5C //  (t) 2 w , x  ,“  ‘2 I  / / Ii  ,t 1 1(x w  •0 12  1 -  /  01 ,  •0 Ii  FO  Figure 2.2: Coordinate frames for a two link flexible manipulator in contact with a deformable surface.  Chapter 2. Manipulator Modelling  22  where L 2 is the length of link 2, s = sin 0 2 (t) and c 2 = cos & 2 (t), accounts for the rigid body 2 components of rotation and translation between frames F 2 and F’. An additional rotation and translation due to the structural deformation of link 1 must also be considered. Based on ideas introduced by Book [31], the structural deformation of link 1 relative to the undeformed centreline is described by the transformation matrix,  D,(t)  where  cosa,  1 —sina  1 sina  1 cosa  10 0 w  0  0  10  0  0  01  0  0 (2.3)  =  10 = to  and  wi(0, t)  a  0 8w, = to, 0 = (Ix,  that is, a 1 is the local, instantaneous slope of link 1 at its tip. Premultiplying 2 H ( t) by D ( 1 t), we obtain the homogeneous transformation matrix 1 H ( 2 , t) which accounts for both the rigid and flexible body effects in the geometric description of link 2 relative to and projected on to F’:  (t) 12 IT  where  a2=  =  D,(t) 1 11 ( 2 , t)  =  2 1 Ca  2 1 a  0  a1 2 8  Ca 1 2  0 L2sa 1 2 +  0  0  1  0  0  0  0  1  10 W  (2.4)  (t)] and caj sin [w’ (t) + 2 10 (t) + 0 0 (t)]. Finally, premultiplication 2 = cos [w 2  of 2 H,, ( (t) gives 0 1 , 0 t) by H H ( 2 , t) which describes the geometry of link 2 relative to and projected on to the world coordinate frame F°: C 1 2  H ( 2 , 0 t)  =  H ( 1 , 0 t) H,, () 2  1a 3 2  1 0 L,c  —  1+ 0 L,s  w, s 0 , +  2 1 L2CIa  +  2 L231a  C 1 W 1 0  ia 5 2  2 1 C1a  0  0  1  0  0  0  0  1  =  ,  (2.5)  Chapter 2. Manipulator Modelling  where  1a2=  [ ( t) + sin 1  w’ ( 0 t)  23  12 cos [8 (t)j and ci 2 (t) + 1 +8  w ( 0 t)  (t)j. 2 +0  The structural deformation of link 2 introduces additional rotation and translation which is described by 2 0 —sin  2 cos  where  20 w  cos c 2  20 0 w  0  0  10  0  0  01  sin c2  D ( 2 t)  =  0  and  (0,t) 2 w  2 c  =  20 w  (2.6)  2 8w = 2 Ux  The position, relative to and projected on to F dm located a , , of an element of mass, 1 1 distance —x 1 from the tip of link 1 is —xl  r(t)=  (—x t) w 1 ‘  .  (2.7)  0 1 Premultiplying r(t) by 0 H ( 1 , t) gives the position, relative to and projected on to F°, of : 1 drn 1 (L =  r(t)  1 (L  —  xi)ci )s 1 x  —  s 1 w  c 1 +w  (2.8)  1 Likewise, the position, relative to and projected on to F , 2 , of an element of mass, dm 2  Chapter 2. Manipulator Modelling  24  located a distance —a 2 from the tip of link 2 is  (—a t) 2 w ,  r(t)  (2.9) 0 1  and premultiplication by H (t) gives the position, relative to and projected on to F°, of 2 , 0 : 2 dm c 1 L  —  s+ 1 L  11)131  2 + (L  C 1 W  2 + (L  —  ) ci 2 x 12  —  )3 2 1a22 +  W2C1cii2  (2.10)  1 Differentiating r(t) and r(t) with respect to time yields the velocities, v(t) and v(t), of the elemental masses dm 1 and dm , relative to and projected on to .F°: 2 —  1 [(L —  v(t)  =  r(t)  1 [(L  —  x) s 1 + wici]  ) 1 x  Oi  —  6 +  Ci  C 1 W  (2.11)  —:‘‘  0 and —  v(t)  =  -f-r(t)  (Lisi + c 10 Si w ) 1  (Lici  —  s 1 w ) 1 0 Si  —  —  iosi  —  2 (L  1 + (L c 10 zij 2  —  —  dt  +222  ) 122 2 x  1 (ê  +  1+ (ô  +  82)  12 xc ) 2  (& +  1 (ô  W  +  + —  2 i2S1a  +w+ &2)  —  82)  82)  th2Clai2  0 0 (2.12)  Chapter 2. Manipulator Modelling  25  In these expressions dotted quantities denote derivatives with respect to t and primed quantities denote derivatives with respect to x. The position vectors, r(t) and r(t), and the velocity vectors, v(t) and v(t), are used in section 2.4 to form the kinetic energy, potential energy and dissipation function expressions for the manipulator. 2.3.2  Flexible Link Model  Structurally flexible manipulator links are distributed parameter systems and thus have an infinite number of degrees of freedom and must be modelled by partial differential equations with both temporal and spatial independent variables. The inherent difficulty of solving partial differential equations with space dependent coefficients, particularly in satisfying boundary conditions, makes the use of approximate solution methods attractive, if not essential. For this research the method of assumed modes [41] is used. That is, the spatial co ordinate is discretised into a series expansion of admissible functions each multiplied by a generalised coordinate. The mode shape functions of a cantilever beam are employed as the set of admissible functions. For the case of an Euler-Bernoulli beam, in which the effects of rotary inertia and shear deformation are neglected, the mode shape functions are readily derived in closed form [66]. While it is clearly evident that the boundary conditions for each member of a chain of flexible manipulator links connected by torque producing actuators are much more complex than those for a cantilever beam, it should be noted that the method of assumed modes permits the use of any set of admissible functions; the number of terms in the series determines the accuracy of the approximation. In particular the issue of whether a pinned—free or a cantilever (fixed—free) beam model provides the more appropriate set of admissible functions has been resolved both experimentally [43] and analytically [49]. The accuracy of the predicted dynamic response, with a given number of included modes, is  Chapter 2. Manipulator Modelling  26  z  F (xt) S  +  dS dx dx  —  M M  +  dM dx dx  —  w(x,t)  _1_  dx  Figure 2.3: Free body diagram of a differential beam element for the derivation of the Euler—Bernoulli beam bending equation. found to be greater when cantilever beam mode shape functions are employed than when those for a pinned—free beam are used. Force and moment balances on a differential element of a beam (see Figure 2.3) lead to the beam equation for an Euler—Bernoulli beam [66j: ôw(xi)  (EI(x)0’t))  +  0,  (2.13)  where p(x) is the mass distribution along the length of the beam and EI(x) is the distri bution of flexural rigidity. This partial differential equation is separable. Letting w(x, t)  =  4(x)?/(t), the partial derivatives can be expressed as: =  ô w 2 (x,t)  and  qb  8x2  —  —  and the beam equation rewritten as =  (EI”)”  -  (2.14)  Setting each side of equation (2.14) equal to a separation constant, w , yields 2 =0  (2.15)  Chapter 2. Manipulator Modelling  27  and (EJqY’)”  —  w p 2 q  =  0.  (2.16)  Equation (2.15), subject to a pair of initial conditions, describes the temporal variation of the shape of the beam while equation (2.16), combined with a set of four boundary conditions, describes the spatial variations. It is this latter equation that provides the mode shape functions which will be used as admissible functions in modelling the flexible link dynamics. Assuming p and El to be constant along the length of the beam, equation (2.16) is rewritten as =  —  0,  (2.17)  where =  (2.18) Equation (2.17) is a linear homogeneous ordinary differential equation with constant coef ficients for which the solution is 4(z)  =  Asinh/3x + B cosh3x + Csin/3x + D cos!3x.  (2.19)  The boundary conditions which facilitate evaluation of the arbitrary constants in equa tion (2.19) are, for a cantilever beam of length L, zero displacement and slope at the fixed end and zero shear and moment at the free end; in mathematical terms w=o  EI  8w  =  0  =  0  EI4  =  0  =  =  0,  (2.20)  or  ‘7H=L  —  0  IL  0  =  0.  (2.21)  Chapter 2. Manipulator Modelling  28  Application of these boundary conditions to equation (2.19) yields the frequency equation: cos5L coshBL + 1  =  0,  (2.22)  for which there are an infinite number of solutions, {i3, i  =  1,.  .  .  ,  oo}, each of which is  related to the natural frequency, w, of one of the modes of the beam by equation (2.18). The corresponding mode shape function for the ith mode is q(x)  =  cosh,Bx  —  cos/3x  —  x 2 (sinh,8  —  sinf3x)  (2.23)  In this form the mode shape functions are normalised such that JL =  L  (2.24)  and they are orthogonal, that is,  f  L  4j(x)j(x) dx  -0,  i  j.  (2.25)  These mode shape functions are used as admissible functions for the method of assumed modes which is employed in the derivation of the equations of motion in section 2.4. The slope of the mode shape function for the ith mode is 4(x)  =  =  _,i  [sinix + sin jx  +  (::  these slope expressions are used to calculate  =  , 0 w  (cos/3x  —  cosh/3ix)] (2.26)  the local, instantaneous slopes of  the links at their tips which appear in the structural deformation transformation matrices given by equations (2.3) and (2.6). A final aspect of the kinematics of a flexible beam that must be noted is the fact that as the beam deflects, its length, measured along the nominal (undeformed) centerline decreases. The magnitude of this axial foreshortening can be calculated by considering an element of the deflected beam as shown in Figure 2.4. The decrease in length of the beam element is  Chapter 2. Manipulator Modelling  29  1 Figure 2.4: Axial foreshortening of a differential beam element. d  =  ds  —  dx. From the geometry of the situation we see that In  dx ds  cos  cos  ( qSbJj  (2.27)  \i=1  that is, the cosine of the local, instantaneous slope. Thus d  cos =  (2.28)  [1  and so =  jL  [i_ cos  ds.  (2.29)  Rewriting the cosine function in this integral as a series expansion, \2  /n  L  1  =  j  we see that the to 2.3.3  —  1—  Z=121  —  () 214!  —  ds,  (2.30)  is O(’].). Since the elastic deformations of the links, which are proportional  are assumed to be small the foreshortening of the links will be neglected. Contact Model  A manipulator making and breaking contact with its environment must be viewed not as a single dynamic system but rather as two closely related systems. At the instant of contact  30  Chapter 2. Manipulator Modelling  a kinematic constraint, such that the position, velocity and acceleration of the contacting surfaces are identical, is enforced on the system for the duration of the contact. The contact force arises from the interaction of the contacting surfaces. Conversely, on breaking contact the kinematic constraint is removed, the surfaces move independently and the contact force is, by definition, zero. In order to predict the contact force which will arise between the manipulator tip and the environment surface a contact force model is required. The model used in the present research is based on results reported by Johnson [62] which show that the dynamic response of an elastic half-space can be modelled with good approximation by a linear elastic spring in parallel with a viscous damper, the latter representing the energy dissipated by the stress waves radiating through the material surrounding the region of contact. This model neglects reflection of these stress waves. As indicated in the inset of Figure 2.1, this model is adopted by representing the contacting surfaces of the link and the environment as masses on damped linear elastic foundations. The contact coordinate frame, .Fc, is located at the point of initial contact between the manipulator tip and the environment. Relative to and projected onto the world frame, F°, the origin of .Fc is located at The contact masses, m and  me,  Xw+Yw.  represent the inertia of the contact regions of the  manipulator tip and the environment, respectively. These masses should not be confused with the total mass of those bodies. It is the compression of these regions that gives rise to the contact force. The deformation of the surfaces during contact is represented by the motion,  ei+Ei  of the total contact mass m (the sum of the contact region masses m  and me). The contact coordinate frame, Fc, is chosen with the same orientation as the fixed world frame, F°. This is tantamount to assuming that the contact force is measured in world coor dinates, a valid assumption since such transformations are easily and routinely performed by the hardware and software associated with wrist force sensors. The transformation between  Chapter 2. Manipulator Modelling  fC  31  and .F 2 is thus simply a rotation of  =  H ( 2 t)  —  [8 ( 1 t) + w’ (t) + 8 0 (t) + w 2 (L)]: 0  2 C1i2a  2 1 8 2 1 a  0 0  1cj2 8 2  2 2 1 C1a a  0 0  o  0  10  0  0  01  and hence the transformation between  FC  (2.31)  and F° is a simple translation, obtained by  premultiplying 2 (t) and Ho, 2 (t): 2 H , (t) by D 1  H ( 2 , 0 (t) H 2 t) D ,(t) 2  0 0  c 1 L  —  10 + w 1 s  C1a L 2 1  W2oSlai2  s +c 1 10 + L w 1 0 1 0 L 112 + 1 s 2 c1a 0 w2 2 =  .  001  0  000  1  (2.32)  The position of the contact mass, m, relative to and projected on to F’, is  =  U  (2.33)  0 1 Premultiplying r(t) by 0 H , (t) gives the position vector, relative to and projected onto LF°: c 1 L r(t)  =  Ho,(t)r(t)  —  s + 1 L  10 + L2C1a w 1 s 2 1 wiOc  +  2 1 L281a  =  —  +  1 1 8 W20 2 a  +  2 E  C 2 W 1 12 0  +  Ey .  (2.34)  0 1 The velocity vector, v(t) for the contact mass, relative to and projected onto F°, is obtained  Chapter 2. Manipulator Modelling  32  by differentiating r(t) with respect to t: —  (Lisi + c 10 Oi w ) 1  —  —  12 a 1 (L2s  +  +  wloclaj2) W2o81ai2  v(t)  =  r(t)  =  (Lici  —  10 w ) 1 s  01  c 1 i 0 + +1  12 a 1 C 2 (L  —  ) 2 WioSiai  1 (é  +  +  +‘th2oClcxi2  + E  Wio  +  +  02)  y  0 0 (2.35) The position vector r(t) and velocity vector v(t) are used to derive the kinetic en ergy, potential energy and dissipation function expressions for the contact dynamics in section 2.4.  2.4  Derivation of Equations of Motion  Given the position and velocity vectors for the elemental link masses and the the contact region mass (derived in section 2.3), the equations of motion for the manipulator and envi ronment system are derived using the well known energy method of Lagrangian dynamics. As noted in section 2.2 this approach to the derivation of equations of motion for robotic manipulators is widely used [23,31,48,60]. It has the advantage that the resulting equations are in a form well suited to numerical integration for the purposes of simulation studies. Also, in the case of structurally flexible manipulators, the problem of accounting for the movement of the centres of mass of the bodies as they deform is avoided. The structure of the resulting equations of motion gives clear insight into the interaction of the rigid body, structural and contact dynamics. During the early stages of the research the equations of motion for several simple con figurations of the manipulator and environment system, such as a single flexible link with  Chapter 2. Manipulator Modelling  33  unidirectional contact, were derived manually [22]. The equations for the two flexible link system, presented below, were derived with the assistance of the Maple V algebraic com putation system [67]. Maple V proved invaluable in helping to avoid transcription and other mechanical errors which often arise in the course of manual algebraic manipulations. Furthermore, Maple V includes a rudimentary FORTRAN code generator, which was used to translate (with minimal errors) the equations of motion into source code which served as starting points for the simulation subroutines which calculate the terms of the inertia matrix and the right hand sides of the equations. As well, the J.TEX code generator in Maple V was used to assist in the preparation of the equations of motion for typesetting in this document. The total kinetic energy, 7, of the system is  =  2  JM  v dm 1 +  2 M 1  v dm 2+  mcvT  v,  (2.36)  where Mk is the total mass of link k. The first two terms are the kinetic energies of the links and the last term is the kinetic energy of the contact region. Expanding the vector dot products in equation (2.36) and evaluating the integrals over the link masses (assuming constant distributions of mass over the lengths of the links), the total kinetic energy is rewritten as:  7-  =  o 1 Io  +  Iiiiiioi 1  2 (L + +M +  —  I22j  S (L 2 1 Ô a  —  ‘21j  + +  [102  +  (b +e) fl2]  1 (ô  2 2 ci + (LiL i + [M + 10s2)  +  +  L2WloSai2&1  +  I42i2i] (i  )+ 2 é  1o) 1 c 2 L 2  10 + +w  62)  iitb +  h12’2i  110 8 1 L 2 +M  Chapter 2. Manipulator Modelling  i + 2 + (LiCai 4m  ô 2 WloSal  [(L +  34  +  io) 2 ai  + (L + t4)  (+é+  +  14221  1+ (a W2oClai2)  1 (ô  +  10 W  + 02)]  +m [(Lici  2 w 1 s ) 12 0  1 (  +  1o 1  +  si + 0 —me, (tiii  +m  10 01 + w ) 1 s ) W2o31a 2 1  2 1 (L2C1a  —  ti)  +  i+ ci) Ô 0 12 s 2 1 + (L —m [(Lisi + wi —  +  x  02)]  ci + tb2ocIaj2) Ey 0 + m (zhi  20 + (L w 1 12 c 2 2 — wlocaj2) Ô 1 [— (L18a  tui L o ] ai + 2 2 Wioai 01 + 2 L ) +m [(LiL tv + 1 0 +m 1 Ca + wbosai2) 20 [(L 2  —  5a 0 W2 2 1 ) tub]  1 + t14 (o 0+  Wa W1 C 0 2 1 2  1 (ô  +  10 W  +  02)  02)  u 10 t 8 1 L +]  .  (2.37)  The assumption of constant mass distribution in the links is made for simplicity; any mass distribution function,  pk(xk),  may be chosen and the integrals evaluated in closed  form or numerically, as necessary. The summations in equation (2.37) are the result of the application of the method of assumed modes [41] to discretise the spatial variations of the link motions. The upper limits of summation,  k,  are the number of admissible functions  included in the structural model of link k. The subscripted I factors are integrals of the admissible functions. They are discussed in detail below, following the statement of the system potential energy. The link kinetic energy terms in equation (2.37) are in agreement with an appropriate subset of the kinetic energy terms for a model of an orbitting space platform based mobile flexible manipulator reported by Chan [681. Since the kinematic representation used by Chan is markedly different from that described in section 2.3.1 the agreement of kinetic energy expressions serves to validate the method used here. The total potential energy, 1), of the system is 1)  =  —J  1 M  gT  1 r dm —  J  gT  r dm 2  Chapter 2.  Manipulator Modelling  35  1 1 1 2 ii’tb + 3 +>T 1 8 (Lici +k  —  8 (Ljsi + +k 2  10 W 1 3  +  2 1 L2C1a  10 W 1 C  +  2 1 L231a  +  W2oS1c2  —  xw  —  —  W2oClai2  Yw  E)  2  )2  2  j.  m  —  where g is the gravity field vector. The first two terms are the gravitational potential energies of the links. They are followed by the elastic potential energies of the links due to their structural stiffnesses. The last four terms are the elastic potential energies of the contacting surfaces of the manipulator tip and the environment with the point of initial contact located at  xi  The zero condition for the springs used to represent the  +  contact stiffness is that of zero force. The  bk  factors in equation (2.37) represent the rigid body components of the link  moments of inertia about the joint axis: ‘ok =  1  (Lk  —  Xk)  2  J Mk  (2.39)  dmk.  The other subscripted I factors in equations (2.37) and (2.38) represent integrals which involve the set of admissible functions used as assumed modes to model the link structural dynamics. Given a set of admissible functions, these integrals evaluate to constants in the equations of motion. Employing the cantilever beam mode shape functions derived in section 2.3.2 these integrals are evaluated, for mode i of link k, as follows: ‘iki  =  J  (Lk  —  xk)  qki(xk) dmk  =  2--  (2.40)  k 3 I  where 1 k1 is the solution of the cantilever beam frequency equation (equation (2.22)) for 3 the ith mode of link k and pk is the mass per unit length of link k, ‘2ki  =  J  Mk  2 k(Xk)  dmk  =  pkLk = Mk,  (2.41)  Chapter 2.  Manipulator Modelling  JLk  36  (82)2  Elk  1 3 ki  dxk  =  EIkLk  MkWL,  (2.42)  where Elk is the fiexural rigidity of link k in bending about its joint axis, and, ‘4ki =  I  4’k(xk)dmk =  JMk  pk 2—  2 (sinh/3 L k k  smI k 3 Lk  Ni  / k 3 z \cosh/3kJk+cos/3k:LkJ  (2.43)  All of the I factors are retained as general constants in this derivation and in the subse quent simulations to facilitate the use of any arbitrary set of admissible functions and link dimensions. Assuming that the energy dissipation in the system can be modelled using equivalent viscous damping, the total dissipation function, R., for the system is +  =  1  2 ’ 1 1 b 4 2 +  +  [(Lisi + wi ci)Ôi + (L 0 12 i 2 a+ 1 S 10 +W  +  W2oSlai2  8 [(Lici +b 1 + c 10 +w  1  —  +  —  1 + o + 2) (ê  ) 2 w 1 s 12 0  (o + w + 82)  J  ) 1 s 0 wi  C 2 W 2 0  ) o 2 W 1 a 1 C 2  ci 2 (L + 12  —  Ej  1 ezEx  where the subscripted b terms are equivalent viscous damping coefficients. The total dissi pation function is somewhat analogous to the total potential energy function. The first two terms represent the energy dissipated due to friction in the link joints. They are followed by terms for losses in the links due to structural damping. The last four terms are the energy dissipated in the materials of the contacting surfaces of the manipulator tip and the envi ronment. The dissipation of energy in the links and the contact regions is due to hysterisis associated with cyclic stressing of the materials. The selection of damping coefficient values  Chapter 2. Manipulator Modelling  37  is somewhat complicated by the fact that the energy dissipation is dependent on a number of factors including temperature, stressing frequency and stress level. Lazan [69] provides a collection of experimentally determined loss factors which are the ratio of the energy dissipated per cycle to the maximum strain energy in the material. Assuming harmonic ex citation these data can be used to estimate damping coefficient values of lumped parameter systems following the method described by Meirovitch [70]. However, for more general situ ations Gaylord and Gaylord [71] recommend that the damping in most structural members be assumed to be a few percent of critical. Using equations (2.37) and (2.38) to form the Lagrangian for the system, C’ T  1), the  —  equations of motion are given by Lagrange’s equation, augmented with a disspation term: d (ôL’ o)  &C =  —  oR.  +  Q  (2.45)  —  where q is one of the system variables, 1,  02,  ,i  =  ,j  1,... ,ni},  selected as generalised coordinates and  =  1,... ,n},  e,  Q is the corresponding generalised force. The com  plete set of equations of motion consists of 4 + ni + n 2 strongly coupled, nonlinear, second order, ordinary differential equations. The general structure of each of the equations is  Q + bj + where I  =  I(q) is a vector of inertia terms, j  kT + g(q) + N(q,  =  =  (2.46)  j(t) is a vector of the second time derivatives  of the generalised coordinates (generalised accelerations), is a vector of dissipation coefficients,  4),  Q is a generalised force, b  =  b(q)  4(t) is a vector of the first time derivatives of  the generalised coordinates (generalised velocities), k  =  k(q) is a vector of stiffness terms  (which are generally weakly nonlinear functions of the generalised coordinates due to the geometry of the system), q  q(t) is a vector of the generalised coordinates, g(q) is the  Chapter 2. Manipulator Modelling  38  gravity term and N(q, ) is a collection of centripetal and Coriolis terms which involve products of the generalised velocities. For the O generalised coordinate the generalised force is  , 1 T  the torque applied to joint 1,  and, after simplification and collection of only second time derivative terms on the left hand side, equation (2.45) yields the equation of motion which will be referred to as the rigid body equation for link 1: [i +  102  2 (L + 0 12 + 2 c 2 L w+1 +M ioai L ) 722  22 I 2 iz”,b + i,b 2 I j +1 +L + 0 +m (L + w +  wo  2 (Liat 2  —  —  ) 142jI’2j 2 WioCaj  2 (LlW2oSai2  —  12 C 2 1 L a  2 1 WIOW20Ca  1 o 2  L 2 + -M  1 +L 12 c 2 I:: + m (L  —  2 1 (L1Ca  +  WloSai2)  +  1 2 2jI’2j  —  2 1 (L18a  —  —  oSa 2 ) ]  0 + 12 +m (L + w ca 2 LiL  [102  (Lic L 2 M + 12  —  0 + 12 +m (L + w Ca 2 1 L  —  j=1  2 L1W2OSa  ) 12 Sa 10 +W  W2OC1a 2 1 ] E  +m [Lici  1 8 0 W2 2 1a ] Ey  =  1 T  wiosi + 1 c1a L 2  —  —  W1OW2QCa 2 1 )]  12 (Lia  s 2 w 1 L 1 0 + L2wlosaj2 + 2  10 + L w 1 112 + s 2 —m [Lisi + c —  + L2wlosai2 +  +I22j  w s [c + 1 Li 2+L 0 ] 2 + m 12  +  L  wlocai2)  j=1  +  1 S 0 L2W1 2 ))j a i  I11i11i  1 + ) 2 (L 12 c 2 L + [M +  —  2jo2j  WloW2oCai2)] 02  2 + wlosa ] 12 + [Liai  Ij/7  —  (L + s 35 1 +b  C 1 W 1 0  +  2 1 L251a  +  W2oClai2)  [(Lisi + c 10 + L2Sai2 + w 1  W2oClai2)  6  Chapter 2. Manipulator Modelling  2 1 (L231a  3 (Lici —b +  —  22 w 1 c 0 62 +) +  10 W 1 S  (L2C1a2  2 1 L2C1a  W2OS1a 2 1 ) 62  —  +k (Lisi +  10 W 1 C  8 (Lci —k  112 c 2 s 1 w 1 0+L  —  +gx (MiLsi +  —gy  c L 1 (_M  —  +  39  10 W 1 S  —  —  W2031a 2 1 )  —  + mc +  L2S 1 a 1 2  —  + —  [(Lici  —  tii 1 c 2 12  2 w 1 c 12 0 +)  2 1 (L2S1a  wiosi +  2 1 L2C1a  2 1 + (L2c1a  —  —  (Lici  W2Q81a 2 1 )  (Lisi + 1 c 1 w 0 +1 L2s 1 a 2 +  —  wi s 0 i +L 112 c 2  —  {  2 [ML  W2oC1ci2  i  c 1 L 2 I41j1j + M  —  W1OCa 2 1 )  —  12 + + 2 (Lic  12 + s 2 +2mg (LiL  Ca 2 W L 2 1 0  2 (Lis 12 +1I2 [L  W1OCa 2 1 ) 4 c  2I 1 b 21 1  —  —  2 1 L28a  —  s 1 w 2 M 1 0 + 2 Ciai L M  2 1 (L1Sa  —  +  W2OCa 2 1 )  wlocai2)  +  —  —  2 1 (L1Ca  +  Yw  —  e) E)  812 I42j2j j=1  42j’2j 1  WloW2oSai2)] 82  11)10  1 c ) 0 wi _ 11i 2  —  ‘42j23  2 1 (L1sa  2 1 W1OW20Sa  +  —  —  +2  2 1 (L13a  2 1 W1OCa  —  —  ) w 1 c 0 2  20 w 20 j w )  0 2 Ca 10 L2W 1 )114  }  i  hl1o3ai2)I42j2j  s 1 2 w 2 0  —  L2ioCai2)]  2 + 2(Lisai  —  —  +2  —  )thi 2wi j 2 +0 1 (L28a  ca 1 (L 2  22 w 1 L 1 0 + 02 +m Sa + c L 1 (L L (Lisai 2 2 + [M  WloSai2)  c w 2 L 1 0 + 2  Cak2Wlo]  —  01 —2m [(L 1 s 2 L 2 +2 ocai + Liw L 2 + [M  —  2 2122 b  w 1 s 0 Wo 2 +2 [(Llcai2 + ) 10 + —2mg [(w  y]  S +1 1 L 2 c 1 w 2 M 0 + M +M Siai + L 2  1 C  2 1 (L13a  —  2 1 W2081a  i=1  +  x]  1 8 0 W2 2 1a ) 8 i  W2oSlai2) o  W2oClai2)  —  0 (L13a 2 1 I42j1’2j + 2m  5ai2t’1o) lL j=1  —  1 C 0 W1 2 a  —  20 1 w ) 2o  ‘42j2j  Chapter 2. Manipulator Modelling  +2m (LiL ai 2  —  (Liai L [M + 2  —  wloL2caj2 + 1 22 w 1 s 0 io] L1W2OCa + ) 2  2  WloCai2)+(LlCai2+W1o3ai2)I42j2j  12 s 2 L 1 +m (L  +2 2 Sa 1 [(L  40  —  L 1 w c 2 0 + ca 20 + w 1 L 12  wIocai2)I42j2j  mc(Llsai2 +2  I22j2j2j  —  Zi  WloW2oSai2)]  —  WloCai2  —  o)b 2 w o ]  (2.47) Similarly, for link 2 with the generalised coordinate  2  and the corresponding joint torque,  , the rigid body equation is: 2 r [102  L 2 + M  2 1 (L1Ca  ) w 1 0 + I22j’ 2 +s  12 c 2 1 +m (L + wL + L L::ai 2 + [M  +  L1W2oSa2  —  + mc  —  +  [102  +  I22j  + m (L  +  [102  +  I22j  )] 0 + m (L + w  —  Sa 1 W L 2 1 0  —  +  2  12 s 2 i+c } 2 w 1 —m [L 12 0 ë + m 12 ci 2 [L  2 1 (L13a  +  WloW2oCal2)1  Oi  )] 2 W 12 Sa 0  I12j2j  —  —  +  si 2 w ] 2 0 ë,  02 — =r 2 0 b 3 (L281a —b 2 1  +  —  12 c 2 (L i  W2øClai2)  +  2 1 (L251a  8 (L2C1a b 2 1 —  +  W2OCIa 2 1 )  W2051a 2 1 )  —  [(Lisi + 02  1 s 0 w2 2 1a ) 82  —  (L 1 s 12 + w2ocIai2) (Lici 82 +k  +  2 1 L251a  sl + 0 +l  +  [(Lici  C 1 W 1 0  —  wiosi +  w2oclai2) &1  (L251a + 2 +1  i2os1ai2  2 1 L2C1a  é+1 c 1 ( 0+ —  +  —  w2oslai2) &i  th2oClai2  s 1 w 1 0 + L2c1a 2 1  0 W2oClai2)11 ] 4  ci 2 (L + 12  1 1 5 W2o 2 cx  —  —  ) 2 w 1 s 0 2  Chapter 2. Manipulator Modelling  3 —k  2 1 (L2C1a  -  w2oslai2)  1 sii + L 2 +gx (M L 2 + [M  (WloCai2  41  (Lisi +  a L 2 [M i o i+  m 2 +  2 1 [(L2Sa  Yw  W2oCicj2  —  Ey)  2 fl  C1ai2I422)  —  L1Sa 2 1 )  —  gy  —  2 1 (L1ca  —m 2 sai + Liw2ocaj2 (LjL  —  112 + s 2 +L  c 1 w 1 0  +  iai L 2 (M  —  81ai2I422)  W1oSai2)I42j2j  Cai 2 WioL  +  wlow2oSai2)]  è  +  2Cai2ioI42j2j  22 w 1 c 0 io + 2 olb w o ]] 8 +) o] o 2 + 2mcw  —  ( + &).  (248)  The time variation of the ith mode of the shape of link 1 is given by the generalised co  ordinate  i&  for which the corresponding generalised force is zero. Applying equation (2.45)  yields the deflection equation for link 1: 2 1 (L + + [M  +  [102  L2Cai2)  —  L 12 2 (Lic + + M  +m (L + wL + 12 Ca 2 1 L  +I21j1j  +  {  ) 12 Wioa  —  +  Cai L 2 (M  2 1 (L2Ca  —  —  +  I22j  LlW2oSat2  {M + 2 ci L (M + 210  +rnc [qizo  ca 2 L + m(Li + 12  3ai21oI42j’2j  W203a 2 1 )  —  12 (Lis  10 + w L 2 +s  Sai2I422i) 1io  12oh/’1z 4  +  (102  +  W208a 2 1 )]  —  W1OW2OCa 2 1 )] io] i  Sai2I42i)2)  14]}  —  I22))  Chapter 2. Manipulator Modelling  +m  +  {  12 C 2 [(L a  [cai: +  +  ‘o2  —  ) 2 w 1 s 0 2  + rn (L + w)]  [1n81  J. 1 —b  +  cai c 2 liw  2 1 (L2s1a  31 —b c 1 ( 0 + ci 2 (L + 12 8 +k  +  0 L2S1a2j  +  +gxslI4li  j=1  ci 0 4cJ a, + rn [4i  W2OC1a 2 1 )  ciai 2 + (+L  —  1 8 0 w2 2 1a ) 1jo]  E  L2C1aL2iO  —  +  —  + —  +e+  W1o81  +  1 1 3 W20 2 a  ) 2 w 1 s 120 0 [(Lici  W2o31a 2 1 ) 02  L2S1ai26jO  [(Lisi + wci + L251a 2 + w2oclai2) 01 1  ) 0 W2oC1ai2j  —  Ey  +  1ol  +  —  +  S 1 W 1 0  C 2 W 1 0 2  2 1 (L231a  +  20 tb] w ) 12 + ci  2 1 L2C1a  ci 2 (L + 12  —  —  ) 2 w 1 s 12 0  W2oC1ai294o)  (Liei  ) 0 j 4 W2oS1ai2  (Lisi + c 10 + L w 1 112 + s 2  —  wi s 0 i +  ) 2 w 1 s 12 0Ô 1  2 a 1 L2C  1 1 8 W2o 2 a  1 c 0 w2 2 1a  —  —  —  e) E)  —  {1211  2 + [M  (L2Sai2  L (Llsai2 2 [M  —m (L 12 L 1 2Sa +  {  b 4 I 2 2 + [m . (L 1 10 + ca q 2 qiio)] 12  L251a 2 1 ) 02  1 +0 c 10 2ç/4j 1 L2c1a —k (4  +  2  iQ}  —  + (w2oclai2 +  —  1112o3a 2 1 )] &jo  j=1  s 1 ( 0 + 31 —b  +  —  fl2  + 3=1  =  ]}  (+)  + m (L2ca 2 1  1 4 2j2j  —  122ij  -f-jo —rn  +  42  —  +  ) 10  W1QCa 2 1 )  12 W 1 L C 0 a 2  [M 2 L 2 12 + 2c 12  —  I42j2j  +  cai2  1 4 2j’2j  10 + 2 L 1 s 2+ + m (w  + (Llcai2 +  l111o5ai2) I i2i] 4 2 io  wiocai + 2 L  WloW2oSai2)  12 + s 2 + 2m (L  2 0 1io :12)]  +2 t a L 2 M i4o i+  a2  I42j&2j :1=1  +  Cai t 2 L4o  I42jlI’2j j=1  )] 2 w 1 c 0 1jO 2  Chapter 2. Manipulator Modelling  +m (Sai22o +  (L2sai2  2 i a L 2 [f o i+ +m(w 2 b 20 0+  ai + L [M + 2 +2  {  +  2 1 (L2Sa  [(Sai2I422  j 2 j) 42 Cai2WloI  ‘42j2j  io}  2+ 1 mc(L23a  +  j 2 Sai2I42j  12 o 2 +w 1 W C 0  2 1 Ca  +  W2ocai2)1o)]  Cai2I42j2j+  sai 2 +mc(L o  +2  w2ocaj2)  I22j2j2  S L 2 [M t oiio ai +  Sai + L [M + 2  +  43  +  42 o 1 Cai2W j 2 jJ I  1jo  2 + 1 + m (L2Sa  z) mcsai o i +2  W2ocai2)]  iio  +  —  oti 2 mcw o ) io} 82  W2oCai2)) 1jo  oui mcw o ) +2  —  io] to  (2.49)  Likewise, the deflection equation from the jth mode of link 2 is obtained from (2.45) using the generalised coordinate 2 b i :  [ 1 1 2j  +  (L1CaI2  +  Sai2Wlo)142j  42 + mc 1 2 ] 1220 + [Cai j 82 + 220 + mL  +  [ 1 1 2j  =  /’ 2 —b  12 + s w 1 0 +L 2 ) 2 + m (Lic  &jo1i  ‘22j2j  +  [ 1 1 2j  20 +m  + mL 1 2250 2jo2j  1 2jo  oij  —  +ë joclai 2 mc v  —  b x 8 2joS1ai2  +6rW1o81  bsy2joC1a2  OC 1 6!,W  [(Lisi + wioci + L 112 + ci s 2 20 8 + (L w ) 12 12 + ci s 2 20 w ) 12 +  1 1 5 W20 2 cz  [(Lici +  —  +  2 1 (L251cz  1+ ws  W2oClcxi2  +  2)i4o} 1 + w2oc1a  2 1 L2c1a  2 1 (L2C1a  82  —  w2oslai2)  W2o51a 2 1 )  2 1 Oi + (L2Ca  J 0 th  W2o51a 2 1 )  82  Chapter 2. Manipulator Modelling  +ksx42joS1ai2  (Lici  —  jociai (Lisi + 5 —ksyq 2 +gs1aj2I42j  +  [I22j’2j  +2 +  —  10 + L w 1 s 112 c 2 WioC  —  — —  +  2 1 L2S1a  42j 1 tUloCai2)  + mw 1 2 o 2 0  {I22&2  +  —  W2oClai2  Yw  —  —  ) 1 e  42j 1 9yC1ai2  2 1 (L18a  —  44  02  —  + m (wlocai2  42j1o 1 ai2  —  20 w Sa + ) L 2 1  + I22b2iW 0  m (sa thio 12  —  0  2jo] 5 q  —  20 } w ) 0 t14 20 b  o mcw j o] q (th, + & + 2i4o0 +2 ) 2  22j’c/-’2j 1 [  i  (2.50)  The generalised coordinate for the x direction component (in the world frame) of the contact mass deflection is e and the x direction contact equation is: —m [Lisi + L2S1a 2 + 1 112 + s 2 —m [L 2 1 —ms1  bexx  +  c 1 +ksx (L +m (Lici  01  112 C 2 W ]  —  —  1 ms  1jo1j  m [L231a 2 + 1  2I 02 1 W2OC1a  + mE  kexEx  —b [(Lisi + 1 wioc + +E  +  c1a 0 w2 2 1 ]  2jo2j  —  10 W 1 C  10 W 1 S  —  —  ci 2 (L +m 12  +  2 1 L2S1a  W2oSlcxi2  +  10 + L2c1a w 1 s 2 1  +  (W2oClai2  112 c 2 +2m {(L 12 m [(L2cm 2 +  Oi +  +  —  10 + L2c1a w 1 s 12 —  W2OC1a 2 1 )  2) 1 L281a  —  W2031a 2 1 )  2 1 (L231a  —  +  1 c 0 w2 2 1a ) 02  ol 4 ti  Ex)  0 + m 12 ci 2 (L  —  W2051a 2 1 ) 02  112 c 20 2m?i 0 2 w 1 s ) 12 0 tb + ?d4 —  —  W2051a 2 1 ) 02  1 + zb c ci 2 (L 12 + 12 c 20 + 10  1 8 0 W2 2 1a ) l)1o  +  —  1 3 0 w2 2 1a ) 4] ôi  (2.51)  W2oC1ci2j 02.  Finally, the y direction contact equation, obtained with the generalised coordinate es,, ii  [Lici  —  10 + L2c w 1 s 2 a 1  21 1 W2o31a  1 + mc 1  1jo1i  is:  Chapter 2. Manipulator Modelling  +m [L2C1a 2 1 112 +mc  =  beyEy  —  W2o31a 2 1 ]  2jo2  —  +b [(Lici  45  —  m  [L2Clai2  —  oSi 02 2 W ]  + mE  keyEy —  s 1 w 1 0 +L 112 c 2  iJ- + 1 c + 10  1 c 0 w2 2 1a  —  W2QS1a2)  2 a 1 + (L2c  ci + L 0 3 (Lisi + wi +k 112 + s 2  —  W2oCiaj2  ci 2 (L Si + 12  —  ) 2 w 1 s 12 0 02  ) 2 w 1 s 12 0 Wi ] 0 —  Yw  ) 1 c 1  L 1 s 12 + 1 c 1 w 0 + w2oclai2) 0 + m (L251a +m (Lisi + 2 2+ 1  w2ociai2)  02  ti4 + 2mctb2osiai ti4o 2  +m (L231a 2 + 1  W2oClai2)  + m 2 g [(L251a 2 1  +  lD2oC1ai2)  m [(L281a 2 + 2 1  +  W2oClai2) Wlo  02 +  WioSi  +  2 1 W2oS1  + W2oSlai2]  62.  +  2 1 (L251a  +  W2oCiai2) oj  6  (2.52)  Several steps have been taken in an effort to ensure the correctness of the derivation of equations (2.47) through (2.52). As noted previously, the kinetic energy expression (equa tion (2.37)), from which most of the terms in the equations of motion arise, is in agreement with that derived independently by Chan [68]. Furthermore, Chan’s derivation is in agree ment with two other independent derivations for related systems carried out by Mah [35] and Ng [72]. The application of Lagrange’s equation to the energy expressions to obtain the equations of motion and the subsequent simplification and manipulation of those equa tions was done using the Maple V algebraic computation program. This method protects against errors which are common in manual algebraic manipulation such as transcription errors, sign errors and misapplication of the chain rule of differentiation. Finally, as will be discussed in section 4.5, the FORTRAN coded form of the equations of motion have been validated against the code developed by Mah [35] both by comparison of calculated values  Chapter 2  Manipulator Modelling  46  for the terms in the inertia matrix and on the right hand side, and by comparison of ho mogeneous responses for a variety of initial conditions. Mah’s program has been similarly validated against programs developed independently by Chan [681 and Ng [72]. With reference to the generic equation of motion, (2.46), it is evident that, when contact exists, equations (2.47) through (2.52) describe a system of 4 + ni + n 2 damped, coupled, nonlinear oscillators.  The oscillators are forced by the joint torques in the rigid body  equations which act to change the joint angle generalised coordinates. The coupling among the oscillators ensures that an excitation of any of the generalised coordinates will be transmitted to all of the other generalised coordinates. The inertial component of the coupling is evident in the fact that the inertia terms on the left hand side of each equation are a weighted sum of all of the generalised accelerations. Considering the complete set of equations, the weighting terms can be written as a symmetric generalised inertia matrix. The terms in that matrix can be divided into two groups; those which are inherent in the dynamics of a serially connected chain of flexible links and those which arise due to the existence of contact between the manipulator tip and an environment surface. The same division is true of the nonlinear right hand side terms, N(q, 4), which are functions of the generalised coordinates and velocities. On the other hand the coupling in the dissipation and stiffness terms on the right hand side exists solely due to contact. As will be shown in section 2.5, the contact force exerted by the manipulator on the environment can be expressed as a function of the generalised coordinates. Thus, joint torque inputs will tend to excite oscillations of the generalised coordinates which will be expressed as oscillations of the contact force. The goal of force control is to calculate and apply joint torques which will cause the contact force to quickly follow changes in the desired contact force level. In the absence of contact the system becomes a set of ni +n 2 damped, coupled, nonlinear oscillators with an additional two rotational degrees of freedom. The joint torques act to  Chapter 2. Manipulator Modelling  47  produce rotations of the link bodies about the joints and the inertial and nonlinear cou pling ensures that oscillations of the generalised coordinates associated with the structural dynamics are excited. Equations (2.47) through (2.52) can be transformed to the equations of motion for several other configurations which have been investigated in the course of the research. Setting each element of the set of coefficients {m, bex, zero and dropping the generalised coordinates  E  and  ey  b kex, , , 3 3 b  } to 8 , k 3 k  and equations (2.51) and (2.52)  yields the 2 + ni + n 2 equations of motion for a planar two link flexible manipulator in free motion. The resulting equations are shown in section A.l of Appendix A. These equations govern the motion of the manipulator when it is not in contact with the environment. Note that by dropping the generalised coordinates  and e in the absence of contact, the  motions after contact of the contacting surfaces are neglected. By implication, when contact is broken, the contacting surfaces effectively come immediately to rest without residual deflection. This assumption is reasonable considering that the masses and deflections of the contacting surfaces are small and that neither plastic deformation of the surfaces nor gross motions of the environment are included in the model. Starting from equations (2.47) through (2.52), the equations of motion for a planar two link manipulator with rigid links contacting a deformable environment are obtained by setting each of the coefficients  {I1k, ‘2ki, JJ3ki, ‘4ki,  dropping the generalised sets of coordinates {‘,bi ,i 2  bk,  i  1,.  .  .  ,  rik,  k  1,... ,m } and {b 1 ,j 23  1, 2} to zero, =  1,... ,n } and 2  dropping equations (2.49) and (2.50). The resulting four equations of motion are shown in section A.2. Further operation on this resulting set of equations, namely setting each of the coefficients {m, bex, bey, coordinates e,, and  E  , kex, key, k 3 b x, ksy} to zero, dropping the generalised 8  and dropping equations (A.7) and (A.8) yields the two equations of  motion for a planar, rigid, two link manipulator in free motion, shown in section A.3.  Chapter 2. Manipulator Modelling  48  Finally, the 2 + n 1 equations of motion for a single flexible link in contact (section A.4) and the 1 + n 1 for the same link in free motion (section A.5) are arrived at by setting the coefficients  {IO2,I12,I22,I32,I42,  the generalised coordinates  62,  {b , 2 i  , b€, b 2 b } to zero, dropping 2 , kex, 3 3 k , i = 1,... ,n =  1,.  .  .  } and e, and dispensing with equations 2 ,n  (2.48), (2.50) and (2.51) in the former case and by additionally setting the coefficients {m,  } to zero, dropping the generalised coordinate e, and dispensing with 3 key, k  equation (A.13) in the latter.  2.5  Contact Force Model  Figure 2.5 shows the geometry of the two flexible link manipulator and deformable environ ment system with the link deflections and the environment model much exaggerated in scale (compared to Figure 2.1). The contact force arises from the local compression of the regions surrounding the point of contact between the link (or end-effector) tip and the environment surfaces. The local dynamics of the contacting surfaces are represented, as recommended by Johnson [62], by a contact region mass on a damped, linearly elastic foundation for each surface. The deflection of the combined contact mass from its location at the instant when contact is made represents the local compression of the surfaces. Note that, while the local compression of the manipulator tip surface in general has a component along the axis of the last link, the links are assumed to be stiff in that direction and the effects of global axial compression of the links are ignored. The expressions for the contact force components as a function of the generalised coor dinates of the model will now be derived by considering the geometry of the system and the forces acting on the contact region masses. These expressions are necessary to the sim ulation studies in order to permit the contact force between the link tip and environment  Chapter 2. Manipulator Modelling  49  jkev  I  m= m+ me sy  5 b  yw  y  €  I xw  ox  x Figure 2.5: Manipulator—environment model geometry for derivation of contact force ex pressions.  Chapter 2. Manipulator Modelling  50  m pc k ( 5 e— ô—x)  X)  kex(E ô  1 ( 5 b ) —x  bex’x (a) Total contact mass  5 k  (E  —  —  xtjp  kex(Eôx)<w) F  F b ( 5 —  b ex  *tp)  (b) Manipulator tip contact mass  (c) Environment contact mass  Figure 2.6: Free body diagrams for the x direction contact region mass. surface masses to be calculated from the results of the integration of the equations of mo tion. Furthermore, they are used, in simplified form, to devise the initial controller model coefficients for the force control algorithm in Chapter 5. Based on the free body diagram shown in Figure 2.6a a force balance for the total contact mass, m = m 3 + me, in the x direction yields , 1 më = —k 9 (e  —  —  —  (  —  Sx  —  w)  —  3 (r b  —  —  bexE.  (2.53)  Considering each of the surfaces separately, with the x direction contact force, F, acting equally but in opposite directions on each contact mass as shown in the free body diagrams,  Chapter 2. Manipulator Modelling  51  a force balance on m 3 (Figure 2.5b) gives  —  F  —  3 (e k  —  —  x)  —  3 b  (  =m 3  —  (2.54)  and a force balance on me (Figure 2.5c) gives F  —  kex (e  —  —  x)  =  —  (2.55)  meêYx.  Solving equation (2.53) for ë and substituting the results into equation (2.55), noting that =  —  x 8  —  x, yields: x..  F = (kexx+ bexx)+  x)j.  (2.56)  A similar procedure applied in the y direction yields the expression for that component of the contact force, F: =  ‘rn  (keyEy +  beyy)  +  [ksy  (Ytip  —  Yw  —  e) + b  (th  —  ay)].  (2.57)  Expressions for the world frame manipulator tip position (x 2 and y) and velocity (i 1 and  !‘tip)  components which appear in equations (2.56) and (2.57) were derived as part of  the kinematic analysis in section 2.3 (equations (2.34) and (2.35)). When the tip of the manipulator is just contacting (but exerting no force on) the en vironment the following geometric relationship exists among the link lengths, joint angles and the location of the environment: =  1 cos i9 + L L 2 cos ( + 8)  and Yw = L 1  5111  =  c+L 1 L c 2  8 +L 2 sin (8’ + 6) = 1 L+L s , s 2  where O and O are the particular values of the joint angles at which contact occurs.  (2.58)  Chapter 2. Manipulator Modelling  2.5.1  52  Hertz Contact Model  The values of the contact region masses, m 5 and me, stiffnesses, kex, key,  , and 5 and k  damping coefficients, bex, bey, b , in this lumped parameter contact model must be 3 3 and b related in some way to the material properties of the surfaces in contact. For instance, an aluminium end-effector in contact with a steel assembly jig will have much higher stiffness values and lower mass values than a rubber cover tool contacting a wooden surface. The solution for the deformation of contacting spherical surfaces, derived in 1882 by Hertz, provides some insight into selecting contact mass and stiffness values based on the physical properties of the materials in contact. It must be noted that the Hertz theory is based on the assumptions that the contact occurs between surfaces of non-conforming shape and is quasi-static. As a result, it can only be used as a guide in the selection of coefficient values for the more complex, dynamic contact case being modelled here. One result of Hertz’s analysis is that the decrease in distance, 8, between the centres of two spheres held in contact by a force F is [63] 2  S = F3  {D2  1  (  +  1  where  D  21 1_v  +  1—va2  (2.59)  is the inverse of the effective planar strain modulus of the materials, v 1 and v 2 are the Poisson’s ratios of the materials, E 1 and E 2 are the moduli of elasticity and R 1 and R 2 are the radii of curvature of the spheres. Equation (2.59) can easily be expressed as a force/compression relationship: F = KS,  (2.60)  where K=  (R8) D  and  R=  2 1 R  In this expression R is the effective radius of curvature, that is, the contact between surfaces  Chapter 2. Manipulator Modelling  53  of curvature R 1 and R 2 is equivalent the contact between a surface of curvature R and a flat surface [64]. Note that, unlike in Hooke’s Law for a linear spring, the stiffness in equation (2.60) is a function of the square root of the compression, that is, the stiffness of the material increases the more it is compressed. Given the restrictive assumptions under which equation (2.59) was derived and the fact that in elastic contacts between hard materials the surface deflec tions must be small, the contact stiffness coefficient values used in the present research were obtained by approximating equation (2.60) by its tangent at an appropriate nominal level of contact force. Details of the calculation of the coefficients for specific combinations of materials are included in Chapter 5. It follows from the Hertz theory [63] that the area of contact between spherical surfaces is a circle of radius a  (FDR).  (2.61)  On the basis of this, a first order estimate of the volumes of material moved by the defor mation of the surfaces is that they are equal to the segments of a sphere of radius R and heights S and  62,  that is, =  7rS (R  —  -6)  and  The deflections of the individual surfaces, 6 and  2 V 62,  7r6 (R  —  ) 2 s  (2.62)  .  sum to the total deflection given by  equation (2.59) and are in inverse proportion to the planar strain moduli of the surface materials. These volumes, combined with the densities of the contacting materials, give the mass of material on each surface that is involved in the contact deformation and thus provide a basis for the selection of values for the contact region masses, m 5 and  me.  Chapter 2. Manipulator Modelling  2.6  54  Voltage Controlled DC Electric Motor Actuator Model  In large, flexible manipulator systems such as the Space Shuttle Remote Manipulator Sys tem (SSRMS), or Canadarm, and the proposed Mobile Servicing System (MSS) and Special Purpose Dextrous Manipulator (SPDM) for the Space Station Freedom, the links are driven by direct current electric motors at each joint. A typical DC motor actuator system, con trolled by armature voltage, consists of: • a signal amplifier which raises the control input voltage signal to a level suitable to drive the motor armature; • a current amplifier which produces the current through the motor armature; • the motor itself with its inherent armature resistance and inductance, rotor inertia and mechanical as well as electrical damping; and • a gear box which raises the motor torque to the level required to drive the link. Figure 2.7a shows a block diagram of such a typical DC motor actuator system. The mathematical model of an armature voltage controlled DC motor is as follows [65j. With reference to the armature circuit diagram in Figure 2.7b, the sum of voltage drops around the armature circuit is: Va  where  Va  = Raja + Laja +  Vb,  (2.63)  is the armature voltage, derived from the control input signal, Ra is the electrical  resistance of the armature windings, a is the current through the armature windings and La is the inductance of the windings. The back emf, vb, a feedback voltage proportional to the armature speed, a 0, is a result of the rotation of the motor armature in the magnetic field induced in the stator windings; Vb  KemfOa,  (2.64)  Chapter 2. Manipulator Modelling  55  (a) Block diagram  +  (b) Armature circuit  Figure 2.7: Schematic of direct current motor actuator system.  Chapter 2. Manipulator Modelling  56  where Ke is the back emf coefficient. Equation (2.63) can be rewritten as an ordinary dif ferential equation for the armature current in terms of the motor parameters, the armature speed and the applied armature voltage, the latter being the forcing function: Laja + Raja  Va  Ke,6a.  —  (2.65)  Equation (2.65) constitutes the electrical model of the motor. Mechanically, the motor is represented by an armature inertia, ‘a a frictional damping, ba, proportional to the armature speed, and the load torque,  at the motor side of the gear box. A torque  TL,  balance for the motor yields: 1 a a  The torque produced by the motor, windings,  Tm  —  TL  —  baOa.  proportional to the current in the armature  Tm, iS  or Tm =  Kra,  (2.66)  where K.,. is the motor torque coefficient. Thus, the mechanical model of the motor as viewed from the motor side of the gear box is TL =  K.rja  —  Ia&a  —  (2.67)  baa.  The gear box increases the torque by a factor Ng, the gear ratio, at the expense of decreasing the shaft speed by the same factor such that =  N T 9 L  and  = jIa.  Thus, when viewed from the link side of the gear box at joint k the mechanical equation of the motor is Tk  N K 9 rja  —  2 (2ak + bask) Ng  .  (2.68)  This actuator model is coupled to the rigid body equation of motion for the link being driven by adding the inertia, NIaOk, and damping, Nba8k, terms from equation (2.68)  Chapter 2. Manipulator Modelling  57  to the corresponding terms in the rigid body equation and by using the geared up motor torque, NgKTia, as the joint torque,  Tk.  In order to calculate this joint torque the differential  equation for the armature current (2.65) must be solved simultaneously with the equations of motion since it involves the armature speed which is proportional to the joint rate. Section A.7 shows an example of how the coupling is implemented for the equations of motion for a single flexible link in free motion driven by a DC motor. This linear model of an armature voltage controlled DC motor actuator is widely used and accepted [65]. The signal and current amplifiers are components of such an actuator system that are often overlooked in modelling. They are important not only in that they are part of physical actuator systems but also because they are the source of the dominate nonlinear feature of the actuator system, saturation. The voltage drop which can be applied across the motor armature is limited as is the current which the motor may draw. Figure 2.7a shows the amplifiers as linear devices with limits and they are modelled in the TWOFLEX code in much the same way; if the calculated values of voltage or current exceed the saturation limits the values are clamped at the limits. The actuator model neglects features of the motor dynamics such as non-viscous friction and torsional flexibility in the shaft and gear head. These features are ignored in order to focus on the control issues which arise due to the structural dynamics of the manipulator links. Section 5.6 presents results of several simulations of maneuvers of a prototype Space Station Mobile Servicing System (MSS) arm modelled as a two flexible link manipulator with DC motor actuation.  Chapter 2. Manipulator Modelling  2.7  58  Summary  This chapter has presented the derivation of models for the kinematics and dynamics of a planar two link manipulator with flexible links making contact with a deformable en vironment. Also derived are expressions which relate the contact force to the generalised coordinates of the dynamics model and relationships between the physical properties of the materials in contact and the parameters of the contact dynamics model. The derivation of the contact dynamics model is the primary contribution of the chapter in that, unlike the models used in previously published robotic force control research, the model includes the effects of inertia and damping in the local region surrounding the point of contact, in addition to the contact stiffness. Results from the theory of elasticity, based on the work of Hertz, indicate that the linear contact dynamics model used here is an approximation to the more complex situation in which the contact stiffness and the mass of the region deflected during contact vary nonlinearly with the contact force. However, the Hertz model is also limited by its tacit assumptions of non-conforming surface shapes and quasi-static contact.  Chapter 3  Control Algorithm  3.1  Introduction  This chapter presents derivations and discussions of the various parts of the overall force control strategy that is the focus of the research. At the heart of the control strategy is a multivariable, receding horizon, adaptive, long range predictive control algorithm. Devel oped with the primary goal of providing stable, accurate tracking control of desired levels of contact forces between the manipulator tip and its environment, the algorithm can also be used to control the gross motions of the manipulator when it is not in contact. In the early stages of the research several controller designs were investigated before long range predictive control was selected. The complexity of the nonlinear, coupled dynamics of a flexible manipulator and environment system make conventional PID controller and compensator designs impractical. Some of the difficulties with conventional designs can be overcome with the use of gain scheduling but to do so requires very complete knowledge of the plant dynamics over the operating range, including the time scales of characteristics which are likely to necessitate gain changes. State feedback techniques are better suited to complex dynamic systems. A pole placement tracking controller was designed for force control of a single flexible link and was successful with the significant exception of its inability to remove steady-state errors from the response. A similar deficiency was found to limit the use of linear quadratic optimal state feedback. A scheme to use an adaptive inverse  59  Chapter 3.  Control Algorithm  60  dynamics model of the system to provide state feedforward for the removal of steady-state offsets was devised but the robustness of such a scheme is questionable. The long range predictive control design that is developed in this chapter has the ad vantage of possessing inherent integral action to yield a steady-state error free response. Setpoint tracking is included directly in the formulation, the resulting control law is op timal in the sense of minimising the sum of squares of the difference between the future setpoint levels and the predicted output levels, and the algorithm is easily implemented in an adaptive, digital controller form. Predictive control is well suited to deal with systems exhibiting an abundance of lightly damped modes and non-minimum phase characteristics as is the case in flexible manipulator force control. The inclusion of the expected future effects of these characteristics in the controller cost function results in a control law which produces more effective compensation than one based solely on instantaneous sensor data. The Generalized Predictive Control (GPC) algorithm of Clarke, et al. [29] incorporates many of the beneficial features of earlier predictive algorithms. It is a relatively mature im plementation of the concepts of predictive control which has sparked a great deal of interest and ongoing study of its properties and potential applications. The dynamics of a flexible link manipulator in contact with deformable environment present several challenges to the design of a force control algorithm. The dynamic behaviour is highly coupled, nonlinear, stiff, discontinuous, and nonminimum phase. To deal with the strong coupling, the predictive controller design is based on a multivariable model that includes all of the open ioop system modes and their interactions. Nonlinearities arising from the system dynamics are dealt with by making the controller adaptive, that is, an instantaneous linear model of the system is used and the coefficients of the model are updated regularly, on-line, to reflect the changes in the system due to its nonlinear nature. The open loop system is, in most configurations of practical importance, stiff, in both physical and mathematical senses. The natural frequencies of the open loop modes can easily  Chapter 3.  Control Algorithm  61  span a range of several orders of magnitude in the case of a highly flexible manipulator in contact with a metallic surface. This characteristic of the system causes difficulties in the numerical integration of the equations of motion, as will be discussed in Chapter 4. It also leads to the coefficients of the instantaneous linear model, on which the controller is based, being sensitive to small changes in the system configuration. As in the case of nonlinearities, this difficulty is overcome by using an adaptive control approach. Contact force control is a discontinuous problem due to the fact that the contact force can, by definition, only take on either positive or negative values (depending on the orien tation of the reference frame in which it is measured). When the manipulator tip looses contact with the environment surface the contact force is zero and the error signal available to the controller is independent of the manipulator state. While it is possible that, even with such a degenerate error signal, the controller will successfully return the manipulator tip to the environment surface and continue stable control of the contact force, it is more likely that the system will become unstable with manipulator tip repeatedly bouncing on and off the surface. To avoid this situation the controller is designed to switch from a force control mode to a motion control mode when the contact is unexpectedly broken. The objective of the controller is then to return the manipulator tip to the surface and switch back to force control mode. The switching of modes between force and motion control is governed by a contact control logic block at a level above the force and motion controllers in the manipulator control hierarchy. Two operational strategies for the contact control logic are discussed and demonstrated in Chapter 5. The structural flexibility of the manipulator links makes the contact force response of the system nonminimum phase. Physically, a nonminimum phase system is one for which a positive step input produces a response which is initially negative and subsequently reverses sign. In free motion, the tip of a flexible link subjected to a step torque input oscillates about the path that would be described by the tip if the link were rigid. Due to the inertia of the  Chapter 3.  Control Algorithm  62  link, the tip initially lags and the oscillation is initially negative. This characteristic carries over into the contact force response when the link is acting on a surface. Mathematically, a nonminimum phase system is characterised by the presence of unstable zeros in the transfer function numerator. Predictive control is well suited to dealing with nonminimum phase systems because the prediction horizon can be extended to include the nonminimum phase portion of the response and it can thus be compensated for in the calculation of the control inputs. Section 3.2 presents a review of the single input, single output (SISO), Generalised Pre dictive Control (GPC) algorithm of Clarke, et al. [29,32], a discussion of the control law structure and a derivation of the closed ioop system characteristics. The development of a multivariable predictive control algorithm based on GPC is presented in section 3.3. A new extension of the algorithm with a static equilibrium bias term that takes advantage of available a priori knowledge of the dynamics of force controlled manipulators is presented in section 3.4. This extension is an original contribution of this thesis. The parameters of the predictive control algorithm and their effects on the shaping of the closed ioop response are discussed in section 3.5. A discussion of parameter estimation, focussing on the Exponential Forgetting and Resetting Algorithm (EFRA) is presented in section 3.6, and a description of the structure and logic of the complete adaptive multivariable force/motion controller in section 3.7. The chapter concludes with a discussion of stability and convergence charac teristics of the adaptive, predictive control algorithm (section 3.8).  3.2  Review of SISO GPC Algorithm  This section briefly reviews a nonadaptive, single input, single output (SISO) long range predictive control algorithm, known as the Generalized Predictive Control (GPC) algorithm, developed by Clarke, et al. [29,32]. This discrete time algorithm yields a control law with  Chapter 3. Control Algorithm  63  inherent integral action and robustness to inaccuracies in both model order and time delay. The algorithm is based on prediction of the plant output for a series of future time steps. The predictions are expressed in a form which allows a solution for an optimal (in the sense of minimisation of a quadratic cost function) set of future control inputs that will minimise the error between the predicted plant output and the desired setpoint. This procedure is conducted in a receding horizon context, that is, a new set of control inputs are calculated at each time step and only the first member of the set is used. Receding horizon predictive control predates GPC in the process control literature but, as the name suggests, the GPC algorithm is a generalised form of the method. Indeed, it has been shown that, depending on the choice of parameters, the GPC algorithm results in control laws identical to a variety of well known algorithms [73—75]. The plant to be controlled is modelled in the discrete time domain by an input/output representation of the form A(q’)y(t) = B(q’)u(t  —  1) +  ,  (3.1)  where A(q’) and B(q ) are polynomials, of degree na and nb respectively, in the backward 1 shift operator q’: ) = 1 A(q  1 +1 a q ’+2 a+ q  ) 1 B(q  q + 2 q’ + b 1 0+ b b  where  =  x(t) = x(t 1 q  —  .  ..  ...  + anaq, +  1),  y(t) is the time series of plant output values and the plant input time series is u(t). The last term in equation (3.1) is a disturbance term consisting of (t), an uncorrelated ran dom sequence and L  1  —  q’, the differencing operator. This model assumes that the  disturbances to which the plant is subject can be modelled as steps of random magnitude  Chapter 3.  Control Algorithm  64  occurring at random times [76]. Using the Diophantine identity [77], 1  =  E(q ) 1 A(q’)z + qF(q’),  (3.2)  the plant model can be manipulated to yield a j—step ahead predictor of the plant output: y(t +  j)  =  Fj(q’)y(t) + ) )L\u(t 1 E(q B(q +  j  1) + 1 E(q ) (t +  —  j).  (3.3)  The polynomials 3 E ( q’) and ) 1 ( 3 F q in equation (3.2) are completely defined given A(q’) and  j, and are unique provided that [78]: j  deg(E,)  deg(F,) <deg(A).  and  The coefficients of E+ 1 and F 1 are calculated from E, and F, with minimal computational + 3 effort using simple recursion formulae [29]. The predicted output y(t +  j) is the sum of  filtered time series of the output, the input and the disturbances. Given the fact that the degree of E 3 is, at most,  j  —  1 and the assumed random nature of (t), the contribution  j) of the disturbance sequence is not only unknown but unknowable. Hence, the optimal predictor of y(t + j) given the information available at time step t is to y(t +  (t + jt)  B 3 where G = E 3  =  gjo+gjlq  =  F,y(t) + Gu(t +  +...+gj(nb+j_flq 1  can be written as a term, f(t +  (flb+I  1)•  j  1),  —  (3.4)  The optimal prediction (t+j  It)  j), which consists entirely of past inputs and outputs (in  effect, the free response of the system) summed with terms due to the future control input increments which are yet to be determined. Defining N 2 as the prediction horizon and considering predictions at each of the next 2 steps into the future the following vectors are formed: N =  + 1 t) (t +2 [ut u(t + 1)  r  It) ...  ...  (t + N 2  u(t + N 2  —  (3.5)  It)]: 1)]  ,  and  (3.6)  ]T, =  [f(t+1) f(t+2)  ...  ) 2 f(t+N  (3.7)  Chapter 3.  Control Algorithm  65  where f(t + 1)  =  f(t + 2)  f(t + N ) 2  Fi(q’)y(t) + (Gi(q’)  gb) Au(t)  ( 2 F ) 1 y(t) q + q (G (q’) 2  =  (q’)y(t) + 2 FN  qN2  g2lq  (q’) 2 (GN  —  —  g2o)  Au(t)  ( gN 2 1)q’ N  —  ...  —  o) 2 gN  1u(t). (3.8)  The set of N 2 prediction equations of the form of equation (3.4) are written in matrix form as  5’  =  G’ü + f,  (3.9)  where G’ is an N 2xN 2 lower triangular matrix whose elements are the coefficients of the G 3 polynomials from equation (3.4) corresponding to the yet to be determined future inputs. A quadratic cost function, J , is defined: 1 1 J  =  (5’_w)P(5’_w)+AuTu (3.10)  =  The vector [wt + 1) w(t + 2)  ...  w(t + N ) 2  (3.11)  is a sequence of future setpoints, hence, the first term of this cost function is simply the sum of squares of the errors between the predicted plant output and the desired setpoint at each of the next N 2 steps into the future. The second term is the sum of squares of the future control input increments (i.e., the incremental control energy), weighted by a constant, ). Minimising J 1 with no constraints on future inputs yields the control law  u  =  (c’’ + AI)’ G1T(w  —  f)  (3.12)  Chapter 3.  Control Algorithm  66  which will track the specified setpoint sequence with zero steady state error. Note, from equation (3.6), that the first element of ii is Liu(t) which, given the receding horizon method of applying the control law, is the only input increment which will be used. This control law yields an offset free response but is subject to certain restrictions and deficiencies, not the least of which is the computational load of inverting the N 2xN 2 matrix (G’ TGI  +  at each time step, as is required in an adaptive implementation due to the  updating of the plant model from which G’ is derived. To improve the robustness of the algorithm, and at the same time reduce its computational load, the concept of a control horizon [79] is introduced [29]; that is, after some number of time steps, N< N , the control 2 input level is held constant. N is the control horizon. Imposition of this condition results in the vector ii being truncated from N 2 to N elements and the left most N 2  —  N columns  of the matrix G’ being truncated to yield the N 2 x N matrix G. The resulting control law has the same form as (3.12) but with G substituted for G’:  u  (G’G  +  i)’ GT  (w  —  f).  (3.13)  This control law is less computationally intensive than equation (3.12) because it requires the inversion of an N x N matrix instead of an N 2 xN 2 one and N is generally chosen much smaller than N . Furthermore, the structure of the non-triangular matrix G guarantees 2 that GTG will be nonsingular. It is the inclusion in the GPC algorithm of the control horizon concept that is at the heart of its ability to control difficult plants like flexible link manipulators which exhibit nonminimum phase characteristics and lightly damped or poorly modelled modes. 3.2.1  Control Law Structure and Closed Loop Characteristics  While equation (3.13) gives the control input increments in a form suitable for implementa tion in a digital controller or simulation, insight into the underlying structure of the control  Chapter 3. Control Algorithm  67  law and the closed loop characteristics of the controlled system can be obtained by further manipulation of equation (3.13). Firstly, note that the quantity (GTG + \I)  1  GT  is sim  2 matrix of scalars, in effect a gain matrix which we will designate as K for ply an N x N convenience, and hence the control law becomes üz=K(w—f).  (3.14)  From equation (3.8) we note that the vector f is the predicted free responses for the next N 2 time steps and that these predictions are composed of contributions from the past members of the output time series, y(t), and the time series of input increments, Au(t), known at the present time step. Defining the following vectors of polynomial operators in q’:  Fi(q’) F ( 2 q)  Gi(q) and  —  910  (q) 2 q (G  g=  g2lq  —  920) ,  qN2l  FN ( 2 q) the vector f can be written as f  (GN ( 2 q)  = fy(t)  —  (N 2 gN — 1)q’  —  ...  —  (3.15)  gNo)  + Au(t) in which each term is a vector of  polynomials in q operating on a sampled time series.  in order to write ii and w in  the same form we define the vectors 1  q  q  2 q  and  q,=  qNul  such that ii  =  qLu(t) and w  qN2  = qw(t).  qL\u(t)  =  (3.16)  Thus the control law can be rewritten as  K (qww(t)  —  y(t)  —  Au(t))  (3.17)  Chapter 3.  Control Algorithm  68  w y  Figure 3.1: SISO GPC closed loop system block diagram or (q  + Kg) xu(t)  =  Kqw(t)  —  Kfy(t).  (3.18)  This is a set of N difference equations of the familiar [78] form  R(q’)/Xu(t)  =  T(q’)w(t)  —  S(q’)y(t).  (3.19)  Combining this result with the difference equation (3.1) which describes the open ioop dynamics we see that the block diagram of the closed loop system is as shown in Figure 3.1. The overall closed loop transfer function is obtained by combining equations (3.18) and (3.1) so as to eliminate u(t): [(qu  Due to the structure of  + Kg) LA + KfBq’] y(t)  q  =  KqBq’w(t).  (3.20)  all but the first of the N difference equations in this expression  are non-causal. However the receding horizon implementation of the control law implies that the closed loop system will have the characteristics given by the first (causal) equation, which can be written as [R\A + SBq’] y(t)  =  TBq’w(t).  (3.21)  The characteristic polynomial of the closed loop system is the bracketed quantity on the left hand side. The closed ioop poles in the discrete time domain are obtained by  Chapter 3.  Control Algorithm  69  finding the roots of this polynomial and the closed ioop characteristics (natural frequencies, damping ratios, time constants) of the system in the continuous time domain can be found by using well known [80] relationships between the discrete and continuous time domains. The closed ioop characteristics which result from these calculations are an approximation of those exhibited by the long term response of the system due to the fact that the control law is implemented in an explicitly adaptive, receding horizon context. Nonetheless, it will be shown in Chapter 5 that these approximate characteristics are useful in selecting the controller parameters (horizons and weighting factors) which yield the most desirable response from among the set of parameters which stabilise the system. The foregoing formulation of the control law structure and the resulting expression for the approximate characteristic polynomial of the closed loop system is the result of extension and formalisation of the numerical example in an appendix of Clarke, e al. [29]. It differs in both form and intent from those in the literature [32,33,81]. Other authors have generally expressed the closed ioop structure of the system in a state space representation, in contrast to the transfer function form of equation (3.21). Their objective in so doing has been the investigation of asymptotic stability conditions, comparison to other algorithms, etc. The goal of the analysis presented here is to provide as direct a means as possible of assessing the effects of the controller parameters (horizons and weighting factors) on the closed loop performance of a given system. While the relationship among the parameters and the poles of equation (3.21) is admittedly nontrivial, the numerical calculation and assessment of those poles for a wide range of parameter values is straightforward. 3.3  Multivariable Predictive Control Algorithm  The algorithm derived in the preceding section provides excellent force and motion control for a single manipulator link (as will be demonstrated in Chapter 5). Such a system involves  Chapter 3.  Control Algorithm  70  a direct relationship between one input (the joint torque) and one output (the joint angle or contact force).  For a multiple link manipulator with torque producing actuators at  each joint a more complex situation exists in that several output quantities (contact force components, joint angles, tip position or velocity components, etc.) can, in principle, be controlled and each input contributes to each output. While the situation sometimes exists in which the coupling among the various inputs and outputs is weak and the system can be controlled by multiple single input, single output feedback ioops, this is generally not so in the case of manipulators with flexible links. Successful control of these systems requires a control algorithm in which the effect of all inputs on each of the outputs is accounted for. This is referred to as multivariable or multiple input, multiple output (MIMO) control. The primary reference papers for the GPC algorithm (Clarke, et  cii  [29, 32]) make no  mention of the extension of the algorithm to the multivariable case. It was not until the appearance of the Clarke and Mohtadi paper [33] over two years later that my attention was drawn to the multivariable derivation presented in Shah, et al. [82]. Thus, the generic multivariable predictive control algorithm presented below was derived independently of, but has a similar structure to, that which has appeared in the literature. In the course of that derivation it became evident that the extension of the algorithm with a new, static equilibrium bias term (derived in section 3.4) would provide performance improvements in manipulator contact force control applications. The formulation of the discrete time domain closed loop characteristic polynomial derived in section 3.2.1 and extended to the multivariable case in section 3.3.1 differs from the formulations of the closed loop structure previously reported in the literature [32, 33,81]. The formulation derived here is shown to be particularly useful for tuning the control algorithm in specific applications. For the purpose of deriving a multivariable predictive control algorithm based on the GPC algorithm reviewed in section 3.2, consider first a general multivariable system in the  Chapter 3. Control Algorithm  71  continuous time domain, described by Y(s)  =  H(s) U(s),  (3.22)  where Y(s) is an 0 n x l vector of outputs, U(s) is an 1 n x l vector of inputs and H(s) is an no  x n matrix of transfer functions, all transformed to the Laplace domain. Equation (3.22)  can be written as a system of n 0 linear algebraic equations: Yj(s)  =  i  Hk(s)Uk(s),  1,...  =  (3.23)  . 0 ,n  Each of the H k(s) terms is a transfer function in the form of a ratio of polynomials in 2 the Laplace operator, s. Supposing that these transfer functions can be transformed from the Laplace (continuous time) domain to the Z (discrete time) domain, the system can be described by a collection of difference equations in the backward shift operator, q : 1 A(q’) y(t)  =  Bk(q’)uk(t  1),  —  i  where y(t) is the time series of values of the ith output,  =  1,.. ,n , 0  Uk(t),  .  (3.24)  is the time series of values  of the kth input, and where A(q’) and Bk(q) are polynomials in q: A(q’)  =  1 +a q’ + a 1 q+ 2  Bk(q’)  =  0 + bIk b q+ 2 q’ + bk 1  .  .  .  + anaq, + bkflq  ...  Note that in inertially coupled systems like robotic manipulators the denominators of all of the 2 H k (s) transfer functions are identical and thus, upon transformation to the discrete time domain, only a single A(q’) polynomial is required. If disturbance terms, d(t), modelled as randomly occurring steps of random amplitude: d(t)  =  t),  where  A  =  1  —  , 1 q  (3.25)  and ( 4 t) is a random, uncorrelated sequence, are added to equation (3.24) the system model is analogous to the single input, single output CARIMA (Controlled AutoRegressive and Integrated Moving Average) model [76] used in section 3.2.  Chapter 3.  Control Algorithm  72  From this point the derivation of the optimal output prediction equations is very similar to that described in section 3.2, except that we are now considering each element of a set of output equations, {y, i  =  1,... , 0 n } , that depends on several inputs,  k  {uk,  =  1,.  .  .  rather than a relationship between a single input/output pair. With this in mind, the major results for the multivariable derivation can be stated as follows: • Diophantine identities: 1  E(q’) A(q )+q 1 3 F(q’),  =  =  1,..  1),  i  i  .  ,  , 0 n  (3.26)  • Optimal j-step ahead output predictors: th(t + jt) where  =  F(q’)y(t) +  Gk(q’)  Note that for each of the  =  G(q’)(t  E ( 2 q’) Bk(q’)  +j  and  —  Uk(t  +j  1,.  1)  —  .  .  (3.27)  ‘no,  tUk(t  +  j  —  1).  j steps in the future (t + j t) is composed of a series of filtered  past outputs and input increments which are known at time step  t  and a series of future  input increments which are yet to be determined, that is: (t + 1  It)  Fi(q ) 1 y(t) +  [G(q’)  —  j Uk(t) 0 g1k  +  gk k 0 (t),  i—  (t  +2  It)  =  F ( 2 q’)y(t) + +  (gi q 1 ’ +  [G(q)  ) Uk(t 0 g2k  —  g2k,q  + 1),  i  —  gi2] qük(t)  1,.  .  etc.  .  ‘no,  (3.28)  At this point a control horizon, N, analogous to that defined in section 3.2 is introduced, that is, it is assumed that after some number of time steps, N  <  , the control inputs will 2 N  be held constant. Introduction of this control horizon has similar benefits to those observed  Chapter 3. Control Algorithm  73  in the SISO case, namely, the computational complexity of the algorithm is significantly reduced and the robustness is improved. The optimal output prediction equations for N 2 steps into the future, subject to an N step control horizon, can be written in matrix form as: i  +,  =  =  1,...  (3.29)  , 0 ,n  ]T  where  Sri  + 1 t) (t + 2 t)  =  Uk =  k 3 G  )t) Fi(q ( 1 y+  u(t) u(t + 1) 9ilko  0  1 912k  0 912k  =  ...  ...  u(t + N  .  .  .  k(N_l) 2 gN  k(N g2N ) 2  .  [G,(q)  —  —  l 0 gllk  .  .  0  .  .  1)]  —  0  ...  .  [G(q’)  (q’)y 2 F ( 1 t) + = 1 f  {  +N  ,  and  0 k N 2 g  Uk(t)  1 g,.’ciq  —  ] qLUk(t) 0 g,  k=1  (q’)y(t) 2 FN  +  [G1k(q’)  —  k 2 g1N ) 1 q__1) (  —... —  ko} 2 giN  q_)Auk(t)  Note that each element of f. consists only of contributions from the past (i.e. known) inputs and outputs, filtered by coefficients derived from the known system model. As a result, the elements of f 1 can be viewed as predictions of the free response of the ith output if no further control action were applied. Defining  =  [w + 1) w (t + 2) 1  ...  (t + N 1 w )], 2  i  =  1,... ,n , 0  (3.30)  Chapter 3. Control Algorithm  74  as the set of vectors of setpoints for each of the outputs for the next N 2 time steps, the vector, ê, of optimal predictions of the output errors is:  e.  =  i= 1,...,n . 0  (3.31)  The cost function, J , which will be minimised to yield the set of optimal control input 2 increments is: +  ‘c  >  U  (3.32)  ii.  Minimisation of this cost function will minimise the sum of squares of the optimal predictions of the output errors.  At the same time, the energy of the control input increments is  minimised to an extent that depends of the value of ). The control energy is represented by the second term in equation (3.32) (the sum of squares of the control input increments). The expansion of equation (3.32) is shown in section B.1. Partial differentiation of that result with respect to Urn, an arbitrary member of the set of control input increment vectors, gives: = Urn  2  >  G  Gk Uk  + .icUm +  C (f,  —  w)].  (3.33)  i=lk=1  The set of control input increments,  {Um,m  2 0J —=O  1,.. . ,n}, that minimises J 2 is given by: . 2 m=1,...,n  (3.34)  GUm  Substituting into this expression from equation (3.33) and rearranging yields: m=1,...,n  (3.35)  i=1 k—_i  This is a system of linear, algebraic equations which must be solved simultaneously to yield  Chapter 3. Control Algorithm  75  the desired control input increments. Written in matrix form the system is: G  1 G  G  2 G  G  G’  ...  ...  +  riT  rT  ‘-11  ‘‘21  12 G  G  .  .  1 G  11 G  12 G  2 G  21 G  22 G  1 G  Gn2  .  1 W  ...  2 G’  —  ii  ...  ...  .  .  2 G  .  2 U  Un  ii  —f 2 w  (3.36)  ‘1n  T 1 2n  0 W,.  ç —  The matrix of Gk terms can be considered as a composite matrix (that is a matrix, each of whose elements are matrices), G, with the result that the control law can be written more compactly as  ui  —f 1 w  2 U =  (I+GTG)  —1  GT  —f 2 w  : 0 w  —  (3.37)  f,  which is recognizably analogous to the SISO control law, equation (3.13). Each of the Urn vectors in equation (3.35) is N. x 1 and the w and f 2 vectors are N 2 thus the vector of Urn vectors on the left hand side of equation (3.36) is vector of (w,  —  ><  1,  x 1 and the  ) vectors on the right hand side is n 1 f 2 x 1. Correspondingly, each of N 0  the Gzrn matrices in equation (3.35) is N 2 x N and thus the product matrices G G k 2 are N x N symmetric matrices. Hence, the left hand side matrix in equation (3.36) is an . These dimensions of 2 N 0 N x n 0 n N matrix and the right hand side matrix is n 2 N x n 0 the left hand side matrix imply that a unique set of control inputs exists only if the number of inputs and outputs are equal. If the number of outputs exceeds the number of inputs the  Chapter 3.  Control Algorithm  76  system is underspecified and not all the the control objectives can be satisfied. With more inputs than outputs the system is overspecified and additional criteria must be introduced to obtain a unique set of controls. In the case of robotic control this means that the number of force, torque or motion components that can be controlled is equal to the number of actuators available. 3.3.1  Control Law Structure and Closed Loop Characteristics  The method used in section 3.2.1 to investigate the structure of the SISO GPO control law can be extended, using composite matrices, to explore the structure of the multivariable control law and the characteristics of the resulting closed ioop system. Replacing the com posite matrix  + GTG)  1  GT  in equation (3.37) by K, the control law can be written  in a form analogous to equation (3.14): 1 ü  1 w =K  :  —  (3.38)  :  w, where the gain matrix K is an nN x m 2 matrix of scalars (or, equivalently, an n x n N 0 0 composite matrix of N x N 2 gain matrices). The f vectors can be written as vectors of polynomials in q operating on sampled time series by employing the vector f defined in equation (3.15) and extending the definition of from the same equation to I, —1\ ‘-ii.q i I_v  —1 q (Gk2(q =  qN2l  Yzklo  )  q 1 gk2  —1 —  gik2o)  (3.39)  (GjkN ( 2 q_1) —  —  .  .  .  —  gikN o 2 )  Chapter 3. Control Algorithm  77  Thus  1 =  0  0  0  yi(t)  0  y(t)  Au ( 1 t) +  Lu(t) .  2 fN  0  0  yn(t)  2 g  n1  (3.40)  Lu(t)  Likewise, employing the vectors q and q, defined in equation (3.16), the vectors of  iiks  and ws are written in the form of polynomials in q’ operating on sampled time series: ii 112  =  q  0  0  q,  •.•  0  zui(t)  0  Au ( 2 t) (3.41)  0  0  q.  q  0  0  wi(t)  0  q  0  W2(t)  and  W2  =  (3.42)  w,,  0  0  q  Using these expressions the control law can be rewritten as ui(t)  wi(t)  U ( 2 t)  (quI+KG)z  yi(t)  W ( 2 t)  u(t) This is a set of n N difference equations. 1  y ( 2 t)  KfI  =KqI  w ( 0 t)  .  (343)  Chapter 3. Control Algorithm  78  With the difference equations (3.24) which describe the open loop dynamics of the system expressed in matrix operator form: u(t)  yi(t)  A(q’)  (t) 2 y =  (t) 2 u  1 B(q’)q  (344)  u(t)  y(t)  where  11 B ) 1 (q  12 B (q’)  21 B ) 1 (q  22 B ) 1 (q  ) (q 1 Bi(q 02 B ) 1  .  .  .  ...  ...  (q’) 1 B (q) 2 B  (q’) 0 B  the vector of y (t)s can be eliminated to yield 2 ui(t) (qui  + KG) AA + KfIBq’]  : u(t)  wi(t) =  AKqI  (t) 2 w :  (3.45)  (t) 0 w  This expression is a closed ioop transfer function relationship between the setpoints and the control inputs. Manipulation of equations (3.43) and (3.44) to arrive at the more conven tional closed ioop transfer function representation relating the setpoints to the outputs is problematic because it involves the inversion of the B matrix of q’ polynomial operators. Fortunately, because the inputs and outputs are linearly related by the control law, this difficulty affects only the eigenvectors of the closed ioop representation; the eigenvalues of the matrix on the left hand side of equation (3.45) are the poles of the closed ioop system. The calculation of the approximate, continuous time domain, closed ioop characteristics from these poles is shown in Chapter 5 to be a useful method of tuning the multivariable control law.  Chapter 3.  3.4  Control Algorithm  79  Static Equilibrium Bias Term  Generic, linear input/output models are used to describe the systems on which the deriva tions of the SISO GPC algorithm (section 3.2) and its multivariable counterpart (section 3.3) are based. While this structure results in a control algorithm that is applicable to many diverse systems, it is desirable to customise the control algorithm for a particular system by using additional knowledge about that system and thereby improve the control. In the case of control of contact forces between a robotic manipulator and its environment the dynamic model of the system can be used to form a relationship between the desired force levels (setpoints) and the actuator input levels required to maintain those forces when the system is in static equilibrium. This information is introduced into the control algorithm by augmenting the cost function with a term that weights the sum of squares of the deviation of the actual total control input levels from the calculated static equilibrium levels. This term is referred to as a static equilibrium bias term and its addition to both the SISO and multivariable predictive control algorithms is an original contribution of this thesis. The static equilibrium bias term acts as a proportional feedforward term in the control law. This term has the beneficial effect of reducing the step response rise time but this improvement comes at the expense of introducing a small amount of overshoot. Simulation results discussed in section 5.3.4 illustrate these characteristics. The function which maps the set of setpoint levels, {w, i  =  1,.  .  step onto the corresponding set of static equilibrium control inputs,  .  ,  }, at a given time 0 n  {Ük,  k  =  1,.  .  .  ,n}, is  obtained from the equations of motion of the system and the contact force model. Details of the necessary manipulations of equations (2.47) through (2.52), (2.56) and (2.57) to obtain the mapping function for the case of a two flexible link manipulator are shown in Appendix C.  Chapter 3.  Control Algorithm  80  Defining vectors, analogous to the vector of future setpoints (equation (3.30)), for the actual control inputs,  =  {  Uk,  and the calculated static equilibrium inputs,  Uk,  Uk(t  + Nu)]  k = 1,... ,n  (3.46)  tLk(t  + N)  k  (3.47)  u(t+ 1) uk( + 2)  and =  [Ük(  + 1)  Uk(t  + 2)  1,.  .  .  ,n,  the static equilibrium bias term for the cost function is the sum of squares of differences between  Uk  and T  -  Jse  >Z(uk_tlk)  -  (3.48)  (Uk —Uk).  This term is weighted by a factor, ,X, and added to the predicted output error and incre mental control energy cost function terms given by equations (3.32) to yield the overall cost function 2 + .X J eJse 8 =  + cÜÜk +  The vector of control inputs, input increments, Defining  i  Uk,  Uk,  —  uk)T (Uk  Uk).  (3.49)  is formed from the (previously defined) vector of control  by adding the future increments to the current input value, u=  Uk(t).  as a unit vector and L as a unit lower triangular matrix, 1 1 IL=  .  100...0 and  1 Uk  \se(Uk  L=  110...0  1l1...1  can be written as: Uk  = u + LUk.  (3.50)  Chapter 3.  Control Algorithm  81  Substituting equation (3.50) into equation (3.49) yields: 3 = J  : eej +  \c Ü  U  + se  (u  H- Lfik  + Lük  Uk)  —  Uk).  (3.51)  Since J 3 is a linear combination of J 2 and Jse the principle of superposition allows the results of the expansion and minimisation of 18e to be added to the results given by equation (3.35) to obtain the overall control law. Expanding Jse and differentiating with respect to Urn (an arbitrary member of the set of control input increment vectors, see section B.2 for details) gives: (3.52) The set of control input increments,  {Um,m  OJse --—=0, oum  = 1,... ,n}, that minimises Jse is given by: m=1,...,m.  (3.53)  Applied to equation (3.52), this condition yields: LTLIIm  LT  (m —uji.),  rn= 1,...,n . 2  (3.54)  Adding this set of equations, weighted by .se, to (3.35) gives >GnGikUk +AcUm+seLTLUm = >G irzl kr1  (w _fj)+AeLT (Urn  _UjL),  jzl  m=1,...,n  (3.55)  an augmented system of linear algebraic equations which can be solved for {Um,m = 1,.. 2 ,n } . These equations can be written in matrix form similar to equation (3.36): .  G  1 G’  G  G  ...  ...  ...  2 G  G 1 1  G 1 2  G 2 1  G 2 2  G 0 2  ...  ...  G 1 1 2 G  Chapter 3. Control Algorithm  82  LTL  0  0  1 U  0  LTL  0  2 U  0  0  L’ L  1 Un  -  +.\3e  ‘T  ...  rIT  11 LW  Wi  ‘21  GT 12  iT  ‘22  2 0 n  -‘iT 1 “1n  CIT ‘2n  non  CIT  2 W  T  )iseLT  0  0  AseLT  wna  ...  ...  +  fi  —  2 f  —  —  -  0 fTl  0  1 U  0  2 U  0  —  —  U , 1 i 0  —  —  L 2 U  (3.56) 0  0  ...  .X e 3 LT  iin  —  p 1 u  This linear system has the same dimensions as equation (3.36). The matrix LT is N x N and the vector of static equilibrium biases on the right hand side is n N x 1. The 2 requirement of an equal number of inputs and outputs for a unique solution also holds. With the matrix of Gk terms represented by a composite matrix; G, equation (3.56) can be rewritten as —f 1 w —f 2 w =  1 K  (3.57)  0 w  —  f  —  u,i  -  Chapter 3. Control Algorithm  83  where —1  LTL 1 K  =  o  )I + GTG +  ...  0  LTL...  0  0  :  :  o  o  GT  (3.58)  : ... LTL  and —1  e 3 A 2 K  AcI+GTG+Ase  LTL  0  ...  o  LTL  ...  .  o  .  o  ...  0  LT  0  ...  0  0 .  0 .  LT  ...  o  LTL  0  0  ... LT (3.59)  Comparing this result to equation (3.37) it is evident that the control laws differ slightly in the structure of the inverted quantity in K and K . The more significant difference is the 1 addition of the second (K ) term in equation (3.57). This term make a contribution to the 2 control input which is proportional to the difference between the expected static equilibrium inputs  (u)  and the current levels (u). The magnitude of the contribution is governed by  the value of ?. Thus, the term produces a proportional feedforward action in the control law.  3.5  Control Algorithm Parameters and Tuning  The multivariable predictive control algorithm has four tuning parameters: 1. N , the prediction horizon; 2 2. N, the control horizon; 3. ), the input increment weighting factor; and,  Chapter 3.  ‘ae,  Control Algorithm  84  the static equilibrium bias term weighting factor.  The prediction horizon specifies the number of time steps into the future for which predicted system outputs will be calculated and included in the control law. Clarke, et al. [29] recommend that N 2 be set comparable to the rise time of the open ioop system. The prediction horizon should also be far enough in the future to include the negative-going part of a nonminimum phase open ioop response. Too short a prediction horizon causes the controller to work with insufficient information and the control is likely to be poor or even unstable. Increasing N 2 causes the poles of the closed ioop system to move toward the locations of the open loop poles. The control horizon is the number of time steps in the future after which the control input level will be held constant, that is, all future increments set to zero. This horizon has significant effect on the computational load of the algorithm since the left hand side matrix of equation (3.56) (which must be inverted) has dimensions n N x 2 0 n N . Clarke, et al. [29] recommend that N,. be greater than or equal to the number of unstable or poorly damped zeros in the open ioop system. In the multivariable form of the algorithm the control horizon should be at least equal to the number of inputs to the system. Increasing N,. generally increases the magnitude and rate of change of the control input signals. To a certain extent this increased activity leads to higher performance control but too large a value for N,. may result in excessive ringing of the control signals and significant high frequency components in the step response transients. The input increment weighting factor governs the contribution to the cost function of the sum of squares of control input increments term, that is, the incremental control energy. With ) set to zero this term has no effect on the cost function or the control law. The use of a nonzero  has been found to introduce a pole on the negative real axis into the  continuous time representation of the closed loop system. This pole moves toward the origin  Chapter 3. Control Algorithm  as  85  is increased. In the context of contact force control this additional pole may have a  stabilising effect for the case of a rigid link manipulator but its interaction with the poles due to other modes in the flexible link case generally results in a slower step response. The static equilibrium bias term weighting factor controls the extent to which that term influences the control law. A value of zero for Xse results in no static equilibrium bias. Increasing Ase causes the control inputs to tend toward the calculated static equilibrium values. Thus the static equilibrium bias term can be viewed as a proportional feedforward term in the control law with a gain related to )se. The fact that the contribution of this term to the overall control input signal is larger when the output is far from the setpoint than when it is near means that the response will generally be faster as ) is increased. However, as the results in Chapter 5 show, this performance improvement comes at the expense of introducing overshoot into the response. Clarke, et al. [29] include one additional parameter in the GPC algorithm: the delay horizon, N . This horizon serves to reduce the computational load of the algorithm in 1 situations where there is a delay between the application of a control input increment and its effect appearing on system outputs. Such situations are common in process control applications but in the case of robotic force control there is no delay, provided that the control law calculations can be completed with a single time step, and N 1 is set to 1. In principle, a different prediction horizon value, N , 2 2 could be used for each output in the multivariable algorithm. Similarly, independent values for N, X, and Ase for each input could also be specified. The resulting algorithm is substantially more complex to implement and no clear advantage can be seen in using multiple horizon and weighting factor values.  Chapter 3. Control Algorithm  3.6  86  Parameter Estimation  As was noted at the beginning of this chapter the inherently stiff nature of the dynamics of a manipulator in contact with its environment as well as the strong coupling and nonlinear dynamic effects present a significant challenge in the design of a controller for contact forces. This challenge is met, in part, by implementing the control algorithm in an explicitly adaptive context, that is, the values of the coefficients of the polynomials in the controller model (equation (3.24)) are estimated on-line from the time series of calculated control inputs and sensed contact force outputs. This parameter estimation is accomplished by a recursive least squares (RLS) algorithm. The basic RLS algorithm [77] is described by a pair of update equations, one for the vector of parameter estimates,9 and the other for the covariance matrix, P: (Yk  + 1 1) IE k  rki ((‘k  —  —  ‘  k—i —  —  ‘k-i)  (3.60)  k  1  I  ‘I’k  .iT E k—i + Yj  0  >  U,  ‘Pk  where cbk is the regressor vector of input and output data values and yk is the value of the system output at time step k. This algorithm has optimal convergence and stability properties when the parameters to be estimated are time invariant. Unfortunately, the basic algorithm is unsuitable for tracking time varying parameters because its gain converges to zero. A large number of modified RLS algorithm have been developed to overcome this and other limitations. In the early stages of the research the U/D RLS algorithm of Bierman [83] was employed. The U/D RLS algorithm factorises the covariance matrix into the product of an upper tri angular and a diagonal matrix (U/D factorisation) and thereby ensures that the covariance matrix will always be positive definite. Tracking of time varying parameters is facilitated by use of an exponential forgetting factor to weight older data less heavily than new. The  Chapter 3.  Control Algorithm  87  effect of this weighting is to establish a nonzero lower bound on the minimum eigenvalue of the covariance matrix and thereby prevent the occurrence of zero elements in the gain term of the parameter update equation (3.60). Unfortunately, exponential forgetting can, in the absence of sufficient excitation in the input data, lead to covariance wind-up; the unbounded growth of the eigenvalues of the covariance matrix which leads to corresponding growth of the elements in the gain term of equation (3.60). All of the adaptive control results presented in Chapter 5 employ a newer modified RLS algorithm developed, by Salgado, et al. [841, known as EFRA which stands for Exponential Forgetting and Resetting Algorithm. As well as using the exponential data weighting nec essary to track time varying parameters the EFRA algorithm also incorporates exponential resetting of the covariance matrix in such a way as to give new data unprejudiced treatment. Furthermore, the algorithm provides a means of establishing bounds on the maximum and minimum eigenvalues of the covariance matrix, thus avoiding covariance wind-up.  The  update equations for EFRA are:  =e +  LePklbk  (3.62)  (Yk -  = where ce,  / e 3 , e 8,  —  ePk_1kPk_1  l+41Pk—1çbk  +e1  SeP_i,  (3.63)  and ) are constants. Salgado, et al. [84] give the following guidelines for  the values of these constants: •  adjusts the gain of the least squares algorithm; typically  • /3e  is a small constant which is directly related to the minimum eigenvalue of P;  typically/3e • 6 e  e E  [0.1, 0.5];  [0,0.01];  is a small constant that is inversely related to the maximum eigenvalue of P;  typically  Se E  [0, 0.011;  Chapter 3. Control Algorithm  88  Xe is the usual exponential forgetting factor; typically A, E [0.9,0.99]. Figure 3.2 illustrates the superior performance of EFRA compared to the U/D RLS algorithm. The system being identified is described by a simple moving average: Yk  that is,  Uk  auk  where  + buk_l,  is a square wave centred about  [i + sgn (sin k)],  Uk =  (3.64)  with a period of 100 time steps. The parameter  values change in time according to the following schedule: k  a  b  [1,789]  0.1  0.2  [790,999]  0.2  0.4  [1000, 1489]  0.4 0.8.  The vector of parameter estimates is T  with initial value =  90 =  [  a  b  and the regressor vector is cIJk—_[Uk  Uk_lj.  The constants for EFRA are =  0.95,  ae = 0.5,  13e =  0.005,  and for the U/D RLS algorithm the forgetting factor is  Se ?u/d  =  0.005, 0.95.  Figure 3.2 shows the evolution with time of the parameter values calculated by EFRA (Figure 3.2a) and the U/D RLS algorithm (Figure 3.2b). Comparing the EFRA and U/D RLS results, the most striking difference is the presence of spikes in the estimates calculated by U/D RLS. These spikes are very poor estimates of the actual parameter values. Using  Chapter 3.  Control Algorithm  89  .8 .7 U) C).  0  0  100  200  300  400  500  600  700  800  900  1000  1100  1200  800  900  1000  1100  1200  Time Step (a) EFRA parameter estimates  3.5 3.0 2.5 2.0 ci) i.5 0  E 4-.  Cl)  w  .5  ci)  0  .5 -2.0  -2.5 -3.0  0  100  200  300  400  500  600  700  Time Step (b) U/D RLS parameter estimates  Figure 3.2: Comparison of EFRA and U/D RLS parameter estimation algorithms.  Chapter 3.  Control Algorithm  90  those values in a controller model leads to inaccurate calculation of the required control inputs which, in turn, yields poor control and perhaps even instability. Finally, it should be noted that the update equations (3.62) and (3.63) for EFRA are equally applicable to single input, single output systems as above and to multivariable systems of the form of equation (3.24). For a multivariable system the vector of parameter estimates and the regressor vector are augmented to include the coefficients and data from each of the inputs:  =  [  ]T  1 a  ...  anai  b 1 0  ...  1 b1b  ...  bno  ...  (3.65)  and  ik  =  {  Yk  ...  Yk—na  Ulk  ...  U1(k_nbi)  ...  ...  (3.66)  At each time step the estimation algorithm must be executed once for each system output. 3.7  Adaptive Controller Implementation  The overall control strategy implementation is shown in Figure 3.3. This strategy allows control of both the contact forces and the gross motions of the manipulator when it is not in contact. In the latter case the joint angles or velocities are taken as the system outputs. Due to the discontinuous nature of the system dynamics, the transfer function models used in these two modes of control are necessarily distinct and of different order. The force and motion models are identified and maintained in parallel so that switching between control modes can be accomplished smoothly. The contact control logic block controls selection of the appropriate (force or motion) control law, based on data from the force sensor and joint encoders and the trajectory planner, as well as controlling whether or not the transfer function models are updated by the parameter estimation blocks. The force and motion  Chapter 3.  Control Algorithm  Figure 3.3: Control strategy block diagram.  91  Chapter 3. Control Algorithm  92  setpoint programs are assumed to be provided by the higher levels of the control hierarchy which include the operator, a task planner and a trajectory generator. With the parameter estimation blocks enabled, the controller is explicitly adaptive. At each time step the coefficients of the transfer function model currently in use are estimated using the Exponential Forgetting and Resetting Algorithm (EFRA) [84]. The estimated coefficients are assumed to be instantaneously exact and the set of control input increments for that time step is calculated. At the next time step a new set of coefficients is estimated and used to calculate a new set of control input increments. The performance of the overall control strategy in situations which necessitate switching between force and motion control is discussed in section 5.4. 3.8  Stability and Convergence  Stability and convergence are important and difficult issues in the design of adaptive con trollers. For a given set of values for the model coefficients the stability of the control algorithm part of an adaptive controller can be considered. In the case of the CPC al gorithm it has been shown [33] that, with a finite prediction horizon 2 (N ) , the resulting closed—loop control is stable provided that: 1. the open—loop system model is stabilizable and detectable, 2. the delay (N ) and control (Nu) horizon are selected with respect to the number of 1 system model states (n) such that N 3. the control weighting factor  ()  =  1 N  is small (i.e.  n and N 2 =  —*  —  1 N  n  —  1, and  0).  It should be noted that this is only a sufficient (but not necessary) condition for which stability of the algorithm can be proven. The GPC algorithm is demonstrably stable for sets of parameters (N ,N 1 , Nu, and 2  ?) other than those which satisfy the above conditions.  Chapter 3.  Control Algorithm  93  Given this result, the issue of stability of the adaptive GPO controller becomes one of whether or not the system identification algorithm provides model coefficients which describe a stabilizable and detectable open—loop system. The Exponential Forgetting and Resetting system identification algorithm (EFRA) [84] discussed in section 3.6 is of the recursive least squares (RLS) family. The asymptotic properties of this type of algorithm are well established [85]. However, those asymptotic properties are concerned with the “best possible predictor” for the system which the algorithm can determine. The relationship between that predictor and the stabilizable and detectable model of the open—loop system which is required for stability of the adaptive GPO controller is dependent on such factors as the number of model coefficients and the system input. In light of these factors, it is evident that an analysis of the dynamics of the system to be controlled is necessary to select an appropriate number of model coefficients and to select initial values for those coefficients which satisfy the stabilizability and detectability requirements of the adaptive GPO algorithm. Augmentation of that analysis with a series of identification experiments on the system would most certainly be beneficial.  39  Summary  This chapter presents the development of a manipulator contact force control strategy based on an explicitly adaptive implementation of a predictive control algorithm. The control algorithm is based on the Generalised Predictive Control (GPO) algorithm of Clarke, et at. [29]. This form of predictive control algorithm was selected over other control methods because it incorporates the following desirable features: • inherent integral action; • setpoint tracking;  Chapter 3.  Control Algorithm  94  • ease of implementation in explicitly adaptive, digital form, and; • inclusion of non-minimum phase and lightly damped mode characteristics in predictor model. The primary contributions of this chapter are the extension of the generic predictive control algorithm with a new static equilibrium bias term that allows knowledge of the system dynamics to be employed to improve the controller performance, development of a new formulation of the closed loop characteristic polynomial which is directly useful in controller tuning, and introduction of a contact control logic level in the control hierarchy to deal with the discontinuity involved in making and breaking contact between the manipulator tip and the environment surface.  Chapter 4  Dynamics and Control Simulation  4.1  —  TWOFLEX  Introduction  The simulation computer program TWOFLEX incorporates the dynamic model of the pla nar two flexible link manipulator and deformable environment derived in Chapter 2 and the adaptive single- and multi-variable long range predictive control algorithms derived in Chapter 3. The code is written in FORTRAN. It was originally developed on a VaxStation 3200 under the VMS operating system and subsequently ported to a Sun SparcStation under SunOS. Both versions of the program use double precision arithmetic throughout. Results of the runs are stored in text files as tabulated time series of related groups of vari ables and as plots of the evolution of selected variables with time. The dynamics models of the links, actuator, and contact regions are coded in such a way that the program can easily simulate subsystems of the two flexible link manipulator system (e.g. a single rigid or flexible link or two rigid links). Likewise, the controller structure to be simulated (i.e. SISO or multivariable, adaptive or constant parameter, force, motion or combined control, etc.) is also selectable. The particular configuration of dynamics model and controller to  be used for a given run is specified by a set of parameter values which are read from a data file each time TWOFLEX commences execution. This chapter begins with a description of the overall structure of the TWOFLEX program in section 4.2. Section 4.3 presents the details of the numerical integration of the equations of motion for the manipulator/environment system. The description of the program concludes  95  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  96  in section 4.4 with a discussion of the routines that simulate the discrete time control algo rithm. The structure of these various parts of the program are illustrated with flowcharts. A complete listing of the documented source code for the TWOFLEX program and the associ ated subroutine libraries (approximately 18,200 lines of FORTRAN code in total) is available in the UBC CAMROL (Computer Aided Manufacturing and Robotics Laboratory) Report number R.92—2 [34]. Section 4.5 describes several tests which were conducted to validate the operation of the program by comparison to a simulation of a multibody space station  based flexible manipulator developed by Mah [35]. The chapter concludes with a summary of the features of the TWOFLEX code.  4.2  General Description of the TWOFLEX Program  A flowchart of the overall structure of the TWOFLEX program is shown in Figure 4.1. The steps in the code execution, as represented in Figure 4.1, are described in the following paragraphs. The program begins with the reading and processing of data from the input files. These  data specify the configuration of the system for the run, the initial conditions, and, nu merous logical flags to select various options in the code. The main input file for a typical  run consists of about 50 lines of data. The input routines use free formatting and permit comments to be interspersed with the data by placing them on lines beginning with an  *  character. Auxiliary input files contain the setpoint information for the controller. Process ing of the input data includes calculation of various quantities needed in the simulation. In particular, values of the mode shape functions and the corresponding inertia integral factors,  ‘mkt,  for the equations of motion are calculated at this point.  Output initialisation involves opening of, and writing headings to, the various files which  will receive output data as the simulation proceeds. A summary of the configuration for  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  97  False  Figure 4.1: Flowchart of the overall structure of the TWOFLEX dynamics and control simula tion program.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  98  the run is written out at this point, both to serve as a reference when reviewing stored run results and to provide a check on the validity of the input data. Next, the initial state of contact is checked. If the initial conditions are such that the apparent separation between the tip of the manipulator and the surface of the environment is negative, contact exists and the components of the contact force are calculated. In the case of positive separation, contact does not exist and the contact force is zero. Following these initialisation steps, the main loop of the simulation begins. This loop is controlled by the value of the integration time variable, t , and proceeds in steps of dt. 2  The value of dt, is generally selected to be quite small since the main loop includes the integration of the equations of motion which simulates the response of a continuous time system. Within the main ioop, the integration time, is checked to determine if it is modulo dtd,  the discrete time step. Only if that condition is true are the discrete time control  algorithm routines executed. The discrete time control algorithm consists of the contact control logic, the parameter estimation algorithm and the control algorithm itself, which calculates the input signals to be applied to the actuators. Section 4.4 describes these parts of the program in detail. Flags in the input data file can be set to disable the execution of the control algorithm and instead apply one of a set of open loop input signals to the system. The available open loop input signals include impulse, step, ramp, square wave, sine wave and white noise. These signals are useful in observing the forced, but uncontrolled, response of the system. Regardless of the source of the input signal, the main loop continues with the integra tion of the equations of motion over the time interval dt. The DGEAR routine from the International Mathematical and Statistical Library (IMSL) [86] is used for the integration. A discussion of the features of DGEAR and the details of the form of the equations which are actually integrated is included in section 4.3.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  99  After completion of the integration time step, the new state of the system is checked to determine if the contact state has changed. If the manipulator tip is in contact with the environment surface the components of the contact force are calculated. The new state of the system and a collection of data calculated from it are written as text to the time series output data files. Some of those values are also stored for later plotting.  Finally, the system state is checked to determine if any of a set of limits on quantities such as the manipulator joint angles have been surpassed. This limit checking allows runs in which the system state “blows up” to be halted before the entire duration of the integration has been calculated while preserving, for later analysis, the evolution of the response up to the time at which the run is terminated. If the time specified for the end of the integration has not been reached the main loop is repeated, after incrementing the integration time, t, by dt . When the loop has been 2 completed for the last time the final results of the simulation are written to the output data files and plotted.  4.3  Integration of Equations of Motion  The equations of motion for the manipulator/environment system are integrated numerically using the IMSL routine DGEAR to yield the approximate continuous time response of the system. The DGEAPL routine is based on the work of Gear [87]. It is a backward differencing method, meaning that it uses backward difference formulae to approximate derivatives. The algorithm incorporates adaptive step size selection which allows optimal performance to be achieved by subdividing the interval over which the integration is being performed as necessitated by the characteristics of the equations, without user intervention. The DGEAR routine was selected for two reasons: 1. it is specifically designed for integration of stiff systems of differential equations, and,  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  100  2. it is well suited for integrations from which results are required at frequent, small increments of the independent variable. A system of differential equations is said to be stiff if there are two or more very different scales of the independent variable which simultaneously effect the dependent variables [88]. A more mathematically rigorous definition [86] is that the system of first order ordinary differential equations given as 2 y’=f , 1 , ...,y y (x,y )  is stiff if some of the eigenvalues of the Jacobian matrix of partial deLivatives,  (4.1)  of  are  large and negative. As was noted in Chapter 3, the system of differential equations (2.47) through (2.52) is stiff because the natural frequencies of the system can easily span several orders of magnitude and thus the generalised coordinates are composed of variations with several, widely varying time scales. The simulation uses a small time step, dt , in the main integration ioop in order to 1 adequately reflect the continuous time nature of the dynamics in contrast to the discrete time control algorithm. Small time steps are also required to adequately resolve high frequency components of the response in graphical presentation of the simulation results. Finally, and most importantly, small integration time steps are necessary to properly detect the making and breaking of contact between the manipulator tip and the environment. If too large a time step is used the manipulator will appear to penetrate the environment during the time step in which contact is made and an erroneously large transient contact force will result. The maximum permissible time step is dependent on the stiffnesses of the contacting surfaces.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  101  The DGEAR routine integrates systems of first order, ordinary differential equations. Equations (2.47) through (2.52) are a system of second order ordinary differential equa tions of the form:  Jj=b(q,4).  (4.2)  Such a system is easily transformed to first order by the substitution: 1 q  =  and  q  2 q  =  4  (4.3)  which yields, I  0  41  q2  0 J  q  2 , 1 b(q ) q  .  (4.4)  In this representation, J is the generalised inertia matrix, q 1 is the vector of generalised coordinates, 4i  =  q is the vector of generalised velocities, 42 is the vector of generalised  accelerations and b (q ,q 1 ) is the vector of values of the right hand sides of the equations 2 of motion. The matrices, I and 0, are identity and zero matrices, respectively. The state vector for the system is [q 1  q ] 2 T•  The DGEAR routine calls a user provided routine which must return the first time deriva tive of the state vector. The generalised velocity part of that vector is simply copied from the most recently calculated system state and the generalised acceleration part is calculated by the steps shown in the flowchart in Figure 4.2. Since the dynamics of the DC motor actuators are represented by first order, ordinary differential equations the state variables from those models are simply concatenated onto the end of the state vector. Likewise, right hand sides of the actuator model equations are concatenated onto the right hand side of equations (4.4) for simultaneous solution with the equations of motion.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  102  Calculate Inertia Matrix  1  Calculate Right Hand Side Vector  L  1  Add actuator coupling terms to inertia matrix and RHS vector  ‘I;  Invert Inertia Matrix  Multiply RHS vector by Inertia Matrix Inverse to yield Generalised Acceleration Vector  Figure 4.2: Flowchart of the calculation of the generalised acceleration vector. 4.4  Discrete Time Control Algorithm  The control algorithm described in Chapter 3 is implemented in a series of subroutines which are executed only when the integration ioop time, t, is a whole multiple of the discrete time step, dtd, (that is, t mod dtd  =  0). Figure 4.3 shows a flowchart of the steps in the control  algorithm. The first step in the control algorithm is the contact control logic block. This part of the algorithm takes, as inputs, signals from the manipulator force sensor and joint encoders, information from the trajectory planner and instructions from the operator. These data are analysed and used to generate the sequences of future setpoints, to determine which control mode (force or motion) should be used, and to decide whether or not the parameter estimation algorithm should be activated to update the controller model coefficients. The primary purpose of the contact control logic is to govern the actions of the control algorithm  Chapter 4. Dynamics and Control Simulation  TWOFLEX  hitialise Diophantine Identity Polynomials for Recursion Formulae  Figure 4.3: Flowchart of the discrete time control algorithm.  103  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  104  during the time period immediately before and after a change in the contact state. This function is particularly important when the manipulator unexpectedly comes into contact with the environment, in which case the impact velocity may be large and result in contact force transients which the predictive control algorithm cannot immediately compensate for. Some examples of the categories of programs that the contact control logic can execute are:  • assuming the trajectory planner has an accurate world model of the manipulator and its surroundings, the speed of approach to a surface and the time at which the control should be switched between motion to force modes can be specified and the contact control logic generates appropriate setpoint sequences and switching signals; • in unknown or uncertain surroundings the contact control logic switches the control mode from motion to force when a nonzero force signal is sensed and from force to motion when the signal goes to zero; • when contact is sensed by the force sensor the contact control logic switches to force control mode and briefly modifies the contact force setpoint sequence from that pro vided by the trajectory planner to a form suitable to compensate for the contact force transient; the setpoint modification is based on an heuristic rule. Contact control logic programs are discussed in more detail, with examples of the resulting closed ioop responses in Chapter 5. If the contact control logic program specifies that the control should be adaptive in the current time step the coefficients of the controller model are updated using one of the parameter estimation algorithms discussed in section 3.6. Both the U/D RLS algorithm and EFRA are implemented in the TWOFLEX code. The algorithm to be used is selected by a flag set in the input data file. The algorithm parameters are also included in the data read at the beginning of the run.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  105  The actual calculation of the control input signal increments begins with the initialisation of the polynomial coefficients in the Diophantine identity recursion formulae. With those coefficients and the coefficients from the controller model, the free response vectors, f , for 1 one time step into the future are calculated. Following the initialisation step, the Diophantine identity polynomial coefficients and free response vectors are calculated for each of the future time steps up to the prediction horizon, N . 2  The final step in the predictive part of the algorithm is the calculation of the future static equilibrium input sequences from the specified future setpoint sequences. This step is only applicable in the case of force control since a direct relationship is available between desired contact forces and the static equilibrium input levels that would be required to produce them. For computational efficiency the control horizon is checked before beginning to calculate the control input increments. If N  =  1 the calculation can be quickly performed as a scalar  operation in the case of a SISO system or by Cramer’s rule for a two input, two output MIMO system. For N > 1, the future step response matrices, 1 G k , and their transposes  are formed and the left hand side matrix and right hand side vector terms in equation (3.56) are constructed. The system of linear algebraic equations described by these terms is solved for the sequences of future control input increments. Finally, the control input increments for the next time step are added to the control input signals. The entire discrete time control algorithm described above is embedded in a general input calculation subroutine in the TWOFLEX code. That routine uses flags set in the input data file to select the type of input to be used, either the control algorithm or the one of the open loop inputs described in section 4.2. The input calculation routine also applies the appropriate actuator saturation limits to the inputs.  Chapter 4. Dynamics and Control Simulation  4.5  —  TWOFLEX  106  Validation  The simulation of the unforced manipulator dynamics was validated by comparison with the results of a program developed by Mah [35]. Mah’s program simulates the dynamics of an orbiting platform and manipulator system modelled as a chain of flexible links connected by flexible joints. It has been validated [89] against two other simulation programs for related systems independently developed by Chan [68] and Ng [72]. Two types of validation test were carried out: 1. the calculated inertia matrices and right hand side vectors for the equations of motion with specified initial conditions were compared; and, 2. the evolution with time of the values of the generalised coordinates were compared for simulations of the free response of the system from various initial states. The calculated initial values of the elements of the inertia matrices and right hand side vectors were compared for the seven cases listed in Table 4.1. The links were modelled as 1.Om long, 2.0cm diameter aluminium rods in all cases. For each of these cases the initial inertia matrix and right hand side vector were in agreement to at least three significant digits.  The agreement was least good for terms which depend on the integrals of the  flexible link mode shape functions.  This is due to the fact that Mah calculates those  integrals numerically whereas the analytical expression for those integrals for a cantilever beam are included in the TWOFLEX code. Comparisons of free response simulation results were made for the three cases listed in Table 4.2. In the first two cases the links were modeled as 1.Om long, 2.0cm diameter, aluminium rods. In case 3 the second link was 1.4m long. There is no damping in the system. Good agreement was obtained in all three cases. Figures 4.4 shows the response of the joint angle generalised coordinates,  1  and 02, for the first 0.5s of case 2. The results  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  107  Initial Conditions Case 1 2 3 4 5 6 7  Manipulator Structure Link 1 Link 2 Flexible (1 mode) Flexible — (1 mode) Rigid Rigid Flexible Flexible (1 mode) (1 mode) Flexible Rigid (1 mode) Rigid Flexible (1 mode) Flexible Flexible (1 mode) (1 mode) —  8-  62  [0]  [0]  0  —  0  62  [rad/sj 0  [rad/s]  0  —  /11  2/’21  [—j 0  [—] —  ,1’21  [1/sj 0  [1/s]  0.1  —  —  LO  —  30 0  30 0  0.1 0  0.1 0  —  —  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  1.0  1.0  0.1  0.1  Table 4.1: Configurations and initial conditions for validation of inertia matrix and right hand side vector terms.  Initial Conditions Case 1 2 3  Manipulator Structure Link 1 Link 2 Rigid Flexible (1 mode) Flexible Flexible (1 mode) (1 mode) Flexible Flexible (1 mode) (1 mode)  6  62  [0]  []  20  2  /‘11  21  -25  [rad/s] 0  [rad/s] 0  [—]  [—]  -20  -75  0  0  0.05  -0.1  0  0  35  35  0  0  -0.05  0.1  0  0  —  0  -  ‘/ii  [1/s] —  /‘21  [1/s] 0.1  Table 4.2: Configurations and initial conditions for free response simulation validation runs.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  108  -4 -6 -8 -10 -12 U)  2.  -14 -16 -18 -20 -22  0  .05  .10  .15  .20  .25  .30  .35  .40  .45  .50  .30  .35  .40  .45  .50  Time [s] (a) 81  response  -70 -75 -80 U)  -85  a) a)  -90  U) L.  V  c’J  -95 -100 -105 -110  0  .05  .10  .15  .20  .25  Time [s] (b) 82 response  Figure 4.4: Comparison of joint angle responses between TWOFLEX and Mah [89] for free response validation.  Chapter 4. Dynamics and Control Simulation  —  TWOFLEX  109  from the TWOFLEX program and those from Mah [89] are initially coincident and differ by  only a small amounts after 0.5s (5000 integration time steps). The responses of the modal amplifier generalised coordinates,  and ‘çb , for case 2 are shown in Figure 4.5 and 21  illustrate the same excellent agreement.  4.6  Summary  The TWOFLEX program is a versatile simulation of the dynamics and control of a planar, two flexible link manipulator and a deformable environment. The code is modular, ex tensively documented and includes optionally compiled code to generate debugging output from almost every subroutine. The integration of the continuous time equations of motion and the calculation of the discrete time control algorithm are conceptually and structurally separated in the code, making it a valuable tool for the investigation of both the open and closed ioop behaviour of the system.  Chapter 4. Dynamics and Control Simulation  -  -  -  .07 • 06 • 05 • 04 .03 • 02 .01 0 .01 .02 03 .04 05 06 .07 .08  —  TWQFLEX  110  .  -  -  .  -  .  -  -  0  .05  .10  .15  .20  .25  .30  .35  .40  .45  .50  .30  .35  .40  .45  .50  Time [s] (a)  b 1 1 response  TWOFLEX----j .12 .10 08 • 06 04 • 02 0  c’J -  .  —  .  -  .  -  .  02 04 06 08  -.10 -.12  0  .05  .10  .15  .20  .25  Time [s] (b)  21  response  Figure 4.5: Comparison of modal amplifier responses between TWOFLEX and Mah [89] for free response validation.  Chapter 5  Force Control Algorithm Performance and Evaluation  5.1  Introduction  This chapter presents results and discussion of the analysis of the dynamics of flexible link manipulator force control systems and the results of numerous runs of the TWOFLEX dynam ics and control simulation program. Analysis of the eigenstructure of the system reveals orderly changes in the dominant natural mode with increasing effective contact stiffness. These changes have important implications for the selection of the controller parameters. The simulation results demonstrate the capabilities and evaluate the performance of the adaptive, multivariable, long range predictive control algorithm developed in Chapter 3. The results are organised roughly in order of increasing complexity of both the manipula tor/environment system and the maneuvers which are simulated. This organisation seeks to emphasise the basic performance of the control algorithm before introducing such com plications as making and breaking contact, highly nonlinear dynamics, and actuator effects. An alternative view of the contents of this chapter is shown in Figure 5.1. The topics which will be addressed can be broadly classified as: • linear analysis of various manipulator configurations in contact with their working environments; • force control step response results for various manipulator configurations; and • the problem of maintaining control when contact is made or broken.  111  Chapter 5. Force Control Algorithm Performance and Evaluation  112  Motion I StopStrategy (5.4.1) Making and Breaking Contact Force Setpoint Modification Strategy (5.4.2)  Figure 5.1: Topics discussed in Chapter 5. Section numbers in which the discussion of each topic appears are shown in parenthesis.  Chapter 5. Force Control Algorithm Performance and Evaluation  113  Figure 5.1 indicates the sections in which each of these topics are discussed for each of the manipulator configurations considered. The chapter begins with an analysis of the eigenvalues and eigenvectors of the equations of motion for a single link/environment system. Analytical approximations to the trends observed in the numerical evaluation of the eigenvalues are developed and the implications of changes in the modal structure of the system for contact force control are discussed. The performance of the controller for force control of a single flexible link is discussed in sec tion 5.3. The calculation, from the physical parameters of a particular link and environment, of the various parameters and coefficients required by the TWOFLEX program is presented as well as the derivation of the structure and initial coefficient values of the discrete time transfer function model for the control algorithm. Simulated step and ramp response results are shown to demonstrate the performance of the controller. The effects of the controller horizons and weighting factors in shaping the closed ioop response are also discussed in the course of Section 5.3. The discontinuous nature of the manipulator/environment system dynamics is considered in section 5.4 with a discussion of the problems involved in main taining control during the making and breaking of contact. The use of contact control logic to overcome those problems is discussed along with the presentation of simulation results for two different contact control logic strategies. The performance of the multivariable form of the control algorithm is demonstrated by considering force control for a two link manip ulators in section 5.5; both rigid link and flexible link manipulators are discussed. Results of simulations of two links of a prototype model of the DC motor driven Space Station Mobile Servicing System (MSS) manipulator are presented and discussed in sections 5.6. The chapter concludes with a summary of the force control algorithm performance. Some of the work presented in sections 5.3 and 5.4 has already been published [22,27].  Chapter 5. Force Control Algorithm Performance and Evaluation  114  1 L  1  k  /; Figure 5.2: Single rigid link in contact with a deformable environment. 5.2  Linear Analysis  This section presents a linear analysis of the dynamics of a single link in contact with a deformable environment. Particular emphasis is placed on the interaction between the structural stiffness of the manipulator link and the stiffnesses of the contacting regions of the link tip and the environment. Using analytical and numerical analysis of the eigenvalues and eigenvectors of systems of linearised equations of motion it is shown that, for a given link, this interaction results in distinct changes in the modal structure of the link/environment system as the effective contact stiffness increases. The implications of these modal changes for contact force control are discussed. 5.2.1  Rigid Link  Consider first a single rigid link which is the limiting case of infinite structural stiffness. The equations of motion for the system shown in Figure 5.2 are given in section A.6. Simplifying those equations by neglecting damping, gravitational effects and the small coupling inertia  Chapter 5. Force Control Algorithm Performance and Evaluation  115  terms between the link and contact mass allows them to be linearised about the nominal state  6  =  0 and written as: 10181 =  ui(t)  mcfy =  3 —k  where the generalised coordinates  {i,  —  (E  1 (L L 3 k e 1  —  ) 1 L6  (5.1)  —  keyEy,  (5.2)  e} are now perturbations about the nominal state.  For the sake of generality the joint torque, r , has been replaced by a general control input, 1 ui(t). To study the eigenstructure of the system these equations are transformed by applying the Laplace transform and rewriting in matrix form: 1  J(s)  =  ui(s),  (5.3)  0 where =  s 0 1 2 1+k 1 L 5  1 L 3 —k  (5.4)  .  1 —kL  2 + (k ms 3 + k)  The characteristic equation of the system is given by setting the determinant of J(s) to zero: =4 3 + key) + mck (k 01 ms 0 I 1 + [I yL] 8  2  + ksykeyL  =  0.  (5.5)  The natural frequencies of the system (eigenvalues of J(s)) are given by the roots of equa tion (5.5). We will first investigate the variation of the natural frequencies with contact stiffness numerically by considering a 1.Om long, 2.0cm diameter aluminium rod treated as a rigid link. Given the density of aluminium of 2690kg/rn 3 such a link has a mass of 0.8451kg and a moment of inertia about the joint axis of 1 101 = M  2 (d)  +  = 0.2817kg m . 2  (5.6)  Chapter 5. Force Control Algorithm Performance and Evaluation  116  The contact region masses, m 3 and me, are taken as 0.19 each.  Figure 5.3 shows the  variation of the natural frequencies of the system with effective contact stiffness ranging from 50N/m to 5MN/m. The effective contact stiffness is defined as keff  (5.7)  ksy+key  The data for this plot was generated by numerical calculation of the eigenvalues of J(s) for various values of k 3 and key. The plot clearly indicates that the natural modes of the system are well separated in frequency and both frequencies increase in proportion to the square root of the contact stiffness. These observation can be confirmed analytically using equation (5.5). Substituting  (jw) for s,  2 + ksykeyL = 0, im(jw) + [Ioi(ksy + key) + mkL} (jw) 0 I 4  (5.8)  gives a quadratic equation of the form +bx+c=0, 2 ax  (5.9)  the roots of which are the squares of the natural frequencies of the system. The coefficients of equation (5.8) are such that 2 b >> 4ac and so the quadratic formula can be approximated using the binomial expansion to yield: (sw) 2  =  iF —b+ 12 b .—4ac)  —  b+b(1 -  =  4ac)]  :  /  1  1  4ac  1  2 4ac  -  —  or  (5.10)  Chapter 5. Force Control Algorithm Performance and Evaluation  11111111  I  I  1111111  I  I  1111111  117  I  11111111  I  I  1111111  iø  111114  -  •  ksy+key  1  •  1/2  2[ms+me  -  iø N >%  C-)  C  a)  -  1/2  :  ioe  10_2  I  I  iii  101  I  I  liii  0 i0  I  i  i  101  102  I  i iø  I  4 i0  Effective Contact Stiffness [kN/m] Figure &3: Variation of the natural frequencies with effective contact stiffness for a single rigid link/environment system.  Chapter 5. Force Control Algorithm Performance and Evaluation  118  Note that for the case shown in Figure 5.3, and indeed for most cases of interest, m  <<101,  therefore, ksy + key m  2  keL  and  (5.11)  101  These results confirm the correlation of the natural frequencies with the square root of the contact stiffness that is shown in Figure 5.3. Also note that the natural frequencies are inversely proportional to the square root of the link moment of inertia and the contact region mass. In what follows we consider a given contact mass and link inertia and concentrate on the effect of changing contact stiffness on the system. Based on the approximate natural frequencies given by equations (5.11) the modal eigen vectors of J(s) are approximated by 1 E  [ei  (5.12)  ] 2 e 1  (1_)L  For surfaces of comparable stiffness, such as is the case in metal on metal contact, the ratio of key to k 3 is of order 1. Combining this condition with the assumption that rn,, << we see that the first element of the e 1 eigenvector is very much smaller than the second. This result is confirmed by the numerically determined eigenvectors for the case shown in Figure 5.3 with key = k . The eigenvectors, normalised to unit magnitude, are found to be 3 independent of the effective contact stiffness:  E =  {  1 e e 2  j  —1.0640 x iO  0.8943  1.0  0.4472  .  (5.13)  The relative magnitudes of the elements of the eigenvectors indicate the contribution of each of the generalised coordinates to the natural modes, that is:  [:1  E  (5.14)  Chapter 5. Force Control Algorithm Performance and Evaluation  It is evident that the  119  mode, which corresponds to w , the higher of the two natural 1  frequencies, is composed almost entirely of the e, generalised coordinate. Physically, this composition means that the high frequency mode is the oscillation of the contact region mass, m, between the virtually stationary link tip and the environment body. The other mode is the oscillation of the link about its joint. This mode includes an e component because the contact region mass, m, moves in phase with the link tip. Because m the energy of the  2  <<101  mode is concentrated in the link motion. The ratio of the components  of this mode is determined by the contact stiffnesses. In equation (5.13) the ratio is 2 to 1 because  =  key for that case.  Control of the system is effected by the relationship of the natural modes to the contact force and the control input. The contact force exerted by the link tip on the environment is given by equation (2.57). For a single rigid link with damping effects neglected it can be linearised to:  T  !!!&k L 1 mc  =  (5.15)  .  1( —  j. m 8  —  Noting that the contact force is expressed as a linear combination of the generalised coor dinates, the matrix ET can be used as a linear transformation to obtain an expression for the contact force in terms of the natural modes: =  (ETcy)T[1]  1 L Li_ ( 1 i+)  mk,,,  1 —  .  1 + (1 L  —  ) (:::  1) L 1 —  ?72  (5.16)  Chapter 5. Force Control Algorithm Performance and Evaluation  120  For comparably stiff surfaces we see that the contact force is composed almost entirely of the  2  natural coordinate, that is, oniy the low frequency mode contributes significantly to  the contact force. The numerical values confirm this observation: —5.3201 x iO  F =  (5.17)  .  0.4472 The ET transformation can be used to show the relative excitation of the natural modes due to a step change in the control input. Noting that the control input, 1 u ( t), acts on the i 8  generalised coordinate, the relative excitation of the natural modes is given by:  ET  1 ui(t)  0  1 —1.0640 (5.18)  0.8943 This result implies that the control input excites almost exclusively the  2  coordinate.  To summarise, analysis of the eigenvalues and eigenvectors of a linearised, undamped model of an infinitely stiff link in contact with the environment under the assumptions that the contact region mass is small compared to the moment of inertia of the link (m <<I) and that the contacting surfaces are comparable (k  k, and m 3  me) reveals that:  1. the natural frequencies of the system are widely separated in frequency and, for a given link and surface, are proportional to the square root of the effective contact stiffness; 2. the high frequency mode is the vibration of the surfaces of the link tip and the envi ronment in the region of contact; 3. the contact force is almost entirely due to the low frequency mode, and;  Chapter 5. Force Control Algorithm Performance and Evaluation  121  110 Wi : 1  101 1111 1211 1311  k  T y  Figure 5.4: Single flexible link in contact with a deformable environment. 4. the control input primarily excites the low frequency mode. The implications of these results for control of the contact force, using the discrete time predictive control algorithm developed in Chapter 3, are that the high frequency mode can be ignored and the sampling rate selected to satisfy the Nyquist criterion for w . Because 2 the control input is directly coupled to the contact force output through the w 2 mode the system falls in the class of systems described by Clarke, et al [29] as “easy” to control. For such systems a prediction horizon of N 2  10 and a control horizon of N  1 are  recommended to obtain satisfactory control. As noted at the outset the rigid link case is primarily of interest for comparison as the limiting case of infinite link stiffness. 5.2.2  Flexible Link  The equations of motion for a single flexible link in contact (see Figure 5.4) are given in section A.4. Modelling the link with a single structural mode and neglecting damping,  Chapter 5. Force Control Algorithm Performance and Evaluation  122  gravity and link to contact mass inertial coupling the equations of motion, linearised about  {  ; è  i4  0, are: = ui(t)  + 111161  211 I 1 1 +b më  =  —  = I3111I’11  3 (e —k  —  1 (L L 3 k 0 +b 1 110 q 1 1  —  3110 (L k 110 q 1 1 6 +b 1  e9 L 1  —  ,bii) 1 q 10  —  (5.19)  -  (5.20)  —  (5.21)  keyE!,.  The joint torque, r (t), has been replaced with a general control input, u 1 (t), and the 1 generalised coordinated  e,} are now perturbations about the nominal state.  {i,  As in section 5.2.1 we will begin by considering the natural frequencies of the system. In preparation, the Laplace transform is appled to the linearised equations of motion and they are rewritten in matrix form: 1 J(s)  bii(s)  0  =  (s), 1 u  (5.22)  0 where 2+k Is 1 L 8 J(s)  =  1s2  2 +0 s 11 Lj Liqii s 3 k 211 + 1 2 1 L 3 —k  cb 1 L 3 k , + 11 1311  çb 5 k + 10  —k 1 4 5 10  1 L 8 —k —k 1 b 5 10  .  (5.23)  3 + key 2 + k m.s  The derivation of analytic expressions for the eigenvalues of J(s) is rather involved. More immediate insight can be gained by solving for the eigenvalues numerically and considering the data with the results of section 5.2.1 as a guide. Considering the same 1.Om long, 2.0cm diameter aluminium rod used in section 5.2.1, the physical parameters of the link model must now include quantities related to the structural characteristics of the link. The modulus of elasticity for aluminium is 7.31 x 10’°N/m 2  Chapter 5. Force Control Algorithm Performance and Evaluation  Mode Number (i) 1  123  Mode Shape Function Integrals Ifl  ‘21z  ‘31i  4’iiO  0.4807  0.8451  7096.0  2.0  Table 5.1: Mode shape function integral factor values for the first mode of a 1.Om long, 2.0cm diameter, aluminium cantilever beam. which, combined with the link dimensions, gives a flexural rigidity in transverse bending of: 1 El  =  Ed  =  574.024N m.  (5.24)  The first mode of a cantilever beam is used as the admissible mode shape function for the link structure. Assuming uniformly distributed mass and rigidity, the values of the mode shape function integral factors are shown in Table 5.1. These values are calculated, using closed form expressions for the integrals, from the physical parameters of the link in the input processing step of the TWOFLEX program. Figure 5.5 shows the natural frequencies for the flexible (one mode) link (in contact) with effective contact stiffnesses varying from 50N/m to 5MN/m. The lower of the two natural frequencies for the same link, modelled with infinite structural stiffness, is shown as a dashed line for reference. As in the case of the rigid link the highest natural frequency is very closely approximated by 2 =  3 + key k  (5.25)  The intermediate frequency, w , is independent of the contact stiffness for small values 2 and proportional to the square root for large ones. Conversely, w , the frequency of the 3 slowest mode, is indistinguishable from the rigid link frequency for small contact stiffnesses and constant for large values. The isolation of the w 1 mode, which is the vibration of the contact region, from the other two modes suggests the consideration of a simplified model of the system, in which the contact dynamics are replaced by a steady state approximation (see Figure 5.6). Neglecting the contact region mass, m, and replacing the contact region  Chapter 5. Force Control Algorithm Performance and Evaluation  I  I  11111111  I  Ilillif  I  11111111  I  11111111  I  124  I  1111111  I  111111:  iMode Flexible Link —  1  ncr  —  -  Equivalent jinJ  4  10 3  >%  0 C  kN/m  keff  0)  =  0•  0) U Cu I  1 z  102  10’  io io_2  I  10_i  I  II 11111  100  I  I  II  I’ 111111  101  I  I  IlIlill  I  1111111  2 jØ  Effective Contact Stiffness [kN/mj  Figure 5.5: Variation of natural frequencies with effective contact stiffness for a single flexible link with one structural mode (El 1 = 574N m ). Lower frequency for an equivalent rigid 2 link is shown dashed.  Chapter 5. Force Control Algorithm Performance and Evaluation  125  110 Wi (p 1  101  1111 1211 1311  k eff /  Figure 5.6: Single flexible link in contact with a deformable environment approximated by a steady state contact dynamics model. stiffnesses, k 3 and key, with the corresponding effective contact stiffness, keff, yields the following equations of motion: :loi8i  +  ui(t)  Iiiiii =  211 I 1 1 +b  =  —  8+b 1 keff L (L ), & 1 0 1 11 + ). 11 b 110 keffci1o (L  I3iii1  (5.26) (5.27)  This reduced system can be used to explain the asymptotic behaviour of the lower two natural frequencies shown in Figure 5.5. The characteristic equation of this system is (IOiI2i.  +  —  z2)  [IiI311  +  4 (w) 10 keff (IoiqS  211 I +L  —  2IiiiLiqiio)] 2 (jw) + I likeffL = 0. (5.28) 3  Note that the coefficient of (jw) 2 is dominated by the ‘oi’3n term for small effective contact stiffnesses and by keff  211 1 +L  —  ) 1 4 1 L 111 21 10 for large values of keff. With this in  mind, the binomial expansion approximation of the quadratic formula used in section 5.2.1 reveals that, for a given link, the lower two natural frequencies approach, for small values of kff —  ‘o1’311 —  —  211  T2  -‘-iii  and  = keffL 101  (5.29)  Chapter 5. Force Control Algorithm Performance and Evaluation  and for large  126  keff: 2  iq + L 0 (I 211 T keff 10  —  110 q 1 L 111 2I ) S  —  1011211  —  and  —  L 211 (Io +I  These results predict very closely of effective contact stiffness. to those given coefficient of  keff  over  by  2 (jw)  which the  The  the  in equation  values of  occurs when  (5.28)  transition occurs  2I 1 L 111 110  ) 1 L 111 2I 110  and  3 w  shown in Figure  the  frequencies given  the  magnitudes of  are approximately equal.  fall  1011311 —  2 w  —  transition from  (5.30)  equations  211 I +L  r2  ‘T  2  in  the  5.5 by  the  at  the extremes  equations  (5.29)  two terms in  Empirically,  the  the  values of  range  keff < 10i  1011311  211 I +L  —  2I 1 L 111 110  (5.31)  For the case shown in Figure 5.5 this result implies that the transition is centred at keff 40.7kN/m. The changes in the modal structure of the system as the contact stiffness increases are further illuminated by considering the eigenvectors. Unlike the rigid link case, the normalised eigenvectors of J(s) are not independent of the contact stiffness. For the purposes of control the most important effects of this dependence on contact stiffness are variations in the contribution of the natural modes to the contact force and the excitation of the natural modes by the control input. The contact force, derived from equation (2.57) by neglecting the damping and link 2 terms and linearising is:  =  [  1 mL  mello  5 (m  —  me)  ]  Ii  (5.32)  Using the matrix of eigenvectors, ET, to transform the components of the contact force ex pression from generalised coordinate space to natural coordinate space produces the results  Chapter 5. Force Control Algorithm Performance and Evaluation  .45  I  I  I  I  127  111111  I  .40 .35 .30  -  -  .25  a) a) 0  -  -  g.15 .r3O  -  I...  0E05  Zo 0  0 05  -  -.10  -  .  20 102  I  1111111  101  I  1111111  10  I  I  101  huh  102  uiiul  I  I  11111  iø  Effective Contact Stiffness [kN/m]  Figure 5.7: Variation of the natural mode components of the contact force with effective contact stiffness for a single flexible link with one structural mode. Contact force compo nents are normalised by k . 8  Chapter 5. Force Control Algorithm Performance and Evaluation  128  shown in Figure 5.7. These data are for the 1.Om long, 2.0cm aluminium link modelled with one flexible mode and were obtained by calculating the eigenvectors of J(s) and per forming the transformation on the contact force components numerically. The figure shows that at low values of contact stiffness the contact force is composed of components of the 72  and  natural modes, with  the contribution of  773  dominating. As the effective contact stiffness increases  disappears and  772  dominates. The contribution of  m  to the contact  force is negligible. In terms of control this implies that as the contact stiffness increases the contact force changes from being similar to that which would arise were the link rigid to being dominated by the structural characteristics of the link. In particular this means that the required sampling rate is governed by w 3 at low contact stiffness but by w 2 at high values. To complete the picture we consider the effect of the control input on the excitation of the natural modes. Figure 5.8 shows the components of the natural coordinates that are the result of the numerical transformation from generalised coordinate to natural coordinate space of the input vector, [1  0  T 01  from equation (5.22). In contrast to the contribution of  the natural modes to the contact force, the excitation of the  772  and  773  modes by the control  input remains relatively constant over the range of contact stiffnesses, with most excitation. The  771  773  receiving the  mode undergoes negligible excitation. For contact force control  this result means that, without making any contribution the the controlled variable at high contact stiffness, the  773  mode remains important as the primary route of input of control  energy into the system. All of the results presented above are for a specific link structural stiffness. Reducing the flexural rigidity of the link from 574.024N m 2 to 40.ON m 2 produces the results shown in Figure 5.9. The pattern of the variation of natural frequencies with contact stiffness is similar but it is shifted to the right and downward. That is, the horizontal asymptotes of w 2 3 dominance to w 2 dominance and w 3 occur at lower frequencies and the transition from w  Chapter 5. Force Control Algorithm Performance and Evaluation  1.0  i  I  I  1111111  I  11111111  I  huuH1  I  uhuHIl  129  I  IIIIII  I  I  IIIIiii  I  hII  I  I  11111.,  113  .8  .7  .6 0 0  ox  W .5  ::  .4  .3  .2  -  -  -  11  0 2 1W  101  101  I  102  i&  I  I  4 i0  Effective Contact Stiffness [kN/m]  Figure 5.8: Variation with effective contact stiffness of the relative excitation of the natural modes of a single flexible link with one structural mode in contact.  Chapter 5. Force Control Algorithm Performance and Evaluation  1  130  5 I  I  I  1111111  I  IIIIIIj  I  I  1111111  I  I  1111111  I  I  11111!  1 Mode Flexible Link Equivalent Rigid Link 1  4  •i::r i’  -  >‘  C) C  a) C ci) I-. LL (1 D (I,  z  k f 9 f  =  2.8 kN/m  1  101  1  ø  11111111  10_2  10_i  11111111  0 i0  i 11111111  101  111111111  102  iø  Effective Contact Stiffness [kN/m]  Figure 5.9: Variation with effective contact stiffness of system natural frequencies for a soft link (El 1 = 40N m ). Lower frequency for an equivalent rigid link is shown dashed. 2 .  Chapter 5. Force Control Algorithm Performance and Evaluation  occurs at lower effective contact stiffness (keff  131  2.8kN/m).  The modelling of the link structure using a single admissible mode shape function con veniently permits analytical approximation of the asymptotic behaviour of the natural fre quencies. However, additional modes may be required in the model, depending on the degree of accuracy with which the continuum dynamics of the link structure must be represented. Figure 5.10 shows the natural frequencies varying with effective contact stiffness for a link with its structural characteristics modelled by the first four modes of a cantilever beam with a flexural rigidity of El 1 = 200N m. As in the previous cases the highest frequency mode, associated with the vibration of the surfaces in the contact region, is proportional to the square root of keff and well separated from the other modes. For iow contact stiffnesses the lowest frequency mode is proportional to the square root of keff and the system is domi nated by rigid link-like behaviour. With increasing keff the slowest mode becomes constant and each of the higher frequency modes successively passes through a transition from one constant frequency to a higher one. During the transitions each of the modal frequencies becomes proportional to k!ff and dominates the system. This progression of dominate modes is clearly shown in Figure 5.11 in which the variation with effective contact stiffness of the natural mode components of the contact force are plotted. While the modal composition of the contact force changes markedly with increasing keff the excitation of the natural modes by the control input remains roughly constant as shown in Figure 5.12. In terms of contact force control the results of this linear analysis imply that, at a given effective contact stiffness, a flexible link/environment system in which the link structure is modelled by several structural modes is analogous to a system with one structural mode for the purposes of selecting an appropriate controller sampling interval because the contact force is dominated by one of the modes. On the other hand, since the control input supplies energy to all but the highest frequency mode, the controller model and parameters must be specified with due attention to the true effective order of the system.  Chapter 5. Force Control Algorithm Performance and Evaluation  1  ô I  L —  1  1  132  IIIIII  I  I  1111111  I  11111111  I  11111111  I  1111111  i  itiiiil  I  I  1 Mode Flexible Link —  —  Equivalent Rigid Link  4  z >. C) C 0 D 0  U D  (‘3  z 1  101  1  ø  iw  I  10=1  .::  10  101  102  iø  Effective Contact Stiffness [kN/m]  Figure 5.10: Variation with effective contact stiffness of system natural frequencies for a flexible link with four structural modes (El 1 = 200N ma). Lowest frequency for an equivalent rigid link is shown dashed. .  Chapter 5. Force Control Algorithm Performance and Evaluation  133  40 35 30  .25 cl) -D 0 LL  o  .20  ct  —o co  15  ci)—  10  ,-  OE  zo  05  C)  0  -  .05  -.10  -.15  -  .20 1  10_I  101 102 i0 0 Effective Contact Stiffness [kN/m]  3 i0  1  4  Figure 5.11: Variation of the natural mode components of the contact force with effective contact stiffness for a single flexible link (El 1 = 200N m ) with four structural modes. 2 Contact force components are normalised by k . 3 .  Chapter 5. Force Control Algorithm Performance and Evaluation  1.0  .8  I  11111111  I  I  I  1111111  I  I  1111111  I  IIIIII  I  134  I  1111111  I  I  111111  14  -  .7  w 0 o ox  .6  •.  h0)  a) 4  .3  .2  I  0  1Ø_2  liii.  I  I  1111111  I  I  Ijilill  IIInI  10_i  Effective Contact Stiffness [kN/mj  Figure 5.12: Variation with effective contact stiffness of the relative excitation of the natural modes of a single flexible link (El 1 = 200N rn ) with four structural modes. 2 .  Chapter 5. Force Control Algorithm Performance and Evaluation  5.3  135  Force Control for a Single Flexible Link  The first demonstration of the performance of the adaptive, long range predictive force control algorithm is for the case of a single flexible link in contact with a deformable en vironment surface. The physical parameters of the particular system that was simulated are given in the following sections along with the calculation from those parameters of the coefficient and parameter values used in the TWOFLEX program. 5.3.1  Link Model Parameters  The link is modelled as an aluminium rod 2.0cm in diameter and 1.Om long. From these dimensions the volume of the link is: 1 V  =  1 dL  =  3.1415 x 10 . 3 m 4  (5.33)  Given the density of aluminium of 2690kg/rn , the mass per unit length (p1) of the link is 3 0.8451kg/rn. The moment of inertia about the joint axis is: =  1 M  2 (d)  +  =  0.2817kg m . 2  (5.34)  Finally, the modulus of elasticity for aluminium is 7.31 x 10 2 which, combined with N/m the link dimensions, gives a flexural rigidity in transverse bending of: 1 El  =  Ed  =  574.024N m . 2  (5.35)  The gravitational field is assumed to be perpendicular to the plane of the link motion and therefore has no effect. The structural dynamics of the link are modelled by using the mode shapes of a cantilever beam as admissible functions. Based on the uniformly distributed mass and rigidity values calculated above the natural frequencies of the beam and the values of the mode shape  Chapter 5. Force Control Algorithm Performance and Evaluation  Mode Number (i) 1 2 3 4 5  Natural Frequency (Hz)  ) 2 (Ivg.m  ) 2 (kg.m  14.6  0.4807  91.4  7.6707 x 10—2  255.9 501.5 829.0  2.7395 x 10—2 1.3980 x 10—2 8.4569 x i0  0.8451 0.8451 0.8451 0.8451 0.8451  136  Mode_Shape Function Integrals ‘31i  ) 2 (N.m 7096 2.7870 x 2.1850 x 8.3907 x 2.2929 x  (m) 2.0  1O  -2.0  106 106  2.0 -2.0 2.0  io  Table 5.2: Natural frequencies and mode shape function integral factor values for the first five modes of a 1.Om long, 2.0cm diameter, aluminium cantilever beam function integral factors for the equations of motion for the first five modes are listed in Table 5.2. All of these data were calculated from the physical parameters of the link in the input processing step of the TWOFLEX program. 5.3.2  Controller Model  While the dynamic simulation of a single flexible link in contact is based on the equations of motion (A.11) through (A.13), the force control algorithm requires a linear model in the form of a discrete time domain transfer function. The equations of motion can be linearised about a nominal zero state, transformed into algebraic equations using the Laplace operator and written in matrix form, as demonstrated in section 5.2.2, to obtain a matrix equation of the form: 1 0 J(s)  (S) 12 ?,b  0 (5.36) 0  eu(s)  0  Chapter 5. Force Control Algorithm Performance and Evaluation  137  Likewise, the contact force equation can be linearised and expressed in the form: 9i(s)  1 i(s) L’ F=c  i2(3)  (5.37)  /‘in (s)  e,(s) These expressions are combined to yield the continuous time transfer function for the system: JH  F(s) Ui(S)  -  1 cT J(•  —  (.5 38 )  (—1)’+ J 1 where  is the ij minor of the n  x  n matrix J(s).  The discrete time transfer function which constitutes the controller model is obtained from equation (5.38) by substitution of numerical values and transformation to the discrete time domain using the selected sampling rate and assuming zero-order hold sampling. The specific controller models for various cases are given in the following sections. 5.3.3  Soft Contact Case (keff = 3.lkN/m)  We begin by considering the performance of the control algorithm in the case of relatively soft  3 (k  ke = 6200N/m) contacting surfaces on the link tip and the environment. The  contact region masses on each surface are taken to be m 5 = me = 1.0g. The link structure is modelled using the first two cantilever beam mode shapes as admissible functions. Portions of this work have already been published [22,27].  Chapter 5. Force Control Algorithm Performance and Evaluation  138  Controller Model m = me = 1.0g, 1 ke = 6200N/m, 5 n = 2 and the values given in Table 5.2  With k  equation (5.38) yields the continuous time transfer function  Fr(s) u(s)  0.1944s6  —  + 1.0486 x 106s4 9.6132 x lOis2 + 7.6021 x l0 1.8731 x 10_6s8 + 18.933s6 + 4.6472 x 1Os + 8.0802 x 1012s2 + 7.6021 x 1016 (5.39) —  which has open loop natural frequencies of 15.81Hz, 66.29Hz, 303.5Hz and 409.0Hz. Fig ure 5.13 shows the plot of natural frequencies versus effective contact stiffness for this system with keff  3100N/m marked. The relationships among the modal natural frequen  cies shown in Figure 5.13 are more complicated than those seen in the similar figures in section 5.2. This increased complexity is due to the fact that the contact region mass of 2.Og is an appreciable fraction of the link moment of inertia (0.2817kg in ). With this in mind, 2 .  we still expect the system dynamics to be dominated by the 15.81Hz and 66.29Hz modes and use a sampling interval of 4ms which satisfies the Nyquist criterion for the 66.29Hz mode. The two higher frequency modes are neglected because keff is such that they are in the range in which the 303.5Hz mode frequency is independent of keff and the 409.0Hz mode frequency is proportional to kff; this would not be the case if the effective contact stiffness were significantly lower or the contact region mass appreciably higher. While the choice of 250Hz (h = 4ms) sampling implies that the two highest frequency modes will be aliased, the controller performs relatively well owing to the relatively low excitation of these modes. With a sampling interval of 4ms and zero order hold sampling equation (5.39) is trans formed to give the discrete time transfer function controller model: F(q’) u(q) —14.7 2.38q + 12.8q 2 + 5.36(q 3 +q 5 2.38q 7 6 14.7q ) + 12.8q 4 1 0.3lq 1 + O.8 6q 0.059q 4 0. 2 3 q 4 4 O. 6 0.31q q 4 6q + 0.8 4 7 + q_8 —  —  —  —  —  —  —  —  Chapter 5. Force Control Algorithm Performance and Evaluation  iø  I  .  I  1111111  I  1111111  I  1111111  1111111  I  139  I  IIIIII  iø  kif  3 1 kN/m  iø >. C.) C  a,D  D  .10 2  z  101  iOø  102  I  I  iiiiiI 101  I  I  iiiiiil  I  iIiiIiiII  101  I  I  iiiiiil  IIIIiiI  102  3 i0  4 i0  Effective Contact Stiffness [kN/m]  Variation of natural frequencies of a single flexible link (El 1 structural modes) in contact. Figure 5.13:  574N m, two  Chapter 5. Force Control Algorithm Performance and Evaluation  140  30 25  ‘20 a) C.) 0  15 olO 5 0 Time [s] Figure 5.14: Contact force response of a single flexible link on a soft surface 8 = ke = 6200N/m). (k (5.40) Step Response Figure 5.14 shows a series of ramp and step responses from the closed loop system. The link is initially stationary and just touching the environment surface, such that the contact force is zero. The parameters for the predictive control algorithm are a delay horizon (N ) 1 of 1 time step, a prediction horizon (N ) of 20 time steps, a control horizon (Nu) of 12 time 2 steps, a control increment weighting factor (Ar) of 1.0 and a static equilibrium bias term weighting factor  se)  of 0. The controller time step is 4.Oms. The controller is adaptive,  with the model coefficients being updated at every time step.  Chapter 5. Force Control Algorithm Performance and Evaluation  141  In Figure 5.14 the desired contact force setpoint is represented by the dashed line and the contact force response of the system calculated by the simulation is the solid line. Initially, the setpoint is ramped at 50N/s and the response is seen to track very well, with the exception of some minor perturbations during the first lOOms which are caused by the system identification algorithm making fairly rapid changes in the model coefficients. Following the ramp is a series of steps and dwells of various magnitudes and durations. The calculated force response to these steps is very quick however, there is generally some overshoot and oscillation about the new setpoint level. The magnitude of the overshoots and oscillations decreases with subsequent steps as the system model is refined by the identification algorithm. The inherent integral action of the control law ensures that there is no steady state error. The overall controller performance illustrated by Figure 5.14 and the other results presented in this chapter is superior to that demonstrated by the other control methods (see section 3.1) which were investigated. One characteristic of the predictive controller that must be noted is its inherent tendency to anticipate changes in the setpoint level. This is a result of the control law being based on the minimisation of not only the current force error but also all of the predicted future errors up to the prediction horizon. This characteristic is most clearly observed in the deviation from the setpoint which appears at approximately 1.30s, after a period of quiescence. Also, during the lOOms dwell at iON which begins at O.85s the almost immediate anticipation of the next setpoint change by the controller is, at least in part, responsible for the system never coming to rest at iON. 5.34  Hertz Contact Model Case (keff  5MN/m)  Next we consider very much stiffer contact regions on the link tip and the environment. The parameters of the contact dynamics model are derived from the Hertz contact model  described in section 2.5. The link structural dynamics are modelled by a single cantilever  Chapter 5. Force Control Algorithm Performance and Evaluation  Radius of Curvature [rn] Elastic Modulus [N/rn ] 2 Poisson’s Ratio Density [kg/rn ] 3 Eff. Planar Strain Modulus [N/rn ] 2 Eff. Radius of Curvature [m] Total Contact Deflection [m] Eff. Contact Stiffness [N/rn] Contact Radius [m] Surface Deflection [m] Linear Contact Stiffness [N/rn] Estimated Contact Mass [kg]  142  Steel Aluminium Bearing Plate 0.002 1000 2.1 x 1011 7.1 x 1010 0.300 0.334 7849 2690 1010 7.9145 x 0.002 1.9986 x 10_6 5.0037 x 106 6.3222 x iO 5.1405 x iO 1.4845 x 106 1.9453 x i0 6.7365 x 106 1.3031 x 10_il 3.7235 x lO  Table 5.3: Material properties and Hertz model parameters for steel on aluminium contact. beam mode. Contact Model Parameters  The contact between the link tip and the environment is modelled as a 4mm diameter steel bearing attached to the link tip and making contact with a flat aluminium plate. The physical properties of the contacting materials are summarised at the top of Table 5.3. From these data the effective planar strain modulus and radius of curvature are calculated. Assuming a nominal contact force level of iON, equation (2.59) gives a total deflection of the contacting surfaces of 1.9986 x 6 10 m . The deflections of the individual surfaces are inversely proportional to their planar strain moduli. A tangent to the force deflection curve given by equation (2.59) at iON yields estimates of the contact stiffnesses of the surfaces, modelled as linear springs. Finally, equations (2.61) and (2.62) give the radius of the contact region and estimates of the mass of material on each surface moved during contact. Tile results of these calculations for the case of a steel bearing on an flat aluminium surface are listed in the lower portion of Table 5.3. These calculated contact mass and stiffness values  Chapter 5. Force Control Algorithm Performance and Evaluation  are used for the variables m , me, k 3 , and 3  143  in the equations of motion.  Controller Model Using the contact model parameter values calculated above and those for the link dynamics with one structural mode calculated in section 5.3.1, equation (5.38) yields a model of the open loop system with natural frequencies of 60.42Hz, 945.7Hz, and 114.9MHz. The latter frequency corresponds to the mode dominated by the vibration of the contact region mass between the stationary environment surface and link tip. In contrast to the situation in section 5.3.3 the contact region mass is very small compared to the link moment of inertia. Though no experimental data was sought, the analytical results developed in section 5.2.2 suggest that, under the condition of large contact stiffness and small contact mass (compared to the link moment of inertia) the contact region mass vibration mode is neither significantly excited by the control input nor does it contribute appreciably to the contact force and can thus be ignored in the controller model. Hence, by introducing the effective contact stiffness, keff and dropping the y direction contact equation we arrive at the linearised equations of motion with a steady state approximation of the contact dynamics given by equations (5.26) and (5.27). Under these assumptions the contact force model given by equation (2.57) reduces to: F = c c  =  kff  ,  {  1 L  where (5.41)  and continuous time domain transfer function relating the control input, u(s), to the contact force, Fr(s), is: F(s) u(s)  —  —  1  J(5)I —  Chapter 5. Force Control Algorithm Performance and Evaluation  1 L 211 keff (1  —  Ii&i)  52  144  + keffI3llL1  5 42  where (1011211  +  —  [1011311  I2) 4  + keff  L 211 +I  —  )} 1 L 111 2I 110  2  (5.43)  +keffI l 3 lL.  Substituting numerical values into this expression gives: Fr(s) u(s)  —  —  —5.8193 x 10 2 + 3.5506 X 10 s 5 6.9922 x 10s + 2.4768 x 10 2 + 3.5506 x 1010’ s 5  (  .  )  Figure 5.15 shows the variation of the natural frequencies with effective contact stiffness for the system described by equation (5.44). At keff = 5.0037MN/m the natural frequencies are 60.42Hz and 945.7Hz and the latter frequency is proportional to kff. A sampling frequency of 2857Hz (h  0.35ms) satisfies the Nyquist criterion for the  945.7Hz mode and zero order hold sampling yields the discrete time transfer function: F(q’) u(q’)  —  —  —2.294 + 2.303 (q + q 3 ) 2.294q 2 —1 q4 1 4 2 3 .0 2 3 q’ + 2.105q .0 2 3 4 q + —  —  (5 45)  —  Equation (5.45) provides the initial coefficient values for the force controller model. The co efficient values are updated at each time step by the EFRA system identification algorithm. The coefficients are also used to tune the controller by determining the closed ioop charac teristics of the system for various controller horizons and weighting factors, as described in section 3.2.1.  Step Response The 114.9MHz mode in the dynamics mode of the system indicates that the equations of motion are very stiff. As a result the integration algorithm time step must be very small  Chapter 5. Force Control Algorithm Performance and Evaluation  145  k I  I  =5.0037MN/m  off  II  ff=61.55N/rn:  >‘  0  a)D  c,. U (0 L.. D  z  10  I  I  iiiiiil  I  I  iiiiiil  I  I  111111  I  I  I  1111111  I  I  I  IIIII  —  102  101  j2  iø  Effective Contact Stiffness [kN/m]  Figure 5.15: Variation of natural frequencies of a single flexible link (with one structural mode) in contact.  Chapter 5. Force Control Algorithm Performance and Evaluation  2 N  N  18 20 19 21  1 2 3 4  Overdamped Mode Dominant Pole -45.21 -96.76 -84.38 -99.13  146  Underdamped Mode fd [Hz] C 857.5 0.304 1429 0.359 1429 0.562 1429 0.239  Table 5.4: Stabilising controller horizons and closed loop characteristics for steel on alu minium contact (keff = 5.0037MN/m) for a single flexible link with one structural mode. fd = damped natural frequency and (= damping ratio. and long run times are expected. Figure 5.16 shows the initial 12.5ms of a step response which required 50.3 hours of CPU time to calculate on a Sun SparcStation 2. Also shown is the response calculated using a steady state approximation of the contact dynamics which required only 23.2s of CPU time. The results are almost indistinguishable and hence it is concluded that, under conditions when the contact dynamics mode is well separated from the other modes of the system, the contact dynamics can be approximated by their steady state response with substantial savings in computation time and negligible difference in the computed results. Using equation (5.45) and the expression for the closed loop transfer function derived in section 3.2.1, four combinations of prediction horizon (N ) and control horizon (Ne) 2 which yield stable control were identified. They are listed, with corresponding approximate continuous time domain closed loop characteristics, in Table 5.4. In all four cases the closed loop is characterised by one overdamped mode (a pair of poles on the negative real axis) and one underdamped mode (a complex conjugate pair of poles in the left half plane). The natural frequency and damping ratio of the underdamped mode are such that its oscillation disappear long before the response, dominated by the smaller magnitude pole of the overdamped mode, reaches the setpoint. Figure 5.17 shows the step response of the adaptive predictive force controller with N 2 = 20, N,, = 2, ), = 0, and .Xse = 0. The response shown starts from a stable 5N contact force level in order to focus on the step  Chapter 5. Force Control Algorithm Performance and Evaluation  I  I  I  3.2 3.0  I  I  .006  I .007  I  147  Steady State Approximation  -  FuliContact  2.8  2.6:I 2.4  -  2.2  -  2.0  -  ci) C.) C  -  -  1.6  8  -  1.4  -  1.2  -  1.0  -  .8  -  .4  -  .2  -  ‘  0 0  .001  .002  I .003  I .004  I .005  I  .008  .009  .010  .011  .012  .013  Time [SI Figure 5.16: Comparison of results from simulations using full contact dynamics and steady state approximation.  Chapter 5. Force Control Algorithm Performance and Evaluation  148  22  20  18  16  14  z 12  10 0  0  8  6  4  2  0 0  .02  .04  .06  .08  .10  .12  .14  .16 Time  .18  .20  .22  .24  .26  .28  .30  .32  [s]  Figure 5.17: Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (ken = 5MN/m) with 2 N = 20, N = 2, ) = 0, and ‘se 0.  Chapter 5. Force Control Algorithm Performance and Evaluation  149  response performance of the controller and avoid the complications due to the discontinuity in the system dynamics which occurs at ON as contact is initiated. Control through the discontinuity is discussed in section 5.4. The N 2  =  20 and N  =  2 combination of horizons offers the best compromise of rise time  and control signal activity. The rise time is related to the magnitude of the dominate closed loop pole. The closer that pole is to the origin (in the continuous time domain), the longer the rise time. The rise times achieved with N with N  =  2 and, while the rise time with N  =  =  1 and N  3 are both longer than that  4 is less than with N  2, the control input  signal becomes excessively active and subject to ringing. In all cases prediction horizons less than those in Table 5.4 yield an unstable closed ioop and increasing N 2 causes the slow mode to become slightly underdamped, introducing overshoot into the response. Increasing the control increment weighting factor,  ,  the response because the closed ioop poles introduced by  from zero has negligible effect on are generally close to the origin  in the discrete time domain or they interact with the poles of the fast mode; in either case the slow mode remains dominant. Figure 5.18 shows superimposed step response simulations for 0.4. As  .kse  ‘\se  ranging from 0 to  is increased the rise time decreases but at the expense of increasing overshoot,  which may be undesirable. It is evident from these observations that the static equilibrium bias term in the control law acts as a proportional feedforward term in which the gain is proportional to  se  The inherent tendency of the predictive controller to anticipate changes in the setpoint level is apparent in the step responses shown.  With a 20 step prediction horizon and  O.35ms sampling interval the controller begins to respond to setpoint changes 7ms before they occur. The initial sign of the response to a step change in setpoint is opposite to that of the step due to the nonminimum phase nature of the system dynamics.  Chapter 5. Force Control Algorithm Performance and Evaluation  150  18  16  14  12  z C) 0 U  10  0 0  8  C)  6  4  2  0 0  .01  .02  .03  .04  .05  .06  .07  .08  .09  Time [s] Figure 5.18: Force control step responses for a single flexible link (with one structural mode), steel on aluminium contact (keff = 5MN/m) with 0 e 0.4. 3 A  Chapter 5. Force Control Algorithm Performance and Evaluation  5.3.5  ke  =  151  62kN/m Contact Stiffness Case  The sampling rate of nearly 3kHz required to obtain the results shown in section 5.3.4 means that implementation of the controller would require very high performance hardware and actuators. The high sampling rate is required because the effective contact stiffness of 5MN/m is very much greater than the 7096N/m modal stiffness of the link and thus the dynamics are dominated by the high frequency mode whose natural frequency is pro portional to k!ff as can be seen in Figure 5.15. Inclusion of additional structural modes in the dynamics and controller models results in a dominant mode with even higher natural frequency and thus higher sampling requirements. However, it must be recalled that the Hertz contact model used to determine the contact stiffnesses and masses is based strictly on the local deformations of the surface during quasi-static contact. An, et al. [141 used a parameter estimation algorithm to determine experimentally the effective contact stiffness of a steel bearing on an aluminium surface and found it to be 61554N/m. Unfortunately sensor limitations prevented the determination of effective mass and damping coefficients from their experimental data. It can be surmised that, while the local effective contact stiffness of a pair of surfaces may approach that given by the Hertz model, more global deformation of the bodies results in much lower actual effective contact stiffnesses. This proposition is somewhat supported by the presence of an initial spike in the data collected by An, et al. (reproduced as Figure 5.19) which suggests that the contact stiffness upon initial contact is much greater than that which prevails once contact is well established. Controller Model  To obtain an effective contact stiffness of 61554N/m the values in Table 5.3 were scaled to maintain relative stiffnesses of the steel and aluminium surfaces with the result that the steel surface stiffness becomes k 3  2.3930  ><  N/m and that of the aluminium k 5 10  =  Chapter 5. Force Control Algorithm Performance and Evaluation  152  IC 80000  70000 60000  soooo  Final Value: 61554 N/rn  40000 30000 20000 10000 0 0.0  0.2 0.4 0.6 0.8 Estimated Stiffness [or Aluminum Surface (N/rn)  1.0  Figure 5.19: Experimental contact stiffness estimate for steel on aluminium. Reproduced from An, et al. [14] 8.2870 x 4 10 N /m. As can be seen in Figure 5.15, these values imply that, in contrast to the situation at higher effective contact stiffness, the system characteristics are composed of contributions from both modes. The natural frequencies of the open ioop modes are 50.65Hz and 125.0Hz, thus sampling at 384.6Hz (h  2.6ms) satisfies the Nyquist criterion. The  resulting discrete time transfer function model for the controller is: F(q ) 1 ) 1 u(q  —  —2.144 + 3.084 (q 3 1 +q ) 2.144q 2 —1 1 + 0.7709q 0.4456q 2 3 456q + q_4 4 0. —  1  —  (5 46)  —  Step Response  The approximate closed ioop poles, calculated from the control law and the linearised system dynamics, show that for N,.  =  1 and N,.  =  2 all values for the prediction horizon result  in both of the closed ioop modes being uriderdamped. With a 3 step control horizon the approximate closed ioop poles show that for certain prediction horizons the closed ioop system has one overdamped mode and one underdamped mode, otherwise both modes are underdamped. With N,.  =  4 there is always one overdamped and one underdamped closed  loop mode. Table 5.5 lists the approximate closed ioop characteristics of the system for several combinations of prediction and control horizon.  Chapter 5. Force Control Algorithm Performance and Evaluation  2 N  N  5 7 8 10 5 6 7 8 9 10  3 3 3 3 4 4 4 4 4 4  Overdamped Mode Dominant Pole -181.3 -99.07 -58.56 -190.1 -183.7 -171.4 -79.75 -44.51 -80.90 -75.68  153  Underdamped Mode fd [Hz] C 192.3 0.200 192.3 0.321 192.3 0.342 192.3 0.411 192.3 0.142 192.3 0.166 192.3 0.289 192.3 0.301 192.3 0.312 192.3 0.314  Table 5.5: Stabilising controller horizons and closed ioop characteristics for steel on alu minium contact (keff = 61.554kN/m) with a single flexible link with one structural mode. damped natural frequency and C = damping ratio. fd The dominant pole locations and damping ratios listed in Table 5.5 suggest that the 2 N  10, N  =  3 controller will provide the fastest rise time combined with the least 192.3Hz  disturbance. As Figure 5.20 shows, this is indeed the case, however this performance comes at the expense of the controller adjusting the system well in advance (26ms) of setpoint changes because of the 10 step prediction horizon. This potentially undersirable anticipation effect is markedly reduced with N 2  5 and N,  =  4 without much increase in rise time but  the response exhibits slight overshoots due to the low damping of the 192.3Hz mode (see Figure 5.21). The next best set of controller horizons is N 2  =  5, Ne,,  =  3 which produces the  response shown in Figure 5.22. The rise time of this response is slightly greater than that of the two previous ones but the 192.3Hz mode is sufficiently well damped to avoid overshoot and the setpoint change anticipation time is limited to l3ms. These results emphasise the need for simulations in the tuning of the controller due to the approximate nature of the closed ioop characteristics calculated based on linearised models, and to the fact that those characteristics reveal nothing about the relative excitation levels of the closed loop modes.  Chapter 5. Force Control Algorithm Performance and Evaluation  154  20  18  16  14  z  12 C) 0 U  10 0 C)  8  6  4  2 0  .02  .04  .06  .08  .10  .12  .14  .16  .18  .20  .22  .24  .26  .28  .30  .32  Time [s]  Figure 5.20: Force control step response for a single mode flexible link (with one structural mode), steel on aluminium contact (keff = 61.554kN/m) with N 2 10, N = 3, = 0, ‘se =  Chapter 5. Force Control Algorithm Performance and Evaluation  155  22  20  18  16  z 0 0 IL  14  12  C) C  0  10  8  6  4  2 0  .02  .04  .06  .08  .10  .12  .14  .16 .18 Time [s]  .20  .22  .24  .26  .28  .30  .32  Figure 5.21: Force control step response for a single mode flexible link (with one structural mode), steel on aluminium contact (keff 2 61.554kN/m) with N 5, N = 4, = 0, ‘se 0.  Chapter 5. Force Control Algorithm Performance and Evaluation  156  20  18  16  14  z 12 C.)  0 U C.) Cu  0  10  0  8  6  4  2 0  .02  .04  .06  .08  .10  .12  .14  .16  .18  .20  .22  .24  .26  .28  .30  .32  Time [s]  Figure 5.22: Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (keff = 61.554kN/m) with N 2 5, N = 3, \ = 0, ‘\se 0.  Chapter 5. Force Control Algorithm Performance and Evaluation  157  Table 5.5 indicates a potential difficulty with the 3 step control horizon in that the closed ioop system does not always have an overdamped mode. This implies a possible lack of robustness in that the effects of unmodelled dynamics or sensor noise may cause the estimated controller model coefficients to change such that controller horizons which initially provided an overdamped closed loop mode would no longer do so. For this reason the 4 step control horizon is preferable. Figure 5.23 shows step responses for the controller with N 2 = 7 and N = 4. The responses are free of overshoot and steady state offset with a rise time of 24ms. It can be seen from Table 5.5 that the closed ioop characteristics do not vary monotonically with the controller horizons. This is due to the fact that the horizons are integer multiples of the sampling interval, h, and this results in an aliasing effect. We now consider the effect of additional structural modes in the link model. Figure 5.24 shows the natural frequencies of the linearised equations of motion for the single link system with the link structure modelled by the first three mode shapes of a cantilever beam. At keff = 61.554kN/m the open ioop natural frequencies are 37.49Hz, 113.5Hz, 231.5Hz and 664.9Hz. It is evident from Figure 5.24 that the frequencies of the latter two open loop  modes which arise due to the additional admissible functions are independent of keff Under these circumstances the results of section 5.2.2 imply that the additional modes make minimal contribution to the contact force and thus the sampling rate need not be increased to resolve them. On the other hand, the control input channels energy into all of the modes and so they should be included in the controller model. The result of theses two conclusions is the controller model F,(q ) 1 ui(q’) —0.41 1.43q’ + 2.56q 2 + 3.08(q 3+q 5 1.43q 6 0.41q ) + 2.56q 4 7 1 + 0.79q’ + l.50q 2 + 0.21q 3 + O.6 q+5 4 1q + 1.50q 2 O. 6 + 0.79q 7 + q_8 —  —  —  —  —  (5.47)  Chapter 5. Force Control Algorithm Performance and Evaluation  158  20  18  16  14  z  12  ci,  F: 6  4  2  0 0  .02  .04  .06  .08  .10  .12  .14  .16  .18  .20  .22  .24  .26  .28  .30  .32  Time [s]  Figure 5.23: Force control step response for a single flexible link (with one structural mode), steel on aluminium contact (keff = 61.554kN/m) with N , = 4, A, = 1 2 = 7, N = 0  Chapter 5. Force Control Algorithm Performance and Evaluation  108  I  11111111  I  I  I  I  I  I liii  I  k  I  I  I I 1111  =  61 554 kN/m  I  159  I  I  I  I  I  1——I-—I—1—i•  1  -icr  iø  >‘  () C  a,  z w  iø  U (15 I.  D  z  1  -+-------1  1  1  ø  I  iø  iiiiiil  iø  I  I  1111111  101  I  I  iiiliiil 102  I  iiiiiil  1  111111  iø  Effective Contact Stiffness [kN/m]  Figure 5.24: Variation of the natural frequencies of a single flexible link (El 1 with three structural modes) in contact.  =  574N m  Chapter 5. Force Control Algorithm Performance and Evaluation  N 2 6 7 8 9 10  N 4 4 4 4 4  Overdamped Mode Dominant Pole -217.5 -157.8 -105.8 -81.56 -53.99  192.3Hz Mode  C 0.517 0.584 0.624 0.622 0.638  160  Underdamped Modes fd [Hz] C f [Hz] C 105.5 0 192.3 0.307 105.3 149.4 0 0.201 105.3 145.0 0.122 0 105.3 0 151.5 0.140 105.3 143.5 0 0.125  Table 5.6: Stabilising controller horizons and closed ioop characteristics for steel on alu minium contact (keff = 61.554kN/m) with a single flexible link with three structural modes. fd = damped natural frequency and C = damping ratio. in which the two high frequency modes are aliased to 153.1Hz and 104.3Hz due to the relatively low sampling rate. The aliasing of these modes presents little difficulty because of their minimal excitation. Table 5.6 lists the approximate closed loop characteristics calculated from the linearised model of the system for several prediction horizon values with a control horizon of N = 4. As in the previous cases, the closed loop response is composed of an overdamped mode, the smallest magnitude pole of which defines the rise time, and an underdamped mode with a natural frequency of one half the sampling frequency. In addition, the aliased high frequency modes appear as lightly damped modes in the closed loop. Simulated step responses show that, as was observed in the case of the one mode link, the fast rise times produced by short prediction horizons (N 2 = 6, 7) are accompanied by overshoot because the underdamped modes do not die out sufficiently before the response reaches the set point level. Figure 5.25 shows the response with N 2 5.3.6  8 which is free of overshoot.  Summary  In summary, this section has illustrated the calculation of the parameters of the link, contact and controller models based on the physical dimensions and material properties of particular physical systems. The link model was based on a 1.Om long, 2.0cm diameter aluminium  Chapter 5. Force Control Algorithm Performance and Evaluation  161  22  28  18  16  14  z 12  .10 0  C.) 8  6  4  2  8 0  .02  .04  .06  .08  .10  .12  .14  .16 Time  .18  .20  .22  .24  .26  .28  .30  .32  [SI  Figure 5.25: Force control step response for single flexible link (with three structural modes), steel on aluminium contact (keff 61.554kN/m), with N 2 8, N = 4, = 0, = 0. .  Chapter 5. Force Control Algorithm Performance and Evaluation  162  rod with its structural dynamics modelled using the mode shape functions of a cantilever beam. For the case of relatively soft (kff = 3100N/m) contacting surfaces, with excitation of the contact dynamics due to the contact mass being significant compared to the link moment of inertia, simulation results demonstrate stable control with fast rise time and minor overshoot. Contact model parameters calculated from the Hertz contact model (derived in sec tion 2.5), based on a 4.0mm diameter steel bearing as an end effector on the link contacting a planar aluminium surface, resulted in very stiff contacting surfaces (keff  5.0037MN/m).  The calculation of the closed ioop response of this system was found to be very time consum ing due to the mathematically stiff structure of the equations of motion, however a steady state approximation of the contact dynamics portion of the model was shown to substan tially reduce the computation time without significantly affecting the results. Approximate closed loop characteristics calculated from the controller model and the linearised equations of motion were successfully used to tune the controller for the case of a one mode flexible link. However, control of this system requires 2857hz sampling and the effective contact stiffness is so much greater than the link structural stiffness that, for any reasonable number of admissible functions used to model the link structure, the dynamics are dominated by the highest frequency mode and an even higher sampling rate is necessary. Experimental results from An, et al. [14] suggest that the effective contact stiffness of 5.0037MN/m given by the Hertz contact model is an extreme upper bound and that a more  reasonable value is keff = 61.554kN/m. At this level of effective contact stiffness the system was found to be dominated by the two modes with the lowest natural frequencies, as the results of the linear analysis in section 5.2.2 suggest should be the case. The approximate closed loop characteristics, calculated from the control law and the linearised system model, are used to select stable, robust controller parameters for the case of the link structure modelled by a single admissible function.  Chapter 5. Force Control Algorithm Performance and Evaluation  163  Finally it has been shown that when additional admissible functions are included in the link structure model the number of modes in the controller model should be increased but, due to the domination of the system dynamics by the lower frequency modes, the sampling rate need not be increased. These results establish the value of the plots of the variation in modal natural frequencies with effective contact stiffness (obtained from linear analysis) in determining the number of significant modes and required sampling rate for specific configurations.  5.4  Making and Breaking Contact  The circumstances surrounding the making and breaking of contact between the manipula tor tip and the environment surface pose a difficult control problem. When contact exists the contact force provides a measure of the state of the entire manipulator and environment system as is exemplified by the expressions for the contact force components in terms of the generalised coordinates that were derived in section 2.5. On the other hand, when the manipulator tip is not in contact with the environment surface the contact force is, by def inition, zero and therefore the controlled variable provides no information about the state of the system. Thus the contact force is a positive indefinite function of the system state which exhibits a discontinuity at zero. This lack of information in the absence of contact presents two difficulties for the force control algorithm. Firstly, the control input levels which the force control algorithm calcu lates in attempting to make or regain contact will not reflect the position or velocity of the manipulator tip with respect to the surface. In most cases this will result in the tip even tually arriving at the surface with a substantial contact velocity as the controller attempts to remove the error between the force setpoint level and the zero contact force. Without special actions the force controller is likely to have difficult controlling the force transient  Chapter 5. Force Control Algorithm Performance and Evaluation  164  which results from a non-zero contact velocity. The second difficulty arises in the operation of the system identification algorithm used in the adaptive controller because the algorithm updates the model coefficients on the basis of the difference between the actual and pre dicted output with the tacit assumption that the model and the system being identified are continuous through zero. In order to overcome these difficulties it is necessary to control the motion of the manip ulator when it is not in contact as well as the forces it exerts on the surface when it is. The predictive control algorithm derived in Chapter 3 can be readily applied to the task of con trolling the manipulator joint angles simply by providing it with a transfer function model which relates the joint angles to the control inputs. To successfully use the combination of force and motion controllers to operate through the discontinuity when contact is made or broken a level of control above the basic predictive control algorithm is introduced. This higher level control is referred to as the Contact Control Logic. Like the circumstances it is intended to deal with, it is discontinuous and, to some extent, heuristic in nature. On the basis of sensor inputs and data supplied by the operator and the trajectory generator the contact control logic generates the setpoint programs for the predictive control algo rithm, selects between force and motion control algorithms and chooses whether or not to allow system identification to proceed. The relationship of the contact control logic to the manipulator, sensors and other elements of the control structure is shown in Figure 5.26. The contact events which must be dealt with by the contact control logic can be broadly classified as expected (or intentional) and unexpected. An example of an intentional contact event is the situation where the operator or task planner accurately knows the location of the surface to be contacted. Under these circumstances motion control can be used to move the manipulator tip to a location adjacent to the surface and to reduce its approach velocity sufficiently to avoid a violent collision and accompanying high level of transient contact force. From this state a smooth transition to force control can be made. The role  Chapter 5. Force Control Algorithm Performance and Evaluation  Figure 5.26: Block diagram of the overall control structure.  165  Chapter 5. Force Control Algorithm Performance and Evaluation  166  of the contact control logic in this situation is to switch between motion and force control at the appropriate time as directed by the operator or task planner. In contrast, an example of an unexpected contact event is the situation when, due to imperfect knowledge of the manipulator’s working environment, it collides with a surface. If the collision is due to an object being unexpectedly in the path of manipulator motion the contact control logic must take measures to minimise the collision force and bring the manipulator to rest, pending corrective action from the operator or task planner. On the other hand, if the collision is due to inaccurate knowledge of the location of a surface on which the manipulator was intended to exert force the role of the contact control logic is to switch to force control mode, minimise the force transient that results from the collision and reach the desired force level setpoint. This latter case is the focus of the remainder of this section.  Two operational strategies for the contact control logic which seek to  achieve sustained, controlled contact force control following an unexpected contact event are proposed and their performance is examined by way of simulations. The analysis, results and discussion presented in this section have already been published [27]. 5.4.1  Stop Motion Strategy  The first, relatively simple, strategy is referred to as the stop motion strategy. The contact control logic is programmed such that the control mode is switched from motion to force when a non-zero contact force is sensed and motion control is resumed if contact is broken. In addition, when contact occurs the motion control setpoint is changed so that when the motion controller resumes operation, rather than continuing the with motion that that was being executed prior to contact, the joint angles at which contact was made will be maintained. Figure 5.27 shows the results of a simulation in which this strategy is used. A single link manipulator is used, modelled on a 1Mm long, 2.0cm diameter aluminium rod with the  Chapter 5. Force Control Algorithm Performance and Evaluation  167  (a) Joint Angle Response  30  20 i:; 15 0 C  o  0  10  Time [sJ  (b) Contact Force Response  Figure 5.27: Joint angle and contact force responses for a single flexible link colliding with a 6200N/m surface. Control mode switching is based on sensed contact force and the motion control setpoint is modified due to the contact event.  Chapter 5. Force Control Algorithm Performance and Evaluation  168  structural dynamics represent by the first two modes of the rod considered as a cantilever beam. The link is initially in motion with constant joint velocity toward a surface with a contact stiffness of 6200N/m. The predictive control algorithm parameters are a delay horizon (N ) of 1 time step, a prediction horizon (N 1 ) of 40 time steps, a control horizon 2 (Na) of 12 time steps and a control increment weighting factor  ()  of 1.0. The controller  time step is 4.Oms. The motion controller is adaptive but the model coefficients for the force controller are held constant. A non-adaptive (i.e. fixed model coefficients) force controller has been used in order to focus attention of the issues surrounding the transition from motion to force control. The problems that arise when adaptive force control is used during this transition period are discussed below. The simulated joint angle and contact force responses (solid lines) and corresponding setpoints (dashed lines) are shown in Figures 5.27a and 5.27b, respectively. The motion controller shows good tracking of the joint angle ramp during the initial part of the maneu ver. The desired angular velocity of the joint is 0.5rad/s which gives a nominal velocity at the link tip of O.5m/s (neglecting the vibrational motion of the flexible link). After 0.257s the link tip contacts the environment surface with a velocity of 0.536m/s. At this point the control mode is switched from motion to force due to the sensing of a non-zero contact force and the force controller attempts to bring the system to rest at the specified contact force setpoint of 8.ON. Upon switching control modes the motion control setpoint for future use is modified to hold the joint angle stationary at 7.56°, the joint angle at which contact was detected. Due to the non-zero velocity of the link tip at the instant of contact the contact force rises rapidly to approximately 16N and, before the force controller can have any significant effect on the system the link rebounds from the surface and motion control is resumed. This cycle is repeated twice more with force control being established on the third contact.  Chapter 5. Force Control Algorithm Performance and Evaluation  169  After sustained contact has been achieved an adaptive force controller (i.e. one for which the model coefficients are updated using a system identification algorithm at each time step) is the correct choice. However a non-adaptive controller (i.e. fixed model coefficients) is required during the transition period while the tip is bouncing on the environment surface. During this period the brief intervals of rapidly changing force levels interspersed with periods of zero force cause the system identification algorithm to produce wildly inaccurate estimates of the model coefficients. Using these coefficients in the control law can result in a loss of control. A solution to this problem is to delay the start of force model identification until sustained contact is assured. A delay of 25 time steps is adequate for the case shown in Figure 5.27. However, this approach requires that some means be devised to decide that sustained contact has been achieved; a difficult proposition considering the number of factors on which the duration of the transition period (number of bounces) depends. In addition to the difficulties involved in using adaptive force control during the transi tion period, the strategy of bouncing on the surface until force control can be achieved is, for most applications, not acceptable. Furthermore, it has been observed in other simulation runs that the number of bounces can be much greater than the three seen in Figure 5.27b, depending on the dynamic characteristics of the link and the surface. Worst of all, there is, apparently, no guarantee that stable force control will in fact be achieved at all using this strategy. While various refinements of the motion control strategy during the bounces might produce better results, observation of the contact force transient which occurs during the first bounce suggests another course of action. 5.4.2  Force Setpoint Modification Strategy  Comparison of the contact force transient during the first period of contact in Figure 5.27b with the corresponding transient for a collision between the link and the environment at the same velocity during which no attempt is made to control the contact force shows very clear  Chapter 5. Force Control Algorithm Performance and Evaluation  170  10  k  / Figure 5.28: Simple 1 degree of freedom model of a single link in contact with an environ ment. similarity between the transients. In both cases the transient closely resembles the positive portion of a sinusoid, the period of which correlates closely with the natural frequency of the first mode of the open loop system. Based on this observation it is apparent that the force controller has almost no effect on the contact force level immediately following contact and that the response of the system at that time is entirely due to the open ioop dynamics, in particular the velocity of the link at the instant of contact. This suggestion is confirmed by consideration of the simple single degree of freedom model of the link and contact dynamics shown in Figure 5.28. The equation of motion of this system without forcing is 8 = 0 2 I8 + kL  (5.48)  Given initial conditions at t = 0 of 8 = 0 and & =&, the particular solution of equation (5.48) is 8(t) = —sinwt, Li.)  2 = wherew  2 kL  (5.49)  Chapter 5. Force Control Algorithm Performance and Evaluation  171  In this model the contact force is the force generated by the compression of the spring, that is F(t)  =  kLO(t) kL8 sinwt w  =  Fksinwt,  (5.50)  where Fk= With Ic set to the value of the effective stiffness of the link and environment surfaces (considered in series) and  10  set equal to the rigid body moment of inertia of the link  about the joint axis, equation (5.50) gives a very good prediction of the initial contact force transient shown in Figure 5.27b. Note that the contact force transient described by equation (5.50) is entirely a result of the initial condition velocity,  which correlates with to the velocity with which the link tip  contacts the surface. The classical form of transfer function model on which the predictive force control algorithm is based, however, assumes quiescent initial conditions (that is, the system is stationary at the instant of contact). Thus, a fundamental incompatibility exists between the system for which control is desired and the model on which the controller is based. Viewing the situation in terms of energy, the force controller expects the system to have zero energy when it begins operation. The system however has non-zero energy, in the amount of the kinetic energy of the link at the instant of contact. If stable force control is to be achieved without letting the link bounce off the surface the controller must overcome the initial energy of the system. This is confirmed by simulations similar to those shown in Figure 5.27, in which higher contact force setpoints are specified. In those simulations the initial portion of the contact force transient is similar to the corresponding uncontrolled collision transient, however the force controller (which is more energetic by virtue of the  Chapter 5. Force Control Algorithm Performance and Evaluation  172  25  20 z 0 C.) 0 I]  15  4.-  C-)  (U  4.-  0  10  C-)  5  0 .0  .1  .2  .3  Time [s] Figure 5.29: Contact force response for a single flexible link making contact with a 6200N/m surface at 0.536m/s. higher setpoint it is seeking to achieve) overcomes the initial energy of the system and brings the system to rest at the desired contact force level. Figure 5.29 shows the contact force response with a setpoint of 20N. As a consequence of these observations a force setpoint modification strategy is proposed for the contact control logic. Differentiating equation (5.50) with respect to time and solving for k at a time immedi ately after contact when t is small gives k  This result, combined with Fk  =  1(t)  (5.51)  —v—-.  L6  2 /J6 and w  =  from equations (5.50) and (5.49),  make it possible to use the joint encoder data just prior to the instant of contact and the force sensor data immediately following contact to estimate the peak force and the duration  Chapter 5. Force Control Algorithm Performance and Evaluation  173  20  15  z ci) C)  10 C 0  C)  5  0 .0  .1  .2 Time [sj  .3  Figure 5.30: Contact force response of a single flexible link colliding with a 6200N/m surface with force setpoint modification.  ()  of the contact that will occur if the force controller does not act. The peak force is  effectively a measure of the initial energy of the system which the controller must overcome. In order to do this the specified setpoint is compared to the estimated peak force. If the setpoint is found to be less than the estimated peak the former is modified to the estimated peak force level for a period of time equal to predicted duration of the uncontrolled collision contact transient. This enables the controller to exert the energy necessary to overcome the initial kinetic energy of the link. Figure 5.30 shows the contact force response for the same conditions as that shown in Figure 5.27 but with contact transient prediction and contact force setpoint modification included in the contact control logic. Contact occurs at 0.257s with a link tip velocity of  Chapter 5. Force Control Algorithm Performance and Evaluation  174  0.536m/s. At 0.260s the force controller estimates the effective stiffness of the contacting surfaces to be 3041N/m (compared to the actual value of 3100N/m) and predict s a peak force of 15.69N and an uncontrolled contact duration of 30.2ms. Based on these predict ions the contact force setpoint is raised to 15.69N for 30.2ms and a stable 8.ON contac t force level is achieved without the link leaving the surface as happened in Figure 5.27b. The initial overshoot in Figure 5.30 is largely due to the initial contact velocity of the link and cannot be removed by a controller which has no provision for non-zero initial conditions. In any case, the overshoot is significantly less that that which eventually occurs in Figure 5.27b. The model, based on equation (5.50), used to predict the peak contact force and cor responding uncontrolled contact duration is a very simple one. The necessary calcula tions require a total of 2 additions, 5 multiplications and 2 square root operations. (Two ad ditions and one multiplication can be eliminated from this total if the force rate can be obtained from the force sensor.) As such, the setpoint modification can easily be put into effect within one or two time steps after contact is sensed. Furthermore, it has been noted in simulation results that a lag of one or two sampling intervals between the instant of contact and the completion of the prediction calculation (and application of the setpoin t modification) does not adversely effect the control. 5.5  Force Control for Two Link Manipulators  Consideration of a manipulator with two or more links greatly increases the complexity of the analysis and control problems. In this section force control for planar manipulators with two links is addressed. Both rigid link and flexible link manipulators are considered. The two joint torques available as control inputs for a two link manipulator permit control of two world frame components of contact force.  Chapter 5. Force Control Algorithm Performance and Evaluation  175  In this what follows, similar analytical techniques to those used in section 5.2 are em ployed to gain insight into the modal structure of the system and the results are applied to the selection of appropriate controller parameters. Because of the strongly coupled na ture of the system the multivariable control algorithm is necessary. The derivation of an appropriate multivariable controller model from the linearised equations of motion is pre sented. Simulated step responses are used to evaluate the performance of the closed loop force control for both rigid link and flexible link manipulators. 5.5.1  Linear Analysis  In this section a linear analysis of a two link manipulator in contact with a deformable en vironment is presented. The analysis follows the same general approach that was employed for a single link system in section 5.2. Rigid Links  We begin by considering the case of rigid links which is the limiting case of infinite link stiffness. As well, the characteristics of rigid link manipulators are of practical interest in their own right since they typify the vast majority of existing industrial manipulators. Fig ure 5.31 shows the configuration and important parameters for a manipulator with two rigid links exerting forces on the environment in the r and y world frame coordinate directions. The equations of motion for this system are given in section A.2. Neglecting damping, gravitational effects and inertia coupling between the links and the contact region mass, replacing the joint torques 2 {Tj(t),r ( t)} with the general control inputs 2 {ui(t),u ( t)} and linearising about the nominal state  {  2 B  Ex  Lii  x  y  ] [ =  0  ...  0  j  Chapter 5. Force Control Algorithm Performance and Evaluation  176  key  + me 5 m  k  (x t) 2 w ,  102  2 2 M L  2 U  (t) 1 w , x  y  ILM 01 1  x I  Figure 5.31: Two rigid link manipulator in contact with a deformable environment.  Chapter 5. Force Control Algorithm Performance and Evaluation  177  gives [101  +  102  2 (L + 1 C) + m (L + L + 2L 2 L c)] 2 L 1 + M  1  c + m (L + L 1 L 2 c)] 2 1 + [102 + M [L . s +L j s 2 —m 1 =  u ( 1 t)  —  ] c 2 + m [Lic + L  ) [(Ljs + L s 2 ) Oi + 2 s 2 5 (List + L k L +j 0 s  L [(Lic + L ) c )0 +L c 2 0 c 2 —k (Lic + 2  [I2  M + m (L + L c 1 L c)] 2 1 + 2  i  +  (5.52)  —  [102  2] 2 + mL  62  ë + mL s 2 —mL ë c 2 =  u ( 2 t)  —  )0 + 2 s 2 L + e] 0 s s [(List + L 2 L 3 k  c [(Lic + L 2 L 8 —k )8 + L c 2 0 c 2 ) s 2 m(Lis + L  i 8  6 s 2 + mL  —  më  =  (5.53)  —  5 [(List + L —k )0 + L s 2 8 + e] s 2  —  kexEx (5.54)  ) 01 + mL c 2 m (Lic + L 6 + mi c 2  =  8 [(Lic + L —k )0 +L c 2 0 c 2  — —  (5.55)  where the generalised coordinates {0, 02, e, e} are now perturbations about the nominal state. Figure 5.32 shows the numerically calculated eigenvalues of the system with the following parameters: 0  =  O’,  and  =  8 k  =  kex  =  90°, 101  =  102  =  0.2817kg m , M 2 2  =  0.8451kg, L 1  =  2 L  =  1.Om,  key. The eigenvalues are shown for the range of effective contact  stiffness, keff, from 50N/m to 5MN/m. As was seen in Figure 5.3, the upper frequency line in Figure 5.32 is well separated from the other frequencies. That line is actually two superimposed lines representing the natural frequencies of the contact mass vibration modes  Chapter 5. Force Control Algorithm Performance and Evaluation  1  6  1  4  I  11111111  I  I  113 3  1111111  I  IIIIII  _L rksx÷kexl 2t Lm+ mej  178  I  1/2  IIIIIIj  I  L ksy+key 4  I  1/2  2r [m÷ me  ‘Ic’ >‘  C)  C a)  a) U-  1  z 10’  1  ø 111  101  1  .1 —  101  1  3  1  4  Effective Contact Stiffness [kN/m]  Figure 5.32: Variation of the natural frequencies with effective contact stiffness for a two link manipulator with identical, rigid links in contact with an isotropic (kxeff = kyeff) envi ronment.  Chapter 5. Force Control Algorithm Performance and Evaluation  (  and  7)4)  179  in the x and y coordinate directions. These lines are closely approximated by  f  lk+k sX ex = 27r m 5 + me  2  and  —  They are coincident because k  =  kex  f  =  1  —  K 2  k+k ey sj 3 + me m  (5.56)  811 = key for the case shown. k  For most cases of practical interest the contact mass is small compared to the other inertias in the system with the result that the high frequency modes associated with its local vibration have minimal effect on the rest of the system. As was done in section 5.2.2, we now introduce a steady state approximation for the contact dynamics and focus the analysis on the remaining modes. Neglecting the contact region mass, m, and replacing the directional contact stiffnesses, k 8 and key, with the corresponding effective 8 and kex, k contact stiffnesses, kff and kyeff, allows the linearised equations of motion to be reduced to: [101  +  102  2 (L + LiL c)] 2 +M  = ui(t)  —  1+ ë  LiL M c ] [i + 2  ) [(List + L s 2 ) s 2 kxeff (List + L  ] e s 2 +L  ) [(Lic + L c 2 ] 8 c 2 )8+L c 2 kyeff (Lic + L  [102  LiL M c j + 2  (t) 2 = u  —  2  (5.57)  +  )6 s 2 j 8 s 2 s [(List + L 2 kffL i + L  ) &i + L c 2 ]. 8 c 2 C [(Lic + L 2 kyeffL  (5.58)  After a great deal of algebraic manipulation the characteristic equation for this system can be written as L) 102 2 +M  —  MLLc2] (jw) 4  c)j (k 2 2 (L + LiL ff + zks2) L 11 + [[I +102 + M +102  )2] s 2 c4) + Ak (List + L 2 L 1 [kyeff (L + L + 2L  Chapter 5. Force Control Algorithm Performance and Evaluation  —  180  2+M L 02 (21 LJ) [kyeff (Lic + L 2 ) + txk (List + L 2 ) S2]} (jw) s 2 2  2 = 0, +kxeffkyeffLL.3 where Lik = kxeff  —  (559)  jeff. The structure of equation (5.59) provides important insights into 1 k  the physical behaviour of the system. Firstly, note that the substitution, kxeff  kyeff + 1k,  used to arrive at equation (5.59) separates the overall effects of the contact stiffness from those due to a difference in the contact stiffnesses in the x and y directions. In particular, this substitution reveals that the nominal joint angle of link 1, 8, affects the natural frequencies oniy when kff  kyeff. Secondly, in the case of exact alignment of the links (8  00  or  180°), the (jw)° term disappears. This circumstance results in one of the natural frequencies of the system being zero which implies that the two components of contact force cannot be simultaneously controlled because the manipulator lacks sufficient degrees of freedom. This is an analogous situation to the familiar degenerate condition which arises in motion control when planar links are exactly aligned. Equation (5.59) can be solved to yield an expression which closely approximates the natural frequencies of the links and keff (,w)2  kyeff  and  2  modes. For the cases typified by Figure 5.32 (identical  keff) we have:  )— MLc L 2 = 2 (z + M 2  [_  (i +  L2s2) 2 M  + (i + ML4s2)  2].  (5.60)  While this expression is somewhat more complex than those obtained in the case of a single link (equation (5.11)), it displays the came general structure.  The natural frequencies  are directly proportional to kff and L and inversely proportional to the square root of a combination of inertias. This expression also reveals that the frequencies of the 72  modes vary between bounds as & varies modulo 90°.  At 0  =  00  and  the manipulator  configuration is degenerate and the frequency of ij attains its minimum value, namely zero, while the frequency of  ?)2  is at its maximum. At O = 90° the frequencies are in the ratio  Chapter 5. Force Control Algorithm Performance and Evaluation  of 1 to 2 and the frequency of variation if the frequency of  is at its maximum and that of  i  is much larger than that of  181  is at its minimum. The  2 i  72.  Returning our attention to the model which includes the full contact dynamics, we now consider the eigenvectors of the system and, in particular, the information they provide about the contributions of the natural modes to the contact force components and the exci tation of those natural modes by the control inputs. As in the case of a single rigid link, the eigenvectors, normalised to unit magnitude, are found to be independent of effective contact stiffness for the case where kxeff = kyeff. With the parameter values used in Figure 5.32 the eigenvectors are E  [ei  2 e e 3 e ] 4 —0.667  —1.286 x  0.667 8.087 x  —2.962 x iO’ —2.662 x  1O_6  0.894  1.065 x i0  —0.147  1  4.311 x 10u1  —6.433 x iO’  1.070 x lO  1  10_13  0.333  1O_12  2.662 x  106  (5.61)  With these numerical values the transformation between the generalised coordinates and the natural modes of the system, ‘1)1  T =E  (5.62)  ,  174  clearly shows the isolation of the region mass) from the  and  772  and  modes (high frequency vibration of the contact  modes. The energy of the  772  mode is concentrated in  the oscillation of link 2 about its joint. The h mode includes oscillations of both links about their joints in equal proportions of opposite sign. This particular arrangement of the contributions of the 8 and  62  generalised coordinates to the  77i  and  772  natural coordinates  Chapter 5. Force Control Algorithm Performance and Evaluation  is due to the nominal joint angles,  t  =  and t9  00  =  182  90°. The effect on the 7 h and  12  eigenvectors of changing the nominal joint angle of the second link is shown in Figure 5.33. The relatively small contribution of the 8 generalised coordinate to the as  2  mode decreases  increases, as shown in Figure 5.33b. This trend indicates that the majority of the  energy associated with the  mode is in the form of oscillations of link 2 about its joint.  ‘]2  Figure 5.33a shows that the  i  mode combines contributions from 6 and  2  with opposite  signs, reflecting the coupling between the links. The ratio of the components is determined by the lengths and inertias of the links. In the case shown the links are identical and the ratio approaches -2 to 1 as the links are aligned and -1 to 1 as they become perpendicular. The linearised contact force model equations, obtained from equations (2.56) and (2.57) by neglecting damping and structural flexibility terms, are:  F  c  2  rhere  Es  Eu  [  =  -  (List + L ) -L s 2 s 2  (±:  -  i)  o]  (5.63)  and  F  =  5 k  [  ,  where  (Lic + L c 0 2 ) L c 2  (::  -  i)  j.  (5.64)  CD  CD  C  cz  CD  0;  C C  .1  •  I  I  •  \\ \\ \ \ ‘‘ \\\\\ \\\\  \\  I  I  •  I  C  c  II  I  •  C  o  •-j  C  ii  r.3  Ii  I\)  II —  C C  ii  r..)  Generalised Coordinate Components of Natural Mode &  C  0  I  I I  I  Generalised Coordinate Components of Natural Mode —  cc  I  I  I1  I  Chapter 5. Force Control Algorithm Performance and Evaluation  184  Transforming these expression into natural coordinate space we find that, with the eigen vectors given in equation (5.61),  F =  0  7i 7  —0.4472  772  0  773  —5,3248  x 10—6  and  (5.65)  77 T  F  0.3333  77i  —6.4319 x 1013  772  —1.3312 x 10-6  77  x 10_17  77  = —1.4810  (5.66)  These results show that the contact forces, F and F, are composed almost entirely of the 772 and 771 modes, respectively, that is, the high frequency vibrations of the contact region mass contribute minimally to the contact forces. As would be expected from the above discussion of the natural frequencies, the contact force components in natural coordinate space are independent of & when Lk = 0. The effect of varying O in the range from shown in Figure 5.34 where the values of the 7i and  712  00  to 90° is  components of F and F are plotted  for identical links and equal directional contact stiffnesses with keff = 61.554kN/m. When & is small Figure 5.34b shows that F is dominated by the 772 mode, reflecting the fact that a motion about joint 2 results primarily in a force in the y direction. For intermediate values of i9 the motions of both links contribute to F and it is therefore composed of components of both  77i  and 772 At O = 90° the 71i mode dominates F because, in that configuration,  only motion about joint 1 can produce a force in the y direction. The degenerate state of the manipulator which occurs when the links are aligned is reflected by the zero values of both the  1 ‘i  and  72  components of F shown in Figure 5.34b. As éL increases the 772  component increases in magnitude to a maximum at 0 = 90°, in which configuration only  Chapter 5. Force Control Algorithm Performance and Evaluation  185  0 -  .05  -.10 -  0  CLI..  -.15  25  -  =  a)  j2 -  .30  0E  Z  8  -  -  .35 .40 I  I  45  10  0  20  40  30  o (a)  i  and  12  I  50 [degrees]  •  60  70  80  90  60  70  80  90  components of F  .35 .30  .25 .20  /  .15 .10  0u  .05 0 a)  05 10  .-  -  -  0.  0 E -.15 Zo 0 .20 -  -  .25  .  30 •  —.35  I  I  •  •  •  .  0  10  20  30  (b)  and  ]2  40  50  e  [degrees]  components of F  Figure 5.34: Variation of the components of F and F in natural coordinate space with 6 for identical rigid links with equal directional contact stiffnesses, keff = 61.554kN/m.  Chapter 5. Force Control Algorithm Performance and Evaluation  motion about joint 2 can produce a force in the x direction. The  1 i  186  component of F is due  to the coupling between the links. Finally, we consider the relative excitation of the natural modes by the control inputs, 1 and u u , by using 2  ET  to transform the input vectors, [1  0  0  T 01  and [0  1  0  T 01  from generalised to natural coordinate space. As in the single link case, the excitation of the modes corresponding to the local vibrations of the contact region mass are found to be negligible when that mass is small compared to the link inertias. The excitations of the other  (  and  1)2)  modes are shown in Figure 5.35 for values of éL between  00  and 90°. The  geometric relationships which were discussed above in the context of the contributions of the  i 8  and  82  generalised coordinates to the natural modes are reflected in the data shown  in Figure 5.35. The u 1 input primarily excites the  mode, particularly as the links become  perpendicular. On the other hand, u , excited both the 2  and the  1)2  modes when the  links are nearly aligned, with the excitation of 1 h decreasing somewhat as the links become perpendicular. The u 2 input excites both modes, regardless of the configuration of the manipulator, because an input at joint 2 not only drives link 2 but also produces a reaction force on the tip of link 1. In summary, linear analysis of the contact case for a two link manipulator with infinitely stiff links under the assumption that the contact region mass is small compared to the link inertias reveals that: 1. the natural frequencies are proportional to k for a given set of link lengths and inertias; 2. the natural frequencies of the contact region mass vibration modes  ij)  are  widely separated from those due to the links oscillating about their joints (‘ii and  1)2);  3. the natural frequencies of the  i  and  1)2  (i  and  modes are independent of 6 when kseff = kyeff  and, for a given keff, are bounded above and below as 8 varies modulo 90°;  Chapter 5. Force Control Algorithm Performance and Evaluation  187  0 -  .05  11  -.10 -.15 C  a  -  a) .; -  o .  0  x  Lu  -  -  .20  .25 .30 .35  4-.  U)  10  20  30  40  o (a) Joint 1  50  60  70  80  90  60  70  80  90  [degrees] input, u 1  .94  .92 .90  .88 .86  .84 .  x  .82  lii .80  LQ)  .78 .76 .74 72 70 .68 .65  0  10  20  30  40  o  50  [degrees]  (b) Joint 2 input, u 2  Figure 5.35: Relative excitation of the 7li and for a two link rigid manipulator in contact.  72  modes by the control inputs, u , 2 1 and u  Chapter 5. Force Control Algorithm Performance and Evaluation  4. the contact forces, F and F, are composed primarily of the  188  and  2  natural modes  in proportions which depend on the nominal manipulator geometry; and 5. the control inputs excite only the  and  2  modes.  For the purposes of contact force control using a discrete time algorithm these results imply that the high frequency modes can be ignored and the sampling interval selected to satisfy the Nyquist criterion for the  mode frequency when 8 =  2  Employing the multivariable  00.  form of the predictive control algorithm developed in Chapter 3 with a controller model that properly includes the effects of coupling (as discussed in section 5.5.2) allows the system to be easily controlled. The presence of two undamped open loop modes means that a control horizon of N = 2 is needed. The performance of the force control algorithm for this case is demonstrated in section 5.5.3. Flexible Links The equations of motion for a two flexible link manipulator making contact with a deformable environment were derived in Chapter 2 (equations (2.47) through (2.52)). Applying the steady state approximation for the contact dynamics, linearising about the nominal manipulator joint angles O = i9 and 62 = 6 (with all other state variables nominally zero), neglecting damping and gravitational effects, and considering only one flexible mode for each link, the equations of motion are simplified to: 2 [r + 102 + M  (102  LiL M c ] + 2  —  —  i  c) 2 2 (L 1 + L +M  + +  (L + L c)] 2 1  2  +  &lo  [1121  )+ s 2 [kff (List + L  +  (102  LiL M c ) + 2  cI 1 L ] + 421 kyeff  (Lic H-  o] ii  21  )2] 81 c 2 L  ) + k,eff s 2 0+) 10 (L 4 s 2 L s+L 1 [kxeff (s4ii  0 (cq.ii  )] ‘b c 2 10 (Lic + L q c 2 L +)  Chapter 5. Force Control Algorithm Performance and Evaluation  —  —  189  ) + kyeffL s 2 )] 82 c 2 S (List + L 2 [keffL C (Lic + L 2 (List + 2 L+ 2 ) s )] c 2 q kyeffc l o S (Lic + L  [kxeffs q 2 2lo  ‘j&21  (5.67)  [im  =  2 (L 1 + L c) 2 +M 2 +M  +  [I21  +  [Io21o  —  —  —  —  11o  +  (102  +  ) C 1 L 2 M  *1”  (11O  + L ) 1 & c 2 0  c;iio] L 2 + M  2  +  11o] i  c&io) L 2 + M  +  0 [I12j1  11  j 21 0 iii 42 + cI  {kxeff  0 (sii  10 (List + 2 q s 2 L (cq + ) L + kff 110 ) s 10 (Lic + L b c 2 L )j c 2 +)  {kxeff  (Sii  210 + kyeff (c s L 2 210 + c L 2 110 + ) +)  [kxeff  (q11o  10 2 q s 2 L iio + L 5 L + kyeff (cq s ) 1 4 c 2 02 L 02 ] c +)  [kseff  (sqiio  10 7 4 s 2 L 1 S 2 10 2 + + )  0 Icyeff (cqii  1311]  01  ii  qS b21 2 c ] 10 210 4 c 2 L +) (5.68)  [102 =  LiL M c ;] + 2 (t) 2 u  —  [k  —  2*2  ciio] L 2 + M  ii  + I& +  i  44 iI)ii c 2 L )j ) 1 q s 2 0 + 10 i 1 0+ 2 ieffL 1 k 1 C 2 (c4 +L  + kyeffLC]  4)21o 2 [kxeffS  —  q s 2 [kxeffL l o !  [kxeffsi i c 2 o  +  112102  kyeffL l 4 C 2 o]  q4 + s 2 L ) + 10 +  kyeac i q 2 o  12 4 kyeffCl L 0 1 2 ] 2C )  *2 i2 kyeffc 2 q 10 + + 12  l/’21  (5.69)  + I22121  (List + L ) + k,effC s 2 )j c 2 lo (Lic + L 22  q 2 [keffs l o S  *2 i2  02  ] bi + 110 qS 421 + [I1211 + cI  q21oL2s [kxeffs 2  —  [1oio  0 (sqSii  8l 2 1 ceff  [1121 + LicI i1 42 —  +  )j Oi c 2 ) + kyeffL s 2 S (List + L 2 [kxeffL C (Lic + L 2  S 2 [keffL —  i  81  110 + )1 (cq 11 10 q c 2 L Sb  02  1321j I’21.  (5.70)  Chapter 5. Force Control Algorithm Performance and Evaluation  190  Even in this greatly simplified form an algebraic analysis of the system is difficult to ac complish as well as to gain insight from. However, the insights obtained through the single link analysis Section 5.2 and the two rigid link analysis above provide guidance as to the expected characteristics of the system. These characteristics will be explored numerically. Figure 5.36 shows the variation in natural frequencies with effective contact stiffness for a manipulator with identical links, each with a single structural mode, in contact with a surface having identical contact stiffnesses in the x and y directions. The nominal manip =  ulator joint angles are lines for the four modes  (  00  and 6  through  =  i)  900.  The characteristic pattern of the frequency  with the lowest natural frequencies is recognizably  analogous to the single flexible link case. The pattern implies the same type of transition seen in the single flexible case, namely domination of the system dynamics at low contact stiffness by low frequency, rigid link modes higher frequency, structural modes 772  (m  and  (iii  i)  and  2)  giving way to domination by the  at high stiffnesses. The lines for the  1i  and  mode natural frequencies are asymptotic to the frequency lines for the corresponding two  rigid link case when the contact stiffness is small. As the effective contact stiffness increases these frequencies both become independent of keff. The link structural modes each intro duce a mode  (773  and  i)  whose natural frequency at low contact stiffnesses is independent  of keff while at high stiffnesses the frequency is proportional to kff. The lines for the highest frequency modes  (  and  776)  are superimposed in Figure 5.36 because the contact surfaces  are isotropic (keff = kyeff). These modes are the local vibrations of the contact region mass in the x and y directions. Other characteristics of the two flexible link system that follow the trends observed in the earlier linear analyses are: 1. the natural frequencies of the four low frequencies modes are independent of O when kxeff  =  kyeff and, for a given keff, they are bounded above and below as O varies  Chapter 5. Force Control Algorithm Performance and Evaluation  1  6  1  ø  191  N > C.)  ci) D  I 1ø_2  1  g_1  1  1  gi  1  1  iø  Effective Contact Stiffness [kN/m] Figure 5.36: Variation of the natural frequencies with effective contact stiffness of a two , one structural mode each) links 2 link manipulator with identical, flexible (El = 574N m in contact with an isotropic (kzeff = kyeff) environment.  Chapter 5. Force Control Algorithm Performance and Evaluation  modulo  900;  192  furthermore, the frequency of the ‘h mode varies over a much larger  range and in opposition to the frequencies of the other modes; 2. additional admissible functions in the link models introduce additional intermediate frequency modes, each of which dominates the dynamics over a specific range of keff in a manner analogous that for the single link case shown in Figure 5.10; 3. in the range of contact stiffnesses where they are independent of keff, the natural frequencies of all but the two highest frequency modes are proportional to the flexural rigidities of the links; 4. the centres of the ranges of effective contact stiffness values in which the transitions among dominant modes occur are also proportional to the flexural rigidities of the links in the same fashion as illustrated in Figure 5.9. There are two features of particular interest in Figure 5.36 which are not explained by the linear analyses of the simpler configurations. Firstly, the fact that the  and  772  frequency lines both asymptotically approach the same frequency for large values of keff. This coincidence of frequencies is due to the links having identical lengths and inertias. Secondly, the curvatures of the  and  773  mode frequency lines in the transition range of keff  are somewhat sharper than the curvatures of the  772  and  mode lines. This characteristic is  a result of there being, in reality, two overlapping transition ranges. The overlap is also due to the links being identical. Figure 5.37 shows the situation when link 2 is twice as long and twice as massive as link 1. The pattern of transfer of dominance from one mode to another is greatly complicated by the different link lengths but the established characteristics are still recognisable. The variation with keff of the contributions of the natural modes to the contact forces illustrates the affect on control of the transfer of dominance among the various modes. The  Chapter 5. Force Control Algorithm Performance and Evaluation  I  I  IIIITT  I  I  1111111  I  I  1111111  I  I  193  1111111  ,1  5 i0  1  4  Ti  N  z  > C) iø C  a)  a) U  Ti  ( 1 L..  z 101  2 Ti  111 101  111i1111  10_i  ig  I  101  i  i  1  Effective Contact Stiffness [kN/m] Figure 5.37: Variation of the natural frequencies with effective contact stiffness for a two flexible link manipulator (El = 574N m, one structural mode each) with L 1 = 1.Om and = 2.Om.  Chapter 5. Force Control Algorithm Performance and Evaluation  194  linearised contact force model equations for the case of two flexible links, each with a single structural mode are  ‘I’ii F  =  c  02  where  ,  21  Ey  me  =  — 8 k  [  —  (List + L ) s 2  —  210 —L s 2 L s 2 (sii + )  (‘  —  i)  o]  (5.71)  and 01  02  where  I’21 Ex  E!I  =  y 3 k  [  ) c 2 (Lic + L  210 L c 2 L c 2 +)  Ci21io  o  (rn —  i)  ].  (5.72)  Figure 5.38 shows the results of the transformation of equations (5.71) and (5.72) from generalised to natural coordinate space with the same parameter values as were used in Figure 5.36. The transfer of dominance from the to the  773  2  to the  mode in F is shown clearly in Figure 5.36. The  7)4  mode in F and from the  i  and  7)6  i  modes make minimal  contributions to F and F. The relative excitation of the natural modes by the control inputs is shown in Figure 5.39. As was found in the linear analysis of the single flexible link, we see that the control inputs  Chapter 5. Force Control Algorithm Performance and Evaluation  195  .20 .15  .z o  a)  .10  0 LL  .05 0  Dc:  4..  0  -  .05  -.10 -.15  a) -  CO  -  -  Zo C-)  -  -  -  .20 .25 .30 .35 .40 .45 101  102  1  4  Effective Contact Stiffness [kN/m] (a) F components • 35 • 30 a)  111  D 0 Ow  .20 .15  c’o  .10  Z_  0  a) +Oc = a)  2 4  c’c co  0  L.  z  -  ()  .05  13  -.10 -.15 10_2  1O  101  102  4 i0  Effective Contact Stiffness [kN/m] (b) F components  Figure 5.38: Variation of the natural mode components of the contact forces with effective contact stiffness for a two flexible link manipulator. F components are normalised by k 3 and F components by k . 3  Chapter 5. Force Control Algorithm Performance and Evaluation  ..:.*..... .**  1.:  L  U)  196  .4  4-.  U)  Q  .2 .1  1121411516  0 10_i  101  0 i0  102  3 i0  4 i0  Effective Contact Stiffness [kN/m] (a) Joint 1.0  I  1  I  input, u 1 I  .8 12  iø_2  14  10_i  101  iø  102  iø  Effective Contact Stiffness [kN/m] (b) Joint  2  input, u 2  Figure 5.39: Variation of the relative excitation of the natural modes with effective contact stiffness for a two flexible link manipulator.  Chapter 5. Force Control Algorithm Performance and Evaluation  197  excite all but the high frequency modes across the entire range of keff. For the particular configuration of identical links with O =  900  the input at joint 1 excites modes  and  773,  those in which the energy is mainly associated with the link 1 generalised coordinates, 8 and ‘çb. Due to the coupling of the links the u 2 input excites all of the first four modes but the  772  5.5.2  and  774  modes are excited to a greater degree than the  i  and  173  modes.  Controller Model  In order to control the contact forces exerted in the x and y directions by the tip of a two link manipulator a multivariable control algorithm must be used. The model on which such a control algorithm is based accounts for the effects of both of the inputs, u 1 and u , 2 on each of the outputs, F, and F. Attempts to control the system using a pair of single input, single output loops generally fail because of the highly coupled nature of the system dynamics. The multivariable predictive control algorithm derived in Chapter 3 requires a linear model of the system in the form of discrete time transfer functions relating both of the inputs to each of the outputs. With the equations of motion linearised about a nominal state, transformation using the Laplace operator allows the system to be described by a  Chapter 5. Force Control Algorithm Performance and Evaluation  198  matrix equation of the form: 1  8(s)  0  0 0  0 0 J(s)  0  1  ui(S)  i(s) 2 b  0 0  ui(s)  i1’2n2(8)  0 0  er(s)  0 0  02(5)  .  (5.73)  0 0 The linearised forms of the contact force expressions are:  1 (s) 1 b  I1ni  (s)  Fr(s)  c  0(s)  Fr(s)  c  i(s) 2 b  i/)2n2(s)  e( s)  with  c  and  as defined in equations (5.71) and (5.72). Combining these expressions  gives the following four transfer function relationships among the contact forces and control  Chapter 5. Force Control Algorithm Performance and Evaluation  199  inputs: rS)  ui(s)  —  1 CJ  rTf \ rS 1  J(s)l’  U2(S)  T C ii  rs)  )I’ 3 J(  U2(S)  —  T CJ(n ) 2 + 1  575  —  and ,,j \ r,Is) ui(s)  —  i-i,  ‘  —  T Cy J(ni+2)  5 76  —  where /  T  ‘11  —  ii  =  Ii2I  (—i)(’+”)  i\(ni+3)  1  i1)  (_i)’) J(+ ) 2  .  and  I 1 J  J(n+2)  = (_i)(fh+71)  ) 12 J(  and J, is the ij minor of the n x n matrix J(s). Note that the transfer functions share a  common denominator, IJ(s). For the case of rigid links the b generalised coordinates are neglected and the number of admissible functions used to model structural dynamics of the links, ri 1 and n , are, of course, set to zero. 2 5.5.3  Force Control for Two Rigid Links  We begin our examination of the performance of the multivariable predictive force control algorithm with the case of a planar two link manipulator with rigid links. As noted in section 5.5.1, this case is of theoretical interest in the context of this thesis because it represents the limiting situation of infinitely link stiffnesses. It is also of practical interest because most existing industrial manipulators have rigid links. For this test case the link models are based on the same physical model of a 2.0cm diameter, 1.Om long aluminium rod that was used throughout section 5.3. The nominal joint angles are t9  00  and 6 =  300  and the tip of the manipulator is in contact with an  isotropic environment having an effective contact stiffness of keff  lOkN/m. The motion  of the manipulator is in the plane perpendicular to the gravitational field.  Chapter 5. Force Control Algorithm Performance and Evaluation  200  Link Dynamics Link (k) 1 2 6(°) 0 30 1.0 1.0 Lk(m) Mk(kg) 0.8451 0.8451 k(kg m 0 1 ) 0.2817 0.2817 2 Contact Dynamics Direction x y (kN/m) 3 k 20 20 20 20 ke(kN/m) 10 10 keff(kN/m) Table 5.7: Parameter values for two rigid link manipulator controller model.  Controller Model Using the values listed in Table 5.7 for the parameters of the link and contact dynamics models the continuous time transfer functions given by equations (5.75) and (5.76) evaluate to: F’(s) ui(s) Fr(s) (s) 2 u F(s) ui(s)  —  —  —  —  —  —  1830s + 0.4330 x 108 2 0.l835s + 77472 + 0.2500 x 108’ 0.9330 x 108 0.1835s + 77472 + 0.2500 x 108’ _352.1s2 + 0.2500 x 108 _7464s2  —  5 78  0.1835s + 2 7747s + 0.2500 x 10’  and F(s) (s) 2 u  —  —  645152  0.2500 x 108 0.1835s + 7747s 2 + 0.2500 x 108 —  5 80  Recalling the results of the linear analysis, for a small contact region mass the dynamics of the system are dominated by the first two natural modes (see Figure 5.32). For an effective contact stiffness of keff = 10k N/rn the natural frequencies of those modes are 9.4Hz and 31.3Hz. A sampling frequency of 100Hz (h  =  lOms) satisfies the Nyquist criterion for those  modes. Assuming zero order hold sampling, the continuous time domain transfer functions  Chapter 5. Force Control Algorithm Performance and Evaluation  201  can be transformed to the following discrete time controller model: F(q ) 1 ) 1 ui(q F(q ) 1 (q 2 u ) 1 —  ui(q’)  —  — —  — —  0.4298 + 0.3909(q 1 +q 3 ) + 0.4298q 2 1 — 0.8861q’ + 0.7200q — 0.8861q 3 + q_4 —1.589 — 0.1793(q 1 +q 3 ) — 1.589q 2 1 — O. 861q’ + 0.7200q 8 2 — 0.8861q 3 + q_4  1 ‘  5 81  ‘  5 82  1  —1.698 x 10—2 + 0.4908(q’ + q 3 q 2 ) — 1.698 x 10 2 q 1 — 0.8861q’ + 2 00q — O. 0.7 3+4 6lq 88 q  5 83  and F(q ) 1 (q’) 2 u  — —  1  —  1.165 — 1.639(q 1 +q 3 ) + 1.165q 2 —‘ q_4 0.886lq’ + O.7 2 0.886lq q 200 3+  5 84  —  These polynomials provide the initial model coefficient values for the multivariable adaptive predictive force control algorithm. They are updated at each time step by a multivariable form of the EFRA system identification algorithm. The coefficients are also used in con junction with the closed loop characteristic polynomial formulation derived in section 3.3.1 to tune the control algorithm and analyse its performance by calculating the approximate closed loop characteristics. Step Response The linear analysis in section 5.5.1 suggests that force control for a structurally rigid two link manipulator using the multivariable predictive control algorithm should be easily ac complished using a sampling interval sufficient to resolve the of N  =  72  mode and a control horizon  2 time steps. The results presented in this section confirm this prediction.  The approximate closed loop characteristics calculated from the closed loop character istic polynomial formulation derived in section 3.3.1 show that virtually any combination of prediction and control horizon values yield stable closed loop performance. However, as the data in Table 5.8 indicate, a control horizon of N  =  2 time steps provides the best  combination of fast rise time and minimal overshoot because the closed loop response is  Chapter 5. Force Control Algorithm Performance and Evaluation  N 2 3 5 8 10  T 5 6 8 10 5 8 5 8  N 1 1 1 1 2 2 2 2 2 3 3 4 4  Overdamped Mode Dominant Pole —  —  —  —  -134.7 -88.30 -60.37 -26.93 9.43 -  —  —  —  —  202  Underdamped Modes c fd [Hz] fd [Hz] 7.40 33.29 0.678 0.088 7.20 0.389 32.37 0.042 7.23 32.04 0.041 0.090 7.35 0.069 32.00 0.035 35.14 0.095 35.35 0.077 35.06 0.075 34.32 0.077 33.91 0.077 40.68 0.032 50.00 0.234 39.76 0.002 0.473 50.00 39.96 0.019 0.092 50.00 39.77 0.005 50.00 0.188 —  —  —  —  —  —  —  —  —  —  Table 5.8: Stabilising controller horizons and closed loop characteristics for a structurally rigid two link manipulator contacting a keff = kN/m surface. Id = damped natural fre quency and damping ratio. composed of one overdamped and one underdamped mode. For all other control horizon values both of the closed ioop modes are underdamped. Figure 5.40 shows the contact force step responses in the world frame x and y directions and the control inputs (joint torques) for the case of a 2 time step control horizon and a prediction horizon of N 2  5. As can  be seen, this combination of controller horizons yields a fast, offset free step response with negligible overshoot. Reducing the prediction horizon to N 2 = 4 moves the dominant pole of the overdamped mode farther to the left on the real axis, thereby producing a somewhat faster rise time but this is accompanied by a slight overshoot due to the 35Hz mode. On the other hand, increasing the prediction horizon only results in slower rise times. 5.5.4  Force Control for Two Flexible Links  To demonstrate the performance of the multivariable predictive control algorithm for the case of flexible links we will examine the case of a two link manipulator with identical links  Chapter 5. Force Control Algorithm Performance and Evaluation  11  11  10  10  9  9  8  8  203  7 ci) C)  C  04  C)  0  6  ci cci C 0  5  0  3  4 3  2 —  0  .05  .10  .15  .20  .25  2  Response Setpoint .30  .35  —  .40  “0  .05  .0  Time [s]  (a)  .15  .20  .25  Response Setpoint .30  .35  .40  Time [SI  direction contact force  (b) y direction contact force  14 13 12 11 10  9 E z 0 1.—a C  05 -  4 3 2  —  0  .05  .10  .15  .20  .25  .30  Joint 1 Joint 2 .35  .40  Time [s]  (c) Joint torques  Figure 5.40: Force control step responses for a structurally rigid two link manipulator 2 = 5, N = 2, ) = 0. contacting a keff = lOkN/m surface with N  Chapter 5. Force Control Algorithm Performance and Evaluation  Link Dynamics Link (k) 1 8(°) 0 1.0 Lk(m) Mk(kg) 0.8451 k(kg.m 0 7J ) 2 0.2817 Tlkl(kg m ) 0.4807 2 (kg.m 0.8451 k 1 ) 2 7096 13k1(N m) k(kg.m) 4 I 0.6617 2.0 cIk1o(m) (m/m) 10 ç14  2.753  204  2 30 1.0 0.8451 0.2817 0.4807 0.8451 7096 0.6617 2.0 2.753  Contact Dynamics Direction x y k(kN/m) 20 20 20 20 ke(kN/m) 10 10 keff(kN/m) Table 5.9: Parameter values for two flexible link manipulator controller model in which the link structural dynamics are modelled by one admissible function per link. The link models are based on the same physical model of a 2.0cm diameter, l.Om long aluminium rod that was used throughout section 5.3. The nominal joint angles are 19 =  00  and 6 = 30° and the tip of the manipulator is in contact with an environment having an isotropic environment with a effective contact stiffness of keff = lOkN/m. The motion of the manipulator is in the plane perpendicular to the gravitational field. Controller Model Table 5.9 lists the numerical values of the link and contact dynamics model parameter for the system. Using these values the transfer functions given by equations (5.75) and (5.76) evaluate to: Fr(s) ui(s)  6 —36.49s —  —  1.56 x 106s4 + 1.95 x  IJ(s)I  10h1s2  + 2.18 x iO’  (5.85)  Chapter 5. Force Control Algorithm Performance and Evaluation  Fr(s)  —  _289.3s6  —  6.67 x  —  2.95 x 10’ s 2  —  205  1.18 x  1016  5 86  -  U2(S)  Fr(s) ui(s)  —  _3.26s6 + 3.47 x i0s  —  2.96 x  1010s2  + 1.26 x 1O’  5 87  -  and F(s) (s) 2 u  —  _29.85s6  —  3.60 x  106.s4  + 2.40 x  10u1s2  —  1.26 x 1O’  (5 88)  where = (1.39 x 10 8 + 88.05s6 + 1.36 x 7 s 4 10 + 4.08 x 4 s  101182  + 1.26 x  1015).  (5.89)  Figure 5.41 shows the natural frequencies of the first four modes of the system as a function of keff with keff = lOkN/m marked. At this effective contact stiffness the frequencies of the 7 h and  772  modes are in the region of approximate proportionality to kff and therefore  make significant contributions to the dynamics. The of keff and the  774  mode frequency is independent  mode frequency is just entering the transition range. The open ioop  natural frequencies of the modes are 9.39Hz, 29.7Hz, 67.5Hz and 102Hz. 250Hz (h = 4ms) to satisfy the Nyquist criterion for the  Sampling at  mode results in the following  discrete time transfer function models for the controller: F(q —1  —  —  Bi(q ) 1 ui + 2 (u B ) 1 q —i q —‘ A  (.  and FY  Bi(q)ui + 2 (q’)u —1 B q , A —i  where A(q’)  =  1— 1.48q’+0.67q 28 —0.34 3 4 + 5 — 6 + 7 , 0.54 0.67 0.34q 1.48q q q (5.92) q  Bi(q’) = —0.81 +3.76q’ —5.20q 25 + 3 +2.46(q ) 4 , (5.93) 7 —5.20q q +3.76q 6 —0.81q 1 ( 2 B ) q  —9.47+18.5q’ , 7 —17.0 + 2 + 3 ) 4 + 5 — 6 —17.0 6.79 18.5 9.47q q q q (5.94) (q q  Chapter 5. Force Control Algorithm Performance and Evaluation  206  iø  1OkN/rn.  101  2//  iOø 10_2  I  I  iiiil  I  iiiiiil 0 i0  I  1 hull  101  uuuuuul 102  lulihul  I  1111111  iø  Effective Contact Stiffness [kN/m] Figure 5.41: Variation of the natural frequencies for a two link manipulator with identical flexible (El = 574N m , one structural mode each) links assuming steady state contact 2 dynamics and an isotropic environment.  Chapter 5. Force Control Algorithm Performance and Evaluation  —0.047q’  —  2 + 0.70(q 0.87q 3+q ) 4  —  5 + 0.34q 0.87q 6  207  —  , (5.95) 7 0.047q  and (q’) 2 B  =  25 —0.78+2.91q’ —2.61q + 3 +0.36(q ) 4 —2.61q q +2.91q 6 —0.78q . (5.96) 7  These polynomials provide the initial model coefficient values for the multivariable adaptive predictive force control algorithm. They are updated at each time step by a multivariable form of the EFRA system identification algorithm. The coefficients are also used as part of the technique developed in section 3.3.1 to analyse the performance of the control algorithm by calculating the approximate closed loop characteristics. Step Response The results plots presented in the first part of this section are from a simulation using the linearised manipulator dynamics. This is done in order to minimise the mismatch be tween the controller model and the system dynamics so that attention can be focussed on the performance of the control algorithm. Later in the section results from the nonlinear dynamics simulation are shown. These results demonstrate the role that the system identi fication algorithm plays in the adaptive controller, namely, compensating for the presence of unmodelled, nonlinear components of the manipulator dynamics. Examination of the approximate closed loop poles, calculated from the control law and the linearised system dynamics, shows that with N  =  1 all prediction horizons which give  stable control result in all of the closed loop modes being underdamped. The resulting response therefore exhibits undesirable overshoot and oscillations about the setpoints. This poor control is not surprising in light of the fact that there are two dominant modes in the system. Allowing the controller only one time step in which to minimise the predicted error does not provide enough freedom to deal with two modes. Better control is obtained with N  =  2, that is, the control horizon equal to the number of dominant open loop  Chapter 5. Force Control Algorithm Performance and Evaluation  N 2 9 15 18 20 22 6 25 8 10 12  iç 2 2 2 2 2 3 3 4 4 4  Overdamped Mode Dominant Pole -91.87 -103.6 -39.86 -25.58 -17.55 -41.98 -49.95 -92.27 -78.50 -65.09  Id [Hz] 23.20 12.43 30.91 31.96 30.89 37.15 35.63 35.16 35.57 35.78  Underdamped Modes fd [Hz] C fd [Hz] C 0.675 32.74 0.222 101.2 36.51 0.889 0.174 98.99 0.447 35.51 0.297 100.1 0.187 39.02 0.423 99.63 0.165 44.43 0.351 99.44 0.040 0.045 125.0 89.77 99.62 0.008 0.002 125.0 0.010 96.12 0.012 125.0 0.029 95.24 0.014 125.0 0.016 0.010 125.0 96.50  208  C 0.055 0.052 0.060 0.044 0.048 0.290 0.847 0.342 0.391 0.466  Table 5.10: Stabilising controller horizons and closed ioop characteristics for a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface. Id = damped natural frequency and C = damping ratio. modes. The closed ioop response is composed of one overdamped and three underdamped modes. The top part of Table 5.10 shows the approximate continuous time closed ioop characteristics for various values of N . In this case the closed loop performance is largely 2 governed by the time constant of the overdamped mode and the damping ratio of the lowest frequency underdamped mode. With a 2 step control horizon, N 2 = 20 provides the response with the fastest rise time and no overshoot, as can be seen in Figures 5.42a and 5.42b. A shorter prediction horizon causes the rise time to be such that the oscillations of the underdamped modes do not completely damp out before the setpoint is reached and the response overshoots. Longer prediction horizons result in unnecessarily slow responses. With a control horizon of 3 time steps the closed system generally has four under damped modes, but for selected prediction horizons (see Table 5.10) the response has one overdamped mode and three underdamped ones. In the case of four underdamped modes one of the modes is always of very low frequency and has a relatively high damping ratio. While the step response with these characteristics will, by definition, always exhibit over shoot, if the damping ratio of the lowest frequency mode is sufficiently high the overshoot  Chapter 5. Force Control Algorithm Performance and Evaluation  209  14  12  F’  F:  a)C-) 0 U C 0  C-)  —  2  0  Response Setpoint  .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60  Time [s]  (a) a direction contact force  Response Setpoint 0  .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60  Time [s]  (b) y direction contact force  30 28 26 24 22  18  a)D  0  16 14  12 -,  10 8 6 4  2 0  .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60  Time tsl  (c) Joint torques  (d) Link tip deflections  Figure 5.42: Force control step responses for linear dynamics simulation of a two flexible link 2 = 20, (one structural mode each) manipulator contacting a keff = l0kN/m surface with N = 2, 0. N  Chapter 5. Force Control Algorithm Performance and Evaluation  210  will be negligible. As noted in section 5.3.5 the possibility of one of the closed ioop modes changing from being overdamped to being underdamped may be cause for concern regarding the robustness of the control law. With N  =  4 the controller has a control horizon equal to the total number of modes  in the open loop system. Table 5.10 indicates that the lowest frequency closed ioop mode which dominates the response when N  2 has been completely removed and a light  =  damped 125Hz mode (half the sampling frequency) has appeared. The dominant pole of the overdamped mode is generally more negative than is the case with Nu < 4, indicating that the rise time is decreased. Figure 5.43 shows the step responses for the case of N 2 and N  =  4. The faster response of the N  4 controller compared to the N  =  =  8  2 is  the result of a much more active control input signal as can be seen by comparing Figures 5.42c and 5.43c. Increasing the prediction horizon from 8 time steps generally increases the damping of the closed ioop modes, thereby reducing the ripple in the response but only at the expense of increased rise times. The step responses examined so far have concentrated on the case of simultaneous changes in the x and y direction contact force setpoints. Another important aspect of the closed ioop performance, given the strongly coupled nature of the system, is the interaction of the outputs. This aspect is illustrated by commanding a step change in one output while holding the other setpoint constant. Figure 5.44 shows the responses under these conditions that results from controller horizons of N 2 2 corresponding results with N  =  8 and N  =  =  20 and N 4.  =  2 and Figure 5.45 shows the  These figures show that the disturbance  of one component of the contact force which occurs in response to a step change in the setpoint of the other component is of much larger magnitude and longer duration when the N  =  2 controller is used rather than N  =  4. The difference in duration of the disturbance  is due in part to the anticipation characteristic of predictive control and results from the difference in prediction horizons. The difference in magnitude is due to the difference in  Chapter 5. Force Control Algorithm Performance and Evaluation  211  16  14  12  z ci) C) 0 LL C) Cu C  0  C.)  10 ci) C) 0  8  IL  C)  (8 C 0  6  0  4  —  2  .05  .10  Response Setpoint  .15 .20 .25 .30 .35 .40 .45 .50 .55.  Time [s]  (a)  direction contact force  —  Response Setpoint  35 .40 .45 .50 .55  Time [s]  (b) y direction contact force  32  30 28 26  24  18 D  216 1214 C 0 —,  10 8 6 4 2  0  0  .05 .10 .15 .20  .25  .30  .35 .40 .45 .50 .55 .60  Time [s]  (c) Joint torques  Time [s]  (d) Link tip deflections  Figure 5.43: Force control step responses for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2 = 8, = 4, ) = 0. N  Chapter 5. Force Control Algorithm Performance and Evaluation  212  16  14  12  2’ C) 0 U  C 0  C.)  10  II  ci) C) 0  8  Ll  6  C) cci C 0  C.)  4  Response Setpoint  2  0  .1  .2  .3  .4  .5  .6  .7  Time [s]  (a)  direction contact force  .8  .9  —  -  1.0  0  .1  .2  .3  .4  .5  .6  Response Setpoint  .7  .8  .9  1.0  Time [s]  (b) y direction contact force  34 32 30 28 26 24  E  22  -20 1)  D 18  0  16 C  14  O 12  -3  10  a 6  4 2 0  Time [s]  (c) Joint torques  Time [s]  (d) Link tip deflections  Figure 5.44: Force control step responses showing output interactions for linear dynam ics simulation of a two flexible link (one structural mode each) manipulator contacting a 0. 2 = 20, N 2, keff = lOkN/m surface with N  Chapter 5. Force Control Algorithm Performance and Evaluation  213  16 20  14  18 12  G)  16 14  10  o  0 12  LI  8  o  6  10  o  E 0  0 6  4  -.  I  .1  .2  .3  .4  .5  .6  4  Response Setpoint  2  0  1’  .7  .8  .9  —  2  1.0  -  .1  .2  .3  .4  Time [s]  (a)  .5  .6  Response Setpoint  .7  .8  .9  1.0  .8  .9  1.0  Time [s]  direction contact force  (b) y direction contact force  40  35  30  25  20 0 0  -)  10  5  .1  .2  .3  .4  .5  .6  Time [s]  (c) Joint torques  .1  .2  .3  .4  .5  .6  .7  Time [s]  (d) Link tip deflections  Figure 5.45: Force control step responses showing output interactions for linear dynam ics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with N 2 = 8, N 4, 0.  Chapter 5. Force Control Algorithm Performance and Evaluation  214  the lowest closed ioop natural frequencies and the degree to which they are excited. The disturbance of the y direction contact force due to a step change in the x direction is less significant than when the situation is reversed but the trends of magnitude and duration of the disturbance are comparable. The generally better performance in the y direction shown in all of the above figures is due primarily to the 8  0°, 0  30° configuration of the  manipulator. All of the preceding simulation results in this section are based on a simulation of the linearised equations of motion. Under those circumstances the controller model and the system dynamics are exactly matched. When the full, nonlinear dynamics of the system are considered the situation is somewhat more complicated. The nonlinearities represent, from the point of view of the controller, unmodelled dynamics. The controller is able to cope with the unmodelled dynamics due to its explicitly adaptive implementation, that is, at every time step the system identification algorithm calculates a new estimate of the coefficients of the controller model. These coefficients are presumed to accurately represent the instantaneous system dynamics in a linear form and are used to calculate the control input for the next time step. Figure 5.46 shows a series of step responses from the nonlinear dynamics simulation for the same manipulator and environment configuration used above. The controller has a prediction horizon of N 2  =  20 time steps and a control horizon of N  =  2  steps. During the early part of the response the mismatch between the controller and system dynamics results in strong excitation of the high frequency modes. The high frequency structural vibrations of the links are evident in the plot of the local defiections of the link tips shown in Figure 5.46d. These vibrations are also evident in the plots of the contact force components (Figures 5.46a and 5.46b). In order to control the high frequency modes the control inputs are forced to switch rapidly over relatively large amplitudes (Figure 5.46c). While this high frequency activity results in a relatively poor response with a great deal of overshoot it also provides rich excitation for the system identification algorithm with the  Chapter 5. Force Control Algorithm Performance and Evaluation  215  20 18 16  a) o  o  14  11  12 10  C  o  o  Q) 0  o  U  (a C 0  8  C)  6 4 —  2 0  0  .2  .4  .6  .8  1.0  1.2  Response Setpoint 1.4  Time [SI  (a) a direction contact force  1,6  —  1.8  0  .2  .4  .6  .8  1.0  1.2  Response Setpoint 1.4  1.6  1.8  Time Is]  (b) y direction contact force  45 40  35 3Ø  z a)  25  0’  S 1 0 —,  20  15 10  Time [s]  (c) Joint torques  (d) Link tip deflections  Figure 5.46: Force control step responses for nonlinear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/m surface with =2O,N=2,)=O. 2 N  Chapter 5. Force Control Algorithm Performance and Evaluation  216  result that after 0.8s (200 time steps) the controller model coefficients have been sufficiently well adjusted to compensate for the unmodelled nonlinear dynamics and the subsequent step responses are much improved. It is not surprising that the effect of the unmodelled nonlinear dynamics is much more pronounced in the case of the two link manipulator than was observed in the single link results when the structure of the equations of motion for the two cases are compared. The number of centripetal and Coriolis terms in the two link equations of motion far exceeds those in the single link equations. In particular the two link equations have a large number of terms which represent interactions between the joint angle and modal amplifier generalised coordinates both for a given link and between one link and the other. The results shown in Figure 5.46 demonstrate the limitations of the application of a strictly linear controller to a highly nonlinear system. Several possibilities for improvement of the closed loop performance can be considered. The simplest is filtering of the high fre quency component of the response which would reduce the amplitude of the disturbances but would likely also increase the time required for the identification algorithm to arrive at a suitable model. A more fruitful approach might be to model the disturbances due to the nonlinear unmodelled dynamics as coloured noise and include the noise model in the controller model [32]. In either case the effect of the nonlinear dynamics can be substan tially reduced by specifying setpoint programs which would produce less excitation of the structural dynamics modes that the step changes shown in Figure 5.46. Ramps or higher order polynomials would provide smoother transitions between setpoint levels and therefore reduce the level of excitation. Reformulation of the predictive control algorithm on the basis of a nonlinear model (for example [90]) could also be considered.  Chapter 5. Force Control Algorithm Performance and Evaluation  5.5.5  217  Summary  In summary, this section has presented an analysis of force controlled two link manipulators with rigid or flexible and an evaluation of the performance of the multivariable, adaptive, predictive control algorithm applied to the control of world frame contact force components exerted by the manipulator tip on environment surfaces. Linear analysis of the case of infinitely stiff links reveal the expected extension of char acteristics which were identified in the single link case while at the same time illustrating the added complexities of the two link system. The modal frequencies are found to vary with k!ff for a given set of link lengths and inertias and the two highest frequency modes are due to the local vibrations of the contact region mass. The frequencies are independent of é1  when the contact is isotropic (kxeff =  900.  =  kyeff) and are bounded above by the values for  The contributions of the natural modes to the contact force components are also  found to vary with the nominal manipulator geometry. With the addition of modes due to the structural flexibility of the links, linear analysis reveals a transfer of dominance among the modes similar to that seen in the single link case, but with the expected additional dependence on joint angles. A transfer function model of the system which relates the world frame x and y direction components of the contact force to the joint inputs was derived on the basis of the linearised equations of motion. Specific parameter values were selected for a manipulator with two identical links in contact with a relatively stiff (keff  =  lOkN/m) isotropic (k  =  k) environ  ment. The links were based on the physical model of 1.Om long, 2.0cm diameter aluminium rods. Cases in which the link are assumed rigid as well as having their structural dynamic modelled using the first mode shape function of the rods considered as a cantilever beams have been addressed.  Chapter 5. Force Control Algorithm Performance and Evaluation  218  As in the single link case, the approximate closed ioop characteristics calculated from the control law were found to be very useful in selecting controller horizons which produced the fastest rise times and least overshoots from among the large set of horizons which stabilise the system.  5.6  Force Control for DC Motor Actuated Space Station Manipulator  As a final demonstration of the performance of the multivariable predictive force control algorithm we will examine the case of a simple prototype model of the two long links of the proposed Mobile Servicing System (MSS) for the Space Station Freedom. These massive, long, slender links are driven by geared direct current motors at their joints. The physical parameter values for the system are listed in Table 5.11. The top part of the table shows the link model parameter values which are based largely on those used by Chan [68]. The MSS manipulator operates in a microgravity environment in which the gravitational field is approximately balanced by the orbital centripetal acceleration and thus the gravitational loading of the links is neglected in the model. The center section of the table shows the parameters used to model the DC motors. Since the motor parameters are not publically available the values shown are estimates based on assumptions of a maximum motor power of 400W and a maximum torque delivery of 1500N m. Due to the mass of the links, the natural frequencies of the cantilever beam modes used as admissible functions to model the link structural dynamics are much lower than in the previously considered cases. In fact the frequency of the first mode is only about 0.3Hz. Even when the tip of the second link is in contact with a surface with very high effective contact stiffness the natural frequencies are all less than 30Hz (see Figure 5.47). The controller model structure is similar to that derived in section 5.5.2. The control inputs are the input voltages to the motors. These voltages are amplified to produce the  Chapter 5. Force Control Algorithm Performance and Evaluation  Link Dynamics Link (k) 1 0 (°) Lk(m) 7.5 Mk(kg) 1600 IOk(kg m ) 2 3.0 x i0 Ilkl(kg m) 6826 (kg.m) k 2 1 1600 13k1(N.m) 6388 4 ( 1 C kg.m) 1 1253 2.0 4k1o(m) 0.3671 qYklO(m/m) .  2 30 7.5 1600 3.0 x iO 6826 1600 6388 1253 2.0 0.3671  Actuator Dynamics Joint (k) 1 Iak(kg m ) 2 0.5 Ngk () 50. 1.0 Rak(Q) 1.5 Kk(N rn/A) 0.5 Kem(V s/rad) 4.0 K(V/V) 20. 2akmax(A) 5.0 Vznkmax(V)  2 0.5 50. 1.0 1.5 0.5 4.0 20. 5.0  Contact Dynamics Direction x 40 k(kN/m) 40 ke(kN/m) 20 keff(kN/m)  y 40 40 20  .  219  Table 5.11: Physical parameter values for a two link MSS model with DC motor actuators.  Chapter 5. Force Control Algorithm Performance and Evaluation  10  220  2 I  I  1111111  I  I  1111111  I  11111111  11111111  I  I  1111111  I  I  I  101  10  D  I  -  z  I  10  I -  11,  1  _2  i  ii  10_i  I  I  I ii  I  I  1&  I  I I  ii  102  I  II  I  iø  Effective Contact Stiffness [kN/m] Figure 5.47: Variation of the modal natural frequencies for DC motor actuated two link MSS model.  Chapter 5. Force Control Algorithm Performance and Evaluation  221  armature circuit voltages which in turn produce the armature currents to which the torques supplied by the motors are proportional. Thus, the discrete time transfer function models for the controller are of the form: F(q —1 )  Bi(q ) 1 vi + 2 (v B ) 1 q _.i q = ) 1 A(q  (5.97)  and FY  —  —  Bi(q ) 1 vi + 2 (q’)v —1 B q ) 1 A(q  5 98  With the manipulator in contact with a surface such that the effective contact stiffness is 2OkN/m, the open loop natural frequencies of the first four modes of the system are 0.30Hz, 0.90Hz, 1.5Hz and 2.4Hz. As can be seen in Figure 5.47, the  mode is, at keff = 2OkN/m,  in transition between being independent of lCeff and being proportional to kff and therefore contributes significantly to the contact force. To satisfy the Nyquist criterion for the mode a sampling frequency of 7.7Hz (h = l3ms) is used. Figure 5.48 shows the contact force responses from the linearised dynamics model of the MSS controlled by the adaptive, multivariable predictive control algorithm with N 2 = 14, N = 2, k = 0 and .Xse = 0. Note that the time scale of the figures in this section is much longer than that of the previously shown results owing to the much greater mass of the MSS links compared to aluminium rod links used in the previous cases. The y direction contact force setpoint in Figure 5.48 consists of a smooth transition between force levels. The transition is constructed by matching exponential functions of the form et 1  —  et at the midpoint of the transition. The  —  1 and  direction goal is to maintain a constant  force level. Figure 5.49 shows the responses of the full nonlinear dynamics model with the same setpoints and controller parameters. These results illustrate the improved per formance for the nonlinear case (compared to Figure 5.46) which is obtained by using a smooth setpoint transition as opposed to a step change. The relatively small magnitude of the level change also contributes to the improved performance. Both of these factors result  Chapter 5. Force Control Algorithm Performance and Evaluation  5.5  .4.5  5.0  4.0  /7 I  4.5 4.0  1/  z a)0  222  3.0  0  3.0 2.5  U 0  2.5  (a  2.0  C 0  C 0  0 2.0  0  1.5  1.5  r—  1 .0 .5  1 .0  Response  —  .5  . ..---.  .1,1  2  4  6  8  0  10 12 14 16 18 20 22 24 26 28 30  0  2  4  6  8  Time [s]  1,1.  .1,  I.  Response Setpoint .1.1.1.  10 12 14 16 18 20 22 24 26 28 30  Time [s]  (b)  (a) n direction contact force  y  direction contact force  E E C  0 C)  0  a)  0.  I  >  0.  C -J  -  06  0  2  4  6  8  10 12 14 16 18 20 22 24 26 28 30 Time [s]  (c) Motor input voltages  2  4  6  8  10 12 14 16 18 20 22 24 26 28 30  Time [s]  (d) Link tip deflections  Figure 5.48: Force control responses for linear dynamics simulation of a two flexible link Space Station MSS contacting a keff = 2OkN/m surface.  Chapter 5. Force Control Algorithm Performance and Evaluation  5.0  .  5.5  223  ...  .  4.5  5.0 4.0 4.5 3.5  a)0  3.5 0  3.0  0  Li_ C) a) C  U  2.5 0 C.) 2.0  0  2.5 2.0  0  1.5 1.0  — Response Setpoint  .5  r— L-  .5  -----  .  .1  0  2  .1.1..  .1.  4  6  8  .1.1.1.  0  10 12 14 16 18 20 22 24 28 28 30  .1.  0  4  2  6  4  .1.1.  Time [s]  z direction contact force  (b) y direction contact force 1.5  •  .35  .1.1.  10 12 14 16 18 20 22 24 26 28 30  8  Time [s]  ( a)  Response Setpoint  —  ---  .30  Joint 1 Joint 2  1  ii  I L  1 .0  —  —  —  ...  -  -  link 1 link 2  -  a  .25  E .20  .  -.5  C  .2 -1.0  .15  (a  .10 > .05 .,j I’..  C  —.  0  —  .  -  -  05  .10 .15  .  20  0  2  4  6  8  10121416182022242628 30  Time [s]  (c) Motor input voltages  14 16 18 20 22 24 26 28 30  Time [s]  (d) Link tip deflections  Figure 5.49: Force control responses for nonlinear dynamics simulation of a two flexible link Space Station MSS contacting a keff = 2OkN/m surface.  Chapter 5. Force Control Algorithm Performance and Evaluation  224  in less excitation of the nonlinear dynamics of the system which are not included in the con troller model. While the improvement in the system performance is encouraging, additional filtering and/or reformulation of the control algorithm, as discussed in section 5.5.4, would be necessary to meet the expected operational performance specifications for the MSS. 5.7  Summary of Force Control Performance  The simulation results in this chapter have demonstrated the viability of the use of a multivariable, explicitly adaptive, long range predictive control algorithm to control the contact forces exerted by manipulators on surfaces in their working environment. The following configurations were considered: • a single flexible link with its structural dynamics modelled by one, two and three ad missible mode shape function in contact with surfaces with effective contact stiffnesses ranging from 3.1kN/n to 5MN/m; • the same link, initially under motion control, making unexpected contact at an impact velocity of approximately O.5m/s with a surface; • a planar, two link manipulator with rigid links in contact with an isotropic effective contact stiffness of lOkN/m; • the same two link manipulator with the structural flexibility of the link modelled by one admissible mode shape function for each link, also making contact with a surface such that the isotropic effective contact stiffness is keff = lOkN/m; and; • a prototype model of the two long links of the DC motor actuated Mobile Servicing System (MSS) manipulator for the proposed Freedom Space Station, with its tip in contact with a keff = 2OkN/m surface.  Chapter 5. Force Control Algorithm Performance and Evaluation  225  The single flexible link step response results demonstrate that with a controller model of appropriate order and a sampling rate selected to resolve the dominant mode, as indicated by the shape of a frequency versus effective contact stiffness plot, there are generally a wide range of control algorithm horizon and weighting factor values for which the closed ioop will be stable. The inherent integral action of the control algorithm guarantees that the response will not have a steady-state offset. A new formulation for the discrete time domain closed ioop characteristic polynomial has been used to select control algorithm horizons and weighting factors to satisfy additional performance requirements such as minimal overshoot and rise time. The inclusion of a static equilibrium bias term in the controller has been shown to decrease rise time at the expense of introducing a slight amount of overshoot into the response. The importance of selecting a reasonably accurate initial controller model implies that knowledge of the structural dynamics of the manipulator and the contact characteristics of the surfaces likely to be encountered in the workspace is necessary to ensure successful control. The problem of maintaining control while unexpectedly making contact with a surface is a challenging one because the complex dynamics of the system are compounded by the presence of a discontinuity. To cope with this problem a contact control logic level has been introduced into the manipulator control hierarchy. Two operational strategies for the contact control logic have been demonstrated and shown to allow control to be maintained through contact events, albeit with some unavoidable contact force transients. Of par ticular interest is the fact that under the setpoint modification strategy there is a direct relationship between the impact velocity and the magnitude of the initial force transient. This relationship allows manipulator motions, in particular tip velocities, to be planned on the basis of the transient contact force levels to which nearby surfaces can be acceptably subjected, should a collision occur.  Chapter 5. Force Control Algorithm Performance and Evaluation  226  The step response results for the case of a two link manipulator with a rigid structure show that the controller has the potential to provide excellent force control performance for this large subset of existing industrial manipulators. In this context, the predictive control algorithm could be readily incorporated into a combined force and motion control structure, such as the hybrid methodology of Raibert and Craig [12]. This case is also of interest as the limiting situation of infinite structural stiffness because it offers validation of some of the fundamental conclusions of the linear analysis developed in this chapter. With the inclusion of structural flexibility the dynamics of the two link manipulator become markedly more complex and nonlinear. The results demonstrate that the insight gained from the linear analysis of the system and the calculation of the roots of the close loop characteristic polynomial offer guidance in this case, as in the less complex cases, in the construction of the controller model and the selection of control algorithm horizons and weighting factors to provide the most desirable response. The simulations also show that the strongly coupled structure of the manipulator dynamics, combined with the optimal (in the sense of minimising a cost function based on predicted output errors) form of the control law, make total isolation of the controlled quantities impossible. That is, the response to a step change commanded for one component of contact force will result in some disturbance of the other component. The highly nonlinear nature of the two flexible link manipulator dynamics presents some difficulty for the inherently linear control algorithm. The unmodelled, nonlinear dynamics are compensated for, to some extent, by the adaptive structure of the controller but it has been found that it is possible to excite the unmodelled dynamics to such an extent that stability is lost. Several potential remedies to this problem are possible including filtering of the contact force signals, using smooth setpoint transitions instead of steps, inclusion of a coloured noise model in the control algorithm formulation, and rederivation of the predictive control algorithm on the basis of a nonlinear model.  Chapter 5. Force Control Algorithm Performance and Evaluation  rpIle  227  force control simulations for the Space Station Mobile Servicing System (MSS) ma  nipulator model demonstrate the effectiveness of the controller with a manipulator having radically different dynamic parameters from those considered previously. The model in cludes voltage controlled DC electric motor actuators which drive the joints through gear boxes. The force control results show that the use of smooth setpoint transitions attenuates, to some extent, the difficulties caused by unmodelled, nonlinear dynamics.  Chapter 6  Summary of Major Results  This thesis has developed and demonstrated an overall strategy for the control of contact forces exerted by a structurally flexible manipulator on surfaces in its working environ ment. The control strategy is centered on an explicitly adaptive implementation of a pre dictive control algorithm based on the Generalised Predictive Control algorithm of Clarke, et al. [29]. The thesis also presents analyses of the linearised equations governing the manip ulator/environment system in several configurations, using a combination of analytic and numerical techniques. The results of these analyses provide insight into the fundamental dynamics which dominate the force controlled system. These insights are used to guide the selection of the appropriate controller model and parameters for particular configurations. The major contributions of the research reported in this thesis are: • Kinematic and dynamic models of flexible link manipulator interacting with surfaces in its working environment has been developed. The model includes the effects of local inertia, energy dissipation and surface stiffnesses of the contacting surfaces of the manipulator tip and the environment. Analysis shows that when the mass of the surface regions deflected due to contact is comparable to the inertias of the links the inertia effects in the contact model become important. • Linear analyses of various configurations of manipulators in contact with environment surfaces have been carried out using a combination of analytic and numerical tech niques. The results of these analyses provide new insights, not apparently available  228  Chapter 6. Summary of Major Results  229  in the literature, into the fundamental dynamics of the system using established lin ear analysis methods. In particular, they reveal that the contact force response is dominated by different open loop modes, depending on the effective stiffness of the contacting surfaces and that the transfer of dominance among the modes follows an orderly progression from lower to higher frequency modes as the effective contact stiff ness is increased in a given configuration. The analyses also show that the control inputs tend to provide excitation to all but the very high frequency contact vibration modes. The implications of these two results for the design of discrete time domain contact force controllers are that the loop must be closed at a sampling frequency suf ficient to resolve the dominant mode in the contact force response and the controller model must include the dynamics of all of the excited modes. The linear analyses provide guidance in determination of the dominant and significantly excited modes for a particular manipulator configuration. • A new general formulation for the closed loop characteristic polynomial of a system controlled by the Ceneralised Predictive Control (GPC) algorithm has been obtained in the discrete time domain. The formulation is applicable to both the original single input, single output form of the algorithm and to its multivariable extension. Cal culation of the approximate closed loop characteristics of the system from this result has been shown to be useful in tuning the control law for particular configurations. • The predictive control algorithm has been extended with a static equilibrium bias term in the cost function. This extension is applicable to both SISO and multivariable forms of the algorithm. The addition of this new term augments the feedback control law with a proportional action feedforward component. The static equilibrium bias term is particularly applicable in the contact force control application because of the  Chapter 6. Summary of Major Results  230  easily defined static equilibrium relationship between joint torques and contact force components. • Contact control logic is introduced as a first step in the integration of the predictive control algorithm into an overall force/motion control strategy. In the form presented, the primary function of the contact control logic is to deal with the discontinuity involved in making and breaking contact and collisions between the manipulator tip and the environment surface due to unexpected contact events. • Detailed simulation results for one and two flexible link manipulators have been pre sented. The results demonstrate that the predictive control algorithm can be tuned to provide fast step responses with minimal overshoot for a wide variety of manipu lator configurations and contact stiffnesses. The importance of the effective contact stiffness in the dynamic structure of the system indicates the need for some a priori knowledge of the surfaces a given manipulator is likely to contact during particular operations. • Two operational strategies for the contact control logic have been demonstrated in the situation of a collision due to an unexpected contact event. The setpoint modifi cation strategy was found to be superior to the stop motion strategy, which tends to induce bouncing on the surface. In the setpoint modification strategy joint and force sensor data around the instant of contact are used to predict the characteristics of the expected force transient and the contact force setpoint is then briefly increased to allow the controller to supply more energy to overcome the unexpected force transient due to the kinetic energy transferred from the link to the surface. • The explicitly adaptive implementation of the predictive force control algorithm using the Exponential Forgetting and Resetting Algorithm (EFRA) for system identification  Chapter 6. Summary of Major Results  231  has been found well capable of compensating for unmodelled structural modes in the single link case. The unmodelled nonlinear dynamics in the two link case present a greater difficulty which significantly limits the performance of the controller. 6.1  Recommendations for Further Research  The following directions for future research stemming from the results of this thesis are recommended: • integration of the predictive control algorithm for force and motion control of flexible link manipulators into the framework for simultaneous force and motion control such as the hybrid position/force control framework of Raibert and Craig [12]; • extension of the contact control logic concept to deal with a wider range of making and breaking contact events; this may be a suitable application for fuzzy control; • improvement of the force control algorithm in situations where significant unmodelled nonlinear dynamics exist; possible actions are simple filtering, inclusion of a coloured noise model in the controller model, selection of setpoint programs less likely to excite the nonlinear dynamics, and reformulation of the predictive controller on the bassis of a nonlinear model. Implementation Issues Early in the course of the research force control experiments were conducted in which a version of the damping algorithm introduced by Whitney [3] was implemented on a Puma 560 robot equipped with a JR 3 force sensor. Edge following and peg—in—hole operations were successfully demonstrated. Unfortunately, the closed architecture of the Puma controller, its relatively slow ioop closing time (‘-. 28ms), and the unsuitability of the interpreted Val  Chapter 6. Summary of Major Results  232  II controller programming language for extensive mathematical computations precluded the implementation of a more complex predictive force control algorithm. The experiments with the Puma 560 robot and the JR 3 force sensor demonstrated two characteristics of the force sensor which can pose difficulties.  Firstly, under some  circumstances the raw sensor signal was found to be quite noisy. The JR 3 sensor firmware includes digital filtering capabilities which allowed a much cleaner signal to be obtained without appreciable loss of accuracy for the operations undertaken. The second problem was the tendency of the sensor to measure inertial forces when the manipulator was executing large motions. This problem was overcome by only activating the force control algorithm when contact was imminent and by establishing a deadband around zero force in the control algorithm. With more powerful computational hardware this problem could perhaps be reduced by calculating an estimate of the inertia force at the sensor and applying it to the sensor signal as a bias. The analytical and simulation results presented in this thesis serve to establish the viabil ity of the adaptive, multivariable predictive algorithm and contact control logic for contact force control applications. Several considerations which arise from these results will have to be addressed in the course of future experimental and eventual practical implementations. The selection of the appropriate sampling rate for the discrete time controller has been discussed throughout the simulation results sections. It has been shown that the necessary sampling rate as well as the structure of the controller model and the initial values of the coefficients of that model are closely related to the dynamic characteristics of the manipu lator links and the effective stiffness and inertia of the contact region that exists between the manipulator tip and the environment. Knowledge of the link dynamics can be obtained by spectral analysis of their structural vibrations. The contact characteristics which are re quired are the effective values for the particular end-effector in contact with the particular environment surface. These values will account for the effects of all sources of compliance  Chapter 6. Summary of Major Results  233  which are relevant to the contact operation. In addition to the local characteristics of the contacting surfaces, sources of compliance might include the presence of discrete compliance devices such as a Remote Compliance Centre (RCC) at the manipulator wrist, force sensor compliance, manipulator joint compliance, global deformation of the environment surface, etc. A procedure such as that used by An, et al. [14] to identify the effective contact stiffness using the manipulator sensors would seem to be the best available approach to obtaining values for the effective contact characteristics. The need to incorporate the characteristics of particular end-effector and environment surfaces in the controller model suggests that, for operation in a relatively structured workspace, a “model scheduling” strategy might improve the performance of the controller in comparison to depending entirely on the system identification algorithm to adjust the model coefficients when a different end-effector is used or a different surface contacted. Such model scheduling could easily be incorporated in the contact control logic. The computational complexity of the control and identification algorithms governs the type and speed of processing hardware required for successful implementation. The calcu lations required in the contact control logic and the predictive part of the control algorithm are relatively simple. However, the calculation of the control input increments from the results of the predictor calculations is somewhat more taxing because it involves the cal culation of a matrix pseudo-inverse, the order of which increases with the control horizon (Na) and the number of inputs and outputs. The identification algorithm must be executed independently for each system output at each time step and thus lends itself to parallel processing. The versions of the control and identification algorithms coded in TWOFLEX execute at approximately 20% to 50% of the speed required for real time operation on a SparcStation 2. The routines are coded in FORTRAN and were written with emphasis on making them understandable and maintainable, not necessarily time efficient. Rewriting of  Chapter 6. Summary of Major Results  234  the routines in a language and form intended for real time operation should allow successful implementation using existing hardware.  Bibliography  [1] I. Winton. Breathing life into the SPDM. in Proceedings of 6th CASI Conference: Symposium on Space Station. Canadian Aeronautics and Space Institute, November 1989. [2] D.E. Whitney. Historical perspective and state of the art in robot force control. Inter national Journal of Robotics Research, 6(1):3—14, 1987. [3] D.E. Whitney. Force feedback control of manipulator fine motions. ASME Journal of Dynamic Systems, Measurement and Control, 98:91—97, June 1977.  [41  J.K. Salisbury. Active stiffness control of a manipulator in cartesian coordinates. In Proceedings of the 19th IEEE Conference on Decision and Control, pages 95—100, De cember 1980.  [5] N. Hogan. Impedance control: An approach to manipulation. Part I Theory. ASME Dynamic Journal of Systems, Measurement and Control, 107:1—7, March 1985. —  [6] N. Hogan. Impedance control: An approach to manipulation. Part II Implementa tion. ASME Journal of Dynamic Systems, Measurement and Control, 107:8—16, March 1985. —  [7] N. Hogan. Impedance control: An approach to manipulation. Part III Applications. ASME Journal of Dynamic Systems, Measurement and Control, 107:17—24, March 1985. —  [8] M.T. Mason. Compliance and force control for computer controlled manipulators. IEEE Tranactions on Systems, Man and Cybernetics, SMC-11:418—432, June 1981. [9] C. Wu. Compliance control of a robot manipulator based on joint torque servo. Inter national Journal of Robotics Research, 4(3):55—71, 1985. [10] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipula tors, Part I: The fundamental concepts of compliant motion. IEEE Journal of Robotics and Automation, RA-2:83—92, 1986. [11] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipula tors, Part II: Design method. IEEE Journal of Robotics and Automation, RA-2:93—105, 1986.  235  Bibliography  236  [12] M.H. Raibert and J.J.. Craig. Hybrid position/force control of manipulators. ASME Journal of Dynamic Systems, Measurement and Control, 102:126—133, June 1981. [13] J.K. Salisbury and J.J. Craig. Articulated hands: Force control and kinematic issues. International Journal of Robotics Research, 1(1):4—17, 1982. [14] C.H. An, C.G. Atkenson, and J.M. Hollerbach. Model-Based Control of a Robot Ma nipulator. The MIT Press Series in Artificial Intelligence. The MIT Press, Cambridge, Massachusetts, 1988. [15] 0. Khatib. A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation, RA 3:43—53, 1987. [16] C.H. An and J.M. Hollerbach. The role of dynamic models in cartesian force control of manipulators. International Journal of Robotics Research, 8(4):51—72, August 1989.  [171 Uchiyama:1989. Control of robot arms. JSME International Journal, Series III, 32(1):1—9, 1989. [18] M.W. Spong. Force feedback control of flexible joint manipulators. In Proceedings of the ASME Winter Annual Meeting, pages 177—183, 1987. [19] M.W. Spong. On the force control porblem for flexible joint manipulators. Transactions on Automatic Control, 34(1):107—111, January 1989.  IEEE  [20] S.W. Tilley, RH. Cannon, Jr., and R. Kraft. End point force control of a very flexible manipulator with a fast end effector. In Proceedings of the ASME Winter Annual Meeting, 1986.  [21] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a one—link force— controlled flexible manipulator. Journal of Robotic Systems, 5(5):443—451, 1988. [22] D.J. Latornell and D.B. Cherchas. Force control for a flexible manipulator link using generalized predictive control. In M. Jamshidi and M. Saif, editors, Robotics and Manufacturing Recent Trends in Research, Education and Applications, volume 3, pages 569—576, New York, 1990. ASME Press. —  [23] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a two—link force— controlled flexible manipulator. ASME Journal of Dynamic Systems, Measurement and Control, 112:661—666, December 1990. [24] F.G. Pfeiffer. Path and force control of elastic manipulators. In Proceedings of the IEEE Conference on Decision and Control, pages 514—519, 1990.  Bibliography  237  [25] F. Matsuno, Y. Sakawa, and T. Asano. Quasi-static hybrid position/force control of a flexible manipulator. In Proceedings of the IEEE Conference on Robotics and Automation, pages 2838—2843, April 1991. [26] D. Li. Tip—contact force control of one—link flexible manipulator: An inherent perfor mance limitation. In Proceedings of the American Control Conference, pages 697—701, 1990. [27] D.J. Latornell and Cherchas D.B. Force and motion control of a single flexible manip ulator link. Robotics é4 Computer—Integrated Manufacturing, 9(2):87—99, April 1992.  [28] A.P. Tzes and S. Yurkovich. Flexible—link manipulator force control. In Proceedings of the American Control Conference, pages 194—199, 1990. [29] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control The basic algorithm. Automatica, 23(2):137—148, 1987.  —  Part I.  [30] J. Denavit and R.S. Hartenberg. A kinematic notation for lower-pair mechanisms based on matrices. ASME Journal of Applied Mechanics, pages 215—221, June 1955. [31] W.J. Book. Recursive lagrangian dynamics of flexible manipulator arms. International Journal of Robotics Research, 3(3):87—101, 1984. [32] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control Extensions and interpretations. Automatica, 23(2): 149—i 60, 1987.  —  Part II.  [33] D.W. Clarke and C. Mohtadi. Properties of generalized predictive control. Automatica, 25(6):859—875, 1989. [34] D.J. Latornell. Source listings for the TWOFLEX program A dynamics and control simulation for structurally flexible robotics manipulators making contact with their environment. Report CAMROL R.92-2, University of British Columbia, Department of Mechanical Engineering, Computer Aided Manufacturing and Robotics Laboratory (CAMROL), September 1992. —  [35] H.W. Mah. On the Nonlinear Dynamics of a Space Platform Based Mobile Manip ulator. PhD thesis, Department of Mechanical Engineering, University of British Columbia, October 1992. [36] R.P. Paul. Robotic Manipulators: Mathematics, Programming and Control. The MIT Press Series in Artificial Intelligence. The MIT Press, Cambridge, Massachusetts, 1981. [37] K.S. Fu, R.C. Gonzolez, and C.S.G. Lee. Robotics: Control, Sensing, Vision, and Intelligence. McGraw-Hill Inc., New York, NY, 1987.  Bibliography  238  [38] J.J. Craig. Introduction to Robotics. Addison-Wesley, Reading, Massachusetts, 1986. [39] W.J. Book. Analysis of massless elastic chains with servo controlled joints. ASME Journal of Dynamic Systems, Measurement and Control, 110(3):187—192, 1979. [40] Liang-Wey Chang and J.F. Hamilton. The kinematics of robotic manipulators with flexible links using and equivalent rigid link system (ERLS) model. ASME Journal of Dynamic Systems, Measurement and Control, 113:48—53, March 1991. [41] L. Meirovitch. Computational Methods in Structural Dynamics. Sijthoff and Noordhoff, Alphen aan den Rijn, The Netherlands, 1980. [42] N.G. Chalhoub and A.G. Ulsoy. Dynamic simulation of a leadscrew driven flexible robot arm and controller. ASME Journal of Dynamic Systems, Measurement and Control, 108:119—126, June 1986. [43] G.G. Hastings and W.J. Book. A linear dynamic model for flexible robotic manipula tors. IEEE Control Systems Magazine, pages 61—64, February 1987. [44] M. Benati and A. Morro. Dynamics of chain of flexible links. ASME Journal of Dynamic Systems, Measurement and Control, 110:410—415, December 1988.  [451  E. Barbieri and U. Ozguner. Unconstrained and constrained mode expansions for a flexible slewing link. ASME Journal of Dynamic Systems, Measurement and Control, 110:416—421, December 1988.  [46] G. Naganathan and A.H. Soni. Coupling effects of kinematics and flexibility in manip ulators. International Journal of Robotics Research, 6(1):75—84, 1987. [47] J.D. Lee and B.-L. Wang. Dynamic equations for a two—link flexible robot arm. Com puters and Structures, 29(3):469—477, 1988. [48] Liang-Wey Chang and J.F. Hamilton. Dynamics of robotic manipulators with flexi ble links. ASME Journal of Dynamic Systems, Measurement and Control, 113:54—59, March 1991. [49] S. Cetinkunt and Wen-Lung Yu. Closed—loop behaviour of a feedback—controlled flexi ble arm: A comparative study. International Journal of Robotics Research, 10(3):263— 275, June 1991. [50] P.A. Blelloch and K.S. Carney. Modal selection in structural dynamics. In Proceedings of the 7th International Modal Analysis Conference, pages 742—749, January 1989. [51] J.T. Spanos and W.S. Tsuha. Selection of component modes for the simulation of flexible multibody spacecraft. In Proceedings of AAS/AIAA Astrodynamics Specialist Conference, August 1989.  Bibliography  239  [52] P.A. Blelloch and KS. Carney. Selection of component modes. American Institute of Aeronautics and Astronautics, 90(AIAA-90-1201-CP):105—1 12, 1990. [53] P.c. Hughes. Dynamics of a flexible manipulator arm for the space shuttle. In Pro ceedings of AAS/AIAA Astrodynamics Specialist Conference, Jackson Hole, Wyoming, September 1977. [54] F.A. Kelly and R.L. Huston. Modelling of flexibility effects in robot arms. In Proceed ings of the 1983 Joint Automatic Control Conference, 1983. [55] P.C. Hughes. Dynamics of a chain of flexible bodies. Journal of Astronautical Sciences, 27(4):359—380, 1979.  [56] V.J. Modi and A.M. Ibrahim. Dynamics of the orbiter based flexible members. In Proceedings of the 14th International Symposium on Space and Technology, Tokyo, May 1984. [57] M.W. Walker and D.E. Orin. Efficient dynamic computer simulation of robotic mech anisms. ASME Journal of Dynamic Systems, Measurement and Control, 104(3):205— 211, September 1982. [58] W.M. Silver. On the equivalence of Lagrangian and Newton—Euler dynamics for ma nipulators. In Joint Automatic Control Conference, 1983. Paper TA-2A. [59] K.H. Low and M. Vidyasagar. Dynamic study of flexible manipulators with open and closed chain mechanisms. In Proceedings of the 1987 ASME Winter Annual Meeting, volume 6, pages 277—286. American Society of Mechanical Engineers, December 1987. [60] K.H. Low and M. Vidyasagar. A Lagrangian formulation of the dynamic model for flexible manipulator systems. ASME Journal of Dynamic Systems, Measurement and Control, 110(2):175—181, June 1988. [61] Y. Huang and C.S.G. Lee. Generalization of Newton—Euler formulation of dynamic equations to nonrigid manipulators. ASME Journal of Dynamic Systems, Measurement and Control, 110:308—315, September 1988. [62] K.L. Johnson. Contact Mechanics. Cambridge University Press, Cambridge, UK, 1985. [63] L.D. Landau and E.M. Lifshitz. Theory of Elasticity, volume 3 of Course of Theoretical Physics. Pergamon Press, 1959. [64] K.L. Johnson. One hundred years of Hertz contact. Proceedings of the Institution of Mechanical Engineers, 196:363—378, 1982.  Bibliography  240  [65] C.W. de Silva. Control Sensors and Actuators. Prentice-Hall, Inc., Englewood Cliffs, NJ, 1989. [66] R.L. Bisplinghoff, H. Ashley, and R.L. Halfman. Aeroelasticity. Assison-Wesley, Read ing, Massachusetts, 1955. [67] B.W. Char, K.O. Geddes, 0.11. Gonnet, M.B. Monagan, and S.M. Watt. Maple User’s Guide. Waterloo, Ontario, 5th edition, March 1988. [68] J.K.W. Chan. Dynamics and control of an orbiting space platform based mobile flexible manipulator. Master’s thesis, Department of Mechanical Engineering, University of British Columbia, April 1990. [69] B.J. Lazan. Damping of Materials and Members in Structural Mechanics. Pergamon Press, 1968. [70] L. Meirovitch. Elements of Vibration Analysis. McGraw-Hill, 2nd edition, 1986. [71] E.H. Gaylord and C.N. Gaylord. Structural Engineering Handbook. McGraw-Hill, 3rd edition, 1990. [72] C.-K.A. Ng. Dynamics and Control of Orbiting Flexible Systems: A Formulation with Applications. PhD thesis, Department of Mechanical Engineering, University of British Columbia, April 1992. [73] D.W. Clarke and P.J. Gawthrop. Self-tuning controller. lEE Proceedings, Part D, 122:929—934, 1975. [74] V. Peterka. Predictor-based self-tuning control. Automatica, 20(1):39—50, 1984. [75] B.E. Ydstie. Extended horizon adaptive control. In Proceedings of the 9th Triennial IFAC World Congress, Budapest, 1984. [76] P.S. Tuffs and D.W. Clarke. Self-tuning control of offset: A unified approach. lEE Proceedings, Part D, 132:100—110, 1985. [77] G.C. Goodwin and K.S. Sin. Adaptive Filtering Prediction and Control. Prentice-Hall, Inc., Englewood Cliffs, NJ, 1984. [78] K.J. Aström and B. Wittenmark. Computer Controlled Systems Prentice-Hall, Inc., Englewood Cliffs, NJ, 1984.  Theory and Design.  [79] R.C. Cutler and B.L. Ramaker. Dynamic matrix control A computer control algo rithm. In Proceedings of the 1980 Joint Automatic Control Conference, volume 1, San Francisco, 1980. —  Bibliography  241  [80] K. Ogata. Discrete—Time Control Systems. Prentice-Hall, Inc., Englewood Cliffs, NJ, 1987. [81] A.-L. Elshafel, G. Dumont, and A. Elnaggar. Perturbation analysis of GPC with onestep control horizon. Automatica, 27(4):725—728, 1991. [82] S.L. Shah, C. Mohtadi, and D.W. Clarke. Multivariable adaptive control without a prior knowledge of the delay matrix. System and Control Letters, 9:295—306, 1987. [83] G.J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, 1977. [84] M.E. Salgado, G.C. Goodwin, and R.H. Middleton. Modified least squares algorithm incorporating exponential resetting and forgetting. International Journal of Control, 47(2):477—491, 1988. [85] L. Ljung and T. Soderstrom. Theory and Practice of Recursive Identification. The MIT Press Series in Signal Processing, Optimization, and Control. The MIT Press, Cambridge, Massachusetts, 1983. [86] International Mathematical and Inc. Statistical Libraries. IMSL Library: Reference Manual. Houston, Texas, 1980. [87] W.C. Gear. Numerical Initial Value Problems in Ordinary Differential Equations. Prentice-Hall, Inc., Englewood Cliffs, NJ, 1971. [88] W.H. Press, B.P. Flannery, S.A. Teukoisky, and W.T. Vetterling. Numerical Recipes. Cambridge University Press, Cambridge, UK, 1986. [89] H.W. Mah. Private Communication, July 1991. [90] S. Chen and S.A. Billings. Representations of non-linear systems: The NARMAX model. International Journal of Control, 49(3):1013—1032, 1989.  Appendix A  Equations of Motion for Various Manipulator Configurations  This appendix contains the equations of motion for several other configurations of manip ulators which were studied during the course of the research. These equations were arrived at by transforming equations (2.47) through (2.52), derived in section 2.4, as noted in each section.  Planar Two Link Flexible Manipulator in Free Motion  A.1  Setting each element of the set of coefficients {m, be, bex, b , kex, key, k, k 3 } to zero 3 ,b 6 and dropping the generalised coordinates e , and , and equations (2.51) and (2.52) yields 2 the 2+n 1 +n 2 equations of motion for a planar two link flexible manipulator in free motion. Rigid body equation for link 1: + + +  102  2 (L + w 0+L 12 + 2 c 2 1 ioai L ) +M  ‘21ii  ‘11j1j  +  I22j  —  2 (Lis 12  —  ) 2 wiocai  2 (L 1 + 2 Ca L ) + [M  ) 4 I i 2 2 ]  i  I42i2] —  ::12 fli  +  102  L 12 2 ca + 1 (L + -JvI  ) 2 Wioai  + :22 b 225  —  42 I 23 ) çb 12 wioca  2 (Liai  jz1  +  [102  +  L (Lic 2 12 + + M  I12j/’2j 3=1  2+ + [Liai  1 j  Sa 1 W )+ 12 0 I22j  Sa 1 W j 12 0 3=1  242  —  12 (Lis  —  )EI w 1 c ] 422 0 2  14 c’  Appendix A. Equations of Motion for Various Manipulator Configurations  =  —  8 8 b 1 1  +gx  (MiLisi + ci  c 1 w 2 M M +1 s 1 L 0 + 12 +2 8ia + L 2 M  —gy  (MiLici  Lici 2 +M  +  {  243  —  si  2 1 [2 (L18a +412  —  12 2 (Lis [L  s 1 w 2 M 1 0 + 2 ciai L M  wlocai2)  zL4()  —  —  8iai2I422)  a2  2(Lic + + 12  WloCai2)  —  —  2 1 (L2Sa  ) Wio] 10 + 2w  —  2I  2 I 22jb2j2j  —  +2  2 1 [(L1Ca  +  WioSai2) Wo  L (Liai 2 2 + [M  +  L 2 PV1  Sa 1 (L 2  Ca 1 W ) 12 0  —  +  0 cai2wlo) z  —  I’ + 2 (Lis 12  Cai2Wio]  2 (Lic::  —  +  wiocai2)  i 4 I } 2 2  ] 4 I 22  3a 1 W ) 12 0  2  —  +2  2 1 (L15a  —  wio) 1 c 2  j=i fl2  12 + c 1 +2 (L  L (Lia 2 12 + [1 +2 (Lia 12  —  I42J2J] 82  Sai2Wlo)  ) 12 Wi:Ca  —  ) 1 c 0 wi 2  +  12 (Lic:  j’J 4 I j 2 2  42525 ) 2 wio&ai ] I  ti4.  I22j’/J2j’/)2j  —  j=i  +  (A.1)  5=1  Deflection equation for ith mode of link 1:  +  [  1 + 2 (L Cai L )  (Lic + L 12 2 +M  —  sl2  iOI42525] &jo  +  [102  —  WloSai2)  —  210 + 2 ai L (M +I21iii + [M  (L13a2  2 S  w1ocai2)I42525]  ) 4 I 2525  1jo1i i  i  Appendix A. Equations of Motion for Various Manipulator Configurations  c L 2 + [(M  +  —  —  a2  I42i?2)  +  aj2  1422) &jo  +  (102  244  ]  +  22 I ) 5 I)  +  I22J) io]  l L 2 [(:  b+ 2 I  lo 12 Ca  3=1 =  b 1 —b  +gs1I4j +  2=1 —  —  {121i1i  —  I31iI’1i  9yC1I4li  2 + [M  +  ) 10  +  422 iio I 2 Cai ]  L 12 2 [M (Ljs —w ) 1 c 10 2 + (Liai 2 + 42 )I wiosai ] 2 )  Sa + 2 ML 12  +  ai (L 2  1.  Cai2 I42j)2j 2 3=1  +2 2 ai + L [M o  8a12  2 2 a L [M i :+ i sai + L -M + 2  12 Cc  0}  2 0 1o  +  I42j’2j  I22j2j2  2 ai  iio  +  io}  Ôi  2 qi ççb 42 >1 8 0 j=1  +2  {  a L [M o 2 &io i+  —  0+  ‘22j2 j=1  +2  Sai2I42j2j  +  1io  12 + s L 2 M  a2  )  42j2 1  W 10  j=1  —  (A.2)  .  Rigid body equation for link 2:  [102  L (Lic 2 12 + + M  WioSai2)  +  —  12 (Lisa  —  )I wiocai ] 2 i 42  Appendix A. Equations of Motion for Various Manipulator Configurations  [  2  Cai L 2 + [ZM  —  :b 4 I i 2 ] 2  a2  ,  +  102  +  3 2 1 1 2jh 2  2  +  j=1  245  22 I + b]  12j, 1 2 j j=1  62 b 2 0  =  iai + L (M +gx 2  C1ai2I422)  L (wioca 2 2 + [M  ) 2 Liai  gy  I42j’1/’2j  +2  j=1  +  Ciai L 2 (M  —  S1ai2I422)  )I 2 Wio8ai ] 2 + 422 (Licai  S L 2 M i )io ai + 2caj zbio 2  —  —  è  6 2=1  (A.3)  2).  Deflection equation for jth mode of link 2:  12j 1 [  +  j 4 1 12 +ca 2 =  ’çb —b 2  1o1i  —  A.2  +  12j 1  jo1i  +  ‘12j2  +1 22j2j  ’b 3 I 2 2  +gxsla 2 1 I42j  +2  12 s ) 0 w, ] Ii + 42  2 1 (L1Ca  —  {I22j&2j2  gyClai2I42j  —  i 2 ib 22 + [I  —  12 (Lis  w,o 1 s 4 I 23 2 +b 22 I } 0 W 2  —  )I 1 c 0 w, ] 42 26  22 (ti4 + O + 2ti4 I 2 ). 2 & 0 +b  (A.4)  Planar Two Rigid Link Manipulator in Contact  Setting each of the coefficients  ,i = 2 {I1ki,I2k,I3ki,I4k:, bk  ping the generalised coordinates  1,...  ,k,  j,j 2 1,... ,n,} and {i/  =  k  =  1, 2} to zero, drop  } and dropping 2 1,... ,n  equations (2.49) and (2.50) yields the equations of motion for a two rigid link manipulator in contact with a deformable environment.  Appendix A. Equations of Motion for Various Manipulator Configurations  246  Rigid body equation for link 1: [i +  102  2 (L + 1 ) + m (L + L + 2L c 2 L )] c 2 L 1 +M  c + m (L + L 1 L 2 )] c 2 1 + [102 + M  1 ë  2 ë  12 ë + mi,, 1 s 2 L [L + ] c 12 c 2 L —m [Lisi + ] =  —  12 [(Lisi + ) s 2 L 12 a s 2 L 1 3 (Lisi + ) +b  —  L 1 s 2 22  3 (Lici + ) —b 12 [(Liei + ) c 2 L 12 i + 0 c 2 L 12 c L 2 12 (Lici + L s 2 L 12 c 2 8 (Lisi + ) +k , (Lici + ) 9 —k 12 (Lisi + L c 2 L 12 s 2  —  —  —  Yw  s+ 2 1 L 2 si L M ) +gx (MiLiSi + M 2+ s [(M 2 L 1 +L  mc)  —  —  éj  Err)  E)  —  c + 2 1 L 2 ci L M ) , (MiLici + M 1 g  6 + (M 2 + 2m)  (A.5)  Rigid body equation for link 2:  [102  c + m (L + L 1 L 2 )] öi + [I + mcL] c 2 1 + M  2  si + ë 2 —mL ë 12 c 2 mL = T 2  —  —b 1 s 2 L 3 2 [(Lisi + ) 12 + s L 2 1+& 12 Ô s 2 L ci [(Lici + ) 2 L 3 —b 12 01 c 2 L  6 1 c L 2 2  —  —  12 c 2 +k 1 s 2 L 8 2 (Lici + L si L 2 +g-M  —  — — —  ci L 2 g-M  —  ci (Lisi + L 2 L 8 k 12 s 2  L+ 2 (M  x direction contact equation: 12 s 2 L —m [Lisi + ]  i 8  —  rncL2s122  + mE  mc)  .  -  71w  —  (A.6)  Appendix A. Equations of Motion for Various Manipulator Configurations  =  —  247  kexEx  12 s 2 L 8 [(Lisi + ) —b  &i  c 2 +m [Liciô + L  (o + 9 + 28)].  12 + s L 2 +6  5 (Lici + L 12 c 2 +k  — —  (A.7)  y direction contact equation: ] 1 c 2 28 m [Lici + L =  beyEy  —  —  12 + c mL 2 6  keyEy  12 i9 + 0 c 2 L 12 c L 2 8 [(Lici + ) +b  12 s 2 + k (Lisi + L  —  Yw  —  cy)  —  12 s 2 +m [LisiÔ + L A.3  (ô + 8 + 2O )] 12  (A.8)  .  Planar Two Link Rigid Manipulator in Free Motion  Further operation on the set of equations in section A.2, namely setting each of the coef ficients {m, bex, bey, bx, b , kex, key, 3  } to zero, dropping the generalised coordinates 8 k  e and e, and dropping equations (A.7) and (A.8) yields the 2 equations of motion for a planar rigid link manipulator in free motion. Rigid body equation for link 1: + =  Ti  +gx  —  102  M (L + L )] 1 + c 2 1 +2  91 + M b 1 8 s [9 + 1 L 2  {z +  Lic L M ] 2  82  8182]  (MiLiSi + M s+ 2 1 L 2 si L M )  —  c + 2 1 L 2 ci L M ) gy (MiLiCi + M  (A.9)  Rigid body equation for link 2: [102  LiC L M ] + 2  81  + 10282  2 T  —  82 + M b 2 8 L (gxsl2 2  —  ) 12 gyc  —  . (A.lo) L 2 M  Appendix A. Equations of Motion for Various Manipulator Configurations  A.4  248  One Flexible Link in Contact  The 2 + n 1 equations of motion for a single flexible link in contact are arrived at by setting the coefficients  {I2, 1 i2i, 122:, ‘32i, 142:,  the generalised coordinates  , bex, b 2 b , kex, k 8 ,i 5  1,  .  .  .  } to zero, dropping 2 n  ,  ,j = 1,... ,n} and e and dispensing with equations 23 {b  &2,  (2.48), (2.50) and (2.51). Rigid body equation for link: + m (L + =  —  0 ) ]O+  + mCLi  1o1j  + m [Lici  —  wi s 0 i] ë  b 8 11  3 (Lici —b 3 (Lici —k  [(Lici  WioSi)  —  —  —  s 1 w ) 1 08 ci 0 i + i  wi s 0 i) (Lisi + c 10 w 1  +9x (MiLisi -t-  Cl  z1:4lbl)  —2  +  mcwioio)  (I2liiili  71w  —  —  C L 1 (M  )  —i  6.  (A.11)  Deflection equation for ith mode of link: [liii  =  + mLiioj  1 —bi1/)  (gxSi  —  ‘2l:li  +  iioii  10 m 1 +c  I3iil/’li  —  ci [(Lici 8110 —b +  +  —  gyci) ‘41i  s 1 w 8 ) 1 0  +  —  1+ b 21 + (I  WioCi]  mcwioqijo)  —  c 8 k 1 110 (Lisi +  C 1 W 1 0  6  —  71w  —  (A.12)  y direction contact equation: m [Liei =  beyy  —  —  8 [(Lici +b  s 1 w ] 1 0 9 i  1 + mc  1jo1j  + më  keyEy —  +m (Lisi +  3 1 W ) 1 0 8 i C 1 W ) 1 0  —  é+  C1] 1 W 0  8 + 2mzbi si8i 0  8 (Lisi + +k  C 1 W 1 0  —  71w  —  (A.13)  Appendix A. Equations of Motion for Various Manipulator Configurations  A.5  249  One Flexible Link in Free Motion  The 1 + n 1 equations for the a single flexible link in free motion are obtained from the equations in section A.4 by setting the coefficients {m, bey, bsy, the generalised coordinate  E  key,  } to zero, dropping 3 k  and dispensing with equation (A.13)  Rigid body equation for link: + =  — 1 T  911 + g b  (MiLisi  +  C  I411)  —  gy (MiLici  —  1 14ii)  . & b 1 ,b 21 —2I  (A.14)  Deflection equation for ith mode of link: 111191 + A.6  21j’Ii = 1  —b 1 ’ 12 1  —  b 3 I . 1 1 + (gs 1  —  gci)I i 4 ,+  I21ibn8  (A.15)  One Rigid Link in Contact  From the equations in section A.4, the 2 equations of motion for a single rigid link in contact are obtained by setting the coefficients {1 ,i = 1,. 311 211 ,1 111  ,ni} to zero, dropping the  generalised coordinates {& ,i = 1,... ,m}, and dispensing with equation (A.12). 11 Rigid body equation for link: ë c 1 + mLj 8 + mL =  1 T  —  81 b 1 8  —  yLici (L 8 b Ô c 1  —  c (Lisi 1 L 3 k  —  +gx (MiLisi)  —  gy (MiLici)  —  Yw  —  e) (A.16)  2mwi i 9 i 0 .  y direction contact equation: ci8 + më 1 mL =  beyy  —  8 (LiciÔi keyEy + b  —  y (Lisi 3 +k  —  Yw  —  e) + mLisi8.  (A.17)  Appendix A. Equations of Motion for Various Manipulator Configurations  A.7  250  DC Motor Actuated Single Flexible Link in Free Motion  This DC motor actuator model is coupled to the rigid body equation of motion for the link being driven by adding the inertia, NIak, and damping, Ng bak, terms from equa 2 tion (2.68) to the corresponding terms in the rigid body equation and by using the geared up motor torque, N KTja, as the joint torque, 9  ‘rk.  In order to calculate this joint torque the  differential equation for the armature current (2.65) must be solved simultaneously with the equations of motion since it involves the armature speed which is proportional to the joint rate. For example, the equations of motion for a single flexible link in free motion driven by a DC motor are as follows. Rigid body equation for link:  [101  01 I 1 N +]  NgiKriiai  —  ö  +  I11i1i  1+g bO  (MiLiSi  +  1 1 C  1i1i)  —  gy (MiLici  —  i I411) 3  (AJ8)  ibibi 2 —2>I 8 i. Deflection equation for link: 21 I . 1 Iii + b  =  1211 —b  b 3 I . 1 1  + (gzsi  —  gci)I i 4 .+b 21 I 1  (A.19)  Armature current equation for motor: Laijai + KernfNgll = Kv 1  —  Raijai  (A.20)  Appendix B  Cost Function Expansions  This appendix contains the details of the algebraic expansion and differentiation of the cost functions defined for the control algorithms developed in Chapter 3. B.1  Multivariable Predictive Control Algorithm  For the multivariable algorithm the cost function is the sum of squares of the optimal predicted output errors, ê, plus the sum of squares of the control input increments, Uk, the latter weighted by a positive factor, ): J  = ê”êi  (B.1)  + XcZU’Uk.  Expanding the first term using equation (3.31): =  T n 1 k 2 G Uk+fw  G k 2 Uk+fw  kz1  k1  fl  +cU11k k1  mt  (,G k 1 uk) =1  n,  +(f_w)T  +(f—w)  k=1  /n  k=1  \  fn  i=1  kzrl  krl  +(f_w)T (f-w)]  k=1  fn  (>U’G) (Gkuk) +2(UG)  =  +.AU’Uk  ) 1 (f—w  k1  +cilUk.  251  (B.2)  Appendix B. Cost Function Expansions  252  Differentiating with respect to Urn, an arbitrary member of the set of control input increment vectors gives: [G  =  no =  Gim]+2GL(fi_Wi)+2?icUk  ni  2  no  G Gk Uk +  .\cUm  G (f,  +  w)  —  (B.3)  .  =1 k—_i  B.2  Static Equilibrium Bias Term  The static equilibrium bias term is the sum of squares of the differences between the actual control inputs,  Uk,  and the calculated static equilibrium inputs,  >Z (ujc  Jse =  —  Uk)  T  ük:  (B.4)  -  (Uk  Uk).  —  k=1  Substituting for  Uk  from equation (3.50): ft  0  Jse  (iL  =  + Luk  -  -  Uk)  T (ukp  + Lu,.  -  —  Uk)  kzrl (uj  + uLT  —  + LUk  u)  Uk)  =  =  2 + (u) Differentiating with respect to  Urn,  ‘  + Ü Uj  —  2ujtT  Uk].  (B.5)  an arbitrary member of the set of control input increment  vectors yields: =  2 [LT L Uk + uLT  =  2 [LT L Uk + LT (uj  —  LT u]  —  ü)]  (B.6)  Appendix C  Static Equilibrium Function Derivation  This appendix presents the details of the algebraic manipulations of the equations of motion and contact force model for a two flexible link manipulator in contact with a deformable environment to yield the function that maps the set of desired contact force levels (setpoints) onto the corresponding set of static equilibrium torques (control inputs). This function is used to obtain the static equilibrium bias term for the control algorithm cost function that is derived in section 3.4. The static equilibrium relationships for the system are obtained by setting to zero all of the time derivatives in the equations of motion ((2.47) through (2.52)). From the rigid body equations this yields, for link 1:  o = T1 112 + w2oclai2) (Lici s 2 +L  s+ 1 8 (L +k  10 w 1 c  3 (Liei —k  wiosi + L 112 c 2  —  —  W2031a 2 1 )  —  wiosi + L2c1a 2 1  (Lisi + wioci +  2 1 L251a  —  +  1 1 8 W2o 2 a  W2oClai2  — —  —  Yw  —  (C.1) and, for link 2:  o  =  12 s 2 i+ 2 + k (L r  W2oClai2)  (Lici  5 (L2c1a —k 2 1  W2081a 2 1 )  s + 1 (L  —  —  S 1 W 1 0  +  2 1 L2C1a  10 W 1 C  +  2 1 L251a  1 1 5 W2o 2 a  +  2 1 W2OC1a  —  —  Xw 71w  —  en).  The deflection equations for the ith mode of link 1 and the jth mode of link 2 yield:  253  (C.2)  Appendix C. Static Equilibrium Function Derivation  o  254  —  (q + 0 1 s q 1 s 2 L 12 + 8 10 +k 51 —k (q j oci + 12 ci 2 L 0 6  —  ) 0 w2oc1aj2q5  W2oS1ai2q.o)  (Lici  —  (Lisi +  Wiosi Wioi  +  L2Clai2  +  2 1 L281a  —  1 1 9 W2o 2 a  — —  +  W2oClai2  —  Yw  —  (C.3) and o  =  1b 3 I 2 2+1 çb2jos1a (Lici 3 ks 2 ksy42joC1ai2  +  2 1 L2C1a  (Lisi + wioci +  2 1 L231a  —  o. 1 W  —  +  W2oSlcx2 2 11 W2OC1c  —  —  —  Yw  e) Ey).  —  (C.4)  Finally, the static equilibrium relationships obtained from the x and y direction contact equations are: o  =  o  =  e+k 6 —k 8 (Lici  —  s 1 w 1 0 + L2C1a 2 1  —  W2oS1c2  —  Xw  —  Ex)  (C.5)  eu).  (C.6)  and keyEy +  (Lisi +  C 1 W 1 0  +  2 1 L2S1a  +  2 1 W2oCh  —  Yw  —  Setting the time derivatives in the contact force model (equations (2.56) and (2.57)) to zero yields: F  rn 3 kezEx +  me  8 k  (C.7)  —  —  and 3 (Ytip F!, = keyEy + k  —  Tfl::  en),  (C.8)  1 1 5 W2o 2 a  (C.9)  W2oClai2.  (C.1O)  Yw  where the world frame tip position components are given by c 1 L  —  s 1 w 1 0 + L2C1a 2 1  —  and Ytip  s + 1 L  C 1 W 1 0  +  2 1 L2S1cz  +  Appendix C. Static Equilibrium Function Derivation  255  The static equilibrium defiections of the contact region mass are obtained by substituting equations (0.9) and (0.10) into equations (0.5) and (C.6) and solving for e and e: (xj  =  —  x)  (0.11)  yw).  (0.12)  and ksy+key With these results e and  Ey  (Ytip  —  can be eliminated from the contact force model to give F  =  F  =  kxeff  (xt  —  x)  (0.13)  yw).  (0.14)  and kyeff (Ytip  Substituting (C.11) through (0.14) into (0.1) and (0.2) and solving for r 1 and r : 2 1 T  =  F (Lici  —  si + L2C 0 wi 2 a 1  —  W2oS1a2)  —  s + wc 1 1 + F (L  2 1 L231a  +  w2oc1a2)  (0.15)  and =  F (L2C1a 2 1  —  81a 0 w2 2 1 )  —  F, (L2S1a 2+ 1  (0.16)  2). 1 W2OC1a  Similar substitutions into (0.3) and (0.4) yield the following expressions for the static equilibrium modal amplifiers: F  [4)i3i  +  2 1 (L231a  +  2) 1 w2jocla  ] 4  —  F [4i ci + 0  2 1 (L2C1a  20 w ) 12 + si  44cJ (0.17)  and 31 I i 1 /)  2 1 Fxq2jos1a  —  (0.18)  jocia 2 Fyb . 12  These expressions are not explicit relationships between F and F and of complications which arise from the link 1 tip slope terms (c and  1 ib  and b 2 because  Equations (0.17)  Appendix C. Static Equilibrium Function Derivation  256  and (C.18) could, in principle, be solved by simultaneous iteration to give static equilibrium values of bh and /‘ for given contact forces. However, assuming the tip deflection of link 1 is small compared to its length a will be small and the slope terms can be neglected, resulting in the following approximate expressions for the static equilibrium modal deflections: 1j  131i  si 10 (F  —  ) 1 c 0 F  (C.19)  ). 1 c 20 Fb 2  (C.20)  and 2 b  —-—  (F 1 s 20 2  —  ‘32j  The same approximation applied to equations (C.15) and (C.16) gives: F (Lid  —  —  F (Lisi  s +L 1 ’ 0 i 12 + c 2  2io28i2)  12 + s 2 0 i ci + L  (C.21)  —  and —  2 T  F 2 (L d i2  2  2O2s12)  —  12 + s 2 F (L  .  (C.22)  These final four equations provide a close approximation to the desired relationship between the contact force level setpoints and the joint torques required to maintain them under static equilibrium conditions. Note that in the case of a single link the complications due to the link tip slope terms do not arise and exact expressions can be obtained.  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0081044/manifest

Comment

Related Items