Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Master-slave force-reflecting resolved motion control of hydraulic mobile machines Zhu, Ming 1994-12-31

You don't seem to have a PDF reader installed, try download the pdf

Item Metadata

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

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,  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

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

Comment

Related Items