FORCE CONTROL FOR ROBOTIC MANIPULATORSWITH STRUCTURALLY FLEXIBLE LINKSByDouglas John LatornellB.Sc. ions. (Mechanical Engineering) Queen’s UniversityM.Sc. (Mechanical Engineering) Queen’s UniversityA THESIS SUBMITTED IN PARTIAL FULFILLMENT OFTHE REQUIREMENTS FOR THE DEGREE OFDOCTOR OF PHILOSOPHYinTHE FACULTY OF GRADUATE STUDIESDEPARTMENT OF MECHANICAL ENGINEERINGWe accept this thesis as conformingto the required standardTHE UNIVERSITY OF BRITISH COLUMBIA1992© Douglas John Latornell, 14 August 1992In presenting this thesis in partial fulfilment of the requirements for an advanceddegree at the University of British Columbia, I agree that the Library shall make itfreely available for reference and study. I further agree that permission for extensivecopying of this thesis for scholarly purposes may be granted by the head of mydepartment or by his or her representatives. It is understood that copying orpublication of this thesis for financial gain shall not be allowed without my writtenpermission.________________________________Department of / iç7The University of British ColumbiaVancouver, CanadaDate aki /92DE-6 (2/88)AbstractThis thesis reports on the development of strategies for the control of contact forces exertedby a structurally flexible robotic manipulator on surfaces in its working environment. Thecontroller is based on a multivariable, explicitly adaptive, long range predictive controlalgorithm. A static equilibrium bias term which is particularly applicable to the contactforce control problem has been incorporated into the control algorithm cost function. Ageneral formulation for the discrete time domain characteristic polynomial of the closedloop system has been derived and shown useful in tuning the controller.Kinematic and dynamic models of a robotic manipulator with structurally flexible linksinteracting with its working environment are derived. These models include inertia anddamping effects in the contact dynamics in addition to the contact stiffness employed inmost previous work. Linear analyses of the dynamic models for a variety of manipulatorconfigurations reveal that the controlled variable, the contact force, is dominated by differentopen 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. Aspecial purpose simulation program, TWOFLEX, which includes the dynamics models of themanipulator and the environment as well as the control algorithm was developed duringthe research. The configurations investigated using the simulation include a single flexiblemanipulator link, two link manipulators with both rigid and flexible links, and a two linkprototype model of the Mobile Servicing System (MS S) manipulator for the proposed SpaceStation, Freedom. The results show that the controller can be tuned to provide fast contactforce step responses with minimal overshoot and zero steady-state error. The problem of11maintaining control through the discontinuous situation of unexpectedly making contactwith a surface is addressed with the introduction of a contact control logic level in thecontrol hierarchy.111AbstractList of TablesTable of Contents11vi”List of FiguresNomenclatureAcknowledgements1 Introduction1.1 Introduction1.2 Motivation1.3 Survey of Related Research1.4 Contributions of the Thesis1.5 Outline of the Thesis . .xxvixxv2 Manipulator Modelling2.1 Introduction2.2 Literature Review2.3 Kinematics2.3.1 Manipulator Configuration2.3.2 Flexible Link Model .2.3.3 Contact Model14141519192529112369Model .iv2.4 Derivation of Equations of Motion 322.5 Contact Force Model 482.5.1 Hertz Contact Model 522.6 Voltage Controlled DC Electric Motor Actuator Model 542.7 Summary 583 Control Algorithm 593.1 Introduction 593.2 Review of SISO GPC Algorithm 623.2.1 Control Law Structure and Closed Loop Characteristics 663.3 Multivariable Predictive Control Algorithm 693.3.1 Control Law Structure and Closed Loop Characteristics 763.4 Static Equilibrium Bias Term 793.5 Control Algorithm Parameters and Tuning 833.6 Parameter Estimation 863.7 Adaptive Controller Implementation 903.8 Stability and Convergence 923.9 Summary 934 Dynamics and Control Simulation— TWOFLEX 954.1 Introduction 954.2 General Description of the TWOFLEX Program 964.3 Integration of Equations of Motion 994.4 Discrete Time Control Algorithm 1024.5 Validation 1064.6 Summary 109v111• • • . 111• • • . 114• • 114• • • . 121• • • • 135• 135• • 136• • • . 137• • 141• • • . 151• . . • 160• . . . 163• . . . 166• • . . 169• . . . 174• . . . 175• . • . 197• . . . 199202• . . . 217• . . . 218• . . . 2246 Summary of Major Results6.1 Recommendations for Further Research5 Force Control Algorithm Performance and Evaluation5.1 Introduction5.2 Linear Analysis5.2.1 Rigid Link5.2.2 Flexible Link5.3 Force Control for a Single Flexible Link5.3.1 Link Model Parameters5.3.2 Controller Model5.3.3 Soft Contact Case (keff 3.lkN/m)5.3.4 Hertz Contact Model Case (kff 5MN/m)5.3.5 keff = 62kN/m Contact Stiffness Case .5.3.6 Summary5.4 Making and Breaking Contact5.4.1 Stop Motion Strategy5.4.2 Force Setpoint Modification Strategy .5.5 Force Control for Two Link Manipulators5.5.1 Linear Analysis5.5.2 Controller Model5.5.3 Force Control for Two Rigid Links . .5.5.4 Force Control for Two Flexible Links . .5.5.5 Summary5.6 Force Control for DC Motor Actuated Space Station Manipulator5.7 Summary of Force Control Performance228231viBibliographyAppendices235242A Equations of Motion for Various Manipulator ConfigurationsA.1 Planar Two Link Flexible Manipulator in Free MotionA.2 Planar Two Rigid Link Manipulator in ContactA.3 Planar Two Link Rigid Manipulator in Free MotionA.4 One Flexible Link in ContactA.5 One Flexible Link in Free MotionA.6 One Rigid Link in ContactA.7 DC Motor Actuated Single Flexible Link in Free Motion242242245247248249249250B Cost Function ExpansionsB.1 Multivariable Predictive Control AlgorithmB.2 Static Equilibrium Bias TermC Static Equilibrium Function Derivation 253251• 251• 252viiList of Tables4.1 Configurations and initial conditions for validation of inertia matrix and righthand side vector terms 1074.2 Configurations and initial conditions for free response simulation validationruns 1075.1 Mode shape function integral factor values for a single flexible link with onestructural mode 1235.2 Natural frequencies and mode shape function integral factor values for singleflexible link run 1365.3 Material properties and Hertz model parameters for steel on aluminium contact 1425.4 Stabilising controller horizons and closed ioop characteristics for steel onaluminium contact (keff = 5.0037MN/m) for a single flexible link with onestructural mode 1465.5 Stabilising controller horizons and closed ioop characteristics for steel onaluminium contact (keff = 61.554kN/m) with a single flexible link with onestructural mode 1535.6 Stabilising controller horizons and closed ioop characteristics for steel onaluminium contact (keff = 61.554kN/m) with a single flexible link with threestructural modes 1605.7 Parameter values for two rigid link manipulator controller model 2005.8 Stabilising controller horizons and closed ioop characteristics for a structurally rigid two link manipulator contacting a keff = kN/m surface 202viii5.9 Parameter values for two flexible link manipulator controller model 2045.10 Stabilising controller horizons and closed ioop characteristics for a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/msurface 2085.11 Physical parameter values for a two link MSS model with DC motor actuators.219ixList of Figures2.1 Geometry of a two link flexible manipulator in contact with a deformablesurface 192.2 Coordinate frames for a two link flexible manipulator in contact with a deformable surface 212.3 Free body diagram of a differential beam element 262.4 Axial foreshortening of a differential beam element 292.5 Manipulator—environment model for derivation of contact force expressions 492.6 Free body diagrams for the x direction contact region mass 502.7 Schematic of direct current motor actuator system 553.1 SISO GPC closed loop system block diagram 683.2 Comparison of EFRA and U/D RLS parameter estimation algorithms. . 893.3 Control strategy block diagram 914.1 Flowchart of the overall structure of the TWOFLEX dynamics and control simulation program 974.2 Flowchart of the calculation of the generalised acceleration vector 1024.3 Flowchart of the discrete time control algorithm 1034.4 Comparison of joint angle responses for free response validation 1084.5 Comparison of modal amplifier responses for free response validation 1105.1 Topics discussed in Chapter 5 1125.2 Single rigid link in contact with a deformable environment 114x5.3 Variation of the natural frequencies with effective contact stiffness for a singlerigid link/environment system 1175.4 Single flexible link in contact with a deformable environment 1215.5 Variation of the natural frequencies with effective contact stiffness for a singleflexible link with one structural mode 1245.6 Single flexible link in contact with a deformable environment approximatedby a steady state contact dynamics model 1255.7 Variation of the natural mode components of the contact force with effectivecontact stiffness for a single flexible link with one structural mode 1275.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. . . 1295.9 Variation with effective contact stiffness of system natural frequencies for asoft link (El1 = 40N . m2) 1305.10 Variation with effective contact stiffness of system natural frequencies for aflexible link with four structural modes 1325.11 Variation of the natural mode components of the contact force with effective contact stiffness for a single flexible link (El1 = 200N . m2) with fourstructural modes 1335.12 Variation with effective contact stiffness of the relative excitation of the natural modes of a single flexible link (El1 200N . m) with four structuralmodes 1345.13 Variation of natural frequencies of a single flexible link (El1 = 574N m2,two structural modes) in contact 1395.14 Contact force response of a single flexible link on a soft surface (k5 = ke =6200N/m) 140xi5.15 Variation of natural frequencies of a single flexible link (with one structuralmode) in contact 1455.16 Comparison of results from simulations using full contact dynamics andsteady state approximation 1475.17 Force control step response for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 5MN/m) with N2 = 20, N = 2,= 0, and Age = 0 1485.18 Force control step responses for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 5MN/m) with 0 Age 0.4. . . 1505.19 Experimental contact stiffness estimate for steel on aluminium 1525.20 Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (kff = 61.554kN/m) with N2 = 10,N=3,A=0,A80 1545.21 Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (kff = 61.554kN/m) with N2 = 5, N.4, A = 0, A = 0 1555.22 Force control step response for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 61.554kN/m) with N2 = 5, N3, A = 0, Age 0 1565.23 Force control step response for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 61.554kN/m) with N2 = 7, N =4,Ac0,A8e 1585.24 Variation of the natural frequencies of a single flexible link (El1 = 574N m2with three structural modes) in contact 159xii5.25 Force control step response for single flexible link (with three structuralmodes), steel on aluminium contact (keff = 61.554kN/m), with N2 = 8,N, = 4, = 0, )ise = 0 1615.26 Block diagram of the overall control structure 1655.27 Joint angle and contact force responses for a single flexible link colliding witha 6200N/m surface 1675.28 Simple 1 degree of freedom model of a single link in contact with an environment 1705.29 Contact force response for a single flexible link making contact with a 6200N/msurface at 0.536m/s 1725.30 Contact force response of a single flexible link colliding with a 6200N/msurface with force setpoint modification 1735.31 Two rigid link manipulator in contact with a deformable environment . . . 1765.32 Variation of the natural frequencies with effective contact stiffness for a tworigid link manipulator in contact 1785.33 and 2 mode shapes for various values of for a two rigid link manipulator.1835.34 Variation of contact force components in natural coordinate space with Owith keff = 61.554kN/m 1855.35 Relative excitation of the natural modes of a two link rigid manipulator bythe control inputs 1875.36 Variation of the natural frequencies with effective contact stiffness of a twolink manipulator with identical, flexible (El = 574N m2, one structuralmode each) links in contact with an isotropic (kxeff kyeff) environment. . . 1915.37 Variation of the natural frequencies with effective contact stiffness for a twoflexible link manipulator (El = 574N m2, one structural mode each) with= 1.Om and L2 = 2.Om 193xlii5.38 Variation of the natural mode components of the contact forces with effectivecontact stiffness for a two flexible link manipulator 1955.39 Variation of the relative excitation of the natural modes with effective contactstiffness for a two flexible link manipulator 1965.40 Force control step responses for a structurally rigid two link manipulatorcontacting a keff = lOkN/m surface with N2 = 5, Nu = 2, c = 0 2035.41 Variation of the natural frequencies for a two link manipulator with identicalflexible links assuming steady state contact dynamics 2065.42 Force control step responses for linear dynamics simulation of a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/msurface with N2 20, N = 2, 0 2095.43 Force control step responses for linear dynamics simulation of a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/msurfacewithN2=8,N=4,)=0 2115.44 Force control step responses showing output interactions for linear dynamicssimulation of a two flexible link (one structural mode each) manipulatorcontacting a keff = lOkN/m surface with N2 = 20, N = 2, 0 2125.45 Force control step responses showing output interactions for linear dynamicssimulation of a two flexible link (one structural mode each) manipulatorcontacting a keff = lOkN/m surface with N2 = 8, N = 4, = 0 2135.46 Force control step responses for nonlinear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting a keff = lOkN/msurface with N2 = 20, N = 2, , = 0 2155.47 Variation of the modal natural frequencies for DC motor actuated two linkMSS model 220xiv5.48 Force control responses for linear dynamics simulation of a two flexible linkSpace Station MSS contacting a keff = 2OkN/m surface 2225.49 Force control responses for nonlinear dynamics simulation of a two flexiblelink Space Station MSS contacting a keff = 2OkN/m surface 223xvNomenclatureRoman lettersA(q1) discrete time input/output model denominator polynomial of degree maa radius of circular contact area in the Hertz contact modelB(q’) discrete time input/output model numerator polynomial of degree mbBk(q’) discrete time input/output model numerator polynomial relating kth input toith output in a discrete time multivariable system modelba equivalent viscous damping coefficient for DC motor armature frictional dampingbex equivalent viscous damping factor for contact damping of the environmentsurface in the x directionbey equivalent viscous damping factor for contact damping of the environmentsurface in the y directionbk, equivalent viscous damping factor for structural damping of mode i of link kb3 equivalent viscous damping factor for contact damping of the manipulator tipsurface in the x directionb3 equivalent viscous damping factor for contact damping of the manipulator tipsurface in the y directionbek equivalent viscous damping factor for friction in joint kc, vector of generalised coordinate components for linearised model of Fc, vector of generalised coordinate components for linearised model of Fc1 cosine of joint angle 1 (Ox)c2 cosine of joint angle 2 (&2)ca2 cosine of [w’10(t) + 2(t)]Clai2 cosine of [61(t) + w0(t) + 2(t)]xviD,(t) homogeneous transformation matrix to describe the structural deformation oflink 1 relative to coordinate frameD2(t) homogeneous transformation matrix to describe the structural deformation oflink 2 relative to coordinate frame F2D inverse of effective planar strain modulus in the Hertz contact modeld(t) disturbance term for ith output in a multivariable system modeldm, an element of mass on link 1dm2 an element of mass on link 2E matrix whose columns are the eigenvectors of a linear systemE(q’) a backward shift polynomial which satisfies the Diophantine identityEk modulus of elasticity of the material of surface k in the Hertz contact modelElk fiexural rigidity of link k in bending about its joint axisê vector of optimal predictions of future output errors for the ith output of amultivariable systemek eigenvector corresponding to the kth natural frequency of a linear systemF applied force in the Hertz contact modelF3(q’) a backward shift polynomial which satisfies the Diophantine identityFk peak contact force during collision transienta direction component of contact force, relative to and projected onto F°F y direction component of contact force, relative to and projected onto F°F° fixed world coordinate frame with mutually perpendicular unit direction vectors; i, i and iF’ coordinate frame for link 1 with mutually perpendicular unit direction vectors;i, i and iF2 coordinate frame for link 2 with mutually perpendicular unit direction vectors;i, i and iFC coordinate frame at the location of initial contact with mutually perpendicularunit direction vectors; i, i and ixviif vector of q’ polynomial operatorsf vector of free response components of the ith predicted output from time stept + 1 to t + N2; subscript is dropped in SISO casef(t + j) free response part of (t + jt)f natural frequency in Hzfd damped natural frequency in HzG’ N2 x N2 matrix of G polynomial coefficientsG2k N2 x N matrix of G:jk polynomial coefficients obtained by truncating the leftmost N2— N columns from Gk; subscripts dropped in SISO caseC, polynomial product of E(q1)and B(q’)g gravity field vectorg1 vector of q’ polynomial operators; subscripts dropped in SISO caseH(s) n0 x n, matrix of transfer functions in the Laplace domainHo,,(t) homogeneous transformation matrix between coordinate frames F1 and F°H0,2(t) homogeneous transformation matrix between coordinate frames F2 and F°H1,2(t) homogeneous transformation matrix from coordinate frame F2 to frame F’H’,2(t) rigid body component of coordinate transformation from frame F2 to frameF’H2,(t) homogeneous transformation matrix between frames FC and F2H2k(s) Laplace domain transfer function relating the kth input to the ith output; theik element of matrix H(s)I identity matrix‘ok rigid body component of the moment of inertia of link k about joint axis k7-a DC motor armature inertia‘mki mth inertia integral for the ith admissible mode shape function of link ki mode index for link 1 in dynamics model;output index for multivariable system modelxviiia DC motor armature currentJ(s) Laplace transformed matrix of left hand side terms in a system of linearisedequations of motionj ij minor of matrix J(s)J1 quadratic cost function for SISO CPC algorithmJ2 cost function for multivariable predictive control algorithmJ3 overall cost function for multivariable predictive control algorithmJae static equilibrium bias cost functionj mode index for link 2 in dynamics model;time step index in predictive control algorithmKepj DC motor back emf coefficientKT DC motor torque coefficientk link index in dynamics model;input index for multivariable system model;surface index for Hertz contact modelkff effective contact stiffnesske stiffness of the environment surface in the x directionkey stiffness of the environment surface in the y directionk stiffness of the manipulator tip surface in the z directionk3 stiffness of the manipulator tip surface in the y directionL a unit lower triangular matrixL1 length of link 1L2 length of link 2La DC motor armature inductance£ Lagrangian; £ = T — VMk total mass of link kxixm total contact mass; sum of m3 and meme mass of contact region of environmentm3 mass of contact region of manipulator tipN2 prediction horizon in predictive control algorithmNg actuator gear ratioN control horizon in predictive control algorithmn2 number of inputs in multivariable system modelk number of admissible functions included in the structural model of link kn0 number of outputs in multivariable system modelma degree of discrete time input/output model denominator polynomialnb degree of discrete time input/output model numerator polynomialP covariance matrix in a recursive least squares (RLS) parameter estimationalgorithmQ a generalised forceq vector of q’ polynomial operatorsq vector of q’ polynomial operatorsq a generalised coordinateq’ backward shift operator; q’x(t) — 1)R effective radius of curvature of the surfaces in the Hertz contact modelRa DC motor armature resistanceRk radius of curvature of surface k in the Hertz contact model? total energy dissipation functionr(t) position vector, relative to and projected onto F°, for an element of mass onlink 1r(t) position vector, relative to and projected onto F°, for an element of mass onlink 2xxr(t) position vector, relative to and projected onto F°, for the total contact massr(t) position vector, relative to and projected onto .F’, for an element of mass onlink 1r(t) position vector, relative to and projected onto F2, for an element of mass onlink 2r(t) position vector, relative to and projected onto FC, for the total contact masss the Laplace operators1 sine of joint angle 1 (6)2 sine of joint angle 2 (82)sine of [w’10(t) + 82(t)]1a2 sine of [&1(t) + w() + 82(t)]T total kinetic energytimeU(s) n x 1 vector of Laplace transformed inputs to a multivariable systemUk(s) Laplace transform of kth input in a multivariable system modelUk vector of kth control inputs for N2 time steps into the future11k vector of static equilibrium values for kth input for N2 time steps into thefutureUk vector of increments for kth input from time step t to t + N2 — 1; subscript isdropped in SISO caseUk(t) time series of values of the kth input in a discrete time multivariable systemmodel; subscript dropped in SISO caseLuk(t) increment for kth control input at time step t; subscript dropped in SISO caseu value of the kth control input at the current time stepVk volume of material moved due to the deflection of surface k in the Hertz contactmodelV total potential energyxxiv(t) velocity vector, relative to and projected onto F°, for an element of mass onlink 1v(t) velocity vector, relative to and projected onto F°, for an element of mass onlink 2Va DC motor armature voltagevb DC motor back emf voltagew2 vector of future setpoint levels for ith output from time step t + 1 to t + N2;subscript dropped in SISO casewk(xk, t) displacement of the actual centreline from the nominal centreline of the undeformed link kwk0 total deflection of the tip of link k due to bendingxk distance measured along the nominal centreline of link kx direction coordinate of the point of initial contact, relative to and projectedonto F°x direction component of manipulator tip position, relative to and projectedonto F°x direction component of manipulator tip velocity, relative to and projectedonto F°Y(s) n0 x 1 vector of Laplace transformed outputs of a multivariable systemY(s) Laplace transform of ith input in a multivariable system model5r vector of optimal predictions for ith output from time step t + 1 to t + N2;subscript dropped for SISO casey2(t) time series of values of the ith output in a discrete time multivariable systemmodel; subscript dropped in SISO case(t + j t) optimal prediction of y(t -f- j) given the information available at time step t;subscript dropped in SISO caseYw y direction coordinate of the point of initial contact, relative to and projectedonto F°Ytip y direction component of manipulator tip position, relative to and projectedonto F°xxiithj y direction component of manipulator tip velocity, relative to and projectedonto F°Greek lettersa1 local, instantaneous slope of link 1 at its tip; a1 w’10a2 local, instantaneous slope of link 2 at its tip; a2 w0 =ae parameter estimation gain adjustment constant in the exponential forgettingand resetting algorithm (EFRA)/3e constant directly related to the minimum eigenvalue of the covariance matrixin the exponential forgetting and resetting algorithm (EFRA)I3ki solution of the cantilever beam frequency equation for the ith mode of link kA differencing operator; 1—S total deformation of contacting surfaces in Hertz contact mode8e constant inversely related to the maximum eigenvalue of the covariance matrixin the exponential forgetting and resetting algorithm (EFRA)5k deflection of surface k in the Hertz contact modele x direction generalised coordinate for contact dynamics modele y direction generalised coordinate for contact dynamics modeldamping ratio71k kth natural coordinate of a system‘9 vector of parameter estimates in a recursive least squares (RLS) parameterestimation algorithm8 joint angle for link 182 joint angle for link 28 nominal value of joint angle 1O nominal value of joint angle 28a DC motor armature speed8 joint angle rate at the instant of contactxxiiiX predictive controller input increment weighting factorexponential forgetting factor constant in the exponential forgetting and resetting algorithm (EFRA)‘se static equilibrium bias term weighting factor in predictive control algorithma unit vector11k Poisson’s ratio of the material of surface k in the Hertz contact modelaxial foreshortening of a beam due to bendingd axial foreshortening of a differential beam element due to bending(t) time series of uncorrelated random noise valuespk mass per unit length of link kr1 torque applied to link 1 by the actuator at joint 1r2 torque applied to link 2 by the actuator at joint 2-rL load torque applied to the DC motorTm DC motor armature torquec/k regressor vector of past input and output data values for recursive least squares(RLS) parameter estimation algorithmqfi ith admissible mode shape function for link 1q52j jth admissible mode shape function for link 2q5 slope of ith admissible mode shape function for link 1; dqi/dxic4 slope of jth admissible mode shape function for link 2; dq23/dxith modal amplifier for link 1/2j jth modal amplifier for link 2bli first time derivative of ith modal amplifier for link 1; d12/dt1’2j first time derivative jth modal amplifier for link 2; di/’1/dtb1 second time derivative of ith modal amplifier for link 1;d2’e/..’11/dtb2 second time derivative jth modal amplifier for link 2;db11/dtw natural frequency in rad/sxxivAcknowledgementsI wish to thank my research supervisor, Dr. Dale Cherchas, for his many contributions tothe research presented in this thesis. From the initial stages of selecting the topic throughthe 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 andDr. Stan Hutton, during the early stages of the research and their comments which helpedto improve the finished thesis are gratefully acknowledged. I also wish to express my appreciation to Dr. Ray Gosine, Dr. Chris Ma and Dr. Clarence de Silva for their helpfulcomments during and following my oral examination and to Dr. Andrew Goldenberg of theUniversity of Toronto, the external examiner, for his careful reading of my thesis and hisinsightful comments.Funding for the project was provided by the Natural Sciences and Engineering ResearchCouncil (NSERC) in the form of a Post Graduate Scholarship granted to the author and anOperating Grant (OP 00004682) to Dale Cherchas. The British Columbia Advanced SystemInstitute (ASI) also provided funds for scholarships and research equipment. The Universityof British Columbia Department of Mechanical Engineering supported the author throughteaching 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 Department of Mechanical Engineering and Tom Nicol of University Computing Services manyaspects of the research and the thesis preparation would have been much more time consuming, if not impossible.xxvThe experience of doing doctoral research and writing this thesis would have been verymuch more trying were it not for many people not directly involved with the work. Ithank my parents for their ongoing support and encouragement of all of my endeavours andmy brother for his occasional, and usually humorous, reminders of the wide and differentworld 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 andcommiserations.The music of Johannes Brahms, Franz Liszt and George Thorogood helped to smoothsome of the rough spots in the writing of the thesis and CBC Stereo provided almostconstant accompaniment.Finally, I express my deepest thanks to my wife, Susan. She has always been there tolisten, especially when no one else would or could, and she has endured the worst of me overthe past few years. Through all of this she has been a constant source of encouragement.xxviChapter 1Introduction1.1 IntroductionThis thesis discusses research centred on the development of strategies for the control ofthe contact forces exerted by a structurally flexible robotic manipulator on surfaces inits working environment. The ability to sense and control the forces exerted by a roboticmanipulator on its environment has, for some time, been recognised as a necessary and beneficial extension of robotic system capabilities. Force sensing and control offers improvedperformance of tasks which involve contact of the end—effector with the environment byproviding robustness to variations in location and shape of the objects being manipulated.One example of such improved performance is the detection and alleviation of jammingparts during insertion tasks in assembly operations. In addition, force control makes possible tasks which are extremely difficult, if not impossible, under purely positional controlsuch as deburring of stamped metal parts, grinding of welded joints and polishing operations on objects with complex surface shapes. Structural flexibility in the manipulatorlinks presents particular challenges in the context of force control because of the complexdynamics which are introduced between the actuators at the manipulator joints and theforce sensor at the manipulator wrist or tip. These challenges have been met in this studywith the development of a multivariable predictive control algorithm. The algorithm can bereadily implemented as an explicit adaptive controller which provides the controller with theability to compensate for the time varying, nonlinear components of the system dynamics.1Chapter 1. Introduction 21.2 MotivationThis research was motivated both by the academic challenges of understanding and controlling a complex dynamic system and by the need for high performance force controlcapabilities in robotic manipulation. A particular application which has provided inspiration and direction for this work is the identified need [1] for contact force control in thedevelopment of autonomous and telerobotic manipulator systems for the proposed SpaceStation, Freedom.In academic terms, the problem is of interest because the dynamics of even a rigidmanipulator with several links in free motion are strongly coupled and highly nonlinear.The addition of structural dynamics in the links and interaction between the manipulatortip and a working surface increases the number of coupled degrees of freedom in the systemand introduces time varying components into the dynamics. Specifying the modulation ofthe contact forces as the control objective and using the torques generated by actuatorsat the manipulator joints as the inputs to achieve this objective poses a difficult controlproblem because of presence of complex dynamics between the actuators and the sensor.Furthermore, the control problem is inherently discontinuous because the contact force is apositive 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 thereforeprovides no information about the system state. Thus a force control algorithm whichprovides stable, acceptably shaped closed ioop responses must be augmented with a controlstrategy to provide smooth, stable, predictable performance in the absence of contact andduring the transitions of making and breaking contact.The importance of compliance, associated with either the manipulator or the environment, in achieving stable force control with existing industrial robots and controllers is wellChapter 1. Introduction 3established [2]. Manipulators with structurally flexible links or flexible joints naturally provide compliance which is distributed over the entire manipulator structure. This distributedcompliance is helpful in achieving high performance force control by absorbing the impulsewhich occurs when contact is made. Large initial contact forces are currently avoided inmost 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 linksmust be included in the controller design.Space based manipulators with structurally flexible links will, in the near future, berequired to operate under conditions where force control is essential. The Space Shuttle Remote Manipulator System (SSRMS or Canadarm) and the Mobile Servicing System (MSS)and Special Purpose Dextrous Manipulator (SPDM) are all examples of this type of manipulator. Among the tasks which have been proposed for these manipulators are the assemblyof the structural elements of the Space Station Freedom, maintenance and cleaning activities 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 forcesin the presence of significant structural flexibility of the manipulator. As the capabilities ofthese space based systems are proven, new, light-weight earth based manipulators designedfor high speed motion can also be expected to exhibit structural flexibility which will needto be accounted for in the design of both force and motion controllers.1.3 Survey of Related ResearchThe control of the contact forces exerted by robotic manipulators on their working environments has been an active field of research since the 1970’s. Among the earliest reportsin the literature on this subject is the work of Whitney [3]. Though terminology has, inChapter 1. Introduction 4some cases, changed, most of the issues identified by Whitney remain important in current research. The distinction made by Whitney between gross motions (large movementsof the end-effector from one point to another in the workspace) and fine motions (smallmotions during which the possibility or certainty of contact with the environment exists)reflects the fundamentally discontinuous nature of the overall force and motion control ofmanipulators. A particular problem which arises from this discontinuity is that of unexpected collisions between the end-effector and the environment. In the course of analysinga linear force feedback control law Whitney highlights the issue of force controller stabilityand reveals the intimate relationship between controller performance and compliance in themanipulator/sensor/environment system.The problem of simultaneously combining force and motion control was a focus of earlyresearch. The methodologies for solving this problem generally focus on the use of feedbackto 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 selectedcoordinate frame. Fundamental to all of these methods are the needs to select a coordinateframe appropriate to the force/motion task being undertaken and to specify the task in termsof desired force/motion trajectories in that frame. This specification of the manipulatormotion subject to constraints imposed by the task geometry was referred to as compliantmotion by Mason [8].Many force/motion control methodologies implicitly combine the task specification inCartesian 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 byRaibert and Craig [12] is primarily a control system framework for Cartesian compliantmotion into which servo level force and position control algorithms appropriate to particularmanipulator 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 theChapter 1. Introduction 5Cartesian task space. Indeed, it has been shown by An, et al. [14] that, unless carefullystructured, hybrid position/force control can be subject to a kinematic instability due to thematrix inversion of the Jacobian in the feedback path. The more general operational spaceformulation of Khatib [lSj also provides such a framework for non-Cartesian task spacesand kinematically redundant manipulators. Another distinction, cited by An and Hollerbach[16], between the hybrid position/force method and the operational space formulation is thefact that the manipulator dynamics model is included in the latter.In summarising more than a decade of progress in force control Whitney [2j notes thatthe 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 onlyin passing”. Particularly neglected areas of research cited by Whitney include the problemof collisions, the effects of manipulator compliance and the general theory of task spaceconstraint specification. Uchiyama [17] offers a similar list of strengths and weaknesses inthe field.All of the work cited above, and indeed the vast majority of the work to date in roboticmanipulator contact force control, has been focused on completely rigid manipulators. Thisis hardly surprising given the complexity and difficulty of the rigid manipulator problem andthe many fruitful areas for research which were available without considering the additionalcomplexity of dynamics due to flexibility in the joints and links of the manipulator. However, driven by the desire for lighter weight, higher performance industrial manipulators andproposed teleoperated and autonomous applications of large, space based manipulators, attention has been turned to non-rigid manipulators. An increasingly large body of literatureexists which deals with the modelling of joint and link flexibility as well as the problem oftip position control. Comparatively recently, the problems posed in the context of contactforce control by manipulators with significant flexibility in their joint assemblies [18, 19] andstructurally flexible links [20--25] have begun to be addressed. Given the acknowledged linkChapter 1. Introduction 6between physical compliance in the manipulator/sensor/environment system and the performance of force control systems, the possibility exists that manipulators with flexibility mayprovide higher performance contact force control than totally rigid manipulators, providedthat the additional dynamics introduced by the flexibility can be adequately compensatedfor in the controller design.Research into force control with flexible link manipulators is still in its early stages withthe result that, as in the early stages of rigid manipulator force control, attention is beingfocused on stability issues (see Chiou and Shahinpoor [21,23]), fundamental understandingof 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 andYurkovich [28], Matsuno, et al. [25]). The focus of the research reported in this thesis is thedevelopment and application of control algorithms for flexible link manipulator force controland the first steps in the integration of that control algorithm into an overall force/motioncontrol strategy.It is to be expected that, as these fundamental issues are more fully explored and asflexible link research manipulators with more than one or two links are constructed, flexiblelink manipulator contact force control research will draw from and merge with the mainstream of force control research. Indeed the beginnings of this merger is already evident inthe work of Matsuno, et al. [25] which employs the hybrid position/force control frameworkfor a flexible link manipulator configuration.1.4 Contributions of the ThesisThe primary contribution of the research reported in this thesis is the application of apredictive control algorithm in the context of an overall control strategy for the control of thecontact forces exerted by a structurally flexible manipulator on surfaces in its environment.Chapter 1. Introduction 7The models and techniques used in the course of the research which lead to the achievementof this goal have several important features.Modelling: Kinematic and dynamic models of a robotic manipulator with structurallyflexible links interacting with surfaces in its working environment are derived. In contrast tothe simple linear spring model for contact dynamics used in much of the research reportedin the literature, the model derived here includes the effects of the local inertia and dampingas well as the stiffness of the contacting regions of the manipulator tip and the environmentsurface. Analysis shows that a pure stiffness model is sufficient when the mass of thecontacting regions is small, but when the mass of the surface regions deflected due to contactis comparable to the inertias of the links the additional dynamics in the environment modelbecome important. The kinematics and dynamics models are derived in a form which canbe 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 formapplicable 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 controllaw, in particular in selecting the algorithm parameters (horizons and weighting factors) toproduce fast step response rise times and minimal overshoots from among the large set ofparameter values which provide stable control for a given system configuration.A static equilibrium bias term extension has been derived for the predictive control algorithm. It is applicable to both the single and multivariable version of the algorithm. Aterm which weights the deviation of the control inputs from the levels required for staticChapter 1. Introduction 8equilibrium is added to the control algorithm cost function. This addition results in a proportional action feedforward term in the control law which generally reduces step responserise times. The static equilibrium bias term is particularly applicable to manipulator contact force control because there exists a clear static equilibrium relationship between thejoint torques and the contact force components.Contact control logic is introduced to deal with the problem of the discontinuity whichexists in combined force and motion control when contact is made or broken. The contactcontrol logic is implemented at a level just above the joint servo level control laws in themanipulator control hierarchy. Two operational strategies for the contact control logicwhich seek to provide smooth control through unexpected contact events are demonstrated.Linear Analysis: Linear analyses of a variety of configurations of manipulators in contact with environment surfaces provide insight into the fundamental dynamics of the system. The analyses use a combination of analytic and numerical techniques to shown thatthe contact force is dominated by different open ioop modes of the system depending onthe effective contact stiffness of the surfaces. The transfer of dominance among the modesfollows an orderly progression as the effective contact stiffness is increased in a given manipulator/environment configuration. In contrast, the control inputs are shown to provideexcitation to all of the open loop modes. The implications of these results for the implementation of a discrete time predictive force controller are that the controller model shouldinclude all modes of the manipulator dynamics but the sampling rate can be selected onthe basis of mode which makes the dominant contribution to the contact force.Simulation: A general purpose computer simulation program, called TWOFLEX, whichincludes the dynamics model and control algorithms was developed. The dynamics modelin the program can be configured to represent either a single link or a two link planarChapter 1. Introduction 9manipulator with revoliite joints. The links can be rigid or flexible or, in the two linkcase, a combination. The presence or absence of contact between the manipulator tip andthe environment surface is determined by monitoring the relative positions of the tip andthe environment. Contact can be made and broken repeatedly and the dynamics modelis modified accordingly on each change of the contact status. The program was used toexplore the dynamics characteristics of the system and to evaluate the performance of theforce control algorithm and the overall force and motion control strategy.Force Control Performance Evaluation: Simulations results from the TWOFLEX program demonstrate the performance of the explicitly adaptive force and motion control strategy based on the predictive control algorithm for the following configurations and situations:• manipulators with one and two flexible links in contact with environment surfaceswith effective contact stiffnesses varying over a wide range;• prototype model of the two DC electric motor driven, 7.5m long, main links of theMobile Servicing System (MSS) for the proposed Space Station, Freedom; and• use of contact control logic to maintain control through the discontinuous situation ofunexpectedly making contact with an environment surface.1.5 Outline of the ThesisAs an overview, the remainder of the thesis is divided into chapters which discuss thedetails of the manipulator and environment models, the derivation of the control algorithm,the structure the computer simulation program TWOFLEX used to investigate the algorithmperformance, the analysis of the system and the performance of the control algorithm appliedto various manipulator configurations, and a statement of the major results of the researchand recommendations for future work in this area. Following the final chapter is a detailedChapter 1. Introduction 10list of references. The thesis concludes with appendices which provide the mathematicaldetails of the equations of motion for several manipulator configurations, the expansionand minimisation of the control algorithm cost functions, and the derivation of the staticequilibrium bias function.Chapter 2 presents the detailed mathematical models of the manipulator, environmentand actuator systems. It begins with a brief review of the published literature concerningthe modelling of the kinematics, structural characteristics and dynamics of manipulatorswith flexible links as well as the modelling of contact force generation and direct currentelectric motor actuators. In section 2.3 a kinematic representation of the manipulator andenvironment system based on a modified form of Denavit—Hartenberg homogeneous transformation matrices [30,31] is developed. The modification allows the kinematic effects dueto 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 energyand equivalent viscous dissipation functions, and ultimately the equations of motion for atwo 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 tovarious subset configurations of interest is described. The equations resulting from thesesimplifications are included in Appendix A. Section 2.5 presents expressions which relatethe contact force components to the generalised coordinates of the dynamics model. Thesection also includes a discussion of the parameters of the contact dynamics model on thebasis of the material properties of the contacting surfaces. The chapter concludes with thederivation of a model for a voltage controlled DC electric motor actuator and a discussionof 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 atthe heart of the force control strategy is derived. Following an introductory discussion ofthe particular challenges involved in force control for flexible link manipulators, section 3.2Chapter 1. Introduction 11reviews the Ceneralised Predictive Control (GPC) algorithm [29, 32, 33] from which manyof the ideas used in the multivariable algorithm are drawn. The section also includesa derivation of the approximate closed ioop characteristics of a generic GPC controlledsystem. Section 3.3 presents the derivation of a generic multivariable predictive controlalgorithm. This derivation is supported by Appendix B in which the detailed expansionand minimisation of the controller cost functions appear. For specific applications such asforce control, in which a clearly defined static equilibrium state exists, a new, augmentedform of the algorithm is derived by including a static equilibrium bias term (with detailsin Appendices B and C) in the cost function, as described in section 3.4. This term addsa proportional feedforward element to the control law. In section 3.5 the horizon andweighting factor parameters of the control algorithm are discussed in terms of their roles inthe tuning of the controller. The chapter concludes with three sections in which parameterestimation algorithms are reviewed, the implementation of the force control algorithm inthe context of an overall adaptive force and motion control strategy is presented, and andthe stability and convergence characteristics of the algorithm are discussed.The computer program TWOFLEX is described in Chapter 4. This program is a simulationof the continuous time dynamics of the planar two flexible link manipulator and environment system dynamics, the discrete time multivariable predictive control algorithm and theoverall force and motion control strategy. It was developed in the course of the researchto provide a facility for testing and evaluating the performance of the control algorithm aswell as for the study of the open ioop dynamics of the system. The chapter is divided intosections 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 implementation of the discrete time control algorithm (section 4.4) are discussed. A completelisting of the fully documented program (approximately 18,200 lines of FORTRAN) as well asa guide to its use is available as a UBC—CAMROL report [34]. The chapter concludes withChapter 1. Introduction 12the results of a set of validation tests in which the homogeneous response of the manipulator dynamics model in the program was compared with that from a program developedby Mah [35] which simulates an orbiting platform and manipulator system modelled as achain of flexible links connected by flexible joints.Chapter 5 presents the results of linear analyses of the dynamics of several manipulatorand environment configurations, discusses the implications of those analyses for successfulforce control, and presents a collection of simulation results which evaluate the performanceof the control algorithms under a variety of circumstances. In section 5.2 linear analysisof a single link in contact with a deformable environment reveals that the domination ofthe system dynamics by particular modes changes in an orderly fashion as the effectivecontact 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 isdemonstrated in section 5.3 for the case of a single flexible link. Contact with three surfaceswith widely differing effective contact stiffnesses is examined and it is shown how the resultsof the linear analysis and the approximate closed ioop characteristics calculated from thecontrol law are applied to select high performance controllers with fast rise time and negligible overshoot. The problem of dealing with the discontinuous nature of the relationshipthat exists between the contact force and the system state when contact is made or broken 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 operationalstrategies for the contact control logic are proposed and the overall system performance isexamined. In section 5.5 the analysis and performance evaluation of the multivariable forcecontroller for a two link manipulator contacting an environment surface is presented. Thelinear analysis techniques applied in the single link case are employed and reveal expectedextensions of characteristics from that case as well as additional complexities which dependChapter 1. Introduction 13on the configuration of the manipulator, in particular the joint angles. The results of thisanalysis, in conjunction with the approximate closed ioop characteristics calculated fromthe rnultivariable control law are used to select controller parameters which provide fastrise times and minimal overshoots. The controller performance is demonstrated for bothrigid and flexible link manipulators. The effect of interaction between the contact forcesresponses and the influence of the highly nonlinear manipulator dynamics on the controllerperformance are also discused. A final application example is presented in section 5.6 inwhich force control of a prototype model of two links of the Space Station Freedom MobileServicing System (MSS) manipulator is demonstrated. The long, massive links of the MSSare driven by voltage controller DC electric motor actuators which introduce additional dynamics as well as saturation type nonlinearities. Portions of the work presented in Chapter 5have already been published 122,27].The thesis concludes with a summary of the major results and a statement of the originalcontributions of this research in Chapter 6. Several recommendations for directions in futureresearch in this area are also included.Chapter 2Manipulator Modelling2.1 IntroductionThe manipulator model, which forms the basis for the computer simulation program TWOFLEXdescribed in Chapter 4 and the simulation study results presented in Chapter 5 is developedin this chapter. The model is that of a planar two link manipulator with structurally flexiblelinks which can make contact with a deformable environment. The model consists of fourparts:1. the manipulator configuration model, that is, the kinematics and dynamics of theserial chain of links;2. the link model, that is, the structural kinematics and dynamics of each link as aflexible body;3. the contact model, that is, the kinematics and dynamics of the regions of the manipulator tip and the environment that make contact. It is the interaction of these regionsthat 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 transformthe control signals into driving torques applied to the links.This chapter begins with a review of the literature on the modelling of the kinematics anddynamics of structurally flexible manipulators, contact dynamics, and DC motor actuatorsystems. In section 2.3 the kinematic models of a planar two link manipulator with flexible14Chapter 2. Manipulator Modelling 15links and a deformable environment are presented. Following, in section 2.4, is the derivationof the equations of motion of the system and a discussion of the general structure of thoseequations and the nature of the coupling among them. Section 2.5 presents the derivation ofthe expressions for the contact force in terms of the generalised coordinates of the dynamicsmodel as well as a discussion of the selection of numerical values for the parameters of thecontact model. A model of an armature voltage controlled DC electric motor to be used asan actuator is derived in section 2.6 and the coupling of the motor model to the equationsof motion is discussed.2.2 Literature ReviewKinematics: The use of Denavit—Hartenberg [30] homogeneous transformation matricesto 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 kinematicsdue to structural flexibility of manipulator links was made by Book [39]. This approach isattractive in that it is a natural extension of a widely used method.A homogeneous transformation matrix approach, slightly different in form from that ofBook [39], is used in section 2.3.1 to obtain the position and velocity vectors for elementsof 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 veryrecently been proposed by Chang and Hamilton [40]. The concept of their Equivalent RigidLink System (ERLS) is to separate the rigid body and the structural dynamics of themanipulator such that the global motion is described by a large rigid body motion with asuperimposed small motion due to the structural flexibility.Structural Dynamics: The consideration of the structural flexibility in the links of arobotic manipulator introduces a challenging analytical problem, namely, the links can noChapter 2. Manipulator Modelling 16longer be viewed as lumped masses whose motions are described by the motions of theircentres of mass; instead the links must be viewed as distributed parameter systems. As aresult, the motions are explicitly dependent on spatial as well as temporal variables and mustbe described in terms of partial differential equations. Closed form solutions for systemsother than those with very simple geometry and uniform distributions of mass and stiffnessare rare and, as a result, approximate solution methods, generally involving discretisationof the spatial coordinates, are widely used. Two major types of spatial discretisation areavailable [41]: expansion of the solution into a finite series of given functions and thelumped parameter approach. Series expansion methods are more widely used than thelumped 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 flexiblelink manipulator modelling employs either the method of assumed modes [23, 42—45] or thefinite element method [46—48], both of which are Rayleigh—Ritz type methods. The use ofseries expansion discretisation methods raises the issues of selection of appropriate functionsfor use in the series and of the number of terms required in the series [43,49]. For highlyflexible systems consisting of many bodies, such as flexible spacecraft like the Space StationFreedom, the finite element method is often used and an important issue is the selection ofappropriate 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 closedform. These functions are employed as admissible functions in the method of assumed modesin the derivation of the equations of motion for the planar two flexible link manipulator insection 2.4.Equations of Motion: Early analyses of the dynamics of manipulators with structurallyflexible links, such as that by Hughes [53], were largely inspired by the development of theteleoperated Space Shuttle Remote Manipulator System (SSRMS), or Canadarm. AnalysesChapter 2. Manipulator Modelling 17of more general flexible robotic manipulators, such as that of Kelly and Huston [54], oftendrew 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 matrices to describe both joint and link deflection kinematics and uses Lagrange’s equationto 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 alternative approach to the modelling of link flexibility, which is briefly discussed by Book, isthe introduction of “imaginary” joints into a strictly rigid link dynamics model such as theNewton—Euler formulation of Walker and Orin [57]. Book concludes that the computationalcomplexity of the Lagrangian approach is significantly greater than that of an equivalentNewton—Euler formulation. The work of Silver [58], however, suggests that this need notnecessarily be so. In any case, the Lagrangian formulation provides better physical insightinto the interaction of structural and rigid body dynamics.The majority of recent analyses of flexible manipulator dynamics employ energy methodsto 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 spatialcoordinates of the link structural dynamics.Dynamic analyses of flexible link manipulators making contact with their working environment are rare in the literature. Most researchers [21, 23, 24, 26] model the contactforce as arising due to the compression of a simple linear spring connecting the manipulatortip to the environment, thus lumping the contact dynamics into a single element. In thecourse of the present research a more complete development of the dynamics of a singleflexible link contacting a deformable environment has been achieved and reported in theliterature [22,27]. That model includes a dynamic model of the areas of contact betweenthe manipulator tip and the environment surface. The model considers the stiffnesses ofChapter 2. Manipulator Modelling 18the contacting surfaces, energy dissipation within the materials and the inertia of the surface material in the regions surrounding the point of contact on each surface. Section 2.4presents an expanded version of that development. The equations of motion for a planartwo flexible link manipulator in contact with a deformable environment are derived usingLagrangian 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 “controlanalyses of arms in contact with an environment are rare” and that most papers “containno model of the origin of the contact forces themselves. Those that do are restricted tomodelling force as arising from deformation of some elastic elements.”The literature on the solid mechanics of contact is extensive [62], however, the vastmajority of these analyses assumes that the contact is quasi-static. Analyses of dynamiceffects are generally concerned with the generation and propagation of stress waves in thecontacting bodies. For contact which does not involve plastic deformation of the surfaceJohnson [62] shows that the dynamic response of an elastic half-space can be modelledwith good approximation by an elastic spring in parallel with a viscous damper, the latterrepresenting the energy dissipation of the stress waves. The theory of elasticity, in particularthe 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 deflectionsunder quasi-static conditions.The contact model included in the dynamic analysis of section 2.4 uses the parallel springand damper model as well as a surface mass. Results from the theory of elasticity are usedin section 2.5.1 to provide estimates of the numerical values of the model coefficients basedon physical material properties.Chapter 2. Manipulator Modelling 19Figure 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 actuator system discussed in section 2.6 is widely used and accepted models are readily availableboth in the general literature [65] and for specific application to robotics [14].2.3 Kinematics2.3.1 Manipulator Configuration ModelFigure 2.1 shows a schematic representation of a planar two link manipulator with structurally flexible links in contact with a deformable environment. The geometry of the systemFyFxy w1(x,t)xChapter 2. Manipulator Modelling 20is described in terms of the four coordinate frames shown in Figure 2.2. Frame F° is thefixed, world coordinate frame with the joint of link 1 located at its origin. Frame F’ isfixed at the tip of link 1, Frame F2 is fixed at the tip of link 2 and frame FD is fixed at thepoint of contact between the surfaces of link 2 and the environment. The orientation of thenominal centreline of link 1 with respect to the world frame is given by the angle 19-, measured counter-clockwise from the i axis of F°. Likewise, 82, measured counter-clockwisefrom the i axis of F1, gives the orientation of the nominal centreline of link 2 with respectto 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 measuredalong the nominal centreline relative to F’.The geometric relationships among the coordinate frames can be compactly describedusing homogeneous transformation matrices [30]. The transformation between frames F’and F° isc1 —s 0 L,c,s, c1 0 L1sH0,,(t) = (2.1)00100001where L, is the length of link 1, .s,= sin81(t) and c1= cos8i(t). Similarly, the transformation,C2 2 0 L2cH,2(t) = L2s (2.2)000 1Chapter 2. Manipulator Modelling01, •0Ii21Figure 2.2: Coordinate frames for a two link flexible manipulator in contact with a deform-C2ii02)2T//w2(x,t) ,“5C‘2I//•012Iiw1 ( x1, t1- /FOable surface.Chapter 2. Manipulator Modelling 22where L2 is the length of link 2, s2= sin02(t) and c2= cos &2(t), accounts for the rigid bodycomponents of rotation and translation between frames F2 and F’.An additional rotation and translation due to the structural deformation of link 1 mustalso be considered. Based on ideas introduced by Book [31], the structural deformation oflink 1 relative to the undeformed centreline is described by the transformation matrix,cosa, —sina1 0 0sina1 cosa1 0 w10D,(t) = (2.3)0 0 100 0 018w,0where to10 = wi(0, t) and a = to,0 =(Ix,that is, a1 is the local, instantaneous slope of link 1 at its tip. Premultiplying H2(t) byD1(t), we obtain the homogeneous transformation matrix H1,2(t) which accounts for boththe rigid and flexible body effects in the geometric description of link 2 relative to andprojected on to F’:Ca12 a12 08a 2 Ca 2 0 L2sa 2 + W10IT12(t) = D,(t) 11,2(t) = 1 1 1 (2.4)0 0 1 00 0 0 1where a2= sin [w’10(t) + 2(t)] and caj2= cos [w0(t) +02(t)]. Finally, premultiplicationof H,,2(t) by H0,1(t) gives H0,2(t) which describes the geometry of link 2 relative to andprojected on to the world coordinate frame F°:C12 31a2 0 L,c1 — w,0s, + L2CIa15ia2 C1a12 0 L,s1 + W10C + L231aH0,2(t) = H0,1(t)H,,2() = , (2.5)0 0 1 00 0 0 1Chapter 2. Manipulator Modelling 23where 1a2= sin[1(t) + w’0(t) + 82(t)j and ci12 cos [81(t) + w0(t) +02(t)j.The structural deformation of link 2 introduces additional rotation and translation whichis described bycos2 —sin2 0 0sin c2 cos c2 0 w20D2(t) (2.6)0 0 100 0 018w2where w20 = w2(0,t) and c2 = w20 =Ux2The position, relative to and projected on to F1, of an element of mass, dm1, located adistance —x1 from the tip of link 1 is—xlw1(—x t)r(t)= ‘. (2.7)01Premultiplying r(t) by H0,1(t) gives the position, relative to and projected on to F°, ofdrn1:(L1 — xi)ci— w1sr(t) = (L1 x1)s +w1c (2.8)1Likewise, the position, relative to and projected on to F2, of an element of mass, dm2,Chapter 2. Manipulator Modelling 24located a distance —a2 from the tip of link 2 isw2(—a,t)r(t) (2.9)01and premultiplication by H0,2(t) gives the position, relative to and projected on to F°, ofdm2:L1c— 11)131 + (L2 — x2)ci12 —L1s + W1C + (L2 2)31a22 + W2C1cii2 (2.10)1Differentiating r(t) and r(t) with respect to time yields the velocities, v(t) and v(t),of the elemental masses dm1 and dm2, relative to and projected on to .F°:— [(L1—x) s1 + wici] Oi —v(t) = r(t) [(L1 — x1) Ci—:‘‘6 + W1C (2.11)0and— (Lisi + w10c)Si — iosi — (L2 — x2) 122 (ê1 + + 82)(ô1 + + 82) — i2S1av(t) = -f-r(t)(Lici— w10s)Si — zij10c + (L2 — x2)c12 (ô1 + w + 82)dt+222 (& + W + &2) — th2Clai200(2.12)Chapter 2. Manipulator Modelling 25In these expressions dotted quantities denote derivatives with respect to t and primedquantities 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 insection 2.4 to form the kinetic energy, potential energy and dissipation function expressionsfor the manipulator.2.3.2 Flexible Link ModelStructurally flexible manipulator links are distributed parameter systems and thus have aninfinite number of degrees of freedom and must be modelled by partial differential equationswith both temporal and spatial independent variables. The inherent difficulty of solvingpartial differential equations with space dependent coefficients, particularly in satisfyingboundary conditions, makes the use of approximate solution methods attractive, if notessential.For this research the method of assumed modes [41] is used. That is, the spatial coordinate is discretised into a series expansion of admissible functions each multiplied by ageneralised coordinate. The mode shape functions of a cantilever beam are employed as theset of admissible functions. For the case of an Euler-Bernoulli beam, in which the effectsof rotary inertia and shear deformation are neglected, the mode shape functions are readilyderived in closed form [66]. While it is clearly evident that the boundary conditions for eachmember of a chain of flexible manipulator links connected by torque producing actuators aremuch more complex than those for a cantilever beam, it should be noted that the method ofassumed modes permits the use of any set of admissible functions; the number of terms inthe series determines the accuracy of the approximation. In particular the issue of whethera pinned—free or a cantilever (fixed—free) beam model provides the more appropriate set ofadmissible functions has been resolved both experimentally [43] and analytically [49]. Theaccuracy of the predicted dynamic response, with a given number of included modes, isChapter 2. Manipulator Modelling 26Figure 2.3: Free body diagram of aEuler—Bernoulli beam bending equation.found to be greater when cantilever beam mode shape functions are employed than whenthose for a pinned—free beam are used.Force and moment balances on a differential element of a beam (see Figure 2.3) lead tothe 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 distribution of flexural rigidity. This partial differential equation is separable. Letting w(x, t) =4(x)?/(t), the partial derivatives can be expressed as:= qb andand the beam equation rewritten as-= (EI”)” (2.14)zF (xt)MdSS + — dxdxw(x,t)_1_ dxdMM + — dxdxdifferential beam element for the derivation of theô2w(x,t) —8x2—Setting each side of equation (2.14) equal to a separation constant, w2, yields=0 (2.15)Chapter 2. Manipulator Modelling 27and(EJqY’)”— w2pq = 0. (2.16)Equation (2.15), subject to a pair of initial conditions, describes the temporal variationof the shape of the beam while equation (2.16), combined with a set of four boundaryconditions, describes the spatial variations. It is this latter equation that provides themode shape functions which will be used as admissible functions in modelling the flexiblelink dynamics.Assuming p and El to be constant along the length of the beam, equation (2.16) isrewritten as— = 0, (2.17)where=(2.18)Equation (2.17) is a linear homogeneous ordinary differential equation with constant coefficients for which the solution is4(z) = Asinh/3x + B cosh3x + Csin/3x + D cos!3x. (2.19)The boundary conditions which facilitate evaluation of the arbitrary constants in equation (2.19) are, for a cantilever beam of length L, zero displacement and slope at the fixedend and zero shear and moment at the free end; in mathematical terms8ww=o = 0orEI = 0 EI4 = 0, (2.20)= 0 = 0‘7H=L — 0 IL = 0. (2.21)Chapter 2. Manipulator Modelling 28Application 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 isrelated 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 isq(x) = cosh,Bx — cos/3x — (sinh,82x— sinf3x) (2.23)In this form the mode shape functions are normalised such thatJL= L (2.24)and they are orthogonal, that is,Lf 4j(x)j(x) dx -0, i j. (2.25)These mode shape functions are used as admissible functions for the method of assumedmodes 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 is4(x) ==_,i [sinix + sin jx+ (:: (cos/3x — cosh/3ix)](2.26)these slope expressions are used to calculate = w0, the local, instantaneous slopes ofthe links at their tips which appear in the structural deformation transformation matricesgiven 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 asthe beam deflects, its length, measured along the nominal (undeformed) centerline decreases.The magnitude of this axial foreshortening can be calculated by considering an element ofthe deflected beam as shown in Figure 2.4. The decrease in length of the beam element isChapter 2. Manipulator Modelling 29Rewriting the cosine function2.3.3 Contact Modeldx Incos cos ( qSbJds \i=1 j(2.29)(2.30)1Figure 2.4: Axial foreshortening of a differential beam element.d = ds — dx. From the geometry of the situation we see that(2.27)(2.28)that is, the cosine of the local, instantaneous slope. Thusd= [1cosand so= jL[i_ cos ds.in this integral as a series expansion,\2 /nL ()=j1 — 1— Z=121—214! — ds,we see that the is O(’].). Since the elastic deformations of the links, which are proportionalto are assumed to be small the foreshortening of the links will be neglected.A manipulator making and breaking contact with its environment must be viewed not as asingle dynamic system but rather as two closely related systems. At the instant of contactChapter 2. Manipulator Modelling 30a kinematic constraint, such that the position, velocity and acceleration of the contactingsurfaces are identical, is enforced on the system for the duration of the contact. The contactforce arises from the interaction of the contacting surfaces. Conversely, on breaking contactthe kinematic constraint is removed, the surfaces move independently and the contact forceis, by definition, zero.In order to predict the contact force which will arise between the manipulator tip andthe environment surface a contact force model is required. The model used in the presentresearch is based on results reported by Johnson [62] which show that the dynamic responseof an elastic half-space can be modelled with good approximation by a linear elastic springin parallel with a viscous damper, the latter representing the energy dissipated by thestress waves radiating through the material surrounding the region of contact. This modelneglects reflection of these stress waves. As indicated in the inset of Figure 2.1, this model isadopted by representing the contacting surfaces of the link and the environment as masseson damped linear elastic foundations. The contact coordinate frame, .Fc, is located at thepoint of initial contact between the manipulator tip and the environment. Relative to andprojected onto the world frame, F°, the origin of .Fc is located at Xw+Yw.The contact masses, m and me, represent the inertia of the contact regions of themanipulator tip and the environment, respectively. These masses should not be confusedwith the total mass of those bodies. It is the compression of these regions that gives riseto the contact force. The deformation of the surfaces during contact is represented by themotion, ei+Ei of the total contact mass m (the sum of the contact region masses mand me).The contact coordinate frame, Fc, is chosen with the same orientation as the fixed worldframe, F°. This is tantamount to assuming that the contact force is measured in world coordinates, a valid assumption since such transformations are easily and routinely performed bythe hardware and software associated with wrist force sensors. The transformation betweenChapter 2. Manipulator Modelling 31fC and .F2 is thus simply a rotation of — [81(t) + w’0(t) + 82(t) +w0(L)]:C1i2a2 81a2 0 0H2(t)= 81cj22 C1a12a 0 0 (2.31)o 0 100 0 01and hence the transformation between FC and F° is a simple translation, obtained bypremultiplying H2,(t) by D2(t) and Ho,2(t):1 0 0 L1c — w10s +L2C1a1 W2oSlai20 1 0 L1s + w10c + L2s112 + w20c1a2H0,2(t)D2(t)H2,(t) = . (2.32)001 0000 1The position of the contact mass, m, relative to and projected on to F’, is= U (2.33)01Premultiplying r(t) by H0,(t) gives the position vector, relative to and projected onto LF°:L1c— w10s + L2C1a1—W2081a2+ E2L1s + wiOc + L281a1 + W20C112 + Eyr(t) = Ho,(t)r(t) = . (2.34)01The velocity vector, v(t) for the contact mass, relative to and projected onto F°, is obtainedChapter 2. Manipulator Modelling 32by differentiating r(t) with respect to t:— (Lisi + w10c)Oi — — (L2s1a+ wloclaj2) + +W2o81ai2 + Ev(t) = r(t) = (Lici — w10s)01 +i10c + (L2C1a2 — WioSiai2)(é1 + Wio + 02)+‘th2oClcxi2 + y00(2.35)The position vector r(t) and velocity vector v(t) are used to derive the kinetic energy, potential energy and dissipation function expressions for the contact dynamics insection 2.4.2.4 Derivation of Equations of MotionGiven the position and velocity vectors for the elemental link masses and the the contactregion mass (derived in section 2.3), the equations of motion for the manipulator and environment 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 roboticmanipulators is widely used [23,31,48,60]. It has the advantage that the resulting equationsare 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 themovement of the centres of mass of the bodies as they deform is avoided. The structure ofthe 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 configurations of the manipulator and environment system, such as a single flexible link withChapter 2. Manipulator Modelling 33unidirectional contact, were derived manually [22]. The equations for the two flexible linksystem, presented below, were derived with the assistance of the Maple V algebraic computation system [67]. Maple V proved invaluable in helping to avoid transcription andother mechanical errors which often arise in the course of manual algebraic manipulations.Furthermore, Maple V includes a rudimentary FORTRAN code generator, which was usedto translate (with minimal errors) the equations of motion into source code which servedas starting points for the simulation subroutines which calculate the terms of the inertiamatrix and the right hand sides of the equations. As well, the J.TEX code generator inMaple V was used to assist in the preparation of the equations of motion for typesetting inthis document.The total kinetic energy, 7, of the system is= 2 JMv dm1 +1M2v dm2 + mcvT v, (2.36)where Mk is the total mass of link k. The first two terms are the kinetic energies of thelinks and the last term is the kinetic energy of the contact region. Expanding the vectordot products in equation (2.36) and evaluating the integrals over the link masses (assumingconstant distributions of mass over the lengths of the links), the total kinetic energy isrewritten as:7-= Io1o+1Iiiiiioi+ ‘21j(b +e)+M2 (L + + [102+fl2] (ô1 + + é2) + iitb+I22j + [M2 (LiL2ci+ L2WloSai2&1 +L2c12o) + h12’2i— (L1Sa2Ô — + 10s2) I42i2i] (i + w10 + 62) +M2L1810Chapter 2. Manipulator Modelling 34+ (LiCai2i+ WloSal2ô+ai2io) 14221 + ( + é + + ti)4m [(L + + (L + t4) (a1 + +—m [(Lisi + wi0ci) Ô1 + (L2si12 + W2oClai2) (ô1 + W10 + 02)] x+m [(Lici — w10s)01 + (L2C1a1 —w20s112)(1 + 1o + 02)]—me, (tiii0s + W2o31a12) + m (zhi0ci + tb2ocIaj2) Ey+m [— (L18a12— wlocaj2) w20Ô1 + (L2c12 — W205a12)tub] (ô1 + W10 + 02)+m [(LiL2a +L2Wioai)01 + L2tuio] (o1 + t140 + 02)+m [(L1Ca2+ wbosai2)01tv20 + W102Ca12+L18tu0] . (2.37)The assumption of constant mass distribution in the links is made for simplicity; anymass distribution function, pk(xk), may be chosen and the integrals evaluated in closedform or numerically, as necessary. The summations in equation (2.37) are the result of theapplication of the method of assumed modes [41] to discretise the spatial variations of thelink motions. The upper limits of summation, k, are the number of admissible functionsincluded in the structural model of link k. The subscripted I factors are integrals of theadmissible functions. They are discussed in detail below, following the statement of thesystem potential energy.The link kinetic energy terms in equation (2.37) are in agreement with an appropriatesubset of the kinetic energy terms for a model of an orbitting space platform based mobileflexible manipulator reported by Chan [681. Since the kinematic representation used byChan is markedly different from that described in section 2.3.1 the agreement of kineticenergy expressions serves to validate the method used here.The total potential energy, 1), of the system is1) =— J gT r dm1 — J gT r dm2M1Chapter 2. Manipulator Modelling 351 1 1 2+>T3ii’tb +2+k8(Lici — W103 + L2C1a1 — W2oS1c2 — xw — E)+k8(Ljsi + W10C + L231a1 + W2oClai2 Yw — )22 j. 2mwhere g is the gravity field vector. The first two terms are the gravitational potentialenergies of the links. They are followed by the elastic potential energies of the links dueto their structural stiffnesses. The last four terms are the elastic potential energies of thecontacting surfaces of the manipulator tip and the environment with the point of initialcontact located at xi + The zero condition for the springs used to represent thecontact stiffness is that of zero force.The bk factors in equation (2.37) represent the rigid body components of the linkmoments of inertia about the joint axis:1 2‘ok = (Lk — Xk) dmk. (2.39)J MkThe other subscripted I factors in equations (2.37) and (2.38) represent integrals whichinvolve the set of admissible functions used as assumed modes to model the link structuraldynamics. Given a set of admissible functions, these integrals evaluate to constants inthe equations of motion. Employing the cantilever beam mode shape functions derived insection 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)I3kwhere 13k1 is the solution of the cantilever beam frequency equation (equation (2.22)) forthe ith mode of link k and pk is the mass per unit length of link k,‘2ki= J k(Xk)2dmk = pkLk = Mk, (2.41)MkChapter 2. Manipulator Modelling 3613kiJLkElk(82)2dxk = EIkLk MkWL, (2.42)where Elk is the fiexural rigidity of link k in bending about its joint axis, and,I pk (sinh/3k2Lk smI3kLk N‘4ki = 4’k(xk)dmk = 2— i (2.43)JMk /3kz \cosh/3kJk+cos/3k:LkJAll of the I factors are retained as general constants in this derivation and in the subsequent simulations to facilitate the use of any arbitrary set of admissible functions and linkdimensions.Assuming that the energy dissipation in the system can be modelled using equivalentviscous damping, the total dissipation function, R., for the system is= +1 2+ b12’4 +[(Lisi + wi0ci)Ôi + (L2ia12 + W2oC1a2)(ê1 + o + 2)+W10S + W2oSlai2 + J+b8 [(Lici— wi0s1)+ (L2ci12 —w20s112)(o + w + 82)+w10c+ W20C — Ej1 1ezExwhere the subscripted b terms are equivalent viscous damping coefficients. The total dissipation function is somewhat analogous to the total potential energy function. The first twoterms represent the energy dissipated due to friction in the link joints. They are followed byterms for losses in the links due to structural damping. The last four terms are the energydissipated in the materials of the contacting surfaces of the manipulator tip and the environment. The dissipation of energy in the links and the contact regions is due to hysterisisassociated with cyclic stressing of the materials. The selection of damping coefficient valuesChapter 2. Manipulator Modelling 37is somewhat complicated by the fact that the energy dissipation is dependent on a numberof factors including temperature, stressing frequency and stress level. Lazan [69] providesa collection of experimentally determined loss factors which are the ratio of the energydissipated per cycle to the maximum strain energy in the material. Assuming harmonic excitation these data can be used to estimate damping coefficient values of lumped parametersystems following the method described by Meirovitch [70]. However, for more general situations Gaylord and Gaylord [71] recommend that the damping in most structural membersbe 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), theequations of motion are given by Lagrange’s equation, augmented with a disspation term:d (ôL’ &C oR.o) = ——+ Q (2.45)where q is one of the system variables,1, 02, ,i = 1,... ,ni}, ,j = 1,... ,n}, e,selected as generalised coordinates and Q is the corresponding generalised force. The complete set of equations of motion consists of 4 + ni + n2 strongly coupled, nonlinear, secondorder, ordinary differential equations. The general structure of each of the equations isQ + bj + kT + g(q) + N(q, 4), (2.46)where I = I(q) is a vector of inertia terms, j=j(t) is a vector of the second time derivativesof the generalised coordinates (generalised accelerations), Q is a generalised force, b = b(q)is a vector of dissipation coefficients,=4(t) is a vector of the first time derivatives ofthe 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 thegeometry of the system), q q(t) is a vector of the generalised coordinates, g(q) is theChapter 2. Manipulator Modelling 38gravity term and N(q, ) is a collection of centripetal and Coriolis terms which involveproducts of the generalised velocities.For the O generalised coordinate the generalised force is T1, the torque applied to joint 1,and, after simplification and collection of only second time derivative terms on the left handside, equation (2.45) yields the equation of motion which will be referred to as the rigidbody equation for link 1:[i + 102 + M2 (L + w0 +L12c2 +L2ioai)722+I2iz”,b1j+I22i,b — 2 (Liat — WioCaj2)142jI’2j+m (L +w0+L + wo — 2 (LlW2oSai2—WIOW20Ca12 L12Ca2 L2W10Sa12))j i+ I11i11i+ [M2 (L1 +L2c12)— I:: + m (L1 + L2c12 —2oSa)] L+ 1o2 + -M2L(L1Ca12+ WloSai2) + 122jI’2j — (L18a12— wlocai2)j=1 j=1+m (L + w0 + LiL2ca12 — L1W2OSa2+ L2wlosai2 + W1OW2QCa2)]+ [102 +ML(Lic12+W10Sa2)+I22j — (Lia12+m (L + w0 +L12Ca2 —L1w20s2 + L2wlosaj2 + WloW2oCai2)] 02+ + m [c12Li +s12w0 + L2] 2jo2j + [Liai2 + wlosa12] Ij/7—m [Lisi + w10c + L2s112 + W2OC1a12]E+m [Lici — wiosi +L2c1a — W2081a12]Ey= T1 —+b35 (L1s + W10C + L251a1 + W2oClai2) [(Lisi + w10c + L2Sai2 + W2oClai2) 6Chapter 2. Manipulator Modelling 39(L231a1+w20c12)62 — W10S — — (L2S1a1 +w20c112)— x]—b3 (Lici — W10S + L2C1a1 — W2031a12) [(Lici — wiosi + L2C1a1 — W2081a12)8i+ (L2C1a2 — W2OS1a12)62 + mc + tii2c112 + (L2c1a12— W2oSlai2) o— y]+k (Lisi + W10C + L2S1a+ W2oClai2) (Lici — wi0s +L2c112 — W2081a1 ——e)—k8 (Lci — w10s + L2c112 — W2Q81a12)(Lisi + w10c + L2s1a+ W2oC1ci2 Yw — E)+gx (MiLsi + C1 + M2L1S + M2w10c+ M2LSiai +—gy (_M1Lc— i I41j1j + M2L1c— M2w10s+ M2LCiai—812 I42j2ji=1 j=1+ { [ML2(L13a12 — W1OCa2) + 2 (Lic12 + WloSai2) 142j’2j+2mg (LiL2s12 +L1W20Ca —L2w10c2 + WloW2oSai2)] 82+1I2 [L2 (Lis12 — W1OCa2)4c — (L28a12+ 2wi0)thj2I1b1 — 2122 b2+2 [(Llcai2 +w10s2)Wo— Cak2Wlo] ‘42j23 + 2 (L13a1 — w10c2) ‘42j2j—2mg [(w10 + L28a1 + W2OCa12)11)10 — (L1sa12 — W1OCa2 — w20)w20j—2m0[(L1L2s2 + Liw2ocai + W1OW20Sa2— L2W10Ca2)1140} i+ [M2L(L1Sa12 — wlocai2) + (L1ca2+ hl1o3ai2)I42j2j+m0(L1L2Sa +L1w20c2 +w102s— L2ioCai2)]+ [M2L(Lisai2—c12wi0)11i_ + 2(Lisai —+2 (L1Ca1 + 5ai2t’1o) lL I42j1’2j + 2m0 (L13a12 — W10Ca12— w20)12oj=1Chapter 2. Manipulator Modelling 40+2m (LiL2a— wloL2caj2 + L1W2OCa2+w102s2)io] 2+ [M2L(Liai WloCai2)+(LlCai2+W1o3ai2)I42j2j+m(L1L2s2 —w10L2c+L1w20ca2 + WloW2oSai2)] Zi+2 [(L1Sa— wIocai2)I42j2j— I22j2j2j +2mc(Llsai2 — WloCai2 —w2o)b](2.47)Similarly, for link 2 with the generalised coordinate 2 and the corresponding joint torque,r2, the rigid body equation is:[102 + M2L(L1Ca12+w10s2)+ I22j’ — (L13a12—+m (L + wL +L12c2 — L1W2oSa2 +L2W10Sa + WloW2oCal2)1 Oi+ [M2L::ai — + mc — Sa12W20)]+ [102 + I22j + m (L+ [102 + I22j + m (L + w0)] 2 + I12j2j +—m [L2si12 +w20c112}ë + m [L2ci12 —w20si]ë,=r2—b02—b3 (L281a1 + W2øClai2) [(Lisi + W10C + L251a1 + w2oclai2) &1+ (L251a1 + W2OCIa12)02 + +l0sl + i2os1ai2 + (L251a1 + W2oClai2)1140]b8 (L2C1a1 — W2051a12) [(Lici — wiosi + L2C1a1 — w2oslai2) &i— (L2ci12— w20s1a2)82 — é +(10c+ th2oClai2 + (L2ci12 —w20s12)+k8(L2s112 + w2ocIai2) (Lici — w10s + L2c1a1 W2o51cx2—Chapter 2. Manipulator Modelling 41—k3 (L2C1a1- w2oslai2) (Lisi + w10c +L2s112 + W2oCicj2 Yw—Ey)1 fl2+gx (M2Lsii+ C1ai2I422)— gy (M2Liai— 81ai2I422)+ [M2L(WloCai2— L1Sa2) — (L1ca12+ W1oSai2)I42j2j—m (LjL2sai + Liw2ocaj2 WioL2Cai + wlow2oSai2)] è— [M2Laiio+ 2Cai2ioI42j2j ++2m [(L2Sa12+w20c12)io +w2olb]] 8— + 2mcwo](+ &). (248)The time variation of the ith mode of the shape of link 1 is given by the generalised coordinate i& for which the corresponding generalised force is zero. Applying equation (2.45)yields the deflection equation for link 1:+ [M2 (L1 + L2Cai2)—3ai21oI42j’2j + m(Li + L2ca12 — W208a12)]+ [102 + M2L(Lic12 + Wioa12)+ I22j — (Lis12 —+m (L + wL +L12Ca2 — LlW2oSat2 +L2w10s+ W1OW2OCa2)] io] i+I21j1j + {M210 + (M2Lci Sai2I42i)2)+rnc [qizo + (L2Ca12— W203a12)14]} 412oh/’1z+ { (M2LCai — Sai2I422i) 1io + (102 + I22))Chapter 2. Manipulator Modelling 42+m [(L2Ca12—w20s12) + ( +)] }+ { [cai: — 142j2j + m (L2ca12— 1112o3a2)] &jo+ ‘o2 + 122ij + rn (L + w)] iQ} 2fl2-f-jo + cai2liwI42b2+ [m1.(L2q10 + ca12qiio)]3=1 j=1 j=1—rn [1n81 + (L2s1a12+ W2OC1a12)4cJ a, + rn [4i0ci + (+L2ciai — w2081a12)1jo] E= —b1J.——b3(10s + L2S1a2j0+ W2oC1ai2j0)[(Lisi + wci + L251a1 + w2oclai2) 01+ (w2oclai2 + L251a12)02 + e + W1o81 + W2031a2+ (L231a1+w20ci12)tb]—b3(10c+ L2C1aL2iO —w20s1120)[(Lici — W10S + L2C1a1 —w20s112)Ô1+ (L2ci12— W2o31a12)02 — Ey + 1ol + W20C12 + (L2ci12 —w20s112)+k8 + L2S1ai26jO + W2oC1ai294o) (Liei — wi0s + L2C1a2 W2o81a2—— e)—k (410c + L2c1a12ç/4j0— W2oS1ai24j0)(Lisi + w10c + L2s112 + w20c1a2 — E)+gxslI4li —+ {1211 + [M2 (L2Sai2 + 10) + cai2 142j’2j + m (w10 + L2s12 +w20c12)] 1jO—[M2L(Llsai2 — W1QCa2) + (Llcai2 + l111o5ai2)I42i2i] io—m(L1L2Sa2 +L1W20Ca —L2wiocai + WloW2oSai2)+ { [M2L12 + 2c12 I42j2j + 2m (L2s12 + :12)] 1io02+2 MLaiti4o + a2 I42j&2j + Cai2tL4o I42jlI’2j:1=1 j=1Chapter 2. Manipulator Modelling 43+m (Sai22o + (L2sai2 + w2ocaj2)2[fLaiio+ I22j2j2 + Cai2WloI4j)+m(w20b+ (L2Sa12+ W2ocai2)1o)] io}+ [M2Lai+ Cai2I42j2j+ mc(L23a12+ W2ocai2)]+2 { [MLSaitoiio + Sai2I42j2j+ Cai2W1oI4jJ2+mc(L2saio+w2oC12W0+ 1jo — + mcw2oti) io} 82+ [M2LSai + Ca12 ‘42j2j + m (L2Sa12+ W2oCai2)) 1jo+2 [(Sai2I422 + mcsai2zio) iio — + mcw2oui) io] to (2.49)Likewise, the deflection equation from the jth mode of link 2 is obtained from (2.45) usingthe generalised coordinate b2i:[12j + (L1CaI2 + Sai2Wlo)142j + m (Lic12 + w10s2 + L2) 2jo1+ [Cai2142 + mc1220] &jo1i + [12j + mL22501 oij+[12j + mL220j82 + ‘22j2j + m20 2jo2j — +mc2joclaiëv= —b2/’—b8x2joS1ai2 [(Lisi + wioci +L2s112 +w20ci12)8 + (L2s12 +w20ci12)82+6rW1o81 + W2051cz2+ (L251cz1+ w2oc1a12)i4o}bsy2joC1a2 [(Lici — ws1 + L2c1a1 — w2oslai2) Oi + (L2Ca12 W2o51a12)826!,W1OC + W2oClcxi2 + (L2C1a1 W2o51a12)th0JChapter 2. Manipulator Modelling 44+ksx42joS1ai2 (Lici — w10s +L2c112 ————ksyq52jociai (Lisi + WioC + L2S1a1 + W2oClai2— Yw — e1)+gs1aj2I42j— 9yC1ai2142j+ [I22j’2j — (L18a12— tUloCai2)142j + m (wlocai2 — L1Sa2 + w20)q52jo] 0+2 {I22&2 + mw2o0102— ai2142j1o + I22b2iW0— m (sa12thio — w20t140)b20}i+[122j’c/-’2j + mcw2oqjo] (th, + & + 2i4o0) (2.50)The generalised coordinate for the x direction component (in the world frame) of thecontact mass deflection is e and the x direction contact equation is:—m [Lisi + L2S1a1 + W10C + W2C112]01 — ms1 1jo1j—m [L2s112 + w20c1a2] — m [L231a1 + W2OC1a12I02—ms112 2jo2j + mEbexx — kexEx—b [(Lisi + wioc1 + L2S1a1 + W2OC1a12)Oi + (L231a1 + w20c1a2)02+E + W10S + W2oSlcxi2 + (W2oClai2 + L281a12)ti4ol+ksx (L1c — w10s + L2c1a1 — — — Ex)+m (Lici — w10s + L2c1a12 W2031a12)0 + m(L2ci12 — W2051a12)02+m(L2ci12 —w20s112)tb + 2m?i0c112?d4+2m {(L2c112 — W2051a12)02 + 10c + zb20c12 + (L2ci12 — w2031a12)4] ôi+2m [(L2cm1 — W2081a12)l)1o + W2oC1ci2j 02. (2.51)Finally, the y direction contact equation, obtained with the generalised coordinate es,, is:ii [Lici— w10s + L2c1a2 W2o31a121 1 + mc1 1jo1iChapter 2. Manipulator Modelling 45+m [L2C1a1— W2o31a12]—m [L2Clai2 — W2oSi]02+mc112 2jo2 + mE= beyEy— keyEy+b [(Lici — w10s +L2c112 — W2QS1a2) Si + (L2ci12 —w20s112)02+ iJ-10c + w20c1a2+ (L2c1a2—w20s112)Wi0]+k3 (Lisi + wi0c +L2s112 + W2oCiaj2— Yw c11)+m (Lisi +L2s112 + w10c + w2oclai2) 0 + m (L251a1+ w2ociai2) 02+m (L231a1+ W2oClai2) ti4 + 2mctb2osiaiti4+2mg [(L251a12+ lD2oC1ai2) 02 + WioSi + W2oS11+ (L251a1 + W2oCiai2) oj 6+2m [(L281a12+ W2oClai2) Wlo + W2oSlai2] 62. (2.52)Several steps have been taken in an effort to ensure the correctness of the derivation ofequations (2.47) through (2.52). As noted previously, the kinetic energy expression (equation (2.37)), from which most of the terms in the equations of motion arise, is in agreementwith that derived independently by Chan [68]. Furthermore, Chan’s derivation is in agreement 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 obtainthe equations of motion and the subsequent simplification and manipulation of those equations was done using the Maple V algebraic computation program. This method protectsagainst errors which are common in manual algebraic manipulation such as transcriptionerrors, sign errors and misapplication of the chain rule of differentiation. Finally, as willbe discussed in section 4.5, the FORTRAN coded form of the equations of motion have beenvalidated against the code developed by Mah [35] both by comparison of calculated valuesChapter 2 Manipulator Modelling 46for the terms in the inertia matrix and on the right hand side, and by comparison of homogeneous responses for a variety of initial conditions. Mah’s program has been similarlyvalidated 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 contactexists, equations (2.47) through (2.52) describe a system of 4 + ni + n2 damped, coupled,nonlinear oscillators. The oscillators are forced by the joint torques in the rigid bodyequations which act to change the joint angle generalised coordinates.The coupling among the oscillators ensures that an excitation of any of the generalisedcoordinates will be transmitted to all of the other generalised coordinates. The inertialcomponent of the coupling is evident in the fact that the inertia terms on the left hand sideof each equation are a weighted sum of all of the generalised accelerations. Considering thecomplete set of equations, the weighting terms can be written as a symmetric generalisedinertia matrix. The terms in that matrix can be divided into two groups; those which areinherent in the dynamics of a serially connected chain of flexible links and those whicharise due to the existence of contact between the manipulator tip and an environmentsurface. The same division is true of the nonlinear right hand side terms, N(q, 4), whichare functions of the generalised coordinates and velocities. On the other hand the couplingin 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 theenvironment can be expressed as a function of the generalised coordinates. Thus, jointtorque inputs will tend to excite oscillations of the generalised coordinates which will beexpressed as oscillations of the contact force. The goal of force control is to calculate andapply joint torques which will cause the contact force to quickly follow changes in the desiredcontact force level.In the absence of contact the system becomes a set of ni +n2 damped, coupled, nonlinearoscillators with an additional two rotational degrees of freedom. The joint torques act toChapter 2. Manipulator Modelling 47produce rotations of the link bodies about the joints and the inertial and nonlinear coupling ensures that oscillations of the generalised coordinates associated with the structuraldynamics are excited.Equations (2.47) through (2.52) can be transformed to the equations of motion forseveral other configurations which have been investigated in the course of the research.Setting each element of the set of coefficients {m, bex, b3, b3, kex, k3, k8} tozero and dropping the generalised coordinates E and ey and equations (2.51) and (2.52)yields the 2 + ni + n2 equations of motion for a planar two link flexible manipulator in freemotion. The resulting equations are shown in section A.l of Appendix A. These equationsgovern 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, themotions after contact of the contacting surfaces are neglected. By implication, when contactis broken, the contacting surfaces effectively come immediately to rest without residualdeflection. This assumption is reasonable considering that the masses and deflections of thecontacting surfaces are small and that neither plastic deformation of the surfaces nor grossmotions of the environment are included in the model.Starting from equations (2.47) through (2.52), the equations of motion for a planartwo link manipulator with rigid links contacting a deformable environment are obtainedby setting each of the coefficients {I1k, ‘2ki, JJ3ki, ‘4ki, bk, i 1,. . . , rik, k 1, 2} to zero,dropping the generalised sets of coordinates {‘,bi2,i 1,... ,m1} and {b23,j = 1,... ,n2} anddropping equations (2.49) and (2.50). The resulting four equations of motion are shownin section A.2. Further operation on this resulting set of equations, namely setting eachof the coefficients {m, bex, bey, b3, kex, key, k8x, ksy} to zero, dropping the generalisedcoordinates e,, and E and dropping equations (A.7) and (A.8) yields the two equations ofmotion for a planar, rigid, two link manipulator in free motion, shown in section A.3.Chapter 2. Manipulator Modelling 48Finally, the 2 + n1 equations of motion for a single flexible link in contact (section A.4)and the 1 + n1 for the same link in free motion (section A.5) are arrived at by settingthe coefficients {IO2,I12,I22,I32,I42, b2, b€, b3, kex, k3,i = 1,... ,n2} to zero, droppingthe generalised coordinates 62, {b2,i = 1,. . . ,n2} and e, and dispensing with equations(2.48), (2.50) and (2.51) in the former case and by additionally setting the coefficients{m, key, k3} to zero, dropping the generalised coordinate e, and dispensing withequation (A.13) in the latter.2.5 Contact Force ModelFigure 2.5 shows the geometry of the two flexible link manipulator and deformable environment 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 regionssurrounding the point of contact between the link (or end-effector) tip and the environmentsurfaces. The local dynamics of the contacting surfaces are represented, as recommendedby Johnson [62], by a contact region mass on a damped, linearly elastic foundation for eachsurface. The deflection of the combined contact mass from its location at the instant whencontact is made represents the local compression of the surfaces. Note that, while the localcompression of the manipulator tip surface in general has a component along the axis of thelast link, the links are assumed to be stiff in that direction and the effects of global axialcompression of the links are ignored.The expressions for the contact force components as a function of the generalised coordinates of the model will now be derived by considering the geometry of the system andthe forces acting on the contact region masses. These expressions are necessary to the simulation studies in order to permit the contact force between the link tip and environmentChapter 2. Manipulator Modelling 49y€IxFigure 2.5: Manipulator—environment model geometry for derivation of contact force expressions.Ijkevsym= m+ meywb5xw oxChapter 2. Manipulator Modellingmpc50k5(e— ô—x)b5(—x1)kex(E ô X)bex’xb5(— *tp)(a) Total contact massFigure 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 motion. Furthermore, they are used, in simplified form, to devise the initial controller modelcoefficients 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 contactmass, m = m3 + me, in the x direction yieldsmë = —k9 (e1, — —— ( — Sx — w) — b3 (r — — bexE. (2.53)Considering each of the surfaces separately, with the x direction contact force, F, actingequally but in opposite directions on each contact mass as shown in the free body diagrams,k5 (E— —xtjpF F(b) Manipulator tip contact masskex(Eôx)<w)b ex(c) Environment contact massChapter 2. Manipulator Modelling 51a force balance on m3 (Figure 2.5b) gives— F — k3 (e — — x) — b3 ( — = m3 (2.54)and a force balance on me (Figure 2.5c) givesF — kex (e — — x)— = meêYx. (2.55)Solving equation (2.53) for ë and substituting the results into equation (2.55), noting that=— 8x — x, yields:F = (kexx+ bexx)+ x.. x)j. (2.56)A similar procedure applied in the y direction yields the expression for that component ofthe contact force, F:= (keyEy + beyy) + [ksy (Ytip — Yw — e) + b (th—ay)]. (2.57)‘rnExpressions for the world frame manipulator tip position (x2 and y) and velocity (i1and !‘tip) components which appear in equations (2.56) and (2.57) were derived as part ofthe 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 environment the following geometric relationship exists among the link lengths, joint anglesand the location of the environment:= L1 cos i9 + L2 cos ( + 8) = L1c + L2cand Yw = L1 5111 8 + L2 sin (8’ + 6) = L1s + L2s, (2.58)where O and O are the particular values of the joint angles at which contact occurs.Chapter 2. Manipulator Modelling 522.5.1 Hertz Contact ModelThe values of the contact region masses, m5 and me, stiffnesses, kex, key, and k5, anddamping coefficients, bex, bey, b3 and b3, in this lumped parameter contact model must berelated in some way to the material properties of the surfaces in contact. For instance, analuminium end-effector in contact with a steel assembly jig will have much higher stiffnessvalues and lower mass values than a rubber cover tool contacting a wooden surface. Thesolution 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 physicalproperties of the materials in contact. It must be noted that the Hertz theory is based onthe assumptions that the contact occurs between surfaces of non-conforming shape and isquasi-static. As a result, it can only be used as a guide in the selection of coefficient valuesfor 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 oftwo spheres held in contact by a force F is [63]2 1 1 1_v2 1—vaS = F3 {D2 ( + where D 1 + 2 (2.59)is the inverse of the effective planar strain modulus of the materials, v1 and v2 are thePoisson’s ratios of the materials, E1 and E2 are the moduli of elasticity and R1 and R2are the radii of curvature of the spheres. Equation (2.59) can easily be expressed as aforce/compression relationship:F = KS, (2.60)where(R8) R12K= and R=DIn this expression R is the effective radius of curvature, that is, the contact between surfacesChapter 2. Manipulator Modelling 53of curvature R1 and R2 is equivalent the contact between a surface of curvature R and aflat surface [64].Note that, unlike in Hooke’s Law for a linear spring, the stiffness in equation (2.60) is afunction of the square root of the compression, that is, the stiffness of the material increasesthe 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 deflections must be small, the contact stiffness coefficient values used in the present research wereobtained by approximating equation (2.60) by its tangent at an appropriate nominal levelof contact force. Details of the calculation of the coefficients for specific combinations ofmaterials are included in Chapter 5.It follows from the Hertz theory [63] that the area of contact between spherical surfacesis a circle of radiusa (FDR). (2.61)On the basis of this, a first order estimate of the volumes of material moved by the deformation of the surfaces is that they are equal to the segments of a sphere of radius R andheights S and 62, that is,= 7rS (R—-6) and V2 7r6 (R— s2) . (2.62)The deflections of the individual surfaces, 6 and 62, sum to the total deflection given byequation (2.59) and are in inverse proportion to the planar strain moduli of the surfacematerials. These volumes, combined with the densities of the contacting materials, givethe mass of material on each surface that is involved in the contact deformation and thusprovide a basis for the selection of values for the contact region masses, m5 and me.Chapter 2. Manipulator Modelling 542.6 Voltage Controlled DC Electric Motor Actuator ModelIn large, flexible manipulator systems such as the Space Shuttle Remote Manipulator System (SSRMS), or Canadarm, and the proposed Mobile Servicing System (MSS) and SpecialPurpose Dextrous Manipulator (SPDM) for the Space Station Freedom, the links are drivenby direct current electric motors at each joint. A typical DC motor actuator system, controlled by armature voltage, consists of:• a signal amplifier which raises the control input voltage signal to a level suitable todrive 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 inertiaand 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 dropsaround the armature circuit is:Va = Raja + Laja + Vb, (2.63)where Va is the armature voltage, derived from the control input signal, Ra is the electricalresistance of the armature windings, a is the current through the armature windings andLa is the inductance of the windings. The back emf, vb, a feedback voltage proportional tothe armature speed, 0a, is a result of the rotation of the motor armature in the magneticfield induced in the stator windings;Vb KemfOa, (2.64)Chapter 2. Manipulator Modelling 55(b) Armature circuitFigure 2.7: Schematic of direct current motor actuator system.(a) Block diagram+Chapter 2. Manipulator Modelling 56where Ke is the back emf coefficient. Equation (2.63) can be rewritten as an ordinary differential equation for the armature current in terms of the motor parameters, the armaturespeed 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 motoris represented by an armature inertia, ‘a a frictional damping, ba, proportional to thearmature speed, and the load torque, TL, at the motor side of the gear box. A torquebalance for the motor yields:1aa Tm — TL — baOa.The torque produced by the motor, Tm, iS proportional to the current in the armaturewindings, orTm = Kra, (2.66)where K.,. is the motor torque coefficient. Thus, the mechanical model of the motor asviewed from the motor side of the gear box isTL = K.rja — Ia&a— baa. (2.67)The gear box increases the torque by a factor Ng, the gear ratio, at the expense of decreasingthe shaft speed by the same factor such that= N9TL and = jIa.Thus, when viewed from the link side of the gear box at joint k the mechanical equation ofthe motor isTk N9Krja — Ng2 (2ak + bask) . (2.68)This actuator model is coupled to the rigid body equation of motion for the link beingdriven by adding the inertia, NIaOk, and damping, Nba8k, terms from equation (2.68)Chapter 2. Manipulator Modelling 57to the corresponding terms in the rigid body equation and by using the geared up motortorque, NgKTia, as the joint torque, Tk. In order to calculate this joint torque the differentialequation for the armature current (2.65) must be solved simultaneously with the equationsof 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 ofmotion 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 usedand accepted [65]. The signal and current amplifiers are components of such an actuatorsystem that are often overlooked in modelling. They are important not only in that theyare part of physical actuator systems but also because they are the source of the dominatenonlinear feature of the actuator system, saturation. The voltage drop which can be appliedacross the motor armature is limited as is the current which the motor may draw. Figure 2.7ashows the amplifiers as linear devices with limits and they are modelled in the TWOFLEX codein much the same way; if the calculated values of voltage or current exceed the saturationlimits the values are clamped at the limits. The actuator model neglects features of themotor dynamics such as non-viscous friction and torsional flexibility in the shaft and gearhead. These features are ignored in order to focus on the control issues which arise due tothe structural dynamics of the manipulator links.Section 5.6 presents results of several simulations of maneuvers of a prototype SpaceStation Mobile Servicing System (MSS) arm modelled as a two flexible link manipulatorwith DC motor actuation.Chapter 2. Manipulator Modelling 582.7 SummaryThis chapter has presented the derivation of models for the kinematics and dynamics ofa planar two link manipulator with flexible links making contact with a deformable environment. Also derived are expressions which relate the contact force to the generalisedcoordinates of the dynamics model and relationships between the physical properties of thematerials in contact and the parameters of the contact dynamics model. The derivationof the contact dynamics model is the primary contribution of the chapter in that, unlikethe models used in previously published robotic force control research, the model includesthe effects of inertia and damping in the local region surrounding the point of contact, inaddition to the contact stiffness. Results from the theory of elasticity, based on the work ofHertz, indicate that the linear contact dynamics model used here is an approximation to themore complex situation in which the contact stiffness and the mass of the region deflectedduring contact vary nonlinearly with the contact force. However, the Hertz model is alsolimited by its tacit assumptions of non-conforming surface shapes and quasi-static contact.Chapter 3Control Algorithm3.1 IntroductionThis chapter presents derivations and discussions of the various parts of the overall forcecontrol strategy that is the focus of the research. At the heart of the control strategy is amultivariable, receding horizon, adaptive, long range predictive control algorithm. Developed with the primary goal of providing stable, accurate tracking control of desired levelsof contact forces between the manipulator tip and its environment, the algorithm can alsobe 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 longrange predictive control was selected. The complexity of the nonlinear, coupled dynamicsof a flexible manipulator and environment system make conventional PID controller andcompensator designs impractical. Some of the difficulties with conventional designs can beovercome with the use of gain scheduling but to do so requires very complete knowledgeof the plant dynamics over the operating range, including the time scales of characteristicswhich are likely to necessitate gain changes. State feedback techniques are better suitedto complex dynamic systems. A pole placement tracking controller was designed for forcecontrol of a single flexible link and was successful with the significant exception of itsinability to remove steady-state errors from the response. A similar deficiency was found tolimit the use of linear quadratic optimal state feedback. A scheme to use an adaptive inverse59Chapter 3. Control Algorithm 60dynamics model of the system to provide state feedforward for the removal of steady-stateoffsets 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 advantage 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 optimal in the sense of minimising the sum of squares of the difference between the futuresetpoint levels and the predicted output levels, and the algorithm is easily implemented inan adaptive, digital controller form. Predictive control is well suited to deal with systemsexhibiting an abundance of lightly damped modes and non-minimum phase characteristicsas is the case in flexible manipulator force control. The inclusion of the expected futureeffects of these characteristics in the controller cost function results in a control law whichproduces more effective compensation than one based solely on instantaneous sensor data.The Generalized Predictive Control (GPC) algorithm of Clarke, et al. [29] incorporatesmany of the beneficial features of earlier predictive algorithms. It is a relatively mature implementation of the concepts of predictive control which has sparked a great deal of interestand ongoing study of its properties and potential applications.The dynamics of a flexible link manipulator in contact with deformable environmentpresent several challenges to the design of a force control algorithm. The dynamic behaviouris highly coupled, nonlinear, stiff, discontinuous, and nonminimum phase. To deal with thestrong coupling, the predictive controller design is based on a multivariable model thatincludes all of the open ioop system modes and their interactions. Nonlinearities arisingfrom the system dynamics are dealt with by making the controller adaptive, that is, aninstantaneous linear model of the system is used and the coefficients of the model areupdated 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 bothphysical and mathematical senses. The natural frequencies of the open loop modes can easilyChapter 3. Control Algorithm 61span a range of several orders of magnitude in the case of a highly flexible manipulator incontact with a metallic surface. This characteristic of the system causes difficulties in thenumerical integration of the equations of motion, as will be discussed in Chapter 4. It alsoleads 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 forcecan, by definition, only take on either positive or negative values (depending on the orientation of the reference frame in which it is measured). When the manipulator tip loosescontact with the environment surface the contact force is zero and the error signal availableto the controller is independent of the manipulator state. While it is possible that, evenwith such a degenerate error signal, the controller will successfully return the manipulatortip to the environment surface and continue stable control of the contact force, it is morelikely that the system will become unstable with manipulator tip repeatedly bouncing onand off the surface. To avoid this situation the controller is designed to switch from a forcecontrol mode to a motion control mode when the contact is unexpectedly broken. Theobjective of the controller is then to return the manipulator tip to the surface and switchback to force control mode. The switching of modes between force and motion control isgoverned by a contact control logic block at a level above the force and motion controllers inthe manipulator control hierarchy. Two operational strategies for the contact control logicare discussed and demonstrated in Chapter 5.The structural flexibility of the manipulator links makes the contact force response ofthe system nonminimum phase. Physically, a nonminimum phase system is one for which apositive step input produces a response which is initially negative and subsequently reversessign. In free motion, the tip of a flexible link subjected to a step torque input oscillates aboutthe path that would be described by the tip if the link were rigid. Due to the inertia of theChapter 3. Control Algorithm 62link, the tip initially lags and the oscillation is initially negative. This characteristic carriesover into the contact force response when the link is acting on a surface. Mathematically, anonminimum phase system is characterised by the presence of unstable zeros in the transferfunction numerator. Predictive control is well suited to dealing with nonminimum phasesystems because the prediction horizon can be extended to include the nonminimum phaseportion of the response and it can thus be compensated for in the calculation of the controlinputs.Section 3.2 presents a review of the single input, single output (SISO), Generalised Predictive Control (GPC) algorithm of Clarke, et al. [29,32], a discussion of the control lawstructure and a derivation of the closed ioop system characteristics. The development ofa multivariable predictive control algorithm based on GPC is presented in section 3.3. Anew extension of the algorithm with a static equilibrium bias term that takes advantage ofavailable a priori knowledge of the dynamics of force controlled manipulators is presented insection 3.4. This extension is an original contribution of this thesis. The parameters of thepredictive control algorithm and their effects on the shaping of the closed ioop response arediscussed in section 3.5. A discussion of parameter estimation, focussing on the ExponentialForgetting and Resetting Algorithm (EFRA) is presented in section 3.6, and a descriptionof the structure and logic of the complete adaptive multivariable force/motion controller insection 3.7. The chapter concludes with a discussion of stability and convergence characteristics of the adaptive, predictive control algorithm (section 3.8).3.2 Review of SISO GPC AlgorithmThis section briefly reviews a nonadaptive, single input, single output (SISO) long rangepredictive 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 withChapter 3. Control Algorithm 63inherent 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 senseof minimisation of a quadratic cost function) set of future control inputs that will minimisethe error between the predicted plant output and the desired setpoint. This procedure isconducted in a receding horizon context, that is, a new set of control inputs are calculatedat each time step and only the first member of the set is used. Receding horizon predictivecontrol predates GPC in the process control literature but, as the name suggests, the GPCalgorithm is a generalised form of the method. Indeed, it has been shown that, dependingon the choice of parameters, the GPC algorithm results in control laws identical to a varietyof well known algorithms [73—75].The plant to be controlled is modelled in the discrete time domain by an input/outputrepresentation of the formA(q’)y(t) = B(q’)u(t — 1) + , (3.1)where A(q’) and B(q1)are polynomials, of degree na and nb respectively, in the backwardshift operator q’:A(q1) = 1 +a1q’ +a2q + . .. + anaq,B(q1) = b0 + b1q’ + b2q + ... +where q1x(t) = x(t — 1),y(t) is the time series of plant output values and the plant input time series is u(t). Thelast term in equation (3.1) is a disturbance term consisting of (t), an uncorrelated random sequence and L 1—q’, the differencing operator. This model assumes that thedisturbances to which the plant is subject can be modelled as steps of random magnitudeChapter 3. Control Algorithm 64occurring at random times [76]. Using the Diophantine identity [77],1 = E(q)A(q’)z + qF(q’), (3.2)the plant model can be manipulated to yield a j—step ahead predictor of the plant output:y(t + j) = Fj(q’)y(t) +E(q1)B(L\u(t + j — 1) + E(q1) t + j). (3.3)The polynomials E3(q’) and F3(q1)in equation (3.2) are completely defined given A(q’)and j, and are unique provided that [78]:deg(E,) j and deg(F,) <deg(A).The coefficients of E+1 and F3+1 are calculated from E, and F, with minimal computationaleffort using simple recursion formulae [29]. The predicted output y(t + j) is the sum offiltered time series of the output, the input and the disturbances. Given the fact that thedegree of E3 is, at most, j — 1 and the assumed random nature of (t), the contributionto y(t + j) of the disturbance sequence is not only unknown but unknowable. Hence, theoptimal predictor of y(t + j) given the information available at time step t is(t + jt) = F,y(t) + Gu(t + j — 1), (3.4)where G3= E3B = gjo+gjlq1+...+gj(nb+j_flq (flb+I 1)• The optimal prediction (t+j It)can be written as a term, f(t + j), which consists entirely of past inputs and outputs (ineffect, the free response of the system) summed with terms due to the future control inputincrements which are yet to be determined.Defining N2 as the prediction horizon and considering predictions at each of the nextN2 steps into the future the following vectors are formed:= + 1 t) (t +2 It) ... (t + N2 It)]: (3.5)[ut u(t + 1) ... u(t + N2 — 1)] , and (3.6)r= [f(t+1) f(t+2) ... f(t+N2)]T,(3.7)Chapter 3. Control Algorithm 65wheref(t + 1) = Fi(q’)y(t) + (Gi(q’) gb) Au(t)f(t + 2) F2(q1)y(t) + q (G2q’) g2lq — g2o) Au(t)f(t + N2) = FN2(q’)y(t) + qN2 (GNq’)— gN2(-1)q’ — ...— gN2o) 1u(t).(3.8)The set of N2 prediction equations of the form of equation (3.4) are written in matrix formas5’ = G’ü + f, (3.9)where G’ is an N2 x N2 lower triangular matrix whose elements are the coefficients of the G3polynomials from equation (3.4) corresponding to the yet to be determined future inputs.A quadratic cost function, J1, is defined:J1 = (5’_w)P(5’_w)+AuTu= (3.10)The vector[wt + 1) w(t + 2) ... w(t + N2) (3.11)is a sequence of future setpoints, hence, the first term of this cost function is simply the sumof squares of the errors between the predicted plant output and the desired setpoint at eachof the next N2 steps into the future. The second term is the sum of squares of the futurecontrol input increments (i.e., the incremental control energy), weighted by a constant, ).Minimising J1 with no constraints on future inputs yields the control lawu = (c’’ + AI)’ G1T(w — f) (3.12)Chapter 3. Control Algorithm 66which will track the specified setpoint sequence with zero steady state error. Note, fromequation (3.6), that the first element of ii is Liu(t) which, given the receding horizon methodof 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 anddeficiencies, not the least of which is the computational load of inverting the N2 x N2 matrix(G’ TGI + at each time step, as is required in an adaptive implementation due to theupdating of the plant model from which G’ is derived. To improve the robustness of thealgorithm, and at the same time reduce its computational load, the concept of a controlhorizon [79] is introduced [29]; that is, after some number of time steps, N< N2, the controlinput level is held constant. N is the control horizon. Imposition of this condition resultsin the vector ii being truncated from N2 to N elements and the left most N2— N columnsof the matrix G’ being truncated to yield the N2 x N matrix G. The resulting control lawhas 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 theinversion of an N x N matrix instead of an N2 x N2 one and N is generally chosen muchsmaller than N2. Furthermore, the structure of the non-triangular matrix G guaranteesthat GTG will be nonsingular. It is the inclusion in the GPC algorithm of the controlhorizon concept that is at the heart of its ability to control difficult plants like flexiblelink manipulators which exhibit nonminimum phase characteristics and lightly damped orpoorly modelled modes.3.2.1 Control Law Structure and Closed Loop CharacteristicsWhile equation (3.13) gives the control input increments in a form suitable for implementation in a digital controller or simulation, insight into the underlying structure of the controlChapter 3. Control Algorithm 67law and the closed loop characteristics of the controlled system can be obtained by furthermanipulation of equation (3.13). Firstly, note that the quantity (GTG + \I)1 GT is simply an N x N2 matrix of scalars, in effect a gain matrix which we will designate as K forconvenience, 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 thenext N2 time steps and that these predictions are composed of contributions from the pastmembers 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 inq’:Fi(q’) Gi(q)— 910F2(q) q (G2q) g2lq— 920)and g= , (3.15)FN2(q) qN2l (GNq)— gN2(—1)q’— ... — gNo)the vector f can be written as f = fy(t) + Au(t) in which each term is a vector ofpolynomials in q operating on a sampled time series. in order to write ii and w inthe same form we define the vectors1 qq q2q,= and (3.16)qNul qN2such that ii = qLu(t) and w = qw(t). Thus the control law can be rewritten asqL\u(t) = K (qww(t)—y(t)—Au(t)) (3.17)Chapter 3. Control Algorithm 68ory(q + Kg) xu(t) = Kqw(t) — Kfy(t).This is a set of N difference equations of the familiar [78] formR(q’)/Xu(t) = T(q’)w(t)— S(q’)y(t).(3.18)(3.19)Combining this result with the difference equation (3.1) which describes the open ioopdynamics we see that the block diagram of the closed loop system is as shown in Figure 3.1.The overall closed loop transfer function is obtained by combining equations (3.18) and(3.1) so as to eliminate u(t):[(qu + Kg) LA + KfBq’] y(t) = KqBq’w(t). (3.20)Due to the structure of q all but the first of the N difference equations in this expressionare non-causal. However the receding horizon implementation of the control law impliesthat 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 onthe left hand side. The closed ioop poles in the discrete time domain are obtained bywFigure 3.1: SISO GPC closed loop system block diagramChapter 3. Control Algorithm 69finding 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 foundby using well known [80] relationships between the discrete and continuous time domains.The closed ioop characteristics which result from these calculations are an approximationof those exhibited by the long term response of the system due to the fact that the controllaw is implemented in an explicitly adaptive, receding horizon context. Nonetheless, itwill be shown in Chapter 5 that these approximate characteristics are useful in selectingthe controller parameters (horizons and weighting factors) which yield the most desirableresponse from among the set of parameters which stabilise the system.The foregoing formulation of the control law structure and the resulting expression forthe approximate characteristic polynomial of the closed loop system is the result of extensionand formalisation of the numerical example in an appendix of Clarke, e al. [29]. It differs inboth form and intent from those in the literature [32,33,81]. Other authors have generallyexpressed the closed ioop structure of the system in a state space representation, in contrastto the transfer function form of equation (3.21). Their objective in so doing has been theinvestigation of asymptotic stability conditions, comparison to other algorithms, etc. Thegoal of the analysis presented here is to provide as direct a means as possible of assessingthe effects of the controller parameters (horizons and weighting factors) on the closed loopperformance of a given system. While the relationship among the parameters and the polesof equation (3.21) is admittedly nontrivial, the numerical calculation and assessment ofthose poles for a wide range of parameter values is straightforward.3.3 Multivariable Predictive Control AlgorithmThe algorithm derived in the preceding section provides excellent force and motion controlfor a single manipulator link (as will be demonstrated in Chapter 5). Such a system involvesChapter 3. Control Algorithm 70a direct relationship between one input (the joint torque) and one output (the joint angleor contact force). For a multiple link manipulator with torque producing actuators ateach joint a more complex situation exists in that several output quantities (contact forcecomponents, joint angles, tip position or velocity components, etc.) can, in principle, becontrolled and each input contributes to each output. While the situation sometimes existsin which the coupling among the various inputs and outputs is weak and the system can becontrolled by multiple single input, single output feedback ioops, this is generally not so inthe case of manipulators with flexible links. Successful control of these systems requires acontrol 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 nomention of the extension of the algorithm to the multivariable case. It was not until theappearance of the Clarke and Mohtadi paper [33] over two years later that my attentionwas drawn to the multivariable derivation presented in Shah, et al. [82]. Thus, the genericmultivariable 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 ofthat derivation it became evident that the extension of the algorithm with a new, staticequilibrium bias term (derived in section 3.4) would provide performance improvementsin manipulator contact force control applications. The formulation of the discrete timedomain closed loop characteristic polynomial derived in section 3.2.1 and extended to themultivariable case in section 3.3.1 differs from the formulations of the closed loop structurepreviously reported in the literature [32, 33,81]. The formulation derived here is shown tobe particularly useful for tuning the control algorithm in specific applications.For the purpose of deriving a multivariable predictive control algorithm based on theGPC algorithm reviewed in section 3.2, consider first a general multivariable system in theChapter 3. Control Algorithm 71continuous time domain, described byY(s) = H(s) U(s), (3.22)where Y(s) is an n0xl vector of outputs, U(s) is an n1xl vector of inputs and H(s) is anno x n matrix of transfer functions, all transformed to the Laplace domain. Equation (3.22)can be written as a system of n0 linear algebraic equations:Yj(s) = Hk(s)Uk(s), i = 1,... ,n0. (3.23)Each of the H2k(s) terms is a transfer function in the form of a ratio of polynomials inthe Laplace operator, s. Supposing that these transfer functions can be transformed fromthe Laplace (continuous time) domain to the Z (discrete time) domain, the system can bedescribed by a collection of difference equations in the backward shift operator, q1:A(q’) y(t) = Bk(q’)uk(t— 1), i = 1,... ,n0 (3.24)where y(t) is the time series of values of the ith output, Uk(t), is the time series of valuesof the kth input, and where A(q’) and Bk(q) are polynomials in q:A(q’) = 1 +a1q’ +a2q + . . . + anaq,Bk(q’) = b0 + bIk1q’ + bk2q+ ... + bkflqNote that in inertially coupled systems like robotic manipulators the denominators of allof the H2k(s) transfer functions are identical and thus, upon transformation to the discretetime domain, only a single A(q’) polynomial is required. If disturbance terms, d(t),modelled as randomly occurring steps of random amplitude:d(t) = t), where A = 1— q1, (3.25)and 4(t) is a random, uncorrelated sequence, are added to equation (3.24) the system modelis analogous to the single input, single output CARIMA (Controlled AutoRegressive andIntegrated Moving Average) model [76] used in section 3.2.Chapter 3. Control Algorithm 72From this point the derivation of the optimal output prediction equations is very similarto that described in section 3.2, except that we are now considering each element of a set ofoutput equations, {y, i = 1,... , n0}, that depends on several inputs, {uk, k = 1,. . .rather than a relationship between a single input/output pair. With this in mind, the majorresults for the multivariable derivation can be stated as follows:• Diophantine identities:1 = E(q’) A(q1 + q3 F(q’), i = 1,.. . , n0, (3.26)• Optimal j-step ahead output predictors:th(t + jt) = F(q’)y(t) + G(q’)(t + j — 1), i 1,. . . ‘no, (3.27)where Gk(q’) = E2(q’) Bk(q’) and Uk(t + j — 1) tUk(t + j — 1).Note that for each of the j steps in the future (t + j t) is composed of a series of filteredpast outputs and input increments which are known at time step t and a series of futureinput increments which are yet to be determined, that is:(t + 1 It) Fi(q1)y t) + [G(q’)— g1k0j Uk(t) +gk0k(t),i—(t + 2 It) = F2(q’)y(t) + [G(q)— g2k,q — gi2] qük(t)+ (gi1q’ + g2k0) Uk(t + 1), i 1,. . . ‘no,etc. (3.28)At this point a control horizon, N, analogous to that defined in section 3.2 is introduced,that is, it is assumed that after some number of time steps, N < N2, the control inputs willbe held constant. Introduction of this control horizon has similar benefits to those observedChapter 3. Control Algorithm 73in the SISO case, namely, the computational complexity of the algorithm is significantlyreduced and the robustness is improved. The optimal output prediction equations for N2steps into the future, subject to an N step control horizon, can be written in matrix formas:= +, i = 1,... ,n0 (3.29)where Sri=+ 1 t) (t + 2 t)...+ N]TUk= { u(t) u(t + 1) ... u(t + N — 1)]9ilko 0 ... 0912k 912k0 . . . 0G3k = . . . , andgN2k(N_l) g2Nk() . . . g2Nk0Fi(q1)y(t)+ [G(q’)— gllk0Uk(t)F2(q’)y1t + [G,(q) — g,.’ciq1—g,0]qLUk(t)f1= k=1FN2(q’)y(t) + [G1k(q’) — g1N2k()q__1) —...—giN2ko} q_)Auk(t)Note that each element of f. consists only of contributions from the past (i.e. known) inputsand outputs, filtered by coefficients derived from the known system model. As a result, theelements of f1 can be viewed as predictions of the free response of the ith output if no furthercontrol action were applied.Defining= [w + 1) w1(t + 2) ... w1(t + N2)], i = 1,... ,n0 (3.30)Chapter 3. Control Algorithm 74as the set of vectors of setpoints for each of the outputs for the next N2 time steps, thevector, ê, of optimal predictions of the output errors is:e. =i= 1,...,n0 (3.31)The cost function, J2, which will be minimised to yield the set of optimal control inputincrements is:+ ‘c > U ii. (3.32)Minimisation of this cost function will minimise the sum of squares of the optimal predictionsof the output errors. At the same time, the energy of the control input increments isminimised to an extent that depends of the value of ). The control energy is representedby 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 thatresult with respect to Urn, an arbitrary member of the set of control input increment vectors,gives:= 2 > G Gk Uk + .icUm + C (f, — w)]. (3.33)Urn i=lk=1The set of control input increments, {Um,m 1,.. . ,n}, that minimises J2 is given by:0J2—=O m=1,...,n2. (3.34)GUmSubstituting into this expression from equation (3.33) and rearranging yields:m=1,...,n (3.35)i=1 k—_iThis is a system of linear, algebraic equations which must be solved simultaneously to yieldChapter 3. Control Algorithm 75the desired control input increments. Written in matrix form the system is:G G1 ... G1 G11 G12 ... ii+G G2... G2 G21 G22 ... G2 U2G G’ . . . G1 Gn2 . . . UnriT rT‘-11 ‘‘21 W1 — iiG12 G ... G’2 w2—f(3.36)T ç‘1n 2n1 W,.0 —The matrix of Gk terms can be considered as a composite matrix (that is a matrix, each ofwhose elements are matrices), G, with the result that the control law can be written morecompactly asui w1—fU2—1 w2—f= (I+GTG) GT:(3.37)w0— f,which is recognizably analogous to the SISO control law, equation (3.13).Each of the Urn vectors in equation (3.35) is N. x 1 and the w and f2 vectors are N2 >< 1,thus the vector of Urn vectors on the left hand side of equation (3.36) is x 1 and thevector of (w, — f1) vectors on the right hand side is n0N2 x 1. Correspondingly, each ofthe Gzrn matrices in equation (3.35) is N2 x N and thus the product matrices G G2kare N x N symmetric matrices. Hence, the left hand side matrix in equation (3.36) is ann0N x n2N matrix and the right hand side matrix is n0N x n0N2. These dimensions ofthe left hand side matrix imply that a unique set of control inputs exists only if the numberof inputs and outputs are equal. If the number of outputs exceeds the number of inputs theChapter 3. Control Algorithm 76system is underspecified and not all the the control objectives can be satisfied. With moreinputs than outputs the system is overspecified and additional criteria must be introducedto obtain a unique set of controls. In the case of robotic control this means that the numberof force, torque or motion components that can be controlled is equal to the number ofactuators available.3.3.1 Control Law Structure and Closed Loop CharacteristicsThe method used in section 3.2.1 to investigate the structure of the SISO GPO control lawcan be extended, using composite matrices, to explore the structure of the multivariablecontrol law and the characteristics of the resulting closed ioop system. Replacing the composite matrix + GTG)1GT in equation (3.37) by K, the control law can be writtenin a form analogous to equation (3.14):ü1 w1=K:— :(3.38)w,where the gain matrix K is an nN x m0N2 matrix of scalars (or, equivalently, an n x n0composite matrix of N x N2 gain matrices).The f vectors can be written as vectors of polynomials in q operating on sampled timeseries by employing the vector f defined in equation (3.15) and extending the definition offrom the same equation toI_v I, —1\‘-ii.q i Yzklo—1 —1q (Gk2(q ) gk21q — gik2o)= (3.39)qN2l (GjkNq_1)—— . . .— gikN2o)Chapter 3. Control Algorithm 77Thus1 0 0 yi(t) Au1(t)= 0 0 y(t) + Lu(t). (3.40)fN2 0 0 yn(t) n1 g2 Lu(t)Likewise, employing the vectors q and q, defined in equation (3.16), the vectors of iiksand ws are written in the form of polynomials in q’ operating on sampled time series:ii q 0 0 zui(t)112 = 0 q, •.• 0 Au2(t)(3.41)0 0 q.andq 0 0 wi(t)W2 = 0 q 0 W2(t)(3.42)w,, 0 0 qUsing these expressions the control law can be rewritten asui(t) wi(t) yi(t)U2(t) W2(t) y2(t)(quI+KG)z =KqI KfI . (343)u(t) w0(t)This is a set of n1N difference equations.Chapter 3. Control Algorithm 78With the difference equations (3.24) which describe the open loop dynamics of the systemexpressed in matrix operator form:yi(t) u(t)A(q’) y2(t) = B(q’)q1 u2(t) (344)y(t) u(t)whereB11(q) B12(q’) . . . B1(q’)B21(q1) B22(q1)... B2(q)Bi(q1)B02(q1)... B0(q’)the vector of y2(t)s can be eliminated to yieldui(t) wi(t)w2(t)(qui + KG) AA + KfIBq’]:= AKqI:(3.45)u(t) w0(t)This expression is a closed ioop transfer function relationship between the setpoints and thecontrol inputs. Manipulation of equations (3.43) and (3.44) to arrive at the more conventional closed ioop transfer function representation relating the setpoints to the outputs isproblematic 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, thisdifficulty affects only the eigenvectors of the closed ioop representation; the eigenvalues ofthe 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 characteristicsfrom these poles is shown in Chapter 5 to be a useful method of tuning the multivariablecontrol law.Chapter 3. Control Algorithm 793.4 Static Equilibrium Bias TermGeneric, linear input/output models are used to describe the systems on which the derivations 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 manydiverse systems, it is desirable to customise the control algorithm for a particular systemby using additional knowledge about that system and thereby improve the control. In thecase of control of contact forces between a robotic manipulator and its environment thedynamic model of the system can be used to form a relationship between the desired forcelevels (setpoints) and the actuator input levels required to maintain those forces when thesystem is in static equilibrium. This information is introduced into the control algorithm byaugmenting the cost function with a term that weights the sum of squares of the deviationof the actual total control input levels from the calculated static equilibrium levels. Thisterm is referred to as a static equilibrium bias term and its addition to both the SISO andmultivariable predictive control algorithms is an original contribution of this thesis.The static equilibrium bias term acts as a proportional feedforward term in the controllaw. This term has the beneficial effect of reducing the step response rise time but thisimprovement comes at the expense of introducing a small amount of overshoot. Simulationresults discussed in section 5.3.4 illustrate these characteristics.The function which maps the set of setpoint levels, {w, i = 1,. . . , n0}, at a given timestep onto the corresponding set of static equilibrium control inputs, {Ük, k = 1,. . . ,n}, isobtained from the equations of motion of the system and the contact force model. Detailsof the necessary manipulations of equations (2.47) through (2.52), (2.56) and (2.57) toobtain the mapping function for the case of a two flexible link manipulator are shown inAppendix C.Chapter 3. Control Algorithm 80Defining vectors, analogous to the vector of future setpoints (equation (3.30)), for theactual control inputs, Uk, and the calculated static equilibrium inputs, Uk,= { u(t+ 1) uk( + 2) Uk(t + Nu)] k = 1,... ,n (3.46)and= [Ük( + 1) Uk(t + 2) tLk(t + N) k 1,. . . ,n, (3.47)the static equilibrium bias term for the cost function is the sum of squares of differencesbetween Uk and- T-Jse >Z(uk_tlk) (Uk —Uk). (3.48)This term is weighted by a factor, ,X, and added to the predicted output error and incremental control energy cost function terms given by equations (3.32) to yield the overall costfunctionJ2 + .X8eJse=+ cÜÜk + \se(Uk — uk)T (Uk Uk). (3.49)The vector of control inputs, Uk, is formed from the (previously defined) vector of controlinput increments, Uk, by adding the future increments to the current input value, u= Uk(t).Defining i as a unit vector and L as a unit lower triangular matrix,1 100...01 110...0IL= . and L=1 1l1...1Uk can be written as:Uk = u + LUk. (3.50)Chapter 3. Control Algorithm 81Substituting equation (3.50) into equation (3.49) yields:J3 = : eej + \c Ü U + se (u H- Lfik Uk) + Lük — Uk). (3.51)Since J3 is a linear combination of J2 and Jse the principle of superposition allows the resultsof 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 (anarbitrary member of the set of control input increment vectors, see section B.2 for details)gives:(3.52)The set of control input increments, {Um,m = 1,... ,n}, that minimises Jse is given by:OJse--—=0, m=1,...,m. (3.53)oumApplied to equation (3.52), this condition yields:LTLIIm LT (m —uji.), rn= 1,...,n2 (3.54)Adding this set of equations, weighted by.se, to (3.35) gives>GnGikUk +AcUm+seLTLUm = >G (w _fj)+AeLT (Urn _UjL),irzl kr1 jzlm=1,...,n (3.55)an augmented system of linear algebraic equations which can be solved for {Um,m =1,.. . ,n2}. These equations can be written in matrix form similar to equation (3.36):G G’1 ... G11 G12 ... G11G G ... G2 G21 G22 ... G2... G02Chapter 3. Control Algorithm 82LTL 00 LTL+.\3e00LU1 -U2Un1 -— fi— f2— fTl0— 0U1— U1,i— 0U2 — U2LWiW2wna0 0 ... L’‘T rITLW11‘21GT iT CIT12 ‘22 n02-‘iT CIT T“1n ‘2n non)iseLT 0... 00 AseLT ... 00 0 ... .X3eLT+ (3.56)iin—u1pThis linear system has the same dimensions as equation (3.36). The matrix LT is N xN and the vector of static equilibrium biases on the right hand side is n2N x 1. Therequirement 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 asw1—fw2—f= K1 (3.57)w0— f— u,i -Chapter 3. Control Algorithm 83where—1LTL 0 ... 0o LTL... 0K1 = )I + GTG +: : :GT (3.58)o o ... LTLand—1LTL 0 ... 0 LT 0 ... 0o LTL... 0 0 LT ... oK2A3e AcI+GTG+Ase . . . .o o ... LTL 0 0 ... LT(3.59)Comparing this result to equation (3.37) it is evident that the control laws differ slightly inthe structure of the inverted quantity in K and K1. The more significant difference is theaddition of the second (K2) term in equation (3.57). This term make a contribution to thecontrol input which is proportional to the difference between the expected static equilibriuminputs (u) and the current levels (u). The magnitude of the contribution is governed bythe value of ?. Thus, the term produces a proportional feedforward action in the controllaw.3.5 Control Algorithm Parameters and TuningThe multivariable predictive control algorithm has four tuning parameters:1. N2, the prediction horizon;2. N, the control horizon;3. ), the input increment weighting factor; and,Chapter 3. Control Algorithm 84‘ae, the static equilibrium bias term weighting factor.The prediction horizon specifies the number of time steps into the future for whichpredicted system outputs will be calculated and included in the control law. Clarke, etal. [29] recommend that N2 be set comparable to the rise time of the open ioop system.The prediction horizon should also be far enough in the future to include the negative-goingpart of a nonminimum phase open ioop response. Too short a prediction horizon causesthe controller to work with insufficient information and the control is likely to be poor oreven unstable. Increasing N2 causes the poles of the closed ioop system to move toward thelocations of the open loop poles.The control horizon is the number of time steps in the future after which the controlinput level will be held constant, that is, all future increments set to zero. This horizonhas significant effect on the computational load of the algorithm since the left hand sidematrix of equation (3.56) (which must be inverted) has dimensions n0N x n2N. Clarke,et al. [29] recommend that N,. be greater than or equal to the number of unstable or poorlydamped zeros in the open ioop system. In the multivariable form of the algorithm thecontrol horizon should be at least equal to the number of inputs to the system. IncreasingN,. generally increases the magnitude and rate of change of the control input signals. Toa certain extent this increased activity leads to higher performance control but too largea value for N,. may result in excessive ringing of the control signals and significant highfrequency components in the step response transients.The input increment weighting factor governs the contribution to the cost function ofthe 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. Theuse of a nonzero has been found to introduce a pole on the negative real axis into thecontinuous time representation of the closed loop system. This pole moves toward the originChapter 3. Control Algorithm 85as is increased. In the context of contact force control this additional pole may have astabilising effect for the case of a rigid link manipulator but its interaction with the polesdue 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 terminfluences 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 equilibriumvalues. Thus the static equilibrium bias term can be viewed as a proportional feedforwardterm in the control law with a gain related to )se. The fact that the contribution of thisterm to the overall control input signal is larger when the output is far from the setpointthan 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 theexpense of introducing overshoot into the response.Clarke, et al. [29] include one additional parameter in the GPC algorithm: the delayhorizon, N1. This horizon serves to reduce the computational load of the algorithm insituations where there is a delay between the application of a control input increment andits effect appearing on system outputs. Such situations are common in process controlapplications but in the case of robotic force control there is no delay, provided that thecontrol law calculations can be completed with a single time step, and N1 is set to 1.In principle, a different prediction horizon value, N22, could be used for each output inthe multivariable algorithm. Similarly, independent values for N, X, and Ase for each inputcould also be specified. The resulting algorithm is substantially more complex to implementand no clear advantage can be seen in using multiple horizon and weighting factor values.Chapter 3. Control Algorithm 863.6 Parameter EstimationAs was noted at the beginning of this chapter the inherently stiff nature of the dynamics ofa manipulator in contact with its environment as well as the strong coupling and nonlineardynamic effects present a significant challenge in the design of a controller for contactforces. This challenge is met, in part, by implementing the control algorithm in an explicitlyadaptive context, that is, the values of the coefficients of the polynomials in the controllermodel (equation (3.24)) are estimated on-line from the time series of calculated controlinputs and sensed contact force outputs. This parameter estimation is accomplished by arecursive least squares (RLS) algorithm.The basic RLS algorithm [77] is described by a pair of update equations, one for thevector of parameter estimates,9 and the other for the covariance matrix, P:+ 1 k(Yk — ‘k-i) (3.60)1)—rki ((‘k ‘I’kIE k — ‘ k—i— 1 .iT 0 > U,I + Yj E k—i ‘Pkwhere cbk is the regressor vector of input and output data values and yk is the value ofthe system output at time step k. This algorithm has optimal convergence and stabilityproperties when the parameters to be estimated are time invariant. Unfortunately, the basicalgorithm is unsuitable for tracking time varying parameters because its gain converges tozero. A large number of modified RLS algorithm have been developed to overcome this andother 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 triangular and a diagonal matrix (U/D factorisation) and thereby ensures that the covariancematrix will always be positive definite. Tracking of time varying parameters is facilitatedby use of an exponential forgetting factor to weight older data less heavily than new. TheChapter 3. Control Algorithm 87effect of this weighting is to establish a nonzero lower bound on the minimum eigenvalueof the covariance matrix and thereby prevent the occurrence of zero elements in the gainterm 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; theunbounded growth of the eigenvalues of the covariance matrix which leads to correspondinggrowth 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 RLSalgorithm developed, by Salgado, et al. [841, known as EFRA which stands for ExponentialForgetting and Resetting Algorithm. As well as using the exponential data weighting necessary to track time varying parameters the EFRA algorithm also incorporates exponentialresetting 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 andminimum eigenvalues of the covariance matrix, thus avoiding covariance wind-up. Theupdate equations for EFRA are:=e + LePklbk (Yk-(3.62)=— ePk_1kPk_1+e1 SeP_i, (3.63)l+41Pk—1çbkwhere ce, /3e, 8e, and ) are constants. Salgado, et al. [84] give the following guidelines forthe values of these constants:• adjusts the gain of the least squares algorithm; typically e E [0.1, 0.5];• /3e is a small constant which is directly related to the minimum eigenvalue of P;typically/3e [0,0.01];• 6e is a small constant that is inversely related to the maximum eigenvalue of P;typically Se E [0, 0.011;Chapter 3. Control Algorithm 88Xe 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 RLSalgorithm. The system being identified is described by a simple moving average:Yk auk + buk_l, where Uk = [i + sgn (sin k)], (3.64)that is, Uk is a square wave centred about with a period of 100 time steps. The parametervalues 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=Twith initial value 90= [ a band the regressor vector iscIJk—_[Uk Uk_lj.The constants for EFRA are= 0.95, ae = 0.5, 13e = 0.005, Se = 0.005,and for the U/D RLS algorithm the forgetting factor is ?u/d 0.95.Figure 3.2 shows the evolution with time of the parameter values calculated by EFRA(Figure 3.2a) and the U/D RLS algorithm (Figure 3.2b). Comparing the EFRA and U/DRLS results, the most striking difference is the presence of spikes in the estimates calculatedby U/D RLS. These spikes are very poor estimates of the actual parameter values. UsingChapter 3. Control Algorithm3.53.02.50 2.0ci)i.5E4-.Cl)w .50ci).5-2.0-2.5-3.0089.8.7U)C).00 100 200 300 400 500 600 700 800 900 1000 1100 1200Time Step(a) EFRA parameter estimates100 200 300 400 500 600 700Time Step800 900 1000 1100 1200(b) U/D RLS parameter estimatesFigure 3.2: Comparison of EFRA and U/D RLS parameter estimation algorithms.Chapter 3. Control Algorithm 90those values in a controller model leads to inaccurate calculation of the required controlinputs 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 areequally applicable to single input, single output systems as above and to multivariablesystems of the form of equation (3.24). For a multivariable system the vector of parameterestimates and the regressor vector are augmented to include the coefficients and data fromeach of the inputs:= [ a1 ... anai b10 ... b1b ... bno ... ]T (3.65)andik= { 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 ImplementationThe overall control strategy implementation is shown in Figure 3.3. This strategy allowscontrol of both the contact forces and the gross motions of the manipulator when it is notin 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 usedin these two modes of control are necessarily distinct and of different order. The force andmotion models are identified and maintained in parallel so that switching between controlmodes can be accomplished smoothly. The contact control logic block controls selectionof the appropriate (force or motion) control law, based on data from the force sensor andjoint encoders and the trajectory planner, as well as controlling whether or not the transferfunction models are updated by the parameter estimation blocks. The force and motionChapter 3. Control Algorithm 91Figure 3.3: Control strategy block diagram.Chapter 3. Control Algorithm 92setpoint programs are assumed to be provided by the higher levels of the control hierarchywhich include the operator, a task planner and a trajectory generator.With the parameter estimation blocks enabled, the controller is explicitly adaptive. Ateach time step the coefficients of the transfer function model currently in use are estimatedusing the Exponential Forgetting and Resetting Algorithm (EFRA) [84]. The estimatedcoefficients are assumed to be instantaneously exact and the set of control input incrementsfor that time step is calculated. At the next time step a new set of coefficients is estimatedand used to calculate a new set of control input increments.The performance of the overall control strategy in situations which necessitate switchingbetween force and motion control is discussed in section 5.4.3.8 Stability and ConvergenceStability and convergence are important and difficult issues in the design of adaptive controllers. For a given set of values for the model coefficients the stability of the controlalgorithm part of an adaptive controller can be considered. In the case of the CPC algorithm it has been shown [33] that, with a finite prediction horizon (N2), the resultingclosed—loop control is stable provided that:1. the open—loop system model is stabilizable and detectable,2. the delay (N1) and control (Nu) horizon are selected with respect to the number ofsystem model states (n) such that N = N1 n and N2 — N1 n — 1, and3. the control weighting factor () is small (i.e. = —* 0).It should be noted that this is only a sufficient (but not necessary) condition for whichstability of the algorithm can be proven. The GPC algorithm is demonstrably stable forsets of parameters (N1,N2,Nu, and ?) other than those which satisfy the above conditions.Chapter 3. Control Algorithm 93Given this result, the issue of stability of the adaptive GPO controller becomes oneof whether or not the system identification algorithm provides model coefficients whichdescribe a stabilizable and detectable open—loop system. The Exponential Forgetting andResetting system identification algorithm (EFRA) [84] discussed in section 3.6 is of therecursive least squares (RLS) family. The asymptotic properties of this type of algorithmare well established [85]. However, those asymptotic properties are concerned with the “bestpossible predictor” for the system which the algorithm can determine. The relationshipbetween that predictor and the stabilizable and detectable model of the open—loop systemwhich is required for stability of the adaptive GPO controller is dependent on such factorsas 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 systemto be controlled is necessary to select an appropriate number of model coefficients and toselect initial values for those coefficients which satisfy the stabilizability and detectabilityrequirements of the adaptive GPO algorithm. Augmentation of that analysis with a seriesof identification experiments on the system would most certainly be beneficial.39 SummaryThis chapter presents the development of a manipulator contact force control strategy basedon an explicitly adaptive implementation of a predictive control algorithm. The controlalgorithm is based on the Generalised Predictive Control (GPO) algorithm of Clarke, etat. [29]. This form of predictive control algorithm was selected over other control methodsbecause 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 predictormodel.The primary contributions of this chapter are the extension of the generic predictive controlalgorithm with a new static equilibrium bias term that allows knowledge of the systemdynamics to be employed to improve the controller performance, development of a newformulation of the closed loop characteristic polynomial which is directly useful in controllertuning, and introduction of a contact control logic level in the control hierarchy to deal withthe discontinuity involved in making and breaking contact between the manipulator tip andthe environment surface.Chapter 4Dynamics and Control Simulation— TWOFLEX4.1 IntroductionThe simulation computer program TWOFLEX incorporates the dynamic model of the planar two flexible link manipulator and deformable environment derived in Chapter 2 andthe adaptive single- and multi-variable long range predictive control algorithms derived inChapter 3. The code is written in FORTRAN. It was originally developed on a VaxStation3200 under the VMS operating system and subsequently ported to a Sun SparcStationunder 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 variables and as plots of the evolution of selected variables with time. The dynamics modelsof the links, actuator, and contact regions are coded in such a way that the program caneasily simulate subsystems of the two flexible link manipulator system (e.g. a single rigidor 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 tobe used for a given run is specified by a set of parameter values which are read from a datafile each time TWOFLEX commences execution.This chapter begins with a description of the overall structure of the TWOFLEX program insection 4.2. Section 4.3 presents the details of the numerical integration of the equations ofmotion for the manipulator/environment system. The description of the program concludes95Chapter 4. Dynamics and Control Simulation— TWOFLEX 96in section 4.4 with a discussion of the routines that simulate the discrete time control algorithm. 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 associated subroutine libraries (approximately 18,200 lines of FORTRAN code in total) is availablein the UBC CAMROL (Computer Aided Manufacturing and Robotics Laboratory) Reportnumber R.92—2 [34]. Section 4.5 describes several tests which were conducted to validatethe operation of the program by comparison to a simulation of a multibody space stationbased flexible manipulator developed by Mah [35]. The chapter concludes with a summaryof the features of the TWOFLEX code.4.2 General Description of the TWOFLEX ProgramA flowchart of the overall structure of the TWOFLEX program is shown in Figure 4.1. Thesteps in the code execution, as represented in Figure 4.1, are described in the followingparagraphs.The program begins with the reading and processing of data from the input files. Thesedata specify the configuration of the system for the run, the initial conditions, and, numerous logical flags to select various options in the code. The main input file for a typicalrun consists of about 50 lines of data. The input routines use free formatting and permitcomments 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. Processing 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 integralfactors, ‘mkt, for the equations of motion are calculated at this point.Output initialisation involves opening of, and writing headings to, the various files whichwill receive output data as the simulation proceeds. A summary of the configuration forChapter 4. Dynamics and Control Simulation— TWOFLEX 97Figure 4.1: Flowchart of the overall structure of the TWOFLEX dynamics and control simulation program.FalseChapter 4. Dynamics and Control Simulation — TWOFLEX 98the run is written out at this point, both to serve as a reference when reviewing stored runresults 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 theapparent separation between the tip of the manipulator and the surface of the environmentis negative, contact exists and the components of the contact force are calculated. In thecase 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 loopis controlled by the value of the integration time variable, t2, and proceeds in steps of dt.The value of dt, is generally selected to be quite small since the main loop includes theintegration of the equations of motion which simulates the response of a continuous timesystem. Within the main ioop, the integration time, is checked to determine if it is modulodtd, the discrete time step. Only if that condition is true are the discrete time controlalgorithm routines executed.The discrete time control algorithm consists of the contact control logic, the parameterestimation algorithm and the control algorithm itself, which calculates the input signals tobe 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 algorithmand instead apply one of a set of open loop input signals to the system. The available openloop 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 integration of the equations of motion over the time interval dt. The DGEAR routine from theInternational 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 areactually integrated is included in section 4.3.Chapter 4. Dynamics and Control Simulation — TWOFLEX 99After completion of the integration time step, the new state of the system is checked todetermine if the contact state has changed. If the manipulator tip is in contact with theenvironment surface the components of the contact force are calculated. The new state ofthe system and a collection of data calculated from it are written as text to the time seriesoutput 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 quantitiessuch as the manipulator joint angles have been surpassed. This limit checking allows runs inwhich the system state “blows up” to be halted before the entire duration of the integrationhas been calculated while preserving, for later analysis, the evolution of the response up tothe time at which the run is terminated.If the time specified for the end of the integration has not been reached the main loopis repeated, after incrementing the integration time, t, by dt2. When the loop has beencompleted for the last time the final results of the simulation are written to the output datafiles and plotted.4.3 Integration of Equations of MotionThe equations of motion for the manipulator/environment system are integrated numericallyusing the IMSL routine DGEAR to yield the approximate continuous time response of thesystem. The DGEAPL routine is based on the work of Gear [87]. It is a backward differencingmethod, meaning that it uses backward difference formulae to approximate derivatives.The algorithm incorporates adaptive step size selection which allows optimal performanceto be achieved by subdividing the interval over which the integration is being performed asnecessitated by the characteristics of the equations, without user intervention. The DGEARroutine 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 1002. it is well suited for integrations from which results are required at frequent, smallincrements of the independent variable.A system of differential equations is said to be stiff if there are two or more very differentscales of the independent variable which simultaneously effect the dependent variables [88].A more mathematically rigorous definition [86] is that the system of first order ordinarydifferential equations given asy’=f(x,y1,y2...,y) (4.1)ofis stiff if some of the eigenvalues of the Jacobian matrix of partial deLivatives, arelarge 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 severalorders of magnitude and thus the generalised coordinates are composed of variations withseveral, widely varying time scales.The simulation uses a small time step, dt1, in the main integration ioop in order toadequately reflect the continuous time nature of the dynamics in contrast to the discrete timecontrol algorithm. Small time steps are also required to adequately resolve high frequencycomponents of the response in graphical presentation of the simulation results. Finally,and most importantly, small integration time steps are necessary to properly detect themaking and breaking of contact between the manipulator tip and the environment. If toolarge a time step is used the manipulator will appear to penetrate the environment duringthe time step in which contact is made and an erroneously large transient contact force willresult. The maximum permissible time step is dependent on the stiffnesses of the contactingsurfaces.Chapter 4. Dynamics and Control Simulation — TWOFLEX 101The DGEAR routine integrates systems of first order, ordinary differential equations.Equations (2.47) through (2.52) are a system of second order ordinary differential equations of the form:Jj=b(q,4). (4.2)Such a system is easily transformed to first order by the substitution:q1 = q and q2= 4 (4.3)which yields,I 0 41 q2. (4.4)0 J q b(q1,q2)In this representation, J is the generalised inertia matrix, q1 is the vector of generalisedcoordinates, 4i = q is the vector of generalised velocities, 42 is the vector of generalisedaccelerations and b (q1,q2) is the vector of values of the right hand sides of the equationsof motion. The matrices, I and 0, are identity and zero matrices, respectively. The statevector for the system is [q1 q2]T•The DGEAR routine calls a user provided routine which must return the first time derivative of the state vector. The generalised velocity part of that vector is simply copied fromthe most recently calculated system state and the generalised acceleration part is calculatedby the steps shown in the flowchart in Figure 4.2. Since the dynamics of the DC motoractuators are represented by first order, ordinary differential equations the state variablesfrom those models are simply concatenated onto the end of the state vector. Likewise, righthand sides of the actuator model equations are concatenated onto the right hand side ofequations (4.4) for simultaneous solution with the equations of motion.Chapter 4. Dynamics and Control Simulation— TWOFLEX 102Calculate InertiaMatrix1Calculate Right HandL Side Vector1Add actuator couplingterms to inertia matrixand RHS vector‘I;Invert InertiaMatrixMultiply RHS vectorby Inertia Matrix Inverseto yield GeneralisedAcceleration VectorFigure 4.2: Flowchart of the calculation of the generalised acceleration vector.4.4 Discrete Time Control AlgorithmThe control algorithm described in Chapter 3 is implemented in a series of subroutines whichare executed only when the integration ioop time, t, is a whole multiple of the discrete timestep, dtd, (that is, t mod dtd = 0). Figure 4.3 shows a flowchart of the steps in the controlalgorithm.The first step in the control algorithm is the contact control logic block. This part of thealgorithm takes, as inputs, signals from the manipulator force sensor and joint encoders,information from the trajectory planner and instructions from the operator. These dataare analysed and used to generate the sequences of future setpoints, to determine whichcontrol mode (force or motion) should be used, and to decide whether or not the parameterestimation algorithm should be activated to update the controller model coefficients. Theprimary purpose of the contact control logic is to govern the actions of the control algorithmChapter 4. Dynamics and Control Simulation TWOFLEX 103hitialise DiophantineIdentity Polynomialsfor Recursion FormulaeFigure 4.3: Flowchart of the discrete time control algorithm.Chapter 4. Dynamics and Control Simulation— TWOFLEX 104during the time period immediately before and after a change in the contact state. Thisfunction is particularly important when the manipulator unexpectedly comes into contactwith the environment, in which case the impact velocity may be large and result in contactforce 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 andits surroundings, the speed of approach to a surface and the time at which the controlshould be switched between motion to force modes can be specified and the contactcontrol logic generates appropriate setpoint sequences and switching signals;• in unknown or uncertain surroundings the contact control logic switches the controlmode from motion to force when a nonzero force signal is sensed and from force tomotion when the signal goes to zero;• when contact is sensed by the force sensor the contact control logic switches to forcecontrol mode and briefly modifies the contact force setpoint sequence from that provided by the trajectory planner to a form suitable to compensate for the contact forcetransient; the setpoint modification is based on an heuristic rule.Contact control logic programs are discussed in more detail, with examples of the resultingclosed ioop responses in Chapter 5.If the contact control logic program specifies that the control should be adaptive inthe current time step the coefficients of the controller model are updated using one of theparameter estimation algorithms discussed in section 3.6. Both the U/D RLS algorithmand EFRA are implemented in the TWOFLEX code. The algorithm to be used is selected bya flag set in the input data file. The algorithm parameters are also included in the dataread at the beginning of the run.Chapter 4. Dynamics and Control Simulation— TWOFLEX 105The actual calculation of the control input signal increments begins with the initialisationof the polynomial coefficients in the Diophantine identity recursion formulae. With thosecoefficients and the coefficients from the controller model, the free response vectors, f1, forone time step into the future are calculated.Following the initialisation step, the Diophantine identity polynomial coefficients andfree response vectors are calculated for each of the future time steps up to the predictionhorizon, N2.The final step in the predictive part of the algorithm is the calculation of the futurestatic equilibrium input sequences from the specified future setpoint sequences. This stepis only applicable in the case of force control since a direct relationship is available betweendesired contact forces and the static equilibrium input levels that would be required toproduce them.For computational efficiency the control horizon is checked before beginning to calculatethe control input increments. If N = 1 the calculation can be quickly performed as a scalaroperation in the case of a SISO system or by Cramer’s rule for a two input, two outputMIMO system. For N > 1, the future step response matrices, G1k, and their transposesare 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 solvedfor the sequences of future control input increments. Finally, the control input incrementsfor the next time step are added to the control input signals.The entire discrete time control algorithm described above is embedded in a generalinput calculation subroutine in the TWOFLEX code. That routine uses flags set in the inputdata file to select the type of input to be used, either the control algorithm or the one ofthe open loop inputs described in section 4.2. The input calculation routine also appliesthe appropriate actuator saturation limits to the inputs.Chapter 4. Dynamics and Control Simulation — TWOFLEX 1064.5 ValidationThe simulation of the unforced manipulator dynamics was validated by comparison with theresults of a program developed by Mah [35]. Mah’s program simulates the dynamics of anorbiting platform and manipulator system modelled as a chain of flexible links connected byflexible joints. It has been validated [89] against two other simulation programs for relatedsystems independently developed by Chan [68] and Ng [72]. Two types of validation testwere carried out:1. the calculated inertia matrices and right hand side vectors for the equations of motionwith specified initial conditions were compared; and,2. the evolution with time of the values of the generalised coordinates were comparedfor 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 sidevectors were compared for the seven cases listed in Table 4.1. The links were modelled as1.Om long, 2.0cm diameter aluminium rods in all cases. For each of these cases the initialinertia matrix and right hand side vector were in agreement to at least three significantdigits. The agreement was least good for terms which depend on the integrals of theflexible link mode shape functions. This is due to the fact that Mah calculates thoseintegrals numerically whereas the analytical expression for those integrals for a cantileverbeam are included in the TWOFLEX code.Comparisons of free response simulation results were made for the three cases listed inTable 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 thesystem. Good agreement was obtained in all three cases. Figures 4.4 shows the response ofthe joint angle generalised coordinates, 1 and 02, for the first 0.5s of case 2. The resultsChapter 4. Dynamics and Control Simulation— TWOFLEX 107Initial ConditionsManipulator Structure 8- 62 62 /11 2/’21 ,1’21Case Link 1 Link 2 [0] [0] [rad/sj [rad/s] [—j [—] [1/sj [1/s]1 Flexible— 0 — 0 0 — 0 —(1 mode)2 Flexible— 0 0 — LO 0.1 —(1 mode)3 Rigid Rigid 30 30 0.1 0.1 — — —4 Flexible Flexible 0 0 0 0 0 0 0 0(1 mode) (1 mode)5 Flexible Rigid 0 0 0 0 0 0 0 0(1 mode)6 Rigid Flexible 0 0 0 0 0 0 0 0(1 mode)7 Flexible Flexible 0 0 0 0 1.0 1.0 0.1 0.1(1 mode) (1 mode)Table 4.1: Configurations and initial conditions for validation of inertia matrix and righthand side vector terms.Initial ConditionsManipulator Structure 6 62 2 /‘11 21-‘/ii /‘21Case Link 1 Link 2 [0] [] [rad/s] [rad/s] [—] [—] [1/s] [1/s]1 Rigid Flexible 20 -25 0 0— 0— 0.1(1 mode)2 Flexible Flexible -20 -75 0 0 0.05 -0.1 0 0(1 mode) (1 mode)3 Flexible Flexible 35 35 0 0 -0.05 0.1 0 0(1 mode) (1 mode)Table 4.2: Configurations and initial conditions for free response simulation validation runs.Chapter 4. Dynamics and Control Simulation— TWOFLEX-70-75-80-85-90-95-100-105-110Time [s]1080 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50Time [s](a) 81 response-4-6-8-10-12U)2. -14-16-18-20-22U)U)a)L.a)Vc’J.50(b) 82 responseFigure 4.4: Comparison of joint angle responses between TWOFLEX and Mah [89] for freeresponse validation.0 .05 .10 .15 .20 .25 .30 .35 .40 .45Chapter 4. Dynamics and Control Simulation— TWOFLEX 109from the TWOFLEX program and those from Mah [89] are initially coincident and differ byonly a small amounts after 0.5s (5000 integration time steps). The responses of the modalamplifier generalised coordinates, and ‘çb21, for case 2 are shown in Figure 4.5 andillustrate the same excellent agreement.4.6 SummaryThe 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, extensively documented and includes optionally compiled code to generate debugging outputfrom almost every subroutine. The integration of the continuous time equations of motionand the calculation of the discrete time control algorithm are conceptually and structurallyseparated in the code, making it a valuable tool for the investigation of both the open andclosed ioop behaviour of the system.Chapter 4. Dynamics and Control Simulation— TWQFLEX.07• 06• 05• 04.03• 02.010- .01- .02-. 03- .04-. 05-. 06- .07- .080Time [s](a) b11 response110.12.1008• 0604• 020-. 02—. 04-. 06-. 08-.10-.120TWOFLEX----jTime [s].05 .10 .15 .20 .25 .30 .35 .40 .45 .50c’J.50(b) 21 responseFigure 4.5: Comparison of modal amplifier responses between TWOFLEX and Mah [89] forfree response validation..05 .10 .15 .20 .25 .30 .35 .40 .45Chapter 5Force Control Algorithm Performance and Evaluation5.1 IntroductionThis chapter presents results and discussion of the analysis of the dynamics of flexible linkmanipulator force control systems and the results of numerous runs of the TWOFLEX dynamics and control simulation program. Analysis of the eigenstructure of the system revealsorderly 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 theadaptive, multivariable, long range predictive control algorithm developed in Chapter 3.The results are organised roughly in order of increasing complexity of both the manipulator/environment system and the maneuvers which are simulated. This organisation seeksto emphasise the basic performance of the control algorithm before introducing such complications 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 whichwill be addressed can be broadly classified as:• linear analysis of various manipulator configurations in contact with their workingenvironments;• force control step response results for various manipulator configurations; and• the problem of maintaining control when contact is made or broken.111Chapter 5. Force Control Algorithm Performance and EvaluationI Stop MotionStrategy(5.4.1)Making andBreaking ContactForce SetpointModification Strategy(5.4.2)112Figure 5.1: Topics discussed in Chapter 5. Section numbers in which the discussion of eachtopic appears are shown in parenthesis.Chapter 5. Force Control Algorithm Performance and Evaluation 113Figure 5.1 indicates the sections in which each of these topics are discussed for each of themanipulator configurations considered.The chapter begins with an analysis of the eigenvalues and eigenvectors of the equationsof motion for a single link/environment system. Analytical approximations to the trendsobserved in the numerical evaluation of the eigenvalues are developed and the implicationsof changes in the modal structure of the system for contact force control are discussed. Theperformance of the controller for force control of a single flexible link is discussed in section 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 presentedas well as the derivation of the structure and initial coefficient values of the discrete timetransfer function model for the control algorithm. Simulated step and ramp response resultsare shown to demonstrate the performance of the controller. The effects of the controllerhorizons and weighting factors in shaping the closed ioop response are also discussed in thecourse of Section 5.3. The discontinuous nature of the manipulator/environment systemdynamics is considered in section 5.4 with a discussion of the problems involved in maintaining control during the making and breaking of contact. The use of contact control logicto overcome those problems is discussed along with the presentation of simulation resultsfor two different contact control logic strategies. The performance of the multivariable formof the control algorithm is demonstrated by considering force control for a two link manipulators in section 5.5; both rigid link and flexible link manipulators are discussed. Resultsof simulations of two links of a prototype model of the DC motor driven Space StationMobile Servicing System (MSS) manipulator are presented and discussed in sections 5.6.The chapter concludes with a summary of the force control algorithm performance. Someof the work presented in sections 5.3 and 5.4 has already been published [22,27].Chapter 5. Force Control Algorithm Performance and Evaluation 114Figure 5.2: Single rigid link in contact with a deformable environment.5.2 Linear AnalysisThis section presents a linear analysis of the dynamics of a single link in contact witha deformable environment. Particular emphasis is placed on the interaction between thestructural stiffness of the manipulator link and the stiffnesses of the contacting regions of thelink tip and the environment. Using analytical and numerical analysis of the eigenvalues andeigenvectors 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/environmentsystem as the effective contact stiffness increases. The implications of these modal changesfor contact force control are discussed.5.2.1 Rigid LinkConsider first a single rigid link which is the limiting case of infinite structural stiffness. Theequations of motion for the system shown in Figure 5.2 are given in section A.6. Simplifyingthose equations by neglecting damping, gravitational effects and the small coupling inertiaL11k/;Chapter 5. Force Control Algorithm Performance and Evaluation 115terms between the link and contact mass allows them to be linearised about the nominalstate 6 = 0 and written as:10181 = ui(t)—k3L1(L1e— (5.1)mcfy = —k3 (E — L61) keyEy, (5.2)where the generalised coordinates {i, e} are now perturbations about the nominal state.For the sake of generality the joint torque, r1, has been replaced by a general control input,ui(t).To study the eigenstructure of the system these equations are transformed by applyingthe Laplace transform and rewriting in matrix form:1J(s) = ui(s), (5.3)0where= 101s2 + k5L1 —k3L1. (5.4)—kL1 ms2 + (k3 + k)The characteristic equation of the system is given by setting the determinant of J(s) tozero:=I01ms4+ [I01(k3+ key) + mck8yL] 2 + ksykeyL = 0. (5.5)The natural frequencies of the system (eigenvalues of J(s)) are given by the roots of equation (5.5). We will first investigate the variation of the natural frequencies with contactstiffness numerically by considering a 1.Om long, 2.0cm diameter aluminium rod treatedas a rigid link. Given the density of aluminium of 2690kg/rn3 such a link has a mass of0.8451kg and a moment of inertia about the joint axis of101 = M1(d)2+ = 0.2817kg m2. (5.6)Chapter 5. Force Control Algorithm Performance and Evaluation 116The contact region masses, m3 and me, are taken as 0.19 each. Figure 5.3 shows thevariation of the natural frequencies of the system with effective contact stiffness rangingfrom 50N/m to 5MN/m. The effective contact stiffness is defined askeffksy+key (5.7)The data for this plot was generated by numerical calculation of the eigenvalues of J(s)for various values of k3 and key. The plot clearly indicates that the natural modes of thesystem are well separated in frequency and both frequencies increase in proportion to thesquare root of the contact stiffness. These observation can be confirmed analytically usingequation (5.5). Substituting (jw) for s,I0im(jw)4+ [Ioi(ksy + key) + mkL} (jw)2 + ksykeyL = 0, (5.8)gives a quadratic equation of the formax2+bx+c=0, (5.9)the roots of which are the squares of the natural frequencies of the system. The coefficientsof equation (5.8) are such that b2 >> 4ac and so the quadratic formula can be approximatedusing the binomial expansion to yield:2 iF 12(sw) = — —b+ b .—4ac)- :b+b(1 4ac)]1 / 1 4ac 1 4ac2=-— or (5.10)Chapter 5. Force Control Algorithm Performance and Evaluation 11711111111 I I 1111111 I I 1111111 11111111 I I 1111111 I 111114iø-1/2• 1 ksy+key•2[ms+me -iøN>%C-)C-a):1/2ioe I I iii I I liii I i i I i I10_2 101 i00 101 102 iø i04Effective Contact Stiffness [kN/m]Figure &3: Variation of the natural frequencies with effective contact stiffness for a singlerigid link/environment system.Chapter 5. Force Control Algorithm Performance and Evaluation 118Note that for the case shown in Figure 5.3, and indeed for most cases of interest, m <<101,therefore,2 ksy + key and keL (5.11)m 101These results confirm the correlation of the natural frequencies with the square root ofthe contact stiffness that is shown in Figure 5.3. Also note that the natural frequencies areinversely proportional to the square root of the link moment of inertia and the contact regionmass. In what follows we consider a given contact mass and link inertia and concentrate onthe effect of changing contact stiffness on the system.Based on the approximate natural frequencies given by equations (5.11) the modal eigenvectors of J(s) are approximated byE [ei e2]1(5.12)1 (1_)LFor surfaces of comparable stiffness, such as is the case in metal on metal contact, the ratioof key to k3 is of order 1. Combining this condition with the assumption that rn,, <<we see that the first element of the e1 eigenvector is very much smaller than the second.This result is confirmed by the numerically determined eigenvectors for the case shown inFigure 5.3 with key = k3. The eigenvectors, normalised to unit magnitude, are found to beindependent of the effective contact stiffness:—1.0640 x iO 0.8943E = { e1 e2 j . (5.13)1.0 0.4472The relative magnitudes of the elements of the eigenvectors indicate the contribution ofeach of the generalised coordinates to the natural modes, that is:[:1 E (5.14)Chapter 5. Force Control Algorithm Performance and Evaluation 119It is evident that the mode, which corresponds to w1, the higher of the two naturalfrequencies, is composed almost entirely of the e, generalised coordinate. Physically, thiscomposition means that the high frequency mode is the oscillation of the contact regionmass, m, between the virtually stationary link tip and the environment body. The othermode is the oscillation of the link about its joint. This mode includes an e componentbecause the contact region mass, m, moves in phase with the link tip. Because m <<101the energy of the 2 mode is concentrated in the link motion. The ratio of the componentsof this mode is determined by the contact stiffnesses. In equation (5.13) the ratio is 2 to 1because = key for that case.Control of the system is effected by the relationship of the natural modes to the contactforce and the control input. The contact force exerted by the link tip on the environmentis given by equation (2.57). For a single rigid link with damping effects neglected it can belinearised to:T!!!&k L1= mc. (5.15)1( j.—m8—Noting that the contact force is expressed as a linear combination of the generalised coordinates, the matrix ET can be used as a linear transformation to obtain an expression forthe contact force in terms of the natural modes:= (ETcy)T[1]L1 1Li_1(i+) mk,,, —. (5.16)L1 + (1 — ) (::: — 1) L1 ?72Chapter 5. Force Control Algorithm Performance and Evaluation 120For comparably stiff surfaces we see that the contact force is composed almost entirely ofthe 2 natural coordinate, that is, oniy the low frequency mode contributes significantly tothe contact force. The numerical values confirm this observation:1F —5.3201 x iO=. (5.17)0.4472The ET transformation can be used to show the relative excitation of the natural modesdue to a step change in the control input. Noting that the control input, u1(t), acts on the8i generalised coordinate, the relative excitation of the natural modes is given by:ET1ui(t)0—1.0640(5.18)0.8943This result implies that the control input excites almost exclusively the 2 coordinate.To summarise, analysis of the eigenvalues and eigenvectors of a linearised, undampedmodel of an infinitely stiff link in contact with the environment under the assumptions thatthe contact region mass is small compared to the moment of inertia of the link (m <<I)and that the contacting surfaces are comparable (k k, and m3 me) reveals that:1. the natural frequencies of the system are widely separated in frequency and, for agiven link and surface, are proportional to the square root of the effective contactstiffness;2. the high frequency mode is the vibration of the surfaces of the link tip and the environment 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 121Wi1:110Figure 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 timepredictive control algorithm developed in Chapter 3, are that the high frequency mode canbe ignored and the sampling rate selected to satisfy the Nyquist criterion for w2. Becausethe control input is directly coupled to the contact force output through the w2 mode thesystem falls in the class of systems described by Clarke, et al [29] as “easy” to control.For such systems a prediction horizon of N2 10 and a control horizon of N 1 arerecommended to obtain satisfactory control. As noted at the outset the rigid link case isprimarily of interest for comparison as the limiting case of infinite link stiffness.5.2.2 Flexible LinkThe equations of motion for a single flexible link in contact (see Figure 5.4) are given insection A.4. Modelling the link with a single structural mode and neglecting damping,101 1111 1211 1311kTyChapter 5. Force Control Algorithm Performance and Evaluation 122gravity and link to contact mass inertial coupling the equations of motion, linearised about{ ; è i4 0, are:+ = ui(t) — k3L1(L10 +q110b1-(5.19)111161 +I211b1 = I3111I’11 —k3110 (L16 +q110b — (5.20)më = —k3 (e — L1e9 —q110,bii)— keyE!,. (5.21)The joint torque, r1(t), has been replaced with a general control input, u1(t), and thegeneralised coordinated {i, e,} are now perturbations about the nominal state.As in section 5.2.1 we will begin by considering the natural frequencies of the system.In preparation, the Laplace transform is appled to the linearised equations of motion andthey are rewritten in matrix form:1J(s) bii(s) = 0 u1(s), (5.22)0whereIs2 + k8L1 1s2 +k3L1cb1, —k8L1J(s)= Lj11s2 +k3Liqii0 1211s + 1311 +k5çb10 —k5b110 . (5.23)—k3L1 —k54110 m.s2 + k3 + keyThe derivation of analytic expressions for the eigenvalues of J(s) is rather involved. Moreimmediate insight can be gained by solving for the eigenvalues numerically and consideringthe 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, thephysical parameters of the link model must now include quantities related to the structuralcharacteristics of the link. The modulus of elasticity for aluminium is 7.31 x 10’°N/m2Chapter 5. Force Control Algorithm Performance and Evaluation 123Mode Mode ShapeNumber Function Integrals(i) Ifl ‘21z ‘31i 4’iiO1 0.4807 0.8451 7096.0 2.0Table 5.1: Mode shape function integral factor values for the first mode of a 1.Om long,2.0cm diameter, aluminium cantilever beam.which, combined with the link dimensions, gives a flexural rigidity in transverse bending of:El1 = Ed = 574.024N m. (5.24)The first mode of a cantilever beam is used as the admissible mode shape function for thelink structure. Assuming uniformly distributed mass and rigidity, the values of the modeshape function integral factors are shown in Table 5.1. These values are calculated, usingclosed form expressions for the integrals, from the physical parameters of the link in theinput 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 twonatural frequencies for the same link, modelled with infinite structural stiffness, is shownas a dashed line for reference. As in the case of the rigid link the highest natural frequencyis very closely approximated by2 = k3 + key (5.25)The intermediate frequency, w2, is independent of the contact stiffness for small valuesand proportional to the square root for large ones. Conversely, w3, the frequency of theslowest mode, is indistinguishable from the rigid link frequency for small contact stiffnessesand constant for large values. The isolation of the w1 mode, which is the vibration of thecontact region, from the other two modes suggests the consideration of a simplified modelof 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 regionChapter 5. Force Control Algorithm Performance and Evaluationncr 310>%0C0)0•0)UCuI1 102zFigure 5.5: Variation of natural frequencies with effective contact stiffness for a single flexiblelink with one structural mode (El1 = 574N m2). Lower frequency for an equivalent rigidlink is shown dashed.1241 4I 11111111 I I Ilillif I 11111111 I 11111111 I I 1111111 I 111111:iModeFlexible Link— —- EquivalentjinJkeff=kN/m10’ioio_2 10_iI’I I II 11111 I I II100 101 jØ2Effective Contact Stiffness [kN/mj111111 I I IlIlill I 1111111Chapter 5. Force Control Algorithm Performance and Evaluation 125Wi1 (p110stiffnesses, k3 and key, with the corresponding effective contact stiffness, keff, yields thefollowing equations of motion::loi8i + Iiiiii = ui(t) — keffL (L18 +&10b1), (5.26)+I211b1 = I3iii1 keffci1o (L11 + 110b). (5.27)This reduced system can be used to explain the asymptotic behaviour of the lower twonatural frequencies shown in Figure 5.5.The characteristic equation of this system is(IOiI2i. — z2) (w)4+ [IiI311 + keff (IoiqS10 +I211L — 2IiiiLiqiio)] (jw)2 +I3likeffL = 0. (5.28)101 1111 1211 1311k effFigure 5.6: Single flexible link in contact with a deformable environment approximated bya steady state contact dynamics model./Note that the coefficient of (jw)2 is dominated by the ‘oi’3n term for small effective contactstiffnesses and by keff +1211L — 21111L40)for large values of keff. With this inmind, the binomial expansion approximation of the quadratic formula used in section 5.2.1reveals that, for a given link, the lower two natural frequencies approach, for small valuesof kff— ‘o1’311—— T2211-‘-iiiand = keffL101(5.29)Chapter 5. Force Control Algorithm Performance and Evaluation 126and for large keff:2 — keff (I0iq10 + T211L —2I111LqS0)— 1011211and‘T r22__________________________________—(Io+I211L—2I111L110)These results predict very closely the values of w2 and w3 shown in Figure 5.5 at the extremesof effective contact stiffness. The transition from the frequencies given by equations (5.29)to those given by equations (5.30) occurs when the magnitudes of the two terms in thecoefficient of (jw)2 in equation (5.28) are approximately equal. Empirically, the values ofkeff over which the transition occurs fall in the range1011311 1011311+I211L —2I111L110keff < 10i +I211L —2I111L110(5.31)For the case shown in Figure 5.5 this result implies that the transition is centred at keff40.7kN/m.The changes in the modal structure of the system as the contact stiffness increasesare further illuminated by considering the eigenvectors. Unlike the rigid link case, thenormalised eigenvectors of J(s) are not independent of the contact stiffness. For the purposesof control the most important effects of this dependence on contact stiffness are variations inthe contribution of the natural modes to the contact force and the excitation of the naturalmodes by the control input. The contact force, derived from equation (2.57) by neglectingthe damping and link 2 terms and linearising is:= [ mL1 mello (m5 — me) ] Ii (5.32)Using the matrix of eigenvectors, ET, to transform the components of the contact force expression from generalised coordinate space to natural coordinate space produces the resultsChapter 5. Force Control Algorithm Performance and Evaluation 127.45 I I I I 111111 I.40.35-.30 -.25a)-a)0-g.15.r3O-I...0E05Zo0005--.10-. 20102 101 10 101 102 iøEffective Contact Stiffness [kN/m]Figure 5.7: Variation of the natural mode components of the contact force with effectivecontact stiffness for a single flexible link with one structural mode. Contact force components are normalised by k8.I 1111111 I 1111111 I I huh uiiul I I 11111Chapter 5. Force Control Algorithm Performance and Evaluation 128shown in Figure 5.7. These data are for the 1.Om long, 2.0cm aluminium link modelledwith one flexible mode and were obtained by calculating the eigenvectors of J(s) and performing the transformation on the contact force components numerically. The figure showsthat at low values of contact stiffness the contact force is composed of components of the72 and natural modes, with dominating. As the effective contact stiffness increasesthe contribution of 773 disappears and 772 dominates. The contribution of m to the contactforce is negligible. In terms of control this implies that as the contact stiffness increasesthe contact force changes from being similar to that which would arise were the link rigidto being dominated by the structural characteristics of the link. In particular this meansthat the required sampling rate is governed by w3 at low contact stiffness but by w2 at highvalues.To complete the picture we consider the effect of the control input on the excitation ofthe natural modes. Figure 5.8 shows the components of the natural coordinates that arethe result of the numerical transformation from generalised coordinate to natural coordinatespace of the input vector, [1 0 01T from equation (5.22). In contrast to the contribution ofthe natural modes to the contact force, the excitation of the 772 and 773 modes by the controlinput remains relatively constant over the range of contact stiffnesses, with 773 receiving themost excitation. The 771 mode undergoes negligible excitation. For contact force controlthis result means that, without making any contribution the the controlled variable at highcontact stiffness, the 773 mode remains important as the primary route of input of controlenergy into the system.All of the results presented above are for a specific link structural stiffness. Reducing theflexural rigidity of the link from 574.024N m2 to 40.ON m2 produces the results shownin Figure 5.9. The pattern of the variation of natural frequencies with contact stiffness issimilar but it is shifted to the right and downward. That is, the horizontal asymptotes of w2and w3 occur at lower frequencies and the transition from w3 dominance to w2 dominanceChapter 5. Force Control Algorithm Performance and Evaluation 1291.0 i I I 1111111 I 11111111 I IIIIII I I IIIIiii113.8.7.600oxW.5::.4.3 -.2 -- 110 I huuH1 I uhuHIl I I hII I I 11111., I I1W2 101 101 102 i& i04Effective Contact Stiffness [kN/m]Figure 5.8: Variation with effective contact stiffness of the relative excitation of the naturalmodes of a single flexible link with one structural mode in contact.Chapter 5. Force Control Algorithm Performance and Evaluation•i::r i’ ->‘C)Ca)Cci)I-.LL(1D(I,z1301 51 4I I 1111111 I I IIIIIIj I I 1111111 I I 1111111 I I 11111!_____1 ModeFlexible LinkEquivalentRigid Link1k9ff= 2.8 kN/m1011ø 11111111 11111111 i 11111111 11111111110_2 10_i i00 101 102 iøEffective Contact Stiffness [kN/m]Figure 5.9: Variation with effective contact stiffness of system natural frequencies for a softlink (El1 = 40N . m2). Lower frequency for an equivalent rigid link is shown dashed.Chapter 5. Force Control Algorithm Performance and Evaluation 131occurs at lower effective contact stiffness (keff 2.8kN/m).The modelling of the link structure using a single admissible mode shape function conveniently permits analytical approximation of the asymptotic behaviour of the natural frequencies. However, additional modes may be required in the model, depending on the degreeof 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 linkwith its structural characteristics modelled by the first four modes of a cantilever beamwith a flexural rigidity of El1 = 200N m. As in the previous cases the highest frequencymode, associated with the vibration of the surfaces in the contact region, is proportional tothe square root of keff and well separated from the other modes. For iow contact stiffnessesthe lowest frequency mode is proportional to the square root of keff and the system is dominated by rigid link-like behaviour. With increasing keff the slowest mode becomes constantand each of the higher frequency modes successively passes through a transition from oneconstant frequency to a higher one. During the transitions each of the modal frequenciesbecomes proportional to k!ff and dominates the system. This progression of dominate modesis clearly shown in Figure 5.11 in which the variation with effective contact stiffness of thenatural mode components of the contact force are plotted. While the modal composition ofthe contact force changes markedly with increasing keff the excitation of the natural modesby 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 giveneffective contact stiffness, a flexible link/environment system in which the link structure ismodelled by several structural modes is analogous to a system with one structural mode forthe purposes of selecting an appropriate controller sampling interval because the contactforce is dominated by one of the modes. On the other hand, since the control input suppliesenergy to all but the highest frequency mode, the controller model and parameters must bespecified with due attention to the true effective order of the system.z>.C)C0D0UD(‘3zi itiiiiliøEffective Contact Stiffness [kN/m]Figure 5.10: Variation with effective contact stiffness of system natural frequencies fora flexible link with four structural modes (El1 = 200N . ma). Lowest frequency for anequivalent rigid link is shown dashed.Chapter 5. Force Control Algorithm Performance and Evaluation 132LI IIIIII I I 1111111 I 11111111 I 11111111 I 1111111 I I_____1 ModeFlexible Link——— EquivalentRigid Link1 ô11 411011 ø I .::iw 10=1 10 101 102Chapter 5. Force Control Algorithm Performance and Evaluationcl)-D 0o LLct—ococi)—,-OEzoC)133403530.25.201510050- .05-.10-.15- .201 10_I i00 101 102 i03Effective Contact Stiffness [kN/m]1 4Figure 5.11: Variation of the natural mode components of the contact force with effectivecontact stiffness for a single flexible link (El1 = 200N . m2) with four structural modes.Contact force components are normalised by k3.Chapter 5. Force Control Algorithm Performance and Evaluation 1341.0 I 11111111 I I 1111111 I I 1111111 I I IIIIII I I 1111111 I I 111111.8- 14.7.60w •.ooxh0)a)4.3.20 I liii. I I 1111111 I I Ijilill IIInI1Ø_2 10_iEffective Contact Stiffness [kN/mjFigure 5.12: Variation with effective contact stiffness of the relative excitation of the naturalmodes of a single flexible link (El1 = 200N . rn2) with four structural modes.Chapter 5. Force Control Algorithm Performance and Evaluation 1355.3 Force Control for a Single Flexible LinkThe first demonstration of the performance of the adaptive, long range predictive forcecontrol algorithm is for the case of a single flexible link in contact with a deformable environment surface. The physical parameters of the particular system that was simulatedare given in the following sections along with the calculation from those parameters of thecoefficient and parameter values used in the TWOFLEX program.5.3.1 Link Model ParametersThe link is modelled as an aluminium rod 2.0cm in diameter and 1.Om long. From thesedimensions the volume of the link is:V1 = dL1 = 3.1415 x 104m3. (5.33)Given the density of aluminium of 2690kg/rn3,the mass per unit length (p1) of the link is0.8451kg/rn. The moment of inertia about the joint axis is:= M1 (d)2 + = 0.2817kg m2. (5.34)Finally, the modulus of elasticity for aluminium is 7.31 x 10N/m2which, combined withthe link dimensions, gives a flexural rigidity in transverse bending of:El1 = Ed = 574.024N m2. (5.35)The gravitational field is assumed to be perpendicular to the plane of the link motion andtherefore has no effect.The structural dynamics of the link are modelled by using the mode shapes of a cantileverbeam as admissible functions. Based on the uniformly distributed mass and rigidity valuescalculated above the natural frequencies of the beam and the values of the mode shapeChapter 5. Force Control Algorithm Performance and Evaluation 136Mode Natural Mode_Shape Function IntegralsNumber Frequency‘31i(i) (Hz) (Ivg.m2) (kg.m2) (N.m2) (m)1 14.6 0.4807 0.8451 7096 2.02 91.4 7.6707 x 10—2 0.8451 2.7870 x 1O -2.03 255.9 2.7395 x 10—2 0.8451 2.1850 x 106 2.04 501.5 1.3980 x 10—2 0.8451 8.3907 x 106 -2.05 829.0 8.4569 x i0 0.8451 2.2929 x io 2.0Table 5.2: Natural frequencies and mode shape function integral factor values for the firstfive modes of a 1.Om long, 2.0cm diameter, aluminium cantilever beamfunction integral factors for the equations of motion for the first five modes are listed inTable 5.2. All of these data were calculated from the physical parameters of the link in theinput processing step of the TWOFLEX program.5.3.2 Controller ModelWhile the dynamic simulation of a single flexible link in contact is based on the equationsof motion (A.11) through (A.13), the force control algorithm requires a linear model in theform of a discrete time domain transfer function. The equations of motion can be linearisedabout a nominal zero state, transformed into algebraic equations using the Laplace operatorand written in matrix form, as demonstrated in section 5.2.2, to obtain a matrix equationof the form:10?,b12(S) 0J(s) (5.36)0eu(s) 0Chapter 5. Force Control Algorithm Performance and Evaluation 137Likewise, the contact force equation can be linearised and expressed in the form:9i(s)L’1i(s)F = ci2(3) (5.37)/‘in (s)e,(s)These expressions are combined to yield the continuous time transfer function for the system:JHF(s) 1cT — 5 38Ui(S)- J(• (. )(—1)’+ J1where is the ij minor of the n x n matrix J(s).The discrete time transfer function which constitutes the controller model is obtainedfrom equation (5.38) by substitution of numerical values and transformation to the discretetime domain using the selected sampling rate and assuming zero-order hold sampling. Thespecific 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 relativelysoft (k3 ke = 6200N/m) contacting surfaces on the link tip and the environment. Thecontact region masses on each surface are taken to be m5 = me = 1.0g. The link structure ismodelled using the first two cantilever beam mode shapes as admissible functions. Portionsof this work have already been published [22,27].Chapter 5. Force Control Algorithm Performance and Evaluation 138Controller ModelWith k ke = 6200N/m, m5 = me = 1.0g, n1 = 2 and the values given in Table 5.2equation (5.38) yields the continuous time transfer functionFr(s) 0.1944s6 + 1.0486 x 106s4 — 9.6132 x lOis2 + 7.6021 x l0u(s) — 1.8731 x 10_6s8 + 18.933s6 + 4.6472 x 1Os + 8.0802 x 1012s2 + 7.6021 x 1016(5.39)which has open loop natural frequencies of 15.81Hz, 66.29Hz, 303.5Hz and 409.0Hz. Figure 5.13 shows the plot of natural frequencies versus effective contact stiffness for thissystem with keff 3100N/m marked. The relationships among the modal natural frequencies shown in Figure 5.13 are more complicated than those seen in the similar figures insection 5.2. This increased complexity is due to the fact that the contact region mass of 2.Ogis an appreciable fraction of the link moment of inertia (0.2817kg . in2). With this in mind,we still expect the system dynamics to be dominated by the 15.81Hz and 66.29Hz modesand use a sampling interval of 4ms which satisfies the Nyquist criterion for the 66.29Hzmode. The two higher frequency modes are neglected because keff is such that they arein the range in which the 303.5Hz mode frequency is independent of keff and the 409.0Hzmode frequency is proportional to kff; this would not be the case if the effective contactstiffness were significantly lower or the contact region mass appreciably higher. While thechoice of 250Hz (h = 4ms) sampling implies that the two highest frequency modes will bealiased, the controller performs relatively well owing to the relatively low excitation of thesemodes.With a sampling interval of 4ms and zero order hold sampling equation (5.39) is transformed to give the discrete time transfer function controller model:F(q’)u(q)—14.7— 2.38q + 12.8q + 5.36(q + q4) + 12.8q5 — 2.38q6 — 14.7q71— 0.3lq1 + O.84q2— 0.46q3— 0.059q4 — O.46q + 0.84q6— 0.31q7+ q_8Chapter 5. Force Control Algorithm Performance and Evaluation 139iø. I 1111111 I 1111111 I 1111111 I 1111111 I I IIIIIIiøkif 3 1 kN/miø>.C.)Ca,DD 2.10z101iOø I I iiiiiI I I iiiiiil I iIiiIiiII I I iiiiiil IIIIiiI102 101 101 102 i03 i04Effective Contact Stiffness [kN/m]Figure 5.13: Variation of natural frequencies of a single flexible link (El1 574N m, twostructural modes) in contact.Chapter 5. Force Control Algorithm Performance and Evaluation 1403025‘20a)C.)015olO50Time [s]Figure 5.14: Contact force response of a single flexible link on a soft surface(k8 = ke = 6200N/m).(5.40)Step ResponseFigure 5.14 shows a series of ramp and step responses from the closed loop system. Thelink is initially stationary and just touching the environment surface, such that the contactforce is zero. The parameters for the predictive control algorithm are a delay horizon (N1)of 1 time step, a prediction horizon (N2) of 20 time steps, a control horizon (Nu) of 12 timesteps, a control increment weighting factor (Ar) of 1.0 and a static equilibrium bias termweighting 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 141In Figure 5.14 the desired contact force setpoint is represented by the dashed line andthe 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, withthe exception of some minor perturbations during the first lOOms which are caused bythe 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 someovershoot and oscillation about the new setpoint level. The magnitude of the overshootsand oscillations decreases with subsequent steps as the system model is refined by theidentification algorithm. The inherent integral action of the control law ensures that thereis no steady state error. The overall controller performance illustrated by Figure 5.14 andthe other results presented in this chapter is superior to that demonstrated by the othercontrol methods (see section 3.1) which were investigated.One characteristic of the predictive controller that must be noted is its inherent tendencyto anticipate changes in the setpoint level. This is a result of the control law being based onthe minimisation of not only the current force error but also all of the predicted future errorsup to the prediction horizon. This characteristic is most clearly observed in the deviationfrom 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 anticipationof the next setpoint change by the controller is, at least in part, responsible for the systemnever 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 modeldescribed in section 2.5. The link structural dynamics are modelled by a single cantileverChapter 5. Force Control Algorithm Performance and Evaluation 142Radius of Curvature [rn] 0.002 1000Elastic Modulus [N/rn2] 2.1 x 1011 7.1 x 1010Poisson’s Ratio 0.300 0.334Density [kg/rn3] 7849 2690Eff. Planar Strain Modulus [N/rn2] 7.9145 x 1010Eff. Radius of Curvature [m] 0.002Total Contact Deflection [m] 1.9986 x 10_6Eff. Contact Stiffness [N/rn] 5.0037 x 106Contact Radius [m] 6.3222 x iOSurface Deflection [m] 5.1405 x iO 1.4845 x 106Linear Contact Stiffness [N/rn] 1.9453 x i0 6.7365 x 106Estimated Contact Mass [kg] 1.3031 x 10_il 3.7235 x lOTable 5.3: Material properties and Hertz model parameters for steel on aluminium contact.beam mode.Contact Model ParametersThe contact between the link tip and the environment is modelled as a 4mm diameter steelbearing attached to the link tip and making contact with a flat aluminium plate. Thephysical 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 ofthe contacting surfaces of 1.9986 x 106m. The deflections of the individual surfaces areinversely proportional to their planar strain moduli. A tangent to the force deflection curvegiven 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 contactregion and estimates of the mass of material on each surface moved during contact. Tileresults of these calculations for the case of a steel bearing on an flat aluminium surface arelisted in the lower portion of Table 5.3. These calculated contact mass and stiffness valuesSteel AluminiumBearing PlateChapter 5. Force Control Algorithm Performance and Evaluation 143are used for the variables m3, me, k3, and in the equations of motion.Controller ModelUsing the contact model parameter values calculated above and those for the link dynamicswith one structural mode calculated in section 5.3.1, equation (5.38) yields a model of theopen loop system with natural frequencies of 60.42Hz, 945.7Hz, and 114.9MHz. The latterfrequency corresponds to the mode dominated by the vibration of the contact region massbetween the stationary environment surface and link tip. In contrast to the situation insection 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.2suggest that, under the condition of large contact stiffness and small contact mass (comparedto the link moment of inertia) the contact region mass vibration mode is neither significantlyexcited by the control input nor does it contribute appreciably to the contact force and canthus 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 equationsof 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 , wherec = kff { L1 (5.41)and continuous time domain transfer function relating the control input, u(s), to the contactforce, Fr(s), is:F(s) — 1u(s)— J(5)I—Chapter 5. Force Control Algorithm Performance and Evaluation 144keff (12 1L1— Ii&i) 52 + keffI3llL1 5 42where(1011211 — I2) 4+ [1011311 + keff +I211L —2I111L110)}2+keffI3llL. (5.43)Substituting numerical values into this expression gives:Fr(s)— —5.8193 x 105s2 + 3.5506 X 10u(s)— 6.9922 x 10s + 2.4768 x 105s2 + 3.5506 x 1010’ ( . )Figure 5.15 shows the variation of the natural frequencies with effective contact stiffness forthe system described by equation (5.44). At keff = 5.0037MN/m the natural frequenciesare 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 the945.7Hz mode and zero order hold sampling yields the discrete time transfer function:F(q’)— —2.294 + 2.303 (q + q2) — 2.294q3—1 (5 45)u(q’)— 1—2.043q’ + 2.105q —2.043q + q4Equation (5.45) provides the initial coefficient values for the force controller model. The coefficient 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 characteristics of the system for various controller horizons and weighting factors, as described insection 3.2.1.Step ResponseThe 114.9MHz mode in the dynamics mode of the system indicates that the equations ofmotion are very stiff. As a result the integration algorithm time step must be very smallChapter 5. Force Control Algorithm Performance and Evaluation10 —102 101 j2 iøEffective Contact Stiffness [kN/m]Figure 5.15: Variation of natural frequencies of a single flexible link (with one structuralmode) in contact.145k =5.0037MN/moffI I IIff=61.55N/rn:>‘0a)Dc,.U(0L..DzI I iiiiiil I I iiiiiil I I 111111 I I I 1111111 I I I IIIIIChapter 5. Force Control Algorithm Performance and Evaluation 146N2 N Overdamped Mode Underdamped ModeDominant Pole fd [Hz] C18 1 -45.21 857.5 0.30420 2 -96.76 1429 0.35919 3 -84.38 1429 0.56221 4 -99.13 1429 0.239Table 5.4: Stabilising controller horizons and closed loop characteristics for steel on aluminium contact (keff = 5.0037MN/m) for a single flexible link with one structural mode.fd = damped natural frequency and (= damping ratio.and long run times are expected. Figure 5.16 shows the initial 12.5ms of a step responsewhich required 50.3 hours of CPU time to calculate on a Sun SparcStation 2. Also shown isthe response calculated using a steady state approximation of the contact dynamics whichrequired only 23.2s of CPU time. The results are almost indistinguishable and hence it isconcluded that, under conditions when the contact dynamics mode is well separated fromthe other modes of the system, the contact dynamics can be approximated by their steadystate response with substantial savings in computation time and negligible difference in thecomputed results.Using equation (5.45) and the expression for the closed loop transfer function derivedin section 3.2.1, four combinations of prediction horizon (N2) and control horizon (Ne)which yield stable control were identified. They are listed, with corresponding approximatecontinuous time domain closed loop characteristics, in Table 5.4. In all four cases the closedloop 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 itsoscillation disappear long before the response, dominated by the smaller magnitude poleof the overdamped mode, reaches the setpoint. Figure 5.17 shows the step response of theadaptive predictive force controller with N2 = 20, N,, = 2, ), = 0, and .Xse = 0. Theresponse shown starts from a stable 5N contact force level in order to focus on the stepChapter 5. Force Control Algorithm Performance and Evaluation 147I I I I I I3.2______Steady State3.0 - Approximation2.8 FuliContact2.6:I2.4-2.2 -2.0-ci)-C.)C-1.6 -8 1.4 -1.2 -1.0 -.8 -.4 -.2 -0 ‘ I I I I I0 .001 .002 .003 .004 .005 .006 .007 .008 .009 .010 .011 .012 .013Time [SIFigure 5.16: Comparison of results from simulations using full contact dynamics and steadystate approximation.Chapter 5. Force Control Algorithm Performance and Evaluation 1482220181614z121000864200 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Time [s]Figure 5.17: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (ken = 5MN/m) with N2 = 20, N = 2, ) = 0, and‘se 0.Chapter 5. Force Control Algorithm Performance and Evaluation 149response performance of the controller and avoid the complications due to the discontinuityin the system dynamics which occurs at ON as contact is initiated. Control through thediscontinuity is discussed in section 5.4.The N2 = 20 and N = 2 combination of horizons offers the best compromise of rise timeand control signal activity. The rise time is related to the magnitude of the dominate closedloop pole. The closer that pole is to the origin (in the continuous time domain), the longerthe rise time. The rise times achieved with N = 1 and N 3 are both longer than thatwith N = 2 and, while the rise time with N = 4 is less than with N 2, the control inputsignal becomes excessively active and subject to ringing. In all cases prediction horizonsless than those in Table 5.4 yield an unstable closed ioop and increasing N2 causes the slowmode to become slightly underdamped, introducing overshoot into the response.Increasing the control increment weighting factor,,from zero has negligible effect onthe response because the closed ioop poles introduced by are generally close to the originin the discrete time domain or they interact with the poles of the fast mode; in either casethe slow mode remains dominant.Figure 5.18 shows superimposed step response simulations for‘\se ranging from 0 to0.4. As.kse is increased the rise time decreases but at the expense of increasing overshoot,which may be undesirable. It is evident from these observations that the static equilibriumbias term in the control law acts as a proportional feedforward term in which the gain isproportional to seThe inherent tendency of the predictive controller to anticipate changes in the setpointlevel is apparent in the step responses shown. With a 20 step prediction horizon andO.35ms sampling interval the controller begins to respond to setpoint changes 7ms beforethey occur. The initial sign of the response to a step change in setpoint is opposite to thatof the step due to the nonminimum phase nature of the system dynamics.Chapter 5. Force Control Algorithm Performance and EvaluationzC)0U00C)Time [s]Figure 5.18: Force control step responses for a single flexible link (with one structuralmode), steel on aluminium contact (keff = 5MN/m) with 0 A3e 0.4.1501816141210864200 .01 .02 .03 .04 .05 .06 .07 .08 .09Chapter 5. Force Control Algorithm Performance and Evaluation 1515.3.5 ke = 62kN/m Contact Stiffness CaseThe sampling rate of nearly 3kHz required to obtain the results shown in section 5.3.4means that implementation of the controller would require very high performance hardwareand actuators. The high sampling rate is required because the effective contact stiffnessof 5MN/m is very much greater than the 7096N/m modal stiffness of the link and thusthe dynamics are dominated by the high frequency mode whose natural frequency is proportional to k!ff as can be seen in Figure 5.15. Inclusion of additional structural modes inthe dynamics and controller models results in a dominant mode with even higher naturalfrequency and thus higher sampling requirements. However, it must be recalled that theHertz contact model used to determine the contact stiffnesses and masses is based strictlyon the local deformations of the surface during quasi-static contact. An, et al. [141 used aparameter estimation algorithm to determine experimentally the effective contact stiffnessof a steel bearing on an aluminium surface and found it to be 61554N/m. Unfortunatelysensor limitations prevented the determination of effective mass and damping coefficientsfrom their experimental data. It can be surmised that, while the local effective contactstiffness of a pair of surfaces may approach that given by the Hertz model, more globaldeformation of the bodies results in much lower actual effective contact stiffnesses. Thisproposition is somewhat supported by the presence of an initial spike in the data collectedby An, et al. (reproduced as Figure 5.19) which suggests that the contact stiffness uponinitial contact is much greater than that which prevails once contact is well established.Controller ModelTo obtain an effective contact stiffness of 61554N/m the values in Table 5.3 were scaledto maintain relative stiffnesses of the steel and aluminium surfaces with the result thatthe steel surface stiffness becomes k3 2.3930 >< 105N/m and that of the aluminium k =Chapter 5. Force Control Algorithm Performance and Evaluation 152IC 800007000060000soooo Final Value: 61554 N/rn4000030000200001000000.0 0.2 0.4 0.6 0.8 1.0Estimated Stiffness [or Aluminum Surface (N/rn)Figure 5.19: Experimental contact stiffness estimate for steel on aluminium. Reproducedfrom An, et al. [14]8.2870 x 104N/m. As can be seen in Figure 5.15, these values imply that, in contrast tothe situation at higher effective contact stiffness, the system characteristics are composed ofcontributions from both modes. The natural frequencies of the open ioop modes are 50.65Hzand 125.0Hz, thus sampling at 384.6Hz (h 2.6ms) satisfies the Nyquist criterion. Theresulting discrete time transfer function model for the controller is:F(q1)— —2.144 + 3.084 (q1 + q2) — 2.144q3—1 (5 46)u(q1) 1— 0.4456q1+ 0.7709q2 —0.456q3+ q_4Step ResponseThe approximate closed ioop poles, calculated from the control law and the linearised systemdynamics, show that for N,. = 1 and N,. = 2 all values for the prediction horizon resultin both of the closed ioop modes being uriderdamped. With a 3 step control horizon theapproximate closed ioop poles show that for certain prediction horizons the closed ioopsystem has one overdamped mode and one underdamped mode, otherwise both modes areunderdamped. With N,. = 4 there is always one overdamped and one underdamped closedloop mode. Table 5.5 lists the approximate closed ioop characteristics of the system forseveral combinations of prediction and control horizon.Chapter 5. Force Control Algorithm Performance and Evaluation 153N2 N Overdamped Mode Underdamped ModeDominant Pole fd [Hz] C5 3 -181.3 192.3 0.2007 3 -99.07 192.3 0.3218 3 -58.56 192.3 0.34210 3 -190.1 192.3 0.4115 4 -183.7 192.3 0.1426 4 -171.4 192.3 0.1667 4 -79.75 192.3 0.2898 4 -44.51 192.3 0.3019 4 -80.90 192.3 0.31210 4 -75.68 192.3 0.314Table 5.5: Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with one structural mode.fd damped natural frequency and C = damping ratio.The dominant pole locations and damping ratios listed in Table 5.5 suggest that theN2 10, N = 3 controller will provide the fastest rise time combined with the least 192.3Hzdisturbance. As Figure 5.20 shows, this is indeed the case, however this performance comesat the expense of the controller adjusting the system well in advance (26ms) of setpointchanges because of the 10 step prediction horizon. This potentially undersirable anticipationeffect is markedly reduced with N2 5 and N, = 4 without much increase in rise time butthe response exhibits slight overshoots due to the low damping of the 192.3Hz mode (seeFigure 5.21). The next best set of controller horizons is N2 = 5, Ne,, = 3 which produces theresponse shown in Figure 5.22. The rise time of this response is slightly greater than that ofthe two previous ones but the 192.3Hz mode is sufficiently well damped to avoid overshootand the setpoint change anticipation time is limited to l3ms. These results emphasise theneed for simulations in the tuning of the controller due to the approximate nature of theclosed ioop characteristics calculated based on linearised models, and to the fact that thosecharacteristics reveal nothing about the relative excitation levels of the closed loop modes.Chapter 5. Force Control Algorithm Performance and Evaluation 15420181614z12C)0U100C)8642Time [s]Figure 5.20: Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (keff = 61.554kN/m) with N2 10, N = 3, = 0,‘se =0 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Chapter 5. Force Control Algorithm Performance and Evaluation 1552220181614z00IL 12C)C01086420 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Time [s]Figure 5.21: Force control step response for a single mode flexible link (with one structuralmode), steel on aluminium contact (keff 61.554kN/m) with N2 5, N = 4, = 0,‘se 0.Chapter 5. Force Control Algorithm Performance and Evaluation20181614z12C.)0UC.)Cu100086421560 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Time [s]Figure 5.22: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (keff = 61.554kN/m) with N2 5, N = 3, \ = 0, ‘\se 0.Chapter 5. Force Control Algorithm Performance and Evaluation 157Table 5.5 indicates a potential difficulty with the 3 step control horizon in that theclosed ioop system does not always have an overdamped mode. This implies a possiblelack of robustness in that the effects of unmodelled dynamics or sensor noise may causethe estimated controller model coefficients to change such that controller horizons whichinitially provided an overdamped closed loop mode would no longer do so. For this reasonthe 4 step control horizon is preferable. Figure 5.23 shows step responses for the controllerwith N2 = 7 and N = 4. The responses are free of overshoot and steady state offset witha rise time of 24ms.It can be seen from Table 5.5 that the closed ioop characteristics do not vary mono-tonically with the controller horizons. This is due to the fact that the horizons are integermultiples 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.24shows the natural frequencies of the linearised equations of motion for the single link systemwith the link structure modelled by the first three mode shapes of a cantilever beam. Atkeff = 61.554kN/m the open ioop natural frequencies are 37.49Hz, 113.5Hz, 231.5Hz and664.9Hz. It is evident from Figure 5.24 that the frequencies of the latter two open loopmodes which arise due to the additional admissible functions are independent of keffUnder these circumstances the results of section 5.2.2 imply that the additional modesmake minimal contribution to the contact force and thus the sampling rate need not beincreased to resolve them. On the other hand, the control input channels energy into all ofthe modes and so they should be included in the controller model. The result of theses twoconclusions is the controller modelF,(q1) —ui(q’)——0.41 — 1.43q’ + 2.56q + 3.08(q + q4) + 2.56q5 — 1.43q6 — 0.41q71 + 0.79q’ + l.50q2 + 0.21q3 + O.64q+ O.21q5+ 1.50q6 + 0.79q + q_8(5.47)Chapter 5. Force Control Algorithm Performance and Evaluation 1582018161412zci,F:64200 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Time [s]Figure 5.23: Force control step response for a single flexible link (with one structural mode),steel on aluminium contact (keff = 61.554kN/m) with N2 = 7, N1, = 4, A, = = 0Chapter 5. Force Control Algorithm Performance and EvaluationI 11111111 I I159I I I I liii I I I I I 1111 I I I I I I 1——I-—I—1—i•k = 61 554 kN/m-icr>‘()Ca,zwU(15I.Dz1081iøiø1111 ø-+--------I iiiiiil I I 1111111 I I iiiliiil I iiiiiil 1 111111iø iø 101 102 iøEffective Contact Stiffness [kN/m]Figure 5.24: Variation of the natural frequencies of a single flexible link (El1 = 574N mwith three structural modes) in contact.Chapter 5. Force Control Algorithm Performance and Evaluation 160Overdamped Mode 192.3Hz Mode Underdamped ModesN2 N Dominant Pole C fd [Hz] C f [Hz] C6 4 -217.5 0.517 105.5 0 192.3 0.3077 4 -157.8 0.584 105.3 0 149.4 0.2018 4 -105.8 0.624 105.3 0 145.0 0.1229 4 -81.56 0.622 105.3 0 151.5 0.14010 4 -53.99 0.638 105.3 0 143.5 0.125Table 5.6: Stabilising controller horizons and closed ioop characteristics for steel on aluminium contact (keff = 61.554kN/m) with a single flexible link with three structural modes.fd = damped natural frequency and C = damping ratio.in which the two high frequency modes are aliased to 153.1Hz and 104.3Hz due to therelatively low sampling rate. The aliasing of these modes presents little difficulty becauseof their minimal excitation.Table 5.6 lists the approximate closed loop characteristics calculated from the linearisedmodel 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, thesmallest magnitude pole of which defines the rise time, and an underdamped mode with anatural frequency of one half the sampling frequency. In addition, the aliased high frequencymodes appear as lightly damped modes in the closed loop. Simulated step responses showthat, as was observed in the case of the one mode link, the fast rise times produced by shortprediction horizons (N2 = 6, 7) are accompanied by overshoot because the underdampedmodes do not die out sufficiently before the response reaches the set point level. Figure 5.25shows the response with N2 8 which is free of overshoot.5.3.6 SummaryIn summary, this section has illustrated the calculation of the parameters of the link, contactand controller models based on the physical dimensions and material properties of particularphysical systems. The link model was based on a 1.Om long, 2.0cm diameter aluminiumChapter 5. Force Control Algorithm Performance and Evaluation 1612228181614z12.100C.)864280 .02 .04 .06 .08 .10 .12 .14 .16 .18 .20 .22 .24 .26 .28 .30 .32Time [SIFigure 5.25: Force control step response for single flexible link (with three structural modes),steel on aluminium contact (keff 61.554kN/m), with N2 8, N = 4, = 0, . = 0.Chapter 5. Force Control Algorithm Performance and Evaluation 162rod with its structural dynamics modelled using the mode shape functions of a cantileverbeam. For the case of relatively soft (kff = 3100N/m) contacting surfaces, with excitationof the contact dynamics due to the contact mass being significant compared to the linkmoment of inertia, simulation results demonstrate stable control with fast rise time andminor overshoot.Contact model parameters calculated from the Hertz contact model (derived in section 2.5), based on a 4.0mm diameter steel bearing as an end effector on the link contactinga 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 consuming due to the mathematically stiff structure of the equations of motion, however a steadystate approximation of the contact dynamics portion of the model was shown to substantially reduce the computation time without significantly affecting the results. Approximateclosed loop characteristics calculated from the controller model and the linearised equationsof motion were successfully used to tune the controller for the case of a one mode flexiblelink. However, control of this system requires 2857hz sampling and the effective contactstiffness is so much greater than the link structural stiffness that, for any reasonable numberof admissible functions used to model the link structure, the dynamics are dominated bythe highest frequency mode and an even higher sampling rate is necessary.Experimental results from An, et al. [14] suggest that the effective contact stiffness of5.0037MN/m given by the Hertz contact model is an extreme upper bound and that a morereasonable value is keff = 61.554kN/m. At this level of effective contact stiffness the systemwas found to be dominated by the two modes with the lowest natural frequencies, as theresults of the linear analysis in section 5.2.2 suggest should be the case. The approximateclosed 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 structuremodelled by a single admissible function.Chapter 5. Force Control Algorithm Performance and Evaluation 163Finally it has been shown that when additional admissible functions are included inthe link structure model the number of modes in the controller model should be increasedbut, due to the domination of the system dynamics by the lower frequency modes, thesampling rate need not be increased. These results establish the value of the plots of thevariation in modal natural frequencies with effective contact stiffness (obtained from linearanalysis) in determining the number of significant modes and required sampling rate forspecific configurations.5.4 Making and Breaking ContactThe circumstances surrounding the making and breaking of contact between the manipulator tip and the environment surface pose a difficult control problem. When contact existsthe contact force provides a measure of the state of the entire manipulator and environmentsystem as is exemplified by the expressions for the contact force components in terms ofthe generalised coordinates that were derived in section 2.5. On the other hand, when themanipulator tip is not in contact with the environment surface the contact force is, by definition, zero and therefore the controlled variable provides no information about the stateof the system. Thus the contact force is a positive indefinite function of the system statewhich exhibits a discontinuity at zero.This lack of information in the absence of contact presents two difficulties for the forcecontrol algorithm. Firstly, the control input levels which the force control algorithm calculates in attempting to make or regain contact will not reflect the position or velocity of themanipulator tip with respect to the surface. In most cases this will result in the tip eventually arriving at the surface with a substantial contact velocity as the controller attemptsto remove the error between the force setpoint level and the zero contact force. Withoutspecial actions the force controller is likely to have difficult controlling the force transientChapter 5. Force Control Algorithm Performance and Evaluation 164which results from a non-zero contact velocity. The second difficulty arises in the operationof the system identification algorithm used in the adaptive controller because the algorithmupdates the model coefficients on the basis of the difference between the actual and predicted output with the tacit assumption that the model and the system being identified arecontinuous through zero.In order to overcome these difficulties it is necessary to control the motion of the manipulator when it is not in contact as well as the forces it exerts on the surface when it is. Thepredictive control algorithm derived in Chapter 3 can be readily applied to the task of controlling the manipulator joint angles simply by providing it with a transfer function modelwhich relates the joint angles to the control inputs. To successfully use the combination offorce and motion controllers to operate through the discontinuity when contact is made orbroken a level of control above the basic predictive control algorithm is introduced. Thishigher level control is referred to as the Contact Control Logic. Like the circumstances itis intended to deal with, it is discontinuous and, to some extent, heuristic in nature. Onthe basis of sensor inputs and data supplied by the operator and the trajectory generatorthe contact control logic generates the setpoint programs for the predictive control algorithm, selects between force and motion control algorithms and chooses whether or not toallow system identification to proceed. The relationship of the contact control logic to themanipulator, 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 broadlyclassified as expected (or intentional) and unexpected. An example of an intentional contactevent is the situation where the operator or task planner accurately knows the location ofthe surface to be contacted. Under these circumstances motion control can be used tomove the manipulator tip to a location adjacent to the surface and to reduce its approachvelocity sufficiently to avoid a violent collision and accompanying high level of transientcontact force. From this state a smooth transition to force control can be made. The roleChapter 5. Force Control Algorithm Performance and Evaluation 165Figure 5.26: Block diagram of the overall control structure.Chapter 5. Force Control Algorithm Performance and Evaluation 166of the contact control logic in this situation is to switch between motion and force controlat 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 toimperfect 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 motionthe contact control logic must take measures to minimise the collision force and bring themanipulator to rest, pending corrective action from the operator or task planner. On theother hand, if the collision is due to inaccurate knowledge of the location of a surface onwhich the manipulator was intended to exert force the role of the contact control logic isto switch to force control mode, minimise the force transient that results from the collisionand reach the desired force level setpoint. This latter case is the focus of the remainderof this section. Two operational strategies for the contact control logic which seek toachieve sustained, controlled contact force control following an unexpected contact eventare proposed and their performance is examined by way of simulations. The analysis, resultsand discussion presented in this section have already been published [27].5.4.1 Stop Motion StrategyThe first, relatively simple, strategy is referred to as the stop motion strategy. The contactcontrol logic is programmed such that the control mode is switched from motion to forcewhen 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 themotion controller resumes operation, rather than continuing the with motion that thatwas being executed prior to contact, the joint angles at which contact was made will bemaintained.Figure 5.27 shows the results of a simulation in which this strategy is used. A singlelink manipulator is used, modelled on a 1Mm long, 2.0cm diameter aluminium rod with theChapter 5. Force Control Algorithm Performance and Evaluation20i:; 150Co 100(b) Contact Force Response167Figure 5.27: Joint angle and contact force responses for a single flexible link colliding with a6200N/m surface. Control mode switching is based on sensed contact force and the motioncontrol setpoint is modified due to the contact event.(a) Joint Angle Response30Time [sJChapter 5. Force Control Algorithm Performance and Evaluation 168structural dynamics represent by the first two modes of the rod considered as a cantileverbeam. The link is initially in motion with constant joint velocity toward a surface witha contact stiffness of 6200N/m. The predictive control algorithm parameters are a delayhorizon (N1) of 1 time step, a prediction horizon (N2) of 40 time steps, a control horizon(Na) of 12 time steps and a control increment weighting factor () of 1.0. The controllertime step is 4.Oms. The motion controller is adaptive but the model coefficients for the forcecontroller are held constant. A non-adaptive (i.e. fixed model coefficients) force controllerhas been used in order to focus attention of the issues surrounding the transition frommotion to force control. The problems that arise when adaptive force control is used duringthis transition period are discussed below.The simulated joint angle and contact force responses (solid lines) and correspondingsetpoints (dashed lines) are shown in Figures 5.27a and 5.27b, respectively. The motioncontroller shows good tracking of the joint angle ramp during the initial part of the maneuver. The desired angular velocity of the joint is 0.5rad/s which gives a nominal velocity atthe link tip of O.5m/s (neglecting the vibrational motion of the flexible link). After 0.257sthe link tip contacts the environment surface with a velocity of 0.536m/s. At this pointthe control mode is switched from motion to force due to the sensing of a non-zero contactforce and the force controller attempts to bring the system to rest at the specified contactforce setpoint of 8.ON. Upon switching control modes the motion control setpoint for futureuse is modified to hold the joint angle stationary at 7.56°, the joint angle at which contactwas detected. Due to the non-zero velocity of the link tip at the instant of contact thecontact force rises rapidly to approximately 16N and, before the force controller can haveany significant effect on the system the link rebounds from the surface and motion controlis resumed. This cycle is repeated twice more with force control being established on thethird contact.Chapter 5. Force Control Algorithm Performance and Evaluation 169After sustained contact has been achieved an adaptive force controller (i.e. one for whichthe 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) isrequired 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 withperiods of zero force cause the system identification algorithm to produce wildly inaccurateestimates of the model coefficients. Using these coefficients in the control law can result in aloss of control. A solution to this problem is to delay the start of force model identificationuntil sustained contact is assured. A delay of 25 time steps is adequate for the case shownin Figure 5.27. However, this approach requires that some means be devised to decidethat sustained contact has been achieved; a difficult proposition considering the number offactors 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 transition period, the strategy of bouncing on the surface until force control can be achieved is, formost applications, not acceptable. Furthermore, it has been observed in other simulationruns 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, thereis, apparently, no guarantee that stable force control will in fact be achieved at all usingthis strategy. While various refinements of the motion control strategy during the bouncesmight produce better results, observation of the contact force transient which occurs duringthe first bounce suggests another course of action.5.4.2 Force Setpoint Modification StrategyComparison of the contact force transient during the first period of contact in Figure 5.27bwith the corresponding transient for a collision between the link and the environment at thesame velocity during which no attempt is made to control the contact force shows very clearChapter 5. Force Control Algorithm Performance and Evaluation 170Figure 5.28: Simple 1 degree of freedom model of a single link in contact with an environment.similarity between the transients. In both cases the transient closely resembles the positiveportion of a sinusoid, the period of which correlates closely with the natural frequency ofthe first mode of the open loop system. Based on this observation it is apparent that theforce controller has almost no effect on the contact force level immediately following contactand 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 freedommodel of the link and contact dynamics shown in Figure 5.28. The equation of motion ofthis system without forcing isI8 + kL28 = 0 (5.48)Given initial conditions at t = 0 of 8 = 0 and & =&, the particular solution of equation (5.48)iskL28(t) = —sinwt, wherew2 =10k/Li.)(5.49)Chapter 5. Force Control Algorithm Performance and Evaluation 171In this model the contact force is the force generated by the compression of the spring, thatisF(t) = kLO(t)kL8sinwtw= 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 linkabout the joint axis, equation (5.50) gives a very good prediction of the initial contact forcetransient shown in Figure 5.27b.Note that the contact force transient described by equation (5.50) is entirely a result ofthe initial condition velocity, which correlates with to the velocity with which the link tipcontacts the surface. The classical form of transfer function model on which the predictiveforce control algorithm is based, however, assumes quiescent initial conditions (that is, thesystem is stationary at the instant of contact). Thus, a fundamental incompatibility existsbetween the system for which control is desired and the model on which the controller isbased.Viewing the situation in terms of energy, the force controller expects the system to havezero energy when it begins operation. The system however has non-zero energy, in theamount of the kinetic energy of the link at the instant of contact. If stable force control isto be achieved without letting the link bounce off the surface the controller must overcomethe initial energy of the system. This is confirmed by simulations similar to those shownin Figure 5.27, in which higher contact force setpoints are specified. In those simulationsthe initial portion of the contact force transient is similar to the corresponding uncontrolledcollision transient, however the force controller (which is more energetic by virtue of theChapter 5. Force Control Algorithm Performance and Evaluationz0C.)0I]4.-C-)(U4.-0C-)172.0 .1 .2 .32520151050Time [s]Figure 5.29: Contact force response for a single flexible link making contact with a 6200N/msurface at 0.536m/s.higher setpoint it is seeking to achieve) overcomes the initial energy of the system and bringsthe system to rest at the desired contact force level. Figure 5.29 shows the contact forceresponse with a setpoint of 20N. As a consequence of these observations a force setpointmodification strategy is proposed for the contact control logic.Differentiating equation (5.50) with respect to time and solving for k at a time immediately after contact when t is small gives1(t)k —v—-. (5.51)L6This result, combined with Fk = /J6 and w2= from equations (5.50) and (5.49),make it possible to use the joint encoder data just prior to the instant of contact and theforce sensor data immediately following contact to estimate the peak force and the durationChapter 5. Force Control Algorithm Performance and Evaluation 1732015zci)C)10C0C)50Time [sjFigure 5.30: Contact force response of a single flexible link colliding with a 6200N/m surfacewith force setpoint modification.() of the contact that will occur if the force controller does not act. The peak force iseffectively 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 thesetpoint is found to be less than the estimated peak the former is modified to the estimatedpeak force level for a period of time equal to predicted duration of the uncontrolled collisioncontact transient. This enables the controller to exert the energy necessary to overcome theinitial kinetic energy of the link.Figure 5.30 shows the contact force response for the same conditions as that shown inFigure 5.27 but with contact transient prediction and contact force setpoint modificationincluded in the contact control logic. Contact occurs at 0.257s with a link tip velocity of.0 .1 .2 .3Chapter 5. Force Control Algorithm Performance and Evaluation 1740.536m/s. At 0.260s the force controller estimates the effective stiffness of the contactingsurfaces to be 3041N/m (compared to the actual value of 3100N/m) and predicts a peakforce of 15.69N and an uncontrolled contact duration of 30.2ms. Based on these predictionsthe contact force setpoint is raised to 15.69N for 30.2ms and a stable 8.ON contact forcelevel is achieved without the link leaving the surface as happened in Figure 5.27b. The initialovershoot in Figure 5.30 is largely due to the initial contact velocity of the link and cannotbe removed by a controller which has no provision for non-zero initial conditions. In anycase, the overshoot is significantly less that that which eventually occurs in Figure 5.27b.The model, based on equation (5.50), used to predict the peak contact force and corresponding uncontrolled contact duration is a very simple one. The necessary calculationsrequire a total of 2 additions, 5 multiplications and 2 square root operations. (Two additions and one multiplication can be eliminated from this total if the force rate can beobtained from the force sensor.) As such, the setpoint modification can easily be put intoeffect within one or two time steps after contact is sensed. Furthermore, it has been notedin simulation results that a lag of one or two sampling intervals between the instant ofcontact and the completion of the prediction calculation (and application of the setpointmodification) does not adversely effect the control.5.5 Force Control for Two Link ManipulatorsConsideration of a manipulator with two or more links greatly increases the complexity ofthe analysis and control problems. In this section force control for planar manipulators withtwo links is addressed. Both rigid link and flexible link manipulators are considered. Thetwo joint torques available as control inputs for a two link manipulator permit control oftwo world frame components of contact force.Chapter 5. Force Control Algorithm Performance and Evaluation 175In this what follows, similar analytical techniques to those used in section 5.2 are employed to gain insight into the modal structure of the system and the results are appliedto the selection of appropriate controller parameters. Because of the strongly coupled nature of the system the multivariable control algorithm is necessary. The derivation of anappropriate multivariable controller model from the linearised equations of motion is presented. Simulated step responses are used to evaluate the performance of the closed loopforce control for both rigid link and flexible link manipulators.5.5.1 Linear AnalysisIn this section a linear analysis of a two link manipulator in contact with a deformable environment is presented. The analysis follows the same general approach that was employedfor a single link system in section 5.2.Rigid LinksWe begin by considering the case of rigid links which is the limiting case of infinite linkstiffness. As well, the characteristics of rigid link manipulators are of practical interest intheir own right since they typify the vast majority of existing industrial manipulators. Figure 5.31 shows the configuration and important parameters for a manipulator with two rigidlinks exerting forces on the environment in the r and y world frame coordinate directions.The equations of motion for this system are given in section A.2. Neglecting damping,gravitational effects and inertia coupling between the links and the contact region mass,replacing the joint torques {Tj(t),r2(t)} with the general control inputs {ui(t),u2(t)} andlinearising about the nominal state{ B2 Ex Lii x y ] = [ 0 ... 0 jChapter 5. Force Control Algorithm Performance and Evaluation 176keyk m5+ mew2(x,t)102 L2 M2y w1(x,t)U2ILM01 1xIFigure 5.31: Two rigid link manipulator in contact with a deformable environment.Chapter 5. Force Control Algorithm Performance and Evaluation 177gives[101 + 102 + M2 (L +L12C) + m (L + L + 2L1Lc)] 1+ [102 + M2L1c + m (L +L12c)]—m [L1.s + L2sj + m [Lic + L2c]= u1(t)— k5 (List + L2s)[(Ljs + L2s)Oi + L2s0+ j—k (Lic + L2c)[(Lic + L2c)0 + L2c0—(5.52)[I2 + M2L1c+ m (L +L12c)] i + [102 + mL2] 62—mL2së+ mL2cë= u2(t)—k3L2s[(List + L2s)0 + L2s0+ e]—k8L2c[(Lic + L2c)8 + L2c0— (5.53)m(Lis + L2s)8i + mL2s6— më = —k5 [(List + L2s)0 + L2s8+ e] — kexEx(5.54)m (Lic + L2c)01 + mL2c6+ mi = —k8 [(Lic + L2c)0 + L2c0——(5.55)where the generalised coordinates {0, 02, e, e} are now perturbations about the nominalstate.Figure 5.32 shows the numerically calculated eigenvalues of the system with the followingparameters: 0 = O’, = 90°, 101 = 102 = 0.2817kg m2, M2 = 0.8451kg, L1 = L2 = 1.Om,and = kex = k8 key. The eigenvalues are shown for the range of effective contactstiffness, keff, from 50N/m to 5MN/m. As was seen in Figure 5.3, the upper frequencyline in Figure 5.32 is well separated from the other frequencies. That line is actually twosuperimposed lines representing the natural frequencies of the contact mass vibration modesChapter 5. Force Control Algorithm Performance and Evaluation1 4‘Ic’>‘C)Ca)a)U-- 1z10’101—1Effective Contact Stiffness [kN/m]Figure 5.32: Variation of the natural frequencies with effective contact stiffness for a twolink manipulator with identical, rigid links in contact with an isotropic (kxeff = kyeff) environment.1 6 I 11111111 I I 1111111 I IIIIII I IIIIIIj I I1131/2 1/2_L rksx÷kexl L ksy+key3 2t Lm+ mej 4 2r [m÷ me1781 41 ø111.1101 1 3Chapter 5. Force Control Algorithm Performance and Evaluation 179( and 7)4) in the x and y coordinate directions. These lines are closely approximated bylk+k 2 1 k+kf = — sX ex and f = — sj ey (5.56)27r m5 + me 2K m3 + meThey are coincident because k = kex k811 = key for the case shown.For most cases of practical interest the contact mass is small compared to the otherinertias in the system with the result that the high frequency modes associated with itslocal 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 theanalysis on the remaining modes. Neglecting the contact region mass, m, and replacingthe directional contact stiffnesses, k8 and kex, k8 and key, with the corresponding effectivecontact stiffnesses, kff and kyeff, allows the linearised equations of motion to be reducedto:[101 + 102 + M2 (L + LiL2c)] ë1 + [i + M2LiLc]2= ui(t)— kxeff (List + L2s)[(List + L2s) +L2se]kyeff (Lic + L2c)[(Lic + L2c)8 +L2c8] (5.57)[102 + M2LiLcj += u2(t)— kffL2s[(List + L2s)6i +L2s8jkyeffL2C[(Lic + L2c)&i +L2c8]. (5.58)After a great deal of algebraic manipulation the characteristic equation for this systemcan be written as+ M2L) 102 — MLLc2] (jw)4+ [[I +102 + M2 (L + LiL2c)j (k11ff + zks2) L+102 [kyeff (L + L + 2L1Lc4) + Ak (List +L2s)2]Chapter 5. Force Control Algorithm Performance and Evaluation 180— (2102L+M2LJ) [kyeff (Lic + L2) + txk (List + L2s)S2]} (jw)2+kxeffkyeffLL.3 = 0, (559)where Lik = kxeff — k1jeff. The structure of equation (5.59) provides important insights intothe 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 fromthose due to a difference in the contact stiffnesses in the x and y directions. In particular, thissubstitution reveals that the nominal joint angle of link 1, 8, affects the natural frequenciesoniy when kff kyeff. Secondly, in the case of exact alignment of the links (8 00 or180°), the (jw)° term disappears. This circumstance results in one of the natural frequenciesof the system being zero which implies that the two components of contact force cannotbe simultaneously controlled because the manipulator lacks sufficient degrees of freedom.This is an analogous situation to the familiar degenerate condition which arises in motioncontrol when planar links are exactly aligned.Equation (5.59) can be solved to yield an expression which closely approximates thenatural frequencies of the and 2 modes. For the cases typified by Figure 5.32 (identicallinks and keff kyeff keff) we have:(,w)2= 2 (z + M2L)— MLc2 [_ (i + M2L2s2) + (i + ML4s2) 2]. (5.60)While this expression is somewhat more complex than those obtained in the case of a singlelink (equation (5.11)), it displays the came general structure. The natural frequenciesare directly proportional to kff and L and inversely proportional to the square root of acombination of inertias. This expression also reveals that the frequencies of the and72 modes vary between bounds as & varies modulo 90°. At 0 = 00 the manipulatorconfiguration 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 ratioChapter 5. Force Control Algorithm Performance and Evaluation 181of 1 to 2 and the frequency of is at its maximum and that of i2 is at its minimum. Thevariation if the frequency of i is much larger than that of 72.Returning our attention to the model which includes the full contact dynamics, we nowconsider the eigenvectors of the system and, in particular, the information they provideabout the contributions of the natural modes to the contact force components and the excitation of those natural modes by the control inputs. As in the case of a single rigid link, theeigenvectors, normalised to unit magnitude, are found to be independent of effective contactstiffness for the case where kxeff = kyeff. With the parameter values used in Figure 5.32 theeigenvectors areE [ei e2 e3 e4]—0.667 —1.286 x 1O_12 —2.962 x iO’ —2.662 x 1O_60.667 0.894 1.065 x i0 2.662 x 106(5.61)8.087 x 10_13 —0.147 1 4.311 x 10u10.333 —6.433 x iO’ 1.070 x lO 1With these numerical values the transformation between the generalised coordinates andthe natural modes of the system,‘1)1=ET , (5.62)174clearly shows the isolation of the and modes (high frequency vibration of the contactregion mass) from the and 772 modes. The energy of the 772 mode is concentrated inthe oscillation of link 2 about its joint. The h mode includes oscillations of both linksabout their joints in equal proportions of opposite sign. This particular arrangement of thecontributions of the 8 and 62 generalised coordinates to the 77i and 772 natural coordinatesChapter 5. Force Control Algorithm Performance and Evaluation 182is due to the nominal joint angles, t = 00 and t9 = 90°. The effect on the 7h and 12eigenvectors of changing the nominal joint angle of the second link is shown in Figure 5.33.The relatively small contribution of the 8 generalised coordinate to the 2 mode decreasesas increases, as shown in Figure 5.33b. This trend indicates that the majority of theenergy associated with the‘]2 mode is in the form of oscillations of link 2 about its joint.Figure 5.33a shows that the i mode combines contributions from 6 and 2 with oppositesigns, reflecting the coupling between the links. The ratio of the components is determinedby the lengths and inertias of the links. In the case shown the links are identical and theratio 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 c2rhereEsEu= [ - (List + L2s) -L2s (±: - i) o] (5.63)andF , where= k5- [ (Lic + L2c) L2c 0 (:: - i) j. (5.64)\\ \\\\\I\)r.3r..)\‘‘IIIiiiiiII\\\\\c•-jo—\\\\CCCCC.1•II•II•II•ccGeneralisedCoordinateComponentsofNaturalMode&GeneralisedCoordinateComponentsofNaturalModeIIII—0; CD cz C CD CDC C0 CI I1 I IChapter 5. Force Control Algorithm Performance and Evaluation 184Transforming these expression into natural coordinate space we find that, with the eigenvectors given in equation (5.61),0 7iF = —0.4472 772 (5.65)0 773—5,3248 x 10—6 77andT0.3333 77iF —6.4319 x 1013 772= (5.66)—1.3312 x 10-6 77—1.4810 x 10_17 77These results show that the contact forces, F and F, are composed almost entirely of the 772and 771 modes, respectively, that is, the high frequency vibrations of the contact region masscontribute minimally to the contact forces. As would be expected from the above discussionof the natural frequencies, the contact force components in natural coordinate space areindependent of & when Lk = 0. The effect of varying O in the range from 00 to 90° isshown in Figure 5.34 where the values of the 7i and 712 components of F and F are plottedfor 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 thata motion about joint 2 results primarily in a force in the y direction. For intermediate valuesof i9 the motions of both links contribute to F and it is therefore composed of componentsof 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 stateof the manipulator which occurs when the links are aligned is reflected by the zero valuesof both the ‘i1 and 72 components of F shown in Figure 5.34b. As éL increases the 772component increases in magnitude to a maximum at 0 = 90°, in which configuration onlyChapter 5. Force Control Algorithm Performance and Evaluation 1850- .05-.10- 0CLI..-.15- 25= a) j2- .300EZ 8 - .35- .4045I I I •0 10 20 30 40 50 60 70 80 90o [degrees](a) i and 12 components of F.35.30.25.200u .15 /.10.050a) .-- 05- 10-0.0 E -.15Zo0- .20- .25-. 30—.35 .• I • • I •0 10 20 30 40 50 60 70 80 90e [degrees](b) and ]2 components of FFigure 5.34: Variation of the components of F and F in natural coordinate space with 6for identical rigid links with equal directional contact stiffnesses, keff = 61.554kN/m.Chapter 5. Force Control Algorithm Performance and Evaluation 186motion about joint 2 can produce a force in the x direction. The i1 component of F is dueto the coupling between the links.Finally, we consider the relative excitation of the natural modes by the control inputs,u1 and u2, by using ET to transform the input vectors, [1 0 0 01T and [0 1 0 01Tfrom generalised to natural coordinate space. As in the single link case, the excitation ofthe modes corresponding to the local vibrations of the contact region mass are found to benegligible when that mass is small compared to the link inertias. The excitations of theother ( and 1)2) modes are shown in Figure 5.35 for values of éL between 00 and 90°. Thegeometric relationships which were discussed above in the context of the contributions ofthe 8i and 82 generalised coordinates to the natural modes are reflected in the data shownin Figure 5.35. The u1 input primarily excites the mode, particularly as the links becomeperpendicular. On the other hand, u2, excited both the and the 1)2 modes when thelinks are nearly aligned, with the excitation of 1h decreasing somewhat as the links becomeperpendicular. The u2 input excites both modes, regardless of the configuration of themanipulator, because an input at joint 2 not only drives link 2 but also produces a reactionforce on the tip of link 1.In summary, linear analysis of the contact case for a two link manipulator with infinitelystiff links under the assumption that the contact region mass is small compared to the linkinertias reveals that:1. the natural frequencies are proportional to k for a given set of link lengths andinertias;2. the natural frequencies of the contact region mass vibration modes (i and ij) arewidely separated from those due to the links oscillating about their joints (‘ii and 1)2);3. the natural frequencies of the i and 1)2 modes are independent of 6 when kseff = kyeffand, for a given keff, are bounded above and below as 8 varies modulo 90°;Chapter 5. Force Control Algorithm Performance and Evaluation0- .05-.10-.15.94.92.90.88.86.84.82. xlii.80LQ).78.76.747270.68.650Figure 5.35: Relative excitation of thefor a two link rigid manipulator in contact.187Caa) .;o0. xLu4-.U)11- .20- .25- .30- .3510 20 30 40 50o [degrees](a) Joint 1 input, u160 70 80 9010 20 30 40 50 60 70 80o [degrees](b) Joint 2 input, u2907li and 72 modes by the control inputs, u1 and u2,Chapter 5. Force Control Algorithm Performance and Evaluation 1884. the contact forces, F and F, are composed primarily of the and 2 natural modesin proportions which depend on the nominal manipulator geometry; and5. the control inputs excite only the and 2 modes.For the purposes of contact force control using a discrete time algorithm these results implythat the high frequency modes can be ignored and the sampling interval selected to satisfythe Nyquist criterion for the 2 mode frequency when 8 = 00. Employing the multivariableform of the predictive control algorithm developed in Chapter 3 with a controller model thatproperly includes the effects of coupling (as discussed in section 5.5.2) allows the system tobe easily controlled. The presence of two undamped open loop modes means that a controlhorizon of N = 2 is needed. The performance of the force control algorithm for this caseis demonstrated in section 5.5.3.Flexible LinksThe equations of motion for a two flexible link manipulator making contact with a deform-able environment were derived in Chapter 2 (equations (2.47) through (2.52)). Applyingthe steady state approximation for the contact dynamics, linearising about the nominalmanipulator 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 foreach link, the equations of motion are simplified to:[r + 102 + M2 (L +L12c)] i+ + M2 (L1 + L2c) &lo + (102 +M2LiLc)o] ii+ (102 + M2LiLc]2 + [1121 +L1cI421]21— [kff (List +L2s)+ kyeff (Lic H- L2c)2] 81— [kxeff (s4ii0 +L2s410)(L1s+ L2s)+ k,eff (cq.ii0 +L2cq10)(Lic + L2c)]‘bChapter 5. Force Control Algorithm Performance and Evaluation 189— [keffL2S(List + L2s)+ kyeffL2C(Lic + L2c)]82— [kxeffs2q2lo (List + L2s)+ kyeffc2qSlo(Lic + L2c)] ‘j&21(5.67)[im + M2 (L1 + L2c) 11o + (102 + M2L1C)11o] i*1”+ [I21 + M2 (11O +L2c&10) + +M2Lc&io) 11+ [Io21o +M2Lc;iio] 2 + [I12j10+cI42iii0j21=— {kxeff (sii0 +L2sq10)(List + L2s)+ kff (cq110 +L2cb10)(Lic + L2c)j 01— {kxeff (Sii +L2s10)+ kyeff (c110 +L2c10)+ 1311] ii— [kxeff (q11o +L2sq10)L2s+ kyeff (cq5iio +L2c410)L2c]02— [kseff (sqiio +L2s410)S127210 + Icyeff (cqii0 +L2c410)c2qS10]b21(5.68)[102 + M2LiLc;] i + [1oio +M2Lciio] ii + I& += u2(t)— [kxeffL2S(List + L2s)+ kyeffL2C(Lic + L2c)jOi[keffL2S(sqSii0 +L2sq10)+k1ieffL2C2(c41i0+L2c4410)jiI)ii— ceff128l + kyeffLC] 02 — [kxeffLsq!lo+ kyeffLClo]l/’21 (5.69)[k 2*2[1121 + LicI42i1i + [I1211 +cI421qS110]bi + 112102 + I22121— [kxeffS24)21o (List + L2s)+ k,effC22lo(Lic + L2c)j 81—[keffs2qSlo +L2sq410)+ kyeac2qio(cq110 +L2cqS10)1b11[kxeffsq21oL2 + kyeffCl4)0L21]02*2 i2*2 i2— [kxeffsi2cio+ kyeffc1q210 + 1321j I’21. (5.70)Chapter 5. Force Control Algorithm Performance and Evaluation 190Even in this greatly simplified form an algebraic analysis of the system is difficult to accomplish as well as to gain insight from. However, the insights obtained through the singlelink analysis Section 5.2 and the two rigid link analysis above provide guidance as to theexpected characteristics of the system. These characteristics will be explored numerically.Figure 5.36 shows the variation in natural frequencies with effective contact stiffness fora manipulator with identical links, each with a single structural mode, in contact with asurface having identical contact stiffnesses in the x and y directions. The nominal manipulator joint angles are = 00 and 6 = 900. The characteristic pattern of the frequencylines for the four modes ( through i) with the lowest natural frequencies is recognizablyanalogous to the single flexible link case. The pattern implies the same type of transitionseen in the single flexible case, namely domination of the system dynamics at low contactstiffness by low frequency, rigid link modes (iii and 2) giving way to domination by thehigher frequency, structural modes (m and i) at high stiffnesses. The lines for the 1i and772 mode natural frequencies are asymptotic to the frequency lines for the corresponding tworigid link case when the contact stiffness is small. As the effective contact stiffness increasesthese frequencies both become independent of keff. The link structural modes each introduce a mode (773 and i) whose natural frequency at low contact stiffnesses is independentof keff while at high stiffnesses the frequency is proportional to kff. The lines for the highestfrequency modes ( and 776) are superimposed in Figure 5.36 because the contact surfacesare isotropic (keff = kyeff). These modes are the local vibrations of the contact region massin the x and y directions.Other characteristics of the two flexible link system that follow the trends observed inthe earlier linear analyses are:1. the natural frequencies of the four low frequencies modes are independent of O whenkxeff = kyeff and, for a given keff, they are bounded above and below as O variesChapter 5. Force Control Algorithm Performance and Evaluation 1911 6N>C.)ci)DI1 ø1ø_2 iøEffective Contact Stiffness [kN/m]Figure 5.36: Variation of the natural frequencies with effective contact stiffness of a twolink manipulator with identical, flexible (El = 574N m2, one structural mode each) linksin contact with an isotropic (kzeff = kyeff) environment.1 g_1 1 1 gi 1 1Chapter 5. Force Control Algorithm Performance and Evaluation 192modulo 900; furthermore, the frequency of the‘h mode varies over a much largerrange and in opposition to the frequencies of the other modes;2. additional admissible functions in the link models introduce additional intermediatefrequency modes, each of which dominates the dynamics over a specific range of keffin 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 naturalfrequencies of all but the two highest frequency modes are proportional to the flexuralrigidities of the links;4. the centres of the ranges of effective contact stiffness values in which the transitionsamong dominant modes occur are also proportional to the flexural rigidities of thelinks in the same fashion as illustrated in Figure 5.9.There are two features of particular interest in Figure 5.36 which are not explainedby the linear analyses of the simpler configurations. Firstly, the fact that the and 772frequency 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 keffare somewhat sharper than the curvatures of the 772 and mode lines. This characteristic isa result of there being, in reality, two overlapping transition ranges. The overlap is also dueto the links being identical. Figure 5.37 shows the situation when link 2 is twice as long andtwice as massive as link 1. The pattern of transfer of dominance from one mode to anotheris greatly complicated by the different link lengths but the established characteristics arestill recognisable.The variation with keff of the contributions of the natural modes to the contact forcesillustrates the affect on control of the transfer of dominance among the various modes. TheNz>C)Ca)a)U(L..zI I IIIITT I I 1111111 I I 1111111 I I 1111111 ,1TiTiChapter 5. Force Control Algorithm Performance and Evaluation 193i051 4iø1101101Effective Contact Stiffness [kN/m]Figure 5.37: Variation of the natural frequencies with effective contact stiffness for a twoflexible link manipulator (El = 574N m, one structural mode each) with L1 = 1.Om and= 2.Om.Ti211110_i ig111i1111 I i i101 1Chapter 5. Force Control Algorithm Performance and Evaluation 194linearised contact force model equations for the case of two flexible links, each with a singlestructural mode are‘I’ii02F = c , where21Eyme= k8— [ — (List + L2s) — (sii +L2s10) —L2s (‘ — i) o](5.71)and0102whereI’21ExE!I= k3y [ (Lic + L2c) +L2c10) L2c Ci21io o (rn — i) ].(5.72)Figure 5.38 shows the results of the transformation of equations (5.71) and (5.72) fromgeneralised to natural coordinate space with the same parameter values as were used inFigure 5.36. The transfer of dominance from the 2 to the 7)4 mode in F and from the ito the 773 mode in F is shown clearly in Figure 5.36. The i and 7)6 modes make minimalcontributions 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 inputsChapter 5. Force Control Algorithm Performance and Evaluation 195.20.15.10.050- .05-.10-.15- .20- .25- .30- .35- .40- .45101 102Effective Contact Stiffness [kN/m](a) F components1 4111a).z 0o LLDc:4.. 0a)COZoC-)• 35• 30a)D 0Ow.20.15c’oZ_ .100a) +-Oc= a)c’cco 0L.z - .05()-.10-.1510_2 102 i04Effective Contact Stiffness [kN/m](b) F componentsFigure 5.38: Variation of the natural mode components of the contact forces with effectivecontact stiffness for a two flexible link manipulator. F components are normalised by k3and F components by k3.2 4131O 101Chapter 5. Force Control Algorithm Performance and Evaluation 1961.:..:.*..... .**L U) .44-.U)Q .2.1 1121411516010_i i00 101 102 i03 i04Effective Contact Stiffness [kN/m](a) Joint 1 input, u11.0 I I I.812 14iø_2 10_i iø 101 102 iøEffective Contact Stiffness [kN/m](b) Joint 2 input, u2Figure 5.39: Variation of the relative excitation of the natural modes with effective contactstiffness for a two flexible link manipulator.Chapter 5. Force Control Algorithm Performance and Evaluation 197excite all but the high frequency modes across the entire range of keff. For the particularconfiguration 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, 8and ‘çb. Due to the coupling of the links the u2 input excites all of the first four modes butthe 772 and 774 modes are excited to a greater degree than the i and 173 modes.5.5.2 Controller ModelIn order to control the contact forces exerted in the x and y directions by the tip of atwo link manipulator a multivariable control algorithm must be used. The model on whichsuch a control algorithm is based accounts for the effects of both of the inputs, u1 and u2,on each of the outputs, F, and F. Attempts to control the system using a pair of singleinput, single output loops generally fail because of the highly coupled nature of the systemdynamics.The multivariable predictive control algorithm derived in Chapter 3 requires a linearmodel of the system in the form of discrete time transfer functions relating both of theinputs to each of the outputs. With the equations of motion linearised about a nominalstate, transformation using the Laplace operator allows the system to be described by aChapter 5. Force Control Algorithm Performance and Evaluation 198matrix equation of the form:8(s) 1 00 00 002(5) 0 1 ui(S)J(s). (5.73)b2i(s) 0 0 ui(s)i1’2n2(8) 0 0er(s) 0 00 0The linearised forms of the contact force expressions are:b11(s)I1ni (s)Fr(s) c 0(s)Fr(s) c b2i(s)i/)2n2(s)e( s)with c and as defined in equations (5.71) and (5.72). Combining these expressionsgives the following four transfer function relationships among the contact forces and controlChapter 5. Force Control Algorithm Performance and Evaluation 199inputs:rTf \ TrS) CJ1 rS1— CJ(n1+2) 575ui(s)— J(s)l’ U2(S)—and,,j \ T i-i, ‘ Tr,Is) — C ii rs) — Cy J(ni+2) 5 76ui(s) J(3)I’ U2(S) —whereT / i\(ni+3) 1‘11 i1)—Ii2I . (_i)’) J(+2)ii = and J(n+2) =(—i)(’+”) J1I (_i)(fh+71) J(12and J, is the ij minor of the n x n matrix J(s). Note that the transfer functions share acommon denominator, IJ(s). For the case of rigid links the b generalised coordinates areneglected and the number of admissible functions used to model structural dynamics of thelinks, ri1 and n2, are, of course, set to zero.5.5.3 Force Control for Two Rigid LinksWe begin our examination of the performance of the multivariable predictive force controlalgorithm with the case of a planar two link manipulator with rigid links. As noted insection 5.5.1, this case is of theoretical interest in the context of this thesis because itrepresents the limiting situation of infinitely link stiffnesses. It is also of practical interestbecause most existing industrial manipulators have rigid links.For this test case the link models are based on the same physical model of a 2.0cmdiameter, 1.Om long aluminium rod that was used throughout section 5.3. The nominaljoint angles are t9 00 and 6 = 300 and the tip of the manipulator is in contact with anisotropic environment having an effective contact stiffness of keff lOkN/m. The motionof the manipulator is in the plane perpendicular to the gravitational field.Chapter 5. Force Control Algorithm Performance and Evaluation 200Link DynamicsLink (k) 1 26(°) 0 30Lk(m) 1.0 1.0Mk(kg) 0.8451 0.845110k(kg m2) 0.2817 0.2817Contact DynamicsDirection x yk3(kN/m) 20 20ke(kN/m) 20 20keff(kN/m) 10 10Table 5.7: Parameter values for two rigid link manipulator controller model.Controller ModelUsing the values listed in Table 5.7 for the parameters of the link and contact dynamicsmodels the continuous time transfer functions given by equations (5.75) and (5.76) evaluateto:F’(s) — 1830s2 + 0.4330 x 108ui(s) — 0.l835s + 77472 + 0.2500 x 108’Fr(s) — _7464s2— 0.9330 x 1085 78u2(s) — 0.1835s + 77472 + 0.2500 x 108’F(s) — _352.1s2 + 0.2500 x 108ui(s) — 0.1835s + 7747s2 + 0.2500 x 10’andF(s) — 645152— 0.2500 x 1085 80u2(s) — 0.1835s + 7747s2 + 0.2500 x 108Recalling the results of the linear analysis, for a small contact region mass the dynamicsof the system are dominated by the first two natural modes (see Figure 5.32). For an effectivecontact stiffness of keff = 10k N/rn the natural frequencies of those modes are 9.4Hz and31.3Hz. A sampling frequency of 100Hz (h = lOms) satisfies the Nyquist criterion for thosemodes. Assuming zero order hold sampling, the continuous time domain transfer functionsChapter 5. Force Control Algorithm Performance and Evaluation 201can be transformed to the following discrete time controller model:F(q1) — 0.4298 + 0.3909(q1 + q2) + 0.4298q3 1 5 81ui(q1) — 1— 0.8861q’ + 0.7200q — 0.8861q3+ q_4 ‘F(q1)— —1.589 — 0.1793(q + q2) — 1.589q3 1 5 82u2(q1) — 1— O.861q’ + 0.7200q2— 0.8861q3+ q_4 ‘_______— —1.698 x 10—2 + 0.4908(q’ + q2) — 1.698 x 102q3 5 83ui(q’) — 1 — 0.8861q’ + 0.720q — O.886lq3+ q4 qandF(q1) — 1.165 — 1.639(q + q2) + 1.165q3—‘ 5 84u2(q’) — 1 — 0.886lq’ + O.7200q— 0.886lq3+ q_4These polynomials provide the initial model coefficient values for the multivariable adaptivepredictive force control algorithm. They are updated at each time step by a multivariableform of the EFRA system identification algorithm. The coefficients are also used in conjunction with the closed loop characteristic polynomial formulation derived in section 3.3.1to tune the control algorithm and analyse its performance by calculating the approximateclosed loop characteristics.Step ResponseThe linear analysis in section 5.5.1 suggests that force control for a structurally rigid twolink manipulator using the multivariable predictive control algorithm should be easily accomplished using a sampling interval sufficient to resolve the 72 mode and a control horizonof N = 2 time steps. The results presented in this section confirm this prediction.The approximate closed loop characteristics calculated from the closed loop characteristic polynomial formulation derived in section 3.3.1 show that virtually any combinationof prediction and control horizon values yield stable closed loop performance. However, asthe data in Table 5.8 indicate, a control horizon of N = 2 time steps provides the bestcombination of fast rise time and minimal overshoot because the closed loop response isChapter 5. Force Control Algorithm Performance and Evaluation 202Overdamped Mode Underdamped ModesN2 N Dominant Pole fd [Hz] c fd [Hz]3 1 — 7.40 0.678 33.29 0.0885 1— 7.20 0.389 32.37 0.0428 1— 7.23 0.090 32.04 0.04110 1— 7.35 0.069 32.00 0.035T 2 -134.7 35.14 0.095 — —5 2 -88.30 35.35 0.077 — —6 2 -60.37 35.06 0.075 — —8 2 -26.93 34.32 0.077 — —10 2 - 9.43 33.91 0.077 — —5 3 — 40.68 0.032 50.00 0.2348 3 — 39.76 0.002 50.00 0.4735 4— 39.96 0.019 50.00 0.0928 4— 39.77 0.005 50.00 0.188Table 5.8: Stabilising controller horizons and closed loop characteristics for a structurallyrigid two link manipulator contacting a keff = kN/m surface. Id = damped natural frequency and damping ratio.composed of one overdamped and one underdamped mode. For all other control horizonvalues both of the closed ioop modes are underdamped. Figure 5.40 shows the contact forcestep responses in the world frame x and y directions and the control inputs (joint torques)for the case of a 2 time step control horizon and a prediction horizon of N2 5. As canbe seen, this combination of controller horizons yields a fast, offset free step response withnegligible overshoot. Reducing the prediction horizon to N2 = 4 moves the dominant poleof the overdamped mode farther to the left on the real axis, thereby producing a somewhatfaster rise time but this is accompanied by a slight overshoot due to the 35Hz mode. Onthe other hand, increasing the prediction horizon only results in slower rise times.5.5.4 Force Control for Two Flexible LinksTo demonstrate the performance of the multivariable predictive control algorithm for thecase of flexible links we will examine the case of a two link manipulator with identical linksChapter 5. Force Control Algorithm Performance and Evaluation 203(a) direction contact force (b) y direction contact force1413121110E9z01.—aC05-4320 .05 .10 .15 .20 .25 .30 .35 .40Time [s](c) Joint torquesFigure 5.40: Force control step responses for a structurally rigid two link manipulatorcontacting a keff = lOkN/m surface with N2 = 5, N = 2, ) = 0.111098C04C)32— ResponseSetpoint111098765432ci)C)0cicciC000 .05 .10 .15 .20 .25 .30 .35 .40 “0 .05 .0 .15 .20 .25 .30 .35 .40Time [s] Time [SI— ResponseSetpoint— Joint 1Joint 2Chapter 5. Force Control Algorithm Performance and Evaluationin 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 longaluminium rod that was used throughout section 5.3. The nominal joint angles are 19 = 00and 6 = 30° and the tip of the manipulator is in contact with an environment having anisotropic environment with a effective contact stiffness of keff = lOkN/m. The motion ofthe manipulator is in the plane perpendicular to the gravitational field.Controller ModelTable 5.9 lists the numerical values of the link and contact dynamics model parameter forthe system. Using these values the transfer functions given by equations (5.75) and (5.76)evaluate to:Fr(s) —36.49s6 — 1.56 x 106s4 + 1.95 x 10h1s2 + 2.18 x iO’ui(s)— IJ(s)ILink Dynamics204Link (k) 1 28(°) 0 30Lk(m) 1.0 1.0Mk(kg) 0.8451 0.84517J0k(kg.m2) 0.2817 0.2817Tlkl(kg m2) 0.4807 0.480712(kg.m) 0.8451 0.845113k1(N m) 7096 7096I4k(kg.m) 0.6617 0.6617cIk1o(m) 2.0 2.0ç1410(m/m) 2.753 2.753Contact DynamicsDirection x yk(kN/m) 20 20ke(kN/m) 20 20keff(kN/m) 10 10Table 5.9: Parameter values for two flexible link manipulator controller model(5.85)Chapter 5. Force Control Algorithm Performance and Evaluation 205Fr(s) — _289.3s6 — 6.67 x— 2.95 x 10’2s— 1.18 x 10165 86U2(S) -Fr(s)—_3.26s6 + 3.47 x i0s — 2.96 x 1010s2 + 1.26 x 1O’ 5 87ui(s)-andF(s) — _29.85s6— 3.60 x 106.s4 + 2.40 x 10u1s2 — 1.26 x 1O’ (5 88)u2(s)where= (1.39 x 104s8+ 88.05s6 + 1.36 x 107s4 + 4.08 x 101182 + 1.26 x 1015). (5.89)Figure 5.41 shows the natural frequencies of the first four modes of the system as afunction of keff with keff = lOkN/m marked. At this effective contact stiffness the frequenciesof the 7h and 772 modes are in the region of approximate proportionality to kff and thereforemake significant contributions to the dynamics. The mode frequency is independentof keff and the 774 mode frequency is just entering the transition range. The open ioopnatural frequencies of the modes are 9.39Hz, 29.7Hz, 67.5Hz and 102Hz. Sampling at250Hz (h = 4ms) to satisfy the Nyquist criterion for the mode results in the followingdiscrete time transfer function models for the controller:F —1— Bi(q1)ui +B2(q1)u —i(q— A —‘ q (.andF Bi(q)ui +B2(q’)u —1Y A —i q ,whereA(q’) = 1— 1.48q’+0.67q2—0.34q3+0.55671 48q,(5.92)Bi(q’) = —0.81 +3.76q’ —5.20q2+2.46(qq4)—5.20 +3.76q6—0.81q7, (5.93)B2(q1)—9.47+18.5q’—17.0q+6. 9() 17.18.59.47 (5.94)Chapter 5. Force Control Algorithm Performance and Evaluation 206iø1OkN/rn.1012//iOø I I iiiil I iiiiiil I hull1 uuuuuul lulihul I 111111110_2 i00 101 102 iøEffective Contact Stiffness [kN/m]Figure 5.41: Variation of the natural frequencies for a two link manipulator with identicalflexible (El = 574N m2, one structural mode each) links assuming steady state contactdynamics and an isotropic environment.Chapter 5. Force Control Algorithm Performance and Evaluation 207—0.047q’— 0.87q2 + 0.70(q3+ q4) — 0.87q5+ 0.34q6— 0.047q7, (5.95)andB2(q’) = —0.78+2.91q’ —2.61q+0.36(q3q4)—2. 15+2.91q6—0.78q7. (5.96)These polynomials provide the initial model coefficient values for the multivariable adaptivepredictive force control algorithm. They are updated at each time step by a multivariableform of the EFRA system identification algorithm. The coefficients are also used as part ofthe technique developed in section 3.3.1 to analyse the performance of the control algorithmby calculating the approximate closed loop characteristics.Step ResponseThe results plots presented in the first part of this section are from a simulation usingthe linearised manipulator dynamics. This is done in order to minimise the mismatch between the controller model and the system dynamics so that attention can be focussed onthe performance of the control algorithm. Later in the section results from the nonlineardynamics simulation are shown. These results demonstrate the role that the system identification algorithm plays in the adaptive controller, namely, compensating for the presenceof unmodelled, nonlinear components of the manipulator dynamics.Examination of the approximate closed loop poles, calculated from the control law andthe linearised system dynamics, shows that with N = 1 all prediction horizons which givestable control result in all of the closed loop modes being underdamped. The resultingresponse therefore exhibits undesirable overshoot and oscillations about the setpoints. Thispoor control is not surprising in light of the fact that there are two dominant modes inthe system. Allowing the controller only one time step in which to minimise the predictederror does not provide enough freedom to deal with two modes. Better control is obtainedwith N = 2, that is, the control horizon equal to the number of dominant open loopChapter 5. Force Control Algorithm Performance and Evaluation 208Overdamped Mode Underdamped ModesN2 iç Dominant Pole Id [Hz] C fd [Hz] C fd [Hz] C9 2 -91.87 23.20 0.675 32.74 0.222 101.2 0.05515 2 -103.6 12.43 0.889 36.51 0.174 98.99 0.05218 2 -39.86 30.91 0.447 35.51 0.297 100.1 0.06020 2 -25.58 31.96 0.187 39.02 0.423 99.63 0.04422 2 -17.55 30.89 0.165 44.43 0.351 99.44 0.0486 3 -41.98 37.15 0.040 89.77 0.045 125.0 0.29025 3 -49.95 35.63 0.008 99.62 0.002 125.0 0.8478 4 -92.27 35.16 0.010 96.12 0.012 125.0 0.34210 4 -78.50 35.57 0.029 95.24 0.014 125.0 0.39112 4 -65.09 35.78 0.016 96.50 0.010 125.0 0.466Table 5.10: Stabilising controller horizons and closed ioop characteristics for a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/m surface. Id =damped natural frequency and C = damping ratio.modes. The closed ioop response is composed of one overdamped and three underdampedmodes. The top part of Table 5.10 shows the approximate continuous time closed ioopcharacteristics for various values of N2. In this case the closed loop performance is largelygoverned by the time constant of the overdamped mode and the damping ratio of thelowest frequency underdamped mode. With a 2 step control horizon, N2 = 20 provides theresponse with the fastest rise time and no overshoot, as can be seen in Figures 5.42a and5.42b. A shorter prediction horizon causes the rise time to be such that the oscillations ofthe underdamped modes do not completely damp out before the setpoint is reached andthe response overshoots. Longer prediction horizons result in unnecessarily slow responses.With a control horizon of 3 time steps the closed system generally has four underdamped modes, but for selected prediction horizons (see Table 5.10) the response has oneoverdamped mode and three underdamped ones. In the case of four underdamped modesone 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 overshoot, if the damping ratio of the lowest frequency mode is sufficiently high the overshootChapter 5. Force Control Algorithm Performance and Evaluation12F:2302826242218a)D 1601412-, 1086420(a) a direction contact force(c) Joint torques(b) y direction contact force(d) Link tip deflectionsFigure 5.42: Force control step responses for linear dynamics simulation of a two flexible link(one structural mode each) manipulator contacting a keff = l0kN/m surface with N2 = 20,N = 2, 0.14209F’a)C-)0UC0C-)0— ResponseSetpoint.05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60Time [s]ResponseSetpoint0 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60Time [s].05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60Time tslChapter 5. Force Control Algorithm Performance and Evaluation 210will be negligible. As noted in section 5.3.5 the possibility of one of the closed ioop modeschanging from being overdamped to being underdamped may be cause for concern regardingthe robustness of the control law.With N = 4 the controller has a control horizon equal to the total number of modesin the open loop system. Table 5.10 indicates that the lowest frequency closed ioop modewhich dominates the response when N = 2 has been completely removed and a lightdamped 125Hz mode (half the sampling frequency) has appeared. The dominant pole ofthe overdamped mode is generally more negative than is the case with Nu < 4, indicatingthat the rise time is decreased. Figure 5.43 shows the step responses for the case of N2 = 8and N = 4. The faster response of the N 4 controller compared to the N = 2 isthe result of a much more active control input signal as can be seen by comparing Figures5.42c and 5.43c. Increasing the prediction horizon from 8 time steps generally increases thedamping of the closed ioop modes, thereby reducing the ripple in the response but only atthe expense of increased rise times.The step responses examined so far have concentrated on the case of simultaneouschanges in the x and y direction contact force setpoints. Another important aspect of theclosed ioop performance, given the strongly coupled nature of the system, is the interactionof the outputs. This aspect is illustrated by commanding a step change in one output whileholding the other setpoint constant. Figure 5.44 shows the responses under these conditionsthat results from controller horizons of N2 = 20 and N = 2 and Figure 5.45 shows thecorresponding results with N2 = 8 and N = 4. These figures show that the disturbanceof one component of the contact force which occurs in response to a step change in thesetpoint of the other component is of much larger magnitude and longer duration when theN = 2 controller is used rather than N = 4. The difference in duration of the disturbanceis due in part to the anticipation characteristic of predictive control and results from thedifference in prediction horizons. The difference in magnitude is due to the difference inChapter 5. Force Control Algorithm Performance and Evaluationzci)C)0LLC)CuC0C.)323028262418D2161214C0—, 1086420(a) direction contact force(c) Joint torques(b) y direction contact force(d) Link tip deflectionsFigure 5.43: Force control step responses for linear dynamics simulation of a two flexible link(one structural mode each) manipulator contacting a keff = lOkN/m surface with N2 = 8,N = 4, ) = 0.211161412108642— ResponseSetpointci)C)0C)(8C00IL.05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55.Time [s]— ResponseSetpoint35 .40 .45 .50 .55Time [s]0 .05 .10 .15 .20 .25 .30 .35 .40 .45 .50 .55 .60Time [s] Time [s]Chapter 5. Force Control Algorithm Performance and Evaluation2’C)0UC0C.)212Figure 5.44: Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting akeff = lOkN/m surface with N2 = 20, N 2, 0.1614121086420ResponseSetpointci)C)0LlC)cciC0C.)II.1 .2 .3 .4 .5Time [s].6 .7 .8 .9 1.0(a) direction contact force0 .1 .2 .3— Response- Setpoint.4 .5 .6 .7 .8 .9 1.0Time [s](b) y direction contact force343230282624E 22-201)D 1801614CO 12-310a6420Time [s] Time [s](c) Joint torques (d) Link tip deflectionsChapter 5. Force Control Algorithm Performance and Evaluation20181614G)o 012LI10Eo 0o 0642252000-)Figure 5.45: Force control step responses showing output interactions for linear dynamics simulation of a two flexible link (one structural mode each) manipulator contacting akeff = lOkN/m surface with N2 = 8, N 4, 0.2131614121086420Response-. Setpoint1’I .1 .2 .3 .4 .5 .6 .7 .8 .9 1.0Time [s](a) direction contact force— Response- Setpoint4035.1 .2 .3 .4 .5 .6 .7 .8 .9 1.0Time [s](b) y direction contact force30105.1 .2 .3 .4 .5 .6Time [s](c) Joint torques.1 .2 .3 .4 .5 .6 .7 .8 .9 1.0Time [s](d) Link tip deflectionsChapter 5. Force Control Algorithm Performance and Evaluation 214the lowest closed ioop natural frequencies and the degree to which they are excited. Thedisturbance of the y direction contact force due to a step change in the x direction is lesssignificant than when the situation is reversed but the trends of magnitude and duration ofthe disturbance are comparable. The generally better performance in the y direction shownin all of the above figures is due primarily to the 8 0°, 0 30° configuration of themanipulator.All of the preceding simulation results in this section are based on a simulation of thelinearised equations of motion. Under those circumstances the controller model and thesystem dynamics are exactly matched. When the full, nonlinear dynamics of the systemare considered the situation is somewhat more complicated. The nonlinearities represent,from the point of view of the controller, unmodelled dynamics. The controller is able tocope with the unmodelled dynamics due to its explicitly adaptive implementation, thatis, at every time step the system identification algorithm calculates a new estimate of thecoefficients of the controller model. These coefficients are presumed to accurately representthe instantaneous system dynamics in a linear form and are used to calculate the controlinput for the next time step. Figure 5.46 shows a series of step responses from the nonlineardynamics simulation for the same manipulator and environment configuration used above.The controller has a prediction horizon of N2 = 20 time steps and a control horizon of N = 2steps. During the early part of the response the mismatch between the controller and systemdynamics results in strong excitation of the high frequency modes. The high frequencystructural vibrations of the links are evident in the plot of the local defiections of the linktips shown in Figure 5.46d. These vibrations are also evident in the plots of the contact forcecomponents (Figures 5.46a and 5.46b). In order to control the high frequency modes thecontrol 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 ofovershoot it also provides rich excitation for the system identification algorithm with theChapter 5. Force Control Algorithm Performance and Evaluationa) Q)o 0o oU(aC Co 0o C)Figure 5.46: Force control step responses for nonlinear dynamics simulation of a two flexiblelink (one structural mode each) manipulator contacting a keff = lOkN/m surface withN2=2O,N=2,)=O.21511— ResponseSetpoint201816141210864204540350 .2 .4 .6 .8 1.0 1.2 1.4 1,6 1.8Time [SI(a) a direction contact force0 .2 .4 .6— ResponseSetpoint.8 1.0 1.2 1.4 1.6 1.8Time Is](b) y direction contact force3Øza) 250’1S 200—, 1510Time [s](c) Joint torques (d) Link tip deflectionsChapter 5. Force Control Algorithm Performance and Evaluation 216result that after 0.8s (200 time steps) the controller model coefficients have been sufficientlywell adjusted to compensate for the unmodelled nonlinear dynamics and the subsequent stepresponses are much improved.It is not surprising that the effect of the unmodelled nonlinear dynamics is much morepronounced in the case of the two link manipulator than was observed in the single linkresults when the structure of the equations of motion for the two cases are compared. Thenumber of centripetal and Coriolis terms in the two link equations of motion far exceedsthose in the single link equations. In particular the two link equations have a large number ofterms which represent interactions between the joint angle and modal amplifier generalisedcoordinates 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 astrictly linear controller to a highly nonlinear system. Several possibilities for improvementof the closed loop performance can be considered. The simplest is filtering of the high frequency component of the response which would reduce the amplitude of the disturbancesbut would likely also increase the time required for the identification algorithm to arriveat a suitable model. A more fruitful approach might be to model the disturbances due tothe nonlinear unmodelled dynamics as coloured noise and include the noise model in thecontroller model [32]. In either case the effect of the nonlinear dynamics can be substantially reduced by specifying setpoint programs which would produce less excitation of thestructural dynamics modes that the step changes shown in Figure 5.46. Ramps or higherorder polynomials would provide smoother transitions between setpoint levels and thereforereduce the level of excitation. Reformulation of the predictive control algorithm on thebasis of a nonlinear model (for example [90]) could also be considered.Chapter 5. Force Control Algorithm Performance and Evaluation 2175.5.5 SummaryIn summary, this section has presented an analysis of force controlled two link manipulatorswith 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 componentsexerted by the manipulator tip on environment surfaces.Linear analysis of the case of infinitely stiff links reveal the expected extension of characteristics which were identified in the single link case while at the same time illustratingthe added complexities of the two link system. The modal frequencies are found to varywith k!ff for a given set of link lengths and inertias and the two highest frequency modesare due to the local vibrations of the contact region mass. The frequencies are independentof when the contact is isotropic (kxeff = kyeff) and are bounded above by the values foré1 = 900. The contributions of the natural modes to the contact force components are alsofound to vary with the nominal manipulator geometry. With the addition of modes due tothe structural flexibility of the links, linear analysis reveals a transfer of dominance amongthe modes similar to that seen in the single link case, but with the expected additionaldependence on joint angles.A transfer function model of the system which relates the world frame x and y directioncomponents of the contact force to the joint inputs was derived on the basis of the linearisedequations of motion. Specific parameter values were selected for a manipulator with twoidentical links in contact with a relatively stiff (keff = lOkN/m) isotropic (k=k) environment. The links were based on the physical model of 1.Om long, 2.0cm diameter aluminiumrods. Cases in which the link are assumed rigid as well as having their structural dynamicmodelled using the first mode shape function of the rods considered as a cantilever beamshave been addressed.Chapter 5. Force Control Algorithm Performance and Evaluation 218As in the single link case, the approximate closed ioop characteristics calculated from thecontrol law were found to be very useful in selecting controller horizons which produced thefastest rise times and least overshoots from among the large set of horizons which stabilisethe system.5.6 Force Control for DC Motor Actuated Space Station ManipulatorAs a final demonstration of the performance of the multivariable predictive force controlalgorithm we will examine the case of a simple prototype model of the two long links of theproposed 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 physicalparameter values for the system are listed in Table 5.11. The top part of the table showsthe link model parameter values which are based largely on those used by Chan [68]. TheMSS manipulator operates in a microgravity environment in which the gravitational fieldis approximately balanced by the orbital centripetal acceleration and thus the gravitationalloading of the links is neglected in the model. The center section of the table shows theparameters used to model the DC motors. Since the motor parameters are not publicallyavailable the values shown are estimates based on assumptions of a maximum motor powerof 400W and a maximum torque delivery of 1500N m. Due to the mass of the links, thenatural frequencies of the cantilever beam modes used as admissible functions to model thelink structural dynamics are much lower than in the previously considered cases. In factthe frequency of the first mode is only about 0.3Hz. Even when the tip of the second linkis in contact with a surface with very high effective contact stiffness the natural frequenciesare all less than 30Hz (see Figure 5.47).The controller model structure is similar to that derived in section 5.5.2. The controlinputs are the input voltages to the motors. These voltages are amplified to produce theChapter 5. Force Control Algorithm Performance and Evaluation 219Link DynamicsLink (k) 1 2(°) 0 30Lk(m) 7.5 7.5Mk(kg) 1600 1600IOk(kg m2) 3.0 x i0 3.0 x iOIlkl(kg . m) 6826 682612(kg.m) 1600 160013k1(N.m) 6388 638841C(kg.m) 1253 12534k1o(m) 2.0 2.0qYklO(m/m) 0.3671 0.3671Actuator DynamicsJoint (k) 1 2Iak(kg m2) 0.5 0.5Ngk () 50. 50.Rak(Q) 1.0 1.0Kk(N . rn/A) 1.5 1.5Kem(V s/rad) 0.5 0.5K(V/V) 4.0 4.02akmax(A) 20. 20.Vznkmax(V) 5.0 5.0Contact DynamicsDirection x yk(kN/m) 40 40ke(kN/m) 40 40keff(kN/m) 20 20Table 5.11: Physical parameter values for a two link MSS model with DC motor actuators.Chapter 5. Force Control Algorithm Performance and Evaluation 220210 I I 1111111 I I 1111111 I 11111111 11111111 I I 11111111011 0D- Iz I10-I11,1_2 i ii I I I ii I I I I I ii I I I I II I10_i 1& 102 iøEffective Contact Stiffness [kN/m]Figure 5.47: Variation of the modal natural frequencies for DC motor actuated two linkMSS model.Chapter 5. Force Control Algorithm Performance and Evaluation 221armature circuit voltages which in turn produce the armature currents to which the torquessupplied by the motors are proportional. Thus, the discrete time transfer function modelsfor the controller are of the form:—1 Bi(q1)vi +B2(q1)v_.iF(q )= A(q1) q (5.97)andF — Bi(q1)vi +B2(q’)v —1 5 98Y— A(q1) qWith the manipulator in contact with a surface such that the effective contact stiffness is2OkN/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 thereforecontributes significantly to the contact force. To satisfy the Nyquist criterion for themode a sampling frequency of 7.7Hz (h = l3ms) is used.Figure 5.48 shows the contact force responses from the linearised dynamics model of theMSS controlled by the adaptive, multivariable predictive control algorithm with N2 = 14,N = 2, k = 0 and .Xse = 0. Note that the time scale of the figures in this section is muchlonger than that of the previously shown results owing to the much greater mass of theMSS links compared to aluminium rod links used in the previous cases. The y directioncontact 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 and1 — et at the midpoint of the transition. The direction goal is to maintain a constantforce level. Figure 5.49 shows the responses of the full nonlinear dynamics model withthe same setpoints and controller parameters. These results illustrate the improved performance for the nonlinear case (compared to Figure 5.46) which is obtained by using asmooth setpoint transition as opposed to a step change. The relatively small magnitude ofthe level change also contributes to the improved performance. Both of these factors resultChapter 5. Force Control Algorithm Performance and Evaluation5.55.04.54.0a)0C003.02.52.01.51 .0.50>0.z0U0(aC00EEC0C)a)0.IC-J222Figure 5.48: Force control responses for linear dynamics simulation of a two flexible linkSpace Station MSS contacting a keff = 2OkN/m surface..4.54.0/7I1/3.02.52.01.51 .0— Response.5 ...---. Setpoint0.1,1 1,1. .1, I. .1.1.1.0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s](b) y direction contact forcer— Response2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s](a) n direction contact force- 060 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s](c) Motor input voltages2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s](d) Link tip deflectionsChapter 5. Force Control Algorithm Performance and Evaluation5.55.04.53.50Li_C)a)C0C.)2.52.01.51.0.5.35.30.25.20.15(a.10>.05C— 0-. 05- .10- .15-. 20223(c) Motor input voltages (d) Link tip deflectionsFigure 5.49: Force control responses for nonlinear dynamics simulation of a two flexible linkSpace Station MSS contacting a keff = 2OkN/m surface.5.04.54.03.53.02.52.0a)00U00... .— Response r— Response----- Setpoint .5. L- Setpoint.1 .1. .1.1.. .1.1.1. 0 .1. .1.1. .1.1.0 2 4 6 8 10 12 14 16 18 20 22 24 28 28 30 0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30Time [s] Time [s]( a) z direction contact force (b) y direction contact force4• 1.5— Joint 1 1 I 1 .0 — — — ... link 1--- Joint 2 ii L--- link 2aE. -.5C.2-1.0.,j I’.. —.0 2 4 6 8 10121416182022242628Time [s]30 14 16 18 20 22 24 26 28 30Time [s]Chapter 5. Force Control Algorithm Performance and Evaluation 224in less excitation of the nonlinear dynamics of the system which are not included in the controller model. While the improvement in the system performance is encouraging, additionalfiltering and/or reformulation of the control algorithm, as discussed in section 5.5.4, wouldbe necessary to meet the expected operational performance specifications for the MSS.5.7 Summary of Force Control PerformanceThe simulation results in this chapter have demonstrated the viability of the use of amultivariable, explicitly adaptive, long range predictive control algorithm to control thecontact forces exerted by manipulators on surfaces in their working environment. Thefollowing configurations were considered:• a single flexible link with its structural dynamics modelled by one, two and three admissible mode shape function in contact with surfaces with effective contact stiffnessesranging from 3.1kN/n to 5MN/m;• the same link, initially under motion control, making unexpected contact at an impactvelocity of approximately O.5m/s with a surface;• a planar, two link manipulator with rigid links in contact with an isotropic effectivecontact stiffness of lOkN/m;• the same two link manipulator with the structural flexibility of the link modelled byone admissible mode shape function for each link, also making contact with a surfacesuch 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 ServicingSystem (MSS) manipulator for the proposed Freedom Space Station, with its tip incontact with a keff = 2OkN/m surface.Chapter 5. Force Control Algorithm Performance and Evaluation 225The single flexible link step response results demonstrate that with a controller model ofappropriate order and a sampling rate selected to resolve the dominant mode, as indicatedby the shape of a frequency versus effective contact stiffness plot, there are generally a widerange of control algorithm horizon and weighting factor values for which the closed ioopwill be stable. The inherent integral action of the control algorithm guarantees that theresponse will not have a steady-state offset. A new formulation for the discrete time domainclosed ioop characteristic polynomial has been used to select control algorithm horizons andweighting factors to satisfy additional performance requirements such as minimal overshootand rise time. The inclusion of a static equilibrium bias term in the controller has beenshown to decrease rise time at the expense of introducing a slight amount of overshoot intothe response. The importance of selecting a reasonably accurate initial controller modelimplies that knowledge of the structural dynamics of the manipulator and the contactcharacteristics of the surfaces likely to be encountered in the workspace is necessary toensure successful control.The problem of maintaining control while unexpectedly making contact with a surfaceis a challenging one because the complex dynamics of the system are compounded by thepresence of a discontinuity. To cope with this problem a contact control logic level hasbeen introduced into the manipulator control hierarchy. Two operational strategies for thecontact control logic have been demonstrated and shown to allow control to be maintainedthrough contact events, albeit with some unavoidable contact force transients. Of particular interest is the fact that under the setpoint modification strategy there is a directrelationship between the impact velocity and the magnitude of the initial force transient.This relationship allows manipulator motions, in particular tip velocities, to be planned onthe basis of the transient contact force levels to which nearby surfaces can be acceptablysubjected, should a collision occur.Chapter 5. Force Control Algorithm Performance and Evaluation 226The step response results for the case of a two link manipulator with a rigid structureshow that the controller has the potential to provide excellent force control performance forthis large subset of existing industrial manipulators. In this context, the predictive controlalgorithm 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 asthe limiting situation of infinite structural stiffness because it offers validation of some ofthe fundamental conclusions of the linear analysis developed in this chapter.With the inclusion of structural flexibility the dynamics of the two link manipulatorbecome markedly more complex and nonlinear. The results demonstrate that the insightgained from the linear analysis of the system and the calculation of the roots of the closeloop characteristic polynomial offer guidance in this case, as in the less complex cases, inthe construction of the controller model and the selection of control algorithm horizons andweighting factors to provide the most desirable response. The simulations also show thatthe strongly coupled structure of the manipulator dynamics, combined with the optimal (inthe sense of minimising a cost function based on predicted output errors) form of the controllaw, make total isolation of the controlled quantities impossible. That is, the response to astep change commanded for one component of contact force will result in some disturbanceof the other component.The highly nonlinear nature of the two flexible link manipulator dynamics presents somedifficulty for the inherently linear control algorithm. The unmodelled, nonlinear dynamicsare compensated for, to some extent, by the adaptive structure of the controller but it hasbeen found that it is possible to excite the unmodelled dynamics to such an extent thatstability is lost. Several potential remedies to this problem are possible including filtering ofthe contact force signals, using smooth setpoint transitions instead of steps, inclusion of acoloured noise model in the control algorithm formulation, and rederivation of the predictivecontrol algorithm on the basis of a nonlinear model.Chapter 5. Force Control Algorithm Performance and Evaluation 227rpIle force control simulations for the Space Station Mobile Servicing System (MSS) manipulator model demonstrate the effectiveness of the controller with a manipulator havingradically different dynamic parameters from those considered previously. The model includes voltage controlled DC electric motor actuators which drive the joints through gearboxes. The force control results show that the use of smooth setpoint transitions attenuates,to some extent, the difficulties caused by unmodelled, nonlinear dynamics.Chapter 6Summary of Major ResultsThis thesis has developed and demonstrated an overall strategy for the control of contactforces exerted by a structurally flexible manipulator on surfaces in its working environment. The control strategy is centered on an explicitly adaptive implementation of a predictive 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 manipulator/environment system in several configurations, using a combination of analytic andnumerical techniques. The results of these analyses provide insight into the fundamentaldynamics which dominate the force controlled system. These insights are used to guide theselection 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 surfacesin its working environment has been developed. The model includes the effects oflocal inertia, energy dissipation and surface stiffnesses of the contacting surfaces ofthe manipulator tip and the environment. Analysis shows that when the mass of thesurface regions deflected due to contact is comparable to the inertias of the links theinertia effects in the contact model become important.• Linear analyses of various configurations of manipulators in contact with environmentsurfaces have been carried out using a combination of analytic and numerical techniques. The results of these analyses provide new insights, not apparently available228Chapter 6. Summary of Major Results 229in the literature, into the fundamental dynamics of the system using established linear analysis methods. In particular, they reveal that the contact force response isdominated by different open loop modes, depending on the effective stiffness of thecontacting surfaces and that the transfer of dominance among the modes follows anorderly progression from lower to higher frequency modes as the effective contact stiffness is increased in a given configuration. The analyses also show that the controlinputs tend to provide excitation to all but the very high frequency contact vibrationmodes. The implications of these two results for the design of discrete time domaincontact force controllers are that the loop must be closed at a sampling frequency sufficient to resolve the dominant mode in the contact force response and the controllermodel must include the dynamics of all of the excited modes. The linear analysesprovide guidance in determination of the dominant and significantly excited modesfor a particular manipulator configuration.• A new general formulation for the closed loop characteristic polynomial of a systemcontrolled by the Ceneralised Predictive Control (GPC) algorithm has been obtainedin the discrete time domain. The formulation is applicable to both the original singleinput, single output form of the algorithm and to its multivariable extension. Calculation of the approximate closed loop characteristics of the system from this resulthas been shown to be useful in tuning the control law for particular configurations.• The predictive control algorithm has been extended with a static equilibrium biasterm in the cost function. This extension is applicable to both SISO and multivariableforms of the algorithm. The addition of this new term augments the feedback controllaw with a proportional action feedforward component. The static equilibrium biasterm is particularly applicable in the contact force control application because of theChapter 6. Summary of Major Results 230easily defined static equilibrium relationship between joint torques and contact forcecomponents.• Contact control logic is introduced as a first step in the integration of the predictivecontrol 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 discontinuityinvolved in making and breaking contact and collisions between the manipulator tipand the environment surface due to unexpected contact events.• Detailed simulation results for one and two flexible link manipulators have been presented. The results demonstrate that the predictive control algorithm can be tunedto provide fast step responses with minimal overshoot for a wide variety of manipulator configurations and contact stiffnesses. The importance of the effective contactstiffness in the dynamic structure of the system indicates the need for some a prioriknowledge of the surfaces a given manipulator is likely to contact during particularoperations.• Two operational strategies for the contact control logic have been demonstrated inthe situation of a collision due to an unexpected contact event. The setpoint modification strategy was found to be superior to the stop motion strategy, which tends toinduce bouncing on the surface. In the setpoint modification strategy joint and forcesensor data around the instant of contact are used to predict the characteristics ofthe expected force transient and the contact force setpoint is then briefly increased toallow the controller to supply more energy to overcome the unexpected force transientdue to the kinetic energy transferred from the link to the surface.• The explicitly adaptive implementation of the predictive force control algorithm usingthe Exponential Forgetting and Resetting Algorithm (EFRA) for system identificationChapter 6. Summary of Major Results 231has been found well capable of compensating for unmodelled structural modes in thesingle link case. The unmodelled nonlinear dynamics in the two link case present agreater difficulty which significantly limits the performance of the controller.6.1 Recommendations for Further ResearchThe following directions for future research stemming from the results of this thesis arerecommended:• integration of the predictive control algorithm for force and motion control of flexiblelink manipulators into the framework for simultaneous force and motion control suchas 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 makingand breaking contact events; this may be a suitable application for fuzzy control;• improvement of the force control algorithm in situations where significant unmodellednonlinear dynamics exist; possible actions are simple filtering, inclusion of a colourednoise model in the controller model, selection of setpoint programs less likely to excitethe nonlinear dynamics, and reformulation of the predictive controller on the bassisof a nonlinear model.Implementation IssuesEarly in the course of the research force control experiments were conducted in which aversion of the damping algorithm introduced by Whitney [3] was implemented on a Puma560 robot equipped with a JR3 force sensor. Edge following and peg—in—hole operations weresuccessfully demonstrated. Unfortunately, the closed architecture of the Puma controller,its relatively slow ioop closing time (‘-. 28ms), and the unsuitability of the interpreted ValChapter 6. Summary of Major Results 232II controller programming language for extensive mathematical computations precludedthe implementation of a more complex predictive force control algorithm.The experiments with the Puma 560 robot and the JR3 force sensor demonstratedtwo characteristics of the force sensor which can pose difficulties. Firstly, under somecircumstances the raw sensor signal was found to be quite noisy. The JR3 sensor firmwareincludes digital filtering capabilities which allowed a much cleaner signal to be obtainedwithout appreciable loss of accuracy for the operations undertaken. The second problem wasthe tendency of the sensor to measure inertial forces when the manipulator was executinglarge motions. This problem was overcome by only activating the force control algorithmwhen contact was imminent and by establishing a deadband around zero force in the controlalgorithm. With more powerful computational hardware this problem could perhaps bereduced by calculating an estimate of the inertia force at the sensor and applying it to thesensor signal as a bias.The analytical and simulation results presented in this thesis serve to establish the viability of the adaptive, multivariable predictive algorithm and contact control logic for contactforce control applications. Several considerations which arise from these results will have tobe addressed in the course of future experimental and eventual practical implementations.The selection of the appropriate sampling rate for the discrete time controller has beendiscussed throughout the simulation results sections. It has been shown that the necessarysampling rate as well as the structure of the controller model and the initial values of thecoefficients of that model are closely related to the dynamic characteristics of the manipulator links and the effective stiffness and inertia of the contact region that exists betweenthe manipulator tip and the environment. Knowledge of the link dynamics can be obtainedby spectral analysis of their structural vibrations. The contact characteristics which are required are the effective values for the particular end-effector in contact with the particularenvironment surface. These values will account for the effects of all sources of complianceChapter 6. Summary of Major Results 233which are relevant to the contact operation. In addition to the local characteristics of thecontacting surfaces, sources of compliance might include the presence of discrete compliancedevices such as a Remote Compliance Centre (RCC) at the manipulator wrist, force sensorcompliance, 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 stiffnessusing the manipulator sensors would seem to be the best available approach to obtainingvalues for the effective contact characteristics.The need to incorporate the characteristics of particular end-effector and environmentsurfaces in the controller model suggests that, for operation in a relatively structuredworkspace, a “model scheduling” strategy might improve the performance of the controllerin comparison to depending entirely on the system identification algorithm to adjust themodel coefficients when a different end-effector is used or a different surface contacted. Suchmodel scheduling could easily be incorporated in the contact control logic.The computational complexity of the control and identification algorithms governs thetype and speed of processing hardware required for successful implementation. The calculations required in the contact control logic and the predictive part of the control algorithmare relatively simple. However, the calculation of the control input increments from theresults of the predictor calculations is somewhat more taxing because it involves the calculation 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 executedindependently for each system output at each time step and thus lends itself to parallelprocessing. The versions of the control and identification algorithms coded in TWOFLEXexecute at approximately 20% to 50% of the speed required for real time operation on aSparcStation 2. The routines are coded in FORTRAN and were written with emphasis onmaking them understandable and maintainable, not necessarily time efficient. Rewriting ofChapter 6. Summary of Major Results 234the routines in a language and form intended for real time operation should allow successfulimplementation using existing hardware.Bibliography[1] I. Winton. Breathing life into the SPDM. in Proceedings of 6th CASI Conference:Symposium on Space Station. Canadian Aeronautics and Space Institute, November1989.[2] D.E. Whitney. Historical perspective and state of the art in robot force control. International Journal of Robotics Research, 6(1):3—14, 1987.[3] D.E. Whitney. Force feedback control of manipulator fine motions. ASME Journal ofDynamic Systems, Measurement and Control, 98:91—97, June 1977.[41 J.K. Salisbury. Active stiffness control of a manipulator in cartesian coordinates. InProceedings of the 19th IEEE Conference on Decision and Control, pages 95—100, December 1980.[5] N. Hogan. Impedance control: An approach to manipulation. Part I — Theory. ASMEJournal of Dynamic Systems, Measurement and Control, 107:1—7, March 1985.[6] N. Hogan. Impedance control: An approach to manipulation. Part II — Implementation. ASME Journal of Dynamic Systems, Measurement and Control, 107:8—16, March1985.[7] N. Hogan. Impedance control: An approach to manipulation. Part III — Applications.ASME Journal of Dynamic Systems, Measurement and Control, 107:17—24, March1985.[8] M.T. Mason. Compliance and force control for computer controlled manipulators.IEEE Tranactions on Systems, Man and Cybernetics, SMC-11:418—432, June 1981.[9] C. Wu. Compliance control of a robot manipulator based on joint torque servo. International Journal of Robotics Research, 4(3):55—71, 1985.[10] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipulators, Part I: The fundamental concepts of compliant motion. IEEE Journal of Roboticsand Automation, RA-2:83—92, 1986.[11] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipulators, Part II: Design method. IEEE Journal of Robotics and Automation, RA-2:93—105,1986.235Bibliography 236[12] M.H. Raibert and J.J.. Craig. Hybrid position/force control of manipulators. ASMEJournal of Dynamic Systems, Measurement and Control, 102:126—133, June 1981.[13] J.K. Salisbury and J.J. Craig. Articulated hands: Force control and kinematic issues.International Journal of Robotics Research, 1(1):4—17, 1982.[14] C.H. An, C.G. Atkenson, and J.M. Hollerbach. Model-Based Control of a Robot Manipulator. The MIT Press Series in Artificial Intelligence. The MIT Press, Cambridge,Massachusetts, 1988.[15] 0. Khatib. A unified approach for motion and force control of robot manipulators:The operational space formulation. IEEE Journal of Robotics and Automation, RA3:43—53, 1987.[16] C.H. An and J.M. Hollerbach. The role of dynamic models in cartesian force controlof manipulators. International Journal of Robotics Research, 8(4):51—72, August 1989.[171 Uchiyama:1989. Control of robot arms. JSME International Journal, Series III,32(1):1—9, 1989.[18] M.W. Spong. Force feedback control of flexible joint manipulators. In Proceedings ofthe ASME Winter Annual Meeting, pages 177—183, 1987.[19] M.W. Spong. On the force control porblem for flexible joint manipulators. IEEETransactions on Automatic Control, 34(1):107—111, January 1989.[20] S.W. Tilley, RH. Cannon, Jr., and R. Kraft. End point force control of a very flexiblemanipulator with a fast end effector. In Proceedings of the ASME Winter AnnualMeeting, 1986.[21] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a one—link force—controlled flexible manipulator. Journal of Robotic Systems, 5(5):443—451, 1988.[22] D.J. Latornell and D.B. Cherchas. Force control for a flexible manipulator link usinggeneralized predictive control. In M. Jamshidi and M. Saif, editors, Robotics andManufacturing— Recent Trends in Research, Education and Applications, volume 3,pages 569—576, New York, 1990. ASME Press.[23] B.C. Chiou and M. Shahinpoor. Dynamic stability analysis of a two—link force—controlled flexible manipulator. ASME Journal of Dynamic Systems, Measurementand Control, 112:661—666, December 1990.[24] F.G. Pfeiffer. Path and force control of elastic manipulators. In Proceedings of theIEEE Conference on Decision and Control, pages 514—519, 1990.Bibliography 237[25] F. Matsuno, Y. Sakawa, and T. Asano. Quasi-static hybrid position/force controlof a flexible manipulator. In Proceedings of the IEEE Conference on Robotics andAutomation, pages 2838—2843, April 1991.[26] D. Li. Tip—contact force control of one—link flexible manipulator: An inherent performance limitation. In Proceedings of the American Control Conference, pages 697—701,1990.[27] D.J. Latornell and Cherchas D.B. Force and motion control of a single flexible manipulator link. Robotics é4 Computer—Integrated Manufacturing, 9(2):87—99, April 1992.[28] A.P. Tzes and S. Yurkovich. Flexible—link manipulator force control. In Proceedingsof the American Control Conference, pages 194—199, 1990.[29] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control— Part I.The basic algorithm. Automatica, 23(2):137—148, 1987.[30] J. Denavit and R.S. Hartenberg. A kinematic notation for lower-pair mechanisms basedon matrices. ASME Journal of Applied Mechanics, pages 215—221, June 1955.[31] W.J. Book. Recursive lagrangian dynamics of flexible manipulator arms. InternationalJournal of Robotics Research, 3(3):87—101, 1984.[32] D.W. Clarke, C. Mohtadi, and P.S. Tuffs. Generalized predictive control— Part II.Extensions and interpretations. Automatica, 23(2): 149—i 60, 1987.[33] D.W. Clarke and C. Mohtadi. Properties of generalized predictive control. Automatica,25(6):859—875, 1989.[34] D.J. Latornell. Source listings for the TWOFLEX program— A dynamics and controlsimulation for structurally flexible robotics manipulators making contact with theirenvironment. Report CAMROL R.92-2, University of British Columbia, Departmentof Mechanical Engineering, Computer Aided Manufacturing and Robotics Laboratory(CAMROL), September 1992.[35] H.W. Mah. On the Nonlinear Dynamics of a Space Platform Based Mobile Manipulator. PhD thesis, Department of Mechanical Engineering, University of BritishColumbia, October 1992.[36] R.P. Paul. Robotic Manipulators: Mathematics, Programming and Control. The MITPress Series in Artificial Intelligence. The MIT Press, Cambridge, Massachusetts, 1981.[37] K.S. Fu, R.C. Gonzolez, and C.S.G. Lee. Robotics: Control, Sensing, Vision, andIntelligence. McGraw-Hill Inc., New York, NY, 1987.Bibliography 238[38] J.J. Craig. Introduction to Robotics. Addison-Wesley, Reading, Massachusetts, 1986.[39] W.J. Book. Analysis of massless elastic chains with servo controlled joints. ASMEJournal of Dynamic Systems, Measurement and Control, 110(3):187—192, 1979.[40] Liang-Wey Chang and J.F. Hamilton. The kinematics of robotic manipulators withflexible links using and equivalent rigid link system (ERLS) model. ASME Journal ofDynamic Systems, Measurement and Control, 113:48—53, March 1991.[41] L. Meirovitch. Computational Methods in Structural Dynamics. Sijthoff and Noordhoff,Alphen aan den Rijn, The Netherlands, 1980.[42] N.G. Chalhoub and A.G. Ulsoy. Dynamic simulation of a leadscrew driven flexiblerobot arm and controller. ASME Journal of Dynamic Systems, Measurement andControl, 108:119—126, June 1986.[43] G.G. Hastings and W.J. Book. A linear dynamic model for flexible robotic manipulators. IEEE Control Systems Magazine, pages 61—64, February 1987.[44] M. Benati and A. Morro. Dynamics of chain of flexible links. ASME Journal ofDynamic Systems, Measurement and Control, 110:410—415, December 1988.[451 E. Barbieri and U. Ozguner. Unconstrained and constrained mode expansions for aflexible slewing link. ASME Journal of Dynamic Systems, Measurement and Control,110:416—421, December 1988.[46] G. Naganathan and A.H. Soni. Coupling effects of kinematics and flexibility in manipulators. International Journal of Robotics Research, 6(1):75—84, 1987.[47] J.D. Lee and B.-L. Wang. Dynamic equations for a two—link flexible robot arm. Computers and Structures, 29(3):469—477, 1988.[48] Liang-Wey Chang and J.F. Hamilton. Dynamics of robotic manipulators with flexible links. ASME Journal of Dynamic Systems, Measurement and Control, 113:54—59,March 1991.[49] S. Cetinkunt and Wen-Lung Yu. Closed—loop behaviour of a feedback—controlled flexible arm: A comparative study. International Journal of Robotics Research, 10(3):263—275, June 1991.[50] P.A. Blelloch and K.S. Carney. Modal selection in structural dynamics. In Proceedingsof the 7th International Modal Analysis Conference, pages 742—749, January 1989.[51] J.T. Spanos and W.S. Tsuha. Selection of component modes for the simulation offlexible multibody spacecraft. In Proceedings of AAS/AIAA Astrodynamics SpecialistConference, August 1989.Bibliography 239[52] P.A. Blelloch and KS. Carney. Selection of component modes. American Institute ofAeronautics and Astronautics, 90(AIAA-90-1201-CP):105—1 12, 1990.[53] P.c. Hughes. Dynamics of a flexible manipulator arm for the space shuttle. In Proceedings of AAS/AIAA Astrodynamics Specialist Conference, Jackson Hole, Wyoming,September 1977.[54] F.A. Kelly and R.L. Huston. Modelling of flexibility effects in robot arms. In Proceedings of the 1983 Joint Automatic Control Conference, 1983.[55] P.C. Hughes. Dynamics of a chain of flexible bodies. Journal of Astronautical Sciences,27(4):359—380, 1979.[56] V.J. Modi and A.M. Ibrahim. Dynamics of the orbiter based flexible members. InProceedings of the 14th International Symposium on Space and Technology, Tokyo,May 1984.[57] M.W. Walker and D.E. Orin. Efficient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104(3):205—211, September 1982.[58] W.M. Silver. On the equivalence of Lagrangian and Newton—Euler dynamics for manipulators. In Joint Automatic Control Conference, 1983. Paper TA-2A.[59] K.H. Low and M. Vidyasagar. Dynamic study of flexible manipulators with open andclosed chain mechanisms. In Proceedings of the 1987 ASME Winter Annual Meeting,volume 6, pages 277—286. American Society of Mechanical Engineers, December 1987.[60] K.H. Low and M. Vidyasagar. A Lagrangian formulation of the dynamic model forflexible manipulator systems. ASME Journal of Dynamic Systems, Measurement andControl, 110(2):175—181, June 1988.[61] Y. Huang and C.S.G. Lee. Generalization of Newton—Euler formulation of dynamicequations to nonrigid manipulators. ASME Journal of Dynamic Systems, Measurementand Control, 110:308—315, September 1988.[62] K.L. Johnson. Contact Mechanics. Cambridge University Press, Cambridge, UK, 1985.[63] L.D. Landau and E.M. Lifshitz. Theory of Elasticity, volume 3 of Course of TheoreticalPhysics. Pergamon Press, 1959.[64] K.L. Johnson. One hundred years of Hertz contact. Proceedings of the Institution ofMechanical Engineers, 196:363—378, 1982.Bibliography 240[65] C.W. de Silva. Control Sensors and Actuators. Prentice-Hall, Inc., Englewood Cliffs,NJ, 1989.[66] R.L. Bisplinghoff, H. Ashley, and R.L. Halfman. Aeroelasticity. Assison-Wesley, Reading, Massachusetts, 1955.[67] B.W. Char, K.O. Geddes, 0.11. Gonnet, M.B. Monagan, and S.M. Watt. Maple User’sGuide. Waterloo, Ontario, 5th edition, March 1988.[68] J.K.W. Chan. Dynamics and control of an orbiting space platform based mobile flexiblemanipulator. Master’s thesis, Department of Mechanical Engineering, University ofBritish Columbia, April 1990.[69] B.J. Lazan. Damping of Materials and Members in Structural Mechanics. PergamonPress, 1968.[70] L. Meirovitch. Elements of Vibration Analysis. McGraw-Hill, 2nd edition, 1986.[71] E.H. Gaylord and C.N. Gaylord. Structural Engineering Handbook. McGraw-Hill, 3rdedition, 1990.[72] C.-K.A. Ng. Dynamics and Control of Orbiting Flexible Systems: A Formulation withApplications. PhD thesis, Department of Mechanical Engineering, University of BritishColumbia, April 1992.[73] D.W. Clarke and P.J. Gawthrop. Self-tuning controller. lEE Proceedings, Part D,122:929—934, 1975.[74] V. Peterka. Predictor-based self-tuning control. Automatica, 20(1):39—50, 1984.[75] B.E. Ydstie. Extended horizon adaptive control. In Proceedings of the 9th TriennialIFAC World Congress, Budapest, 1984.[76] P.S. Tuffs and D.W. Clarke. Self-tuning control of offset: A unified approach. lEEProceedings, Part D, 132:100—110, 1985.[77] G.C. Goodwin and K.S. Sin. Adaptive Filtering Prediction and Control. Prentice-Hall,Inc., Englewood Cliffs, NJ, 1984.[78] K.J. Aström and B. Wittenmark. Computer Controlled Systems Theory and Design.Prentice-Hall, Inc., Englewood Cliffs, NJ, 1984.[79] R.C. Cutler and B.L. Ramaker. Dynamic matrix control — A computer control algorithm. In Proceedings of the 1980 Joint Automatic Control Conference, volume 1, SanFrancisco, 1980.Bibliography 241[80] K. Ogata. Discrete—Time Control Systems. Prentice-Hall, Inc., Englewood Cliffs, NJ,1987.[81] A.-L. Elshafel, G. Dumont, and A. Elnaggar. Perturbation analysis of GPC with one-step control horizon. Automatica, 27(4):725—728, 1991.[82] S.L. Shah, C. Mohtadi, and D.W. Clarke. Multivariable adaptive control without aprior knowledge of the delay matrix. System and Control Letters, 9:295—306, 1987.[83] G.J. Bierman. Factorization Methods for Discrete Sequential Estimation. AcademicPress, 1977.[84] M.E. Salgado, G.C. Goodwin, and R.H. Middleton. Modified least squares algorithmincorporating exponential resetting and forgetting. International Journal of Control,47(2):477—491, 1988.[85] L. Ljung and T. Soderstrom. Theory and Practice of Recursive Identification. TheMIT Press Series in Signal Processing, Optimization, and Control. The MIT Press,Cambridge, Massachusetts, 1983.[86] International Mathematical and Inc. Statistical Libraries. IMSL Library: ReferenceManual. Houston, Texas, 1980.[87] W.C. Gear. Numerical Initial Value Problems in Ordinary Differential Equations.Prentice-Hall, Inc., Englewood Cliffs, NJ, 1971.[88] W.H. Press, B.P. Flannery, S.A. Teukoisky, and W.T. Vetterling. Numerical Recipes.Cambridge University Press, Cambridge, UK, 1986.[89] H.W. Mah. Private Communication, July 1991.[90] S. Chen and S.A. Billings. Representations of non-linear systems: The NARMAXmodel. International Journal of Control, 49(3):1013—1032, 1989.Appendix AEquations of Motion for Various Manipulator ConfigurationsThis appendix contains the equations of motion for several other configurations of manipulators which were studied during the course of the research. These equations were arrivedat by transforming equations (2.47) through (2.52), derived in section 2.4, as noted in eachsection.A.1 Planar Two Link Flexible Manipulator in Free MotionSetting each element of the set of coefficients {m, be, bex, b6,b3, kex, key, k, k3} to zeroand dropping the generalised coordinates e2, and,and equations (2.51) and (2.52) yieldsthe 2+n1+n2 equations of motion for a planar two link flexible manipulator in free motion.Rigid body equation for link 1:+ 102 + M2 (L + w0 +L12c2 +L2ioai)+ ‘21ii + I22j — 2 (Lis12 — wiocai2) I42)2i] i+ ‘11j1j + [M2 (L1 + L2Ca)— ::12I42i2]fli+ 102 + -JvI2L(L1ca2 + Wioai2)+ :2-2 5b— (Liai2 wioca12)I42çb23 14 c’jz1 j1+ [102 + M2L(Lic12 +W10Sa2)+I22j — (Lis12 —w10c2)EI422]+ I12j/’2j + [Liai2 + W10Sa2j3=1 3=1242Appendix A. Equations of Motion for Various Manipulator Configurations 243=— b811+gx (MiLisi + ci + M2L1s+ M2w10c+ M2L8ia12 +—gy (MiLici— si + M2Lici — M2w10s+ M2Lciai — 8iai2I422)+ { [2 (L18a12 — WloCai2) + 2(Lic12 + a2+412 [L2 (Lis12 — wlocai2) zL4()— (L2Sa12+ 2w10)Wio]—2I—2I22jb2j2j+2 [(L1Ca1 + WioSai2) Wo — Cai2Wio] I’ + 2 (Lis12 — wiocai2) I42i2}+ [M2L(Liai2 W10Ca2)+ (Lic::2 + W103a2) I422]+ PV12L(LSa2 — cai2wlo) z0 — 2 + 2 (L15a1 —c12wio)j=i+2 (L1c2 + Sai2Wlo)fl2I42J2J] 82+ [12L(Lia12 — Wi:Ca12)+ (Lic:12 + wio&ai)I42525]+2 (Lia12 — wi0c12) I42j’J2 — I22j’/J2j’/)2j ti4. (A.1)j=i 5=1Deflection equation for ith mode of link 1:+ [ (L1 + L2Cai) — sl2 iOI42525] &jo + [102+M2L(Lic12 + WloSai2) — (L13a2 — w1ocai2)I42525] i+I21iii + [M210 + (M2Lai S2 I42525)i1jo1iAppendix A. Equations of Motion for Various Manipulator Configurations 244+ [(M2Lc—a2 I42i?2) + + I22)5)]+[(:2Ll— aj2 1422) &jo + (102 + I22J) io]I2b+ Ca12lo3=1 2=1= —b1b— I31iI’1i+gs1I4j — 9yC1I4li+ {121i1i + [M2 (L2ai+ 10) + Cai2I422]iio— [M2L(Ljs12 —w10c2)+ (Liai2 + wiosai2)I42]0}+ ML2Sa12 + 2Cai2 I42j)2j 1o021. 3=1+2 [MLaio+ 8a12 I42j’2j + ai2 iio2 [MLaii:+I22j2j2 + io} Ôi+ -M2Lsai + Cc12>142ççb qi08j=1+2 { [MLaio& o + Sai2I42j2j + 1io—‘22j2 0 + M2Ls12 + a2 142j2 10Wj=1 ) j=1+2—.(A.2)Rigid body equation for link 2:[102 + M2L(Lic12 + WioSai2) + — (Lisa12 — wiocai2)I42]Appendix A. Equations of Motion for Various Manipulator Configurations 245+ [ZM2LCai— a22I42:b2i], [ +I22b]+ 102 + 122jh23 2 + 12j,2j=1 j=1= b6202+gx (M2Liai+ C1ai2I422)— gy (M2LCiai — S1ai2I422)+ [M2L(wioca2 Liai2) (Licai2 + Wio8ai2)I422]è— M2LSaii)io + 2cajzbio I42j’1/’2j + 2 6j=1 2=1+ 2). (A.3)Deflection equation for jth mode of link 2:[12j + (L1Ca12+s12w,0)I4]i+ca124j 1o1i + 12j jo1i + ‘12j2 +122j2j= —b2’ç—I32’b2+gxsla12I42j— gyClai2I42j + [I22ib — (Lis12 —w,0c12)I4]6+2 {I22j&2j2—s12I423w,o +I22bW0} +I22b(ti4 + O + 2ti40&). (A.4)A.2 Planar Two Rigid Link Manipulator in ContactSetting each of the coefficients {I1ki,I2k,I3ki,I4k:, bk2,i = 1,... ,k, k = 1, 2} to zero, dropping the generalised coordinates 1,... ,n,} and {i/2j,j = 1,... ,n2} and droppingequations (2.49) and (2.50) yields the equations of motion for a two rigid link manipulatorin contact with a deformable environment.Appendix A. Equations of Motion for Various Manipulator Configurations 246Rigid body equation for link 1:[i + 102 + M2 (L +L12c)+ m (L + L + 2L1Lc)]ë1+ [102 + M2L1c+ m (L +L12c)]ë2—m [Lisi + L2s12]ë + mi,, [L1c + L2c12]=—+b3 (Lisi + L2s12) [(Lisi + L2s12)a1 — L2s122——b3 (Lici + L2c12) [(Liei + L2c12)i +L2c120 — éj+k8 (Lisi + L2s12)(Lici + L2c12 —— Err)—k9, (Lici + L2c12)(Lisi + L2s12— Yw E)+gx (MiLiSi + M2L1s+ M2Lsi)— g1, (MiLici + M2L1c+ M2Lci)+L1L2s[(M2+ mc) 6 + (M2 + 2m) (A.5)Rigid body equation for link 2:[102 + M2L1c+ m (L +L12c)] öi + [I + mcL] 2—mL2sië+ mL2c12ë= T2 ——b3L2s12 [(Lisi + L2s12)Ô1 +L2s12& +—b3L2ci [(Lici + L2c12)01 — L2c126—+k8L2s12(Lici + L2c12———k8L2ci(Lisi + L2s12- 71w —+g-M2Lsi— g-M2Lci— (M2L+ mc) . (A.6)x direction contact equation:—m [Lisi + L2s12]8i — rncL2s122 + mEAppendix A. Equations of Motion for Various Manipulator Configurations 247=— kexEx—b8 [(Lisi + L2s12)&i +L2s126+ + k5 (Lici + L2c12 ——+m [Liciô + L2c (o + 9 + 28)]. (A.7)y direction contact equation:m [Lici + L2c12]8 — mL2c126+= beyEy — keyEy+b8 [(Lici + L2c12)i9 +L2c120—+ k (Lisi + L2s12— Yw — cy)+m [LisiÔ + L2s12 (ô + 8 + 2O12)] . (A.8)A.3 Planar Two Link Rigid Manipulator in Free MotionFurther operation on the set of equations in section A.2, namely setting each of the coefficients {m, bex, bey, bx, b3, kex, key, k8} to zero, dropping the generalised coordinatese and e, and dropping equations (A.7) and (A.8) yields the 2 equations of motion for aplanar rigid link manipulator in free motion.Rigid body equation for link 1:+ 102 + M2 (L +L12c)]1 + {z + M2Lic]82= Ti — b9181 +M2L1s[9 + 8182]+gx (MiLiSi + M2L1s+ M2Lsi)— gy (MiLiCi + M2L1c+ M2Lci) (A.9)Rigid body equation for link 2:[102 + M2LiC]81 + 10282 T2 — b822 + M2L(gxsl2 — gyc12) — M2L. (A.lo)Appendix A. Equations of Motion for Various Manipulator Configurations 248A.4 One Flexible Link in ContactThe 2 + n1 equations of motion for a single flexible link in contact are arrived at by settingthe coefficients {I2,1i2i, 122:, ‘32i, 142:, b2, bex, b8, kex, k5, i 1, . . . , n2} to zero, droppingthe generalised coordinates &2, {b23,j = 1,... ,n} and e and dispensing with equations(2.48), (2.50) and (2.51).Rigid body equation for link:+ m (L +0)] O + + mCLi 1o1j + m [Lici — wi0si] ë=— b811—b3 (Lici— WioSi) [(Lici — w10s)8i +i0ci—k3 (Lici — wi0si) (Lisi + w10c— 71w —+9x (MiLisi -t- Cl z1:4lbl) (M1LC—i )—2 (I2liiili + mcwioio) 6. (A.11)Deflection equation for ith mode of link:[liii + mLiioj + ‘2l:li + iioii +m10c= —bi1/)1—I3iil/’li—b8110ci [(Lici—w10s)8— + WioCi] —k8110c(Lisi + W10C— 71w —+ (gxSi — gyci) ‘41i + (I21b1+ mcwioqijo) 6 (A.12)y direction contact equation:m [Liei — w10s]9i + mc1 1jo1j + më= beyy — keyEy+b8 [(Lici — W103)8i—é + W10C1] + k8 (Lisi + W10C— 71w —+m (Lisi + W10C)8 + 2mzbi0si8i (A.13)Appendix A. Equations of Motion for Various Manipulator Configurations 249A.5 One Flexible Link in Free MotionThe 1 + n1 equations for the a single flexible link in free motion are obtained from theequations in section A.4 by setting the coefficients {m, bey, bsy, key, k3} to zero, droppingthe generalised coordinate E and dispensing with equation (A.13)Rigid body equation for link:+= T1— b911 + g (MiLisi + C I411)— gy (MiLici— 1 14ii)—2I21,b1b&. (A.14)Deflection equation for ith mode of link:111191 +121j’Ii = —b12’1 —I31b1.+ (gs1 — gci)I4i,+ I21ibn8 (A.15)A.6 One Rigid Link in ContactFrom the equations in section A.4, the 2 equations of motion for a single rigid link in contactare obtained by setting the coefficients{1111,23 i= 1,. ,ni} to zero, dropping thegeneralised coordinates {&11,i = 1,... ,m}, and dispensing with equation (A.12).Rigid body equation for link:+ mLj 8 + mL1cë= T1 — b811 —b8yLici (L1cÔ——k3L1c(Lisi Yw—e)+gx (MiLisi) — gy (MiLici) — 2mwi0i9. (A.16)y direction contact equation:mL1ci8 + më= beyy— keyEy + b8 (LiciÔi—+ k3y (Lisi— Yw—e) + mLisi8. (A.17)Appendix A. Equations of Motion for Various Manipulator Configurations 250A.7 DC Motor Actuated Single Flexible Link in Free MotionThis DC motor actuator model is coupled to the rigid body equation of motion for thelink being driven by adding the inertia, NIak, and damping, Ng2bak, terms from equation (2.68) to the corresponding terms in the rigid body equation and by using the gearedup motor torque, N9KTja, as the joint torque, ‘rk. In order to calculate this joint torque thedifferential equation for the armature current (2.65) must be solved simultaneously with theequations of motion since it involves the armature speed which is proportional to the jointrate. For example, the equations of motion for a single flexible link in free motion drivenby a DC motor are as follows.Rigid body equation for link:[101 + N1I01]ö + I11i1iNgiKriiai — bO1 + g (MiLiSi + C1 1 1i1i) — gy (MiLici— 3i I411)—2>I2ibibi8 . (AJ8)Deflection equation for link:Iii +I21b1.= —b1211 I31b1.+ (gzsi — gci)I4i.+I21b1 (A.19)Armature current equation for motor:Laijai + KernfNgll = Kv1 — Raijai (A.20)Appendix BCost Function ExpansionsThis appendix contains the details of the algebraic expansion and differentiation of the costfunctions defined for the control algorithms developed in Chapter 3.B.1 Multivariable Predictive Control AlgorithmFor the multivariable algorithm the cost function is the sum of squares of the optimalpredicted output errors, ê, plus the sum of squares of the control input increments, Uk, thelatter weighted by a positive factor, ):J = ê”êi + XcZU’Uk. (B.1)Expanding the first term using equation (3.31):n T flG2kUk+fw1 G2kUk+fw +cU11kkz1 k1 k1mt(,G1kuk) +(f_w)T +(f—w) +.AU’Ukk=1 k=1 k=1n, /n \ fn fn= (>U’G) (Gkuk) +2(UG) (f—w1)i=1 kzrl krl k1+(f_w)T (f-w)] +cilUk.==1(B.2)251Appendix B. Cost Function Expansions 252Differentiating with respect to Urn, an arbitrary member of the set of control input incrementvectors gives:= [G Gim]+2GL(fi_Wi)+2?icUkno ni no= 2 G Gk Uk + .\cUm + G (f, — w) . (B.3)=1 k—_iB.2 Static Equilibrium Bias TermThe static equilibrium bias term is the sum of squares of the differences between the actualcontrol inputs, Uk, and the calculated static equilibrium inputs, ük:T-Jse= >Z (ujc — Uk) (Uk — Uk). (B.4)k=1Substituting for Uk from equation (3.50):ft0 - T-Jse = (iL + Luk - Uk) (ukp + Lu,. — Uk)kzrl=(uj + uLT — u) + LUk Uk)=+ (u)2 ‘ + Ü Uj — 2ujtT Uk]. (B.5)Differentiating with respect to Urn, an arbitrary member of the set of control input incrementvectors yields:= 2 [LT L Uk + uLT — LT u]= 2 [LT L Uk + LT (uj — ü)] (B.6)Appendix CStatic Equilibrium Function DerivationThis appendix presents the details of the algebraic manipulations of the equations of motionand contact force model for a two flexible link manipulator in contact with a deformableenvironment 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 isused to obtain the static equilibrium bias term for the control algorithm cost function thatis derived in section 3.4.The static equilibrium relationships for the system are obtained by setting to zero allof the time derivatives in the equations of motion ((2.47) through (2.52)). From the rigidbody equations this yields, for link 1:o = T1+k8(L1s+ w10c +L2s112 + w2oclai2) (Lici — wiosi + L2c1a1 — W2o81a2———k3 (Liei— wiosi +L2c112 — W2031a12)(Lisi + wioci + L251a1 + W2oClai2— Yw—(C.1)and, for link 2:o = r2 + k (L2si12 + W2oClai2) (Lici — W10S + L2C1a1 W2o51a2—Xw—k5 (L2c1a1 — W2081a12)(L1s + W10C + L251a1 + W2OC1a1— 71w — en). (C.2)The deflection equations for the ith mode of link 1 and the jth mode of link 2 yield:253Appendix C. Static Equilibrium Function Derivation 254o—+k8(q10s +L2s112q0+ w2oc1aj2q50)(Lici — Wiosi + L2Clai2 — W2o91a2———k5(q1joci +L2ci1260— W2oS1ai2q.o) (Lisi + Wioi + L281a1 + W2oClai2— Yw —(C.3)ando = I321b2+ks3çb2jos1a1(Lici — W1o. + L2C1a1 — W2oSlcx2 —— e)ksy42joC1ai2 (Lisi + wioci + L231a1 + W2OC1c11— Yw—Ey). (C.4)Finally, the static equilibrium relationships obtained from the x and y direction contactequations are:o = —k6e+ k8 (Lici — w10s + L2C1a1 — W2oS1c2 — Xw— Ex) (C.5)ando = keyEy + (Lisi + W10C + L2S1a1 + W2oCh1— Yw — eu). (C.6)Setting the time derivatives in the contact force model (equations (2.56) and (2.57)) tozero yields:rn3 meF kezEx + k8 — — (C.7)andF!, = keyEy + k3(Ytip — Yw en), (C.8)Tfl::where the world frame tip position components are given byL1c— w10s + L2C1a1 — W2o51a2 (C.9)andYtip L1s + W10C + L2S1cz1 + W2oClai2. (C.1O)Appendix C. Static Equilibrium Function Derivation 255The static equilibrium defiections of the contact region mass are obtained by substitutingequations (0.9) and (0.10) into equations (0.5) and (C.6) and solving for e and e:=(xj— x) (0.11)andksy+key (Ytip — yw). (0.12)With these results e and Ey can be eliminated from the contact force model to giveF = kxeff (xt — x) (0.13)andF = kyeff (Ytip yw). (0.14)Substituting (C.11) through (0.14) into (0.1) and (0.2) and solving for r1 and r2:T1 = F (Lici — wi0s + L2C1a2 — W2oS1a2) — F (L1s + wc1 + L231a1 + w2oc1a2)(0.15)and= F (L2C1a1 — w2081a12) — F, (L2S1a1 + W2OC1a12). (0.16)Similar substitutions into (0.3) and (0.4) yield the following expressions for the staticequilibrium modal amplifiers:F [4)i3i + (L231a1 + w2jocla12)4] — F [4i0ci + (L2C1a1+w20si12)44cJ(0.17)andI31/)1i Fxq2jos1a12— Fyb2jocia12. (0.18)These expressions are not explicit relationships between F and F and ib1 and b2 becauseof complications which arise from the link 1 tip slope terms (c and Equations (0.17)Appendix C. Static Equilibrium Function Derivation 256and (C.18) could, in principle, be solved by simultaneous iteration to give static equilibriumvalues of bh and /‘ for given contact forces. However, assuming the tip deflection of link 1 issmall compared to its length a will be small and the slope terms can be neglected, resultingin the following approximate expressions for the static equilibrium modal deflections:1j (F10si —F0c1) (C.19)131iandb2—-—(F20s12 — Fb20c12). (C.20)‘32jThe same approximation applied to equations (C.15) and (C.16) gives:F (Lid — i0’1s+ L2c12 + 2io28i2)— F (Lisi—0ici + L2s12 + (C.21)andT2 F (L2di2— 22O2s12) — F (L2s12 + . (C.22)These final four equations provide a close approximation to the desired relationship betweenthe contact force level setpoints and the joint torques required to maintain them under staticequilibrium conditions. Note that in the case of a single link the complications due to thelink tip slope terms do not arise and exact expressions can be obtained.
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Force control for robotic manipulators with structurally...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Force control for robotic manipulators with structurally flexible links Latornell, Douglas John 1992
pdf
Page Metadata
Item Metadata
Title | Force control for robotic manipulators with structurally flexible links |
Creator |
Latornell, Douglas John |
Date Issued | 1992 |
Description | 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 (MSS) 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 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. |
Extent | 6105955 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
FileFormat | application/pdf |
Language | eng |
Date Available | 2008-12-17 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
IsShownAt | 10.14288/1.0081044 |
URI | http://hdl.handle.net/2429/3013 |
Degree |
Doctor of Philosophy - PhD |
Program |
Mechanical Engineering |
Affiliation |
Applied Science, Faculty of Mechanical Engineering, Department of |
Degree Grantor | University of British Columbia |
GraduationDate | 1992-11 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
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
Cite
Citation Scheme:
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>
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