UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Simulation and adaptive control of a robot arm Marchand, Pauline Anne 1985

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

Item Metadata

Download

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

Full Text

SIMULATION AND ADAPTIVE CONTROL OF A ROBOT ARM  by PAULINE ANNE MARCHAND B.Eng., M c G i l l U n i v e r s i t y , 1980  A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF  MASTER OF APPLIED. SCIENCE  in THE FACULTY OF GRADUATE STUDIES Department of Mechanical  We accept  t h i s t h e s i s as  to t h e r e q u i r e d  Engineering  conforming  standard  THE UNIVERSITY OF BRITISH COLUMBIA June, 1985  © PAULINE ANNE MARCHAND, 1985  In p r e s e n t i n g  t h i s t h e s i s i n p a r t i a l f u l f i l m e n t of  requirements f o r an advanced degree a t the  the  University  o f B r i t i s h Columbia, I agree t h a t the L i b r a r y s h a l l make it  f r e e l y a v a i l a b l e f o r reference  and  study.  I  further  agree t h a t p e r m i s s i o n f o r e x t e n s i v e copying o f t h i s t h e s i s f o r s c h o l a r l y purposes may department o r by h i s o r her  be granted by the head o f representatives.  my  It i s  understood t h a t copying or p u b l i c a t i o n o f t h i s t h e s i s f o r f i n a n c i a l gain  s h a l l not be  allowed without my  permission.  Department O f  Mechanical  Engineering  The U n i v e r s i t y o f B r i t i s h Columbia 1956 Main Mall Vancouver, Canada V6T 1Y3 Date  DE-6  13/81)  May 31, 1985  written  ABSTRACT  The equations of motion describing a robot's dynamics are coupled and nonlinear, making the design of an optimum c o n t r o l l e r d i f f i c u l t using c l a s s i c a l techniques.  In t h i s work an e x p l i c i t adaptive control law i s  proposed based on a discrete l i n e a r model f o r each l i n k and on the minimization of a quadratic performance c r i t e r i o n .  The system parameters  are recursively estimated at each c o n t r o l step using l e a s t squares.  A  computer simulation of the resulting scheme i s performed to evaluate the controller.  The simulation model i s based on the f i r s t three l i n k s of an  existing robot, includes motor dynamics and treats the wrist assembly as a load mass.  Simulated test paths requiring movement of the outer two  links  indicate that the controller adapts and that i t s behaviour i s stable and convergent.  - ii  -  TABLE OF CONTENTS Page  ABSTRACT  i i  TABLE OF CONTENTS  i i i  LIST OF TABLES  v  LIST OF FIGURES  vi  NOMENCLATURE  v i i  ACKNOWLEDGEMENTS  xi  CHAPTER 1  INTRODUCTION 1.1 1.2 1.3  2  4  1 3 5  ADAPTIVE CONTROL STRATEGY 2.1 2.2  3  Preliminary Remarks L i t e r a t u r e Review Scope and Objectives  Preliminary Remarks Design of the C o n t r o l l e r 2.2.1 Admissible Controls 2.2.2 D i s c r e t e Model Structure 2.2.3 Control Law 2.2.4 I d e n t i f i c a t i o n Procedure 2.2.5 Replacement of True Parameters  14 15 16 16 21 26 29  THE SIMULATION MODEL 3.1 3.2  The RSI Robot Kinematics 3.2.1 D i r e c t Kinematics 3.2.2 Inverse Kinematics 3.2.3 Trajectory Planning  36 37 38 42 45  3.3  Dynamics 3.3.1 Actuators 3.3.2 Manipulator Dynamics 3.3.3 Simulation Equations  46 46 47 54  3.4  Summary  56  EVALUATION OF THE CONTROL STRATEGY 4.1  One Degree-of-Freedom Case 4.1.1 Results and Discussion  68 70  4.2  RSI Robot Simulation 4.2.1 Results and Discussion  73 75  - iii -  TABLE OF CONTENTS (Continued) Page  5  CONCLUSIONS AND RECOMMENDATIONS FOR FUTURE WORK 5.1  Conclusions  140  5.2  Recommendations f o r Future Work  141  BIBLIOGRAPHY APPENDIX  I II III IV V VI  144 RECURSIVE LEAST SQUARES THE A AND T MATRICES THE PSEUDO INERTIA MATRIX J DERIVATION OF THE LOAD TERM L J FOR THE RSI ROBOT SETTING UP THE INTEGRATION ROUTINE ±  ±  ±  - iv-  148 155 157 159 162 164  LIST OF TABLES Page  Table I  Parameter Values of the RSI Robot  32  II  Kinematic Parameters of the RSI Robot  33  III  D i s c r e t e Model Parameters and C o e f f i c i e n t s f o r the Known Case  83  IV  Test Paths f o r the Single-Link Simulation  84  V  Run Parameters f o r the Single-Link Simulation  85  VI  C i r c l e Trajectory - Parameters of the Simulation Runs ....  86  VII  Line at 45° - Parameters of the Simulation Runs  87  VIII  H o r i z o n t a l L i n e - Parameters of the Simulation Runs  88  -  v  -  LIST OF FIGURES Page  Figure 1  Trajectory f o r a two-link arm  2  Schematic of robot c o n t r o l system  10  (a)  joint control  11  (b)  endpoint c o n t r o l  11  3  Robot coordinate frames  12  4  Schematic of the simulated system  13  5  Schematic of a MRAS  34  6  Schematic of a STR  35  7  The RSI robot  58  8  P o s i t i o n of a point r e l a t i v e to two frames  59  9  Link coordinate frames f o r the RSI robot  60  10  Vector d e f i n i t i o n s f o r the inverse kinematics  61  11  Workspace of l i n k s 2 and 3 of the RSI robot  62  12  Trapezoidal v e l o c i t y law  63  13  D i r e c t i o n cosines f o r endpoint t r a j e c t o r y  64  14  Schematic of motor, drive t r a i n , and l i n k  65  15  Schematic of single-degree-of-freedom case  16  Program flowchart - known case, one degree of freedom  17  Program flowchart - Adaptive case, one degree of freedom .. 91  18  Known c o n t r o l - e f f e c t of varying e, T = 0.010 s  92  19  Known c o n t r o l - e f f e c t of varying  94  - vi -  e, T = 0.020 s  89 90  LIST OF FIGURES (Continued) Page  20  E f f e c t of disturbance  95  21  Feedforward i n a p r o p o r t i o n a l c o n t r o l l e r  97  22  Adaptive c o n t r o l - no disturbance  98  23  Known vs. adaptive c o n t r o l , step inputs  99  24  Known vs. adaptive c o n t r o l , ramp input  100  25  Known vs. adaptive c o n t r o l , trapezoid  101  26  E f f e c t of zero i n i t i a l estimates  102  27  Program flowchart - Adaptive case, RSI robot  103  28  Configuration f o r the C i r c l e Path  104  29  C i r c l e simulations - e f f e c t of loads  105  30  C i r c l e simulations loads - e f f e c t of A  107  31  C i r c l e simulations - e f f e c t of i n i t i a l estimates  108  32  Configuration f o r the 45° l i n e  109  33  45° l i n e simulations  110  34  Configuration f o r the h o r i z o n t a l l i n e  115  35  H o r i z o n t a l l i n e simulations  116  - vii -  NOMENCLATURE  Subscripts i  r e f e r s to the i  joint  d  desired, disturbance  m  motor  act  actuator  1  load  a,b  continuous t r a n s f e r f u n c t i o n v a r i a b l e s  aj ,a ,bj^ ,b ,h  d i s c r e t e model parameters  02,02,03,0^,05  control coefficients  g  gravity  m  mass  n  gear r a t i o , order of system  2  2  x  2  l o c a t i o n of centre of mass, l i n k 2  x  3  l o c a t i o n of centre of mass, l i n k 3 l o c a t i o n of mass, load  u  c o n t r o l input  y  system output  y^  reference input  k  sampling i n s t a n t  q  s h i f t operator  t  time  v  velocity  a  acceleration  - viii -  T  sampling period  K  c o n t r o l input constant  K^,  motor constant  K  back emf constant  e  K  k i n e t i c energy  L  Lagrangian  P  p o t e n t i a l energy  R  armature r e s i s t a n c e  I  performance c r i t e r i o n  D  c o e f f i c i e n t i n manipulator equations of motion  J  inertia  C  damping constant  W  weighting on the c o n t r o l  X  forgetting factor  e  pseudo-gain  a  d i r e c t i o n cosine  B  d i r e c t i o n cosine  6  j o i n t angle  x  torque  E{}  -  expected value  G(s)  continuous time t r a n s f e r f u n c t i o n  G(z)  d i s c r e t e time t r a n s f e r  e(k)  white noise  v(k)  disturbance sequence  function  - ix -  Vectors and matrices are i n bold type z  cartesian position  p^(k)  desired endpoint p o s i t i o n  *r  p o s i t i o n vector r w i t h respect t o i ' *  u  c o n t r o l input  y  system output (angular p o s i t i o n )  y.  reference input (angular p o s i t i o n )  g  gravity  J  pseudo-inertia matrix  T  transformation matrix  A  transformation matrix  Y  output measurement vector  p  centre of mass  6  parameter vector  4>  measurement vector  4  information matrix  d  • ARMA  autoregressive moving average  STR  self-tuning regulator  MRAS  model reference adaptive c o n t r o l  - x -  1  coordinate frame  ACKNOWLEDGEMENTS  The author would l i k e to express thanks to Mr. Alan Steeves f o r assistance with the VAX computer and to Ms. Jocelyn Hung f o r help with the plotting. The guidance and encouragement from her supervisors, Professor Peter D. Lawrence and Professor Dale B. Cherchas, were greatly appreciated. This research was supported by the Department of E l e c t r i c a l Engineering and the Department of Mechanical  - xi -  Engineering.  1.  CHAPTER 1 INTRODUCTION  1.1  Preliminary Remarks Present-day robots have l i m i t e d c a p a b i l i t i e s .  They are generally used  for executing r e p e t i t i v e tasks.  They have high weight-to-payload r a t i o s  and require powerful actuators.  Because of the v a r i a b l e s t r u c t u r e of the  robot the c o n t r o l l e r s which regulate the j o i n t movements are designed f o r the worst case.  Thus, l e s s than optimum performance i s achieved at the  other operating conditions.  In t h i s work an adaptive c o n t r o l l e r which  can adjust to changes i n the system dynamics i s proposed, and i t s behaviour i s studied by computer s i m u l a t i o n . An example of a change i n the system dynamics as the robot moves i s e a s i l y seen by considering F i g . 1 which i l l u s t r a t e s a simple two-link  arm,  i n i t i a l l y i n p o s i t i o n A, whose t i p traces a s t r a i g h t - l i n e path u n t i l p o s i t i o n B i s reached.  The t o t a l i n e r t i a f e l t by the motor d r i v i n g the  f i r s t l i n k w i l l change because as the second l i n k moves, the distance between i t s centre of mass and the j o i n t of the f i r s t l i n k changes. The disturbances a c t i n g on each l i n k a l s o vary.  G r a v i t y loads depend  on p o s i t i o n . V e l o c i t y e f f e c t s such as c e n t r i p e t a l forces are present and are v a r i a b l e as w e l l .  Robots have been designed f o r slow speeds so that  these disturbances are r e l a t i v e l y small. A schematic of a j o i n t c o n t r o l system i s shown i n F i g . 2 ( a ) .  Assuming  that the desired j o i n t angular p o s i t i o n as a f u n c t i o n of time, 6,(t) i s d  known, the idea i s to compare i t to the measured j o i n t p o s i t i o n 8 ( t ) and form an e r r o r s i g n a l . Using knowledge of the system dynamics, a c o n t r o l law operating on the e r r o r i s designed so that c e r t a i n performance s p e c i -  2.  f i c a t i o n s are met and disturbances, x,, are r e j e c t e d s a t i s f a c t o r i l y . d  the system dynamics change, the c o n t r o l law i s no longer optimum.  When  I t would  be advantageous i f the c o e f f i c i e n t s of the c o n t r o l l e r could be v a r i e d on-line as the system parameters and the disturbances changed.  These  considerations have l e d to the study of more complex c o n t r o l laws, and motivate the present work. A second motivation i s that t e c h n o l o g i c a l advances i n sensor  develop-  ment and information processing are beginning to make more s o p h i s t i c a t e d control strategies feasible.  F i g . 1 may be examined again.  In order that  the endpoint of the arm f o l l o w the s t r a i g h t l i n e path, i t i s necessary that the movements of both l i n k s be p r e c i s e l y coordinated.  The j o i n t  c o n t r o l l e r s each i n d i v i d u a l l y tend to d r i v e the e r r o r between the desired angle and the measured angle of t h e i r l i n k to zero.  In order to o b t a i n  good endpoint p o s i t i o n a l accuracy, not only must the j o i n t c o n t r o l l e r s be adequately designed, but f l e x i b i l i t y i n the l i n k s and backlash i n the gear t r a i n must be absent. motors.  R i g i d l i n k s imply heavier designs and hence powerful  Payloads must be l i g h t r e l a t i v e to the c o n s t r u c t i o n of the robot  to prevent d e f l e c t i o n of the l i n k s and to avoid introducing extra g r a v i t y disturbances which would d i s r u p t the c o n t r o l l e r s .  I f , however, i t was  possible to measure the p o s i t i o n of the endpiece and use that measurement f o r feedback, the above design l i m i t a t i o n s could be e l i m i n a t e d .  This  approach i s termed endpoint c o n t r o l and i s schematically i l l u s t r a t e d i n Fig.  2(b).  One d i f f i c u l t y i s that the feedback measurement i s coded i n  terms of a f i x e d base reference frame whereas the c o n t r o l a c t i o n takes place r e l a t i v e to j o i n t space.  This idea i s i l l u s t r a t e d i n F i g . 3.  P i s uniquely defined by the vector p a i r ( x , y ) .  Point  By f i x i n g a coordinate  frame on each l i n k , such as ( x j - y , ) on l i n k 1, i t i s p o s s i b l e to f i n d the  3.  r e l a t i o n s h i p between (x,y) and ( 8 ^ 8 2 ) . not unique.  The mapping 6 -*• x , however, i s  F i g . 3 shows the "elbow up" s o l u t i o n .  s t r a t e g i e s must deal with these issues.  Endpoint c o n t r o l  The i n t e n t of t h i s study was  to  formulate a c o n t r o l scheme which might be s u i t a b l e for extension to endpoint c o n t r o l , and would f u n c t i o n adaptively at the j o i n t l e v e l .  1.2  L i t e r a t u r e Review Several researchers have tackled the two problems mentioned above.  There are b a s i c a l l y two approaches i n the design of these new  advanced  control strategies.  The f i r s t approach i s to obtain a more s o p h i s t i c a t e d  model of the system.  Using that information the required c o n t r o l inputs  may  be a n t i c i p a t e d .  the system.  The second approach presumes much l e s s knowledge of  Instead, input-output data from the system i s used to i n f e r  i t s s t r u c t u r e and to provide adequate c o n t r o l .  A short d e s c r i p t i o n of some  of the work done i n these areas f o l l o w s , as i t applies to r o b o t i c s . Whitney [1] proposed resolved rate c o n t r o l to c o n t r o l j o i n t v e l o c i t i e s obtained from hand or endpiece p o s i t i o n s and v e l o c i t i e s . paper.  His i s a c l a s s i c  Luh, Walker and Paul [2] propose a resolved a c c e l e r a t i o n technique  to close the loop around the hand of the robot by p r e - c a l c u l a t i n g the required torques given desired angular p o s i t i o n , v e l o c i t y and tions.  accelera-  This method requires a d e t a i l e d model of the manipulator.  Freund  [3] has suggested nonlinear compensations to decouple the equations of motion of the manipulator.  G o l l a , Garg, and Hughes [4] proposed l i n e a r  m u l t i v a r i a b l e c o n t r o l f o r assigning the closed loop poles of the system by state feedback.  Cvetkovic and Vukobratovic [5] examine several two-stage  c o n t r o l schemes where the c o n t r o l s i g n a l c o n s i s t s of a nominal preprogrammed value and a compensating term.  Vukobratovic and Stokic  [6]  4.  study a few c o n t r o l s t r a t e g i e s which take dynamics i n t o account i n d i f f e r ent degrees.  Hogan and Cotter [7] have suggested Cartesian impedance  c o n t r o l f o r c l o s i n g the loop around the hand. In general the above techniques require a d e t a i l e d model of the robot. Modelling e r r o r s would lead to decreased e f f e c t i v e n e s s of the c o n t r o l laws. A second disadvantage of these methods i s that they t y p i c a l l y require large amounts of c a l c u l a t i o n between the sampling i n s t a n t s when a c o n t r o l s i g n a l i s downloaded to the j o i n t actuator.  Other researchers have taken the view  of using an adaptive c o n t r o l l e r , which pre-supposes much l e s s knowledge of the system.  Lee [8] proposes that a pre-calculated nominal torque be  supplemented by inputs c a l c u l a t e d from i d e n t i f i c a t i o n of the perturbed system about the nominal t r a j e c t o r y .  B a l e s t r i n o et a l . [9] proposed a  model reference approach and design the c o n t r o l law using h y p e r s t a b i l i t y theory.  Only the bounds on the parameters need to be known.  Further study  of the model reference technique i s done by N i c o s i a and Tome! [10], who i n v e s t i g a t e convergence.  Recently, Kim and Shin [11] have described a  method of averaged dynamics.  Young [12] proposed s l i d i n g mode c o n t r o l  based on the theory of v a r i a b l e structures systems, i n which a s i m i l a r requirement of knowledge of only the bounds of the parameters i s needed. The present work i s based on that of Koivo and Guo [13] and motivated by that of Lobbezoo [14]. Lobbezoo showed that f o r the case of a twodegree-of-freedom robot, good performance was achieved by estimating the transformation matrix x •*• 8 rather than using the a n a l y t i c a l formulation.  He used r e c u r s i v e l e a s t squares to i d e n t i f y the e n t r i e s of the  transformation matrix.  Koivo and Guo proposed that the robot system be  modelled by an autoregressive moving average (ARMA), and the parameters  5.  r e c u r s i v e l y i d e n t i f i e d using the l e a s t squares technique.  A c o n t r o l law  based on a quadratic performance c r i t e r i o n i s derived and the estimated parameters are s u b s t i t u t e d i n t o t h i s law at each c o n t r o l step. a system model structure based on e m p i r i c a l t e s t s .  Koivo chose  I n t h i s work an  a n a l y s i s of the continuous-time system representation i s done and i t i s shown why a successful d i s c r e t e model has the structure given.  The same  c o n t r o l law design i s followed, but the c o n t r o l l e d v a r i a b l e was chosen as angular p o s i t i o n rather than angular v e l o c i t y , since Koivo's method necess i t a t e d an ad-hoc adjustment f o r p o s i t i o n e r r o r s .  Angular p o s i t i o n was  chosen also because, i f the c o n t r o l l e r was successful, as an avenue f o r future i n v e s t i g a t i o n , i t was structured so that the transformation  from  base coordinates to j o i n t coordinates could be included i n the adaptation.  1.3  Objectives  and Scope of the Study  The motivation behind the present study was presented i n Section 1.1. The general objective was to i n v e s t i g a t e and test v i a computer simulation a c o n t r o l law f o r improved j o i n t c o n t r o l . the design i s discussed  next.  Formulation of the c r i t e r i a f o r  The choice of a c o n t r o l strategy depends on  many f a c t o r s and c o n s t r a i n t s , since performance must be s p e c i f i e d w i t h i n a complex framework of i n t e r a c t i n g and sometimes c o n f l i c t i n g goals. For example, performance can c r i t i c a l l y depend on an e f f i c i e n t t r a j e c tory planner.  Each robot j o i n t has l i m i t s of a c c e l e r a t i o n and v e l o c i t y ; a  s o p h i s t i c a t e d t r a j e c t o r y planner ensures that the commanded sequence of 6  r e s u l t s i n 6 and 8  w e l l w i t h i n these bounds, and a l s o ensures  smooth t r a n s i t i o n s between path segments.  When these conditions are met,  l e s s rigorous demands are placed on the j o i n t actuators and t h e i r controllers.  The r e l a t i o n s h i p between t r a j e c t o r y planning and c o n t r o l l e r  6.  requirements i s not studied here, but i t i s one i l l u s t r a t i o n that performance of the robot depends on a host of f a c t o r s . However, i t i s p o s s i b l e t o e s t a b l i s h a framework f o r the design and f i v e broad c r i t e r i a were s p e c i f i e d f o r the design of the c o n t r o l l e r .  These  are (1) the formulation would be d i g i t a l , (2) an adaptive technique would be used, (3) the structure would permit extension to endpoint c o n t r o l , (4) the s i m u l a t i o n model would be r e a l i s t i c , and (5) the c o n t r o l l e r would be geared to large motions (as opposed to adjustment or assembly).  These f i v e  points are now discussed. The great m a j o r i t y of the newer approaches t o robot c o n t r o l a r e , on the t h e o r e t i c a l l e v e l , treated using continuous time techniques.  This i s  probably due t o the r e l a t i v e newness of d i g i t a l c o n t r o l and hence l a c k of f a m i l i a r i t y with i t .  The behaviour of the system under c o n s i d e r a t i o n - the  robot - i s of course described by continuous-time equations.  C o n t r o l laws  are designed from system models, and many of the proposed laws are by extension derived i n continuous-time form [2,3,4,9,10].  However, i t i s  i m p l i c i t that the implementation of these c o n t r o l laws must be done d i g i t a l l y , i . e . , the c a l c u l a t i o n of the c o n t r o l s i g n a l s i s done on a microcomputer and at a prescribed sampling rate the signals are downloaded to a d i g i t a l - t o - a n a l o g converter and on t o the actuators of the j o i n t s .  Issues  such as the e f f e c t of sampling time and computational delays are impossible to analyze given the present formulation of these c o n t r o l laws.  I t seems  to make more sense to use d i g i t a l theory i n the f i r s t place f o r the design of the c o n t r o l l e r .  This i s the approach chosen i n t h i s work.  The second c r i t e r i a i s adaptation, and i t r e f e r s to the class of c o n t r o l laws from which the strategy studied i n t h i s work i s chosen.  As i s  pointed out i n the l i t e r a t u r e review, there are b a s i c a l l y two philosophies  7. behind the advanced c o n t r o l laws which have been studied f o r robots.  One  of the approaches i s to improve the system model used i n the design of the controller.  Because the equations d e s c r i b i n g the system are coupled and  non-linear, large numbers of c a l c u l a t i o n s are t y p i c a l l y required i n the c o n t r o l law.  The goal i s t o compensate using exact knowledge of the  system, which i n p r a c t i c e i s not achievable.  The second approach i s to use  some form of adaptive c o n t r o l , which presupposes much l e s s knowledge of the system, and which o f f e r s a p o s s i b i l i t y f o r compensating f o r changing system parameters.  The survey paper by Astrom [15] i s a good overview of adaptive  c o n t r o l , covering many of the issues i n the f i e l d and l i s t i n g many references.  In t h i s work an adaptive c o n t r o l strategy s i m i l a r t o the one  proposed by Koivo [13] i s studied. The t h i r d o b j e c t i v e which the c o n t r o l l e r had t o meet was that i t s structure be chosen with an eye to future developments, so that sensory information from the environment could be added to i t .  In p a r t i c u l a r ,  incorporating Lobbezoo's adaptive transformations to attempt endpoint c o n t r o l was considered a p o s s i b i l i t y . Computer s i m u l a t i o n i s a u s e f u l v e h i c l e of experimentation as the f i r s t step i n evaluating a c o n t r o l l e r .  The costs and p i t f a l l s of hardware  implementation are avoided a t the i n i t i a l design stage. Simulation i s used i n t h i s work to t e s t the c o n t r o l l e r .  However, an e x i s t i n g robot i s used as  the prototype f o r b u i l d i n g the s i m u l a t i o n model, rather than a f i c t i t i o u s robot of a r b i t r a r y c h a r a c t e r i s t i c s .  In a d d i t i o n , the actuators are  included i n the system model. The l a s t c r i t e r i o n i s that the design be made f o r "large" motions. The design and performance of a j o i n t c o n t r o l l e r i s influenced by the type of task performed by the robot.  One can c l a s s i f y operating modes i n t o  8. three groups: i)  large motions,  II)  f i n e motions,  III)  assembly tasks.  Assembly tasks r e q u i r e that compliance and force feedback be considered, and are not studied here.  In f i n e motions, s i m p l i f y i n g assumptions of slow  speeds and l i n e a r i z a t i o n s about the operating point may be made which change the focus of the a n a l y s i s .  This study i s geared to large motions,  which include a few types. The task objective can be t r a c k i n g of a path, object tracking ( f o r example conveyor b e l t following) or simply moving from point A t o B. Here some point-to-point motion i s considered as w e l l as path t r a c k i n g .  These c r i t e r i a are to be met w i t h i n the broad objective of  improved j o i n t c o n t r o l . I t i s evident that attempts t o study a six-degree-of-freedom robot without having acquired expertise on lower orders of magnitude would r e s u l t i n the major trends being obscured by secondary e f f e c t s . T h e scope of the work i s l i m i t e d to three-degrees-of-freedom. Dynamically speaking, as f a r as the f i r s t three l i n k s are concerned, i n many cases a good approximation i s to treat the w r i s t assembly as a point mass.  This i s the approach taken  here: the six-degree-of-freedom system i s s p l i t i n t o two subsystems.  The  lower subsystem, that i s the f i r s t three l i n k s , i s analyzed. The simulat i o n i s c a r r i e d out f o r two l i n k s since i t was f e l t that the d i s t i n g u i s h i n g features of the c o n t r o l law can be i l l u s t r a t e d by that case.  For the  chosen robot the dynamic e f f e c t s of the second and t h i r d l i n k s upon the f i r s t are minor compared to the coupling between the second and t h i r d links. The system t o be studied i s i l l u s t r a t e d i n schematic form i n F i g .  4.  /  \  TRAJECTORY FOR A TWO-LINK ARM  10.  a)  )  *  Joint Controller  u  Motor  Joint  b) Robot  End Point Controller  u  Motors  Joints  Fig.2 SCHEMATIC OF ROBOT CONTROL SYSTEMS a) joint control b) endpoint control  6  Position Sensor  Fig.3  ROBOT ARM COORDINATE SYSTEMS  User path I parameters Trajectory Planner  x (k) 4  Inverse Kinematics  Adaptive Controller  Fig. 4 SCHEMATIC OF THE SIMULATED SYSTEM.  u(k)  11 1 .  r  l i  Actuators  T(t)  Robot Arm  1^ i i  Position Sensor "Camera"  13. Chapter 2 covers the design of the c o n t r o l l e r block.  Trajectory planning,  inverse kinematics, and the "camera" area are treated i n 3.2, while the dynamics model of the robot i s developed i n 3.3. r e s u l t s are discussed i n Chapter 4.  The s i m u l a t i o n set-up and  Chapter 5 summarizes the conclusions  which may be drawn from the i n v e s t i g a t i o n , and suggests d i r e c t i o n s f o r further study.  14.  CHAPTER 2 ADAPTIVE CONTROL STRATEGY  2.1 P r e l i m i n a r y Remarks A p a r t i c u l a r choice of c o n t r o l strategy i s now formulated given the goals discussed i n the I n t r o d u c t i o n . The objectives are to design, using d i g i t a l techniques and i n a r e a l i s t i c framework, an adaptive c o n t r o l law for  j o i n t c o n t r o l which could be reformulated to include sensory informa-  t i o n from the environment, f o r example i n the form of p o s i t i o n feedback from the hand of the robot.  The common s i x degree-of-freedom  separated i n t o two three degree-of-freedom  subsystems.  robot i s  One of these sub-  systems, c o n s i s t i n g of the f i r s t three l i n k s , i s examined.  The proposed  c o n t r o l l e r i s tested by computer s i m u l a t i o n of the dynamic system. In order to use d i g i t a l techniques to analyze a continuous-time system, i t i s necessary t o sample the continuous time s i g n a l s .  This means  that the continuous f u n c t i o n f ( t ) i s replaced by i t s value at d i s c r e t e time instants.  I f the sampling period i s T, the f u n c t i o n f ( t ) i s represented by  f ( k T ) , where k i s the sampling i n s t a n t , k = {...-1,0,1,...}. For s i m p l i c i t y the n o t a t i o n w i l l be f ( k ) . R e f e r r i n g again t o F i g . 4 , a c l o s e r look i s taken a t the adaptive c o n t r o l l e r block.  The c o n t r o l l e r has two inputs: a d i s c r e t e sequence of  desired j o i n t angle v e c t o r s 6 ^ ( k ) , and a d i s c r e t e sequence of observed system output vectors 6 ( k ) . Except as noted otherwise, i n t h i s chapter the reference input 8<i(k.) w i l l be denoted by y ^ ( k ) and s i m i l a r l y the measured output v a r i a b l e w i l l be denoted y ( k ) , i n order to conform with standard n o t a t i o n i n the c o n t r o l s l i t e r a t u r e . c o n t r o l l e r has one output, the computed v a r i a b l e u ( k ) .  The  Ways of  organizing the c a l c u l a t i o n s i n an adaptive framework are now examined.  15.  There are b a s i c a l l y two approaches: the Model Reference Adaptive System (MRAS) and the Self-Tuning Regulator (STR).  Each arose independ-  e n t l y but i t has been shown that i n some cases the methods are equivalent, for example Landau [16] and Egardt [17]. F i g . 5 shows a block diagram of a MRAS. The heavy l i n e s form an ordinary feedback system.  Performance  s p e c i f i c a t i o n s are i m p l i c i t l y s p e c i f i e d by a reference model chosen by the designer.  The i d e a l output y  m  of the reference model i s compared to the  output y of the system, and the error i s used to adjust the parameters of the c o n t r o l l e r .  This outer loop i s the adaptive part.  This s t r u c t u r e has  been c a l l e d d i r e c t because the c o n t r o l l e r i s adjusted d i r e c t l y . approach, STR, i s i l l u s t r a t e d i n F i g . 6.  An ordinary feedback loop i s  present here, too, again i d e n t i f i e d by heavy l i n e s . adjustment mechanism. the system parameters.  The second  The other loop i s the  Input-output data of the system are used to i d e n t i f y Then, the estimated system parameters are used i n  the c o n t r o l l e r design block to c a l c u l a t e the c o n t r o l l e r parameters. i s an e x p l i c i t STR, because the system i s i d e n t i f i e d e x p l i c i t l y .  This  It is  sometimes p o s s i b l e to organize the c a l c u l a t i o n s such that the mapping from system i d e n t i f i c a t i o n + c o n t r o l l e r parameters i s t r i v i a l ; t h i s i s then an implicit  2.2  STR.  Design of the C o n t r o l l e r The component parts of the design are now addressed.  A STR approach  was chosen f o r t h i s study, c l o s e l y f o l l o w i n g the work of Koivo and Guo [13], but i n contrast to that work the c o n t r o l l e d v a r i a b l e i s chosen as p o s i t i o n rather than v e l o c i t y .  Discussion i s organized according to the  framework described by Ljung and Soderstrom [18], who i d e n t i f y f i v e items i n t h e i r d i s c u s s i o n on adaptive c o n t r o l .  These are:  16.  i)  The choice of the set of admissible c o n t r o l s u(k)  ii)  The choice of a model s t r u c t u r e M(0)  to represent the true  system i i i ) The s e l e c t i o n of a design method to determine the c o n t r o l law i n terms of the model parameters 6 iv)  The  s e l e c t i o n of an i d e n t i f i c a t i o n procedure to determine the  system parameters v)  The s u b s t i t u t i o n of the estimated parameters i n t o the c o n t r o l law  A t t e n t i o n i s now  2.2.1  turned to these i s s u e s .  Admissible Controls I t i s assumed that the computation time required f o r the c o n t r o l law  i s a s u b s t a n t i a l p o r t i o n of the sampling time T.  Therefore y(k) i s not  a v a i l a b l e f o r computing u ( k ) ; there i s a delay of one sampling period ( t h i s i s the sort of thing that cannot be handled a n a l y t i c a l l y i n the continuous time case).  The "best" predicted value of y(k) can be used  i n s t e a d , based on measurements up to time (k-1).  The n o t a t i o n y(k|k-d)  denotes the best p r e d i c t i o n of y(k) given information up to time (k-d). Admissible c o n t r o l s therefore c o n s i s t of the reference s i g n a l y^, past c o n t r o l s , and past output measurements.  2.2.2  D i s c r e t e Model Structure A d i s c r e t e model i s required such that i t s s t r u c t u r e i s s u i t a b l e both  for c o n t r o l law determination and for i d e n t i f i c a t i o n of the system parameters.  The continuous-time representation of the actuator/robot  arm  system dynamics i s f i r s t examined i n order to determine s u i t a b l e d i s c r e t e candidate s t r u c t u r e s .  17. The mathematical representation of the manipulator part of the system i s given by a set of second-order, coupled, h i g h l y non-linear  equations  (see Chapter 3 ) . I t i s assumed that the l i n k s of the manipulator have high s t r u c t u r a l r i g i d i t y such that d e f l e c t i o n of the l i n k s may be ignored.  For  an n degree-of-freedom system the matrix form of the equations i s  D(6)9  where  8  D(8)  and  +  8  C(8,8) +  C(8,0)  +  =  j  T  (2.1)  are j o i n t p o s i t i o n s and v e l o c i t i e s r e s p e c t i v e l y ,  i s an nxn i n e r t i a matrix,  tions,  G(6)  8  i s an n x l vector of j o i n t a c c e l e r a -  i s an n x l vector of c e n t r i p e t a l and C o r i o l i s e f f e c t s , and  G(8) i s an n x l vector of g r a v i t a t i o n a l f o r c e s , torque vector and  x^  x i s an n x l input  i s an n x l vector of miscellaneous unmodelled  disturbances such as loads and f r i c t i o n .  This representation assumes there  i s no contact force at the t i p of the manipulator. As f a r as the j o i n t actuators are concerned, Vukobratovic  [6] suggests  that the i ^ actuator can be represented by a general s t a t e space model of t  the form A  where  ^*^±*^±  a r e  i  X  i  +  b  i i N  +  f  T  i  ( 2 , 2 )  i s a saturation function, x ^  constant matrices, th  i s the s t a t e vector of the i  i  actuator of dimension m^;  x^] where x^ i s the (m^-2)xl vector of remaining s t a t e s .  x^ =  ®i'  The s a t u r a t i o n  f u n c t i o n accounts f o r the f a c t that actuators have power l i m i t a t i o n s . I t i s l a t e r shown (Section 3.3) that f o r the robot used i n the s i m u l a t i o n the model f o r the actuators i s of the form J  i  e  i  +  C  A  +  T  I  =  K  i i u  (2.3)  18.  which can be w r i t t e n i n the above form, m^=2. I t i s possible to combine (2.1) and (2.3) i n t o a state space form  ff(x(t)» u ( t )  , t)  (2.4)  e i s a 2nxl vector.  where  The f ( x ( t ) , u ( t ) , t ) can be l i n e a r i z e d using a Taylor series expansion and the r e s u l t i n g equation d i s c r e t i z e d , i . e form  6x  A6x  +  B6u  (2.5)  Assuming zero-order hold sampling T the d i s c r e t e equivalent of (2.5) i s  x ( k + l ) = *x(k)  +  Tu(k)  where I  +  *  r  J  A  /  B  At  e  AT  *  e  A t  dt  B  .„  dt  This method i s computationally i n t e n s i v e . For the purpose of designing an adaptive c o n t r o l l e r a d i f f e r e n t approach was taken.  The g r a v i t y , C o r i o l i s , c e n t r i p e t a l and d i s t u r b i n g  19.  e f f e c t s are a l l t r e a t e d as disturbances.  The coupling i s a l s o assumed a  disturbance such that the m u l t i v a r i a b l e three degree-of-freedom system i s reduced to three s i n g l e - i n p u t / s i n g l e output systems.  A system model of the  form  [  J  i  + D  i i  ]  i  e  +  C  A  +  x  d  = A  V  i  ( 2  *  7 )  i s proposed f o r each l i n k and the d i s c r e t e equivalent found, as f o l l o w s . Rewritten to s i m p l i f y the n o t a t i o n  J8 + C6 +  The d i s t u r b i n g torques  T  d J  = Ku  (2.8)  are modelled as a time varying sequence v(k) of  unknown mean and variance.  The continuous time remaining p o r t i o n can be  expressed i n Laplace transform notation by  Y  <> S  * s ( J s + C) K  =  G(s) U(s)  =  *7U*kJ>  U  <> S  In standard form,  < s)  G  where  a = ^J  K  *  (2 9)  20.  Using a zero-order h o l d , the equivalent d i s c r e t e t r a n s f e r f u n c t i o n i s  (^)Z{^1}  G(z) -  where z{ } denotes the z-transform.  The r e s u l t i s , with a sampling time  T, G(z)  =  bz(e- +aT-l) + ^ - a T e ^ aT  a(z-l)(z-e  1 ( 2  .  1 Q )  )  _ B(z) A(z)  With some manipulation, replacing z by the s h i f t operator q, where q y ( k ) - 1  = y ( k - l ) , the equation  can be expressed as y(k) + ajyCk-l) + a y(k-2) 2  -  bju(k-l) + b u(k-2) 2  (2.11)  where a  x  =  -(1 + e" )  (2.12)  aT  —aT  a  2  -  e  b  :  =  b/a (e"  b  -  b/a (1 - e"  2  (2.13) aT  + aT - 1) aT  (2.14)  - aT e" ) aT  (2.15)  The disturbance v(k) can be modelled i n more than one way. For example, i t can be assumed that i t i s f i l t e r e d white noise, i . e . 0  21.  C U " ) e(k)  v(k)  1  (2.16)  with 1 + q" + q ~ +  C(q )  1  _ 1  2  Since i t i s expected that the disturbance would have non-zero mean over a f i n i t e time i n t e r v a l i t i s modelled as  v(k)  =  h + e(k)  (2.17)  where h i s an o f f s e t term and e(k) i s white noise.  This method of  subtracting a b i a s term out i s described i n Isermann [19].  Thus the  complete d i s c r e t e model of each l i n k f o r the purpose of c o n t r o l design and parameter estimation i s  y(k) + y ( k - 1 ) + a y(k-2) = S l  2  The c o e f f i c i e n t s a j , a , b j , b , h 2  2  b u ( k - l ) + b u(k-2) + h + e(k) 1  2  (2.18)  must be estimated.  The treatment of the c o n t r o l law d e r i v a t i o n and the estimation s e c t i o n i s general - polynomials A ( q ) and B ( q ) are used instead of the above _ 1  s p e c i f i c model.  _ 1  This was done so that m o d i f i c a t i o n s could have been made  more e a s i l y i n the simulation should the model (2.18) have proven to be inadequate.  2.2.3  C o n t r o l Law The chosen model s t r u c t u r e f o r each l i n k i s  A(q )y(k) _ 1  =  B ( q ) u ( k ) + h + e(k) _1  (2.19)  22.  with A(q )  -  1 + a j q " + a q ~ + . . . . + a q"™  (2.20)  B(q )  =  bjq  (2.21)  - 1  _ 1  1  2  2  - 1  + b q 2  - 2  +  + b q"  r  r  h i s a f o r c i n g or o f f s e t term and e(k) i s white noise, (zero mean, uncorrelated, random).  The proposed model f o r the robot system has m=2 and  r=2. Minimum variance c o n t r o l laws, based on a quadratic performance criterion  I  =  E{y(k) } 2  have enjoyed some success i n adaptive systems applied to the r e g u l a t i o n of processes, where the goal i s t o keep the output constant a t some reference value.  See Astrom e t a l . [20] f o r references.  This provides some motivation f o r proposing a modified quadratic c r i t e r i o n based on the d e v i a t i o n of the output from the d e s i r e d l e v e l and on the magnitude of the c o n t r o l e f f o r t :  I  = E{[y (k+1) - y ( k + l ) ] + eu(k) |(k-1)} 2  (2.22)  2  d  where E{f|k—1} denotes the expected value of the f u n c t i o n f given measurements up t o time ( k - 1 ) , and e i s a weighting f a c t o r .  In other words  the c o n t r o l e f f o r t at time k, u ( k ) , i s chosen to minimize the p o s i t i o n a l e r r o r which w i l l r e s u l t due t o that c o n t r o l one step ahead, but a penalty i s attached to large c o n t r o l e f f o r t s .  There i s a delay of one sampling  period because the i n f l u e n c e of the c o n t r o l u(k) w i l l only be f e l t a t  23.  y(k+l).  This i s r e f l e c t e d i n the system model by the f a c t that the leading  c o e f f i c i e n t of the B ( q ) polynomial i s zero, b =0. _ 1  Q  Therefore a p r e d i c t i o n  of y(k+l) i s needed. The expectation i s taken w i t h respect to time (k-1) because of the previously mentioned r e s t r i c t i o n that y(k) i s not a v a i l a b l e f o r computing u(k).  I f the sampling time T was very long compared to the  computational  time to obtain u ( k ) , one could assume that y(k) was a v a i l a b l e . The problem i s stated as f o l l o w s :  I  -  i t i s desired to minimize  E{[y (k+1) - y ( k + l ) ] + e u ( k ) | k - l } 2  2  d  subject to the c o n s t r a i n t  A ( q ) y(k)  =  _ 1  B ( q ) u(k) + h + e(k)  (2.23)  _ 1  Expanding I gives  I  =  E{y (k+1) - 2 y ( k + l ) y(k+l) + y ( k + l ) + e u ( k ) | k - l }  -  y ( k + l ) + e u ( k ) - 2y (k+l) E{y(k+1) |k-l} d d  2  2  2  d  2  2  J  +  E{y (k+1)|k-1}  (2.24)  2  where y, may be taken outside the expectation operator because i t i s a d known sequence, and s i m i l a r l y u(k) because i t i s c a l c u l a t e d . From the c o n s t r a i n t equation (2.23) y(k) may be w r i t t e n  y(k)  =  - a j y ( k - l ) - ... -a y(k-m) + b u ( k - l ) + . m  +  b u(k-r) + h + e(k) r  x  (2.25)  24. M u l t i p l y i n g (2.25) by the s h i f t operator q gives  y(k+l)  =  q y(k)  = -a,y(k) - ... -a y(k-m+l) + b,u(k) + ... i m i  +  b u ( k - r + l ) + h + e(k+l)  (2.26)  r  which i s a p r e d i c t i o n of y ( k + l ) . Denote R by R  =  [-a y(k-l) - ... -a y(k-m+l) + b u ( k - l ) + ... 2  +  m  2  b u ( k - r + l ) + h + e(k+l)] r  (2.27)  such that R i s a f u n c t i o n only of past outputs and c o n t r o l s y ( k - i ) , u ( k - i ) , i > 1 and of the e(k+l).  y(k+l)  =  y (k+l) 2  Then  - a y ( k ) + bju(k) + R t  (a (k)) i y  +  2  + (b (k)) l U  2  + R  2(b Ru(k) - R y ( k ) x  S l  a i  2  b (k)u(k)) i y  Examining the l a s t two terms of I which contain i n Equation (2.24) the expressions E{y(k+l)|k-l} and E{y (k+1)|k-1} 2  25.  and comparing w i t h equations (2.25) and (2.27) reveals that the expectations may be expressed by i)  past outputs y ( k - l ) , y(k-2)... and inputs u ( k ) , u ( k - l ) . . .  ii)  noise terms e ( k ) , e(k+l) f o r which by assumption E{e}=0  i i i ) terms of the form E{ey} and E{eu}, which are also zero since the noise i s assumed independent of y and u, and iv)  terms containing E { e } , which i s unknown, however t h i s i s 2  immaterial s i n c e 3l/3u(k) i s the quantity of i n t e r e s t . Performing the d i f f e r e n t i a t i o n and s e t t i n g the r e s u l t equal to zero to determine the minimum gives  31 3u(k)  0 + 2eu(k) - 2y (k+l)bj + 2 b ^ ( k )  +  2b R - 2 a b y ( k ) :  1  1  2[eu(k) + b ^ R - a ( k ) - y ( k + l ) ] i y  d  2[eu(k) + b ( y ( k + l ) - y ( k + l ) ] 1  d  => 0  Thus the c o n t r o l law i s  eu(k) + bjtyU+llk-l) - y(k+l)] = 0 d  (2.28)  where y ( k + l | k - l ) denotes the predicted value of y(k+l) given information up  26. to time ( k - 1 ) . This value w i l l be obtained by s u b s t i t u t i n g the parameter A A A  estimates a, b, h i n t o (2.28) as explained l a t e r .  2.2.4  I d e n t i f i c a t i o n Procedure  There are many methods f o r r e c u r s i v e l y i d e n t i f y i n g the parameters of an equation, f o r example, recursive l e a s t squares, extended l e a s t squares, r e c u r s i v e maximum l i k e l i h o o d , s t o c h a s t i c approximations, instrumental v a r i ables, to name a few.  There are i n a d d i t i o n several nonlinear techniques.  Within each method there are v a r i a t i o n s or design f a c t o r s t o choose from. Another consideration f o r each method i s the p a r t i c u l a r computational algorithm used.  The same method implemented w i t h d i f f e r e n t computational  algorithms gives d i f f e r e n t performance [21].  An example i s computer  round-off e r r o r occuring due to poorly structured c a l c u l a t i o n s .  Thus  choosing an estimator which w i l l perform adequately i s not easy. I t was decided t o go w i t h b a s i c r e c u r s i v e l e a s t squares.  This tech-  nique i s known to be robust under many s i t u a t i o n s and well-conditioned algorithms were r e a d i l y a v a i l a b l e .  A d e r i v a t i o n of the r e c u r s i v e l e a s t  squares equations i s given i n Appendix I .  A(q )y(k) _1  =  A model of the form  B(q" )u(k) + v(k) 1  i s postulated with polynomials  A(q )  =  1 + a q~  B(q )  =  b  - 1  _ 1  + a q[ ^ ^ • • • + a q-m m  :  - 1 i q  2  + b q"I 2  2  + ...  n  -r  and v(k) some disturbance of unknown characters.  Defining the f o l l o w i n g  vectors  6  T  =  [a a 1  4> (k) =  ... a  2  b  1  b  2  ... b ] f  (2.29)  [-y(k-l) - y(k-2) ... - y(k-m) u ( k - l ) ... u ( k - r ) ] (2.30)  T  The system model may be w r i t t e n  y(k)  6 t(k)  =  T  + v(k)  (2.31)  A 'natural' estimate y(k) i s  J(k)  =  6 *(k)  (2.32)  T  The l e a s t squares p r i n c i p l e s t a t e s that the best estimate 6 i s the one that minimizes the square of the error £(k) = y(k) - y ( k ) , that i s the criterion  1(6) =  N I £ (k) k=l  (2.33)  2  The r e s u l t i s  8  =  (*%r  l  provided the inverse e x i s t s .  * Y T  The information matrix $ i s  (2.34)  28. T  •  •00  y(D  (1)  and  Y(N)  =  y(N)  T •  00  T  The matrix  ($  P ( k ) . Singu-  i s known as the covariance matrix  l a r i t y or near s i n g u l a r i t y of P ( k ) w i l l cause the estimates to be inaccurate. A r e c u r s i v e form of the equations may be derived. 8(k)  -  9(k-l)  +  K(k-l)  =  P(k-l)  *(k)  -  [I  K(k-l)  POO  "  K(k-l)  [y(k)  * (k)  [X +  T  6 (k-l)  -  T  P(k-l)  * (k)] P(k-l)  The r e s u l t i s *00]  fOOT  1  (2.35)  /X  A  The s c a l a r X i s a f o r g e t t i n g f a c t o r used when 8 ( k ) i s time-varying, and i t permits discounting of o l d data.  I f 6 i s constant X i s set to 1.  In order t o estimate the o f f s e t h the parameter vector 8 i s augmented by h and the measurement vector if i s augmented w i t h a 1. The recursive equations (2.35) are not well-conditioned from a numerical point of view [21]. Problems were encountered i n a p p l i c a t i o n s , o f t e n because P ( k ) f a i l e d to remain non-negative d e f i n i t e .  Various  adhoc methods of circumventing the d i f f i c u l t i e s have been used, but the a l t e r n a t i v e approach of r e s t r u c t u r i n g the covariance update using a square root type f a c t o r i z a t i o n has been shown to be superior.  The covariance  T  matrix i s factored i n a form  D D  t r i a n g u l a r and D i s diagonal.  In order to avoid p o t e n t i a l  1  /  2  or  UDD  where  D  i s upper  i n s t a b i l i t i e s i n the recursive l e a s t squares estimation portion of the  29.  simulation, equation (2.35) are not used d i r e c t l y .  Instead an algorithm  presented by Bierman [21] i s implemented (as a subroutine). UDU  I t uses  T  factorization.  2.2.5  Replacement of the True Parameters The c o n t r o l law design i s based on the certainty-equivalence p r i n -  ciple:  the c o n t r o l l e r i s designed as i f the system parameters were known,  and then the estimated values are s u b s t i t u t e d i n t o the c o n t r o l law.  This  concept i s a s p e c i a l case of the Separation Theorem used i n Linear Quadratic Gaussian (LQG) theory.  The Separation Theorem i s used when  optimal c o n t r o l i s used on state space models.  I t enables the design of  the c o n t r o l l e r to be done separately from the estimation of the s t a t e s , and i t may be shown to be v a l i d [22] i f (1) the system can be described by l i n e a r equations w i t h known parameters, (2) the c o n t r o l i s optimized according to a quadratic performance c r i t e r i o n , and (3) the estimation i s optimized f o r Gaussian disturbances, i . e . i f the noise i s white.  In the  case of adaptive c o n t r o l the system parameters are of course unknown, and the certainty-equivalence p r i n c i p l e holds i f the above three c o n d i t i o n s are met, except that the parameters should be s t a t i s t i c a l l y independent.  This  c o n d i t i o n i s probably not s a t i s f i e d f o r the case studied here, since there i s probably some c o r r e l a t i o n between the estimated parameters.  However,  s a t i s f a c t o r y r e s u l t s have been obtained by previous researchers i n v e s t i g a t ing adaptive schemes based on certainty-equivalence, even i f i t i s not r i g o r o u s l y j u s t i f i e d on a t h e o r e t i c a l l e v e l . An estimate f o r y(k) given measurements up to (k-1) i s  30. y(k|k-l)  6T(k-D  =  = -a y ( k - l ) - a y(h-2) + b x  2  u(k-l)  :  + b u(k-2) + h  (2.36)  2  Moving ahead one step, an estimate of y(k+l) given measurements up to (k-1) *  1 8  y(k+l|k-l)  =  6T(k-l)  (2.37)  At time (k-1), 6(k-l) i s the best f i l t e r e d estimate of 9 i n the l e a s t squares sense. At time k, i n the absence of measurements, 0(k-l) i s the best p r e d i c t i o n of 0 i n the l e a s t squares sense. One may think that the uncertainty  i n P(k) i s very high, and so K(k-l) goes t o  zero: no weight i s given to new measurements because they are absent. So 8 ( k - l ) i s the best p r e d i c t i o n a t time (k+d), and +(k+l) i s given by  *(k+l) -  [y(k|k-l) y ( k - l ) u(k) u ( k - l ) 1]  (2.38)  S u b s t i t u t i n g (2.36) i n t o (2.38) and placing the r e s u l t i n (2.37) w i l l give the estimate y ( k + l | k - l ) .  u(k)  Then the c o n t r o l law (2.28) w i l l give  = W[y (k+1) - ( d  y ( k - l ) + c y(k-2)  C j  2  + c u ( k - l ) + c u(k-2) + c ) ] 3  where  h  5  (2.39)  31. A  w  (2.40)  c1  (2.41) (2.42) (2.43) (2.44) (2.45)  c5  The v a r i a b l e W i s a "weight" or gain and the c^ are c o n t r o l c o e f f i c i e n t s . Equation (2.39) gives the c o n t r o l u(k) at each sampling i n s t a n t k. The choice of the adaptive c o n t r o l strategy has been described w i t h respect to f i v e points or t o p i c s .  This d i v i s i o n was made for convenience  so that some s o r t of o r d e r l y framework could be followed. i s much i n t e r a c t i o n and overlap between the f i v e areas.  Evidently there  Many v a r i a t i o n s on  the above themes are p o s s i b l e and concocting an adaptive c o n t r o l strategy i s not a problem. not easy.  However, a n a l y s i s , e s p e c i a l l y of the o v e r a l l scheme, i s  Two important considerations are o v e r a l l s t a b i l i t y of the system  and convergence of the c o n t r o l l e r to the optimal one. A general theory i s l a c k i n g although a few proofs e x i s t f o r s p e c i a l cases, see f o r example Ljung and Soderstrom [18], Goodwin and S i n [23], Egardt [24], Narendra and Lin  [25], Goodwin, Ramadge, and Caines [26].  E m p i r i c a l i n s i g h t may be  gained through s i m u l a t i o n , thus the d i s c u s s i o n on s t a b i l i t y and convergence i s postponed u n t i l the simulation r e s u l t s are covered. s i m u l a t i o n package i s treated i n the f o l l o w i n g chapter.  Development of the  32.  Table I Parameter Values f o r the RSI Robot  Description  mass of l i n k 1 mass of l i n k 2 mass of l i n k 3  Symbol  Value  l  5.652  kg  "2  5.440  kg  0.755  kg  m  m  3  1  I  centre of mass, l i n k 2  x  2  centre of mass, l i n k 3 moment of i n e r t i a , l i n k 1 moment of i n e r t i a , l i n k 2 moment of i n e r t i a , l i n k 3  mm  71  mm  I,l y y  4.03xl0  - 2  2zz  2.43xl0  - 1  3.83xl0  - 3  X  3yy  length of l i n k 2  a  armature resistance  R  motor constant  -211  2  moment of i n e r t i a , motors  J  counter emf constant  K  _ act  gear r a t i o  n  1.413xlO  400  2  2  ohm  4.308xl0-  e  kg-m  mm  356  4.3xl0  2  kg-m  0.75  *T  kg-m  - 2  2  -Jt  N-m/ amp kg-m  2  volt-s/rad  33.  Table I I Kinematic Parameters f o r the RSI Robot  JOINT 1  a  i  0  1 2 3  Notes:  (i) a  2  d  a  i  a  i  IT l  2  2  0  0  0  0  d  ir  " 2  i s the length of l i n k 2, which i s 356  mm.  ( i i ) dj i s the distance between the o r i g i n of frame 1 and the base frame 0, and may be chosen to be zero.  Reference Model  Controller controller parameters Adjustment Mechanism  Fig.5 BLOCK DIAGRAM OF MRAS.  u  System  35.  Parameter Estimation controller parameters,, Controller  I  Fig.6 BLOCK DIAGRAM OF STR.  System  36.  CHAPTER 3 THE SIMULATION MODEL  3.1  The RSI Robot In t h i s work the RT-3 Robot manufactured by Robotics System  I n t e r n a t i o n a l (Sydney, B.C.)  serves as prototype f o r the s i m u l a t i o n . I t i s  subsequently r e f e r r e d to as the RSI robot.  This small general purpose  robot was purchased i n the f a l l of 1984 by the Department of E l e c t r i c a l Engineering a t U.B.C. F i g . 7 i l l u s t r a t e s i t s main c h a r a c t e r i s t i c s . RSI i s a manipulator with s i x revolute l i n k s .  The  I t has no o f f s e t s , so the  k i n e m a t i c a l s t r u c t u r e i s s i m p l i f i e d , and i t has a s p h e r i c a l w r i s t , which i s common i n i n d u s t r i a l manipulators (e.g. Puma 560) and which f u r t h e r f a c i l i tates the a n a l y s i s .  The j o i n t s are d r i v e n through a gear t r a i n by DC  motors with armature c o n t r o l .  P r e s e n t l y , there are s i x analogue PID servo  boards, each c o n t r o l l i n g one j o i n t of the robot. a l l y by adjusting two gain screws.  Each loop i s tuned manu-  Voltage c o n t r o l s i g n a l s are downloaded  from a small computer to the servo boards. The main advantage i n using t h i s robot f o r the study i s that i t s c h a r a c t e r i s t i c s are known.  Dimensions are a v a i l a b l e from a set of draw-  ings; masses and p r i n c i p a l moments of i n e r t i a f o r each l i n k were obtained by A l l e n [27]; motor c h a r a c t e r i s t i c s were measured from step response tests.  These values are tabulated ( f o r the f i r s t three l i n k s ) i n Table I .  Since so much i s known about t h i s robot, one i s f a i r l y confident that a reasonable s i m u l a t i o n model can be obtained once the theory has been applied to i t . The t h e o r e t i c a l background necessary to implement a computer simulat i o n of the RSI robot includes kinematics and dynamics.  F i r s t , the  37. kinematics of the arm i s described, and the importance of t h i s area i s discussed.  Then, the equations of motion are derived. The  made i n developing the dynamics model are noted.  assumptions  These aspects are now  examined i n d e t a i l .  3.2  Kinematics The d i r e c t kinematics problem i s to f i n d the p o s i t i o n and o r i e n t a t i o n  of the end e f f e c t o r , or t o o l , of the robot given the j o i n t angles 0. This i s easy.  The inverse kinematics problem i s to determine the j o i n t  angles which w i l l r e s u l t i n a desired t o o l o r i e n t a t i o n and p o s i t i o n r e l a t i v e to f i x e d base coordinates. This i s generally not easy.  Both of these  problems need t o be solved f o r the RSI robot. The d i r e c t kinematics analysis i s needed because the d i f f e r e n c e between d e s i r e d and a c t u a l endpiece p o s i t i o n w i l l provide a measure of error and hence performance of the c o n t r o l l e r . p o s i t i o n i s of course known a_ p r i o r i .  The desired endpiece  This information i s downloaded from  a higher l e v e l c o n t r o l l e r : the path planner.  The a c t u a l endpiece p o s i t i o n  i s e i t h e r measured or c a l c u l a t e d from measurements of j o i n t angles.  In an  advanced r e a l robot p o s i t i o n might be measured by a v i s i o n system. However, most present-day robots use potentiometers or o p t i c a l encoders at the j o i n t s and measure j o i n t angles. inferred.  Endpiece p o s i t i o n may then be  In t h i s computer s i m u l a t i o n of the manipulator, j o i n t angles  w i l l be "measured" by numerically i n t e g r a t i n g the equations of motion. "Noise" may be added to simulate i n a c c u r a c i e s of a r e a l system.  Using the  d i r e c t kinematics, the p o s i t i o n of the endpiece i s then c a l c u l a t e d . In order to r e l a t e p o s i t i o n s on one l i n k r e l a t i v e to another, a coordinate frame i s attached to each l i n k .  This i s described i n greater  38.  d e t a i l below.  Transformation of vectors from one coordinate frame to  another i s done by a 4x4 homogeneous transformation matrix T. The T matrices are used i n the dynamics s e c t i o n to obtain the equations of motion of the manipulator.  So, a kinematic a n a l y s i s i s necessary.  The inverse kinematics problem must be solved as w e l l because an elementary  t r a j e c t o r y planner i s needed i n the s i m u l a t i o n . The user w i l l  input the parameters of a t r a j e c t o r y which the endpiece must f o l l o w . For a l i n e , f o r example, the i n i t i a l p o s i t i o n , the slope, and the v e l o c i t y of the endpiece i s a set of inputs.  The path planner w i l l c a l c u l a t e the  i n p o s i t i o n at each sampling i n s t a n t k.  increment  Using inverse kinematics t h i s  vector of desired c a r t e s i a n p o s i t i o n i s converted to a vector of the desired j o i n t angles.  Each angle i s downloaded to i t s corresponding  joint  c o n t r o l l e r which sends (hopefully) the appropriate voltage s i g n a l u(k) to the j o i n t motor.  The system i s i l l u s t r a t e d i n F i g . 2 ( a ) .  the hand i s not closed;  The loop around  with feedback from the end e f f e c t o r , the system  would look as i n F i g . 2(b).  3.2.1  D i r e c t Kinematics Returning to kinematics, a means of r e l a t i n g p o s i t i o n s on one l i n k to  a f i x e d frame or to another moving l i n k i s e s t a b l i s h e d . The points are covered here.  principal  D e t a i l s and examples are found i n Paul [28].  An  a l t e r n a t i v e reference i s Lee's paper [29], which gives a good e x p o s i t i o n of robot arm kinematics. Given a point P and two coordinate frames F^ and F  2  as shown i n F i g .  8, the p o s i t i o n of P r e l a t i v e to Fj i s expressed as a vector whereas the p o s i t i o n of P r e l a t i v e to F  2  i s expressed as 2 r .  *r, A left  superscript w i l l denote which coordinate system the vector i s r e f e r r e d t o .  39.  The vectors 1 r and * r are r e l a t e d by  1  where  T  -  d +R  l  2  2  r  (3.1)  i s a r o t a t i o n matrix and *d i s the distance between the  o r i g i n s of the two frames. The transformation between * r and 2 r may be e f f e c t e d w i t h one matrix by using homogeneous coordinates.  An e x t r a element i s added to  vectors, so that given  The vector • i s defined x y z (I)  X  w i t h x' - — , y' •=  Y  Z  , z' = —.  The element u i s commonly taken to be 1.  Now, transformations are expressed as  where  *2  (3.2) .0  0  0  40.  The A transformation matrices may be chained together to r e f e r a vector through a s e r i e s of coordinate frames, as follows  A  1 2 A  A  l y ±  The base frame i s denoted by "0" and s u p e r s c r i p t s u s u a l l y omitted.  Chains  of A matrices have been c a l l e d T tranformation matrices,  T  A ±  (3.3)  v  In order to r e l a t e p o s i t i o n s and v e l o c i t i e s of points on each l i n k of the robot to a f i x e d based frame, a coordinate frame i s attached to each link.  Standard p r a c t i c e i n r o b o t i c s i s to use the  convention [30], f o r e s t a b l i s h i n g coordinate frames. general l i n k a g e .  Denavit-Hartenberg I t i s a p p l i c a b l e to a  The base frame i s f i x e d and i s numbered '0'.  Link 1 i s  attached to the base by j o i n t 1, l i n k 2 i s attached to l i n k 1 by j o i n t 2 and so on.  For a rotary manipulator, the j o i n t v a r i a b l e 6^ describes  r o t a t i o n of j o i n t i about the to  z^  origin. rule,  axis.  from the cross product The =  x  *^  The •  x^  a x i s i s chosen normal  The i n t e r s e c t i o n i s the  a x i s completes the right-hand z  i  x  x  i  *  ^  e  ^ H o w ^ S parameters describe the  r e l a t i o n s h i p between neighbouring i)  a^ i s the distance between  ii)  d^ i s the distance between  i i i ) <x^ i s the angle between the r i g h t hand r u l e .  links:  x^  and  measured along  and  measured along  and  z^  measured about  x^  x^  , using  41. Some f l e x i b i l i t y i s afforded by choosing the above parameters i n such a way as to a l i g n an a x i s i n a desired d i r e c t i o n .  Paul [28] describes how  to place coordinate frames on the robot l i n k s , but Lee [29] gives an algorithm which i s e a s i e r to f o l l o w .  In any case, f o r a r o t a r y  manipulator, the c l e a r e s t e x p o s i t i o n i s Hollerbach's [31], from which the parameter d e s c r i p t i o n s a^, d^ and  are taken.  The r e s u l t of applying the  above procedure to the RSI robot i s given i n F i g . 9.  The choice of  coordinate frames i s important, since the whole d e s c r i p t i o n of the robot's behaviour r e s t s on i t , from t r a j e c t o r i e s to dynamics equations.  Since  Hollerbach's paper contains the most e f f i c i e n t inverse kinematics s o l u t i o n developed to date (by Featherstone [32]), h i s choice of coordinate frames i s followed.  Table I I gives the kinematic parameters for the RSI robot.  The next step i s to o b t a i n the coordinate frames.  matrices  from the choice of  Using the above d e f i n i t i o n s , a vector  Sr  expressed i n  l i n k i coordinates may be re-expressed i n terms of l i n k i - 1 coordinates as * i)  by performing the f o l l o w i n g sequence of sub-transformations: f i r s t , r o t a t e by an angle 6^ about p a r a l l e l to the  ii)  t o  b r i n g the  axis  axis  second, t r a n s l a t e along  distance  a  d^ so that  x^ and  X j _ ^ coincide i i i ) t h i r d , t r a n s l a t e along  =  x  i - l  a  distance ^ a  t o  b r i n g the two  o r i g i n s together iv)  f i n a l l y , r o t a t e by an angle ot^ about  x^ to b r i n g the coordinate  frames i n t o coincidence.  The r e s u l t s i s the general matrix  A . given by  42.  cos8^ A.  -sinG^cosct^  sln8^sina^  a^cos9^  cosO^cosa^  -cosO^sina^  a^sinS^  0  sina^  0  0 A^  For the RSI robot, the r e s u l t i n g  (3.4)  cosa.  matrices, i = 1,2,3  are given i n  Appendix I I , along w i t h the transformation matrices T. The d i r e c t kinematics have now been covered. p o s i t i o n vector defined with respect to the i ' * *v, 6  lt  3.2.2  1  In summary, given a  l i n k coordinate frame,  t h i s vector may be transformed to base coordinates given the angles 8^ and the matrix  by the simple m u l t i p l i c a t i o n  Inverse Kinematics The inverse kinematics are now addressed.  Featherstone's  technique  [32] as described by Hollerbach [31] i s used f o r the f i r s t three l i n k s of the RSI robot.  The method s p l i t s the s o l u t i o n at the w r i s t of the robot,  so i t i s s u i t a b l e i n the present case. The vector  i s defined as the vector from the coordinate p*  o r i g i n 0 to coordinate o r i g i n i , and  i s the vector from o r i g i n i - 1 to  o r i g i n i . These d e f i n i t i o n s are i l l u s t r a t e d i n F i g . 10.  The vector p^  i s known, since i t represents the desired cartesian p o s i t i o n of the endpiece w i t h respect to the f i x e d base frame. i n a plane perpendicular to the x - y Q  0  Since j o i n t s 2 and 3 move  plane, the f i r s t angle i s e a s i l y  found Q  1  =  tan"  1  (—£)  (3.5)  43.  Note that a degeneracy occurs when p^^ = p^ p o s i t i o n along the z  Q  = 0, corresponding t o a w r i s t  axis.  To f i n d the next two angles, p  lf  i s expressed i n l i n k 1 coord-  i n a t e s , reducing the problem t o a planar two-link one.  Pw  Then  Defining  P<* " P i  "  p. =  - d ^  P|f  ( 3 > 6 )  Expressed i n l i n k 1 coordinates  l' P  1]  0 w z  (3.7)  where r has been defined from  r  =  2  2 P l + X  +  (3.8)  2 P l + y  By the cosine r u l e  s i n 6,  =  a  2  +  z - (r + p ) 2  2  2  ^—  2 a  (3.9)  2 l z  And cos 6  3  - ± /l - (sin6 )  z  3  where the + s i g n i s elbow down and the - sign i s elbow up. Then,  (3.10)  44.  tan  ( r-) cos8  - 1  (3.11)  3  Finally, 8  2  *p  i s found by expressing  +  *2  *2  w  z  i  Z  as  (3.12)  3  a cos8  2  -  z^ s i n ( 8 + 8 )  a sin6  2  +  z^ cos(8 +8 )  2  2  2  3  2  (3.13)  3  0 1  Solving simultaneously gives  L  sin8.  p  ( a , - (z..sIn8o)) - p 1  a  1  2 2  + z  - 2a (z  2 x  2  a  sin6 )  a  (3.14)  3  p wx + sin8„(z cos8_) z l 3 ^ N  cos8.  1  (z.,cos8 )  n  (3.15)  2 ~ ^ i s*- ^) z  11  so that  8 2 9  =  tan  _ i  sin8. ( s-) cos8  (3.16)  x  2  Care must be exercised i n the s p e c i f i c a t i o n of the quadrants f o r the 8^. The inverse kinematics have been covered, so that given a p r e - s p e c i f l e d vector p^ d e s c r i b i n g the d e s i r e d w r i s t p o s i t i o n , the j o i n t angles 8^, 8  2  and 8 may be determined. 3  45.  3.2.3  Trajectory Planning S p e c i f i c a t i o n of a d i s c r e t e time s e r i e s p^(k) i s covered next.  F i r s t , the workspace of the RSI robot was defined. From the j o i n t l i m i t s , given i n Table I I , a p r o j e c t i o n of the workspace ( i n the l i n k s two and three i s given by F i g . 11.  X Q - Z  0  plane) f o r  The o r i g i n of the w r i s t frames,  or endpiece of the robot i n t h i s case, may be positioned anywhere i n t h i s space.  I t was decided to s p e c i f y s t r a i g h t l i n e paths f o r t e s t i n g the  c o n t r o l law. The i n i t i a l p o s i t i o n and the target p o s i t i o n are s p e c i f i e d .  It i s  assumed that the arm s t a r t s from the rest and that a t r a p e z o i d a l v e l o c i t y law i s followed i n between, as i l l u s t r a t e d i n F i g . 12. completely s p e c i f i e d by a, t j , t , t « 2  are  This type of v e l o c i t y p r o f i l e i s  3  sometimes c a l l e d "bang-coast-bang",  The p r o f i l e i s  [33] since high i n i t i a l a c c e l e r a t i o n s  required to bring the endpiece to the t r a v e l v e l o c i t y , and at the end  of the motion high decelerations are applied to zero i n and stop on the f i n a l position. Using ideas discussed by Duncan [34], the l i n e path i s defined by d i r e c t i o n cosines a and g at p o i n t p ^ ( k ) , as shown i n F i g . 13.  The  v e l o c i t y v(k) i s known from the v e l o c i t y p r o f i l e described above, so the distance t r a v e l l e d i n time I n t e r v a l T  (from time k to time k+1) i s  As  -  v(k) T  (3.17)  Pltx  (k+1)  =  P^k)  + As cosa  (3.18)  Pl|z  (k+1)  =  P« (k) + As cosg  (3.19)  hence,  tz  This method of d e f i n i n g a path may be e a s i l y extended to 3-space by s p e c i f y i n g another d i r e c t i o n cosine.  46.  3.3  Dynamics The f u l l equations of motion are required i n order to simulate the  r e a l behaviour of the robot.  They also served i n choosing a s i m p l i f i e d  model of the dynamic system f o r the purpose of designing a c o n t r o l l e r .  As  f a r as the simulation i s concerned we wish to r e l a t e the output s i g n a l of the c o n t r o l l e r , the voltage u ( k ) , to the r e s u l t i n g motion of the l i n k s , the p o s i t i o n y ( k ) .  The vectors u(k) and y ( k ) are conventional  notation i n c o n t r o l l i t e r a t u r e , which i s why they are used.  However, i n  the dynamics a n a l y s i s which f o l l o w s the measured output v a r i a b l e y ( k ) which i s angular p o s i t i o n , w i l l be denoted by 8(k) and by 0 i n the continuous representation. The system c o n s i s t s of the actuators, DC motors i n t h i s case, and the manipulation linkage.  The current from the c o n t r o l l e r i s applied to the  motors, which produce a torque T m  at each j o i n t i .  This torque i s m u l t i -  i  p l i e d through a s e r i e s of gears, so that a torque  i s a v a i l a b l e at the  output shaft to drive j o i n t i to the desired p o s i t i o n .  The a n a l y s i s i s  d i v i d e d i n t o the actuator dynamics, then the manipulator dynamics, and f i n a l l y the r e l a t i o n s h i p between the two.  3.2.1  Actuators The actuators are DC motors w i t h armature voltage c o n t r o l , i n other  words t h e i r speed i s c o n t r o l l e d by the armature voltage u. r e f e r s to the i * t  1  j o i n t and i s omitted f o r now.  Subscript i  A counter emf p r o p o r t i o n a l  to the armature speed 6 i s induced m E  c  =  K 6 e m  The c i r c u i t equation f o r the armature i s  (3.20)  47. u - E  where the time constant i s T  =  c  RI  L = —. a R a  a  + T  di -j-—— a dt  Since the Inductance L  (3.21)  a  i s very small  the l a s t term i s normally neglected. The torque developed by the motor i s p r o p o r t i o n a l to the armature current and i s given by T  =  m  KI l a  (3.22)  D  S u b s t i t u t i n g from the preceding equations gives  T  T R K  M  m  T e • " "T~^ K K  u  K  (3.23)  6  m  m  which i s an expression f o r the torque produced by the motor i n terms of the c o n t r o l input u, known constants K^,, K  3.3.2  e>  R and the armature speed 6^.  Manipulator Dynamics The equations of motion f o r the linkage are developed next.  They are  e a s i l y obtained by a s t r a i g h t f o r w a r d though tedious a p p l i c a t i o n of Lagrange's equations.  The general form of the equations f o r a manipulator  i s derived i n Paul [28].  This d e r i v a t i o n i s sketched here.  Then i t i s  shown how the r e s u l t i s a p p l i e d to the f i r s t three l i n k s of the RSI robot. The Lagrangian L i s w r i t t e n as L  =  K - P  (3.24)  where K i s the k i n e t i c energy of the system and P the p o t e n t i a l energy. The equations of motion f o r an n-degree of freedom system are given by  48.  d  where the  9L  (3.25)  i - 1. ... .n  are any convenient generalized coordinates and  generalized f o r c e .  i s the  The equation  dW  constitutes a definition  n  (3.26)  of the generalized f o r c e , with dW being the work  done by the e x t e r n a l forces during a v i r t u a l displacement dq. For the case of a revolute manipulator, the generalized coordinates may be chosen as the j o i n t angle  Then the generalized forces  displacements:  are simply the net torques x^ a p p l i e d  , at each j o i n t i . An expression f o r the k i n e t i c energy i s computed next.  Consider a  d i f f e r e n t i a l mass dm on l i n k i . I t s p o s i t i o n i n l i n k i coordinates i s given by  *r  .  In base coordinates, i t s p o s i t i o n i s  (3.27)  and i t s v e l o c i t y i s  49.  J  J-l  The v e l o c i t y  (~)  squared i s  =  2  (r  Trace  r )  (3.29)  T  S u b s t i t u t i n g from equation (3.29) gives  A-  1  1  3  j=l k=l  T  *  4  4  ' " j  T  j  1  (3.30)  k  J  The k i n e t i c energy of the p a r t i c l e of mass dm on l i n k i s then  d K  i  =  d m  1  =  (f>  2  i i 3T, , j Trace [ z Z « i (*r j = l k-1 " j  . _  3T *  dm r ) ^ A  T  2  3 9  k  8 § ]  j  (3.31)  k  The k i n e t i c energy of the e n t i r e l i n k i s found by summing over the l i n k  K  l  "  ./ link i  d  K  i  T  -  i i 3T i Trace [ Z Z * < J *r j=l k=l j l i n k i  V  3T  d.) ^  k  5 8J J  (3.32)  50.  The expression i n round brackets i s pseudo i n e r t i a matrix  .  See  Appendix I I f o r d e t a i l s . The t o t a l k i n e t i c energy i s found by summing over the n l i n k s  K  =  i Z K i=l  -  \  i i 3T 1 Trace [ I I i=l j = l k=l j  1  d  H  J.  6.6. ]  The p o t e n t i a l energy of the system i s found next.  (3.33)  I f g r a v i t y i s expressed  as a vector g and the centre of mass of l i n k i i s given by p (both i n base coordinates), the p o t e n t i a l energy of l i n k i i s  T  i  =  ~ i m  8  * *i  =  ~ i m  8  where m^ i s the mass of l i n k i .  P  i  =  " i m  g T  T  i  l  p  p  i  Alternatively P  may be w r i t t e n  i  The t o t a l p o t e n t i a l energy Is therefore  - I  i=l  m  g  T  T  4  p  (3.34)  f  The next step i n the d e r i v a t i o n i s to form the Lagrangian L = K-P perform the required d i f f e r e n t i a t i o n s .  and  This i s not done here, as i t does  not add anything to the understanding of the equations.  D e t a i l s are  51.  adequately covered i n Paul's book [28]. an  n  The r e s u l t i n g general equation f o r  degree of freedom manipulator i s  Z D j=l  6  1 J  +  J  n n Z Z j = l k=l  D  96  +D  =  T,  1  i=l,...n  (3.35)  1  where n Z Trace p=max I , j  D J  n D  i  * t p=max i , j , k  s j k  A  J  D  Trace  1  Terms of the form  n -Z m p=i P  3T  j  J  3T  -jf-) i  r  'soir  T  3 T  3T  2  j  J P  (3.36)  T  i f - )  k  v  (  3  -  3  7  )  i  3T  T  g --P-Pp i  (3.38)  T  3  6  P  represent e f f e c t i v e i n e r t i a at j o i n t i.  The  D^^  terms correspond to coupling i n e r t i a between j o i n t s i and j . Terms of the form  give the c e n t r i p e t a l forces at j o i n t i due to v e l o c i t i e s at j o i n t  j , and terms of the form D^j^ given the C o r i o l i s forces at i due to movement of j and k.  F i n a l l y , the  terms represent g r a v i t y l o a d i n g .  Applying Equation (3.35) to the RSI robot i s s t r a i g h t f o r w a r d . However, since the w r i s t i s being treated as a mass, the equation must be modified s l i g h t l y and load term (3.35).  w i l l appear on the LHS of Equation  The a n a l y s i s i s e s s e n t i a l l y the same as the one j u s t described,  and s p e c i f i c d e t a i l s r e l a t i n g to the RSI are covered i n Appendix IV. A p p l i c a t i o n of Equation (3.35) i s o u t l i n e d here.  Using the k i n e -  m a t i c a l d e f i n i t i o n s of s e c t i o n 3.2, one forms the transformation matrices T.  52. Appendix I I l i s t s these matrices f o r the RSI robot.  3T 9 o  matrix d i f f e r e n t i a t i o n s are performed. 3T do  3T  These terms are of the form T-T-,  2  » ^  ™  do^dOj  •  Then, the required  J  Using the pseudo i n e r t i a matrices  the matrix products i s found and terms c o l l e c t e d .  p  , the t r a c e of each of  Appendix I I I gives the  general form of J whereas Appendix V gives the i n e r t i a matrices used f o r the RSI robot, as w e l l as the center of mass vectors p r e l a t i v e to l i n k coordinates.  I t i s assumed that the center of mass of each l i n k l i e s along  one of the l i n k axes.  This i s not an unreasonable assumption given the  construction of the RSI robot.  The r e s u l t i s that the products of I n e r t i a  are zero and a l l of the o f f - d i a g o n a l terms of the J matrix cancel except f o r two.  The e f f e c t i s to considerably reduce the number of terms  i n the equations of motion, without s a c r i f i c i n g too much accuracy. Assuming that l i n k 1 i s locked i n p o s i t i o n , the following equations are obtained:  x  T  =  2  3  -  D  D  + D  2  3  +  D  2 2  e  33 3 6  + D  2  +  D  2 3  e  32 2 6  + 2D  3  +  D  2 2 3  e e  322 2 §  2  +  L  + D  3  2 3 3  e  + L  3  3  < 3  where  D  = g[a (m +m )cos8 + m x c o s 0  2  '22  2  =  ^2  2  a Z Z  3  2 2 m  2  2  2  2 a m x + ^3yy 2  2  2  2  - m z sin(8 +8 )] 3  a  (3.39)  2  3  2  3  2 3 ~ 2m z a sin8m  3  3  2  4 0  >  53. 23  =  D  233  =  ~ 2 3 3  D  233  =  -a  D  D  I  3  " a m z 8ine3  y y  2  a  3  m  z  c o s 9  3  m  3 3  c o 8 0  3  2  z  = -m gz sin(6 +8 )  3  3  D  33  D  32  D  3  2  3  3 3  s i n 6  - h yy  =  I  3  " a  y y  m 2  z  = a m z cos6  3 2 2  2  3  3  The s t r u c t u r e of L  L  3  = L  2  + L  2  3  (3.41)  3  2  and L  2 2  8  3  + L  2  i s necessarily similar  2 3  6  3  + 2L  2 2 3  e 6 2  + L 33 3 8  3  2  (3.42)  .2 3  J  =  L  3  +  L  33 3 9  L  32 2 9  L  322 2  (3.43)  9  Denoting the load mass by m^ and assuming that i t s centre of mass with respect t o l i n k 3 coordinates i s p ^ 3  L  2  = g[m^a 2  L  2 2  2  cos6 - m^z^ s i n ( 6 + e ) ] 2  _ ,  = m^[a + z^ 2  = [0 0  - 2a z^sin9 )] 2  3  3  z^  1 ] , then  54.  = m^[z^ - a z ^ s i n 9 ]  L 23  2  3  L 223 = -m.-a-z.. cosB m-.aoz-.cos8  =  —  = -m gz 1  L 33 L  32  1  sin(9 +9 ) 2  3  = m,z  =  m  ^[ ^ z  2  " 2 i a  z  s  i 9 ] n  3  (3.44)  A f i n a l comment on the dynamics i s that instead of using the Lagrang!an approach, the Newton-Euler method could have been used t o develop a simulation algorithm.  The l a t t e r approach c o n s i s t s of a set of  forward-backward r e c u r s i v e equations which can be organized i n an e f f i c i e n t manner.  The paper by Walker and Orin [35] evaluates four versions of the  algorithm.  Although computational e f f i c i e n c y may have been gained by using  a Newton-Euler algorithm, i t i s f e l t that t h i s issue i s of l e s s e r importance i n the present context.  For one t h i n g , only the motion of two and a t  most three l i n k s i s being simulated.  On the other hand, the Lagrangian  formulation provides p h y s i c a l i n s i g h t i n t o the behaviour of the system from a c o n t r o l s point of view.  3.3.3  Simulation Equations  The f i n a l step i s t o r e l a t e the torque x  m  developed by the motor of  j o i n t i , given i n Equation (3.23) to the torque x. a v a i l a b l e at the j o i n t  55.  output shaft (Equation 3.35).  Consider F i g . 14, which i s a schematic of  motor, drive t r a i n , and l i n k , and the corresponding free-body diagrams. Taking moments about the motor shaft and neglecting f r i c t i o n gives  J  where J  act^  6 = T - F.r, m^ m^ ii  (3.45)  _ i s the i n e r t i a of the i * " * actuator, F i s the contact force and act^ ' 1  r the p i n i o n r a d i u s .  On the j o i n t shaft the net torque, previously defined  as T ^ , i s simply  T  i  =  n  i i i F  r  " i i C  6  ( 3  where n^ i s the gear r a t i o and C^0^ i s a f r i c t i o n term.  *  4 6 )  E l i m i n a t i n g F and  r from the equations gives  n.J 8 i a c t ^ m^  = n.x - x - C, 6. i m^ i i i  N  (3.47) '  S u b s t i t u t i n g from Equation (3.23) gives  nK_ V a c t / i  * <TT>i»i -  nK_K  HPVmf  T  i "  C  i i 6  Motor speed and a n g u l a r a c c e l e r a t i o n a r e r e l a t e d to j o i n t a n g u l a r and a c c e l e r a t i o n by t h e g e a r r a t i o n  9 = n6 m^ ii  < ' > 3  4 8  velocity  56. and e  mi  so that the f o l l o w i n g expression Is obtained  (3.49)  This equation forms the basis f o r the dynamical s i m u l a t i o n , with given by Equation (3.35). In order to e f f e c t the s i m u l a t i o n on computer the equations of motion (3.49) must be transformed i n t o a set of f i r s t order equations.  This i s  done i n Appendix VI.  3.4  Summary Together w i t h the kinematics developed i n s e c t i o n 3.2, the dynamics  equations formulated here can be regarded as an experimental simulation package onto which d i f f e r e n t c o n t r o l l e r s could be connected.  D e t a i l s of  the computer programs are covered i n the following chapter.  This chapter  has developed the equations necessary to implement the algorithms. A t r a j e c t o r y planning scheme has been formulated using d i r e c t i o n cosines and i n i t i a l i z a t i o n parameters.  The output i s a vector p ( k ) , a lf  d i s c r e t e time s e r i e s d e s c r i b i n g the desired p o s i t i o n of the robot endpoint. The inverse kinematics have been described, such that transformed to the corresponding j o i n t coordinates  Pj|  ( k ) may  6.(k)  .  be I t has been  d  shown how the a c t u a l j o i n t p o s i t i o n 8(k) may be transformed, using d i r e c t kinematics, to a c a r t e s i a n vector x(k) f o r comparison w i t h  P„(k).  57.  A dynamics model of the robot, i n c l u d i n g motors, has been developed. The r e s u l t i n g equations are second-order, coupled, and nonlinear. i b i l i t y of the l i n k s , backlash, and s t a t i c f r i c t i o n are ignored.  FlexThese  equations may be numerically integrated so that given a c o n t r o l input u(k) and a set of i n i t i a l c o n d i t i o n s 8(k) and 6 ( k ) , the r e s u l t i n g angular positions and v e l o c i t i e s one i n t e g r a t i o n step f u r t h e r may be found, i . e . 9(k + t  step  ), 6(k + t  step  ).  The a n a l y s i s has been done w i t h respect t o an e x i s t i n g robot c a l l e d the RSI, f o r which kinematical and dynamical data i s a v a i l a b l e , enabling a r e a l i s t i c s i m u l a t i o n t o be performed.  The s i m u l a t i o n model can a c t as a  testbed f o r various c o n t r o l s t r a t e g i e s which use 6(k) and 6 ,(k) to d  calculate u(k).  Evaluation of the adaptive c o n t r o l l e r i s the t o p i c of  d i s c u s s i o n of the next chapter.  Pages 58 and 59 numbering not used.  F  2  FigS  POSITION OF POINT P RELATIVE TO TWO FRAMES  62.  FIG.9  RSI  FRAMES  63."  elbow  FIG. 10  VECTOR  DEFINITIONS  F I G . 11  WORKSPACE  Fig.12 TRAPEZOIDAL VELOCITY LAW  66.  F I G . 13 D I R E C T I O N  COSINES  67.  F I G . 14  DRIVE  SCHEMATIC  68. CHAPTER 4 EVALUATION OF THE CONTROL STRATEGY  4.1  One-Degree-of-Freedom Case Since the behaviour of the robot system r e s u l t i n g from the a c t i o n of  the proposed c o n t r o l was unknown, and since bugs can e a s i l y s l i p i n t o computer programs, i t was decided to proceed i n a modular fashion.  The  dynamics model of the RSI robot was put aside, and instead the adaptive c o n t r o l law i s tested on a simple, one-degree-of-freedom, second order system of the form J6 + C9  where  +  T . = Ku  (4.1)  a  i s some disturbance and J , C, and R are constant.  diagram of the simulated system i s shown i n F i g . 15.  A block  In the f u l l simula-  t i o n discussed i n the next s e c t i o n the dotted p o r t i o n i s one j o i n t and i t s actuator, w i t h x,j representing i n t e r a c t i o n from other l i n k s and g r a v i t y e f f e c t s ; the i n e r t i a J i s v a r i a b l e because of the movements of the other links.  For t h i s i n i t i a l study, however, J , C, K and  were assumed  constant. In f a c t , to s i m p l i f y things f u r t h e r and to g a i n i n s i g h t i n t o the behaviour of the c o n t r o l law under i d e a l circumstances, i t was f i r s t assumed that the system model parameters were known and the estimation p o r t i o n of the c o n t r o l law was shut o f f . Thus the c o n t r o l l e r has f i x e d c o e f f i c i e n t s c a l c u l a t e d according to the equations of Chapter 2.  In t h i s way i t was  possible to see i f the c o n t r o l law works at a l l f o r the c l a s s of system models i n v e s t i g a t e d here.  Also, using t h i s approach some knowledge of  performance standards i s obtained.  69.  Once these i n i t i a l t e s t s were done and a working skeleton computer program was developed, complexity was added piece by piece.  The f i r s t  module to be added to the " s i n g l e l i n k " simulation was the estimator.  This  was an important step, since t h e o r e t i c a l d e f i c i e n c i e s were l i k e l y to show up a t t h i s stage. I t was f e l t that i f the adaptation worked f o r the one-degree-of-freedom, constant parameter case, there was a good chance that i t would work f o r slowly varying parameters i n the two-link case. E v i d e n t l y , we were able to progress to t h i s stage. This s e c t i o n covers the simulations f o r the one-degree-of-freedom system under two s i t u a t i o n s : (a)  system model parameters are known and need not be estimated,  (b)  system model parameters are unknown, but constant, and the l e a s t squares estimator must be used. Equation (4.1) i s put i n a form s u i t a b l e f o r i n t e g r a t i o n by a  subroutine package [36]. Using the equation f o r the t r a n s f e r f u n c t i o n (2.9), w i t h T, a = C/J, and b = K/C, the model parameters may be c a l c u l a t e d from Equations (2.12) to (2.15), and the c o n t r o l c o e f f i c i e n t s from Equations (2.40) to (2.45). The values of J , C, and K were taken as the approximate values f o r the second l i n k of the RSI arm with  J  =  n J 2  _ + I, act zz z  2  C  = —s^-Ji  x  1.05  70. The disturbance D was taken as the g r a v i t y load of a mass of 8 kg placed at 250 mm from the axis of r o t a t i o n of the l i n k .  The r e s u l t i n g values f o r the  model parameters and c o n t r o l c o e f f i c i e n t s are given i n Table I I I f o r two sampling  periods.  Five t e s t paths were chosen to t e s t the c o n t r o l law: two step i n p u t s , two ramps, and one t r a p e z o i d a l v e l o c i t y law.  The c h a r a c t e r i s t i c s of the  t e s t reference inputs are given i n Table IV. Two programs were w r i t t e n . One simulates the known case, with no estimation, and i s o u t l i n e d i n flowchart form i n F i g . 16.  The second  simulates adaptive c o n t r o l with the estimation and design blocks of F i g . 15 tuned i n . F i g . 17 gives a flowchart of that program. Several runs were made and the s a l i e n t v a r i a b l e s are given i n Table V. The f i r s t runs t e s t the behaviour of the c o n t r o l law w i t h the exact c o e f f i c i e n t s matched to the system model.  Subsequent runs test the  adaptive strategy and i n some cases the response i s compared to the i d e a l s i t u a t i o n when the parameters are known.  4.1.2  Results are now  discussed.  Results and Discussion  E f f e c t of Varying e The f i r s t runs t e s t the e f f e c t of tuning by using the pseudo-gain e for  the known case.  Lower e means l e s s penalty on the c o n t r o l e f f o r t . , The  disturbance was set to zero, and the small step input was the reference signal. For each of the sampling times i t was attempted to f i n d an e g i v i n g s i m i l a r shapes of underdamped response, c r i t i c a l l y damped response, and overdamped response. Fig. 0.020 s.  18a,b,c shows the r e s u l t f o r T = 0.010  s and F i g . 19a,b,c f o r T =  71. The range f o r T = 0.010 s i s  O.OlxlO  e  -5  to  O.lOxlO  to  0.35xl0  -5  The range f o r T » 0.020 s i s  E  =  0.05xl0  -5  -5  If i t i s considered that the c o n t r o l e f f o r t goes as 1/e then a broader band i s obtained f o r T = 0.010 s and the system i s harder to tune. c r i t i c a l l y damped values were chosen f o r subsequent  The  runs.  I t i s seen from the response curves that the steady s t a t e e r r o r i s zero, as expected since the d i s c r e t e t r a n s f e r function contains a l / ( z - l ) f a c t o r (corresponding to 1/s i n the continuous time case).  F i g . 19d shows  the e f f e c t of s e l e c t i n g e quite low: The c o n t r o l s i g n a l shows o s c i l l a t i o n s and the system overshoots.  E f f e c t of Disturbances F i g . 20a shows the response f o r the adaptive case given a step input, T = 0.020 s. There i s a steady state error of 0.013 radians, and the steady state c o n t r o l input s e t t l e s a t approximately the value of the disturbance.  The same type of behaviour occurs at T = 0.010 s.  Thus, i t  seems that the adaptive c o n t r o l l e r i s able to measure the o f f s e t , but the response i s j u s t l i k e a p r o p o r t i o n a l c o n t r o l l e r .  Because the disturbance  enters ahead of the n a t u r a l i n t e g r a t i n g terms i n the system i t cannot be subtracted out.  The best that the system can do i s to feed- forward an  equivalent torque, as shown i n F i g . 20b. The c o n t r o l law  72.  e ( k ) + b^yCk+l) - y ( k + l ) ) U  d  - 0  i n fact when rearranged, i n t e r p r e t i n g y ( k + l ) - y ( k + l ) as an e r r o r s i g n a l d  looks l i k e a p r o p o r t i o n a l c o n t r o l l e r . When a disturbance of the same magnitude i s added t o the known c o n t r o l case the same steady state e r r o r occurs ( F i g . 20c). When the disturbance i s removed from the adaptive case the response i s just as good as i n the case of known parameters ( F i g . 22), at both sampling rates. For a l a r g e r step input, F i g . 23 shows a comparison between the responses produced by a known c o n t r o l l e r and the ones produced by the adaptive one.  A disturbance i s present i n the adaptive runs, and so a  steady state error occurs.  The r i s e time i s s l i g h t l y longer i n the  adaptive case.  Ramps Two ramp inputs were simulated and r e s u l t s between known and adaptive c o n t r o l compared.  F i g . 24a and b shows the r s u l t s .  There i s a time l a g i n  p o s i t i o n , which decreases w i t h l a r g e r g a i n ( s m a l l e r e ) .  When there i s no  disturbance the steady state error goes to zero a t the end of the ramp.  Trapezoidal Law F i g . 25 i l l u s t r a t e s t h i s case: the response of both of the c o n t r o l l e r s i s very s i m i l a r , showing that adaptation i s taking place and that the adaptive law appears t o be s t a b l e and convergent.  73.  I n t e r e s t i n g l y the parameter estimates d i d not converge t o t h e i r true values, but the c o n t r o l c o e f f i c i e n t s converged to a f i x e d  relationship  between each other.  E f f e c t of I n i t i a l Estimates In the preceding runs non-zero i n i t i a l values were used f o r the parameter estimates.  Three runs were done with zero i n i t i a l c o n d i t i o n s , and  w i t h an i n i t i a l covariance matrix of P ( 0 ) = 100,000 I .  The adap-  t i v e c o n t r o l l e r was s t a b l e , F i g . 26a, however i t became unstable a t P(0) = 10,000 I , as i l l u s t r a t e d by F i g . 26b. The c o n t r o l l e r saturated a t 10 v o l t s so the covariance matrix became s i n g u l a r , being locked i n at those values of u ( k - l ) , u(k-2).  When the upper l i m i t on u(k) was removed the  c o n t r o l l e r was once again stable, as shown i n F i g . 26c.  4.2  RSI Robot Simulation Since the adaptive c o n t r o l l e r seemed t o perform f a i r l y w e l l f o r the  single-degree-of-freedom case, where an equation of motion with constant c o e f f i c i e n t s was used, the s i m u l a t i o n was extended to the system model of the RSI robot.  Here, the i n e r t i a terms vary, and the disturbances due to  g r a v i t y , C o r i o l i s , and c e n t r i p e t a l e f f e c t s vary, s i n c e the equations a r e coupled and nonlinear.  I t was conjectured that adaptation would occur, but  the e f f e c t of the varying disturbances was uncertain.  Their magnitude  might make the c o n t r o l l e r worse; on the other hand, the fact that they are time-varying might i n j e c t enough e x c i t a t i o n i n t o the estimation and improve performance. The d e t a i l e d development of the c o n t r o l law and of the s i m u l a t i o n model equations i s done i n the previous chapters.  The segments which make  74. up the simulation programs are now b r i e f l y described.  The set-up was made  for simulating the f i r s t three l i n k s of the RSI robot, however during the simulations i t i s assumed that l i n k one i s locked i n p o s i t i o n . I t was f e l t that the major features of the behaviour of the c o n t r o l l e r could be assessed by examining the response of two i n t e r a c t i n g l i n k s .  The computer  programs were i n any case structured f o r a t h r e e - l i n k s i m u l a t i o n , and some subroutines include three l i n k s but 8j and 9^ are set t o zero. One of the segments of the simulation c o n s i s t s of the dynamics model of the RSI robot. The equations of motion are, from Equation  nK_  ..  <1T>i  i "  u  i  T  <  +  n 2 j  act>i  6  i  +  ( 3 . 4 9 )  nK K 2  ("iP  +  C  >i i 9  1  * > > l  2  3  The voltage u i s the input from the c o n t r o l l e r to the system. t i e s K^,, K , R are motor constants; n i s the gear r a t i o ; «J g  act  The q u a n t i i s the motor  i n e r t i a ; and C i s a f r i c t i o n f a c t o r . The j o i n t r o t a t i o n i s 9, and the torque T at the j o i n t shaft f o r l i n k s two and three i s given by  T  T  =  2  3  =  D + D 9 2  D  3  +  2 2  D  33 3 6  +  2  D  + D 9 2 3  3  + 2D  3 2 2 + 322 2 e  D  §  +  2 2 3  L  6 9 2  3  + D  2 3 3  9  3  + L  2  3  where the D q u a n t i t i e s contain trigonometric functions and robot p h y s i c a l parameters, and  accounts f o r a load a t the end of the t h i r d l i n k .  This dynamic model of the robot i s transformed f i r s t order equations (Appendix V I ) .  i n t o a set of equivalent  An i n t e g r a t i o n package i s used such  that, f o l l o w i n g i n i t i a l i z a t i o n , i t may be r e c u r s i v e l y c a l l e d , given u ( k ) ,  75. the input from the c o n t r o l l e r , to produce the a c t u a l response y (k+1) A  8 (k+1)  =  a  one sampling time f u r t h e r .  I t i s assumed that there i s the  equivalent of a zero-order hold i n the system, so that u(k) i s constant f o r t < kT < ( t + kT) where T i s the sampling time, and k the sampling i n s t a n t . A package c a l l e d DDE was used [36].The i n t e g r a t i o n step s i z e was set to T/20. The second segment i s the c o n t r o l block.  The c o n t r o l law was found to  be e u(k) + b [ y ( k + l | k - l ) - y ( k + l ) ] 1  d  =  0  where e i s a pseudo-gain and y^(k+l) i s the desired p o s i t i o n one step ahead. S u b s t i t u t i n g the p r e d i c t i o n y ( k + l | k - l ) given by Equation (2.37), the r e s u l t i n g c o n t r o l i s given by Equation (2.39)  u(k) =  W[y (k+l)-(c y(k-l)+c y(k-2)+c u(k-l)+c u(k-2)+c )] d  1  2  3  l4  5  where the c o e f f i c i e n t s are given by Equations (2.40) to (2.45) i n terms of a  l 2 ,a  '^2  L i m i t s of ±10 v o l t s are placed on u ( k ) , to r e f l e c t the a c t u a l s a t u r a t i o n l i m i t s of the RSI motors. The model c o e f f i c i e n t s  a^,a ,b ,b ,h 2  1  2  are estimated a t each step  using r e c u r s i v e l e a s t squares (Eq. 2.35) and these estimates used to update the c o n t r o l c o e f f i c i e n t s .  The r e c u r s i v e l e a s t squares estimation i s imple-  mented as a subroutine and follows Bierman's f a c t o r i z a t i o n of the covariance matrix [21]. The e f f e c t of varying the f o r g e t t i n g f a c t o r X, varying the i n i t i a l  9 parameter estimates, and varying the i n i t i a l value of the covariance matrix P were i n v e s t i g a t e d . The f i n a l segment of the s i m u l a t i o n , which completes the diagram given i n F i g . 4, c o n s i s t s of assorted kinematic operations.  One i s the t r a n s -  formation of the vector of a c t u a l angular p o s i t i o n s y ( k ) to the a c t u a l c a r t e s i a n p o s i t i o n z ( k ) , f o r comparison w i t h d e s i r e d c a r t e s i a n p o s i t i o n p^Ck)  of the endpiece of the RSI robot.  i s user-defined.  The vector  p^Ck) = ^ ( ^ Z  K  Given the i n i t i a l i z a t i o n parameters c o n s i s t i n g of d i r e c -  t i o n cosines, i n i t i a l p o s i t i o n , i n i t i a l v e l o c i t y , and desired v e l o c i t y p r o f i l e , the t r a j e c t o r y planning algorithm outputs p^(k) at each time k.  This desired endpiece p o s i t i o n i s transformed using the inverse k i n e -  matics subroutine i n t o the vector of j o i n t angles, the program i s organized so that  y.(k+l)  y^C*) * Timing of  i s available to calculate  d  u(k).  A flow chart of simulation program 'R' i s given on F i g . 27.  Similari-  t i e s w i t h the block schematic of F i g . 4 r e f e r r e d t o e a r l i e r may be noted. 4.2.1 Results and Discussion Three paths were chosen t o i n v e s t i g a t e the behaviour of the controller.  Results of the simulation are discussed i n each case.  Circle The f i r s t path simulated was a simple c i r c l e .  J o i n t 3 i s required t o  stay locked i n p o s i t i o n at -45°, while j o i n t 2 s t a r t s from r e s t and i s commanded t o move a t 24°/s counter clockwise. configuration.  F i g . 28 i l l u s t r a t e s the  This case i s e s s e n t i a l l y the same as the ramp input f o r the  single-degree-of-freedom  case described i n the previous s e c t i o n . The  77.  d i f f e r e n c e i s that the i n e r t i a term i s varying, that i s , the c o e f f i c i e n t of 8^, the angular a c c e l e r a t i o n .  In a d d i t i o n , the disturbances are varying.  The second l i n k must r e j e c t g r a v i t y loads, which switch signs as the arm swings around. effects. .2 D  3 22®2  A load at the end of the t h i r d l i n k w i l l aggravate these  The t h i r d l i n k , i n order t o remain s t a t i o n a r y , must r e j e c t the  disturbance as w e l l as g r a v i t y loads.  Several runs were made under d i f f e r e n t conditions.  The parameters f o r  each run are given i n Table VI. In each case the arm s t a r t s from r e s t . The quoted v e l o c i t y of 24°/s corresponds to 15 seconds f o r one r e v o l u t i o n and a t i p v e l o c i t y of about 20 cm/s. The endpoint i s chosen as the o r i g i n of the w r i s t frames.  Lower v e l o c i t i e s were t r i e d but are not included here  because p l o t t i n g would have required too many p o i n t s and i n any case the performance requirements were l e s s s t r i n g e n t .  The v e l o c i t y of 24°/s f o r  l i n k two i s probably close t o maximum performance since the motor runs a t over 80% capacity on the up legs and o c c a s i o n a l l y saturates (Fig. 29a). Several runs were required f o r tuning the pseudo-gain e which weights the  control.  Values of E = 1 . 2 x l 0 2  -6  and e = 1 . 8 x l 0 3  -6  were found to be  s a t i s f a c t o r y and were not v a r i e d from run t o run. The f o r g e t t i n g f a c t o r s were i n t i a l l y set to X = .9 f o r both l i n k s .  For the f i r s t few runs the  i n i t i a l parameter estimates 8 were chosen as the l i n e a r i z e d values about the i n i t i a l p o s i t i o n .  Covariance matrices of F ( 0 ) = 100 I 2  and P j ( 0 ) = 1000 I proved adequate i n that case. E f f e c t of Magnitude of Disturbance With the above c o n d i t i o n s , the f i r s t s i m u l a t i o n was performed by s e t t i n g g = 0 and the load mass m^ = 0.  The load mass i s a c t u a l l y the  equivalent of the w r i s t assembly of the RSI robot, s l i g h t l y over 2 kg.  78. S e t t i n g g r a v i t y to zero simulates a planar arm and reduces disturbances. F i g . 29b shows the path traced out by the endpoint: excellent p o s i t i o n a l c o n t r o l i s achieved.  The gap i s a combination of l a g and steady s t a t e  error occurring at the end of the run.  For reasons s i m i l a r to the s i n g l e -  l i n k case, the steady state e r r o r i s not r e j e c t e d , and again the c o n t r o l l e r behaves as a proportional c o n t r o l l e r , l a c k i n g i n t e g r a t i n g q u a l i t i e s .  Fig.  29c i s a p l o t of j o i n t angle 3 vs time and shows that i t i s locked at 45° as required. time.  F i g . 29d shows a plot of angle two and the c o n t r o l input vs  As expected f o r a ramp input there i s a l a g i n p o s i t i o n (of 3° -  6°).  The c o n t r o l s i g n a l i s generally  well-behaved.  A second simulation was performed, t h i s time i n c l u d i n g g r a v i t y , but no load.  The r e s u l t s are s i m i l a r as f a r as tracking i s concerned ( F i g . 29e);  however the torque requirements are more v a r i a b l e , r e q u i r i n g a higher u ( k ) 2  as the arm goes up and lower inputs as i t goes down.  The t h i r d l i n k  o s c i l l a t e s ever so s l i g h t l y about i t s desired set point ( F i g . 2 9 f ) . In the t h i r d run a load term was added and the above e f f e c t s are more pronounced, as shown by F i g . 29g.  The response, however i s good ( F i g .  29h). E f f e c t of Varying X The next two runs show the e f f e c t of varying the f o r g e t t i n g f a c t o r X i n the estimation links.  routine.  As an extreme case i t was set to .5 f o r both  (The recommended range i s 0.9 < X < 1 ) .  corresponds to very slowly varying parameters.  R e c a l l that X close to 1 F i g . 30a shows that the  c o n t r o l u ( k ) becomes jumpy and that the v e l o c i t y becomes e r r a t i c . 2  At the  other extreme, X = 0.995, the c o n t r o l i s very smooth however the system i s slow to respond around t = 11 seconds when the arm passes 9 s t a r t s to f i g h t g r a v i t y ( F i g . 30b).  2  = 270° and  79. E f f e c t of Varying I n i t i a l Estimates The f i n a l three runs tested the e f f e c t of s e t t i n g the i n i t i a l parameter estimates to zero.  In other words no knowledge of the system i s  assumed (except the o r d e r ) .  Thus a very high covariance matrix i s chosen,  P(0) = 100,000 I . From F i g . 31a, i t i s seen that l i n k 3 droops s l i g h t l y before a d j u s t i n g back to 45° a f t e r about 2.5 seconds of adaptation.  The  second l i n k s i m i l a r l y hunts around before f i n d i n g out where i t i s supposed to be headed, as evidenced by the i n i t i a l negative torque ( F i g . 31b). These r e s u l t s i n d i c a t e some degree of s t a b i l i t y and convergence of the controller. A l a r g e covariance matrix i n d i c a t e s a higher degree of u n c e r t a i n t y i n the estimates.  To i l l u s t r a t e a poor choice, i n the following run the  i n i t i a l estimates were s e t to zero but P(0) was chosen f a i r l y low - 100 I.  The r e s u l t s i s the response of F i g . 31c. A c l o s e look a t the p l o t s  of j o i n t angles vs time, however, reveals ( F i g . 31d) that j o i n t 2 converges to i t s proper c o n t r o l l e r .  J o i n t 3 stays locked a t u(k) = -10 v o l t s f o r 9  seconds, however the adaptation seems to c l i c k on at that point when the c o n t r o l l e r output switches to +10 v o l t s t o b r i n g back j o i n t 3 t o -45°. This type of behaviour i s of course unacceptable i n an a c t u a l system but i t does i n d i c a t e some robustness i n convergence.  I t i s speculated that i f  bounds on u(k) were not present, adaptation would take place a l o t f a s t e r . An a l t e r n a t i v e t o powerful actuators i s a d j u s t i n g the adaptation speed via  the f o r g e t t i n g f a c t o r A. A few values were t r i e d f o r j o i n t 3, since  the response of j o i n t 2 was acceptable.  At X, = 0.85, j o i n t 3 i n i t i a l l y  drooped but re-adjusted to w i t h i n one degree of i t s desired value a f t e r 2 seconds, even w i t h P(0)= 100 I and i n i t i a l 8 = [ 0 ] .  80.  Summary The above r e s u l t s were encouraging.  Good response and adaptation were  achieved when the i n i t i a l parameter estimates were chosen as the l i n e a r i z e d values of the i n i t i a l p o s i t i o n , despite the presence of g r a v i t y and other disturbances.  A f o r g e t t i n g f a c t o r close to 1 gives smoother c o n t r o l  s i g n a l s but the system i s slow to respond to a disturbance.  Zero i n i t i a l  estimates of the model parameters requires that care be exercised i n choosing  X and P ( 0 ) . However, convergent and s t a b l e behaviour was observed.  L i n e at 45° The second t e s t path involved motion of both l i n k s . the arm was required to trace a l i n e at 45° i n the coordinates. the robot.  X Q ~ Z Q  The endpoint of plane of base  F i g . 32 i l l u s t r a t e s the i n i t i a l and f i n a l c o n f i g u r a t i o n of The arm s t a r t s at r e s t with l i n k s 2 and 3 out-stretched ( 6 0 =  2  and 6 =-90°). A choice i s s p e c i f i e d f o r e i t h e r elbow up or elbow down 3  t r a c i n g of the l i n e .  The endpoint i s to f o l l o w a t r a p e z o i d a l v e l o c i t y law:  a c c e l e r a t i o n to the c r u i s e v e l o c i t y i n t j seconds, constant v e l o c i t y up to time t = t  2  and then d e c e l e r a t i o n to bring the t i p p o s i t i o n to r e s t on the  desired target point at time t . 3  VII.  The run parameters are given i n Table  Although the t i p v e l o c i t i e s may  appear slow (5 cm/s  or 15  cm/s  depending on the run) the j o i n t r a t e f o r l i n k 3 bcomes q u i t e high and the motor saturates during a p o r t i o n of the path.  This s i t u a t i o n i l l u s t r a t e s  the importance of (1) good t r a j e c t o r y planners which foresee l i m i t a t i o n s on j o i n t rates i n c e r t a i n areas of the workspace, and (2) proper design of the k i n e m a t i c a l s t r u c t u r e of the robot so that the workspace i s usable. E v i d e n t l y the complications increase when more degrees of freedom are involved.  81.  For each run, X  = X  2  3  = 0.9  and the gains are  E  2  = 1.2xl0  - 6  and  1.8xl0 . - 6  Low Cruise V e l o c i t y The f i r s t run includes g r a v i t y and a load mass, and i s done under conservative conditions of a c c e l e r a t i o n of 2.5 cm/s to reach a c r u i s e 2  v e l o c i t y of 5 cm/s.  The i n i t i a l conditions are s u b s t i t u t e d i n the equa-  t i o n s of motion to obtain i n i t i a l model parameter estimates. ance parameter h i s s e t to zero. Fig.  The d i s t u r b -  The r e s u l t i n g endpoint trace i s shown i n  33a, where a reference l i n e i s i n d i c a t e d as w e l l f o r convenience.  Maximum d e v i a t i o n from the desired p o s i t i o n occurs a t the beginning and i s about 7 mm, decreasing angles vs time.  s t e a d i l y to 1 mm or l e s s .  F i g . 33b show j o i n t  The c o n t r o l l e r was l e f t on f o r 2 seconds past the  lO-second simulation time; steady state e r r o r decreases s l i g h t l y . In the second run conditions are the same except f o r higher a c c e l e r a t i o n of 5 cm/s . 2  The c r u i s e v e l o c i t y i s s t i l l 5 cm/s.  t i o n from the path i s worse a t the beginning  initial The devia-  ( F i g . 33c) possibly due to  the i n i t i a l disturbance parameter h being set to zero.  The motor f o r j o i n t  3 saturates from t = .68 s to t = 1.84 s because the geometry requires high j o i n t rates i n that i n t e r v a l , as shown i n F i g . 33d. The same conditions were s p e c i f i e d f o r the t h i r d run except that the elbow up s p e c i f i c a t i o n was given instead of elbow down.  Path f o l l o w i n g  improves a t the beginning, as shown i n F i g . 33e. The explanation f o r t h i s i s that the i n i t i a l motion of l i n k 2 i s up, rather than down, and l i n k 3 does not have a tendency to be dragged down. good.  F i n a l p o s i t i o n a l accuracy i s  82.  High Cruise V e l o c i t y In the f o u r t h s i m u l a t i o n run the c r u i s e v e l o c i t y i s increased to 15 cm/s.  The response shown i n F i g . 33f i s s i m i l a r to that of the other runs.  Comparing voltage l e v e l s , p l o t t e d i n F i g . 33g, shows that f a s t e r v a r i a t i o n s are required at the higher v e l o c i t y .  The i n i t i a l covariance matrix  estimates were increased t o 10,000 I , but l i t t l e improvement occurs i n the d e v i a t i o n pattern at the beginning. ing  I n the f i f t h run g r a v i t y and l o a d -  e f f e c t s were removed, and much b e t t e r performance r e s u l t s , as the p l o t  of F i g . 33h i n d i c a t e s .  Thus one may conclude that i t takes some time f o r  the c o n t r o l l e r t o adjust t o the disturbances. F i n a l l y , a run was made with zero i n i t i a l estimates f o r the model parameters and a high P ( 0 ) . R e s u l t i n g t r a j e c t o r y of the endpoint i s shown i n F i g . 3 3 i . Stable and adaptive behaviour of the c o n t r o l l e r once again occurs.  Summary For the 45° l i n e the two j o i n t c o n t r o l l e r s succeed i n achieving path tracking w i t h i n 3 mm with the exception of the i n i t i a l start-up period where e r r o r s as high as 7 mm occur.  High j o i n t r a t e s i n l i n k 3 preclude  t i p v e l o c i t i e s higher than 15 cm/s from being achieved. model parameter estimates of zero, the c o n t r o l l e r s adapt.  Even with i n i t i a l Steady s t a t e  accuracy i s w i t h i n .5° f o r each j o i n t .  Horizontal Line The f i n a l t e s t path i s a h o r i z o n t a l l i n e .  The robot s t a r t s from r e s t  with j o i n t 2 a t 60° and j o i n t 3 a t -120°, as shown i n F i g . 34a. A t r a p e z o i d a l v e l o c i t y law i s followed, as described f o r the previous l i n e  83.  path. the  F i n a l c o n f i g u r a t i o n i s 6 = 96.5° and 8 = -180°. 2  run parameters.  3  Table V I I I gives  This path was more d i f f i c u l t to tune and at the f a s t e r  v e l o c i t y i t was necessary t o decrease e, which means that the c o n t r o l e f f o r t was penalized l e s s than f o r e i t h e r the c i r c l e or 45°-line case. Better performance r e s u l t e d w i t h higher i n i t i a l covariances. In the f i r s t run the planar, no load arm was simulated with A = .99 and c r u i s e v e l o c i t y 6.3 cm/s. Maximum e r r o r i s 4 mm.  No d e f i n i t e trends  were evident when e i t h e r A or P(0) were v a r i e d ( F i g . 35a). In the next run load and g r a v i t y were re-introduced by the t i p v e l o c i t y was reduced.  Maximum error decreased to 2 mm, with A = .9, as  seen i n F i g . 35b. Thus v e l o c i t y disturbances seem to predominate f o r t h i s path.  Varying e The pseudo-gain e was the next parameter which was v a r i e d . i t by a f a c t o r of ten improved performance s i g n i f i c a n t l y . a c t u a l paths are p r a t i c a l l y i n d i s t i n g u i s h a b l e .  Decreasing  The desired and  I t was p o s s i b l e t o adjust  e f o r t h i s path because the required range of c o n t r o l signals u(k) i s small. tion.  Hence, there I s no danger of prolonged periods of actuator saturaTwo runs are shown here.  The f i r s t , F i g . 35c was f o r X = 0.99 and  the other, F i g . 35d, was f o r X = 0.85. The lower f o r g e t t i n g f a c t o r r e s u l t s i n a s l i g h t l y jagged t r a j e c t o r y but absolute error i s as good as f o r X = 0.99.  Varying P(0) had no s i g n i f i c a n t e f f e c t e i t h e r . The f i n a l run described was the r e s u l t of an oversight but i t shows  that the adaptive c o n t r o l l e r i s extremely robust and gives strong i n d i c a tions that i t i s both stable and convergent.  The simulation data f i l e s  were i n i t i a l i z e d f o r 6, = 60° and 6, « -120°, which corresponds t o an elbow  84.  up c o n f i g u r a t i o n .  However, the elbow s p e c i f i c a t i o n f o r the t r a j e c t o r y was  given as "down" ( F i g . 34b). The r e s u l t i n g endpoint movement i s shown i n F i g . 35e. I t i n d i c a t e s that the arm whips down, and then returns to f o l l o w the s p e c i f i e d path to w i t h i n 0.1 mm.  85. Table I I I . Discrete Model Parameters and C o e f f i c i e n t s for the Known Case  Parameter  Sampling Time 0.010 s - 1.8339  l  a  a  2  l b  c  C  - 1.6955  0.8339  0.6955  4.736xl0  b  c  0.020 s  2  l 2 3  -5  4.458xl0"  5  2.5294  1.788xl0  _1+  1.584x10-'* 2.1791  - 1.5294  - 1.1791  1.314xl0  _lt  4.616xl0 ' _  +  8.176xl0  _Jt  2.686xl0  -lt  K/J  =  1.0055  C/J  =  18.1589  86.  Table IV. Test Paths f o r the Single-Link Simulation  Reference Input Function y ^ ( t ) Time I n t e r v a l , Name  [rad]  [8]  [deg]  STEP 1  0 < t < 5  0.15  8.6  STEP 2  0 < t < 5  0.50  28.6  RAMP 1  0 < t < 5  0.10 t  RAMP 2  0 < t < 5  0.40 t  TRAPEZOID  0 < t < 1.6  0.125 t  5.7 t 22.9 t  1.6 < t < 3.6  0.32 + 0.4(t-1.6)  3.6 < t < 5.2  1.12 + 0.4(t-3.6) - .125(t-3.6)  5.2 < t < 7.5  Note:  7.2 t  2  2  18.3 + 22.9(t-1.6) 64.2 + 22.9(t-3.6) - 7.2(t-3.6)  2  1.44  I n a l l cases y(t=0) = 0  82.5  2  jr. Table V. Run Parameters f o r the Single Degree of Freedom Simulation  Run No.  T samp  e  Path  2,3,4  0.010  variable  STEP 1  0  5,6,7,13  0.020  variable  STEP 1  0  8,9  0.020  c r i t . damped  STEP 1  .859  10  0.010  c r i t . damped  STEP 1  .859  11  0.020  c r i t . damped  STEP 1  0  12  0.010  c r i t . damped  STEP 1  0  14  0.020  c r i t . damped  STEP 2  .859  15  0.020  c r i t . damped  RAMP 1  .859  16  0.020  c r i t . damped  RAMP 2  .859  17  0.020  c r i t . damped  TZOID  .859  D/K  88.  Table VI.  Run No.  g [m-s ] -1  C i r c l e Trajectory - Parameters of the Simulation Runs  l [kg] m  X  3  P (0)  *3<°>  2  I n i t i a l Estimates  21  0  0  .9  .9  100  1,000  non-zero  22  9.81  0  .9  .9  100  1,000  non-zero  23  9.81  2.046  .9  .9  100  1,000  non-zero  28  9.81  2.046  .5  .5  1,000  1,000  non-zero  29  9.81  2.046  .995  .995  1,000  1,000  non-zero  30  9.81  2.046  .95  .95  100,000  100,000  0  31  9.81  2.046  .95  .95  100  100  0  32  9.81  2.046  .95  • 85  100  100  0  In a l l runs: gain i n i t i a l velocity i n i t i a l position desired v e l o c i t y  e  2  = .12 x 1 0  e  3  = .18  y  2  =  0  y  3  =  0  y  2  =  0  y  3  =  -45°  y  2  =  0.42 rad/s  y  3  =  0  =  15 s  -  0.020 s  =  0.001 s  time f o r one r e v o l u t i o n sampling time i n t e g r a t i o n step  t samp t step  x l o  - 5  -  5  Table V I I .  Run No.  g  m  [m-s ] -1  Line a t 45° - Parameters of the Simulation Runs  P (0)  l  2  *3<°>  I n i t i a l Estimates A  [kg]  A  £  A  A  -2  *l » a » i » 2 ' b  a [m-s ]  b  2  V  2  fc  C i  [s]  [m-s ] x  3 elbow  t  24  9.81  2.046  100  1,000  non-zero  .025  .05  2  8  25  9.81  2.046  100  1,000  non-zero  .05  .05  1  6  7 down  26  9.81  2.046  100  1,000  non-zero  .05  .05  1  6  7 up  33  9.81  2.046  10,000  10,000  non-zero  .05  .15  3  4  7  34  0  0  10,000  10,000  non-zero  .05  .15  3  4  7 down  35  9.81  2.046  0  .05  .15  3  4  7  100,000 100,000  In a l l runs: gain forgetting  factor  e  2  .12  x  10  e  3  .18  x  10  x  =  0.9  h  =  0.9  =  0  =  0  ES  0  =  -90°  =  0.020 s  =  0.001 s  =  0.020 s  2  i n i t i a l velocity h  initial  position  v  2  v  3  desired v e l o c i t y h  sampling time  t  i n t e g r a t i o n step  t  d i r e c t i o n cosines  samp = step = a e  0.001 s 180° 90°  10  down  down down  Table V I I I .  Run No.  g  [m-s ] -1  H o r i z o n t a l Line - Parameters of the Simulation Runs  2'3<°>  p  "l  e  2  e  3  elbow  V  2 [s]  fc  O i 1]  [m-s  [kg]  H  27  0  0  1,000  .12  .18  up  .063  1  8  9  36  9.81  2.046  1,000  .12  .18  up  .05  2  6  8  37  0  0  10,000  .012  .012  down  .05  2  6  8  39  9.81  2.046  10,000  .012  .012  up  .15  2  6  8  40  9.81  2.046  10,000  .012  .012  up  .15  2  6  8  In a l l runs: i n i t i a l velocity initial  position  Y  v  .  2  sampling  time  integration acceleration  step  0 0 60°  3 a  -120°  8  90°  v  d i r e c t i o n cosines  2  samp 'step a  180°  0.020 s 0.001 s 0.025 m/s  2  91.  a) clock  yjW  +  •9"  Digital Controller  u(k)  u(t) D/A  G(s)  b)  o-  Digital Controller  u(k)  Fig.15 SAMPLED DATA SYSTEM a) sampling a continuous time system b) equivalent discrete representation  G(z)  y(k)  y(k)  y(t) A/D  92.  wfr'TE T D  P i 6?.  10  PLOT  £\L£$  P£0&£AM  x  r ° '  Fi-c^C^AST.  PROGRAM *Q' START  READ iNPdTpUES INITIALIZE VA/RiTE TO C O M IVJPOPM6.T\'O»J PiLE  <TA.LL RETURN!  "PATH g«i  C A L C U L A T E WRITE  s.  6L  TO PLOT  Ck)  PILES  i£, < 4 4 4 c  UPDATE  5  <£(*0  ciAL^ULATE, COKJTAOLL,ed CoEFPiCiEMTS t'A/.X. »MT£6RATl'cOK-> SU6R, flETUftKJ viO«*i> v/fe+i)  go  17  TO  PROGRAM V  fop  FL©WCWA,R-T  0.20  to 2  0.00 -0.05 2  3  TIME  (sec)  CO  O > r-  cc O  LILILU _J O  QC Z  o u  FIG.18a  2  3  TIME  (sec)  Known Control - run 2  FIG.18b  Known Control - run 3  0.20 co c  0.15  £  0.10  Q  'I -I  CO  2  0.00  /  \  /  \  0.05-  t  /  \  CD  I  /  \  V  /  -0.05 0  1  2  3  TIME  (sec)  CO  5  8  r-  QC  O  LU LU LU I  o QC  r-  Z  o u  -r 2  TIME FIG.18c  (sec)  Known Control - run 4  4  5  FIG. 19a  Known control T^O.020 s epsilon= 0.20E-05  0.20  CO  12  >  8  cc O LL UL LU _J  o rr  O O  2  3  TIME  (sec)  4H  0  -4 -8H -12  FIG.19b  2  3  TIME  (sec)  Known Control - run 6  FIG.19C  Known Control - run 7  0.20  2 TIME  3 (sec)  2 TIME  3 (sec)  12 O >  8H  4  O u_  0  LU -J  O  -4-  D C  -8  O  -12  FIG.19d  1^  4  Known Control - run 13  0.20  to c to  O  0.15  0.05-  CO  2  'I •I I  \ \ \  f  o.oo  v  I  -0.05  FIG.20  -  —  I  I  2  3  TIME  (sec)  TIME  (sec)  1 1 1  _ y  I  4  Adaptive Control - run 8  TIME  FIG.20c  (sec)  Known Control - run 9  103.  0.20  O  0.05  CO  2  0.00  -0.05  2 TIME  3 (sec)  r  4  1X01 s  0.20  c CO  •5 co O H CO  O fx.  T = .02s FfG.22  ADAPTIVE  D= 0  0.75  0.00-  TIME  (sec)  TIME  (sec)  0.75  0.00  FIG.23  Known vs Adaptive Control - run 14  0.20  TIME  (sec)  TIME  (sec)  0.20  FIG.24a  Known vs Adaptive Control - run 15  1.5  TIME  (sec)  TIME  (sec)  1.5  FIG.24b  Known vs Adaptive Control - run 16  107.  1.76 1.50-  TIME  (sec)  TIME  (sec)  1.75 1.50-  FIG 25  Known vs Adaptive Control - run 17  1.81.6-  TIME FIG. 26a  (sec)  Adaptive Control zero Initial estimates, P=100,000  109.  1.5  n  0  1  1  FIG. 26c  1  2  1  3  1  4  TIME  (sec)  TIME  (sec)  1  5  1  6  Adaptive Control initial estimates zero, P"=10000  1  7  PROGRAM  V  R'  INPUT  WRiTE  TO  ROKJ  itJFoaMATtOO  STOP  \VAiT£  TP  PljDT  PI'UES  PATH  tzAU.  •CALL  IhiVEfi-SB  cCALCULAT£ UJAI'TTE  T O  UPDATE  ^OJvePT  KlNi  lL(Ja.)  P L O T  P<L£"$  \Q (fe}  T P  £A(Ll£<->\ArJ  \  FIG. 28  CIRCLE  CONTROL EFFORT to  CO  (volts) 03  I  POSITION (radians)  I  -0.6  1  -0.4  FIG.29b  1  -0.2  !  ,  0.0 0.2 X o <m)  Endpoint Trace - run 21  p-  0 4  -0.763 -  • -  A /A  co  c  CD  a  co -0.785-  CO O  o.  -0.787  FIG. 29f  6  e  TIME  (sec)  circle - joint tV3  -0.78535  co c co T5 CD  z o  -0.78540-  CO o  o. -0.78545 TIME  FIG. 29c  (sec)  circle - joint 3  12  15  CONTROL EFFORT K>  00  -tk  O  *  (volts) 00  h  CONTROL EFFORT W  00  A  O  •&»  (volts) 03  NJ  116.  0.6-1  n  -0.6  i  i  -0.4  -0.2  ;  Xo  FIG.29e  I  1  1  0.0  0.2  0.4  (m)  Endpoint Trace - run 22  1  0.6  118.  FIG.29h  Endpoint Trace - run 23  ANG. VELOCITY (,ad/s)  CONTROL EFFORT  (volts)  TIME  TIME  FIG. 30b  (sec)  (sec)  circle - joint 2  TIME  FIG. 31a  circle — joint 3  TIME  FIG. 31b  (sec)  (sec)  circle - joint 2  122.  FIG.31C  Endpoint Trace - run 31  8  i  i  0  3  i  6 TIME  I  9  I  I  12  15  12  15  (sec)  circle — joint 2  0  3  6 TIME  FIG. 31d  9 (sec)  circle — joint 3  125.  0.4-,  0.3H  0.2H  O.OH  -O.H  '  r  0-1  FIG.33a  0.2  1  0.3 Xo  — i  0.4 (m)  Endpoint Trace - run 24  1  0.5  —|-  06  45 degree line - joint 2  FIG. 33b  line at 45 deg. — joint 3  127.  0.4-,  0.3-{  0.2-\  E  0.1O Nl  0.0H  -o.H  -0.2-|  0.1  1  0.2  r—  0.3  Xo  FIG.33C  1  1  0.4  0.5  (m)  Endpoint Trace - run 25  1  0.6  128.  TIME  (sec)  45 degree line - joint 3  FIG. 33d  45 degree line - joint 3  129.  •3H  •2H  o.H  O.OH o.H  O-| 2  0.1  ,  0.2  1  0.3  Xo  FIG.33£  1  0.4  (m)  Endpoint Trace - run 26  1  0.5  1  0.6  FIG.33f  Endpoint Trace - run 33  TIME  (sec)  45 degree line - joint 2  TIME  FIG. 33g  (sec)  45 degree line - joint 3  0.5-1  i  0.1  FIG.33h  l  0.2  l  0.3 Xo  l  0.4 (m)  Endpoint Trace - run 34  I  0.5  I  0.6  133.  0.5-1  -0.1-1 0.1  FIG.33i  1 0.2  1 0.3 Xo  1 0.4  (m)  Endpoint Trace - run 35  1 0.5  1 0.6  ,1  FIG. 3 4  HORIZONTAL  LINE  0.475-.  0.450H  0.425-  0.400-  0.375H  0.350  0.325-|  -0.1  T  0.0  1  1  0.1 Xo  FIG.35a  0.2 (m)  Endpoint Trace - run 27  1  0.3  I  0.4  136.  0.475 -i  0.450  0.425H  O N  0.400H  0.375H  0.350H  0.325-  -0.1  FIG.35b  0.0  —I  0.1 Xo  1—  0.2 (m)  Endpoint Trace - run 36  — i  0.3  1  0.4  0.350-  -0.1  FIG.35c  1  0.0  1  0.1 Xo  r  0.2 (m)  Endpoint Trace - run 39  — i  0.3  1  0.4  138.  0.475-.  0.450M  0.425M  O M  0.400H  0.375  0.350  0.325-  -0.1  0.0  FIG.35d  0.1 Xo  0.2 (m)  Endpoint Trace - run 40  —I  0.3  1 0.4  0.425  N  0.375H  0.350H  0.325H  0.300 H  0.275-)— 0.0  0.5  Xo  FIG.35e  (m)  Endpoint Trace - run 37  140. CHAPTER 5 CONCLUSIONS 5.1  Conclusions The following conclusions may be drawn:  i)  A computer s i m u l a t i o n package of a robot system has been developed which w i l l enable a user to t e s t various c o n t r o l s t r a t e g i e s . model i s based on an e x i s t i n g robot. l i n k f l e x i b i l i t y are not modelled.  The  S t a t i c f r i c t i o n , backlash, and Features of the simulator i n c l u d e :  (a)  an elementary t r a j e c t o r y planner  (b)  an inverse kinematics subroutine  (c)  d i r e c t kinematics f o r endpoint movement monitoring  (d)  the manipulator and actuator dynamics  (e)  a c o n t r o l l e r , at present adaptive  (f)  input f i l e s f o r varying dynamical parameters and i n i t i a l conditions.  The s i m u l a t i o n package has been s t r u c t u r e d f o r three l i n k s w i t h load mass.  I t could be used as a t e s t bed f o r evaluating d i f f e r e n t c o n t r o l  laws. ii)  An e x p l i c i t adaptive c o n t r o l l e r was derived from f i r s t p r i n c i p l e s . The c o n t r o l l e r i s based on l i n e a r modelling of each l i n k using an ARMA model, r e c u r s i v e l e a s t squares parameter estimation, and o p t i m i z a t i o n of a quadratic performance index.  In contrast t o Koivo's [13] v e l o -  c i t y c o n t r o l l e r , i n which approximate compensation f o r p o s i t i o n e r r o r i s necessary, t h i s work uses p o s i t i o n c o n t r o l d i r e c t l y , such that these compensations are avoided.  141. i i i ) The c o n t r o l law i s tested using the simulation package f o r degrees-of-freedom and motion along three paths.  two  Results i n d i c a t e  that the c o n t r o l l e r adapts and that i t i s stable and convergent.  The  c o n t r o l e f f o r t i s w e l l behaved, e x h i b i t i n g no chatter or high frequency components. iv)  The r e s u l t s i n d i c a t e that the type of c o n t r o l law proposed here could be a f e a s i b l e approach to improved robot c o n t r o l .  5.2  Recommendations f o r Future Work These may  be divided i n t o two groups: f i r s t , possible e l a b o r a t i o n  on  the e x i s t i n g simulator; and second, extensions r e q u i r i n g more t h e o r e t i c a l development. i)  Very l i t t l e e f f o r t would be needed to extend the simulator to three degrees-of-freedom, c o n s i s t i n g only of (1) obtaining and programming the equations of motion i n the form of a subroutine c a l l e a b l e by  the  i n t e g r a t i o n package and (2) v e r i f y i n g the quadrant s p e c i f i c a t i o n s f o r the inverse kinematics subroutine. ii)  A s e n s i t i v i t y a n a l y s i s could be done, keeping the same c o n t r o l law  but  modifying the dynamical parameters of the robot. I i i ) The simulation runs done i n t h i s work involved parameter estimation each c o n t r o l step.  at  An analysis could be done on the e f f e c t of e s t i -  mating l e s s often than i s c o n t r o l l e d .  I f performance does not  degrade, t h i s set-up i s advantageous since the computational burden i s reduced.  142. iv)  An a n a l y s i s of the number of computations necessary to implement the proposed c o n t r o l law was not c a r r i e d out, and hence comparisons w i t h other laws i s not p o s s i b l e .  As f a r as f e a s i b i l i t y i s concerned,  computational burden i s an important aspect, although advances i n computer technology are making t h i s l e s s c r i t i c a l every year. v)  Since the o f f s e t or bias term does not seem to be e f f e c t i v e l y e l i m i n a ted by the parameter h i n a l l cases, the disturbances may need to be modelled i n a d i f f e r e n t way, f o r example as f i l t e r e d white noise v(k)  =  CCq" ) e(k) 1  This approach would lead t o parameter estimation using extended l e a s t squares, f o r which convergence r e s u l t s are a v a i l a b l e [18]. vi)  The c o n t r o l law which r e s u l t s by optimizing the performance c r i t e r i o n I  -  {(y (k+l) - y(k+l)  2  d  +  does not contain any i n t e g r a t i n g terms.  eu (k)|(k-l)} 2  In p r a c t i c e , feedback must  include an i n t e g r a t o r , otherwise unacceptably large steady s t a t e errors r e s u l t due to unmodelled e f f e c t s such as f r i c t i o n . i n t e g r a t i n g term can be achieved by choosing a modified I a term of the form y(u(k) - u ( k - l ) ) . 2  Adding an containing  Of course, complexity w i l l  increase and so w i l l the number of c a l c u l a t i o n s necessary to a r r i v e a t u(k). v i i ) As an a l t e r n a t i v e to adding more terms to the performance c r i t e r i o n , a d i f f e r e n t structure of adaptive c o n t r o l could be examined.  Instead of  using adaptive c o n t r o l to c o n t r o l each j o i n t d i r e c t l y the adaptation  143. would take place one h i e r a r c h i c a l l e v e l higher.  C l a s s i c a l analogue  PID servo loops at each j o i n t , a f t e r a l l , perform quite w e l l once they are tuned to the o p e r a t i o n a l c h a r a c t e r i s t i c s of the p a r t i c u l a r task. Their l i m i t a t i o n i s that as the configurations of the task change performance degrades because the loop gains are no longer optimum. One can envisage a supervisory adaptive c o n t r o l l e r which would a l t e r the gains of the feedback loops as operating c o n d i t i o n s change.  The  problem, however, would have to be cast i n a d i f f e r e n t form t h e o r e t i c a l l y than i s presented i n t h i s work.  v i i i ) E x t e n s i o n to endpoint c o n t r o l i s perhaps the most i n t e r e s t i n g aspect, but a m u l t i v a r i a b l e approach i s necessary.  In other words the p o l y -  n o m i a l s A ( q ) and B ( q ) are replaced by matrix polynomials. - 1  - 1  This  approach i s e s p e c i a l l y amenable t o robots w i t h s p h e r i c a l w r i s t s s i n c e the inverse kinematics can be s p l i t at the w r i s t , so that (3x3) systems can be studied.  144.  BIBLIOGRAPHY 1.  Whitney, D.E. "The Mathematics of Coordinated Control of P r o s t h e s i s and Manipulators", ASME Journal of Dynamic Systems, Measurement, and C o n t r o l , Dec. 1972, pp. 303-309.  2.  Luh, J.Y.S., Walker, M.W. and P a u l , R.P., "Resolved A c c e l e r a t i o n C o n t r o l of Mechanical Manipulators", IEEE Trans, on Automatic C o n t r o l , V o l . 25, No. 3, pp. 468-474.  3.  Freund, E. "Fast Nonlinear Control with A r b i t r a r y Pole Placement f o r I n d u s t r i a l Robots and Manipulators", Robotics Research, V o l . 1, No. 1, 1982, pp. 65-78.  4.  G o l l a , D.F., Garg, S.C. and Hughes, P.C. "Linear State-Feedback Control of Manipulators", Mech. Machine Theory, V o l . 16, 1981, pp. 93-103.  5.  Cvetkovic, V. and Vukobratovic, M. "One Robust, Dynamic C o n t r o l Algorithm f o r Manipulation Systems," I n t e r n a t i o n a l J o u r n a l of Robotics Research, V o l . 1, Winter 1982, pp. 15-28.  6.  Vukobratovic, M. and S t o k i c , D. "Is Dynamic Control Needed i n Robotic Systems, and i f so, t o What Extent?", I n t e r n a t i o n a l J o u r n a l of Robotics Research, V o l . 2, Summer 1983, pp. 18-34.  7.  Hogan, N. and Cotter, S.L. "Cartesian Impedance Control of a Nonl i n e a r Manipulator", WAM Robotics Symposium, 1982.  8.  Lee, C.S.G. and Chung, M.J. "An Adaptive Control Strategy f o r Mechanical Manipulators", IEEE Transactions on Automatic C o n t r o l .  9.  B a l e s t r i n o , A., De Maria, G. and S c i a v i c c o , L. "An Adaptive Model Following Control f o r Robotic Manipulators", ASME Journal of Dynamic Systems, Measurement, and C o n t r o l , V o l . 105, Sept. 1983, pp. 143-151.  10.  N i c o s i a , S. and Tomei, P. "Model Reference Adaptive Control Algorithms f o r I n d u s t r i a l Robots", Automatica, V o l . 20, No. 5, 1984, pp. 635-644.  11.  Kim, B'.K. and Shin, K.G. "Suboptimal Control of I n d u s t r i a l Manipul a t o r s w i t h a Weighted Minimum Time-Fuel C r i t e r i o n " , IEEE Trans, on Automatic C o n t r o l , V o l . AC-30, No. 1, 1985, pp. 1-10.  12.  Young, K.K.D. " C o n t r o l l e r Design f o r a Manipulator Using Theory of V a r i a b l e Structure Systems", IEEE Trans., V o l . SMC-8, Feb. 1978, pp. 101-109.  13.  Koivo, A.J. and Guo, T. "Adaptive Linear C o n t r o l l e r f o r Robotic Manipulators", IEEE Trans, on Automatic C o n t r o l , V o l . AC-28, Feb. 1983, pp. 162-171.  145.  14.  Lobbezoo, A.J., e t a l . "Robot Control Using Adaptive Transformat i o n s " , U.B.C. Technical Paper, June 1984.  15.  Astrom, K.J. "Theory and A p p l i c a t i o n s of Adaptive Control - A Survey", Automatica, V o l . 19, May 1983, pp. 471-486.  16.  Landau, I.E. "Model Reference Adaptive C o n t r o l l e r s and Stochastic Self-Tuning Regulators - A U n i f i e d Approach", ASME Journal of Dynamic Systems, Measurement, and C o n t r o l , V o l . 103, Dec. 1981, pp. 404-416.  17.  Egardt, B. " U n i f i c a t i o n of Some Discrete Time Adaptive Control Schemes," IEEE Trans, on Automatic C o n t r o l , V o l . AC-25, 1980, pp. 697-703.  18.  Ljung, L. and Soderstrom, T. Theory and P r a c t i c e of Recursive I d e n t i f i c a t i o n , MIT Press, Cambridge, Mass., 1983.  19.  Isermann, R.  20.  Astrom, K.J., Borisson, U., Ljung, L. and Wittenmark, B. "Theory and A p p l i c a t i o n s of Self-Tuning Regulators", Automatica, V o l . 13, 1977, pp. 457-476.  21.  Bierman, G.J. F a c t o r i z a t i o n Methods f o r Discrete Sequential Estimation, Academic Press, New York, 1977.  22.  Bar-Shalom, Y. and Tse, E. "Dual E f f e c t , Certainty Equivalence and Separation i n Stochastic C o n t r o l " , IEEE Trans, on Automatic C o n t r o l , V o l . AC-19, 1974, pp. 494-500.  23.  Goodwin, G.C. and S i n , K.S. "Adaptive F i l t e r i n g , P r e d i c t i o n , and C o n t r o l " , P r e n t i c e - H a l l , Englewood C l i f f s , N.J., 1983.  24.  Egardt, B. S t a b i l i t y of Adaptive C o n t r o l l e r s , (Lecture Notes i n Control and Information S e r i e s ) , No. 20, Springer-Verlag, B e r l i n , 1979.  25.  Narendra, K.S. and L i n , Y.H. "Stable Discrete Adaptive C o n t r o l " , IEEE Trans, on Automatic C o n t r o l , V o l . AC-25, 1980, pp. 456-461.  26.  Goodwin, G.C, Ramadge, P.J. and Caines, P.E. "Discrete Time Stochastic Adaptive C o n t r o l " , SIAM Journal of Control and Optimization, V o l . 19, pp. 829-853.  27.  A l l e n , C. "Moments of I n e r t i a of the RT-3 Robot", UBC Technical Report, Aug. 1984.  28.  Paul, R.P. Robot Manipulators: Mathematics, Programming, and C o n t r o l , 1st ed., MIT Press, Cambridge, Mass., 1981.  29.  Lee, G.C.S. "Robot Arm Kinematics, Dynamics, and Control", Computer, Dec. 1982, pp. 62-80.  D i g i t a l C o n t r o l Systems, Springer-Verlag, B e r l i n 1981.  146. 30.  Denavit, J . and Hartenburg, R.S. "A Kinematic Notation f o r Lower-Pair Mechanisms Based on Matrices", ASME Journal of Applied Mechanics, V o l . 22, June 1955, pp. 215-221.  31.  Hollerbach, J.M. and Sahar, G. " W r i s t - P a r t i t i o n e d , Inverse Kinematic A c c e l e r a t i o n s and Manipulator Dynamics", I n t e r n a t i o n a l J o u r n a l of Robotics Research", V o l . 2, Winter 1983, pp. 61-76.  32.  Featherstone, R. " P o s i t i o n and V e l o c i t y Transformations Between Robot End-Effector Coordinates and J o i n t Angles", I n t e r n a t i o n a l J o u r n a l of Robotics Research, V o l . 2, Spring 1983, pp. 35-45.  33.  Brady, M. et a l . , (eds.) Robot Motion: Planning and C o n t r o l , MIT Press, Cambridge, Mass., 1982.  34.  Duncan, J.P.  35.  Walker, M.W. and Orin, D.E. " E f f i c i e n t Dynamic Computer Simulation of Robotic Mechanisms", ASME J o u r n a l of Dynamic Systems, Measurement, and C o n t r o l , V o l . 104, Sept, 1982, pp. 205-211.  36.  Shampine, L.F. and Gordon, M.K. Computer S o l u t i o n of Ordinary D i f f e r e n t i a l Equations: The I n i t i a l Value Problem, Freeman, 1974.  37.  Cannon, R.H. and Schmitz, E. " I n i t i a l Experiments on the Endpoint Control of a F l e x i b l e One-Link Robot", I n t e r n a t i o n a l J o u r n a l of Robotics Research, V o l . 3, F a l l 1984, pp. 62-75.  38.  B i n f o r d , T.O. and Cannon, R.H. " F i r s t Annual Report on Endpoint Control o f F l e x i b l e Robots", Stanford U n i v e r s i t y , May 1984.  39.  Astrom, K.J. and Wittenmark, B. Computer-Controlled Systems, P r e n t i c e - H a l l , Englewood C l i f f s , N.J., 1984.  40.  F r a n k l i n , G.F. and Powell, D.J., D i g i t a l C o n t r o l of Dynamic Systems, Addison-Wesley, C a l i f o r n i a , 1980.  41.  Gelb, A. (ed.), A p p l i e d Optimal Estimation, MIT Press, Cambridge, Mass., 1974.  42.  Cherchas, D.B. "Robotics: Kinematics, Dynamics and C o n t r o l " , UBC Course Notes, 1983.  43.  Davies, M.S.  44.  Bohn,  45.  Raven, F.H. Automatic C o n t r o l Engineering, 3rd ed., McGraw-Hill Book Co., New York, 1968.  46.  R i t g e r , P.D. and Rose, N.J. D i f f e r e n t i a l Equations w i t h A p p l i c a t i o n s , McGraw-Hill Book Co., New York, 1968.  "CAD/CAM Algorithms", UBC Course Notes, 1983.  "Systems C o n t r o l " , UBC Course Notes, 1983.  . "Optimal F i l t e r i n g and C o n t r o l " , UBC Course Notes, 1984.  147.  47.  Narendra, K.S. and Monopoli, R.V. , eds. A p p l i c a t i o n s of Adaptive C o n t r o l , Academic Press, New York, 1980.  48.  Landau, Y.D. Adaptive C o n t r o l : The Model Reference Approach, Marcel Dekker, New York, 1979.  49.  H a r r i s , C.J. and B i l l i n g s , S.A., eds. Self-Tuning and Adaptive Control: Theory and A p p l i c a t i o n s , Peter Peregrinus L t d . , Stevenage, U.K., 1981.  148.  APPENDIX I RECURSIVE LEAST SQUARES  A d e r i v a t i o n of the standard recursive l e a s t squares algorithm i s included here f o r the sake of completeness.  The l e a s t squares estimate i s  derived f i r s t ; then a recursive form of the s o l u t i o n i s derived. Consider a system model defined by the equation  y(k)  =  6  T  *(k) + v(k)  where y(k) i s the computed v a r i a b l e , v(k) i s unknown noise 8 8  and  i s the vector of parameters to be estimated T  - [B e l  2  ••••0 ] n  4> i s a vector of measurements  T  I f an estimate of y(k) i s denoted by y(k) then the error i s  €(k) = y(k) - y ( k )  The p r i n c i p l e of l e a s t squares says that the parameter vector 0 should be chosen i n such a way as to minimize the c r i t e r i o n f u n c t i o n  1(9) =  N I i(k) k=l  2  (1)  149.  where N I s the number of measurements taken and u>(k) I s a p o s i t i v e number allowing d i f f e r e n t weights to be attached to d i f f e r e n t observations. Introducting the vectors  * (N)  - [  e (N)  = [e  T  T  y i  x  y  2  ...y ]  e  2  ...e ]  N  N  and the matrix  + (2) T  + (N) T  and dropping the N f o r the time being, the c r i t e r i o n f u n c t i o n may be re-written  =  1(8)  e e  (2)  Substituting e = Y - Y  ,  Y = c>8  i n t o (2) gives  1(8)  =  (Y-*8)A  (Y-o.8)  =  YTY  -  (•e) 1 ^  -  Y Y  -  2Y  T  -  T  YT«>e T T  «>e  +  e «> <>e  +  «>8)T  «>e  150.  Performing the minimization w i t h respect t o 9 gives  3  -  1  36  $ Y  0 - 2  * $6  +2  T  T  Thus the best estimate 6 i n the l e a s t squares sense i s given by  ( • • r  •  1  (3)  *  provided the inverse e x i s t s . A recursive form of (3) i s now found i n order to include the ( N + l ) ^ A  observation i n the estimate 8 . The argument N i s reintroduced i n the notation at t h i s point.  When a new measurement i s obtained, the matrix  $ and the vector Y w i l l be • (N+l)  [  I  •(H)  T •  f '  Y  ( N + 1 )  =  (N+i)J The estimate 6(N+1) i s , from ( 1 )  ^(N+l)  =  [+ (H+1> T  •(N+l)]-l  /(N+l)  noting that  O  N • 0 0 •(») - l k=l  The above Equation (4) may be r e - w r i t t e n  *O0 • 0 0  Y-  (N)l  t y(N+i)J  Y(N+1)  (4)  151. $(N+1)  -  I* (N)  *(N)  T  x  -  +  KN+1) / ( N + l ) ] "  [+ (N) Y(N)  +  T  [• • +  • • ]  i) T (N+l) y(N+l) ]  [• * +  4  1  ** ]  (5)  where the arguments have again been suppressed f o r convenience. Rewriting,  IWi)  = (f^r +  A  1  («) «) T  +  +  + wV*  [(«> * T  tt )-»  *  T  -  (o'c))- ] 1  y  R e c a l l i n g that  6(N)  =  (eft)"  1  <>Y T  and observing that  +  T  Then (5) may be w r i t t e n  tt )"l  -  T  T  T /\  4%  -  W ) ( « ) 0 - 1 <) Y T  T  T  A  $(N+1)  =  6(N)  +  =  [* (N+1)  [y(N+l)  K(N)  -  t (N+l)9(N)l T  where  K(N)  •(N+l)]"  T  1  *(N+1)  In order to obtain a recursive form f o r K(N), the matrix P(N) i s introduced  P(N)  .-  [/(N)  *(N)]-  1  Then  P(N+1)  .=  +  t t  1  ]  -  1  At t h i s point the "matrix i n v e r s i o n lemma" i s used.  LEMMA. Let A , matrices.  C,  and  P(N+1)  1  _ 1  B)  be non-singular square  Then  [A+BCD]  Taking  (C~ -H)A  - 1  =A  A  may be w r i t t e n as  _ 1  -A~ B 1  [C^-WA^Bl"  = <(>%, B  =  DA"  1  C  1  - I, D =  *  T  153.  P(N)  -  P(N)*[1  +  * P ( N ) I*]" t  -  F(N)n F(N)  T  1  P(N)  T  (8)  So,  K(N)=  P(N+1) t C N + l )  P  (  N  )  K  N  +  1  )  T  [1  K(N)  =  P(N)K N + D  P(N+1)  -  P(N) -  P(N+1)  -  [I  +  [1  *  +  fff+D  + P(N)4>] T  P(N) t ] "  1  (9)  and,  -  K(N)f ( H + l ) P(N) T  K(N)f (H+l)] P(N) T  (10)  The recursive l e a s t squares estimation c o n s i s t s of Equations ( 6 ) , (9) and (10). A weighting f a c t o r may be included i n the c r i t e r i o n f u n c t i o n to account f o r time-varying  1(8)  -  parameters  N E u(k) fc(k) k=l  2  The weighting function chosen f o r t h i s study was the common exponential forgetting factor  u(k)  =  X ~ N  ,  k  0 < X < 1  In that case the recursive l e a s t squares equations become  8(10  -  £(k+D + K(K-l)[y(k)  K(K-l)  =  P ( k - l ) Kk)  P(k)  -  [I  -  I* +  K ( k - l ) /(k)]  -  t (k) t(k-l)] T  *V) P ( k - l ) P(k-l)  *(k)]-!  /X  The reader i s r e f e r r e d t o Lying and Soderstrom [18] who show how the modified equations are obtained.  155. APPENDIX I I THE  cos(8 ) i s denoted C  A  AND  T  MATRICES  and sin(6 ) i s denoted S  S u b s t i t u t i n g values from Table I I i n t o the general expression  A^  given i n s e c t i o n 3.2.1 gives r  l  0  l 0  0  l  0  1  - l 0  0  0  0  l 1  C  2  "2  0  a  2  S  2  C  2  0  a  2  0  0  1  0  0  0  0  1  3  0  - 3  0  3 0  0  3 0  0 0  0  1  s  *2  -  C  s  c  S  S  *3  S  0  c  0  d  S  C  -1 0  The transformation matrices  T^ , r e l a t i n g vectors expressed i n  coordinate frame i to base coordinates are e a s i l y found by m u l t i p l i c a t i o n  T  l  =A l  C  1 2  -c  S  1 2 C  - 1 2  2  C  C  S  0  a  2 l 2  C  U  2 1 2  2  " l 0  0  0  a S +d 1 1  S  l S 2  S  S  l  C  C  S  2  C  2  156. c (c c -s s ) 1  S  2  3  2  1^ 2 3 2 3^ C  2  _ S  S  s c +c c 2  3  2  " l( 2 3"* 2 3^ c  3  3  C  C  S  S  - l  -s (c c +s c )  0  -S S 4C C  c  1  2  2  3  3  2  2  3  3  a  2 l 2  a  2 l 2  a  2 2  C  S  C  C  S  0 These matrices are used to obtain the equations of motion using the Lagrangian approach.  + d  l  157. APPENDIX I I I THE PSEUDO-INERTIA MATRIX Jj  From equation (3.32), the pseudo-inertia matrix  J.  •  r  J link i  r  was defined  dm  Expanding gives  i i  z* 1  y  z dm  dm  J x y dm  J x z dm  /*"x dm  dm  dm  J y z dm  /*y dm  dm  J y z dm  J C * ) dm  /*z dm  /*"y dm  J*z dm  / dm  Defining  = / ( y + z ) dm 2  xx  = J ( x + z ) dm 2  yy  xy I  xz  I  yz  2  = J ( x + y ) dm 2  zz  2  = /xy dm  = fxz dm  = jyz dm  2  1  2  158.  mx  = Jx dm  my  = /y dm  mz  = /z dm  Then i t i s possible to express the diagonal terms as -I i  x  = /(V  dm  -  . I T  T  z  \2  A  +1 rr—E£  x x  i  r,i- \2 J = J( z ) dm = (A  +1  xx  -i  +i  yy «  zz I +1 - I xx vy zz  and Jj^ may be writte more concisely as  *ix  I.ixy  I.  I.  ixy *ixz m  i i x  *ixz  iyz  I. iyz V i  X  iz  m z i  m  X  V l n  i  i i  m  I«l i  159. APPENDIX IV DERIVATION OF THE LOAD TERM#.  The load term L^ may account f o r the w r i s t assembly or f o r an a r b i t r a r y mass placed at the end of the t h i r d l i n k of the RSI robot.  The  Lagrangian approach i s a p p l i e d i n much the same way as i t i s f o r the manipulator i t s e l f , the a p p l i c a b l e equation being  d  .9K .  The p o t e n t i a l energy i s found f i r s t .  3K  8P  The p o s i t i o n of the mass i s described  by  [0  0  z  1]  x  or, i n l i n k 1 coordinates, a f i x e d frame i n the simulation, by  A2A 3p 3  1  Using r e s u l t s from Appendix I I and performing the m u l t i p l i c a t i o n gives  -z  sin(8 +6 ) + a  2  cos6  2  z^ cos(9 +0 ) + a  2  cos9  2  1  2  3  2  3  0  160.  The g r a v i t y vector i n l i n k 1 coordinates i s given by  g  [0  T  g  0  1]  The p o t e n t i a l energy P i s then  P  -  P  =  lg  T 1  p  1  which gives  m  g[a sin6  1  2  Next, the k i n e t i c energy i s found.  2  + z^ c o s ( 8 + 6 ) ] 2  3  In base coordinates the p o s i t i o n of the  mass i s  Then I t s v e l o c i t y i s  dp  3  3T  3  ^~ i=i *T =  e±  3pl  Since l i n k 1 i s locked i n p o s i t i o n , only the l a s t two terms contribute.  The square of the v e l o c i t y i s  161. dp.  T  (.JT-)  =  2  (p ]L  Trace  px  )  and the k i n e t i c energy i s  dp  lv2 <dT>  K  =  2  K  -  •|-m [e ( a - 2 a z  m  l  g i v i n g the r e s u l t  1  +  —  z  2  2  .2  2  6  3  2  _  + 2(z  sinGg + z ) 2  1  _  2  - a z 2  .  1  sine )6 0 ] 3  2  3  S u b s t i t u t i n g K and P i n t o Equation (3.25) and performing the required operations give 6 ^ 2 and (3.43).  The r e s u l t i s given i n Equations (3.42) and  162. APPENDIX V J . FOR THE RSI ROBOT  From Appendix I I I , the general expression f o r  *ix  *ixy  *ixz  *ixy  hy  I .i y z  V i  *ixz  I. iyz  *iz  V i  V i  V i  m  mx 1  i  m  is  i i x  i  Referring to F i g . 9 , i t i s assumed that the centre of mass of each l i n k l i e s along one of the l i n k axes, so that  Pi  - [0  P2  - £  pj  = [0  y  0  1  0  1]  ^  0  2  0  z  3  1]  Then the pseudo-inertia matrices  X  lx  f o r i = 1,2,3 are  0  0  0  h  n^y  0  z  0  0  0  X  0  m  iy"i  lz  0  0 m  i  L  2  m X2 2  X  0  L  0  0  m x 2  2  2  0  Z  0  3z  0  0  0  0  0  I,  0  0  0  0  •3  X  0  m  m z 3  3  3 3 z  m  3  164. APPENDIX V I SETTING UP THE INTEGRATION SUBROUTINE  The f u l l  e q u a t i o n s of motion a r e g i v e n i n s e c t i o n 3.3.3.  groups o f terms t o s i m p l i f y t h e n o t a t i o n ,  T  2  T  3  = (TT>3  C  3  •  •I  "  •2  F,  ( R ^2  =  4  Fc  fi  D  C  gl 2( 2 a  m  +  m  3^  +  m  2 2l x  Z  =  , 2 2 + n^J „ + a,m + a,m, + 2 a m x '2Z aCt2 ^ ' £.£./.  I,  9  Z  =  m z a 3  3  2  = I,  o  F  < *-R^>3  V*3 3  =  3  F  we l e t  •'yy  =  Iq dyy  +  . aCt3  n  0  d  9  0  +1, 3yy  Identifying  165. 8  L  L  L  2  «  2  3  V  < 2 V>*  =  a +  m  " Vi*  5  Using the variables defined above we l e t  A  B  C  E  =  D  + L  2  2  -  (Fj + L^cosej, - ( F + L ) s i n ( 9 + 63)  =  D  2 2  -  L  3  "  2  D  23  + L - L  +  L  2  2  2 2  - 2(F + L^) s i n 0  3  1+  ;  23  =  D  =  ( F + L ) - (F^ + L^) s i n 63  =  "(D  =  D  =  =  + L  3 2  5  - ( D  3 2  5  + L  2 2 3  3 2 2  233  + L +  L  2 2 3  )  3 2 2  233>  (F,, + L^) cos 6 3  166. "  P  =  Q  3  D  -(F  ° -  +  33  D  F  6  + L  L  3  L ) 8in(62 +  +  2  +  L  2  63)  33  5  Then the equations of motion may be r e - w r i t t e n  T u 2  -C 9  2  2  .2  = A + B6 + C9 - 2 E 8 9 - E 8  2  2  3  2  3  3  2  3U3  T  ~  C  3^3  =  P  QQ3  +  +  C 6  2  +  E  ®2  C o l l e c t i n g 6 2 and 8 3 to one side e  - £ [T u  3  2  °2  2  - £ t 3 3 T  U  -c e 2  -  C  3§3  2  - A + 2E8 e 2  ~  P  "  E §  2^  3  + E § ] - | e' 3  " rJ '  2  e  3  These second-order equations are transformed to a set of f i r s t - o r d e r equations by l e t t i n g  l  °  "2  "  x  =e  x  \  3  -  °2 §  9  3 2  3  so we obtain  l " 2  x  8  *2 - 3 6  X  3  K  = x  =  x  l 2  F i n a l l y , s u b s t i t u t i n g f o r 6 and 6 we obtain 2  x  l  [-TT-HT3u3 - C x C -QB  =  3  [— C  *2  "  =  =  C  2_  ][T u 2  2  2  - C x, - A + 2Ex,x + Ex,] 2  ^  Q B  2  2  B [— C  \  2  1  1  2  9  2 1  I — - — ^ ] [ T u - C x, - A + 2Ex,x + Ex*] C -QB *  "  3  - P - Ex ]  3  2  ~  x  3  x  x  2_  2  2  ] [T u 6  Q B  3  3  *  ?  - C x 3  23  2  2  - P - Ex, ] 1  J  l  2  This i s the form of the equations integrated by the DDE subroutine.  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

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

Comment

Related Items