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.
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Simulation and adaptive control of a robot arm
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Simulation and adaptive control of a robot arm Marchand, Pauline Anne 1985
pdf
Page Metadata
Item Metadata
Title | Simulation and adaptive control of a robot arm |
Creator |
Marchand, Pauline Anne |
Publisher | University of British Columbia |
Date Issued | 1985 |
Description | The equations of motion describing a robot's dynamics are coupled and nonlinear, making the design of an optimum controller difficult using classical techniques. In this work an explicit adaptive control law is proposed based on a discrete linear model for each link and on the minimization of a quadratic performance criterion. The system parameters are recursively estimated at each control step using least squares. A computer simulation of the resulting scheme is performed to evaluate the controller. The simulation model is based on the first three links 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 its behaviour is stable and convergent. |
Subject |
Robots Manipulators (Mechanism) |
Genre |
Thesis/Dissertation |
Type |
Text |
Language | eng |
Date Available | 2010-05-28 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0080811 |
URI | http://hdl.handle.net/2429/25118 |
Degree |
Master of Applied Science - MASc |
Program |
Mechanical Engineering |
Affiliation |
Applied Science, Faculty of Mechanical Engineering, Department of |
Degree Grantor | University of British Columbia |
Campus |
UBCV |
Scholarly Level | Graduate |
Aggregated Source Repository | DSpace |
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
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0080811/manifest