MODEL REFERENCE ADAPTIVE CONTROL OF A MANIPULATOR IN CARTESIAN COORDINATES by HENRY HERBERT VOSS B.A.Sc., The Un i v e r s i t y of B r i t i s h Columbia, 1983 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 t h i s thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA August, 1986 © Henry Herbert Voss, 1986 In p r e s e n t i n g t h i s t h e s i s i n p a r t i a l f u l f i l m e n t of the requirements f o r an advanced degree at the U n i v e r s i t y o f B r i t i s h Columbia, I agree t h a t the L i b r a r y s h a l l make i t f r e e l y a v a i l a b l e f o r r e f e r e n c e and study. I f u r t h e r agree t h a t p e r m i s s i o n f o r e x t e n s i v e copying o f t h i s t h e s i s f o r s c h o l a r l y purposes may be granted by the head o f my department or by h i s or her r e p r e s e n t a t i v e s . I t i s understood t h a t copying or p u b l i c a t i o n of t h i s t h e s i s f o r f i n a n c i a l gain s h a l l not be allowed without my w r i t t e n p e r m i s s i o n . Department of M^C^vtical L iA K v p o s i t i o n and v e l o c i t y gain ID^ natural frequency £ damping r a t i o Z summation Ia^ actuator i n e r t i a a,3 c o e f f i c i e n t s i n j o i n t c o n t r o l l e r equation a,b c o e f f i c i e n t s i n reference model equation new p o s i t i o n gain e error between reference model and system F(e) cost function q cost function c o e f f i c i e n t - i x -6j>6 2 d i f f e r e n c e between c o e f f i c i e n t s i n j o i n t c o n t r o l l e r and reference model equations A 6 j j A 6 £ change i n c o e f f i c i e n t s VF(e) gradient of cost f u n c t i o n v Lagrange m u l t i p l i e r d/dt d e r i v a t i v e with respect to time 3/3t p a r t i a l d e r i v a t i v e with respect to time u 3y/3a to 3y/3b state space v a r i a b l e TQ s i m p l i f i c a t i o n v a r i a b l e A(t) l i n e a r time varying matrix i n state space equation F_(z,t) nonlinear time varying matrix i n state space equation R slope of ramp input ¥ fundamental s o l u t i o n matrix C monodrony matrix - x -ACKNOWLEDGEMENT I would l i k e to thank Professor Dale B. Cherchas for the guidance and encouragement he provided i n the development of t h i s work, Professor Peter D. Lawrence for stimulating my i n t e r e s t i n th i s area, and my wife Claudia f o r her love and support throughout. Some funding f o r t h i s project was obtained from NSERC Grant No. A4682. - x i -CHAPTER 1 1. INTRODUCTION 1.1 Review of Robotics In today's i n d u s t r i a l world, the quest f o r higher l e v e l s of produc-t i v i t y has focused much att e n t i o n on the l a t e s t advance i n automation, the i n d u s t r i a l robot. It represents a new l e v e l i n programmable automa-t i o n with the advantage of being adaptable to a va r i e t y of job assignments. It has the a b i l i t y to change tasks primarily by software changes with few, i f any, hardware changes. This has attracted much att e n t i o n i n industry, evidenced by the exponential growth of robots i n existence and the increasing number of robot manufacturers. The f l e x i b i l i t y of robots i s evident i n the v a r i e t y of tasks to which they are applied. They are p a r t i c u l a r l y well suited to low volume batch production jobs where d i s c r e t e parts can be manufactured on demand with small setup costs. Tasks which are highly r e p e t i t i v e or are i n hazardous environments are s u i t a b l e to f i r s t generation robots while newer, more sophisticated robots are entering f i e l d s where higher accuracy and performance l e v e l s are required. T y p i c a l tasks could include pick and place operations, spot and seam welding, i n s t a l l a t i o n or removal of components, spray painting, packaging, d r i l l i n g , sanding and grinding, among many others. The importance of robots has focussed media a t t e n t i o n on t h i s f i e l d . Unfortunately t h i s o v e r - p u b l i c i z a t i o n has led to a romanticized view of robotics by the p u b l i c . The c a p a b i l i t i e s are far below general expectations (Luh [1983]). To understand why, i t i s necessary to examine the structure of robots and t h e i r controls. 2. An i n d u s t r i a l robot i s a programmable mechanical manipulator. It can take many d i f f e r e n t forms as indicated i n Figure 1.1. T y p i c a l l y , a robot can have s i x j o i n t s and s i x degrees of freedom, each with i t s own drive, which enables i t to reach any point i n i t s 'workspace' with any o r i e n t a t i o n . The drives of these robots may be e i t h e r hydraulic, pneumatic, or e l e c t r i c and each would have a feedback control loop such as shown i n Figure 1.2. The p o s i t i o n - e r r o r and velocity-damping gains i n the control diagrams are set to ensure the natural frequency of the c h a r a c t e r i s t i c equation of the closed loop c o n t r o l l e r i s l e s s than h a l f of that of the p a r t i c u l a r j o i n t , and also to have a damping r a t i o of one. Feedforward terms used usually include g r a v i t y compensation of the manipulator i t s e l f and could also include terms for complaint torque, which can be c a l c u l a t e d by methods described by Paul [1981]. Each j o i n t can be viewed as a p o s i t i o n a l c o n t r o l l e r and thus the robot as a p o s i t i o n i n g device. This form of c o n t r o l i s c a l l e d conventional, or proportional-d e r i v a t i v e c o n t r o l . This has been found to work adequately but perform-ance has been l i m i t e d i n the areas of speed, payload, and accuracy. It i s for these reasons that during the l a s t decade there has been inten-sive research i n t h i s f i e l d . It has also been motivated by great improvements i n computational power as well as developments i n the area of adaptive c o n t r o l theory. The j o i n t c o n t r o l l e r s work i n j o i n t coordinates, that i s they respond to desired and actual j o i n t p o s i t i o n e r r o r s . A desired path must be converted i n t o a set of corresponding j o i n t coordinate setpoints by an inverse kinematic routine. Although these setpoints t h e o r e t i c a l l y can lead to the corr e c t endpiece path, the f a c t that the j o i n t Figure 1.1. Four Major Robot Coordinate Systems Figure 1 . 2 . Typical Feedback Loop 5. c o n t r o l l e r s are driven by errors between act u a l and desired p o s i t i o n leads to two problems: 1) as the j o i n t s are i n s e r i e s , errors i n j o i n t p o s i t i o n w i l l multiply to become s i g n i f i c a n t endpoint e r r o r s , 2) to keep tr a j e c t o r y errors small, many j o i n t setpoints corresponding to points along the desired path must be used. This i s the focus of t h i s t h e s i s . The use of many setpoints along a path l i m i t s performance for path following tasks such as movement i n a s t r a i g h t l i n e . The generated force or torque from each actuator i s primarily dependent on the current p o s i t i o n e r r o r . If many setpoints are used these e r r o r s are small and thus actuator output w i l l be small and the robot w i l l perform slowly. If on the other hand few setpoints were used the robot would perform fas t e r but not necessarily along a desired path. A method i s proposed which uses a model reference adaptive c o n t r o l technique to update gains such that errors between the actual path and desired path f o r each j o i n t are minimized. This allows better path following using fewer set points such that the robot can perform i t s task more e f f i c i e n t l y with low t r a j e c t o r y error. The form of the conventional c o n t r o l l e r i s retained, but with changable gains which would be c o n t r o l l e d by a supervisory c o n t r o l l e r . These gains change smoothly so the actuator output also changes smoothly. The rapid output changes that would occur under other adaptive c o n t r o l s t r a t e g i e s such as s e l f - tuning or v a r i a b l e structure systems ( s l i d i n g mode) adaptive c o n t r o l are not encountered. 1.2 L i t e r a t u r e Review The area of r o b o t i c c o n t r o l has been the subject of intensive research during the l a s t decade. Studies into applying d i f f e r e n t types 6. of adaptive c o n t r o l as w e l l as enhancing conventional c o n t r o l have been performed. Currently no i n d i v i d u a l control method has shown clear s u p e r i o r i t y over others. Implementation of these schemes has been done i n research laboratories but not yet on commercially produced models. Early work was done by Whitney [1969] on resolved motion rate control which derived j o i n t v e l o c i t i e s from desired endpiece positions and v e l o c i t i e s . This had p a r t i c u l a r a p p l i c a t i o n to prostheses and manipulators i n which desired paths i n endpiece or world coordinates was desired. This involves the use of the inverse Jacobian matrix which can be d i f f i c u l t to c a l c u l a t e on-line for a s i x degree of freedom robot or impossible i f the Jacobian matrix i s singular. The method developed suitable transfer functions which allowed d i r e c t control of c a r t e s i a n motion by j o y s t i c k or other form of c o n t r o l l e r . This method i s l i m i t e d because of computational i n t e n s i t y and the accuracy of the model required. Paul [1981] developed methods f o r c a l c u l a t i n g feedforward compensa-tio n terms for conventional c o n t r o l l e r s . This i s normally unrealizable due to computational demands and also to unknown or varying parameters of the robot and payload. For example the actuator c h a r a c t e r i s t i c s w i l l change with operating temperature and time and also the payload w i l l have variance i n both weight and i n e r t i a . It has proved useful for c a l c u l a t i n g the simpler yet s i g n i f i c a n t g r a v i t y compensation terms f o r the manipulator i t s e l f , which i s commonly used i n commercial robots. Luh, Walker,and Paul [1980(b)] extended the work of Whitney to include resolved accelerations but the problems of i n v e r t i n g the Jacobian matrix and d e t a i l e d modelling of the manipulator s t i l l e x i s t . 7. Other researchers have derived adaptive co n t r o l s t r a t e g i e s which depend on accurate representation of the manipulator and i t s payload. Vukobratovic and K i r c a n s k i [1983] use a two stage procedure incorporat-ing nominal dynamics and perturbation about zero error states. This involves using a l i n e a r time-invariant model of the manipulator about a zone of high i n e r t i a l coupling between degrees of freedom which i s tr a j e c t o r y and load dependent. N i c o s i a and Tomei [1984] use a model reference adaptive system approach which uses a unit vector control law to define c o n t r o l a c t i o n . R e a l i z i n g the computational demand presented by schemes which require much pr i o r knowledge of the system, other researchers have worked on c o n t r o l s t r a t e g i e s which presuppose l i t t l e information about the system. Young [1978] proposes a variable structure control system which uses the bounds of the system parameters to determine l i m i t s on control action. This method ensures asymptotic s t a b i l i t y but su f f e r s from 'chatter' due to switching at f i n i t e frequency as the e r r o r i s reduced. This method has been referred to as ' s l i d i n g mode' because c o n t r o l a c t i o n i s along a switching l i n e i n the phase plane. Balestrino, De Maria, and Sciavicco [1983] propose an adaptive model following c o n t r o l system which also has two d i s t i n c t c o n t r o l modes but less chatter i n actuator output. The s e l f - t u n i n g regulator introduced i n 1973 by Astrom and Wittenmark has been applied to manipulators by Koivo and Guo [1983], Marchand [1985], and recently Koivo [1985]. An autoregressive moving average model of the process i s assumed where the parameters of the model are estimated r e c u r s i v e l y by l e a s t squares techniques. Control action based on quadratic performance c r i t e r i a and estimated parameters 8. i s used at each time step. Koivo and Guo used j o i n t v e l o c i t y as co n t r o l l e d variable while Marchand used j o i n t p o s i t i o n . Koivo recently extended the m u l t i v a r i a b l e model to include coordinate transformations so that the output vector of the model i s i n world coordinates while the input vector i s j o i n t actuator forces or torques. This shows the adapt-a b i l i t y of the s e l f - t u n i n g regulator. Interest has also been sparked by i t s use of d i s c r e t e time models which are highly suited to computer c o n t r o l l e r s . The approaches mentioned a l l require a 'learning' period i n which the c o n t r o l l e r learns about i t s environment, and must also have persistent e x c i t a t i o n , such that there i s always new information i n t r o -duced to the system at a reasonable rate. Algorithms which use d i f f e r -ent or v a r i a b l e forgetting f a c t o r s , used i n the recursive parameter estimation scheme, have been discovered but have not yet been applied to manipulators. The s e l f - t u n i n g regulator actuator output i s very rough, the output t y p i c a l l y o s c i l l a t e s i r r e g u l a r l y between i t s upper and lower bounds. This may induce undesirable high frequency v i b r a t i o n p a r t i c u -l a r l y at the j o i n t s near the endpiece which t y p i c a l l y have high natural frequency. This has not been reported as only preliminary experimental studies on a c t u a l manipulators have been published. Dubowsky and DesForges [1979] use a simple second order reference model f o r each j o i n t and update the gains i n the conventional c o n t r o l l e r such as to minimize a quadratic cost function. This method i s computa-t i o n a l l y much simpler than other methods and has been shown experiment-a l l y to work. This thesis extends t h i s approach but uses a model which i s based on desired behaviour In Cartesian coordinates, and u t i l i z e s a d i f f e r e n t cost function. 9. The general task of path following has been looked at from several angles. Resolved motion and resolved acceleration methods have been mentioned but are not s u i t a b l e f o r accurate t r a j e c t o r y following. Computed torque techniques have been derived by Luh, Walker and Paul [1980(a)] f o r c a l c u l a t i n g torque p r o f i l e s i n order to a t t a i n desired j o i n t v e l o c i t i e s and p o s i t i o n s . This requires a comprehensive and accurate model of the manipulator and payload. Optimum tr a j e c t o r y planning to minimize consumed energy has been investigated by Vukobratovic and K i r c a n s k i [1982]. This o f f - l i n e technique could be used for high speed motions as well as movement of heavy loads but does not involve c o n t r o l adaptation to ensure path following. Suboptimal control using minimum time-fuel c r i t e r i a has been investigated by Kim and Shin [1985]. A suboptimal feedback c o n t r o l l e r i s used i n which averaged manipulator dynamics are updated at each sample i n t e r v a l . Results of r e a l time implementation of t h i s scheme have not been published. The i n t e n t i o n of t h i s project i s to develop an algorithm which, when compared with conventional methods, w i l l provide better path following performance. 10. CHAPTER 2 ROBOT DYNAMIC RESPONSE SIMULATION 2.1 Introduction Simulation of the dynamic response of a robot on a computer is an expedient way to test control algorithms. This is accomplished by numerically integrating the dynamic equations of motion over time. The dynamic equations are highly coupled and nonlinear, making the simula-tion computationally intensive and, without simplifications, difficult to realize in real time. However this method is widely used by researchers to check the performance of control strategies, in path following, in minimum time-fuel optimization, and in stability. The first step in the development of the simulation model is to develop the kinematic equations of motion. These relate the position and orientation of the end of the robot, (in the case of a revolute robot) to the angles of the joints. Inverse kinematic equations are then derived which determine the required joint angles to achieve a particular endpiece position and orientation. Both these equations are required for determination of specific trajectories and for the evalua-tion of robot performance when compared with the desired trajectory. For example, current research is being done in the Department of Mechanical Engineering at the University of British Columbia in the development of off-line trajectory planning for use in welding. A prescribed path is developed and downloaded to the robot as a series of desired joint setpoints. The robot responds by trying to achieve this series of setpoints and thus follow the desired path. The actual path may differ from the desired path as the joint controllers are driven by 11. errors which are proportional to the diffe r e n c e between the a c t u a l and desired j o i n t p o s i t i o n . If these errors are not reduced to zero there w i l l be a discrepancy between the desired and actual path. The second step i s the development of the dynamic equations of motion. These can be derived by the Lagrangian method. The equations are highly coupled and nonlinear and include a l l dynamic e f f e c t s . External e f f e c t s can also be modelled provided information about the external disturbance i s known. The c o e f f i c i e n t s i n the dynamic equations vary with p o s i t i o n and v e l o c i t y and must be c a l c u l a t e d each i n t e g r a t i o n i n t e r v a l during simulation. Previous work by Boucher [1985] developed these c o e f f i c i e n t s f o r the PUMA 560 robot but were not e f f i -cient when used i n computer simulation because they had not been s i m p l i f i e d by use of trigonometric i d e n t i t i e s and al g e b r a i c c a n c e l l a -t i o n . They have been rederived i n a condensed form for the same robot with the l a s t three l i n k s f i x e d i n t h e i r zero p o s i t i o n . This has allowed considerably f a s t e r simulation run time (by a factor of approxi-mately 60) with the same accuracy. The robot simulation algorithm steps are as follows. A path planning routine downloads j o i n t setpoints, which are functions of time, to the j o i n t c o n t r o l l e r s , which i n turn output corresponding voltages to the motor simulation routines which then i n turn output a torque to drive the p a r t i c u l a r j o i n t . Using t h i s torque i n the dynamic equations the angular accelerations are found and integrated with time to simulate the robot performance. J o i n t angles are measured, and through a d i r e c t kinematic routine the actual path of the endpiece i s determined. Actual and desired paths are then compared to evaluate the performance of the control algorithm. The motor model used i n t h i s simulation i s simple 12. and assumes a constant armature resistance and zero f r i c t i o n . In actual robots the armature resistance would be a function of operating tempera-ture, the f r i c t i o n would be nonzero, have s t a t i c and dynamic components, and be nonlinear i n nature. However, th i s s i m p l i f i e d l i n e a r approach i s a close approximation which provides i n s i g h t to the c o n t r o l algorithm performance. In t h i s t h e s i s two robots are simulated: a PUMA 560 which i s i n the Computer Aided Manufacturing and Robotics Laboratory i n Department of Mechanical Engineering at the U n i v e r s i t y of B r i t i s h Columbia and an ide a l two l i n k manipulator with massless l i n k s . These are shown i n Figure 2.1. 2.2 Kinematics The forward or d i r e c t kinematics i s a method by which the p o s i t i o n and o r i e n t a t i o n of the end ef f e c t o r or f i n a l l i n k of a robot can be determined by the current values of the j o i n t angles or p o s i t i o n s . The opposite problem of fi n d i n g the required j o i n t p o s i t i o n given a desired endpiece l o c a t i o n and o r i e n t a t i o n i s c a l l e d the inverse kinematic problem and may not r e s u l t i n a unique s o l u t i o n . A convention has been developed f o r f i n d i n g the forward kinematic s o l u t i o n . This method can be found i n Paul [1981] and i s also well described i n Lee [1982]. A world reference frame i s defined at the base of the robot. From t h i s a series of reference frames are then defined, each corresponding to a degree of freedom according to a convention defined by Denavit and Hartenberg [1955]. See Figure 2.2 for frame assignments f o r the PUMA 560 robot. Each frame can be r e l a t e d to adjoining frames by a homogeneous transform matrix. By pre-multiplying 13. a) Two-Unk Manipulator WAIST (JOINT 1) SHOULDER (JOINT 2) ELBOW (JOINT 3) WRIST ROTATION (JOINT 4) WRIST BEND (JOINT 5) FLANGE (JOINT 6) b) PUMA 560 Robot Figure 2.1. Two-link manipulator and PUMA 560 Robot 14. PUMA ROBOT ARM LINK COORDINATE PARAMETERS JOINT 1 *< RANGE 1 90 -90 0 0 -160 to «160 X 0 0 432 mm 149.5 mm -225 to +45 3 90 •0 0 0 -45 to t-225 4 0 -90 0 432.0 -110 to +170 S 0 90 0 0 -100 to +100 6 0 0 0 56 5 -266 to +266 Note: o r i g i n s of frames Fg and are coincident as are o r i g i n s Origins of ~ of F 2 and F^. F. and F c are also coincident. 4 5 Figure 2.2. Frame Assignments for PUMA Robot 15. a vector in frame j by the homogeneous transform matrix A. . we get the i » J corresponding vector in reference frame i . p i = h , i Pj [ 2 ' 1 ] By successively multiplying these transforms matricies together we can get a transform matrix relating a vector in world coordinates with one in the reference frame of the final link. This is valuable as i t is the path of an end effector or tool gripped by the last link that we are interested in. T. , = A - , • A . . A • A • A , . A [2.2] •=-0,6 —0»1 -1 ,2 —2,3 — 3 , 4 -4 ,5 —5,6 PO * l 0 , 6 * P 6 I2'3! The inverse kinematic solution is not as straightforward and involves trigonometric and geometric Insight. There is no general technique used for solving this problem as each geometric robot configuration is unique although such classes as the PUMA share the same form of solution. The transformation matrix from the world coordinate frame to the endpiece coordinate frame can be written as a series of vectors. T0 > 6 = [£»2.»£»£J t 2 - 4 l By looking at the values of the elements of this matrix and using algebra and trigonometric identities the solution for particular joint 1 6 . v a r i a b l e s can be found. See Cherchas [1985] f o r the method used f o r solving the PUMA class robot. 2.3 Dynamics The dynamic equations of motion are required to simulate the motion of the robot. They specify the angular a c c e l e r a t i o n of a j o i n t of a revolute robot given the torque applied to a l l j o i n t s , the current configuration and v e l o c i t i e s of the j o i n t s , and the loading of the robot by g r a v i t y and other forces. They are highly nonlinear and coupled. These equations can be found by using the Lagrangian approach. The Lagrangian L i s defined as the di f f e r e n c e between the k i n e t i c and poten-t i a l energy of the robot. L = K - P [2.5] The corresponding force or torque can be found as follows T = fL l i 1L_ \o 61 1 8 " i 1 1 These can be found for a l l l i n k s . Once t h i s i s done, algebraic manipu-l a t i o n can be done to solve f o r angular a c c e l e r a t i o n (or l i n e a r acceler-ation i n the case of a prismatic j o i n t ) of each j o i n t . The c o e f f i c i e n t s can be wr i t t e n i n matrix form as follows: n 8T 3T T °ij = = ^ S J P 8 ? ) [ 2 ' 7 ] J p=max i , j H2 i 17. n 3 2 T 9 T T D i j k p=max I,j,k f =P_ j _ Z L i l»q ; J 3 q k P 9q± J [2.8] D i p=l n E [2.9] The i n e r t i a l terms J . remain constant but the transform matricies T and i -p t h e i r d e r i v a t i v e s change with p o s i t i o n . The motor model used i n both simulations i s i d e a l i z e d . The armature resistance i s assumed constant and the e f f e c t of f r i c t i o n i s neglected. This leads to a model which has a constant motor torque r e l a t i o n s h i p . This i s j u s t i f i e d because i t i s s u f f i c i e n t as a f i r s t approximation i n the study of the c o n t r o l algorithm behaviour. The e f f e c t of f r i c t i o n can be viewed as part of the v e l o c i t y gain term. I f f r i c t i o n would be included then the v e l o c i t y gain term would be smaller by an appropriate amount. 2.3.1 Two Link Manipulator An i d e a l two l i n k manipulator as shown i n Figure 2.1(b) was simulated to test control algorithms. It i s comprised of two massless l i n k s with point masses at the end of each l i n k . The manipulator hangs v e r t i c a l l y down i n the i n i t i a l p o s i t i o n . The actuators are i d e a l , f r i c t i o n l e s s , and are located at the beginning of each j o i n t . The forward kinematic s o l u t i o n can be solved simply by trigonometry. The equations are as follows: x = i1 s i n &1 + i2 s i n ^ + 6 2) [2.10] y = -t1 cos 6j - l2 cos ( 6 1 + 6 2 ) 18. [2.11] The inverse solution has two solutions corresponding to elbow-up and elbow-down p o s i t i o n s . The equations are: 9 2 = arc tan2 (±(((2£j* 2) 2 - ( x 2 + y 2 - i f - i f ) 2 ) 2 ) 1 ' 2 - , (x2 + y2 - t l - i f ) ) [2.12] Ij = arc tan 2 ( ( ( x 2 + y 2 ) + * 2 c o s 92^ + ^ £2 s i n 2 - ( i j + l2 cos 6 2 ) 3 / ( x £ 2 2(sin e 2) 2 + y(£j + £ 2 cos e 2)U 2 s i n 9 2>), ((x2 - ( t l + i2 cos 6 2) 2)/(y(£ 1 + Z2 cos 6 2) + x z2 s i n 6 2 ) ) [2.13] with + for elbow l e f t and - for elbow r i g h t . The subroutine used i n simulation studies i s given i n Appendix 2.1. It was tested by a computer program which incrementally went through a l l possible angles by 1 degree by f i r s t c a l c u l a t i n g endpoint p o s i t i o n by forward kinematics, then using t h i s p o s i t i o n as the input to the inverse kinematic routine, and compared i n i t i a l and computed angles f o r discrepancies. The routine was found to work c o r r e c t l y . The dynamic equations can be found by a p p l i c a t i o n of the Lagrangian technique. The sol u t i o n i s as follows: *. = D 1 1 S I + D 1 2 » 2 + D , » 2 » « + DuA 62 + D.2 . V . 19. T 2 = D 1 2 6 1 + D 2 2 6 2 + D 2 1 1 5 1 + D 2 + I a 2 6 2 l 2 ' 1 5 ! with the dynamic c o e f f i c i e n t s as follows: D l l = ^ m l + m 2 ^ £ l 2 + m 2 £ 2 + 2 m 2 * l * 2 c o s 6 2 [2.16] D 1 2 = m 2 £ 2 2 + m 2 4 l * 2 c o s 6 2 [2-17] D 2 2 = m 2 £ 2 2 D i 1 2 " 2miZil2 S i n 6 2 [2.18] D 1 2 2 = ~ m 2 £ l * 2 s i n 6 2 [2-19] D 2 1 1 = V l * 2 S l n 9 2 [ 2 ' 2 0 ] D1 = ( m i + m 2 ) i £j sinOj + m 2 g % 2 s i n ^ + 6 2) [2.21] D 2 = m 2 i fc2 sinCBj + 8 2 ) [2.22] 2.3.2 PUMA Manipulator This work was i n i t i a l l y a continuation of that started by Boucher [1985] and thus b u i l t upon the simulation model for the PUMA 560 which he developed. The kinematic s o l u t i o n t r a n s f e r matricies can be found i n Boucher [1985]. These are common for the PUMA class and only change due to l i n k length and o f f s e t d i f f e r e n c e s . The dynamic simulation by Boucher was i n e f f i c i e n t for intensive c o n t r o l study because of the high computational burden of c a l c u l a t i n g 20. the dynamic c o e f f i c i e n t s . The reason f o r t h i s i s that h i s method was developed using the symbolic manipulation package REDUCE available on the Amdahl 5850 mainframe computer at U.B.C. Although the s o l u t i o n i s correct, i t i s very cumbersome computationally as the dynamic equations are f o r a l l s i x l i n k s . Although h i s simulation had the l a s t three l i n k s f i x e d , the package used did not make use of any trigonometric i d e n t i t i e s nor c a n c e l l a t i o n s . The c o e f f i c i e n t s must be evaluated each numerical in t e g r a t i o n time i n t e r v a l , ( t y p i c a l l y every 0.005 seconds). The Fortran code f o r these c o e f f i c i e n t s occupied approximately 1800 l i n e s . For example a simulation of twenty seconds of robot movement would take approximately twenty minutes to run on a VAX 11/7 50. In an e f f o r t to improve the simulation, the equations for the dynamic c o e f f i c i e n t s were again solved f o r the PUMA 560 robot but with the l a s t three l i n k s f i x e d . The c a l c u l a t i o n was car r i e d out by hand and a l l s i m p l i f i c a t i o n s were used. The resultant formulae were checked against those generated by Boucher and found to be correct. The formulae are as follows: -97.472 cos8 2 - 44.211 sin(6 2+6 3) [2.24] -44.166 sin(6 2+6 3) [2.25] D 11 22.232 + 0.0225(sin6 2) 2 + 3.964(cos6 2)2 + 1 . 5 6 8(sin ( 6 2 + e 3 ) ) 2 + 0 . 0 2 6(cos(6 2+9 3)) 2 + 3.894 cos(6 2) sin ( e 2+6 3) [2.26] 21. D22 = 5 , 5 8 1 + 3 , 8 9 4 S l n 9 3 I2-27] D 3 3 = 1.595 [2.28] D 1 2 = 2.833 s i n 8 2 - 0.674 cos(6 2+0 3) [2.29] D 1 3 = -0.674 cos(8 2+9 3) [2.30] D 2 3 = 1.595 + 1.947 s i n 8 3 [2.31] D112 = ~ 3 ' 9 4 1 s i n 0 2 cos8 2 + 1.542 sin(8 2+8 3) cos(8 2+6 3) + 1.947(cos0 2 cos(8 2+6 3) - s i n 9 2 sin(9 2+9 3)) [2.32] D n 3 = 1.542 sin(8 2+9 3) cos(8 2+9 3) + 1.947 cos9 2 cos(8 2+0 3) [2.33] D 1 2 3 = 0.674 sin(8 2+6 3) [2.34] D ] 2 2 = 2.833 cos8 2 + 0.674 sin(8 2-l-9 3) [2.35] D211 = 3 , 9 4 1 s i n 9 2 c o s 9 2 " 1 , 5 4 2 s l n ( 9 2 + 9 3 ) cos(8 2+8 3) + 1.947(sin9 2 sln(9 2+8 3) - cos9 2 cos(9 2+9 3)) [2.36] D 2 2 3 = 1.947 cos8 3 [2.37] D 2 3 3 - 1.947 cos8 3 [2.38] 22. D 3 1 l = - 1 ' 5 4 2 sin(6 2+6 3) cos(6 2+e 3) - 1.947 cose 2 cos(9 2+0 3) [2.39] D 3 2 2 = -19.47 cos6 3 [2.40] 2.4 Advanced Continuous Simulation Language The simulation of a p h y s i c a l process i s a h e l p f u l t o o l i n the evaluation of hardware and software design. A language has been developed expressly f o r t h i s purpose. It i s c a l l e d Advanced Continuous Simulation Language and w i l l henceforth be referred to by the acronym ACSL. It i s a Fortran preprocessor, that i s i t takes the statements written i n the ACSL language, and converts them into s u i t a b l e Fortran code which then can be compiled and run on systems which have s u i t a b l e Fortran compilers. The advantage of using t h i s language over conven-t i o n a l numerical Integration i s extensive: the i n t e g r a t i o n routines are b u i l t into the preprocessor, the code required i s quite simple, user i n t e r a c t i o n and on-line c o n t r o l of the simulation i s possible, and many advanced operators are b u i l t into the language. For a complete d e s c r i p -t i o n of the language, see the ACSL User Guide/Reference Manual, t h i r d e d i t i o n , M i t c h e l l and Gauthier, Assoc., Inc., Concord, Mass., 1981. The simulation of a robot i s performed by numerically i n t e g r a t i n g the dynamic equations of motion. This i s done by assuming l i n e a r i t y over a small time i n t e r v a l , t y p i c a l l y 0.005 sec, with respect to the dynamic c o e f f i c i e n t s . After each time i n t e r v a l Integration the c o e f f i c i e n t s are re c a l c u l a t e d . This process i s repeated throughout the simulation, and the motion of the robot can be found by examining the time h i s t o r y of the j o i n t angles, and through forward kinematics, the time h i s t o r y of the endpiece. The ACSL program has four main sections: 1) I n i t i a l - parameter i n i t i a l i z a t i o n at the s t a r t of the simulation. 2) Dynamic - outside loop through which communication with the user occurs every CINT seconds. 3) Derivative - i n t e g r a t i o n loop where numerical i n t e g r a t i o n takes place. 4) Terminal - ending part of the program which occurs when the time variable i n the dynamic loop exceeds the maximum time l i m i t . The above i s shown as a flowchart i n Figure 2.3. A simple example of an ACSL program of the two l i n k manipulator i s given i n Appendix 1.1. In t h i s program the manipulator responds to a desired setpoint 6, f o r each j o i n t . In a more complete simulation des these setpoints would be functions of time and would be determined by a t r a j e c t o r y planner. INITIAL 2 FROM EXECUTIVE ON START INITIAL CODE BLOCK END ( INITIALIZE \ INTEGRATN \ «. ROUTINE / STATE HAS INITIAL COND-ITIONS TRANSFERRED AND DERIVATIVE ROUTINE IS USED TO EVALUATE DERIVATIVES ONCE READY FOR DATA LOGGING STOP- FALSE DYNAMIC END TERMINAL END DERIVATIVE / OVVRGCOME \ / DERIVATIVE \ f <4jf-> l 1 END ( WRITE LAST \ OUTPUT & \ PREPAR / o RETURN TO EXECUTIVE Figure 2.3. ACSL Flowchart 2 5 . CHAPTER 3 ROBOT CONTROL 3.1 Conventional Control The control method used i n robots currently i n use i n industry i s c l a s s i f i e d as conventional c o n t r o l . To reach a point i n the robot's workspace an inverse kinematic routine i s used to determine the required j o i n t angles. The j o i n t c o n t r o l l e r s then t r y to a t t a i n these desired j o i n t angles c a l l e d setpoints. The method used i s to provide a force or torque proportional to the e r r o r between the actu a l and desired j o i n t p o s i t i o n and a negative or opposite force or torque proportional to the v e l o c i t y to provide some damping such that the j o i n t behaves i n a c r i t i c a l l y or overdamped manner (assuming that overshoot i s undesirable). This j o i n t c o n t r o l l e r i s the most rudimentary form of c o n t r o l l e r and should not be confused with higher l e v e l s of c o n t r o l , such as the path planning or obstacle avoidance routines. An analogy i n the case of a human would be the neurological impulses a c t i v a t i n g a muscle to contract and the sensory feedback used to determine p o s i t i o n . For example, with eyes closed, grasp a p e n c i l i n each hand and try with arms outstretched to touch the ends of the pencils together. Each muscle must a t t a i n a c e r t a i n degree of f l e x i o n or extension for the pencils to touch. V i s i o n i s a higher l e v e l of control, allowing endpoint p o s i t i o n feedback. This i s an expensive option i n robotics that i s s t i l l the subject of much research and w i l l not be considered i n thi s t h e s i s . The theory behind conventional c o n t r o l i s w e l l o u t l i n e d i n Paul [1981]. Only the key points w i l l be covered here. Define the following: 2 6 . Table 3.1 C o n t r o l l e r of D e f i n i t i o n s 8. des. Desired j o i n t angle i 6 i Actual j o i n t angle K Actual j o i n t v e l o c i t y " e i J o i n t a c c e l e r a t i o n T i Torque generated by actuator Kmi Motor torque constant (Nm/volt) E f f e c t i v e Mass V i Voltage generated by c o n t r o l l e r P i P o s i t i o n Gain v i V e l o c i t y Gain The subscript i w i l l be dropped as the analysis i s i d e n t i c a l for each l i n k . The c o n t r o l l e r i s as follows Motor output torque i s T = K V [3.2] m Joint a c c e l e r a t i o n can be approximated by 27. [3.3] If one examines the dynamic equations, the predominant terms are and l a . These are combined i n t o one term which i s c a l l e d the e f f e c -t i v e mass because they represent the i n e r t i a component of the dynamics. Combining these into one equation we get 6 - =2 [K (6. - 6) - K 6] [3.4] Em L p des » J After rearranging we can get K E m e + 6 + e = e, [3.5] K K K des m p p This Is i n the form of a second order l i n e a r d i f f e r e n t i a l equation. Because of t h i s s i m i l a r i t y we can deduce by inspection a natural frequency and damping r a t i o for the j o i n t c o n t r o l l e r . Comparison gives - / TTY~ m p oi = / [3.6] n Em K /~r~ = _JL / _J£_ Em K P [3.7] This imposes r e s t r i c t i o n s on the gains and K^ . It i s strongly desirable to avoid e x c i t i n g any natural frequencies i n the robot, hence 28. avoiding resonance, so we choose an upper l i m i t on the p o s i t i o n gain such that the natural frequency of the c o n t r o l l e r i s l e s s than h a l f the natural frequency of the p a r t i c u l a r j o i n t . Thus K i s l i m i t e d as P follows a ) 2 Em n l i n k .„ „. K p < K 4 [ 3 ' 8 ] m Having the l i m i t on K and knowing that to avoid overshoot a damping P r a t i o of one or greater Is desirable, the upper l i m i t on can be found. /Em K K < 2/ „ P [3.9] K m The c o n t r o l l e r block diagram i s shown i n Figure 3.1. A problem e x i s t s i n that external forces applied to the robot w i l l generate set-point errors that w i l l not reduce to zero. For example assume an external torque equal to T g x t applied at j o i n t i . One can see that there w i l l be equilibrium of the c o n t r o l l e r when: T = K K (6, - 9) [3.10 ext m p des The most i n f l u e n t i a l external force i s g r a v i t y . This can be compensated f o r , f o r the manipulator I t s e l f , by the c a l c u l a t i o n of the external gravity torque on-line and feeding t h i s forward to the j o i n t c o n t r o l l e r s as a d d i t i o n a l voltage. EXTERNAL TORQUES Figure 3.1. Joint Control ler Block Diagram 30. add K [3.11] m V , = V + V . . t o t a l add [3.12] This i s shown i n Figure 3.2. This i s c a l l e d g r a v i t y compensation and i s commonly used i n robots. Looking at the form of the dynamic equations again one can see there are other torques which we could compensate f o r . These are due to c e n t r i p e t a l , c o r i o l i s , and torque reaction e f f e c t s . These are more d i f f i c u l t to c a l c u l a t e and generally only come i n t o e f f e c t during higher speed motion, thus are not important for endpiece control but for t r a j e c t o r y following. I t i s important to note that the robot i s only capable of compen-sating for i t s own dynamics and not that of payload nor external forces and torques, as the design of the c o n t r o l l e r i s f i x e d and there i s no load estimation methods currently i n use. It i s for t h i s reason that robots are generally b u i l t quite robustly and have small payloads or l i m i t e d performance. For example the PUMA 560 i n the Department of Mechanical Engineering at U.B.C. weighs 120 pounds but has an i n e r t i a l payload of 5.7 in-oz-sec i n order to maintain i t s s p e c i f i e d accuracy. Needless to say t h i s types of over-building leads to higher cost i n order to achieve desired performance. It i s for t h i s reason that there has been i n t e n s i v e research i n t o the area of r o b o t i c controls, such that robots can reach t h e i r true mechanical c a p a b i l i t i e s . Figure 3.2. Joint Control ler Block Diagram Including Gravity Compensation 32. 3.2 Model Reference Adaptive Control In Jo i n t Coordinates To improve upon the c o n t r o l strategy determined by conventional methods, and hence improve the c a p a b i l i t i e s of the robot, one must look towards new adaptive c o n t r o l s t r a t e g i e s . One such strategy i s model reference adaptive c o n t r o l . The p r i n c i p l e behind t h i s i s to e s t a b l i s h a mathematical model of the true system that w i l l perform i n a desired manner when subjected to the same Input as the true system, and then adjust the parameters of the true system such that i t follows the response of the reference model. The model and the system, i n t h i s case the robot j o i n t c o n t r o l l e r , are driven by the same input, which i s the desired j o i n t angle. The output of the model and the system i s used to form an e r r o r which i s used i n an adaption scheme to update the gains of the true system such that t h i s e r r o r i s reduced towards zero. This process i s shown i n Figure 3.3. This form of adaption i s used for a l l j o i n t c o n t o l l e r s . To examine the process f u l l y , f i r s t re-examine the form of the j o i n t c o n t r o l l e r equation [3.5]. As can be seen, i t i s of the form of a second order l i n e a r equation. This i s not correct, as dynamic e f f e c t s and disturbances have been neglected. To examine the f u l l model one must examine the true dynamic model n n n T i J - l I D i j j 8. + I j=l k=l Z D .. 8, 6, + l a . 8. + D. i j k j k i i l [3.13] REFERENCE MODEL ADAPTIVE GAIN ADJUSTMENTS ERROR MANIPULATOR DYNAMICS Figure 3.3 Model Reference Adaptive Control Block Diagram 34. We can lump a l l the dynamic e f f e c t s and disturbance e f f e c t s i n t o a single time-varying term T . This i s written as follows: Q I sr n n n T.. f = E D 6 + £ I D, ., 6. 6, + D, [3.14] d i s t i j j k = 1 i ] k j k i 1 J T i " T d i s t + <°ii + I a i > 6 i t 3 ' 1 5 ^ As was noted e a r l i e r , the most s i g n i f i c a n t term i n T,. i s the d i s t gravity term. A l l other dynamic terms w i l l go to zero as the v e l o c i t y goes to zero. For now we ignore T i n the development of the model fl X s t reference adaptive c o n t r o l strategy but w i l l come back to i t l a t e r . Looking at the form of Equation [3 .5 ] , we can see the predominant dynamic c h a r a c t e r i s t i c s . This leads us to assume a form of a reference model which has s i m i l a r dynamics. We can write t h i s as follows: — y e + — y , + y - = e. [3.16] o ref oi ref ^ r e f des 1 ui * n n Where the natural frequency i s chosen to be the same as that of the p a r t i c u l a r j o i n t c ontroler and the damping r a t i o i s chosen to be 1.0 f o r c r i t i c a l damping. C r i t i c a l damping i s chosen such that there i s no overshoot. This could be a l t e r e d to 0.707 to get the f a s t e s t response, but as tr a j e c t o r y following i s the purpose of t h i s research, c r i t i c a l damping was chosen. We rewrite the equation f o r the j o i n t c o n t r o l l e r as follows 3 5 . ae + ee + e - e. [3.i7i des 1 1 We do the same for the reference model 3 * r e f + b ?ref + ?ref = 6des I3'18] C l e a r l y i f T., was equal to zero, the two would perform i n d i s t r exactly the same manner. It must now be shown that these disturbances can be viewed as changes i n the gains of the system. If we rewrite Equation [3.5] but include T,, we get: M d i s t E m i + £ 8 + e = « [ 3. 1 9 ] K K K K K des p m m p p Upon rearrangement Define ( ^ + 7 T T ) E ' + ^ S + 8 = 9 « - 1 3 - 2 0 ] p m K K o p m p r Em 6 K AK = E [3.21] P Em 6 + T d i s t Using t h i s we can now write [3.5] as K 6 + 6 + 6 = 6, [3.22] K AK K des m p p 36. Now we have the same form with a d i f f e r e n t p o s i t i o n gain. To compensate for t h i s we should change the gain to AK^. This would also lead to a change i n the v e l o c i t y gain i n order to maintain the desired damping r a t i o . However t h i s i s computationally as d i f f i c u l t as pre c a l c u l a t i n g dynamic and external e f f e c t s for feedforward compensation i n conventional c o n t r o l . This d e r i v a t i o n has shown that disturbances can be modelled as changes i n gains. In order to have the system perform i n a s i m i l a r manner as the reference model we define a cost function and use an optimization technique to minimize t h i s cost function. Define as follows: e = y - 6 [3.23] •'ref 1 1 k = y . - 6 [3.24] ^ r e f e = y . - 6 [3.25] •'ref 1 ' These terms describe the error i n performance between the system and reference model. We now define two cost functions: f i ( e ) = I ( q o e + v + v ) 2 [3-26] f 2 ( E ) = I ( q 0 e 2 + q i e 2 + q ^ ) [3.27] where q 0» q x , q 2 are weighting c o e f f i c i e n t s . 3 7 . The reason f o r d e f i n i n g both i s that the f i r s t has been used by Dubowsky and DesForges [1979] In t h e i r research and has advantage i n proving s t a b i l i t y but would have l i n e s of zero cost when there a c t u a l l y was discrepancies between the system and the reference model, i . e . : q 0 f j ( e ) = 0 when q 2e = 0 and e = - — e The second i s a purely quadratic cost function and i s the l o g i c a l choice of the two as i t y i e l d s a smooth cost surface that i s zero only at the o r i g i n . Both are examined i n t h i s s e c t i o n but the second [3.27] i s used i n subsequent sections of t h i s t h e s i s . Before the gain update process i s outlined the following assumptions are made: 1) The motor torque constant K and the e f f e c t i v e mass Em of the m system vary slowly compared to the basic time constants of the physical process. 2) These parameters also vary slowly when compared with the rates of the adjusting mechanism which adjusts the p o s i t i o n and v e l o c i t y gains. 3) The adjusting mechanism i s such that i t adjusts the p o s i t i o n gain at a rapid rate when compared with the rate of change of the cost function. A l l of the above assumptions allow the l i n e a r i z a t i o n of the process with respect to time. Define 6, = a - a [3.28] 6 2 = 6 - b 3 8 . [3.29] If the two ^2»^ 2 a r e z e r o then the cost function i s also at, a minimum. We chose the method of steepest descent to make a change A6 1 }A6 2 so as to e f f e c t a maximum decrease i n the cost function f ^ e ) . To do t h i s we move down the gradient. We define the gradient as follows: 3 ^ ( 0 SfjCe) V f l ( e ) " - 3 T T - + - 3 6 7 - t3'3°] Thus we choose to move towards zero cost by making a step as follows: 3f (e) A 6 i " -vl67- t 3 ' 3 1 ! 3 ^ ( 0 A 6 2 - -V-367- [ 3 - 3 2 ] Where v i s an a r b i t r a r y p o s i t i v e constant (formally known as Lagrange m u l t i p l i e r ) of value between zero and u n i t y . This s p e c i f i e s the size of the step with unity being a step equal to the gradient. When the cost surface i s not smooth v i s chosen to be l e s s than u n i t y , ( t y p i c a l l y 0.1-0.25). As the cost surface i n t h i s case i s quite well defined and smooth, v i s chosen to be uni t y and w i l l be ignored f o r the r e s t of t h i s a n a l y s i s . It w i l l s t i l l be used i n the simulation program to examine the c o n t r o l algorithm with smaller step s i z e but w i l l not be noted further. 39. Now that the changes i n A6j and A6 2 are s p e c i f i e d , the corresponding changes i n a and 6 must be found A6j = A(a - a) [3.33] A6j = Aa [3.34] A6 2 = A(B - b) [3.35] A6 2 = Ag [3.36] S i m i l a r l y Thus 3f,(e) Aa = - - g ; — [3.37] SfjCe) A8 = - - g g [3.38] We wish to change the value of the gains but do not have exact knowledge of a or g. To get around t h i s we assume that a and g are fixed and a and b are allowed to vary. Then A6 = - Aa [3.39] A6 2 = - Ab [3.40] So the step that must be taken i s [3.41] 40. 3 ^ ( 0 Ab = +-ab" [3.42] It i s assumed that Aa, Ab are small when compared with a,b. We see from the above that Aa « -Aa [3.43] Ag » -Ab [3.44] If we make these changes over time i n t e r v a l At and l e t At approach zero, we can get the instantaneous rate of change of a,g. 3 f l < E > a = — - [3.45] afjCe) § - - g ^ - [3.46] To f i n d the rate of change we must f i n d the d i f f e r e n t i a l s . We examine again the cost function: - "J ( q 0 e + + q 2 e ) 2 [3.47] We now f i n d the corresponding d i f f e r e n t i a l s : 8 f i < e > •• a* a : a* 1 , • . 9e . 3e . 3e. 3a as e = y , - 8 r e f = (q 0e + q ^ + q 2 O(q 0 q x + q 2 ^ [3.48] [3.49]; _ _ [3.50] 3a 3a ' 3 a 3a ' 3a 3a [3.51] 3e _ 3 y r e f . 3e _ 3 y r e f . 3e _ 3 y r e f [3.50]; Taking d e r i v a t i v e s of the reference model equation 41. 7 r e f , . ' r e f , J r e f or l e t then thus S i m i l a r l y f o r y r e f + a - l a - + b - l T - + - T a - = ° t3.52] 3 y r e f 9 y r e f 9 y r e f 3 y f u = - g p [3.54] 9 y r e f " 9 y r e f [ 3 > 5 5 ] ; au + bu + u = - y r e f [3.57] 3b l e t 3 y. Tb co = *6 f [3.58] we can get aio + b) [3.61] As and Em are slowly changed we can approximate 5 = l ^ f - l [ 3 . 6 2 ] m p Thus K p P K = - - K [3.64] p a p But the value of a i s not e x p l i c i t l y known and i s d i f f i c u l t to ca l c u l a t e . To get around t h i s we go back to the assumption that a i s close to a. Using t h i s we get: K = - — K [3.65] p a p To f i n d 3 we take the d e r i v a t i v e of 3. K K • V V 3 = ^ - K [3.66] and get K p K 2 P P K - 3 K - - K [3.67] v p a v The rates of change of both the p o s i t i o n and v e l o c i t y gains are i n t e -grated with time to f i n d new values f o r these v a r i a b l e s . 4 3 . As r e s u l t s have been published, Dubowsky and Des Forges [1979], only one simulation r e s u l t , a step input from the PUMA 560 computer simluation i s presented. Figure 3.4 shows the d i f f e r e n c e between conventional c o n t r o l and the model reference control scheme presented and Figure 3.5 shows the gain adaption with time. Note that when the p o s i t i o n and v e l o c i t y difference between the reference model and the j o i n t c o n t r o l l e r become small - near the desired setpoint - the adaption process slows and the gains become constant. This poses a problem i n that the gains may s e t t l e at values which would give a steady state natural frequency that could be close to the natural frequency of the j o i n t . This has not been commented on i n the l i t e r a t u r e but a reasonable method to avoid t h i s would be to superimpose an exponential decay to the conventional j o i n t c o n t r o l l e r gain values with a time constant equal to that of the j o i n t c o n t r o l l e r model. This would allow gain adaption during the i n i t i a l high a c c e l e r a t i o n phase of movement but reduce the gains to reasonable values with appropriate natural frequency when steady state i s reached. This was not examined i n simulation studies of t h i s model reference control algorithm. The second cost function Equation [3.27] would s l i g h t l y modify Equation [3.63] to the following: a = (q 0eu + q ^ u + q 2eu) [3.68] And Equation [3.66] would change to: (5 = (q0ca) + q^eiL + q 2eto) [3.69] The rate of change for the gains would remain the same. Response of Manipulator to Step Change of Theta 2 From 90 to 45 Degrees Figure 3.4. Response of PUMA 560 to step change in theta 2 from 90 to 45 degrees using conventional control and model reference adaptive control in j o i n t coordinates. Change of Position and Velocity Gain Using Model Reference Adaptive Control Step Change Theta 2 from 90 to 45 Degrees A Poatlon Gain X Velocity G«ln 2 6 3 Time (seconds) 36 « 5 6.6 Figure 3.5. Change in Position and Velocity Gains for PUMA 560 responding to step change in theta 2 from 90 to 45 degrees. 46. 3.3 Model Reference Adaptive Control i n J o i n t Coordinates With Observer The model reference adaptive control algorithm described i n the l a s t s e c t i o n can be modified by making use of the information a v a i l a b l e . An observer i s proposed which i s used to estimate the e f f e c t i v e mass from a c a l c u l a t e d a c c e l e r a t i o n which i n turn modifies the update algor-ithm by using an estimated value for a i n the gain update algorithm. The l o g i c behind t h i s i n v e s t i g a t i o n i s that i t may be possible to improve the performance i n terms of more rapid gain adaptation i f a more reasonable value of the e f f e c t i v e mass i s used. The robot j o i n t c o n t r o l l e r has access to j o i n t p o s i t i o n and v e l o -c i t y measurements. By using t h i s information along with the sampling i n t e r v a l , an estimate of the acceleration can be made. This can be used and an estimated value of a can be solved f o r from Equation [3 .5] and be used i n Equation [3.64]. The relevant parameters i n t h i s study are the choice of the sample i n t e r v a l and the accuracy of the measurements. The sample i n t e r v a l was chosen such that i t s frequency would be greater than twice that of the n a t u r a l frequency of the manipulator l i n k . This i s acceptable for large l i n k s such as the f i r s t three l i n k s of a revolute robot l i k e the PUMA clas s where the natural frequencies are low but might prove to be too fast for l i n k s with higher frequencies due to computational l i m i t a t i o n s . This i s compensated f o r by the much greater changes i n e f f e c t i v e mass of the f i r s t three l i n k s as compared with the l a s t three. To include the e r r o r s i n r e a l measurement devices, the standard method used i n computer simulation i s to introduce band l i m i t e d white noise to the s p e c i f i c measurement reading. For example the measurement of the current angle would be the correct angle plus some noise. The mean of the measured angle would be the correct angle and 47. the standard d e v i a t i o n used f o r angle measurements was 0.001 radians (0.057 degrees) and for voltage measurements 0.005 v o l t s . These were chosen such that the measurement would be quite accurate. The PUMA 560 has an accuracy of 5.7557E-03 degrees for the waist, 4.17407E-03 degrees f o r the shoulder and 6.7098E-03 degrees f o r the elbow j o i n t s , so the chosen simulation standard deviation i s higher than necessary. If the observer routine proved to be an improvement then f u r t h e r consideration of these parameters would be warranted. Simulation study r e s u l t s are given i n Section 5.2. 3.4 Model Reference Adaptive Control i n Cartesian Coordinates An adaptive model reference c o n t r o l strategy i s proposed that would improve the speed of a manipulator and at the same time improve the t r a j e c t o r y or path following of the endpiece. The c o n t r o l l e r would allow the use of fewer setpoints along the t r a j e c t o r y and therefore would have higher j o i n t servo setpoint e r r o r s and thus higher actuator torque r e s u l t i n g i n higher speed. Due to the algorithm, i t would also attempt to follow a desired Cartesian path. The method proposed uses the p r i n c i p l e s of model reference adaptive c o n t r o l but defines a model i n Cartesian rather than j o i n t coordinates. This allows the s p e c i f i c a t i o n of the t r a j e c t o r y i n the reference model and thus a cost function can be formed based on e r r o r s between the actual and desired t r a j e c t o r y while the c o n t r o l l e r i s s t i l l using the normal t r a j e c t o r y setpoints. This i s shown i n Figure 3.6. The reference model used i n simulation studies using the two l i n k manipulator i s as follows: x ( t ) : 1 x + — x n + x = [3.70] x des 2 n FORWARD KINEMATICS REFERENCE MODEL INVERSE KINEMATICS DES ADAPTIVE GAIN ADJUSTMENTS ERROR CONTROLLER MANIPULATOR DYNAMICS > Figure 3.6 Model Reference Adaptive Control in Cartesian Coordinates Block Diagram 49. y ( t ) : — y + (0 2 n 25 • y + y = [3.71] 0) y. des n This form of equation i s used, as i t has a response that when converted i n t o appropriate j o i n t values i s s i m i l a r to that of the conventional c o n t r o l l e r model. They are both s i m i l a r to a second order l i n e a r d i f f e r e n t i a l equation. Figures 3.7 and 3.8 show the Cartesian and j o i n t t r a j e c t o r i e s . In theory any form of reference model that would have t h i s c h a r a c t e r i s t i c could be used but only the form of Equations [3.70] and [3.71] were examined. To examine the theory, few assumptions must be made: 1) The tr a j e c t o r y must be att a i n a b l e . The robot must be able to a t t a i n t h i s path by working within the actuator output l i m i t s . 2) The desired path should be close to that which could be attained by a robot working under conventional control between setpoints. For example a s t r a i g h t l i n e i n world coordinates would be acceptable while a large deviation from t h i s would not. 3) The setpoints chosen along the path must lead to a reference model which when converted to j o i n t coordinates can be approximately represented by a second order l i n e a r d i f f e r e n t i a l equation. This means that the form of the reference model when converted to j o i n t coordinates i s s i m i l a r to that of the j o i n t c o n t r o l l e r . The simulation was r e s t r i c t e d to s i n g l e endpoint change but i t must be r e a l i z e d t h i s could be extended to a tr a j e c t o r y by combining several endpoint changes i n succession. In the simulation studies used i n t h i s section only a two-link manipulator i s used and thus i n the Reference Model X Coordinate Trajectory 0.30 -i o 0 0.5 1 1.6 2 Time a) X Trajec tory Figure 3 .7 . Reference Model Cartesian Coordinate Reference Model Y Coordinate Trajectory -1.24-1 Time b) Y Tra jec tory T r a j e c t o r i e s . Reference Model Theta 1 Trajectory Reference Model Theta 2 Trajectory 52. discussion below, the reference model and the manipulator r e f e r to only two Cartesian coordinates and two j o i n t l i n k s . The reference model f or an endpoint p o s i t i o n change from ( x n , y n ) to (x, ,y, ) with c r i t i c a l u u des des damping, would be: -0) t x(t) = ( x n - x, )(1 + oo t)e n + x, [3.72] ' v 0 des v n des 1 1 = < y 0 - y d e s ) ( 1 + V ) e V + ydes i 3 - 7 3 i The choice of the damping r a t i o and natural frequency are made such that the response does not v i o l a t e the assumptions. In t h i s case the following values were chosen for both x and y: a) = 7.0 ; £ = 1.0 n The choice of these depends upon the traj e c t o r y but a safe rule of thumb would be to choose c r i t i c a l damping and set the natural frequency close to that of the l i n k with the lowest natural frequency. This would avoid v i o l a t i n g assumption 1 and 2 above. T r a j e c t o r i e s that involve movement primarily of the higher natural frequency l i n k s could use higher n a t u r a l frequency f o r the reference model, however, t h i s was not examined. The reference model gives a p o s i t i o n along a tr a j e c t o r y i n world coordinates as a function of time. At regular gain update i n t e r v a l s the corresponding j o i n t values required to a t t a i n the reference model p o s i t i o n are calculated by the inverse kinematic routine. This i s 5 3 . acceptable because these j o i n t values can be precalculated instead of being calculated on-line, avoiding computational burdens. A cost function comparing the true j o i n t values and the desired reference model j o i n t values such as Equation [3.27] i s formed. This i s minimized by adjusting the gains. Assumption 3 above allows the w r i t i n g of the reference model for each j o i n t i n j o i n t coordinates as follows: — y — y *+y * - e. [3.74] o -'ref u) ref •'ref des 1 1 oo *• n n This i s of s i m i l a r form as the j o i n t c o n t r o l l e r model and thus the following form can be assumed: a y - + b y . + y . - 6, [3.75] •'ref ref ' r e f des 1 Using the following cost function f 2 C O - j ( q 0 e 2 + q ^ 2 + q 2 e 2 ) [3-76] with weighting c o e f f i c i e n t s The following rates of change for the gains can be found: q Qeu + q xeu + q 2eu [3.77] 54. B = qQea) + q^u) + q 2eaj [3.78] K = P 2-K a p [3.79] K = v B K — K p a v [3.80] The value f o r a i s found from the observer routine. Results from simulation studies of t h i s c o n t r o l scheme are presented i n Section 5.3. 55. CHAPTER 4 STABILITY ANALYSIS 4.1 Introduction S t a b i l i t y of a r o b o t i c c o n t r o l scheme i s more d i f f i c u l t and com-pl i c a t e d than other control schemes, as a robotic system i s highly nonlinear and time varying. The dynamic parameters change with p o s i t i o n and v e l o c i t y as well as with external load conditions and there are unmodelled parameters which are not taken i n t o account i n the general a n a l y s i s . The system i s also of high order - usually of degree s i x for a manipulator such as that of the PUMA type. To ensure s t a b i l i t y i s to design the control parameters such that the robot w i l l perform within a desired set of c r t i e r i a , f o r example choosing the gains of a conven-t i o n a l c o n t r o l l e r such that the c o n t r o l l e r natural frequency i s s p e c i f i e d and c r i t i c a l damping i s ensured. The current case of adaptive control i s much more d i f f i c u l t , as conventional s t a b i l i t y analysis methods do not apply. The algorithm used i n t h i s paper i s of a simple form, each j o i n t having i t s own reference model. The model i s i n Cartesian coordinates and behaves i n a desired manner. The adaptive c o n t r o l scheme used adjusts the gains of the conventional j o i n t c o n t r o l l e r s such that the j o i n t s move i n a manner close to that of the reference model - the model having been converted from world or Cartesian coordinates to j o i n t coordinates v i a an inverse kinematic routine. Although the adaptive control scheme i s applied to each j o i n t independently, the i n t e r a c t i o n of the j o i n t s s t i l l plays an important part i n the true movement of the j o i n t s and cannot be neglected. It i s treated as a disturbance i n the 56. scheme presented and includes the e f f e c t s of c o r i o l i s , c e n t r i p e t a l , and j o i n t r eaction torques and could include external and payload torques. It has been shown that these can be viewed as a change i n the propor-t i o n a l gain term and are thus included i n the algorithm. Gravity compensation i s not included, as i t i s estimated d i r e c t l y and fed forward to the j o i n t c o n t r o l l e r s . 4.2 State Space Equations The conventional method used to prove s t a b i l i t y i s to show that the relevant parameters remain stable or go towards zero as time progresses. In t h i s case i t i s prudent to examine the behaviour of the e r r o r between the actual manipulator and the reference model, and also a parameter i n the gain update routine. Thus f o r relevant parameters choose the following: z x - y - 9 [4.1 z 2 = y - 9 [4.2] z 3 = a - a [4.3] The l a s t term represents the error i n the i d e a l gains and the actual gains. Using these parameters and incorporating the gain update algorithm used, the following state space matrix equation can be formed: Define: TQ = y - | y [4.4] Then 57. 2 z 2 = - i z, - — z, - i TQ z, - — (z TQ + z + b(y-B)) [4.6] i- a * a ' a 6 an. K * 1 ' ; •• •< z 3 = ~ ( q O u " ~ u ) z l " ( q l u " 7 ^ 2 u ) z 2 + q 2 u a 2 " Z 3 + q 2 ^ T S " ^ Z l + K y " Z 2 > + T 0 > Z 3 ) [ 4 * 7 J This i s s i m i l a r to that presented by Dubowsky and DesForges In t h e i r paper with minor changes. These can be rewritten i n t o the following form: z = A(t) z + F(z,t) [4.8] It i s comprised of a l i n e a r time varying term and a nonlinear time varying term. To c a l c u l a t e s t a b i l i t y i n the large - meaning i n a general region instead of i n the v i c i n i t y of c e r t a i n points - one could use the second method of Lyapunov which requires f i n d i n g a time varying d e f i n i t e function V (which can be e i t h e r p o s i t i v e or negative d e f i n i t e ) i n a region D such that the time d e r i v a t i v e , comprised of the p a r t i a l d e r i v a t i v e of the function V and the Eulerian d e r i v a t i v e (ignoring the time part of the function V), i s of the opposite s i g n f o r asymptotic s t a b i l i t y , or of opposite sign and possibly zero for general s t a b i l i t y . The major problem with t h i s method i s that there i s no technique f o r f i n d i n g t h i s function V. T y p i c a l methods involve writing an equation which represents the energy of the system or i s of quadratic form, (to ensure definiteness of the function) but these do not always work. In t h i s p a r t i c u l a r case, f i n d i n g the function V i s extremely d i f f i c u l t because of the nature of the terms i n the state space equation and i s not considered. 58. Another possible method i s to assume a reference model t r a j e c t o r y ( i n j o i n t coordinates) that w i l l reduce the equations to a form which i s time i n v a r i a n t and even possibly l i n e a r and prove s t a b i l i t y f o r these cases. This i s what Dubowsky and DesForges did i n t h e i r paper. The problem with t h i s i s that i t proves s t a b i l i t y f o r those p a r t i c u l a r t r a j e c t o r i e s chosen but not n e c e s s a r i l y for a l l t r a j e c t o r i e s . For example, using a ramp input and the same cost function as used by Dubowsky and DesForges Equation [3.26] w i l l lead to a time i n v a r i a n t form which can be analyzed by f i n d i n g the eigenvalues of the l i n e a r matrix (the nonlinear matrix i s neglected as only small perturbations about the zero vector are considered). This w i l l allow one to f i n d bounds on the weighting c o e f f i c i e n t s i n the cost function. Another example i s to assume a p e r i o d i c input (thus s t i l l time varying) and using Floquet theory which enables one to again e f f e c t i v e l y look at the eigenvalues of a new set of equations. These are good as s t a r t i n g points for f i n d i n g the behaviour of the system, but due to the basic assumption that the model w i l l perform i n the manner of a second order l i n e a r d i f f e r e n t i a l system and since there i s no e x p l i c i t way of chang-ing the state space representation to e i t h e r a l i n e a r time i n v a r i a n t or a periodic one, j u s t i f i c a t i o n of t h i s premise i s not proved. It should be noted again that i n the published papers i n the area of adaptive robotic controls which do have a form of s t a b i l i t y a n a l y s i s , the models used are of r i g i d form, (meaning e l a s t i c i t y i s ignored), and the motor models are assumed to have a l i n e a r motor-torque constant with possibly some purely viscous f r i c t i o n . In r e a l i t y we have to deal with non-rigid l i n k s (although conventional practice i s to over-build l i n k s to make them very r i g i d ) and motors which change t h e i r torque-amperage 5 9 . c h a r a c t e r i s t i c s with operating temperature as w e l l having s t a t i c and dynamic f r i c t i o n . These are only a few of the discrepancies which are neglected i n a n a l y s i s . No mention i s given to how much any of the assumed parameters can change and s t i l l maintain s t a b i l i t y of the c o n t r o l l e r . A perturbation technique could be used i n extreme case examples, where s t a b i l i t y i s marginal, and would provide i n s i g h t into the s e n s i t i v i t y of the system to the assumptions made. S t a b i l i t y analysis simply proves that a c e r t a i n parameter does not become unbounded. In our case the parameter i s the erro r between the manipulator and the reference model. The method used to make the manipulator behave l i k e the model i s to use an optimization technique to minimize a cost function. In t h i s case the cost function i s of quadratic form and involves the erro r between the manipulator and the reference model F(e) = j ( q 0 ( y - 6 ) 2 + q^ HO2) [4 .9] where qgjq^ are weighting c o e f f i c i e n t s of p o s i t i o n and v e l o c i t y errors and the a c c e l e r a t i o n weighting c o e f f i c i e n t i s set at zero. The gain update algorithm used minimizes t h i s cost function by moving down the cost surface i n the d i r e c t i o n of steepest descent (down the gradient). If on one hand we wish to prove s t a b i l i t y by making sure the error between the manipulator and model does not become unbounded, and on the other hand, are using an optimization technique to update the gains such that a cost function i s minimized and the cost function i s a p o s i t i v e d e f i n i t e function of the errors that we wish to ensure are stable, we are i n e f f e c t ensuring the system i s stable. One can look at the cost 60. function as Lyapunov function. The gain update technique ensures the movement i s always i n a negative gradient, e f f e c t i v e l y always moving towards a lower cost. So without a c t u a l l y f i n d i n g a Lyapunov function which encompasses the domain we are operating i n , what we i n t u i t i v e l y can see the system should be stable, providing the following: a) The cost function i s p o s i t i v e d e f i n i t e with a smooth surface. b) The update algorithm does move down the cost surface. c) The assumptions i n the gain update algorithm are reasonable. The t r a j e c t o r i e s studies i n t h i s t hesis are d i s c r e t e point to point movements. Provided the gains stay p o s i t i v e and there are no a d d i t i o n a l forces or torques which are not compensated f o r , the e r r o r between the manipulator and reference model w i l l always go to zero. This i s due to the form of the j o i n t c o n t r o l l e r s , i . e . , the e r r o r w i l l always be driven towards zero. So one cannot look at the response of the cost function over various t r a j e c t o r i e s and make any v a l i d claims about s t a b i l i t y . To examine t h i s then one must look at a t r a j e c t o r y which the error w i l l not go towards zero or w i l l o s c i l l a t e i n an unstable fashion. We now have j u s t i f i c a t i o n to look at the reference model t r a j e c t o r i e s which would do t h i s . 4.3 Ramp Input As mentioned e a r l i e r using a ramp input and cost function Equation [3.26] w i l l allow us to change the state space model to a time i n v a r i a n t model upon which we can apply conventional eigenvalue c r i t e r i a , provid-ing that we only examine small perturbations about the zero vector (such that we can ignore the nonlinear term). This a n a l y s i s i s useful as the 61. form of the cost function Equation [3.26] i s s i m i l a r to Equation [3.27] used i n simulation studies i n regions of i n t e r e s t , i . e . during simula-t i o n s there i s usually both p o s i t i o n and v e l o c i t y e r r o r between the manipulator and the reference model. Let us examine t h i s f i r s t and make observations about how s t a b i l i t y i s a f f e c t e d by the cost f u n c t i o n weighting c o e f f i c i e n t s . Using a ramp input and cost function [3.26] the l i n e a r matrix w i l l now take the following time inv a r i a n t form: A = 0 -1/a , 2 p£ 1 -b/a q l q 0 Rf 0 -(l/a)*R 0 [4.10] where R = slope of the ramp. It has been assumed that the r e l a t i o n s h i p between alpha and beta can be represented by a constant which i s approximately equivalent to b/a, i . e . : a v. [4.11] a b a This i s v a l i d i n t h i s case even though both a and b are not true constants. S t a b i l i t y of the system can be found from examining the eigenvalues of the A matrix. Figures 4.1 (a),(b) and (c) indicate how s t a b i l i t y i s a f f e c t e d f o r d i f f e r e n t slopes of the ramp input and also f o r d i f f e r e n t natural frequencies of the l i n k s . As the slope of the ramp i s increased the region of s t a b i l i t y becomes smaller. The same e f f e c t i s observed for an increase i n natural frequency. Only the f i r s t quadrant of the qg.qj plane i s shown as t h i s i s the only area of i n t e r e s t , ( i . e . the Stability of System Using Ramp Input Slope 0.1 Radians per Second Link 1 (Wn - 31 rad/sec) Stability of System Using Ramp Input Slope 0.2 radians per second Link 1 (Wn = 31 rad/sec) Stabto 4 e qO Stability of System Using Ramp Input Slope 0.2 radians per second Link 2 (Wn = 62 rad/sec) b) R=0.2,Wn=31 c) R=0.2,Wn=62 a) R=0.1,Wn=31 Figure 4.1. S t ab i l i t y Analysis for Ramp Input Showing Regions of S t a b i l i t y for Different Link Frequencies and Ramp Slopes. 6 3 . c o e f f i c i e n t s of the cost function are chosen such that the function i s p o s i t i v e d e f i n i t e ) . We observed from the graphs that s t a b i l i t y r e s t r i c -t i o n s e f f e c t the amount of weight that can be placed on the p o s i t i o n error without r e a l l y l i m i t i n g the v e l o c i t y error weighting. This would prove b e n e f i c i a l f o r following a ramp input where i t i s d e s i r a b l e to match the v e l o c i t y of the reference model and the manipulator. If the nature of the c o n t r o l l e r i s to follow setpoints along a path, as i s done currently i n most robotic c o n t r o l l e r s , and there i s a reference model which describes a d e s i r a b l e path, more emphasis must be placed on the p o s i t i o n e r r o r . As the cost function used i n the algorithm presented i s s i m i l a r i n nature to Equation [3.26] (but does not lead to a time invariant l i n e a r form when a ramp input i s used), for simulation studies examined the weighting of the p o s i t i o n e r r o r i s greater than that of the v e l o c i t y error and i n the general stable range for t y p i c a l j o i n t move-ment v e l o c i t i e s . It must be noted that i n the cost function used here, the cost c o e f f i c i e n t s correspond to the square of those of Equation [3.27], used i n the simulation studies presented, due to the form of the equations. 4.4 Floquet Analysis The other method of analyzing the s t a b i l i t y of the system i s to use Floquet a n a l y s i s . See Minorsky [1962] for a complete d e s c r i p t i o n of the background theory. In t h i s method i t i s assumed that the reference model ( i n j o i n t coordinates) moves i n a periodic nature. For example: 6 ( t ) = d sin(tot) [4.12] 6 4 . If we use t h i s we can rewrite the l i n e a r state-space matrix A such that a f t e r a period T, A(t) = A(t+T). Provided this condition exists we can apply Floquet a n a l y s i s . We assume that the system has a s o l u t i o n of the form: I.e., the states z can be related to the states a f t e r one time period by a matrix which i s a time dependent fundamental set of solu t i o n s . For periodic systems the matrix f(t+T) i s also a fundamental set of solutions and i s re l a t e d to the ^ ( t ) by a constant matrix C c a l l e d the monodromy matrix. The condition required for s t a b i l i t y i s that the eigenvalues of the monodromy matrix l i e within the un i t c i r c l e on the complex plane. These eigenvalues are c a l l e d the c h a r a c t e r i s t i c m u l t i p l i e r s of the system. Solving f o r C e x p l i c i t l y can be quite d i f f i c u l t so a numerical method i s used. F i r s t assume that t = 0. Thus: Also assume that *(()) i s the i d e n t i t y matrix. To f i n d y(T), which w i l l also be equal to the matrix C, one integrates the system of equations over time T with i n i t i a l conditions corresponding to columns of the i d e n t i t y matrix to f i n d each column of ¥(T). z(t+T) = Y(t) z ( t ) [4.13] ¥(t+T) = * ( t ) * C [4.14] z(T) = f(T) * z(0) [4.15] 6 5 . Thus the method i s as follows: 1) Integrate over T z(t) = A(t) z ( t ) with i n i t i a l conditions i ) z(0) = [ 1 0 0 ] T i i ) z(0) = [ 0 1 0 ] T i i i ) z(0) = [ 0 0 1 ] T 2) Each s o l u t i o n z(T) corresponds to a column of Y(T) which i s the same as C. 3) Find the eigenvalues of f ( T ) This analysis was c a r r i e d out on the control method proposed. The eigenvalues are given i n Table 4.1 but p l o t s are not provided as there i s very l i t t l e movement of the eigenvalues over a wide range of weight-ing c o e f f i c i e n t s . The eigenvalues are both complex and r e a l . The r e a l root s t a r t s at (1,0), on the complex plane, f o r low values of the weighting c o e f f i c i e n t s and moves slowly towards the o r i g i n . The complex roots s t a r t on the r e a l axis, move off and r e j o i n quickly, and diverge very slowly i n opposite d i r e c t i o n s along the a x i s . The point at which they move o f f and r e j o i n i s approximately the same (to 6th decimal place) and depends on the r a t i o of the frequency of the input compared with the system. When th i s i s low, near 1, the point of i n t e r e s t i s very close to the o r i g i n (1.83E-03); when t h i s r a t i o i s high (100) the point i s near (1,0) (0.938). It must be noted that during the i n v e s t i g a t i o n the eigenvalues changed very l i t t l e over a wide range of weighting c o e f f i c i e n t s (0.2 to 1000), and the main difference noted 66. Table 4.1 Floquet Analysis; Eigenvalues of the Monodromy Matrix W = 0) n D = 1 Degree q Roots 0.2 1.8420359E--03 1.8305918E-03 0.9999996 0.5 1.84 614 84E' -03 1.8265110E-03 0.9999990 1.0 1.8483700E' -03 1.8243190E-03 0.9999980 10.0 1.8715638E -03 1.8017698E-03 0.9999799 1000.0 2.2273287E' -03 1.5169947E-03 0.9979912 w = 2 * a) n D = 1 Degree q Roots 0.01 4.2425767E' -02 ± 12.1579186E-05 0.9999999 0.2 4.2527061E' -02 4.2324629E-02 0.9999994 0.5 4.2584617E' -02 4.2267474E-02 0.9999986 1.0 4.2649560E -02 4.2203121E-02 0.9999971 5.0 4.2878937E -02 4.1978020E-02 0.9999856 20.0 4.3346610E -02 4.1526776E-02 0.9999423 1000.0 4.9427010E' -02 4.6521144E-02 0.9971212 w = 10 * i % D = 1 Degree q Roots 0.01 0.5328203 0.5323635 1.0000000 0.2 0.5328646 0.5323190 1.0000000 0.5 0.5329032 0.5322807 1.0000000 1.0 0.5329032 0.5322808 1.0000000 10.0 0.5329149 0.5322690 1.0000000 100.0 0.5328509 0.5323330 0.9999999 1000.0 0.5332383 0.5319464 0.9999992 w = 100 * D = 1 Degree q Roots 0.2 0.9380051 + ; 11.2396299E-04 1.0000000 0.5 0.9380050 + ; 13.7376249E-05 1.0000000 1.0 0.9381173 0.9378930 1.0000000 10.0 0.9380050 + 18.8973262E-05 1.0000000 1000.0 0.9380662 0.9379442 0.9999996 6 7 . between i n v e s t i g a t i o n s using d i f f e r e n t frequency r a t i o s was the depart-ure and a r r i v a l point of the complex roots. The complex roots only l e f t the r e a l axis f o r very small values of the weighting c o e f f i c i e n t s (approx. 0.001 to 0.01) which were not of applicable i n t e r e s t . This a n a l y s i s shows that f o r a reference model input which i s purely s i n u s o i d a l the algorithm described i s stable. This has relevance i f a learning s i g n a l of high frequency (compared with the natural frequency of the system) i s used before any t r a j e c t o r y movement so that there can be gain adjustment p r i o r to path movement. This method i s not applicable to the current algorithm i n p r i n c i p l e , as the objective i s to follow a reference t r a j e c t o r y and not to perform as a prescribed second order system. This does, however, show that for a wide range of si n u -s o i d a l reference inputs the update algorithm i s s t a b l e and that the trend for lower frequencies i s that the complex roots move closer to the o r i g i n and the r e a l root stays at (1,0). This implies s t a b i l i t y f o r lower frequencies. Floquet analysis for very low frequencies was not c a r r i e d out. 4.5 Summary In summary, s t a b i l i t y a n a l y s i s f o r r o b o t i c systems i s d i f f i c u l t due to the complex nature of the system. S t a b i l i t y i s examined for small perturbations about the zero vector f o r a ramp input using cost function Equation [3.26] and regions of s t a b i l i t y are plotted. The cost function used i n simulation studies, Equation [3.27], has a better surface shape than Equation [3.26] and so should have s i m i l a r and probably better s t a b i l i t y c h a r a c t e r i s t i c s . Equation [3.27] could not be examined i n the same manner as i t does not reduce the l i n e a r part of the state space 6 8 . equation to a time i n v a r i a n t form. Floquet a n a l y s i s i s c a r r i e d out about the zero vector for inputs of varying frequencies and the system i s found to be s t a b l e over the range examined. An i n t u i t i v e proof of s t a b i l i t y i s given which compares the algorithm to the Lyapunov s t a b i l i t y c r i t e r i a . The cost function used i s p o s i t i v e d e f i n i t e and the gain update follows the negative gradient of the cost function. Provided the assumptions made are reasonable and v a l i d , the method should be asymptotically stable. 69. CHAPTER 5 RESULTS AND OBSERVATIONS 5.1 Trajectory Paths Four t r a j e c t o r i e s were chosen to represent t y p i c a l robot motion f o r simulation studies of the two-link manipulator. These were s p e c i f i e d by s t a r t i n g and ending p o s i t i o n s i n c a r t e s i a n coordinates. They are given i n Table 5.1. Table 5.1 Trajectory S t a r t i n g and Ending P o s i t i o n s Trajectory S t a r t i n g P o s i t i o n Ending P o s i t i o n 1 (0.25,-1.25) (0.15,-1.30) 2 (0.15,-1.30) (0.15,-1.25) 3 (0.05,-0.50) (0.15,-0.50) 4 (0.15,-0.55) (0.15,-0.65) The f i r s t represents an a r b i t r a r y motion, the second i s the reverse of the f i r s t and the t h i r d and fourth are motions i n the x and y d i r e c t i o n s r e s p e c t i v e l y , which would correspond to motions of accommodation or desired endpiece movements. The s t a r t i n g and ending p o s i t i o n s of the two l i n k manipulator are shown i n Figure 5.1. Table 5.2 shows the s t a r t i n g and ending angles for each j o i n t f o r each t r a j e c t o r y . Note that the angles moved are not large. This i s t y p i c a l of incremental setpoint changes which are used today i n conventional robot t r a j e c t o r i e s . Trajectory 1 & 2 Trajectory 3 Figure 5 .1. Two-link Manipulator Start ing and Ending Positions Trajectory 4 71. Table 5.2 S t a r t i n g and Ending J o i n t Angles f o r A l l T r a j e c t o r i e s Trajectory J o i n t 1 (radians) J o i n t 2 (radians) 1 Start End 6.10844 6.05246 1.18640 1.08998 2 Start End 6.05246 6.10844 1.08998 1.18640 3 Start End 0.02925 0.08802 3.07087 2.92906 4 Start End 6.20968 6.04917 2.75183 2.50647 5.2 Observer Routine Simulation The observer routine described i n Section 3.3 was implemented on the two l i n k manipulator simulation using model reference adaptive c o n t r o l i n j o i n t coordinates. Results are most c l e a r l y seen from Figures 5.2 through 5.5 for the two-link manipulator for the four t r a j e c t o r i e s . The observer routine r e s u l t s show l e s s e r r o r from the most d i r e c t path between the s t a r t i n g and ending p o s i t i o n s . The path taken can be explained because each j o i n t converges upon i t s desired ending angle at d i f f e r e n t rates corresponding to the d i f f e r e n t n a t u r a l frequencies of the i n d i v i d u a l j o i n t c o n t r o l l e r s . For example, i n t r a j e c t o r y four, j o i n t two converges quickly and j o i n t one more slowly. This r e s u l t s i n large errors between the path taken and the most d i r e c t path. Figures 5.6 and 5.7 show the time response of the j o i n t angles f o r t r a j e c t o r y four. Trajectory 1 -1.26 E -1.27--1.28--1.29--1.30 Start with observer no observer Direct Path — i 1 1 1 1 1 1 1 1 1 0.15 0.16 0.17 018 0.19 0.20 0.21 0.22 0.23 0.24 0.26 X (metres) Figure 5.2. Trajectory 1 Observer Simulation Results rO Figure 5.3. Trajectory 2 Observer Simulation Results Trajectory 3 Figure 5.4. Trajectory 3 Observer Simulation Results Trajectory 4 Figure 5.5. Trajectory 4 Observer Simulation Results Trajectory 4 Figure 5.6. Trajectory 4 Observer Simulation Theta 1 Response Trajectory 4 Figure 5.7. Trajectory 4 Observer Simulation Theta 2 Response -4 7 8 . The observer routine slows the j o i n t response because of an increase In e f f e c t i v e mass and hence a decrease i n the natural frequency r e s u l t i n g i n a change i n the damping c h a r a c t e r i s t i c s of the c o n t r o l l e r . When t h i s i s included In the gain update algorithm, t h i s increase i n the estimated alpha i s r e f l e c t e d i n a slower response time. This i s p a r t i c u l a r l y evident for j o i n t two. Figure 5.7 shows an underdamped response f o r the no observer routine response while the observer response s t i l l i s c r i t i c a l l y or overdamped. By slowing the response of t h i s j o i n t there i s l e s s discrepancy between the time response of the two j o i n t c o n t r o l l e r s , and the path i n cartesian coordinates i s c l o s e r to the most d i r e c t path. The observer routine provides a more r e a l i s t i c estimate of the e f f e c t i v e mass as i t varies with time and thus modifies the gain update algorithm to include the d i f f e r e n t observed natural frequency of the j o i n t . This i s c r u c i a l i n maintaining the desired damping r a t i o of the j o i n t c o n t r o l l e r such that the r i s k of an underdamped response i s reduced. 5.3 Simulation of Model Reference Adaptive C o n t r o l l e r i n Cartesian Coordinates The model reference adaptive c o n t r o l algorithm proposed i n Section 3.4 was implemented on the two l i n k manipulator simulation program. The observer routine previously described was also used i n the gain update routine part of t h i s algorithm. The four t r a j e c t o r i e s were examined by computer simulation and the resultant paths plotted and compared with those of a constant gain manipulator simulation. The desired reference model t r a j e c t o r y was a stra i g h t l i n e between the s t a r t i n g and ending points with the damping 7 9 . r a t i o of 1.0 and natural frequencies of 7.0 Hz f o r both x and y paths. Gains were i n i t i a l i z e d at values which corresponded to those of a constant gain c o n t r o l l e r . Figures 5.8 through 5.11 show r e s u l t s obtained. In each case there was s i g n i f i c a n t improvement i n that the path followed was c l o s e r to the desired s t r a i g h t l i n e than the path achieved by use of constant gains. A comparison of the maximum perpendicular e r r o r from the desired s t r a i g h t l i n e to the simulation paths y i e l d s improvements of 22% for t r a j e c t o r y 1, 50% f o r 2, 55% f o r 3, and 17% f o r 4, f o r adaptive c o n t r o l over conventional c o n t r o l . The f i r s t three t r a j e c t o r i e s a l l show p a r t i c u l a r improvement when near the ending point i n that the adaptive c o n t r o l t r a j e c t o r y i s very close to the desired s t r a i g h t l i n e . This i s not shown i n t r a j e c t o r y four, as the j o i n t two response remains f a s t r e l a t i v e to the j o i n t one response. The adaptive control scheme attempts to adjust the gains of the j o i n t c o n t r o l l e r s such that the applied actuator voltage i s reduced or increased to compensate i n d i r e c t l y for the discrepancy between the desired j o i n t t r a j e c t o r y , r e s u l t i n g i n the desired Cartesian path and the actual j o i n t t r a j e c t o r y . Figures 5.12 through 5.19 show how the p o s i t i o n and v e l o c i t y gains f o r the j o i n t c o n t r o l l e r s change with time for the four t r a j e c t o r i e s . It can be seen that the v e l o c i t y gain adjusts i n the same fashion as the p o s i t i o n gain f o r a l l t r a j e c t o r i e s , but not p r o p o r t i o n a l l y . The damping r a t i o s for the j o i n t c o n t r o l l e r s change but as can be seen from the t r a j e c t o r y graphs, the manipulator responds i n a c r i t i c a l l y damped fashion. 5.4 E f f e c t of Various Parameters In the proposed model reference adaptive control scheme, various parameters have been selected f o r the simulation r e s u l t s presented. The Trajectory 1 0.14 0.16 0.18 0.20 0.22 0.24 0.26 X (metres) Figure 5.8. Control Method Simulation Results Trajectory 1 00 o Trajectory 2 finish Control Method Conventional Adaptive Ideal Path start 0.14 0.16 0.18 0.20 0.22 X (metres) 0.24 0.26 5.9. Control Method Simulation Results Trajectory 2 Trajectory 3 -0.4995 -| Figure 5.10. Control Method Simulation Results Trajectory 3 00 ho Trajectory 4 Figure 5.11. Control Method Simulation Results Trajectory 4 00 Trajectory 1 Joint 1 Figure 5.12. Position and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 1 in Trajectory 1 Simulation oo Trajectory 1 Joint 2 Figure 5.13. Position and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 2 in Trajectory 1 Simulation OO Oi 160 -i Trajectory 2 Joint 1 T3 CO O 0 CO * •-» o > 140-120-100-O c CO 80-60 40-\ \ Gain Position Velocity 20- — i — 1.5 0.5 Time (seconds) Figure 5.14. Position and Velocity Gain Adaption Using Model Reference in oo Cartesian Coordinates for Joint 1 in Trajectory 2 Simulation 250 Trajectory 2 Joint 2 0.5 1.5 Time (seconds) Figure 5.15. Position and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 2 in Trajectory 2 Simulation 00 160-1 Trajectory 3 Joint 1 TJ 2 "o CD CO * O > 140-120-100-"O co O c CO 80 60-40-\ Gain Position Velocity \ 20-0.5 1 1.5 Time (seconds) 2.5 Figure 5.16. Position and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 1 in Trajectory 3 Simulation 00 00 Trajectory 3 Joint 2 250-200-^ 150-100-50-Gain Position Velocity 0.5 1 1.5 Time (seconds) 2.5 Figure 5.17. Posit ion and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 2 in Trajectory 3 Simulation CO ^ 3 Trajectory 4 Joint 1 200 -I Time (seconds) Figure 5.18. Posit ion and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 1 in Trajectory 4 Simulation o Trajectory 4 Joint 2 250-1 T3 200 o *<° > T3 CO O > 100 c co (5 50 \ \ 0.5 1 1.5 2 Time (seconds) 2.5 3 Gain Position Velocity 3.5 Figure 5.19. Position and Velocity Gain Adaption Using Model Reference in Cartesian Coordinates for Joint 2 in Trajectory 4 Simulation 92. choice of these was based on simulation r e s u l t s and i n t u i t i o n . The e f f e c t s of varying these are examined below. 5.4.1 Choice of Reference Model Parameters The reference model i s given In Equations [3.70 and 3.71]. The choice of the natural frequency and damping r a t i o was made to give a j o i n t response time close to the n a t u r a l response time of the constant gain j o i n t c o n t r o l l e r s . The e f f e c t of changing the natural frequency i s shown i n Figures 5.20(a) and (b). Here the reference model t r a j e c t o r y i s along a straight l i n e between the s t a r t i n g and ending points as indicated on Figures 5.9 and 5.10. From both f i g u r e s i t can be seen that the best t r a j e c t o r y r e s u l t s from the choice of 7 Hz as the r e f e r -ence model natural frequency. High values r e s u l t e d i n poor performance; a choice of 20 Hz i n t r a j e c t o r y 3 simulation resulted i n an unstable response and an o s c i l l a t o r y response i n t r a j e c t o r y 2. A value of 7 Hz for the reference model was used i n a l l other simulation studies. No study of the e f f e c t of varying the damping r a t i o of reference model t r a j e c t o r i e s was done as c r i t i c a l damping was assumed reasonable and desirable. 5.4.2 Choice of Update Frequency The gain update frequency was chosen to be higher than the Nyquist frequency of the l i n k with the highest frequency to avoid a l a i s i n g , and low enough to be r e a l i s t i c given the computational demands of the control algorithm (such that i t could be implemented using current microprocessor technology). In theory, one could choose a d i f f e r e n t gain Tajectory 2 Reference Model Natural Frequency Analysis Tajectory 3 Reference Model Natural Frequency ) Trajectory 2 b ) Trajectory 3 Figure 5.20. Natural Frequency Analysis for Trajectories 2 and 3 94. update frequency f o r each j o i n t , but as i t was determined that f a s t e r updating i s more desirable the l i m i t was determined by the computational i n t e n s i t y . In simulation studies any sample frequency could be chosen since the simulation does not run i n r e a l time. The i d e a l frequency from an engineering standpoint would be one which was f a s t e r than the Nyquist frequency but slow enough to be e f f e c t i v e without performance penalty so that slower and hence cheaper c o n t r o l l e r s could be used. The sample frequency chosen was the same for a l l l i n k s , since the control algorithm i s the same f o r each j o i n t , therefore i d e n t i c a l c o n t r o l l e r s could be used. This reduces the costs when compared with a custom c o n t r o l l e r f o r each j o i n t . Figures 5.21(a) and (b) show t r a j e c t o r y 2 and 3 simulation r e s u l t s . These graphs show the d i f f e r e n t t r a j e c t o r i e s of the manipulator with the proposed algorithm using d i f f e r e n t gain update i n t e r v a l s . The natural frequency of l i n k one was 5 Hz and l i n k 2 was 10 Hz. As could be expected sample i n t e r v a l s which were greater than 0.02 (20 Hz) gave poor r e s u l t s as the Nyquist frequency i s twice the n a t u r a l frequency. Trajectory two r e s u l t s show l i t t l e improvement for sample i n t e r v a l s (less than 0.05), while t r a j e c t o r y three r e s u l t s show best r e s u l t s f o r an i n t e r v a l of 0.03 seconds and s i m i l a r but s l i g h t l y worse resu l t d for smaller sample i n t e r v a l s . This r e s u l t was s u p r i s i n g and must go without explanation at t h i s time. The gain update i n t e r v a l was chosen to be 0.03 seconds for a l l other simulation studies i n t h i s t h e s i s . 5.4.3 Choice of Weighting C o e f f i c i e n t s The weighting c o e f f i c i e n t s i n Equation [3.27] were i n i t i a l l y chosen to correspond with those used by Dubowsky and DesForges, although as mentioned, the cost functions d i f f e r thus the c o e f f i c i e n t s used should Tajectory 2 Gain Update Interval Analysis Tajectory 3 Gain Update Interval Analysis Figure 5.21. Gain Update Interval Analysis for Trajector ies 2 and 3 9 6 . be r e l a t e d to the square of the ones used i n [3.26]. The upper l i m i t on these i s imposed by s t a b i l i t y constraints for c e r t a i n j o i n t responses as was discussed i n Chapter 4. As the t r a j e c t o r i e s simulated do not f i t into those classes, no l i m i t on these c o e f f i c i e n t s can be a n a l y t i c a l l y determined. S t i l l , reasonable values can be assumed from the ramp s t a b i l i t y r e s u l t s and from simulation studies. Figures 5.22 and 5.23 show t r a j e c t o r y r e s u l t s varying the value Q to which both of the cost c o e f f i c i e n t s were set to. Trajectory two r e s u l t s show l i t t l e d i f f e r e n c e f o r the range of values used, while t r a j e c t o r y three r e s u l t s show improvement for higher values of Q. As the purpose of the proposed algorithm i s to provide better t r a j e c t o r y following i n c a r t e s i a n coordinates the decision was made to choose a value of p o s i t i o n cost c o e f f i c i e n t higher than the v e l o c i t y cost c o e f f i c i e n t . Higher values for these parameters w i l l increase the performance but introduce a higher r i s k of i n s t a b i l i t y . Ideally these values could be chosen based on which t r a j e c t o r y i s chosen and on extensive simulation r e s u l t s . To keep the represented algorithm simple, the c o e f f i c i e n t s were kept constant for simulation studies. Trajectory 2 Weighting Coefficient Analysis — , 1 — 1 1 1 i 0.16 0.18 0.20 0.22 0.24 0.26 X (metres) Figure 5.22. Weighting Coeff ic ient Analysis for Trajectory 2 Trajectory 3 Weighting Coefficient Analysis -0.4998-] •0.5008 H 1 1 1 1 1 1 0.04 0.06 0.08 0.10 0.12 0.14 0.16 X (metres) Figure 5.23. Weighting Coeff ic ient Analysis for Trajectory 3 CO 99. CHAPTER 6 CONCLUSIONS AND RECOMMENDATIONS 6.1 Conclusions The use of an observer routine to estimate the e f f e c t i v e mass as seen by the j o i n t c o n t r o l l e r has been shown to improve the model reference adaptive c o n t r o l scheme i n j o i n t coordinates, p a r t i c u l a r l y with respect to maintaining a c r i t i c a l l y or overdamped response. This was assumed to hold f o r the c o n t r o l scheme i n ca r t e s i a n coordinates and no analysis was done to show differ e n c e i n the gain update methods. The model reference adaptive c o n t r o l scheme presented i n t h i s thesis has been shown to have better t r a j e c t o r y following between setpoints than a conventional proportional d e r i v a t i v e c o n t r o l scheme with respect to a stra i g h t l i n e path between s t a r t i n g and ending points. A two-link manipulator computer simulation showed improvement f o r a l l t r a j e c t o r i e s examined. This would lead to the use of fewer setpoints f o r a robot to execute a desired path w i t h i n c e r t a i n path er r o r bounds and thus fa s t e r performance over conventional control methods due to the la r g e r servo e r r o r s . The following conclusions can be drawn regarding the co n t r o l scheme parameters: a) The reference model should have a na t u r a l frequency close to that of the l i n k with the slowest natural frequency. b) The gain update frequency must be more than double the natural frequency of the l i n k In question. In the case of a l l j o i n t c o n t r o l l e r s having gain updates at the same frequency, t h i s 100. frequency must be more than twice that of the highest natural frequency l i n k . No advantage was noted for gain update frequencies i n excess of three times the natural frequency. The choice of cost c o e f f i c i e n t s was examined but no conclusions could be drawn about optimal values. Reasonable l i m i t s could not be drawn from s t a b i l i t y a n a l y s i s as the studies of ramp input were f o r a d i f f e r e n t cost funtion, and Floquet analysis showed s t a b i l i t y over a wide range of values. Simulation studies with values q Q = 0.707 q x = 0.202 are given and show the improvement noted. No instances of i n s t a b i l i t y were encountered. A reasonable l i m i t on both q Q and equal to 2 can be drawn from simulation studies but are not conclusive and could be d i f f e r e n t f o r d i f f e r e n t manipulators. 6.2 Recommendations The following recommendations can be made: 1. The model reference adaptive control scheme i n c a r t e s i a n coordinates be implemented on the PUMA robot computer simulation and r e s u l t s compared with other adaptive control schemes. This i s d i f f i c u l t as there i s no standard t e s t f o r comparison purposes but the t r a j e c t o r y simulated by Nicosia and Tomei [1984] using a three l i n k manipulator could be attempted. Comparison should be made on absolute path error, t o t a l t r a j e c t o r y time, and computational load. 101. 2. As the scheme presented uses g r a v i t y compensation i n the j o i n t c o n t r o l l e r , studies into load estimation techniques and thus e f f e c -t i v e load estimators which could be used i n g r a v i t y compensation of the load mass, should be c a r r i e d out. I n i t i a l studies of the model reference c o n t r o l scheme i n j o i n t coordinates showed that i t would not work without the g r a v i t y compensation. If e f f e c t i v e g r a v i t y compensation which Included load mass was possible, the c o n t r o l scheme presented could be implemented on an actual robot and then evaluated. 3. Simulation studies of elaborate paths where many setpoints were used i n succession should be c a r r i e d out to determine the behaviour of the gains over an extended period. This would also determine i f an exponential decay to the o r i g i n a l gain values would be necessary. This may lead to further information as to s u i t a b l e cost function c o e f f i c i e n t l i m i t s . 4. A high frequency learning s i g n a l superimposed on the desired response was not used i n t h i s algorithm. This could be examined to determine i f i t i s of b e n e f i t . The work presented i s the f i r s t published which uses model r e f e r -ence adaptive control i n the world coordinates. It has the following d i s t i n c t advantages over other forms of adaptive c o n t r o l : (a) The form of the conventional c o n t r o l l e r i s retained. This would allow the use of a supervisory computer c o n t r o l l e r operating separate from the j o i n t c o n t r o l l e r s with the only i n t e r a c t i o n being 102. the s e t t i n g of the gains. This would allow easy implemetation on current robots with minor hardware modifications. The adaption scheme i s not as computationally intensive as others that have been presented with few exceptions. This would allow i t to be r e a l i z a b l e with the microprocessor technology a v a i l a b l e . The scheme does not require much information about the manipulator system, thus i t ' s not subject to modelling errors and the e f f e c t s of unknown parameters. It does, however, need e f f e c t i v e g r a v i t y compensation. 103. REFERENCES Advanced Continuous Simulation Language User Guide/Reference Manual, Third E d i t i o n , M i t c h e l l and Gauthier, Assoc., Inc., Concord, Mass., 1981. Anex, R.P. J r . and Hubbard, Mont, "Modelling and Adaptive Control of a Mechanical Manipulator," ASME Journal of Dynamic Systems, Measurement, and Control, Sept. 1984, pp. 211-217. Asada, H., Knade, T. and Takeyama, I., "Control of a Direct-Drive Arm," ASME Journal of Dynamic Systems, Measurement, and Control, Sept. 1983, pp. 136-143. Balestrino, A., De Maria, G. and Sciavicco, L., "An Adaptive Model Following Control f o r Robotic Manipulators," ASME Journal of Dynamic Systmes, Measurement, and Control, Sept. 1983, pp. 143-151. Book, W.J. and Majette, M., "Controller Design for F l e x i b l e , D i s t r i b u t e d Parameter Mechanical Arms V i a Combined State Space and Frequency Domain Techniques," ASME Journal of Dynamic Systems, Measurement, and Control, Dec. 1983, pp. 245-254. Boucher, Daniel Charles, Closed Loop End Piece Control of a Servo Controlled Manipulator. Thesis, Master of Applied Science, U n i v e r s i t y of B r i t i s h Columbia, June 1985. Cherchas, D.B., "Robotics: Kinematis, Dynamics and Control," U.B.C. Course Notes, 1985. 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. Desa, S. and Roth, B., "Synthesis of Control Systems for Manipulators Using M u l t i v a r i a b l e Robust Servomechanism Theory," International Journal of Robotics Research, F a l l 1985, pp. 18-34. Donalson, Dale D. and Leondes, C.T., "A Model Referenced Parameter Tracking Technique f o r Adaptive Control Systems," IEEE Transactions on Applications and Industry, Vol. 82, No. 68, Sept. 1963, pp. 241-262. Dubowsky, S., Des Forges, D.T., "The Appl i c a t i o n of Model References Adaptive Control to Robotic Manipulators," ASME Journal of Dynamic Systems, Measurement, and Control, Sept. 1979, pp. 193-200. Dumont, G., "A P r a c t i c a l Guide to Self-Tuning Regulators," ISA Spring Symposium, Columbus, Ohio, A p r i l 1982, pp. 199-205. Elgazzar, S., " E f f i c i e n t Solution for the Kinematic V e l o c i t i e s and Accelerations f o r the PUME 560 Robot," National Research Council, Canada, October 1985, ERB-974 MRCC No. 25052. 104. Hang, Chang-Chleh and Parks, P a t r i c , C , "Comparative Studies of Model Reference Adaptive Control," IEEE Transactions on Automatic Control, Oct. 1973, pp. 419-428. Judd, Robert P. and Falkenburg, Donald R., "Dynamics of Nonrigid A r t i c u l a t e d Robot Linkages," IEEE Transactions on Automatic Control, V o l . AC-30, May 1985, pp. 499-502. Kim, Byung Kook and Shin, Kang G., "Suboptimal Control of I n d u s t r i a l Manipulators with a Weighted Minimum Time-Fuel C r i t e r i o n , " IEEE Transactions on Automatic Control, V o l . AC-30, Jan. 1985, pp. 1-10. K i r c a n s k i , M. and Vukobratovic, M., "Trajectory Planning for Redundant Manipulators i n the Presence of Obtacles," I n s t i t u t e Mihailo Pupin, Beograd, Yugoslavia, 5th CISM, 1984. Koivo, A.J., "Self-Tuning Manipulator Control i n Cartesian Base Coordinate System," ASME Journal of Dynamic Systems, Measurement, and Control, December 1985, pp. 316-323. Koivo, A n t t i H. and Guo, Ten-Huei, "Adaptive Linear Controller for Robotic Manipulators," IEEE Transactions on Automatic Control, Vol. AC-28, Feb. 1983, pp. 162-171. Lee, G.C.S., "Robot Arm Kinematics, Dynamics, and Control," Computer, Dec. 1982, pp. 62-80. Lee, C.S.G. and Lee, B.H., "Resolved Motion Adaptive Control for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurements, and Control, June 1984, pp. 134-142 (also i n IEEE Transactions on Automatic Control, Sept. 1984). Lee, C.S.G. and Chung, M.J., "An Adaptive Control Strategy for Mechanical Manipulators," IEEE Transactions on Automatic Control, Sept. 1984, pp. 837-840. L i n , Ching-Fang, "Advanced Controller Design for Robot Arms," IEEE Transactions on Automatic Control, A p r i l 1984, pp. 350-353. Luh, J.Y.S., Fisher, William D. and Paul, Richard P.C., "Joint Torque Control by a Direct Feedback f o r I n d u s t r i a l Robots," IEEE Transactions on Automatic Control, Feb. .983, pp. 153-161. Luh, J.Y.S., Walker, M.W. and Paul, R.P.C., "On-Line Computational Scheme f o r Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurement and Control, Vol. 102, June 1980(a), pp. 69-76. Luh, J.Y.S., Walker, M.W. and Paul, R.P.C., "Resolved-Acceleration Control of Mechanical Manipulators," IEEE Transactions on Automatic Control, V o l . AC-25, No. 3, June 1980(b), pp. 468-474. 105. Luh, J.Y.S., "An Anatomy of I n d u s t r i a l Robots and Their Controls," IEEE Transactions on Automatic Controls, Feb. 1983, pp. 133-152. Luo, Gong-Liang and Sardis, George N., "Robust Compensation for a Robotic Manipulator," IEEE Transactions on Automatic Control, June 1984, pp. 564-567. Marchard, Pauline Anne, Simulation and Adaptive Control of a Robot Arm, Thesis, Master of Science, U n i v e r s i t y of B r i t i s h Columbia, June 1985. Minorsky, Nicholas, Nonlinear O s c i l l a t i o n s , D. Van Nostrand Company, Inc., 19 62, pp. 118-162. Narendra, Kumpati S. and Valavani, Lena S., "Direct and Indirect Model Reference Adaptive Control," Automatica, Vol. 15, 1979, pp. 653-664. Nicosia, S. and Tomei, P., "Model Reference Adaptive Control Algorithms f o r I n d u s t r i a l Robots," Automaticia, Vol. 20, No. 5, 1984, pp. 635-644. Paul, R.P., Robot Manipulators: Mathematics, Programming, and Control, 1st Ed., MIT Press, Cambridge, Mass., 1981. Takahashi, Yasundo, Rabins, Michael J . and Auslander, David M., Control and Dynam!c Systerns, Addison-Welsey Publishing Company, Inc., 1970, pp. 536-549. Takegaki, Morikazu and Arlmoto, Suguru, "A New Feedback Method for Dynamic Control of Manipulators," ASME Journal of Dynamic Systems, Measurement and Control, June 1981, pp. 119-125. Van Den Bosch, P.P.J, and Tjahjad i , P.I., "Model Updating Improves MRAC Performance," IEEE Transactions on Automatic Control, Dec. 1984, pp. 1106-1108. Vukobratovic, M. and Ki r c a n s k i , N., "Decoupled Control of Robots Via Asymptotic Regulators," IEEE Transactions on Automatic Control, Oct. 1983, pp. 978-985. Vukobratovic, M. and Kir c a n s k i , M., "A Method for Optimal Synthesis of Manipulation Robot T r a j e c t o r i e s , " ASME Journal of Dynamic Systems, Measurement, and Control, June 1982, pp. 188-193. Vukobratovic, M. and Stokic, D., "Is Dynamic Control Needed i n Robotic Systems, and i f so, to What Extent," I n t e r n a t i o n a l Journal of Robotics Research, Summer 1983, pp. 18-34. Walker, M.W. and Orin, D.E., " E f f i c i e n t Dynamic Computer Simulatino of Robotic Mechanisms," ASME Journal of Dynamic Systems, Measurement, and Control, Sept. 1982, pp. 205-211. Whitney, Daniel E., "Resolved Motion Rate Control of Manipulators and Human Prostheses," IEEE Transactions on Automatic Control, June 1969, pp. 47-53. 106 . Wu, Shi-Haur, "Compliance Control of a Robot Manipulator Based on J o i n t Torque Servo," International Journal of Robotics Research, F a l l 1985, pp. 55-71. Wu, Shi-Haur and Lee, Chung C , "Estimation of the Accuracy of a Robot Manipulator," IEEE Transactions on Automatic Control, March 1985, pp. 304-306. Young, K.K.D., "Controller Design for a Manipulator Using Theory of Variable Structure Systems," IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-8, No. 2, February, 1978, pp. 210-218. 107. APPENDICES APPENDIX A SIMPLE TWO LINK MANIPULATOR SIMULATION 109. Simple Two L i n k Manipulator S i m u l a t i o n program 2 l i n k robot s i m u l a t i o n "THIS IS A SIMPLE 2 LINK MANIPULATOR SIMULATION. NO ADAPTIVE CONTROL" "SCHEMES ARE USED. THIS IS TO SIMULATE CONVENTIONAL CONTROL" n "pa rame te r s that can be changed o n - l i n e d u r i n g s i m u l a t i o n " c i n t = 0.03 $ "communication i n t e r v a l " wn1= 31.42 $ wn2= 62.83 $"Correspond to 5Hz, 10Hz" damp1= 1.0 $ damp2= 1.0 $"of t r u e Manipulator " "Gains & motor torque c h a r a c t e r i s t i c s " km1= 100. $ km2= 80. kp1=150. $ kp2=l85. kv1=60. $ kv2=37. kpl=100. $ k v l = l 0 0 . $ " l i m i t s on k_dots " i n i t i a l $ "Set up parameters f o r 2 l i n k robot s i m u l a t i o n program " i n t e g e r i , j , k , l " Define v a r i a b l e s " th1,th2 Angles of J o i n t s 1 and 2 " t h _ v , t h _ a Angular v e l o c i t i e s and a c c e l e r a t i o n s " _ i c , _ v i c i n i t i a l c o n d i t i o n s of p o s i t i o n and v e l o c i t y " f o r reference model and t r u e j o i n t angles 11,12 l e n g t h of l i n k 1, 2 " ml,m2 mass at end of l i n k 1, 2 " *p_,kv_ P o s i t i o n and v e l o c i t y g a i n of l i n k _ " d i , d i j , d i j k Dynamic c o e f f i c i e n t s i n governing dynamic " equations " t o _ torque d e s i r e d from c o n t r o l l e r " t _ torque from motor (bounded w i t h g r a v i t y " compensation) " km_ motor torque constant " _wn,_damp n a t u r a l freqency and damping r a t i o of _ " Set up i n i t i a l c o n d i t i o n s " th1des=0.3 $ th2des=0.l86 $ " u n i t s r a d i a n s " th1ic=0. $ th2ic=0. e l b f l g = 1 . $ " S p e c i f y elbow o r i e n t a t i o n " g=9.8 $ " G r a v i t y " 11=1.0 $ 12=0.5 m1=4. $ m2=3. 110. Simple Two Li n k Manipulator S i m u l a t i o n kpid=kp1 $ kpic2=kp2 kvic1=kv1 $ kvic2=kv2 th2vic=0. th1vic«=0. " Define a c t u a t o r i n e r t i a s ia1=50. $ ia2=l5. n . n end $ " of i n i t i a l " dynamic d e r i v a t i v e s l = s i n ( t h 1 ) d = c o s ( t h l ) s2=sin(th2) c2=cos(th2) s12=sin(th1+th2) c12=cos(thl+th2) d11=(m1+m2)*l1**2 + m2*12**2 + 2.*m2*l1*12*c2 + ia1 d21=m2*l2**2 + m2*H*12*c2 d22=m2*12**2 + ia2 d112=-2.*11*12*m2*s2 d122=-1.*m2*11*12*s2 d21 1=m2*H*12*s2 d1 = (m1+m2)*g*H*sl + m2*g*12*sl 2 d2=m2*g*12*s12 "Fin d torque " to1=km1*(kp1*(thldes-th1)-kvl*thlv) + d l to2=km2*(kp2*(th2des-th2) - kv2*th2v) + d2 "Note that when d1,d2 are added t h i s ... corresponds t o g r a v i t y compensation" " L i m i t torques t o reasonable values " t1 = bound(-200.,200.,tol) t2 = bound(-100.,100.,to2) "Fi n d a c c e l e r a t i o n s " th1a=(d22/(d22*d11-d21**2))*(t1-d21/d22*t2 + d2l * d 2 1 l / d 2 2 * t h l v * * 2 - d112*th1v*th2v - dl22*th2v**2 - d1 + d21*d2/d22) th2a={1./d22)*(t2- d21*th1a - d211*th1v**2 - d2) 111. Simple Two L i n k Manipulator S i m u l a t i o n t h 1 v=integ(th1a,th1vic) th2v=integ(th2a,th2vic) t h 1 = i n t e g ( t h 1 v , t h 1 i c ) t h 2 = i n t e g ( t h 2 v , t h 2 i c ) end S " O f d e r i v a t i v e termt ( ( t . g t . 5 . 0 ) . o r . ( ( t . g t . 0 . 5 ) . a n d . ( ( a b s ( t h 1 v ) ) . I t . 0 . 0 0 1 ) . a n d . ( ( a b s ( t h 2 v ) ) . l t . 0 . 0 0 1 ) ) ) " 5 sec s i m u l a t i o n max " end $ " O f dynamic " t e r m i n a l end $ "of t e r m i n a l " end $ »*_*_*_*_*_*_*_*_* 0 f program *_*_*_*_*_*_*_*_*" APPENDIX B TWO LINK MANIPULATOR MRAC SIMULATION Two Link MRAC S i m u l a t i o n program 2 _ l i n k _ r o b o t s i m u l a t i o n n " I n i t i a l parameters t h a t you want to change when e x e r c i s i n g " the model must be p l a c e o u t s i d e of the i n i t i a l block c i n t = 0.03 $ kc = 1.0 ydamp= 1.0 ywn=7.0 $ $ xdamp= 1.0 xwn=7.0 wn1= 31.42 dampl= 1.0 traj=1 $ $ § wn2= 62.B3 $"Correspond to 5Hz, 10Hz" damp2= 1.0 $"of t r u e Manipulator " "Choose t r a j e c t o r y d e f a u l t " kml= 100. kp1=l50. kv1=60. "kpl=l00." $ S $ $" km2= 80. kp2=l85. kv2=37. kvl = l 0 0 . " $ " l i m i t s on k_dots i f d e s i r e d q0=0.707 S q1=0.202 i n i t i a l $ "Set up parameters f o r 2 l i n k robot s i m u l a t i o n program " 113. i n t e g e r i , j , k, 1 a r r a y o t h l ( 2 ) , o t h 2 ( 2 ) , o t h 1 v ( 2 ) , o t h 2 v ( 2 ) Define v a r i a b l e s th1,th2 x.y t h _ v , t h _ a xv,xa,yv,ya r t h 1 , r t h 2 r t h _ v , r t h _ a _ i c , _ v i c u,w adot,bdot 11 ,12 ml ,m2 kp_,kv_ t d _ o t h _ Oth_v,oth_a kpdot(_) kvdot(_) d i , d i j , d i j k Angles of J o i n t s 1 and 2 World c o o r d i n a t e s of the Desi r e d Reference Model endpoint l o c a t i o n Angular v e l o c i t i e s and a c c e l e r a t i o n s Endpoint v e l o c i t y and a c c e l e r a t i o n i n world Coordinates of Reference Model Reference model angular p o s i t i o n j o i n t s 1 & 2 Reference model angular v e l o c i t y and a c c e l e r a t i o n J o i n t c o o r d i n a t e s i n i t i a l c o n d i t i o n s of p o s i t i o n and v e l o c i t y f o r r e f e r e n c e model and t r u e j o i n t angles Psuedo v a r i a b l e s used i n gain update r o u t i n e v a r i a b l e s used i n gain update l e n g t h of l i n k 1, 2 mass a t end of l i n k 1, 2 P o s i t i o n and v e l o c i t y gain of l i n k _ Estimated d i s t u r b a n c e torque ~ Observed J o i n t angle (white n o i s e added) C a l c u l a t e d observed v e l , a c c e l from o t h _ D e s i r e d Time d e r i v a t i v e of p o s i t i o n gain D e s i r e d time d e r i v a t i v e of v e l o c i t y gain Dynamic c o e f f i c i e n t s i n governing dynamic 114. Two Link MRAC S i m u l a t i o n equations " torque d e s i r e d from c o n t r o l l e r " torque from motor (bounded w i t h g r a v i t y " compensation) " motor torque constant " n a t u r a l freqency and damping r a t i o of _ " " Set up i n i t i a l c o n d i t i o n s " i f ( t r a j . e q . 1 ) CALL t r a j 1 ( x i c , y i c , x v i c , y v i c , x d e s , y d e s ) i f ( t r a j . e q . 2 ) CALL t r a j 2 ( x i c , y i c , x v i c , y v i c , x d e s , y d e s ) i f ( t r a j . e q . 3 ) CALL t r a j 3 ( x i c , y i c , x v i c , y v i c , x d e s , y d e s ) i f ( t r a j . e q . 4 ) CALL t r a j 4 ( x i c , y i c , x v i c , y v i c , x d e s , y d e s ) c a l l o p f i l e " T h i s Subroutine w i l l r e t u r n s t a r t i n g and d e s i r e d ending c o n d i t i o n s " " i n world c o o r d i n a t e s . Now use Inverse Kinematic subroutine to f i n d " " corresponding i n i t i a l c o n d i t i o n s i n j o i n t c o o r d i n a t e s " e l b f l g = 1 . C a l l i k i n ( x i c , y i c , e l b f l g , t h 1 i c , t h 2 i c ) r t h l i c = t h 1 i c $ r t h 2 i c = t h 2 i c $ " Ref Model I n i t i a l C o n d i t i o n s " th1=th1ic $ th2=th2ic C a l l i k i n ( x d e s , y d e s , e l b f l g , t h 1 d e s , t h 2 d e s ) "Desired ending p o s i t i o n " " setup model reference gain update parameter i n i t i a l c o n d i t i o n s " u i d = 0 . $ uic2=0. uvic1=0. $ uvic2=0. wic1=0. S wic2=0. wvic1=0. S wvic2=0. g=9.8 $ " G r a v i t y " 11=1.0 $ 12=0.5 ml =4. S m2 = 3. kpic1=kp1 $ kpic2=kp2 kvic1=kv1 $ kvic2=kv2 t h 2 v i c = ( c o s ( t h 1 i c ) * y v i c - s i n ( t h l i c ) * x v i c ) / ( 1 2 * s i n ( t h 2 i c ) ) + ( c o s ( t h 1 i c + t h 2 i c ) * y v i c - s i n ( t h 1 i c + t h 2 i c ) * x v i c ) / ( H * s i n ( t h 2 i c ) ) t h 1 v i c = ( H * s i n ( t h 2 i c ) + 11 * s i n (th1 i c ) *cos ( t h l i c + t h 2 i c ) + l 2 * c o s ( t h 1 i c + t h 2 i c ) * s i n ( t h 1 i c + t h 2 i c ) ) / ( H * * 2 * c o s ( t h 1 i c ) * s i n ( t h 2 i c ) + H * l 2 * s i n ( t h 2 i c ) * c o s ( t h 1 i c + t h 2 i c ) ) * x v i c -( l 1 * c o s ( t h 1 i c ) * c o s ( t h 2 i c ) + 1 2 * c o s ( t h 1 i c + t h 2 i c ) ) / t o t km_ wn, damp Two Link MRAC S i m u l a t i o n ( H * * 2 * c o s ( t h 1 i c ) * s i n ( t h 2 i c ) + H * 1 2 * s i n ( t h 2 i c ) * c o s ( t h 1 i c + t h 2 i c ) ) * y v i c " Standard d e v i a t i o n s f o r white noise added to observers astdev=0.00l § vstdev=0.001 " Define a c t u a t o r i n e r t i a s ia1=50. $ ia2=15. " Define an e f f i c t i v e mass which f o r now w i l l be set " t o the a c t u a t o r i n e r t i a eml=ia1 $ em2=ia2 end $ " of i n i t i a l " dynamic d e r i v a t i v e p r o c e d u r a K s l ,c1 ,s2,c2, s1 2,c 1 2= ) $ "Block 1" s1=sin(th1) d = c o s ( t h 1 ) s2=sin(th2) c2=cos(th2) s l 2 = s i n ( t h l + t h 2 ) c12=cos(th1+th2) d11=(ml+m2)*l1**2 + m2*12**2 + 2.*m2*l1*12*c2 + ia1 d2l=m2*12**2 + m2*11*12*c2 d22=m2*12**2 + ia2 d112=-2.*11*12*m2*s2 d122=-1 .*m2*H*12*s2 d2l1=m2*l1*l2*s2 d1 = (m1+m2)*g*H*s1 + m2*g*l2*s12 d2=m2*g*12*s12 end $ "of p r o c e d u r a l block 1 " procedural(th1a,th2a=s1,s2,d,c2,s12,c12) $"Block 2" " C a l l r o u t i n e or develope r o u t i n e t o f i n d torque " to1=km1*(kp1*(th1des-th1)-kvl*th1v) + d1 to2=km2*(kp2*(th2des-th2) - kv2*th2v) + d2 " L i m i t torques t o reasonable v a l u e s " t1 = bound(-200.,200.,tol) t2 = bound(-l00.,100.,to2) th1a=(d22/(d22*d11-d21**2))*(t1-d21/d22*t2 + d2l*d211/d22*th1v**2 - dl12*th1v*th2v - d122*th2v**2 - d1 + d21*d2/d22) 116 Two L i n k MRAC S i m u l a t i o n th2a=(1./d22)*(t2- d21*th1a - d211*th1v**2 - d2) end $ "of pro c e d u r a l Block 2" p r o c e d u r a l ( t h l v , t h 2 v , t h l r t h 2 = t h 1 a , t h 2 a ) t h 1 v = i n t e g ( t h 1 a , t h 1 v i e ) th2v= i n t e g ( t h 2 a , t h 2 v i c ) t h 1 = i n t e g ( t h 1 v , t h 1 i c ) t h 2 = i n t e g ( t h 2 v , t h 2 i c ) end $ "of pro c e d u r a l Block 3" procedural(x,y=th1,th2) $" Block 4" " update r e f e r e n c e model " xa= xwn**2 * (xdes-x) - 2 * xdamp*xwn*xv ya= ywn**2 * (ydes-y) - 2 * ydamp*ywn*yv xv=integ(xa ,xvie) y v = i n t e g ( y a , y v i c ) x = i n t e g ( x v , x i c ) y = i n t e g ( y v , y i c ) c a l l i k i n ( x , y , e l b f l g , r t h 1 , r t h 2 ) r s 1 = s i n ( r t h 1 ) r c l = c o s ( r t h i ) r s 2 = s i n ( r t h 2 ) rc2=cos(rth2) r s i 2 = s i n ( r t h l + r t h 2 ) rc12=cos(rth1+rth2) rth2v=(rc1*yv - r s 1 * x v ) / ( l 2 * r s 2 ) + (rc12*yv - rs12*xv)/(11*rs2) rth1 v = ( H * r s 2 + I1*rs1*rc12 + 12*rc 1 2*r s 1 2 ) / ( I l * * 2 * r c 1 * r s 2 + H * 1 2 * r s 2 * r c 1 2) * xv -( I 1 * r c 1 * r c 2 + I 2 * r c 1 2 ) / (11**2*rc1*rs2 + 11*12*rs2*rc12) * yv rth2 a = ( ( H * r c 1 + 12*rc12)*ya - (I1*rs1+I2*rs12)*xa -H**2*rth1v**2 -( 1 2 * * 2 + 2 . * H * l 2 * r c 2 ) * ( r t h 1 v + r t h 2 v ) * * 2 ) / ( l 1 * l 2 * r s 2 ) r t h l a = (xa + ( I l * r s 1 + I 2 * r s 1 2 ) * r t h 1 v * * 2 + 2 . * l 2 * r s l 2 * r t h 1 v * r t h 2 v + 12*rs12*rth2v**2 - ( r c l 2 / 1 1 / r s 2 ) * ( (11*rc1+12*rc12)*ya ( I l * r s 1 + I 2 * r s 1 2 ) * x a - I1**2*rth1v**2 (12**2+2.*11*12*rc2)*(rth1v+rth2v)**2 ) ) / ( H * r c 1 + 1 2 * r c l 2 ) " Now we have a ref e r e n c e model i n t h e t a l and theta2 " Two Link MRAC S i m u l a t i o n " We do a gain update r o u t i n e i n the same f a s h i o n as before " " but we have a problem i n the wn and damping r a t i o are " not known e x p l i c i t l y f o r the r e f model. So choose an " approximate value which corresponds to the r e a l manipulator adot1=q0*e1*u1 + q1*evl*uv1 bdot1=q0*e1*w1 + q1*ev1*wv1 adot2=q0*e2*u2 + q1*ev2*uv2 bdot2=q0*e2*w2 + q1*ev2*wv2 " update observer m a t r i c i e s " " i f t p - t = 0 then bypass update of v e l and a c c e l e r a t i o n s " end $ "of p r o c e d u r a l Block 4" procedural(kpl,kvl,kp2,kv2=) $ " Block 6" kp1 = i n t e g ( k p d o t 1 , k p i d ) $ kv1=integ(kvdot1,kvic1) kp2=integ(kpdot2,kpic2) $ kv2=integ(kvdot2,kvic2) end $ " of procedural Block 6" proce d u r a l ( x t r u e , y t r u e , x e r r , y e r r = x , y , s 1 , c 1 , s 1 2 , c 1 2 ) xtrue=H*s1 +12*sl2 ytrue=-1 .*H*c1-l2*c12 xerr=x-xtrue yerr=y-ytrue end $ "of block 7" end $ " O f d e r i v a t i v e "procedural(oth1 roth2,oth1v,oth2v=x,y)" $"Block 5" oth1(2)=oth1(1) oth2(2)=oth2(1) oth1v(2)=oth1v(1) oth2v(2)=oth2v(1) ua1=wn1**2*(-1.*rth1a - 2.*damp1*uvl/wn1 wa1=wn1**2*(-1.*rth1v - 2.*damp1*wv1/wn1 ua2=wn2**2*(-1.*rth2a - 2.*damp2*uv2/wn2 wa2=wn2**2*(-1.*rth2v - 2.*damp2*wv2/wn2 u1 ) wl ) u2) w2) u v l = i n t e g ( u a l , u v i c 1 ) $ uv2=integ(ua2,uvic2) wv1=integ(wa1,wvic1) $ wv2=integ(wa2,wvic2) u 1 = i n t e g ( u v l , u i c l ) $ wl=integ(wvl,wic1) u2=integ(uv2,uic2) $ w2=integ(wv2,wic2) e l = r t h l - t h 1 e v l = r t h l v - t h 1 v . $ $ e2=rth2-th2 ev2=rth2v-th2v Two Link MRAC S i m u l a t i o n oth1(1 ) = O U(0.01,th1,astdev) oth2(1)=ou(0.01,th2,astdev) o v i ( 1 ) = o u ( 0 . 0 l , ( t o l / k m l ) . v s t d e v ) ov2(1 ) = O U(0,01,(to2/km2),vstdev) "end" $ "procedural block 5" i f ( ( t - t p ) . e q . O . ) go t o l a b e l 5 o t h 1 v ( 1 ) = ( o t h l ( 1 ) - o t h 1 ( 2 ) ) / ( t - t p ) o t h 2 v ( 1 ) = ( o t h 2 ( 1 ) - o t h 2 ( 2 ) ) / ( t - t p ) o t h 1 a = ( o t h 1 v ( 1 ) - o t h 1 v ( 2 ) ) / ( t - t p ) o t h 2 a = ( o t h 2 v ( 1 ) - o t h 2 v ( 2 ) ) / ( t - t p ) td1=em1*oth1a - km1/em1*(kp1*(th1des-oth1(2))-kv1*othlv(2)) td2=em2*oth2a - km2/em2*(kp2*(th2des-oth2(2))-kv2*oth2v(2)) i f (othla.eq.O.) go t o l a b e l 2 alpha1=eml/km1/kp1 + td1/oth1a/km!/kp1 go t o l a b l 4 l a b e l 2 . . continue alpha 1=em1/km1/kp1 l a b l 4 . . continue i f (oth2a.eq.0.) go t o l a b e l 3 alpha2=em2/km2/kp2 + td2/oth2a/km2/kp2 go t o l a b l 4 a l a b e l 3 . . continue alpha2=em2/km2/kp2 l a b l 4 a . . continue k p d l = - l . * a d o t l / a l p h a 1 * k p l kvd1=bdot1 * kp1-adot1/alpha 1 * kv1 kpd2=-1.*adot2/alpha2*kp2 kvd2=bdot2*kp2-adot2/alpha2*kv2 "use the next 4 l i n e s i f l i m i t s on gain d e r i v a t i v e s are used" "kpd o t 1 = k c * ( b o u n d ( ( - 1 * k p l ) , k p l , k p d l ) ) " " k v d o t 1 = k c * ( b o u n d ( ( - 1 * k v l ) , k v l , k v d l ) ) " "kpdot2=kc*(bound((-1*kpl),kpl,kpd2))" "kvdot2=kc*(bound((-1*kvl),kvl,kvd2))" "go t o l a b e l 5 i f g a i n l i m i t s not used" kpdot1=kpd1 $ kpdot2=kpd2 119. Two Li n k MRAC S i m u l a t i o n kvdot1=kvd1 kvdot2=kvd2 go t o l a b e l 6 l a b e l 5 . . continue kpdot1=0. kvdot1=0. $ $ kpdot2=0. kvdot2=0. I a b e l 6 . . continue tp-t $ "Update time i n t e r v a l " termt ( t . g t . 5 . 0 . o r . ( ( ( a b s ( x e r r ) ) . I t . 0 . 0 0 0 1 ) .and. ( ( a b s ( y e r r ) ) . I t . 0 . 0 0 0 1 ) . a n d . ( ( t . g t . 0 . 5 ) . a n d . ( ( a b s ( t h i v ) ) . I t . 0 . 0 0 1 ) .and.((abs(th2v)).It.0.001)) )) " 5 sec s i m u l a t i o n max " c a l l p l o t ( t , x , y , t h l , t h 2 , r t h l , r t h 2 , k p l , k p 2 , k v 1 , k v 2 , x e r r , y e r r , ... xt r u e . y t r u e ) end $ " O f dynamic " t e r m i n a l c a l l c s f i l e end $ "of t e r m i n a l " end $ "*-*-*-*-*-*-*-*-* 0 f program *_*-*_*_*_*-*-*-subroutine t r a j l ( x o , y o , x v o , y v o , x f , y f ) r e a l xo,yo,xvo,yvo,xf,yf Subrouine f o r i n i t i a l c o n d i t i o n s xf=0.15 y£=-1.3 xvo=0. yvo=0. xo=0.25 yo=-1.25 r e t u r n end subroutine t r a j 2 ( x o , y o , x v o . y v o , x f , y f ) r e a l xo,yo,xvo,yvo,xf,yf Subrouine f o r i n i t i a l c o n d i t i o n s xf=0.25 yf=-1.25 xvo=0. yvo=0. xo=0.15 yo=-1.3 r e t u r n end subroutine t r a j 3 ( x o , y o , x v o , y v o , x f , y f ) r e a l xo,yo,xvo,yvo,xf,yf Subrouine f o r i n i t i a l c o n d i t i o n s xo=0.05 yo=-0.5 xf=0.15 yf=-0.5 xvo=0. yvo=0. r e t u r n end subroutine t r a j 4 ( x o , y o , x v o , y v o , x f , y f ) r e a l xo,yo,xvo,yvo,xf,yf Subrouine f o r i n i t i a l c o n d i t i o n s xo=0.15 yo=-0.55 xf=0.15 yf=-0.65 xvo=0. yvo=0. r e t u r n end 121. c T h i s i s an inv e r s e kinematic subroutine f o r a 2 l i n k manipulator c c Inputs are x,y which are world c o o r d i n a t e s c and e l b f l a g which s p e c i f i e s upper or lower elbow c Outputs are th1,th2 the angles of the j o i n t s s u b r o u t i n e i k i n ( x , y , e l b f l a g , t h 1 , t h 2 ) c r e a l x , y , 1 1 , 1 2 , t h 1 , t h 2 , s l , c 1 , s 2 , c 2 , s 1 2 , c 1 2 , e l b f l a g , p i , . e r r o r / 0 . 0 0 1 / pi=3.1415926 c c e l b f l a g i s a f l a g s e t t o determine i f the elbow i s i n upper or c lower elbow p o s i t i o n c d e f i n e e l b f l a g = 1 t o mean upper elbow c and e l b f l a g = -1 t o mean lower elbow c 11 = 1. 12=0.5 c c S t a r t by d e f i n i n g th1,th2 and f i n d i n g x,y c then using the x,y c a l c u l a t e backwards to f i n d th1,th2 c s1=sin(th1) cl=cos(th1) S2=sin(th2) c2=cos(th2) s12=sin(th1+th2) c12=cos(th1+th2) c c Now c a l c u l a t e backwards t o ensure c o r r e c t backwards or i n v e r s e kinematics c c th2s=((2*11*12)**2-(x**2+y**2-l1**2-12**2)**2) i f ( t h 2 s . l t . 0 . ) th2s - -1. * th2s t h 2 = a t a n 2 ( ( e l b f l a g * s q r t ( t h 2 s ) ) , . (x**2+y**2-11**2-12**2)) i f (th2.1t.0.) th2=2.*pi+th2 c i f ( x . e q . O . . o r . ( a b s ( s i n ( t h 2 ) ) ) . I t . e r r o r ) go t o 65 th1s=( (x**2+y**2)*(H+12*cos(th2) )+x*y*12*sin(th2) -. (11+12*cos(th2))**3)/(x*12**2*sin(th2)**2+ . y * ( 1 1 + 1 2 * c o s ( t h 2 ) ) * 1 2 * s i n ( t h 2 ) ) t h 1 c = ( x * * 2 - ( H + l 2 * c o s ( t h 2 ) ) * * 2 ) / . (y*(11+12*cos(th2))+x*12*sin(th2)) go t o 66 65 th1s=x th1c=-1.*y 66 continue t h 1 = a t a n 2 ( t h 1 s , t h 1 c ) i f ( t h l . I t . ( 0 . - e r r o r ) ) t h l = 2 . * p i + t h l r e t u r n e n d s u b r o u t i n e p l o t ( t , x , y , t h 1 , t h 2 , r t h l , r t h 2 , k p l , k p 2 , k v 1 , k v 2 , x e r r , y e r r , x t r u e , y t r u e ) r e a l t , x , y , t h 1 , t h 2 , r t h 1 , r t h 2 , k p 1 , k p 2 , k v 1 , k v 2 , x e r r , y e r r , x t r u e , y t r u e w r i t e ( 1 2 , * ) t , x t r u e , y t r u e , x , y , x e r r , y e r r w r i t e ( 1 3 , * ) t , t h 1 , t h 2 , r t h 1 , r t h 2 w r i t e ( 1 4 , * ) t , k p 1 , k p 2 , k v 1 , k v 2 r e t u r n end s u b r o u t i n e o p f i l e O o p e n ( I 2 , f i l e = ' x y . d a t ' , s t a t u s = ' u n k n o w n ' ) o p e n ( 1 3 , f i l e = ' t h . d a t ' , s t a t u s = ' u n k n o w n ' ) o p e n ( 1 4 , f i l e = ' g n . d a t ' , s t a t u s = ' u n k n o w n ' ) r e t u r n end s u b r o u t i n e c s f i l e O c l o s e ( 1 2 ) c l o s e ( 1 3 ) c l o s e ( 1 4 ) r e t u r n end APPENDIX C PUMA 560 MANIPULATOR MRAC SIMULATION 125. MRAC S i m u l a t i o n o f PUMA 560 ROBOT SIMULATION PROGRAM FOR ACSL PROGRAM ROBOT SIMULATION I N I T I A L $ " s e t up p a r a m e t e r s from a s u b r o u t i n e s e p a r a t e from " "t h e a c s l p r o g r a m , t h i s w i l l a l l o w u s e r t o d e f a u l t " " t o t h e v a l u e s f r o m t h e l a s t r u n o f t h e program or " " i n p u t new i n i t i a l v a l u e s . " INTEGER I , J , 11 LOGICAL VFLAG,EFLAG ARRAY TH(3),THV(3),THA(3),THDES(3),THDG(3),THVDG(3), Y ( 3 ) , Y V ( 3 ) , Y A ( 3 ) , Y A O L D ( 3 ) , Y D E S ( 3 ) , Y I C ( 3 ) , Y V I C { 3 ) , T Y V ( 3 ) , TO(3),THAOLD(3),YDAMP(3),WN(3),EM(3), KP(3),KV(3),KPDOT(3),KVDOT(3), U(3),UV(3),UA(3),W(3),WV(3),WA(3), UIC<3),UVIC(3),WIC(3),WVIC(3), X ( 3 ) , X V ( 3 ) , A D 0 T ( 3 ) , B E T A D ( 3 ) , T U V ( 3 ) , T W V ( 3 ) CALL PSTUP(TH10,TH20,TH30,TH1V0,TH2V0,TH3V0,THDES(1),THDES(2),.. THDES(3),TEND) " i n i t i a l i z e a n g l e s and a n g u l a r v e l o c i t i e s " TH(1)=TH10 $ TH(2)=TH20 $ TH(3)=TH30 THV(1)=TH1V0 $ THV(2)=TH2V0 $ THV(3)=TH3V0 " s e t i n i t i a l g a i n s t o v a l u e s u s e d i n pd c o n t r o l l e r " KP1=30.0 $ KP2=25.0 $ KP3=20.0 KV1=13.0 $ KV2=14.0 $ KV3=6.8 KP(1)=KP1 $ KP(2)=KP2 $ KP(3)=KP3 KV(1)=KV1 $ KV(2)=KV2 $ KV(3)=KV3 ' i n i t i a l i z e r e f model d i f f eqn v a l u e s u & w " U I C ( 1 ) = 0 . $ U I C ( 2 ) = 0 . $ U I C ( 3 ) = 0 . UVIC(1)=0. $ UVIC(2)=0. $ UVIC(3)=0. WIC(1)=-THV(1) $ WIC(2)=-THV(2) $ WIC(3)=-THV(3) WVIC(1)=0. $ WVIC(2)=0. S WVIC(3)=0. ' nb. i n i t i a l i z e r e f e r e n c e model as w e l l " DO IL1 1=1,3 Y I C ( I ) = T H ( I ) $ Y V I C ( I ) = T H V ( I ) $ YDES(I)=THDES(I) IL1..CONTINUE " s e t up e r r o r f l a g s and c o n s t a n t s " VFLAG = .FALSE. $ EFLAG = .FALSE. $ " I N I T I A L I Z E FLAGS" CONSTANT PI=3.14159265, IA1 = 2.40 , IA2 = 6.00 , IA3 = 4. CONSTANT ESLN=0.001,VLOW=0.001 " i n i t i a l i z e t h d g , t h v d g f o r o u t p u t t o p l o t f i l e s " THDG(1)=TH(1)*180./PI $ THDG(2)=TH(2)* 180./PI THDG(3)=TH(3)*180./PI THVDG(1)=THV(1)*180./PI § THVDG(2)=THV(2)*180./PI THVDG(3)=THV(3)*180./PI 126. MRAC S i m u l a t i o n of PUMA 560 " i n i t i a l i z e n a t u r a l f r e q . and damping f o r ea c h j o i n t o f r e f model" DO H 2 1 = 1,3 YDAMP(I)=0.707 IL2..CONTINUE " use n a t u r a l f r e q c l o s e t o i n i t i a l m a n i p u l a t o r model" WN(1) = 3.16 $ " s q r t o f 10" WN(2)= 4.47 $ " s q r t of 20 " WN(3)= 5.77 $ " s q r t of 100/3 " " a l s o i n i t i a l i z e c o s t f u n c t i o n c o e f f i c i e n t s " Q0=0.707 $ Q1=0.202 " t r y o p e n i n g f i l e s f o r w r i t i n g t o f o r p l o t f i l e s " CALL OPFILE END $ " I N I T I A L " DYNAMIC DERIVATIVE " B ecause of p r o b l e m s w i t h t h e s o r t i n g of t h i s r e l a t i v e l y " " c o m p l i c a t e d r o u t i n e I w i l l use p r o c e d u r a l b l o c k s t o a l l o w " " t h e program t o f u n c t i o n i n t h e o r d e r I w i s h " PROCEDURAL(A,B,C,D12,D13,D23,D11,D22,D33=) " Do not p u t t h . t h v i n as i n p u t s f o r c o r r e c t s o r t i n g of p r g " " DEFINE SHORTENED COS & SIN SUBSTITUTIONS " S2=SIN(TH(2)) S C2 = COS(TH(2)) S3=SIN(TH(3)) $ C3 = COS(TH(3)) S23=SIN(TH(2)+TH(3)) $ C23=COS(TH(2)+TH(3)) "- USE THESE IN CALCULATING THE DYNAMIC COEFFICIENTS — " D2=-97.472*C2 - 44.2l07*S23 D3=-44.1657*S23 D11=22.2316 + 0.0225*S2**2 + ... 3.9636*C2**2 + 1.5682*523**2 + ... 0.0264*C23**2 +3.8938*C2*S23 D22=5.5B07 + 3.8938*S3 D33=1.5946 D12=2.8332*S2 - 0.6738*C23 D13=-0.6738*C23 D23=1.5946 + 1.9469*S3 D lJ2=-3.9411*S2*C2 + ... 1.5418*S23*C23 + ... 1.9469*(C2*C23-S2*S23) D113=1.5418*S23*C23 + ... 1.9469*C2*C23 D123=0.6738*S23 D122=2.8332*C2 + 0.6738*S23 D211=3.9411*S2*C2 - ... 1.5418*S23*C23 + ... 1.9469*(S2*S23-C2*C23) MRAC S i m u l a t i o n o f PUMA 560 D223=1.9469*C3 D233=1.9469*C3 D3 1 1 =-1.5418*S23*C23 - ... 1.9469*C2*C23 D322=-1.9469*C3 « CALCULATE A,B,C " A=2*D112*THV(1)*THV(2) + 2*D113*THV(1)*THV(3) + 2*D123*THV(2)*THV(3) + D122*THV(2)**2 B=D211*THV(1)**2 + 2*D233*THV(2)*THV(3) + D233*THV(3)**2 + D2 C=D311*THV(1)**2 + D322*THV(2)**2 + D3 END S " o f P r o c e d u r a l B l o c k 1 " " P r o c e d u r a l B l o c k 2 : c a l c u l a t i o n o f e f f e c t i v e mass terms " PROCEDURAL(CC1,CC2,CC3=D11,D22,D33,IA1,IA2,IA3) " EMi IS THE E F F E C T I V E MASS. USED IN CONTROL SUBROUTINE " EM(1)=D11+IA1 $ CC1=1./EM(1) EM(2)=D22+IA2 $ CC2=1./EM(2) EM(3)=D33+IA3 $ CC3=1./EM(3) END $ " of P r o c e d u r a l B l o c k 2 " " P r o c e d u r a l B l o c k 3 : c a l l c o n t r o l l e r & f i n d t o r q u e s PROCEDURAL(T1,T2,T3=THDES) CALL CONTROL(TH,THV,THDES,T,KP,KV,TO) " now i n c l u d e g r a v i t y c o m p e n s a t i o n " " IGNORE FOR NOW BECAUSE USING DIFFERENT ALGORITHM " " BUT KEEP IN MIND FOR USE I F I T SEEMS NECESSARY T1=TO(1) T2=TO(2) + D2 T3=TO(3) + D3 T1=T0(1) "$" T2=TO(2) "$" T3=TO(3)" END $ " of p r o c e d u r a l B l o c k 3 " " P r o c e d u r a l B l o c k 4 : f i n d a n g u l a r a c c e l e r a t i o n s " PROCEDURAL(THA=CC1,CC2,CC3,D12,D13,D23,T1,T2,T3,A,B,C) THA(3)=1./(1,-CC3*((D13*CC1*D12-D23)*(1./ ... ( 1 . - ( C C 2 * D 1 2 * C C 1 * D 1 2 ) ) ) . . . *CC2*(DI2*CC1*D13-D23)+D13*CC1*D13))*CC3*(T3-C- . D13*CC1*(T1-A) + (D13*CC1*D12-D23)/ ... (1.-CC2*D12*CC1*D12) *CC2*(T2-B-D12*CC1*(T1-A))) THA(2)=1./(1.-CC2*D12*CC1*D12)*CC2*(T2-B-D12*CC1*(T1-A)+ (D12*CC1*D13-D23) * T H A ( 3 ) ) THA(1)=CC1*(T1-A-D12*THA(2)-D13*THA(3)) END $ " o f P r o c e d u r a l B l o c k 4 " " P r o c e d u r a l B l o c k 5 : u p d a t e p o s i t i o n and v e l o c i t y 128. MRAC S i m u l a t i o n of PUMA 560 PROCEDURAL(TH,THV,X,XV=THA) CALL XFERB(THA,3,THAOLD) $ " p u t t h a i n t o a r r a y t h a o l d " TH(1)=INTEG(THV(1),TH10) TH(2)=INTEG(THV(2),TH20) TH(3)=INTEG(THV(3),TH30) THV(1)=INTEG(THA(1),TH1V0) THV(2)=INTEG(THA(2),TH2V0) THV(3)=INTEG(THA(3),TH3V0) CALL XFERB(TH,3,X) $ CALL XFERB(THV,3,XV) " p u t t h , t h v i n t o a n o t h e r a r r a y t o a v o i d g e t t i n g m i s s i n g d e r i v a t i v e " " e r r o r . " END $ " o f P r o c e d u r a l B l o c k 5 " " f " P r o c e d u r a l B l o c k 6 : up d a t e r e f model " PROCEDURAL(YA,YV,Y=YDES) "Now t h e dynamic e q u a t i o n s of- t h e r e f model " DO L1 1=1,3 YA(I)=WN(I)**2 * YDES(I) - 2*YDAMP(I)*WN(I)*YV(I) ... -WN(I)**2 * Y ( I ) YAOLD(I)=YA(I) L1..CONTINUE YV=INTVC(YA,YVIC) $ CALL XFERB(YV,3,TYV) $ Y =INTVC(TYV,YIC) " REDUNDANCY IS USED TO ACCESS ANGULAR VELOCITY TERMS " END $ " of P r o c e d u r a l B l o c k 6 " n " P r o c e d u r a l B l o c k 7 : u p d a t e U £. W " " Now u p d a t e t h e p o s i t o n and v e l o c i t y g a i n s " " use t h e method d e s c r i b e d by Dubowsky & D e s f o r g e s " " nb. kv,kp a r e not i n o u t p u t l i s t o t h e r w i s e t h i s b l o c k " " would be p o s i t i o n e d i n t h e wrong s p o t ( b e f o r e c o n t r o l l e r ) " PROCEDURAL(W,WV,WA,U,UV,UA=Y,YV,X,XV) DO L2 J=1,3 UA(J ) = W N ( J ) * * 2 * ( - 1 . * Y A O L D ( J ) - 2 . 0 * Y D A M P ( J ) * U V ( J ) - U ( J ) ) WA(J)=WN(J)**2*( -1.*YV(J)-2.0*YDAMP(J)*WV(J)-W(J) ) L2..CONTINUE WV=INTVC(WA,WVIC) $ CALL XFERB(WV,3,TWV) $ W=INTVC(TWV,WIC) UV=INTVC(UA,UVIC) $ CALL XFERB(UV,3,TUV) $ U=INTVC(TUV,UIC) END $ " of P r o c e d u r a l B l o c k 7 " n n " P r o c e d u r a l B l o c k 8 : u p d a t e g a i n s " PROCEDURAL(=Y,X,YV,XV,U,W,UV,WV,UA,WA) TEMP=Q0*( Y ( 1 ) - X ( D ) + Q1*( Y V O ) - X V ( O ) AD0T(1)=TEMP * (Q0*U(1) + Q1*UV(1)) BETAD(1)=TEMP * (Q0*W(1) + Q1*WV(1)) KPDOT(1)=KC * (-1.0*ADOT(1)*KP(1)*WN(1)**2) KVD0T(1)=KC * (KP(1)*BETAD(1) - KV(1)*ADOT(1)*WN(1)**2 ) 129. MRAC S i m u l a t i o n o f PUMA 560 IF(ABS(KPDOT(1)).GT.ESLN) KP(1)=INTEG(KPDOT(1),KP1) IF(ABS(KVDOT(1)).GT.ESLN) KV(1)=INTEG(KVDOT(1),KV1) TEMP=Q0*( Y ( 2 ) - X ( 2 ) ) + Q 1 * ( Y V ( 2 ) - X V ( 2 ) ) ADOT(2)=TEMP * (Q0*U(2) + Q 1*UV(2)) BETAD(2)=TEMP * (Q0*W(2) + Q1*WV(2)) KPDOT(2)=KC*(-2.0*ADOT(2)*KP(2)*WN(2)**2) KVDOT(2)=KC*(KP(2)*BETAD(2) - K V ( 2 ) * ADOT(2)*WN(2)* * 2 ) IF(ABS(KPDOT(2)).GT.ESLN) KP(2)=INTEG(KPDOT(2),KP2) IF(ABS(KVDOT(2)).GT.ESLN) KV(2)=INTEG(KVDOT(2),KV2) TEMP=Q0*( Y ( 3 ) - X ( 3 ) ) + Q 1 * ( Y V ( 3 ) - X V ( 3 ) ) ADOT(3)=TEMP * (Q0*U(3) + Q 1*UV(3)) BETAD(3)=TEMP * (Q0*W(3) + Q1*WV(3)) KPDOT(3)=KC*(- 1.0*ADOT(3)*KP(3)*WN(3)** 2) KVDOT(3)=KC*(KP(3)*BETAD(3) - KV(3)*ADOT(3)*WN(3)**2 ) IF(ABS(KPDOT(3)).GT.ESLN) KP(3)=INTEG(KPDOT(3),KP3) IF(ABS(KVD0T(3)).GT.ESLN) KV(3)=INTEG(KVDOT(3),KV3) END $ " o f P r o c e d u r a l B l o c k B " T1 END $ " OF DERIVATIVE " o u t p u t v a r i a b l e s t o f i l e f o r p l o t t i n g " CALL PLFILE(T,THDG,TH,THV,THA,Y,YV,YA,KP,KV) IF((THV(1).LT.VLOW).AND.(THV(2).LT.VLOW).AND.(THV(3).LT.VLOW)' .AND.(T.GT.1.0)) VFLAG=.TRUE. TERMT (T.GE.TEND) "DEFINE TEND=ENDING TIME" VFLAG=VELOCITY FLAG (WHEN VEL .LT. A SMALL #)" EFLAG=ERROR FLAG " " CONVERT ANGLES FROM RADIANS TO DEGREES FOR EASY INTERPRETATION" THDG(1)=TH(1)* 180./PI $ THDG(2)=TH(2)* 180./PI THDG(3)=TH(3)* 180./PI THVDG(1)=THV(1)*180./PI $ THVDG(2)=THV(2)* 180./PI THVDG(3)=THV(3)*180./PI END $ "OF DYNAMIC" TERMINAL " C l o s e t h e f i l e s opened a t t h e b e g i n n i n g o f t h e program " CALL C S F I L E END $ " o f T e r m i n a l " END $ " OF PROGRAM " 130. C * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * c C CONTROLLER SUBROUTINE TO BE USED BY ACSL IN THE ROBOT C SIMULATION PROGRAM C N.B. T h i s has been ammended f o r model r e f c o n t r o l C C * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * SUBROUTINE CONTROL(TH,THV,THDES,T,KP,KV,TO) REAL T H ( 3 ) , T H V ( 3 ) , T H D E S ( 3 ) , T , T O ( 3 ) , V ( 3 ) , K P ( 3 ) , K V ( 3 ) C C DEFINE VARIABLES: C TH(3) c u r r e n t a n g u l a r p o s i t i o n s ( r a d ) C THV(3) c u r r e n t a n g u l a r v e l o c i t i e s ( r a d / s e c ) C THDES(3) d e s i r e d e n d i n g a n g u l a r p o s i t i o n s ( r a d ) C T t i m e ( s e c ) C T O O ) t o r q u e s p r o d u c e d by t h e a c t u a t o r s (n m) C KP(3) p r o p o r t i o n a l g a i n s ( v o l t s / r a d ) C KV(3) v e l o c i t y g a i n s ( v o l t s e c / r a d ) C V ( 3 ) v o l t a g e s a p p l i e d to' a c t u a t o r s ( v o l t s ) C C t h e c o n v e r s i o n from a p p l i e d v o l t a g e t o p r o d u c e d t o r q u e i s done C by a motor s i m u l a t i o n s u b r o u t i n e C C C D e t e r m i n e a p p l i e d v o l t a g e DO 100 1=1,3 V ( I ) = K P ( I ) * ( T H D E S ( I ) - T H ( I ) ) - K V ( I ) * T H V ( I ) 100 CONTINUE C C FIND TORQUES BY CALLING MOTOR SUBROUTINE CALL MOTOR(V,THV,T,TO) RETURN END C * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * C C MOTOR SIMULATION SUBROUTINE C c SUBROUTINE MOTOR(V,THV,T,TO) REAL V ( 3 ) , T , T H V ( 3 ) , T O ( 3 ) , K M ( 3 ) , B ( 3 ) C C D e f i n e v a r i a b l e s C V ( i ) a p p l i e d v o l t a g e s C T t i m e C T O ( i ) r e s u l t a n t t o r q u e s C T H V ( i ) a n g u l a r v e l o c i t i e s ( r a d / s e c ) C K M ( i ) motor t o r q u e c o n s t a n t (n m / v o l t ) C B ( i ) v i s c o u s damping t e r m s ( v o l t s e c / r a d ) C n o t e t h e s e have a l l been a l t e r e d t o r e f l e c t t h e c o r r e c t C j o i n t a n g u l a r v e l o c i t i e s n o t t h e a r m a t u r e a n g u l a r v e l o c i t y C B(1)=4.5 131. B(2)=4.5 B(3)=4.5 KM(1)=10.0 KM(2)=10.0 KM(3)=10.0 C DO 200 1=1,3 T O ( I ) = K M ( I ) * ( V ( I ) - T H V ( l ) * B ( I ) ) 200 CONTINUE C C t h i s i s o n l y a p r e l i m i n a r y model C a more advance model w i l l i n c l u d e t h e e f f e c t s o f c h a n g i n g C a r m a t u r e r e s i s t a n c e s ( w h i c h i s why t h e v a l u e of t i m e i s C s e n t a s a p a r a m e t e r t o t h i s s u b r o u t i n e ) RETURN END 132. Q***************************************** **************************** C C SUBROUTINE PRGSTP : PROGRAM SETUP C PURPOSE : TO I N I T I A L I Z E VARIABLES USED IN THE C ACSL PROGRAM IN A VERY USER FRIENDLY WAY C Q********************************************************************* c SUBROUTINE PSTUP(TH10,TH20,TH30,TH1VO,TH2V0,TH3V0,TH1DES, . TH2DES,TH3DES,TEND) C C THIS SUBROUTINE IS A INIT I A L I Z A T I O N ROUTINE FOR THE MAIN C PROGRAM. IT SETS UP THE ROBOT I N I T I A L CONDITIONS, THE DESIRED C ENDING CONDITIONS, PRINTOUT INTERVAL, AND MAXIMUM TIME FOR THE C SIMULATION. C C VARIABLES: C C VARIABLE DEFAULT c STARTING POSITION c TH 1 0 { SAME AS THE c TH20 PREVIOUS c TH30 SIMULATION } c STARTING VELOCITY c TH1V0 0.0 DEG/SEC c TH2V0 0.0 DEG/SEC c TH3V0 0.0 DEG/SEC c ENDING POSITION c TH1DES { SAME AS THE c TH2DES PREVIOUS c TH3DES SIMULATION ) c FINISHING TIME c TEND { PREVIOUS TEND } c c THE VALUES ARE STORED IN A F I L E CALLED ROBOT SETUP.DAT c THIS F I L E IS AN 'OLD' F I L E WHICH CAN BE READ AFTER THE c PROGRAM TERMINATES INTEGER YES/*Y'/,syes/'y'/,NO/'N'/,sno/'n'/,HELP/'H'/, . shelp/'h*/,ANSWER REAL TH10,TH20,TH30,TH1V0,TH2V0,TH3V0,TH1DES,TH2DES, TH3DES,TEND,PI/3.141592654/ BYTE ESC DATA ESC/27/ OPEN (UNIT= 9,NAME='ROBOT_SETUP.DAT',TYPE='UNKNOWN' , . FORM='FORMATTED') C ALL WRITE AND READ STATEMENTS TO F I L E ROBOT_SETUP CAN C BE NOW ADDRESSED THROUGH UNIT #9 C C What I have t o do f i r s t i s r e a d i n a l l d a t a f r o m t h e s t o r a g e 133. C f i l e i f t h e u s e r wants t o change any p a r a m e t e r s . Ask t h e C u s e r i f any p a r a m e t e r s need c h a n g i n g . WRITE(6,2) ESC,ESC,ESC,ESC,ESC,ESC,ESC,ESC,ESC,ESC 2 FORMAT(' ',A1,*[2J',A1,'[?61',A1,'#3',A1, . '[4m Robsim ',A1,'[m',/, ' ',A1,'#4',A1, '[4m Robsim ',A1,'[m', . /,' ',A1,'[4m',59X,'By Henry Voss',A1,'[m') WRITE(6,5)ESC 5 FORMAT(' ' ,A1,'[1B',' H e l l o . Welcome t o t h e Robot S i m u l a t i o n Program.' ,//' I f you would l i k e t o s k i p t h e p a r a m e t e r i n i t i a l i z a t i o n ', ' p a r t of t h e program',/' p l e a s e t y p e Y . T h i s w i l l l e a v e t h e ' , ' p a r a m e t e r s a s t h e y were i n t h e l a s t s i m u l a t i o n ' ) READ(9,4) TEND,TH10,TH20,TH30,TH1VO,TH2V0,TH3V0, . TH1DES,TH2DES,TH3DES 4 FORMAT(///,'+',T21,F4.1,//,'+',T14,F6.2,/,'+',T14,F6.2, /,'+',T14,F6.2,//,'+',T14,F6.2,/,'+' ,T14,F6.2,/,'+',T14,F6.2,//,'+',T14,F6.2,/, '+',T14,F6.2,/,'+',T14,F6.2 ) WRITE(6,6) TEND,TH10,TH20,TH30,TH1VO,TH2V0,TH3V0, . TH1DES,TH2DES,TH3DES,ESC,ESC 6 FORMAT(' ',' The o l d v a r i a b l e v a l u e s a r e as f o l l o w s : ' , /,' F i n i s h i n g Time ',F4.1,' ( s e c o n d s ) ' , /,' S t a r t i n g A n g l e s (deg) ', /, ' T h e t a 1 ' ,F6.2, /,' T h e t a 2 ' ,F6.2, /,' T h e t a 3 ' ,F6.2, /,' S t a r t i n g V e l o c i t i e s ( d e g / s e c ) ' , /,' T h e t a 1 ' ,F6.2, /, ' T h e t a 2 ' ,F6.2, /,' T h e t a 3 ',F6.2,/,' D e s i r e d A n g l e s ( d e g ) ' , /,' T h e t a 1 ' ,F6.2, /,' T h e t a 2 ' ,F6.2, /,' T h e t a 3 ' ,F6.2, /,' ',A1,'[4m P l e a s e t y p e Y o r N ',A1,'[m') READ(5,11) ANSWER IF (ANSWER.EQ.YES.OR.ANSWER.EQ.syes) GO TO 70 WRITE(6,10) ESC,ESC FORMAT(' I f you would l i k e t o s k i p t h e i n t r o d u c t o r y s e c t i o n ' ,/,' ',A1,'[4m P l e a s e t y p e Y f o r y e s { or N f o r no }',Al,'[m') READ(5,11) ANSWER FORMAT(A1) IF(ANSWER.EQ.YES.or.ANSWER.EQ.syes) GO TO 30 IF(ANSWER.NE.NO.AND.ANSWER.NE.sno) GO TO 9 WRITE(6,12) FORMAT(' ','You w i l l be a s k e d t o i n p u t o r a c c e p t a s d e f a u l t ', /' v a l u e s u s e d i n t h e s i m u l a t i o n p r o g r a m . ' / ' A b r i e f ', ' d e s c r i p t i o n o f t h e v a r i a b l e s i s a s f o l l o w s : ' , /'OFINISH TIME FINISHING TIME FOR THE SIMULATION', /* ',12x,' { STARTING TIME = 0.0 SECONDS }', /' I N I T I A L JOINT ANGLES - -THE STARTING ANGLES FOR EACH ROBOT', 9 10 1 1 1 2 134. . /' ',13x,' JOINT. ONLY THE FIRST THREE LINKS', . /' ',13x,' CAN BE SPECIFIED, THE REST ARE SET', . /' ',13x,' AT ZERO BY DEFAULT.', . /' FINAL JOINT ANGLES - THE DESIRED JOINT ANGLES, AGAIN ', . /' ',8x,' THE FIRST THREE LINKS ONLY', . //' DURING INPUT YOU CAN TYPE: "Y" - YES , ACCEPT DEFAULT', . /' ',13x,' "N" - NO INPUT NEW VALUES', . /' ',13x,' "H" - HELP , START INPUT OVER', . //' DURING DATA INPUT PLEASE INPUT IN THE CORRECT FORMAT', . /' EX: n n . n n n MEANS TWO NUMBERS BEFORE THE DECIMAL AND THREE AFTER') 13 WRITE(6,14)ESC,ESC 14 FORMATC ' , A 1 [ 4 m P R E S S Y TO CONTINUE',A1,'[m') READ(5,15) ANSWER 15 FORMAT(AI) IF(ANSWER.NE.YES.AND.ANSWER.NE.syeS) GO TO 13 C 30 CONTINUE TEND = 5.0 WRITE(6,31)ESC,ESC 31 FORMAT{' ','FINISHING TIME { DEFAULT = 5.0 SEC ) ', . /,' ',A1,'[4m DO YOU WANT DEFAULT { Y OR N }',A1,'[m') READ(5,15) ANSWER IF (ANSWER .EQ. HELP.OR.ANSWER.EQ.shelp) GO TO 30 IF (ANSWER .EQ. YES.OR.ANSWER.EQ.syes) GO TO 40 I F (ANSWER .NE. NO.AND.ANSWER.NE.sno) GO TO 30 WRITE(6,32) 32 FORMAT(' ','NEW VALUE FOR FINISHING TIME IN SECONDS . { FORMAT n n . n } ' ) READ(5,33)TEND 33 FORMAT(F4.1) 40 WRITE(6,41)ESC,ESC 41 FORMATC ','STARTING JOINT ANGLES',/'{ DEFAULT = OLD STARTING VALUES FOR ALL', . ' JOINTS }'/,' ',A1'[4m DO YOU WANT DEFAULT { Y OR N }', A1,'[m') WRITE(6,44) TH10,TH20,TH30 READ(5,15)ANSWER IF (ANSWER .EQ. HELP.OR.ANSWER.EQ.she lp) GO TO 30 I F (ANSWER .EQ. YES.OR.ANSWER.EQ.syes) GO TO 45 I F (ANSWER .NE. NO.AND. ANSWER.NE.sno) GO TO 40 WRITE(6,42) 42 FORMAT(' ',*INPUT DESIRED STARTING JOINT ANGLES IN . DEGREES ONE ENTRY AT . A TIME *,/' STARTING WITH JOINT 1, ENDING WITH JOINT 3 { FORMAT n n n . n n } ' ) READ(5,43) TH10,TH20,TH30 43 FORMAT(F6.2) 44 FORMAT(' OLD VALUES:',/' THETA 1 ',F6.2, . /' THETA 2 ',F6.2,/' THETA 3 ',F6.2) C NOW VELOCITIES * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 45 CONTINUE 135. TH1V0=0. TH2V0=0. TH3V0=0. WRITE(6,46)ESC,ESC 46 FORMAT(' THE DEFAULT STARTING VALUES FOR THE ANGULAR ', . 'VELOCITIES ARE AT 0.0 DEG/SEC',/' WOULD YOU L I K E TO USE ', . 'THE DEFAULT VALUES',A1,'[4m { Type Y f o r y e s }',A1,'[m') READ(5,15)ANSWER IF(ANSWER.EQ.YES.OR.ANSWER.EQ.syes) GO TO 50 IF(ANSWER.EQ.HELP.OR.ANSWER.EQ.shelp) GO TO 30 IF(ANSWER.NE.NO.AND.ANSWER.NE.sno) GO TO 45 WRITE(6,47) 47 FORMAT(' P l e a s e i n p u t new s t a r t i n g a n g u l a r v e l o c i t i e s ' , 'one a t a time s t a r t i n g w i t h j o i n t 1:') READ(5,43) TH1VO,TH2V0,TH3V0 50 WRITE(6,51) 51 FORMAT(' ','THE OLD ENDING JOINT VALUES IN DEGREES ARE . AS FOLLOWS:') 52 FORMAT(F6.2) 1=1 WRITE(6,5 4)I,TH1DES 1 = 2 WRITE(6,54)I,TH2DES 1 = 3 WRITE(6,54)1,TH3DES 54 FORMAT(' ',' JOINT ',11,' *,F6.2) 100 CONTINUE WRITE(6,55)ESC,ESC 55 FORMAT(' ','DO YOU WISH TO KEEP THESE VALUES ',A1,'[4m . Y OR N } ?',A1,'[m') READ(5,15) ANSWER IF (ANSWER .EQ. HELP.OR.ANSWER.EQ.shelp) GO TO 30 IF (ANSWER .EQ.YES .OR.ANSWER.EQ.syes) GO TO 62 IF (ANSWER .NE. NO.AND. ANSWER.NE.sno ) GO TO 100 WRITE(6,56) 56 FORMATC ','PLEASE INPUT NEW ENDING JOINT ANGLES STARTING . WITH JOINT 1', . /' AND ENDING WITH JOINT 3 { FORMAT nnn.nn } i n d e g r e e s ') WRITE(6,57) 57 FORMATC DESIRED THETA 1 ') READ(5,52) TH1DES WRITE(6,59) 59 FORMATC DESIRED THETA 2 ') READ(5,52) TH2DES WRITE(6,61) 61 FORMATC DESIRED THETA 3 ') READ(5,52) TH3DES C*********************************************************** C Now r e w r i t e t h e p a r a m e t e r s t o t h e d a t a s t o r a g e f i l e C I f I do t h i s h e r e i t w i l l s i m p l y t a c k i t o n t o t h e end o f t h e C c u r r e n t f i l e so I w i l l c l o s e t h e f i l e t h e n open i t a g a i n 62 CONTINUE c l o s e ( u n i t = 9) 136. OPEN(UNIT=9,NAME='ROBOT_SETUP.DAT',TYPE='UNKNOWN', . FORM='FORMATTED') WRITE(9,65) TEND,TH10,TH20,TH30,TH1V0,TH2V0,TH3V0 . ,TH1DES,TH2DES,TH3DES 65 FORMATC ' , 6 0 ( ' * ' ) , / T 4 , ' R o b o t S i m u l a t i o n P a r a m e t e r s ' , . / 6 0 ( ' * ' ) , / ' 0 F i n i s h i n g t i m e = ' , F 4 . l , / ' ' ,' S t a r t i n g J o i n t A n g l e s ( i n D e g r e e s ) ' , . /' T h e t a 1 = ',F6.2,/' T h e t a 2 = ',F6.2, . /' T h e t a 3 = ',F6.2,/'0', ' S t a r t i n g j o i n t v e l o c i t i e s ( D e g / s e c ) ' , . /' T h e t a 1 = ',F6.2,/' T h e t a 2 = ',F6.2, . /' T h e t a 3 = ',F6.2,/'0', ' D e s i r e d E n d i n g J o i n t A n g l e s ' , . /' T h e t a 1 = ',F6.2,/* T h e t a 2 = \ F 6 . 2 , . /' T h e t a 3 = ' ,F6.2) 70 CONTINUE TH10=TH10*PI/180. TH20=TH20*PI/1B0. TH30=TH30*PI/1B0. TH1VO=TH1VO*PI/180. TH2V0=TH2V0*PI/180. TH3V0=TH3V0*PI/180. TH1DES=TH1DES*PI/1B0. TH2DES=TH2DES*PI/180. TH3DES=TH3DES*PI/I 80. CLOSE(UNIT=9) RETURN END 137. C-C C c c-c c c c c c-c c c c c c c c c c c c c c c c c c c c c c-c c THIS IS THE MODEL REFERENCE GAIN UPDATE SUBROUTINE PURPOSE: TO UPDATE THE PROPORTIONAL AND VELOCITY GAINS BY MINIMIZING A QUADRATIC ERROR FUNCTION BY USING A STEEPEST DESCENT METHOD SUBROUTINE GAIN_UPDATE(Y,YV,YA,TH,THV,THA,THDES,KP,KV,EM) REAL Y ( 3 ) , Y V ( 3 ) , Y A ( 3 ) , T H ( 3 ) , T H V ( 3 ) , T H A ( 3 ) , K P ( 3 ) , K V ( 3 ) , K , . E M ( 3 ) , Q 0 ( 3 ) , Q 1 ( 3 ) , F ( 3 ) , F E ( 3 ) , F E D O T ( 3 ) , D F E A ( 3 ) , D F E D O T A ( 3 ) , . D F E B ( 3 ) , D F E D O T B ( 3 ) , A L P H A ( 3 ) , B E T A ( 3 ) , K M ( 3 ) , T H D E S ( 3 ) , D A ( 3 ) , D B ( 3 ) , . DKP(3),DKV(3),MIN INTEGER FLAG(3) FLAG(1)=0 FLAG(2)=0 FLAG(3)=0 DEFINE VARIABLES: Y,YV,YA TH,THV,THA THDES KP,KV DKP,DKV EMi Q0,Q1 F FE,FEDOT DFEA, DFEDOTA DFEB, DFEDOTB ALPHA,BETA KM DA ,DB MODEL POSITION,VELOCITY,ACCELERATION MANIPULATOR POSITION,VELOCITY,ACCELERATION DESIRED POSITION PROPORTIONAL AND VELOCITY GAINS INCREMENTAL CHANGES IN GAINS EFFECTIVE MASS LINK i WEIGHTING COEFFICIENTS IN COST FUNCTION COST FUNCTION DERIVATIVES OF COST FUNCTION WITH RESPECT TO POSTION ERROR, VELOCITY ERROR DERIVATIVE OF ABOVE WITH RESPECT TO ALPHA DERIVATIVE OF ABOVE WITH RESPECT TO BETA SYMBOLIC VARIABLES FOR TERMS IN MANIPULATOR DYNAMIC EQUATION ACTUATOR TORQUE CONSTANT INCREMENTAL CHANGES IN ALPHA,BETA { TO MINIMIZE THE COST FUNCTION } INCREMENTAL STEP FRACTION ( 0 < K < 1 ) MULTIPLIES THE LINEARIZED STEP DFE.DFEDOT TO ACHIEVE MINIMUM COST FUNCTION F I N I T I A L I Z E VARIABLES K = 0.5 MIN = 0.005 KM(1)=10. KM(2)=10. KM(3)=10. DO 20 1=1,3 Q0(I)=0.707 138. QI (I)=0.202 C A L P H A ( I ) = ( E M ( I ) / K M ( l ) ) / K P ( l ) C B E T A ( I ) = K V ( I ) / K P ( I ) 20 CONTINUE C C C EVALUATE DERIVATIVES OF COST FUNCTION C DO 50 1=1,3 C F E ( I ) = Q 0 ( I ) * * 2 * ( Y ( I ) - T H ( I ) ) + Q 0 ( I ) * Q 1 < I ) * ( Y V ( I ) - T H V ( I ) ) F E D O T ( I ) = Q 0 ( I ) * Q 1 ( I ) * ( Y ( I ) - T H ( I ) ) + Q 1 ( I ) * * 2 * ( Y V ( I ) - T H V ( I ) ) 50 CONTINUE C C I F F E AND FEDOT ARE BOTH CLOSE TO ZERO THEN RETURN C Must f l a g w h i c h g a i n s do not need u p d a t i n g by l o o k i n g a t t h e C D e r i v a t i v e v a l u e s DO 60 1=1,3 I F ( F E ( I ) . L T . M I N .AND. FEDOT(I).LT.MIN) FLAG(I)=1 60 CONTINUE C THIS SHOULDN'T CHANGE THE VALUES OF KP,KV c DO 100 1=1,3 C IF ( F L A G ( I ) . E Q . 1 ) GO TO 80 C s k i p p a r a m e t e r u p d a t e i f f e , f e d o t a r e s m a l l C D F E A U )=Q0U )**2 * THA(I ) + Q0 (I )*Q1 (I )*THA(I )/BETA(I ) D F E D O T A ( I ) = Q 0 ( I ) * Q 1 ( I ) * T H A ( I ) + Q 1 ( I ) * * 2 * T H A ( I ) / B E T A ( I ) D F E B ( I ) = Q 0 ( I ) * * 2 * T H V ( I ) + Q 0 ( l ) * Q 1 ( I ) / B E T A ( I ) * * 2 * (THDES(I) - T H ( I ) - A L P H A ( I ) * T H A ( I ) ) D F E D O T B d ) = Q 0 ( I ) * Q 1 ( l ) * T H V ( I ) + Q 1 ( 1 ) * * 2 / B E T A ( I ) * * 2 * (THDES(I) - T H ( I ) - A L P H A ( I ) * T H A ( I ) ) C C C C FIND INCREMENTAL CHANGE IN ALPHA, BETA NEEDED TO MINIMIZE COST C FUNCTION DA ( I ) = 1 . / ( 1. - DF E A ( I ) / D F E D O T A ( I ) ) * . ( K * F E ( I ) * D F E D O T B ( I ) / D F E D O T A ( I ) / D F E B ( I ) - K*FEDOT(I)/DFEDOTA(I)) D B ( I ) = 1 . / D F E B ( I ) * ( - 1 . * K * F E ( I ) - D F E A ( I ) * D A ( I ) ' ) C DKP(I)= - 1 . * D A ( I ) * K M ( I ) * K P ( I ) * * 2 / E M ( I ) DKV(I)= D B ( I ) * K P ( I ) + K V ( I ) / K P ( I ) * DKP(I) C K P ( I ) = K P ( I ) + DKP(I) K V ( I ) = K V ( I ) + DKV(I) C 80 CONTINUE CONTINUE RETURN END C SUBROUTINE P L F I L E C c C P u r p o s e : t o w r i t e v a r i a b l e s u s e d i n Robsim t o o u t p u t f i l e s C so t h a t t h e y can be p l o t t e d by T e l a g r a f C SUBROUTINE PLFILE(T,THDG,TH,THV,THA,Y,YV,YA,KP,KV) REAL T , T H D G ( 3 ) , T H { 3 ) , T H V ( 3 ) , T H A ( 3 ) , Y ( 3 ) , Y V ( 3 ) , Y A ( 3 ) , . K P ( 3 ) , K V ( 3 ) C WRITE(12,*)T,THDG(1),THDG(2),THDG(3) W R I T E ( 1 3 , * ) T , T H ( 1 ) , T H ( 2 ) , T H ( 3 ) , Y ( 1 ) , Y ( 2 ) , Y ( 3 ) W R I T E ( 1 4 , * ) T , T H V ( 1 ) , T H V ( 2 ) , T H V ( 3 ) , Y V ( 1 ) , Y V ( 2 ) , Y V ( 3 ) W R I T E ( 1 5 , * ) T , T H A ( 1 ) , T H A ( 2 ) , T H A ( 3 ) , Y A ( 1 ) , Y A ( 2 ) , Y A ( 3 ) W R I T E ( 1 6 , * ) T , K P ( 1 ) , K P ( 2 ) , K P ( 3 ) , K V ( 1 ) , K V ( 2 ) , K V ( 3 ) RETURN . END SUBROUTINE O P F I L E ( ) OPEN(12,FILE='PF1.DAT',STATUS='unknown') 0PEN(13,FILE='PF2.DAT',STATUS='unknown') OPEN(14,FILE='PF3.DAT*,STATUS='unknown') OPEN(15,FILE='PF4.DAT',STATUS='unknown') OPEN(16,FILE='PF5.DAT',STATUS='unknown') RETURN END SUBROUTINE C S F I L E O CLOSE (12) CLOSE (13) CLOSE (14) CLOSE (15) CLOSE (16) RETURN END