UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Robotic manipulator modelling and control Shi, Pingnan 1987

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

Item Metadata

Download

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

Full Text

ROBOTIC MANIPULATOR MODELLING AND CONTROL by PINGNAN SHI A THESIS SUBMITTED IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES THE DEPARTMENT OF ELECTRICAL ENGINEERING We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA January 1987 ® Pingnan Shi, 1987 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at The University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the Head of my Department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. THE DEPARTMENT OF ELECTRICAL ENGINEERING The University of British Columbia 2075 Wesbrook Place Vancouver, Canada V6T 1W5 Date: January 1987 ABSTRACT In this thesis, a special control strategy is proposed which treats manipulators as a unique class of systems with their own unique characteristics. The objectives of proposing the control strategy are to obtain 1) no or tolerable overshoot; 2) smaller settling time; and 3) higher accuracy. Two two-link manipulators and one three-link manipulator were simulated and controlled by the proposed control strategy. The results are provided in the thesis. The proposed control strategy has the structure of local and global control. The local control takes care of the links. Its objective is to drive the links to the desired positions, disregarding the effects caused by gravity torques and the coupling and the interaction between links. The objective of the global control is to compensate for the effects ignored by the local control. The local control law has the feature of variable structure control. The decision concerning the control to be switched to depends on the region in the the phase-plane the trajectorj' of the subsystem (link) is in. The switching time is determined by the state of the subsystem in the phase-plane via a simple criterion. A simplified model is also proposed and used to construct the control strategy. The model is built up with emphasis on the effective moment of inertia and gravity torques. ii i i i TABLE OF CONTENTS ABSTRACT i i List of Figures vi List of Tables v i i i i Acknowledgements x INTRODUCTION 1 CHAPTER 1 BRIEF REVIEW OF LITERATURE 4 1.1 Conventional Control 5 1.2 Adaptive Control 7 1.3 Optimal Control 9 1.4 Nonlinear Control 12 1.5 Variable Structure Control 14 CHAPTER 2 SIMULATION OF THREE MANIPULATORS 19 CHAPTER 3 MODELLING OF MANIPULATORS FOR THE PURPOSE OF CONTROL 23 iv CHAPTER 4 MODEL TEST WITH CONVENTIONAL CONTROL 32 4.1 Realization of the conventional control 33 4.2 Control of SYSTEM 1 36 4.3 Control of SYSTEM 2 40 4.4 Control of SYSTEM 3 43 4.5 Conclusion of test 47 CHAPTER 5 THE PROPOSED CONTROL STRUCTURE 48 CHAPTER 6 THE PROPOSED CONTROL STRATEGY 57 6.1 The proposed control strategy 61 6.2 Completion of the proposed control strategy .72 6.3 Test of the proposed control strategy 89 CHAPTER 7 CONCLUSION 100 REFERENCE 101 APPENDIX A SIMULATION OF THREE MANIPULATORS 106 A.1 Simulation of SYSTEM 1 107 A.2 Simulation of SYSTEM 2 111 A. 3 Simulation of SYSTEM 3 117 APPENDIX B SETTLING TIME AND CHATTERING AREA 122 B. 1 Settling times 122 B.2 Chattering area 124 APPENDIX C SUBROUTINES USED IN THIS THESIS 127 vi List of Figures Page Fig. 1.5.1 The state space 15 Fig. 1.5.2 Structure of the two-joint manipulator 18 Fig. 1.5.3 System's response 18 Fig. 1.5.4 A typical control signal 18 Fig. 2.1 Structure of SYSTEM 1 19 Fig. 2.2 Structure of SYSTEM 2 20 Fig. 2.3 Structure of SYSTEM 3 21 Fig. 3.1 Structure of a two-link manipulator 26 Fig. 3.2 The corresponding vector representation 26 Fig. 3.3 Relationship between joints and links 29 Fig. 3.4 30 Fig. 4.1.1 The control block diagram for the ith link 35 Fig. 4.2.1 Structure of SYSTEM 1 37 Fig. 4.2.2 Results of controlling SYSTEM 1 39 Fig. 4.3.1 Structure of SYSTEM 2 40 Fig. 4.3.2 Results of controlling SYSTEM 3 42 Fig. 4.4.1 Structure of SYSTEM 3 44 Fig. 4.4.2 Results of controlling SYSTEM 3 46 Fig. 5.1 The proposed control structure 49 Fig. 5.2 The detailed portion for the ith link 56 Fig. 6.1 Tracking a curve in time domain 58 Fig. 6.2 Overshoot 59 Fig. 6.3 Trajectory of the response in the phase plane 59 Fig. 6.1.1 Trajectories under conventional control 62 v i i Fig. 6.1.2 Trajectories stating from i n i t i a l zero velocity 64 Fig. 6.1.3 65 Fig. 6.1.4 The relationship of t and t 68 Fig. 6.1.5 68 Fig. 6.1.6 The trajectories for the proposed control strategy 71 Fig. 6.2.1 The phase plane 74 Fig. 6.2.2 The trajectory under the P control 75 Fig. 6.2.3 Effect of sampling time and time delay 77 Fig. 6.2.4 79 Fig. 6.2.5 $ as a function of state x 80 Fig. 6.2.6 D and a function of x 81 Fig. 6.2.7 Trajectories in the ideal case 83 Fig. 6.2.8 Manipulator's trajectories for the proposed control strategy 84 Fig. 6.2.9 Time domain response for e (t) 85 Fig. 6.2.10 Structure of the switching subroutine. 86 Fig. 6.2.11 The control block diagram for the ith link 88 Fig. 6.3.1 Results of controlling SYSTEM 1 90 Fig. 6.3.2 Results of controlling SYSTEM 2 92 Fig. 6.3.3 Results of controlling SYSTEM 3 95 Fig. 6.3.4 Following a square wave 97 Fig. 6.3.5 Following a square wave 98 Fig. 6.3.6 Results for conventional control 99 Fig. 6.3.7 Results for the proposed control strategy 99 Fig. A.1.1 Structure of SYSTEM 1 107 Fig. A.2.1 Structure of SYSTEM 2 111 Fig. A.3.1 Structure of SYSTEM 3 117 v i i i Fig. B.2.1 Chattering area 124 v i i i i Table 6.3.1 Results Table 6.3.2 Results Table 6.3.3 Results Table 6.3.4 Results Table 6.3.5 Results Table 6.3.6 Results List of Tables Page for the proposed control strategy 91 for conventional control 91 for the proposed control strategy 93 for conventional control 93 for the proposed control strategy 94 for conventional control 94 X Acknowledgements I wish to thank Dr. E. V. Bohn for his advice and help throughout my work on this thesis. Thanks are also due to Dr. P. D. Lawrence and Dr. R. Ward for helpful discussions with them. 1 INTRODUCTION Manipulator control has attracted the interest of many researchers because of its high potential for improving manufacturing productivity and the working environment. Although so many efforts have been made on this subject, there is s t i l l lack of such a control strategy which considers manipulators as a special class of systems with their own unique characteristics [1], The objective of this thesis is to establish such a control strategy. The d i f f i c u l t y of the manipulator control lies in the complexity of the manipulator systems,which is characterized by its nonlinearity of the dynamics and by the interaction and the coupling between links. The goal of manipulator control is to drive the links of a manipulator to the desired position without overshoot or with an overshoot which is below a tolerence level. In the literature, there are about two approaches to deal with the problem of manipulator control. One approach considers the manipulator as a group of second order linear subsystems (with or without some nonlinear terms), and adopt an existing control scheme for i t [2,3,16,19,21,25]. The other approach approximates the exact dynamic equation of the manipulator and obtain the required torques from the approximated model [6,27,29]. The advantage of the f i r s t approach is that the control algorithms are easy to implement. The disadvantage is that the modelling errors are very large. This causes high requirements on actuators, low speed, and low accuracy. The advantage of the 2 second approach is that the modelling errors are small, thus high accuracy can be achieved. The disadvantage is that the control algorithms are very complicated, thus d i f f i c u l t and expensive to implement. In this thesis, the ideas of the two approaches are employed, and a trade-off between the accuracy of the modelling and simplicity of the control algorithm is introduced. An approximate model was established which takes into consideration the uniqueness of manipulator systems. This enables the model to be used in a general sense. Based on the model, a control structure was proposed, which splits the whole control problem into global control and local control problems. The local control deals with a decoupled and uninteracted system, which makes the control synthesizing simple and general. The global control couples the local control signals and inputs feedforward terms according to the physical structure of the manipulator. This compensates for the coupling and the interaction effects between the links and other terms ignored in the local control synthesizing. A control strategy based on the proposed control structure was developed which forces the manipulator system • to possess a desired dynamics by forcing i t s trajectory to 'slide' on a sliding surface. The control strategy is in a form of nonlinear PD control, i t s gains vary according to the state of the system. The contributions of this thesis are: 1) a useful model for control synthesizing was established and a simple way to calculate the effective moments of inertia and a method to approximate the effective moments of inertia were proposed; 2) A 3 unique contro l s tructure was proposed. This enables a systematic design of a contro l strategy and enables such a contro l strategy to be e a s i l y modified to d i f f eren t manipulators; 3) the contro l strategy developed gives the manipulator a spec i f i ed response without overshoot or with a s i g n i f i c a n t l y reduced overshoot compared with conventional c o n t r o l . 4 CHAPTER 1 BRIEF REVIEW OF THE LITERATURE In the past decade many control schemes on robotic manipulators have been proposed and some of them have found applications in practice. These control schemes can be classified into the following classes, 1) Conventional control; 2) Adaptive control; 3) Optimal control; 4) Nonlinear control; 5) Variable structure control; 6) Dynamic Control. The f i r s t five control schemes are essentially feedback control while 6) is a open loop control. Dynamic control needs very accurate modelling. It is very sensitive to any disturbance either caused by modelling error or external noise. Some ideas of the f i r s t five control schemes are employed in developing the proposed control strategy. A brief knowledge of them is helpful for the understanding of the proposed control strategy. Therefore, they are further described in the following sections. The classification used here is based on essential characteristics of the existing control schemes. A well developed control scheme may have features of some of the control schemes classified here. 5 1.1 Conventional control In the literature, conventional control means using PID control to control a manipulator. The advantages of this control scheme are i t s ease of implementation and its robustness in the presence of disturbances and system's dynamic uncertainty. PID control has been well used in practice for many years and in many other areas, the techniques are well developed. It is natural to apply PID control to manipulators. The PID'control have attracted the interest of many researchers [2,21,24]. As matter of fact, in some tutorial materials [16,19,25], only PID control is mentioned. One approach for conventional control is to assume that each link (subsystem) of a manipulator is a second order linear system plus a nonlinear term, Ti = JtfJ'ei + Beffji + f(6,6) (1.1-1) where Jeff. is the effective moment of inertia of the ith link, B ^ j is the effective damping coefficient , and T; is the torque on the ith link. f(6,6) groups the gravity torque, centrifugal and c o r i o l i s torques, and other interaction terms. The control law is (1.1-2) where B a^ f is the estimated value of B^ r . , and ~&i > *s called tracking error and B^x is the desired angle for the ith 6 link. The effect of the term t{9,8) is either neglected or compensated for by some feedforward terms [24,2,21,16,2]. Applying the control law (1.1-2) to the system (1.1-1), we can get the following standard equation upon the assumption that a l l estimates are correct and the effect of the term t{6,6) is compensated for, V, + 2f,-w,-£,- + w, 2 £, = o (1.1-3) where u*=KPi/Jt!U 2f,w, = KVi/Jtffi hence Kpi^J.urf (1.1-4) KVi = 2JC]}. c,-u,i (1 .1 - 5) where u- and are the natural frequency and the damping ratio respectively, and are specified according to the requirement of performance. $. is usually set to 1.0 ( c r i t i c a l damping), which causes the system to have the fastest no-overshoot response. Conventional control is very simple and the control effect can be visualized by the natural frequency u- and the damping ratio S ; and most important i t is very robust. It has been widely used in practice in other areas. For conventional control, the d i f f i c u l t y is that the nonlinearity, interaction, and gravity torque may cause the control to require high gains Kv an Kp, which would raise the requirement for the driving devices. 7 1.2 Adaptive control Because of the complexity and uncertainty of manipulators, some researchers suggested using adaptive control. The basic idea is to set up a model with unfixed parameters, these parameters are estimated during the control process, and then the model is used for refining the control law [ 3 ,4 ,8 ] . For adaptive control, usually an autoregressive model is used, which can be written in the following form Y{k) = A(q-1)Y(k) + B(g-1)U(k-d) + H+E(k) (1.2-1) where the sampling period is omitted in the argument. The argument k refers to the sampling instant, k=0,1,2,..., d is a positive integer specifying the time delay as an integer multiple of the sampling period. The constant m-dimensional vector H is a forcing term, which includes the effects of the gravitational forces. The ith components of the m-dimensional output vector Y and the input U are the output y and the input u of the joint I respectively, and i=1,,..,m. The equation error E(.) represents a random, zero-mean, Gaussian white noise with the covariance R. The argument q~ 1 is a backward shift operator, i.e., q~1Y(k)=Y(K-1). The MxM matrices A and B are polynomials defined by A(q~1) = A1q-1 + ... + Anq~n B(q-1) = B1q-1 + ... + B„ g -» 8 where n is a positive integer specifying the order of the model. B 0 is nonsingular, and det(B(q~ 1)) has a l l zeros s t r i c t l y outside the unit c i r c l e . The parameters A1r...An and B0,...Bn can be estimated by using Least Square or Maximum Likelihood or other estimation methods. Among those methods, the Least Square is the most popular one due to its simplicity. The control law is based on either a chosen performance, which is to be minimized or maximized, or a chosen closed-loop characteristic. One of the most popular adaptive control schemes is based on pole placement. This scheme can give a feeling about the system in terms of the pole location of the closed-loop system, which, in the case of second order systems, can be further interpreted to the well-known natural frequency co and the damping ratio Gary G. Leiningger in his paper [3] proposed a control scheme using the pole placement method, which is proposed by Wellstead [4] , Adaptive control has the advantage of modifying the model on line. However, for the existing approaches on robotics, the models chosen are not appropriate. They actually treat a manipulator as a general unknown system without benefitting from the intensive studies on the dynamics of manipulator systems. Since we do know something about manipulators, for instance, moments of inertia, i t just doesn't make sense to try to estimate everything. It is actually an abuse of the adaptive control. However, their advantage of adjusting the control according to the environment shouldn't be overlooked. 9 1.3 O p t i m a l c o n t r o l Due t o t h e c o m p l e x i t y of the dynamics of t h e m a n i p u l a t o r , t h e r e a r e o n l y s u b o p t i m a l c o n t r o l schemes d i s c u s s e d i n the l i t e r a t u r e [ 5 , 2 0 ] . M. V o k o b r a t o v i c and D. S t o k i c proposed a c o n t r o l scheme [5] which d i v i d e s t h e m a n i p u l a t o r system i n t o s e v e r a l d e c o u p l e d subsystems, and a p p l i e s l o c a l c o n t r o l f o r each subsystem i g n o r i n g the c o u p l i n g e f f e c t s between them and then a g l o b a l c o n t r o l i s i n t r o d u c e d t o reduce the u n d e s i r e d i n f l u e n c e of c o u p l i n g among the subsystems. Suppose t h a t i n t h i s s t a g e of the c o n t r o l s y n t h e s i s , the f o l l o w i n g s t a n d a r d q u a d r a t i c c r i t e r i o n s h o u l d be m i n i m i z e d : where Q 6 R , Q = d i a g ( Q ; ) , R€R , R = d i a g ( r ( ) , Q ;eR '* ', J / i ^ I a r e symmetric n o n n e g a t i v e d e f i n i t e m a t r i c e s , r s ^ R 1 , r r >0 ^iei, FteO, and I i s t h e s e t I = { i | i = 1,2,...,n). Assuming t h a t each of t h e n DOF (Degree Of Freedom) of the system S M i s powered by one a c t u a t o r S,- , i=1,...,n, whose model of d e v i a t i o n from nominal can be w r i t t e n i n the form of l i n e a r systems ( e x c e p t f o r the n o n l i n e a r i t y of the i n p u t a m p l i t u d e s a t u r a t i o n t y p e ) , Si: Xt = AiXi + biN(t, u.) + /,AP<, X,(0) ~given,4fi€l ( 1 . 3-2 ) (1.3-1) where X ;^R n' i s t h e v e c t o r of t h e d e v i a t i o n of t h e a c t u a t o r S; 10 model state from the nominal, n ; is the order of the actuator S; model, U',£PJ is deviation of the input from the nominal control (scalor input to actuator), A-(eRn''xn; b,£Rn:, and f;eRn'. AP ;£R 1 is the deviation of the generalizied force from the nominal force acting in the ith DOF, i.e., the load upon the ith actuator, N(t,u-) is the input nonlinearity of the amplitude saturation type. When synthesizing the local control, the following free subsystem S; should be considered (free of coupling f; AP ) sT: X, = AiXi + 6,-JV( *, u,-), X,(0) + given, fie I (1.3-3) If we minimize the criterion (1.3-1) with the constraint (1.3-3), the local control synthesis reduces to the minimization of the criterion WO), «?(*)) = / °° « 2 n'(X, TQ,X, + u\uu\)dt, Vie I (1.3-4) J 0 ~ n for each subsystem Sj (1.3-3) separately, and J= I J; . i = 1 Under the assumption that the pair (A*,b;) is completely controllable and when the input constraint N(t,u;) is not taken into account, the optimal control which minimizes (1.3-4) is in the following form, ^(t)=-r-1b!Kixi(t) (1.3-5) 11 where K,£Rn'*n'is the symmetric positively definite matrix which is the solution of algebraic Riccati equation. If the amplitude constraint on the input N(t,uLj) is taken into account, i t can be shown that the exponential stability of the subsystem (1.3-3) can be guaranteed. The decentralized control (1.3-5) does not take in account the dynamics of the mechanism S. Thus, a global control is introduced to improve the control. M. Vukobratovic and D. Stokic suggested two different realizations of the global control AP to be considered. 1) Since the coupling AP; among subsystems S; in the robots and manipulators is represented by generalized forces which can be measured directly, i t is possible to introduce force feedback as the global control. In this case the global control is represented by nonlinear feedback with respect to total coupling [6]. 2) The second opportunity to realize the global control is by on-line calculation of the coupling among subsystems. However, since the computation for 2) might be very complex in some cases, by this on-line calculation a l l advantages of the decentralized control w i l l be lost. The criterion of the global control design is that the global 'control should reduce the suboptimality of the control applied [5], but i t should be also chosen so as to preserve the simplicity of the control structure. 12 1.4 Nonlinear control Nonlinear control has some distinct advantages over linear control in accomplishing certain performances [23,32]. C. Samson in his paper [32] analyzed the stability of a large set of control schemes for nonlinear systems whose equations encompass the dynamic equation of any rigi d manipulator. The system is defined by the following equation M(.)X(t)+E(.)X(t)+F(.)X(t)+G(.)=U(.) (1.4-1) where (.)=(X(t),X(t),t), and X(t) is a nx1 column vector. For a manipulator, a possible arrangement (not unique) of the various terms appearing in the equation above is the following: X is a vector of the n joint coordinates. M(.) is the kinetic energy matrix, also called the manipulator inertia matrix. U is the vector grouping the forces and torques produced by the actuators and acting on the manipulator. E(. )=0. F(.)X is the vector grouping the co r i o l i s and centrifugal forces produced during a manipulator movement. G(.) is a composite vector which groups the parasitic forces acting on the manipulator such as the forces of gravity and f r i c t i o n . C. Samson showed that a control law with the following form wi l l ensure the stability of the closed-loop system 13 U(.)=M( .)[-Kp(.)e(t)-Kv(.)e(t)] + +E(.)X(t)+F(.)X(t)+H(t) (1.4-2) as long as M(.), F ( . ) , G(.) and M(.), ' E ( . ) , ? ( . ) , U(.), Kp, and Kv satisfy certain conditions (see [32]) which are normally satisfied in the case of manipulators. Here, e(t)=X(t)-Xd(t), Xd(t) is the desired trajectory. M(.), E ( . ) , and F(.) are the estimated values of M(.), E ( . ) , F(.) respectively. H(.)=M(.)Xd(t)+G(.). The control law in equation (1.4-2) is in the form of PD control, but the gains Kv(.) and Kp(.) are nonlinear functions of of e and e. In the case of manipulators, i f the Kp and Kv are constants and large enough, the local stability of the closed-loop system can be obtained. However, the global stability of the closed-loop system wi l l usually be obtained only if nonlinear gains Kp and Kv are used. Therefore, C. Samson suggested a control scheme that the nonlinear gain Kv become large when needed (for instance, the tracking error become large) and stay small otherwise. 14 1.5. Variable structure control Kar-Keung D. Young in his paper [9] proposed a control scheme based on the Theory of Variable Structure Systems (VSS), which is refered as variable structure control. The Theory of VSS has been developed in the USSR in the last two decades [10]-[13] and has found applications in control of a wide range of processes in the steel, power, chemical, and aerospace industries. The salient feature of VSS is that the so-called sliding mode occurs on a switching surface. While in sliding mode, the system remains insensitive to parameter variations and disturbances. It is this insensitivity property of VSS that encourages i t s adaption for the manipulator control problem. In recent years, the application of VSS to robotics attracted some researchers [9,14,15,17]. Consider a nonlinear system, X=f(X,U) (1.5-1) where X^R is the state vector of the system, U^ R is the control vector. The goal of the control is to bring the system from one state X 0 to another state X, according to a desired dynamics. The desired dynamics is defined by S£{X:s(X)=0}, which divides the whole state space to two p a r t s ^ and>^2 (see Fig. 1.5.1). 15 F i g . 1.5.1 The s t a t e space I f the t h e r e e x i s t s two s e t s of c o n t r o l laws and U 2, where U, d r i v e s the t r a j e c t o r y of the system towards S i n , and U 2 d r i v e s t he system towards S i n then w i t h a p p r o p r i a t e s w i t c h i n g between the two c o n t r o l l a w s , the system w i l l be f o r c e d t o ' s l i d e ' on S towards s t a t e X,, and thus p o s s e s s e s the dynamics d e f i n e d by S. S i s c a l l e d s l i d i n g s u r f a c e . By f o r c i n g the system t o have c e r t a i n dynamics, the d i s t u r b a n c e s accompanying the system a r e r e j e c t e d [ 1 2 ] , In t h e case of m a n i p u l a t o r c o n t r o l , t h e dynamics of a s i x -j o i n t m a n i p u l a t o r can be r e p r e s e n t e d by D(8)8=Q(8,8)+G(8)g+\3 (1.5-2) where Bid) i s a 6x6 symmetric m a t r i x , G ( 0 ) , Q{8,8), U, 8, 8, and 8 a r e 6X1 v e c t o r s . g i s the g r a v i t a t i o n a l c o n s t a n t , 8, 8, 8 denote t h e a n g u l a r p o s i t i o n , v e l o c i t y , and a c c e l e r a t i o n r e s p e c t i v e l y . Assume t h a t t h e i n v e r s e of D(0) e x i s t s and d e f i n e t h e v e c t o r X by 16 X = ( e e 6 ; t ? 0 6 ) = (e 1 f... ,e6 ; v , v 6 ) = (E,V). For a set point regulation problem, the i n i t i a l states P(t 0) and V ( t 0 ) , and the desired position are given. The desired velocity is V =0. A feedback control U(P,V) has to be found such that P(t)-*-Pj and V ( t ) — 0 . In terms of the position error E(t)=P(t)-Pc| , the goal is to nullify the error. Therefore, the following state equations are obtained i \ (1.5-3) - vi = fi{E + Pd) V) + bi(E + Pd)U, i = 1,6 where f ; is ith component of f(P,V)=D"1(P)[Q(P,V)+G(P)g] and b; is the ith row of the matrix B(P)=D - 1(P). Thus the whole state space is decoupled to 6 subspaces, and accordingly the sliding surface is decoupled to six sliding lines. For this problem, let the feedback control U(P,V) be a variable structure control, that is ,^{P,V),ifti{ti, » . )>0 « , ( P , F ) = (1.5-4) ^u-{P,V)fifti{ei,Vi) < 0 where e; , v- are the state variable of (1.5-3) and «.•(*« v^ = titi + Vi,Ci > 0 , i= l, ...,6 (1 .5-5) are the switching planes. The synthesis problem involved is to choose the feedback control ut(P,v) and u^(P,V) so that the sliding mode occurs on the intersection of the switching planes (1 .5-5) . By solving for u- from the algebraic equations s{=0, i = 1 , . . . , 6 , we see that the equivalent control Ue<j exists and is unique, 17 Ue, = -D(P){f{P, V) + CV) (1.5-6) with C=diag(c,,...,c6 ). The the equation of the sliding mode is obtained e, = - c , e , , t = i,...,6 (1.5-7) Equation (1.5-7) represents six uncoupled f i r s t order linear systems, each one of them represents the dynamics of a single degree of freedom when the system is forced into a sliding mode. Furthermore, the dynamics in a sliding mode depends only on parameters c; 's which are the design parameters. The manipulator in a sliding mode is therefore insensitive to interactions between the joints and load variations. Recognizing that -c ;, i=1,...,6 are the eignvalues of (1.5-7), the choice of positive C;'s guarantees asymptotic stability of the system in a sliding mode. The choice of large values of c- 's can effectively speed up the motion in a sliding mode. D. Young selected the hierarchical control method provided in [13] to reduce the conditions on ut and u~, in (1.5-4) to guarantee the existence of a sliding mode on the intersection of the switching planes. He has provided the method in his paper [9]. D. Young has actually applied his algorithm to a two-joint manipulator which is simulated by a hybrid computer. The structure of the manipulator is given in Fig. 1.5.2 and the result is in Fig. 1.5.3 18 F i g . 1.5.4. A t y p i c a l c o n t r o l s i g n a l The d i s a d v a n t a g e s of v a r i a b l e s t r u c t u r e c o n t r o l a r e : 1) the d e t e r m i n a t i o n of t h e s w i t c h i n g t i m e i s d i f f i c u l t ; 2) the t r a j e c t o r i e s j i t t e r on t h e s w i t c h i n g p l a n e s . 19 CHAPTER 2 SIMULATION OF THREE MANIPULATORS In order to verify the proposed model and control strategy,two two-link manipulators and one three-link manipulator are simulated on computer. For the sake of reference, they are called SYSTEM 1, SYSTEM 2, and SYSTEM 3 respectively. The simulation is done by using a simulation program called FORSIM. Further information about this program can be found in UBC document UBC FORSIM. 1) Simulation of SYSTEM 1 The physical structure of SYSTEM 1 is shown in Fig. 2.1. x Fig. 2.1 The physical structure of SYSTEM 1. Where the f i r s t link and the second link are in the same plane. The link variables are 0 1 f 82, and the lengths of the links are d, and d 2. Lumped equivalent masses m1 and m2 are assumed. A subroutine has been written for the simulation program FORSIM to simulate SYSTEM 1. The parameters for the simulation are chosen as m1=m2=1 (lb.) and d,=d2=2 ( f t . ) . Detail of simulation and 20 l i s t of the subroutine are in Section 1, Appendix A. 2) Simulation of SYSTEM 2 The manipulator SYSTEM 2 has the same links as SYSTEM 1, but the physical structure is different. A rough plot of the SYSTEM 2 -is shown in Fig. 2.2. 2-3 Fig. 2.2 A rough plot of SYSTEM 2. Where the f i r s t link is in the XOZ plane and the plane composed by the f i r s t and second links is orthogonal to XOZ plane. The link variables are By, B2, and the lengths of the links are &y and d 2. Lumped equivalent masses m, and m2 are assumed. A subroutine has been written for the simulation program FORSIM to simulate SYSTEM 2. The parameters for the simulation are chosen as m^mjsl (lb) and d!=d2 = 2 ( f t ) . Detail of simulation and l i s t of the subroutine are in Section 2, Appendix A. 3) Simulation of a three link manipulator SYSTEM 3 The model of a three link manipulator is taken from Morgan's paper [14], which consists of three links of a robotic manipulator closest to the base. For the sake of reference, i t is called SYSTEM 3 here. 21 The model and the angles are presented in Fig. 2.3. x X Fig. 2.3 Physical structure of SYSTEM 3. Lumped equivalent masses m, and m2 are assumed, and an additional mo ent of inertia J, is included only for the base of the manipulator. From the base outward, the manipulator consists of a vertical axis or waist and two horizontal axes or shoulder and elbow. The link variables are 9y, 62, and 83, and the lengths of the links are 1, and 1 2 . 22 A subroutine has been written for the simulation program FORSIM to simulate SYSTEM 3. The parameters for the simulation are chosen as J^O.0243 kgm2, m, = 0.624 kg, m2 = 0.782 kg, and 11 = 1 2 = 0.23 (ni). List of the subroutine is provided in Section 3, Appendix A. 23 CHAPTER 3 MODELLING OF MANIPULATORS FOR THE PURPOSE OF CONTROL Modelling here refers to the procedure of finding a simplified representation for the manipulator systems for the purpose of control. In the literature, there are basically two ways to model a manipulator, which distinguishes the two main approaches in robotics. One is to consider i t as a simple linear system or such a system plus a nonlinear term, the other way is to use the detailed dynamic equation. When the model is too simple, the control scheme has to overcome a large modelling error, which causes low accuracy, low speed, and requires high output of the driving devices. When the model is too detailed, the control scheme has to solve complex equations, which makes the control algorithm expensive and d i f f i c u l t to implement, and in some cases may result in low speed since the calculations take a certain time. Therefore, they both limit the performance of a control scheme. For a better performance, the solution is to construct a model which is sophisticated enough to maintain the meaningful characteristics of the manipulator system and yet simple enough to form a control algorithm easily. For the convenience of discussion, certain notations and terms have to be made. A manipulator system is denoted as S, and refered to as the system. The ith link is denoted as S;, and refered to as the ith subsystem. For a n-link manipulator S, according to the Euler-Lagrange equation, the dynamics o f the ith link S* i s , 24 where T- is the torque at the ith link, 8-{ is the angle of the ith link. L=K-P, K and P are the kinetic energy and potential energy of the system respectively. Substitute L=K-P into (3-1), we obtain, T _ ±,dK dK. _ d_dP d_P Since P doesn't contain 6 term, (3-2) becomes, d,dK, dK dP . . r--=*w-^ +^ ( 3 _ 3 ) which contains two parts: the torque caused by kinetic energy which includes centrifugal and coriol i s torques (refered to as kinetic torque), and the torque caused by potential energy (refered to as gravity torque). In other form, (3-3) can be rewritten as, Ti=Tki+TBt (3-4) where, _ d dK dK . . r t ,-*W ' w ; ( 3 _ 5 ) and TK. represents the kinetic torque, and Tp. represents the gravity torque. TK. is a function of the angles and their f i r s t and second order derivatives. Tp; is a function of angles only. To maintain the characteristics of the manipulator system, the model for the ith subsystem should have the same structure as that in (3-4), i.e., TM — + rpM (3- 7) 25 where T. r e p r e s e n t s the i n p u t t o r q u e , T K. r e p r e s e n t s t h e k i n e t i c t o r q u e and Tp. the g r a v i t y t o r q u e used i n the model. When a e x a c t model i s used, Gr o u p i n g up the n e q u a t i o n s g i v e n by ( 3 - 3 ) , f o r the whole m a n i p u l a t o r , the dynamics becomes T = M{6,6)6 + F{6,6)6+G{6) (3-7) where MtR n x f l i s the i n e r t i a m a t r i x , Ft?6R n x i i s the v e c t o r g r o u p i n g the c o r i o l i s and c e n t r i f u g a l t o r q u e s , and G(0)crR n x l i s the v e c t o r g r o u p i n g the g r a v i t y t o r q u e s . The c o u p l i n g i s i n t r o d u c e d t h r o u g h the elements my; ( i = 1,...,n; j=1,...,n; i * j ) of m a t r i x M, and i\j ( i = 1 ,... ,n; j = 1 ,... ,n; i * j ) of m a t r i x F. The above model has t o be s i m p l i f i e d f o r the s i m p l i c i t y of d e s i g n and i m p l e m e n t a t i o n of a c o n t r o l s t r a t e g y . Look a t t h e e q u a t i o n ( 3 - 7 ) , t h e k i n e t i c t o r q u e w i l l go t o z e r o when the system g e t s t o the s t a t i c s t a t e , hence, the o n l y t o r q u e l e f t i s the g r a v i t y t o r q u e . I t ' s c l e a r t h a t i t ' s the g r a v i t y t o r q u e a f f e c t s the p o s i t i o n a c c u r a c y i n t h e s t a t i c s t a t e . The p o t e n t i a l energy P i s q u i t e easy t o o b t a i n , which has the f o r m n P = Y,mighi (3-11) where h; i s t h e d i s t a n c e from the mass c e n t e r of t h e i t h l i n k t o the ground. Suppose we have a t w o - l i n k m a n i p u l a t o r w i t h t h e s t r u c t u r e shown i n F i g . 3.1. 26 Fig. 3.1 The structure of a two-link manipulator where the f i r s t link is in the XOZ plane and the plane composed of the f i r s t and second links is orthogonal to XOZ plane. Look at the corresponding vector representation for the system (Fig.3.2). Fig. 3.2 The corresponding vector representation From Fig. 3.2, we get, d^d^osf^i+d^int^k _~ 1"; — d 2=-(d 2cos0 2)—+d 2sin0 2j a, 27 hence, "r\ =d\ +d 2 =d 1cose 1i+d 1sin0 1k-d 2cost? 1cosc? 2i-d 2sin0 1cosc9 2k+d 2sinc? 2j = (d, -d 2cos0 2 )cos0, i + (d, -d2cost?2) sint? 1 k+d2sint92 j Therefore, h, =d, sine?, h 2 = (d1-d2cosf92)sin©1 and P=m,gh!+m2gh2 = (m 1d 1sint9 1+m 2(d 1-d 2cosc9 2)sine 1)g (3-12) Therefore, To 1 = (m1d1cosf?1+m2 (d 1-d 2cost? 2 Jcost?, )g (3-13) • T 5 2=m 2d 2sin t9 2sin(t9 1 )g (3-14) For a high position accuracy, a highly accurate gravity torque is needed. In addition, Toj can be easily obtained. Therefore, Tg; remains exactly the same as in the Euler-Lagrange equation (3-6) and wil l be used as the feedforward term. Thus, the only simplification we can do is on the kinetic torque. For the ith link, the kinetic torque i s , Ti* = Mi(0, i)t'+ Ft{6,8)6 (3-15) where M? (6,6)^R n i" is the ith row of matrix M, and FifcR™' the ith row of matrix F. The effects of the coriolis and the centrifugal torques are found by some researchers [16,21,28] to be insignificant in the environment of feedback control which is the essential nature of the control strategy we are going to design. Therefore, F ; ( 6 , 6 ) 8 is neglected in modelling. Hence, 28 the k i n e t i c t o r q u e used i n m o d e l l i n g i s , Tg - Mi{6,6)6 3 = 1 (3-16) where m^ j ( i = j ) i s the e f f e c t i v e moments of i n e r t i a , n o r m a l l y denoted as Jeg"; [ 1 6 ] . To o b t a i n c o n s i s t e n t t e r m i n o l o g y , we use the same n o t a t i o n J e ^ . as P a u l ' s . J e ^ . i s composed of the moment of i n e r t i a of the i t h l i n k i t s e l f and the moments of i n e r t i a i n t e r a c t e d from o t h e r l i n k s . They have a r e l a t i o n s h i p a s , J*Jfi =  Jo< + Jint< (3-17) where J o ; r e p r e s e n t s the moment of i n e r t i a of the i t h l i n k i t s e l f , and J ; n t ; r e p r e s e n t s the moment of i n e r t i a r e s u l t i n g from the i n t e r a c t i o n s between o t h e r l i n k s and t h e i t h l i n k . , I*. . T h e r e f o r e , the model we propose (denoted as S ) i s i n the f o l l o w i n g form, J«ff 1 m i 2 • • • m m m2! . M T = m m s *s 9 i g 2 • k / • (3-18) where T M€: R n x i i s the t o r q u e v e c t o r used i n the model, which has a r e l a t i o n s h i p w i t h the a c t u a l t o r q u e T a s , T"=T + E (3-19) where E i s c a l l e d the m o d e l l i n g e r r o r v e c t o r . I n our c a s e , E=-F{6,6)6 (3-20) Note t h a t the m o d e l l i n g e r r o r s a r e z e r o when t h e system i s i n ste a d y s t a t e . T h e r e f o r e , the mode l i n g e r r o r s won't a f f e c t the 29 static accuracy. Jo; can be obtained from equation, Jo, = « * $ (3-21) The problem now is how to get J f n t j • One approach is to get i t from the Euler-Lagrange equation, but to solve this equation is d i f f i c u l t , a simpler method is developed below. On the joint I (see Fig. 3.3), the moments of inertia of the ith link and a l l the sequential links are applied. Fig. 3.3 Relationship between joints and links. If we consider the center of joint I as the center of the interaction moments of inertia of a l l the sequential links applied upon the ith link, the interaction moment of inertia from the jth link to joint I (denoted as J.nt ( i , j)) i s , /wM=n*|XU/)\ 8 (3-21) where d r ( i , j ) is the vector from joint J+1 to the axis of joint I (see Fig. 3.4) 30 / I \ F i g . 3 . 4 And, ./.nt, = Jint(i, i + 1) + - + Jint{i, n) ( 3 - 2 2 ) For example, suppose we have t h e same t w o - l i n k m a n i p u l a t o r as shown i n F i g . 3 . 1 . S i n c e j o i n t 1 l i e s i n the X O Z p l a n e , «C(1,2) = K - ^eo«tf2)eorf1T+ (rfi - d2eo»e2)Bine1k ( 3 " 2 3 ) \t(l,2f= (d1 - d^cosB^2 ( 3 - 2 4 ) T h e r e f o r e , the i n t e r a c t i o n moment of i n e r t i a i s , Jint, =m2(d1- d2cos82f ( 3 - 2 5 ) For t he purpose of c o n t r o l , i f f u r t h e r s i m p l i f i c a t i o n i s needed, because of the c o n s i d e r a t i o n mentioned above, we s h o u l d s i m p l i f y the i n t e r a c t i o n moments of i n e r t i a o n l y . One approach i s t o t a k e t h e average of | d r ( i , j ) | f o r c a l c u l a t i n g t he i n t e r a c t i o n moment of i n e r t i a . A sim p l e a v e r a g i n g i s t o average t h e maximum and minimum v a l u e s of | d r ( i , j ) | i n the f u l l range of a n g l e s c o n c e r n e d . The l o n g e s t d i s t a n c e from j o i n t I t o the end of the j t h l i n k i s , 31 dmaz(i,j) = ma^\dr(i,j)\) = ^ d k (3-26) jt=i and the shortest distance i s , dmin(i,j) = min(\dT{i,j)\)= min[\di 0,<L+!...Qj-i dj\) (3-27) where represents operator + or -. We could use the average of d t T K * x ( i , j ) and dm"m(i,j) in calculating the interaction moment of inertia, i.e., dav = (</mal(l,j) + dmin{i,]))/2 (3-28) where d&v represents the average. The coupling terms m;j( 0,0)0 are also found to be insignificant [26]. Therefore, i f further simplification is required, they can be neglected from the modelling. Thus, the model S becomes, T = 0 Jeft2 0 0 • * 0 2 9i 92 < / v - , 9n (3-29) which is a decoupled system. For the ith subsystem, the decoupled model S i s , (3-30) 32 CHAPTER 4 MODEL TESTING WITH CONVENTIONAL CONTROL To verify the model proposed in the previous chapter, conventional control is designed based on the model and applied to control the three manipulators described in Chapter 2. 33 4.1 R e a l i z a t i o n of c o n v e n t i o n a l c o n t r o l From here on, c o n v e n t i o n a l c o n t r o l r e f e r s t o PD c o n t r o l w i t h damping r a t i o e q u a l t o 1.0 ( c r i t i c a l damping). In c o n v e n t i o n a l c o n t r o l , c e r t a i n f e e d f o r w a r d terms a r e used t o compensate f o r g r a v i t y t o r q u e , and i n some c a s e s , the c e n t r i f u g a l and the c o r i o l i s t o r q u e s [ 2 , 1 6 ] . The model of (3-30) i s used here f o r s y n t h e s i z i n g c o n v e n t i o n a l c o n t r o l , which has the form (4.1-1) The c o n v e n t i o n a l c o n t r o l law i s , KVie,:+ KP.t,:+ Tti (4.1-2) where TT i s the c o n t r o l s i g n a l , e ; =0a; -0 ; and 0d(. i s the d e s i r e d a n g l e . S u b s t i t u t e (4.1-2) t o ( 4 . 1 - 1 ) , we o b t a i n , JtJi\\ + AV, ^ + KPi ^ = 0 (4.1-3) where &.=0 i s assumed. (4.1-3) can be r e w r i t t e n i n the s t a n d a r d form, c" + 2f,W,£, + == o (4.1-4) w i t h 34 KVl.,= 2sW e / / i (4 . 1 - 5 ) KPi=cj*Je„. (4 . 1 - 6 ) where $; and w ; are the damping r a t i o and the natural frequency re spec t ive ly . i s normally set to 1.0 ( c r i t i c a l damping) to get the fastest non-overshoot response. o)-( i s a design parameter, which has an upper l i m i t re la ted to the physical c a p a b i l i t y of the actuator and the system s t r u c t u r a l resonant frequency [2] . Since the c a l c u l a t i o n of gravi ty torques w i l l consume c e r t a i n time, the contro l law is broken into two par t s , T C f C , rrC i — 1 ti T J K with T% — Kvi^i + Kpt€i T ; , can be r e a l i z e d by using an analog c i r c u i t , thus becomes a t y p i c a l continuous PD c o n t r o l . T\2 can be generated by a microprocessor, and used as a feedforward term. The whole process i s shown in F i g . 4 . 1 . 1 35 M i c r o p r o c e s s o r (generate feed-forword term) 0; Link I j Q. 111 Analog c i r c u i t F i g . 4.1.1. The c o n t r o l process The purpose of using a continuous PD c o n t r o l i s to keep c o n t r o l of the system while the d i g i t a l c o n t r o l p a r t i s i n a sampling p e r i o d , to ensure the s t a b i l i t y of the system. c The c a l c u l a t i o n of T; 2 has to be f i n i s h e d w i t h i n the sampling c y c l e . Thus, the sampling time should be g r e a t e r than the time needed f o r the c a l c u l a t i o n . In the s i m u l a t i o n s i n the f o l l o w i n g s e c t i o n s , the sampling time i s set to 20 (ms), which, i n p r a c t i c a l enviroment, should be l a r g e enough f o r the c c a l c u l a t i o n of Tj 2 s i n c e i t mainly c o n s i s t s of s i n e and c o s i n e f u n c t i o n s . In our case, the sampling time can be i n c r e a s e d without l o s i n g the s t a b i l i t y of the system. In one experiment, i t was set to 120 (ms) and the r e s u l t was s t i l l q u i t e good. - E v e n t u a l l y , the c o n t r o l law i s r e a l i z e d as a combination of analog and d i g i t a l c o n t r o l s . Due to the l a c k of hardware, the whole p r o c e s s i s simulated on a computer. The above c o n t r o l law was a p p l i e d t o the t h r e e simulated manipulators d e s c r i b e d i n Chapter 2 to t e s t the v a l i d i t y of the proposed model. 36 4.2 Control of SYSTEM 1 For the purpose of control, we only need to know two things, the effective moment of inertia and the gravity torque of each link. The moment of inertia for link 1 its e l f i s , Jo, = m1d\ = 4.0(rt>/<2) Since the maximum and the minimum distances between joint 2 and joint 0 are, dmat = d! + ds= 4.0(/t) dmin = di - dz = 0.0(ft) the average (dav ) i s , dav = (c/max + dmin)/2.0 = 2.0(/t) the interaction moment of inertia for link 1 i s , Jinu =m2d\v = 4.0(/6/f5) Hence the effective moment of inertia of link 1 i s , J*Sh = Jim, + =4.0 + 4.0= 8.0(/6/^ ) and the effective moment of inertia for link 2 is J,lh = J0t = tn24 = 4.0(/fc/<2) The calculation of the gravity torques is more complicated since they are related to the structure of the system. The structure of SYSTEM 1 is shown in Fig. 4.2.1. 37 Fig. 4.2.1 the structure of SYSTEM 1 The potential energy of each link i s , P,=m,gy, ==—m •, g ,(3,00519, P 2 =ni2gy2 =-m 2g(d 1cosc9 1+d2COs (c9i+c9 2 ) ) P=P,+P2 =-m ,gdiCost9i-ni 2g(diCOSf9,+d 2cos(c9 1+c9 2)) Therefore, the gravity torques are, 3P Tg,=— 369, =migdiSin0i+m2gd,sint9i+m2gd2sin(e, + 0 2) = (m,+m2 )gd ,sint?1+m2gd2sin(c91+02) =(4sin0,+2sin(0,+02))g 3P Tg 2 = 302 =m2gd2sin(0,+02) =(2sin(0,+02))g 38 The natural frequency co\ ( i = 1,2) i s set to 10.0 (rad/sec) , thus the gains are , =160.0 ( f t 2 l b / s e c ) KVt— 2c2w2Jtfh =80.0 ( f t 2 l b / s e c ) KPl = u\JeJJl =800.0 ( f t 2 l b / s e c 2 ) KPt = u\Jcfh =400.0 ( f t 2 l b / s e c 2 ) The des ired angles for both l i n k 1 and l i n k 2 are ir/2. The simulation resu l t s are shown in F i g . 4 .2 .2 . The absolute pos i t i on errors for both l i n k s are less than 0.0002 (rad) . SYSTEM Ts Motion Under The Proposed Control 5-Legend x (i II • (vt«w,t,-0) B ( t , - 0 ) 0.5 I Tracking Errors (a) SYSTEM Ts Motion Under Conventional Control Legend x H A ' D (»,> SYSTEM Ts Motion Under Conventional Control F i g . 4.2.2 40 4.3 Control of SYSTEM 2 Since the difference between SYSTEM 1 and SYSTEM 2 is only the physical structure, we can use the effective moments of inertia obtained in the previous section, thus the feedback gains Kv-( and Kp; {i — 1 ,2) are the same as those in the previous section. However, we have to calculate the gravity torques. The structure of SYSTEM 2 is shown in Fig. 4.3.1. 2-' X 3 Fig. 4.3.1 the structure of SYSTEM 2 The potential energy of the f i r s t link i s , Pi-n»igzi =m, gd, sine?, The potential energy of the second link is more complicated. Since - d~i _ d 2=-d 2cos0 2—+d 2sin0 2j =-d 2cos0 2(d,cos0,i+d 1sine,k)/d,+d 2sin0 2j =-d 2cos0 2cos0,i-d 2cos0 2sin0 yk+d2sin02j 41 d 1+d 2=d 1 c o s t 9 1i+d 1 s i n t 5 1k-d 2cosc92Cos0 1i-d 2 c o s c 9 2 s i n c 9 1k+ d 2 s i n c 9 2 J hence z 2=d 1sim5 1 -d2Cost9 2sinc9 1 therefore, P 2 = m 2 g z 2 =m 2g(d 1sin0 1-d 2cos0 2sin0 1) P=P,+P2 =m,gd,sin0,+m2gd,sin0,-m2gd2cos02sin0, The gravity torques are, 3P Tg 1 =  =m 1gd 1cos0 1 + m 2 g d 1cos0 1 - m 2 g d 2cos0 1cos0 2 =(m,d1+m2(d1-d2cos02))gcos0, =((4 - 2 0 0 5 0 2 ) 0 0 5 0 ,)g 3P Tg2= 302 =m2gd2sin0,sin02 =(2sin0,sin0 2)g The desired angles for both link 1 and link2 are also ir/2. The design parameter to. is set to 10.0 (rad/sec) The simulation results are shown in 4.3.2. The absolute position errors for both links are less than 0.0001 (rad). SYSTEM 2's Motion Under Convent ional Control SYSTEM 2's Motion Under Convent ional Control 2 I • Time F i g . 4.3.2 43 4.4 Control of SYSTEM 3 SYSTEM 3 is taken from Monson's paper. The purposes of doing this are: 1) to see if the proposed model is adequate for a more-than-two-link manipulator, and 2) to obtain a more objective result. By the same procedure as in section 4.2, we can obtain the effective moments of inertia for the three links: =0.0987 (kgm2) =0.0744 (kgm2) JeJJ, — Jot + Jint, =0.0414 (kgm2) The natural frequency (i = 1,2,3) is also set to 10.0 (rad/sec), hence the control parameters are, =1.9740 (kgm2/sec) KPi = u\Jtfh =9.87 (kgm2/sec2) KVt = 2w2Jefh =1.4880 (kgm2/sec) A>, = w| JtJh =7.44 (kgm2/sec2) AV, = 2u>3 JeJh =0.828 (kgm2/sec) Kpt = "fa/ft 44 =4.14 (kgm2/sec2) The structure of SYSTEM 3 is shown in Fig. 4.4.1. x Fig. 4.4.1 the structure of SYSTEM 3 The potential energies are, P 1 =0 P2«migzi =m,gl! sin0 2 P3=m2gz2 =m2g(l,sinc?2 + l 2sinc? 3) P=P1+P2+P3 =m1gl1sint92+m2g(l1sin£?2+l2sine3) Therefore, the gravity torques are, 3P Tg,« = 0 3P T g 2 = — d e 2 -(n^gli+m 2gl, )cosfl 2 «(0.3233cos02)g 45 3P Tg3= 903 =m 2gl 2cos0 3 = 0. 1798COS03 The desired angles for the three links are ir/2. The simulation results are shown in Fig. 4.4.2. The absolute position errors are less than 0.0001 (rad). SYSTEM 3's Motion Under Conventional Control ^ \ 7 Legend «•>) » ) ) • 1 1 ' . ..)) 89 (t»,*w <,-o) . ,-01 . ,-01 - 0 . 5 0 0 .5 1 1.5 2 Tracking Errors (a) SYSTEM 3's Motion Under Conventional Control Legend • <*,> 69 (»,) S SYSTEM 3's Motion Under The Proposed Control Legend x Q (*.) 47 4.5 Conclusion of the testing From the results shown in the previous sections, we can see that the proposed model worked very well for the conventional control. There are overshoots in controlling SYSTEM 1 and 3, which are more due to the insufficiency of conventional control than the proposed model. 48 CHAPTER 5 THE PROPOSED CONTROL STRUCTURE Since we wish to establish a control strategy for a class of manipulators, a control structure specially for manipulators should be established f i r s t . The proper control structure can only be made up based on the unique characteristics of manipulators. The uniqueness of manipulators is that they are basically composed of links joined together. The differences between manipulators are: 1) types of links; 2) the ways in which their links are joined together. Realizing these facts, a local and global control structure seems appropriate. A local and global control structure has been used by other researchers [5]. In their case, the synthesizing of the local control is based on a subsystem free of coupling (see Chapter 1). However, the subsystem is not free of interaction. Therefore, the design of the local control has to account for the effects of other links. If the structure of the system is changed, the local control has to be changed too. Since a manipulator is simply several links joined together, we could design a local control for each link as a separate system. A global control is then introduced to couple the local control signals in the same way as the dynamics of links coupled together. In this way, the local control won't have to change i f the physical structure of the manipulator changes. Note that the tasks we assign to the local and global controls are essentially different from that in [5]. 49 In our case, the objective of the local control is to drive the link to the desired position without having to account for the effects caused by coupling, interaction and gravity torque. The global control collects the information from a l l of the links and generate commands for each link according to the criterion of the performance required. Essentially, the global control should calculate the desired angular displacements for each link and generate the feedforward terms to compensate for the effects ignored by the local control. The control structure is shown in Fig. 5.1. Slob*] c o n t r o l Fig. 5.1 the proposed control structure Consider a n-link manipulator, i t s dynamics i s , Jeff i ^ 1 2 • • • nm V < f i i • • • £ i n g i (e T m2 1 • • • + f 2 i • • * 2 q2(6) • • • • On • * f nn y • • =M{d,6)d+F(e,e)e+G(e) The inertia matrix M can be expressed in the form (5-1) 50 M(0,0)= J o , 0 0 J o n m n i J ;r»t 1 m 12 • • • m m m2 , J i n t (5-2) Then, T, J o , 0 . 0 J o n JinTi m 1 2 m2 , m m i n T n +F0+G = MB(6, 6)6 + Me(6,6)6 + F6 + G(6) (5-3) L e t , s -* ^ U i J o , 0 ... 0 0 Jo, 0n (5-4) which i s a system f r e e of i n t e r a c t i o n and c o u p l i n g , and denoted as S . The subsystem of (5-4) i s denoted as S-, . The d e s i g n of the l o c a l c o n t r o l i s based on t h i s system S , i . e . , T i U i L u 2 • • • T L • u n k s (5-5) where T L6R n i < l i s the v e c t o r g r o u p i n g t h e l o c a l c o n t r o l s i g n a l s . The r e s t of t h e r i g h t hand terms of e q u a t i o n (5-3) s h o u l d be 51 accounted for by the global control. They are given by T S -rpG Jirvr 1 n* i 2 ... m m m 2 1 J i r>T 2 • m J iriT n 65 +F(69,69)t9+G(69) (5-6) 6 nxi where T € R is the vector grouping the global control signals, From equations (5-5) and (5-4), "jo, 0 ... 0 0 0 6: Jo, T2-1/JO, 0 1/Jon T, T2-(5-7) hence, 52 ' i n t i m > 2 . . . m m 2 1 J int: ™0i J i n t n J \rx\ i m y 2 • • • H2 i J i n t 2 e, in 1/Jo, 0 . . . 0 ^ n i J> nT nJ 1° 1 /Jo n J ; I I T I / J O I D I 1 2 / J O 2 ... m i n/Jon 1112,/JO, J*int2/^02 m n i / J o , J i n r n / J O n (5-8) v v Let a- =J'tnT; / J o ; ( i = 1 , . . . , n ) , which i s c a l l e d in terac t ion r a t i o , and 7y=m;j / J o ; ( i = 1 , . . . , n ; j = 1, coupling r a t i o . Therefore, , n ; i ^ j ) , which i s c a l l e d T F F  1 n O l 7 1 2 7 2 1 a 2 • 7 m 7 2 n 7ni + F0 + G(0) = n(J)7,l + /'(^ + ( 5 - 9 ) The t o t a l c o n t r o l i s the sum of l o c a l and g lobal c o n t r o l s , r T = r i + T c (5-10) where T T e R n x i i s the vector grouping the t o t a l contro l s i g n a l s . Both i n t e r a c t i o n r a t i o and coupl ing r a t i o depend on the 53 angles of the links. Their formulas can be obtained from Euler-Lagrange equation or from experimental measurement. The task of a manipulator is that i t ' s end-effector would move to a prescribed position and pick up a load and move i t to another prescribed position. If we could consider the loading at the end-effector as a mass change at the end-effector, the effect of loading to each link could be considered as the change of the interaction ratio and coupling ratio for each link. Therefore, one task of the global control is that i t should update the interaction ratio and the coupling ratio according to the loading. The interaction ratio and coupling ratio may be updated by on-line calculation of a preset formula or adaptively from the input and output information. According to the model of (3-18), the global control i s , a-i 7 i 2 • • • Tin 72 1 0-2 • • • 72n on \ T'l ' g i ( 0 ) s g 2 ( e ) • + • / • • (5-11) If we neglect the effect of coupling, i.e., 7.- =0, the global control i s , TI 1 a, 0 . . . 0 0 a 2 . . • 0 T2-9 i ( 0 ) g 2 ( 0 ) 9n<*> (5-12) Since the local control deals with a simple decoupled and uninteracted system, i t may be realized by simple control 54 schemes, for instance, the PD control. For a f u l l understanding, Let's realize the local control by using a PD control. For the ith subsystem S. , the dynamics is , Jo)\=Tt (5-13) Assuming Bj- =0, Ci=-TtL/J0i (5-14) where e; = Bj.-B is the tracking error. Let v; =e; , the state expression for S; is , £, = V, (5-15) TL The basic goal of the local control t\ is to drive the subsystem in such a way that e; —- 0 and v; 0. For the above subsystem, the PD control law i s , T(L == KVlVi + KPiei (5-16) Apply the control law (5-16) to the subsystem (5-15), (5-17) Kvi Vi = — V i — C i which in the standard form i s , £, = Vi (5-18) Ci = -2f.-w.-u,- - wft,-with, AV. = 2 s W 0 i ' (5-19) KPi=u>Uoi (5-20) The control gains Kv. and Kp(. are only related to the moments of 55 inertia of ith link i t s e l f . The eignvalues of subsystem (5-18) are, = -Sifi -w.^/sf -1 (5-21) *s = -sv* + w.y/sf -1 (5-22) As long as $. and o>; are positive. X, and X2 are negative, hence the system is stable and i t s state (e(,u-) goes to zero asymptot i c a l l y . Therefore, for the whole system, the local control law i s , Kv, 0 ... 0 0 Kv2 0 Kp, 0 0 Kp2 Kvr f 1 ' *> e 1 « 2 (5-23) which f u l f i l l s the basic goal. The detailed portion of the proposed control structure for the ith link is like that shown in Fig. 5.2. 56 -from o)obal control I adjust I r T f + J 7 M I L 6d; link i 69/ 0/ T r oral CorttroL t o ^ l o b f t l c o n t r o l Fig. 5.2 the detailed portion for the ith link The proposed control structure makes i t very easy to design a control strategy systematically. F i r s t , local controls would be constructed for each link neglecting the other links. Then a global control is constructed accounting for the physical structure of the composed manipulator. In addition, the proposed control structure suggests that we could design standard controllers for the links, and those links then could be used to form manipulators with different physical structures. 57 CHAPTER 6 THE PROPOSED CONTROL STRATEGY Based on the control structure we proposed, we have two tasks to accomplish, one is the design of global control and another is the design of local control. Based on the model we proposed, the global control is straightforward. It just simply couple the local control signals and generate the feedforward terms. Therefore, the problem really is how to design the local control. Even though local control only deals with a simple decoupled and uninteracted system, due to the special requirement of manipulator control and the complexity of manipulator systems, a simple PD control is inadequate. Examples are the cases of controlling SYSTEM 1 and SYSTEM 3, even though the damping ratio is set to 1.0, the responses s t i l l have overshoots. The typical and most general task for manipulators is to track a prescribed trajectory in space. Correspondingly, the angle and/or angular velocity of each link would have to track prescribed curves in time domain. The curve is input at each setpoint, and the angle would follow the curve in the way depicted in Fig. 6.1. 58 p r e s c r i b e d c u r v e a c t u a l response Fig. 6.1 Tracking a curve in time domain Therefore, trajectory tracking can be broken to a problem that the angle and/or angular velocity of each link reaches a desired position within a specified f i n i t e time (AT), which is actually a position control problem. However, one thing should be kept in mind that the starting angular velocity at the setpoint is not necessarily zero. Specially for manipulators, the angle is required not only to reach the desired position but also not to excess the desired position, i.e., no overshoot. Overshoots of the links of a manipulator may cause the end-effector to hit the object to be picked up, either damage the object or damage i t s e l f . Therefore, for manipulator control, i t is important to avoid overshoot or make overshoot below a tolerence level. A typical case of overshoot is shown in Fig. 6 . 2 . 59 overshoot / /ill Ik / / 0 i F i g . 6.2 Overshoot In the phase p l a n e of the t r a c k i n g e r r o r e ; -dd- -8; , the t r a j e c t o r y of the response shown i n F i g . 6.3 i s shown i n F i g . 6.3. X o F i g . 6.3 T r a j e c t o r y of the response i n t h e phase p l a n e e\ a x i s d e v i d e s the phase p l a n e i n t o two r e g i o n s ( { x-:c ;>0}) and ( { x ; : c ; < 0 } ) . To a v o i d o v e r s h o o t , the key t h i n g i s t o make sure the t r a j e c t o r y won't c r o s s t h e e. a x i s t o t h e o t h e r r e g i o n than where i t ' s i n i t i a l s t a t e X 0 i s i n . 60 In summary, th essential requirements of manipulator control i s : 1) to drive the links to the desired position within a predictable finite time; 2) to guarantee there is no overshoot or the overshoot is below the tolerance level. Other requirements are common in control problems, that i s , small settling time, small energy consumption, and high accuracy. There are many control strategies proposed in literature to deal with the f i r s t requirement, but none of them explicitely deals with the second one. In this chapter, we propose a control strategy which satisfies both of the requirements. It is very hard to compare different control schemes simply because i t ' s hard to set up the same experiment enviroment, and some control schemes are expensive to implement. Therefore, in this thesis, we only compare the proposed control strategy with the conventional control both because of its simplicity and practical value. The comparison is carried out in three aspects: 1) overshoot; 2) accuracy; 3) settling time. 61 6.1 The proposed contro l strategy In th i s sec t ion , d iscuss ion is car ied out based on the assumption that the modelling i s accurate, i . e . , no modelling errors and a lso the g lobal contro l compensates per fec t ly the c e n t r i f u g a l , c o r i o l i s and gravi ty torques. Therefore, the l o c a l contro l only deals with a perfect second-order l i n e a r system S as in (5 -4) . For the system above, there are n p a i r s of measurable v a r i a b l e s , i . e . , the angles and angular v e l o c i t i e s of the n l i n k s , ( 8 , , 8 , ) , ( 8 2 , 8 2 ) > • • • > ( 8 n >#n)• Suppose we know the des ired angle and angular v e l o c i t y for each l i n k , ( #c/i , 841 ) 1 ( 8 < j 2 , 8 d 2 ) , . . . , ( 8dn , 8d„ ) , then we can measure n pa i rs of tracking errors (e v y ) , { e 2 , v 2 ) , . . . ( e ,v ) , where ( e ; , u t ) = ,9j. ) - ( 8 i , 8 i ) , V ie { i : i = 1 ,2, . . . , n ) . With these var iab les we can construct a state space which has 2n dimensions, i . e . , X= [*n,*i2; »2i ,«22; *»i,*»2] (6.1-1) where X J ^ C J , x ; 2 = i'; > ^ n(3 X i s the state var iab le of the system. I t ' s obviously very d i f f i c u l t to do ana lys i s in the above state space. The approach taken here i s to p a r t i t i o n (6.1-1) into n subspaces, each of which i s constructed from two var iab le s x ^ and x ; 2 r i . e . , the tracking error e- and i t s f i r s t order d e r i v a t i v e u ; of the i t h l i n k , V i € { i :1,2, . . . , n } which forms a phase plane. Consider conventional contro l in phase plane . The dynamics of the i t h subsystem S-( of system S- (refer to chapter 5) i s , 62 v v, = - y - = u, After applying conventional control, link becomes, r «> = *>•• 1 ^ • 0 2 V, = —iUjVi — w, t, The responses are, ( 6 . 1 - 2 ) the dynamics of the ith ( 6 . 1 - 3 ) , £,(*) = (£,(0) + M O ) + w,-£.(0))Of-Wil ( 6 . 1 - 4 ) Vi{t) = M O ) " "."MO) + ^ . ( 0 ) ) ^ ' * ( 6 . 1 - 5 ) From above equations, we can draw several typical trajectories of the subsystem for different i n i t i a l states (see Fig. 6 . 1 . 1 ) . <>CXi)=0 Fig. 6.1.1 Trajectories under PD control The thick solid line is part of the system's trajectories and also the tangent for the trajectories at the origin," which is defined by equation 0(x- ) = u5 +<J; e; =0, and denoted as *. * and v-t , e; axes divide the phase plane into six regions. They are: 63 A, = :{x- : e - >0,u ; >0}, A 3 = : {x ( : e ; >O,0(x; )<0], A 2=:{x ;:u;<0,0(x-)>0}, A, = : {x; : c ; <0, v{ <0}, A 5 =: {x-, : u ; >0, ^ ( x ; )<0}, and A 6 =: {x ; : e-<0, 0(x; )>0}. Note t h a t i f t r a j e c t o r y of the subsystem g e t s i n t o r e g i o n A 3 or A 6 , an ov e r s h o o t w i l l o c c u r . For t h i s r e a s o n , we c a l l A 3 and A 6 o v e r s h o o t r e g i o n s f o r c o n v e n t i o n a l c o n t r o l of a l i n e a r second o r d e r model. To ensure no o v e r s h o o t , the a b s o l u t e v a l u e of the i n i t i a l v e l o c i t y has t o meet the f o l l o w i n g i n e q u a l i t y t o s t a y out of t h e o v e r s h o o t r e g i o n . I t i s n e c e s s a r y t o s a t i s f y (6.1-6) when the e n d - e f f e c t o r g e t s v e r y c l o s e t o the o b j e c t t o a v o i d impact. One approach i s t o make sure t h a t t h e v e l o c i t y a t the s e t p o i n t ( r e f e r e d as i n i t i a l v e l o c i t y ) i s z e r o when the e n d - e f f e c t o r i s c l o s e s t t o the o b j e c t . C o n s i d e r the s i t u a t i o n when t h e i n i t i a l v e l o c i t y i s z e r o . Some t y p i c a l t r a j e c t o r i e s s t a r t i n g from z e r o i n i t i a l v e l o c i t y i n the phase p l a n e i s shown i n F i g . 6.1.2. v,(0)| <eji\c40)\ (6.1-6) 64 F i g . 6.1.2 T r a j e c t o r y s t a r t i n g from zero i n i t i a l v e l o c i t y The t a r g e t set i s denoted as {x ; : | e-, | <6}. The s e t t l i n g time tf i s d e f i n e d as, \tt{t)\<6 Vte[t:t>t,] (6.1-7) For c o n v e n t i o n a l c o n t r o l , the s e t t l i n g time tj; i s , t = l / n ( l + w.t/) + l / n ( i i ^ ) (6.1-8) For c o n v e n t i o n a l c o n t r o l , the c o n t r o l s i g n a l i s , ^ = -2w,i>j(«) - w?ef{*) (6.2 - 9 ) To ensure no overshoot, the magnitude of the c o n t r o l s i g n a l i s reduced by i n t r o d u c i n g the damping term 2co; ( t ) t o slow down the response of the subsystem, which r e s u l t s i n a l a r g e s e t t l i n g time. To speed up the response of the subsystem without overshoot, we can d r i v e the t r a j e c t o r y t o the tangent l i n e *, and then keep the t r a j e c t o r y ' r i d i n g ' on the l i n e (see F i g . 6 .1 .3 ) 65 Fig. 6.1.3 Any PD control with damping ratio $; less than 1.0 (under damping) can drive the trajectory to The smaller the damping ratio, the faster the response, therefore we choose $.=0.0. The PD control with $; =0 is actually a P control, the control signal i s , m = -*?£.{*), Vz> 6 [*•: U*W*M ^ ° l (6.1-10) To keep the trajectory 'riding' on line the control has to satisfy the condition +(xt) = ti.-H w.-e, = o (6.1-11) Substitute the dynamics equation (6.1-2) in (6.1-11), gives tt. = ~WiV,{t), Vxi e [n: •*(*,•) = 0] (6.1-12) which is a derivative control. Equation "(6.1-12) can be rewritten in the form, U i = -2w,t , ,(t)-W?£,(t), Vz. €[«,•: (^x,-) = 0] ( 6 . 1 - 1 3 ) which is a PD control with damping ratio equals to ' 1.0. Note that the above control strategy is in the form of switching control. Since line • is used to determine the switching, 4> is 66 called switching line, and because the trajectory stays on $>, i t is also called sliding line [9,17]. Since control of (6.1-12) or (6.1-13) keeps the trajectory 'sliding' on the sliding line, we c a l l i t sliding control. The settling time 't; for the control strategy i s , where t, is the time for the trajectory to reach the sliding line, t 2 is the time to hit the target set when the trajectory is on the sliding line (see Fig. 6.1.3). It is easily shown that, tf = h + k (6.1-14) and, 2a,-, u,: 1 6 ' (6.1-15) hence, tf = - — /r»2 + — / n ( - V ) 4CJ, 2u{ UJ, o (6.1-16) 67 Compare with the settling time tj for conventional control, from (6.1-8) and (6.1-16), we get wit: = u>it, + 0.4388 - in(l + w,-t>) (6.1-17) = u/itf + a Therefore, as long as a<0, tr. <tj . For a<0, /n(l + w^/) > 0.4388 > 0.55088 (6. 1 - 1 8 ) which leads, £,(0) > (1 + Wit,) = 1.1186(5 Wjt / = 0.66088 ^ > 1.1186 (6.1-19) e; (0) Normally, 6<0.05e-(0), that i s , > 20.0 » 1.1186, 6 therefore tj<t^ . The relationship of t and t is shown in Fig. 6.1.4. It is seen that the proposed control strategy results in a significant decrease in the settling time. 68 Fig. 6.1.4 The relationship of tf and t f . Suppose the trajectory is in the overshoot region A 3 for conventional control (see Fig. 6.1.5). A t e \ l \ M \\<x O i l * ) , ^ ) ) Fig. 6.1.5 The task is to bring the trajectory to the origin while the trajectory remains in the region A 3. There are many curves that the trajectory can follow towards origin. Suppose we want the trajectory to follow a curve composed of straight lines 1, and 1 2 shown in Fig. 6.1.5. This can be achieved by taking 69 d^l = -Dt = d%<vM (6.1-20) dti €,(0) to follow 1,. d; is the tangent of 1,. When the trajectory reaches the sliding line, the control switches to sliding control defined by, p = -Di = -aji (6.1-21) da to follow 1 2. For the subsystem, its dynamics has to satisfy, ~ = P=Di (6.1-22) e, da where D; is a piecewise constant. That yields D. J0i Vi T!< = DiJ0,Vi (6.1-23) which is actually a derivative control, whose standard form i s , Tt^K^Vi (6.1-24) In our case, Ki.=DiJ0i (6.1-25) In summary, the proposed control strategy in the preliminary stage i s : when the trajectory is in region A 1 fA 2,A», or A S ( a PD control law is used; when the trajectory is in region A 3 or A 6, a derivative control law is used; when the 70 trajectory i s on the s l i d i n g l i n e , the s l i d i n g control i s used, In mathematical form, the proposed control strategy i s , I• KVi vt{t).+ KPt €i(t) X i e A x Uk2 C/A, C/As Tt= J (6. 1-26) with, KVt=2cfiJiJ0i (6.1-27) / 0 tfx,-€ [ r ( : t»,-*(x.-) < 0] ?.• = • (6.1-28) v, 1 tyxi £ [r, : e,u, > 0or(f>(xi) — 0 K P . = u * j 0 i (6.1-29) Kdt^DiJo, (6.1-30) • W l. Vx,- 6 : *(z<) = 0] A = j (6.1-31) L- fa e [x, : £ ^ ( z < ) < 0] The above control serves as the l o c a l c o n t r o l . The proposed control strategy in (6.1-26) i s a switching c o n t r o l , switching between the PD,P,and D controls according to the state of the subsystem in the phase plane. The t r a j e c t o r i e s for the proposed control strategy in the phase plane are shown in F i g . 6.1.6. 71 Fig. 6.1.6 The trajectories for the proposed control strategy Note that the Overshoot region (shaded area) for the proposed control strategy depends on the parameter D-, of the derivative control and can be made significantly smaller than the corresponding region for conventional control. 72 6.2 Completion of the proposed control strategy The control strategy proposed in the previous section is in a preliminary stage in the sense that it doesn't take into account practical problems of implementation. One problem is the presence of modelling errors. Due to the complexity of the manipulators, an exact dynamic model is d i f f i c u l t to obtain. For the design of control algorithms such a dynamic model is neither very useful nor even required. Most control algorithm designs are based on a simplified system model. However, i t is important that the control strategy is robust, that i s , modelling errors should not significantly affect the control performance. The exact dynamics of the ith subsystem has the form, (6.2-1) where e represents the modelling error, which is in the form, n n = E a«C' hi + £ M«- hi (6.2-2) Consider a state space region defined by possible trajectories. The modelling error can be approximated by an appropriate averaging procedure over this region by expression of the form n 53 ao(0, 6)VJ = a,\9,6)vi (6.2-3) 73 X > « ( M f o = ( 6 . 2 - 4 ) 3 = 1 where a-, and b; are bounded, i . e . , |a ; | ^ a M , and |b; | ^ b M . Therefore, e, = o,-u,- + 6,f, ( 6 . 2 - 5 ) Under the conventional c o n t r o l , the dynamics of the subsystem i s , h — ( 6 . 2 - 6 ) tl,- = 0,'f i + 6,-£j — 2u,'Vt- — w2 £, which y i e l d s , €, + (2w,- - a,-)e, + (w? - bi)ei = 0 ( 6 . 2 - 7 ) which i s always stable as long as, ".->! ' ( 6 . 2 - 8 ) w? > ( 6 . 2 - 9 ) However, since the actual damping r a t i o $M i s , fac = r-r^T ( 6 . 2 - 1 0 ) in the case of a-, >0 and b ;<0, S a c " 1 1 ' overshoot occurs . Therefore, for conventional c o n t r o l , i t i s not poss ible to guarantee no overshoot in the presence of modelling e r r o r s . For example, in c o n t r o l l i n g SYSTEM 1 and SYSTEM 3 by conventional c o n t r o l (refer to Chapter 4 ) , the overshoot was caused by modelling e r r o r s . This i s one of the reasons for adopting a non-conventional non- l inear contro l s trategy . For the proposed c o n t r o l s trategy, when the t r a j e c t o r y i s 74 in the region A 2 or A s, a P control is used (see Fig. 6.2.1). •0\ 0 ^ \ ? / A* A> Fig. 6.2.1 The phase plane Under the P control, the dynamics of the subsystem i s , / ii = Vi T (6.2-11) • i. 2 v t>,- = 0 ,-Ui + 0j€,- — w, e, which yields, k', - o.ti, + (w,2 - bi)a = 0 (6.2-12) The roots for (6.2-12) are, X i a=* + V^ZMEE (6.2-13) 2 - 2 When a ? - 4 ( w ? - 6 I - ) < 0 w?>6,.+ ^  (6.2-14) 4 the subsystem has an oscillatory response and the trajectory w i l l reach the sliding line. The cases of a; ^ 0 and a-, <0 are shown in Fig. 6.2.2. 75 (<0 G;>o Cb) Q. ;=0 (c) O.;<0 Fig. 6.2.2 the trajectory under the P control When the trajectory is in region A3 or A 6, according to the proposed control strategy, a derivative control is used. With the derivative control, the dynamics of subsystem i s , ^ ii = -DiVi •+ aiVi + 6,e, The tangent of the trajectory of (6.2-15) is, dvi „ , t . -j1 = -Dt+ Oi+bi-± dti Vi (6.2-15) (6.2-16) In order to reach sliding line, the condition <JVj Vj{t) dti e,<t) (6.2-17) must be satisfied. It follows that, 7 6 Di > -Therefore, i f we chose (6.2-18) our objective can be achieved. The problem with conventional control is that i t requires that the actual damping ratio be exactly 1.0, which is impossible to achieve in the presence of modelling errors. For the proposed control strategy, the only requirement is that the trajectory reach the sliding line. Furthermore, when the trajectory 'rides' on the sliding line, the dynamics is specified by the sliding line only, and is then insensitive to modelling errors [9,17], Therefore, the proposed control strategy is more robust than conventional control. However, since our control is non-linear switching control, the effect caused by the sampling interval and the imperfection of the switching mechanism must be taken into account. The proposed control strategy has the nature of switching control, thus a microprocessor is required to monitor the trajectory in the phase plane and perform the switchings. Because of the use of sampled data, the effect of sampling time should be considered. Furthermore the imperfection of the switching mechanism, especially the time delay should be considered. 77 Because of the sampling time and the time delay, there will be errors in switching, thus the trajectory 'chatters' along the sliding line within a triangular region (see Fig. 6.2.3). (a) Effect of time delay (b) Effect of sampling time Fig. 6.2.3 Effect of sampling time and time delay. In Fig. 6.2.3 the dashed trajectory indicates the effect of time delay, and »-•» indicates trajectory within one sampling interval. The triangular region caused by sampling time is defined by Ase{x; : tf>*tf>f <0} and (x; ) = u(- , <pi (x; )=w; +cof e; . The parameter CJ* and CJ£ can be obtained from equations (6.2-19) wL = oJitg(- + uiiT.) 4 (6.2-20) where T* is the sampling time. The triangular region caused by time delay is defined by Ad€{x; :^£f<0} and ^ (x; ) = u; +w+e- , <t>i(x; ) = i»; +w^e ; . The parameter to+ and can be obtained from 78 equat ions - -—n T .— (6 . 2 - 2 1 ) u>i = uttg(-A + w,Trf) (6.2-22) where is the time delay. The detailed derivation is in the appendix. Chattering is undesirable both in its e l f and in the fact that i t represents a 'high frequency' signal component in the state trajectory, which may excite unmodelled 'high frequency' dynamics [17]. To avoid undesirable 'chattering', one approach is to decrease the sampling time. The second approach is to improve the switching mechanism. The third approach is to design the two sets of control laws on either side of the sliding line in such a way that the 'chattering' is reduced to below a specified tolerence level [17]. The f i r s t two approaches are outside of the interest of this thesis. We concentrate on the third one in the following discussion. One cause of the 'chattering' is the discontinuity of the control function on the sliding line [17], Consider a general case of sliding mode control, which is shown in Fig. 6.2.4 79 t); c a u s e d by u. Fig. 6.2.4 Control law u, moves the trajectory towards sliding line $ in the region A 2 and away from $ in the region A 3 and control law u 2 moves the trajectory towards $ in the region A 3 and away from * in region A 2. Consequently the trajectory 'chatters' on the sliding line. In order to reduce 'chattering', Slotine and Sastry proposed that in a neighborhood of the sliding line the control laws are approximated so that they are continuous in the neighborhood [17], However, since the determination of the neighborhood is related to the time delay , sampling interval and the control laws used, i t is very d i f f i c u l t to determine the neighborhood in a general form. Therefore, we make the control laws continuous in the whole phase plane, thus the d i f f i c u l t y of determining the neighborhood is avoided. In region A 2 , on one boundary (x;£{x-( : u; =0}), the control law is a P control, on the other boundary (x;£{x;:#(xj)=0}) the control law is a PD control with damping ratio $.=1.0. 80 Therefore, we introduce a variable damping ratio, and make the damping ratio continuous in the region A 2. We choose the damping ratio as, * = " ^ T r t (6 .2-23) which has the property that S j ^ l on 0<$j<1 inside A2, and 5j =0 when t>j=0.0. $.( as a function of x j is shown as a solid line in Fig. 6 . 2 . 5 . Fig. 6 . 2 . 5 $ as a function of state x-( Other forms of $. may be chosen as shown by dashed lines in Fig. 6 . 2 . 5 . However, the simplicity of implementation should be taken into account. In region A 3, a derivative control is used. On one boundary fxj6{x; :4>(x\) = 0}), the control parameter is D; =«« . Inside A3, D; is subject to inequality ( 6 . 2 - 1 8 ) . We choose the control parameter D; as A- = w.-(i + ft+ftw.~) (6 .2-24) which is continuous and has the property that =w. on *. D; as 81 a function of state x ; is shown as a solid line in Fig. 6.2.6, Fig. 6.2.6 D; as function of x«, other forms of D; (shown in dashed lines in Fig. 6.2.7) may be chosen as long as they meet the constraints. Parameter D; in (6.2-24) has a upper bound (1+0-)o;, which is used to account for the fact that the actuator has physical restraints limiting the magnitude of the control signal. One concern for parameter 0- is that i t should be large enough to ensure D; >-u-, (t) / e - (t) , that i s , U t i l + 0 i + t o M t ) ) > _ ? t t ) ".73,(1 /?,> -V4{t) ' ' €i(t) Vj UJiti (6.2-25) The upperbound of 0; depends on the physical capability of the a'ctuator. In summary, the proposed control strategy i s , 82 ,KVi v.it) + KPi «,•(*) Vx, S [xi: £t^ (z<) > 0] A/x, € [x,-: £,<^(x,) < 0] (6.2-26) where, Kvt (x.) = 2f,-(ii)w< J 0 i 1 ".-(0 W,'£ , - (<) Vxi € [x, : £,-v,- > 0] tfz,-€[z,-:et-*(zI)<0] (6.2-27) (6.2-28) KPi = up0i (6.2-29) A*(z,-) = I>.-(x,>,. (6.2-30) Z),(x.) = (1 + ft + A " . ^ Vx, € [z,-: £.•*(*.) < 0] (6.2-31) In the case of perfect modelling, no time delay, and infin i t e l y high sampling frequency, the trajectories of the subsystem for the proposed control strategy look like the ones depicted in Fig. 6.2.7 83 F i g . 6.2.7 T r a j e c t o r i e s in the ideal case It can be noticed that the overshoot region (shaded area) of the proposed c o n t r o l strategy is much smaller than that of the conventional c o n t r o l . In r e a l i t y , because of the modelling e r r o r s , the imperfection of the switching mechanism, the sampling time, e t c . , the manipulator's t r a j e c t o r i e s for the proposed c o n t r o l strategy have the form shown in F i g . 6 .2 .8 . 84 Fig. 6.2.8 Manipulator's trajectories for the proposed control The combined effect of the sampling time and time delay results in a triangular fuzzy region. This fuzzy region can be represented by A=: {x -( : <p. <p.£0}. <t>. = v- +CJ_ e- and #4 = v ;+CJ +e ; . The proposed control strategy results in a fast response for the trajectory to enter the region A. The trajectory then remains in A until the target set is reached. The time-domain response is illustrated in Fig 6.2.9. 85 F i g . 6.2.9 Time-domain response for c (t) where t A i s the time when the trajectory enters A. A subroutine has been written for the switching. The switching i s determined by the sign of the product of e. & 0 ; ( x ; ) , and i>; & <t> ( x ; ) . Consider equations (6. 2-29) - (6. 2-33 ), l e t , (6.2-35) thus, (6.2-36) (6.2-37) The structure of the subroutine i s shown in F i g . 6 . 2 . 1 0 86 1 Set up 1 PD C o n t r o l Fig. 6.2.10 The structure of the switching subroutine The l i s t of subroutine is shown below. 87 1 SUBROUTINE PDADAP(ERROR,Dt ?ROR.OMEGA,GAINP,GAINPS,GAINV,GAINVS, 2 1 NLINK) 3 C 4 C S u b r o u t i n e D e s c r i p t i o n : 5 C t h i s s u b r o u t i n e i s t o change the c o n t r o l parameters GAINP 6 C and GAINV a d a p t i v e l y a c c o r d i n g t o system's t r a j e c t o r y i n 7 C phase p l a n e . 8 C 9 c 10 c V a r i a b l e D e s c r i p t i o n : 11 c ERROR: A r r a y of the t r a c k i n g e r r o r s 12 c DERROR: A r r a y of the f i r s t d e r i v a t i v e s of the t r a c k i n g e r r o r s 13 c OMEGA: A r r a y of the d e s i g n parameters 14 c GAINP: A r r a y of the i n i t i a l p o s i t i o n g a i n s 15 c GAINV: A r r a y of the i n i t i a l v e l o c i t y g a i n s 16 c GAINPS: A r r a y of the changed p o s i t i o n g a i n s 17 c GAINVS: A r r a y of the changed v e l o c i t y g a i n s 18 c NLINK: Number of l i n k s 19 c BETA: A r r a y of c o n t r o l parameters f o r D c o n t r o l 20 c 21 IMPLICIT REAL 'S (A-H.O-Z) 22 REAL 'S ERROR(NLINK).DERROR(NLINK) 23 REAL 'S OMEGA(NLINK) 24 REAL 'S GAINP(NLINK).GAINV(NLINK),GAINVS(NLINK),GAINPS(NLINK) 25 DATA BETA/8.DO/ 26 ALPHA=(1.DO+BETA)/2.DO 27 DO 10 I«1.NLINK 28 CURVE=ERROR(I)*DERROR(I)/OMEGA( I ) 29 GAINVS(I)»GAINV(I) 30 GAINPS(I)=GAINP(I) 31 SIGN1=DERROR(I)*CURVE 32 SIGN2«=ERR0R( I )'CURVE 33 IF (SIGN1.GT.O.O) GOTO 20 34 2ETA= 1 .0 35 IF (ERROR(I).EO.C.O) GOTO 30 36 ZETA=-DERROR(I)/OMEGA(I)/ERROR( I ) 37 30 G A I N V S ( I ) e ZE TA *GAINV( I ) 38 GOTO 10 39 20 CONTINUE 40 IF (SIGN2 GE.0.0) GOTO 10 4 1 GAINVS(I)=(ALPHA*GAINV(I)+BETA'GAINP(I)'ERROR(I )/DERROR(I ) 42 GA I N P S ( I ) B 0 . 0 43 10 CONTINUE 44 RETURN 45 END The controller block diagram resulting from the proposed control strategy for the ith link is shown in Fig. 6.2.11. 88 frvw\ global control adjust ' ••/T;- I,T;^,--.T^ 6d: SI T C tc^loboA Control link i 6; loca( control I 6 1 3a. n SWitoh —I Stat* rnon'itor Centre I Fig. 6.2.11 The controller block diagram for the ith link 89 6.3 Test of the proposed control strategy The proposed control strategy has been applied to control SYSTEM 1, SYSTEM 2 and SYSTEM 3. The parameter 0- is chosen as 8.0, and u> • is chosen as 10.0. The global control is in the form as in equation (5-11). Coupling ratios are assumed as 0.0 in a l l the following cases. For SYSTEM 1 and SYSTEM 2, sampling time is 20 (ms) for global control and 10 (ms) for local control. For SYSTEM 3, sampling time is 20 (ms) for both global and local controls. 1 ) Position control a) controlling of SYSTEM 1 The control parameters for the local controls for link 1 and link2 are KPi = «JJ0| = 400.0 K Vi = 2OJ,J0i = 80.0 where i={l,2}. The interaction ratios are, an = 0.0 The gravity torques are (refer to section 4.2, Chapter 4) Tg, = (4«n0x + 2«n(0! + 02))g Tg, = (2«n(0! + Bz))g The desired angles for both links are ir/2. I n i t i a l states are 0,(O)=02(0)=0.0 (rad), and d,(0)=62(0)=0.0 (rad/sec). The results are shown in Fig. 6.3.1 and table 6.3.1. Note that there SYSTEM Ts Motion Under The Proposed Control -_ _ 1 1 1 —1— -0.5 0 0.5 1 1-5 l Tracking Errors ( a ) SYSTEM Ts Motion Under The p roposed Control Time (b) SYSTEM Ts Motion Under The Proposed Control B I — . 0.5 2.5' Time (c) F i g . 6.3.1 91 is no 'chattering'. Table 6.3.1 Results for the proposed control strategy overshoot settling time position error (rad.) link 1 0.0 0.484 (sec.) -0.439514x10"* link2 0.0 0.564 (sec.) 0.179905x10"* The overshoot is measured as ( S i m M - 6 j - )/0</,- , settling time is defined here as the time when the response settles down to within 1% of the steady state value, and position error is measured at time t = 2 ( s e c ) . The results for conventional control are shown in Fig. 4.2.2, and table 6.3.2. Table 6.3.2 Results for conventional control overshoot settling time position error (rad.) link 1 0.10746 1.004 (sec.) -0.205055x10"* link 2 0.044073 0.902 (sec.) -1.64421X10-5 The improvement is significant, b) Controlling of SYSTEM 2 Control parameters are the same as in a) since the links are the same. The only difference is in the gravity torques. They are = ((4 - 2eoa02)eoa61)g Tg, = (2ain61ain62)g The control specification is the same as in a). The results are shown in Fig. 6.3.2, and table 6.3.3. Note that there is no 'chattering' either. SYSTEM 2's Motion Under The Proposed Control 01 Legend a K ) l x !<•.,».)) • ( » I » W I « I - 0 1 ED U^ t^j-O) Tracking Errors (a) SYSTEM 2's Motion Under The proposed Control 2 -Legend • (•_,) SYSTEM 2's Motion Under The Proposed Control 1 0 -8-6-2 -i Legend X ( » , ) 0.5 I 1.5 Time ( c ) 2.5 F i g . 6.3.2 93 Table 6.3.3 Results for the proposed control strategy overshoot settling time position error (rad.) link 1 0.0 0.556 (sec.) 0.83657X10" 8 link2 0.0 0.520 (sec.) 0.615104X10-8 The results for conventional control are shown in Fig. 4.3.2, and table 6.3.4. Table 6.3.4 Results for conventional control overshoot settling time position error (rad.) link 1 0.0 0.748 (sec.) 0.133181x10"6 link 2 0.0 0.608 (sec.) -0.147483X10" 6 The improvement is clear. c) Controlling of SYSTEM 3 The control parameters for the local controls for link 1 and link2 are Kp,^, 2Jo,=2.43 Kp2=cj2 2 J o 2 = 3.3 Kp3=w3 2 J o 3 = 4 . 1 4 Kv1=2w,Jo1=0.486 K V 2 = 2CJ 2 Jo 2 = 0.66 Kv3 = 2a)3Jo3 = 0.828 The interaction ratios are, O i = Jinti J0l = 3.0617 Q 2 = Jmtj Jo, = 1.2545 as = Jint, Jo. = 0.0 The gravity torques are (refer to section 4.4, Chapter 4) =0.0 94 Tg, = O.Z2UgcoB$2 Tg, = 0.1798?co«ff3 The desired angles for a l l the links are TT/2. I n i t i a l states are 6,(0)= 62(0)=63(0)=0.0 (rad), and Si(O)=02(O)=03(0)=0.0 (rad/sec). The results are in Fig. 6.3.3, and table 6.3.5. Table 6.3.5 Results for the proposed control strategy overshoot settling time position error (rad.) link 1 0.0 0.618 (sec.) 0.458547xl0~t link 2 0.0 0.478 (sec.) -0.805364X10"7 link3 0.0 0.472 (sec.) -0.785945X10" 7 Results under conventional control are shown in Fig. 4.4.2, and table 6.3.6. Table 6.3.6 Results for conventional control overshoot settling time position error (rad.) link 1 -1.2143x10 0.774 (sec.) 0.222393X10" 4 link 2 1 .253x10 0.528 (sec.) -0.86681 3x1 0"7 link 3 6.9373x10 0.702 (sec.) 0.564103X10" 7 It is clear that the proposed control strategy gives better results. 2) Trajectory following Comparison between the proposed control strategy and conventional control is done on SYSTEM 1. Two cases of trajectory following are investigated. One is to follow a square wave in angle, another is to follow a straight line in Cartesian SYSTEM 3's Motion Under The Proposed Control 5-i 1 : 1 1/1 o Time (b) SYSTEM 3's Motion Under The Proposed Control 10-8- if t 6- 1 1 in .2 I V O 4-Velc j \ 2-1! \ Legend 1 ~ m B & ( • , ) 0 1 -2- l 1 1 1 1 0 0.5 1 1.5 2 2.5 Time F1g. 6.3.3 96 space. a) Following a square wave The square wave has a cycle of 2.0 seconds. The results for the proposed control strategy are shown in Fig. 6.3.4. The results for conventional control are shown in Fig. 6.3.5. It's clear the proposed control strategy is better. b) Following a straight line The straight line is in the forth quadrant of the Cartesian space. It is converted to the desired angles for link 1 and link2 at every set point. The desired angles are then used as commands for the controllers. The results for conventional control are shown in Fig. 6.3.6, and the results for the proposed control strategy are shown in Fig. 6.3.7. It is seen that the trajectory following under the proposed control strategy is more accurate, especially at the beginning, than that under conventional control. SYSTEM 1's Motion Under The Proposed Control Strategy 1.2 i i Legend Time ( a ) SYSTEM Vs Motion Under The Proposed Control Strategy Legend 4 tl.l SYSTEM 1's Motion Under The Proposed Control Strategy o-l- Legend " _L«4>__ Time (C) SYSTEM Ts Motion Under The Proposed Control Strategy Legend tk it,i F1g. 6.3.4 SYSTEM fs Motion Under Conventional Control F1g. 6.3.5 SYSTEM 1's Motion Under Conventional Control i i — — — , Legend A L I N E D X L I N E R x axis SYSTEM 1's Motion Under The Proposed Control Strategy 1-1 1 100 CHAPTER 7 CONCLUSION A c o n t r o l s t r a t e g y for r o b o t i c m a n i p u l a t o r s i s proposed in the t h e s i s . A l o c a l and g l o b a l c o n t r o l s t r u c t u r e has been proposed for the purpose of c o n s t r u c t i n g such a c o n t r o l s t r a t e g y . For c o n s t r u c t i n g the c o n t r o l s t r a t e g y , a model for a g e n e r a l m a n i p u l a t o r i s a l s o proposed and a method of o b t a i n i n g the model parameters i s sugges ted . The proposed c o n t r o l s t r a t e g y has the form of n o n l i n e a r PD c o n t r o l , whose g a i n s vary a c c o r d i n g to the s t a t e of the system i n such a way the the system i s f o r c e d to a ' s l i d i n g mode' on a s p e c i f i e d s l i d i n g s u r f a c e , r e s u l t i n g in d e s i r e d dynamics . For the t e s t i n g and v e r i f i c a t i o n of the c o n t r o l s t r a t e g y , two t w o - l i n k m a n i p u l a t o r s and a t h r e e -l i n k m a n i p u l a t o r have been s i m u l a t e d on computer . The r e s u l t s of the exper iments on these s i m u l a t e d m a n i p u l a t o r s are p r e s e n t e d in the t h e s i s , which v e r i f i e d the c a p a b i l i t y of the proposed c o n t r o l s t r a t e g y and the model . Comparison between the proposed c o n t r o l s t r a t e g y and c o n v e n t i o n a l c o n t r o l i s c a r r i e d o u t . R e s u l t s show the advantages of the proposed c o n t r o l s t r a t e g y over c o n v e n t i o n a l c o n t r o l . 101 REFERENCE [1] M. Vukubratovic, D. Stokic, "Scientific Fundamentals of Robotics Control of Manipulation Robots," [2] J. Y. S. Luh, "Conventional Controller Design for Industrial Robots - A Tutorial, " IEEE Transactions On System, Man, And Cybernetics, Vol. SMC-13, No. 3, pp. 298-316, May/June, 1983. [3] G. G. Leininger, "Adaptive Control of Manipulators Using Self-tuning Method, " pp. 801-815. [4] P. E. Wellstead, J. M. Edmunds, D. Prayer, P. Zamker, "Self-tuning Pole/Zero Assignment Regulators, " Int. J. Contr., Vol. 80, No. 1, pp. 1-26, 1979. [5] M. Vukubratovic and D. Stokic, "Contribution to Suboptimal Control of Manipulation Robots, " IEEE Trans. On Auto. Contr., Vol. AC-28, No. 10, pp. 981-985, Oct. 1983. [6] M. Vukubratovic and D. Stokic, "One Enginerring Concept of Dynamic Control of Manipulators, " J. Dynamic Syst. Meas. Contr. Trans. ASME, Vol. 102, pp. 108-118, June 1981. [7] M. Vukubratovic and D. Stokic, "Choice of Decoupled Control Law of Large-Scale Mechanical Systems, " In Proc. 2nd Symp. Large-scale Syst.: Theory and Appl., Toulous, France, 1980; also in Large-scale Syst. Vol. 2, 1981. 102 [8] M. Takegaki, and S. Arimoto, "An Adaptive Trajectory Control of Manipulators, " Int. J. Control, Vol. 35, No. 2, pp. 219-230, 1981 . [9] K. K. D. Young, "Controller Design for a Manipulator Using Theory of Variable Structure Systems, " IEEE, Trans. On Syst., Man, and Cyber., Vol. SMC-8, No. 2, Feb. 1978. [10] S. V. Emelyanov, "Variable Structure Control Systems, " Moscow, Nauka, 1967; also Muncher-Wien: Oldenburg Verlag, 1969. [11] S. V. Emelyanov, "Theory of Variable Structure Systems, " Moscow, Nauka, 1970. [12] U. Itkis, "Control Systems of Variable Structure, " New York: Wiley, 1976. [13] V. I. Utkin, "Sliding Modes and Their Application to Variable Structure Systems, " Moscow: Nauka, 1974. [14] R. G. Morgan, and U. Uzguner, "A Decentralized Variable Structure Control Algorithm for Robotic Manipulators, " IEEE Journal of Robotics and Automation, Vol. RA-1, No. 1, pp. 57-65, March, 1985. [15] R. G. Morgan, "A decentralized Variable Structure Control Algorithm for Robotic Manipulators, " M. S. Thesis, Dept. Of Electrical Eng., Ohio State Univ., Columbus, OH, Dec. 1985. 103 [16] R. P. Paul, "Robot Manipulators: Mathematics, Programming, and Control, " The MIT Press, Cambridge, Massachusetts and london, England. [17] J. J. Slotine and S. S. Sastry, " Tracking Control of non-linear Systems Using Sliding Surfaces, with Application to Robot Manipulators, " Int. J. Control, Vol. 38, No. 2, pp. 465-492, 1983. [18] Filippov, A. F. , Am. Math. Soc. Trans. Pp. 62-199, 1960. [19] J. Craig, " Introduction to Robotics: Mechanics and Control, " Addison Wesley, Spring 1985. [20] B. K. Kim, K. G. Shin, "Suboptimal Control of Industrial Manipulators with a Weighted Minimum Time-Fuel Criterion, " IEEE Trans on Automatic Control, Vol. AC-30, No. 1, pp. 1-10, Jan. 1985. [21] J. Y. S. Luh, W. D. Fisher, R. P. C. Paul, "Joint Torque Control by a Direct Feedback for Industrial Robots. " IEEE Tans. On Automatic Control, Vol. AC-28, No. 2, pp. 153-161, Feb. 1983. [22] S. Dubowsky, D. T. DesForges, "The Applications of Model-Referenced Adaptive Control to Robotic Manipulator, " J. Of Dynamic Systems, Measurement, and Control, Vol. 101, pp. 193-200, Sept. 1979. 1 04 [23] E. G. Gilbert, I. J. Ha, "An Approach to nonlinear Feedback Control with Applications To Robots, " 1983, IEEE Transactions, pp. 134-138, 1983. [24] J. E. Brandeberry, K. S. Rattan, R. E. Siferd, " Design and Implementation of Servo-Control for a Robotic Arm, " IEEE Transactions, pp. 189-196, 1983. [25] A. K. Bejczy, "Introduction to Robot Mechanics, Models, Sensors and Control, " Lecture Notes, American Control Conference, San Diego, CA, June 5, 1984. [26] M. Brady, J. M. Hollerbach, and etc., "Robot Motion: Planning and Control, " The MIT Press, Cambridge, Massachusets and London, England. [27] J. Y. S. Luh, M. W. Walker, and R. P. Paul, "On-line Computational Scheme for Mechanical Manipulators, " Journal of Dynamic Systems, Measurement, and Control, Vol. 102, pp. 69-76, June 1980. [28] A. J. Koivo and T. H. Guo, "Adaptive Linear Controller for Robotic Manipulators, " IEEE Transactions On Automatic Control, Vol. AC-28, No. 2, pp. 69-76, Feb. 1983. [29] M. Takegaki and S. Arimoto, "A New Feedback Method for Dynamic Control of manipulator, " Journal of Dynamic System, Measurement, and Control, Vol. 102, pp. 119-125, June 1981. 105 [30] M. Vukobratovic and D. Stokic, "Approximative Models in Dynamic Control of Robotic Systems, " Institute Mihailo Pupin, Beograd, Yugoslavis. [31] J. M. Hollerbach, "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity, " IEEE Transaction on Systems, Man, and Cybernetics, Vol. SMC-10, No. 11, pp. 730-736, November, 1980. [32] C. Samson, "Robust Non Linear Control of Robotic Manipulators, " IEEE Transactions, pp. 1211-1216, 1983. APPENDIX A SIMULATION OF THREE MANIPULATORS 107 A. 1 Simulation of SYSTEM 1 The physical structure of SYSTEM 1 is shown in Fig. A.1.1 3 x Fig. A.1.1 The physical structure of SYSTEM 1. According to the Euler-Lagrange dynamic equation: 3 3L 9L T- =—( — ) (A.1-1) 3t d6- 30; where 0-( is the angle of the ith link, 0; is the corresponding velocity, and T. is the corresponding torque. The lagrangian L is defined as the difference between the kinetic energy K and the potential energy P. K=K,+K2, P=P,+P2, and they are K, = 0.5m,u,2 =O.5m,d120,2 P, = m,gy, = -m1gd,cos6,1 1 08 K2 = 0.5m2u22 = 0.5m 2(x 2 2+y 2 2) = 0.5m2(d, 20, 2+d 2 2 (0,+0 2) 2 + 2(3,032 (0, + 62)6 }cos62) P 2 = m2gy2 = -m2g(d,cos0,+d2cos(0,+ 0 2 ) ) Hence, K = K,+K2 = 0.5(m,d, 20, 2+m2 (d, 2 0 2 2+d2 (0,+0 2) 2+2d,d 2 (0,+0 2 ) 0 , c o s 0 2 ) ) P = P,+P2 = -migdiCos0i-m 2g(d,cos0i+d 2cos(0i+0 2)) L = K-P = 0 . 5 (m!d n 2 0 , 2+m2 (d, 2 0 2 2 + d 2 ( 0 , + 0 2) 2+2d ,d2 ( 6 , + 0 2) 0 , c o s 0 2 ) ) + +migdiCOS0,+m 2g(d,cos0i+d 2cos(0,+0 2)) Substitute to (A.1-1), yields T, = 8,0,+5,02+0, T 2 = a 20,+b 20 2+c 2 where the coefficients are a, = (m,+m2)d,2+m2d22+2m2d,d2cos02 b, = m 2d 2 2+m 2d,d 2cos0 2 (A.1-4) c, = -2m2d1d20 , 0 2sin0 2-m 2d 1d 20 2 2 s i n 0 2 + (in,+iri2)gd,sin0,+ +m 2gd 2sin(0,+0 2) (A.1-2) (A.1-3) 109 a 2 = m 2d 2 2 +m 2d,d 2cos0 2 b 2 = m 2d 2 2 (A.1-5) c 2 = m 2d,d 20y 2sin0 2+m 2gd 2sin(0,+ 0 2) From equations (A.1-2) and (A.1-3), we can derive the following equat ions, .. b 2 (T,-c , )-b, ( T 2 - c 2 ) . , 8,= =f , (By , 8 2 r e i ,82,Ty ,T 2) (A. 1-6) a 1 b 2 _ a 2 b 1 a , ( T 2 - c 2 ) - a 2 ( T 1 - c 1 ) . . 82 = = f 2 (01,02,01 , 0 2 ,T, ,T 2) (A. 1-7) a, b 2 - a 2 b 1 Now, we've got two es s e n t i a l equations (A.1-6) and (A.1-7). Let 03=0,, and 0«=0 2, we can derive a group of f i r s t order d i f f e r e n t i a l equations, (8y = 0 3 * 0 2 = 0 n, \ (A.1-8) 0 3 = 0, = fy (0 \ , 6 2,03,0«,Ty ,T 2) 04 = 02 =f2(01 r02,03 f00»T,,T 2) I have written a subroutine for the program FORSIM to solve the d i f f e r e n t i a l equation group (A.1-8). The parameters for the simulation are chosen as m1=m2=1 (lb.) and d,=d2=2 ( f t . ) 110 1 SUBROUTINE EXPONE(RMASS,RLENG,TORQUE.THETA.DTHETA) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n 4 C T h i s s u b r o u t i n e i s t o s i m u l a t e the motion of SYSTEM 1. 5 C 6 C 7 IMPLICIT REAL*B (A-H.O-Z) 8 REAL*8 PARA(2,3).RMASS(2),RLENG(2),TORQUE(2 ),THETA(4).DTHETA(4) 9 DATA GRA/32.174/ 10 C 11 C V a r i a b l e D e s c r i p t i o n 12 C P A R A ( a r r a y ) : C o n t a i n i n g the parameters f o r s i m u l a t i n g EXPERI.I. 13 C RMASS(array): C o n t a i n i n g the masses of the composing l i n k s . 14 C R L E N G ( a r r a y ) : C o n t a i n i n g the l e n g t h e s of the l i n k s . 15 C TOROUE(array): C o n t a i n i n g the Input t o r q u e s . 16 C T H E T A ( a r r a y ) : C o n t a i n i n g the v a l u e s of the a n g l e s of the l i n k s . 17 C DT H E T A ( a r r a y ) : C o n t a i n i n g of the v a l u e s of the a n g u l a r v e l o c i t i e s . 18 C 19 C 20 C Parameters f o r l i n k one 21 C 22 PARA(1.1) = (RMASS(1)+RMASS(2))*RLENG(1)*RLENG(1) + RMASS(2)*RLENG( 2 23 1)*RLENG(2)+2.0*RMASS(2)*RLENG(1)*RLENG(2)*DC0S(THETA(2)) 24 PARA(1,2)=RMASS(2)*RLENG(2)*RLENG(2)+RMASS(2)*RLENG(1)* R L E NG(2)* 25 1DC0S(THETA(2)) 26 PARA(1,3)=-2.*RMASS(2)*RLENG(1)*RLENG(2)*THETA(3)*THETA(4)*DSIN( 27 1THETA(2))-RMASS(2)*RLENG(1)*RLENG(2)* THETA(4)«THETA(4)*DSIN(THET 28 1A(2)) + (RMASS(1)+ RMASS(2 ) ) *GRA'RLENG(1)»DSIN(THETA(1))+RMASS(2)* 29 1GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 30 C 31 C Parameters f o r l i n k two 32 C 33 PARA(2.1)=PARA(1,2) 34 PARA(2,2)=RMASS(2)*RLENG(2)*RLENG(2) 35 PARA(2,3)=RMASS(2)*RLENG(1)*RLENG(2)*THETA(3)*THETA(3)*DSIN(THET 36 1A(2))+RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 37 C 38 E = PARA(1.1 )*PARA(2.2)-PARA(2.1 )*PARA(1,2) 39 C 40 C 41 C The d i f f e r e n t i a l e q u a t i o n s f o r s i m u l a t i n g EXPERI.I 42 C 43 DTHETA(1)=THETA(3) 44 DTHETA(2) = THETA( 4) 45 DTHETA(3)=((TOROUE(1)-PARA(1.3))*PARA(2.2)-(TOROUE(2)-PARA(2,3)) 46 1*PARA(1,2))/E 47 DTHETA(4)=((TOROUE(2)-PARA(2,3))*PARA(1,1)-(TOROUE(1)-PARA(1.3)) 48 1*PARA(2,1 ) )/E 49 C 50 RETURN 51 END 111 A.2 Simulation of SYSTEM 2 The manipulator SYSTEM 2 has the same links as SYSTEM 1, but the physical structure is different. A rough plot of the SYSTEM 2 is shown in Fig. A.2.1 Fig. A.2.1. A rough plot of SYSTEM 2. Where the f i r s t link is in the XOZ plane and the plane composed by the f i r s t and second links is orthogonal to XOZ plane. From Fig. A.2.1, we can get d, = x , i+z,k = d^osfl, i+dtsint^ k "dT d2 = -(d 2cos0 2)— + d 2sin0 2 j d, dicosd,i+d,sine,k _^ = -(d 2cos0 2) + d 2sin0 2 j d, = -d2cost?!cose?2i~d2sine ,cos02k+d2sin02 j (A.2-1) (2.2-2) The kinetic and potential energy are 1 12 K, = 0.5m)U!2 = 0. 5m n d, 2 1 9 ! 2 P, = m,gz, = m 1gd lsin0, K2 = 0.5m2u22 = 0.5m2|r|2 and ~r~ = d,+dt = (d 1-d 2cos0 2)cos0 1i+d 2sin0 2j+(d 1-d 2cos0 2)sin0 1k = {-[(d 1-d 2cos0 2)sin0j0\ + (d 2sin0 2cos0 1)0 2}i + + (d 2cos0 2 ) 02~j + { (d,-d 2cos0 2 ) 0 ,cos0 ,+d2e2sin0 , sin0 2 }k fr"! 2 = t (d2cos692-d, ) 0 , sin0,+d2 02 sin0 2cos0, ] 2+(d 20 2cos0 2) 2+ + [(d 1-d 2cos0 2)0\cos0 1+d 20 2sin0 1sin0 2] 2 = [(d 1-d 2cos0 2)0,] 2+(d 20 2sin0 2) 2+(d 20 2cos0 2) 2 = [ ( d , - d 2 c o s e 2 ) 0 i l 2 + ( d 2 0 2 ) 2 K2 = 0.5m 2{[(d 1-d 2cos0 2)0 l] 2+(d 20 2) 2} P 2 = m2gz2 = m 2g(d,-d 2cos0 2)sin0, K = K,+K2 = 0.5[m,d, 29, 2+m 2d 2 20 2 2+m 2(d,-d 2cos0 2) 20, 2] P = P,+P2 = [m 1d 1sin0 1+m 2(d 1-d 2cos0 2)sin0 1]g L = K-P = O.Bdihd^+m, (d 1-d 2cos0 2) 2)0 1 2 + O.5m2d2 2 0 2 2--g(m 1d 1+m 2(d,-d 2cos0 2))sin0. 1 1 3 Hence, 9L 90, 9L 90, and also, 9L d62 9L 90, = -(m,di+m2(d,-d2cos02))gcos0 = (m,d, 2+in2 (d,-d 2cos0 2) 2 )0, d 9L 9L T, = — ( — ) dt 90, 90, = (m,di 2+m 2(di-d2COS0 2) 2)0,+2m 2(di-d 2cos0 2)d 20,0 2sin0 2+ +(m,di+m2(d,-d2cos02))gcos0, (A.2-3) = 0, 2m 2(d,-d 2cos0 2)d 2sin0 2-ni 2gd 2sin0 1sin0 2 = m 2d 2 20 2 d 9L 9L T 2 = — < — ) dt 902 d62 = m 2d 2 20 2+m 2gd 2sin0,sin02--m2 (d,-d 2cos0 2 )d20*, 2 s i n 0 2 (A. 2-4) Let's rewrite (A.2-3) and (A.2-4) as following 1 1 4 T 2 = a 20 2+b 20, 2+c 2 where a, = ir^d, 2+m2 (d,-d 2cos02) 2 b, = 2m 2(d,-d 2cos0 2)d 2sin0 2 (A.2-5) c, = (m 1d 1+m 2(d 1-d 2cos0 2))gcos0! a 2 = m 2d 2 2 b 2 = -m 2(d,-d 2cos0 2)d 2sin0 2 (A.2-6) c 2 = m 2gd 2sin0,sin0 2 It's easy to derive the accelerations as functions of T,, T 2, 0 , , 02, and etc. 0, = ( T r b t M j - c J / a , (A.2-7) 02 = (T 2-b 20, 2-c 2)/a 2 (A.2-8) Let 0, = 03, and 02 = 0„. Then, i t is straight forward to derive the f i r s t order differential equation group. 02 03 0« 03 0« (T,-b, 030i,-c, )/a, ( T 2 - b 2 0 3 2 - c 2 ) / a 2 (A.2-9) A subroutine has been written for the program FORSIM to solve the differential equation group (A.2-9), which is listed the next page. 116 1 SUBROUTINE EXPTWO(RMASS,RLENG,TOROUE,THETA.DTHETA) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n 4 C T h i s s u b r o u t i n e i s t o s i m u l a t e the motion of the SYSTEM 2. 5 C 6 C 7 C V a r i a b l e D e s c r i p t i o n 8 C TORQUE(array): C o n t a i n i n g the Input t o r q u e s . 9 C T H E T A ( a r r a y ) : C o n t a i n i n g a n g l e v a l u e s of the composing l i n k s . 10 C D T H E T A ( a r r a y ) : C o n t a i n i n g the a n g l e v e l o c i t y v a l u e s of the 11 C composing l i n k s . 12 C RMASS(array): C o n t a i n i n g masses of the composing l i n k s . 13 C R L E N G ( a r r a y ) : C o n t a i n i n g l e n g t h e s of the composing l i n k s . 14 C 15 IMPLICIT REAL*8 (A-H.O-Z) 16 REAL*8 RMASS(2),RLENG(2) ,PARA(2,3),T0R0UE(2),THETA(4 ) ,DTHETA(4) 17 DATA GRA/32 . 174/ 18 C 19 C Parameters f o r l i n k one 20 C 21 PARA( 1 . 1) = RMASS(1 )»RLENG(1)* *2+RMASS(2)*(RLENG(1)-RLENG(2)*DC0S( 22 1THETA(2 ) ) ) * *2 23 PARA(1,2)=2.0*RMASS(2)*(RLENG(1)-RLENG(2)*DC0S(THETA(2)))»RLENG( 24 12)*DSIN(THETA(2)) 25 PARA{1,3)=(RMASS(1)*RLENG(1)+RMASS(2)*(RLENG(1)-RLENG(2)*DC0S(TH 26 1ETA(2))))*GRA*DC0S(THETA(1)) 27 E1=PARA(1,1) 28 C 29 C Parameters f o r l i n k two 30 C 31 PARA(2.1)=RMASS(2)*RLENG(2)*RLENG(2) 32 PARA(2.2) = -RMASS(2)*(RLENG(1 ) - RLENG(2)*DC0S(THETA(2)))*RLENG(2)* 33 1DSIN(THETA(2) ) 34 PARA(2.3)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1))*DSIN(THETA(2)) 35 E2=PARA(2,1) 36 C 37 C The d i f f e r e n t i a l e q u a t i o n s f o r s i m u l a t i n g the motion of EXPERI.I 38 C 39 DTHETA(1)=THETA ( 3) 40 DTHETA(2)=THETA(4) 4 1 DTHETA(3) = (T0RQUE(1 )-PARA( 1 . 2 ) *THETA(3 ) *THETA(4)-PARA(1,3))/E1 42 DTHETA(4)=(T0RQUE(2)-PARA(2,2)*THETA(3)*THETA(3)-PARA(2.3))/E2 43 C 44 RETURN 45 END 1 17 A.3 Simulation of a three link manipulator SYSTEM 3 The model of a three link manipulator is taken from Morgan's paper [14], which consists of three links of a robotic manipulator closest to the base. For the sake of reference, i t is called SYSTEM 3 here. The model and the angles are presented in Fig. A.3.1. x Fig. A.3.1. Physical structure of SYSTEM 3. Lumped equivalent masses m, and m2 are assumed, and an additional moment of inertia Jy is included only for the base of the manipulator. From the base outward, the manipulator consists 1 18 pf a v e r t i c a l a x i s o r w a i s t and two h o r i z o n t a l a x e s o r s h o u l d e r and e l b o w . The l i n k v a r i a b l e s a r e 0 1 f 0 2 , and 0 3 , and the l e n g t h s of the l i n k s a r e 1, and 1 2 . The m a t r i x f o r m u l a t i o n of t he dynamics i s g i v e n b e l o w . D e t a i l s of the d e r i v a t i o n may be found i n [ 1 5 ] . ( 0 2 , 0 3 ) o 0 0 . 0 a 2 2 0 a 2 3 ( 6 2 , 9 3 ) a 3 3 / * < b i i ( 0 2 r ^ s ) ^ l 0 2 + b 1 2 ( e 2 , B 3 ) B y d 3 ~ b 2 i ( 0 2 , 0 3 ) 0 3 2 - b 2 2 ( 0 2 , 0 3 ) 0 i 2 _ b 2 1 ( 0 2 f 0 3 ) 0 2 2 - b 3 2 ( 0 2 , 0 3 ) 0 1 2 r 2 ( 0 2 , 0 3 ) g r 3 ( 0 2 ) g T, T 2 T 3 (A .3 -1 ) a n ( e 2 , e 3 ) = (m,+m 2) 1 1 2 c o s 2 ( 0 2 ) + m 2 l i l 2 c o s ( 0 2 ) c o s ( 0 3 ) + + m 2 l 2 2 c o s 2 ( 0 3 ) + J ! a 2 2 = ( m ^ m j ) ! , 2 3 2 3 ( 0 2 r 0 3 ) = m 2 l i l 2 s i n ( 0 2 , 0 3 ) a 3 3 = m 2 l 2 2 b i i ( 0 2 , 0 3 ) = 2 ( m T + m z ) 1 , 2 s i n ( 0 2 ) c o s ( 0 2 ) + m 2 l ! l 2 s i n ( 0 2 ) c o s ( 0 3 ) b i 2 ( 0 2 , 0 3 ) = m 2 l i l 2 c o s ( 0 2 ) s i n ( 0 3 ) + 2 m 2 l 2 2 s i n ( 0 3 ) c o s ( 0 3 ) b 2 i ( 0 2 , 0 3 ) = m 2 l i l 2 s i n ( 0 2 - 0 3 ) b 2 2 ( 0 2 , 0 3 ) = ( m 1 - m 2 ) l , 2 s i n ( 0 2 ) c o s ( 0 2 ) + + m 2 l , l 2 c o s ( 0 2 ) s i n ( 0 3 ) b 3 2 ( 0 2 , 0 3 ) = m 2 l 2 2 s i n ( 0 3 ) cos ( 0 3 ) + m 2 l , l 2 c o s ( 0 2 ) s i n ( 0 3 ) r 2 = - ( m , + m 2 ) 1 , c o s ( 0 2 ) 1 1 9 r 3 = -m 2l2cos(0 3) The values of the parameters used are J, = 0.0243 kgm2, m, = 0.624 kg, m2 = 0.782 kg, and 1, =12 =0.23 m [Morgan]. The above matrix equation has to be rewritten to form a f i r s t order differential equation group. 01 0 2 a , , 0 0 0 a 2 2 a 2 3 0 a 2 3 a 3 3 — I / . h, , 0 i 0 2 + b , 2 0 , e 3  _ b 2 , 0 3 2~b2 2 9 , 2 ^2 1 02 2 - b 3 2 0 1 2 '0 * / 'V, T, r 2 g + T 2 r 3 T 3 l e a d s to b , , 0 , 0 2 + b , 2 0 , 0 3 T, 0 1 = +  a 1 1 a 1 1 = f , ( 0 , , 0 2 , 0 3 , 0 , , 0 2 , 0 3 , T , ) •• ^ . . • « 0 2 = - [ - b 2 i a 3 3 0 3 _ b 2 2 a 3 3 0 , - a 2 3 b 2 , 0 2 + a 2 3 b 3 2 0 , + A + ( a 3 3 r 2 - a 2 3 r 3 ) g + a 3 3 T 2 - a 2 3 T 3 ] = f 2 ( 0 1 , 0 2 t 0 3 r 0 1 i 0 2 i 0 3 # T 2 , T 3 ) 4 . 1 • » . • 0 3 = - [ b 2 1 3 2 3 0 3 2 + ^ 2 2 a 2 3 0 1 + 3 2 3 5 2 1 0 2 - 5 3 2 a 2 2 0 1 + A + ( - a 2 3 r 2 + a 2 2 r 3 ) g - a 2 3 T 2 + a 2 2 T 3 ] . . . = f 3 ( 0 , , 0 2 , 0 3 > 0 1 f 0 2 i 0 3 f T 2 f T 3 ) where A = a 2 2 a 3 3 _ a 2 3 2 • 1 , • . Let 0 « = 0 , , 0 5 = 0 2 i 0 6 = 0 3 f we get a f i r s t order differential equation group. 1 20 e: -= 0, e\ -= 05 e 3 = = 06 0, = - f l (0 , , 0 2 f 0 3 ,0, , 0 2 ,03 ,T,) 0 5 = = f 2 (01 ,02,03 ,01 • r 0 2 ,03 ,T 2,T 3) 06 = = f 3 ( 0 , ,02,03 , 0 i ,02 ,03 ,T 2,T 3) (A.3-2) A subroutine has been written to solve the differential equation group above. The subroutine is listed in the next page. 121 1 SUBROUTINE MANI3L(MASS,LENGTH,TOROUE,THETA.DTHETA) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n 4 C T h i s s u b r o u t i n e i s to s i m u l a t e the motion of a t h r e e l i n k 5 C m a n i p u l a t o r c a l l e d SYSTEM 3. 6 C 7 8 IMPLICIT REAL'S (A-H.O-Z) 9 REAL'S MASS(3),LENGTH(3).T0R0UE(3).THETA(6),DTHETA(6) 10 DATA GRA,Rd1/9.8,0.0243/ 1 1 C 12 C V a r i a b l e D e s c r i p t i o n 13 C MASS : C o n t a i n i n g the masses of the composing l i n k s . 14 C LENGTH: C o n t a i n i n g the l e n g t h e s of the l i n k s . 15 C TOROUE: C o n t a i n i n g the in p u t t o r q u e s . 16 C THETA : C o n t a i n i n g the v a l u e s of the a n g l e s of the l i n k s . 17 C DTHETA: C o n t a i n i n g of the v a l u e s of the a n g u l a r v e l o c i t i e s . 18 C 19 C 20 C Parameters 21 C 22 ALFA 1 1 = (MASS(2)+MASS(3))'(LENGTH(2)*DC0S(THETA(2)))"2 23 1 +MASS(3)*LENGTH(2)*LENGTH(3)'DCOS(THETA(2)) 24 2 *DC0S(THETA(3)) + MASS(3)*(LENGTH(3)»DCOS(THETA ( 3 ) ) ) " 2 25 3 +RJ1 26 ALFA22 = (MASS(2)+MASS(3))*LENGTH(2) " 2 27 ALFA23 •= MASS(3)*LENGTH(2)'LENGTH(3)*DSIN(THETA(2)+THETA(3)) 28 ALFA33 = MASS( 3 ) *LENGTH( 3 ) " 2 29 BETA 11 = 2.*(MASS(2)+MASS(3))*LENGTH(2)"2*DSIN(THETA(2)) 30 1 *DC0S(THETA(2))+MASS(3)*LENGTH(2)*LENGTH(3) 31 2 *DSIN(THETA(2))'DCOS(THETA(3)) 32 BETA12 = MASS(3)'LENGTH(2)*LENGTH(3)'DCOS(THETA( 2)) 33 1 *DSIN(THETA(3)) + 2.*MASS(3)*LENGTH(3)* *2*DSIN(THETA(3)) 34 1 »DC0S(THETA(3)) 35 BETA21 = MASS(3)*LENGTH(2 ) *LENGTH(3)*DSIN(THETA(2)-THETA(3)) 36 BETA22 = (MASS(2)-MASS(3))*LENGTH(2) "2*DSIN(THETA(2)) 37 1 »DC0S(THETA(2))+MASS(3)*LENGTH(2)'LENGTH*3) 38 1 *DSIN(THETA(2))*DCOS(THETA(3)) 39 BETA32 » MASS(3)* LENGTH(3)**2*DSIN(THETA(3))'DCOS(THETA(3)) 40 1 +MASS(3)*LENGTH(2)*LENGTH(3)*DC0S(THETA(2)) 41 1 *DSIN(THETA(3)) 42 GAMA2 " -(MASS(2)+MASS(3))*LENGTH(2)*DCOS(THETA(2)) 43 GAMA3 "= -MASS(3)»LENGTH(3)'DCOS(THETA( 3)) 44 P21 = ALFA23'BETA32-ALFA33*BETA22 45 P22 = -ALFA23*BETA21 46 P23 «= -ALFA33*BETA21 47 P24 « ALFA33*GAMA2-ALFA23*GAMA3 48 P31 » ALFA23*BETA22-BETA32*ALFA22 49 P32 « ALFA22*BETA21 50 P33 •= ALFA23*BETA21 51 P34 « ALFA22*GAMA3-ALFA23*GAMA2 52 DELTA «= ALFA22*ALFA33-ALFA23*ALFA23 53 C 54 C The d i f f e r e n t i a l e q u a t i o n s f o r s i m u l a t i n g MANI3L 55 C 56 DTHETA(1)=THETA(4) 57 DTHETA(2)=THETA(5) 58 DTHETA(3)«THETA(6) 59 DTHETA(4 )=(BETA11'THETA(4 ) *THETA(5)+BETA12*THETA(4)*THETA( 6) 60 1 +-TORQUE (1))/ALFA11 61 D T H E T A ( 5 ) « ( P 2 1 * T H E T A ( 4 ) " 2 + P 2 2 * T H E T A ( 5 ) " 2 + P 2 3 * T H E T A ( 6 ) " 2 62 1 +P24*G+ALFA33*T0R0UE(2)-ALFA23'TORQUE(3))/DELTA 63 DTHETA(6) = (P31*THETA(4)"2+P32*THETA(5)"2+P33*THETA(6)"2 64 1 +P34*G-ALFA33*T0RQUE(2)+ALFA22»T0RQUE(3))/DELTA 65 C 66 RETURN 67 END 122 APPENDIX B SETTLING TIME AND CHATTERING AREA B.1 Settling time for the proposed control strategy and conventional control For the conventional control, the response of ith subsystem in the case of zero i n i t i a l velocity i s , At the settling time (denoted as tj ), where 5 is the target set. That leads, M0) + ^e,(0)</KWi1' -u,tf + ln(l + Uitf) = M^j) tf = +-ln{l + uitf)+-ln(^) (B . 1 -1 ) For the proposed control, the response of the ith subsystem before reaching sliding line $ in the case of zero i n i t i a l velocity i s , £,(<) = ei(0)co»Uit Vi(t) = — £i(0)u!iainuit At the time t=t, ( t, is the time the trajectory reaches #), U i € i ( t i ) + Vi(ti) = (j,£,(0)co«w,ti - UiCi(0)tinuiti = 0 123 cotoJiti — einuiti = 0 sin(u>iti - -) = 0 4o>,-On the sliding line Vi(t) + U>iei(t) = 0 At the time t = tj (t^ is the settling time), That leads, e-wi(«r«i) = Since C-(O = e.-(0)co«£) 4 hence i , , i , i . .t,(0). = h + J-ln2+-ln(^) ( B . 1 - 2 ) 124 B.2 Chattering area Chattering area is caused by time delay in the switching, caused by time delay in switching mechanism, or sampling time, or combined effect of both (see Fig. B.2.1). Fig. B.2.1 Chattering area Denote the time delay in the switching as T. Before the subsystem's trajectory reaches •.€{x-i : u;+CJ. e; =0} , the subsystem is under the P control, thus its response i s , £,(<) = £,-(0)eO8u)i< vt{*) = -£,(0)w,*t'na\t At time t=t,+T, the trajectory reaches *.. Thus, vfa + T) + w-e,-^ + T) = -£,-(0)w,-M'na>,-(j5i + T) + w.e^Ojeosw i^!, + T) = 0 which leads, »inui(ti + T) ijj = ijj • COBUJifa + T) = wMufa + T)) 125 (B.2-1) Before the subsystem's trajectory reaches * +£{x ::v t +u.e; =0}, the subsystem is under the D control, thus the response is where t 0 is the time when the trajectory is on the sliding line. On the sliding line, Vi(to) + Uiti(to) = 0 thus, Vi{to) + 2>,<,(*o) = u.-(<o) -f u>,£,(*o) + (Di - Ui)a(to) Substitute (B.2-3) to (B.2-2) Vi(t) + Diti{t) = (Di - w,)£,(<b) At time t=t0+T, the trajectory reaches 4>t, u.-(»b + T)+ w,£,(io + T) = 0 That leads (Dt - a-+) f,(<o +T) = (Di - w,)e,(fe) «<(*) + Dtn(t) = Vi{io) + DiCtM (B.2-2) = (A--w.-Mfe) (B.2-3) M M (B.2-4) «.<*> + T) From equation (B.2-2), we obtain M0=[^(^(t-t')-l) + l]c,«o) (B.2-5) Therefore, <.{to + T)=[%(e- 1)+!]<.(*>) That leads 1 26 W L ( e - D , T _ l ) + 1 APPENDIX C SUBROUTINES USED IN THIS THESIS 128 1 2 C 3 C 4 c 5 c 6 c 7 c-8 9 10 1 1 12 c 13 c. 14 c 15 c 16 c 17 18 19 20 21 22 c 23 c 24 c 25 c 26 c 27 c 28 c 29 c 30 c 31 c 32 c 33 c 34 c 35 c 36 c 37 c 38 c 39 c 40 c 41 c 42 c 43 c 44 c 44 . 5 c 45 46 47 48 49 50 51 52 53 54 c 55 c 56 c 57 c T h i s s u b r o u t i n e 1s to p r o v i d e d l f f e r e n t a l l q u a t l o n s and n e c e s s a r y d a t a f o r the mam program FORSIM to s i m u l a t e the con-v e n t i o n a l c o n t r o l on SYSTEM 1. SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD,CONVGD,KFLAG,NO.FIX,ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT.DTMAX,DTMIN,DTOUT,EMAX,ICHECK,NIMPL,IPLOT , 1JPLOT.TFIN V a r i a b l e D e s c r i p t i o n : a n g l e s of SYSTEM 1's l i n k s , a n g u l a r v e l o c i t i e s , the d e s i r e d a n g l e s , d e s i r e d a n g u l a r v e l o c i t i e s , the masses of l i n k s , the l e n g t h e s of l i n k s , moments of I n e r t i a of l i n k s , i n t e r a c t e d moments of i n e r t i a , l o c a l c o n t r o l s i g n a l s , g l o b a l c o n t r o l s i g n a l s , t o t a l c o n t r o l s i g n a l s , the p o s i t i o n g a i n s , the v e l o c i t y g a i n s , n a t u r a l f r e q e n c i e s . damping r a t toes, g r a v i t y t o r q u e s . I n t e r a c t i o n r a t l o e s . t r a c k i n g e r r o r s . f i r s t o r d e r d e r i v a t i v e s of e r r o r s , r a t i o . f a c t o r of the sampli n g time. COMMON /INEGLT/ THETA(4 ) COMMON /DERIVT/ DTHETA(4) REAL *8 THETAD(2),DTHTAD(2),RMASS(2),RLENG(2),RJ0(2).RJIN( 2) REAL*8 OMEGA(2),ZETA(2),GAINP(2).GAINV(2).GT(2) REAL*8 TL0CAL(2 ) ,TGL0BL(2),TT0TAL(2) REAL'S ERR0R(2),DERR0R(2).DE(2),CLM(2,2) INTEGER ISAM.ITIMES DATA RMASS.RLENG.OMEGA.ZETA/2*1.0.2*2.0.2*10.0,2*1.0/ DATA NL I NK , IT I ME S/2 , 10/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . THETA: A r r a y of DTHETA A r r a y of THETAD A r r a y of DTHTAD A r r a y of RMASS: A r r a y of RLENG: A r r a y of RJO : A r r a y of RJIN: A r r a y of TLOCAL A r r a y of TGLOBL A r r a y of TTOTAL A r r a y of GAINP: A r r a y of GAINV: A r r a y of OMEGA: A r r a y of ZETA : A r r a y of GT: A r r a y of FACTOR A r r a y of ERROR: A r r a y of DERROR A r r a y of ISAM: Sampl Ing ITIMES 1nteger 129 58 C 59 60 IF (T.NE.O.00) GOTO IOOO 61 I2LINK=2*NLINK 62 ISAM=0 63 C 64 C The d e s i r e d a n g l e s THETAD, and t h e i r f i r s t o r d e r d e r i v a t i v e s 65 C DTHTAD. 66 C 67 68 CALL DESIRE(THETAD,DTHTAD,NLINK) 69 70 C 71 C The I n i t i a l c o n d i t i o n s of the a n g l e s THETA and t h e i r f i r s t 72 C o r d e r d e r i v a t i v e s DTHETA. 73 C 74 75 CALL INITIA(THETA,DTHETA,I2LINK) 76 77 IF(IPARAM.EQ.O) WRITE(6,40) (RMASS(I),I=1,2),(RLENG(I ) , I = 1 , 2 ) 78 79 C 80 C E v a l u a t e the moments of I n e r t i a R<JO, I n t e r a c t e d moment o f 81 C I n e r t i a RJIN and I n t e r a c t i o n r a t i o FACTOR and feedback g a i n s 82 C GAINP and GAINV. 83 C 84 85 CALL MOINER(RMASS,RLENG,RJO,NLINK) 86 CALL PDGAIN(OMEGA,ZETA,RJO,GAINP,GAINV,NLINK) 87 CALL COUINT(RJO.CLM) 89 C 90 C . 91 C. S e c t i o n 3 : C o n t r o l P r o c e s s . 92 C. 93 C 94 95 96 97 10O0 IF ( I OUT . NE . 1 ) GOTO 2000 98 ISAM=ISAM+1 99 IF (ISAM.GT.ITIMES) ISAM=1 100 IF (ISAM.NE.1) GOTO 3000 101 C 102 C C a l c u l a t i n g the g r a v i t y t o r q u e GT. 103 C 104 CALL GRATOR(RMASS,RLENG,THETA,GT) 105 106 C 107 C Measure the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 108 C ones THETA, and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA. 109 C 1 10 111 3000 CALL DETERR(THETAD.DTHTAD,THETA,DTHETA.ERROR,DERROR,I2LINK,NLINK) 112 DO 10 1=1.NLINK 113 10 DE(I ) =-OMEGA(I)* ERROR(I) 114 C 115 C The l o c a l c o n t r o l law. _ 116 C 130 117 1 16 CALL LOCALC(ERROR.DERROR.GAINP.GAINV.TLOCAL.NLINK) 1 19 120 C 121 C The g l o b a l c o n t r o l law 122 C 123 124 CALL GLOBLC(TLOCAL,CLM,GT,TGLOBL,NLINK) 125 126 C 127 C The t o t a l c o n t r o l law 128 C 129 130 CALL TOTALC(TLOCAL.TGLOBL,TTOTAL.NLINK) 131 132 C 133 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I 134 C 135 136 20O0 CALL EXPONE(RMASS,RLENG,TTOTAL,THETA.DTHETA) 137 138 C. . 139 C. 140 C . S e c t i o n 4 : Output. 141 C. 142 C 143 144 IF(IOUT.EQ.O) RETURN 145 WRITE(6,50)T,DT,THETA 146 40 FORMAT('M1=',F12.6,2X.'M2=',F12.6.2X.'D1='.F12.6.2X,'D2=',F12.6, 147 1/. 16X, 'TIME'.16X. 'STEP',16X, 'THETA1' , 12X, 'THETA2' , 12X, 'DTHETA1' , 148 111X,'DTHETA2') 149 50 F0RMAT(6F20.6) 150 WRITE(11,60) T,ERROR,DERROR.DE 151 WRITE(12.60) T,THETAD,THETA,DTHETA(3).DTHETA(4) 152 WRITE(13,60) T,TTOTAL 153 WRITEC14.60) T,GAINP,GAINV 154 60 F0RMAT(9(1X.G12.6)) 155 RETURN 156 END 157 C--158 SUBROUTINE DESI RE(THETAD,DTHTAD,NLINK) 159 IMPLICIT REAL*8 (A-H.O-Z) 160 REAL'S THETAD(NLINK).DTHTAD(NLINK) 161 DATA PI/3.1415926/ 162 THETAD(1 )=PI/2 .0 163 THETAD(2)=PI/2.0 164 DTHTAD(1)=0.0 165 DTHTAD(2)=0.0 166 RETURN 167 END 168 C--169 SUBROUTINE INITIA(THETA.DTHETA.I2LINK) 170 IMPLICIT REAL*8 (A-H.O-Z) 17 1 REAL'S THETA(I2LINK),DTHETA(I2LINK) 172 NLINK=I2LINK/2 173 DO 10 1=1,NLINK 174 10 THETA(I)=0.0 131 175 DO 20 1=1.NLINK 176 THETA(I+NLINK)=0.0 177 20 DTHETA(I)=THETA(I+NLINK) 178 RETURN 179 END 180 C 1B1 SUBROUTINE GRATOR(RMASS.RLENG,THETA.GT) 182 IMPLICIT REAL*B (A-H.O-Z) 183 REAL*8 THETA(4).RMASS(2).RLENG(2).GT(2) 184 DATA GRA/32 . 174/ 185 GT(1) = ((RMASS(1) + RMASS(2))*RLENG(1)*DSIN(THETA(1)) + RMASS( 2)*RLEN 186 1G(2)*DSIN(THETA(1)+THETA(2)))*GRA 187 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1) + THETA( 2 ) / 188 RETURN 189 END 190 C 191 SUBROUTINE COUINT(RJO,CLM) 192 C 193 C T h i s s u b r o u t i n e i s to c a l c u l a t e the c o u p l i n g m a t r i x . 194 C 195 IMPLICIT REAL*8 (A-H.O-Z) 196 REAL*8 CLM(2.2),RJO(2) 197 CLM(1.1)=RJ0(2)/RJ0(1) 198 CLM(2,2)=O.DO 199 CLM(1,2)=O.DO 200 CLM(2.1)=0.D0 201 RETURN 202 END 132 1 2 C 3 C 4 C 5 C 5.5 C 6 c-7 8 9 10 1 1 c 12 c 13 c 14 c 15 c 16 17 18 19 20 21 22 23 24 25 26 27 28 29 c 30 c 31 c 32 c 33 c 34 35 36 37 38 c 39 c 40 c 4 1 c 42 43 44 45 c 46 c 47 c 48 c 49 50 51 52 53 54 c 55 c 56 c 57 c T h i s s u b r o u t i n e 1s to p r o v i d e d l f f e r e n t a l l q u a t l o n s and n e c e s s a r y d a t a f o r the main program FORSIM to s i m u l a t e c o n v e n t i o n a l c o n t r o l on SYSTEM X SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD,CONVGD,KFLAG,NO.FIX,ABDUMP, I PARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT,DTMAX,DTMIN,DTOUT,EMAX,ICHECK.NIMPL,I PLOT, 1JPLOT.TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4 ) REAL»8 THETAD(2),RMASS(2),RLENG(2),TLOCAL(2),OMEGA(2),ZETA(2) REAL*8 TGLOBL(2),TT0TAL(2) ,GAINP(2),GAINV(2).RJ0(2).DE(2) REAL*8 ERR0R(2).DERR0R(2) ,DTHTAD(2),GT(2),CLM(2,2) INTEGER ISAM.ITIMES DATA RMASS,RLENG.OMEGA.ZETA/2*1.0.2*2.0,2*10.0.2*1.0/ DATA NLINK,ITIMES/2.10/ S e c t i o n 2 I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAM=0 The d e s i r e d a n g l e s THETAD(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHTAD(I), (1=1,2) CALL DESIRE(THETAD,DTHTAD,NLINK) The i n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1.NLINK) CALL INITIA(THETA,DTHETA,I2LINK) I F ( I PARAM.EO.0) WRITE(6,40) RMASS.RLENG E v a l u a t e the moments of i n e r t i a R J ( I ) , accumulated moment of i n e r t i a R J A C ( I ) . f a c t o r FACTOR(I). and the feedback g a m s GAINP(I) and GAINV(I). (1=1,NLINK) 133 58 C 59 60 CALL M0INER(RMA5S.RLENG,RJO,NLINK) 61 CALL PDGAIN(OMEGA.ZETA,RJO,GAINP.GAINV,NLINK) 62 CALL COUINT(RJO.CLM) 64 65 C 66 C. 67 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 68 C. 69 C 70 71 72 C 73 C Determlng the Input t o r q u e s T T O T A L ( a r r a y ) . 74 C 75 76 1000 IF(IOUT.NE.1) GOTO 2000 77 ISAM=ISAM+1 78 IF(ISAM.GT.ITIMES) ISAM=1 79 IF(ISAM.NE.1) GOTO 3000 80 C 81 C The g r a v i t y t o r q u e G T ( I ) , (1=1,2) 82 C 83 84 CALL GRATOR(RMASS,RLENG,THETA,GT) 85 86 C 87 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 88 C ones THETA(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I) 89 C 90 91 30O0 CALL DETERR(THETAD,DTHTAD,THETA,DTHETA.ERROR.DERROR,I2LINK,NLINK) 92 DO 10 1=1.NLINK 93 10 DE(I ) = -OMEGA(I)* ERROR(I) 94 95 C 96 C The l o c a l c o n t r o l law. 97 C 98 99 CALL LOCALC(ERROR.DERROR,GAINP.GAINV.TLOCAL.NLINK) 10O 101 C 102 C The g l o b a l c o n t r o l law 103 C 104 105 CALL GLOBLC(TLOCAL,CLM,GT,TGLOBL.NLINK) 106 107 C 108 C The t o t a l c o n t r o l law 109 C 110 111 CALL TOTALC(TLOCAL,TGLOBL,TTOTAL.NLINK) 1 12 113 C 114 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.II 115 C 116 134 1 17 2000 CALL EXPTW0(RMASS.RLENG,TTOTAL,THETA.DTHETA) 1 16 1 19 C 120 C. 121 C. S e c t i o n 4 : Output. 122 C . 123 c 124 125 IF(IOUT.EO.O) RETURN 126 WRITE(6,50)T,DT,THETA 127 40 FORMAT('M1='.F12.6,2X,'M2='.F12.6,2X.'D1=',F12.6.2X,'D2=',F12.6, 128 1/,16X,'TIME',16X,'STEP',16X,'THETA1'.12X.'THETA2'.12X,'DTHETA1', 129 111X, 'DTHETA2' ) 130 50 F0RMAT(6F20.6) 131 WRITE(12,60) T.THETAD,THETA,TTOTAL 132 60 F0RMAT(9(1X,G12.6) ) 133 WRITE(11,60) T,ERROR.DERROR.DE 134 RETURN 135 END 136 c 137 SUBROUTINE DESI RE(THETAD.DTHTAD,NLINK) 138 IMPLICIT REAL*8 (A-H.O-Z) 139 REAL*8 THETAD(NLINK),DTHTAD(NLINK) 140 DATA PI/3.1415926/ 141 THETAD( 1 )=PI/2.0 142 THETAD(2 )=PI/2 .0 143 DTHTAD(1)=0.0 144 DTHTAD(2)=0.0 145 RETURN 146 END 147 C 148 SUBROUTINE INITIA(THETA.DTHETA.I2LINK) 149 IMPLICIT REAL*8 (A-H.O-Z) 150 REAL ' S THETA(I2LINK).DTHETA(I2LINK) 151 NLINK-I2LINK/2 152 DO 10 1=1.NLINK 153 10 THETA(I)=0.0 154 DO 20 1=1.NLINK 155 THETA(I+NLINK)=0.0 156 20 DTHETA(I)=THETA(I+NLINK) 157 RETURN 158 END 159 c 160 SUBROUTINE GRATOR(RMASS.RLENG.THETA,GT) 161 IMPLICIT REAL ' S (A-H.O-Z) 162 REAL ' S THETA(4),RMASS(2),RLENG(2),GT(2) 163 DATA GRA/32.174/ 164 GT(1)=GRA*(RMASS(1)*RLENG(1)+RMASS(2)*(RLENG(1)-RLENG(2)*DCOS(TH 165 1ETA(2))))'DCOS(THETA(1)) 166 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1))*DSIN(THETA(2)) 167 RETURN 168 END 169 c 170 SUBROUTINE COUINT(RJO.CLM) 171 C 172 C T h i s s u b r o u t i n e i s t o c a l c u l a t e the c o u p l i n g m a t r i x . 173 C 174 IMPLICIT REAL ' S (A-H.O-Z) 135 175 REAL*8 CLM(2,2),RdO(2) 176 CLM(1. 1 )=RJ0(2)/RJ0(1 ) 177 CLM(2.2)=O.DO 178 CLM(1,2)=0.D0 179 CLM(2.1)=0.D0 180 RETURN 181 END 136 1 2 C 3 C 4 C 5 C 6 C 7 C-8 9 10 11 12 C 13 C 14 C 15 C 1G C 17 18 19 20 21 22 23 24 25 26 27 28 29 30 C 31 C . 32 C . 33 C. 34 C 35 36 37 38 39 C 40 C 41 C 42 C 43 44 45 C 46 c 47 c 48 c 49 50 51 c 52 c 53 c 54 55 56 57 c 58 c T h i s s u b r o u t i n e Is t o p r o v i d e d l f f e r e n t a l l q u a t l o n s and n e c e s s a r y d a t a f o r the main program FORSIM to s i m u l a t e c o n v e n t i o n a l c o n t r o l on SYSTEM 3. SUBROUTINE UPDATE IMPLICIT REAL'8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT.METHOD.CONVGD,KFLAG,NO.FIX,ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERO/ T,DT,DTMAX.DTMIN,DTOUT,EMAX,ICHECK.NIMPL,IPLOT , 1UPL0T,TFIN COMMON /INEGLT/ THETA(6) COMMON /DERIVT/ DTHETA(6) REAL*8 THETAD(3),RMASS(3) ,RLENG(3).TLOCAL(3),OMEGA(3),ZETA(3) REAL*B TGLOBLO),TT0TAL(3) ,GAINP(3),GAINV(3),RJ0(3),DE(3) REAL*8 ERR0R(3) .DERRORO) , DTHTAD ( 3 ) , GT ( 3 ) , CLM( 3 . 3 ) INTEGER ISAM.ITIMES DATA RMASS,RLENG,OMEGA,ZETA/O.0.0.624.0.782.3*0.23,3*10.,3»1.0/ DATA NLINK,ITIMES/3,10/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1OO0 I2LINK=2*NLINK ISAM=0 The d e s i r e d a n g l e s THETAD(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHTAD(I). (1=1.NLINK). CALL DESIRE(THETAD.DTHTAD,NLINK) The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1,NLINK). CALL INITIA(THETA.DTHETA.I2LINK) Output t h e parameters f o r the dynamic e q u a t i o n s . IF(IPARAM.EQ.O) WRITE(6,40) RMASS.RLENG E v a l u a t e the moments of I n e r t i a R J O ( I ) . I n t e r a c t e d moment of 137 59 C i n e r t i a R J I N ( I ) and f a c t o r FACTOR(I) and f e e d back g a i n s 60 C GAINP(I) a n d G A I N V ( I ) , (1=1.NLINK) 61 C 62 63 CALL MOINER(RMASS,RLENG.RJO.NLINK) 64 RJO(1)=0.0243 65 CALL PDGAIN(OMEGA.ZETA,RJO.GAINP,GAINV,NLINK) 66 CALL COUINT(RJO.CLM) 67 68 C 69 C. 70 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 71 C. 72 C 73 74 75 C 76 C Determing the input t o r q u e s TTOTAL(array) . 77 C 78 79 10OO IF(I0UT.NE.1) GOTO 2000 80 ISAM=ISAM+1 81 IF(ISAM.GT.ITIMES) ISAM=1 82 IF(ISAM.NE.1) GOTO 3000 83 C 84 C The g r a v i t y t o r q u e G T ( I ) , (1=1,NLINK). 85 C 86 87 CALL GRATOR(RMASS,RLENG,THETA.GT) 88 89 C 90 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 91 C ones. ERROR(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I). 92 C 93 94 30O0 CALL DETERR(THETAD,DTHTAD.THETA,DTHETA.ERROR.DERROR,I2LINK, 95 1 NLINK) 96 DO 11 1=1.NLINK 97 1 1 DE(I)=-OMEGA(I)»ERROR(I) 98 C 99 C The l o c a l c o n t r o l law. 100 C 101 102 CALL LOCALC(ERROR.DERROR.GAINP.GAINV.TLOCAL.NLINK) 103 104 C 105 C The g l o b a l c o n t r o l law. 106 C 107 108 CALL GLOBLC(TLOCAL.CLM.GT.TGLOBL.NLINK) 109 1 10 C 1 1 1 C The t o t a l c o n t r o l e f f e c t . 1 12 C 1 13 1 14 CALL TOTALC(TLOCAL,TGLOBL,TTOTAL.NLINK) 1 15 1 16 C 138 117 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e MANI3L. 118 C 119 120 2000 CALL MANI3L(RMASS,RLENG.TTOTAL,THETA,DTHETA) 121 122 C 123 C. 124 C. S e c t i o n 4 : Output. 125 C. 12S C 127 128 IF(IOUT.EQ.O) RETURN 129 WRITE(6,50)T,DT,THETA 130 40 FORMAT( ,M1=',F12.G,1X,'M2='.F12.6,1X.'M3='.F12.6,1X,'L1=',F12.G, 131 1 1X, ' L2= ' , F 12 . 6 , 1X, 'L3=',F 12.6,/,16X'TIme',1GX, 'Step',16X, 132 2 'Thetal',12X,'Theta2',12X,'Theta3',1IX,'Dthetal',11X, 133 3 'Dtheta2',11X.'DthetaS') 134 50 FORMAT!8G12.6) 135 WRITE(12,60) T,THETAD.THETA,TTOTAL 136 60 FORMAT(13(1X.G12.6)) 137 WRITEC11.60) T,ERROR.DERROR.DE 138 RETURN 139 END 140 C 141 SUBROUTINE DESIRE(THETAD,DTHTAD.NLINK) 142 C 143 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e 1s to s e t up the d e s i r e d 144 C a n g l e s , THETAD(I) and DTHTAD(I). (1=1,NLINK). 145 C 146 IMPLICIT REAL'S (A-H.O-Z) 147 REAL'S THETAD(NLINK),DTHTAD(NLINK) 148 DATA PI/3. 1415926/ 149 THETAD(1)=PI/2.0 150 THETAD(2)«PI/2.0 151 THETAD(3)=PI/2.0 152 DTHTAD(1)=0.O 153 DTHTAD(2)=0.0 154 0THTAD(3)=0.O 155 RETURN 156 END 157 C 158 SUBROUTINE INITIA(THETA,DTHETA,I2LINK) 159 C 160 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e 1s t o s e t up the i n i t i a l 161 C c o n d i t i o n s . 162 C 163 IMPLICIT REAL'S (A-H.O-Z) 164 REAL'S THETA(I2LINK),DTHETA(I2LINK) 165 NLINK=I2LINK/2 166 DO 10 1=1,NLINK 167 10 THETA(I)=0.0 168 DO 20 1=1,NLINK 169 THETA(I+NLINK)=0.0 170 20 DTHETA(I)=THETA(I+NLINK) 17 1 RETURN 172 END 173 C 174 SUBROUTINE GRATOR(RMASS.RLENG,THETA,GT) 139 175 C 176 C T h i s s u b r o u t i n e Is t o e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 177 C 178 IMPLICIT REAL ' S (A-H.O-Z) 179 REAL ' S THETA(6).RMASS(3),RLENG(3),GT(3) 180 DATA GRA/9.8/ 181 GT(1)*0.0 182 GT(2)=RMASS(2)*GRA'RLENG(2)*DC0S(THETA(2))+RMASS(3)*GRA'RLENG(2) 183 1 *DC0S(THETA(2)) 184 GT(3)=RMASS(3)*GRA*RLENG(3)'DCOS(THETA(3)) 185 RETURN 186 END 187 C-188 SUBROUTINE COUINT(RJO.CLM) 189 C 190 C T h i s s u b r o u t i n e Is t o c a l c u l a t e the c o u p l i n g m a t r i x . 191 C 192 IMPLICIT REAL ' S (A-H.O-Z) 193 REAL'8 CLM(3,3),RJ0(3) 194 CLM(1.1) = (RJO(3) + RJO(2) )/RJ0(1) 195 CLM(1,2)=0.D0 196 CLM(1,3)=O.DO 197 CLM(2,1)=0.D0 198 CLM(2,2)=RJ0(3)/RJ0(2) 199 CLM(2,3)=0.D0 200 CLM(3.3)=0.D0 201 CLM(3,1)=0.D0 202 CLM(3.2)=0.D0 203 CLM(3,3)=0.D0 204 RETURN 205 END 140 1 C = 2 C 3 C 4 C 5 C 6 C 7 C 8 C-9 10 1 1 12 13 C 14 C. 15 c. 16 c. 17 c 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 c 33 c. 34 c. 35 c. 36 c 37 38 39 40 4 1 42 c 43 c 44 c 45 c 46 47 48 c 49 c 50 c 51 c 52 53 54 55 c 56 c 57 c 58 E S S S 9 S S E S S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e 1s t o p r o v i d e d i f f e r e n t a l l e q u a t i o n s and n e c e s s a r y d a t a f o r the mam program FORSIM t o s i m u l a t e the pr o p o s e d c o n t r o l on SYSTEM 1. G l o b a l and l o c a l c o n t r o l l e r s have d i f f e r e n t s a m p l i n g i n s t a n t s . SUBROUTINE UPDATE IMPLICIT REAL*8(A-H.0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD.CONVGD,KFLAG,NO.FIX,ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT,DTMAX,DTMIN,DTOUT,EMAX,ICHECK.NIMPL.I PLOT, 1JPLOT.TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL*8 THETAD(2),DTHTAD(2).RMASS(2),RLENG(2),RdO(2) REAL'S OMEGA(2),ZETA(2),GAINP(2),GAINPS(2),GAINV(2),GAINVS(2) REAL'S GT(2),TL0CAL(2),TGL0BL(2),TT0TAL(2) REAL'8 ERR0R(2) ,DERR0R(2 ) ,DE(2),CLM(2,2) INTEGER ISAMG,ISAML,ITIMEG,ITIMEL,I2LINK,NLINK DATA RMASS,RLENG,OMEGA.ZETA/2*1.O.2*2.0,2*10.0,2*1.0/ OATA NLINK,ITIMEG,ITIMEL/2,10,5/ S e c t i o n 2 I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAMG=0 ISAML=0 The d e s i r e d a n g l e s THETAD(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHTAD(I), (1=1.NLINK) CALL DESIRE(THETAD.DTHTAD,NLINK) The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1,NLINK). CALL INITIA(THETA,DTHETA.I2LINK) Output v a l u e s of the mass & l e n g t h of the m a n i p u l a t o r ' s l i n k s . 141 59 IF(IPARAM.EQ.O) WRITE(6,40) RMASS,RLENG 60 61 C 62 C E v a l u a t e moment o f I n e r t i a R J O ( I ) , i n t e r a c t e d moment of 63 C I n e r t i a R J I N ( I ) , d e t e r m i n e the I n t e r a c t i o n f a c t o r FACTOR(I) and 64 C f e e d back g a i n s GAINP(I) and GAINV(I), (1=1,NLINK). 65 C 66 67 CALL MOINER(RMASS,RLENG.RJO.NLINK) 68 CALL PDGAIN(OMEGA,ZETA.RJO,GAINP,GAINV.NLINK) 69 CALL COUINT(RJO.CLM) 70 c 71 C. 72 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 73 C . 74 c 75 76 77 C 78 C The c o n t r o l a l g o r i t h m . The g l o b a l c o n t r o l has a sampling 79 C i n s t a n t ITIMEG times of the s i m u l a t i o n s t e p DT. The l o c a l 80 C c o n t r o l has a sa m p l i n g i n s t a n t ITIMEL times of DT. 81 C 82 83 1000 IF(IOUT.NE.1) GOTO 2000 84 ISAMG=ISAMG+1 85 IF(ISAMG.GT.ITIMEG) ISAMG=1 86 ISAML=ISAML+1 87 IF(ISAML.GT.ITIMEL) ISAML=1 88 IF (ISAMG.NE.1) GOTO 3000 89 C 90 C The g r a v i t y t o r q u e G T ( I ) , (1=1,NLINK) 91 C 92 CALL GRATOR(RMASS,RLENG,THETA,GT) 93 94 C 95 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 96 C ones, ERROR(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I). 97 C 98 99 30O0 CALL DETERR(THETAD,DTHTAD,THETA,DTHETA,ERROR.DERROR,I2LINK, 100 1 NLINK) 101 IF (ISAML.NE.1) GOTO 4000 102 C 103 C Determine the feedback g a i n s GAINPS & GAINVS a d a p t i v e l y a c c o r d i n g 104 c t o ERROR and DERROR. 105 c 106 107 CALL PDADAP(ERROR,DERROR,OMEGA,GAINP,GAINPS,GAINV,GAINVS,NLINK) 108 c 109 c S l i d i n g l i n e s D E ( I ) , (1=1,NLINK). 1 10 c 11 1 40OO DO 20 I • 1, NLINK 112 20 DE ( I ) = -OMEGA(I)*ERROR(I) 113 i 114 C 1 15 C The l o c a l c o n t r o l law. 1 16 c 142 1 17 118 CALL LOCALC(ERROR,DERROR.GAINPS.GAINVS,TLOCAL.NLINK) 1 19 120 C 121 C The g l o b a l c o n t r o l law. 122 C 123 124 CALL GLOBAL(TLOCAL,CLM,GT,TGLOBL.NLINK) 125 126 C 127 C The t o t a l c o n t r o l e f f e c t . 128 C 129 130 CALL TOTALC(TLOCAL,TGLOBL,TTOTAL,NLINK) 131 132 C 133 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I. 134 C 135 136 20O0 CALL EXPONE(RMASS,RLENG.TTOTAL,THETA.DTHETA) 137 138 C 139 C. 140 C. S e c t i o n 4 : Output. 141 C. 142 C 143 144 IF(IOUT.EQ.O) RETURN 145 WRITE(6,50)T,DT,THETA 146 40 FORMAT('M1='.F12.6,2X.'M2='.F12.6.2X,'L1='.F12.6,2X,'L2='.F12.6. 147 1 /,16X,'Time'.16X,'Step',16X,'Thetal',12X,'Theta2'.12X, 148 2 'Dthetal'.11X,'Dtheta2') 149 50 FORMAT(6(1X,G12.6)) 150 WRITE(11.60) T,ERROR,DERROR,OE 151 WRITE(12,60) T,THETAD,THETA,DTHETA(3),DTHETA( 4) 152 WRITE( 13.60) T,TTOTAL 153 WRITE(14,60) T.GAINPS,GAINVS 154 60 FORMAT(9( 1X.G12.6)) 155 RETURN 156 END 157 c========================"=========================================== 158 SUBROUTINE DESIRE(THETAD,DTHTAD.NLINK) 159 C 160 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e Is t o se t up the d e s i r e d 161 C a n g l e s . THETAD(I) and DTHTAD(I). (1=1.NLINK). 162 C 163 IMPLICIT REAL'S (A-H.O-Z) 164 REAL'S THETAD(NLINK),DTHTAD(NLINK) 165 DATA PI/3.1415926/ 166 THETAD(1)*PI/2.0 167 THETAD(2)=PI/2.0 168 DTHTAD(1 ) =0.O 169 DTHTAD(2)=0.0 170 RETURN 171 END 173 SUBROUTINE INITIA(THETA.DTHETA,I2LINK) 174 C 143 175 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e i s to s e t up the i n i t i a l 176 C c o n d i t i o n s . 177 C 178 IMPLICIT REAL*8 (A-H.O-Z) 179 RE AL *8 THETA(12LINK),DTHETA(12LINK) 180 NLINK=I2LINK/2 181 DO 10 1=1.NLINK 182 10 THETA(I)=0.0 183 DO 20 1=1.NLINK 184 THETA(I+NLINK)=0.0 185 20 DTHETA(I)=THETA(I+NLINK) 186 RETURN 187 END 188 c===================================================================== 189 SUBROUTINE GRATOR(RMASS,RLENG,THETA.GT) 190 C 191 C T h i s s u b r o u t i n e Is t o e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 192 C 193 IMPLICIT REAL*8 (A-H.O-Z) 194 REAL*8 THETA(4),RMASS(2).RLENG(2),GT(2) 195 DATA GRA/32.174/ 196 GT(1) = ((RMASS(1)+RMASS(2))*RLENG(1)*DSIN(THETA( 1)) + RMASS( 2)*RLEN 197 1 G(2)*D,SIN(THETA( 1 ) + THETA(2)))'GRA 198 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 199 RETURN 200 END 201 C 202 SUBROUTINE COUINT(RJO.CLM) 203 C 204 C T h i s s u b r o u t i n e 1s t o c a l c u l a t e the c o u p l i n g m a t r i x . 205 C 206 IMPLICIT REAL*8 (A-H.O-Z) 207 REAL'S CLM(2,2).RJO(2) 208 CLM(1. 1 )=RJ0(2)/RJ0( 1) 209 CLM(2,2)=0.00 210 CLM(1,2)=0.D0 211 CLM(2,1)=0.D0 212 RETURN 213 END \ 144 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 4 1 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 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 S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e i s to p r o v i d e d l f f e r e n t a i l e q u a t i o n s and n e c e s s a r y d a t a f o r the main program FORSIM t o s i m u l a t e the pr o p o s e d c o n t r o l on SYSTEM 2. G l o b a l and l o c a l c o n t r o l s have d i f f e r e n t s a m p l i n g i n s t a n t s . SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT.METHOD.CONVGD.KFLAG.NO.FIX.ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT.DTMAX,DTMIN,DTOUT.EMAX.ICHECK.NIMPL,I PLOT. 1JPLOT.TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL*8 THETAD(2),DTHTAD(2),RMASS(2),RLENG(2),RJO(2) REAL*8 OMEGA(2),ZETA(2),GAINP(2),GAINPS(2),GAINV(2),GAINVS(2) REAL*8 GT(2),TLOCAL(2),TGL0BL(2),TT0TAL(2) REAL*8 ERROR(2),DERR0R(2 ) ,DE(2) ,CLM(2,2) INTEGER ISAMG,ISAML,ITIMEG,ITIMEL,I2LINK.NLINK DATA RMASS,RLENG.OMEGA,ZETA/2*1.0.2*2.0,2*10.0,2*1.0/ DATA NLINK,ITIMEG,ITIMEL/2,10,5/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 10O0 I2LINK=2*NLINK ISAMG=0 ISAML=0 The d e s i r e d a n g l e s THETAD(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHTAD(I), (1=1,NLINK) CALL DESIRE(THETAD,DTHTAD,NLINK) The I n i t i a l c o n d i t i o n s o f the a n g l e s THETA (I ) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1,NLINK). CALL INITIA(THETA,DTHETA,I2LINK) Output v a l u e s of the mass & l e n g t h of the m a n i p u l a t o r ' s l i n k s . 145 59 IF(IPARAM.EO.0) WRITE(6,40) RMASS,RLENG 60 61 C 62 C E v a l u a t e the auto-moments of I n e r t i a R J O ( I ) , I n t e r a c t e d moment of 63 C I n e r t i a R J I N ( I ) , d e t e r m i n e the I n t e r a c t i o n f a c t o r FACTOR(I) and 64 C f e e d back g a i n s GAINP(I) and GAINV(I). (1=1,NLINK). 65 C 66 67 CALL MOINER(RMASS.RLENG,RJO,NLINK) 68 CALL PDGAIN(OMEGA,ZETA.RJO.GAINP,GAINV,NLINK) 69 70 71 c C. 72 C . S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 73 C . 74 C 75 76 77 C 78 C The c o n t r o l a l g o r i t h m . 79 c 80 81 10O0 IF(I0UT.NE.1) GOTO 2000 82 ISAMG=ISAMG+1 83 IF(ISAMG.GT.ITIMEG) ISAMG=1 84 ISAML=ISAML+1 85 IF(ISAML.GT.ITIMEL) ISAML=1 86 IF (ISAMG.NE.1) GOTO 3000 87 C 88 C The g r a v i t y t o r q u e G T ( I ) . (1=1,NLINK) 89 C 90 CALL COUINT(THETA,CLM) 91 CALL GRATOR(RMASS,RLENG,THETA,GT) 92 93 C 94 C E v a l u a t e the d i f f e r e n c e s between'the d e s i r e d a n g e l s and the a c t u a l 95 C ones, ERROR(I ), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I). 96 C 97 98 30OO CALL DETERR(THETAD,DTHTAD,THETA,DTHETA,ERROR,DERROR,I2LINK, 99 1 NLINK) 100 101 IF (ISAML.NE.1) GOTO 40O0 102 C 103 C Determine the s w i t c h c o n d i t i o n and s e t up the a p p r o p r i a t e c o n t r o l . 104 C 105 106 CALL PDADAP(ERROR,DERROR,OMEGA,GAINP.GAINPS,GAINV,GAINVS,NLINK) 107 C 108 C S l i d i n g l i n e s D E ( I ) . (1=1,NLINK). 109 C 1 10 1 1 1 4000 DO 20 I = 1. NLINK 112 20 D E ( I ) » -OMEGA(I)*ERROR(I) 1 13 1 14 C 1 15 C The l o c a l c o n t r o l law. 1 16 C 146 117 118 CALL LOCALC(ERROR,DERROR,GAINPS,GAINVS,TLOCAL.NLINK) 1 19 120 C 121 C The g l o b a l c o n t r o l law. 122 C 123 124 CALL GLOBLC(TLOCAL,CLM.GT.TGLOBL.NLINK) 125 126 C 127 C The t o t a l c o n t r o l e f f e c t . 128 C 129 130 CALL TOTALC(TLOCAL.TGLOBL,TTOTAL,NLINK) 131 132 C 133 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I. 134 C 135 136 2000 CALL EXPTWO(RMASS.RLENG.TTOTAL,THETA,DTHETA) 137 138 C 139 C. 140 C. S e c t i o n 4 : Output. 141 C. 142 C 143 144 IF(IOUT.EO.O) RETURN 145 WRITE(6,50)T,DT,THETA 146 40 FORMAT('M1=',F12.6.2X,'M2=',F12.6.2X,'L1='.F12.6,2X,'L2=',F12.6, 147 1 /.16X,'Time'.16X,'Step'.16X,'Thetal'.12X.'Theta2'.12X. 148 2 'Dthetal'.11X,'Dtheta2') 149 50 FORMAT(6G12.6) 150 WRITE(11.60) T,ERROR,DERROR,DE 151 WRITEM2.60) T , THETAD , THETA , DTHETA( 3 ), DTHETA( 4 ) 152 WRITEO3.60) T.TTOTAL 153 WRITE(14,60) T,GAINPS,GAINVS 154 60 FORMAT(9(1X.G12.6)) 155 RETURN 156 END 157 c====== ======================= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 158 SUBROUTINE DESIRE(THETAD,DTHTAD,NLINK) 159 C 160 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e 1s t o se t up the d e s i r e d 161 C a n g l e s , THETAD(I) and DTHTAD(I). (1=1,NLINK). 162 C 163 IMPLICIT REAL'S (A-H.O-Z) 164 REAL*8 THETAD(NLINK),DTHTAD(NLINK) 165 DATA PI/3. 1415926/ 166 THETAD( 1 )=PI/2 .0 167 THETAD(2)=PI/2.0 168 DTHTAD(1)=0.0 169 DTHTAD(2)=0.0 170 RETURN 171 END 172 C===================================================================== 173 SUBROUTINE INITIA(THETA.DTHETA,I2LINK) 174 C 147 175 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e i s to s e t up the i n i t i a l 176 c c o n d i t i o n s . 177 c 178 IMPLICIT REAL*8 (A-H.O-Z) 179 REAL*8 THETA(I2LINK),DTHETA(I2LINK) 180 NLINK=I2LINK/2 181 DO 10 1=1.NLINK 182 10 THETA(I)=0.0 183 DO 20 1=1,NLINK 184 THETA(I+NLINK)=0.0 185 20 DTHETA(I)=THETA(I+NLINK) 186 RETURN 187 188 END 189 SUBROUTINE GRATOR(RMASS.RLENG,THETA,GT) 190 IMPLICIT REAL*8 (A-H.O-Z) 191 REAL 'S THETA(4).RMASS(2),RLENG(2).GT(2) 192 DATA GRA/32.174/ 193 GT(1)=GRA*(RMASS(1)*RLENG(1)+RMASS(2)*(RLENG(1)-RLENG(2)*DCOS 194 1 (THETA(2))))*DCOS(THETA(1)) 195 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1) ) *DSIN(THETA(2)) 196 RETURN 197 END 198 C--199 SUBROUTINE COUINT(THETA.CLM) 200 C T h i s s u b r o u t i n e i s to c a l c u l a t e the c o u p l i n g m a t r i x . 201 C 202 IMPLICIT REAL 'S (A-H.O-Z) 203 REAL 'S CLM(2,2).THETA(4) 204 CLM(1.1)=1.DO 205 CLM(2.2)=O.DO 206 CLM(1,2)=0.D0 207 CLM(2.1)=0.D0 208 RETURN 209 END 148 1 2 3 4 5 e 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 4B 49 50 51 52 53 54 55 56 57 58 C = C c c c c c-c. c. c. c. c. c. c. c. c. c. c c c c c c c S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e i s t o p r o v i d e d l f f e r e n t a i l e q u a t i o n s and n e c e s s a r y d a t a f o r the main program FORSIM to s i m u l a t e the p r o p o s e d c o n t r o l on SYSTEM 3. SUBROUTINE UPDATE IMPLICIT REAL*8(A-H.0-2) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD,CONVGD,KFLAG,NO,FIX,ABDUMP.IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT,DTMAX,DTMIN,DTOUT,EMAX,ICHECK,NIMPL,IPLOT, UPLOT , TF IN COMMON /INEGLT/ THETA(6) COMMON /DERIVT/ DTHETA(6) REAL'S RMASS(3),RLENG(3),CLM(3.3) REAL'S OMEGA(3),ZETA(3),RJ0(3).GAINP(3),GAINV(3) REAL'S THETAD(3),DTHTAD(3),ERR0R(3),DERR0R(3),DE(3) REAL*8 GAINPS(3).GAINVS(3),GT(3),TLOCAL(3),TGLOBL(3),TT0TAL(3) INTEGER ISAMG.ISAML.ITIMEG,IT IMEL.12LINK,NLINK DATA RMASS,RLENG,OMEGA.ZETA/O.0,0.624.0.782,3*0.23.3*10.,3*1.0/ DATA NLINK,ITIMEG,ITIMEL/3,10,10/ S e c t i o n 2 I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAMG=0 ISAML=0 The d e s i r e d a n g l e s THETAD(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHTAD(I), (1=1.NLINK). CALL DESIRE(THETAD,DTHTAD.NLINK) The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I). (1=1.NLINK). CALL INITIA(THETA,DTHETA,I2LINK) Output the parameters f o r the dynamic e q u a t i o n s . IF(IPARAM.EO.O) WRITE(6,40) RMASS,RLENG 149 59 SO C G1 C E v a l u a t e the moments of I n e r t i a R J O ( I ) . i n t e r a c t e d moment of 62 C I n e r t i a R J I N ( I ) and f a c t o r FACTOR(I) and f e e d back g a i n s 63 C GAINP(I) a n d G A I N V ( I ) , (1=1,NLINK) 64 C 65 66 CALL MOINER(RMASS,RLENG,RJO,NLINK) 67 RJO(1)=0.0243 68 CALL PDGAIN(OMEGA,ZETA,RJO,GAINP,GAINV,NLINK) 69 CALL COUINT(RJO,CLM) 70 C 71 C. 72 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 73 C. 74 C 75 76 77 C 78 C Determlng the Input t o r q u e s T T O T A L ( a r r a y ) . 79 C 80 81 1000 IF(IOUT.NE.1) GOTO 2000 82 ISAMG=ISAMG*1 83 IF(ISAMG.GT.ITIMEG) ISAMG=1 84 ISAML=ISAML+1 85 IF(ISAML.GT.ITIMEL) ISAML=1 86 IF(ISAMG.NE.1) GOTO 3000 87 C 88 C The g r a v i t y t o r q u e G T ( I ) , (1=1.NLINK). 89 C 90 CALL GRATOR(RMASS.RLENG.THETA. GT) 91 92 C 93 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 94 C ones, ERROR(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I ) . 95 C 96 97 30O0 CALL DETERR(THETAD,DTHTAD,THETA,DTHETA,ERROR.DERROR,I2LINK, 98 1 NLINK) 99 100 C 101 C Determine the s w i t c h time and s e t up the c o n t r o l parameters. 102 C 103 104 IF(ISAML.NE.1) GOTO 4000 105 CALL PDADAP(ERROR,DERROR,OMEGA.GAINP.GAINPS,GAINV,GAINVS,NLINK) 106 C 107 C SI1d1ng 11nes. 108 C 109 110 40OO DO 20 I = 1, NLINK 111 20 D E ( I ) = -OMEGA(I)*ERROR(I) 1 12 113 C 114 C The l o c a l c o n t r o l law. 115 C 116 150 117 CALL LOCALC(ERROR,DERROR,GAINPS,GAINVS,TLOCAL.NLINK) 1 18 1 19 C 120 C The g l o b a l c o n t r o l law. 121 C 122 123 CALL GL0BLC(TLOCAL,CLM,GT,TGLOBL.NLINK) 124 125 C 126 C The t o t a l c o n t r o l e f f e c t . 127 C 128 129 CALL TOTALC(TLOCAL.TGLOBL.TTOTAL.NLINK) 130 131 C 132 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e MANI3L. 133 C 134 135 2000 CALL MANI3L(RMASS.RLENG,TTOTAL,THETA.DTHETA) 136 137 C 138 C. 139 C. S e c t i o n 4 : Output. 140 C. 141 C 142 143 IF(IOUT.EO.O) RETURN 144 WRITE(6,50)T,DT,THETA 145 40 FORMAT('M1=',F12.6.1X,'M2='.F12.6.1X.'M3='.F12.6.1X,'L1='.F12.6. 146 1 1X.'L2='.F12.6, 1X,'L3=' .F12.6./,16X'T1me',16X,'Step',16X, 147 2 'Thetal',12X,'Theta2'.12X.'Theta3',11X,'Dthetal ' , 11X , 148 3 'Dtheta2'.11X.'Dtheta3') 149 50 F0RMAT(8G12.6) 150 WRITE(11,60) T,ERROR,DERROR,DE 151 WRITE*12.60) T.THETAD,THETA 152 WRITE(13,60) T,TTOTAL 153 WRITE*14,60) T.GAINPS,GAINVS 154 60 FORMAT*13(1X.G12.6)) 155 RETURN 156 END 157 c===================================================================== 158 SUBROUTINE DESI RE(THETAD,DTHTAD,NLINK) 159 C 160 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e Is t o s e t up the d e s i r e d 161 C a n g l e s . THETAD(I) and DTHTAD(I). (1=1.NLINK). 162 C 163 IMPLICIT REAL*8 (A-H.O-Z) 164 REAL*8 THETAD(NLINK),DTHTAD(NLINK) 165 DATA PI/3.1415926/ 166 THETAD(1 ) = PI/2.0 167 THETAD(2)=PI/2.0 168 THETAD(3)=PI/2.0 169 DTHTAD(1)=0.0 170 DTHTAD(2)=0.0 171 DTHTAD(3)=0.0 172 RETURN 173 END 174 c===================================================================== 151 175 SUBROUTINE INITIA(THETA,DTHETA,I2LINK) 176 C 177 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e 1s to s e t up the i n i t i a l 178 C c o n d l t I o n s . 179 C 180 IMPLICIT REAL*8 (A-H.O-Z) 181 REAL 'S THETA(I2LINK),DTHETA(I2LINK) 182 NLINK=I2LINK/2 183 DO 10 1=1,NLINK 184 10 THETA(I)=0.0 185 DO 20 1=1,NLINK 186 THETA(I+NLINK)=0.0 187 20 DTHETA(I)=THETA(I+NLINK) 188 RETURN 189 190 END 191 SUBROUTINE GRATOR(RMASS,RLENG,THETA,GT) 192 C 193 C T h i s s u b r o u t i n e Is t o e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 194 C 195 IMPLICIT REAL*8 (A-H.O-Z) 196 REAL 'S THETA(6),RMASS(3).RLENG(3).GT ( 3) 197 DATA GRA/9.8/ 198 GT(1)=0.0 199 GT(2)=RMASS(2)'GRA'RLENG(2)*DC0S(THETA(2)) + RMASS(3 ) »GRA*RLENG(2 ) 200 1 *DC0S(THETA(2) ) 201 GT(3)=RMASS(3)*GRA*RLENG(3)*DC0S(THETA(3)) 202 RETURN 203 END 204 C--205 SUBROUTINE COUINT(JO.CLM) 206 C T h i s s u b r o u t i n e i s t o c a l c u l a t e the c o u p l i n g m a t r i x . 207 C 208 IMPLICIT REAL 'S (A-H.O-Z) 209 REAL*8 CLM(3.3),dO(3) 210 CLM(1.1)=(dO(2)+dO(3))/dO(1) 21 1 CLM(1,2)=0.D0 212 CLM(1,3)=0.D0 213 CLM(2.1)=0.D0 214 CLM(2,2)=dO(3)/dO(2) 215 CLM(2,3)=0.D0 216 CLM(3,1)=0.D0 217 CLM(3.2)=O.DO 218 CLM(3,3)=0.D0 219 RETURN 220 END 152 1 C = 2 C 3 C 4 C 5 C 6 C 6.5 C 7 C-8 9 10 1 1 12 C 13 C. 14 C. 15 C . 16 C 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 C 32 C . 33 C. 34 C . 35 C 36 37 38 39 41 C 42 C 43 C 44 C 45 46 47 48 C 49 C 50 C 51 52 53 54 C 55 c 56 c 57 c 58 c S u b r o u t i n e D e s c r i t l o n : T h i s s u b r o u t i n e 1s t o p r o v i d e d l f f e r e n t a l l q u a t l o n s and n e c e s s a r y d a t a f o r the main program FORSIM to s i m u l a t e c o n v e n t i o n a l c o n t r o l on SYSTEM 1 f o r t r a j e c t o r y f o l l o w i n g ( f o l l o w i n g a square wave) . SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT.METHOD.CONVGD.KFLAG.NO,FIX.ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT,DTMAX,DTMIN,DTOUT,EMAX,ICHECK,NIMPL,IPLOT, UPLOT , TF IN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL'S THETAD(2 ) ,DTHTAD(2),RMASS(2),RLENG(2),RJ0(2),RJIN(2) REAL'S OMEGA(2),ZETA(2).GAINP(2),GAINV(2) REAL'S GT(2),FACT0R(2),DGAIN(2),TL0CAL(2),TGLOBL(2),TT0TAL(2) REAL*8 ERROR(2),DERROR(2 ) .CURVE(2),DE(2) INTEGER ISAM,ITIMES,I2LINK,NLINK DATA RMASS.RLENG.OMEGA.ZETA/2*1.0,2*2.0.2*10.0,2*1.0/ DATA NLINK.ITIMES/2.10/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAM=0 The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I). (1=1.NLINK). CALL INITIA(THETA,DTHETA.I2LINK) Output v a l u e s of the mass & l e n g t h of the m a n i p u l a t o r ' s l i n k s . IF(IPARAM.EQ.O) WRITE(6.40) RMASS.RLENG E v a l u a t e moment of I n e r t i a RUO(I), i n t e r a c t e d moment of I n e r t i a R J I N ( I ) , d e t e r m i n e the I n t e r a c t i o n f a c t o r FACTOR(I) and f e e d back g a i n s GAINP(I) and GAINV(I). (1=1,NLINK). 153 59 SO CALL MOINER(RMASS,RLENG,RJO,NLINK) 61 CALL PDGAIN(OMEGA,ZETA,RJO,GAINP.GAINV.NLINK) 62 CALL INTMOl(RJO,RJIN,NLINK) 63 CALL EVAFAC(RJIN,RJO,FACTOR,NLINK) 64 65 C 66 C. 67 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 68 C. 69 C 70 71 72 C 73 C The c o n t r o l a l g o r i t h m . The g l o b a l c o n t r o l has a sampl i n g 74 C I n s t a n t ITIMES times of the s i m u l a t i o n s t e p DT. 75 C 76 77 1000 IF(IOUT.NE.1) GOTO 20O0 78 ISAM=ISAM+1 79 IF(ISAM.GT.ITIMES) ISAM=1 82 IF (ISAM.NE.1) GOTO 3000 83 C 84 C The g r a v i t y t o r q u e G T ( I ) , (1=1,NLINK) 85 C 86 CALL DESIRE(T,THETAD,DTHTAD,NLINK) 87 CALL GRATOR(RMASS,RLENG,THETA,GT) 88 89 C 90 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 91 C ones, ERROR(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I ) . 92 C 93 3000 CALL DETERR(THETAD,DTHTAD,THETA,DTHETA,ERROR.DERROR,I2LINK, 94 1 NLINK) 103 C 104 C S l i d i n g l i n e s D E ( I ) , (1=1.NLINK). 105 C 106 4000 DO 20 I = 1. NLINK 107 20 D E ( I ) = -OMEGA( I)*ERROR( I ) 108 109 C 110 C The l o c a l c o n t r o l law. 111 C 1 12 113 CALL LOCALC(ERROR,DERROR.GAINP,GAINV.TLOCAL,NLINK) 1 14 1 15 C 116 C The g l o b a l c o n t r o l law. 117 C 1 18 119 CALL GLOBLC(TLOCAL.FACTOR.GT.TGLOBL,NLINK) 120 121 C 122 C The t o t a l c o n t r o l e f f e c t . 123 C 124 125 CALL TOTALC(TLOCAL,TGLOBL.TTOTAL.NLINK) 126 1 54 127 C 128 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I. 129 C 130 131 2000 CALL EXPONE(RMASS,RLENG.TTOTAL.THETA,DTHETA) 132 133 C : 134 C. 135 C. S e c t i o n 4 : Output. 136 C. 137 C 138 139 IF(IOUT.EO.O) RETURN 140 WRITE(6,50)T,DT,THETA 14 1 40 FORMAT ( 'M1=',F12.6,2X,'M2=',F12.6.2X,'L1=',F12.6.2X,'L2=',F12.6. 142 1 /,16X,'Time'. 16X.'Step',16X,'Thetal'.12X,'Theta2' , 12X, 143 2 'Dthetal',11X,'Dtheta2') 144 50 F0RMAT(6(1X.G12.6)) 145 WRITE(11,60) T.ERROR.DERROR.DE 146 WRITEO2.60) T , THE TAD, THETA , DTHETA ( 3 ). DTHETA ( 4 ) 147 WRITE(13.60) T,TTOTAL 148 WRITE(14,60) T,GAINPS,GAINVS 149 60 FORMAT(9(1X.G12.6)) 150 RETURN 151 END 152 c===================================================================== 153 SUBROUTINE DESIRE(T,THETAD.DTHTAD,NLINK) 154 C 155 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e 1s t o set up the d e s i r e d 156 C a n g l e s . THETAD(I) and DTHTAD( I ), (1 = 1,NLINK). 157 C 158 IMPLICIT REAL*8 (A-H.O-Z) 159 REAL*8 THETAD(NLINK),DTHTAD(NLINK) 160 THETAD(1)=0.O 161 THETAD(2)=0.0 162 DTHTAD(1)=0.0 163 DTHTAD(2)=DTHTAD(1) 164 IF (T.GT.1.0.AND.T.LT.2) RETURN 165 IF (T.GT.3.0) RETURN 166 THETAD(1)=1.O 167 THETAD(2)=1.0 168 RETURN 169 END 170 C===================================================================== 171 SUBROUTINE INITIA(THETA,DTHETA.I2LINK) 172 C 173 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e i s t o se t up the I n i t i a l 174 C c o n d i t i o n s . 175 C 176 IMPLICIT REAL'S (A-H.O-Z) 177 REAL*8 THETA(I2LINK),DTHETA(I2LINK) 178 NLINK=I2LINK/2 179 DO 10 1=1.NLINK 180 10 THETA(I)=0.0 181 DO 20 1=1,NLINK 182 THETA(I*NLINK)=0.O 183 20 DTHETA(I)=THETA(I+NLINK) 184 RETURN 155 185 END 186 C 187 SUBROUTINE GRATOR(RMASS,RLENG,THETA,GT) 188 C 189 C T h i s s u b r o u t i n e 1s t o e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 190 C 191 IMPLICIT REAL-8 (A-H.O-Z) 192 REAL'S THETA(4),RMASS(2).RLENG(2),GT(2) 193 DATA GRA/32.174/ 194 GT( 1 ) = ((RMASS( 1 )+RMASS(2 ) ) *RLENG(1)*DSIN(THETA(1)) + RMASS( 2) *RLEN 195 1 G(2)*DSIN(THETA(1)+THETA(2)))*GRA 196 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 197 RETURN 198 END 156 1 2 C 3 C 4 C 5 C 6 C 6.5 C 7 C-8 9 10 11 12 C 13 C 14 C 15 C 16 C 17 18 19 20 21 22 23 24 25 26 27 28 29 30 C 31 c. 32 c. 33 c. 34 c 35 36 37 38 39 c 40 c 4 1 c 42 c 43 44 45 46 47 48 c 49 c 50 c 51 c 52 c 53 54 55 56 57 S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e Is to p r o v i d e d l f f e r e n t a i l q u a t l o n s and n e c e s s a r y d a t a f o r the main program FORSIM to s i m u l a t e c o n v e n t i o n a l c o n t r o l on SYSTEM 1 f o r t r a j e c t o r y f o l l o w i n g ( f o l l o w i n g a s t r a i g h t 11ne). SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD,CONVGD,KFLAG,NO,FIX,ABDUMP,IPARAM LOGICAL CONVGD,FIX COMMON /RESERD/ T,DT.DTMAX,DTMIN,DTOUT,EMAX.ICHECK,NIMPL,IPLOT, 1UPL0T,TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL*8 THETAD(2),RMASS(2),RLENG(2),TLOCAL(2),OMEGA(2),ZETA(2) REAL'S TGLOBL(2),TTOTAL(2),GAINP(2),GAINV(2),Rd(2),DE(2) REAL*8 ERR0R(2),DERROR(2),RdIN(2).DTHTAD(2),GT(2),FACTOR(2) INTEGER ISAM,ITIMES DATA RMASS.RLENG,OMEGA,ZETA/2*1.0.2*2.0,2*10.0,2*1.0/ DATA NLINK,ITIMES/2.10/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAM=0 The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1,NLINK) CALL INITIA(THETA,DTHETA,I2LINK) IF(IPARAM.EQ.O) WRITE(6,40) (RMASS(I),1=1,2),(RLENG(I),1=1,2) E v a l u a t e the moments of I n e r t i a R d ( I ) , accumulated moment of i n e r t i a RdAC(I) and f a c t o r FACTOR(I) and f e e d back g a i n s GAINP(I) and GAINV(I). (1=1.NLINK) CALL MOINER(RMASS,RLENG,Rd,NLINK) CALL PDGAIN(OMEGA,ZETA.Rd,GAINP,GAINV,NLINK) CALL INTMOI(RJ.RdIN,NLINK) CALL EVAFAC(RdlN.Rd.FACTOR,NLINK) 157 58 C 59 C. 60 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 61 C. 62 C 63 64 65 C 66 C Determing the in p u t t o r q u e s T T 0 T A L ( a r r a y ) . 67 C 68 69 10O0 IF(IOUT.NE.1) GOTO 20O0 70 ISAM=ISAM+1 71 IF (ISAM.GT.ITIMES) ISAM=1 72 IF (ISAM.NE.1) GOTO 3000 73 C 74 C The g r a v i t y t o r q u e G T ( I ) . (1=1.2) 75 C 76 CALL DESIRE(T.THETAD.DTHTAD,NLINK) 77 CALL GRATOR(RMASS,RLENG,THETA,GT) 78 79 C 80 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 81 C ones THETA(I). and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I) 82 C 83 84 30O0 CALL DETERR(THETAD,DTHTAD.THETA,DTHETA,ERROR,DERROR,I2LINK.NLINK) 85 DO 10 1=1.NLINK 86 10 DE(I ) =-OMEGA(I)*ERROR(I) 87 C 88 C The l o c a l c o n t r o l law. 89 C 90 91 CALL LOCALC(ERROR,DERROR.GAINP,GAINV,TLOCAL,NLINK) 92 93 C 94 C The g l o b a l c o n t r o l law 95 C 96 97 CALL GLOBLC(TLOCAL.FACTOR,GT.TGLOBL,NLINK) 98 99 C 100 C The t o t a l c o n t r o l law 101 C 102 103 CALL TOTALC(TLOCAL.TGLOBL.TTOTAL,NLINK) 104 105 C 106 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I 107 C 108 109 2000 CALL EXPONE(RMASS.RLENG,TTOTAL.THETA.DTHETA) 1 10 111 C 1 12 C . 113 C. S e c t i o n 4 : Output. 1 14 C . 115 C 158 1 16 117 IF(IOUT.EQ.O) RETURN 118 WRITE(6,50)T.DT,THETA 119 40 FORMAT('M1=',F12.6.2X.'M2='.F12.6,2X,'D1='.F12.6.2X.'D2=',F12.6, 120 1/,16X. 'TIME'.16X, 'STEP'.16X, ' THETA1'.12X,'THETA2'.12X,'DTHETA1', 121 1 1 1X,'DTHETA2') 122 50 F0RMAT(6F20.6) 123 XR=RLENG(1 )*DSIN(THETA( 1 ) )+ RLENG(2)*DSIN(THETA (1) + THETA( 2)) 124 YR=-RLENG(1)*DCOS(THETA(1))-RLENG(2)*DCOS(THETA(1)+THETA(2)) 125 IF (T.GT.4.0) GOTO 5OO0 126 XD=T 127 YD=XD-4.0 128 5000 WRITE(11.60) T,ERROR,DERROR.DE 129 WRITE(12.60) T,THETAD.THETA,DTHETA(3),DTHETA( 4) 130 WRITE(13,60) T.TTOTAL,XD,YD,XR,YR 131 WRITE(14,60) T.GAINP,GAINV 132 60 FORMAT(9(1X,G12.6)) 133 RETURN 134 END 135 c===================================================================== 136 SUBROUTINE INITIA(THETA,DTHETA,I2LINK) 137 IMPLICIT REAL*8 (A-H.O-Z) 138 REAL*8 THETA(I2LINK),DTHETA(I2LINK) 139 DATA PI/3.1415926/ 140 NLINK=I2LINK/2 141 DO 10 1=1.NLINK 142 10 THETA(I)=0.0 143 DO 20 1=1,NLINK 144 THETA(I+NLINK)=PI/4.O 145 20 DTHETA(I)=THETA(I+NLINK) 146 RETURN 147 END 149 SUBROUTINE GRATOR(RMASS,RLENG,THETA,GT) 150 IMPLICIT REAL'S (A-H.O-Z) 151 REAL*8 THETA(4),RMASS(2).RLENG(2),GT(2) 152 DATA GRA/32.174/ 153 GT(1) = ((RMASS(1 ) + RMASS(2 ) ) *RLENG(1)"DSIN(THETA(1))+RMASS(2)»RLEN 154 1G(2)*DSIN(THETA(1)+THETA(2)))*GRA 155 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 156 RETURN 157 END 158 c===================================================================== 159 SUBROUTINE DESIRE(T.THETAD.DTHTAD.NLINK) 160 C 161 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e 1s t o s e t up the d e s i r e d 162 C a n g l e s . THETAD(I) and DTHTAD(I), (1=1.NLINK). 163 C 164 IMPLICIT REAL*8 (A-H.O-Z) 165 REAL'S THETAD(NLINK),DTHTAD(NLINK),RLENG( 2) 166 DATA PI/3.1415926/ 167 DATA RLENG/2*2.0/ 168 IF (T.GT.4.0) RETURN 169 X=T 170 Y=X-4.0 171 CALL PATHPL(X.Y.RLENG.THETAD) 172 DTHTAD(1)=0.0 173 DTHTAD(2)=DTHTAD(1) 174 RETURN 175 END 159 1 2 C 3 C 4 C 5 C 6 C 6.5 C 7 c-8 9 10 11 12 C 13 C 14 C 15 C 16 C 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 C 32 C 33 C 34 C. 35 C 36 37 38 39 40 4 1 C 42 C 43 C 44 C 45 46 47 48 C 49 C 50 C 51 52 53 54 c 55 c 56 c 57 c S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e i s to p r o v i d e d l f f e r e n t a i l q u a t l o n s and n e c e s s a r y d a t a f o r the main program FORSIM t o s i m u l a t e the proposed c o n t r o l on SYSTEM 1 f o r t r a j e c t o r y f o l l o w i n g ( f o l l o w i n g a square wave) . SUBROUTINE UPDATE IMPLICIT REAL'8(A-H,0-Z) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT,METHOD,CONVGD,KFLAG.NO.FIX,ABDUMP,IPARAM LOGICAL CONVGD.FIX COMMON /RESERD/ T,DT.DTMAX.DTMIN,DTOUT,EMAX,ICHECK,NIMPL,IPLOT , 1JPLOT,TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL'S THETAD(2),DTHTAD(2),RMASS(2),RLENG(2),RJ0( 2) REAL'S 0MEGA(2).ZETA(2),GAINP(2),GAINPS(2).GAINV(2),GAINVS(2) REAL'S GT(2).TLOCAL(2),TGLOBL(2),TTOTAL(2) REAL*8 ERR0R(2) .DERROR(2) ,DE(2),CLM(2,2) INTEGER ISAMG,ISAML.ITIMEG,IT IMEL.I2LINK,NLINK DATA RMASS,RLENG,OMEGA,ZETA/2*1.0,2*2.0,2*10.0,2*1.0/ DATA NLINK,ITIMEG,ITIMEL/2.10.5/ S e c t i o n 2 I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAMG=0 ISAML=0 The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I), (1=1,NLINK). CALL INITIA(THETA,DTHETA.I2LINK) Output v a l u e s of the mass ft l e n g t h of the m a n i p u l a t o r ' s l i n k s . IF(IPARAM.EQ.O) WRITE(6,40) RMASS,RLENG E v a l u a t e moment of I n e r t i a R J O ( I ) , I n t e r a c t e d moment of i n e r t i a R J I N ( I ) . d e t e r m i n e the i n t e r a c t i o n f a c t o r FACTOR(I) and f e e d back g a i n s GAINP(I) and GAINV(I), (1=1.NLINK). 160 58 C 59 60 CALL MOINER(RMASS,RLENG.RJO.NLINK) 61 CALL PDGAIN(OMEGA, ZETA,RJO,GAINP,GAINV,NLINK) 62 CALL COUINT(RJO.CLM) 64 65 C . . . 66 C. 67 C. S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . 68 C. 69 C 70 71 72 C 73 C The c o n t r o l a l g o r i t h m . The g l o b a l c o n t r o l has a sampl i n g 74 C I n s t a n t ITIMES times of the s i m u l a t i o n s t e p DT. 75 C 76 77 10O0 IF (IOUT.NE.1) GOTO 2000 78 ISAMG=ISAMG+1 79 IF(ISAMG.GT.ITIMEG) ISAMG=1 80 ISAML=ISAML+1 81 IF(ISAML.GT.ITIMEL) ISAML=1 82 IF (ISAMG.NE.1) GOTO 3000 83 C 84 C The g r a v i t y t o r q u e G T ( I ) , (1=1,NLINK) 85 C 86 CALL DESIRE(T,THETAD,DTHTAD,NLINK) 87 CALL GRAT0R(RMASS,RLENG,THETA,GT) 88 89 C 90 C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l 91 C ones, ERROR(I), and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I). 92 C 93 3CO0 CALL DETERR(THETAD.DTHTAD.THETA,DTHETA,ERROR.DERROR.I2LINK, 94 1 NLINK) 95 IF (ISAML.NE.1) GOTO 4000 96 C 97 C Determine the feedback g a i n s GAINPS & GAINVS a d a p t l v e l y a c c o r d i n g 98 C t o ERROR and DERROR. 99 C 1CO 101 CALL PDADAP(ERROR,DERROR,OMEGA,GAINP.GAINPS,GAINV,GAINVS.NLINK) 103 C 104 C S l i d i n g l i n e s D E ( I ) , (1=1,NLINK). 105 C 106 4OO0 DO 20 I = 1, NLINK 107 20 D E ( I ) = -OMEGA(I)*ERROR(I) 108 109 C 110 C The l o c a l c o n t r o l law. 11 1 C 1 12 113 CALL LOCALC(ERR0R,DERROR,GAINPS,GAINVS,TLOCAL,NLINK) 1 14 1 15 C 116 C The g l o b a l c o n t r o l law. 117 C 161 1 18 119 CALL GLOBLC(TLOCAL,CLM,GT.TGLOBL,NLINK) 120 121 C 122 C The t o t a l c o n t r o l e f f e c t . 123 C 124 125 CALL TOTALC(TLOCAL,TGLOBL.TTOTAL,NLINK) 126 127 C 128 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I. 129 C 130 131 2000 CALL EXPONE(RMASS,RLENG,TTOTAL,THETA,DTHETA) 132 133 C 134 C. 135 C. S e c t i o n 4 : Output. 136 C. 137 C 138 139 IF(IOUT.EO.0) RETURN 140 WRITE(6,50)T,DT,THETA 14 1 40 FORMAT('M1=',F12.6,2X.'M2='.F12.6.2X.'L1='.F12.6,2X.'L2='.F12.6, 142 1 /,16X,'Time',16X,'Step',16X,'Thetal',12X.'Theta2',12X, 143 2 'Dthetal',11X,'Dtheta2') 144 50 FORMAT(6(1X,G12.6)) 145 WRITE(11,60) T,ERROR,DERROR,DE 146 WRITE(12,60) T,THETAD,THETA,DTHETA(3).DTHETA(4) 147 WRITEO3.60) T.TTOTAL 148 WRITE(14,60) T.GAINPS.GAINVS 149 60 FORMAT(9(1X.G12.6)) 150 RETURN 151 END 152 C===================================================================== 153 SUBROUTINE DESIRE(T,THETAD,DTHTAD.NLINK) 154 C 155 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e i s t o s e t up the d e s i r e d 156 C a n g l e s , THETAD(I) and DTHTAD(I), (1=1.NLINK). 157 C 158 IMPLICIT REAL*8 (A-H.O-Z) 159 REAL*8 THETAD(NLINK).DTHTAD(NLINK) 160 THETAD(1)=0.0 161 THETA0(2)=0.O 162 DTHTAD(1)=0.0 163 DTHTAD(2)=DTHTAD(1) 164 IF (T.GT.1.O.AND.T.LT.2) RETURN 165 IF (T.GT.3.0) RETURN 166 THETAD(1)=1.O 167 THETAD(2)=1.O 168 RETURN 169 END 170 C===================================================================== 171 SUBROUTINE INITIA(THETA,DTHETA,I2LINK) 172 C 173 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e 1s t o s e t up the I n i t i a l 174 C c o n d i t i o n s . 175 C 162 176 IMPLICIT REAL*8 (A-H.O-Z) 177 REAL*8 THETA(I2LINK).DTHETA(12LINK) 178 NLINK=I2LINK/2 179 DO 10 1*1.NLINK 180 10 THETA(I)=0.0 181 DO 20 1=1.NLINK 182 THETA(I+NLINK)=0.0 183 20 DTHETAd )= THETA(I+NLINK ) 184 RETURN 185 186 END 187 SUBROUTINE GRATOR(RMASS.RLENG,THETA,GT) 188 C 189 C T h i s s u b r o u t i n e 1s to e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 190 C 191 IMPLICIT REAL'S (A-H.O-Z) 192 REAL*8 THETA(4),RMASS(2),RLENG(2),GT(2) 193 DATA GRA/32.174/ 194 GT(1)=((RMASS(1J+RMASS(2))*RLENG(1)*DSIN(THETA(1))+RMASS(2)*RLEN 195 1 G(2)*DSIN(THETA(1)+THETA(2)))*GRA 196 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 197 RETURN 198 END 199 C--200 SUBROUTINE COUINT(RJO.CLM) 201 C 202 C T h i s s u b r o u t i n e Is t o c a l c u l a t e the c o u p l i n g m a t r i x . 203 C 204 IMPLICIT REAL'S (A-H.O-Z) 205 REAL*8 CLM(2,2),RJ0(2) 206 CLM(1, 1 )=RJ0(2 )/RJO(1 ) 207 CLM(2.2)=O.DO 208 CLM(1,2)=0.D0 209 CLM(2.1)=0.D0 210 RETURN 211 END 163 57 5B 59 60 61 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 78 . 5 78.7 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 100 101 102 103 104 105 106 107 108 109 1 IO 1 1 1 1 12 1 13 1 14 CALL MOINER(RMASS,RLENG,RJO.NLINK) CALL PDGAIN(OMEGA.ZETA.RJO,GAINP,GAINV,NLINK) CALL COUINT(RJO.CLM) S e c t i o n 3 : S p e c i f i c a t i o n of E q u a t i o n s . C C The c o n t r o l a l g o r i t h m . The g l o b a l c o n t r o l has a sampl i n g C I n s t a n t ITIMES times of the s i m u l a t i o n s t e p DT. C 1000 IF(IOUT.NE.1) GOTO 2000 ISAMG=ISAMG+1 IF(ISAMG.GT.ITIMEG) ISAMG=1 ISAML=ISAML+1 IF(ISAML.GT.ITIMEL) ISAML=1 IF (ISAMG.NE.1) GOTO 3000 C C The g r a v i t y t o r q u e G T ( I ) , (1=1,NLINK) C CALL DESIRE(T,THETAD,DTHTAD.NLINK) CALL GRATOR(RMASS,RLENG,THETA,GT) C C E v a l u a t e the d i f f e r e n c e s between the d e s i r e d a n g e l s and the a c t u a l C ones. ERROR(I). and t h e i r f i r s t o r d e r d e r i v a t i v e s DERROR(I). C 3000 CALL DETERR(THETAD,DTHTAD.THETA.DTHETA.ERROR,DERROR,I2LINK. 1 NLINK) IF (ISAML.NE.1) GOTO 4000 C C Determine the feedback g a i n s GAINPS & GAINVS a d a p t i v e l y a c c o r d i n g C t o ERROR and DERROR. C CALL PDADAP(ERROR,DERROR,OMEGA,GAINP,GAINPS,GAINV,GAINVS.NLINK) C C S l i d i n g l i n e s D E ( I ) . (1=1.NLINK). C 4CO0 DO 20 I = 1. NLINK 20 D E ( I ) = -OMEGA(I)*ERROR(I) C C The l o c a l c o n t r o l law. C CALL LOCALC(ERROR.DERROR,GAINPS.GAINVS.TLOCAL.NLINK) C C The g l o b a l c o n t r o l law. C 164 1 2 C 3 C 4 c 5 c 6 c 6 5 c 7 c-8 9 10 1 1 12 c 13 c 14 c 15 c 16 c 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 c 32 c 33 c 34 c 35 c 36 37 38 39 39 5 40 c 41 c 42 c 43 c 44 45 46 47 c 48 c 49 c 50 51 52 53 c 54 c 55 c 56 c S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e Is t o p r o v i d e d i f f e r e n t a l l q u a t i o n s and n e c e s s a r y d a t a f o r the main program F0RSIM to s i m u l a t e the proposed c o n t r o l s t r a t e g y on SYSTEM 1 f o r t r a j e c t o r y f o l l o w i n g ( f o l l o w i n g a s t r a i g h t 1 ( n e ). SUBROUTINE UPDATE IMPLICIT REAL*8(A-H,0-Z ) S e c t i o n 1 : S t o r a g e . COMMON /CONTRL/ IOUT.METHOD,CONVGD,KFLAG,NO.FIX.ABDUMP,IPARAM LOGICAL CONVGD.FIX COMMON /RESERD/ T,DT,DTMAX.DTMIN,DTOUT,EMAX,ICHECK.NIMPL,IPLOT, 1JPLOT,TFIN COMMON /INEGLT/ THETA(4) COMMON /DERIVT/ DTHETA(4) REAL*8 THETAD(2),DTHTAD(2),RMASS(2),RLENG(2),RJ0( 2) REAL*8 OMEGA(2),ZETA(2),GAINP(2),GAINPS(2),GAINV(2),GAINVS(2) REAL*8 GT(2).TLOCAL(2).TGLOBL(2),TTOTAL(2) REAL'S ERROR(2),DERROR(2),DEI 2).CLM(2.2) INTEGER ISAMG.ISAML.ITIMEG.ITI MEL,12LINK,NLINK DATA RMASS.RLENG,OMEGA,ZETA/2*1.0.2*2.0.2*10.0.2*1.0/ DATA NLINK.ITIMEG.ITIMEL/2,10.5/ S e c t i o n 2 : I n i t i a l C o n d i t i o n . IF (T.NE.O.OO) GOTO 1000 I2LINK=2*NLINK ISAMG=0 ISAML=0 The I n i t i a l c o n d i t i o n s of the a n g l e s THETA(I) and t h e i r f i r s t o r d e r d e r i v a t i v e s DTHETA(I). (1=1,NLINK). CALL IN1TIA(THETA,DTHETA,I2LINK) Output v a l u e s of the mass tV l e n g t h of the m a n i p u l a t o r ' s l i n k s . IF(IPARAM.EO.O) WRITE(6.40) RMASS,RLENG E v a l u a t e moment of i n e r t i a R J O ( I ) , i n t e r a c t e d moment of I n e r t i a R J I N ( I ) . d e t e r m i n e the i n t e r a c t i o n f a c t o r FACTOR(I) and f e e d back g a i n s GAINP(I) and GAINV(I), (1=1,NLINK). 165 1 15 116 CALL QLOBLC(TLOCAL,CLM,GT,TGLOBL,NLINK) 1 17 1 18 C 119 C The t o t a l c o n t r o l e f f e c t . 120 C 121 122 CALL TOTALC(TLOCAL.TGLOBL.TTOTAL.NLINK) 123 124 C 125 C D i f f e r e n t i a l E q u a t i o n s To S i m u l a t e EXPERI.I. 126 C 127 128 2000 CALL EXPONE(RMASS,RLENG,TTOTAL.THETA,DTHETA) 129 130 C 131 C. 132 C. S e c t i o n 4 : Output. 133 C. 134 C 135 136 I F ( I OUT.EQ.O) RETURN 137 WRIT E(6,50)T,DT,THETA 138 40 FORMAT('M1=',F12.6,2X.'M2=',F12.6,2X.'L1='.F12.6.2X,'L2=',F12.6, 139 1 /.16X,'Time'.16X.'Step',16X.'Thetal',12X,'Theta2',12X, 140 2 'Dthetal',11X,'Dtheta2') 141 50 FORMAT(6(1X.G12.6)) 142 XR=RLENG(1)*DSIN(THETA(1))+RLENG(2)*DSIN(THETA ( 1) + THETA(2)) 143 YR=-RLENG(1 ) *DCOS(THETA(1))-RLENG(2)*DCOS(THETA(1) + THETA ( 2)) 144 IF (T.GT.4.0) GOTO 5000 145 XD = T 146 YD=XD-4.0 147 5000 WRITE(11.60) T.ERROR,DERROR.DE 148 WRITE(12.60) T.THETAD,THETA,DTHETA(3),DTHETA(4) 149 WRITEO3.60) T . TTOTAL . XD . YD . XR . YR 150 WRITE(14.60) T,GAINPS,GAINVS 151 60 FORMAT(9(1X.G12.6)) 152 RETURN 153 END 154 c===================================================================== 155 SUBROUTINE DESIRE(T,THETAD,DTHTAD,NLINK) 156 C 157 C S u b r o u t i n e D e s c r i p t i o n : T h i s s u b r o u t i n e Is to s e t up the d e s i r e d 158 C a n g l e s , THETAD(I) and DTHTAD(I). (1=1.NLINK). 159 C 160 IMPLICIT REAL*8 (A-H.O-Z) 161 REAL'S THETAD(NLINK).DTHTAD(NLINK).RLENG( 2) 162 DATA PI/3. 1415926/ 163 DATA RLENG/2*2.0/ 164 IF (T.GT.4.0) RETURN 165 X=T 166 Y=X-4.0 167 CALL PATHPL(X.Y.RLENG,THETAD) 168 DTHTAD(1)=0.O 169 DTHTAD(2)=DTHTAD(1) 170 RETURN 171 END 172 C===================================================================== 166 173 SUBROUTINE INITIA(THETA.DTHETA.I2LINK) 174 C 175 C S u b r o u t i n e D e s c r i p t i o n : t h i s s u b r o u t i n e 1s to s e t up the i n i t i a l 176 C c o n d l t i o n s . 177 C 178 IMPLICIT REAL*8 (A-H.O-Z) 179 REAL*8 THETA(I2LINK),DTHETA(I2LINK) 180 NLINK=I2LINK/2 181 DO 10 1=1,NLINK 182 10 THETA(I)=0.0 183 DO 20 1=1.NLINK 184 THETA(I+NLINK)=0.0 185 20 DTHETA(I)=THETA(I+NLINK) 186 RETURN 187 188 END 189 SUBROUTINE GRATOR(RMASS,RLENG,THETA,GT) 190 C 191 C T h i s s u b r o u t i n e Is t o e v a l u a t e the g r a v i t y t o r q u e G T ( I ) . 192 C 193 IMPLICIT REAL*8 (A-H.O-Z) 194 REAL*8 THETA(4),RMASS(2),RLENG(2),GT(2) 195 DATA GRA/32 . 174/ 196 GT(1) = ((RMASS(1)+RMASS(2) ) *RLENG(1 ) *DSIN(THETA(1)) + RMASS(2)*RLEN 197 1 G(2)*DSIN(THETA(1)+THETA(2)))*GRA 198 GT(2)=RMASS(2)*GRA*RLENG(2)*DSIN(THETA(1)+THETA(2)) 199 RETURN 2O0 END 201 C--202 SUBROUTINE COUINT(RJO.CLM) 203 C 204 C T h i s s u b r o u t i n e Is t o c a l c u l a t e the c o u p l i n g m a t r i x . 205 C 206 IMPLICIT REAL*8 (A-H.O-Z) 207 REAL*8 CLM(2.2),RJ0(2) 208 CLM(1,1) = RJ0(2)/RJ0( 1 ) 209 CLM(2.2)=0.D0 2 10 CLM(1.2)=0.D0 21 1 CLM(2,1)=0.D0 212 RETURN 213 END 167 1 SUBROUTINE MOINER(RMASS,RLENG.RJ.NLINK) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n : 4 C T h i s s u b r o u t i n e Is to e v a l u a t e the moments of I n e r t i a of 5 C the composing l i n k s f o r a m a n i p u l a t o r system. 6 C 7 C V a r i a b l e D e s c r i p t i o n : 8 C RMASS: A r r a y of MASSes o f composing l i n k s 9 C RLENG: A r r a y of LENGthes of composing l i n k s 10 C RJ: A r r a y of moments of i n e r t i a of composing J i n k s 11 C NLINK: Number of LINKs 12 C 13 IMPLICIT REAL*8 (A-H.O-Z) 14 REAL*8 RJ(NLINK),RMASS(NLINK),RLENG(NLINK) 15 DO 10 1=1,NLINK 16 10 RJ(I)=RMASS(I)"RLENG(I)*RLENG(I) 17 RETURN 18 END 1 SUBROUTINE PDGAIN(OMEGA.ZETA.RJ.GAINP.GAINV,NL 1NK) 2 3 C 4 C S u b r o u t i n e D e s c r i p t i o n 5 C 6 C T h i s s u b r o u t i n e c a l c u l a t e the c o n t r o l g a i n s f o r l o c a l c o n t r o l . 7 C 8 C V a r i a b l e D e s c r i p t i o n 9 C 10 C OMEGA: A r r a y of the d e s i r e d n a t u r a l f r e q e n c l e s . 11 C ZETA: A r r a y of the d e s i r e d damping r a t i o e s . 12 C R J ( a r r a y ) : S t o r i n g the moments of i n e r t i a of the l i n k s . 13 C G A I N P ( a r r a y ) : C o n t a i n i n g the P o s i t i o n GAINS. 14 C C A I N V ( a r r a y ) : C o n t a i n i n g the V e l o c i t y GAINS. 15 C NLINK: Number of the composing LINKS. 16 C 17 18 IMPLICIT REAL*8 (A-H.O-Z) 19 REAL*8 OMEGA(NLINK),ZETA(NLINK),RJ(NLINK),GAINP(NLINK) 20 REAL'S GAINV(NLINK) 21 DO 10 1=1,NLINK 22 GAINP(I)=RJ(I)*OMEGA(I)*OMEGA(I) 23 10 GAINV(I)=2.0*ZETA(I)*0MEGA(I)*RJ(I) 24 RETURN 25 END 168 1 SUBROUTINE PATHPL(X,Y,RLENG.THETA) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n : 4 C C a l c u l a t i n g the c o r r e s p o n d i n g d e s i r e d a n g l e s from a s p e c i f i e d 5 C s t r a i g h t 1 i n e . 6 C 7 C V a r i a b l e D e s c r i p t i o n : 8 C X: X element of the C a r t i s i a n space 9 C Y: Y element of the C a r t i s i a n space 10 C RLENG: A r r a y of l e n g t h e s of the l i n k s 11 C THETA: The c o r r e s p o n d i n g a n g l e s of the l i n k s 12 C 13 IMPLICIT REAL*8(A-H,0-Z) 14 REAL*8 THETA(4),RLENG(2).ALPHA.BETA 15 DATA PI/3.1415926/ 16 HALFPI=PI/2 .0 17 R=DS0RT(X»X+Y*Y) 18 01=RLENG(1) 19 D2=RLENG(2) 20 C 21 C c a l c u l a t e t h e t a 2 22 C 23 DX1=(R*R-D1*D1-D2*D2)/(2.0*D1*D2) 24 THETA(2)=DARC0S(DX1 ) 25 C 26 C c a l c u l a t e a l p h a . 27 C 28 DX2=X/Y 29 IF (Y.LT.0.0) DX2=-DX2 30 ALPHA=DATAN(DX2) 31 IF (Y.LT.0.0) GOTO 1000 32 ALPHA=ALPHA+HALFPI 33 IF (X.LT.0.0) ALPHA = ALPHA-PI 34 C 35 C c a l c u l a t e b e t a 36 C 37 1000 DX3=(D1*D1+R*R-D2*D2)/(2.0*D1*R) 38 BETA=DARC0S(DX3) 39 IF(THETA(2).LT.0.0) BETA=-BETA 40 C 41 C c a l c u l a t e t h e t a l . 42 C 43 THETA(1 ) = ALPHA-BETA 44 RETURN 45 END 169 1 SUBROUTINE LOCALC(ERROR,DERROR.GAINP,GAINV,TLOCAL,NLINK) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n 4 C  5 C T h i s s u b r o u t i n e 1s to g e n e r a t e the l o c a l c o n t r l s i g n a l 6 C and s t o r e them 1n T L O C A L ( a r r a y ) . 7 C 8 C 9 C 10 C V a r i a b l e D e s c r i p t i o n 11 C 12 C ERROR: A r r a y of the t r a c k i n g ERRORS d e f i n e d as the 13 C d i f f e r e n c e between the d e s i r e d a n g l e s 14 C and the a c t u a l a n g l e s . I.e. THETAD(I ) -15 C -THETA(I). 16 C DERROR: A r r a y of f i r s t Oder D e r i v a t i v e s of ERROR. 17 C GAINP: A r r a y of the P o s i t i o n GAINs. 18 C GAINV: A r r a y of the V e l o c i t y GAINS. 19 C TLOCAL: A r r a y of the LOCAL c o n t r o l s i g n a l s . 20 C NLINK: Number of the composing LINKs. 21 C 22 C 23 C I n t e r f a c e v a r i a b l e s : ERROR.DERROR,GAINP.GAINV.TLOCAL.NLINK. 24 C 25 26 IMPLICIT REAL'S (A-H.O-Z) 27 REAL'S GAINP(NLINK),GAINV(NLINK),ERROR(NLINK),DERROR(NLINK) 28 REAL'S TLOCAL(NLINK) 29 DO 10 1=1,NLINK 30 10 TLOCAL(I)=GAINV(I)*DERROR(I)+GAINP(I)*ERROR(I) 31 RETURN 32 END 1 SUBROUTINE TOTALC(TLOCAL,TGLOBL,TTOTAL.NLINK) 2 3 C S u b r o u t i n e D e s c r i p t i o n 4 C 5 C T h i s s u b r o u t i n e Is t o g e n e r a t e the t o t a l s i g n a l s ( s t o r e d i n 6 C T T O T A L ( a r r a y ) ) by sumlng the l o c a l c o n t r o l s i g n a l s ( s t o r e d i n 7 C T L O C A L ( a r r a y ) ) and the g l o b a l c o n t r o l s i g n a l s ( s t o r e d 1n 8 C T G L O B L ( a r r a y ) ) . 9 C 10 C V a r i a b l e D e s c r i p t i o n 11 C 12 C TLOCAL: A r r a y of LOCAL c o n t r o l s i g n a l s 13 C TGLOBL: A r r a y of GLOBaL c o n t r o l s i g n a l s 14 C TTOTAL: A r r a y of TOTAL c o n t r o l s i g n a l s 15 C NLINK: Number of LINKS 16 C 17 IMPLICIT REAL'S (A-H.O-Z) 18 REAL'S TLOCAL(NLINK),TGLOBL(NLINK).TTOTAL(NLINK) 19 DO 10 1=1.NLINK 20 10 TTOTAL(I)=TLOCAL(I)+TGLOBL(I) 21 RETURN 22 END 170 1 SUBROUTINE GLOBLC(TLOCAL,CLM,GT,TGLOBL.NLINK) 2 C 3 C S u b r o u t i n e D e s c r i p t i o n : 4 C T h i s s u b r o u t i n e Is t o g e n e r a t e the g l o b a l c o n t r o l 5 C s i g n a l s . / G L O B L C : GLOBaL C o n t r o l . 6 C 7 C 8 C V a r i a b l e D e s c r i p t i o n : 9 C TLOCAL: v e c t o r g r o u p i n g the LOCAL c o n t r o l s i g n a l s . 10 C CLM: CoupLIng M a t r i x . 11 C GT: v e c t o r g r o u p i n g the G r a v i t y Torques. 12 C TGLOBL: v e c t o r g r o u p i n g the GLOBaL c o n t r o l s i g n a l s . 13 C NLINK: Number of the composing LINKS. 14 C I n t e r f a c e v a r i a b l e s : TLOCAL,GT,CLM,TGLOBL.NLINK 15 C 16 17 IMPLICIT REAL*8 (A-H.O-Z) 18 REAL*8 TLOCAL(NLINK ),GT(NLINK) 19 REAL*8 CLM(NLINK,NLINK),TGLOBL(NLINK) 20 DO 10 1=1.NLINK 21 TGLOBL(I)=0.O 22 DO 20 d=1,NLINK 23 20 TGLOBL(I)=CLM(I,d)*TLOCAL(J)+TGLOBL(I) 24 10 TGLOBL(I)=TGLOBL(I)+GT(I) 25 RETURN 26 END 1 SUBROUTINE DETERR(THETAD.DTHTAD.THETA.DTHETA,ERROR.DERROR . I 2L INK 2 1.NLINK) 3 4 C S u b r o u t i n e D e s c r i p t i o n 5 C 6 C T h i s s u b r o u t i n e 1s t o determine the d i f f e r e n c e s between 7 C the d e s i r e d a n g l e s and the a c t u a l ones, and a l s o the c o r r e s -8 C p o n d i n g f i r s t o r d e r d e r i v a t i v e s . 9 C 10 11 C V a r i a b l e D e s c r i p t i o n 12 C -13 C THETAD(array) : c o n t a i n i n g v a l u e s of the d e s i r e d a n g l e s ; 14 C DTHTAD(array): c o n t a i n i n g v a l u e s of the d e s i r e d d e r i v a t i v e s ; 15 C T H E T A ( a r r a y ) : c o n t a i n i n g v a l u e s of the a c t u a l a n g l e s ; 16 C THETAD(array): c o n t a i n i n g v a l u e s of the a c t u a l d e r i v a t i v e s ; 17 C I2LINK: two times of the number of composing l i n k s ; 18 C NLINK: c o n t a i n i n g the number of composing l i n k s . 19 C 20 21 IMPLICIT REAL'S (A-H.O-Z) 22 REAL*8 THETAD(NLINK),DTHTAD(NLINK),THETA(I2LINK),DTHETA(I2LINK) 23 REAL*8 ERR0R(NLINK),DERROR(NLINK) 24 DO 10 1=1,NLINK 25 ERROR(I)=THETAD(I)-THETA(I) 26 10 DERR0R(I)=DTHTAD(I)-DTHETA(I) 27 RETURN 28 END 

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

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

Comment

Related Items