UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

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

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

Item Metadata

Download

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

Full Text

SIMULATION AND ADAPTIVE CONTROL OF A ROBOT ARM by PAULINE ANNE MARCHAND B.Eng., McGill 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 i n THE FACULTY OF GRADUATE STUDIES Department of Mechanical Engineering We accept this thesis as conforming to the required standard THE UNIVERSITY OF June, © PAULINE ANNE BRITISH COLUMBIA 1985 MARCHAND, 1985 In presenting t h i s thesis i n p a r t i a l f u l f i l m e n t of the requirements for an advanced degree at the University of B r i t i s h Columbia, I agree that the Library s h a l l make i t f r e e l y available for reference and study. I further agree that permission for extensive copying of t h i s thesis for scholarly purposes may be granted by the head of my department or by h i s or her representatives. I t i s understood that copying or publication of t h i s thesis for f i n a n c i a l gain s h a l l not be allowed without my written permission. Department Of Mechanical Engineering The University of B r i t i s h Columbia 1956 Main Mall Vancouver, Canada V6T 1Y3 Date May 31, 1985 DE-6 13/81) ABSTRACT The equations of motion describing a robot's dynamics are coupled and nonlinear, making the design of an optimum controller d i f f i c u l t using classical techniques. In this work an explicit adaptive control law i s 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 i s performed to evaluate the controller. The simulation model i s based on the f i r s t 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 i t s behaviour i s stable and convergent. - i i -TABLE OF CONTENTS Page ABSTRACT i i TABLE OF CONTENTS i i i LIST OF TABLES v LIST OF FIGURES v i NOMENCLATURE v i i ACKNOWLEDGEMENTS x i CHAPTER 1 INTRODUCTION 1.1 Preliminary Remarks 1 1.2 Literature Review 3 1.3 Scope and Objectives 5 2 ADAPTIVE CONTROL STRATEGY 2.1 Preliminary Remarks 14 2.2 Design of the Controller 15 2.2.1 Admissible Controls 16 2.2.2 Discrete Model Structure 16 2.2.3 Control Law 21 2.2.4 I d e n t i f i c a t i o n Procedure 26 2.2.5 Replacement of True Parameters 29 3 THE SIMULATION MODEL 3.1 The RSI Robot 36 3.2 Kinematics 37 3.2.1 Direct Kinematics 38 3.2.2 Inverse Kinematics 42 3.2.3 Trajectory Planning 45 3.3 Dynamics 46 3.3.1 Actuators 46 3.3.2 Manipulator Dynamics 47 3.3.3 Simulation Equations 54 3.4 Summary 56 4 EVALUATION OF THE CONTROL STRATEGY 4.1 One Degree-of-Freedom Case 68 4.1.1 Results and Discussion 70 4.2 RSI Robot Simulation 73 4.2.1 Results and Discussion 75 - i i i -TABLE OF CONTENTS (Continued) Page 5 CONCLUSIONS AND RECOMMENDATIONS FOR FUTURE WORK 5.1 Conclusions 140 5.2 Recommendations for Future Work 141 BIBLIOGRAPHY 144 APPENDIX I RECURSIVE LEAST SQUARES 148 I I THE A AND T MATRICES 155 I I I THE PSEUDO INERTIA MATRIX J ± 157 IV DERIVATION OF THE LOAD TERM L ± 159 V J ± FOR THE RSI ROBOT 162 VI SETTING UP THE INTEGRATION ROUTINE 164 - i v -LIST OF TABLES Page Table I Parameter Values of the RSI Robot 32 I I Kinematic Parameters of the RSI Robot 33 I I I Discrete Model Parameters and Coefficients f o r the Known Case 83 IV Test Paths f o r the Single-Link Simulation 84 V Run Parameters for 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 VII I Horizontal Line - Parameters of the Simulation Runs 88 - v -LIST OF FIGURES Page Figure 1 Trajectory f o r a two-link arm 10 2 Schematic of robot control system (a) j o i n t control 11 (b) endpoint control 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 Position of a point r e l a t i v e to two frames 59 9 Link coordinate frames for the RSI robot 60 10 Vector d e f i n i t i o n s for the inverse kinematics 61 11 Workspace of l i n k s 2 and 3 of the RSI robot 62 12 Trapezoidal vel o c i t y law 63 13 Direction cosines for endpoint trajectory 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 89 16 Program flowchart - known case, one degree of freedom 90 17 Program flowchart - Adaptive case, one degree of freedom .. 91 18 Known control - effect of varying e, T = 0.010 s 92 19 Known control - effect of varying e, T = 0.020 s 94 - v i -LIST OF FIGURES (Continued) Page 20 Effect of disturbance 95 21 Feedforward i n a proportional controller 97 22 Adaptive control - no disturbance 98 23 Known vs. adaptive control, step inputs 99 24 Known vs. adaptive control, ramp input 100 25 Known vs. adaptive control, trapezoid 101 26 Effect of zero i n i t i a l estimates 102 27 Program flowchart - Adaptive case, RSI robot 103 28 Configuration for the C i r c l e Path 104 29 C i r c l e simulations - effect of loads 105 30 C i r c l e simulations loads - effect of A 107 31 C i r c l e simulations - eff e c t of i n i t i a l estimates 108 32 Configuration for the 45° l i n e 109 33 45° l i n e simulations 110 34 Configuration for the horizontal l i n e 115 35 Horizontal l i n e simulations 116 - v i i -NOMENCLATURE Subscripts i refers to the i j o i n t d desired, disturbance m motor act actuator 1 load a,b continuous transfer function variables aj ,a 2 ,bj^ ,b2 ,h discrete model parameters 0 2 , 0 2 , 0 3 , 0 ^ , 0 5 control c o e f f i c i e n t s g gravity m mass n gear r a t i o , order of system x 2 location of centre of mass, l i n k 2 x 3 location of centre of mass, l i n k 3 location of mass, load u control input y system output y^ reference input k sampling instant q s h i f t operator t time v v e l o c i t y a acceleration - v i i i -T sampling period K control input constant K^ , motor constant K back emf constant e K k i n e t i c energy L Lagrangian P potential energy R armature resistance 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 i n e r t i a C damping constant W weighting on the control X forgetting factor e pseudo-gain a dir 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 transfer function G(z) discrete time transfer function e(k) white noise v(k) disturbance sequence - i x -Vectors and matrices are i n bold type z cartesian p o s i t i o n p ^ ( k ) desired endpoint position * r position vector r with respect to i ' * 1 coordinate frame u control input y system output (angular position) y. reference input (angular position) d 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 • ARMA autoregressive moving average STR self-tuning regulator MRAS model reference adaptive control - x -ACKNOWLEDGEMENTS The author would l i k e to express thanks to Mr. Alan Steeves for assistance with the VAX computer and to Ms. Jocelyn Hung for help with the pl o t t i n g . 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 Engineering. - x i -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 ratios and require powerful actuators. Because of the variable structure of the robot the controllers which regulate the joint movements are designed for the worst case. Thus, less than optimum performance i s achieved at the other operating conditions. In t h i s work an adaptive controller which can adjust to changes i n the system dynamics i s proposed, and i t s behaviour i s studied by computer simulation. An example of a change i n the system dynamics as the robot moves i s easily seen by considering Fig. 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 position 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 position B i s reached. The t o t a l i n e r t i a f e l t by the motor driving 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 acting on each l i n k also vary. Gravity loads depend on position. Velocity effects such as centripetal forces are present and are variable as w e l l . Robots have been designed for 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 control system i s shown i n F i g . 2(a). Assuming that the desired j o i n t angular position as a function of time, 6,(t) i s d known, the idea i s to compare i t to the measured j o i n t position 8 ( t ) and form an error s i g n a l . Using knowledge of the system dynamics, a control law operating on the error i s designed so that certain performance speci-2. f i c a t i o n s are met and disturbances, x,, are rejected s a t i s f a c t o r i l y . When d the system dynamics change, the control law i s no longer optimum. I t would be advantageous i f the c o e f f i c i e n t s of the controller could be varied on-line as the system parameters and the disturbances changed. These considerations have led to the study of more complex control laws, and motivate the present work. A second motivation i s that technological advances i n sensor develop-ment and information processing are beginning to make more sophisticated control strategies f e a s i b l e . F i g . 1 may be examined again. In order that the endpoint of the arm follow the straight l i n e path, i t i s necessary that the movements of both l i n k s be precisely coordinated. The j o i n t controllers each i n d i v i d u a l l y tend to drive the error between the desired angle and the measured angle of t h e i r l i n k to zero. In order to obtain good endpoint positional accuracy, not only must the j o i n t controllers 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. Rigid l i n k s imply heavier designs and hence powerful motors. Payloads must be l i g h t r e l a t i v e to the construction of the robot to prevent deflection of the l i n k s and to avoid introducing extra gravity disturbances which would disrupt the c o n t r o l l e r s . I f , however, i t was possible to measure the position of the endpiece and use that measurement for feedback, the above design l i m i t a t i o n s could be eliminated. This approach i s termed endpoint control and i s schematically i l l u s t r a t e d i n F i g . 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 fixed base reference frame whereas the control action 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. Point P i s uniquely defined by the vector pair (x,y). By f i x i n g a coordinate frame on each l i n k , such as (xj-y,) on l i n k 1, i t i s possible to f i n d the 3. relationship between (x,y) and (8^82). The mapping 6 -*• x , however, i s not unique. F i g . 3 shows the "elbow up" solution. Endpoint control strategies must deal with these issues. The intent of t h i s study was to formulate a control scheme which might be suitable for extension to endpoint control, and would function adaptively at the j o i n t l e v e l . 1.2 Literature 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 sophisticated model of the system. Using that information the required control inputs may be anticipated. The second approach presumes much less knowledge of the system. Instead, input-output data from the system i s used to i n f e r i t s structure and to provide adequate control. A short description of some of the work done i n these areas follows, as i t applies to robotics. Whitney [1] proposed resolved rate control to control j o i n t v e l o c i t i e s obtained from hand or endpiece positions and v e l o c i t i e s . His i s a c l a s s i c paper. Luh, Walker and Paul [2] propose a resolved acceleration technique to close the loop around the hand of the robot by pre-calculating the required torques given desired angular position, v e l o c i t y and accelera-tions. This method requires a detailed model of the manipulator. Freund [3] has suggested nonlinear compensations to decouple the equations of motion of the manipulator. Golla, Garg, and Hughes [4] proposed l i n e a r multivariable control f o r assigning the closed loop poles of the system by state feedback. Cvetkovic and Vukobratovic [5] examine several two-stage control schemes where the control signal consists of a nominal pre-programmed value and a compensating term. Vukobratovic and Stokic [6] 4. study a few control strategies which take dynamics into account i n d i f f e r -ent degrees. Hogan and Cotter [7] have suggested Cartesian impedance control for closing the loop around the hand. In general the above techniques require a detailed model of the robot. Modelling errors would lead to decreased effectiveness of the control laws. A second disadvantage of these methods i s that they t y p i c a l l y require large amounts of ca l c u l a t i o n between the sampling instants when a control signal 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 less knowledge of the system. Lee [8] proposes that a pre-calculated nominal torque be supplemented by inputs calculated from i d e n t i f i c a t i o n of the perturbed system about the nominal trajectory. Balestrino et a l . [9] proposed a model reference approach and design the control 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 Nicosia and Tome! [10], who investigate convergence. Recently, Kim and Shin [11] have described a method of averaged dynamics. Young [12] proposed s l i d i n g mode control based on the theory of variable 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 for the case of a two-degree-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 formula-ti o n . He used recursive least squares to ide n t i f y the entries of the transformation matrix. Koivo and Guo proposed that the robot system be modelled by an autoregressive moving average (ARMA), and the parameters 5. recursively i d e n t i f i e d using the least squares technique. A control law based on a quadratic performance c r i t e r i o n i s derived and the estimated parameters are substituted into t h i s law at each control step. Koivo chose a system model structure based on empirical tests. In this work an analysis of the continuous-time system representation i s done and i t i s shown why a successful discrete model has the structure given. The same control law design i s followed, but the controlled variable was chosen as angular position rather than angular v e l o c i t y , since Koivo's method neces-sitated an ad-hoc adjustment for position errors. Angular position was chosen also because, i f the controller was successful, as an avenue for future investigation, i t was structured so that the transformation from base coordinates to jo 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 investigate and test v i a computer simulation a control law for improved j o i n t control. Formulation of the c r i t e r i a for the design i s discussed next. The choice of a control strategy depends on many factors and constraints, since performance must be specified within a complex framework of interacting 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 acceleration and ve l o c i t y ; a sophisticated trajectory planner ensures that the commanded sequence of 6 results i n 6 and 8 w e l l within these bounds, and also ensures smooth transitions between path segments. When these conditions are met, less rigorous demands are placed on the j o i n t actuators and t h e i r c o n t r o l l e r s . The relationship between trajectory planning and co 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 perform-ance of the robot depends on a host of factors. However, i t i s possible to establish a framework for the design and f i v e broad c r i t e r i a were specified for the design of the co 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 control, (4) the simulation model would be r e a l i s t i c , and (5) the co 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 majority of the newer approaches to robot control are, on the theoretical l e v e l , treated using continuous time techniques. This i s probably due to the r e l a t i v e newness of d i g i t a l control and hence lack of f a m i l i a r i t y with i t . The behaviour of the system under consideration - the robot - i s of course described by continuous-time equations. Control 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 control laws must be done d i g i t -a l l y , i . e . , the ca l c u l a t i o n of the control signals i s done on a micro-computer and at a prescribed sampling rate the signals are downloaded to a digital-to-analog converter and on to the actuators of the j o i n t s . Issues such as the effect of sampling time and computational delays are impossible to analyze given the present formulation of these control laws. It seems to make more sense to use d i g i t a l theory i n the f i r s t place for 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 refers to the class of control 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 bas i c a l l y two philosophies 7. behind the advanced control laws which have been studied for robots. One of the approaches i s to improve the system model used i n the design of the con t r o l l e r . Because the equations describing the system are coupled and non-linear, large numbers of calculations are t y p i c a l l y required i n the control law. The goal i s to compensate using exact knowledge of the system, which i n practice i s not achievable. The second approach i s to use some form of adaptive control, which presupposes much less knowledge of the system, and which offers a p o s s i b i l i t y for compensating for changing system parameters. The survey paper by Astrom [15] i s a good overview of adaptive control, 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 control strategy s i m i l a r to the one proposed by Koivo [13] i s studied. The t h i r d objective which the controller had to 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 control was considered a p o s s i b i l i t y . Computer simulation i s a useful vehicle of experimentation as the f i r s t step i n evaluating a co n t r o l l e r . The costs and p i t f a l l s of hardware implementation are avoided at the i n i t i a l design stage. Simulation i s used i n this work to test the co n t r o l l e r . However, an existing robot i s used as the prototype f o r building the simulation model, rather than a f i c t i t i o u s robot of arbitrary c h a r a c t e r i s t i c s . In addition, 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 for "large" motions. The design and performance of a joi n t controller i s influenced by the type of task performed by the robot. One can c l a s s i f y operating modes into 8. three groups: i ) large motions, II) fine motions, I I I ) assembly tasks. Assembly tasks require that compliance and force feedback be considered, and are not studied here. In fine motions, simplifying 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 analysis. This study i s geared to large motions, which include a few types. The task objective can be tracking of a path, object tracking (for example conveyor belt following) or simply moving from point A to B. Here some point-to-point motion i s considered as w e l l as path tracking. These c r i t e r i a are to be met within the broad objective of improved j o i n t control. I t i s evident that attempts to study a six-degree-of-freedom robot without having acquired expertise on lower orders of magnitude would result i n the major trends being obscured by secondary effects. The scope of the work i s limited to three-degrees-of-freedom. Dynamically speaking, as far 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 wrist 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 into two subsystems. The lower subsystem, that i s the f i r s t three l i n k s , i s analyzed. The simula-t i o n i s carried out for two l i n k s since i t was f e l t that the distinguishing features of the control law can be i l l u s t r a t e d by that case. For the chosen robot the dynamic effects 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 th i r d l i n k s . The system to be studied i s i l l u s t r a t e d i n schematic form i n Fi g . 4. / \ TRAJECTORY FOR A TWO-LINK ARM 10. a) Joint u Motor ) * Controller Joint b) Robot End Point Controller u Motors Joints 6 Position Sensor Fig.2 SCHEMATIC OF ROBOT CONTROL SYSTEMS a) joint control b) endpoint control Fig.3 ROBOT ARM COORDINATE SYSTEMS User path I parameters Trajectory x4(k) Inverse Adaptive u(k) Planner Kinematics Controller Fig. 4 SCHEMATIC OF THE SIMULATED SYSTEM. l i 1 1 . Actuators T(t) Robot Arm 1 ^  Position Sensor "Camera" 1 r i i 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. The simulation set-up and results are discussed i n Chapter 4. Chapter 5 summarizes the conclusions which may be drawn from the investigation, and suggests directions for further study. 14. CHAPTER 2  ADAPTIVE CONTROL STRATEGY 2.1 Preliminary Remarks A pa r t i c u l a r choice of control strategy i s now formulated given the goals discussed i n the Introduction. 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 control law for joint control which could be reformulated to include sensory informa-t i o n from the environment, for example i n the form of position feedback from the hand of the robot. The common six degree-of-freedom robot i s separated into two three degree-of-freedom subsystems. One of these sub-systems, consisting of the f i r s t three l i n k s , i s examined. The proposed con t r o l l e r i s tested by computer simulation 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 to sample the continuous time signals. This means that the continuous function f ( t ) i s replaced by i t s value at discrete time instants. If the sampling period i s T, the function f ( t ) i s represented by f(kT), where k i s the sampling instant, k = {...-1,0,1,...}. For s i m p l i c i t y the notation w i l l be f ( k ) . Referring again to F i g . 4 , a closer look i s taken at the adaptive controller block. The controller has two inputs: a discrete sequence of desired j o i n t angle vectors 6 ^ ( k ) , and a discrete sequence of observed system output vectors 6 ( k ) . Except as noted otherwise, i n th 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 variable w i l l be denoted y ( k ) , i n order to conform with standard notation i n the controls l i t e r a t u r e . The con t r o l l e r has one output, the computed variable u ( k ) . Ways of organizing the calculations 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-ently but i t has been shown that i n some cases the methods are equivalent, for example Landau [16] and Egardt [17]. Fig. 5 shows a block diagram of a MRAS. The heavy l i n e s form an ordinary feedback system. Performance specifications are i m p l i c i t l y specified 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 structure has been called direct because the controller i s adjusted d i r e c t l y . The second 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 . The other loop i s the adjustment mechanism. Input-output data of the system are used to i d e n t i f y the system parameters. Then, the estimated system parameters are used i n the c o n t r o l l e r design block to calculate the c o n t r o l l e r parameters. This 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 . I t i s sometimes possible to organize the calculations such that the mapping from system i d e n t i f i c a t i o n + controller parameters i s t r i v i a l ; t h is i s then an i m p l i c i t STR. 2.2 Design of the Controller The component parts of the design are now addressed. A STR approach was chosen f o r t h i s study, closely following the work of Koivo and Guo [13], but i n contrast to that work the controlled variable i s chosen as position 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 their discussion on adaptive control. These are: 16. i ) The choice of the set of admissible controls u(k) i i ) The choice of a model structure M(0) to represent the true system i i i ) The selection of a design method to determine the control law i n terms of the model parameters 6 iv) The selection of an i d e n t i f i c a t i o n procedure to determine the system parameters v) The substitution of the estimated parameters into the control law Attention i s now turned to these issues. 2.2.1 Admissible Controls I t i s assumed that the computation time required for the control law i s a substantial portion of the sampling time T. Therefore y(k) i s not available 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 instead, based on measurements up to time (k-1). The notation y(k|k-d) denotes the best prediction of y(k) given information up to time (k-d). Admissible controls therefore consist of the reference signal y^, past controls, and past output measurements. 2.2.2 Discrete Model Structure A discrete model i s required such that i t s structure i s suitable both for control law determination and for i d e n t i f i c a t i o n of the system para-meters. The continuous-time representation of the actuator/robot arm system dynamics i s f i r s t examined i n order to determine suitable discrete candidate structures. 17. The mathematical representation of the manipulator part of the system i s given by a set of second-order, coupled, highly non-linear equations (see Chapter 3). I t i s assumed that the l i n k s of the manipulator have high structural r i g i d i t y such that deflection 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 + C(8,8) + G(6) + j = T (2.1) where 8 and 8 are j o i n t positions and v e l o c i t i e s respectively, D(8) i s an nxn i n e r t i a matrix, 8 i s an nxl vector of j o i n t accelera-tions, C(8,0) i s an nxl vector of centripetal and C o r i o l i s e f f e c t s , and G(8) i s an nxl vector of g r a v i t a t i o n a l forces, x i s an nxl input torque vector and x^ i s an nxl 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 far as the j o i n t actuators are concerned, Vukobratovic [6] suggests that the i t ^ actuator can be represented by a general state space model of the form A i X i + b i N i + f i T i ( 2 , 2 ) where ^*^±*^± a r e constant matrices, i s a saturation function, x ^ th i s the state vector of the i actuator of dimension m^ ; x ^ = ®i' x^] where x^ i s the (m^-2)xl vector of remaining states. The saturation function accounts f o r the fact 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 for the robot used i n the simulation the model f o r the actuators i s of the form J i e i + C A + T I = K i u i (2.3) 18. which can be written i n the above form, m^ =2. I t i s possible to combine (2.1) and (2.3) into a state space form ff(x(t)» u(t) , t) (2.4) where e i s a 2nxl vector. 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 resulting equation discretized, i.e form 6x A6x + B6u (2.5) Assuming zero-order hold sampling T the discrete equivalent of (2.5) i s x ( k +l )= *x(k) + Tu(k) where I + A * AT * B / e A t dt B r A t .„ J e dt This method i s computationally intensive. For the purpose of designing an adaptive c o n t r o l l e r a dif f e r e n t approach was taken. The gravity, C o r i o l i s , centripetal and disturbing 19. effects are a l l treated as disturbances. The coupling i s also assumed a disturbance such that the multivariable three degree-of-freedom system i s reduced to three single-input/single output systems. A system model of the form [ J i + D i i ] e i + C A + x d A = V i ( 2 * 7 ) i s proposed for each l i n k and the discrete equivalent found, as follows. Rewritten to simplify the notation J8 + C6 + T J = Ku (2.8) d The disturbing torques are modelled as a time varying sequence v(k) of unknown mean and variance. The continuous time remaining portion can be expressed i n Laplace transform notation by Y<S> * s(Js K+ C) U<S> = G(s) U(s) In standard form, G< s) = *7U*kJ>  (2*9) where a = ^ -J K 20. Using a zero-order hold, the equivalent discrete transfer function i s G(z) - ( ^ ) Z { ^ 1 } where z{ } denotes the z-transform. The result i s , with a sampling time T, G(z) = bz(e- a T+aT-l) + ^ - a T e ^ 1 ( 2 . 1 Q ) a(z-l)(z-e ) _ B(z) A(z) With some manipulation, replacing z by the s h i f t operator q, where q - 1y(k) = y ( k - l ) , the equation can be expressed as y(k) + ajyCk-l) + a2y(k-2) - bju(k-l) + b2u(k-2) (2.11) where ax = -(1 + e"aT) (2.12) —aT a2 - e (2.13) b: = b/a (e"aT + aT - 1) (2.14) b2 - b/a (1 - e"aT - 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. v(k) CU" 1 ) e(k) (2.16) with C(q _ 1) 1 + q" 1 + q~ 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 where h i s an offset term and e(k) i s white noise. This method of subtracting a bias term out i s described i n Isermann [19]. Thus the complete discrete model of each l i n k for the purpose of control design and parameter estimation i s y(k) + S ly(k-1) + a 2y(k-2) = b 1u(k-l) + b 2u(k-2) + h + e(k) (2.18) The coef f i c i e n t s a j , a 2 , b j , b 2, h must be estimated. The treatment of the control law derivation and the estimation section i s general - polynomials A(q _ 1) and B(q _ 1) are used instead of the above s p e c i f i c model. This was done so that modifications could have been made more eas i l y i n the simulation should the model (2.18) have proven to be inadequate. 2.2.3 Control Law The chosen model structure for each l i n k i s v(k) = h + e(k) (2.17) A(q _ 1)y(k) = B(q _ 1)u(k) + h + e(k) (2.19) 22. with A ( q - 1) - 1 + a j q " 1 + a 2q~ 2 + . . . . + a q"™ (2.20) B(q _ 1) = b j q - 1 + b 2 q - 2 + + b rq" r (2.21) h i s a forcing or offset term and e(k) i s white noise, (zero mean, uncorrelated, random). The proposed model for the robot system has m=2 and r=2. Minimum variance control laws, based on a quadratic performance c r i t e r i o n I = E{y(k) 2} have enjoyed some success i n adaptive systems applied to the regulation of processes, where the goal i s to keep the output constant at some reference value. See Astrom et a l . [20] for references. This provides some motivation for proposing a modified quadratic c r i t e r i o n based on the deviation of the output from the desired l e v e l and on the magnitude of the control e f f o r t : I = E{[y d(k+1) - y ( k + l ) ] 2 + eu(k) 2|(k-1)} (2.22) where E{f|k—1} denotes the expected value of the function f given measurements up to time (k-1), and e i s a weighting factor. In other words the control e f f o r t at time k, u(k), i s chosen to minimize the positional error which w i l l r e s u l t due to that control one step ahead, but a penalty i s attached to large control e f f o r t s . There i s a delay of one sampling period because the influence of the control u(k) w i l l only be f e l t at 23. y(k+l). This i s reflected i n the system model by the f a c t that the leading co e f f i c i e n t of the B(q _ 1) polynomial i s zero, bQ=0. Therefore a prediction of y(k+l) i s needed. The expectation i s taken with 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 available for computing u(k). If the sampling time T was very long compared to the computational time to obtain u(k), one could assume that y(k) was available. The problem i s stated as follows: i t i s desired to minimize I - E{[y d(k+1) - y ( k + l ) ] 2 + eu 2(k)|k-l} subject to the constraint A(q _ 1) y(k) = B(q _ 1) u(k) + h + e(k) (2.23) Expanding I gives I = E{y 2(k+1) - 2y d(k+l) y(k+l) + y 2(k+l) + eu 2(k)|k-l} - y 2(k+l) + eu 2(k) - 2y J(k+l) E{y(k+1) |k-l} d d + E{y2(k+1)|k-1} (2.24) 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 calculated. From the constraint equation (2.23) y(k) may be written y(k) = - a j y ( k - l ) - ... -amy(k-m) + b xu(k-l) + . + b ru(k-r) + h + e(k) (2.25) 24. Multiplying (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 ru(k-r+l) + h + e(k+l) (2.26) which i s a prediction of y(k+l). Denote R by R = [-a 2y(k-l) - ... -amy(k-m+l) + b 2u(k-l) + ... + b ru(k-r+l) + h + e(k+l)] (2.27) such that R i s a function only of past outputs and controls y ( k - i ) , u ( k - i ) , i > 1 and of the e(k+l). Then y(k+l) = -a ty(k) + bju(k) + R y 2(k+l) - ( a i y ( k ) ) 2 + ( b l U ( k ) ) 2 + R 2 + 2(b xRu(k) - S lRy(k) - a i b i y ( k ) u ( k ) ) 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{y2(k+1)|k-1} 2 5 . and comparing with equations (2.25) and (2.27) reveals that the expecta-tions may be expressed by i ) past outputs y ( k - l ) , y(k-2)... and inputs u(k), u ( k - l ) . . . i i ) noise terms e(k), e(k+l) for 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 2}, which i s unknown, however this i s immaterial since 3l/3u(k) i s the quantity of intere s t . Performing the d i f f e r e n t i a t i o n and setting the result equal to zero to determine the minimum gives 31 0 + 2eu(k) - 2y (k+l)bj + 2b^(k) 3u(k) + 2b:R - 2a 1b 1y(k) 2[eu(k) + b^R - a i y ( k ) - y d(k+l)] 2[eu(k) + b 1(y(k+l) - y d(k+l)] => 0 Thus the control law i s eu(k) + bjtyU+llk-l) - yd(k+l)] = 0 (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 substituting the parameter A A A estimates a, b, h into (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 for recursively i d e n t i f y i n g the parameters of an equation, for example, recursive least squares, extended least squares, recursive maximum l i k e l i h o o d , stochastic approximations, instrumental v a r i -ables, to name a few. There are i n addition several nonlinear techniques. Within each method there are variations or design factors to choose from. Another consideration for each method i s the particular computational algorithm used. The same method implemented with d i f f e r e n t computational algorithms gives different performance [21]. An example i s computer round-off error occuring due to poorly structured calculations. Thus choosing an estimator which w i l l perform adequately i s not easy. I t was decided to go with basic recursive least squares. This tech-nique i s known to be robust under many situations and well-conditioned algorithms were readily available. A derivation of the recursive least squares equations i s given i n Appendix I. A model of the form A(q _ 1)y(k) = B(q" 1)u(k) + v(k) i s postulated with polynomials A ( q - 1) = 1 + a : q ~ + a 2q [ ^ ^ • • • + a q mn -m B(q _ 1) = b i q - 1 + b2q" I 2 + ... - r and v(k) some disturbance of unknown characters. Defining the following vectors T 6 = [ a 1 a 2 . . . a b 1 b 2 ... b f] (2.29) 4>T(k) = [-y(k-l) - y(k-2) ... - y(k-m) u(k-l) ... u(k-r)] (2.30) The system model may be written y(k) = 6 T t ( k ) + v(k) (2.31) A 'natural' estimate y(k) i s J(k) = 6 T *(k) (2.32) The least squares p r i n c i p l e states 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 c r i t e r i o n N 1(6) = I £2(k) (2.33) k=l The result i s 8 = (*%r l * T Y (2.34) provided the inverse e x i s t s . The information matrix $ i s 28. • 0 0 T • (1) T • 0 0 and Y(N) = y ( D y(N) T The matrix ($ i s known as the covariance matrix P ( k ) . Singu-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 recursive form of the equations may be derived. The r e s u l t i s 8(k) - 9 ( k - l ) + K ( k - l ) [y(k) - 6 T ( k - l ) * 0 0 ] K ( k - l ) = P ( k - l ) * ( k ) [X + * T ( k ) P ( k - l ) f O O T 1 P O O - [ I " K ( k - l ) * A ( k ) ] P ( k - l ) /X (2.35) The scalar X i s a forgetting factor used when 8(k) i s time-varying, and i t permits discounting of old data. If 6 i s constant X i s set to 1. In order to estimate the offset h the parameter vector 8 i s augmented by h and the measurement vector if i s augmented with a 1. The recursive equations (2.35) are not well-conditioned from a numerical point of view [21]. Problems were encountered i n applications, often 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 alternative approach of restructuring 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 1 / 2 or U D D where D i s upper triangular and D i s diagonal. In order to avoid potential i n s t a b i l i t i e s i n the recursive least 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). I t uses T UDU f a c t o r i z a t i o n . 2.2.5 Replacement of the True Parameters The control law design i s based on the certainty-equivalence p r i n -c i p l e : the controller i s designed as i f the system parameters were known, and then the estimated values are substituted into the control law. This concept i s a special case of the Separation Theorem used i n Linear Quadratic Gaussian (LQG) theory. The Separation Theorem i s used when optimal control 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 states, 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 with known parameters, (2) the control i s optimized according to a quadratic performance c r i t e r i o n , and (3) the estimation i s optimized for Gaussian disturbances, i . e . i f the noise i s white. In the case of adaptive control 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 conditions are met, except that the parameters should be s t a t i s t i c a l l y independent. This condition 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 correlation between the estimated parameters. However, satisfactory r e s u l t s have been obtained by previous researchers investigat-ing adaptive schemes based on certainty-equivalence, even i f i t i s not rigorously j u s t i f i e d on a th e o r e t i c a l l e v e l . An estimate for y(k) given measurements up to (k-1) i s 30. y(k|k-l) = 6 T (k-D = -ax y(k-l) - a 2 y(h-2) + b : u(k-l) + b 2 u(k-2) + h (2.36) Moving ahead one step, an estimate of y(k+l) given measurements up to (k-1) 1 8 * y(k+l|k-l) = 6 T (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 least squares sense. At time k, i n the absence of measurements, 0(k-l) i s the best prediction of 0 i n the least squares sense. One may think that the uncertainty i n P(k) i s very high, and so K(k-l) goes to zero: no weight i s given to new measurements because they are absent. So 8 ( k - l ) i s the best prediction at 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) Substituting (2.36) into (2.38) and placing the result i n (2.37) w i l l give the estimate y(k+l|k-l). Then the control law (2.28) w i l l give u(k) = W[yd(k+1) - ( C j y(k-l) + c 2 y(k-2) + c 3 u(k-l) + ch u(k-2) + c 5 ) ] (2.39) where 31. A w (2.40) c 1 (2.41) (2.42) (2.43) (2.44) c 5 (2.45) The variable W i s a "weight" or gain and the c^ are control c o e f f i c i e n t s . Equation (2.39) gives the control u(k) at each sampling instant k. The choice of the adaptive control strategy has been described with respect to f i v e points or topics. This d i v i s i o n was made for convenience so that some sort of orderly framework could be followed. Evidently there i s much interaction and overlap between the fi v e areas. Many variations on the above themes are possible and concocting an adaptive control strategy i s not a problem. However, analysis, especially of the overall scheme, i s not easy. 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 controller to the optimal one. A general theory i s lacking although a few proofs e x i s t for special cases, see for example Ljung and Soderstrom [18], Goodwin and Sin [23], Egardt [24], Narendra and Lin [25], Goodwin, Ramadge, and Caines [26]. Empirical insight may be gained through simulation, thus the discussion on s t a b i l i t y and convergence i s postponed u n t i l the simulation results are covered. Development of the simulation package i s treated i n the following chapter. 32. Table I Parameter Values for the RSI Robot Description Symbol Value mass of l i n k 1 m l 5.652 kg mass of l i n k 2 "2 5.440 kg mass of l i n k 3 m3 0.755 1 kg centre of mass, l i n k 2 x 2 I -211 mm centre of mass, l i n k 3 71 mm moment of i n e r t i a , l i n k 1 I, lyy 4.03xl0 - 2 kg-m2 moment of i n e r t i a , l i n k 2 X2zz 2.43xl0 - 1 kg-m2 moment of i n e r t i a , l i n k 3 3yy 3.83xl0 - 3 kg-m2 length of l i n k 2 a 2 356 mm armature resistance R 0.75 ohm motor constant *T 4.308xl0-2 N-m/ amp moment of i n e r t i a , motors J _ act 1.413xlO- J t kg-m2 counter emf constant K e 4 . 3 x l 0 - 2 volt-s/rad gear r a t i o n 400 33. Table I I Kinematic Parameters for the RSI Robot JOINT 1 a i d i a i 1 0 d l IT 2 2 a 2 0 0 3 0 0 ir " 2 Notes: ( i ) a 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 u System controller parameters Adjustment Mechanism Fig.5 BLOCK DIAGRAM OF MRAS. 35. controller parameters,, Controller I Parameter Estimation System Fig.6 BLOCK DIAGRAM OF STR. 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 International (Sydney, B.C.) serves as prototype for the simulation. I t i s subsequently referred 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 at U.B.C. Fi 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 . The RSI i s a manipulator with s i x revolute l i n k s . I t has no of f s e t s , so the kinematical structure i s s i m p l i f i e d , and i t has a spherical 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 further f a c i l i -tates the analysis. The j o i n t s are driven through a gear t r a i n by DC motors with armature control. Presently, 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. Each loop i s tuned manu-a l l y by adjusting two gain screws. Voltage control signals are downloaded from a small computer to the servo boards. The main advantage i n using this robot for 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 available 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 for each l i n k were obtained by A l l e n [27]; motor char a c t e r i s t i c s were measured from step response tests. These values are tabulated (for 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 simulation 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 simula-tion 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 assumptions made i n developing the dynamics model are noted. 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 position and orientation of the end effector, 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 orientation and position r e l a -t i v e to fixed base coordinates. This i s generally not easy. Both of these problems need to be solved f o r the RSI robot. The direct kinematics analysis i s needed because the difference between desired and actual endpiece position w i l l provide a measure of error and hence performance of the co n t r o l l e r . The desired endpiece position i s of course known a_ p r i o r i . 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 actual endpiece position i s either measured or calculated from measurements of j o i n t angles. In an advanced real robot position 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 joints and measure j o i n t angles. Endpiece position may then be inferred. In t h i s computer simulation of the manipulator, j o i n t angles w i l l be "measured" by numerically integrating the equations of motion. "Noise" may be added to simulate inaccuracies of a r e a l system. Using the direct kinematics, the position of the endpiece i s then calculated. In order to relate positions 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 section to obtain the equations of motion of the manipulator. So, a kinematic analysis i s necessary. The inverse kinematics problem must be solved as well because an elementary trajectory planner i s needed i n the simulation. The user w i l l input the parameters of a trajectory which the endpiece must follow. For a l i n e , for example, the i n i t i a l position, 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 calculate the increment i n position at each sampling instant k. Using inverse kinematics t h i s vector of desired cartesian position 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 j o i n t controller which sends (hopefully) the appropriate voltage signal 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 loop around the hand i s not closed; with feedback from the end effector, the system would look as i n F i g . 2(b). 3.2.1 Direct Kinematics Returning to kinematics, a means of r e l a t i n g positions on one l i n k to a fixed frame or to another moving l i n k i s established. The p r i n c i p a l points are covered here. Details and examples are found i n Paul [28]. An alternative reference i s Lee's paper [29], which gives a good exposition 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 * r , whereas the po 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 . A l e f t superscript w i l l denote which coordinate system the vector i s referred to. 39. The vectors 1 r and * r are related by 1 T - ld + R 2 2 r (3.1) where i s a rotation matrix and *d i s the distance between the origins of the two frames. The transformation between * r and 2 r may be effected with one matrix by using homogeneous coordinates. An extra element i s added to vectors, so that given The vector • i s defined x y z (I) X Y Z with x' - — , y' •= , 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 refer a vector through a series of coordinate frames, as follows A 1 A 2 A ± l y The base frame i s denoted by "0" and superscripts usually omitted. Chains of A matrices have been c a l l e d T tranformation matrices, T ± A v (3.3) In order to relate positions and v e l o c i t i e s of points on each l i n k of the robot to a fi x e d based frame, a coordinate frame i s attached to each l i n k . Standard practice i n robotics i s to use the Denavit-Hartenberg convention [30], f o r establishing coordinate frames. I t i s applicable to a general linkage. The base frame i s fixed 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 jo i n t variable 6^ describes rotation of j o i n t i about the axis. The x^ axis i s chosen normal to z^ from the cross product x *^ • The inter s e c t i o n i s the o r i g i n . The axis completes the right-hand rule, = z i x x i * ^ e ^How^S parameters describe the relationship between neighbouring l i n k s : i ) a^ i s the distance between and measured along x^ i i ) d^ i s the distance between x^ and measured along i i i ) <x^  i s the angle between and z^ measured about x^ , using the right hand rul e . 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 axis 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 easier to follow. In any case, for a rotary manipulator, the clearest exposition i s Hollerbach's [31], from which the parameter descriptions 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 description of the robot's behaviour rests 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 solution developed to date (by Featherstone [32]), his choice of coordinate frames i s followed. Table I I gives the kinematic parameters for the RSI robot. The next step i s to obtain the matrices from the choice of coordinate frames. 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 * by performing the following sequence of sub-transformations: i ) f i r s t , rotate by an angle 6^  about t o bring the axis p a r a l l e l to the axis i i ) second, translate along a distance d^ so that x^ and X j _ ^ coincide i i i ) t h i r d , translate along = x i - l a distance a ^ t o bring the two origins together iv) f i n a l l y , rotate by an angle ot^  about x^ to bring the coordinate frames into coincidence. The re s u l t s i s the general matrix A . given by 42. A . cos8^ 0 0 -sinG^cosct^ cosO^cosa^ sina^ 0 sln8^sina^ -cosO^sina^ cosa. a^cos9^ a^sinS^ (3.4) For the RSI robot, the re s u l t i n g A^ matrices, i = 1,2,3 are given i n Appendix I I , along with the transformation matrices T. The direct kinematics have now been covered. In summary, given a position vector defined with respect to the i ' * 1 l i n k coordinate frame, *v, t h i s vector may be transformed to base coordinates given the angles 6 l t 8^  and the matrix by the simple m u l t i p l i c a t i o n 3.2.2 Inverse Kinematics The inverse kinematics are now addressed. Featherstone's technique [32] as described by Hollerbach [31] i s used for 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 solution at the wrist of the robot, so i t i s suitable i n the present case. The vector i s defined as the vector from the coordinate o r i g i n 0 to coordinate o r i g i n i , and p * 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 position of the endpiece with respect to the fi x e d base frame. Since j o i n t s 2 and 3 move i n a plane perpendicular to the x Q-y 0 plane, the f i r s t angle i s easily found Q1 = tan" 1 (—£) (3.5) 43. Note that a degeneracy occurs when p^^ = p^ = 0, corresponding to a wrist position along the zQ axis. To f i n d the next two angles, plf i s expressed i n l i n k 1 coord-inates, reducing the problem to a planar two-link one. Defining Pw " P<* " P i Then p. = P | f - d ^ ( 3 > 6 ) Expressed i n l i n k 1 coordinates l ' P w z 0 1] (3.7) where r has been defined from r 2 = P l + X 2 + P l + y 2 (3.8) By the cosine rule a 2 + z 2 - ( r 2 + p 2 ) s i n 6, = ^ — (3.9) 2 a 2 z l And cos 6 3 - ± / l - ( s i n 6 3 ) z (3.10) where the + sign i s elbow down and the - sign i s elbow up. Then, 44. t a n - 1 ( r-) cos8 3 (3.11) F i n a l l y , 8 2 i s found by expressing *p as w *2 *2 + z i Z3 (3.12) a 2cos8 2 - z^ sin(8 2+8 3) a 2 s i n 6 2 + z^ cos(8 2+8 3) 0 1 (3.13) Solving simultaneously gives sin8. Lp (a, - (z..sIn8o)) - 1p (z.,cos8a) a 2 2 + z x 2 - 2 a 2 ( z 1 s i n 6 3 ) (3.14) cos8. 1p + sin8„(zn cos8_) wx z N l 3 ^ a 2 ~ ^ z i s*-11^) (3.15) so that sin8. 8 9 = t a n _ i ( s-) 2 xcos8 2 (3.16) Care must be exercised i n the sp e c i f i c a t i o n of the quadrants for the 8^. The inverse kinematics have been covered, so that given a pre-specifled vector p^ describing the desired wrist position, the joi n t angles 8^, 8 2 and 8 3 may be determined. 4 5 . 3.2.3 Trajectory Planning Specification of a discrete time series 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 projection of the workspace ( i n the X Q - Z 0 plane) for l i n k s two and three i s given by F i g . 11. The o r i g i n of the wrist frames, or endpiece of the robot i n this case, may be positioned anywhere i n t h i s space. I t was decided to specify straight l i n e paths f o r testing the control law. The i n i t i a l p o sition and the target position are specified. It i s assumed that the arm starts from the rest and that a trapezoidal 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. The p r o f i l e i s completely specified by a, t j , t 2 , t3« This type of velo c i t y p r o f i l e i s sometimes c a l l e d "bang-coast-bang", [33] since high i n i t i a l accelerations are required to bring the endpiece to the travel 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 point p^(k), as shown i n F i g . 13. The ve l o c i t y v(k) i s known from the velocity p r o f i l e described above, so the distance tr a v e l l e d i n time Interval T (from time k to time k+1) i s As - v(k) T (3.17) hence, P l t x(k+1) = P ^ k ) + As cosa (3.18) P l | z(k+1) = P«tz(k) + As cosg (3.19) This method of defining a path may be e a s i l y extended to 3-space by specifying another d i r e c t i o n cosine. 4 6 . 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 for the purpose of designing a c o n t r o l l e r . As far as the simulation i s concerned we wish to relate the output signal of the con t r o l l e r , the voltage u ( k ) , to the resulting motion of the l i n k s , the position y ( k ) . The vectors u(k) and y(k) are conventional notation i n control l i t e r a t u r e , which i s why they are used. However, i n the dynamics analysis which follows the measured output variable y(k) which i s angular position, w i l l be denoted by 8(k) and by 0 i n the continuous representation. The system consists of the actuators, DC motors i n t h i s case, and the manipulation linkage. The current from the controller i s applied to the motors, which produce a torque T at each j o i n t i . This torque i s multi-m i p lied through a series of gears, so that a torque i s available at the output shaft to drive j o i n t i to the desired position. The analysis i s divided into the actuator dynamics, then the manipulator dynamics, and f i n a l l y the relationship between the two. 3.2.1 Actuators The actuators are DC motors with armature voltage control, i n other words their speed i s controlled by the armature voltage u. Subscript i refers to the i t * 1 j o i n t and i s omitted for now. A counter emf proportional to the armature speed 6 i s induced m E = K 6 (3.20) c e m The c i r c u i t equation f o r the armature i s 47. d i u - E = RI + T -j-—— (3.21) c a a dt L where the time constant i s T = — . Since the Inductance L i s very small a R a a the l a s t term i s normally neglected. The torque developed by the motor i s proportional to the armature current and i s given by T = K I D (3.22) m l a Substituting from the preceding equations gives T K T K e • TM R u " "T~^ 6 m (3.23) m K K m which i s an expression for the torque produced by the motor i n terms of the control input u, known constants K^ ,, K e > R and the armature speed 6^. 3.3.2 Manipulator Dynamics The equations of motion f o r the linkage are developed next. They are ea s i l y obtained by a straightforward though tedious application of Lagrange's equations. The general form of the equations for a manipulator i s derived i n Paul [28]. This derivation i s sketched here. Then i t i s shown how the result i s applied to the f i r s t three l i n k s of the RSI robot. The Lagrangian L i s written 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 potential energy. The equations of motion f o r an n-degree of freedom system are given by 48. d 9L i - 1. (3.25) ... .n where the are any convenient generalized coordinates and i s the generalized force. The equation constitutes a d e f i n i t i o n of the generalized force, with dW being the work done by the external 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 displacements: Then the generalized forces are simply the net torques x^ applied , at each j o i n t i . An expression for 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 ts position i n l i n k i coordinates i s given by *r . In base coordinates, i t s position i s n dW (3.26) (3.27) and i t s v e l o c i t y i s 4 9 . J - l J The velocity squared i s ( ~ ) 2 = Trace ( r r T) (3.29) Substituting from equation (3.29) gives A- 1 1 3 T * 4 4 T ' " j 1 j=l k=l j 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 = 1 d m ( f > 2 i i 3T, , . _ 3T * (3.30) = j Trace [ z Z « i ( * r dm A r T ) ^ 8 § ] (3.31) 2 j=l k-1 " j 3 9 k j k The k i n e t i c energy of the entire l i n k i s found by summing over the l i n k K l " ./ d K i l i n k i T i i 3T 3T - i Trace [ Z Z * < J * r V d . ) ^ 5 8 J (3.32) j=l k=l j l i n k i k J 50. The expression i n round brackets i s pseudo i n e r t i a matrix . See Appendix I I for 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 i K = Z K i = l i i 3T - \ 1 Trace [ I I J . 6.6. ] (3.33) 1 i = l j=l k=l d H j The potential energy of the system i s found next. If gravity 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 potential energy of l i n k i i s T i = ~ m i 8 * *i = ~ m i 8 p i where m^  i s the mass of l i n k i . A l ternatively P may be written P i = " m i g T T i l p i The t o t a l potential energy Is therefore - I m g T T 4 p f (3.34) i = l The next step i n the derivation i s to form the Lagrangian L = K-P and perform the required d i f f e r e n t i a t i o n s . This i s not done here, as i t does not add anything to the understanding of the equations. Details are 51. adequately covered i n Paul's book [28]. The r e s u l t i n g general equation for an n degree of freedom manipulator i s n n Z D 6 + Z Z D 9 6 +D = T , i=l,...n (3.35) j=l 1 J J j=l k=l 1 1 where n 3T 3T T D - Z Trace J - j f - ) (3.36) J p=max I , j j r i n 3 2 T 3T T D i j k s * A t Trace 'soir J P i f - ) ( 3 - 3 7 ) J p=max i , j , k j k v i n T 3T D - -Z m g T--P-Pp (3.38) 1 p=i P 3 6 i P Terms of the form represent effective i n e r t i a at jo 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 centripetal 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 move-ment of j and k. F i n a l l y , the terms represent gravity loading. Applying Equation (3.35) to the RSI robot i s straightforward. However, since the wrist i s being treated as a mass, the equation must be modified s l i g h t l y and load term w i l l appear on the LHS of Equation (3.35). The analysis i s e s s e n t i a l l y the same as the one just 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. Application of Equation (3.35) i s outlined here. Using the kine-matical d e f i n i t i o n s of section 3.2, one forms the transformation matrices T . 52. Appendix I I l i s t s these matrices for the RSI robot. Then, the required 3T matrix d i f f e r e n t i a t i o n s are performed. These terms are of the form T-T-, 9 o 3T 3 2T » ^ ™ • Using the pseudo i n e r t i a matrices J , the trace of each of do d o ^ d O j p the matrix products i s found and terms collected. Appendix I I I gives the general form of J whereas Appendix V gives the i n e r t i a matrices used for 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 result i s that the products of I n e r t i a are zero and a l l of the off-diagonal terms of the J matrix cancel except for two. The effect 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 position, the following equations are obtained: x 2 = D 2 + D 2 2 e 2 + D 2 3 e 3 + 2 D 2 2 3 e 2 e 3 + D 2 3 3 e 3 + L 2 (3.39) T 3 - D 3 + D 3 3 6 3 + D 3 2 6 2 + D 3 2 2 § 2 + L 3 < 3 - 4 0 > where D 2 = g[a 2(m 2+m 3)cos8 2 + m 2x 2cos0 2 - m 3z 3sin(8 2+8 3)] '22 = ^ 2 Z Z a2 m2 2a 2m 2x 2 + ^3yy a2 m3 ~ 2m 3z 3a 2sin8-53. D23 = I 3 y y " a 2m 3z 38ine3 D233 = ~ a 2 m 3 z 3 c o s 9 3 D233 = - a 2 m 3 z 3 c o 8 0 3 D3 = -m 3gz 3sin(6 2+8 3) D33 - h yy D32 = I 3 y y " a 2 m 3 z 3 s i n 6 3 D 3 2 2 = a 2m 3z 3cos6 3 (3.41) The structure of L 2 and L 3 i s necessarily s i m i l a r L 2 = L 2 + L 2 2 8 2 + L 2 3 6 3 + 2 L 2 2 3 e 2 6 3 + L 233 83 (3.42) .2 J3 = L3 + L33 93 L32 92 L322 92 (3.43) Denoting the load mass by m^  and assuming that i t s centre of mass with respect to l i n k 3 coordinates i s 3 p^ = [0 0 z^ 1], then L 2 = g[m^a2 cos6 - m^z^ sin( 6 2 + e 3 ) ] 2 _ , L 2 2 = m^[a2 + z^ - 2a 2z^sin9 3)] 5 4 . L 23 = m^[z^ - a 2z^ sin 9 3 ] L 223 = -m.-a-z.. cosB = —m-.aoz-.cos8 = -m 1gz 1 sin(9 2+9 3) L 33 = m,z L32 = m ^ [ z ^ 2 " a 2 z i s i n 9 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 to develop a simulation algorithm. The l a t t e r approach consists of a set of forward-backward recursive 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 lesser import-ance i n the present context. For one thing, only the motion of two and at most three l i n k s i s being simulated. On the other hand, the Lagrangian formulation provides physical insight into the behaviour of the system from a controls point of view. 3.3.3 Simulation Equations The f i n a l step i s to relate the torque x m developed by the motor of joi n t i , given i n Equation (3.23) to the torque x. available 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 6 = T - F.r, (3.45) act^ m^  m^  i i where J _ i s the i n e r t i a of the i * " * 1 actuator, F i s the contact force and act^ ' r the pinion radius. On the j o i n t shaft the net torque, previously defined as T ^ , i s simply T i = n i F i r i " C i 6 i ( 3 * 4 6 ) 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. Eliminating F and r from the equations gives n.J 8 = n.x - x - C, 6. (3.47) i act^ m^  i m^  i i i N ' Substituting from Equation (3.23) gives nK_ nK_K V a c t / i * <TT>i»i - HPVmf T i " C i 6 i < 3 ' 4 8 > Motor speed and angular a c c e l e r a t i o n are r e l a t e d to j o i n t angular v e l o c i t y and a c c e l e r a t i o n by the gear r a t i o n 9 = n 6 m^  i i 56. and e m i so that the following expression Is obtained (3.49) This equation forms the basis for the dynamical simulation, with given by Equation (3.35). In order to ef f e c t the simulation on computer the equations of motion (3.49) must be transformed into a set of f i r s t order equations. This i s done i n Appendix VI. 3.4 Summary Together with the kinematics developed i n section 3.2, the dynamics equations formulated here can be regarded as an experimental simulation package onto which different controllers could be connected. Details of the computer programs are covered i n the following chapter. This chapter has developed the equations necessary to implement the algorithms. A trajectory 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 l f(k), a discrete time series describing the desired position of the robot endpoint. The inverse kinematics have been described, such that P j | (k) may be transformed to the corresponding j o i n t coordinates 6.(k) . I t has been d shown how the actual j o i n t position 8(k) may be transformed, using d i r e c t kinematics, to a cartesian vector x(k) for comparison with P„(k). 57. A dynamics model of the robot, including motors, has been developed. The resulting equations are second-order, coupled, and nonlinear. Flex-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. These equations may be numerically integrated so that given a control input u(k) and a set of i n i t i a l conditions 8(k) and 6(k), the resulting angular positions and v e l o c i t i e s one integration step further may be found, i . e . 9(k + t ), 6(k + t ). step step The analysis has been done with respect to an exi s t i n g robot c a l l e d the RSI, for which kinematical and dynamical data i s available, enabling a r e a l i s t i c simulation to be performed. The simulation model can act as a testbed for various control strategies which use 6(k) and 6 ,(k) to d calculate u(k). Evaluation of the adaptive controller i s the topic of discussion of the next chapter. Pages 58 and 59 numbering not used. F2 FigS POSITION OF POINT P RELATIVE TO TWO FRAMES 62. F I G .9 R S I F R A M E S 63." elbow FIG . 1 0 V E C T O R D E F I N I T I O N S F I G . 11 W O R K S P A C E Fig.12 TRAPEZOIDAL VELOCITY LAW 66. F I G . 13 D I R E C T I O N C O S I N E S 67. FIG. 14 D R I V E S C H E M A T I C CHAPTER 4 68. 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 action of the proposed control was unknown, and since bugs can easily s l i p into 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 control law i s tested on a simple, one-degree-of-freedom, second order system of the form J6 + C9 + T. = Ku (4.1) a where i s some disturbance and J , C, and R are constant. A block diagram of the simulated system i s shown i n Fig. 15. In the f u l l simula-t i o n discussed i n the next section the dotted portion i s one j o i n t and i t s actuator, with x,j representing in t e r a c t i o n from other l i n k s and gravity effects ; the i n e r t i a J i s variable because of the movements of the other l i n k s . 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 simplify things further and to gain insight into the beha-viour of the control law under ideal circumstances, i t was f i r s t assumed that the system model parameters were known and the estimation portion of the control law was shut o f f . Thus the controller has fixed c o e f f i c i e n t s calculated according to the equations of Chapter 2. In t h i s way i t was possible to see i f the control law works at a l l for the class of system models investigated here. Also, using t h i s approach some knowledge of performance standards i s obtained. 6 9 . Once these i n i t i a l tests 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 "single l i n k " simulation was the estimator. This was an important step, since theoretical deficiencies were l i k e l y to show up at t h i s stage. I t was f e l t that i f the adaptation worked for the one-degree-of-freedom, constant parameter case, there was a good chance that i t would work for slowly varying parameters i n the two-link case. Evidently, we were able to progress to this stage. This section covers the simulations f o r the one-degree-of-freedom system under two situations: (a) system model parameters are known and need not be estimated, (b) system model parameters are unknown, but constant, and the least squares estimator must be used. Equation (4.1) i s put i n a form suitable for integration by a subroutine package [36]. Using the equation for the transfer function (2.9), with T, a = C/J, and b = K/C, the model parameters may be calculated from Equations (2.12) to (2.15), and the control 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 for the second l i n k of the RSI arm with J = n 2 J _ + I , a c t 2 z z z C = — s ^ - J i x 1.05 70. The disturbance D was taken as the gravity load of a mass of 8 kg placed at 250 mm from the axis of rotation of the l i n k . The resulting values for the model parameters and control c o e f f i c i e n t s are given i n Table I I I for two sampling periods. Five test paths were chosen to test the control law: two step inputs, two ramps, and one trapezoidal vel o c i t y law. The characteristics of the test reference inputs are given i n Table IV. Two programs were written. One simulates the known case, with no estimation, and i s outlined i n flowchart form i n F i g . 16. The second simulates adaptive control with the estimation and design blocks of Fig. 15 tuned i n . F i g . 17 gives a flowchart of that program. Several runs were made and the salient variables are given i n Table V. The f i r s t runs test the behaviour of the control law with the exact coefficients 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 situation when the parameters are known. Results are now discussed. 4.1.2 Results and Discussion Effect of Varying e The f i r s t runs test the e f f e c t of tuning by using the pseudo-gain e for the known case. Lower e means less penalty on the control e f f o r t . , The disturbance was set to zero, and the small step input was the reference sign a l . For each of the sampling times i t was attempted to f i n d an e giving similar shapes of underdamped response, c r i t i c a l l y damped response, and overdamped response. F i g . 18a,b,c shows the result for T = 0.010 s and F i g . 19a,b,c for T = 0.020 s. 71. The range for T = 0.010 s i s e O.OlxlO - 5 to O.lOxlO - 5 The range for T » 0.020 s i s E = 0.05xl0 - 5 to 0.35xl0 - 5 If i t i s considered that the control e f f o r t goes as 1/e then a broader band i s obtained for T = 0.010 s and the system i s harder to tune. The c r i t i c a l l y damped values were chosen for subsequent runs. I t i s seen from the response curves that the steady state error i s zero, as expected since the discrete transfer function contains a l / ( z - l ) factor (corresponding to 1/s i n the continuous time case). F i g . 19d shows the effect of selecting e quite low: The control signal shows o s c i l l a t i o n s and the system overshoots. Effect 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 control input s e t t l e s at 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 just l i k e a proportional c o n t r o l l e r . Because the disturbance enters ahead of the natural integrating 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 control law 7 2 . e U ( k ) + b^yCk+l) - y d(k+l)) - 0 i n fact when rearranged, interpreting y(k+l)-y d(k+l) as an error signal looks l i k e a proportional c o n t r o l l e r . When a disturbance of the same magnitude i s added to the known control case the same steady state error occurs (Fig. 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 (Fig. 22), at both sampling rates. For a larger step input, F i g . 23 shows a comparison between the responses produced by a known controller 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 resu l t s between known and adaptive control compared. Fig. 24a and b shows the r s u l t s . There i s a time lag i n position, which decreases with larger gain (smaller e). When there i s no disturbance the steady state error goes to zero at the end of the ramp. Trapezoidal Law Fi g . 25 i l l u s t r a t e s t h i s case: the response of both of the controllers i s very s i m i l a r , showing that adaptation i s taking place and that the adaptive law appears to be stable and convergent. 73. Interestingly the parameter estimates did not converge to t h e i r true values, but the control coefficients converged to a fixed relationship between each other. Effect 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 for the para-meter estimates. Three runs were done with zero i n i t i a l conditions, and with 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 stable, F i g . 26a, however i t became unstable at P(0) = 10,000 I, as i l l u s t r a t e d by F i g . 26b. The co n t r o l l e r saturated at 10 volts so the covariance matrix became singular, 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 controller was once again stable, as shown i n Fig. 26c. 4.2 RSI Robot Simulation Since the adaptive co n t r o l l e r seemed to perform f a i r l y w e l l for 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 simulation 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 gravity, C o r i o l i s , and cen t r i p e t a l effects vary, since the equations are coupled and nonlinear. It 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 controller worse; on the other hand, the fact that they are time-varying might i n j e c t enough ex c i t a t i o n i n t o the estimation and improve performance. The detailed development of the control law and of the simulation 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 position. I t was f e l t that the major features of the behaviour of the controller could be assessed by examining the response of two interacting l i n k s . The computer programs were i n any case structured for a three-link simulation, and some subroutines include three l i n k s but 8j and 9^  are set to zero. One of the segments of the simulation consists of the dynamics model of the RSI robot. The equations of motion are, from Equation ( 3 . 4 9 ) nK_ .. n 2K K <1T>i u i " T i + < n 2 jact>i 6 i + ("iP + C > i 9 i 1 * l> 2> 3 The voltage u i s the input from the controller to the system. The quanti-t i e s K^ ,, K g, R are motor constants; n i s the gear r a t i o ; «J a c t i s the motor i n e r t i a ; and C i s a f r i c t i o n factor. The j o i n t rotation i s 9, and the torque T at the join t shaft for l i n k s two and three i s given by T 2 = D2 + D 2 29 2 + D 2 39 3 + 2D 2 2 36 29 3 + D 2 3 3 9 3 + L 2 T3 = D3 + D33 63 + D32 e2 + D322 §2 + L3 where the D quantities contain trigonometric functions and robot physical parameters, and accounts for a load at the end of the t h i r d l i n k . This dynamic model of the robot i s transformed into a set of equivalent f i r s t order equations (Appendix VI ) . An integration package i s used such that, following i n i t i a l i z a t i o n , i t may be recursively 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 actual response 8 (k+1) = a y (k+1) one sampling time further. I t i s assumed that there i s the A equivalent of a zero-order hold i n the system, so that u(k) i s constant for t < kT < (t + kT) where T i s the sampling time, and k the sampling instant. A package c a l l e d DDE was used [36].The integration step size was set to T/20. The second segment i s the control block. The control law was found to be e u(k) + b 1[y(k+l|k-l) - y d(k+l)] = 0 where e i s a pseudo-gain and y^(k+l) i s the desired position one step ahead. Substituting the prediction y(k+l|k-l) given by Equation (2.37), the resulting control i s given by Equation (2.39) u(k) = W[y d(k+l)-(c 1y(k-l)+c 2y(k-2)+c 3u(k-l)+c l 4u(k-2)+c 5)] where the coefficients are given by Equations (2.40) to (2.45) i n terms of a l , a2 '^ 2 Limits of ±10 vo l t s are placed on u(k), to r e f l e c t the actual saturation 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 2,b 1,b 2,h are estimated at each step using recursive least squares (Eq. 2.35) and these estimates used to update the control c o e f f i c i e n t s . The recursive least squares estimation i s imple-mented as a subroutine and follows Bierman's fa c t o r i z a t i o n of the covariance matrix [21]. The effect of varying the forgetting factor 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 investigated. The f i n a l segment of the simulation, which completes the diagram given i n F i g . 4, consists of assorted kinematic operations. One i s the trans-formation of the vector of actual angular positions y(k) to the actual cartesian position z ( k ) , for comparison with desired cartesian position p^Ck) of the endpiece of the RSI robot. The vector p^Ck) = Z ^ ( K ^ i s user-defined. Given the i n i t i a l i z a t i o n parameters consisting of direc-t i o n cosines, i n i t i a l position, 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 trajectory planning algorithm outputs p^(k) at each time k. This desired endpiece position i s transformed using the inverse kine-matics subroutine into the vector of j o i n t angles, y^C*) * Timing of the program i s organized so that y.(k+l) i s available to calculate d u(k). A flow chart of simulation program 'R' i s given on Fig. 27. S i m i l a r i -t i e s with the block schematic of F i g . 4 referred to e a r l i e r may be noted. 4.2.1 Results and Discussion Three paths were chosen to investigate the behaviour of the con t r o l l e r . Results of the simulation are discussed i n each case. C i r c l e The f i r s t path simulated was a simple c i r c l e . Joint 3 i s required to stay locked i n position at -45°, while jo i n t 2 starts from rest and i s commanded to move at 24°/s counter clockwise. F i g . 28 i l l u s t r a t e s the configuration. This case i s ess e n t i a l l y the same as the ramp input for the single-degree-of-freedom case described i n the previous section. The 77. difference i s that the i n e r t i a term i s varying, that i s , the co e f f i c i e n t of 8^, the angular acceleration. In addition, the disturbances are varying. The second l i n k must reject gravity loads, which switch signs as the arm swings around. A load at the end of the th i r d l i n k w i l l aggravate these e f f e c t s . The t h i r d l i n k , i n order to remain stationary, must reject the .2 D 3 2 2 ® 2 disturbance as w e l l as gravity loads. Several runs were made under different conditions. The parameters for each run are given i n Table VI. In each case the arm sta r t s from rest. The quoted velocity of 24°/s corresponds to 15 seconds for one revolution 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 wrist 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 points and i n any case the performance requirements were less stringent. The veloc i t y of 24°/s for l i n k two i s probably close to maximum performance since the motor runs at over 80% capacity on the up legs and occasionally saturates (Fig. 29a). Several runs were required for tuning the pseudo-gain e which weights the control. Values of E 2 = 1.2xl0 - 6 and e 3 = 1.8xl0 - 6 were found to be satisfactory and were not varied from run to run. The forgetting factors were i n t i a l l y set to X = .9 for 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 li n e a r i z e d values about the i n i t i a l position. Covariance matrices of F 2 (0)= 100 I and Pj(0)= 1000 I proved adequate i n that case. Effect of Magnitude of Disturbance With the above conditions, the f i r s t simulation was performed by setting g = 0 and the load mass m^  = 0. The load mass i s actually the equivalent of the wrist assembly of the RSI robot, s l i g h t l y over 2 kg. 78. Setting gravity to zero simulates a planar arm and reduces disturbances. Fig. 29b shows the path traced out by the endpoint: excellent positional control i s achieved. The gap i s a combination of lag and steady state error occurring at the end of the run. For reasons similar to the single-l i n k case, the steady state error i s not rejected, and again the co n t r o l l e r behaves as a proportional c o n t r o l l e r , lacking integrating q u a l i t i e s . F i g . 29c i s a plot of j o i n t angle 3 vs time and shows that i t i s locked at 45° as required. Fig. 29d shows a plot of angle two and the control input vs time. As expected f o r a ramp input there i s a lag i n position (of 3° -6°). The control signal i s generally well-behaved. A second simulation was performed, t h i s time including gravity, but no load. The results are similar as far as tracking i s concerned (Fig. 29e); however the torque requirements are more variable, requiring a higher u 2(k) 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 (Fig. 29f). In the t h i r d run a load term was added and the above effects 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 forgetting factor X i n the estimation routine. As an extreme case i t was set to .5 for both l i n k s . (The recommended range i s 0.9 < X < 1). Recall that X close to 1 corresponds to very slowly varying parameters. Fig. 30a shows that the control u 2(k) becomes jumpy and that the v e l o c i t y becomes e r r a t i c . At the other extreme, X = 0.995, the control i s very smooth however the system i s slow to respond around t = 11 seconds when the arm passes 9 2 = 270° and starts to fight gravity (Fig. 30b). 79. Effect of Varying I n i t i a l Estimates The f i n a l three runs tested the ef f e c t of setting the i n i t i a l para-meter estimates to zero. In other words no knowledge of the system i s assumed (except the order). Thus a very high covariance matrix i s chosen, P(0) = 100,000 I. From Fig. 31a, i t i s seen that l i n k 3 droops s l i g h t l y before adjusting back to 45° after about 2.5 seconds of adaptation. The second l i n k s i m i l a r l y hunts around before finding 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 results indicate some degree of s t a b i l i t y and convergence of the co n t r o l l e r . A large covariance matrix indicates a higher degree of uncertainty 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 set to zero but P(0) was chosen f a i r l y low - 100 I. The resu l t s i s the response of F i g . 31c. A close look at the plots of jo i n t angles vs time, however, reveals (Fig. 31d) that jo i n t 2 converges to i t s proper c o n t r o l l e r . Joint 3 stays locked at u(k) = -10 volts for 9 seconds, however the adaptation seems to c l i c k on at that point when the con t r o l l e r output switches to +10 vo l t s to bring back j o i n t 3 to -45°. This type of behaviour i s of course unacceptable i n an actual system but i t does indicate some robustness i n convergence. It i s speculated that i f bounds on u(k) were not present, adaptation would take place a l o t fast e r . An alternative to powerful actuators i s adjusting the adaptation speed vi a the forgetting factor A. A few values were t r i e d for jo 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 within one degree of i t s desired value after 2 seconds, even with P(0)= 100 I and i n i t i a l 8 = [0]. 80. Summary The above results 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 position, despite the presence of gravity and other disturbances. A forgetting factor close to 1 gives smoother control signals 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 choos-ing X and P ( 0 ) . However, convergent and stable behaviour was observed. Line at 45° The second test path involved motion of both l i n k s . The endpoint of the arm was required to trace a l i n e at 45° i n the X Q ~ Z Q plane of base coordinates. 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 configuration of the robot. The arm starts at rest with l i n k s 2 and 3 out-stretched ( 6 2 = 0 and 63=-90°). A choice i s specified for either elbow up or elbow down tracing of the l i n e . The endpoint i s to follow a trapezoidal vel o c i t y law: acceleration to the cruise 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 deceleration to bring the t i p position to rest on the desired target point at time t 3 . The run parameters are given i n Table VII. 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 rate f o r l i n k 3 bcomes quite high and the motor saturates during a portion of the path. This si t u a t i o n i l l u s t r a t e s the importance of (1) good trajectory planners which foresee l i m i t a t i o n s on joi n t rates i n certain areas of the workspace, and (2) proper design of the kinematical structure of the robot so that the workspace i s usable. Evidently the complications increase when more degrees of freedom are involved. 81. For each run, X2 = X 3 = 0.9 and the gains are E 2 = 1 . 2 x l 0 - 6 and 1 . 8 x l 0 - 6 . Low Cruise Velocity The f i r s t run includes gravity and a load mass, and i s done under conservative conditions of acceleration of 2.5 cm/s2 to reach a cruise v e l o c i t y of 5 cm/s. The i n i t i a l conditions are substituted i n the equa-tions of motion to obtain i n i t i a l model parameter estimates. The disturb-ance parameter h i s set to zero. The re s u l t i n g endpoint trace i s shown i n Fig. 33a, where a reference l i n e i s indicated as well for convenience. Maximum deviation from the desired position occurs at the beginning and i s about 7 mm, decreasing steadily to 1 mm or less. Fig. 33b show j o i n t angles vs time. The con t r o l l e r was l e f t on for 2 seconds past the lO-second simulation time; steady state error decreases s l i g h t l y . In the second run conditions are the same except for higher i n i t i a l acceleration of 5 cm/s2. The cruise v e l o c i t y i s s t i l l 5 cm/s. The devia-t i o n from the path i s worse at the beginning ( F i g . 33c) possibly due to the i n i t i a l disturbance parameter h being set to zero. The motor for 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 Fig. 33d. The same conditions were specified f o r the t h i r d run except that the elbow up sp e c i f i c a t i o n was given instead of elbow down. Path following improves at the beginning, as shown i n F i g . 33e. The explanation for 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. F i n a l p o s i t i o n a l accuracy i s good. 82. High Cruise Velocity In the fourth simulation run the cruise v e l o c i t y i s increased to 15 cm/s. The response shown i n Fig. 33f i s similar to that of the other runs. Comparing voltage l e v e l s , plotted i n F i g . 33g, shows that faster variations 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 to 10,000 I , but l i t t l e improvement occurs i n the deviation pattern at the beginning. In the f i f t h run gravity and load-ing effects were removed, and much better performance r e s u l t s , as the plot of Fig. 33h indicates. Thus one may conclude that i t takes some time for the c o n t r o l l e r to adjust to the disturbances. F i n a l l y , a run was made with zero i n i t i a l estimates for the model parameters and a high P ( 0 ) . Resulting trajectory of the endpoint i s shown i n Fig. 33i. Stable and adaptive behaviour of the controller once again occurs. Summary For the 45° l i n e the two j o i n t c ontrollers succeed i n achieving path tracking within 3 mm with the exception of the i n i t i a l start-up period where errors as high as 7 mm occur. High j o i n t rates 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. Even with i n i t i a l model parameter estimates of zero, the cont r o l l e r s adapt. Steady state accuracy i s within .5° for each j o i n t . Horizontal Line The f i n a l test path i s a horizontal l i n e . The robot starts from rest with j o i n t 2 at 60° and joi n t 3 at -120°, as shown i n Fig. 34a. A trapezoidal v e l o c i t y law i s followed, as described for the previous l i n e 83. path. F i n a l configuration i s 6 2 = 96.5° and 8 3 = -180°. Table VIII gives the run parameters. This path was more d i f f i c u l t to tune and at the faster v e l o c i t y i t was necessary to decrease e, which means that the control e f f o r t was penalized less than for either the c i r c l e or 45°-line case. Better performance resulted with 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 cruise v e l o c i t y 6.3 cm/s. Maximum error i s 4 mm. No d e f i n i t e trends were evident when either A or P(0) were varied (Fig. 35a). In the next run load and gravity were re-introduced by the t i p veloc 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 for t h i s path. Varying e The pseudo-gain e was the next parameter which was varied. Decreasing i t by a factor of ten improved performance s i g n i f i c a n t l y . The desired and actual paths are p r a t i c a l l y indistinguishable. I t was possible to adjust e for this path because the required range of control signals u(k) i s small. Hence, there Is no danger of prolonged periods of actuator satura-t i o n . Two runs are shown here. The f i r s t , F i g . 35c was for X = 0.99 and the other, F i g . 35d, was for X = 0.85. The lower forgetting factor results i n a s l i g h t l y jagged trajectory but absolute error i s as good as for 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 either. The f i n a l run described was the result of an oversight but i t shows that the adaptive controller i s extremely robust and gives strong in 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 for 6, = 60° and 6, « -120°, which corresponds to an elbow 84 . up configuration. However, the elbow s p e c i f i c a t i o n for the trajectory was given as "down" (Fig. 34b). The resulting endpoint movement i s shown i n F i g . 35e. I t indicates that the arm whips down, and then returns to follow the specified path to within 0.1 mm. 85. Table I I I . Discrete Model Parameters and Coefficients for the Known Case Parameter Sampling Time 0.010 s 0.020 s a l - 1.8339 - 1.6955 a 2 0.8339 0.6955 b l 4.736xl0 - 5 1.788xl0 _ 1 + b 2 4.458xl0" 5 1.584x10-'* c l 2.5294 2.1791 c 2 - 1.5294 - 1.1791 C 3 1.314xl0 _ l t 4.616xl0_'+ 8.176xl0 _ J t 2.686xl0 - l t K/J C/J = 1.0055 = 18.1589 86. Table IV. Test Paths f o r the Single-Link Simulation , Name Time Interval [8] Reference Input Function y^(t) [rad] [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 5.7 t RAMP 2 0 < t < 5 0.40 t 22.9 t TRAPEZOID 0 < t < 1.6 0.125 t 2 7.2 t 2 1.6 < t < 3.6 0.32 + 0.4(t-1.6) 18.3 + 22.9(t-1.6) 3.6 < t < 5.2 1.12 + 0.4(t-3.6) 64.2 + 22.9(t-3.6) - .125(t-3.6) 2 - 7.2(t-3.6) 2 5.2 < t < 7.5 1.44 82.5 Note: In a l l cases y(t=0) = 0 jr. Table V. Run Parameters for the Single Degree of Freedom Simulation Run No. T samp e Path D/K 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 88. Table VI. C i r c l e Trajectory - Parameters of the Simulation Runs Run No. g [m-s - 1] m l [kg] X3 P 2(0) *3<°> 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 e 2 = .12 x 1 0 - 5 e 3 = .18 x l o - 5 i n i t i a l v e l o c i t y y 2 = 0 y 3 = 0 i n i t i a l position y 2 = 0 y 3 = -45° desired v e l o c i t y y 2 = 0.42 rad/s y 3 = 0 time for one revolution = 15 s sampling time t - 0.020 s samp integration step t = 0.001 s step Table VII. Line at 45° - Parameters of the Simulation Runs Run No. g [m-s - 1] m l [kg] P 2(0) *3<°> I n i t i a l Estimates A A £ A A *l »a 2» bi» b2' a [m-s - 2] V C i [m-s x] fc2 [s] t3 elbow 24 9.81 2.046 100 1,000 non-zero .025 .05 2 8 10 down 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 down 34 0 0 10,000 10,000 non-zero .05 .15 3 4 7 down 35 9.81 2.046 100,000 100,000 0 .05 .15 3 4 7 down In a l l runs: gain e2 .12 x 10 e 3 .18 x 10 forgetting factor x2 = 0.9 h = 0.9 i n i t i a l v e l o c i t y = 0 h = 0 i n i t i a l position v2 ES 0 v 3 = -90° desired v e l o c i t y = 0.020 s h = 0.001 s sampling time t = 0.020 s samp integration step t s t e p = 0.001 s direct i o n cosines a = 180° e 90° Table V I I I . Horizontal Line - Parameters of the Simulation Runs Run No. g [m-s - 1] " l [ k g ] p2'3<°> e2 e3 elbow V O i [m-s 1] fc2 H [s] 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 v e l o c i t y i n i t i a l position d i r e c t i o n cosines sampling time integration step acceleration Y.2 v2 v3 a 8 0 0 60° -120° 180° 90° samp 'step a 0.020 s 0.001 s 0.025 m/s2 91. a) yjW + •9" clock Digital Controller u(k) D/A u(t) G(s) y(t) A/D y(k) b) o- Digital u(k) G(z) Controller y(k) Fig.15 SAMPLED DATA SYSTEM a) sampling a continuous time system b) equivalent discrete representation 92. wfr'TE TD PLOT £\L£$ Pi 6?. 1 0 P £ 0 & £ A M x r ° ' F i - c ^ C ^ A S T . PROGRAM *Q' S T A R T READ iNPdTpUES INITIALIZE VA/RiTE T O COM IVJPOPM6.T\'O»J P i L E  <TA.LL " P A T H RETURN! g« i C A L C U L A T E 6 L Ck) WRITE TO PLOT P I L E S s. i£, < 4 4 4 c5 UPDATE <£(*0 ciAL^ULATE, COKJTAOLL,ed C o E F P i C i E M T S t'A/.X. »MT£6RATl'cOK-> SU6R, flETUftKJ viO«*i> v/fe+i) go T O f o p 1 7 P R O G R A M V FL©WCWA,R-T 0.20 to 2 0.00 -0.05 2 3 TIME (sec) CO O > r-c c O LI-LI-LU _ J O QC Z o u 2 3 TIME (sec) FIG.18a Known Control - run 2 FIG.18b Known Control - run 3 0.20 co 0.15 c CD £ 0 . 1 0 Q 0.05-t CO 2 0.00 -0.05 CO 5 8 r-QC O LU LU LU I o QC r-Z o u 'I -I I \ \ \ \ V / / / / / 0 1 2 3 4 5 TIME (sec) - r 2 TIME (sec) FIG.18c Known Control - run 4 FIG. 19a Known control T^O.020 s epsilon= 0.20E-05 0.20 2 3 TIME (sec) CO > c c O LL UL LU _ J o r r O O 12 8 4 H 0 -4 -8H -12 2 3 TIME (sec) FIG.19b Known Control - run 6 FIG.19C Known Control - run 7 0.20 2 3 TIME (sec) 12 O > O u_ LU - J O D C O 8 H 4 0 - 4 --8 -12 2 3 TIME (sec) 1^ 4 FIG.19d Known Control - run 13 0.20 to 0.15 c to O 0.05-CO 2 o.oo -0.05 f 'I •I I \ \ \ 1 1 1 v - — _ y I I I I 2 3 4 TIME (sec) TIME (sec) FIG.20 Adaptive Control - run 8 TIME (sec) FIG.20c Known Control - run 9 103. 0.20 O 0.05 CO 2 0 .00 -0 .05 2 3 T I M E ( s e c ) 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) 0.75 0.00 TIME (sec) FIG.23 Known vs Adaptive Control - run 14 0.20 TIME (sec) 0.20 TIME (sec) FIG.24a Known vs Adaptive Control - run 15 1.5 TIME (sec) 1.5 TIME (sec) FIG.24b Known vs Adaptive Control - run 16 107. 1.76 1.50-TIME (sec) 1.75 1.50-TIME (sec) FIG 25 Known vs Adaptive Control - run 17 1.8-1.6-TIME (sec) FIG. 26a Adaptive Control zero Initial estimates, P=100,000 109. 1.5 n 1 1 1 1 1 1 1 0 1 2 3 4 5 6 7 TIME (sec) TIME (sec) FIG. 26c Adaptive Control initial estimates zero, P"=10000 P R O G R A M V R ' I N P U T W R i T E T O ROKJ i tJFoaMATtOO \VAiT£ TP PljDT PI'UES tzAU. PATH •CALL IhiVEfi-SB K l N i cCALCULAT£ lL(Ja.) U J A I ' T T E T O P L O T P < L £ " $ UPDATE \Q (fe} STOP ^ O J v e P T T P £A(Ll£<->\ArJ \ FIG. 28 CIRCLE CONTROL EFFORT (volts) to CO 0 3 I POSITION (radians) I 1 1 ! , p--0.6 -0.4 -0.2 0.0 0.2 0 4 Xo <m) FIG.29b Endpoint Trace - run 21 -0.763 co c CD co CO O o. -0.785--0.787 - • -A a / A 6 e T I M E ( s e c ) 12 15 FIG. 29f circle - joint tV3 -0.78535 co c co T5 CD z o CO o o. -0 .78540--0.78545 T I M E ( s e c ) FIG. 29c circle - joint 3 CONTROL EFFORT (volts) CONTROL EFFORT (volts) K> 00 -tk O * 00 h W 00 A O •&» 03 NJ 116. 0.6-1 n i i I 1 1 1 -0.6 -0.4 -0.2 ; 0.0 0.2 0.4 0.6 Xo (m) FIG.29e Endpoint Trace - run 22 118. FIG.29h Endpoint Trace - run 23 ANG. VELOCITY (,ad/s) CONTROL EFFORT (volts) TIME (sec) TIME (sec) FIG. 30b circle - joint 2 TIME (sec) FIG. 31a circle — joint 3 TIME (sec) FIG. 31b circle - joint 2 122. FIG.31C Endpoint Trace - run 31 8 i i i I I I 0 3 6 9 12 15 T I M E (sec) circle — joint 2 0 3 6 9 12 15 T I M E (sec) FIG. 31d circle — joint 3 125. 0.4-, 0.3H 0 . 2 H O.OH -O.H ' r 1 — i 1 — | -0-1 0.2 0.3 0.4 0.5 0 6 Xo (m) FIG.33a Endpoint Trace - run 24 45 degree line - joint 2 FIG. 33b line at 45 deg. — joint 3 127. 0.4-, 0.3-{ 0.2-\ E 0.1-O Nl 0.0H -o.H -0.2-| 1 r — 1 1 1 0.1 0.2 0.3 0.4 0.5 0.6 Xo (m) FIG.33C Endpoint Trace - run 25 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-| , 1 1 1 1 0.1 0.2 0.3 0.4 0.5 0.6 Xo (m) FIG.33£ Endpoint Trace - run 26 FIG.33f Endpoint Trace - run 33 TIME (sec) 45 degree line - joint 2 TIME (sec) FIG. 33g 45 degree line - joint 3 0.5-1 i l l l I I 0.1 0.2 0.3 0.4 0.5 0.6 Xo (m) FIG.33h Endpoint Trace - run 34 133. 0.5-1 -0.1-1 1 1 1 1 1 0.1 0.2 0.3 0.4 0.5 0.6 Xo (m) FIG.33i Endpoint Trace - run 35 ,1 FIG. 3 4 HORIZONTAL LINE 0.475-. 0.450H 0.425-0.400-0.375H 0.350 0.325-| T -0.1 0.0 1 1 1 I 0.1 0.2 X o (m) 0.3 0.4 FIG.35a Endpoint Trace - run 27 136. 0.475 - i 0.450 0.425H 0.400H O N 0.375H 0.350H 0.325--0.1 0.0 — I 1— 0.1 0.2 Xo (m) — i 1 0.3 0.4 FIG.35b Endpoint Trace - run 36 0.350-1 1 r -0.1 0.0 0.1 0.2 Xo (m) — i 1 0.3 0.4 FIG.35c Endpoint Trace - run 39 138. 0.475-. 0.450M 0.425M 0.400H O M 0.375 0.350 0.325--0.1 0.0 0.1 0.2 Xo (m) — I 1 0.3 0.4 FIG.35d Endpoint Trace - run 40 0.425 N 0.375H 0.350H 0.325H 0.300 H 0.275-)— 0.0 0.5 Xo (m) FIG.35e Endpoint Trace - run 37 CHAPTER 5 140. CONCLUSIONS 5.1 Conclusions The following conclusions may be drawn: i ) A computer simulation package of a robot system has been developed which w i l l enable a user to test various control strategies. The model i s based on an e x i s t i n g robot. S t a t i c f r i c t i o n , backlash, and l i n k f l e x i b i l i t y are not modelled. Features of the simulator include: (a) an elementary trajectory planner (b) an inverse kinematics subroutine (c) d i r e c t kinematics for endpoint movement monitoring (d) the manipulator and actuator dynamics (e) a co n t r o l l e r , at present adaptive (f) input f i l e s for varying dynamical parameters and i n i t i a l conditions. The simulation package has been structured for three l i n k s with load mass. I t could be used as a test bed for evaluating di f f e r e n t control laws. i i ) 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, recursive least squares parameter estimation, and optimization of a quadratic performance index. In contrast to Koivo's [13] velo-c i t y c o n t r o l l e r , i n which approximate compensation for position error i s necessary, t h i s work uses pos i t i o n control d i r e c t l y , such that these compensations are avoided. 141. i i i ) The control law i s tested using the simulation package for two degrees-of-freedom and motion along three paths. Results indicate that the controller adapts and that i t i s stable and convergent. The control e f f o r t i s w e l l behaved, exhibiting no chatter or high frequency components. iv) The results indicate that the type of control law proposed here could be a feasible approach to improved robot control. 5.2 Recommendations for Future Work These may be divided into two groups: f i r s t , possible elaboration on the existing simulator; and second, extensions requiring more theoretical 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, consisting only of (1) obtaining and programming the equations of motion i n the form of a subroutine calleable by the integration package and (2) v e r i f y i n g the quadrant specifications for the inverse kinematics subroutine. i i ) A s e n s i t i v i t y analysis could be done, keeping the same control 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 at each control step. An analysis could be done on the effect of e s t i -mating less often than i s controlled. If performance does not degrade, this set-up i s advantageous since the computational burden i s reduced. 142. iv) An analysis of the number of computations necessary to implement the proposed control law was not carried out, and hence comparisons with other laws i s not possible. As far 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 this less c r i t i c a l every year. v) Since the offs e t or bias term does not seem to be e f f e c t i v e l y elimina-ted by the parameter h i n a l l cases, the disturbances may need to be modelled i n a dif f e r e n t way, for example as f i l t e r e d white noise v(k) = CCq" 1) e(k) This approach would lead to parameter estimation using extended least squares, for which convergence results are available [18]. vi ) The control law which results by optimizing the performance c r i t e r i o n I - {(y d(k+l) - y(k+ l ) 2 + eu 2(k)|(k-l)} does not contain any integrating terms. In practice, feedback must include an integrator, otherwise unacceptably large steady state errors result due to unmodelled effects such as f r i c t i o n . Adding an integrating term can be achieved by choosing a modified I containing a term of the form y(u(k) - u ( k - l ) ) 2 . Of course, complexity w i l l increase and so w i l l the number of calculations necessary to arrive at u(k). v i i ) As an alternative to adding more terms to the performance c r i t e r i o n , a dif f e r e n t structure of adaptive control could be examined. Instead of using adaptive control to control each jo 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 , after a l l , perform quite well once they are tuned to the operational cha r a c t e r i s t i c s of the par 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 controller which would a l t e r the gains of the feedback loops as operating conditions change. The problem, however, would have to be cast i n a different form theoretic-a l l y than i s presented i n t h i s work. vi i i ) E x t e n s i o n to endpoint control i s perhaps the most interesting aspect, but a multivariable approach i s necessary. In other words the poly-nomials A(q - 1) and B(q - 1) are replaced by matrix polynomials. This approach i s especially amenable to robots with spherical wrists since the inverse kinematics can be s p l i t at the wrist, so that (3x3) systems can be studied. BIBLIOGRAPHY 144. 1. Whitney, D.E. "The Mathematics of Coordinated Control of Prosthesis and Manipulators", ASME Journal of Dynamic Systems, Measurement, and Control, Dec. 1972, pp. 303-309. 2. Luh, J.Y.S., Walker, M.W. and Paul, R.P., "Resolved Acceleration Control of Mechanical Manipulators", IEEE Trans, on Automatic Control, Vol. 25, No. 3, pp. 468-474. 3. Freund, E. "Fast Nonlinear Control with Arbitrary Pole Placement for I n d u s t r i a l Robots and Manipulators", Robotics Research, Vol. 1, No. 1, 1982, pp. 65-78. 4. Golla, D.F., Garg, S.C. and Hughes, P.C. "Linear State-Feedback Control of Manipulators", Mech. Machine Theory, Vol. 16, 1981, pp. 93-103. 5. Cvetkovic, V. and Vukobratovic, M. "One Robust, Dynamic Control Algorithm for Manipulation Systems," International Journal of Robotics Research, Vol. 1, Winter 1982, pp. 15-28. 6. Vukobratovic, M. and Stokic, D. "Is Dynamic Control Needed i n Robotic Systems, and i f so, to What Extent?", International Journal of Robotics Research, Vol. 2, Summer 1983, pp. 18-34. 7. Hogan, N. and Cotter, S.L. "Cartesian Impedance Control of a Non-li n e a r Manipulator", WAM Robotics Symposium, 1982. 8. Lee, C.S.G. and Chung, M.J. "An Adaptive Control Strategy for Mechanical Manipulators", IEEE Transactions on Automatic Control. 9. Balestrino, A., De Maria, G. and Sciavicco, L. "An Adaptive Model Following Control for Robotic Manipulators", ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 105, Sept. 1983, pp. 143-151. 10. Nicosia, S. and Tomei, P. "Model Reference Adaptive Control Algorithms f o r I n d u s t r i a l Robots", Automatica, Vol. 20, No. 5, 1984, pp. 635-644. 11. Kim, B'.K. and Shin, K.G. "Suboptimal Control of Industrial Manipu-la t o r s with a Weighted Minimum Time-Fuel C r i t e r i o n " , IEEE Trans, on Automatic Control, Vol. AC-30, No. 1, 1985, pp. 1-10. 12. Young, K.K.D. "Controller Design for a Manipulator Using Theory of Variable Structure Systems", IEEE Trans., Vol. SMC-8, Feb. 1978, pp. 101-109. 13. Koivo, A.J. and Guo, T. "Adaptive Linear Controller for Robotic Manipulators", IEEE Trans, on Automatic Control, Vol. AC-28, Feb. 1983, pp. 162-171. 145. 14. Lobbezoo, A.J., et a l . "Robot Control Using Adaptive Transforma-tions", U.B.C. Technical Paper, June 1984. 15. Astrom, K.J. "Theory and Applications of Adaptive Control - A Survey", Automatica, Vol. 19, May 1983, pp. 471-486. 16. Landau, I.E. "Model Reference Adaptive Controllers and Stochastic Self-Tuning Regulators - A Unified Approach", ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 103, Dec. 1981, pp. 404-416. 17. Egardt, B. "Unification of Some Discrete Time Adaptive Control Schemes," IEEE Trans, on Automatic Control, Vol. AC-25, 1980, pp. 697-703. 18. Ljung, L. and Soderstrom, T. Theory and Practice of Recursive  I d e n t i f i c a t i o n , MIT Press, Cambridge, Mass., 1983. 19. Isermann, R. D i g i t a l Control Systems, Springer-Verlag, B e r l i n 1981. 20. Astrom, K.J., Borisson, U., Ljung, L. and Wittenmark, B. "Theory and Applications of Self-Tuning Regulators", Automatica, Vol. 13, 1977, pp. 457-476. 21. Bierman, G.J. Factorization Methods for Discrete Sequential  Estimation, Academic Press, New York, 1977. 22. Bar-Shalom, Y. and Tse, E. "Dual Effect, Certainty Equivalence and Separation i n Stochastic Control", IEEE Trans, on Automatic Control, Vol. AC-19, 1974, pp. 494-500. 23. Goodwin, G.C. and Sin, K.S. "Adaptive F i l t e r i n g , Prediction, and Control", Prentice-Hall, Englewood C l i f f s , N.J., 1983. 24. Egardt, B. S t a b i l i t y of Adaptive Controllers, (Lecture Notes i n Control and Information Series), No. 20, Springer-Verlag, B e r l i n , 1979. 25. Narendra, K.S. and L i n , Y.H. "Stable Discrete Adaptive Control", IEEE Trans, on Automatic Control, Vol. AC-25, 1980, pp. 456-461. 26. Goodwin, G.C, Ramadge, P.J. and Caines, P.E. "Discrete Time Stochastic Adaptive Control", SIAM Journal of Control and Optimization, Vol. 19, pp. 829-853. 27. A l l e n , C. "Moments of Inerti a of the RT-3 Robot", UBC Technical Report, Aug. 1984. 28. Paul, R.P. Robot Manipulators: Mathematics, Programming, and Control, 1st ed., MIT Press, Cambridge, Mass., 1981. 29. Lee, G.C.S. "Robot Arm Kinematics, Dynamics, and Control", Computer, Dec. 1982, pp. 62-80. 146. 30. Denavit, J. and Hartenburg, R.S. "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices", ASME Journal of Applied Mechanics, Vol. 22, June 1955, pp. 215-221. 31. Hollerbach, J.M. and Sahar, G. "Wrist-Partitioned, Inverse Kinematic Accelerations and Manipulator Dynamics", International Journal of Robotics Research", Vol. 2, Winter 1983, pp. 61-76. 32. Featherstone, R. "Position and Velocity Transformations Between Robot End-Effector Coordinates and Jo i n t Angles", International Journal of Robotics Research, Vol. 2, Spring 1983, pp. 35-45. 33. Brady, M. et a l . , (eds.) Robot Motion: Planning and Control, MIT Press, Cambridge, Mass., 1982. 34. Duncan, J.P. "CAD/CAM Algorithms", UBC Course Notes, 1983. 35. Walker, M.W. and Orin, D.E. " E f f i c i e n t Dynamic Computer Simulation of Robotic Mechanisms", ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 104, Sept, 1982, pp. 205-211. 36. Shampine, L.F. and Gordon, M.K. Computer Solution 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 Fl e x i b l e One-Link Robot", International Journal of Robotics Research, Vol. 3, F a l l 1984, pp. 62-75. 38. Binford, T.O. and Cannon, R.H. " F i r s t Annual Report on Endpoint Control of F l e x i b l e Robots", Stanford University, May 1984. 39. Astrom, K.J. and Wittenmark, B. Computer-Controlled Systems, Prentice-Hall, Englewood C l i f f s , N.J., 1984. 40. Franklin, G.F. and Powell, D.J., D i g i t a l Control of Dynamic Systems, Addison-Wesley, C a l i f o r n i a , 1980. 41. Gelb, A. (ed.), Applied Optimal Estimation, MIT Press, Cambridge, Mass., 1974. 42. Cherchas, D.B. "Robotics: Kinematics, Dynamics and Control", UBC  Course Notes, 1983. 43. Davies, M.S. "Systems Control", UBC Course Notes, 1983. 44. Bohn, . "Optimal F i l t e r i n g and Control", UBC Course Notes, 1984. 45. Raven, F.H. Automatic Control Engineering, 3rd ed., McGraw-Hill Book Co., New York, 1968. 46. Ritger, P.D. and Rose, N.J. D i f f e r e n t i a l Equations with Applications, McGraw-Hill Book Co., New York, 1968. 147. 47. Narendra, K.S. and Monopoli, R.V. , eds. Applications of Adaptive  Control, Academic Press, New York, 1980. 48. Landau, Y.D. Adaptive Control: The Model Reference Approach, Marcel Dekker, New York, 1979. 49. Harris, C.J. and B i l l i n g s , S.A., eds. Self-Tuning and Adaptive  Control: Theory and Applications, Peter Peregrinus Ltd., Stevenage, U.K., 1981. 148. APPENDIX I  RECURSIVE LEAST SQUARES A derivation of the standard recursive least squares algorithm i s included here for the sake of completeness. The least squares estimate i s derived f i r s t ; then a recursive form of the solution 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 variable, v(k) i s unknown noise 8 i s the vector of parameters to be estimated 8 T - [Bl e 2 ••••0n] and 4> i s a vector of measurements T If 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 least 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 function N 1(9) = I i ( k ) 2 k=l (1) 149. where N Is the number of measurements taken and u>(k) Is a positive number allowing different weights to be attached to different observations. Introducting the vectors * T(N) - [ y i y 2 ...y N] e T(N) = [ex e 2 ...e N] and the matrix + T (2) +T(N) and dropping the N for the time being, the c r i t e r i o n function may be re-written Substituting 1 ( 8 ) = e e e = Y - Y , Y = c>8 (2) into (2) gives 1 ( 8 ) = ( Y - * 8 ) A (Y-o . 8 ) = Y T Y - (•e)1^ - YT«>e + « > 8 ) T «>e T T T T - Y Y - 2Y «>e + e «> <>e 150. Performing the minimization with respect to 9 gives 3 1 - 0 - 2 $ T Y +2 * T $ 6 36 Thus the best estimate 6 i n the least 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 this point. When a new measurement i s obtained, the matrix $ and the vector Y w i l l be [•(H) I f Y- (N)l T ' Y ( N + 1 ) = • (N+i)J t y(N+i)J • (N+l) The estimate 6(N+1) i s , from ( 1 ) ^(N+l ) = [+ T (H+1> • ( N + l ) ] - l / ( N + l ) Y(N+1) (4) noting that N • 00 •(») - l *O0 • 00 O k=l The above Equation (4) may be re-written 151. $(N+1) - I * T ( N ) *(N) + K N + 1 ) / ( N + l ) ] " 1 x [ + T ( N ) Y ( N ) + i ) T (N+l) y(N+l) ] - [• • + • • ] 4 [• * + ** ] (5) where the arguments have again been suppressed for convenience. Rewriting, IWi) = (f^r 1 A + [(«>T* + wV* - ( o ' c ) ) - 1 ] A + («)T«) + t t T ) - » * y Recalling that 6 ( N ) = ( e f t ) " 1 <>TY and observing that + t t T ) " l - 4% - W T ) ( « ) T 0-1 <)TY T T T /\ Then (5) may be written $(N+1) = 6(N) + K ( N ) [y(N+l) - t T ( N + l ) 9 ( N ) l where K(N) = [* T (N+1) • ( N + l ) ] " 1 *(N+1) In order to obtain a recursive form for K(N), the matrix P(N) i s introduced P ( N ) .- [ / ( N ) * ( N ) ] - 1 Then P(N+1) .= + t t 1 ] - 1 At this point the "matrix inversion lemma" i s used. LEMMA. Let A , C , and ( C ~ 1 - H ) A _ 1 B ) be non-singular square matrices. Then [ A + B C D ] - 1 = A _ 1 - A ~ 1 B [ C ^ - W A ^ B l " 1 D A " 1 Taking A = <(>%, B = C - I, D = * T P(N+1) may be written as 153. P ( N ) - P(N)*[1 + * T P ( N ) I*]" 1 t T P ( N ) (8) So, K(N)= P(N+1) t C N + l ) P ( N ) K N + 1 ) - F ( N ) n T F ( N ) f f f + D [1 + +TP(N)4>] K ( N ) = P ( N ) K N + D [1 + * P ( N ) t ] " 1 (9) and, P(N+1) - P ( N ) - K ( N ) f T ( H + l ) P ( N ) P(N+1) - [ I - K ( N ) f T ( H + l ) ] P ( N ) (10) The recursive least squares estimation consists of Equations (6), (9) and (10). A weighting factor may be included i n the c r i t e r i o n function to account for time-varying parameters N 1(8) - E u(k) fc(k)2 k=l The weighting function chosen for th i s study was the common exponential forgetting factor u(k) = X N~ k , 0 < X < 1 In that case the recursive least squares equations become 8(10 - £(k+D + K(K-l)[y(k) - tT(k) t(k-l)] K ( K - l ) = P(k- l ) Kk) I* + *V) P(k- l) *(k)]-! P(k) - [ I - K(k- l) /(k)] P(k- l) /X The reader i s referred to Lying and Soderstrom [18] who show how the modified equations are obtained. 155. APPENDIX I I THE A AND T MATRICES cos(8 ) i s denoted C and sin(6 ) i s denoted S Substituting values from Table I I into the general expression A^ given i n section 3.2.1 gives r S *2 -*3 c l 0 s l 0 s l 0 - c l 0 0 1 0 d l 0 0 0 1 C2 " S2 0 a2 S2 C2 0 a2 0 0 1 0 0 0 0 1 C3 0 - S3 0 S3 0 C3 0 0 -1 0 0 0 0 0 1 The transformation matrices T^ , r e l a t i n g vectors expressed i n coordinate frame i to base coordinates are easily found by m u l t i p l i c a t i o n T l = A l C1 C2 - c l S 2 S l a 2 C l C 2 S1 C2 - S1 S2 " C l U2 S1 C2 S2 C2 0 a 2S 2+d 0 0 0 1 1 156. c 1 ( c 2 c 3 - s 2 s 3 ) S 1 ^ C 2 2 3 _ S 2 S 3 ^ s 2 c 3 + c 2 c 3 - c l 0 0 " cl( C2 C3"* S2 S3^ - s 1 ( c 2 c 3 + s 2 c 3 ) -S 2S 34C 2C 3 a 2 C l C 2 a 2 S l C 2 a 2 S 2 + d l These matrices are used to obtain the equations of motion using the Lagrangian approach. 157. APPENDIX I I I  THE PSEUDO-INERTIA MATRIX Jj From equation (3.32), the pseudo-inertia matrix was defined J . • J r r dm link i Expanding gives dm J x y dm J x z dm /*"x dm i y dm dm J y z dm /*y dm i z dm J y z dm JC1*) 2 dm /*z dm z 1* dm /*"y dm J*z dm / dm Defining xx yy zz xy = /(y 2 + z 2) dm = J(x 2 + z 2) dm = J(x 2 + y 2) dm = /xy dm I = fxz dm xz I = jyz dm yz 158. mx = Jx dm my = /y dm mz = /z dm Then i t is possible to express the diagonal terms as -I +1 +1 i x = / ( V dm - x x rr—E£ . i - i + i T r,i- \2 J xx yy zz I = J( z ) z dm = « I +1 - I T (A \2 A xx vy zz and Jj^  may be writte more concisely as *ix I. ixy *ixz m i X i I. ixy I. iyz V l *ixz I. iyz X i z nI«l m i x i V i m iz i m i APPENDIX IV DERIVATION OF THE LOAD TERM#. 159. The load term L^ may account for the wrist assembly or for an arbitrary mass placed at the end of the th i r d l i n k of the RSI robot. The Lagrangian approach i s applied i n much the same way as i t i s for the manipulator i t s e l f , the applicable equation being d .9K . 3K 8P The potential energy i s found f i r s t . The position of the mass i s described by [0 0 z x 1] or, i n l i n k 1 coordinates, a fixed frame i n the simulation, by A 2 A 33p 1 Using results from Appendix I I and performing the mu l t i p l i c a t i o n gives - z 1 sin(8 2+6 3) + a 2 cos6 2 z^ cos(9 2+0 3) + a 2 cos9 2 0 The gravity vector i n l i n k 1 coordinates i s given by 160. g T [0 g 0 1] The potential energy P i s then P - l g T 1 p 1 which gives P = m1 g[a 2 s i n 6 2 + z^ cos(8 2+6 3)] Next, the k i n e t i c energy i s found. In base coordinates the position of the mass i s Then Its velocity i s dp 3 3 T 3 ^~ = i = i *Te± 3 p l Since l i n k 1 i s locked i n position, 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 = Trace (p ]L p x ) and the k i n e t i c energy i s K = 2 m l dp lv2 <dT> giving the result K - •|-m1[e2 ( a 2 - 2 a 2 z 1 sinGg + z 2) — .2 _ _ . + z 2 6 3 + 2 ( z 2 - a 2 z 1 s i n e 3 ) 6 2 0 3 ] Substituting K and P into Equation (3.25) and performing the required operations give 6 ^ 2 and The res u l t i s given i n Equations (3.42) and (3.43). 162. APPENDIX V J . FOR THE RSI ROBOT From Appendix I I I , the general expression for i s * i x *ixy * i x z m i x i * i x y hy I. i y z V i * i x z I. i y z * i z V i m 1x i V i V i m i Referring to Fig. 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 y1 0 1] P2 - £ 2 0 0 ^ p j = [0 0 z 3 1] Then the pseudo-inertia matrices f o r i = 1,2,3 are X l x 0 0 0 0 zh 0 n^y 0 0 X l z 0 0 m i y " i 0 m i L 2 X m2X2 0 0 m 2x 2 L 2 Z 0 0 3z 0 0 •3 X 0 0 0 I, 0 0 0 0 0 0 m 3 z 3 m 3z 3 m3 164. APPENDIX VI  SETTING UP THE INTEGRATION SUBROUTINE The f u l l equations of motion are given i n section 3.3.3. I d e n t i f y i n g groups of terms to s i m p l i f y the notation, we l e t T2 = ( R 2^ T3 = (TT>3 C3 • < C * - R ^ > 3 •I " g l a 2 ( m 2 + m 3 ^ + m 2 x 2 l •2 = V*3Z3 , 2 2 F, = I, + n^J „ + a,m9 + a,m, + 2a 0m 9x 0 + 1 , 3 '2Z aCt2 ^ Z ' d £.£./. 3yy F 4 = m 3 z 3 a 2 Fc = I, o •'yy Ffi = Iq + . n D dyy aCt3 165. 8V2 L 2 « L3 = <a2 + V>m* L5 " V i * Using the variables defined above we l e t A = D 2 + L 2 - (Fj + L^cosej, - (F 2 + L 2) sin(9 2 + 63) B = D 2 2 + L 2 2 - L 3 - L 3 - 2(F1+ + L^) sin 0; C " D23 + L23 = D 3 2 + L 3 2 = (F 5 + L 5) - (F^ + L^) sin 63 E = " ( D 2 2 3 + L 2 2 3 ) = D 3 2 2 + L 3 2 2 = - ( D233 + L233> = (F,, + L^) cos 63 166. P " D 3 + L 3 = - ( F 2 + L 2) 8 i n ( 6 2 + 6 3 ) Q ° D 3 3 + L 3 3 - F6 + L5 Then the equations of motion may be re-written . 2 T 2u 2 - C 29 2 = A + B6 2 + C9 3 - 2E8 29 3 - E8 3 2 T 3 U 3 ~ C 3 ^ 3 = P + QQ3 + C 6 2 + E ® 2 Collecting 6 2 and 8 3 to one side e 3 - £ [T 2u 2 - c 2e 2 - A + 2 E 8 2 e 3 + E§ 3] - | e'2 °2 - £ t T 3 U 3 - C 3 § 3 ~ P " E § 2 ^ " r J 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 x l ° ° 2 " 2 " § 3 x 3 = e 2 \ - 9 3 so we obtain x l " 82 *2 - 63 X3 = x l K = x2 F i n a l l y , substituting for 6 2 and 6 3 we obtain x l = [ - T T - H T 3 u 3 - C 3x 3 - P - Ex 2] C2-QB C 2 ~ [— ] [ T 2 u 2 - C 2x, - A + 2Ex,x9 + Ex,] C 2 _ Q B ^ 1 1 2 2 1 *2 " I — -—^][T 2u 2 - C 2x, - A + 2Ex,x ? + Ex*] C2-QB * B 2 " [— ] [T 3u 3 - C 3x 2 - P - Ex, ] C 2 _ Q B 6 * 3 2 1J x3 = x l \ = x2 This i s the form of the equations integrated by the DDE subroutine. 

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

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

Comment

Related Items