Master-slave Force-reflecting Resolved Motion Control of Hydraulic Mobile Machines by Ming Zhu B . S c , Shanghai Jiao Tong University, China, 1987 A THESIS SUBMITTED I N PARTIAL F U L F I L L M E N T O F THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE F A C U L T Y OF G R A D U A T E STUDIES DEPARTMENT OF ELECTRICAL ENGINEERING We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH C O L U M B I A A p r i l 1994 © M i n g Z h u , 1994 In presenting this degree at the thesis in partial fulfilment of University of British Columbia, I agree freely available for reference copying of department this or publication of and study. thesis for scholarly by this his or her £ J '& CAY t The University of British Columbia Vancouver, Canada DE-6 (2/88) requirements that the I further agree purposes representatives. may be It thesis for financial gain shall not permission. Department of the that advanced Library shall make it by the understood be an permission for extensive granted is for that allowed without head of my copying or my written Abstract Issues concerning the design and implementation of master-slave force-reflecting resolved motion control o f hydraulic mobile machines are addressed i n this thesis. Network concepts and linear system theory are used to design and analyze general forcereflecting teleoperator systems to achieve high performance while maintaining stability. A new control structure is proposed to achieve "transparency" for teleoperator systems under rate control. A novel approach to stability analysis of the stiffness feedback strategy proposed i n previous work is provided which, under certain condition, guarantees global asymptotic stability o f the teleoperator system. The system could be either under rate or position control and could be subject to time-delays, nonlinearities or active environments. The closed-form inverse kinematics solutions o f an excavator and a feller-buncher, w h i c h are four and five degree-of-freedom manipulators respectively, are provided to achieve resolvedmotion o f the manipulator's end-effector. Using the U B C magnetically levitated joystick, the master-slave force-reflecting resolved motion control has been successfully implemented on a C A T - 2 1 5 excavator and a C A T - 3 2 5 feller-buncher. Machine experiments demonstrate the effectiveness of this control strategy i n improving productivity and safety o f general hydraulic mobile machines. ii Table of Contents Abstract ii List of Tables vi List of Figures vii Acknowledgments x 1 Introduction 1 1.1 Motivation 3 1.2 Previous Work 6 1.3 9 Thesis Overview 2 Resolved Motion 2.1 10 Overview 10 2.2 Inverse Kinematics of an Excavator 12 2.3 Inverse Kinematics of a Feller-buncher 16 2.3.1 Inverse Kinematics: Approach 1 18 2.3.2 Inverse Kinematics: Approach 2 20 2.3.3 Some Comments on the Inverse Kinematics 24 3 Force Reflection 3.1 25 Network Representation o f Teleoperator System 26 3.2 General Teleoperator Structure 28 3.3 Force-reflecting Control 32 3.3.1 Coordinating Force 32 iii 3.3.2 Force Feedback 34 3.4 Transparency 37 3.4.1 A c h i e v i n g Transparency Under Position Control 37 3.4.2 A c h i e v i n g Transparency Under Rate Control 43 3.4.3 A c h i e v i n g Transparency V i a Impedance Identification 48 4 Stiffness Feedback 50 4.1 Principle 50 4.2 Stability 52 5 System Overview 62 5.1 Master — M a g l e v Joystick 63 5.1.1 System Configuration 64 5.1.2 Sensor and actuator 65 5.1.3 Control about the Remote Center o f Compliance 66 5.2 Slave — Hydraulic M o b i l e Machines 69 5.3 Computing System . ; 6 Machine Experiments 70 72 6.1 Endpoint Force Sensing 72 6.1.1 Modeling 72 6.1.2 Torque Computations 78 6.1.3 Recursive Least-squares L i n k Parameter Estimation 81 iv 6.2 Excavator Experiments 63 90 6.2.1 Resolved M o t i o n Control 91 6.2.2 A p p l y i n g a Desired Force 96 Feller-buncher Experiments 99 6.3.1 Desired Force Tracking 102 6.3.2 Complete Tree Cutting Process 105 6.3.3 Practical Issues 107 111 7 Conclusions 7.1 Thesis Contributions Ill 7.2 Further Work 112 References 113 v List of Tables 2.1 Excavator link Denavit-Hartenberg parameters 13 2.2 Feller-buncher link Denavit-Hartenberg parameters 17 6.3 Piston geometry o f the excavator 78 6.4 Piston geometry of the feller-buncher 78 6.5 Joint linkage geometry o f the excavator 80 6.6 Joint Linkage geometry o f the feller-buncher 80 6.7 G a i n selections for the excavator experiments 90 6.8 M a x i m u m feedback force for the excavator experiments 91 6.9 G a i n selections for the feller-buncher experiments 99 6.10 M a x i m u m feedback force for the feller-buncher experiments 100 6.11 Detent parameters for the feller-buncher experiments 102 vi List of Figures 1.1 Kinemaucally similar master-slave system 1 1.2 Kinematically dissimilar master-slave system 2 1.3 Excavator schematic 4 1.4 Photograph o f a typical excavator 4 1.5 Feller-buncher schematic 1.6 Photograph o f a typical feller-buncher 2.7 Excavator configuration 13 2.8 Projection o f excavator onto plane 14 2.9 Feller-buncher configuration 16 3.10 Ideal teleoperator system 25 3.11 Network concepts 26 3.12 Network representation of teleoperator system 27 3.13 General teleoperator structure: After Lawrence, 1992 29 3.14 Closed-loop stability analysis 31 3.15 Position-position structure 32 3.16 Network representation o f position-position structure 33 3.17 Mechanical equivalence o f position-position structure 33 3.18 Rate-position structure 34 3.19 Position-force structure 35 3.20 Intervenient impedance 40 3.21 A c h i e v i n g transparency under rate control 44 3.22 A c h i e v i n g transparency v i a parameter identification 49 .5 5 vii 4.23 Comparison o f two rate control structures 50 4.24 Stiffness adjustment scheme 51 4.25 Stiffness feedback control b l o c k diagram 52 4.26 Trajectory i n phase plane 57 4.27 Trajectory i n phase plane 58 4.28 Trajectory i n phase plane 60 4.29 Trajectory i n phase plane 61 5.30 Overall system configuration 62 5.31 M a g l e v Joystick system configuration 64 5.32 M a g l e v Joystick assembly 65 5.33 Frame assignment 67 5.34 V M E cage configuration 70 6.35 Two-bar mechanical linkage 79 6.36 Four-bar mechanical linkage 79 6.37 Excavator link parameter identification 82 6.38 Excavator l i n k parameter identification 83 6.39 Excavator link parameter identification 84 6.40 Excavator link parameter identification 85 6.41 Feller-buncher link parameter identification 86 6.42 Feller-buncher link parameter identification 87 6.43 Feller-buncher link parameter identification 88 6.44 Feller-buncher link parameter identification 89 6.45 Excavator resolved motion control: bucket straight up 92 6.46 Excavator resolved motion control: bucket straight down 93 viii 6.47 Excavator resolved motion control: bucket straight out . . . 94 6.48 Excavator resolved motion control: bucket straight i n 95 6.49 Excavator stiffness control: applying a desired force 97 6.50 Excavator force control: applying a desired force 98 6.51 Detent implementation 100 6.52 Conventional position control 101 6.53 Feller-buncher stiffness control: force tracking 103 6.54 Feller-buncher control without force-reflection: force tracking 104 6.55 Feller-buncher stiffness control: tree cutting process 106 6.56 Feller-buncher step response: boom 108 6.57 Feller-buncher step response: stick 109 6.58 Feller-buncher step response: tilt 110 ix Acknowledgments I would like to thank m y supervisor T i m Salcudean for his support and guidance i n m y thesis work. I wish to thank Peter Lawrence and D a n Chan for their invaluable assistance and encouragement during the enduring machine experiments. I would also like to thank N i a l l Parker and T i m Vlaar for their helpful discussions and advice. Special thanks to m y wife Jane Zhang for her support and understanding. Chapter 1 Introduction Due to the fact that fully autonomous robots could not be realized using the latest technology, researchers began to concentrate on combining human versatility and expertise with machines. In some applications such as microsurgery, perhaps it w i l l never be possible to have a robot acting alone. The idea of using a master manipulator to command a slave manipulator can be traced back to the pioneering work by Goertz and his colleagues i n the late 1940s when the first recognizable mechanical master-slave manipulator was built. Later i n the early 1950s, the electrical servo was introduced. M o s t o f these early designs exploit a kinematically similar master and slave because of the simplicity o f the required controller, see Figure 1.1. Corresponding joint servos between the master and slave are tied together through electrical means. A s a result, only position control i n w h i c h the position o f the master is interpreted as a position command to the slave, can be used. Master Manipulator Slave Manipulator Figure 1.1 Kinematically similar master-slave system 1 Chapter 1: Introduction 2 Later, multi-degree-of-freedom joysticks were used as the input device to command the slave manipulator (see Figure 1.2). This structure soon gained its popularity due to the fact that the joystick can be used universally and usually takes less space. Rate control, i n w h i c h the position of the master is interpreted as a velocity command to the slave, combined w i t h conventional position control can be realized. F o r example, the space shuttle remote manipulator system is provided with both position and rate control modes [1]. W h e n the joystick controls the motion of the endpoint o f the slave directly, we call it resolved motion control. Hand Controller S| a v e Manipulator Figure 1.2 Kinematically dissimilar master-slave system Contact information is helpful to the operator i n reducing contact force, therefore reducing damages to the remote object. It also helps an operator probing i n an uncertain environment, and reduces task completion time. A l t h o u g h this information can be provided by visual display, the most natural and efficient way is to transmit it directly to the operator's hand. In the case of multiaxial operation, a visual display is not acceptable to the user. W h e n the contact force is reflected v i a the master actuator to the operator's hand, the teleoperator system is said to be controlled bilaterally, and the operator is said to be kinesthetically coupled to the environment. The early application o f teleoperator systems were mainly i n the nuclear industry to remove radioactive materials without involving the human i n the hazardous environment. The original Chapter 1: Introduction 3 idea o f manipulating objects remotely was soon broadened to include working on a different power scale, for instance, as a "man-amplifier" [2]. Teleoperator systems have the potential to play an important role i n future remote or hazardous operations, such as space servicing, undersea exploration, and mining and i n delicate operations such as microsurgery, microassembly and microchemistry. 1.1 Motivation Hydraulic mobile machines can be found i n various industries including forestry, mining and construction. To operate a conventional hydraulic mobile machine such as an excavator (Figure 1.3) or a feller-buncher (Figure 1.5), an operator uses two joysticks to control the individual displacements o f the hydraulic cylinders o f the machine i n an uncoordinated way. In order to achieve a desired end-effector motion, e.g. a straight line motion, a complicated two-handed maneuver must be executed. It usually takes a few months to train an operator. The advantages of applying resolved motion control to these machines are the following: 1. Simple, intuitive, single-hand operation. 2. H i g h performance, since accurate motion, such as a straight line, can be obtained. 3. Reduced machine wear, since unnecessary joint motion is reduced to a m i n i m u m . 4. Increased productivity and safety, since the machine moves i n an intuitive way. Chapter 1: Introduction Drawing by S. Tafazoli Chapter I: Introduction 5 Chapter I: Introduction Currently, an experienced operator depends solely on cues such as engine noise and machine motion to determine its load. H i g h contact force can easily cause damage to the task, even to the machine itself. The advantages o f applying force reflection to the control of these machines are the following: 1. Improved performance, as more delicate tasks can be achieved. 2. Improved safety, since machine tip-over can be avoided by pushing the joystick back to the center under computer control. 3. Reduced machine wear, as the operator is more able to determine the machine load. For a detailed description o f applications o f these hydraulic mobile machines i n construction and forestry industry, refer to [3]. The application of resolved motion control to general hydraulic machines was first investigated by Lawrence et al. [4]. Later, force-reflecting resolved control was applied by Parker [3] to a three degree-of-freedom log-loader. Our goal i n this project was focusing on the application of force-reflecting resolved control to a wider variety o f machines such as excavators and feller-bunchers and addressing related issues. 1.2 Previous Work The first recognizable mechanical master-slave manipulator was built i n the late 1940s by Goertz and his colleagues. Later i n the early 1950s, an electric servomanipulator system with force reflection capacity was introduced [5]. A survey on different universal joysticks can be found i n [6]. 6 Chapter 1: Introduction A dissimilar master and slave teleoperator system was studied by Jansen et al. [7] i n w h i c h force-reflection based on the master and slave Jacobians was proposed to control a 7 - D O F slave manipulator. A comparison between position and rate control was done by K i m et al. [8]. Position control is recommended for small workspaces with fast manipulators, while rate control is recommend for large workspaces with slow manipulators. The design of force-reflecting teleoperator systems for robot surgery were reported by Sabatini et al. [9]. A m i n i six-degree-of-freedom magnetically levitated wrist designed for microsurgery experiments was designed by Y a n [10]. The human dynamics i n response to visual and force feedback was modelled and incorporated into the controller design and evaluation by L e e et al. [11]. The role o f the controller was defined as modifying the dynamic characteristics o f the master and slave arms to achieve a desirable one. In [12], the force applied by the operator was used locally at the master side to modify its dynamics so that a "heavier" or a "lighter" master can be easily realized. Hard contact with direct force feedback was examined by Hannaford et al. [13]. The effect of the operator i n stabilizing the system was reported as i n much the same manner as local damping feedback o f the hand controller. It was suggested that the human operator impedance be estimated on-line i n order to find the m i n i m u m necessary damping for the system. Salcudean and Wong [14] added damping w h i c h is proportional to the amount o f contact force to the system to maintain hard contact stability, therefore avoiding "sluggishness" during free motion. Force reflection under time delay was investigated by Spong and Anderson [15], v i a scattering theory. Stability was achieved by introducing a control law that prevents the communication block from generating energy. However, L a w n and Hannaford [16] reported a 50% increase of task 1 Chapter 1: Introduction 8 completion time with these passivity based methods. K i m et al. [17] proposed compliant control i n w h i c h the feedback of the contact force is done locally at the slave side without kinesthetic force feedback so that the force feedback loop does not include a long communication time delay. Hannaford [18] argued that local force feedback reduces stability problem at the price of telemanipulation fidelity. Bilateral impedance control was proposed by Hannaford [19], i n w h i c h force and velocity sensing were used at both the master and slave side to identify the impedances o f the operator and environment to make the system transparent. Raju et al. [20] introduced a teleoperator structure i n w h i c h with velocity sensing the gains o f the controller are selected based on the knowledge o f the human operator and the task impedance. In order to achieve "transparency", infinite gains are required. It has been suggested by authors such as Salcudean [21], Lawrence [22] and Yokokohji [23] that the operator hand force should be fed forward to the slave side i n order to achieve transparency. Salcudean and Wong applied this approach to a master-slave manipulator system with identical six-degree-of-freedom magnetically levitated wrists. Yokokohji and Yoshikawa reported similar work on a single degree o f freedom system. Force reflection based on a kinematically similar master-slave teleoperator structure was proposed for the control o f excavators by Ostoja-Starzewski and Skibniewski [24]. A "Smart Shovel" w h i c h can be controlled remotely to remove hazardous waste with force reflection capability was reported i n [25]. The application o f teleoperation ideas to general hydraulic machines was investigated by Lawrence et al. [4]. Resolved motion control o f such machines was found to be useful i n reducing operator training time [26]. Force-reflection for hydraulic machines was previously treated by Chapter 1: Introduction 9 Parker et al. [3]. A three degree-of-freedom log-loader was controlled by a magnetically levitated joystick; a stiffness adjustment scheme was used for force reflection under rate control to realize stable operation. 1.3 Thesis Overview The remainder of this thesis is divided into 7 chapters. Resolved motion control of hydraulic mobile machines is treated i n Chapter 2. Transparency and stability issues related to force reflecting control o f teleoperator system are investigated i n Chapter 3. The analysis o f stiffness feedback control is provided i n Chapter 4. Chapter 5 gives an overview o f the entire system w h i c h includes the master, slave and computing system. Chapter 6 presents results o f machine experiments. Finally Chapter 7 draws a conclusion and makes some suggestions for future work. Chapter 2 Resolved Motion Resolved motion control requires on-line computation of inverse kinematics. In this chapter, closed-form inverse kinematics solutions are provided for typical hydraulic mobile machines, excavators and feller-bunchers, which are four and five DOF manipulators respectively. 2.1 Overview There are two types of problems in the kinematic analysis of manipulators. Thefirsttype is forward kinematics, in which the endpoint location has to be found given the joint displacement. The second type is inverse kinematics, which is the other way around. For serial manipulators, the forward kinematics can be easily carried out by a matrix recursion while the inverse kinematics is complicated because the governing equations are highly nonlinear. In the case of rate control, one approach is to transform the manipulator velocity from Cartesian space to joint space via an inverse Jacobian mapping, that is q = J-\q)V (2.1) where V and q are the manipulator's velocities in Cartesian space and joint space respectively and J is the manipulator's Jacobian. Since position error can accumulate, the inverse Jacobian method suffers from low accuracy. It is known that in order to place an endpoint frame freely in three dimensional space, the manipulator must have at least 6 DOF. For manipulators with 6 or more DOF, the inverse kinematics problem can be formulated as follows: orientation, find the joint displacements. 10 given the endpoint frame position and Chapter 2: Resolved Motion The methods o f solving the inverse kinematics problem fall into two categories: 1. Closed-form solution, i.e. an algebraic equation relating the given position and orientation of the endpoint to only joint displacements (see [27]). 2. Numerical solution, i.e. an iterative algorithm for solving a system o f six nonlinear equations (see [28, 29, 30, 31]). W h e n the manipulator has 6 D O F , the system o f six nonlinear equations has a solution. W h e n the manipulator has more than 6 D O F , the system o f six nonlinear equations is underdetermined, thus there may be multiple solutions. In this case, an objective function can be used to select a "best" solution among them. W h e n the manipulator has less than 6 D O F , the system o f six nonlinear equations is overdetermined, so there may be no solution at all. Instead o f solving a system o f nonlinear equations, one approach is to solve an optimization problem, that is, find a "best" fit i n some sense. This usually requires an iterative procedure. Another approach is to carefully formulate the problem with some constraint i n m i n d so that the solutions can be guaranteed ( hopefully a closed-form solution). The philosophy behind this is never to force a manipulator to do anything beyond its capacity while, at the same time, fully utilizing the degrees of freedom the manipulator provides. F o r a simple example, we should not expect a planar manipulator to go out o f that plane. The inverse kinematics problem can be easily formulated and solved i n this manner for a manipulator with few degrees o f freedom, but as the number of degrees o f freedom increases, the problem becomes more difficult. A few comments about closed-form solutions versus numerical solutions are i n order. Generally speaking, closed-form solutions are preferred. There are two major reasons. First, most of the iterative algorithms are considered "local methods" w h i c h requires an initial close guess to 11 Chapter 2: Resolved Motion 12 the exact solution; they may lack stability and reliability. Numerical solution can not find multiple solutions. Second, iterative algorithms usually require more time to compute a solution, so they are used mainly for off-line applications. The disadvantage of closed-form solutions are their dependence on manipulator structure, that is only certain classes o f manipulators allow closedform solutions. For example, it was found that a sufficient condition for a closed-form solution for a six D O F manipulator is to have three adjacent joint axes intersecting at a common point or any three adjacent joints axes parallel to each other. The problem becomes more critical for kinematically redundant robots, for w h i c h the number o f D O F exceeds the required six coordinates necessary to attain arbitrary location i n the three-dimensional work space. In comparison, many iterative algorithms can treat general manipulators without special considerations. 2.2 Inverse Kinematics of an Excavator A n excavator schematic is shown i n Figure 2.7. Considering the four D O F nature o f excavator, the inverse kinematics problem can be formulated as follows: given the endpoint frame position and its rotation angle along Z a o f frame Cj_, find the four joint angles. endpoint frame position is given i n cylindrical coordinates, that is (z , e angle is expressed as r, e 8 ); e The the rotation 0. Zl The relationship between Cartesian coordinates and cylindrical coordinates is x = r cos 9 e y= r e sin 6 e e (2.2) z = z e There are two reasons for using cylindrical coordinates: first, as we w i l l see later, it is easier to solve the inverse kinematics using cylindrical coordinates; second, it is more natural to map the hand control command into cylindrical coordinates since the operator moves with the cabin. Chapter 2: Resolved Motion 13 Figure 2.7 Excavator configuration di a; 1 0 a 2 h d 2 0 3 h 0 0 4 0 Index Oi 4 0 a,7T a a4 2 0 Tkble 2.1 Excavator link Denavit-Hartenberg parameters Figure 2.8 shows the projection of excavator onto a plane. There are many ways i n solving this inverse kinematics problem. Here we use a geometrical method. First we examine the case when endpoint frame is at the end o f stick, that is, the origins o f frame three and frame four coincide. Due to the physical constraint, only the right and above arm configuration is considered. Chapter 2: Resolved Motion 14 Given (z , r , O ), after a few algebraic manipulations [3], it is easy to derive the following e e e 9i = 6 + tan~ (^L l e -{IT-a) 9= 3 a - cos' i ( ai + al — f — V 2a a 1 2 / = /z2 + ( r - a ) I ) \jrl-cl\ /3 = tan- 1 (-^—] \r- J a = 2 1 r = 1 3 3 . _, - m i /azsina\ (2-3) 15 Chapter 2: Resolved Motion and 0 = 6 4 Zl -d -0 2 (2.4) 3 If the endpoint frame is not at the end of stick, the following procedure can be used. Let the position and orientation of the endpoint frame be C' :(z' , 4 e r' e1 0' , 9' ). e Zi Define the origins of frame C\ and C as 0\ and 0' respectively. If the relation between the two origins is 4 4 O' = C [d ,d ,0]' 4 4 x y +0 4 (2.5) the inverse kinematics solution becomes + tan-' 0 = (-**-) -(ir-a) 3 a = cos -i / Q2 + °3 - / 2 2a a 2 3 r, = y/d* + d* cos [tan-' ^ + 4,) *e = *e- \ l l + l d d [tan' 1 (-) 2 6 j - + 02=P + J (3 - tan- l • _ i 04 = r — ai (a sina\ 3 —02-03 assuming the specified endpoint frame is within the manipulator's work space; Otherwise, solution does not exist. Chapter 2: Resolved Motion 16 2.3 Inverse Kinematics of a Feller-buncher The feller-buncher is a five D O F manipulator w h i c h is mainly used for forestry harvesting. Figure 2.9 shows its schematic. The additional degree o f freedom makes its inverse kinematics problem more difficult to handle than the excavator. In the following, we introduce two closedform solutions for w h i c h one is preferred to the other depending on operation situation. W e w i l l Z5 xmj Y5 95 a5 d5 Figure 2.9 Feller-buncher configuration show h o w the inverse kinematics problem should be formulated so that a closed-form solution is guaranteed. We add a frame C' attached to the feller-buncher head, such that its initial position coincides 5 with C 4 (see Figure 2.7). Define rt' 1 o' ] 1 _ 5 Xn 'Co . 0 r\ 1 5 1 \c* _ 0 1— Xn' o'T (2.7) Chapter 2: Resolved Motion where 5 0 17 T is the homogeneous transformation, having the following special structure <t>kx 6jX e rp 5 ipkx £ o y z e 1 (2.8) T 0 atan2(y, x), but 0, tp, x, y , z where (<j> 0 ip) are Euler angles w.r.t. Co. We observe that < are independent variables. Index 6 di 1 0i 0 2 ^2 0 0 3 03 0 0 4 04 0 05 rf 5 { 1 7T «1 5 a 4 a 5 2 7T 2 0 Table 2.2 Feller-buncher link Denavit-Hartenberg parameters The introduction o f frame C' is important i n solving the inverse kinematics o f feller-buncher. 5 Assume the relationship between C 5 and C' is 5 "C 0 where d 0 5 nil 17 rf„1 05 1 ' / do' 05" 1 0 is a arbitrarily chosen constant vector. 1 0 (2.9) 1 This assumption indicates that C 5 can be positioned arbitrarily on the feller-buncher head. W i t h C' , we can conclude that the homogeneous 5 transformation between Co and C 5 i s i n the following special form / 0 d 1 <f>kX 6jX e e gifrkx t>kX 9jX e< e ifikX e ^ (2.10) 0 1 where d is a constant vector, and <f> = atan2(y, x). Note d,ip,x,y, 0 _|_ 0 z are independent variables. Since the manipulator only has five D O F , we can not achieve arbitrary position with arbitrary orientation. Hence the target frame should be specified to be within the manipulator subspace; otherwise the solution may not exist at all. Here we propose two approaches. Chapter 2: Resolved Motion 18 2.3.1 Inverse Kinematics: Approach 1 Generally, i f the operator specifies <t>dkx 6 jx ip kx e 5fTJ e d e d yd 0 <* J (2.11) Zd 0 1 there may be no solution at all. O n the other hand, i n some applications, it may not be crucial to specify <j>d, for instance, when the operator moves with the cabin. Thus, one solution is given by: (0 = e d ~x~ '*d' = y (2.12) yd z { cf> = atan2(y, x) Define e*ix ^kx e e d g = d = l [ d i x d i f y (2.13) du to obtain that Xd Vd (2.14) <f> = atan2(y, x) Since we have COScf) e^ x d = 1 —sincp sincf) COScf) 0 0' cos<f> d\ x — 0 sincp d\ x — sincf) d\ y + coscf> d\ y (2.15) 1_ 0 thus the equation set (2.14) is equivalent to coscf) d\ — sincf) d sincf) d + coscf) d x lx di + z iy ly + x = Xd + y = y d (2.16) z = Zd cf> = atan2(y, x) Substituting <f> = atan2(y, x) into the second equation o f equation set (2.16), we get coscp d\ x sincf) d\ x — sincf) d\ y + coscf) d\ y + x = x d + tancp x Vd (2.17) Chapter 2: Resolved Motion 19 Multiplying the first equation i n (2.17) by sincp and the second by coscf), we obtain that x sincf> - y coscf> d d -d (2.18) ly Define « = -Vd b* = x (2.19) d c* = -d ly thus we have a* coscp + b* sincf) = c* (2.20) The solution for this equation is cf> = atan2(b*, a*) ± atan2^y/a* + 6* - c*% c*) 2 (2.21) 2 Finally we have '' a X: == V 2/ = = < z = 0 = x — coscf) d\ d\+ sm^> + sincp i i d\ *d - coscf) sincp d\ — coscf) d\ d Vd sincf) d coscf) Ste d Zd x x x - l 2 / y lx y ly (2.22) d\ z o d ^d Here we have located the position and orientation of C' . We can see there are at most two 5 possible solutions. Equations (2.14) should be checked to eliminate any extraneous solutions. It is clear that 0 = V> 5 Solving for 0 1 ; 8, 2 0, we have seen before. 3 9 4 (2-23) is equivalent to the inverse kinematics problem o f the excavator Chapter 2: Resolved Motion 20 Define z„ = z r' e 1/z = 2 + y 2 (2.24) 0' = <t> e the solution can be summarized as following Ol = 0'e 0 = -(7T-a) 3 a = cos a / 2 2 l=y/z* + r = + 3 2a «3 _1 / O2 {r- ) ai r' -ri e T\ = a 4 cos (O'z) (2-25) z = z — 0,4 sin e e 02 = P + 7 /? tori- (—^-"i 1 = \r-aj . _ i/ a sma\ 3 04 = 0^j - 02 - 03 2.3.2 Inverse Kinematics: Approach 2 A five DOF manipulator can easily be found in applications such as welding or spray painting. Since the rotation along the gun axis direction is not important in these applications,fiveDOF is enough to do the job. In [27], the author joined the end effector to the grounded joint of the arm by a pair of hypothetical joints and links, then a complicated procedure was required to solved this hypothetical closed-loop spatial mechanism for joint displacements. Here we propose Chapter 2: Resolved Motion 21 an algebraic method. The inverse kinematics problem can be formulated as follows: given the position of frame C and direction of X , find the corresponding joint angles, that is, given 5 5 rri\ m 3 5/Tl pi p Tij 2 m n\ o\ »5 03 P s (2.26) 2 . 0 0 0 1. where only ( m m , m ) and (pi, p P3) are user specified. Compared with approach 1, l 5 2 3 2 ) this method is desirable in the case where the operator controls the machine from afixedgroundbased location. As before, we have T d <f>kx 9jx ipkx e IT = %T0 1 0 e 4>kx 9jx e e e gi/>fcX ^ _|_ 0 where d is a constant vector, 0 <j> (2.27) 1 = atan2(y, x ) and 0, x, y, 2 are independent variables. Since we have do = [05 0d ] (2.28) a 6 and C^CgC^ 4>kx 6jX e ipkX e -C^CgS^ - C^Sg -S^CgS^, + Cfty S^Sg S^S^ S^CgC^ + £ -SgC^ where = cos<j>, = sin<j) SgS^ Cg etc., we obtain ' C<j>CgC^ - s^s^ < S^CgC^ + c^s^ =m -sgc^ s and (2.29) - - m-i m (2.30) 2 3 'm-ia + d c^sg 5 5 in a + d s sg . ma + dc 2 5 3 5 5 (j> 5 (2.31) + 9 L»3j cp = atan2(y, x) (2.32) Now we have six unknowns (x,y,z,(/>,0,ip) and six equations (only two of (2.30) are independent). The procedure to solve these equations is given by the following steps: Chapter 2: Resolved Motion Step (A) If s ± 0, from (2.31), we have that e sin<f> p - m a -y y tancp = -= = — coscp pi — m^as — x x 2 2 5 Solving for y/x, we get y_ _ p - m a 2 x 2 5 P\ — mifl5 thus we have <f> — atan2(p — rn a , p\ — m\a$) 2 2 5 or (f> — <f> + 7r If s = 0, from (2.31) we get e ( V = P2 - m a \ x = pi - m ! a 2 5 5 we have the similar result (j> = atan2(p — m a , pi — 2 2 5 We conclude that there are at most two solutions for <j>, that is (f> = atan2(p — m a , p\ — m i as) or <f> — <f> + 7r 2 Step (B) If = 0 then 5 ^ 0, from (2.30), we can get 0 = atan2(—ni3, If 2 + m «0) 2 or 0 = 0 + 7r Chapter 2: Resolved Motion 23 From (2.30) we have y/x = s^/c^ = m = 0 -m /m 1 (2.41) 2 3 From (2.31) we have f m\.a 5 2 h 5 2 + d mim sg + rri\X = m\p\ \ m\a - d mim s + m y = m p 5 g 2 2 (2.42) 2 Note the fact that m\ + m\ + ml = 1 (2.43) Adding the two equations in (2.42), we get (2.44) a - mipi + m p 5 2 2 We conclude that there are infinite number of solutions for 0 if for Sip = 0. The necessary condition = 0 is (2.45) a = m pi + m p 5 a 2 2 and m =0 (2.46) 3 Step (C) From (2.30) we have { = m -SgC^ 3 cgc^ = mic^ + m s / 2 •V = <j> 2 c m Multiplying thefirstequation in (2.30) by -s 9 [CTP = -m s 3 9 ~ < (2.41) > <t> \ s m and the second by eg, we have + c (mic^ + m s^) e 2 thus ^ = atan2(c m - s$mi,-m sg (f> 2 3 + cg^m^ + m ^ ) ) 2 (2.49) Chapter 2: Resolved Motion Step (D) 24 T h e solutions for a;, y, z can be obtained as Pi m a + d^s^so ma + dc P2 2 U>3 (2.50) 5 3 5 5 e F r o m the above procedure, w e can see that there are at most four solutions to this problem, but (2.30) and (2.32) should be checked to eliminate extraneous solutions i f any. N o w that we have (f>, 0, ip, x, y, z, the next step to find the required joint angles is the same as seen i n Approach 1. It is clear that approach 1 is easier than approach 2, and introduces fewer extraneous solutions. 2.3.3 Some Comments on the Inverse Kinematics If we know that the target frame is within the manipulator subspace, namely 4> kx 9 jx d e 5/T> e d i/j kx Vd *d d e 0 (2.51) 1 satisfies the constraint, then the inverse kinematics is straightforward, that is <f> = fa O =0 (2.52) d and X y z ~Xd~ — yd _ <f>*kx e jx Tfi kx e e d e d d (2.53) o . d. Z In practice, it is very difficult, i f at a l l possible, to obtain the desired ^T within the d manipulator subspace without first solving the inverse kinematics. we propose here amount to avoid using this assumption. In fact, the two methods Chapter 3 Force Reflection Two fundamental issues i n force-reflection control of master-slave teleoperator system are stability and transparency. In fact, the stability problem is common for almost any closed-loop system. A n ideal teleoperator system should be "transparent"; i n other words, the operator should feel as i f the task object were being handled directly. Yokokhoji and Yoshikawa [23] define an ideal response o f master-slave system is that the position and force responses o f the master and slave arms are absolutely equal respectively. Similarly, Handlykken suggests that an ideal teleoperator system should be represented by an infinitely stiff and weightless mechanical connection between the end-effector o f the master arm and the slave arm (Figure 3.10). Figure 3.10 Ideal teleoperator system Clearly, this definition only applies to those teleoperator systems i n w h i c h slaves are controlled to follow the motion o f the masters faithfully. In a lot o f applications, the position mapping between the master and slave needs to be scaled either down or up. In some applications, for instance i f the master has limited workspace, the slave is often controlled i n rate mode; that is, the position o f the master is interpreted as a velocity instead o f position command to the slave. What is a more general performance measure for teleoperator system? Generally speaking, the "feel" o f an object can be characterized by the object's smoothness and stiffness (or impedance i n the dynamic case). However when a tool is used, it is the impedance o f the task w h i c h becomes the major perception for the human operator. Naturally, we can define a teleoperator system to be 25 Chapter 3: Force Reflection 26 "transparent" i f the impedance "felt" by the operator, or i n other words transmitted to the operator v i a the master-slave system, equals that o f the task [22]. O f course, i n certain applications, such as microsurgery dealing with human tissue, it would be desirable to have a scaled version o f the task impedance [32]. 3.1 Network Representation of Teleoperator System In network theory, an n-port is characterized by the relationship between effort, / (force, voltage) and flow, v (velocity, current). F o r a linear time-invariant (LTI) lumped one port network, this relationship is specified by its impedance, Z(s), according to Z(s) = F(s) (3.1) V(s) where F(s) and V(s) are the Laplace transforms o f / and v respectively. A L T I lumped two-port network can be represented by its hybrid matrix w h i c h is defined as -V = H(s) 2 Vi F h h\ h\ h n 2 2 2 22 one-port V 2 H(S) two-port Figure 3.11 Network concepts Vi F 2 (3.2) Chapter 3: Force Reflection 27 A suggested network representation of a master and slave telemanipulation system is a twoport connected between two one-ports, the operator and the environment [15,19], see Figure 3.12. vh + Operator Z o n e -P + H(S) Task t two-port o r t one-port Figure 3.12 Network representation of teleoperator system Using the hybrid matrix of the two-port and the task impedance (3.3) v e e we can express the impedance "felt" by the operator as Fh _ h\\{l + h Z ) 22 ~v Zt h - e 1+ = h h Z 21 12 e h Z 22 (3.4) e Thus we have the following necessary and sufficient condition for transparency: /in = h 22 =0 (3.5) hi h \ 2 2 — — 1 Another useful concept in network theory is passivity. In the case of master-slave systems, the condition for the total system to be stable is that the system itself must be passive if the operator and environment can be regarded as passive system [23]. One useful tool in determining passivity of a network is scattering theory . Theorem [15]: A system is passive if and only if the norm of its scattering operator is less than or equal to one, that is 1150)11 = sup \->[S*(jw)S(jw)} < 1 (3.6) Chapter 3: Force Reflection 28 where * denotes complex conjugate transpose and A i ( . ) denotes the square root o f the m a x i m u m eigenvalue. For a two-port network, the scattering operator can be related to its hybrid matrix as follows: 1 0 0 -1 (H(s) -I)(H(s) + (3.7) Anderson etc. [15] apply the scattering theory to the two-port time delayed communication circuit w h i c h has the following hybrid matrix H(s) 0 -e- sT e~ sT 0 (3.8) where T is the time delay i n the communication line. It can be shown that the standard method of communicating forces and velocity between the master and slave i n a teleoperator system leads to a nonpassive system for any time delay. This is the major cause o f instabihty for positionposition architecture. Traditionally, a large amount o f damping is added to the system to deal with this problem. This i n turn degrades the system performance making it feel "sluggish". The authors thus introduce an active control law to make the communication block identical to a two-port lossless transmission line. Passivity is a sufficient, but not necessary condition for stability. Quite often the two-port master-slave subsystem is nonpassive while the entire system is stable. The passivity o f the master-slave system guarantee the stability when connected to any passive load. However this guaranteed stability does not come without price. L a w n and Hannaford [16] reported a 50% increase o f task completion time with the above passivity based methods. 3.2 General Teleoperator Structure In a conventional position control teleoperator system, either the contact force or the slave position is fed back to the master to provide force reflection. The first is called direct force Chapter 3: Force Reflection feedback method, and the second is called coordinating force method. From the network point of view, there are no specific reasons for not using the position and force information bilaterally. The four-channel communication scheme gives us more freedom to achieve the desired hybrid matrix which fulfill the requirement for transparency. Here, we adopt much of the formalism presented in the earlier work by Lawrence [22]. A block diagram of a general teleoperator structure is shown in Figure 3.13. Figure 3.13 General teleoperator structure: After Lawrence, 1992 29 Chapter 3: 30 Force Reflection The symbols are defined as follow: Z-m — Cm Z s MS m B = + K m = Ms s C = B + S JS m K./s S (3.9) Zh = the impedance of the operator's hand = Ms + B h + h K /s h Z = the impedance of the environment e - Ms + B + e where M Kjs e and M are the masses of the master and the slave respectively; C and C are the m s m s transfer functions of the controllers; Assuming the operator's hand is a mass, damper and spring system, M , B , and K are the mass, damper and spring coefficients; M , B , and K are h h h e e e the mass, damper and spring coefficients of the task. We can see the position-position and position-force structure are just two special cases of this general structure. The hybrid matrix of this two-port teleoperator system can be easily derived as + C) + C1C4 D C2(Z + C ) — C4 _ [Zm + C )(Z m , S h2 — 2 1 — s s C (Z ">21 = 3 s + C) m m + C\ ' D C2C3 D where D = Z + C - C3C4 s s (3.11) Chapter 3: Force Reflection 31 The transmitted impedance felt by the operator can be derived in terms of the block transfer functions as _ [{Z m + C )(Z m + C ) + CiC ] + Z (Z s s 4 e + C m + m dC ) 2 (z + c - C3C4) + z (i - c c ) 1 s s 2 e - Xi 3 } Z can be thought as the measure of transparency. t In linear control theory, stability of a closed-loop system can be checked by applying Nyquist criterion to its loop transfer function LG Thus LG can be regarded as the measure = G{s)H(s). of stability. Assuming the task is just a passive impedance and reorganizing the block diagram of the general teleoperator system, we have the following for stability analysis. 1 Zs 1~ 3 h C C Z 2m + C m + C s 1~ °3 h C +Z. Z Zm+Cm+Z. Figure 3.14 Closed-loop stability analysis Assume that the individual master and slave control system are stable. Ci, C , C3 and C\ are 2 user specified transfer functions which should also be designed to be stable. In order to ensure stability of the entire system, the Nyquist plot of the inner loop transfer function LG\ and the outer loop transfer function LG 2 LGi C {C\ (Z m J^Q _ Z (Z e {Z m - ZhC ) 4 = + C m + C m + C m m s + Z (l h + Z )(Z h 3 + Zh)(Z s +1- C ) r..\ s - CC) 2 3 + C\C ) + C ) - C (Z C-i s 4 h can not encircle the critical point ( - 1 + jO) on the Nyquist plot. 2 - Ci) (3.13) Chapter 3: Force Reflection 32 3.3 Force-reflecting Control 3.3.1 Coordinating Force The position error between the desired and actual machine positions is usually proportional to the contact force; therefore it is often used for force reflection instead of direct force feedback. Other reasons for its popularity are: position sensing is much easier and less expensive to implement than direct force measuring, and secondly coordinating force (position error) method can be shown to be passive under position control. A block diagram of a position-position structure is shown in Figure 3.15. Operator Figure 3.15 Position-position structure If we choose C = C , the position-position structure can be reformulated using the network m s Chapter 3: Force Reflection concept, see Figure 3.16. 33 It is known that any circuit network made up o f strictly passive •:6 Figure 3.16 Network representation of position-position structure components is itself passive, thus the system can not go unstable. The mechanical equivalent o f the above two-port teleoperator system is shown i n Figure 3.17. Operator Master-slave Task K HAAAAM —V\AAV— H H Me e FVWWH-8- Figure 3.17 Mechanical equivalence of position-position structure It is quite often that the slave is controlled by the master under rate mode, that is the salve velocity is proportional to the master position. F o r position servo, this is realized by adding an integrator to the position forward channel, that is Ci = C s Ks v where K v is the velocity gain. B s + Ks v (3.14) Chapter 3: Force Reflection 34 The coordinating force method can be easily be adapted to rate control. A block diagram of such a rate-position structure is shown i n Figure 3.18. However, a " s t i f f controller (high v d KyS Figure 3.18 Rate-position structure position gain) can make the position error smaller than noise. Therefore it is not always possible to implement this method i n practice. F o r instance, the position gain is so high for hydraulic mobile machines such as excavators and feller-bunchers that there is virtually no position error for feedback considering the noise. 3.3.2 Force Feedback It is well known that direct force feedback with position control can cause instabihty. Parker [3] pointed out that stability problem is even more serious when direct force feedback is used Chapter 3: Force Reflection 35 for rate control. For general position control with direct force feedback, we have the following block transfer functions (see Figure 3.19) K p C = Kj ( -!5) 3 2 C z where K , p Kj = C = 0 4 are the position gain and force feedback gain respectively. The inner loop does not appear i n this structure. The outer loop transfer function becomes Z (^Z & 2 (Z m s + C + Zh + m + C ){Z s m ^C^j + Cm + Zh) Chapter 3: Force Reflection 36 The corresponding characteristic equation is ^ C + Zfi + 7 ? ^ s ) Z (^Z + e m m (Z + C )(Z + C + Z ) s s m m k which can be rewritten as T^-C Z s e ^ (Z + C + Z )(Z + C + Zh) ' s s e m ^ m For stability analysis, it is equivalent to use the following loop transfer function: LG ^ (Z + C + Z )(Z + c + z ) r s s e m m 3 jo, h For position control with direct force feedback, stability can be achieved b y reducing the loop gain J^J, but at the cost o f reduced sensitivity. For general rate control with direct force feedback, we have the following block transfer functions Ks v C = K 2 (3-20) } C =C = 3 where K , v 4 0 Kf are the velocity and force feedback gain respectively. Compared with position control, the difference is that the position gain K is replaced b y K s. p v F o r stability analysis, we have the following loop transfer function: (Z + Cs + Z )(Z s e m + C m + Zh) It is well known from linear system theory, that the addition o f pole at s = 0 to the transfer function has an adverse effect on the system stability. In general, the result is to push the original root l o c i toward the right-half plane. O n the Nyquist plot, adding a pole at s = 0 causes the original Nyquist locus o f G(jw)H(jw) to be rotated b y - 9 0 degree making it easy for it to cross Chapter 3: Force Reflection 37 the critical point (-1,0) on the complex plane. Therefore rate control with direct force feedback tends to be unstable even without time-delay in the system. Therefore it is not trivial to develop a scheme in which contact information is transmitted to the operator while keeping the system stable under rate control. As we pointed out before, the coordinating force method may be one option. However, as we discussed before, it might not be possible in some applications where there are almost no position errors at all. 3.4 Transparency 3.4.1 Achieving Transparency Under Position Control As Lawrence [22] pointed out, the position-position structure does not provide transparency, nor does the position-force structure if the dynamics of the system can not be neglected. It has been suggested by authors such as Salcudean [14], Lawrence [22] and Yokokohji [23] that operator hand applied force should be fed forward to achieve transparency. Salcudean and Wong applied this approach to a master-slave teleoperator system which consists of two identical six DOF magnetically levitated wrist. Yokokohji and Yoshikawa reported similar work on a single degree of freedom system. It is clear that from the structure of the transmitted impedance Z = t [(Z m + C )(Z m s (Z s + C) a + C S + CiC ] + Z (Z + C C3C4) + Z ( l - CC) 4 e e m m 2 + CiC ) 2 (3.22) 3 that in order to make system fully transparent, i.e., Z = Z for any Z t e e (3.23) Chapter 3: Force Reflection 38 the following conditions must be satisfied: (Z m + C.) + C 1 C 4 = 0 + C )(Z m s 1 - cc = 0 Z m + C m 2 3 = Z + C\C 2 s (3.24) + C s — C3C4 For position control, if we set Ci — z + C s s (3.25) Solving the above system of equations, we have two possible solutions = 1 C 2 C = 3 (3.26) 1 — —(Z C4 m + Cm) and C 2 Zm Z Z Zm s C = 3 C4 = s —{Z m ~\~ C + C -\- C m s (3.27) s ~\~ C m + Cm) where the second solution results in Z = \, and therefore it should be eliminated. e The hybrid matrix for this "transparent" telemanipulator system can be derived as H 0 1 -1 0 (3.28) Remarks: 1. It is interesting to note that the end point impedance of the system viewed from the slave side equals: Chapter 3: Force Reflection 39 w h i c h is the impedance o f the human hand. In robotics control, the goal o f impedance control o f a manipulator is to create a desired impedance at its end-effector. However, i n practice, it is not very clear what the best impedance for the manipulator to contact its tasks is. Experiences show human hands offer an adjustable impedance, w h i c h makes them ideal for manipulating almost any object without encountering stability problem. Thus, from the impedance control point o f view, the above fully transparent teleoperator structure provides a way to reconstruct the human impedance at the manipulator end-effector. 2. W e have F -F h = (Z e + C ){V m m - V) h e (3.30) F - F = (Z + C )(V h e s s - V) e h the position error dynamics can be formed as s(Z m where x h + C m +Z s + C )(x -x ) s h = 0 e (3.31) and x are the positions o f the master and slave respectively. Thus the slave tracks e the master position asymptotically. Therefore we indeed have realized the ideal weighdess infinitely stiff bar asymptotically. A p p l y i n g Scattering Theory, we can compute the scattering matrix o f the two-port as follows S(s) 1 0 0 -1 {H(s)-I)(H(s) + iy 1 0 1 1 0 (3.32) The norm o f this scattering matrix is \\S(jw)\\ = sup \*[S*{jw)S(jw)] = 1 (3.33) Therefore, from the network point o f view, this two-port is passive ("lossless" to be exact). Hence when it is connected to any strictiy passive load, the system stability is ensured. L e t ' s assume Chapter 3: Force Reflection 40 the system is connected to two passive one-ports with impedance Zh (operator's hand) and Z e (payload). The inner and outer loop transfer functions become —(z + C )(Z m LGi (Z m + C m e LG 2 s + C m m + C m s + Zh)(Z + C ) m _ Z (Z = Zh{Z + C — Zh) s m s (3.34) + Z -f C ) s s + Z + Cs) s The corresponding characteristic equations are J^Q _|_ ^ _ Zh{Z m (Z m IQ 2 + C m + C m _Q s s m m s + Zh)(Z + C ) -)- 1 - (^e + Zh)(Z Zh(Z + Z -f C ) + C m s + Z + C) _ Q + Cm + Z s s (3.35) s C) s A n y mass-damper-spring system is stable i f the mass, damping and spring coefficients are a l l positive. Thus, the inner loop and outer loop are stable; therefore, the entire system is stable. The above teleoperator control strategy requires acceleration measurement. Furthermore, it needs accurate knowledge o f the mass o f master and slave i n order to completely compensate for their dynamics. This makes it difficult to implement i n practice. O n the other hand, complete transparency might not be desirable since an infinitely stiff and weightless'mechanical bar would drift around i f it is not connected to a load and an operator. A s it is suggested b y Salcudean and Wong [21], a small centering force is desirable to " f i x " the. master and slave system to prevent drifting. This, i n fact, can be interpreted as an intervenient impedance [23] added to the ideal mechanical bar as shown i n Figure 3.20. H W W \ H > Figure 3.20 Intervenient impedance Chapter 3: Force Reflection 41 Consequently, a small force is always needed to move the telemanipulator even without payload. A s we w i l l see that the adding o f intervenient impedance makes it unnecessary to measure accelerations. First let us add some damping and spring term to the master-slave subsystems Z = Ms + B Z = Ms + B m m + Kmc/s mc (3.36) s s + sc K /s sc and set the block transfer function as Ci = C s C =C =1 2 (3.37) 3 C4 = —C Thus the corresponding transmitted impedance becomes m + C ) + C1C4] + Z {Z [(Zm + C )(Z m s a e (Z + C -C C ) Z t S S s s 3 4 ~f Z Z ) _ [ZmC + Z C m m e e (Z + C + s + m CiC ) Z (l-C C ) 2 -\~ Z {Zm ~h C s s + C m + m 3 + C) 2 (3.38) s C) m If we have identical master and slave subsystem, that is Z m = Z , the transmitted impedance s becomes (3.39) Z =Z +Z t m e Thus the operator indeed feels the object's impedance and an intervenient impedance as we expected. The hybrid matrix for this telemanipulator structure becomes H = Z m .- 1 1 (3.40) 0. The end point impedance o f the system viewed from the slave side equals: Z d = -y en = Z m + Z h (3.41) Chapter 3: 42 Force Reflection which is the impedance of the human hand plus an intervenient impedance. Since we have F -F h = ZV e m + C (V h m - h y) e (3.42) F h - F = ZV e s + C (V e s - e V) h In case of identical master and slave, that is Z = Z , the position error dynamics can be m s formed as s(Z m + C + C )(x m s -x ) h (3.43) = 0 e thus the slave tracks the master's position asymptotically. Therefore, we indeed have asymptotically realized the ideal weightless infinitely stiff bar with an intervenient impedance. The stability can be checked as before, starting from inner loop transfer function, LG\ = ^ 44) -Cm{C - Zh) + C + Z )(Z + C ) s (Z m m h s s The characteristic equation is J^Q . | _ ] _ _ ( Z + C + C )(Zh + Z ) s S (Z m + C m M m _ Q (3.45) + Zh)(Z + C ) s S Clearly the inner loop is stable. For the outer loop Q L 2 Z (Z + C + C) + C + Zh) + C (Zh — C ) e (Z + C )(Z s s m m m ^ s m m s Again if we assume the master and slave subsystems are identical Z = Z , then m J^Q _|_]_— (Zfe + Z m (Z m Therefore, the outer loop is stable. + Z )(Z e m + Cm + C ) _ Q s + Zh)(Z + C + s s C) m s (3.47) Chapter 3: Force Reflection 43 However, the above stability analysis is based on the assumption that the master subsystem is exactly the same as the slave subsystem, and the force feedback and feed-forward is accurate. Salcudean [14] reported an error o f only 5% i n the force feed forward could drive the system unstable. In addition, as we discussed before, time delay is always a destabilizing factor i n the system. The robust implementation of above scheme needs further study. 3.4.2 Achieving Transparency Under Rate Control The position-force structure does not provide transparency nor does the rate-force structure. L e t ' s have a look at what the operator feels i f the slave is controlled i n rate mode and direct force feedback is used. The block transfer functions for rate-force control are c = ^- = x Bs + Ks Ks v c where K , Kj v = c 3 ^ v = 0 4 are the velocity gain and force feedback gain respectively. The transmitted impedance becomes _ (Zm + C )(Z m + C ) + Z \[Z s s e Z + C + s s +C m m z e + ^ Therefore it is difficult to say what the operator could feel from this transmitted impedance. F o r large Z e , the impedance can be approximated as Z ^ Zm + Cm + ^p^ Rs 1 t (3.50) v For small Z, e Z ^ t Z m + Cm (3.51) A t these extremes, the operator could only feel some dynamics o f the master-slave system; it has nothing to do with the dynamics of the task. Chapter 3: Force Reflection 44 A s we discussed before, for position control, the bilateral communication o f force information is important to achieve transparency. Here we can extend it to rate control. In position control, a coordinating force is generated by the error between the desired and actual slave position according to Fco. — C s(Xh m where C m = B m + — Xy (3.52) e W e can apply the same idea to rate control. However the coordinating force under rate control takes the form o f (see Figure 3.18) Fco. — CS m [ -rr^ X (3.53) t Therefore it is natural to replace the block transfer function C m structure with i n the general teleoperator as shown i n Figure 3.21. Operator Figure 3.21 Achieving transparency under rate control Chapter 3: Force Reflection 45 The transmitted impedance becomes ( ™ + fe) (Zs + C ) + C l C ] + Z (Z s (Z 4 + C - s e C3C4) + Z (l S e + C1C2) + Z m - CC) 2 (3.54) 3 In order to achieve complete transparency, the following constraints must be satisfied: (z + ^yz m + c) + cc = o s s 1- c c 2 Z m Cn + ~rj V C\ C 2 1 4 (3.55) = 0 3 — Z + C — C3C4 7^ 0 s s For rate control, we set Z + C s Ci (3.56) s K„s After a few algebraic manipulations, we have the following solutions = Ks C 2 C v 1 = 3 (3.57) C4 = -K s \ Z v + m Cn and Z 2 + m -K„s C Z + C Z + C s 1 C3 s — Ii s ( Z C v 4 where the second solution results i n Z e Unfortunately, the control law C s s 4 = (3.58) -f m therefore it should be eliminated. = -K s(^Z v + jf^) m requires the measure o f jerk which is impossible i n practice. Instead, we can use the following simplified control law: Ks v C 2 C CA = Ks v 1 — —Cn (3.59) Chapter 3: Force Reflection 46 The corresponding impedance felt by the operator is as follows (z c. + Zfe + z z ) m m + z (z s e (3. + Cs m + c + f^) s fe) + If we have identical master and slave subsystem, that i s " Z m (3.60) = Z , the transmitted impedance s becomes Zt — Z (3.61) +Z m e Thus we have the same impedance as for position control, w h i c h can be interpreted as the master impedance plus the task impedance. The teleoperator system is transparent. The hybrid matrix o f the master-slave system can be computed as KyS H = (3.62) 0 L K v The transfer function between the master and slave velocities can be derived. Since (3.63) -V e hence Vh (3.64) Ks v therefore the slave i s indeed under rate control. The stability can be checked as before, using the loop transfer functions, _n (MM ^ \K s ZjA K s) m v z [z m + Ts + e LG 2 (z + c )(z s s m +c (3.65) v (Zm + Jt; + h)(Zs + Cs) z +C ^ s z )+c (£- -fe) h m s (3.66) 47 Chapter 3: Force Reflection Under the assumption the master and slave are identical Z — Z , we have the following m s characteristic equations, (Z m LG 1 + z )(z h + 1 = -. {Z m h +l =± (Z m *-L s s s - m j-L + Z )[Z h + Cs + s (3.67) + C) + C ) (Zh + Z Z 2 s r + j ^ + Z )(Z (m + LG + c + fc) s —^ + Z) e —r— (3.68) fe) In order to guarantee stability, we must choose a control law such that the zeros of Z + %P + C, = 0 (3.69) L m KyS fall in the left half of the complex plane. Compared with the position control, we have an additional feedback subsystem in this rate control structure which is no longer guaranteed to be stable. In other words, we also have to make the zeros of the following characteristic equations to be in the left half of the complex plane, Z rn +^ +Z =0 L (3.70) h Zn + ^ - O - (3.71) Ks v which are related to the master subsystem. Equation 3.71 can be written as KM s 3 v m + B Ks 2 mc v + (K K v mc + B )s + K m m = 0 . (3.72) It is known that in order for a polynomial equation to have no roots with positive real part, it is necessary that 1. All the coefficients of the polynomial have the same sign 2. None of the coefficients vanishes. Chapter 3: Force Reflection 48 Thus for stability reason, it is important to have B ^ 0; otherwise, the system can not be stable. mc During rate control, the position correspondence between the master and slave is lost. Therefore the increase o f spring force w i l l not be felt by the operator i f he can only feel the impedance o f the task. This can cause machine tip over (for instance, an excavator) or damage to the tool. F o r these applications, it is more important to transmit the contact force to the operator than to feel the task dynamics. However, as we discussed before, stability is still a problem. 3.4.3 Achieving Transparency Via Impedance Identification A s we discussed before, it is not necessary to cancel the dynamics o f the master-slave system completely. Another interesting structure w h i c h could be used to make the system transparent is v i a parameter identification [19]. Assume the task is merely a linear mass-spring-damper, that is Z = M s + B + -is. e e (3.73) e A regression model can be formulated as f = [x e where ip = [x x x ] T e e e e x x ][M e e and 9 = [M B e B e K] T e e = ye T (3.74) K] T e e On-line recursive least-squares estimation can be used to identify the task impedance parameters. In addition, the resetting and forgetting factor technique can be employed to deal with a changing environment. The estimated task impedance Z then can be implemented i n the master e subsystem control as F = ZV h where Z m = Ms m + B mc + m h + ZV e h (3.75) Chapter 3: Force Reflection 49 Thus the impedance felt by the operator becomes Z = Z t m + Z e (3.76) w h i c h makes the system transparent. A block diagram o f the rate control with impedance identification scheme is shown i n Figure 3.22. Figure 3.22 Achieving transparency via parameter identification Chapter 4 Stiffness Feedback 4.1 Principle It has been shown that direct force feedback under rate control can easily introduce instabihty. Parker [3] proposed a method in which the contact force is used to adjust the stiffness of the joystick instead of being applied directiy to the operator's hand. A comparison of direct force feedback and stiffness feedback is shown in Figure 4.23. z„ Vd ( Rate-force Structure ) ( Rate-stiffness Structure) Figure 4.23 Comparison of two rate control structures The stiffness of the joystick can be adjusted according to K m = K nom + f K sgn(x ) e r h (4.1) K m where K nom is bounded by [K , K ] min max is a constant gain when there is no contact, K is the ratio of stiffness and contact T force, see Figure 4.24. 50 Chapter 4: Stiffness Feedback 51 ( X > 0) h (x < 0) h — : F_max • fe ' Figure 4.24 Stiffness adjustment scheme W i t h i n the linear area, the operator hand applied force equals to fh = M x m h + Bx m h + Kx m h (4.2) = Mx m h + Bx m h + K x nom h + f K \x \ e r h Thus, within this linear area, this stiffness adjustment scheme can be thought o f as a special case of direct force feedback whose feedback gain is linearly proportional to the master position. In the case where the operator holds the master position constant, this gain is fixed; it is the same as direct force feedback. Clearly, this stiffness adjustment scheme does deliver the contact force information to the operator to a certain extent. Numerical simulation and machine experiments show rate control with stiffness adjustment can easily make the system stable. However, a stability analysis is difficult to carry out for such a highly nonlinear system. The nonlinearity comes not only from the stiffness adjusting mechanism but also from the master position deadband since a position deadband is always needed for rate control. Stiffness feedback can be used for position control as well. Chapter 4: Stiffness Feedback 52 4.2 Stability Rate control with stiffness adjustment makes the system highly nonlinear. Liapunov stability analysis seems elusive for such a system [3]. However, careful examination o f this control structure reveals that the system can actually be divided into two separate second-order subsystems: a master system with a varying spring and a slave system w h i c h tracks a desired path set b y the master (see Figure 4.25). W e assume the operator's hand is modeled as a constant mass, damper Master Subsystem Stable Slave Subsytem Figure 4.25 Stiffness feedback control block diagram and spring system. Since the slave subsystem is stable, one only needs to be concerned about the stability o f the master system (mass-damper-spring), that is Mx m where K (f ) m e h + Bx m h + K (f )x m e h = f, B h m is only a function o f the contact force f e > 0, K (f ) m e > 0 and bounded as K m a x (4.3) > K (f ) m e > It is helpful to study the following time-varying system w h i c h is quite similar to our problem x + Bx + K(t)x = Q, where for simplicity we assume unit mass. B>0,K(t)>0 (4.4) Chapter 4: Stiffness Feedback 53 The behavior o f a linear time^varying ( L T V ) system can be quite different from a L T I system. We know that a mass-damper-spring system with constant positive coefficients x + Bx + Kx 0 = (B > 0, K > 0) is always asymptotically stable. However as we w i l l see, its time-varying counterpart x + B(t)x + K(t)x = 0 (B(t) > 0, K(t) > 0) might be unstable. Special care should be taken when dealing with L T V system. Before we start, let us have a taste o f such a difference. Consider the following second-order system x + B(t)x + Kx = 0, B(t) > 0, K > 0 (4.5) Instead o f having a time-varying spring coefficient, the above system has a time-varying damping coefficient. One might conclude that the equilibrium point (0, 0) is asymptotically stable i f the damping coefficient B(t) remains larger than a strictiy positive constant, w h i c h implies constant energy dissipation by the damper. Surprisingly this is not always true! L e t ' s have a look at the following example: x+ (2 + e )x t + Kx = 0 (4.6) with initial condition z ( 0 ) = 2, x(0) = - 1 , the above system has solution x(t) w h i c h goes to x(oo) = 1+ e _ t = 1 instead. In fact, additional constraints are needed i n order to make the equihbrium o f above system asymptotic stable. It can be shown [Slotine and L i ] that one sufficient condition is to satisfy the following B(t) > a > 0 B(t) <b<K (4.7) where a, b are positive numbers. N o w we come back to our problem, that is a second-order system w i t h time-varying spring coefficient x + Bx + K(t)x = 0, B > 0, K(t) > 0 (4.8) Chapter 4: Stiffness Feedback 54 Without any additional constrains, it is easy to show that the system can be driven unstable by changing K(t) even if it is bounded. For instance, a slightly damped mass-damper-spring system goes unstable if we set the spring constant to a large value whenever the mass moves towards the equihbrium point and a small value whenever the mass moves away from it. However, as we will see later, the asymptotic stability can be guaranteed if certain conditions are satisfied. First, we introduce some stability concepts from general LTV system theory. Definition: A linear dynamical equation E: X = A(t)X + D(t)U (4.9) Y = C(t)X is said to be totally stable, or T-stable for short, if and only if for any initial state and for any bounded input, the output as well as all the state are bounded (refer to [33]). It is clear total stability implies BIBO (bounded input/bounded output) stability. The following theorem states that with some conditions on D(t) and C(t), uniformly asymptotic stability implies total stability. Theorem 4-1: Consider the dynamical equation E : X = A(t)X + D(t)U (4.10) Y = C(t)X if the matrix D(t) and C(t) are bounded on ( - o o , oo ), then the uniformly asymptotic stability of the zero state implies the system is T-stable (refer to [33]). It is clear that for a LTI system, if its equUibrium point is asymptotically stable, then the system is T-stable. Next we will prove the following useful theorem, which provides a sufficient condition for asymptotic stability for the master subsystem. Chapter 4: Stiffness Feedback 55 Theorem 4-2: Consider the second-order time-varying mass-damper-spring system x + Bx + K[t)x = 0, Suppose K(t) is bounded by K m a x B > 0, K(t) > 0 > K{t) > K B m i n - AK 2 (4.11) > 0 and > 0 max (4.12) Under these conditions, the system is uniformly asymptotically stable. Further the trajectory o n phrase plan can cross the x axis at most once. Remarks: 1. Since K{t) is bounded by K B 2 m a x > K(t) > K > 0, condition B - 4K 2 m i n max > 0 implies - 4K(t) > 0 for a l l time. Thus; at each time instant, the system can be thought as a overdamped, time-invariant, mass-damper-spring system. 2. This theorem makes no assumption o n K(t). A p p l y i n g Theorem 4 - 1 and Theorem 4 - 2 to the master-slave system, we have the following: Corollary Consider the master subsystem Mx m where K (f ) m Ki m n e h + Bx m h + K (f )x m e = f, h B> 0, K (f ) h m is only a function o f the contact force f e e > 0 and bounded b y K m a x (4.13) > K (f ) m e > > 0. Suppose B 2 m - 4M K m max > 0 (4.14) Under these conditions, the equihbrium point is uniformly asymptotically stable. Further this system is T-stable; that is, any bounded input fh and arbitrary initial condition result i n bounded Chapter 4: Stiffness Feedback 56 states, and bounded output i f the output takes the form o f Y = C(t)[x x] T h h where C(t) is bounded. It is interesting to note that the rate o f change o f K (f ) m stability as long as the its upper bound satisfies B m e does not affect the system asymptotic - 4M K m max > 0. In reality, the task impedance can be strongly nonlinear, e.g. at the transition between contact and free space motion. A quick contact with a hard surface could make the change o f K (f ) m e very rapid, even nonsmooth. Additional nonlinearity comes from the master's position deadband. Nevertheless, stiffness feedback under the condition B m - 4M K m max > 0 can guarantee asymptotic stability no matter i f the payload is passive or active, linear or nonlinear; no matter i f there is a nonlinearity such as the master position deadband; no matter i f there is a time delay i n the communication line. Proof of Theorem 4-2 <Part 1> First, we show that starting from any arbitrary initial state o n the phase plane, after certain time, its trajectory must reach the x axis. Assume the initial state is i n quadrant I V . Since i n this quadrant the velocity is negative, x w i l l decrease all the time. After a certain time, there are at most two possibilities as shown i n Figure 4.26. Chapter 4: Stiffness Feedback 57 dt I I X \ I 3 2 \ 1 v. \ E Figure 4.26 Trajectory in phase plane 1. Trajectory reaches the x axis with x > 0. 2. Trajectory reaches the x axis with x < 0. We only need consider the second case x < 0 and x = 0. It is clear the trajectory w i l l go into quadrant 3. It can be treated later. Assume that the initial state (x , 0 x) 0 is i n quadrant III. Since i n this quadrant, velocity is still negative, x w i l l decrease. Secondly, since we have x = —Bx — K(t)x > K i \xo\ m n > 0 in this quadrant, x w i l l increase. Therefore, the trajectory reaches the x axis with x < 0 i n finite time, as shown i n Figure 4.27. Chapter 4: Stiffness Feedback 58 dX dt I I X I I Figure 4.27 Trajectory in phase plane The same analysis applies to the other two quadrants due to symmetry. <Part 2> Next we show that any trajectory starting from x axis (x — 0) goes to the origin asymptotically without crossing the x axis. We only need to consider the right half of the x axis due to symmetry. Consider the time-invariant overdamped mass-damper-spring system x + Bx + Kx - 0, B > 0, K >0 (4.15) where B - AK > 0 (4.16) We have the following two observations: (Observation 1): Any trajectory starting from (x = 0) goes to the origin asymptotically without crossing the x axis. Chapter 4: Stiffness Feedback 59 Proof: The zero-input response of the system can be derived as follows: x(t) = ae Xlt + be* * 2 (4.17) x(t) = a A i e where A i , A l < + 6A e ^ A 2 A are the roots of the characteristic equation, that is 2 Ai = VB -4K) 2 (4.18) a, b are two constants depending on initial state (x , x) 0 0 _ x (Xi + B) + x 0 0 Ai — A x ( A + B) + x (4.19) 2 0 b = 2 Q Ai — A 2 It is clear the system is asymptotically stable asi—>-oo, a;,x->0. B y way of contradiction, we assume that the trajectory reaches the x axis at finite time t = t, c that is x(t ) = ae ° Xlt c B y assumption we have x Q ^0, x 0 = Since we know A - A = ^B 2 a 2 + be ° = 0 X2t (4.20) = 0, thus (Ai-A )t 2 c b Ai + B a A + B (4.21) 2 - AK > 0, the above equation implies A 4- B Y—z > 1 A + n or Ai < A 2 (4.22) 2 w h i c h contradicts the fact that A i > A . Therefore, it is not possible for any trajectory to reach 2 the x axis i n finite time. (Observation 2): A t any state i n quadrant I V o f the phase plane, the trajectory corresponding to the system with higher stiffness has higher tangent. Chapter 4: Stiffness Feedback 60 Proof: The tangent o f a trajectory at state ( x , x) for the time-invariant mass-damper-spring system can be derived as , „. dx x —Bx — Kx Tangent(K) = — = - = dx x x (4.23) Thus we have Tangent(K + A) - Tangent(K) -Ax > 0 (4.24) i n quadrant I V . F r o m the above two observations, we can sketch the trajectories w h i c h correspond to the systems with constant stiffness K m a x and i f , - respectively when starting from the same state P m n on positive x axis, see Figure 4.28. The two trajectories can not intersect each either. Otherwise it contradicts observation 2. Qudrant four _i i_ Figure 4.28 Trajectory in phase plane N o w consider the second-order time-varying mass-damper-spring system x + Bx + K(t)x = 0, B>0,K(t)>0 (4.25) Chapter 4: Stiffness Feedback 61 w h i c h starts from the same initial state P. First, the trajectory o f this time-variant system must be bounded by curve 1 and curve 2; otherwise, it w i l l contradict Observation 2, see Figure 4.29. Second, since the velocity i n quadrant I V is negative, thus, any trajectory w i l l always move towards to the left. Therefore, we conclude that as t —»• oo, the trajectory w i l l approach the origin. Therefore, the system is globally asymptotically stable. of Theorem 4-2. Figure 4.29 Trajectory in phase plane This completes the proof Chapter 5 System Overview The focus o f our research is to implement real-time, force reflecting, resolved motion control for typical hydraulic mobile machines, an excavator and a feller-buncher. Previous work by Parker [3] employed the same approach to control a three degree-of-freedom ( D O F ) log-loader. Here, the results were extended to the manipulators with four and five D O F to further investigate the application of resolved motion control (rate v.s. position) and force reflecting control (direct force feedback v.s. stiffness feedback) for the control o f the hydraulic mobile machines. Master Slave Wrist Machine 1t 1t It 1t AD/DA AD/DA Desired joint angles DSP DSP ^ 1 Hydraulic pressures and Actual joint angles Figure 5.30 Overall system configuration The complete teleoperator system can be divided into two parts, the master subsystem and the slave subsystem, as shown i n Figure 5.30. A six D O F magnetically levitated joystick as a master is controlled by a local P I D controller with programmable center o f compliance. 62 The Chapter 5: System Overview 63 Joystick position is interpreted as a command to generate a desired machine endpoint position. T h r o u g h an inverse kinematics transformation, the desired joint angles are computed and sent to the hydraulic machine controller for set-point control. The hydraulic cylinder pressures and actual joint angles are measured to calculate the endpoint force through inverse Jacobian transformation. This force is reflected to the master side either directly applied to the operator's hand or used to adjust the stiffness of the joystick. 5.1 Master — Maglev Joystick The master plays a critical role i n any master-slave teleoperator system. It serves as a man-machine interface, therefore has great influence on the overall system performance. Various kinds o f joysticks can be used as the input devices. Passive joysticks are popular due to the fact that they do not require actuators w h i c h makes them less expensive and less complicated to build and maintain. After force-reflection was introduced i n teleoperation, great efforts have been spent i n building ideal active joysticks w h i c h are capable o f reproducing the desired force to the operator's hand while providing enough D O F for operation. M o s t designs use either hydraulic or electric servomotors. The nature of these actuators make it impossible to overcome problems such as friction and backlash w h i c h greafly affect the performance o f the joystick. The six D O F magnetically levitated joystick (Maglev Joystick) w h i c h is based on electrodynamic actuation, originally designed for coarse-fine manipulation, proves to be ideal to be used as a master for force-reflecting teleoperator system. Its novel design eliminate all mechanical linkages, and therefore friction and backlash. 64 Chapter 5: System Overview 5.1.1 System Configuration A complete Maglev Joystick system [34] can be divided into four parts: a magnetically levitated joystick which consists of a fixed "stator" and a moving "flotor", a signal conditioning box, a digital controller with an A/D and D/A interface and a current driver, as shown in Figure 5.31. The Maglev Joystick Assembly is shown in Figure 5.32. D/A current driver coils LED's flotor permanent magnets PSD's stator digital controller A/D I signal conditioning box Figure 5.31 Maglev Joystick system configuration Chapter 5: System Overview 65 Flotor LED Mounting Colums Vertical Magnet Gap PSD Mounts Vertical Coil Horizontal Coil Flotor Base Support Post Horizontal Magnets Stator Base Figure 5.32 Maglev Joystick assembly 3 5.1.2 Sensor and actuator The internal position measuring o f the M a g l e v Joystick is accomplished by optical sensors w h i c h consist o f light-emitting diodes ( L E D ) and position-sensing devices (PSD). Three L E D ' s mounted at the center of the flotor project narrow, coplanar, radial light beams 120° apart respectively onto the surfaces of the P S D ' s w h i c h are located at 120° intervals on the stator. Typical resolutions for such system are approximately 1 \im for translation and 0.001° for rotation. Actuation for the M a g l e v Joystick is based on an electrodynamic, moving coil approach. S i x coils and six pairs of permanent magnets mounted on flotor and stator respectively comprise six actuators w h i c h generate Lorentz forces to " f l y " the flotor. The joystick workspace is limited by the size of the magnetic gap. A large workspace requires large magnets and high electric currents Joystick design by S. E . Salcudean, AutoCAD drawing by C. T. Chen Chapter 5: System Overview 66 which in turn increase the overall size of the wrist and could cause heating problems. The UBC Maglev Joystick has approximately + 5 mm translational motion and + 7° rotational motion from the nominal center. Two coordinate systems are defined: the stator coordinate frame, attached to the joystick stator, and theflotorcoordinate frame, attached to the joystick flotor. The origin of the flotor coordinate frame is located at the intersection of the projection axes of the LED's. Due to the small motion range, the relation between the six coil currents and force-torque acting on the rigid flotor can be approximately expressed as linear transformation = MI where F f and r F (5.1) are the force and torque vector with respect to and expressed in the flotor coordinate frame, I = [Ii 7 h h h hY * 2 s a vector of six coils currents and M is a well- conditioned 6 x 6 constant matrix. 5.1.3 Control about the Remote Center of Compliance The control of the Maglev Joystick can be treated as the control of a free rigid body in space. By means of Remote Center of Compliance (RCC) control, we gain the ability to set the center of compliance of the rigid body anywhere in space. In [34], the center of compliance is assumed to be at the gravity center of the flotor. For the master-slave application, it is desirable to define the center of compliance of the master to be at the center of the handle. In addition to the stator andflotorcoordinate frames, we need to define two extra ones on theflotor:one called the tool frame which is at the remote center of compliance, the other called the gravity center frame which is at the center of gravity. They have the same orientation as the flotor frame, see Figure 5.33. Chapter 5: System Overview Figure 5.33 Frame assignment We define the following symbols: = the flotor's angular velocity i n gravity frame J = flotor's inertia matrix i n gravity frame T = torque acting on the flotor with respect to gravity frame T = torque acting on the flotor with respect to flotor frame f = force acting on the flotor i n flotor frame f = force acting on the flotor i n stator frame G G F F r = G T r T r F G coordinates of point T i n gravity frame — coordinates o f point T i n stator frame = coordinates of point G i n flotor frame Q = rotation matrix from stator frame to flotor frame m = mass o f the flotor g = gravity constant Chapter 5: System Overview For any vector x = [xi x 68 x] a skew-symmetric matrix xx is defined as follows: T 2 3 A XX = 0 -X3 X3 0 -X\ X\ 0 -«2 x-i (5.4) The rigid body dynamics can be described by two differential equations G,. r = -Gj-l(G GjG^ Gj-l 0JX l = ^-+g = T G + T + Q oj( cjX r ) G G + G T m (5.5) Q r G J " ( u x J u) - Q r G G 1 T G G G G T J~ r l G where the first is for rotation, and the second is for translation. Using Euler quaternions, the rotation can be described by a vector p, which is defined by P = [Po (5.6) = [cos{4>l2) sin(4>l2)s ] T where s is the axis of rotation (||s|| = 1) and <j> T is the angle of rotation. The relations between the rotation matrix and Euler quaternions can be summarized as follows: Q = ex ((f>sx) = I + 2(3 p X + 2 / 3 X V 2 0 (Q - Q) T 2(1 + trQ) ' 1 2 P + (3 (3 = 1 2 /3 >0 T 0 0 where trQ denotes the trace of Q. Using the Euler quaternion representation and under the assumption of small angles and small angular velocities, the rigid body dynamic equations can be simplified as r T 1 = —f m + g - G r T x G J - 1 G T A = u ( 5 2 ' 8 ) Chapter 5: System Overview 69 Thus the dynamic equation can be easily transformed to decoupled second-order systems and controlled by the following feedback law: «i = Kp(Pd -•/?) - KvP u = K (r 2 where K ,K , P V K V and K V p - r) - K d T V r T are diagonal gain matrixes. The current-force mapping is defined with respect to the flotor coordinate. The following transformation is performed to compute r and F f = F f G Q~ f*f 1 (5.10) F T = F r a X F f + a T Finally, the six coil currents required can be calculated as (5.11) where M 1 is a constant matrix which can be precomputed. Previously, due to lack o f inertial parameters, a simple P I D control with gravity compensation was used. Experiments show the performance o f such a controller degrades when the center o f compliance is far away from the flotor center. Using parameter identification software developed i n [35], the inertia matrix and the center of gravity o f the joystick (with handle attached) were identified. Remote center of compliance control has been successfully implemented. 5.2 Slave — Hydraulic Mobile Machines Two typical hydraulic mobile machines have been used as slaves for the study o f force reflecting teleoperator systems. The C A T 215 excavator is a four D O F manipulator mainly used i n general construction industry while the C A T 325 feller-buncher with five D O F used i n the forestry industry. The machine joint angles are sensed using resolvers and hydraulic pressures are measured v i a pressure transduces. B o t h machines are outfitted with independent joint controllers due to previous work by P. D . Lawrence, D . Chan and S. Bachman at U B C . Chapter 5: System Overview 5.3 Computing System Based on a V M E Bus structure, all the necessary processors, A / D and D / A boards can be hosted i n one cage. The computing system configuration is shown i n Figure 5.34. Figure 5.34 V M E cage configuration One S P A R C I E processor running the real-time multitasking operating system V x W o r k s and one Skybolt board take care o f the computations o f joystick control, slave path planning and force feedback. The Skybolt board w h i c h uses a 40 M H Z i860 processor is a specially designed M a t h accelerator for U n i x hosts. N o w a new device driver makes it possible to directiy interface with a V x W o r k s host to speed up V x W o r k s applications. The Skybolt, approximately 6 times as fast as a S P A R C I E , performs all the real-time computations while the S P A R C I E handles timer interrupts, collecting A / D data and sending D / A data. A shared memory block is defined at the Skybolt side to communicate with V x W o r k s across the V M E Bus. The real-time control runs at 200 H Z . Another S P A R C running U n i x and a T800 Transputer are included i n the same V M E cage for the set-point control of hydraulic machines. The S P A R C / U n i x is the I/O host and the Transputer performs the real-time computations. 70 Chapter 5: System Overview The S P A R C / V x W o r k s board w h i c h is the host for the V M E cage is connected to Ethernet allowing cross-development from a U n i x host — Sun Workstation. This structure makes a userfriendly environment for software development. Useful tools such as Stethoscope and V x g d b can be used for real-time data collection and program debugging. 71 Chapter 6 Machine Experiments The master-slave multi-axes force-reflecting resolved motion control strategy was successfully implemented on a C A T - 2 1 5 excavator and a C A T - 3 2 5 feller-buncher. M a c h i n e experiments were carried out to investigate position versus rate control, stiffness versus force feedback control. 6.1 Endpoint Force Sensing 6.1.1 Modeling Accurate force sensing is crucial for successful implementation o f force feedback. In our experiments, the hydraulic transducers were mounted i n the hoses close to the hydraulic pump. The hydraulic pressures were used to compute the joint torques. F o r a discussion o f other force sensing methods, refer to [3] for details. Previously, Parker [3] used the Newton-Euler method to derive a model for a log-loader w h i c h is three D O F manipulator. During free motion, the manipulator's joint torques are caused by the following factors: gravity of the links and the dynamic forces due to the machine motion. The dynamic equation for a manipulator is: T = D(q)q + C(q,q)q where r = [r r a 2 ... r ] n T + g(q) is the vector o f generalized forces, q = [q\ (6.1) qi ••• q] T n is the vector o f generalized coordinates o f the system. W h e n the manipulator contacts the environment, the dynamic equation should be modified to include the effect o f external force F, that is T = D{q)q + C(q, q)q + g(q) + J (q)F T t where J is the manipulator Jacobian. 72 (6.2) Chapter 6: Machine Experiments 73 Thus in order to have an accurate measure of F, we need to know not only the joint torque T, but also the dynamic model of the manipulator. As discussed in [36], a key feature of rigid robot dynamics is its linearity in the parameters although the dynamic equation itself is highly nonlinear. The dynamic equation of a manipulator can be written in the following format, which is linear in the parameters T = D(q)q + C(q, q)q + g(q) = Y(q, q, q )P (6.3) where Y (q, q, q) is an nx r matrix of known functions of joint variables and P is an r-dimensional vector of parameters which only depends on link mass, moments of inertia, etc. This property of linearity in parameters makes it easy to identify the dynamic model through a parameter estimation technique such as the Least-squares method. It is possible to identify the complete model of a manipulator by using r = D(q)q + C(q, q)q + g{q) = Y(q, q, q )P (6.4) The joint velocities q and accelerations q are required to obtain P. However, in practice, q is not available even if q can be measured, and digital differentiation of q is noisy. Secondly, even for a two link manipulator, the vector of parameters P contains nine components (see Spong [36]). The complete model of a four orfiveDOF manipulator would be much more complicated. Therefore, in our experiments, only the static case is modeled, that is r = g(q) (6.5) and the contact force at end-effector is approximated by F^(j (q))-\r -g( )) T t q (6.6) Chapter 6: Machine Experiments 74 where r is a vector o f the joint torques computed from the hydraulic pressures, g(q) can be t obtained from the measured joint angles and the link parameters. Thanks to the powerful symbolic computation package Maple, a computer program was written to derive the static model of manipulator automatically using the Euler-Lagrangian formulation. The procedure is straightforward: first, the forward kinematics is performed to locate the gravity center o f each link, then the potential energy o f the system is computed symbolically. Finally, the joint torques are derived by differentiating the potential energy, according to (6.7) There are several advantages o f using a computer program to derive the manipulator model: 1. Speed: the M a p l e program can generate a dynamic model within 2-3 seconds. 2. Correctness: once the program is running, by changing the manipulator link parameters, such as link distance, length and twist, we can easily get the result for manipulators with different configurations, and guarantee its correctness. 3. Efficiency: the simple procedure can be applied to manipulators with any number o f degrees of freedom, and is easy to debug. This can save tedious algebraic manipulation when dealing with robot with high D O F . Besides, M a p l e has many easy-to-use functions to either simplify the results or to shape them to the format y o u like. These all can be done within a few seconds. We have also successfully derived the manipulator Jacobian for the excavator and the fellerbuncher symbolically using the M a p l e program. This is very helpful i n real-time applications. Chapter 6: 75 Machine Experiments Static Model of the Excavator " 0 T = Y(q, q, [n r r T ] , 3 0 0 5 34 C23 ^23 C*234 5 34 .C234 5 34 C23 0 <5"3 0 2 2 2 P = [Pi Pi Pz PA P5 Pef, t 2 = q)P 0 C*234 4 0 0 " c s 2 0 0 2 £234 (6.8) 2 0 0 . = sin (02 Static Model of the Feller-buncher " q)P = . where r = [n r 0 0 0 0 0234(75 0 C234 5234 C*234 5234 C*23 C 3 523 C234C5 C234C5 C*234 0 5234 0 0 523 0 0 0 — 523455 r r r ] , P = [ P P P PA PS Pe Pjf, T 2 3 4 5 a 2 3 2 SA 23 0 0 " c s 0 2 0 2 0 0 0 0 . = sin (6 2 p (6.9) + 9 + 9) 3 4 etc. 76 Chapter 6: Machine Experiments Jacobian of the Excavator The nonzero elements of the Jacobian are as follows: J[0][0] =d 2 J[2][0] =— 0-\C 234 ~ a3C23 j \ m ~ O2C2 1.0 = J[0][1] = 4>5234 a — , J[l][l] = 4C 34 a 2 0-3S23 ~ — + 3C23 a + &2S2 ^2^2 J[5][l] = 1.0 J[0][2] = — G'4'5'234 J[l][2] = 4C234 + a (6.10) 3*523 — a A 3C 3 2 J[5][2] = 1.0 7[0][3] = J[l][3] = — a a 4'5'234 4C 34 2 J[5][3] = 1.0 where J is the manipulator's Jacobian with respect to frame Ci, etc. S234 = sin ( 0 + 03 + 2 #4) Chapter 6: Machine Experiments 77 Jacobian of the Feller-buncher The nonzero elements of the Jacobian are as follows: J[0] [0] = — CI5S5 J[2][0] — — a\ — 0.5a C234-5 — 0.5a C2345 5 J[4][0] < — = 1.0 J[0] [1] = — a S2 2 J[l] [1] = 0.5a C J[5][1] = 1.0 J[0][2] = 5 23 ^3^23 — — CI3S23 4_5 — a 5 + 0.5a C 345 + 4<S234 a — 4-5234 — d C 34 5 — 2 5 2 2 a C 2 2 — 0.5a55 34_5 2 — 0.50552345 - d S 34 + «4C*234 + 5 2 dsC 34 — 0.505^234-5 2 J[l] [2] - 0.5a C 34_5 + 0.5a C2345 — aC3 3 2 0.505^2345 ^5^234 + iC234 + 03^*23 — a 5 1.0 J[5][2] J[0][3] 0.2^2 + ^5^234 ~ a4C 234 — 5 = — 04^234 — d^C234 ~ 0.5a55234-5 — 0.5(1552345 J[l] [3] — 0.5a C234_5 + 0-5a C 345 — ^5234 + 4p234 a 5 5 2 J[5] [3] = 1.0 J[0][4) — 0.5as5234-5 - 0.5O5 52345 J[l] [4] - J[2][4] -0.5a C 2 34-5 + 0.5a C2345 5 -a C 5 J[3] [4] - J[4] [4] = C*234 where 5 -5234 is the manipulator's Jacobian with respect to frame C i , J sin (02 + 0 + 0 ), 3 5 4 5 2 3 4-5 = sin (0 + 0 + 0 - 0 ) 2 3 4 5 etc. 5234 = Chapter 6: Machine Experiments 78 6.1.2 Torque Computations The hydraulic cylinder force can be computed from the hydraulic pressures and the cylinder geometry: Fcyl where P , - , P n out = PinAin ~ Pout-^-out (6.12) are the pressures at the piston head side and rod side respectively, A , - , n A out are the areas at the piston head side and rod side. L i n k Name Head A r e a i n Boom 31.80 19.92 Stick 23.75 14.13 Bucket 15.90 9.96 (in ) 2 Rod Area in (in ) 2 Table 6.3 Piston geometry of the excavator L i n k Name Head Diameter i n (mm) R o d Diameter i n (mm) Boom 120.00 85.00 Stick 150.00 105.00 Tdt 150.00 105.00 Head 152.40 63.50 Table 6.4 Piston geometry of the feller-buncher Referring to the excavator and feller-buncher schematics Figure 1.3 and Figure 1.5, there are only two types o f mechanical linkage on these machines: two-bar linkage and four-bar linkage, see below: Chapter 6: Machine Experiments 79 Figure 6.35 Two-bar mechanical linkage Figure 6.36 Four-bar mechanical linkage Joint torques can easily be derived for a two-bar linkage as the following: , , r = where F cyl ,„ , \F \ cyl a&lsin B\ . 1 n (6.13) is the hydrauhc cylinder force, B is the joint angle, a and b are the linkage lengths. For the torque computation of a four-bar linkage, refer to [37] for detail. Chapter 6: Machine Experiments Boom Stick Bucket Two-bar Linkage Two-bar Linkage Four-bar Linkage a = 1999.8 (mm) b = 624.0 (mm) a = 720.2 (mm) b = 2443.6 (mm) rl = 324.9 (mm), r2= 404.9 (mm) r3 = 484.9 (mm), r4 = 548.6 (mm) 7 = 0.0468407 (rad) a = 0.3469905 (rad) L = 1602.7 (mm) Table 6.5 Joint linkage geometry of the excavator Boom Stick Tilt Head Two-bar Linkage Two-bar Linkage Four-bar Linkage Two-bar Linkage a = 2281.6 (mm) b = 911.2(mm) a - 901.2 (mm) b = 2339 (mm) rl = 481.3 (mm) r2 = 563.7 (mm) r3 = 640 (mm) r4 = 685 (mm) 7 = 0.0728 (rad) a = 0.1831 (rad) L = 2316.3 (mm) a = 634.0 (mm) b = 588.0 (mm) Table 6.6 Joint Linkage geometry of the feller-buncher Chapter 6: Machine Experiments 81 6.1.3 Recursive Least-squares Link Parameter Estimation There are several reasons for using a recursive estimation method. First, it is likely that some experimental data are corrupted due to factors such as temporary sensor or transmitter failure, etc. Since the number o f observations is always limited, these outliers can have a strong effect on the estimated parameters. Thus it is important to evaluate the estimated parameter over the full range of observation so that the residuals can be examined to find out any "bad data" w h i c h is responsible for large residual values. In our case, these outiiers can be eliminated, and the estimation procedure repeated. Secondly, from the plots, we can get information such as convergence. W i t h this information, we can get some idea as to what degree o f accuracy the estimated parameters have. In the case o f nonconvergence, we can reexamine the whole procedure including the way the experiments were carried out, find the cause, then take appropriate measures. During the experiments, the hydraulic machines were commanded to move slowly from one configuration to another randomly while the joint angles and hydraulic pressures were recorded. Then, these data were subsampled for off-line estimation. The initial parameters and the covariance matrix were computed from the first 50 sets of data. The recursive Least-squares estimation results for the excavator and the feller-buncher are shown i n Figure 6.37 — Figure 6.44 i n w h i c h we call each set o f data one "sample". Chapter 6: X -r- 1.5 § 0.5h 10 Machine Experiments L e a s t - s q u a r e s P a r a m e t e r Identification -0.5 sample x 10 200 250 L e a s t - s q u a r e s P a r a m e t e r Identification sample Figure 6.37 Excavator link parameter identification 250 Chapter 6: Machine Experiments Figure 6.38 Excavator link parameter identification Chapter 6: Machine Experiments Figure 6.39 Excavator link parameter identification Chapter 6: O Machine Experiments 50 100 sample 150 Figure 6.40 Excavator link parameter identification 200 250 Chapter 6: x Machine Experiments 10 L e a s t - s q u a r e s P a r a m e t e r Identification E to 200 sample 400 L e a s t - s q u a r e s P a r a m e t e r Identification x 10 100 150 200 sample 250 300 Figure 6.41 Feller-buncher link parameter identification 350 400 Chapter 6: Machine Experiments x 10 L e a s t - s q u a r e s P a r a m e t e r Identification 200 sample x 10 400 L e a s t - s q u a r e s P a r a m e t e r Identification 1.8r 1.6r 1.4r -1.2 CD ^5 11 E 0.8|( I 0.6H 0.4H 0.2H- 200 sample 250 Figure 6.42 Feller-buncher link parameter identification 350 400 Chapter 6: Machine Experiments Least-squares Parameter Identification 1 J J L _ _ I O 1 • 50 100 150 200 sample 250 300 Figure 6.43 Feller-buncher link parameter identification 350 400 Chapter 6: Machine Experiments Figure 6.44 Feller-buncher link parameter identification 89 Chapter 6: Machine Experiments 90 6.2 Excavator Experiments The master-slaver force reflecting resolved motion control was first implemented on a four D O F manipulator — C A T 215 excavator. The machine was operated remotely from a nearby lab. The first set of experiments focused on resolved motion control — position versus rate. Hands-on experiences confirmed the results reported by K i m [8] that for a slow manipulator such as a hydraulic excavator, rate control is preferable. The mismatch o f the fast joystick and the slow excavator could cause oscillation under position control, on the other hand, an operator can easily achieve a smooth operation when the machine is under rate control. W h i l e position control is important i n some delicate operations such as peg-in-hole or microsurgery etc., the application of such control to hydraulic machines is not suitable at least from the operator's point of view. The second set of experiments were carried out to test different force reflection schemes, namely stiffness versus direct force feedback. The following gains were selected for the joystick control during the experiments (see Figure 4.24): Axis K p max K p nom Kp min Kd Deadband X 4.0 0.6 0.3 0.06 1.5 Y 4.0 0.6 0.3 0.06 1.5 Z 4.0 0.6 0.3 0.06 1.5 X Rotation 0.1 0.06 0.01 0.03 2000.0 Y Rotation 0.1 0.06 0.01 0.03 50.0 Z Rotation 0.1 0.06 0.01 0.03 2000.0 Table 6.7 Gain selections for the excavator experiments In the above table, Kp(max) i s the m a x i m u m stiffness value, Kp(nom) is the nominal stiffness value, Kp(min) is the m i n i m u m stiffness value (units: N / m m for translation and 100 Nm/rad for Chapter 6: Machine Experiments 91 rotation); K d is the damping factor (units: N/mm/s for translation and 100 Nm/rad/s for rotation); The units for the joystick deadband are m m for translation and O.OOOlrad for rotation. The contact force for feedback is saturated by the following values: Axis F_max X Y 50 ( K N ) 50 ( K N ) Z 50 ( K N ) X Rotation 40 ( K N m ) X Rotation 40 ( K N m ) X Rotation 40 ( K N m ) Table 6.8 Maximum feedback force for the excavator experiments 6.2.1 Resolved Motion Control The machine was under rate control and operated remotely to achieve straight line motions. Figure 6.45 and Figure 6.46 show the bucket was commanded to go straightly up and down while keeping the bucket angle and radial displacement constant. Figure 6.47 and Figure 6.48 show the bucket was directed to come straightly out from and i n towards the cabin while keeping the bucket angle and height constant. The joystick deadband is 1.5 m m . It is clear from these plots that the motion accuracy is high due to the fact inverse kinematics instead o f Jacobian transformations were used and the joint angles of the excavator were under closed-loop control. Chapter 6: 92 Machine Experiments Excavator Control (resolved motion) i" t > r 1 1 1 ^ 1 1 x i a i i __ 1 i T ; y ! i i i . 1 2 O '•i 0.5 i 1 J I T i m e (1.5 seconds) 2 1 1_ 2.5 Figure 6.45 Excavator resolved motion control: bucket straight up Chapter 6: Machine Experiments 93 Excavator Control (resolved motion) T 0.5 1 .5 Time (seconds) Excavator Control (resolved 7 J O 1 0.5 1 i 1 1 1 1.5 Time (seconds) 2.5 motion) 1 1 2 1 2.5 Figure 6.46 Excavator resolved motion control: bucket straight down 3 Chapter 6: Machine Experiments Figure 6.47 Excavator resolved motion control: bucket straight out 94 Chapter 6: 95 Machine Experiments Excavator Control (resolved motion) 1 \ z X - t ; i y i T i i _L L J j i i 1 ; r T i l X <• O 0.5 1 1.5 Time 2 2.5 3 (seconds) Excavator Control (resolved motion) I \ L — H -40 ~ i r ~ ~ " T 3, £0 •33 J= a> L -20 i f ' theta 1 L r z \ I I 0.5 1.5 Time H I 2.5 (seconds) Figure 6.48 Excavator resolved motion control: bucket straight in o Chapter 6: Machine Experiments 96 6.2.2 Applying a Desired Force This experiment is to test the ability of an operator to apply a desired force to the environment v i a our force reflection master-slave teleoperator system. The operator sat i n the lab and remotely commanded the excavator to contact the ground. A t the beginning, he was allowed to look at the force display on the screen to relate the contact force to the kinesthetic feel at his hand. Then the operator was asked to apply a desired force to the ground depending solely on the information from the joystick. The operator's hand applied forces were measured using JR-3 force sensor. Two force reflection methods were tested — stiffness and direct force feedback. Results are shown i n Figure 6.47 and Figure 6.48 respectively. Both methods achieved reasonably good results i n terms o f controlling the endpoint force. However, direct force feedback method is only possible when the amount of force feedback is severely restricted. Otherwise, stability problems can arise. In stiffness control (Figure 6.47), the operator's hand applied force is proportional to the wrist displacement along corresponding z axis. In direct force feedback (Figure 6.48) while the operator applied - 4 . 5 Newton force to the wrist to counterbalance the feedback force there is almost no wrist displacement i n z axis. Chapter 6: Machine Experiments 97 E x c a v a t o r C o n t r o l (stiffness control) i 1 i X I , ^ 1 > * -1 -o .22 -2 # E, ^ l I _ ^ J fk-- •Q. Q<C J "1 1 y i 1 Fh_z -3 -4 1 1 0.5 1.5 Time (seconds) 2.5 E x c a v a t o r C o n t r o l (stiffness control) x 10 T" T" 3.5h — 2.5 u V * i target I ! ^ i 1 i i 1 1 l 1 i i 1 1 2 i i 1 1 1 CO " i i 1 1 1.5 r L 0.5r- 0.5 1 .5 Time (seconds) 2.5 Figure 6.49 Excavator stiffness control: applying a desired force Chapter 6: 98 Machine Experiments E x c a v a t o r C o n t r o l (force control) Time (seconds) E x c a v a t o r C o n t r o l (force control) t \ I I \ I I I I I | I I \ I I 1 target 1 A 8 % 2 « o " l i l i i I I I l I l I l I I i I I I l i l l i - l 1.5 L 1 , << O 0.5 1 * • i i i + ! 1 1.5 Time (seconds) \ 2 Figure 6.50 Excavator force control: applying a desired force — 1 - — 2.5 — 3 Chapter 6: Machine Experiments 99 6.3 Feller-buncher Experiments The master-slaver force reflecting resolved motion control was also implemented on a five D O F manipulator — C A T 325 feller-buncher. The master M a g l e v Joystick was mounted i n the cabin during the experiments. F i e l d tests such as tree-cutting were also carried out. Experiments show that while the sound o f the engine and the motion o f the machine do provide the operator with information to detect potential machine tip over, the most effective way is v i a force reflection w h i c h automatically pushes the joystick back into its position deadband. Similar to the excavator experiments, position control was found to be unsuitable for the control o f the feller-buncher. Direct force feedback is possible only i f the amount of feedback is severely limited. The following gains were selected for the joystick control during the experiments (see Figure 4.24): Axis K p max K p nom K p min Kd Deadband X 8.0 4.0 3.00 0.06 1.5 Y 4.0 3.0 2.00 0.06 1.5 Z 9.0 4.0 3.00 0.06 1.5 X Rotation 0.8 0.45 0.30 0.04 350.0 Y Rotation 0.8 0.45 0.30 0.04 350.0 Z Rotation 0.26 0.25 0.24 0.04 2000.0 Table 6.9 Gain selections for the feller-buncher experiments In the above table, Kp(max) is the maximum stiffness value, Kp(nom) is the nominal stiffness value, Kp(min) is the m i n i m u m stiffness value (units: N / m m for translation and 100 Nm/rad for rotation); K d is the damping factor (units: N/mm/s for translation and 100 Nm/rad/s for rotation); The units for the joystick deadband are m m for translation and O.OOOlrad for rotation. Chapter 6: 100 Machine Experiments The contact force for feedback is saturated by the following values: X Axis F_max Z Y 30 ( K N ) X Rotation 30 ( K N ) 30 ( K N ) 40 ( K N m ) X Rotation X Rotation 40 ( K N m ) 40 ( K N m ) Table 6.10 Maximum feedback force for the feller-buncher experiments Hydraulic mobile machines such as excavators and feller-bunchers often operate i n harsh environments. B u m p y road conditions can cause undesired motion o f the joystick. Thus it is desirable to have a stiff joystick. O n the other hand, an operator may get tired from manipulating a stiff joystick during normal operation. A l s o , a stiff joystick could easily cause heating problem. Another problem often encountered when using multi-degree joysticks is coupling between joystick axes. A motion i n one direction could easily cause an unexpected motion i n another. In order to solve these problems, a detent scheme was implemented i n the digital P I D controller, as shown i n Figure 6.51. i LL X Deadband ••-"~"" [ Figure 6.51 Detent implementation Chapter 6: 101 Machine Experiments X Figure 6.52 Conventional position control For conventional position control (see Figure 6.52), the restoring force is a linear function of the position error. The bigger the position error, the higher the restoring force. In detent implementation, there are two slopes. Within the joystick position deadband, it is the same as the single slope approach. Outside this band, it has another slope which is lower than the first one. At the edge of the band, there isfixedamount of force drop. Clearly with this detent scheme, we can achieve high stiffness within the position deadband (desirable when machine travels) without having a high restoring force during normal operation which is outside the deadband. An operator can feel a "notch" when the joystick crosses the edge of the deadband. This is a good hint to the operator as to which area he is working in. As a result, decoupling between axes is achieved to a certain degree. Experiments show reduced restoring force outside the deadband makes normal operation easy which in turn reduces undesired motion in other axes. Chapter 6: Machine Experiments 102 X Y Z X Rotation Y Rotation Z Rotation Force Drop 2.00 (N) 2.00 (N) 2.00 (N) 0.8 ( N M ) 0.8 ( N M ) 0 (NM) Kp 4.0 3.0 4.0 0.45 0.45 0.25 Kpl 0.3 0.3 0.4 0.03 0.03 0.25 Table 6.11 Detent parameters for the feller-buncher experiments In the above table, the units for stiffness are N / m m for translation and 100 Nm/rad for rotation 6.3.1 Desired Force Tracking This experiment aimed to investigate the difference between teleoperation with and without force reflection i n controlling the end point force. During the experiment, the operator controlled the machine inside the cabin. The contact force was collected and displayed on a computer screen to provide additional guide to the operator. First, the operator commanded the machine under stiffness feedback to contact a solid rock and tried to follow a desired force trajectory displayed to h i m as a graph on the computer screen shown i n Figure 6.53. Next, the operator repeated the same experiment except that there was no force reflection present, see Figure 6.54. Experiments indicate that it is very hard to track a desired force trajectory without the use o f force reflection. The results clearly demonstrate that the addition of force reflection to the teleoperator system greatly improves the ability o f the operator to control the endpoint force. Chapter 6: Machine Experiments F e l l e r - b u n c h e r C o n t r o l (stiffness control) -2 15 20 Time(seconds) 25 F e l l e r - b u n c h e r C o n t r o l (stiffness x 10 3h ° 2 8 I-K 35 40 control) T ! target S 30 r |i« Fz r | Oh 10 15 20 Time(seconds) 25 30 Figure 6.53 Feller-buncher stiffness control: force tracking 35 40 Chapter 6: 104 Machine Experiments F e l l e r - b u n c h e r C o n t r o l (without force-reflection) 20 25 Time(seconds) x F e l l e r - b u n c h e r C o n t r o l (without 10 target \ ! 40 force-reflection) i* 1 ff. '11 • 4 o O :T"" I n r- I " 10 11 15 20 J 25 V 30 \ 35 Figure 6.54 Feller-buncher control without force-reflection: force tracking 40 Chapter 6: Machine Experiments 105 6.3.2 Complete Tree Cutting Process Quantitative data for complete tree cutting process is shown i n Figure 6.55. The operator first commanded the felling-head o f the feller-buncher to approach a tree. The endpoint force increased rapidly after contact with the tree was made. Through the change o f stiffness o f the joystick, the operator could feel this contact kinesthetically, see the hand applied force (which is approximated by the centering force o f the joystick) i n Figure 6.55. W h i l e this was happening, the rotating saw started cutting the tree, and the grab arm on the felling-head closed and grabbed it. The cut tree was then laid to the side i n bunches. Chapter 6: x Machine Experiments F e l l e r - b u n c h e r C o n t r o l (stiffness 10 control) T /i n 3. 1A Q_ £ Oh CO CO CD O. 1 -V-2h- -\---H /y\ .'1' v I « I ZD A 1 Grab arm finishes s dosind I Grab arm . starts dosiri! o O -4h 130 131 132 133 134 135 Time (seconds) 136 137 Figure 6.55 Feller-buncher stiffness control: tree cutting process 138 139 Chapter 6: Machine Experiments 107 6.3.3 Practical Issues For simplicity, only the static models o f the manipulators were used for force computation. During free motion, the joint torques caused by the manipulators dynamics can introduce big computed-force error. Figure 6.56 — Figure 6.58 show the joint step responses o f the fellerbuncher , from w h i c h we can see that huge hydraulic pressures oscillations during free motion. Clearly, the static model of the manipulator without considering the actuator dynamics is not enough. One option is to completely model the manipulator dynamics. This might be hard i n practice as well because there is large amount o f friction present i n the system. Other methods, such as direct force sensing v i a load cells, as suggested i n [3], were not pursued due to the lack of time. A l t h o u g h the force resolution is not ideal, the experiments show that force reflection did help the operator to control the endpoint force effectively. Chapter 6: Machine Experiments Figure 6.56 Feller-buncher step response: boom Chapter 6: Machine Experiments Figure 6.57 Feller-buncher step response: stick 109 Chapter 6: Machine Experiments time(sac) Figure 6.58 Feller-buncher step response: tilt Chapter 7 Conclusions Issues concerning the design and implementation o f master-slave force-reflecting resolved motion control for hydraulic mobile machines are addressed i n this thesis. A six D O F mag- netically levitated joystick is employed as the teleoperator master w h i c h proves ideal for the application o f force reflection. Machine experiments show rate control is preferable to position control for slow response slave manipulators such as excavators and feller-bunchers. Gravity torque cancellation for end-effector force sensing is effective, but not complete. The dynamic model o f the manipulator is needed i n order to eliminate the dynamic torque, especially during free motion. W h i l e direct force feedback under rate control can easily introduce instability, stiffness feedback provides a promising way i n controlling the endpoint force. 7.1 Thesis Contributions The major contributions of this thesis are as follows: 1. A new control structure is proposed to achieve "transparency" for teleoperator system under rate control. 2. A novel approach to stability analysis o f the stiffness feedback strategy proposed i n previous work is provided which, under certain condition, guarantees global asymptotic stability o f the teleoperator system. The system could be either under rate or position control and could be subject to time-delay, nonlinearity and active environment. 3. Closed-form inverse kinematics solution o f a five D O F manipulator — a feller-buncher — has been found to achieve resolved-motion o f the manipulator's end-effector within its workspace. Ill Chapter 7: Conclusions 11^ 4. Remote Center of Compliance control was implemented for the Maglev Joystick. 5. Master-slave multi-axes force-reflecting resolved motion control was successfully implemented on a CAT-215 excavator and a CAT-325 feller-buncher. Machine experiments were carried out to demonstrate the ability to control forces using stiffness feedback. 7.2 Further Work Present force resolution through pressure sensing is limited. It could be improved by either complete modeling of the manipulator's dynamics or using different force sensing technique such as load cells. Resolved motion control of the feller-buncher near the edge of workspace needs further study. Implementation of the proposed rate control structure which aims to achieve transparency needs to be carried out. 113 References [I] STS-7 Press Information. Rockwell International, 1983. [2] H. Kazerooni, "Human/robot interaction via the transfer of power and information signals: Parts i and ii," in Proceedings of the IEEE International Conference on Robotics and Automation, (Scottsdale, Arizona), pp. 1632-1649, 1989. [3] N. R. Parker, "Application of force feedback to heavy duty hydraulic machines," Master's thesis, University of British Columbia, October 1992. [4] P. D. Lawrence, B. Sauder, U. Wallersteiner, and J. Wilson, 'Teleoperation of forest harvesting machines," in Robotics in Forestry, (Vaudreuil,Quebec), pp. 36-39, September 1990. [5] R. C. Goertz and Thompson, "Electrically controlled manipulator," Nucleonics, pp. 46-47, 1954. [6] N. M. Wong, "Implementaion of a force-reflection telerobot system with magnetically levitated master and wrist," Master's thesis, University of British Columbia, December 1992. [7] J. Jansen, R. Kress, and S. Babcock, "Controller design for a force-reflecting teleoperator system with kinematically dissimilar master and slave," Robotics Research, ASME, vol. 26, 1990. [8] W. S. Kim, F. Tendick, S. R. Ellis, and L. W. Stark, "A comparison of postion and rate control for telemanipulations with consideration of manipulator system dynamics," IEEE Tr. Robotics and Automation, vol. RA-3, pp. 426-436, October 1987. [9] A. M. Sabatini, M. Bergamasco, and P. Dario, "Force feedback-based telemicromanipulation for rbot surgery on soft tissues," in IEEE Engineering in Medicine and Biology Society 11th Annual Internatinal Conference, 1989. [10] J. Yan, "A motion-scaling force-reflecting teleoperation system for microsurgery," Master's thesis, University of British Columbia, March 1994. [II] S. Lee and H. S. Lee, "An advanced teleoperator control system: Design and evaluation," in Proceedings of the IEEE International Conference on Robotics and Automation, (Nice, France), May 10-15 1992. [12] Y. Strassberg, A. A. Goldenberg, and J. K. Mills, "A new control scheme for bilateral teleoperation," Robotics Research, ASME, vol. 26, 1990. [13] B. Hannaford and R. Anderson, "Experimental and simulation studies of hard contact in force reflection teleoperation," in IEEE International Conference on Robotics and Automation, p. 584, 1988. [14] S. E. Salcudean, N. M. Wong, and R. L. Hollis, "Design and control of a foce-reflecting teleoperation system with magnetically levitated master and wrist." August 26 1993. 114 [15] R. J. Anderson and M . W. Spong, "Bilateral control of teleoperators with time delay," Tr. Automatic Control, IEEE vol. 34, pp. 494-501, May 1989. [16] C. A . Lawn and blake Hannaford, "Performance testing of passive communication and control Proceedings of the IEEE International Conference on Robotics and Automation, vol. 13, 1993. in teleoperation with time delay," in [17] W. S. Kim, B. Hannaford, and A . K. Bejczy, "Force-reflection and shared compliant control in operating telemanipulators with time delay," IEEE Tr. Robotics and Automation, vol. 8, pp. 176-185, April 1992. [18] B. Hannaford, "Stability and performance tradeoffs in bi-lateral telemanipulation," in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 17641767, 1989. [19] B. Hannaford, "A design framework for teleoperators with kinesthetic feedback," IEEE Transactions on Robotics and Automation, vol. 5, no. 4, 1989. [20] G. J. Raju, G. C. Verghese, and T. B. Sheridan, "Design issures in 2-port network models of bilateral remote teleoperation," in Robotics and Automation, Proceedings of the IEEE International Conference on pp. 1317-1321, 1989. [21] S. E . Salcudean, N . Wong, and R. Hollis, "A force-reflecting teleoperation system with magnetically levitated master and wrist,"in Proceedings of the IEEE International on Robotics and Automation, Conference (Nice, France), May 10-15 1992. Proceedings of the IEEE International Conference on Robotics and Automation, (Nice, France), May [22] D. A . Lawrence, "Designing teleoperator architechture for transparency," in 10-15 1992. [23] Y . Yokokohji and T. Yoshikawa, "Bilateral control of master-slave manipulators for ideal kinesthetic coupling," in Automation, Proceedings of the IEEE International Conference on Robotics and (Nice, France), May 10-15 1992. [24] M . Ostoja-Starzewski and M . J. Skibniewski, "A master-slave manipulator for excavation Robotics and Autonomous Systems, pp. 333-337, Langreth, "Smart shovel," Popular Science, pp. 82-84,108-109, 1992. and construction tasks," [25] R. 1989. [26] U. Wallersteiner, P. Stager, and P. Lawrence, "A human factors evaluation of teleoperator hand controllers," in Proceeding of International Symposium Teleoperation and pp. 291-296, IFS Ltd., July 1988. [27] K. Sugimoto and J.Duffy, "Analysis of five-degree-of-freedom robot arms," Mechanisms, Transmissions, and Automation in Design, Control, Journal of vol. 105/23, 1983. [28] D. Manocha and J. F. Canny, "Real time inverse kinematics for general 6r manipulators," in Proceedings of the 1992 IEEE International Conference on Robotics and Automation, 1985. [29] T. Yih and Y . Youm, "Matrix solution for the inverse kinematics of robots," [30] A . A . Goldenberg, B. Benhabib, and R. G. Fenton, "A complete generalized solution to the inverse kinematics of robots," IEEE 1985. Journal of Robotics and Automation, vol. RA-1, no. 1, 115 [31] A . Goldenberg and D. Lawrence, "A generalized solution to the inverse kinematics of robotic manipulators," Journal of Dynamic Systems, Measurement, and Control, vol. 107/103, 1985. [32] B. Hannaford, "Scalling, impedance and power flows in force reflecting teleoperation," Robotics Research, ASME, vol. 26, 1990. [33]C.-T. Chen, Linear System Theory and Design. C B S College Publishing, 1984. [34] R. L . Hollis, S. Salcudean, and A . P. Allan, "A six-degree-of-freedom magnetically levitated variable compliance fine-motion wrist: Design, modeling and control," and Automation, vol. 7, pp. 320-332, June 1991. IEEE Tr. Robotics [35] P. J. Hacksel, "Observer based velocity and environment force estimation for rigid body control," Master's thesis, University of British Columbia, October 1993. Robot Dynamics and Control. John Wiley and Sons, 1989. [37] N. Sepehri, Dynamic Simulation and Control of Teleoperated Heavy-Duty Hydraulic Manipulators. PhD thesis, University of British Columbia, September 1990. [36] M . W. Spong and M . Vidyasagar,
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Master-slave force-reflecting resolved motion control...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Master-slave force-reflecting resolved motion control of hydraulic mobile machines Zhu, Ming 1994
pdf
Page Metadata
Item Metadata
Title | Master-slave force-reflecting resolved motion control of hydraulic mobile machines |
Creator |
Zhu, Ming |
Date Issued | 1994 |
Description | Issues concerning the design and implementation of master-slave force-reflecting resolved motion control of hydraulic mobile machines are addressed in this thesis. Network concepts and linear system theory are used to design and analyze general force-reflecting teleoperator systems to achieve high performance while maintaining stability. A new control structure is proposed to achieve "transparency" for teleoperator systems under rate control. A novel approach to stability analysis of the stiffness feedback strategy proposed in previous work is provided which, under certain condition, guarantees global asymptotic stability of the teleoperator system. The system could be either under rate or position control and could be subject to time-delays, nonlinearities or active environments. The closed-form inverse kinematics solutions of an excavator and a feller-buncher, which are four and five degree-of-freedom manipulators respectively, are provided to achieve resolvedmotion of the manipulator's end-effector. Using the UBC magnetically levitated joystick, the master-slave force-reflecting resolved motion control has been successfully implemented on a CAT-215 excavator and a CAT-325 feller-buncher. Machine experiments demonstrate the effectiveness of this control strategy in improving productivity and safety of general hydraulic mobile machines. |
Extent | 4989964 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
File Format | application/pdf |
Language | eng |
Date Available | 2009-01-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.0064920 |
URI | http://hdl.handle.net/2429/3514 |
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 | 1995-05 |
Campus |
UBCV |
Scholarly Level | Graduate |
Aggregated Source Repository | DSpace |
Download
- Media
- 831-ubc_1995-0069.pdf [ 4.76MB ]
- Metadata
- JSON: 831-1.0064920.json
- JSON-LD: 831-1.0064920-ld.json
- RDF/XML (Pretty): 831-1.0064920-rdf.xml
- RDF/JSON: 831-1.0064920-rdf.json
- Turtle: 831-1.0064920-turtle.txt
- N-Triples: 831-1.0064920-rdf-ntriples.txt
- Original Record: 831-1.0064920-source.json
- Full Text
- 831-1.0064920-fulltext.txt
- Citation
- 831-1.0064920.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-0064920/manifest