UFDC Home  Search all Groups  UF Institutional Repository  UF Institutional Repository  UF Theses & Dissertations  Vendor Digitized Files   Help 
Material Information
Subjects
Notes
Record Information

Full Text 
OPTIMAL CONTROL OF ROBOTIC MANIPULATORS BY ALLON GUEZ A DISSERTATION PRESENTED TO THE GRADUATE COUNCIL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 1983 To Ilana, Jonathan and David / ACKNOWLEDGMENTS I would like to express my gratitude to my advisor, Dr. Alexander Meystel, for the guidance, encouragement and invaluable professional support and for his approaches to problems which considerably influenced mine. Special thanks are also to the Center for Intelligent Machines and Robotics (CIMAR) at the University of Florida and its director, Dr. D. Tesar,for the help in selection, formulation and successful completion of the work described in this dissertation. I would like to thank Dr. R. L. Sullivan, Dr. E. Chenette, Dr. P. P. Khargonekar, Dr. M. Rao and Mr. M.Thomas for the helpful dis cussions and positive contributions they have made. I also thank the typist Mrs. Mary Good for the excellent cooperation. Computing was done utilizing the facilities of the Northeast Regional Data Center of the State University System of Florida located on the campus of the University of Florida in Gainesville. This work was supported by the Electrical Engineering Department at the University of Florida. , TABLE OF CONTENTS CHAPTER PAGE ACKNOWLEDGEMENT .. . . . . .... . . . .... iii ABSTRACT . . . . . . . . . . . .. vi I INTRODUCTION AND BACKGROUND . . . . . . . .. 1 1.1 The Problem of Time Optimal Positioning with State Variable Inequality Constraints . . . . . 5 1 .2 Background . . . . . . . . . . . 6 1 .3 Dissertation Organization and Main Results . . .. 14 II DYNAMIC MODELS OF THE MULTILINK MANIPULATOR WITH ACTUATORS. 15 2.1 Manipulator Dynamic Model . . . . . . .... 16 2.2 Actuators Dynamic Model . . . . . . .... 19 2.3 Augmented Model . . . . . . . . .... 22 III A FEEDBACK LINEARIZING AND DECOUPLING TRANSFORMATION (FLDT) 26 3.1 The Decoupling and Linearizing Transformation ..... 27 3.2 FLDT for a Single Link Manipulator (Example 1) . . 38 3.3 FLDT for a Double Link Manipulator (Example 2) . .. 45 IV TIME OPTIMAL POSITIONING CONTROL (TOPC) . . . .. 58 4.1 Mathematical Statement of the TOPC Problem . . .. 58 4.2 Statement of a Restricted TOPC Problem with FLDT . 61 4.3 TOPC with SVIC's for Triple Integrator System ..... 64 4.4 TOPC for a Single Link Manipulator (Example 1) . 81 4.5 TOPC for a Double Link Manipulator (Example 2) . . 87 4.6 Comparison with Approximated BangBang Voltage Control 90 V REALIZATION OF THE FLDT . . . . . . . . .. 104 5.1 Sensitivity of Output Motion to Quantized Voltage. . 105 5.2 Asymptotic Stability for Imperfect FLDT . . .... 153 VI CONTROLLERS STRUCTURES, CONCLUSIONS AND FUTURE WORK ..... 164 6.1 Controller Structures . . . . . . . ... 164 6.2 Conclusions and Future Work . . .. . . . ... . 173 APPENDICES I EQUATIONS OF MOTION FOR A SINGLE LINK MANIPULATOR . . ... 176 II EQUATIONS OF MOTION FOR A DOUBLE LINK MANIPULATOR . . .. 178 III COMPUTER LISTINGS . . . . . . . . . . ... 183 REFERENCES . . . . . . . . . . . . . . 212 BIOGRAPHICAL SKETCH . . . . . . . . . . . ... 218 Abstract of Dissertation Presented to the Graduate Council of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy OPTIMAL CONTROL OF ROBOTIC MANIPULATORS by Allon Guez April, 1983 Chairman: Dr. Alexander M. Meystel Major Department: Electrical Engineering This dissertation addresses the problem of finding the desired optimal trajectories and the optimal control inputs for a rigid multi degreesoffreedom robotic manipulator in a positioning mode. For a general, highly coupled and nonlinear dynamic model an analytical control law is derived and structures of controllers are suggested. The equations of motion of a multilink mechanism and its actuators are unified into a common dynamic model. A nonlinear feedback trans formation which globally linearizes, decouples and places the poles of the closed loop system is derived. Each of the decoupled subsystems represents a single degree of freedom of the manipulator motion. This transformation is realized by online computations based on the measure ments of the manipulator's state variables and reference to a lookup table, and requires no matrix inversion. The set of uncoupled subsystems in a form of triple integrators allows a solution of various optimal control problems for each degree of freedom independently. The time optimal positioning problem of a multidegreesoffreedom robotic manipulator with bounded velocity, acceleration and jerk is posed in a form which guarantees uniqueness of the control and state trajectories. The solution is demonstrated via single and double link examples. The lookup table implementing the feedback transformation intro duces nonanalytic quantizationn) errors. The time optimal trajectories of a double link manipulator are shown to be rather insensitive to this type of error. For a class of errors an existence theorem of the linear feedback controller is proven which guarantees closed loop asymptotic stability at the state space origin. Finally, outlines of various controllers structures are discussed. This dissertation is the first application of the following principles:  Instead of optimizing the real system, a fictitous system should be introduced which is more convenient for optimization and can be obtained from the original system, for example by a state space transformation.  The state space feedback transformation can be realized for de coupling, linearization and poleplacement simultaneously.  The programmed open loop control for a deterministic system can be devised with the assumption that deviations are compensated by a separated subsystem for "control inthesmall." CHAPTER I INTRODUCTION AND BACKGROUND This work deals with the time optimal positioning control of robotic manipulators. These are general purpose,programmable machines possessing certain anthropomorphic characteristics. The robot manipulator system is defined as "a programmable, multifunction manipulator designed to move material, parts, tools or specialized devices through variable programmed motions for the performance of a variety of tasks." In the literature the terms mechanical arm, artificial hand, robotic arm, open loop articulated chain and manipulator are used interchangeably. Although practical examples in this dissertation involve serial mechanisms, much of the theory developed applies to other programmable, multifunction chain structures, designed to provide purposeful mechanical motion. The robot manipulator system is composed of N rigid links, con nected in series by kinematic joints, into an open loop kinematic chain. The joints are rotary or linear (sliding), with the first joint fastened to a fixed support (base), whereas the Nth link functions as the hand, or "endeffector," which usually has a gripper for performing a task. Each joint is assumed to have only one degree of freedom (DOF); i.e., each joint defines a joint axis around, or along which, a link rotates or slides. The number of joints defines the number of degrees of freedom of the robot manipulator. The case of multiple rotational or combined sliding/rotating degrees of freedom joints can be modelled by an equiva lent chain of zero masses and lengths of single degree of freedom joints. The standard dexterity or articulation requirement is six DOF, three DOF for linear and three DOF for angular locating of the end effector with respect to a fixed reference, in which case the robot manipulator is called nonredundant. If N, the total number of DOF is greater than six, the manipulator is redundant. The analysis attempted here is to be equally related to redundant as well as nonredundant manipulators. The description of the manipulator's state can be provided either as a vector specifying the various joint angles or as a position and orienta tion of the end effector. The former representation is called configura tion and is said to be a representation in joint variables (coordinates) space or joint space. The latter is a representation in cartesian space. The locus of time dependent points which the manipulators' joints trace is called a trajectory. In the analysis of robotic manipulators, two main problems can be formulated. In the first, the desired position and orientation of the end effector are given. The joints' positions which will locate the hand at the desired point in the cartesian space with the specified orientation are to be determined. For six DOF manipulators (nonredundant) the number of solutions to this problem is finite and may be obtained (except at singular points, [S5]) by inversion of the geometrical transformation which relates the joint variables to the hand's position and orientation. For a redundant spatial NDOF manipulator there may be infinitely many solutions. Throughout this work, however, we assume that an appropriate solution for this problem exists and is feasible. The second problem is one of manipulators dynamic control: find the structure of the controller and the controls (inputs) which provide the movement of the manipulator system from its present configuration to the desired one, such that constraints on the hand's and joint's path, velocity, acceleration and jerk as well as actuators losses and energy consumption are met. If the motion execution is optimal with respect to some given "goodness criteria" then it is called an optimal control problem. The basic tasks performed by industrial manipulators may usually be reduced to two types, regardless of the complexity of the manipulation task. In the first, sometimes called point to point task, the cartesian path of the manipulator's hand or joints is not specified and may fre quently be arbitrarily chosen. Typical point to point applications include "pick and place" activities, machine loading and unloading, spot welding, etc. The second type of tasks performed by manipulators is sometimes called tracking or "contouring." This type includes applications such as paint spraying, continuous welding processes, some metal cutting operation and grasping objects moving on a conveyor. In this mode the desirable trajectory of the motion is preassigned and is an essential part of the manipulator's task definition. In the point to point task type, the trajectory leading to the desired terminal point is frequently of no importance (except of obstacle avoidance requirement), e.g., the motion of an empty gripper towards a tool or object. In this type of task, optimization procedures may be applied to the control synthesis: minimization of time to reach the desired state, optimal velocity distribution along the path, minimization of energy used and dissipated by actuators which impose constraints to the control, etc. The present work is aimed mainly at this class of tasks. An observation made by many researchers (e.g., [B2], [B5], [L4], [T2] and [V1]) is that in the point to point task the manipulator motion should be partitioned into two modes: "large," and "small." The first, which is referred to as slewingg mode" [V1], "coarse motion" [B5], "transport phase" [B2], or "gross motion" [L4], is the main part of the motion made as the manipulator transports objects or tools from one point to another in the work space. At this part, fast motion for higher productivity is to be emphasized whereas accuracy seems to be less important. The second mode which is denoted as "fine motion" [L4], [B5], or "terminal phase" [B2], is the part of the task where the system is exerting a force, manipulating or having contact with external objects, with a relatively slow motion (if at all) of most parts, performing "rendezvous," "accuratestop" or fine insertions. Since, in this work, we are dealing primarily with time optimization of the manipulator's motion, the slewing mode (or the coarse positioning) is naturally our main concern, which actually implies the possibility to base our research upon a deterministic model. In terms of the complexity of the control analysis, the coarse and fine modes of motion differ significantly. If the system is to be moved slowly, or.by varying one joint at a time (as in the fine mode), then coupling torques, such as the Coriolis and centrifugal terms, are minimal, and a simple solution to the control problem for each of the joints can be found independently. However, in the slewing mode the motion is fast and the entire manipulator configuration changes significantly during a single cycle. For productivity, all joints should be moved simultaneously and the coupled structure of the manipulator dynamics makes this problem highly nonlinear and difficult. The conventional positioning techniques currently implemented in industrial robots are no longer applicable. 1.1 The Problem of Time Optimal Positioning with State Variable Inequality Constraints At this point we do not have the mathematical tools to present a mathematical statement of the major problem discussed in this dissertation. This will be done in Section 4.1, after the results of Chapter II, the manipulators dynamic model, and Chapter III, the decoupling transforma tion have been obtained. Nevertheless, for clarity of the presentation, we define in this section the control problem in general terms, as follows. Given a NDOF manipulator system with its N actuators (DC motors are assumed), where all the relevant parameters of the load and manipu lator are known, and where the initial and final manipulator configura tions are given, find the actuators inputs (voltages) to be applied, the controller structure, and the manipulator state trajectories to change the manipulator configuration from the initial to the final one, in minimum time, without exceeding the preassigned maximum velocities, # accelerations and jerks for all joints. This will be called the time optimal positioning control (TOPC) with state variables inequality constraints (SVIC) problem. It should be clear from the definitions above that we are dealing with slewing mode or coarse positioning, of point to point tasks, since only in this case can the path be freely chosen by the controller. #Constraints of this type were shown to affect the higher harmonic content of the (driving) command signals and reduce the vibratory transient response in the flexible system [T3], [G1], [M2] and [M3]. It is our intention to solve the above stated problem using methods which remain applicable to a larger class of problems, e.g., TOPC problem with actuator losses and energy consumption constraints; time optimal point to manifold control and so on. 1.2 Background In this section we give a brief survey of previous works which are related to the important aspects of the problem. The following topics are discussed: mathematical modelling of multilink manipulators, global linearization and decoupling of manipulatros including actuator dynamics, control and optimal control of robotic manipulators and similar mechanical structures. Various approaches were used in deriving the equations of motion for the type of mechanical linkages described above. The pioneering works by Uicker [U1] and later by Bejczy [B1] and Hollerbach [H3] were based on the Lagrangian mechanics approach. Orin et al. [(01] gave a recursive algorithm using the NewtonEuler formulation. Thomas and Tesar [T4] presented an alternate derivation using the kinematic influence coeffi cients. Horowitz and Tomizuka [H4] derived the dynamic model using the GibbsAppel equations of motion. In all of those derivations, the resulting dynamical model (for control purposes) of the NDOF, serial manipulator, has the following form, which is almost exclusively used in robotic control literature. (See for example [T4], [H4], [B3], [D1], [G5], [K4], [C2], [Y2], [V2], [P3], [G4], [S2] and many others.) T = J(O) 8 + f(0, 0) where 8 is the vector of joint coordinates, T is the vector of applied torques, J(8) is the inertiamatrix anf f is the vector of velocity related and gravity terms. Notice that the input control is assumed to be the torque generated by the actuators as seen in the joints' shafts. These torques may be generated by electrical, hydraulic or pneumatic motors, in all cases however the torques cannot be isntan taneously assigned thus the above model is an approximation. The NDOF manipulator may be assumed to be affected substantially by the dynamics of the N actuators located at its joints. The actuator's dynamics must be accounted for if an optimal trajectory con trol is to be executed. This is especially the case when a fast motion is desirable, where the actuators natural frequencies are significant and, thus, the driving torques can no longer be assumed instantaneously controllable. Very few works in manipulator control were found to include a dynamic model for the manipulators' drives. Paul [Pl] assumes that the manipulator driving torques are instantaneously controllable, but he utilizes approximation curves to account for the loading effects and friction of the actuators. Cvetkovic and Vukobratovic [C2] and Vukobratovic and Kircanski [V2] assume a linear time invariant third order model for electric motors which are the manipulators' drives. As shown inChapter II the dynamic model for the manipulator/ actuators system is a multiinput, multioutput, highly coupled, non linear, and high order (18 state variables for six DOF) set of differential equations. This made any attempt to derive analytical con trol laws for even the simplest tasks impossible. Moreover, even on line numerical integration techniques, which had to be repeated for each control cycle, due to system and load variations, were shown to be an excessive computational burden and to require a huge amount of memory. Thus, the need for model simplification techniques such as decoupling and linearization became essential for fast and efficient control of robotic manipulators. Various linearization methods were used by many authors in the control of robotic manipulators. Kahn [K1] derives a linear model for a three DOF manipulator for finding a time suboptimal controller, by deletion of all second and higher order dynamics in the full model. He found, however, that an average value of the velocity related (nonlinear) terms had to be added to the model in order to guarantee suboptimality. This means that some apriori knowledge of the state trajectory of the system had to be assumed. Other linearization techniques (see Pieper [P4], Whitney [WI], Paul [P1], Vukobratovic [V2], Yuan [Y2], Takegaki [Tl] and many others) assume that the nominal trajectories of the system are given in advance. The dynamic model is linearized about those trajectories, yielding independent joint reference commands for each actuator. In practice, however, the manipulator system, due to the variations of real working conditions, does not follow the nominal trajectories. This degrades the control performance and may even induce unstable responses. To overcome this problem Paul [Pl] utilized precomputed compensations of nonlinear effects as additional inputs. Vukobratovic [V2] designed asymptotic regulator, feedback controllers, for each actuator, to guarantee stability about the nominal paths, and Yuan [Y3] suggested a feed forward compensator to improve the performance of the linear independent joint controllers. Decoupling of the manipulator/actuators system to a set of lower order subsystems, was also attempted by various authors (see for example Hemami [H2], Freund [F2], Guez [G4] and Horowitz [H4]) as a method of simplifying the manipulator control tasks. The literature on decoupling of nonlinear systems is rich but not well organized. Porter [P5], found sufficient conditions for the existence of a decoupling state feedback transformation. Later Majumdar and Choudhury [M1] developed necessary and sufficient conditions for decoupl ing and diagonalization by state feedback for nonlinear system, which are linear in control input. Freund [F3] extended the results obtained by Porter, to the case of nonpurely dynamic systems. (See [K2] for summary of additional results.) For locomotion and robotic manipulator systems, however, the use of nonlinear feedback transformation was first suggested for lineariza tion, Hemami [H2], and later for decoupling, Freund (F2], Guez [G4], Horowitz [H4]. In these works, however, actuator dynamics is not included in the system model (this reduces the model order by 33%) and the decoupling transformation is derived while assuming that the joints driving forces and torques may be instantaneously assigned. Moreover, the nonlinear feedback transformation derived in (F2] requires the on line inversion of a NXN state dependent matrix, where N is the number of DOF of the manipulator. This is an extremely difficult task for the control sampling rate which is generally suitable for manipulative tasks. Also, in [H4] the gravity and load related forces were deleted in the derivation of the linearizing and decoupling transformation. Other decoupling techniques, such as the linear feedback asymptotic regulator by Vukobratovic [V2] and the open loop, feedforward compen sator by Yuan [Y3] were suggested. These methods, however, assumed given nominal trajectories about which the model is linearized, and therefore represent only local decoupling. Another, approximate decoupling method is used by Koivo and Guo [K3] where the first three joints of a six DOF manipulator are associated with the translational motion of the end effector, and the last three joints with its orienta tion. The coupling terms between the variables of these two groups are neglected. As will be shown in Chapters II and III, for a large number of DOF, the feedback decoupling may involve the evaluation of thousands of additions, multiplications and trigonometric operations. Online computations of those may therefore take many minutes for a single iteration and turn out to be impractical for real time control. This, however, is not the only possibility for the feedback decoupling exe cution. Various combinations of evaluations of analytical expressions and references to table lookup are possible as a method of computa tional burden reduction in each cycle. In 1975 Albus [A1] suggested the Cerebellar Model Articulation Controller (CMAC) as a new approach to manipulator control. In his work he describes how the various control signals for a multiDOF manipulator are obtained by referring to a table (in a computer's memory) rather than by mathematical solution of simultaneous equations. CMAC combines input commands and feedback variables into an input vector which is used to address a memory where the appropriate output variables are stored. In [A1] and later in [A2] Albus describes the CMAC memory management algorithm which makes it possible to store the necessary data in a physical memory of a practical size. In fact, as was shown by Raibert ER1], by appropriate paramateriza tion of the equations of motion for the manipulator, divergent procedures for control involving analytical expressions and table lookup can be examined. Each procedure represents a different point on a continuum characterized by the indicator P, the number of parametric variables. As P increases storage requirements increase, but computational com plexity decreases. As shown by Raibert [R1], for a NDOF manipulator, with 3N variables (namely, the joint positions (N), velocities (N) and accelerations (N)), of which P are held constants as parameters whereas the other 3NP variables are represented by R resolution cells (3 ) each, about N arithmetic operations and RP storage cells are required for evaluation of the equations of motion. A variety of other control structures have also been developed. Through the application of variable structure theory, control of a simple manipulator was accomplished by discontinuous feedback using sliding modes which produced a somewhat undesirable discontinuous con trol signal. In [D2], a model reference adaptive controller is proposed which forces each manipulator joint to behave as an underdamped, second order system, with the actual equations of motion not being utilized in the design. In spite of the importance of time optimal (fast) motions of the manipulator for increased productivity, which has been recognized by many (see for example [P3], [K1], [L4], [Z1], [V1], [G3], [C2] and [L3]), very few works are known to be performed in this field. This is, of course, due to the highly nonlinear and highly coupled form of the differential equations which govern the system. As a result, very few attempts to apply optimal control methods in manipulator control were reported. Kahn [K1] presented a suboptimal numerical solution to a minimum time point to point control for three DOF manipulator. The dynamic model used by Kahn did not include the actuators dynamics, and was linearized by neglecting second and higher order terms in the equa tions of motion, some velocity dependent terms however were approximated by their estimated values. In spite of these simplifying assumptions the resulting control law was still dependent on the boundary conditions. Paul, et al., [P3] present a linear programming algorithm for finding the near minimum time path of the manipulator end effector. It deals with path planning by designing a set of time intervals for the transi tions among a sequence of preassigned points in the cartesian space for the end effector. The motion between each pair of points is assumed along a straight line segment with constant velocity. The total traveling time is minimized while observing velocity and acceleration inequality constraints. The manipulator dynamics are not included and since the path is assigned in advance it does not solve the time optimal posi tioning problem. Later this method was extended by Lin et al. [L3] to fitting of cubic polynomials with velocity, acceleration and jerk constraints in planning a minimum traveling time trajectories with given sequence of points. Lynch [L4] derives a sequential mode (one axis at a time) minimum time for two axis manipulator. In a sequential mode coupling between the various axes is significantly reduced, the execution time, however, is much larger than in the one obtained in simultaneous motion of all axes. In [G2] various numerical methods are utilized to solve a three DOF mechanism model for a gymnast per forming a kipup maneuver on a horizontal bar in minimum time. Again, no actuator dynamics are involved in this case and the optimal control has to be computed for each new set of boundary conditions. Other works in the area of optimal control of robotic manipulators deal with minimizations of quadratic functionals of the state variables and the applied torques such as [T5] and [Y1]. However, only second order linear models are used and the actuators dynamics are not accounted for. A related problem, the optimal control of container cranes (minimizing a swing of the load as it moves along its trajectory) is described in [S1], again the actuators are not modelled and the solution is nonanalytic, i.e., has to be recomputed for each task. The only reference found where the actuators dynamics are accounted for in the minimization of the energy dissipated by them was [C2]. It assumes, however, no interaction between the manipulator joints and is therefore suboptimal. In summary, it is found that due to the high order, highly non linear and coupled form of the manipulators equations of motion, the direct application of control theory to the problems of optimal control of manipulator is impractical. Therefore, methods of global lineari zation and decoupling of the dynamic model are essential for the successful solution of the optimal control problem. 1.3 Dissertation Organization and Main Results In Chapter II the dynamic models of the manipulator and actuators are defined and augmented into a single state space model. In Chapter III we derive the feedback decoupling and linearizing transformation for the NDOF manipulator system including its actuator. This transform does not require the inversion of a matrix and it accounts for all torque terms in the dynamic model, including load and gravity. It is demonstrated for a single and double link manipulator. In Chapter IV, the time optimal positioning control problem is accurately stated and solved taking into account various task configura tions. The solution is analytic and need not be repeated for each task. Again, it is simulated for a single and double link manipulator and then compared with bangbang voltage control. In Chapter V the approach taken by our work is shown to be appropriate to slewing mode positioning via a simulation of a quantized decoupling transform, indicating a low sensitivity of the output motion to this type of error. Additionally, an existence theorem is proved regarding the asymptotic stabilizer for a manipulator system controlled by imperfect decoupling feedback. The dissertation is concluded with suggested controller structures, conclusions and topics for future work, which are described in Chapter VI. CHAPTER II DYNAMIC MODELS OF THE MULTILINK MANIPULATOR WITH ACTUATORS In this chapter the dynamic rigid and deterministic model for the Ndegrees of freedom manipulator and its actuators is selected. Section 2.1 describes the rigid and deterministic dynamic model for a serial NDOF manipulator and defines the mathematical properties and the physical meaning of each term in that model. Section 2.2 describes a linear thirdorder time invariant model for the N permanent magnet DC motors which are assumed to serve as the manipulators' actuators. In Section 2.3 both dynamic models are combined into a single model, for which common state variable and differential equations' vectors are defined. The following major assumptions are made in this chapter and will be used throughout this work.  The manipulator is an open loop chain (serial) of N rigid bodies.  All kinematic and dynamic parameters of the manipulator and its actuators are known (given constants), thus, a deterministic model is assumed.  The manipulators' joints are either rotary or linear (sliding).  The actuator's dynamic model is linear and time invariant.  The external loads applied to the manipulator are known and can be expressed as smooth differentiablee) functions of the joints dis placements, velocities and time. s15 2.1 Manipulator Dynamic Model The robot manipulator system is composed of N rigid links, connected in series by kinematic rotary or linear joints, into an open loop kinematic chain. (See Figure 1.) As mentioned above the dynamic model for the manipulator system is given in Equation (2.1.1) and may be described as follows: T = J(0, t) + f(e, W, t) where f(0, W, t) = (0, W, t) + Ti1 WT G (0) W T 2 wT G (0) w T N w) G (0) (S (2.1 .2) and where T S= (01, 92, . ) is the joints linear or angular N tions vector, 9eR ; W WT *T (I, W . ( N) = (81 2 . ) is the j N linear or angular velocity vector, weR ; * T = (w1 2, . N ) T is the joints linear or angular N accelerations vector, LeR ; T T= (T T2, . T ) is the vector of generalized dr forces, (forces or torques acting at sliding or rot Joints respectively) TRN joints respectively) TsR posi points iving ating (2.1 .1 ) Base \ ,,"End E) Joint \ e2 Effector"1 N \ f \ f> 2 \ i N I        __ _ _ _ ., I M Figure 1. NDOF Manipulator A J(8, t) is the NXN, "effective" inertia, symmetric matrix function of the joints' coordinates vector 9, J( ): RN+ RNXN ; * S(8, w, t) is the vector of gravity and external loads related torques/forces, and is smooth vectorial function of the joints coordinates vector 8, the joints rates, w and 7N+1 N time {(8, w, t):R 2N+ R ; G i(), i = 1, 2, . N are the NXN symmetric matrices repre senting the velocity dependent torque weighting matrices and are functions of the joint coordinates vector 9, G (8): RN RN i = 1, 2, . N. It should be noted that the components of the matrices A Gi J(8,t) and G (8) for i = 1, 2, . N are products of trigonometric functions (sines and cosines) of the various joints' coordinates. Also, since the external loads were assumed to be smooth functions of 8, w and the gravity and load related torques vector, 2(8, w, t) and J(8) and Gi(8) are all differentiable with respect to all their arguments. This will be exploited later in Chapter III in deriving the feedback decoupling transformation. Notice also that the socalled "effective inertia" matrix, J(8, t) N at any given time, t, is positive definite for all GER and all possible combinations of sliding and rotary joints. This property of J(8, t) is proved in [H4], but is not surprising since the total * In general 2 is a function of the external loads (torques and forces) and gravity. Since these loads were assumed to be known functions of 8, w and t, it is always possible to obtain the form 2(8, w, t) by substitution. kinetic energy of the NDOF manipulator, which is always a positive T quantity, is given by wT J(O, t) w, and is zero only for zero angular velocity, i.e., J(9, t) is positive definite by definition. (See [T4].) The various components of the above described matrices and vectors possess an important physical interpretation. The ith diagonal entry ^ of J(8, t), j..(ii(, t), represents the effective inertia (including the load, which may vary in time) at joint i; the offdiagonal entry ^ of J(O, t), j ik(,0 t), represents the coupling inertia between joints i and k. Similarly, Gk(8) represents the centripetal force at joint i due to velocity at joint k; Gi () represents the force jk  at joint i due to velocities at joints j and k; and f. (8, W, t) represents the torque developed at joint i due to gravity and external loads. 2.2 Actuators Dynamic Model The NDOF manipulator is assumed to be driven by N actuators located at its joints. Throughout this work, we assume (using an approach similar to the one taken in [C2] and [V2]) that the actuators are permanent magnet, DC motors, with armature current control. The ith actuator is acting Linear models of other types of actuators may be used without loss of generality of the work that follows. on the ith joint through a gear reduction box and may be described as follows. A.. + B.w. + J.w. + T. = r. KT I. 1 1 i 1i 1 i T.i 1 i = 1, 2, . N (2.2.1) L.I. + R.I. + riKV = V. ii ii iV. 1 1 1 where V. is the ith motor input voltage; 1 T. is the ith joint load torque as given in (2.1.1); 1 I. is the ith actuator's armature current; 1 r. is the ith gearbox reduction ratio (r. > 1); 1 1 J. is the ith actuator rotor inertia referred to the output 1 2 shaft, (J. = J r.); (J is the moment of inertia of 1 M. i M. 1 1 the ith motor.) B. is the ith actuator's viscous friction coefficient; I A. is the ith actuator's compliance coefficient; 1 R. is the ith armature ohmic resistance; 1 L. is the ith armature inductance; 1 K is the ith actuator torque constant; 1 and K. is the ith actuator back e.m.f. constant. i In addition to the above given constant parameters, the rated torque and speed, the maximum acceleration and the maximum speed are given. Notice that although all the motors' parameters were assumed known (given), they are in fact, the result of a design procedure which selects the appropriate actuators for the given manipulator and the desired volume of tasks. The N equations (2.2.1) may be rewritten in a vector form as follows: J + T = C I A B w (2.2.2) _=F W + F 1 + L V where T is the vector of joint loading torques as defined in Section 2.1; V is the vector of input voltages with components V.; ~1 I is the vector of armature currents with components I.; ~1 J is a diagonal matrix with J.. = J. > 0 for i = 1, 2, . N, ii 1 and is therefore positive definite; C is a diagonal matrix with C.. = r. KT. > 0 for 11 1 T. 1 i = 1, 2, . N, and is therefore positive definite; A is a diagonal matrix with A.. = A. > 0 for i = 1, 2, . N; B is a diagonal matrix with B 1 B > 0 for i = 1, 2, . N; B is a diagonal matrix with B.. = B. > 0 for i = 1, 2, . N; 1i 1 L is a diagonal matrix with L.. =L. > 0 for i = 1, 2, ... N, 11 1 and is therefore positive definite; r.1 KV r i K F is a diagonal matrix with F = 1L. <0 for 1 1 L i. 11 1 i = 1, 2, . N and is therefore negative definite; R. F is a diagonal matrix with F =  <0 for i = 1, 2,... N 2 2.. L. 11 1 and is therefore negative definite; and where a diagonal matrix M is defined as a matrix M with m.. = 0 for i # j i, j = 1, 2, . N. 13 Equation (2.2.2) defines a combined dynamic model for the N actuators used. In the following section we use Equations (2.1 .1 ), (2.1 .2) and (2.2.2) to obtain an augmented model for the NDOF manipu lator with its actuators. 2.3 Augmented Model In this section we combine the equations of motion for the NDOF manipulator with the dynamic model of its actuators. This augmented model is required for a clear problem formulation and efficient deriva tion of an analytical control law. Using state space notation, let the state variables for the augmented system be the joint coordinates, the joint velocities and the actuators' armature currents. That is, define X1 =e X = (2.3.1) :2  x =J1 3  and T x (X: ,I) X2, X3 ) In terms of X X 1may be written as may be written as and X3, Equations (2.1 .1), (2.1 .2) and (2.2.2) J(X I, t) i2 + f(X1, X2 t) = T f_(x_11 x_2) G1 (Xii x2 T 2 X G (X ) X2 T N 2 G (1 ) X 2 + T = 3 1 2 LI X3 = FX2 + F2X3 + L V Combining (2.3.4) and (2.3.2) we obtain [J + J(X, t)] X2 + F (X, X 2, t) = CX AXI BX2 or in a state variable form Xi =2 i2 = g(X, t) i = F + F + L1 X3 =FIX +F2X +L V (2.3.2) (2.3.3) (2.3.4) (2.3.5) t) = I_(XI, X2, t) + where g(X, t) = J(X_, t)1 [CX3 f(X1, X 2, t) AX BX_2] (2.3.6) and J(Xi t) = J(X_1, t) + J (2.3.7) Thus, Equations (2.3.5), (2.3.6) and (2.3.7) represent the complete state space model for the NDOF manipulator with its actuator. It should be noted that since J and J(X1, t) are positive definite, so is J(Xi t) and is therefore nonsingular and the state N space model exists for any configuration (V eR ) of the manipulator. See Figure 2 for a block diagram of the augmented system. Figure 2. Block Diagram of Augmented System CHAPTER III A FEEDBACK LINEARIZING AND DECOUPLING TRANSFORMATION (FLDT) In the previous chapter it was shown that the dynamic model for the manipulator/actuators system is a multiinput, multioutput, highly coupled, nonlinear, and high order (18 state variables for six DOF) set of differential equations. This made any attempt to derive analytical control laws for even the simplest tasks impossible. Moreover, even on line numerical integration techniques, which had to be repeated for each control cycle, due to system and load variations, were shown to be an excessive computational burden, (see Section 1.2) and to require a huge amount of memory. Thus, the need for model simplification techniques such as decoupling and linearization is essential for fast and efficient control of robotic manipulators. In this chapter, a nonlinear state feedback transformation is introduced to the full manipulator/actuator system via the actuators' input controls. This transformation takes into account the actuator dynamics, gravity and load related terms, requires no matrix inversion, and provides global decoupling of the NDOF system into N linear subsystems. Each decoupled subsystem represents one DOF of motion (link) of the manipulator with the position, rate and acceleration of the DOF as its state variables which preserves physical insight to the control problem and the link's jerk as its input. The dynamics of each DOF of the manipulator may be arbitrarily (and independently of the other DOFs) assigned by a proper choice of a linear feedback. Furthermore, the parameters of the transformation, do not need to be readjusted for different tasks (paths) as is the case in linear control schemes and all axes may have the same dynamics for all configurations and parts of the manipulator's state space. We commence in Section 3.1 by the derivation of the feedback linearizing and decoupling transformation (FLDT) based on the dynamic model presented in Chapter II. Sections 3.2 and 3.3 demonstrate the application of the FLDT for single and double link manipulators. 3.1 The Decoupling and Linearizing Transformation In this section a nonlinear state transformation is derived such that in terms of the new state variables the manipulator/actuator system is linear and decoupled to N subsystems. Each subsystem repre sents a single link (DOF) in a form of a triple integrator dynamics. The equations of motion of the augmented system were given in Section 2.3 and are restated here for convenience. 2 = g: (, t) (3.1.1) = XF + FX3 + L1 I 3 1z12 2:3 where g(X, t) J 1(Xi, t) [C_3 A1 E2 w(x1 x ) __Xi, X2,* t)] T 1 T 2 (3.1.2) W(x1, 2)= X G(X )x_ x G(X )X, , T N T X2 G (X ) x2T and where X = (X X2, X) with X, X and X as defined in Z1 :2'.3 wit ' 3 Equation (2.3.1). In order to obtain a decoupled set of N triple integrator sub systems, let the following nonlinear state transformation be introduced: Y = +(X, t) (3.1 .3) where +(X, t) [X1, X2, g(X, t)]T (3.1.4) and where yCR3N. Equivalently, Equation (3.1 .3) may be written as follows: yi = x y2 = x = x (3.1 .5) Y3 = g(X, t) = X2 i.e., Y i, Y2i and Y3i represent the position, rate and acceleration of the ith joint, respectively. The equations of motion of the system, in terms of the new state vector, Y, are found by applying Equations (3.1.1) and (3.1.2) to (3.1.5). We obtain: #Y is a state variables vector since the mapping g(X) is invertible with respect to X_3. (J (X ) and C exist). Y =2 = =12 or 1 ~ 2 Similarly, for Y :2 2 2 = g (X t) (3.1 .6) i2 = Y3 (3.1 .7 Finally, for Y3 we obtain = ~d  3 = g t) = d [J1 (X t)] (3 0AX BX2 W(X1 X2) at ~3 1l 2 1 (1 'I X2' t)] + j1(x1', t) [CX3 2 W(X) i(X, t)] (3.1 .8 Equation (3.1.8) is the only state variable equation which involves the input vector, V (through X3 ). Thus, the state transformation may be performed via state feedback by a proper choice of the input function. Now, in order to obtain a triple integrator form, we must have Y3 = z (3.1.9 N where Z is the new control input vector, ZeR Thus Equations (3.1.8) and (3.1.9) define the state feedback as follows: ) ) ) 30 z = d [J1 t)] [CX EX W(X, ) Z dt 1 1 2 1' 2 J 1 (_ t) [cX 1AX BX W(X) h(X, t)] , using Equations (3.1 .1) and (3.1 .2) again yield d  Z= d[J (X1, t)] [CX3 AXl BX2 W(X1 X2) 1 1/ (XI, X2, t)] + J (X1, t) [CFX2 + CF2X3 +CL V 12 23 2 2 B g(X, t) W(X) !(X, t)] (3. The feedback transform V( X,Z, t) is obtained from Equation (3.1.10) as follows: V(X, Z, t) = LC1 J(x1, t) {Z d[J1 t)] [3 AX t WL 1 AL B_2 W(X1, X2) _(X1, X2, t)]} + LCI [AX2 + B g(X, t) + W(X) + i(X, t)] L[F lX + F X13] (3. 1 .10) 1.11) d 1 t[J (Xi, t)] = i(x, t) = X N 6[j1 (X,[ t) ] z x2J ax. . a[J1 (X_1,t)] + at al (X x t) :X_ g(X, t) a2 j + (QL1' a2' t) +  at where  f(I1 x2' t)] + X 2 L t) 9 1 (gx) Gi (X.) X T ) G2 1x1 ) 2 j ( T) G 2(1) 2 gTx) GN(x ) X2 OG1 (X1 ) X1j aG2 (XI ) aG (X) l (3G N X1 ax1 T 1 X G (X ) ?W /; g_(x_) w(X) = T 2 X G (X ) g(X) T N X G (X_ ) g(X) 2j 1 X2j] 2 x2j I2 (3.1 .12) T 'N x2 j^ x j1 jl i2 IJ' and where we define A is the Jacobian matrix = of the MX1 vector a with respect to the NX1 vector b. Matrix of the partial derivativesof a matrix NXM with respect to a scalar argument b. 3 8a1 abN 8a2 O b N * aa1 ab, aa2 aa m "aa(b) Fb ! (b lA ab. 8aa1 a2 6aa m 2 am12 ab. ' 3 a22 ab. ' 3 aa m N &n11 C l 1 6b. ' 3 am21 ab. ' 3 amN1 ab. ' 3 OmlM ab. 3 2M ab. I amN2 ab. ' 3 amNM ab. 3 Clearly, in the general case, the feedback transform, V(X, Z, t) as defined in Equations (3.1.11) and (3.1.12), is a function of all state variables, including X3, the armature currents. In the new state space, however, (Y), the armature currents are not directly represented. It is therefore desirable to represent the feedback linearizing/decoupling transformation (FLDT) in terms of Z, t and Y only. That is measurements of the joints' positions, rates and accelerations only are required, and not the currents. This is done as follows. From Equations (3.1.2) and (3.1.5) CX = AX + EX + W(X x,) + 2(X X t) + J(X t) g(X, t) 3 1 :2 2 1 :2 :1  or CX3 = AY + BY + W(Y, Y) + (Y', Y,2' t) + J(Y1, t) Y (3.1.13) :3 :i Z2 7 = l2 1 3 Notice that C and F are both NXN diagonal matrices and therefore commute (i.e., CF = F2C), thus CF X = F [AY + BY + W(Y 2 Y) + Y2, t) 2::~3 2 ~~1i z2 ~ 11' 72\ R ( Z1 2' + J(Y t) Y] (3.1 .14) Also recall that for a nonsingular, square matrix, A(t), the following relation holds: d 11 d A1 d [A1 (t)] = A (t) [A(t)] A~(t) (3.1.15) By applying Equations (3.1.15), (3.1.14) and (3.1.13) in (3.1.11) we obtain after simple manipulations the FLDT as a function of the new state vector Y and time, t, as follows: V(Y, Z, t) = LC {J(Y, t) Z + W(Y) + i(_Y, t) Fw(_, _) :i 2i~' 2 F 2(YIl' 42' t) 12AYl + [A 5C F2 B2 + [B F J(Y 1,' t) + J(Y., 2' t)] 3 } J(YI Y,2' t) = a(Yi, t) aj(Y I t) aI jj Y2 + a t Ti1 Y3 G (Y ) Y2 .3 G (_1) .2 GN(1 ) 2 T [ N z j=1 T 42L a G1 *Y1 ) ^iT a y1 j aG2 (Y) ayI j N 8GN(Yi ) j=i sYj lj~ TGi iY2 G (Y) Y3 I2 G i ] 3 T YG K 2 Y2j 1 2 Y2j1 Y2 (I ) 3 1.(Y, t) a (, 2' t) ( t) 42 + a'(.' 2' t) of( + y1, t) 8aY2 3 + "at (3.1 .17) where (3.1 .16) w(Y) = + Equations (3.1.16) and (3.1.17) define the FLDT. Notice that in view of the assumptions in Chapter II, C exists and is readily available (since C is diagonal, see (2.2.2)), and V(Y Y2 t) was assumed differentiable, thus V(Y, Z, t) in (3.1.16) exists for all t, and 3N all points in the state space YeR Moreover, the FLDT employs only immediately available system matrices such as J(Y1, t), W(Y Y2) and their time derivatives; no matrix inversion is required. In the case that a stationary (time invariant) external load is applied, the inertia matrix J and the load torque vector 9 become time invariant. This simplifies the evaluation of the FLDT which becomes time invariant as well. To summarize, when the FLDT as given in Equations (3.1.16) and (3.1.17) is applied to the system (Equation (3.1.1)) we obtain the following system (which from now on will be denoted as the equivalent open loop system, EOS) Y Y (3.1 .18) 2 3 yz =z1 31*8 3 . In a scalar form Equation (3.1.18) may be rewritten as Y = Y ii 2i Y = Y 2i 3i (3.1.19) Y3i = Z. i = 1, 2, . N ; 3i which, as required, represents N decoupled triple integrator sub systems with the ith input, Z., representing the ith joint's jerk. Figure 3 describes the manipulator/actuators system, the FLDT and the new equivalent, open loop system EOS. Notice that in Equation (3.1.18) the EOS is unstable since its characteristic polynomial may easily be shown to be S3N = 0. Never theless, the EOS is reachable since the 3NX3N matrix M, where 0 0 IN 0 0 0 1N 0 0 1N 0 0 M 0 0 0 IN 0 0 0 IN 0 0 IN 0 \N N N I., 0 0 0 I, 0 0 0 0 0 0 IN N N ,N has full rank (3N) and, therefore, according to the pole placement theorem the EOS may have arbitrarily assigned dynamics by a proper choice of a linear time invariant feedback. In fact, due to the de coupled structure of the EOS, and as can be seen from Equation (3.1.19), similar arguments hold for each ith subsystem which is stabilizable and by a proper choice of ai.., j = 1, 2, 3 in (3.1.20) Z a Yi + ai2Y 2i + a3Y3i (3.1.20) i any characteristic polynomial may be selected for each ith subsystem as well as the EOS. (a) I z 'I ON Y 31 ZY3    y21 Y ~3N 32N ( b 1 N (b). S  (b) Figure 3. (a) FLDT; (b) Equivalent OpenLoop System 3.2 FLDT for a Single Link Manipulator (Example 1) In this section we apply the FLDT developed in Section 3.1 to a single DOF case, i.e., a DC motor with a link attached to its shaft. (See Figure 4.) This example will, of course, demonstrate linearization only since coupling is irrelevant in a single DOF case. The equations of motion for the system are derived in Appendix I and are given by (see Equation (A14)) Xl = X2 X = J [CX3 (X )] (3.2.1) X3 = f1X2 + f2X3 + L1 V where, as in Chapter II X is the link's angle measured with respect to the vertical (downward) axis; is the link's angular velocity; X is the armature current; V is the applied armature voltage; J is the total moment of inertia reflected to the motor 2 2 output shaft (J = JM r + md ); m, d are the link's mass and length respectively;  g is the gravity acceleration; / KT, KV, R, L are the motor's torque constant, back emf, armature resistance and armature inductance respectively;  r is the speed reduction ratio of the gear box; Gravity Figure 4. A Single Link Manipulator and where R K r C = r KT; (X ) = mgd sin(X1 ); f = 2 ; f. = L Following the notations of Section 3.1 let A YI = Xl Y1 =x1 A Y = X (3.2.2) A 1 Y = J [CX3 (X )] and evaluate the FLDT as given in Equation (3.1.16) to this case, noting that W, A, B and J vanish, V(Y, Z) = LC [JZ + mgd Y2 cos(Y ) f2 mgd sin(Y 1) CfY2 f2 J Y3] (3.2.3) Applying the FLDT given in Equation (3.2.3) to the System (3.2.1) we obtain the EOS as follows: Y = Y 1 2 Y2 = Y3 (3.2.4) and Y d [J1 (CX (X )] = J [CX _(X )] = J1 [Cf Y 3 dt 3 1 3 1 1 2 + f JY3 + JZ + I(Y ) CfY 2 f jy ;(Y)] = J1 [JZI (3.2.5) i. e., Y3 = Z Equations (3.2.4) and (3.2.5) describe (as expected) the EOS as a triple integrator system, with Z(t) as the new input, which physically represents the angular jerk of link. It is interesting to notice that, although the EOS state variables Y13 Y2 and Y (i.e., the angular position, rate and acceleration of link) form a linear dynamical system (a triple integrator); the arma ture current, X3, which is not an EOS state variable is nonlinearly dependent on Y and Y as follows (see Equation (3.2.2): X = C [JY + mgd sin(Y )] (3.2.6) For this purpose the step response of the EOS, and the corresponding FLDT (armature applied voltage) and armature current variation were computed in Experiment 1 and shown in Figure 5 as follows. Experiment 1. A FORTRAN program (EXP1) which simulates the system given in Equation (3.2.1) was written (its listings are given in Appendix III). The input voltage was evaluated via the FLDT (3.2.3) for the DC motor PMI MC 19P with a jerk step of 40 rad/s and zero initial conditions and the following parameter values: m = 10, g = 10, d = 1, R = 0.6, L = .0015, r = 10, K = .25, K = .25. Figure 5 describes the step responses of the armature voltage (V) and current (I) and the links angular velocity, acceleration, and position sec. Figure 5. StepResponse of the Equivalent OpenLoop System sec. Figure 5. Continued Figure 5. Continued respectively. It demonstrates that the angular velocity, acceleration and position step responses are, in fact, those of a triple integrator system, whereas, V and I illustrate the expected nonlinear behavior due to gravity. 3.3 FLDT for a Double Link Manipulator (Example 2) In this section we apply the FLDT derived in Section 3.1 to a double link planar manipulator driven by two DC motors. This example demonstrates linearizstion as well as decoupling of the general NDOF manipulator system since it contains all major dynamical components such as gravity, Coriolis and centripetal terms. Figure 6 describes the double link system configuration. The equations of motion for this system are derived in Appendix II using the Lagrange method. We reformulate them here using the notations of Chapter II. The equations of motion are given in Appendix II (Equations (A26) and (A27)) as follows: T =D en9 +D e^ +DO+2 +D^ T1 D 11e1 + D12e2 + D(O 2 + 201e2) + D1 (3.3.1) T = D 8 + D22 2 D(e2) + D 2 12 1 22 2 1 2 where 2 2 D = (m + m d + m d + 2m d d cos(2) 11 1 2 1 2 2 2 12 2 2 D = m d2 + m d d cos(2) 122 = 2 212 2 D =md2 22 2 2 Gravity Figure 6. Double Link Manipulator D = md1d sin(e2 ) D 1 (m1 + m2 ) gd1 sin( ) + m2gd2 sin(e1 + e 2) D = m2gd2 sin(81 + 2 ) and where and second and length Using g is gravity; TV, T2 are generalized torques at the first joints respectively; and mi, d1 and m 2, d2 are the mass of the first and second links respectively. the notations in Chapter II, let D FD11 D121 J(X ) = S LD12 D22J I G^x) X^ Wl (X T= 1 X :1 1 I 2 xI GD(XJ ) x wm 0 D D 0 G 1X) = G2 (X = D D > D 1 V() D . X = e X (eI T (T ^_ 1' T T2) T and notice that D, D. and D. for i, j = 1, 2, are functions of 1 1) X Equation (3.3.1) may be rewritten as follows: ^ T = J(X ) x + f(x x 1 :2 :1 :2 (3.3.2) (X1, x ) = 2 (X )L + W(x1, x2). i^ 1 .1' 2 The DC motors are modelled in a method identical to that in Section 2.3, i.e., JX2 + AXL + EKL + T = C1 L1 x3 = FX2 + FX3 +L' V where X3 is the vector of armature currents (2X1), V is the vector of applied armature voltages (2X1), and where J, A, B, C, FI F2 and L1 are as defined in Section 2.3. Following Section 2.4, the augmented model for the two link with actuators is finally given by i =x2 x2 = g(x) (3.3.3) 3 = F12 + F23 + L1V where g(X) = J1 (X )[(3 W(X ,X) .(X_) AX BX ] :1 :3 ?1' 2 1 "11 L2 Using the transformation Y =X Z1 :1 Y =X 2 = _x2 Y g(X) we obtain the FLDT by applying Equations (3.1 .16) and (3.1 .17) as follows: W(Y) D (m1 + m ) L2(Y_ ) : 2 % 2 gd^2( Y21 2D[ Y22 Y32 2D[Y21 Y31 gd1 Y21 cos (Y11 ) + m2gd2 (Y21 + Y22) ' + Y22) cos(Y11 + Y12) 2 + 3 22 +Y21 Y32] + [22 +2 ] D[Y22 ] = m2 d d2Y22 D = 42d1d2 D 12 D11 =2m2d1d2Y22 sin(Y12 D12 = m2dd2Y22 sin(Y12) Y22] cos(Y1 2 ) sin (YI) 2 D12 0 and the FLDT voltages may be explicitly given as follows: L R R( R, SKV R R1 + 1 1 + 1 + TI B1 Y21 + [1 + L11 + (D11 + J 1 ) 1 31 + [D12 + 1D12] Y32} (3.3.4) and L 2 D Z +R +L+2 +R +2AY 2 T2V2 R R2 + + J +[ +A +D +B I) D I 12 121 2 +3 12 + L 123 1 R2' R R' + r D +[ 22()l (3.3.5) 12 2 2 222 L 2 22 + 212 Y3 where all the parameters used are as defined in Section 2.3, and where Z and Z2 are the angular jerk input of the first and second link respectively. When V1 and V2 as obtained in Equations (3.3.4) and (3.3.5) are applied to the system (3.3.3) we obtain the EOS as two, decoupled triple integrator systems in the form of Equation (3.1.19). As in the previous section, an experiment was conducted to evaluate the step responses of the EOSs (shown in Figures 7 and 8) as follows: Experiment 2. A FORTRAN program (EXP2), which simulates a double link manipulator system driven by two DC motors (PMI MC23S and PMI MC19P for the first and second links respectively) (Equation (3.3.3)), was written. (Its listing are given in Appendix III.) The input voltages were computed via the FLDTs (3.3.4) and (3.3.5) for a jerk step of 2 20 rad/S and zero initial conditions for both links, with the following parameters: m = m2 = 10 [kg] d = d = 1 [m] KI = KT = 0.5 [V S/RAD], [mn/A] = K = 0.25 [V S/RAD], [Nm/A] 2 2 R, = 1 [01 R2 = 0.6 [(] L = 0.00025 [HI L = 0.00015 [H] r1 = r2 = 35 J = 0.002 [kgm2 ] J = 0.001 [kgm2] The step responses of the armature's voltage and current and the links' angular velocity, acceleration and position for the first and second links are shown in Figures 7 and 8 respectively. As can be seen from Figures 7 and 8 the EOS has a step response of two decoupled triple integrator system. Notice also that as in Experiment 1, all nonlinearities are "absorbed" in the armature's currents (Ii 12) and voltages (Vi V2). By the fact that 11 is different from I1 and V is different from V2 whereas 0 = 2, Wl = W 2, W1 = W2 we observe that all coupling effects are also perceivable through the currents and voltages only. First Link's Step Response Figure 7. sec. sec. Figure 7. Continued I.. ~7 I 0 4 .. 4 4 4 I   ....   0.00 0.10 0.20 0.30 0.40 0.50 o.o60 0.70 0.10 0.9 .0 Figure 7. Continued I I I , jf 55 7 8 S..... ........ 0 9 '* ~   ,   9.. Fu L R o l I* ' 0 819 I 0 ; ; .. J 0.O0 0.30 0.20 0.30 O.130 0. 0.60 0.70 Figure 8. Second Link's Step Response sec. ,0 O.60 r0. 70 Q. gO sec. Figure 8. Continued Figure 8. Continued t.La CHAPTER IV TIME OPTIMAL POSITIONING CONTROL (TOPC) Having obtained the form of the feedback linearizing and de coupling transformation in Chapter III for the system model we are now ready for the solution of the time optimal positioning control problem. Section 4.1 presents the mathematical statement of the time optimal positioning control for the NDOF manipulator with state variable inequality constraints and the difficulties encountered in the direct solution. In Section 4.2 the FLDT derived in Chapter III is applied, then the optimal control problem is restated and discussed. The solution to each one of the resulting N uncoupled subsystems is presented in Section 4.3 for various cases. Finally in Sections 4.4 and 4.5 a single and double link manipulator examples are solved respectively. 4.1 Mathematical Statement of the TOPC Problem The time optimal positioning control problem for the NDOF manipu lator may be stated as follows. Given the system defined in Equations (2.3.5), (2.3.6) and (2.3.7) with the following initial conditions: x(0) =x10 x2 (0) = o_ (4.1 .1 ) (0o) = c [f(x10, o0, o) + AXI] ; find the minimum time tf, and an input voltage vector V(t), te[0, t] I to be applied to the system, such that I(tf) = Xf X2 (t f) = 0 (4.1 .2) 2^ f  X 3(tf) = C1 [f_(X_1f, 0_, 0) + AX_ ] and for all te[0, t f] IX < W (4.1.3) x2i M. 1X2il < d 2x2(t) < A. (4.1 .5) dt 1 for i = 1, 2, . .N. Notice that the manipulator is at rest (static) in both boundary 2 points (Equations (4.1.1) and (4.1.2)) since X2 and  are both zero at t = 0 and t = tf. Thus, the problem is to find the optimal control for time optimal point to point positioning, while observing inequality constraints on the velocity, acceleration and jerk of the manipulator's motion. Let us now demonstrate the various difficulties encountered when a direct solution via the maximum principle is attempted. Form the Hamiltonian, H (following [A3]) H = 1 + AX X + X g(X, t) + T[F X + F X + L'1 VI (4.1 .6) 1 :m2 2  3 1:2 2:3  N The adjoint variables X (t), 1 )(t) E R must satisfy d i (H dt i = 1, 2, 3 (4.1.7) i * According to the maximum principle, the optimal control V (t), the optimal trajectory X (t) and the optimal adjoint variable's T vector X (t) = (A_ 2, 3 ) T must satisfy the necessary conditions (2.3.5), (4.1.7) and the relation (4.1 .8) H(X (t), *(t), V (t), t) H(X *(t), A*(t), V(t), t) (4.1 .8) N for all V(t) ER and t e[0, t f] whenever the inequality constraints (4.1 .3), (4.1 .4) and (4.1.5) are strictly satisfied. On a constrained arc, however, (i.e.,when either one of the inequalities becomes an equality) it is not at all clear how to replace relation (4.1.8). #In certain cases, e.g, a scalar state inequality constraints, necessary conditions on a constrained arc are given in [S4] and [M4]. For the general case, however, no theory could be found. At this point the following observations should be noted. The number of constrained arcs and the time instances at which we enter to and exit from constrained arcs are not known and no theory is available for finding them analytically. The set of necessary conditions to be satisfied by an extremal trajectory on a constrained arc is not known (no theory). Even on an unconstrained arc with given points of entry and exit, the resulting two point boundary value problem is a high order, (36 variables for a six DOF manipulator) nonlinear, highly coupled problem. There may well be many time optimal trajectories, i.e. uniqueness is not at all guaranteed. In view of the above, it is concluded that the direct attempt to solve this problem is a formidable task, thus, in the next section we introduce a new statement of the TOPC problem, utilizing the FLDT derived in Chapter III, which enables the unique solution of a restricted TOPC problem. 4.2 Statement of a Restricted TOPC Problem with FLDT In this section we employ the results of Chapter III by assuming that the NDOF manipulator system was linearized and decoupled into N triple integrator subsystems each representing a single DOF of the manipulator by application of the FLDT. Then the TOPC problem is re stated and shown to be a restriction on the original TOPC problem stated in Section 4.1. As shown in Chapter III, when the FLDT given in Equation (3.1.16) is applied to the system described in Equations (2.3.5), (2.3.6) and (2.3.7), the set of N uncoupled, linear, triple integrator subsystems given in Equation (3.1.19) becomes the equivalent open loop system. Let us now state the following optimization problem, which will be denoted as the restricted TOPC problem. For each one of the N subsystems described by Equation (3.1.19), with the initial conditions Y 1(0) : X10 1 Yi(0) = 0 (4.2.1) Y 3(0) = 0 , find the minimum time tf. ,and the optimal jerk input Z (t) to f. 1 be applied to the system on te[0, t f.] such that 1 Y li (tf) = Xlfi Y 2i(t) = 0 (4.2.2) Y 3(t) = 0 1 and for all t e[0, t I lY~il5 <2 M2i wM (4.2.3) IY 3i aM. (4.2.4) and1 and IZJ I Ai (4.2.5) for i = 1, 2, . N, where X and X are the ith compo nents of X and X if in Section 4.1 respectively. The above stated uncoupled set of optimization problems with state variable inequality constraints (SVICs) is closely related to our original TOPC problem stated in Section 4.1 as follows. Recall from Chapter III that Y, = X is the position vector of the manipu lator joints, and that Y = X and Y = EM are the manipulator :2 :2 z3 ) r hemnpuao velocity and acceleration vectors of the manipulator joints where g is given in Equation (2.3.6). Therefore, it is clear that the initial condition (4.1.1) is identical to (4.2.1), that the terminal condition (4.1.2) is identical to (4.2.2) and that the inequality con straints in Equations (4.1.3), (4.1.4) and (4.1.5) are identical to those given in (4.2.3), (4.2.4) and (4.2.5) respectively. In view of this similarity it is clear that if a solution to the restricted TOPC problem is given in the form * * *T Z (t) = (Z Z . Z ) (4.2.6) where < < * Z. 0 t tf 1f Z. = (4.2.7) 1 0 tf. then substitution of Z in the FLDT (3.1.16) yields the optimal input voltage V (t) sought for in the TOPC problem, where t is given by #f See for example the discussion of a similar case in [L1] pages 53 56. tf = max[t ] ; i = 1, 2, . N (4.2.8) 1 1 In other words, we find that an optimal solution to the restricted TOPC problem is automatically (via the FLDT) an optimal solution to the TOPC problem. The opposite relation, however, does not necessarily hold, since the optimal solution to the TOPC problem does not necessi tate that each and every one of the degrees of freedom of motion will move in minimum time. It is intuitively clear that the restrictions of this type, i.e., multiobjective optimization, enable us to obtain uniqueness which we may not otherwise have had. In our case, due to the desirability of an uncoupled control strategy for the various DOFs, this restriction is recommended and from now on by TOPC problem we shall mean the restricted TOPC problem stated in this section. 4.3 TOPC with SVIC's for Triple Integrator System As a result of the restatement of the TOPC problem via the FLDT, we now deal with N independent TOPC problems. In this section we find the time optimal positioning control for various tasks depending upon which of the SVICs is active. We commence by defining a generic triple integrator system for which we find the time optimal positioning control to the origin without state variable inequality constraints. Then depending upon the task at hand, and by exploiting a very useful property of the electro servomechanism system, we define a variety of special cases with regard F_ __ to the "activity" of a state variable inequality constraint. For each one of these cases the TOPC problem is then solved separately. For each ith subsystem, let S=Xf x 10 (4.3.1) 1 1 then the restricted TOPC problem, without SVICs for each subsystem, may be stated as follows. (Without loss of generality we shall assume that E > 0 for all subsystems throughout this work.) Problem PI. Given the system Z Xl = x2 Z: X2 = X3 (4.3.2) x3 = z and the initial conditions X (0) = (4.3.3) x (0) = X (0) = 0 find the minimum time tf and the optimal jerk input Z (t), t e [O,tf] such that X 1(t) = X 2(t f) = X3 (t) = 0 (4.3.4) and IZ(t)I A Vt e[0. t f] (4.3.5) The problem P1 is a special case of the well known time optimal control to the origin of a triple integrator system, for which a solu tion is given in terms of a switching surface, F (see for example [F1] and [S3]) 3 3 2 3/2 F =X + X /3 + e XX + C[X /2 c X 3/2 1 3 2 3 3 2 where e= sgn(X + X /2), and where the optimal control is Z = A sgn(F) (4.3.6) (4.3.7) In the case of P1, however, due to the zero boundary velocity and acceleration conditions, it was possible to obtain a closed form repre sentation of the optimal control, and the state trajectories as follows. Theorem 4.1. The optimal control for P1 is given as follows: A Z (t) = A A 0 < t< f 7 4 tf < 3tf 4 4 3tf< < t t 4 f t = 2(4E 1/3 f A where (4.3.8) (4.3.9) Proof. The system, Z, is fully controllable since 0 o j 0 .0 01 0f 0 1 0 0 0[ 1 0 in addition there is a single input, Z, thus the system Z is normal (in the sense of definition 4.15 in [A3] page 218) and therefore the solution to P1 exists and is unique [A3]. It is therefore suffi cient to find one control function, which together with its resulting state and adjoint variables trajectories satisfy the necessary condi # tions for optimality. * When Z (t), as defined in (4.3.8) and (4.3.9), is applied to the system Z then the acceleration X3, the velocity X2 and the position X1 trajectories are found by simple integration as follows. At tf X (t) = A( t) 3  [A(t t A2 2 At f2tf 2 A6 f 2 Aft f 2 t)2  < tf S 0 t <  4 tf < 3tf 44 3tf < < 4 f < tf 0 t < t 4 tf < 3t f 4 tc < i 4 4 3tf < < 4 t tf #Satisfying the necessary conditions will only indicate extremality. In our case however,it will be shown that it is a minimum. Rank (4.3.10) (4.3.11 ) 3 6 3 2 Atf Atf Atf 2 At3 tf X(t) = + 192 16 t +  t 6 2 26 3 At2f Atf 2 At3 92 A tf + 2t +; Following [A3], let the Hamiltonian function be d H = 1 + 1XX + 2XX + 3 Z then the adjoint variables X.(t) i = 1, 2, 3 m 1 S OH A0 *2 OH 2 7F2 X1 3 OH 3 X3 2 From (4.3.14) we find that X1 (t) = 10 X2 (t) = It + X20 102 A3 (t) = 2 A20t + A30 where X = X.(0) ; i = 1, 2, 3 and t e[0, t Applying the maximum principle [A3] we find that Z (t) = A sgn(X (t)) t f 0 t tf4 0 3tf ft S< 3tf < < 4 ttf defined by (4.3.12) (4.3.13) lust satisfy (4.3.14) (4.3.15) 1f] . f * Z (t) must satisfy (4.3.16) 69 tf but from (4.3.8) Z (t) has two switching times, at t = and 3tf 4 at t = , thus 4 tf 3tf) ^ ^f X (t) = K (t  ) (t  (4.3.17) 3 4 4 Equating coefficients in (4.3.17) and (4.3.15) we find that x10 = 2K 120 = K t (4.3.18) 3 2 30 = 16 K tf It is also known ([A3]) that H(t) must satisfy H(t) = 0 VtEC[0, tf] (4.3.19) in particular, for t = 0+, we obtain H(0') =1 + X 0 + 0+ + X30 A= 0 or S= (4.3.20) 30 In view of (4.3.18) we obtain 2 3K A tf2 1 160 or K (4.3.21) 3 Atf To summarize, the canonical (adjoint plus state) variables which correspond to the control Z (t) in (4.3.8) are given by (4.3.10) through (4.3.12). Since they satisfy the Euler Lagrange equations (4.3.2) and (4.3.14), the condition (4.3.19) and the maximum principle * (4.3.16), then Z (t) must be optimal control. Notice, that the minimum time tf was found by letting X1 (tf) = 0 in Equation (4.3.12) which yields )1/3 tf = 2() Q.E.D. f A The optimal control, state and adjoint variable's trajectories for problem P1 are respectively described in Figure 9. Thus far we only obtained the TOPC for the unconstrained state case. In the presence of SVICs the previous method (maximum principle) does not hold and the approach taken by [S4], [M2] and [L2] will be used. In these references, however, only scalar SVICs exist whereas, as shown in Equations (4.2.3) through (4.2.4), our single DOF subsystem must satisfy multiple SVICs simultaneously. This difficulty is removed by the following important observation concerning the appearance of velocity and acceleration constraints in positioning tasks of servo mechanisms. (In the sequel this observation is treated as an assumption.) No theory was found to exist for the vectorial SVIC case with scalar input systems. x 2 xl 2 2Figure 9. Optimal Control, State and Adjoint Trajectories for Problem P Figure 9. Optimal Control, State and Adjoint Trajectories for Problem P1 Assumption. When accelerating (decelerating) from rest, an electro servomechanism system will always reach its maximum (minimum) accelera tion before achieving its maximum (minimum) speed. This assumption is based on the physical fact that the electrical time constant of the actuator is usually about two order of magnitudes smaller than the mechanical time constant of the loaded actuator, therefore the actuator achieves its maximum current (related to torque or acceleration) much before its maximum speed (which depend on the load's and actuator's inertia) when the maximum voltage (related jerk) is applied to it. Based upon the above given assumption we now may obtain, depending upon the task at hand (i.e., depending upon E) one of the following cases. Case 1 No SVICs are active and therefore the restricted TOPC problem is identical to P1 for which a solution is obtained in Theorem 4.1. 23M (This case occurs if E <  see explanation below.) A Case 2. Constrained acceleration only. This case occurs when the following conditions are both satisfied. 3 > 5 (4.3.22) A2 and 4 2 M > ( M 2 (4.3.23) 4A2 73 Condition (4.3.22) simply means that the maximum acceleration allowed is smaller than thp highest acceleration that would be obtained for the task E, if the unconstrained case (P1) is assumed. (See Figure 9.) Condition (4.3.23) will be explained below. The optimal jerk input Z (t) for Case 2 i.e., when the SVIC VteC[0, tf I (4.3.24) is added to the statement of problem P1, is found by applying the method described in [S4] to this system. The resulting optimal tra jectories and the optimal control are given in Equations (4.3.25) through (4.3.29) and are also described in Figure 10. A 0 * Z (t) = A 0 < OM 0 t <  A SM < tf c M 2 <  A 2 I I t f M t f i tf M < f M !~ S+ A t < tf  f L M M< < , tfj t tf (4.3.25) Ix3 (t) I < (M 74 0 < t c AM ; A I Atf 2  At I < < tf  2 I tf 2 AM < A tf M <  +  t < t At At At2 2 2 cM  2A+ Mt tf 2 I t f AM I A 2 (tf) 8 I Mtf + T ] 2 AM ci t + ac t  M M if tf 2 t f 2 ci A t < tf 0 < < tf M St A M A t  M < < + t  ci aM tf + A f AM tif  t2 I A tf 2] I aM < < tf  t  if A FAt X3(t) = X 2(t) = (4.3.26) t2 A4 2 M A (4.3.27) i X (t) = aM A , '3 Atf 48 < 01M 0 t < S At3 3 2 S IM CIM 6A2 2A At3 Atf At 4 ~ +  9 ,2 Atf 8 ,2 2 Mtf Mtf 8 + 4A aM tf iM t < 2 A t 2 + 2 a 2 + [ m 2 ~+2 [Mt 3 c M 6 A2 At3 Atf 2 2t 6 2 ,3 Atf  6 + 2 M m ]t + f 2A 2 cit OMtf 4 2 Atf + 2t + f2 tf aM 4 ' 2 tf M 2A t f aM < < M ; 2 + A t tf A M < < tf A t tf where a 2 cM OM2 ) tf =  + /( ) + 2. f A + A aM tf As shown in Figure 10 the maximum velocity is obtained at t =  2 and is given by (from Equation (4.3.27)) 4 2 Sf aM am S2 + E+C 2 t CM t2 t +Tt t2 + Mtf 2 (4.3.28) (4.3.29) 4A max A  f (IM I f f aM  zL 2  I 2 + A A ,; Z(t) ___ ._______   m I1 t ^ f A am aim I X2 """ I x 2 xl Figure 10. Optimal Control and State Trajectories for TOPC Problem with Bounded Acceleration (Case 2) 77 In order not to exceed the maximum speed it is necessary for the task E and the constraints a M ) A to satisfy W M > X2 max which is exactly the condition (4.3.23) which is necessary for having Case 2. Case 3. All SVICs are active. This case occurs when the task F is such that condition (4.3.23) is not satisfied, i.e. when < 4 2 aM 4M < 2 2AM V4A Again, as in Case 2 the method for finding the optimal control on con strained arcs in [S4] was used for the restricted TOPC problem (see Section 4.2). The resulting optimal control and state trajectories are given in Equations (4.3.30) through (4.3.34) and are also described in Figure 11. A 0 t < t 0 t < t < t2 02 A t t < t 2 3 Z (t)= 0 t3 t (4.3.30) A t4 t < t5 0 t5 t 5 < 11 A t6 t tf Z t2 t3 t t A 11 x3 0M 1 1 2 Figure 11. TOPC for Case 3 Figure 11. TOPC for Case 3 X 3(t) = X2(t) = At M A(t t3 0 A(t t4 4 aM A(t t f) At2 2 (I 2 + M"t tI At 2 WM (t t32 wM w A (t t2 M 22 4t At E M 2 M At t2f) 0 t < t 0t tI t2 t3 t4 t5 t6  t5) t < t K  t K t t  t <  t <  t 0 t < t t t < t2 t4 1 t < t2 tt t2 t < t3 S tt < t5 t t6 t6 t tif (4.3.31) < t2 < t3 < t4 < t5 < t6 < t  t f (4.3.32) At3 2 3 M t2 t +7M 2 6 A ,2 2A2 A t3 A(M 'M 2 A 3 +A ( +M) t  6 2 M A 2 2aM (7A Aw ) t 2'M tM M 3A v 3 aM 2A+ 3 A 6M 6 3 aM < < ; 0 t t2 S2 < t2  + W M (t t3) E t t < t t3 t # where eM t =  1 A M t M 2 aM t3 = t + t2 (4.3.34) M x M aM "M _ +   1M A M t 4=t ft3 4 = 3  t 5 f 2 t6 f 1 It is interesting to notice that the time optimal trajectory obtained here (Equation (4.3.31)) is identical to the trapezoidal acceleration motion described by Tesar and Matthew [T3], Gnerek, Tesar andMatthew [G1I], and Matthew and Tesar [M2], [M3] for minimizing the transient amplitude of vibrations (high speed content) which reduces the positioning accuracy. X (t) = (4.3.33; WM WM 2 aM + A) Figure 12 describes the various speed trajectories obtained for the above described cases with the path as a parameter. Having obtained the solution to the restricted TOPC problem we complete this chapter by describing two examples, single and double link manipulators in Sections 4.4 and 4.5 respectively. 4.4 TOPC for a Single Link Manipulator (Example 1) In this section we apply the results of Section 4.3 to a single link manipulator system. A load mass m is attached through a rigid link with length d to the shaft of a DC motor, as described in Figure 4. The task is to rotate the load through an angle of E radians in minimum time while observing constraints on the maximum speed, acceleration and jerk of the motion as follows. Experiment 3. The PMI MC19P DC motor was chosen with the following parameters. R = 0.6 Q L = 0.0015 H K = 0.25 Ni/amp K = 0.25 V/rad/sec ~2 JM = 0.001 Kg m2 Maximum Current, I = 100 Amp. max Rated Voltage, VR = 85 V Rated Speed, 0 = 340 rad R S E4 > E3 > 2 > 1 > 0 TOPC Speed Trajectories for Various Paths with SVICs Figure 12. The load was m = 10 Kg d= 1 m The gear reduction ratio was r = 100 and the gravity acceleration was assumed to be g = 10  S The path to be traversed was = 1 rad. (All notations are consistent with those of Chapter II.) The equations of motion for the system (3.2.2) were derived in Appendix I and used in Experiment 1 of Chapter III. Those equations with the FLDT given in Equation (3.2.3) were simulated in a FORTRAN program called FIN1 (its listings are given in Appendix III) with the optimal jerk control as given in Equations (4.3.30) and (4.3.34) for the TOPC problem, Case 3. In addition, the maximum speed, acceleration and jerk were found from the motor specifications as follows: VR 85 "M KV r 0.25 100 = 3 rad/sec from the equations of motion (3.2.2) we obtain 84 angular dX2 2 2 acceleration = [rK I mgd sin(X )]/(md + r J at T 1M thus dX2 2 2 max[] = [rK I max(mgd sin(X ))]/(md + r J) dt M T max 1 M or 25.*100 100 _, 2 2 = 2 00 120 rad/sec 2 M 20 also the maximum jerk was estimated via the electrical time constant (assuming that it takes about four electrical time constants to develop the maximum acceleration) as follows. A z M 120 = 120,000 rad/sec2 1 R 0.6 . 4 40.00015 With the above given parameters, the program FIN1 simulated the system motion from X = 1 rad to the zero position. Figure 13 describes the FLDT voltage, the motor's current, the velocity and position trajectories respectively. As can be seen in Figure 13 the velocity and position trajectories identcally match the expected curves of a TOPC for a triple integrator system, indicating a perfect linearization, the voltage and current of the actuator,however, demonstrate the non linear effects of the load as it changes configuration. 0 sec. Figure 13. TOPC Trajectories in Experiment 3 86 C " .. __ __._...... ..... g 1' q r, r ,) *, I . Q. .Q 0. 2 , :' "1 :" , ao , FCn sec. gie C U2 sec Fiur 13 Continued 4.5 TOPC for a Double Link Manipulator (Example 2) An experiment similar to the one described in Section 4.4 but for a twolink manipulator is described in this section. The system is described in Figure 6. The equations of motion for the system are given in Appendix II and are also used in Experiment 2 of Chapter III. The task is: positioning a loaded double link system, driven by two DC motors, at its null position, starting from 1 radian in both joints, in minimum time, while observing similar state variable inequality con straints as in Experiment 3. Experiment 4. For the second joint we selected the same DC motor as in Experiment 3 (PMI MC19P). For the first joint the motor was PMI MC23S, with the following parameters. R =1 1 L = 0.00025 H 1 K = 0.5 Nm/A KI = 0.5 V/rad/sec 1 JM = 0.002 Kg m2 Maximum current, I = 100 A max1 Rated Voltage, V = 170 V R! Rated speed w = 340 rad/sec R The loads were m = m2 = 10 Kg a1 = O2 = 1 m Equations (3.3.3), (3.3.4) and (3.3.5) were simulated by a FORTRAN program called FIN2 (its listings are given in Appendix III). The optimal jerk inputs Z and Z were programmed as in Experiment 3. In addition the maximum speeds, accelerations and jerks for both links were found from the actuator's specifications and the equations of motion of the system (3.3.3) as follows. VR 170 M R 1 0 3.4 rad/sec K1 V 1 VR 85 w =  = 3.4 rad/sec M K r2 25 The maximum accelerations a M and a M were estiamted by the current/ acceleration equations (X_2 in Equation (3.3.3)) 1 2 I1 = rKT {[JM r + D22 + Q3 + 2Q cos(2)] e + [D22 + Q cos(02)] e2 + Q2 sin(81 + 82) + Q sin(e ) Q sin(82) (42 + 2el 2)} (4.5.1) I2 = r1K [D22 + Q cos(02)] 1 + [D22 + M r2 e2 + sin(9 + 0 ) + Q sin(8 ) e2} (4.5.2) + 2 1 2 2 1 where D22 = 22 Q = m2d1 2 S= (m1 + m )g d (4.5.3) Q2 = m2d2g 3 = Q, d /g and where 1 82 are the first and second joints' coordinates respectively. It was found that in order for the currents, I1 and I 2) not to exceed their maximum values, 100 A, the angular accelera tions a M and a had to be MI M2 = = 55 rad/sec2 M1 M2 for any 01, 92 e(0, 211'. Finally, the maximum jerks, were found in the same way as in Experiment 3. a M 55' 3 41 ^1>^' a M _ _ _ _ = 5 5 0 0 0 r a d / s e c 3 .. . 1 _RI __ . 4 0.00025 LI and a 2 55 3 C3 (iL A  = = 55,000 rad/sec 2 4 R 4 0.6 L 0.00015 With the above given parameters, the program FIN2 simulated the system motion from 01 = 02 = 1 rad at t = 0 to the null position 01 (tf) = 02 (tf) = 0. Figures 14 and 15 describe the voltages, currents and state trajectories that correspond to this motion for the first and second link respectively. As can be seen in Figures 14 and 15 the velocity and position trajectories of both links, identically match the expected curves of a TOPC for a triple integrator system, indi cating complete decoupling and linearization, the actual voltages and currents of the two motors however, demonstrate the expected non linear and coupling effects through the motion. In Figures 14 and 15 the actuators' currents trajectories may give the wrong impression that we actually produce a "bangbang" current (torque) control. This, however, is not true. The misleading form of the current is the result of the fact that both joints traverse the same path with the same velocity and acceleration constraints. In order to demonstrate this, the following experiment was performed. Experiment 5. The program FIN2 was used to repeat Experiment 4 4 ith 0 1(0) = 1 radian and 8 2(0) = .5 radian while all the other parameters were the same as in Experiment 4. The resulting voltages, currents, velocities and positions of both joints are described in Figures 16 and 17 respectively. As shown in Figures 16 and 17, the actuators currents no longer have the "bangbang" shape of Experiment 4. 4.6 Comparison with Approximated BangBang Voltage Control In order to demonstrate the advantages of FLDT in the slewing mode of TOPC, and to emphasize the effects of nonlinearities and coupling terms in the dynamics of manipulator, (if they are not accounted for,) the results of Experiment 4 were compared to those of the current approach in bangbang voltage control for slewing mode motion, as des cribed in Experiment 6. 91 o f y ; ',. *  r g_____ ____ ___ i I I.. ,I.  0  8* O : '. \ O I' I i  i i 1  g ..   .. .. .... ... .... .. .... .... ... ..... ..... _ ... ; .. .._..... .i. . . : 000 0.tp 0.00 01, 2 O,010 0.20 0.'i 0.2 0.l ,0.G 0 sec. Figure 14. TOPC Trajectories of Ist Joint in Experiment 4 sec. Figure 14. Continued Figure 15. TOPC Trajectories of 2nd Joint in Experiment 4 sec. 
Full Text 
xml version 1.0 standalone yes
Volume_Errors Errors PageID P71 ErrorID 4 P383 4 P473 4 P476 4 P515 4 P521 4 P524 4 P569 1003 ErrorText 90 CW: 01890217 