OBSERVER BASED VELOCITY AND ENVIRONMENT FORCE ESTIMATION FOR RIGID BODY CONTROL By Peter J. Hacksel B.A.Sc. University of Toronto, Toronto, 1991. A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES DEPARTMENT OF ELECTRICAL ENGINEERING AND THE INSTITUTE OF APPLIED MATHEMATICS We accept this thesis as conforming to th required standard THE UNIVERSITY OF BRITISH COLUMBIA October 1993 © Peter J. Hacksel, 1993 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. Department of Electrical Engineering and The Institute of Applied Mathematics The University of British Columbia 2075 Wesbrook Place Vancouver, Canada V6T 1W5 Date: NT.- 22, /?1;. Abstract Issues pertaining to the control of a rigid body are considered in this work. A magnetically levitated robotic wrist is here modelled as a rigid body subjected to both control and environment forces. All the algorithms presented here were implemented on the Maglev wrist for experimental verification. A good inertial model of the rigid body was needed for the estimation and control work in this work; a least squares identification procedure, found in the literature, was implemented for this purpose. Algorithms are developed for obtaining dynamic environment force and torque estimates from position errors in a velocity observer. Obtaining torque estimates from a nonlinear velocity observer is achieved by using the linear force observer as an analogue. Stability of the torque observer to a constant environment torque is proven for the one dimensional case only. Simulation and experiments are used to verify the stability in the three dimensional case. Environment force/torque measurements, alone useful for control purposes, are also used to correct a velocity observer. The resulting corrected observer design gives low noise velocity estimates and is robust in that it is not sensitive to errors caused by interaction with the environment. As a demonstration of the merit of the observer based work, a remote center of compliance controller is implemented. An inertial model is needed to provide feedforward that linearizes the dynamics to a. second order system. Corrected observer velocities are used in the feedforward and the derivative component of the control law. The linear dynamics about a remote center is verified by comparing the environment forces measured with the velocities observed. Table of Contents Abstract viii List of Figures^ List of Tables xi Acknowledgements^ 1 1 Introduction 1.1 Motivation ^ 1 1.2 Maglev Wrist ^ 4 1.3 Controller Hardware ^ 6 8 2 Inertial Parameter Identification 2.1 Overview ^ 8 2.2 Notation ^ 8 2.3 Flotor Dynamics ^ 9 2.4 Maglev Wrist Model and Assumptions ^ 11 2.4.1^Magnetic Field and Coils ^ 12 2.4.2^A/D Conversion ^ 13 Least Squares Estimation ^ 13 2.5.1^The Normal Equations 14 2.5.2^Persistency of Excitation and the Input Signal ^ 15 2.5.3^Bias of the Least Squares Estimator ^ 15 2.5.4^Online Estimation and RLS ^ 16 2.5 iv Experimental Results ^ 16 2.6.1^Inertial Properties of Added Components ^ 17 2.7 Error in Velocity And Acceleration Data ^ 18 2.8 Remarks on Inertial Parameter Identification ^ 22 2.6 3 Force/Torque Estimation 28 3.1 Overview ^ 28 3.2 Linear Force Estimator ^ 29 3.2.1^Notation ^ 29 3.2.2^Estimating Force from a Position Observer ^ 29 Nonlinear Torque Estimator ^ 33 3.3 3.3.1^Estimating Torque from an Attitude Observer 4 ^ 33 3.4 Parameter Uncertainty ^ 35 3.5 Simulations ^ 36 3.5.1^Impulse and Step Response^ 37 3.5.2^Large Angle Observations ^ 39 3.6 Experimental Verification With A Force Sensor ^ 39 3.7 Remarks on Force/Torque Estimator ^ 42 Stability Proofs For the Force/Torque Estimators 45 4.1 Overview ^ 45 4.2 Force Estimator Stability ^ 46 4.3 Torque Estimator Stability ^ 47 4.3.1^One Degree of Freedom Model ^ 48 4.3.2^Stability in Three Dimensions ^ 53 ^ 4.4 Remarks on the Stability Proofs 4.5 Extension To Multi-Link Bodies: The Nicosia and Tomei Observer ^ 62 62 5 Velocity Observation ^ 65 5.1 Subtracting the Error Estimate ^ 65 5.2 Correcting a Velocity Observer with A Force Observer ^ 68 5.3 Remarks On Observer Correction ^ 68 6 Control About a Remote Center of Compliance^ 72 6.1 Overview ^ 72 6.2 Experimental Results ^ 73 7 Conclusions^ 79 7.1 Further Work ^ 80 Bibliography^ 81 A Quaternion Algebra^ 84 A.1 Introduction ^ 84 A.2 Properties of the Quaternion ^ 85 A.3 Rotation of A Frame ^ 85 A.4 Inverse of a Rotation ^ 86 B Potential Energy of the Rigid Body ^ 87 C Bound For Corriolis/Centrifugal Term in Serial Link Mechanisms^89 D Operations Manual and Source Code Guide for the UBC Maglev Wrist ^90 D.1 Overview ^ 90 D.2 Hardware Configuration and Setup ^ 90 D.3 Account Setup ^ 91 D.4 Software ^ 91 D.4.1 Start Up Procedure ^ 93 D.4.2 Additional Code ^ 94 vi D.4.3 Modified Code ^ E Sample Source Code^ vii 96 98 List of Figures 1.1 UBC Maglev Wrist - Exploded View ^ 5 1.2 Current Control Architecture of UBC Maglev Wrist ^ 7 2.3 Flotor With Washer Shaped Piece Attached ^ 18 2.4 Motor With Washer Shaped Piece and Steel Block Attached ^ 20 2.5 Flotor With Plate Attached ^ 21 2.6 Flotor With Washer Shaped Piece and JR3 Sensor Attached ^ 24 2.7 Identified Parameter Estimates for Flotor Alone ^ 25 2.8 Sample Acceleration Measurements In Flotor Parameter Identification ^ 26 2.9 Power Spectrum of Acceleration Estimates ^ 27 3.10 Force Estimator ^ 30 3.11 Mass - Spring - Dashpot Model for Error Dynamics ^ 32 3.12 Impulse and Step Response for the Force Estimator ^ 38 3.13 Impulse and Step Response for the Torque Estimator ^ 40 3.14 Torque Estimator Response With Large Initial Error ^ 41 3.15 Force Estimator and JR.:3 Comparison ^ 43 3.16 Torque Estimator and JR3 Comparison ^ 44 4.17 Example Lyapunov Functions for One Dimensional Case ^ 50 4.18 Contour Plot of Example Lyapunov Function ^ 4.19 Phase Plane Representation of Sample Trajectory ^ 51 53 4.20 Physical Model for One Dimensional Torque Estimator ^ 54 5.21 Correcting a Velocity Observer With Output Term ^ 66 viii 5.22 Output Corrected Velocity Observer Experiment ^ 69 5.23 Correcting a Velocity Observer With a Second Observer ^ 70 5.24 Input Corrected Velocity Observer Experiment ^ 71 6.25 Monitored Points On Flotor ^ 74 6.26 Motion of Flotor Attached Points ^ 74 6.27 Three Dimensional Plot of Flotor Trajectories for Rotator ^ 76 6.28 y Axis Rotator: Stiffness in x Axis.^ 77 6.29 y Axis Rotator: Stiffness in y Axis.^ 78 D.30 Detailed Hardware Configuration Using SkyBOLT ^ 92 ix List of Tables 2.1 Notation Used in Inertial Parameter Identification ^ 9 2.2 Results Of Parameter Identification For Flotor Alone ^ 17 2.3 Results Of Parameter Identification For Flotor With Washer Shaped Part Attached 19 2.4 Results Of Parameter Identification For Flotor With Washer Shaped Part and Steel Block Attached ^ 19 2.5 Results Of Parameter Identification For Flotor With Washer Shaped Part and Plate Attached at 56 Degrees to x axis ^ 22 2.6 Results Of Parameter Identification For Flotor With Washer Shaped Part and Plate Attached at 146 Degrees to x axis ^ 23 2.7 Results Of Parameter Identification For Flotor With Washer Shaped Part and J1t3 Force Torque Sensor Attached ^ 23 2.8 Results For Flotor Parameter Identification Using Unfiltered Position Data ^23 Acknowledgements I would like to thank my supervisor, Dr. Salcudean for his guidance and support both towards my thesis and otherwise. He is always willing to make time for his students and to help them with any problems that they have. I would also like to thank the technical staff in the robotics group, in particular Niall Parker and Mark Milligan for cheerfully enduring my never ending series of technical questions. Finally I would like to thank my close friends, Alan Lynch, Brian Perkins and Danny Pettitt for helping Inc keep my sanity at times of crisis. I dedicate this work to Heather or Cristian, whose troubled introduction into this world gave me the resolve. xi Chapter 1 Introduction 1.1 Motivation The UBC magnetically levitated ( Maglev) wrist was designed to accomodate the need for better mechanical manipulators for delicate assembly tasks both under autonomous control and in teleoperation. The wrist has been used both as a master device and a slave, the former usually in the form of a sophisticated joystick capable of force reflection. A Maglev master has been used successfully to control a variety of devices for tasks of varying scale. The applications of the wrist range from control of heavy duty hydraulic equipment [1], to medium scale tasks as a fine scale extention to a CRS 460 robot arm [2], to the micro manipulation task of controlling the tip motion of a scanning tunnelling microscope [3]. The novel design of the Maglev wrist (a rigid flotor is levitated by Lorentz forces created by passing current through coils in a magnetic field) provides a low friction, programmable impedance manipulator ideal for delicate assembly tasks [4], [5]. Creating a manipulator by the levitation of a rigid flotor has the added advantage that the dynamic response of the flotor is well modeled as a rigid body subjected to known forces and torques (known at least when the flotor is not in contact with its environment). As a result, a simple dynamic model of the rigid body is easily derived and parameters are easily identified. A further advantage is that rigid body control has been studied extensively, particularily in the area of spacecraft and sattelite control. Control of a rigid body can be decoupled into the trivial problem of position control and the more involved problem of attitude control. By no means an exhastive list, several attitude control schemes are given in: [6], [7], [8], [9] and [10]. Many applications of the Maglev wrist involve contact with an environment. In such cases 1 Chapter 1. Introduction^ 2 it is common to use of impedance control [11] to maintain stability. When contact with stiff environments is necessary, force control may be more suitable. An excellent treatment of the different approaches to force control is given in [12]. Impedance control involves designing the control law to achieve a desired impedance in the dynamic response of the manipulator to disturbances. For a simple second order controller impedance control can be seen as providing the wrist with a desired stiffness with stability maintained by damping. Ideally, a PD controller is all that is needed to provide a stable controller. Unfortunately, problems arise when the stiffness of either the manipulator or the environment is high. It is shown in [13] that when a compliant, moderately damped manipulator comes in contact with a stiff environment, the controller damping can be insufficient to maintain stability. If damping is insufficient, effects of computational delay or other unmodeled dynamics can destabilize the system. Kim and Hannaford describe some of the basic problems of stability when there is a computational or other time delay in teleoperation [14]. Destabilization is modeled by Kazerooni in many works relating to the use of robotic "extenders" to human appendages [15], [16], [17]. Eppinger and Seering demonstrate problems of instability when the manipulator is under force control and the force sensor dynamics are included [18]. A more general treatment of unmodeled dynamics leading to instability in contact tasks is given by An and Hollerbach [13]. Force control, and force reflection in teleoperation both require knowledge of the environment forces and torques acting on the manipulator. Force/torque sensors may be used for this purpose, but the sensors can be prohibitively expensive. They may only provide estimates of forces and torques acting upon the sensor core; contact of the manipulator itself with the environment may not be always detected. Environment force may be measured by subtracting known control forces (those imposed by the coil currents for the case of the Maglev wrist) from the total force arrived at through numerical double differentiation of position estimates. In most cases this approach would lead to very noisy estimates of the environment force. Filtering the estimates generates a detrimental Chapter I. Introduction^ 3 phase lag and can lead to stability problems in the control. A more suitable method of obtaining low noise force/torque estimates is based on an observer design. Obtaining force estimates from observers has not been extensively treated in the literature, but a few approaches have been suggested. The most notable contibution in terms of estimating environment force from observers is given by Huang and Tzeng in [19] and [20]. An observer is constructed for a generalized robot model whose observed variables are a transformed version of the generalized postion and velocity as well as the environment force. With sonic reasonable restrictions as to the type of input signals, and to the observability of the robot model, it is shown that the observer variables converge exponentially to the true values. The main disadvantage of Huang and Tzeng's approach is that the observer requires knowledge of not only the true position, but also the true velocity. In many cases, and in the case of the Maglev wrist, velocity is not measured directly, and can otherwise only be obtained by numerical differentiation leading to noisy estimates (a separate observer could not be used to obtain the true velocity if environment forces are unknown). While preferable to direct differentiation, Huang and Tzengs approach would still lead to noisy estimates if a low noise velocity signal is not available. A similar approach is found in [21], and is applied to estimating force and torque for a walking robot's two degree of freedom leg. Ohnishi and Murakami [22] also use an observer to estimate joint torques for a serial mechanism. The technique uses a velocity input to produce an estimated environment torque shown to be equivalent to environment torque passed through a first order lowpass filter. Again such an approach is unsuitable if the manipulator velocity cannot be directly measured. Estimation of environment force is presented here using a observer provided only with the position and not the velocity of the rigid body. The response of the observer to environment forces is shown to be a linear second order system. A less obvious use for environment forces estimates is in the correction of a velocity observer. Velocities needed for actuation of a control law are often unavailable. Velocity observer designs Chapter 1. Introduction^ 4 generally need the force acting on the rigid body to produce consistent velocity estimates. It is seen here that estimated environment forces can be used to correct such observers. Control of the levitated flotor previously consisted of a basic PID control strategy about a frame fixed to the stationary stator resulting in a compliance with a fixed center. A remote center of compliance is often more desireable and has many possible applications. A simple example being to provide a natural feel for the maglev wrist when used as a joystick by having the center based at the handle center. More importantly, a remote center of compliance can ensure better performance for particular tasks. A peg insertion to a chamfered hole task, for example, would have better chances of success if the center of compliance were to be programmed at the peg tip, rather than at the manipulator center. Peshkin [23] addresses the problem of choosing the compliance matrix that ensures that the response is error-corrective for any situation that may arise in a simple task. Prespecified rigid environments are chosen for this purpose. The remote center of compliance control design used in this thesis is presented in [5]. The main thrust of this work has been to generate a framework for more sophisticated control to be applied to the wrist flotor. A dynamic model of the wrist is developed and identified, and an algorithm for estimating forces and torques without a sensor is developed. The latter information is used to correct a linear and angular velocity observer and to develop a PID controller with a remote center of compliance. 1.2 Maglev Wrist Previous work on the UBC Maglev wrist [1], [4] gives in depth descriptions of both the Maglev wrist and the controller hardware. Such an in depth presentation here would be redundant. However, the controller hardware has been completely upgraded, and a brief description of the new architecture is relevant and will be presented here. An operations manual is given as Appendix D. The Maglev wrist is shown in exploded view in Figure 1.1 with a basic description of the operating design. Chapter I. Introduction^ 5 Flotor Top LED Mounting Columns Vertical Magnet Assembly PSD Mounts Vertical Coil Horizontal Magnet Assembly Flotor Base Horizontal Coil Support Post Stator Base Figure 1.1: UBC Maglev Wrist - Exploded View The flotor is shaded to differentiate from the unshaded stator. Flat coils present on the flotor are shaded slightly darker and are labeled Cl through C6. Currents are supplied to these coils by lightweight umbillical cables. The coils are situated in a strong magnetic field created by the still darker stator-mounted magnets. Field direction for a sample coil is given on the bottom right pair. The a-, y, z frame shown is refered to in this text as the flotor frame. Light emitted from LED's on the flotor (top flotor part.) are directed through pinholes at PSD's on the stator (top stator part) providing position and orientation information about the flotor frame relative to the stator. Wrist design by S.E. Salcudean, original drawing by C.T.Chen Chapter 1. Introduction^ 6 1.3 Controller Hardware The controller hardware for the Maglev wrist is implemented in a series of processors connected in a VME1 bus arrangement. Memory and I/O may be shared between processors in the VME cage. Wrist coil current drivers and PSD2 sensor drivers are attached to the D/A and A/D cards respectively in the VME cage. A brief description of the hardware configuration is presented here, with an emphasis on details pertaining to the implementation used in this work. The wrist controller processing is divided between two Sparc processors, one running in real time mode under the VX Works operating system and one running Sun OS unix and mated to a SkyBOLT processor. The combination of real time, and batch mode processing provides an efficient use of processing power. Figure 1.2 outlines the control architecture. Two main loops are responsible for the sensing and control pertaining to the wrist operation. The first loop, performed in VX Works, consists of the real time VX Works board handling the timer interrupts, sensor and actuator I/O. The second loop, performed on the SkyBOLT, consists of slower identification and control algorithms. The Sun OS processor shares a section of memory with the VX Works board, meanwhile utilising the processing power of the SkyBOLT's 1860 processor. Memory is shared through the use of a C language structure shared between the two programs. Handshaking is coordinated by use of a common flag, which passes control of the memory area back and forth with each sample update. In the current implementation, the control update operates at a maximum of 500 Hz, half that of the maximum of approximately 1 kHz imposed by the speed of the A/D conversions. 1VME is a bus architecture standard. The cage is composed of slots capable of connecting several pocessors and I/O devices. 2PSD stands for Position Sensing Device. An optical beam on the flotor is directed onto a light sensitive receiver on the fixed stator. Differential voltage on the sensor leads allows determination of the beam location. Three separate light beams directed onto three separate sensors completely determines position and attitude of the flotor. A complete description of the configuration and calculations necessary to determine the latter are given in [4]. Chapter 1. Introduction^ 7 D/A Current Driver Shared Memory Defined in wnst.h I^I SKVBolt MO Processor Sun OS Unix Processor Maglev Wrist VX Works Real Time Processor Running:^" Running: wristsky^wristvw A/D Sensor Elect VMS Bus Figure 1.2: Current Control Architecture of UBC Maglev Wrist Chapter 2 Inertial Parameter Identification 2.1 Overview Most non-adaptive control strategies and observer designs require a dynamic model of the manipulator. One of the advantages of using the maglev manipulator is that it may be modeled well as a rigid body subjected to known forces and torques. Throughout this work a rigid body model is assumed. A rigid body dynamic model may be expressed by a set of nonlinear ordinary differential equations (Newton - Euler equations). While the equations are nonlinear in velocity, by choosing an arbitrary body attached frame they may be expressed as a linear function of the inertial parameters [24]: 0 = [m, me1 mc2, mc3, J11, J12, J13, J22, J23, J3311'. (2.1) , The inertial model of the rigid body is completely described by 0, where m is the mass, ci is the ith component of the direction vector from the arbitrary frame to the center of mass, and Jij is the ij entry of the inertia matrix about the arbitrarily chosen frame. Since the dynamical equations of a rigid body can be shown to be linear in these inertial parameters they may easily be estimated through the standard recursive least squares approach. 2.2 Notation The linear-in-the-parameters form of the dynamical equations is derived by An et. al. [24], and will be presented again here. Results from these derivations will also be used in subsequent chapters. First a brief definition of the notation used in this chapter is presented in Table 2.1. Chapter 2. Inertial Parameter Identification ^ 9 Table 2.1: Notation Used in Inertial Parameter Identification A point in space is denoted by and undertilde. f,r1 XF oF J, g Vector quantites have no undertilde. Coordinate representation of a vector in the flotor frame. The origin of the subscripted frame, in this case the flotor frame. Matrices are in capitals. A subscript on the inertia reflects the frame about which the inertia is computed. In this case the inertia matrix is computed about the center of mass frame. 2.3 Flotor Dynamics The dynamics of a rigid body subjected to an applied force are described by the Newton - Euler equations. Expressed in body coordinates with the center of gravity at the point c and the origin oF they take the form: ftotal = fapplied + mg ^ (2.2) = 7716F = Tapplied^(c OF) X fapplied Jcw^x Where rc is the net torque experienced by the flotor about its center of gravity, ../c is the inertia about the center of mass, and the applied force and torque, fapptied and Tapplied, are those exerted by the coil currents. The mass and moment of inertia about the center of mass are denoted m and Jc respectively. The equations need to be transformed before they can be compared to data observed from the flotor. The center of gravity of the flotor is not accurately known a priori, and hence it is necessary to represent the dynamics in terms of the chosen flotor origin defined as in Figure 1.1. It also turns out that choosing the moment of inertia to be represented about the flotor origin, rather than the center of gravity, will result in dynamics that are linear Chapter 2. Inertial Parameter Identification^ 10 in the inertial parameters. With fapplied (2.3) M^g) (OF — g) + 771 (W X (.+.1 X (c — 0 )) 111(i1 X (c — 0 ) F we substitute for the applied force from (2.3), into (2.2) to obtain rapplied = (C — 0 )F X (7-12 (OF — g) 44.7 x w X (6. — 0s)) IncZ, x (c — o F) ) (2.4 ) Jc4.;)+^x Jcw. Simplification occurs when the moment of inertia is described about the fiotor frame as: j°F = jc 2 [^°F) Xi^• (2.5) Substituting (2.5) into (2.4), and using the identities: [ax] [13x] = [bx] [ax] (2.6) [ax] b= — [bx] a (2.7) and the standard matrix form of the cross product: -^0^—a3^a2 [ax] = a3^0^—a1 (2.8) —a2^al^0 we obtain that fapplied^=^m (OF^Loxwx^c — o^mJ X 7-applied^=^(c — OF) x in (OF — g) d-^cd3 x JoFw. — Equation (2.9) may be represented in matrix form: c — oF (2.9) 11 Chapter 2. Inertial Parameter Identification ^ In mei -ft - mc2 f2 inc3 f3 [OF — g^x] + [w xi [4.,i xi^0^1 J11 o^[(g — oF) x]^[co•] + xi ko J12 (2.10) J13 T2 J22 J23 J33 - A(t)0 _ where: [WO} = - (.01 W2 W3 0 0 0 0 W1 0 W2 W3 0 0 0 W1 0 W2 W (2.11) and c =^ C^0^• F 2.4 Maglev Wrist Model and Assumptions The dynamics presented in Section 2.3 are based on the assumption that the flotor is a rigid body. Further, it is assumed that forces and torques applied to the flotor are measurable in some known fixed frame attached to the flotor. With a practical implementation, these assumptions are not entirely valid. While the rigid body assumption is likely a more than adequate representation of the wrist, our knowledge of the forces and torques exerted on the ;Rotor is less than ideal. A brief description of the various sources of error in the force and position measurements will be presented here. Chapter 2. Inertial Parameter Identification ^ 12 2.4.1 Magnetic Field and Coils Forces are exerted on the flotor by driving a known current into the flat coils situated on the flotor. Each coil is situated between four strong magnets as can be seen from Figure 1.1. The magnetic field is roughly constant in the gap between each magnet pair, but increases significantly near the magnet surfaces. The magnetic field between magnet pairs was measured by use of a hand held gaussmeter. No significant deviation was found between the six magnet groups. Fields remain fairly constant within the operating region of the gap, and increase to nearly 150% of the center field within a millimeter of a magnet surface. The measurements are crude, but errors in the field strength of the magnets were verified to be at least smaller than 10% within 3 to 4 mm of the middle of the gap. For accurate identification, then, it is imperative that the flotor not deviate significantly from the base orientation. In the experiments outlined in Section 2.6 the maximum amplitude of displacement and orientation angle was about 1.0mm and 0.001rad respectively. The force/current relationship is assumed linear, and no corrections have been applied for the effects of eddy current damping, or the dependence of the coil force on its position and orientation in the field. Coil forces are computed from current relations only, and are assumed to act at the coil center in a direction normal to the plane of the coil. The transformations of coil currents to the flotor frame are based on this assumption. The validity of the latter assumptions was not thoroughly investigated and more accurate results than those presented in Section 2.6 might be possible with a more detailed model. The assumptions are valid if the flotor is stationary and each coil is properly centered between each magnet pair. Hence for small displacements and low velocities the assumptions should not significantly affect the results. A better model of the effects of the flotor state variables on the force torque relation would very likely be necessary for online identification in a practical setting as the wrist might be then expected to utilize its full range of motion. Chapter 2. Inertial Parameter Identification ^ 13 2.4.2 A/D Conversion Position sensing in the wrist is achieved through analogue to digital conversion of the PSD sensors. Signals are multiplexed to a 16 bit A/D converter resident in the VME cage. 16 bit resolution is not possible due to noise picked up from various sources. Random noise levels are generally small, but problems do arise in the multiplexing that give rise to significant error. The conversion time for the A/D converter is approximately 50ps, and thus sensing positions for all 12 signals (4 from each PSD), requires approximately 600ps. Thus with the wrist operating at 1 kHz, the last A/D signal is measured more than 1/2 sample later than the first. Hence, position sensing is degraded if the wrist is moving quickly. Acceleration measurements become poor if the system operates at a high frequency due to the poor timing; many regularly spaced spikes appear in the acceleration calculations. Thes spikes disappeared and the calculated accelerations were more reliable with the wrist operating at 500 Hz. Velocity and acceleration data was filtered before being used in the estimation process, see Figures 2.8 and 2.9. 2.5 Least Squares Estimation Estimation of the inertial parameters is, in theory, fairly simple due to the fact that the dynamic equations can be expressed in linear form. There are, however, many practical issues involved in obtaining consistent results. The most common identification techniques, used here, are the Least Squares Algorithm (LS) and its recursive form (RLS),1 the properties of which will be outlined. Inertial parameter estimation was performed both on and off line, using the LS and RLS estimation routines respectively. Force and position measurements were stored in Matlab, and the LS algorithm then applied. Using two completely different implementations of essentially the same estimation procedure, the existence of no obvious programming errors was verified in that the estimates matched to within experimental error. The offline procedure can be expected to produce the best results since good prefiltering of the data could be achieved 1Both forms will be collectively referred to as least squares when referring to properties common to both algorithms Chapter 2. Inertial Parameter Identification ^ 14 without any phase change in the data (by using an IIR filter once in each direction using the Matlab f ilt f ilt option). Linear phase FIR filters were found to be not effective unless very high order filters were used, and a IIR filter was used instead. An eighth order elliptic filter with a cutoff of 42.5Hz was chosen as the IIR filter to smooth position estimates before numerically double differentiating. As is seen in Figures 2.8 and 2.9 the cutoff is determined from the data. Matlab routines are designed for numerical robustness rather than speed of computation. Close correspondence between the online and offline results suggest that online computations provide sufficient accuracy. 2.5.1 The Normal Equations Least squares estimation of a matrix equation of the form (2.10) is usually performed under the assumption that the error model takes the form: y (t) = A MO - s (t)^ (2.12) where E (t) is a. white noise process with covariance matrix E. It is assumed that y(t) and A (t) are sampled at times tn, = nTsamp where Lamp is the sampling interval. Thus if Y E- [y(ti),y(t2),e(t3),...,e(tN)F ^ (2.13) and - y(ti) - A (ti) 012) (t2) Y (6) A(i3) _ A (tN)_ (IN El-- E (ti)E (t2) e+ E(13) (2.14) E(tN)- A8 +r The standard estimator is found from the normal equation: = (ATA)-4 ATy^ (2.15) 15 Chapter 2. Inertial Parameter Identification ^ It is well known that (2.15) minimizes the expected value of the cost function: (2.16) Jo (y — AO)" (y — AG)^ with a minimum cost of E [4] = E [ET 6] = E. 2.5.2 Persistency of Excitation and the Input Signal Parameter identification by least squares requires careful selection of an input signal. Intuitively, it is necessary to have motion in translation as well as rotation in each of the six degrees of freedom in order to estimate all the inertial properties. Numerical difficulties can be expected if any of the six degrees of freedom have significantly greater motion than the others. More rigorously stated, a requirement for the least squares procedure to produce a unique result is that the matrixTiv-E,,,N=1A (t„)T A (t„) have full rank. If the condition number of this matrix is large, i.e., the matrix is nearly singular, then it can be expected that the least squares result is poor. The latter fact is easily seen from the normal equations (2.15). Each of the six elements in the applied force/torque wrench were supplied a separate filtered pseudo-random binary signal to ensure that the motion was sufficient in all directions and orientations. Filtering was applied to the input signal to limit the actual frequency content of the velocity and acceleration of the body as estimates of the latter would be contaminated with noise. A bandlimited signal is more desirable than a pseudo random binary signal, see Figures 2.8 and 2.9. 2.5.3 Bias of the Least Squares Estimator It is well known that the least squares estimator gives biased estimates if the error E is correlated, or in other words, if errors are coloured. Measurement errors are usually well modeled as white noise, however, other sources of error that might produce some colour were presented in Section 2.4. Chapter 2. Inertial Parameter Identification ^ 16 2.5.4 Online Estimation and RLS To facilitate future development of adaptive control schemes, the least squares identification was also implemented recursively. Changes to inertial properties of the wrist flotor could occur if the wrist is used in picking up objects. It might also be possible to extract information about the impedance of contact environments from the changes in the parameter estimates. The main difference in the RLS algorithm as compared to the LS algorithm is the existence of a forgetting factor in the former. It is well known that the forgetting factor weights data in the least squares estimate exponentially in the data samples with the decay rate equal to the forgetting factor. The forgetting factor then affects the adaptation rate of the parameter estimates. Since for the experiments in this work the inertias were expected to remain constant, the forgetting factor was chosen to be large (0.9999). The large forgetting factor ensured that the input signal sufficiently excited all six degrees of freedom and resulted in reliable and results repeatable to within 1%. Filtering in both directions to remove phase effects cannot be done when estimation is done on line. It was found that insufficient noise reduction was possible with FIR filters not requiring excessive computation. To circumvent this problem, position estimates were filtered with a fifth order Butterworth filter with a cutoff of 42.5Hz. The filter was approximated as linear phase in the passband, and a delay of approximately 7 samples (14ms) was accounted for in the identifcation. Naturally this delay results in a 14ms delay in the entire identification procedure, a delay that is unimportant in the static case, and should be fairly unimportant in most applications if inertial parameters are varying. 2.6 Experimental Results Since the flotor inertial parameters have not previously been measured accurately, the results of the estimation procedure could not be verified directly. To test the parameter estimation, various parts, with known inertial properties, were mounted on the flotor, and the resulting changes in the inertia estimated and compared to those expected. The good performance of Chapter 2. Inertial Parameter Identification ^ 17 Table 2.2: Results of the Identification for Flotor Alone Mass (kg) 0.604 Center of Mass e -0.04 0.03 -0.11 Inertia Matrix 0.781 0.003 -0.007 0.003 0.819 -0.005 -0.007 -0.005 1.038 the control strategies used in subsequent chapters also verifies the inertial estimation accuracy. Both the mass and the inertia tensor for a rigid system are the sum of the of the component parts of the masses and inertia tensors respectively. Changes to the inertia then are easily compared directly. The change in the center of mass of the system when a part is added, however, depends on the original (unknown) system, and so the center of mass correction is somewhat more difficult. For the purpose of verifying the correctness of the change of the center of mass, it was assumed that the flotor origin was the center of mass of the flotor itself (consistent with the identification results to within millimeters). The good performance of the observers in the following chapters verifies that the center of mass estimates are reasonably accurate. 2.6.1 Inertial Properties of Added Components Three parts were chosen to add onto the flotor to change its inertia; a washer shaped piece of aluminium, a rectangular aluminium plate, and a steel block. The parts were chosen to have easily computed inertial properties. Five experiments were performed as described in Tables 2.2 through 2.4. Figures 2.3 through 2.4 show the configurations under which the parts were assembled in Experiments 2 through 5. In the third and fourth experiments the washer and plate were assembled in the same manner, except the plate was mounted in the x-y plane with the long end 56 degrees and 146 degrees to the x axis respectively (See Figure 2.5). The algorithms in the following chapters require a JR3 force/torque sensor, and so the inertial properties with the J113 mounted as in Figure 2.6 were also calculated. The inertia of the Chapter 2. Inertial Parameter Identification ^ 18 Top surface of flotor is 1.25" above^ ^14— 2.5' flotor frame 11-0.8T1 0.5" Fheight • Flotor Fwidth Figure 2.3: Flotor With Washer Shaped Piece Attached JR3 is not known, and a uniform density was assumed. The latter assumption provides reasonable but not highly accurate results. However, the previous experiments provide confidence in the results with the JR3 'mounted. Accelerations were computed for both the smoothed and unsmoothed position data, as shown in Figure 2.8. Spectra shown in Figure 2.9 demonstrate the necessity of noise reduction for reasonable acceleration estimates. 2.7 Error in Velocity And Acceleration Data It was found through experiment that prefiltering the velocity and acceleration estimates was necessary to obtain reasonable results. To understand the problems encountered when there is error in the measurement of A consider the model in (2.12) with true velocities and accelerations represented by A and errors in these estimates represented as W. 19 Chapter 2. Inertial Parameter Identification ^ Table 2.3: Results Of Parameter Identification For Flotor With Washer Shaped Part Attached Mass(kg) 0.696 Center of Mass c -0.04 0.03 0.16 Inertia Matrix (gm 2 ) 0.909 0.018 -0.026 0.018 0.906 -0.006 -0.026 -0.006 1.079 Estimated Difference 0.0918 Calculated Difference 0.085 0.00 0.00 0.27 0.00 0.00 0.15 .127 0.015 -0.019 0.124 0.000 0.000 Estimated Values 0.015 0.088 -0.001 0.000 0.124 0.000 -0.019 -0.0010 0.041 0.000 0.000 0.032 Table 2.4: Results Of Parameter Identification For Flotor With Washer Shaped Part and Steel Block Attached Estimated Values Mass(kg) 1.133 Estimated Change 0.5258 Calculated Change 0.530 Center of Mass ( cm) 0.11 -0.00 3.70 0.15 0.04 3.81 0.00 0.00 3.33 Inertia Matrix (gm 2 ) 4.145 0.105 -0.050 0.105 4.250 0.089 1.115 -0.050 0.089 3.364 0.109 -0.043 0.109 3.431 -0.130 -0.043 -0.130 0.077 2.930 0.000 0.000 0.000 2.919 0.000 0.000 0.000 0.063 Chapter 2. Inertial Parameter Identification ^ 20 Figure 2.4: Flotor With Washer Shaped Piece and Steel Block Attached Define: 4:1) A + W (2.17) If we assume W is a white process, the normal equations become _ (4,7,43,)-1^y (2.18) = ((A + W) (A + w)T) (A + w)T y (ATA + 2ATW + WTI47)-1(ATy + WTy) or (AT A + 2ATW WTW)Ô = (AT y WT y) ^(2.19) Chapter 2. Inertial Parameter Identification ^ 21 Figure 2.5: Flotor With Plate Attached Notice that: E [147TW] = E^ (2.20) E [Fry] = 0 E [WT.A] = 0 where E is the covariance of the error W. Then taking expectations of both sides of (2.19), we get the expectation of the biased estimate, O' with E [O1 = (ATA E)-1 (ATA)0,^ (2.21) and where ö (ATA)-1 ATy is the unbiased least squares estimator. Thus, for high noise levels, inertial estimates are lower in magnitude than their true values. In fact, if the noise level is much larger than the signal level, (as is true if no filtering is done to the position data before numerically differentiating), the inertial estimates will be near zero. The deterioration of the inertial estimates in the absence of filtering was confirmed by experiment. Compare Table 2.8 with Table 2.2. 22 Chapter 2. Inertial Parameter Identification ^ Table 2.5: Results Of Parameter Identification For Flotor With Washer Shaped Part and Plate Attached at 56 Degrees to x axis Mass(kg) Center of Mass ( cm ) Estimated Values 0.820 Estimated Difference 0.215 Calculated Difference .200 0.32 0.61 0.28 0.36 0.58 0.39 0.22 0.33 0.42 Inertia Matrix (gm2) 1.400 -0.137 -0.136 0.619 -0.140 -0.135 0.717 -0.246 -0.129 -0.137 1.337 -0.196 -0.140 0.407 -0.164 -0.246 0.518 -0.192 -0.136 -0.196 1.602 -0.135 -0.164 0.503 -0.129 -0.192 0.564 As a result of the sensitivity of the least squares estimator to signal noise, it is essential to prefilter the signal before differentating and using the RLS algorithm. The optimal cutoff for the filter is at the point where signal noise is about the same as the error noise. For a bandlimited signal, such a filter should provide the best results. 2.8 Remarks on Inertial Parameter Identification The experiments performed demonstrate that the identification algorithm provides reasonable estimates (to within about 15%) of the parameters of a complete six degree-of-freedom rigid body model. Adding parts with known inertial characteristics provides an idea of the accuracy of these parameter estimates. The calculated values for the added parts themselves are not completely accurate, the major component of error being introduced in the measurement of the distance of the parts from the fiotor frame origin. The calculated values are probably accurate to within 5%. Better results would likely require a better model of the force/current relationship in determining the true force and torque acting on the flotor. Chapter 2. Inertial Parameter Identification ^ 23 Table 2.6: Results Of Parameter Identification For Flotor With Washer Shaped Part and Plate Attached at 146 Degrees to x axis Estimated Values Mass(kg) 0.822 Estimated Difference 0.218 Calculated Difference 0.200 Center of Mass (cm) -0.61 0.38 0.56 -0.57 0.35 0.67 -0.33 0.22 0.42 Inertia Matrix (gm 2 ) 1.267 0.170 0.263 0.170 1.472 -0.232 0.263 -0.232 1.614 0.486 0.167 0.270 0.167 0.653 -0.227 0.270 -0.227 0.575 0.518 0.246 0.192 0.246 0.717 -0.1292 0.192 -0.129 0.564 Table 2.7: Results Of Parameter Identification For Flotor With Washer Shaped Part and JR3 Force Torque Sensor Attached Estimated Calculated Values (approx.) Mass(kg) 1.019 1.004 Center of Mass (cm) 0.05 -0.04 2.13 0.00 0.00 3.07 Inertia Matrix (gm 2 ) 2.625 0.052 -0.168 0.052 2.774 0.032 -0.168 0.032 1.238 2.231 0.003 -0.007 0.003 2.269 -0.005 -0.007 -0.005 1.088 Table 2.8: Results For Flotor Parameter Identification Using Unfiltered Position Data Estimated Values Mass (kg) 0.360 Center of Mass c 0.00 0.00 0.00 Inertia Matrix 0.532 -.010 0.021 -.010 0.568 -0.013 0.021 -0.013 0.493 Chapter 2. Inertial Parameter Identification ^ 14- 3" H 1.5" Cable Connector Figure 2.6: Flotor With Washer Shaped Piece and JR3 Sensor Attached 24 ^ ^ Chapter 2. Inertial Parameter Identification ^ 25 a (b) 1.2 -0.4 ^^ ^ ^ ^ 0 2 4^6 8 10 Time (s) Figure 2.7: Identified Parameter Estimates for Flotor Alone:(a) Inertia Matrix ioF (h) Mass and Center of Mass Estimates: Dotted lines are the values obtained by the LS algorithm, see Table 2.2. Memory limitations in Stethescope (VX Works program used to collect data) did not allow any more data to be collected in one run. Parameter estimates eventually converge to within 5% of the LS estimates. The mass estimate is seen to converge faster to a more accurate value. The mass is considerably easier to estimate, since the main linear acceleration experienced by the body is due to gravity which is constant and known to considerable accuracy. Chapter 2. Inertial Parameter Identification ^ 26 Figure 2.8: Sample Acceleration Measurements In Flotor Parameter Identification: Values shown are for offline algorithm. Direct numerical differentation leads to noisy estimates as shown. Position estimates were filtered with an eighth order elliptic filter with a cutoff frequency of 42.5 Hz. Filtering was performed in both directions using the Matlab filtfiit option to ensure the phase was not altered. The online algorithm used a similar filter, and samples were shifted by assuming the filter was approximately linear phase in the signal band. 27 Chapter 2. Inertial Parameter Identification^ 10 0 10 -1 1010 2 -3 -4 0 10 CL -0^_5 10 (Ts E ;:-; 10 10- 1010 10 7 9 -9 100 0.02^0.04^0.06^0.08^0.1 Normalized Frequency 0.12 0.14 Figure 2.9: Power Spectrum of Acceleration Estimates: Spectra for both the filtered and unfiltered estimates are shown. Transition from signal to noise is clearly visible. The noise spectrum well approximates white noise on the position estimates, and appears with slope in the acceleration spectrum. Position estimates were filtered with an eighth order elliptic filter with a cutoff frequency of 42.5 Hz before numerical double differentiation. Chapter 3 Force/Torque Estimation 3.1 Overview The objective of this chapter is to present a numerically stable algorithm capable of measuring the environment forces and torques experienced by the maglev flotor without the use of a force torque sensor. The algorithm must output only forces and torques provided by the environment, and must therefore, exclude those due to the driving coils or flotor inertia. A good inertial model, as found in the previous chapter, is necessary for accurate results. The net force experienced by the flotor is the sum of the force provided through the driving coil currents and the force experienced in contacting the environment. The force provided by the coils may be measured reasonably accurately, and it is possible to calculate the environment force by subtracting the inertial force from the known applied force. However, since only position measurements are available, the inertial force can only be obtained through a numerical double differentiation of the position measurements. Direct subtraction of the inertial forces would lead to very poor estimates of the environment force. An alternative approach to force estimation is explored in this chapter. The algorithm presented exploits the properties of two different velocity observers to obtain good estimates of the force and torque provided by the environment. It will be shown that for a second order linear observer the dynamics of the observer error are equivalent to that of a mass - spring - dashpot system driven by a force equal to the environment force. By choosing observer gains sufficiently large to simulate a stiff spring, then measuring the displacement of the simulated spring from its equilibrium (the observer error), a good estimate of a slowly varying environment force can be made. A similar approach is used to estimate the environment torque. A nonlinear observer 28 Chapter 3. Force/Torque Estimation^ 29 based on a Euler quaternionl representation of attitude is shown have error dynamics much like that of a second order linear observer and a similar technique is used to obtain estimates of the torque from the quaternion representation of the observer error. Clearly, the position and the attitude of the rigid body must be known for the observer errors to be measured directly and so it is assumed that this information is available. Accurate estimates of these state variables are provided by the Maglev wrist PSD's. Estimating environment force (or torque) from observer errors is not unlike obtaining force (torque) estimates by measuring the position (orientation) errors of a PD controlled rigid body. Assuming the body is under such a control scheme, dynamic torque estimates may be made from departures in displacements and orientation from the nominal setpoint. However, the quality of such estimates depends on the controller gains, which are usually chosen to suit a specific task. The advantages of using observer based estimation is that observer gains may be set independently of the controller gains. Control strategies other than strict PD control be can also used without any effect on observer estimate quality. See Figure 3.10 for the block diagram of the setup used in the experiments presented later in this chapter. 3.2 Linear Force Estimator 3.2.1 Notation As is the convention the observer estimate of the value x is defined as and the error between x and i, is defined as Ex - I,. A similar definition holds for all other observed variables used in this text. 3.2.2 Estimating Force from a Position Observer A simple state observer for a rigid body of mass in takes the form: _ (ao, a) represents the four parameter Euler quaternion. Refer to appendix A. a= 30 Chapter 3. Force/Torque Estimation^ Figure 3.10: Force Estimator: The error is equivalent to the true environment force, fe,,,, passed through the filter in,2+1,1v estimated environment force 3+kp . f„t,,,,,t,d is the 31 Chapter 3. Force/Torque Estimation^ kp fobs +^f — fen v = (3.22) in^in k, in with kp > 0, k, > 0 and where position and velocity are the state elements observed.2 The dynamics of the rigid body obey Newton's Law: m'b = f = fobs+ fenv ^ where fobs is the force applied by the coil currents, and fen, (3.23) is the environment force. See Figure 3.10. Subtracting the observer equations, (3.22), from the body dynamics, (3.23), results in the error dynamics: Tit fl = (3.24) f Jenv— k, = v— —x in Eliminating f, and f) in (3.24) gives: fen, =^m,""^ (3.25) The error dynamics take the form of that for a mass attached to a damped spring driven by the environment force. It is useful to speak of an imaginary body, whose position is and whose dynamics match that of (3.25). See Figure 3.11. Such intuition will add insight to the workings of the nonlinear torque estimator presented in the next section. For a constant environment force, fent, = 1 fo, — f, kp the steady state equilibrium satisfies = -±; ±". = 0 2Throughout this entire text it will always be assumed kp > 0, k, > (3.26) 0. Chapter 3. Force/Torque Estimation^ 32 Figure 3.11: Mass - Spring - Dashpot Model for Error Dynamics SO k, = — v (3.27) kv ^fo mk P It is important to note that the velocity of the imaginary body is and not i3. At equilibrium, i3 is as in (3.27) and as expected i = 0 . It is also worthwhile to note at this point that at the equilibrium fY is directly proportional to The latter property will be exploited for the purpose of correcting velocity estimates in Chapter 5. The deflection of the equilibrium reflects the stretching of the imaginary spring by that that would be expected under the environment force. Hence at low frequencies, a good estimate of the force can easily be measured as festimated = k24. Notice that no differentiation was needed to obtain this result. Since the values of kp and k, are gains for the observer and not the controller, and are chosen by the designer, it is possible to place the poles of this system arbitrarily. Provided the Chapter 3. Force/Torque Estimation ^ 33 error system resonates at a frequency much higher than that of the input signal, excellent force estimates can be made. There is a practical limit to the chosen magnitudes of kp and lc,. To be accurately measured, forces must be large enough to create a deflection significant enough to be measured above the noise level of the position measurements. A very stiff, heavily damped spring would not deflect significantly for small forces. Thus there is a tradeoff involved in choosing appropriate gains; they must be large enough to ensure reasonable behavior of the observer within the desired bandwidth, and small enough that the signal to noise ratio of the force measurement is sufficiently high. 3.3 Nonlinear Torque Estimator 3.3.1 Estimating Torque from an Attitude Observer When Newton's law for the motion of a rigid body is extended to the Newton - Euler equations (2.2) to include orientation dynamics, the equations become nonlinear. Extending linear estimation and control theory to attitude control has thus proven to be a more challenging proposition. Linear analogues can often provide some insight, and allow for the design of control strategies that may be justified using nonlinear analysis techniques (such as the method of Lyapunov). The angular velocity observer used in this section is expressed in the inertial frame as follows: 1 "Tt(j) = r d [1] + -2 kpf-lesgn (co)^ 1^I3T^ 13^2 ,130/ + x (3.28) ][QT (cz, _ kj-lesgn(e0))] where J is the moment of inertia about the center of mass, /3 is the Euler quaternion representation, and Q is the rotation matrix representation of the body orientation with respect to an inertial frame. e is the quaternion representation of the orientation of the body frame relative to the observer frame. 34 Chapter 3. Force/Torque Estimation ^ Theorem 1 Given the observer in (3.28), for any piecewise-continuous torque function 7:R+ R3, any kp > 0, k, > 0, and any initial conditions e0(0), e(0), w(0), 640), the errors IP (1) 6‘) (Oil and Ile (Oil converge to 0 as t^co and the convergence is eventually exponential. Proof: See Salcudean [25]. • Salcudean s observer was designed using the second order linear velocity observer, described in the previous section, as an analogue. As in the linear case, the observer provides a position estimate, in this case described by a unit quaternion (see appendix A for a discussion of quaternion dynamics.) Dynamics were simulated and shown to be similar to the linear case. Torque estimation can be achieved by again extending the ideas formulated in the force estimator to that of the torque estimator. Basing the torque estimate on a quaternion representation of the differences in orientation between the measured and observer orientations, a torque estimate may be made. While linear concepts, such as frequency response, do not have any meaning in the nonlinear case, we can expect a similar behavior to the linear case. With an environment torque, renv, the error equations for Salcudean's observer become: = eo 1 -- kpJ-le sgn(e0)+Tenv 2 1 -- eTJ 1 (p 1 - 2 2 - - (3.29) kvesgn(eo)) [e01- - (ex)] J-1 (p - kvesgn(e0)) where p Jul - J. The differential equations in (3.29), are stationary for: 2 e = — h-env kp e p^E kve 2kv kp 7 Tenv V1 - eT It is easily shown that for this definition of 6, sgn (ea) = 1 . (3.30) Chapter 3. Force/Torque Estimation^ It is important to note that (3.29) is not stationary for a constant environntent torque but, rather is stationary for 70 = JTemv 35 re „ „ , constant. If the body has the inertia of an uniform sphere, then (3.29) would be stationary for a constant environment torque. We expect that the behavior of the torque estimator should still resemble that of the force estimator in spite of this property. Applying a constant torque to the wrist under PID control has the wrist eventually becoming essentially stationary at which point J is essentially constant. It seems likely that the estimator is stable but not assymptotically stable when J is not a multiple of the identity matrix. The stability region would likely depend on J and co. Simulations and experimental results support this assertion. The quality of the torque estimator can be expected to deteriorate if torques are large enough to create large error angles as the nonlinearity of the system becomes more apparent. In fact, since the error term is described by a normalized quaternion, there is a limit to the deflections allowed, and hence the maximum torques that may be measured. The maximum torque can be seen directly from the equilibrium: 1 -- kpè To 2 kp (3.31) 2 since 110 < 1. Note that a practical limit on the torque should be somewhat smaller, particularly where the torque input has high bandwidth, to allow for overshoot and other tracking errors. 3.4 Parameter Uncertainty Both the force and torque estimators need accurate descriptions of the plant (rigid body inertial parameters) to perform properly. Again appealing to the linear force estimator, some insight can be gained into the effects of parameter uncertainty on the nonlinear observer. Consider the linear force estimator, (3.22), using an estimated mass in m Am : Chapter 3. Force/Torque Estimation^ = p + ^fobs^k (in + Am) (rn + ) 14 36 (3.32) (m + ,6m). Subtracting Newtons law (3.23), f (m Am)ii — Ami) = ,env— = (3.33) k, (m + and combining as in (3.22) through (3.25), f env + A171 = + k,/±. + 112. (3.34) Error in the mass does not change the general form of the error equation, however, it does add an extra term to the forcing function, Arn. There is also a change to the location of the observer poles, but, in general, the change is slight if the mass error is small. The existence of this extra term makes intuitive sense, by specifying a mass that is a fraction A of the true mass, the observer acts as if it were only following the dynamics of a body A of the size. The remaining 1 — A of the body is left unmodeled, and the inertial forces required to move this remaining 1 — A is interpreted by the observer as an environment force. The latter shows that if the dynamics of the flotor (or other body) are significant, good knowledge of the inertia is needed. Conversely, mass estimates derived by other techniques, such as those in Chapter 2, might be verified by generating accelerations on the flotor. While not investigated in this work, it may be possible to tune the inertial parameters until some performance index of the environment force is minimized. 3.5 Simulations Stability of the force estimator is easy to show, and the response of the observer to common inputs follows from linear systems theory. However, the torque estimator stability is much Chapter 3. Force/Torque Estimation ^ 37 harder to show, even when the environment torque is constant over time. Simulations are used here to verify that the torque estimator responds to inputs in a similar manner to that of the force estimator, and can be tuned accordingly. Simulations were performed in the Matlab programming environment. Efforts were made to produce results similar to those that might be expected for the Maglev wrist by choosing inertial parameters similar to that of the wrist. A PID control law was simulated with gains similar to those commonly used in the laboratory experiments. A simple inertia model was used for the simulations. The center of mass was chosen to correspond to the flotor center of mass exactly, the mass was chosen to be 0.5 kg, and the inertia matrix was chosen to be: J= 0.9 0.25 0.25 0.8 _ .2 —.15 .2^ - —.15 gm2 (3.35) 1.0_ In all of the simulations, the position observer gains are kp = 25000N/m and k, = 200(Ns)/m. Poles of the linear observer are then at s=-200+100i and s=-200-1001. Orientation observer gains are kp = 0.16Nm and k, = 0.45Nms. Since orientation dynamics are nonlinear, there are no corresponding poles. A small angle linearization for the simulated system with the inertia matrix diagonal and all elements equal to 0.9gm2 would have the same poles as for the position case. 3.5.1 Impulse and Step Response As was discussed in Section 3.2, the ability of the force estimator to track signals of high frequency content is limited. Thus, we can expect impulses to be significantly degraded, and step responses to have finite rise times, overshoot, etc. Figure 3.12a shows the observer tracking when an impulse and a step are applied to the x and y coordinates at t = 0.2s and t = 0.4s respectively. The desired locations of each component of position are unique for clarity. Figure 3.12b shows the force responses; the qualities of the responses are as expected. ^ 38 Chapter 3. Force/Torque Estimation ^ (a) 15- Observer Position Actual Position 0.1^0.2 0.3 Time (s) 0.4^0.5 06 (b) 150 Observer Force Estimate Enviroment Force 100 -50 -100 ^ 0^0.1^0.2^0.3^0.4 Time (s) 0.5^06 Figure 3.12: Impulse and Step Response for the Force Estimator: (a) Actual and Observer Position (b) Actual Environment and Estimator Force: Force values in (b) are plotted with the x component offset by -75N and the z component offset by +75N for clarity. Chapter 3. Force/Torque Estimation^ 39 The responses of the nonlinear torque estimator to torque impulses and steps are similar to that of the force estimator. Since the dynamics of rotation are almost linear for small orientation changes, it is expected that the response to small torque impulses would be resemble the linear case. Simulations have shown that similar responses are characteristic even for torques corresponding to very large angle orientation errors. An example is shown in Figure 3.13. The observer gains used in both cases correspond to the same second order model as discussed in Section 3.3. 3.5.2 Large Angle Observations The nonfinearity of the observer equations exerts its greatest influence for larger angles. As was discussed in Section 3.2. The amplitude of deflection is not important in the linear case, but, it is worth considering in the nonlinear torque estimation. Figure 3.14 gives an idea of the torque tracking capabilities of the nonlinear observer, where the initial observer error is near the maximum capabilities of the observer. 3.6 Experimental Verification With A Force Sensor As a final verification of the performance of the force/torque estimator technique, the observer results were compared to those found using a JR3 force/torque sensor mounted to the wrist Rotor. The observer algorithm gave consistent results as shown in Figures 3.15 and 3.16. Inertial parameter identification described in Chapter 2 was performed with the sensor mounted. Forces and torques measured by the JR3 sensor were transformed to the center of mass frame to be compared with the force/torque estimator results. Comparison to the JR3 sensor results was hampered by the additional forces and torques appearing by addition of the sensor umbilical cable, which, unlike the wrist umbilical cables, was heavy and stiff. The environment forces caused by this stiff cable would be observed by the force torque estimator, but not by the JR3 sensor. Errors in the force torque estimates could also be caused in part by an error in locating the center of mass. Force torque measurements from the JR3 sensor were Chapter 3. Force/Torque Estimation^ 40 Figure 3.13: Impulse and Step Response for the Torque Estimator: (a) Actual and Observer Orientation (b) Actual Environment and Estimator Torque: Torque values in (b) are plotted with the x component offset by -0.15N and the z component offset by +0.15N for clarity. Significant coupling is seen between the estimate components, particularily when the velocity is large. The coupling is due to the existance of cross terms in the inertia matrix. 41 Chapter 3. Force/Torque Estimation^ (a) 0.8 0.6 - • • • Observer Orientation — Actual Orientation 0.4 t3 E02 -0.4 - 05 0.2^0.3 Time (s) 30 20 (b) Observer Torque Estimate Enviroment Torque 10 2 -10 C. 0 F- -20 -30 -40 500 0.1^0.2^0.3^0.4^0.5 Time (s) 0.6 Figure 3.14: Torque Estimator Response With Large Initial Error: (a) Actual and Observer Orientation (b) Actual Environment and Estimator Torque. 0.7 08 42 Chapter 3. Force/Torque Estimation^ converted to this frame. Other sources of error were mentioned in Section 3.4. Experience with the force/torque sensor in the UBC lab by members of the robotics group suggests that significant unmodeled coupling exists between the Ts and Ty components. This observation was not verified in this work, as the JR3 measurements were not extremely accurate due to the previously mentioned cable effects. Despite the errors mentioned, the correspondence between the JR3 sensor readings and the force/torque estimator is quite close, and provides a verification that the observer algorithm works in practice. The comparison shows that the observer provides force/torque measurements whose accuracy compares favourably with those of the JR3 sensor. 3.7 Remarks on Force/Torque Estimator The force torque estimator algorithm works extremely well, and eliminates the need for a costly force torque sensor. The simulations verify that the behavior of the torque estimator is similar to that of the force estimator. Coupling can be seen in the impulse and step responses when the inertia matrix is not that for a uniform sphere. Performance is still satisfactory for more general inertia models. The performance of the observer is independent of the control strategy, and performs about as well as the JR3 sensor. Environment forces and torques are not restricted to those caused by contact with the sensor surface, but rather include those caused by contact anywhere on the Rotor surface. Added dynamics of the force/torque sensor are also avoided. Chapter 3. Force/Torque Estimation^ 43 Figure 3.15: Force Estimator and JR3 Comparison: Components of Sensor and Estimator Force. (a) x, (b) y, (c) z: Observer gains are k=25000 N/m and kv=200(Ns)/m. 44 Chapter 3. Force/Torque Estimation^ (a) 0.15 0. 1 0.05 -0.15 -0.2 0.25o 0.5 1.5^2 Time (s) 2.5 3 35 2.5 3 35 (b) 0.1 -0.05 0.5 1.5^2 Time (s) (c) 0.06 0.04 0.02 uta^tildt 014114, -0.02 - 0.04 - 0.°6o 0.5 1 1.5^2 Time (s) 2.5 3 35 Figure 3.16: Torque Estimator and JR3 Comparison: Components of Sensor and Estimator Torque. (a) x, (b) y, (c) z: Observer gains are kp=0.16 Nm and kv = 0.45 Nms. Chapter 4 Stability Proofs For the Force/Torque Estimators 4.1 Overview The torque estimator was presented in Chapter 3, and was assumed to act in a similar manner to that of the force estimator. No proof was given to back up this assumption. The nonlinearity of the torque estimator makes an analysis of its response to general time varying torque inputs quite formidable since the tools of linear analysis are no longer applicable. The response of the torque estimator to constant environment torques was studied, and while no direct proof of stability was found, considerable insight into the problem was obtained. The main thrust of the study was to compare techniques that could be used to prove stability of the force estimator to approaches that might prove stability of the torque estimator. Hence, a detailed Lyapunov analysis is presented for the force estimator, and used in comparison with the torque estimator. The concept of the force estimator is extended to observing forces and torques in an N-degree of freedom multi-link manipulator. Application to an observer by Nicosia and Tomei [26] is presented. The observer was proven in [26] to be stable in observing bounded velocities, and application of the force/torque estimator turns out to to be rather trivial. The method of Lyapunov is used throughout these analyses. It is seen that the use of a cross term in a quadratic Lyapunov function is a useful tool for proving stability. In the case of the force estimator it is used to prove exponential stability. While proving stability for a constant environment torque might seem impractical, such a proof would raise confidence that the observer might be stable for reasonably slowly varying torques. Simulation and experimental data provided in Chapter 3 also increase this confidence. 45 ^ 46 Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 4.2 Force Estimator Stability Stability of the force estimator follows directly from linear systems theory. It will, however, be useful for comparison to the nonlinear case to look at Lyapunov functions to prove stability. Considering the system under a constant environment force f,, an intuitive Lyapunov candidate is 1 (^.19T (- f0)) =^712X X + kp x – p1 x – (4.36) with time derivative, 1.71 = – k.T"X. (4.37) This Lyapunov function equals the total energy of the imaginary mass spring dashpot system. It is known [27] that exponential stability cannot be shown using this choice of Lyapunov function since the time derivative Vi is only negative semidefinite (the expression for 1./.1 does not include I.). A cross term can be added in the Lyapunov function (4.36) to obtain 1( V2 = - mx x amx y kpyT y) 2 (4.38) with .fo y = (x – —^ kp (4.39) It is easily shown that (4.38) is positive definite for sufficiently small values of c> 0. Indeed, completing the square in (4.38), we get v2^+ a y)^+ y) + 1 (k a2m) yTy• 711 2 2^2^2 — P^4 (4.40) Thus for 0 < a2 < 41a m , V2 is positive definite, and thus a suitable Lyapunov function candidate. The existence of the cross term results in a negative definite V2 (for sufficiently small a ) and hence proves global exponential stability of (3.24) with a constant environment force. akp^k,^(y+ k,^+ a (^7/2 + 2 V 2kp^ p^p 2 (4.41) Chapter 4. Stability Proofs For the Force/Torque Estimators^ Equation (4.41) is negative definite for 0 < a < 47 8kp kp (k,2,-F4mkp) Note that the cross term parameter a does not have any physical significance, it simply aids in the proof of stability. Without the cross term, the level sets of the Lyapunov function V1 are ellipses, with principal axes corresponding to the axes of and y. With the cross term, the level sets of v2 are rotated, such that their principal axes no longer coincide. 4.3 Torque Estimator Stability If ro = J.rent, is constant, we would expect the equilibria in (3.30) to be asymptotically stable. Stability that is eventually exponential seems likely also since Salcudean's observer has eventual exponential stability [25]. Insight gained from the attempts to prove stability will be provided here. The obvious starting point in this analysis is to extend Salcudean's stability proof from velocity observation to torque observation, in much the same manner as was done in the previous section (4.2) for the force estimator. (If f, is zero, the proofs show exponential stability of the linear velocity observer). Such an attempt was made, and will be presented in Section 4.3.2. A more intuitive analysis is obtained by representing the dynamics in a form more similar to that of the mass - spring - dashpot system (Figure 3.11). Concepts of energy for a physical system would then guide the choice of Lyaponov function. A digression to investigate the usefulness of the more intuitive form will be made. Equilibrium points in (3.30) are the equivalent to the steady state limiting values ± and V for the linear force estimator. To gain some insight into the error dynamics, we again compare to the linear observer. In the force estimator case, the error dynamics were found to correspond to an imaginary mass spring dashpot system driven by the environment force. The position of this imaginary body corresponded to the error. A similar model is sought for the torque estimator. The error dynamics are equivalent to some system with equilibrium orientations deflected from the zero torque situation. For the linear observer, errors are corrected by a damped spring Chapter 4. Stability Proofs For the Force/Torque Estimators^ 48 term imposed on the position error, and the deflection represents a stretching of the spring. For the nonlinear observer, errors in attitude are compensated by the use of a correcting torque, in the direction of the error quaternion (put forth in [25]). The dynamics are damped by the inclusion of a quaternion error term in the velocity integration of the observer. It is thus expected that the equivalent system might have some similarities to the dynamics of a imaginary rigid body with some sort of damped spring holding the body in its equilibrium position. Note that even if such a system were to exist, problems would occur when the orientation error passed through an angle of 7r. Thus a bounded torque input is necessary for stability. 4.3.1 One Degree of Freedom Model While the angular velocity observer is designed to work in three dimensions, it may be simplified to observation of the angular velocity of a body constrained to one rotational degree of freedom. The torque estimator follows as an obvious extension. Stability of the torque estimator in one degree of freedom under a constant environment torque is proved here. The torque estimator for a one degree of freedom system is: Jji) = 0= 'robs^J-1 sin () 2^2 (4.42) c2)^k0J 1 sin^. 2 - where 0 is the angle of the body with respect to the inertial frame and again w represents the angular velocity. Note that the moment of inertia, J, is a constant scalar for a single degree of freedom model. The error dynamics with a constant environment torque, 7-0, are then: = T - 1 k J 1 sin (6) 2 C) - P^2 - k„J-1 sin 2) (4.43) Chapter 4. Stability Proofs For the Force/Torque Estimators^ 49 with equilibrium, (4.44) ky,1-1 sin ( 6) 2 - 0^2sin-1^ 2 )• It will now be shown that this equilibrium is, in fact, globally asymptotically stable. Theorem 2 Given the observer in (4.42), for any piecewise continuous torque function 70 pplied R+ -4 R and constant environment torque 0 < 1rot <^, any kp > 0, k„ > 0, and any I > (2AT7r + 0) for some integer N as t ---+ oo . initial conditions O (0), w (0), W(0), - Proof Assume w.l.o.g. that 0> 0 and consider the Lyapunov function: j L2 V = -2 - kpJ-lcos (-2) -^(-kpJ-lcos^- roc) ,^(4.45) 2 where 0 is defined such that —71" <q < it and U shown that in each semi-infinite strip for any 70 : 0 < trot < 'OE [(N - 2N7r = 0 for some integer N. It is easily - 1)7r, (N)7r), V is positive definite and convex 1V-1• The Lyapunov function (4.45) has the same shape on each semi-infinite strip (see Figures 4.17 and 4.18). Showing that V is positive definite requires only that: f (c-6) = -cos (fl -^cos A +^((k) A \ 2^2 - (4.46) be positive definite for any -1 < A < 1. Note: (4.47) f^= 21 s n (ff) - which is zero for: = 2sin-1 (A), (4.48) Or sin^= A = sin ( ) . 2 2 - (4.49) Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 50 3.5 -0.5 -6^-4^-2^0^2^4^6 x (rad) Figure 4.17: Example Lyapunov Functions for One Dimensional Case: Three cases are shown, Va, Vb and Vc correspond to sin(!)=1.0, sin(!)=0.5, and sin(!) =0 respectively Also f^= 0. V can be shown to be convex for -7r < < 7r since: (4.50) 1" (0) = 1-4 cos^, which is greater than zero provided -71- < < 7r. See Figure 4.17 for examples of V (0, V (e, 0) for varying values of O. The time derivative of V is: J+^(t) - (4.51) ..1c6 (.;*3 -^(--(P i))^ia2) J-1;bsin (-A) ^ 2COS (t) + :C37-0 — —; kv :.2 c°8^• (i:42) k; ^ To(k Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 51 1.5 ^ 0.5 (VI 4G, -0.5 -1.5 -2^0^2 theta (rad) Figure 4.18: Contour Plot of Example Lyapunov Function:sin().o.s, Noting that cos CO > 0 for —7r < q < 7r, and V is seen to be negative semidefinite in that region. Asymptotic stability is the guaranteed in the interior of those level sets of V which do not intersect the lines 0 = —7r. The largest such set, S2, is bounded by: ^— 2 0 — kpcos (2) — To :CA^ (4.52) The sets bounded by (4.52) define stability domains for each of the respective equilibria. Determining behavior of trajectories outside of the stability domains requires some further analysis. Since the Lyapunov function is convex and positive definite on a semi-infinite strip, trajectories must either enter the stability domain or reach the boundary 0 = —7r. Either the trajectory must continually cross that boundary indefinitely, or the trajectory must eventually enter a stability region. The former is shown to be impossible by contradiction. Assume that there exists a trajectory {ö(t), 0 (t)] that passes through the line 0 = 7r an infinite number of times. ^ 52 Chapter 4. Stability Proofs For the Force/Torque Estimators ^ Assume that the values of t for which ä (t) = r are denoted in increasing order as t1 < t2 < t3 < ..., then clearly' V (t(N+1)_) < V (tN). Define t(N-1-1)- 1.7dt.^ (4.53) AVN V (tN) — V (t(N+1)._) = —^ J(N We will show AVN > a > 0 for some constant a for all N. Consider the semi-infinite strip S=0,i4:0—E<T) <0A-E,0<e<7C— 01.^(4.54) Then there exists times t'N, tk with tN < tk < tik < t(N+,)_, for which ã (t) = — e, and (tk) = -1-e, see Figure 4.19. Since V is negative semidefinite ^AVN> — t"N • Vdt^ (4.55) but it is easily shown that on S \ SI, V < < 0 for some constant 0. Therefore AVN > — N Odt^WIC - 67)^ (4.56) tN which is finite for any N. Therefore V (tN) cannot converge to a finite value, which is a contradiction. • The motivation for the choice of Lyapunov function used in Theorem 2 was a physical system whose error dynamics match those of the observer. The model, illustrated in Figure 4.20, consists of a rigid link free to rotate about a frictionless bearing. A rigid rod is fixed in the position corresponding to the desired orientation. A spring and dashpot are connected between the rigid and free rod as described in the figure caption. The latter system would be impossible to build, but provides considerable insight as to the structure of the error dynamics. For example, it may be seen that for small fi, the model becomes much like the one dimensional mass - spring - dashpot system. It might be expected that a region exists in which the convergence is exponential. It is also easy to see that an environment torque strong enough to extend the spring fully would drive the free rod to rotate in perpetuity. There could be no equilibrium for such an environment torque. 1I use V (t) Eii (e(t),j(t)) interchangably for clarity. Choice of definition is clear in context. Chapter 4. Stability Proofs For the Force/Torque Estimators^ 53 (N+1). Figure 4.19: Phase Plane Representation of Sample Trajectory: There is a finite decrease in V as the trajectory crosses S \ C2 between times tiN and Pk. It is easy to imagine that a similar physical model exists in three dimensions, with the free rod able to rotate in any direction, and bounded to a hemisphere, however, as will be seen, this is not the case. The dynamics are close, however, and additional velocity terms exist, and impedes the natural development of a similar proof. 4.3.2 Stability in Three Dimensions Again it is necessary to construct a state representation of the imaginary body. Consider a frame whose orientation corresponds to the error quaternion (e0, e). The angular velocity of this frame, St, can be derived from (3.29): 1 = — [e0.1 2 — (ex)1S2^ (4.57) Or SI = 2 [eal - — (e x)]1e^ (4.58) Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 54 Figure 4.20: Physical Model for One Dimensional Torque Estimator: The equivalent of the mass - spring - dashpot for the orientation case has a rigid link free to rotate about one axis. A spring attached between the free and fixed rod has both ends rigidly attached to frictionless bearings. A dashpot is connected in a similar manner, but has end attached to a roller bearing free to slide along the fixed rod. The angle between the free and fixed rod corresponds to g-. The dynamics of the model correspond to the observer error dynamics only for The system would only continue to correspond if the free rod were to instantly jump from one side to the other, maintaining the same angular velocity, when passing through I öl=r. 55 Chapter 4. Stability Proofs For the Force/Torque Estimators ^ = [eof - (ex)]-1^[e„.1 - (ex)] J-1- (,u - k esgn (e„))) = J-1 Cu - kvesgn(e„)) Note that [e01 - (ex)] has a determinant of ea, so these dynamics are only valid for e, not equal to O. 2 Note also that D is the angular velocity of the error frame and is not equal to co - C). The difference between these representations of error velocity is analogous to the difference between and in the linear case. Then the error dynamics may be expressed as: - kvesgn(e,)) (4.59) — (JD) (it - kvesgn (eo)) dt 1 d = 7env — (JD)^ esgn (e0) - kesgn (e„) dt —^ Equation (4.59) is somewhat less desirable than it first appears. It seems that the dynamics take on the form of that for a rigid body with stiffness and damping in the rotation. It is important to note, however, that the moment of inertia varies with orientation of the real body, and not of that of the imaginary body. Thus the error dynamics do not take on the form of rigid body dynamics. If the orientation of the real body is similar to that of the imaginary body (i.e., the orientation error is small), the dynamics would be similar to that of a rigid body. In the special case where the rigid body is a uniform sphere, the moment of inertia is constant over all orientations, and cflt- (JD) = JD would reflect the moment of inertia of the imaginary body. Again, bodies whose moments of inertia are almost constant over all orientations (the Rotor of the maglev wrist for example) might also be expected to act in a similar manner. The notion of an imaginary body provides some intuition, and motivates the search for a stability proof through the method of Lyapunov. Several attempts were made to find a suitable Lyapunov function, as follows. For simplicity, it was first assumed that the moment of inertia was the identity matrix (i.e., the rigid body is a unit uniform sphere). The latter simplification 2Throughout the remainder of this chapter we will ignore problems encountered when co = 0, in the hope that the issue may be resolved after a suitable Lyapunov function is found. Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 56 eliminates problems with the time-varying inertia matrix. The error equations for a unit uniform sphere are thus, 1 . 7-0 — — k e sgn (eo) 2P 1 é, . -- eT (II — love sgn (eo)) 2 1 e =^[eal- — (ex)] Cu — kve sgn (e0)) A (4.60) or equivalently, 1 ._-_- To — — k esgn (e0) — kvesgn (e0) 2P 1 eo = -- eTC2 2 1 é = -2-kJ — (ex)] Q. (4.61) With no environment torque (To = 0), the latter is a special case of the Salcudean observer [25]. As was mentioned in the overview, there are numerous examples in the literature of quaternion based control laws, whose dynamics are very similar to those shown in (4.61). Wen and Kreutz [8] showed stability of the law: --d-i 5 1 ) = 11- pe — k v (w — w d) (4.62) where in this case e represents the orientation error between desired and actual trajectories, and co and cod are the true and desired angular velocities. Global asymptotic stability for this control was shown along with other variants in [6], [7], [9] and [10]. All of these control laws, however, involve the angular velocity rather than the quaternion derivative é. Lyapunov Candidate Based on Force Estimator The energy of the imaginary system was used as a base for the Lyapunov candidate for the force estimator in (4.36). Likewise a similar energy function is sought for the nonlinear case. Exponential stability of the nonlinear observer was proved in [25], but will be proved again • Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 57 here in a somewhat different manner. The similarities between the proof presented here and the proof given in the force estimator case might provide some help in choosing an appropriate Lyapunov function. Consider:3 = 1^ 2 kp T e SZTS1+ –1)-(1 – eogn (e0))^ 2 2 (4.63) whose time derivative, at points where V1 is differentiable4, is (using (4.60)): kpesgn (e0) – kvesgn (ea)) – kpèosyn (eo) (4.64) 1 – – k Wenn (eo) – kresgn (eo) – kpèosgn (eo) • 2 P kv T kpéosyn (eo) – — SI Sleogn (e0) – kpêosgn (e0) 2 kv --TC2TDeasyn (e0) • Compare (4.64) to (4.37).5 The presence of a constant environment force presents no difficulties in the linear case, but, as will be seen, presents quite a bit of difficulty in the nonlinear case. First exponential stability of (4.61) will be shown. Consider for ea > 0, a > 0: p 1^k^kp 9 T V2^--C2TC/ + (1 - e0)- + Te e + aeTS2 T (4.65) 11 =^(C/TS2 + 2aeTS2T + a2eTe) + (kp – a2) eTe kp — (1 – ea)2 2 11 = -- (12 + ae)T (S-2 + ae) + (2kp – a2) eTe + + kp (1 — e0)2 3Salcudean [25] used V =^_ eosgn (e0))2^eTe to show asymptotic stability of the angular velocity observer. { 1 x>0 '^ By defining sgn (x) =^— VI can be shown to be everywhere differentiable. This definition of —1 x < 0 sgn (x) was used in the observer equations, and the technicalities associated with the behavior when eo = 0 are ignored here in the spirit of first determining the general behavior. 5Salcudean's choice of Lyapunov function has a time derivative —11cpko J—leTe. It is interesting to note that the latter derivative is in terms of quadratic position, while the other choice of angular velocity in the Lyapunov function gives a result in terms of angular velocity. Quaternion error is bounded, angular velocity is not. It seems using 1t rather than p gives a stronger result. ^ 58 Chapter 4. Stability Proofs For the Force/Torque Estimators ^ and for e, <0, a > 0: k 1^k^2^ - aeTS2 V3 =^+ -1)- (1 + eo) 2^2 (S2 - ae)T (S2 - ae)^(kp - a2) eTe (4.66) + -1(1+ e0)2 2 both of which are positive definite for a2 < kp. The time derivative of (4.65) is (using (4.64) and (4.61)) k, a (s-f-re + e-r,-1) _TeocITs-1+ (4.67) fl2=^ k,^1 k = -- eoS2TS2 + a ( - eoS2T52 + eT Pe - ki,e)) 2 2 2 1^ k = -- (k, - a) eoSITSI + a (--LeTe - kveTe) 2 2 k„ 1^k = -- (k, - a)e0f2TC2 - -P-aeTe - — aeoeTC) 2^2 2 = _IL7p_a (e're + 1±140eTfi + k ., e2012T9) + (kea (kv - ct)e ) fiTs-1 2^k P^4k P^ 2 8kp^2^° = Ja, a(e + k, e09 e + k„ eof2 2^2k P^2k P T 1 (a (ke, 4k + 1) _ kv) eacITQ +^ 2^p which is negative definite for 0 < a < =^ 4kplc, l eokv2+41cp . Similarly the time derivative of (4.66) is: kp (e + k, 0.2) (e kv eoci) 2^2k p 1 (4.68) kv) eo1ir9 (a (qe0 p 2^4k^ which is negative definite for 0 < a < 4kpkv e0kv2-4kp I . Again, note similarities to (4.41). Hence we have shown a special case of the already known result through an approach more similar to that used for the force estimator. Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 59 Including a constant environment force to the force estimator required only a slight intuitive modification to the Lyapunov function. A similar modification to the torque estimator Lyapunov function yields less desirable results. Still some promising similarities can be seen. Consider modifying (4.63) appropriately: V4 = L'-frft –E k (e, – easgn (e0))2 --Pk (ê – e)T(e – e) 2^2^2 (4.69) with time derivative (using (3.30)), =eIl T (7-0 – – kP-esgn (eo) – kvesgn (eo)) 2 kpeoê, – kpe-060sgn (e0) r To or, - kpeTe – (4.70) kp eT6 - kp (– =esgn (ea) – kvesgn (e0)) 2 kpeoèosgn (e0) – kpëT k, e0)-SITe- hpeoéosgn (ea) – — eoeogn (e0)12TS2 2^ 2 - kpeoêosgn (e0) – kpêT 1 0-72SZT e – kpeT – – 60e osg n (e0) kv12T12 2 2 kp (e052 – kpe)T é — eoeogn Notice the similarities again in (4.70) and (4.37). The (e0)1c,S2T52 "iTfo term cancels in (4.37), however, the SFr() does not cancel in (4.70). The first two terms in the last line of (4.70) nearly cancel as 1/ is nearly twice é when C2 is nearly parallel to e. The latter result suggests that a more suitable choice of Lyapunov function might provide a proof. Lyapunov Candidate Based on Rotation of Base Frame When analyzing the nature of equilibria for nonlinear differential equations, it is common to shift individual equilibrium points to the origin through a linear transformation. A similar effect may be had on a differential equation based on quaternions. Equilibria shifted from the body orientation, as in (3.30), may be rotated to the body orientation, via transformation in • Chapter 4. Stability Proofs For the Force/Torque Estimators^ 60 the quaternion representation. That is, define' =^0 t,^ (4.71) or, eo = eoe, réT e^ (4.72) = eoe — [eol- — (ex)) é. Note that for e = e = (1, 0). A suitable Lyapunov function might then be 1 1 1 — kp (1 — eosgrt (e0))2 e'4-• 2^2^2 V5 = — Ealrit — (4.73) The latter Lyapunov function is somewhat unusual in that the second term contains a sgn (e0) factor rather than a sgn (a). This makes for less complication in the analysis, and still results in a positive definite Lyapunov function. Note that the second term is zero if and only if Gsgn (eo) = 1. From (4.72) it can be seen that eo = 1 implies eo = é, and likewise eo = —1 im- plies eo = --eo. Thus the second term is zero only for e = (1, 0) or its equivalent e = (-1, 0). The time derivative of V5 is .1./5^er -oSIT^— — k21) esgn (eo) — kvesgn (e0)) — k2,4,sgn (co) • 1 eoirro — --eok Cresgn (eo) — èok,,S2Tesgn (co) — kp (e0e0 ^e) sgn (co) 2 P • eoftTro — eokpêosgn (eo) — eoeokoS2T llsgn (cc,) — kp (e0e, éT e) sgn (co) • --eoeok,S27 itsgn (e0) eostTro — kpéT ésgn (cc,). Again there are problems having the velocity terms cancel. 6See Appendix A for an explanation of the concepts and notation used. (4.74) Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 61 Lyapunov Candidate Based on Salcudean's Observer Salcudean [25] proved the eventual exponential stability of his observer, (3.28), using the alternate representation of dynamics presented in (3.29). The proof was obtained through the use of two Lyapunov functions; the first equation is similar to the energy function described in (3), except a jiTli replaces the kinetic energy term PTS2• The first Lyapunov function gives a , negative semidefinite time derivative and shows that eventually sgn (eo) = 1 (or -1 w.l.o.g.) for all remaining time. The second Lyapunov function utilizes a cross term and shows the eventual exponential stability. While more intuition can be gained by choosing dynamics expressed in terms of 12 rather than it, a comprehensive study requires consideration of the dynamics in this form. First consider the Lyapunov function used in [25] (numbered (17) in the cited paper) with appropriate modifications, V6 =^+ k12 (e0 2^2 cog n (ec,))2^(e —^(e — e ) (4.75) 1./6^kp (e, — eogn (e,)) 6,sgn (eo) kp (eT — éTe) (4.76) — - — with time derivative: • eopTit kp (eoêosgn (e0) — • eatTro • je) 1 eo (-- k pT esp./ (e0)) — kpeoosgn (e0) — kpeT 2 ' eopT + eokp (éosgn (e0) — eT — kpeoêosgn (e0) — kpeTe 1 eatT ro — kpeT — — eok kveTe. 2P Noting the equilibria 3.30, we get 1./6 = eo fiTro eTro —1 eolc kveTe. P 2 Similar problems are again seen with using ,u rather than 11. (4.77) Chapter 4. Stability Proofs For the Force/Torque Estimators ^ 62 4.4 Remarks on the Stability Proofs In spite of a fair deal of effort, a stability proof for the torque estimator remains elusive. It seems likely that a proof would exist for the case where the body is a uniform sphere. It is, however, not clear if such a stability proof might still exist for more general rigid bodies, though a local stability result would still seem likely. It seems from the work done on the one dimensional torque estimator that the potential energy of the three dimensional system involves a term in the error angle rather than the error quaternion. The Lyapunov functions chosen did not seem to be a natural extension of the one proven int the one dimensional case. The major barrier to obtaining a Lyapunov function comes from the inability to find a way of cancelling the frro term in the time derivative. Since there exists no simple function whose derivative is the angular velocity, one might have to resort to generating a Lyapunov function based on some integration of the angular velocity over time. It is not clear to me how a suitable function could be constructed as such. Finding a stability result for the three dimensional case remains an interesting problem. 4.5 Extension To Multi Link Bodies: The Nicosia and Tomei Observer - The observer used here is a simplified version of the Nicosia observer, and is designed to handle a general robot model D (q)4 + c (4,4)4 + G (4) = u (4.78) described in generalized coordinates q. (The Nicosia observer is designed to handle joint friction and link elasticity; an ideal robot will be considered here to avoid unnecessary complication in comparison to the rigid body case. The non-ideal effects are easily included.) As with the case of the rigid body, it is assumed that the position q is available for measurement. It can be shown that for Newtonian systems [26]; IC (MAI^kc 11411 ,^(4.79) ^ 63 Chapter 4. Stability Proofs For the Force/Torque Estimators^ for some k, > 0. With^Xi q —^and Kp a positive definite constant matrix, the simplified version of the Nicosia observer is (4.80) =^k•t4-1^ = D-1 (q) (—C (q, 'Xi) 1 — G (g) K^, with error dynamics D(q)1 C (q,^+ C (q,^ i = —K1 — kD(q) Theorem 3 Assume that = [0,0] of the state--L= i. ^ (4.81) 14^< M for any t > 0. If k„ > 14, , then the equilibrium point [ i , i J defined for (4.81) is asymptotically stable, and a region of attraction is given by S = fx€R2N : 114 < with O(II)) (A(D)kp M)} , A(H))^k^ [Kp^0 1 H =^ 0 D (g)] and where A (*) and (.) (4.82) (4.83) are the minimum and maximum singular values of the given matrix respectively. Proof: [26] • With environment forces and torques, the error dynamics become D (g)."^C (g,^+ C (g,^i =^—^(q) "X"^u,^(4.84) with equilibria again acting like a stretched spring, xi = Kp-luo^ and uo being the enviroment input. (4.85) 64 Chapter 4. Stability Proofs For the Force/Torque Estimators ^ Theorem 4 Under the assumptions of the Nicosia observer, the equilibrium is asymptotically stable with a region of attraction given by S = {x€R2N : — -±11 <^ 4(H)) A(D) k„ A (H))^k, (4.86) M)} with (4.87) [K7,1 uo, 0] .^ Proof: Nicosia's result is easily extended through obvious modifications made to the Lyapunov function given in [26]. Choose a Lyapunov function, v= -2 -i p uol)^ (x^Kp—inol) T H (q) (x [K.^ (4.88) with time derivative =^-2-iTi^(q)"]. — Tj ^ (4.89) C (q,^— uo) — =^(13 (q)^C (q, The result then follows directly from Nicosia's result [26]. Using (4.79) and the velocity bound it can be seen that7 :.T c 2 q, x X1 k, which implies 1 2[ kvA(D) - k, I +^, (4.90) + Al)] (4.91) which is negative semidefinite on S if 4 is bounded as specified. • 7See Appendix C. Chapter 5 Velocity Observation The preceding chapters have concentrated primarily on using velocity observers to obtain force/torque estimates. It is clear that the existence of an environment forcel (real or perceived) as a result of model error, causes errors in both position and velocity estimates. The force estimator may be used to correct the velocity observer so that it might be used for control purposes. Without some correction, velocity observation is almost useless in the presence of significant environment force and/or torque. An illustrative example of such behavior is shown in Figures 5.22 and 5.24. Two ways of correcting the velocity observer results are presented in this chapter. The simplest correction comes by using the force estimate to approximate the velocity error which can then be directly subtracted out (see Figure 5.21). A more sophisticated approach involves using two observers; the first observer is used to calculate the force estimate which is added to the input of the second observer. The latter output more accurately reflects the velocity estimate, even in the presence of a significant environment force (see Figure 5.23). 5.1 Subtracting the Error Estimate Equations (3.26) and (3.30) describe the stationary behavior of the linear and angular velocity observers respectively under a constant force or torque. After an initial transient has subsided, the error in the position estimate is simply a multiple of the error in the velocity estimate. Since the position error,, can be measured directly, velocity errors can be accurately computed. For 1For brevity, in this chapter the words force and velocity observer will be used interchangeably with torque and angular velocity and related symbols except in cases which will be clear in the context. 65 Chapter 5. Velocity Observation^ Figure 5.21: Correcting a Velocity Observer With Output Term The velocity output is corrected by subtracting a multiple of the error .i. 66 67 Chapter 5. Velocity Observation^ the linear case, the velocity estimate is then Vcorrected kv z = V + 771x. (5.92) Assuming the angular velocity observer has dynamic behavior similar to that of the linear observer, the equivalent corrected angular velocity is then Wcorrected = CZ' + (5.93) The term corrected velocity is actually a bit misleading. The corrected velocity in (5.92) is seen to be simply The corrected velocity in (5.92) is (2 (the angular velocity of the observer frame described by i.j). The simplicity of the correction is devalued by the fact that the fact that being an estimate of velocity, is prone to higher levels of noise. Only one integration of the applied force is needed to obtain whereas obtaining '1' requires integrating twice. From the perspective of correcting a velocity estimate, the noise can be thought of as being introduced from the output of the force estimator. The higher noise level can be seen as being introduced force estimator. It has been shown in Section 3.2 that the force estimator has a limited frequency response and that improving the frequency response comes at the expense of adding noise to the force estimate. The primary reason for using a velocity observer is to reduce noise. By using a corrected observer the designer has some control of the amount of noise reduced through choice of observer gains. Some loss of bandwidth may not even be noticeable. Figure 5.22 illustrates how noise was reduced substantially without significantly degrading the velocity estimate. (Only x components of each time varying vector are plotted for clarity.) The wrist was flown under a basic PID control, with no sensor attached. At time t = 0.4s, torque was applied to the wrist by shaking the wrist flotor by hand. Contact was lost with the wrist at t = 1.7s. Figure 5.22a shows the environment torque observed. Serious discrepancies are observed between the velocity obtained through numerical differentiation and the velocity measured using an uncorrected observer as seen in Figure 5.22b. Comparison of the corrected velocity observer output (Figure 5.22c) to that of a velocity obtained through numerical differentiation demonstrates that the corrected velocity observer tracks the true velocity. It should Chapter 5. Velocity Observation ^ be noted that 68 offsets in the observed velocity clearly visible from t = 0.0s to t = 0.43 and t > 2.0s, are removed from the corrected observer. As expected, correcting the velocity observer adds noise to the velocity estimate. However, the noise in the velocity from the corrected observer is less than in the velocity obtained from differentiation. 5.2 Correcting a Velocity Observer with A Force Observer A second approach to correcting a velocity observer is to apply a correction to the input and use the filtering action of the observer to suppress noise. If the input force were known a priori, there would be no need to correct the observer at all. Only the applied force is directly measured, however, a separate force estimator may be used to estimate the environment force. Providing the sum of the applied and estimated forces to the velocity observer input, the low frequency components of the velocity error can be removed without increasing the noise content of the observer estimate. As can be seen in Figure 5.24, this approach can be quite successful. Although there is very little noise content in the estimated velocity, it is apparent that some band limited error does exist. 5.3 Remarks On Observer Correction As it will be shown in the following chapter, a corrected observer produces better control than numerical differentiation. Control using an uncorrected observer is clearly not a good idea when the system model is poor, or environment contact is expected. An uncorrected velocity observer was found to be inadequate for the control of the Maglev system. Even with no environment contact, the Maglev wrist system had sufficient model error to provide detrimental effects on the velocity observer. The sources of these errors were attributed to nonlinearity and offsets in the force/torque relation, and errors in the inertial parameter estimates. ^ 69 Chapter 5. Velocity Observation^ (a) Time (s) (b) 0.5 ^ Time (s) 1.5 ^ ^ 2 25 (c) Corrected Observer 0.5^1^1.5 Time (s) ^ ^ 2 25 Figure 5.22: Output Corrected Velocity Observer Experiment: (a) Observed Torque in Input Corrected Velocity Observer Experiment. (b) Velocity Observed with Uncorrected Observer vs that Obtained by Numerical Differentiation (c) Velocity Observed With Corrected Observer and Difference From Numerical Differentiation: The true angular velocity is found by numerically differentiating the quaternion parameter and calculating ta=[/30—(f3x)]/j. The observer estimated velocity is seen to deviate from the true velocity when torque is applied. The corrected observer closely follows the true velocity in spite of the existence of environment torques and with a significant reduction in the noise level. The difference between the corrected observer velocity and the numerically differentiated velocity is plotted in the bottom of the third figure (offset by 3 rad/sec) and consists mostly of noise. Observer gains are kp =0.16 Nm and =0.45 Nms. Chapter 5. Velocity Observation^ Figure 5.23: Correcting a Velocity Observer With a Second Observer The velocity observer input is corrected to include the force estimate. 70 Chapter 5. Velocity Observation^ 71 (a) 0.2^0.4^0.6^0.8^1^1.2^1.4^1.6^1.8 Time (s) (b) 0.2^0.4^0.6 Time (s) 0.08 ^ 0.8 (c ) 0.06 0.04 0.02 .8 -0.02 -0.04 -0.06 -0.08 0 0.2^0.4^0.6 Time (s) ^ 0.8 Figure 5.24: Input Corrected Velocity Observer Experiment: (a) Observed Force (b) Velocity Observed With Uncorrected Observer (c) Velocity Observed with Corrected Observer vs that Obtained by Numerical Differentiation: The uncorrected observer velocity is again seen to be dramatically affected by the environment force. Close correspondence of corrected observer velocity and true velocity is seen. The filtering action of the second observer leads to less noisy results. Observer gains are k9.25000 N/m and k,=200Ns/m. Chapter 6 Control About a Remote Center of Compliance 6.1 Overview The preceding chapters have been concerned primarily with the identification and observation of the system model and state. The following chapters demonstrate control strategies that depend on the knowledge gained in the first chapters. Prior to this work PID control about the flotor center of mass (approximately the Rotor origin) has been the main control strategy used on the maglev wrist. Hollis et. al. [5] describe a remote center of compliance control strategy for the maglev wrist through a form of computed torque [10]. Prior to this work, the algorithm had not been put into practice as an accurate inertial model was needed to compute feedforward to compensate for gravity torque and to linearize the dynamics. The Newton - Euler equations derived in Chapter 2 can be expressed as fapplied rR =^g^X 0) X rR-F rR x^x Jw)- rR x J-17-1 (6.94) -.I-1 (LI x Jo)) + J-17 with rR as the postion of the remote center relative to the flotor frame. These dynamics can be simplified to = ^vir'R KpirR ^ (6.95) ,*(j^Kv2,^KP2ii by using the following control law [5] fapplied = 171,(K^KPirR)- mg ^ 72 (6.96) Chapter 6. Control About a Remote Center of Compliance^ 73 — muc il {w x x rR TR x J I (c4.) x Jw) — TR x J 1.7-1 rapp/ied ^ x Jw + 2J (00I — x ^00T) ((Kv2ij Kp20)+ 711(.0'4) . Both the inertia about the center of mass frame J and its inverse J - 1 are needed to execute this control law. The inertial parameters estimated in chapter 2 can be used for this purpose. Hollis et. al. [5], describe the many benefits of control about a remote center. Gains of the linearized dynamics, Kp, and Kv, can be chosen to allow the Maglev wrist to simulate many different types of end effector tools. Individual degrees of freedom are easily decoupled, and gains can be chosen to make some axes particularily stiff, and others more compliant. Such a design allows the wrist to simulate a plunger, rotator, slider, etc. 6.2 Experimental Results The remote center of compliance (RCC) controller (6.95) was implemented. Demonstration was performed for many members of the Robotics Group at UBC confirmed the natural feel of the remote center of compliance. Output corrected observers (5.92) and (5.93) were used for the derivative and feedforward terms resulting in a smoother feel in the wrist compliance. The smoother velocity estimate allowed for higher derivative gains in the PID controller, and thus higher stiffnesses were achieved without loss of stability. While there was no extensive testing as to the maximum gains possible, a factor of two increase was made from the highest gains that had been achieved without the use of a velocity observer. In fact, the strong gains frequently allowed control inputs to reach saturation and to blow fuses in the current amplifier. Maximum stiffness achieved was 8000 N/m. In the following tests, designed to demonstrate the performance of the RCC controller, three points were chosen on the wrist for the purposes of plotting: the remote center point, the flotor origin, and the reflection of the remote center through the flotor origin. First the remote center was chosen to be [5,0,0]T cml and the gains were chosen to create stiff position control and compliant orientation control. The wrist was shaken lightly so as to 1Vectors quoted are given in the flotor coordinate frame. Chapter 6. Control About a Remote Center of Compliance ^ 74 Figure 6.25: Monitored Points On Flotor: Three points attached to the flotor frame were chosen for monitoring the motion of the fiotor about its remote center. Figure 6.26: Motion of Flotor Attached Points (y axis): High position gain stiffness ensures remote center at [5,0,0IT cm remains essentially fixed (bottom trace). The image point (top trace) is twice the distance from the remote center than the flotor origin (middle trace). Contact of the flotor with the stator is seen at t=1.6s. Chapter 6. Control About a Remote Center of Compliance^ 75 generate rotation about the remote center. With the light shaking, it was not expected that the wrist point corresponding to the remote center would move significantly. Trajectories plotted in Figure 6.26 show the x component of position of the three points. It is clear that the remote center does not move, while the motion of the reflection point in the component axis is about twice that of the flotor origin. The results support the assertion that the body moves about the remote center. The second test involved creating a rotator about a remote center, this time at [0,0, lf cm. Again the position gains were chosen very large to hold the remote center in place. Orientation gains were chosen such that the x and z axes were 40 times stiffer than the y axis. Again gentle shaking was applied, and the motion was constrained to the x, z plane, with the remote center remaining essentially fixed (Figure 6.27). A final demonstration again involves a rotator this time with the remote center at [0, 0, 5]T cm. Figures 6.28 and 6.29 display the dramatic difference in stiffness achieved by first applying a torque about the compliant y axis and then applying a torque about the stiff x axis. Comparing the environment torque to the quaternion parameter, it is easily verified that control is 40 times as stiff about the x axis as it is about the y axis. Unfortunately these demonstrations are no substitute for a hands-on feel of the truly superb compliant sensation achieved on the UBC Maglev wrist. Chapter 6. Control About a Remote Center of Compliance^ 76 Figure 6.27: Three Dimensional Plot of Flotor Trajectories for Rotator: Motion is constrained to the x, y plane. The remote center (top trajectory) remains essentially fixed, while points 1 and 2 cm below, freely rotate. Chapter 6. Control About a Remote Center of Compliance^ 77 0.02 g 0.015 0.01 8 0.005 0.5^1^1.5^2^2.5 Time (s) 35 Figure 6.28: y Axis Rotator: Stiffness in x Axis: (a) z component of quaternion representation of attitude. (b) x component of observer-based estimated torque: Attempts are made here to rotate the flotor against the natural rotation of the rotator, resulting in high torque levels. Chapter 6. Control About a Remote Center of Compliance^ 78 1 -3 0.5 ^^ 1 1.5^2 Time (s) 2.5 3 35 x 10-3 0 ^ 0.5 ^^ 1 1.5^2 Time (s) ^ 2.5 ^ ^ 35 3 Figure 6.29: y Axis Rotator: Stiffness in y Axis: (a) y component of quaternion representation of attitude. (b) y component of observer-based estimated torque: Placing a small mass on the flotor surface was sufficient to create a significant change in orientation angle. High noise levels on the estimated torque are due to the small magnitude of the measured environment torque. Comparing the torque to quaternion parameter relation with that in Figure 6.28 it is easily seen that the 40 times difference in stiffness is achieved. Chapter 7 Conclusions The objective of this project has been to improve performance in the control of a rigid body, and in particular, control of the UBC Maglev wrist. A remote center of compliance controller was sucessfully implemented and demontrated the effectiveness of all the algorithms developed and presented. The inertial parameter identification algorithm provides estimates accurate to within 15%. Better calibration of the force current relation and more sophisticated modelling of this relation would likely provide higher accuracy. Such a model would include the effect of orientation and position of the flotor coils in the magnetic field. The concept of extracting environment force and torque from an observer was introduced. Unlike observers found in the literature, the force estimates are based on knowledge of position and not of velocity. Some loss of bandwidth is evident in position error based force estimation. Stability of the torque estimator when a constant environment torque was applied was only found for the one dimensional case. Evidence presented suggests that the three dimensional observer would be stable to a constant environment torque when the inertia was that of an uniform sphere. Stationary points did not exist if the inertia matrix differed from that of a uniform sphere, but simulations and experimental evidence suggest this problem is not too severe unless the rigid body velocity is large. A stability result was found for the Nicosia observer for bounded velocities. A major difference between the Nicosia observer and the Salcudean observer is that the former include estimates of the coriolis terms based on the observer velocity. It seems a modification of the Salcudean observer to improve torque estimation might include similar terms. 79 Chapter 7. Conclusions^ 80 Velocity observer correction based on observer-based environment force estimates proved to be effective in producing low noise velocity estimates. To the best of my knowledge this is the first such work that has been presented on this topic. A remote center of compliance controller was implemented, and used to demonstrate the effectiveness of the techniques mentioned previously. Stability of the controller with a remote center was maintained provided the center was not moved more than about 10cm. Improved velocity estimates from the corrected observers, allowed for a increase in controller gains without loss of stability. 7.1 Further Work It was hoped that the inertial parameter identification algorithm could be used to determine information about environment contact impedence. The rate of change of the parameters in relation to the forgetting factor could provide some idea of the orientation and stiffness of a contact plane. No real progress was made in this area, and it remains an open topic for further research. Stability of the observer based torque estimator was not proved in three dimensions, but evidence was given to suggest that such a proof might exist. Problems encountered when the inertia matrix was other than the identity might be resolved by a different choice of observer. Salcudean's observer is the only globally convergent angular velocity observer available in the literature. However, observers based on other attitude representations might still work well. Corrections to the velocity observer could be applied to any observer design, and could be useful in correcting not only the effect of unmodeled inputs (environment forces) but also unmodeled plant dynamics. A remote center of compliance controller using the inertial parameter estimation developed here is at the time of writing being used by Ming Zhu for thesis work pertaining to teleoperation of excavators. The use of a force observer instead of a JR3 sensor has yet to be used in teleoperation. Bibliography [1] N. It. Parker. Application of force feedback to heavy duty hydraulic machines. M.A.Sc. Thesis, University of British Columbia, Vancouver BC., 1988. [2] N. M. Wong. Implementation of a Force-Reflecting Telerobotic System with Magnetically Levitated Master and Wrist. [3] It. L. Hollis, S. Salcudean, and D. W. Abraham. Towards a tele-nanorobotic manipulation system with atomic scale force feedback and motion resolution. In Proceedings of the 3rd IEEE Micro Electro Mechanical Systems, pages 115-119, Napa Valley, CA, Feburary, 1990. [4] S. Salcudean, N.M. Wong, and R.L. Hollis. A Force-Reflecting Teleoperation System with Magnetically Levitated Master and Wrist. In Proceedings of the 1992 IEEE International Conference on Robotics and Automation, Nice, France, May 10-15, 1992. [5] R.L. Hollis, S.E. Salcudean, and A.P. Allan. A six-degree-of-freedom levitated variable compliance fine-motion wrist: Design, modelling, and control. IEEE Transactions on Robotics and Automation, 7(3), June 1991. [6] B. Wie, H. Weiss, and A. Araposthatis. Quaternion feedback regulator for spacecraft eigenaxis rotation. Journal of Guidance, Control and Dynamics, 12(3), May-June 1989. [7] B. Wie and P. Barbara. Quaternion feedback for spacecraft large angle manouvers. Journal of Guidance, Control and Dynamics, 8(3), May-June 1985. [8] J.T. Wen and K. Kreutz. Globally stable control laws for the attitude maneuver problem: Tracking control and adaptive control. In Proc. 27th IEEE Conference on Decision and Control, pages 196-191, Austin, Texas, December 7-9 1988. [9] J.T. Wen and K. Kreutz. The attitude control problem. Journal of Guidance, Control and Dynamics, 36(10), October 1991. [10] J.S.-C. Yuan. Closed-loop manipulator control using quarternion feedback. IEEE Journal of Robotics and Automation, 4(4), August 1988. [11] N. Hogan. Impedance Control: An Approach to Manipulation: Parts I and II. Journal of Dynamic Systems Measurement and Control, 107(1), March 1985. [12] D. E. Whitney. Historical Perstpective and State of the Art in Robot Force Control. International Journal of Robotics Research, 6(1), Spring 1987. [13] C. H. An and J. M. Hollerbach. Dynamic Stability issues in Force Control of Manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1166-1172, March-April 1987. 81 Bibliography^ 82 [14] W. S. Kim andB. Hannaford and A. K. Bejczy. Force-Reflection and Shared Compliant Control in Operating Telemanipulators with Time Delay. IEEE Transactions on Robotics and Automation, 8(2), April 1992. [15] H. Kazerooni and T. I. Tsay. Stability Criteria for Robot Compliant Maneuvers. In Proceedings of the 1988 IEEE Conference on Robotics and Automation, volume 2, pages 1166-1172, Washington, DC, 1988. [16] H. Kazerooni. Stability and Performance of Robotic Systems Worn by Humans. In Proceedings of the 1990 IEEE Conference on Robotics and Automation, volume 1, pages 558-563, Washington, DC, 1990. [17] H. Kazerooni. Human/Robot interaction via the Transfer of Power and Information Signals, Part I Dynamics and Control Analysis. IEEE Transactions on Systems, Man and Cybernetics, 20(2), March - April 1990. [18] S. D. Eppinger andW. P. Seering. Understanding Bandwidth Limitations in Robot Force Control. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 904-909, Raleigh, NC, 1987. [19] H. Huang and W. Tzeng. An observer design for constrained robot systems. In Proceedings of the 27th Conference on Decision and C'ontrol, pages 2261-2263, Austin, TX, December, 1988. [20] H. Huang and W. Tzeng. Robotic force control using estimated contact force. In Proceedings of the 28th Conference on Decision and Control, pages 2158-2263, Tampa, FL, December, 1989. [21] H. Kobayashi and H. Nakamura. Adaptive Force Control for Walking Robots with Torque Observer. In IECON '90, 16th Annual Conference of IEEE Industrial Electronics Society, New York, NY, (1), 1990. [22] K. Ohnishi and T. Murakami. Advanced Motion Control in Robotics. In IECON '89, 15th Annual Conference of IEEE Industrial Electronics Society, Philadelphia, PA, (2), 1989. [23] M. A. Peshkin. Programmed compliance for error corrective assembly. IEEE Transactions on Robotics and Automation, 6(4), August 1990. [24] C. H. An, C. G. Atkeson, and J. M. Hollerbach. Model-Based Control, of a Robot Manipulator, chapter Load Estimation, pages 65-85. MIT Press, 1988. [25] S. Salcudean. A globally convergent angular velocity observer for rigid body motion. IEEE Transactions on Automatic Control, 36(12), December 1991. [26] S. Nicosia and P. Tomie. Robot control using only joint position measurements. IEEE Transactions on Automatic Control, 35(9):1058-1061,1900. [27] J.T Wen and D.S. Bayard. A new class of control laws for robotic manipulators, part i: Non-adaptive case. Mt. J. of Control, 47(5):1361-1385,1988. Bibliography^ 83 [28] J.C.K. Chou. Quarternion kinematic and dynamic differential equations. IEEE Transactions on Robotics and Automation, 8(1), Feburary 1992. Appendix A Quaternion Algebra A.1 Introduction Representation of orientation is, by nature, more complicated than that of position. Unlike the position case, no nonsingular, three parameter representation of orientation exists. As a result several different parametric representations of orientation have been developed. Euler angles, Roll - Pitch - Yaw, and Rodrigues paramters are examples of three parameter representations. The rotation matrix is a well known nine parameter representation. Euler quaternionsl are a nonsingular four parameter representation has become popular recently in attitude control. Several of the properties of the quaternion representation used in this work will be outlined here for reference [28]. Quaternions are a minimal nonsingular representation of orientation, and have a strong intuitive appeal. The quaternion has associated with it an axis, s representing the Euler rotation axis, and a single parameter representing the angle of rotation, 0, about that axis. The following notation will be used to represent a quaternion as a combination of the scalar and vector part: a = (a0, a)^ (A.97) with ao^cos (--) 2 a^sin ((?) s 2 1The term Euler quaternion is shortened to simply quaternion in common usage and in this thesis. 84 (A.98) 85 Appendix A. Quaternion Algebra^ A.2 Properties of the Quaternion The quaternion as presented in (A.98), is a normalized representation of attitude, that is, ao 2 + a T a = 1 ^ (A.99) Associated with any such attitude, is a rotation matrix, Q, whose relation to the quaternion (A.97) is [25] Q = exp(Osx) = 1+ 2a0(ax)d- 2(ax)2 (A.100) Time variation of attitude is directly related to angular velocity, w, through, Q=xQ (A.101) which, through use of (A.100) can be represented in quaternion form as: 1 — Tw = —a (A.102) 2 1 =^[aoI —(ax)]w 1 [.cx01- + (a x )] Bw A.3 Rotation of A Frame A quaternion is a representation of the difference in attitude of two frames. If a base frame is defined, then a quaternion may represent the orientation of some frame with respect to the base frame. The difference between quaternions, can thus be described as the quaternion who's orientation reflects the orientation of one frame with respect to the other. The difference between two frames expressed by quaternions a and_can be itself be expressed as a quaternion 7 = a )3 with, = a000 = a0/3 — aT ^ /30a — ax/3 (A.103) Appendix A. Quaternion Algebra^ A.4 Inverse of a Rotation A quaternion rotation a has a unique inverse a* such that: a 0 a* = (1,0) . This inverse is easily verified as: a* = (a0, —a) 86 Appendix B Potential Energy of the Rigid Body It has proved useful to imagine the error dynamics as corresponding to an imaginary dynamical system. In the case of the torque observer, orientation dynamics are considered. A natural Lyapunov function for the error dynamics, would then be the total energy of the rotating system. Dynamics expressed in equation (4.61) suggest that enviroment torques are exerted on a body with restoring torque lkpesgn (e0) and a damping torque kvesgn (e0). We first consider the potential energy of a rigid body with a restoring torque as stated. For simplicity, assume that sgn (e0) -= 1. The restoring torques over all error orientations can easily be shown to be a conservative field. (e) —kpesgn (e0) = ke^ (B.106) curl (r,. (e))^Ve x r (e) ^0^ (B.107) Hence, there exists a potential function 01) (e), such that —Ve^= Tr, and the potential can be computed as a line integral in e, independent of the path chosen. The natural path about which to choose such an integration, reflects a rotation about the eigenaxis from the base orientation. That is, - 2 I. 41) = — I^eTsdB 2 0.0 0=t f la) (sin (0) 0=0 2 87 (B.108) sT) sdO Appendix B. Potential Energy of the Rigid Body^ 88 0=t —^ 0=0 e) de in (— 2^2 = kp (1 — cos ()) = kp (1 — e0) Note that the above restricts rotation to Oc [0,7r]. A negative rotation results in the same energy, and would have syn (eo) = —1. Hence: (10 = kp (1 — eosgn (e0))^ (B.109) With unit uniform mass (the inertia matrix is the identity) kinetic energy is easily seen as 11-2TO, so the total energy is: 2 1 V = — CFC2 kp (1 — eogn (e0)) 2 (B.110) Note using (A.99)that (B.110) can also be written as: V = 1— 117^-TP (1 — eogn (eo))2^eTe 2 2 Variants of (B.111) are used in chapter 4. (B.111) Appendix C Bound For Corriolis/Centrifugal Term in Serial Link Mechanisms Lemma: Given that = 11411 <^11c(q,4)11 < ke11411 and C (x, y)z = C (x, z) y, it follows that with - xiC 2 k, x1 M) (C.112) Proof: xiC (q,),)- 1 XiC X (C.113) (q,1) C^1)^- 1) LT^.^LT XiC ( q,^q — xiC and thus :.T XiC , i) 1 2 X1 89 k, X M) •^(C.114) Appendix D Operations Manual and Source Code Guide for the UBC Maglev Wrist D.1 Overview This appendix is designed to allow reproduction of the results presented in this work, and to allow for future development. The software described was developed only to provide testing of the algorithms, and serves no other immediate practical purpose. Our focus here will then be to describe the use of the system as it was configured at the time of writing, and to allow for the adaptation of the new routines to a new users own software. D.2 Hardware Configuration and Setup In this Section, we present the hardware configuration necessary to run the code developed for this work. It is recommended that a new user experiment with this configuration first before adapting the routines for other purposes. Processing and I/O is shared between six cards resident in the VME cage, see Figure D.30. At the time of writing, the controller software had been modified to support the use of a 1860 numeric coprocessor resident on the SkyBOLT card. In this configuration, two Sparc processors were required for this purpose, one (proteus) running wrist .sky under the Sun OS operating system, and one (r2d2) running wrist .vw under the VX Works real time operating system. Virtually all position sensing and control calculations are performed in wrist .sky acellerated by the 1860 coprocessor. Real time control is acheived by using the VX Works OS to handle timing of interrupts to collect and pass raw I/O data through the XVME 545 A/D and DVME 628 D/A cards. 90 Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist^91 The configuration of all six cards in the VME cage must be as presented in Figure D.301. Care should be taken to ensure that jumper J1 on the proteus board (located halfway down the board close to the VME connectors) is closed and the same jumper on the r2d2 card is open. Closing the J1 jumper designates that board as the bus master, allowing it to control servicing of requests by the other boards to access the bus. Cables must be connected as in Figure D.30. D.3 Account Setup Use of the SkyBOLT processor and VX Works libraries necessary to compile relevant code requires some customization of a users computer account. SkyBOLT and VX Works libraries are accessable to those in the robotics group through the robot network. It may be necessary to obtain access to the VX Works libraries before compilation may take place. New users should consult with the support staff (Rob Ross at time of writing) to ensure proper access. Several paths, used in the compile stage, need to be defined in the users .cshrc.local file, see Appendix E. A prototype of this file is given below. Again, consult support staff if modifying the .cshrc.local file is not a familiar procedure. D.4 Software Handshaking between proteus (Sun OS) and r2d2 (VX Works) is acheived by using shared memory in the form of a C language structure defined in wrist .h, see Appendix E for a listing of wrist .h. The variable wrist_data - >busy acts as a flag in the handshaking, and ensures wrist .sky updates the control and position calculations only once every control interval. 1Not all boards must actually be in the order presented. However, it is important that proteus be in the leftmost slot thereby making proteus the bus master. New users should adopt the configuration presented to avoid difficulties. Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist ^92 Figure D.30: Detailed Hardware Configuration Using SkyBOLT: Cables are denoted by thin lines in the figure. Cables in the lab are clearly marked and all connector sizes are different so it is nearly impossible to confuse them. Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist^93 D.4.1 Start Up Procedure After having made the appropriate modifications to the user's account, it is possible to fly the Maglev wrist from any terminal on the robot network. It is reccommended that a new user fly the wrist with the original source code before attempting to use the source code used in this work. If such code is note available, then it is reccommended that the new user read this entire appendix befor attempting to fly the Maglev wrist. The start up procedure is identical for both sets of code, and amounts to following the steps listed below. 1) Power upt the VME cage from the small green switch located on the back of the cage. If power is already on, reset proteus from a small reset switch located on the face of the proteus board. Resetting the master causes the entire VME system to reset, causing both proteus and r2d2 to boot from the robot network. Ensure that the signal conditioning box is turned on, and the current driver and its power supply is turned off. 2) Login to your account and select the windows environment of your choice. 3) Allow five minutes for proteus to boot, progress can be monitored by attaching a dumb terminal to the proteus serial port on the proteus face (Not necessary). r2d2 will boot much more quickly than proteus. 4) Once the system has booted remotely log in to proteus by typing rlogin proteus from a free shell. Likewise, log into r2d2 by typing rlogin r2d2 from another free shell. Since r2d2 is running VX Works, once the log in is complete, a ->prompt will replace the usual Unix prompt. 5) Change to your desired directory for both proteus and r2d2. At time of writing this directory would be peterh/wrist/runcode. The new user will likely want to copy the code into a seperate directory. The VX Works operating system does not start up in the users home directory, and the user will have to change to that directory. Mark Milligan has constructed convenient script files to expediate the change of directory to a chosen. See the current support staff in order to construct a similar script. For the code in this thesis, type <peterh in the r2d2 window to Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist ^94 have the directory automatically changed to peterh/wrist/runcode. proteus runs Sun OS, so changing directories are as usual. 6) VX Works requires object code to be loaded before running. Type id < "wrist .vw" to load object code. 7) Run wrist.sky on proteus, and wait until the line Accepting Client appears. The preceding lines displayed on proteus are diagnostics related to proteus mating with the SkyBOLT. 8) The Accepting Client line informs the user that proteus is ready to link up with r2d2. Enter wrist on r2d2 at this point to start the wrist .vw program. 9) The wrist control law is ready to fly the Maglev wrist. The current driver and its power supply may now be turned on at which point the wrist should be flying. D.4.2 Additional Code The source code currently resides in peterh/wrist/runcode and is a modified version of the SkyBOLT code adapted by Mark Milligan. Added to the original source code are six new files; identify .c, parameters .c, observe .c, RCC_control.c, rcclaw.c and timing.h. A brief description of these files is presented below. identify.c This routine updates the least squares recursion. It should be noted that the identification routine is quite computationally expensive, and a 2ms control update (500Hz) was about as fast as the system could be run without the SkyBOLT running out of time between samples. observe.c This routine updates both the linear and angular velocity observers. Observer based force torque estimates are generated here, and corrected velocity outputs are also generated. Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist^95 parameters.c This is a short routine used to store the inertial paramters in SI units. It is also used to compute the inverse of the inertia matrix, as is needed in the observer and RCC control routines. As implemented, this function is called only once, usually at the end of the identification procedure. RCC_control.cand rcclaw.c The RCC_control.c routine was adapted from control.c and prepares variables for the control law rcclaw .c. timing.h This include file is used to select which of the new routines are to be used and when they are to be used. Compile time definitions are used for this purpose. Two types of defines are used; either the define is a binary flag to select or not select an option, or it represents the time, measured in control updates since startup, for the particular option to occur, see Appendix app:code. Most routines require an inertial model, and the user has the choice of either using preestimated default values obtained for wrist 2, or running the identification algorithm beforehand. The DEFAULT define is used to bypass the identification and IDENT is used to run the identification. If identification is chosen (IDENT=1), then the two variables IDENT_START and IDENT_END define the start and stop times of the identification. The latter defines automatically control the generation of the filtered pseudo-random binary input, so the wrist should be expected to vibrate through the identification phase. The inertial parameters are stored to their variables defined in wrist .h at STORE_PARAMS. if the DEFAULT flag is set, default parameters will instead be stored at STORE_PARAMS. The code as written was not designed to run the identification during any of the remaining options as adaptive estimation was not explored in this work. The observer, RCC control and JR3 servicing routines each have thier own selection flag and start time as is seen in Appendix E. The JR3 servicing routine, if selected, runs from startup, but Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist ^96 the offset is removed at RCC_ZERO. The final flag ALL_SIGNALS controls the number of variables initialized in the data collection routine StethoScope. If selected, all variables defined in scope .c are initialized, if not selected, only ones pertaining to identification are initialized. The latter was designed to save space when collecting data for offline identification. The example in Appendix E demonstrates how a RCC control can be achieved for a rigid flotor with unknown inertia. Identification of the inertial parameters is necessary before the observer or RCC control law may be used. Identification is selected and starts at 3000 updates, or six seconds. The delay allows sufficient time for the operator to turn on the wrist currents before identification begins. The identification ends 14 seconds later at 10000 control updates. The latter was found to be a minimum time requirement for reasonable estimation results. Estimated inertial values are converted to SI units and stored at this time. The observer starts 2 seconds later (at 11000 updates), allowing the flotor to come to rest before the observer starts. This minimises transients in the observer startup. And finally at 12000 updates the RCC controller takes over and replaces the PID control used for the identification. D.4.3 Modified Code In addition to the new files, several of the original source code files were modified to service the new variables used in the new code. Modified files include: wrist .h, skybolt .c, main_vx.c. init.c, scope .c, position.c and control .c. All lines of added code in the latter files are bounded by comments with the author's name, see Appendix E for an example. Below we list a brief description of the added source code. skybolt.c Timing of the various routines as described in timing.h are put to practice in skybolt .c. The only other major modification to this routine was the direct inclusion of code for processing the JR3 inputs and transforming them to SI units in the center of mass frame. Appendix D. Operations Manual and Source Code Guide for the UBC Maglev Wrist^97 main_vx.c and init . c These two routines initialize global variables for wrist .vw and wrist . sky respectively. All new variables defined were initialized here. scope.c Variables monitored by StetheScope are defined in this file. Only VX Works variables may be monitored, and most are defined in the shared structure itself defined in wrist .h. Most new variables defined in wrist .h were set up for monitoring by StetheScope. position.c Velocity and acceleration estimates used in the identification procedure are calculated from filtered position and orientation estimates. To save computational effort, old values of position, needed for the filter, are stored in a array, and pointers to each element are updated rather than moving the elements of the array. With each update, the oldest data is overwritten by the new data before pointers are updated. Position and orientation of the body with respect to the remote center are also calculated. control.c An additional routine is added to provide the psuedo random binary signal needed for identification. Appendix E Sample Source Code 98 ^ Oct 22 1993 14:45:48 ^1^*define 2^#define 3^#define 4 5^#define 6 7^#define 8^#define 9^#define 10 11^#define 12^#define 13 14 15^*define 16^#define 17 18^#define 19 ^ IDENT^1 IDENT_START ^3000 IDENT_END^10000 DEFAULT_PARA14S2 0 OBSERVER^1 OBSERVER_START 11000 STORE_PARAMS ^IDENT_END JR3_0N^0 JR3_ZERO^26000 RCC^1 RCC_START^12000 ALL_SIGNALS^1 99 timing.h^ Page 1 Oct 21 1993 16:03:27 ^ wrist.h^ Page 1 1 2 3^/* wrist.h - wrist program header file 4 5 6^/. Added Interupt mode for 545 - M. Milligan Aug 10, 1992 */ 7^/* modified for VxWorks on VME - M. Milligan July 15, 1992 */ 9 10 /* DEFINES •/ 11 12 *define WRIST^2^/* wrist number for position sensing gains and offsets */ 13 *define CURRDRV^1^/* current driver * for gains and offsets*/ 14 *define SCOPE^1^/* 1= install, 0= not installed ./ *define MONITOR^0^/* 1= install, 0= not installed ./ 15 16 *define XVME566^0^/* 1= install, 0= not installed */ *define XVME545^2^/* 0= not installed, 1= *1 installed, 2= *2 installed */ 17 *define DVME628^2^/* 0= not installed,1= *1 installed, 2= *2 installed */ 18 *define INTERUPT^1^/* 1= interupt mode, 0= no ints */ 19 20 *define TEST^0^/* 1= test mode, 0= normal mode */ 21 *define VME_IRO^6^/* vine irg level 1-7 for xvme545 */ 22 *define HOST_NAME *proteus" /* server name to use */ *define SERVER_NUM^5555^/. port number for socket communication */ 23 24 *define FREQUENCY^500^/* PID Loop frequency in Hz */ /* ^ Peter Hacksel April 25 ^ +1 25 *define DIFF_ORD 2 26 27 *define DOUBLE_DIFF_ORD 3 *define BUTTER_ORD 6^/* order of velocity smoothing filter */ 28 29 *define ELLIP_ORD 4 30 *define FILTER_SHIFT 8 /* number of samples shifted by smoothing and 31 differentiating filters */ *define X_RECORD 10 32 *define OMEGA_RECORD 10 33 *define CONTROL_RECORD 10 34 35 *define WRENCH_RECORD 7 *define R_/NIT 0.0001^/* Initial Covariance */ 36 37 36^/* ^ 39 40^*if( INTERUPT == 1) 41^*define^RANDOM 0^/* interupts must be sequential */ 42^*else 43^*define^RANDOM 0^/* 1= random mode, 0= sequential */ 44^*endif 45 46^*if( XVME566 == 1 ) 47^*define^CONV^819.6^/* PSD A/D conversion from counts to volts +/- 2048*/ 48^fiendif^ /* +/- 2048 @ +/- 2.5v . 4096 / 5.0 volts = 819.6 */ 49^*if( XVME545 ) 50^*define^CONV^3276.7 /* PSD A/D conversion from counts to volts +/- 32767*/ 51^*endif^ /* +/- 32767 @ +/- 10.0v = 65535 / 20.0 volts ^3276.7 */ 52 53^*define^G^9.81^/* gravity in m1s^2 */ 54 55^/* defines for scope */ 56^*define NUM_SIGNALS ^32 /* must be a power of 2 */ 57^*define LENGTH_PER_SIGNAL^1024 * 4 58 59^/* misc */ 60^*define FBFORCE^ /* for stiffness feedforce */ 61 62^/. for sparcle */ 63^*define FSRDEFAULT^0x02400000 /* mask out ALL exceptions, allow rounding */ 64 65 66^/ ^ 67^/* STRUCTURES */ 68^/ ^ 69 70^struct WRIST_DATA^/* global shared memory structure: sky/vx */ 71^{ 72^int ad_data[32);^/* data from A/D */ 73^int da_data[8];^/* data to D/A^*/ 74^int busy;^/* handshakeing^*/ 75^int exit_task;^/* shutdown flag ./ 100 Oct 21 1993 16:03:27^ wrist.h^ 76^int sky_control_cnt; /* loop counter on sky */ 77^int vx_control_cnt; /. loop counter on vxworks */ 78^int sky_control_miss; /* miss counter on sky */ 79^int vx_control_miss; /* miss counter on vxworks */ 80^float x[6];^/* xyz position data */ 81 82^/* Peter Hacksel Mar 10 ^ .1 83 84^/* All Values in Standard SI Units !!!*/ 85 86^/* -- Position and Control -- */ 87 88^float F_wrench[6]; 89 90^float beta_old1[3]; 91^float beta_old2[3]; 92^float beta_dot[3]; 93^float omega_dot[3]; 94^float omegaf[3]; 95^float omega[3]; 96^float vf[3]; 97^float v[3]; 98 99^float S_a[3]; 100^float F_a[3]; 101^float F_g[3]; 102 103^float fapp_amp[6]; 104^float fapp_period[6]; 105 106^float x_d[3]; 107^float v_d[3]; 108^float a_d[3]; 109 110 111^/* -- Identification -- */ 112^float th_id[101; 113^float R[107[10]; 114^float A_id[6][10]; 115^float F_wrench_ident[6]; 116^float JR3_wrench[6]; 117^float residual[6]; 118 119^/* -- Inertial Parameters -- */ 120 121^float mass; 122^float J[3][3]; 123^float Jinv[3][3]; 124 125^/* -- Observer -- */ 126^float x_hat[3]; 127^float v_hat[3]; 128^float x_obs_hat[3]; 129^float v_obs_hat[3]; 130 131 132^float beta0; 133^float beta[3] 134^float beta_hat[3]; 135^float beta_hat0; 136^float beta_obs_hat[3]; 137^float beta_obs_hat0; 138 139^float psi[3]; 140^float omega_hat[3]; 141^float psi_obs[3); 142^float omega_obs_hat[3]; 143 144^float F_env[3]; 145^float tau_env[3]; 146^float tau_env_FCC[3]; 147 148^float F_env_corr[3]; 149^float tau_env_corr[3]; 150 Page 2 ^ Oct 21 1993 16:03:27^ wrist.h^ ^151^float force[3]; 152^float torque[3]; 153 154^float J_force[3]; 155^float J_torque[3]; 156 157^float S_omega_obs[3]; 158^float S_v_obs[3]; 159^float F_omega_obs[3]; 160^float F_v_obs[3]; 161 162^/* -- Control -- */ 163 164^float S_x_com[6]; 165^float S_x_coc[6]; 166^float S_x_ref[3]; 167 168 169^float e[3]; 170^float beta_d[3]; 171 172^float start_flag; 173^/* ^ 174^); 175 176^/* end file wrist.h */ 102 Page 3
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Observer based velocity and environment force estimation...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Observer based velocity and environment force estimation for rigid body control Hacksel, Peter J. 1993
pdf
Page Metadata
Item Metadata
Title | Observer based velocity and environment force estimation for rigid body control |
Creator |
Hacksel, Peter J. |
Date Issued | 1993 |
Description | Issues pertaining to the control of a rigid body are considered in this work. A magnetically levitated robotic wrist is here modelled as a rigid body subjected to both control and environment forces. All the algorithms presented here were implemented on the Maglev wrist for experimental verification. A good inertial model of the rigid body was needed for the estimation and control work in this work; a least squares identification procedure, found in the literature, was implemented for this purpose. Algorithms are developed for obtaining dynamic environment force and torque estimates from position errors in a velocity observer. Obtaining torque estimates from a nonlinear velocity observer is achieved by using the linear force observer as an analogue. Stability of the torque observer to a constant environment torque is proven for the one dimensional case only. Simulation and experiments are used to verify the stability in the three dimensional case. Environment force/torque measurements, alone useful for control purposes, are also used to correct a velocity observer. The resulting corrected observer design gives low noise velocity estimates and is robust in that it is not sensitive to errors caused by interaction with the environment. As a demonstration of the merit of the observer based work, a remote center of compliance controller is implemented. An inertial model is needed to provide feedforward that linearizes the dynamics to a. second order system. Corrected observer velocities are used in the feedforward and the derivative component of the control law. The linear dynamics about a remote center is verified by comparing the environment forces measured with the velocities observed. |
Extent | 4279614 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
File Format | application/pdf |
Language | eng |
Date Available | 2008-09-10 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0065239 |
URI | http://hdl.handle.net/2429/1778 |
Degree |
Master of Applied Science - MASc |
Program |
Electrical and Computer Engineering |
Affiliation |
Applied Science, Faculty of Electrical and Computer Engineering, Department of |
Degree Grantor | University of British Columbia |
Graduation Date | 1993-11 |
Campus |
UBCV |
Scholarly Level | Graduate |
Aggregated Source Repository | DSpace |
Download
- Media
- 831-ubc_1993_fall_hacksel_peter.pdf [ 4.08MB ]
- Metadata
- JSON: 831-1.0065239.json
- JSON-LD: 831-1.0065239-ld.json
- RDF/XML (Pretty): 831-1.0065239-rdf.xml
- RDF/JSON: 831-1.0065239-rdf.json
- Turtle: 831-1.0065239-turtle.txt
- N-Triples: 831-1.0065239-rdf-ntriples.txt
- Original Record: 831-1.0065239-source.json
- Full Text
- 831-1.0065239-fulltext.txt
- Citation
- 831-1.0065239.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0065239/manifest