Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Application of force feedback to heavy duty hydraulic machine Parker, Niall R. 1992

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

Item Metadata


831-ubc_1994-0031.pdf [ 1.88MB ]
JSON: 831-1.0065079.json
JSON-LD: 831-1.0065079-ld.json
RDF/XML (Pretty): 831-1.0065079-rdf.xml
RDF/JSON: 831-1.0065079-rdf.json
Turtle: 831-1.0065079-turtle.txt
N-Triples: 831-1.0065079-rdf-ntriples.txt
Original Record: 831-1.0065079-source.json
Full Text

Full Text

Application of Force Feedback to Heavy Duty Hydraulic Machines by Mall R. Parker B.A.Sc. University of British Columbia, Vancouver, 1988.  A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE ifi  THE FACULTY OF GRADUATE STUDIES DEPARTMENT OF ELECTRICAL ENGINEERING We accept this thesis as conforming to the required standard  THE UNIVERSITY OF BRITISH COLUMBIA October 1992 © Niall R. Parker, 1992  In presenting this thesis in  partial fulfilment of the requirements for an advanced  degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department  or  by  his  or  her  representatives.  It  is  understood  that  copying  or  publication of this thesis for financial gain shall not be allowed without my written permission.  (Signature)  Department of The University of British Columbia Vancouver, Canada Date  DE-6 (2/88)  tJ  //2  Abstract  Issues concerning the design and implementation of a force-reflecting controller for conven tional heavy duty hydraulic machines are addressed in this thesis. A computer simulation of a typical hydraulic machine (a feller/buncher) has been developed. It implements a graphical dis play and a dynamic simulation of the endpoint load. A magnetically levitated joystick has been integrated into the simulator to act as the force-reflecting master. The system has been used to evaluate various control strategies for their effectiveness and subjective feel. These studies have shown that simple rate control wifi be unstable in contact with a surface when direct force feed back is applied, and a novel stiffness control scheme was developed to circumvent this problem. The force-reflecting master was also used with a CAT 215 log loader, and experiments using the system to control the endpoint force were performed. The system was capable of repeatedly applying a desired target force based on force feedback to the operator alone. Large errors in the applied forces were due mainly to the poor resolution of the endpoint force sensing. The cylinder pressures in the main actuators were measured and used to derive the endpoint forces, with resolution on the order of 10% of a typical working load.  11  Table of Contents Abstract  1  List of Tables List of Figures  vii  Acknowledgments  ix  1 Introduction  1  1.1 Motivation  3  1.1.1 Forestry  3  1.1.2 Construction  5  1.2 Previous Work  6  1.3 Thesis Contributions  8  2 Resolved Motion  10  2.1 Coordinate Transformation  12  2.2 Jacobian Transformation  14  2.2.1 Jacobian vs Inverse Kinematics  15  2.2.2 Reduced Order Jacobians  18  3 Force Feedback  20  3.1 Position Control  23  3.2 Rate Control  27  3.2.1 Force feedback  27  3.2.2 Stiffness Feedback  29 ill  4 Maglev Joystick  36  4.1 Description  36  4.1.1 Hardware  36  4.1.2 Capabilities  39  4.2 Control  40  4.2.1 Controller Hardware  40  4.2.2 PD  41  4.3 Parameter Identification  43  5 Interprocess Communication  46  5.1 PC & DSP system  46  5.1.1 Serial Communication  48  5.2 VME implementation  50  5.3 Machine implementation  52  6 Graphics Simulator  54  6.1 HOOPS  54  6.1.1 Advantages I Disadvantages  54  6.2 Simulation evolution  55  7 Machine/Operator Considerations  57  7.1 Workspace  57  7.2 Force Sensing  57  7.2.1 Pressure Transducers  57  iv  7.2.2 Endpoint Force Sensors  .  7.3 Force/Position Scaling  58 59  7.3.1 Force  59  7.3.2 Position  60  8 Simulation Experiments  62  9 Machine Experiments  66  9.1 Endpoint Force Sensing  67  9.1.1 Load Cell  67  9.1.2 Pressure Sensors  68  9.1.3 Link parameter estimation  70  9.1.4 Force computation errors  73  9.2 Loader Experiments  74  10 Conclusions  78  10.1 Further Work  79  References  80  V  List of Tables 9.1  Link Parameter Estimates: average of data collected May-June 1992  72  9.2  Link Parameter Estimates: Ground level mapping. Aug. 13 1992  73  9.3  Link Parameter Estimates: Whole workspace mapping. Aug. 13 1992  73  9.4  Results of Endpoint Force Control Experiments  76  vi  List of Figures 1.1  Simple Telemanipulator  1  1.2  Feller/Buncher Assembly  3  1.3  Typical feller/buncher  4  1.4  Typical excavator  6  2.5  Manipulator Hand Controllers  11  2.6  Excavator kinematic parameters  13  2.7  Feller/Buncher Schematic  14  3.8  Hand controller with slave model  20  3.9  Hand controller with slave block diagram  21  3.10  Direct control of master position  22  3.11  Direct control of master force  22  3.12  Position control through compliant hand  23  3.13  Position control with feedback  24  3.14  Position control Root-Locus  25  3.15  Rate control Root-Locus  28  3.16  Joystick Stiffness vs Applied Force  33  3.17  Stiffness controller block diagram  34  3.18  Master and slave response for stiffness control:  =  10 N,  (=  3.19  Master and slave response for stiffness control:  fliaTid =  60 N,  =  4.20  Maglev Joystick: Block Diagram  37  4.21  Maglev Joystick: Isometric view  37  4.22  Maglev Joystick: Exploded view  38 vii  1  34  0.3  35  4.23  Joystick Control Flow  5.24  ram Machine simulator block diag  5.25  VME based simulator  5.26  CAT 215 controller  6.27  OPS simulation Operator viewpoint from HO  8.28  P controller Virtual mass simulation on DS  9.29  Static machine forces  9.30  (tension Sample data from force trials  9.31  ired force Endpoint force applied vs. des  -,  compression ÷)  vu’  Acknowledgments  I would like to thank my supervisors Tim Salcudean and Peter Lawrence for their support and assistance in my thesis work. Many thanks as well to Dan Chan for his invaluable assistance during many late nights completing machine experiments.  I would also like to thank Mark  Milligan and Simon Bachmann for their technical input as well as my friends and fellow students for their advice and assistance throughout my work.  ix  Chapter 1 Introduction The sense of touch is used whenever someone interacts with the environment. It permits one to grasp objects without damaging them and to determine their shape and configuration without visual contact. As long as an object is manipulated directly, force feedback comes about naturally. As tools are used to extend the abilities of the human body, the tool inertia, friction and compliance will obscure the true forces applied to the environment. A telemanipulator (e.g. Fig. 1.1) is a device which permits a person to manipulate an object that is a distance away from the operator. In its simplest form, it consists of a master controller connected (mechanically, electrically, hydraulically etc.) to a manipulator such as a robot arm. Just as force feedback information is useful to a person directly manipulating an object, it is also useful to the operator of a telemanipulator. For this reason, force feedback is often incorporated into telemampulator mechanisms. Heavy duty hydraulic machines, of the type currently in use in the forestry and construction industries, can be treated as telemanipulators and also benefit from the addition of force feedback.  tion command  end effector  Figure 1.1 Simple Telemanipulator  In general tasks, while vision is the primary sense for locating and moving objects, force feedback becomes the primary sense when dealing with tasks that involve contact. For example, 1  Chapter 1: Introduction  2  the insertion of a pin into a hole, or the threading of nut on a stud require control over the forces at the end effector. A perfect model of the endpoint environment, together with a perfect positioning system would enable control of the endpoint force to be simply a matter of controlling the endpoint position. On the other hand, practical systems will require a more direct method to control the forces generated by contact. These forces can be controlled either directly through force feedback to the supervising controller (a computer in the case of pure robotics, an operator in the telerobotics case), or by control of the endpoint compliance (passively or actively). In the teleoperation case, force feedback will give the operator more information and greater control over the machine.  Currently, the hydraulic machines in use in the forestry, mining and construction industries provide no force feedback to the operator.  The operator typically controls the joints of the  machine individually, through a pilot valve system driving the main spool valves. Most pilot valves are controlled directly, through simple levers or a joystick, but some are now controlled electrically. As each of the joints is controlled individually, the operator must coordinate the controls in order to achieve linear motion of the implement. The loading on the machine is determined solely by audio cues and cabin motion.  While they are not generally treated as such, heavy duty hydraulic machines can be handled as telemanipulator systems. Research in telerobotics began in the early 1940’s with the advent of the nuclear program. Hazardous materials require greater separation from humans than can be accomplished with simple tongs, and so master/slave manipulators were developed to bridge the gap. Typically, such devices consist of kinematically equivalent masters and slaves, with the master joint positions driving the slave joints in a position servo loop. In order to reflect the slave forces to the operator, the master has to have a bidirectional link to the slave. Small units  Chapter 1: Introduction  3  simply used cable drive mechanisms, while larger units required active actuators, usually electric or hydraulic. While system inertia, friction and damping detract from the control and feel of the slave endpoint, some complex bilateral teleoperation systems have been developed.  1.1 Motivation The motivation for this research is to provide useful force feedback to operators of heavy duty hydraulic machines. These machines are used in various industhes including forestry, mining and construction.  1.1.1 Forestry The forest industry uses a several different types of hydraulic machines in harvesting, processing and transportation. Feller/bunchers (Fig. 1.2) are hydraulic machines based on an excavator frame. They consist of a cabin mounted on a tracked chassis and a two link arm with a grapple and cutter mounted at the end. The grapple typically has two degrees of freedom, pitch and roll.  grapple  cabin  wrist saw  Figure 1.2 Feller/Buncher Assembly  Chapter 1: Introduction  4  Figure 1.3 Typical feller/buncher  Operation of a feller/buncher consists of moving within reach of the desired tree, then grasping the tree and actuating the cutter to cut the tree at its base. For smaller trees, it may be cut first and then caught as it begins to fall. The cut tree is then laid to the side in bunches, ready to be skidded to the road side for processing and/or transportation. Experienced operators can cut and bunch up to 500 trees/hour, depending on the size of the wood and the teffain. Force feedback in this application could provide the operator with better control when manipulating large trees, better safety through increased awareness of machine tip over, as well as potential for reduced machine wear and wood damage. Cunently, feller/bunchers have a quite short service lifetime, as grapple misalignment increases side loading on the joints. These excessive forces could be minimized by providing force feedback to the operator.  Chapter 1: Introduction  5  While the feller/buncher is the most obvious example of a hydraulic manipulator in the forest industry, there exist other machines that could benefit from force feedback. Log loaders are used at the harvesting site to load logs for transport to sawmills, as well as at intermediate sorting yards. Force feedback can provide the operator with better control of the log dynamics and could improve the ability of the operator to damp out payload swing. Better feel of the endpoint forces would also prevent machine tip over or overload when an excessive load is attempted. Various log processing machines, while seldom in danger of overload, could use force feedback to minimize grip forces of logs, and thus reduce wastage. Excavators are used often for access road construction, and endpoint force feedback could reduce overload and related safety problems.  1.1.2 Construction Excavators are also used in the general construction industry.  The environment in a  construction site can contain many additional hazards not usually found in the forest industry. Pipes, conduits and other buried services need to be avoided in order to prevent costly and potentially dangerous disruptions. Use of manual labour near these services, while much safer, is also much more expensive. Providing the operator with finer resolution of the endpoint forces can help determine the location of sensitive lines before they are accidentally cut. An active joystick can also be used as an operator warning device based on additional sensors or simply predefined danger zones. Machine operation near power lines is an example of a task where the control system could direct operator in a intuitive fashion away from contact with the lines. A particular application in which good control of endpoint forces is required is the use of power hammers for ground compaction. These power hammers are typically mounted at the end  Chapter 1: introduction  6  of an excavator stick, and the downward force applied by the machine must be maintained within certain limits. Too little force will result in excessive oscillation amplitude, while too much force can damage the hammer mechanism. Currently, the force applied depends solely on operator experience, using cues such as noise and vibration levels in the cab.  Figure 1.4 Typical excavator  1.2 Previous Work There has been quite a bit of work in force reflecting master/slave manipulators. Earlier work by Mosher at GE in the development of some exclusively analog systems (e.g. Handiman) is described in [11. An example of more recent work is the dextrous hand with force feedback developed by Burdea et al. [2]. The use of force feedback in manipulation robots has been shown to reduce reaction forces and improve assembly tasks [31. It was found in [4] that force control in Cartesian space was superior to joint space torque control.  Chapter 1: introduction  7  Manipulation tasks require some form of input device. Force feedback introduces additional constraints regarding ease of actuation and magnitude of applied feedback forces and torques. Six degrees of freedom allows complete motion commands with one hand, though it can introduce problems with coupling between rotation and translation. Different hand controllers were reviewed in [5] and while the review did not exclusively address force feedback, it was one of the evaluation parameters. The best configuration was found to be some form of joystick or torque ball. The development of a magnetically levitated device capable of providing high resolution force feedback is described in [6]. An analysis of system requirements for effective force feedback was made in [71. It was found that while forces could be felt at relatively high frequencies (up to 3 kHz) the absolute maximum command frequency would be less than 10 Hz. A summary of operator hand modeling can be found in [81, with the primary finding that, while the human arm is an active element, the response is slow enough that it can be effectively modeled as a passive impedance. The treatment of hydraulic machines as teleoperators has been investigated by Lawrence et al. [9]. The addition of resolved motion to the control of such machines was found to be useful in reducing operator training time [10]. A comparison between position and rate control (without force feedback) was done by Kim et al. [11]. It was found that position control was better in the ideal case for relatively small  manipulator workspaces, though its superiority depends on a fast manipulator. Rate control is more effective for large workspaces and slow manipulators. In addition, rate control with position servo of joints was found to be inferior to rate servo for mechanisms with low natural frequencies. Force reflection in the presence of a time delay was investigated in [121, and a transmission line approach to the communication link was found to assure stability. A comparison between  Chapter 1: introduction  8  force reflection and shared compliant control (SCC) was made by Kim et al.. [131. Shared compliant control is a method of force control that uses a local ioop at the end effector to give the system a programmable compliance, rather than returning the force to the operator and relying on him to provide the necessary compliance. It was found that SCC performed better for systems with long times delays (> 0.5 to 1 seconds). Some work in the application of force feedback to excavators in joint mode control has been done using a master/slave approach. An analysis of the control and feedback requirements was done by Ostoja-Starzewski and Skibniewski [14]. A commercial development of a remotely operated excavator with force feedback using a master/slave style controller is currently underway [151.  1.3 Thesis Contributions This research project has focussed on the addition of useful force feedback to heavy duty hydraulic equipment. A graphical simulator was developed and integrated with a force reflecting joystick. Rather than using kinematic equivalence, a universal master was used, with the necessary coordinate transformations being processed in software. The force reflecting master was also integrated into CAT 215 configured as a log loader with a resolved motion controller and endpoint force sensing via main hydraulic pressures. The integrated system was used to successfully control the applied endpoint force of the machine. A novel approach to force feedback which involves controlling the joystick stiffness has been developed. This method shows promise of providing endpoint force feedback while controffing the machine in rate mode. The remainder of this thesis is divided into nine chapters.  Chapter 2 deals with the  transformation between operator and joint coordinate frames, focussing on the use of the inverse Jacobian. Chapter 3 investigates the stability and feel aspects of force feedback, for both position  Chapter 1: Introduction  9  and rate control. The maglev handcontroller and computer systems used are described in Chapters 4 and 5. The development of the graphics simulator for the machine is detailed in Chapter 6, while Chapter 7 discusses particular aspects of the log loader implementation. Chapter 8 presents the experiments and results performed on the dynamic simulations of force feedback, while Chapter 9 presents the actual machine experiments. Finally Chapter 10 summarizes the conclusions obtained and provides some recommendations for further work.  Chapter 2 Resolved Motion All machine control systems require some form of operator interface. In the case of a teleoperator, this interface is particularly important due to the close interaction between the machine manipulator and the operator. Several different interface methods, with varying degrees of complexity and capabilities have evolved for different teleoperation tasks (Fig. 2.5). The simplest method to control the links of a manipulator is to have a separate control input for each joint. This can be as basic as a switch to move each joint one way or the other, or it  can be a bidirectional proportional control. The machine is typically controlled in rate mode i.e. the velocity of each joint is controlled, though joint position control is possible. This interface was the first developed for many machines and is stifi used for its simplicity on systems such as truck loaders and other manipulators which are slow moving and do not require coordinated control of their joints. Another method of controlling a manipulator is through the direct manipulation of a kinemat ically equivalent master. In this case, the master joint positions are communicated to the slave manipulator, and the slave joint torques can be communicated back to the master. This gives a direct implementation of force feedback and has been used for many remote telemanipulators (e.g. subsea and nuclear industry manipulators). These masters require a relatively large volume (they can only be used for position control), and are specialized to work with only one type of slave manipulator. The separate control inputs of the first method can be combined into one or two multiple input joysticks. As the operator hand does not have to move between controls, these generalized masters simplify the coordination of joint motion, though smooth motion in the end effector 10  Chapter 2: Resolved Motion  11  frame requires a lot of operator training. Unless the manipulator is a Cartesian gantry type robot, motion (and force feedback if used) will not be intuitive. This joint control method can be used for rate or position control, and most hydraulic machines today (e.g. excavators) use it.  °mO  °ml  m2 0  Individual Joint Control 0m2 °mO  Kinematically Equivalent Master \oosios2ze:: 6m1  Slave Manipulator  Ø_ø:en12  Generalized Master: Joint mode,/////”  Generalized Master: Resolved mode  Figure 2.5 Manipulator Hand Controllers  Chapter 2: Resolved Motion  12  An improvement over joint mode control is to use a computer to transform the commands in the operators frame to the machine’s joint space. This resolved (or coordinated) control method gives intuitive control over the end effector, as in/out motion of the master corresponds to an in/out motion of the end effector (similarly for other degrees of freedom). The joint command coordination is handled by the controller, rather than the operator. Resolved motion control also gives a more intuitive frame for force feedback as endpoint reaction forces can be felt to directly oppose motion. This control method can be used for rate or position control. While offering many advantages over conventional controls, it is not commercially available. Most hydraulic machines currently use joint velocity control for historical reasons (it was all that was previously available). Resolved motion control of hydraulic machines has been investigated by Wallersteiner et al. [10], and the more intuitive interface was found to significantly reduce operator training time. For this reason, it is also used here for force feedback applications.  2.1 Coordinate Transformation  The transformation from operator coordinates to joint coordinates can be accomplished via one of two methods. The operator command is given the endpoint frame (t, re, ze) to ensure that the desired motion direction is correct regardless of the radial displacement. This command is first converted to cylindrical coordinates (Oe, re,  Ze)  via ZO  =  then the necessary joint  angles for the desired endpoint position can be found directly from the inverse kinematics.  Chapter 2: Resolved Motion  13  elevation 2 a Ze  1 a r  plan  Figure 2,6 Excavator kinematic parameters  The inverse kinematics problem is to solve for the desired joint angles 01,02,03 given the endpoint coordinates 0,, re, ze and the manipulator parameters. For the excavator (illustrated above), the  solution is quite straightforward.  1 = 0e + tan  03 = —  —  =  —  ()  (2.1)  a  cos_i  +a  —  (a 2a2a3  + (r  —  2 ai)  )  (2.2)  14  Chapter 2: Resolved Motion  02  =  3+ ‘y —  /3=tan 1(  )  —  7=Slfl  (2.3)  aij  —l (a3  TsLnb  While this particular instance is quite simple, with a direct solution available, the solution of the inverse kinematics for a general manipulator is considerably more difficult.  2.2 Jacobian Transformation An alternative to solving the inverse kinematics is to determine the incremental change in position desired and then determine the corresponding joint velocities via the inverse Jacobian. Consider the feller/buncher schematic shown in Figure 2.7.  •,-  5 x  5 /// a  d5  wrist  cabin  boom  stick Figure 2.7 FelIer/Buncher Schematic  Chapter 2: Resolved Motion  15  The Jacobian relates the endpoint velocity () in the base coordinate system 00, x0, yo, zíj to the manipulator velocity in joint space  (j):  =  (2.4)  J(q)  The Jacobian is found easily for any manipulator [161 and for the feller/buncher above is of the form:  J()[zox(osoo)  —o 5 zix(o ) 1  x(o 4 z — 5 ) o ]  (25)  where 01 62  q  E  03  (2.6)  04  05 and joint axes z, z,•  ,  Z4  and the joint centers  0o,O1,  ,  04  are defined in Figure 2.7.  The machine on which the force feedback control system is to be installed is powered by hydraulic actuators. Hydraulic machines have quite complicated dynamics when the actuators and power system are fully accounted for [17] [18], and a joint level controller has already been implemented that controls the pilot valve system to achieve a desired joint position. This controller will be used for this work in force feedback, thus requiring that the resolved motion controller provide joint positions or velocities directly.  2.2.1 Jacobian vs Inverse Kinematics When controlling the machine endpoint in rate control mode, the Jacobian approach has definite advantages. Assuming that the machine command input is the desired joint velocities, these can be calculated directly from the desired endpoint velocities via the inverse (or pseudo-  Chapter 2: Resolved Motion  16  inverse) Jacobian:  (2.7)  =  Using inverse kinematics would require integrating the endpoint velocity command, then differ entiating the joint angles once they have been obtained. These steps are unnecessary using the Jacobian approach (for a joint rate servo). Rate control can also be implemented with a position servo, but this method has poorer performance when the joint natural frequency is low [11] (as it will typically be on a hydraulic machine). When using the Jacobian for position control, the advantages are fewer, and the best approach depends on the machine being modeled. Previously an excavator (4 links, shoulder offset only) and a log loader (5 links (2 free), shoulder offset only) were modeled and controlled using inverse kinematics [18]. Much of the initial modeling in this project was focused on a feller/buncher which is a more complicated machine than either the excavator or the log loader (though it is probably the best application of force feedback). It has 5 links, with an offset at both the shoulder and the joystick. It is these offsets which considerably complicate the inverse kinematics. Position control of a manipulator endpoint in resolved mode when the inverse kinematics are available is trivial, with  determined directly from  iSde  Using the inverse Jacobian requires  the converse of the procedure used to accomplish rate control with inverse kinematics i.e. first the command input must be differentiated, then the commanded velocity in endpoint space converted to joint space, and finally the joint space velocity integrated to get the desired joint angle. When this procedure is followed for a finite sample time, the joint increment determined by this procedure may not be precisely correct. The relationship (2.7) holds for true velocities, but is only approximately true for finite position increments. This is because the Jacobian (and its inverse) is configuration dependent, and as long as the manipulator is moving, the configuration  Chapter 2: Resolved Motion  17  is time dependent. =  (J(t))) ‘i to+t  =  f  to+t  Ldes  =  to  I J  (J(q(t)))’  (2.8)  0 to  (J(q(to)))  -Sd  (J(q(tO))) / 1 eyjd  The error in the calculated joint increment joiTzt will be larger for larger sample times t . 8 In simulation experiments (Chapter 8), it was found that the error was large enough to cause noticeable oscillations in the endpoint motion as the Jacobian converged.  To eliminate this  disturbance, an iterative procedure was used to determine the final joint angle necessary before applying any command to the machine: q  :=  AeTod =  erid  —  AViTOt  (2.9)  =  =  q + joint  repeat until end  0  where the inverse Jacobian could be a pseudo-inverse or reduced order if the manipulator possesses less than six degrees of freedom (the usual case for hydraulic machines). This iterative procedure unfortunately takes more time and reduces the possible update rate. The advantage of the Jacobian is that it is generally available. Currently, the inverse Jacobian of the manipulator is being used instead of the inverse kinematics used previously. While the Jacobian approach fails at manipulator singularities, in the particular cases being examined, these singularities are outside the joint range since the hydraulic actuators interfere with the links.  18  Chapter 2: Resolved Motion  2.2.2 Reduced Order Jacobians When a manipulator has less than six degrees of freedom (e.g. an excavator or feller/buncher), and the control input includes all six, there are two approaches that can be taken when determining the desired joint velocities. In the first instance, all six of the master inputs can be used, and a pseudo-inverse used to obtain the joint velocities which best fit the desired input (in a least squares sense). des  -des 1 !  .current  (2.10)  suTflp t  with vx  des E  (2.11)  then q = 1 !des  (2.12)  where jt  is the Moore-Penrose pseudo-inverse.  JT 1 (JTJ)_  (2.13)  This method requires the computation of a matrix  matrix multiplication, a matrix-vector multiplication and the solution of a linear system of 6 equations (either via LU decomposition and backsubstitution or a matrix inverse and matrix vector multiplication). One way to avoid the pseudo-inverse problem is to only use five degrees of freedom from the joystick command, since the machine being examined (a feller/buncher) does not have a yaw degree of freedom. The Jacobian can then be reduced to a 5x5 matrix. The solution for the  Chapter 2: Resolved Motion  19  joint velocities given the endpoint velocities in Cartesian coordinates (base frame) can then be determined:  =  J  (2.14)  wz  The order of the system is reduced by one, with significant savings in the required computations. Further simplification is possible if the machine has fewer degrees of freedom, and/or there are no wrist offsets (in this case, transformation to cylindrical coordinates will save another degree of freedom). One drawback of using a reduced Jacobian is that operation may seem to be less intuitive. Rotation of the ignored degree of freedom (in this case O) will have no effect on the end point position. This is not thought to be a major problem, and it does avoid a related problem of conflicting commands resulting in a “best” fit which conforms to neither of the operators explicit commands. An operator is likely to quickly determine which motions work and which motions  are ignored.  Chapter 3 Force Feedback  Modem hydraulic machines usually use an electrical link between the joystick and the hydraulic system. This electrical link is unidirectional, isolating the operator from the endpoint forces on the machine. While it is possible to provide visual or audio feedback, these routes typically have a greater processing delay than the more intuitive feedback channel through the operator’s hand. Direct force feedback can be accomplished by using an active joystick and slave force sensors. Force feedback can introduce stability problems into a teleoperation system as the returned force to the master handle can effect the master input. For simplicity, the analysis here will only examine one dimensional motion.  hd  m  operator’s hand  master  r  slave  environment  Figure 3.8 Hand controller with slave model  20  Chapter 3: Force Feedback  21  The coupling between the master and slave depends on the control mode (position or rate) and the feedback mode (force, force derivative or stiffness). The link from master to slave can be described as = H.pI(pos,g  for position (3.15)  =  where  for rate  HrKveIrn  7 represent the slave transfer functions for position and rate respectively and hatted H  variables denote Laplace transforms.  A Xe  endpoint environment  Figure 3.9 Hand controller with slave block diagram  A reasonable approximation for  Hr would be to use a first or second order system With  delay (typical joint response delay for hydraulic machines is on the order of 0.4 s). The combined system response of the master and slave will depend on the particular mode in which the operator is controlling the master. One case is when the operator controls the position of the master directly. When controlling position, the feedback information will be felt as a variable force applied both by and to the hand operator can control  Xm  d = Tt (fI  K i 0 .fe), but if it is assumed that the  directly (i.e. independent of the fedback force) then the feedback loop  is broken, and the system response is simply the open loop slave response. For position control, the block diagram would be:  Chapter 3: Force Feedback  22  0 p e  r a t 0  r Figure 3.10 Direct control of master position  This situation is not very realistic as it requires the operator to have an infinitely stiff hand or actively compensate for applied forces to maintain a particular master position. Another special case is when the operator controls  ffLUTd  only. In this instance, the feedback  information is felt as motion of the master. The operator’s hand model still does not play a role, and the system can be modeled (for position control) as:  A  A  Xe  f hand  Figure 3.11 Direct control of master force  Rate control would be handled in a similar fashion, with  replaced by I(vel and H 8  replaced by . 87 H A more realistic analysis of the system requires some model of the operator hand. While there exist many complicated models of the human operator which account for sensory delays,  Chapter 3: Force Feedback  23  motor delays and voluntary control of hand compliance [7], for this initial analysis a simple passive compliance (i(i, B,) will be used. This is reasonable for a high frequency model as the human nervous system is too slow to play a role, though it does neglect the long term stabilizing input of the human operator.  3.1 Position Control To evaluate the stability of a position control manipulator with direct force feedback, assuming the operator is controlling the master position through a compliant hand (the typical case) the following model, derived from Figure 3.9 will be used.  A  x  endpoint environment  Figure 3.12 Position control through compliant hand  For the ideal case of  1 (i.e. a perfect slave), the system transfer function is given by: —  Xdes  —  R ( 0 B,s + K,) +B’s+K’ 2 m’s  (3.16)  where m  + KposKforml  B’  B,, + B + KposKforBe  K’  Kh + K + KposKjro.rKe  (3.17)  Chapter 3: Force Feedback  24  Examination of the above transfer function indicates that the composite system will act like a simple mass/spring system. Rearranging the block diagram:  A  0 , 1 K  Figure 3.13 Position control with feedback  simplifies root-locus analysis. For the system above, the loop gain I(posKf  can be varied over  any range and remain stable (the system is still a second order mass/spring system with damping) when  = 1. Using a more realistic slave model (e.g. II =  for joint response behaving  like a low pass filter) can introduce open loop poles which will push the closed ioop poles into the right hand plane for large values of 07 Kf The precise behaviour will be depend on the 0 K .. particular machine model and gains chosen, but some general observations can be made. The environment will be most critical when it is stiff and lightly damped. This will place the system zeros near the  jw  axis, away from the real axis. The poles due to the operator hand  and controller combination wifi be much closer to the origin (the hand is much less stiff than the environment). For a first order joint model, the locus can be deflected into the RHP if it is close enough to the  jw  axis, though a stable system can be selected by picking a low enough gain.  Second order systems wifi be more difficult to stabilize (the jw axis will be the asymptotes for high gains) while higher order systems would require some compensation, since simple feedback  Chapter 3: Force Feedback  25  is not enough to stabilize the system anymore. The following are some representative’ root locus plots for non ideal joint models.  1st order joint model  2nd order joint model  200  200  100  100  ‘-  f.  -100  -100  -200  -200  -3(A.)  -200  -100  100  200  300  •-3 U  cH  -200  -100  Real Axis  0 Real Axis  100  200  3 J  Figure 314 Position control Root-Locus  To evaluate the operator feel, one examines the position to force transfer function. For grasping an object of mass m in contact with an environment Be, I(, Newton’s law for the motion of the mass corresponds to: fhand =  2 + Bs + (mis  (3.18)  The force on the hand will be: fliand =  s+ 11 (B  i(li)(Ides  (3.19)  —  The feel of the system will correspond to transfer function from input motion felt,  Xdes  to the force  From (3.18) and (3.19) this will be: flia rid Xd  (B,s + K,) (mis 2 + Bes + Ke) +(Be+B,z)s+(Ke+K,i)) 2 (mis  The model  parameters used  B  0, B= 70 N/m/s, K,= 2 kN/m, H 51  =  =  were:  mi= 1000  3 20  kg, Bl0 kN/m/s, Ke 20 MN/rn, mm= 1 kg, =  —s  and il,  =  Chapter 3: Force Feedback  26  For position control (Fig. 3.9): 2 + Bs + I(c)Ièm + s 71 (m  f!uLnd  =  fref  HspKposIm (3.21)  fyf =  =  Kforfe  2 + Bes + Ke)Is (mis  Assuming a perfect slave and using (3.17) and (3.19) will lead to a transfer function of: fhcnd  s 1 (B,i  2 + (B’ + Kh) (rn’s  -  B,js + (K’  —  K,j)  3 22)  +B’s+K’) 2 (rn’s  Xdes  As this is completely analogous to the simple object case, the feel wifi be natural. Another approach to ensure that the force felt corresponds to the force at the endpoint is to match the joystick impedance to that of the environment [81. fJand = rnT,  rn + B thru + ICc (3.23)  fe  + Bethe + Kx  =  With a command transfer function of x  =  KOSxT,L and a force transfer function of  =  Kforfe one finds that to maintain a corresponding feel at the master, rn ,B and K need to 71 be adjusted such that mrrs + 2 Bcs+Kc mjs 2 +Bs+K  For flat frequency response, this would require  K  =  (3.24)  AposKfc,y.  =  I(posi(fo.,.rnl, B  =  KførB and 0 I(p  KposKforKe. While position control is the most natural mode to apply force feedback to, it does require  a large master workspace or large position scaling factors. Position control has formed the basis of previous investigations of force feedback of hydraulic machines [151[14].  Chapter 3: Force Feedback  27  3.2 Rate Control 3.2.1 Force feedback Rate control, where the position of the master commands the velocity of the slave is used in many machine control mechanisms to provide fine control over a large workspace using a limited master control space. However, it introduces complications when used with force feedback. The main source of these complications is the integration of the command signal, which results in the system only being stable when  Xrr  is zero.  A stability analysis for rate control was done in the same fashion as the position control case. The block diagram wifi be the same as Fig. 3.12 by replacing K 08 with Kuej. In this case the transfer function is:  Xdps  3 mTrLs  —  ) 1 Kvel(BJLs + K B*s 2 R*s + + + KvelKforKe  ( 3 25 ) .  where B* = B, + B + IEveXKforml  (3.26) =  K, + K, + KvelAforBe  The stability of the system can be examined using root locus techniques in a similar fashion to Fig. 3.13. In the instance of H 87  =  1, the system can be made stable by choosing low enough  gains for KvelKf or (analogous to the position case with H  =  ). The extra pole at zero  increases the order of the system by one, so the system will be unstable for second order joint models without additional compensation. Root loci for the analogous cases to Figure 3.14 are shown in Figure 3.152.  2  The model parameters used were: ml= 1000 kg, B=l0 kN/m/s, K= 20 MN/rn, mm= 1 kg, B  =  K,,  =  0, B 70 N/mIs, K= 2 kN/m, Hsri  =  and H r, 8  = s2+5Os+5O  Chapter 3: Force Feedback  28  :y 2nd order jomt model  2fl(  100__IL 0  1<  rN\  -100  200I\ -200  0  u  -iuu  Real Axis  uiu  uu  i  j  Real Axis Figure 3.15 Rate control Root-Locus  Analogous to (3.21) the forces on the master will be: fhd =  2 + Bs + I(c)rrt + fref (m,s 1__  -  =  .Kveixr,t 7 Hs S  ff fe  =  (3.27)  fe 0 K  2 + Bs + K)i (mis 8  Using (3.19) and (3.26), the feel of rate control with direct force feedback and a perfect slave will correspond to: fhand Xdes  —  —  (B,s + K ) (?nms 1 3 + (B* 2 + (K* K,)s + KvelKfoT.Ke) BIL)s 2 + K*s + KuelKforRe) 3 + B*s (mrs —  —  3 28  In order to get a better idea of what this will actual feel like, the special case for no hard contact will be examined. For K  0, the transfer function will simply be  (B,s + Kj)(m 2 + (B* s 1  fhaid Xdes  =  —  B,)s + (K* +B*s+K*) 2 s 1 (m,r —  —  )) 1 K,  ( 329  This corresponds to grasping a mass mrrt in contact with an environment B*, K*. One should note that an increase in the endpoint load wifi not be felt as a mass increase but rather as a  Chapter 3: Force Feedback  29  viscosity increase. In a similar fashion, environmental damping will be felt as a spring force. Rate control with direct force feedback leads to an unnatural feel of the machine load. Analogous to the impedance matching approach for position control, one can derive similar equations for the rate control case: fe  = [mis 2 + Bs + K]I =  fltand =  (3.30)  2 + Bs + I(j [mis 2 + Bs + K] XTfl [mrrs  To maintain similar feel to the position case,  =  5I(forf, i.e. the derivative of the endpoint  force should be returned (after scaling) rather than the force itself. While this will give a more natural feel, it will not provide feedback of constant forces at the endpoint, and thus remove most of the useful information for a contact task. In all cases of rate control looked at thus far, the problem of integration of the command signal for all nonzero  XTrL  remains.  3.2.2 Stiffness Feedback As force feedback with rate control has proved to be unsatisfactory thus far, another approach was taken to provide the operator with a sense of endpoint forces while in rate control mode. The method used is to vary the joystick stiffness, rather than apply a force directly to the handle. The resulting system is nonlinear and difficult to analyze, but provided satisfactory performance in experiments. By varying the stiffness rather than returning a force to the handle, the system will remain stable for zero input. The joystick is never driven away from its neutral point, as there is always a finite restoring force. As long as the joystick itself is stable, its long term behaviour will be at worst that of an isolated mass/spring/damper combination. As will be shown, use of a deadband (as is typically needed for rate operation anyway) improves further on the system by providing  Chapter 3: Force Feedback  30  a portion of the workspace in which the joystick remains a simple linear system, even with a  finite hand input force. The model used in this analysis is the same basic one used for the linear control schemes discussed previously, with K now a function of  f  (Fig. 3.8). For the master:  + Bcthrn + I(c(fe)xrri =  mmârn  (3.31)  flzar&d  where fhd  = By(ides  —  thm) +  ij(Xdes  (3.32)  —  and  Kc(fe) = Kijorri + Kife for the simplified case of one stiffness curve with no saturation. The slave force  (3.33)  fe  depends on  the endpoint environment (Fig. 3.8)  fe  =  mle + Be±e + KeXe  (3.34)  The endpoint velocity th tracks the master position (i.e. rate control)  =  for the ideal slave case (H . 81  =  Kv,iXrrj  (3.35)  1). Substituting (3.33) and (3.34) into (3.31) yields  d 1 f,  =  f + ffb  f  =  mT1Tf +  f  =  [mi± n xrr + BeXt + Kexntfxm] Kvi(r 1  + KrtomXyn  (3.36)  where ffb is the only nonlinear component. Analysis of this nonlinear system is not trivial, but some intuitive arguments can be made regarding its stability.  Chapter 3: Force Feedback  31  A necessary condition for stability of this system is the existence of an equilibrium point. A trivial solution to (3.36) would be be an equilibrium point for ff d 7 =  rn,  =  fiLaud =  const  O X’rr 0, x  0, however a more interesting case would  = =  const  0. If one exists, this would imply  0 and (3.36) could be simplified to  Knornm  However, with  Xrn  + KveiKrBeX + KveiKrKXrrfXm = fltand  (3.37)  0 the integral term will not remain constant. As all other terms on the  LHS are constant, the sum can’t be constant. This implies that  f7La’fd  can’t be constant and  (3.37) is not valid. Therefore, no nontrivial equilibrium point exists for the simple case of no joystick deadband. The key factor in that destabilizes the system is the basis of rate control i.e. for a nonzero input position, there wifi be a nonzero output velocity. In order to maintain a stable position of the endpoint, and hence stable forces at the endpoint, the master must possess a deadband:  1(vei(Xrn  —  Xdb)  for  >  X  (3.38) =  0  for  IxTF  Xj  For motion outside of the deadband the force on the operators hand will now be  ff a 1 d =  As long as  IXm I  —f  Xd.  >  f + 1(veiK’r [mi±rn  + Be(Xrr  —  Xdb)  + Kef(Xm  —  Xdb)] Xm  (3.39)  the integral term in (3.39) will increase. To maintain a constant force,  When the master enters the deadband, endpoint motion will cease and  fe  will  remain constant. In this case, K,. will also be constant, and the joystick motion is reduced to a simple second order linear mass/spring system. As the environment force scales the controller stiffness, the equilibrium value of fJ d will depend on the position of the master within the 1 deadband. Increasing the damping of the master will reduce overshoot, though the concepts of  Chapter 3: Force Feedback  32  critical damping do not apply directly to the non linear system outside of the deadband (i.e. (> 1 does not necessarily imply no overshoot, nor does  (  <  1 imply overshoot).  Stability of the stiffness control method can be divided into two parts. For motion of the joystick within the deadband, as the system is just a simple second order system, it will be stable for any B, K. > 0. Outside of the deadband, it must be shown that the master will enter the deadband within a finite time for any finite input f7ard. An analytical solution via Lyapunov functions has been attempted, so far without success. However, a numerical simulation has shown that the system will converge to the deadband quite quickly for a wide variety of joystick and environment parameters. Practical Implementation The simple linear stiffness with deadband approach outlined above can result in a “stickdown” phenomena, whereby pressing onto the ground with sufficient force will cause the joystick to stiffen up enough that the operator has difficulty in pulling the endpoint up again. Limiting the joystick stiffness would remove most of the useful feedback, and so a dual stiffness approach (Fig. 3.16) was developed (this also gives a more intuitive feel). The joystick stiffness switches between two curves depending on whether the endpoint motion is opposing the endpoint force or being helped by it. While each leg of the stiffness graph can have a different slope to tune the feel in each direction, analysis is simplified using a single slope. The joystick stiffness is dependent on the applied force and the direction of the endpoint:  =  Ii  —  feKrsgn(thTn)  (3.40)  Since the joystick stiffness does not remain constant within the deadband, the joystick may limit cycle as is oscillates about center with a finite ffzand. When using the maglev joystick (ref Chapter  Chapter 3: Force Feedback  33  4), the range of stable stiffness values is limited. To ensure stability of the system under large loads, the value of iç derived above is bounded by the stability range of the joystick.  Joystick Stiffness (Kr) Km  * neg  * pos  tension  Force Applied  compression  Figure 3.16 Joystick Stiffness vs Applied Force  As an analytical proof of this feedback control system is not yet available, the system was tested in a numerical simulation to ensure it would be stable, at least for the range of values applicable to the machine controller. Using nominal values for the hand and environment , it was found that the system was 3 stable for any reasonable (i.e. within the physical range of the joystick) constant input. Even for very low damping, the system eventually converged, and the convergence rate increased with the damping ratio. Some representative plots are shown in Figures 3.18 and 3.19. The simulation parameters used were: mi= 1000 kg, B —0, 6 x= 5 mm, Knom  Ke=  20 MN/rn, mm= 1 kg, xdb= 2 mm,  4 kN/m, Kr= 1.5/rn, Kvei= 0.25 rn/s/mm.  Chapter 3: Force Feedback  34  stiffness feedback  Figure 3.17 Stiffness controller block diagram  Phase plot constant input 10.0 N  5 xl0-  Slave position  E  8  C 0 Go  0 0 c/i  1  2  Master position (m)  3 3 xlO-  0.05  0.1  0.15  0.2  Time (s)  Figure 3.18 Master and slave response for stiffness control: fI d 10  =  10 N,  (  =  1  Chapter 3: Force Feedback  35  Phase plot constant input 60.0 N  3 xlO-  Slave position  5  0.!  0.4  /  4  /  0.3  /  3.5  13 0.1  2.5 2  0  1.5 -0.1  -  -0.2 -0.3  0.5 0  1  2  3  Master position (m)  4  0  0  0.05  3 xlO-  Figure 3.19 Master and slave response for stiffness control:  0.1  0.15  0.2  Time (s) ffand =  60 N,  C  0.3  Chapter 4 Maglev Joystick 4.1 Description An important feature of any force feedback controller is the master control or joystick. As an input device, it must convert the operator hand motion into machine commands. This can be easily accomplished with a wide range of handcontrollers  [51. Cheap computing power can  easily take the master control input and convert it into any desired format (coordinate frame). The difficulty in finding a master controller suitable for force feedback lies in finding one that can reproduce the desired forces on the operator’s hand. Various configurations are possible with the reflected force generated by actuators connected to the joystick handle. Most actuators are not ideal in that they have backlash, friction (static and dynamic) and large inertial loads. By using good mechanical design these problems can be reduced, but not eliminated [191. A recently developed hand controller based on the “Magic Wrist” [61, uses magnetic levitation to “fly” the handle of the joystick.  This removes all  mechanical linkages and their attendant backlash etc. Some additional features of the maglev joystick are programmable stiffness, damping and inertial parameters (within limits set by drive force saturation and control loop speed). The maglev joystick is currently being used as the hand controller for the current research in force feedback as its excellent backdriveability and force resolution are important.  4.1.1 Hardware The maglev joystick (or “wrist”) consists of four basic components:  a fixed “stator”  containing the magnets and position sensing diodes (PSD’s), a moveable “flotor” containing the voice coils and LED’s, a current driver/power supply and a digital controller. 36  Chapter 4: Maglev Joystick  37  flotor  stator  Figure 4.20 Maglev Joystick: Block Diagram  Figure 4.21 Maglev Joystick: Isometric view  Chapter 4: Maglev Joystick  38  Rotor Top— LED Vertco[ Mugnet lop PSI Mounts Vertco1 Co  Elotor  Horzontn Col  Support Post Mognets Stotor Bose  Figure 4.22 Maglev Joystick: Exploded view 4  The stator serves as the base and supporting structure for the high energy permanent magnets which provide the field that the fiotor can fly in. The range of motion is primarily constrained by the size of the stator gaps. Larger gaps permit greater motion but reduce the magnetic field strength (and hence the force that can be generated). Larger gaps (and greater motion range) also require larger PSD’s, which can only be mounted correctly if the overall size of the joystick is increased. As one of the design goals of the maglev joystick was a small mounting volume, the gaps were kept lOmm, giving a motion range of approximately ±5 mm.  Joystick design by S.E. Salcudean, drawings by C.T. Chen.  Chapter 4: Maglev Joystick  39  The flotor forms the mounting frame for the six coils and the three LED ‘s used for the positioning system. The three coils that provide vertical forces are mounted vertically, and the horizontal forces/torques are provided by the other three coils mounted between the vertical ones  (ref Fig. 4.22). The three LED beams are mounted a central post and are nominally directed perpendicular to the center of their corresponding PSD’s. As the primary limiting factor for applying large forces is the heat generated by coil dissipation, the coils are thermally bonded to the aluminum flotor frame to improve heat transfer. In series with each coil is a thermal fuse to cutout the drive current should the flotor temperature ever exceed 95° C. The aluminum skin also provides serves as a passive damper to improve stability through eddy currents. The current driver provides six channels to drive each coil with up to 10 A. This permits the application of up to 60 N to the flotor, though higher transients are possible. Current driver saturation and frequency response (currently  <  1 kHz) limit the maximum applicable forces which  in turn limits the joystick stiffness, damping and effective mass. The controller determines the flotor position and then applies the desired coil currents. Originally a TMS32OC3O DSP board served as the joystick controller. It was later replaced by a VME based SPARC 1E. It is the digital controller which gives the maglev joystick its versatility, as the various mechanical parameters of the flotor can be programmed, with the system capabilities only being restricted by drive saturation and controller update speed.  4.1.2 Capabilities The advantages of using the maglev joystick as a force reflecting master are varied. Primarily, it provides the operator with high fidelity feel of applied forces, not obscured by actuator stiction or backlash. The PSD ‘s used to determine the flotor position are analog devices, and so the limit on the resolution of motion is the A/D resolution and total system noise. Presently, positions  Chapter 4: Maglev Joystick  40  can be detected to less than 1 tim. In addition, the system can return forces to the operator with components of up to 100 Hz, though for stability reasons, some of the high frequency components are removed. The returned forces are currently restricted by the relatively slow speed of the digital controller. The speed could be increased by the addition of more processors and distributing the computing load, or the use of faster processors. For the hydraulic machine applications currently being investigated however, the slow machine response necessitates a reduction in the system bandwidth anyway. The magnitude of the applied forces is quite large (greater than the JPL hand controller [19]), though the maglev joystick cannot apply such forces for very long. This is seldom a problem as large forces are only required for a short time to give good (not mushy) feel and the operator is unlikely to wish to battle large forces for long anyway. The restricted motion range can be a problem to those operators that are used to greater motion and they may feel that precision control has been reduced. However, as the joystick has considerably better resolution than most analog joysticks currently in use, it can actually be used more precisely.  4.2 Control 4.2.1 Controller Hardware The controller that was first used to control the joystick was based on a TMS32OC3O DSP  processor hosted by a standard AT compatible computer. The development environment was MS-DOS, using a C cross compiler for the DSP code generation. Communication between the PC host and the DSP controller was through the ISA bus. The DSP processor performed all of the calculations and analog I/O necessary to fly the joystick, while the PC only handled console I/O and serial communication. The control loop processing was handled by an interrupt routine called at fixed intervals by a timer interrupt. Idle  Chapter 4: Maglev Joystick  41  time was used by a monitor program [20] running on the host to obtain the values of global variables for debugging information. In addition to reading values, variables could also be set, permitting the controller parameters to be tuned quite easily. For ease of integration with existing computer systems on the log loader, the joystick controller code was ported over to a SPARC 1E card running VxWorks. The existing machine controller was housed in a VME card cage, and the use of a SPARC card for the joystick controller permitted using a common environment. An additional benefit of using VxWorks was a much richer development environment. As a real time multitasking operating system, it was possible to run multiple simultaneous tasks, one of which was an interactive shell to read and set global variables. Library routines were provided for timing of individual routines and an additional application package (StethoScope) permits the real-time collection and plotting of data. Unfortunately, the benefits of VxWorks running on a SPARC did not come without a price. The DSP board proved to be 2—3 times faster at running the joystick controller code than the SPARC 1E. While there was still plenty of processing power to accomplish the basic joystick control, the SPARC based system did bog down more quickly as the simulation task (Chapter 8) increased in size. A possible workaround would be to distribute the processing among multiple SPARC boards, each running VxWorks and communicating over the VME bus. While the code has been written as multiple independent tasks to simplify such an implementation, it has not yet been attempted due to a lack of time and hardware.  4.2.2 PD The control algorithm implemented so far has been a basic PD loop, with a small integral term which is limited outside of the joystick center area but is sufficient to eliminate the steady state eor inherent in a PD control scheme.  Chapter 4: Maglev Joystick  42  IReadAfD  I PSD voltages  Compute light spot positions Spot positions (mm)  Compute position via direct kinematics  4,  Flotor translation/rotation  4,  Desired actuator forces (N)  PD control  Force to current transformation  Coil currents (A) Output to D/A  4’ Wait for next sample period  Repeat  Figure 4.23 Joystick Control Flow  The controller input is four analog voltages from each of the three PSD’s. The PSD voltage data is used to compute the position of the LED spot on each PSD. Once the spot locations are know, the location and orientation of the flotor can be determined relative to the stator coordinate frame using geometry and knowledge of the joystick dimensions. The flotor translation is computed in mm while the angle is computed as the vector part of the Euler quatemion for computation savings [6]. For the small angle approximation which applies to the joystick, the physical interpretation of the angle components is approximately the rotation about the axes of the stator frame in thousandths of radians.  Chapter 4: Maglev Joystick  43  Once the position is known, the joystick controller simply a PD controller to the position error. The joystick degrees of freedom are approximately decoupled, so the stiffness and damping parameters can be independently specified. The range of stable values for the PD gains was determined through empirical tuning (manual tuning was the simplest and safest approach to minimize the chance damage to the joystick). Using tuning rules based on Ziegler-Nichols ultimate stability criterion did not improve the system performance. The desired control inputs in the stator frame are transformed to desired control currents in the coil frame through a fixed matrix (another small angle approximation) and the control system output then controls the coils via six current drivers. This control algorithm is capable of flying the joystick flotor quite satisfactorily and has formed the basis of more complicated controllers incorporating force feedback.  4.3 Parameter Identification While the PD control algorithm is simple and quite robust, the joystick could potentially be controlled more effectively if feedforward and other techniques are applied. More advanced control techniques require better parameterization of the joystick. Currently, only the mass of the flotor is known, and this is used for a simple gravity feedforward compensation. The center of gravity location was originally assumed to be located at the center of the joystick, but this led to steady state offset errors in the joystick orientation. The flotor is close to but not completely symmetrical, and this offset was caused by the extra weight of the coil and LED connector on one side. Shifting the center of gravity location slightly to one side (in the feedforward code) was able to correct this. The mass and center of gravity feedforward terms are static only. Inertia cancellation of the flotor will permit the operator to have much more delicate control of the endpoint. While this  Chapter 4: Maglev Joystick  44  is unlikely to be significant in hydraulic machinery applications (!) it could be quite useful to cancel out the tool inertias etc in assembly tasks. These feedforward techniques require a more complete characterization of the joystick parameters. For this reason, estimation of the joystick parameters was attempted. The procedure used is presented in [211 and summarized in [221. Newton’s and Euler’s law for the joystick fiotor are:  f  =  —  mg  (4.41)  n=Ic’+wx (I&)+xf where: c is the vector to the fiotor center of mass,  f, n  are the force and torque vectors applied,  is the acceleration of the flotor center of mass, and I is the inertia of the fiotor about its center of mass. Using the parallel axis theorem and the following definitions:  0 [wx]  0  —W  [.w]  Wy  ‘Z  w  2 —w  W  0 0  2 W  0  W  0  W  2 W  0 0  0  2 W  0  W  2 W  W  W  0 0  (4.42)  one can now write a single matrix equation for the forces and inertial parameters. With p the flotor origin:  m me 2 me  fx  2 me —  —  F— 0  [  [Jixj+[Wx]{Wxj  [(g  —  j) xj  0  1  [.] + [Wx][.w] J  443 ‘12  flY  ‘13  flz  ‘22 ‘23  133 or more compactly:  w  = Aq  (4.44)  Chapter 4: Maglev Joystick  45  It is now a simple problem to determine the unknown vector  4.  As one has only six known  values and ten unknowns, at least two instances must be used. Augmenting A and w with data from multiple data points permits q to be determined by a least squares fit i.e.  4’ = Aw  =  (ATA)ATw  (4.45)  The parameters could also be estimated in a continuous fashion by using a recursive least squares algorithm. The force and torque vectors  f, n  are the applied forces and torques to the fiotor, and the  flotor accelerations and angular velocities are measured by the optical position sensing system. As such, the inertial parameter estimates depend on the correct calibration of the controller output units (forces and torques) as well as the controller input unit scaling. The joystick computations proved to be quite sensitive numerically to the units used, with poorly conditioned matrices considerably degrading performance. A direct calibration using a force/torque sensor (JR3) was ineffective as the force/torque sensor gave incorrect readings due to incorrect mounting of the sensor (the force sensor requires thick mounting plates to prevent deformation of the mounting surface). While it would be nice to pursue it further (correctly mount the sensors etc), this particular avenue of research is not directly needed at this point.  Chapter 5 Interprocess Communication  The computer simulation developed uses multiple independent processors, each performing specific tasks. While it may have been possible to run the whole system on just the SGI Iris VGX, that system lacked the necessary I/O, and is currently running Unix. As a multitasking system, Unix is fine, but given the high update rate necessary to fly the maglev joystick (minimum rate l 30 Hz), the minimum timing resolution of Unix (60 Hz) is insufficient. In order to overcome this limitation, the task of flying the joystick was given to a separate processor. While multiple processors are able to address the performance requirements of the system, they do introduce the added complication of interprocess communication.  5.1 PC & DSP system  The original joystick controller used a ‘386 based PC clone as the host and development environment for a DSP controller mounted on the PC bus. The DSP controller board contained two channels of A/D and D/A on the processor itself as well as two synchronous serial links. The board also contained the DSP memory and glue chips necessary to communicate with the ‘386 through the PC bus. In addition to the on chip analog I/O, the system also contained 32 channels of A/D and 16 channels of D/A on additional boards mounted in the PC. The DSP processor communicated with the extra I/O cards through a dedicated parallel bus (DSP-Link).  46  Chapter 5: Interprocess Communication  47  Operator Viewpoint  Operator Figure 524 Machine simulator block diagram  The joystick controller running on the DSP processor had to communicate with the SGI running the graphical and dynamic portions of the simulation. Possible communication methods include: •  ethernet communication through additional card in PC host  •  synchronous serial communication using DSP serial link  •  parallel communication via DSP-Link  •  serial communication via PC host  •  parallel communication via PC host  •  analog communication from DSP D/A to AID in SGI  Ethernet communications would require the addition of a card and software drivers to the PC host, as well as a driver routine running on the PC to take values from the DSP and transmit them to the SGI. As this would have required quite a bit of additional hardware (all commercially  Chapter 5: Interprocess Communication  48  available though) it was not pursued. Synchronous serial communication using the on chip serial ports has the advantage of direct communication and bypassing the PC host, but unfortunately it would require custom hardware and software drivers on the SGI side of the link. The same would be true of parallel communication via the DSP-Link. Analog communication would permit direct communication and could use available hardware and minimal software, but it would require a more complicated cable and would also be more susceptible to noise. Also, additional analog boards for the SGI would need to be purchased. Communication through the PC host’s I/O ports requires more software and additional complexity, but has the advantage of minimal hardware requirements. The method finally chosen was to communicate using the serial port of the PC host connected to a serial port on the SGI. Both ports conform to RS232C standards and are interfaced directly (though the SGI does use an unconventional connector).  While a parallel interface could  potentially provide faster throughput, with the additional cable and interface requirements on the SGI side, this alternative was deemed to be not worthwhile.  5.1.1 Serial Communication Hardware: Speed limitations The serial link required a driver to be written for the PC side to handle interrupt driven serial communication at 38.4 kbaud. While the PC could handle faster speeds, the SGI was limited by Unix. Unix directly supports character transmission rates of up to 38.4 kbaud, though the effective throughput was lower for the SGI. The priority of the interrupt handler for the SGI serial port was too low, so it was unable to communicate bidirectionally at greater than 9600 baud while running the graphics software. At speeds greater than 9600, the SGI delayed its transmission of characters until after its receive interrupts ceased to be triggered. The highest speed was chosen  Chapter 5: Interprocess Communication  49  to permit the PC to unload its data to its buffers in the minimum time at possible, and give it more time to work as a debug monitor. While the SGI operating system could probably be tuned to improve serial port throughput, it would be at the expense of overall system performance. The basic bottleneck with serial communication was the interrupt overhead associated with the UART handling each character separately. While high speed replacement chips are available for PC serial ports that can handle larger character blocks in hardware, they are not currently available for the SGI. Software Initially, the serial interface was used bidirectionally to communicate between the graphic  and dynamic simulation running on the SGI, and the joystick controller running on the DSP board hosted by the PC. The information sent from the PC to the SGI were six position values (one for each degree of freedom of the joystick) which were then interpreted as the command input. The SGI computed the dynamic forces at the endpoint based on its machine simulation and returned six values for the reflected forces (again one for each degree of freedom). As the serial link speed was marginal, the format of each number sent was a 32 bit float packed into four 8 bit characters, rather than sending each number as its ASCII representation. The floating point format on the PC side is the reverse byte format of the SGI, so the PC handled the conversion to and from a common floating point format. While the data transferred could have been reduced further by packing each number into 16 bits (the maximum resolution of the analog I/O), this would have increased the computational burden on the SGI, and could have at best doubled the transfer rate. As occasional characters were lost on the serial link, the communication code was extended to ensure that such character loss would not corrupt the data flow. Each packet of six floating point numbers was bracketed between start and stop flags. As no individual character would be  Chapter 5. Interprocess Communication  50  unique, the start and stop flags were set to be unique floating point values (+1NF for the start flag and —INF for stop flag). Using these flags, the computers could synchronize at the start of each packet, and ensure the packet was complete by looking for the stop flag. Corrupted packets were discarded. As less than 1% of the packets were lost, the simulation was not significantly affected by the loss (previous values were kept) and more complicated error correction schemes were not required. While the Unix operating system supports high speed interrupt driven serial ports with its tty interface, the PC required the development of an interrupt driver in assembly to directly control the UART of the PC serial port. The driver had many problems in its development. It was derived initially from code developed for PC to PUMA communications and that code had its roots in example code in [23]. The original code was buffered for reads only, and blocked writes until the previous character was transmitted. Addition of an interrupt handler for transmitting, to permit buffering of the output, was only partially successful. While communication was fine between two PC’s, when communicating with the SGI, the transmitter would lose interrupts and crash. This was probably due to timing differences between the two machines UART’s, together with subtle bugs in the interrupt handling (hardware or software). A work around was eventually developed using a pseudo interrupt approach for the transmitter (characters would only be transmitted as long as the receive buffer was interrogated for characters). This approach provided reliable serial communications. While the PC was capable of communicating with another PC using this code at the absolute maximum speed of the UART bit clock (> 120 kbaud), the Unix side was limited to 38.4 kbaud as noted above.  5.2 VME implementation To simplify the machine implementation and use the better debugging facilities available,  Chapter 5: Interprocess Communication  51  the joystick controller was moved from the DSP system to a SPARC 1E running VxWorks. In this instance, the controller now had a serial port compatible with RS232C which could be directly interfaced with the SGI running the graphics. In VxWorks, serial communication is handled in much the same way as Unix, and the communication became a simple port of the code developed for the SGI. The only complication lay in enabling the buffering of serial port data, as the underlying routines are handled differently under VxWorks than Unix. One change that was made to the communication was related to the transfer of the dynamics code to the joystick controller. As the SGI needed to only receive the joint positions, and no longer returned forces to the joystick, the code was simplified to become a unidirectional link.  VME Bus  SGI Iris Figure 5.25 VME based simulator  As the SPARC also has an ethernet connection built in, attempts were made to use the network as the communication route between the two machines. The basic Unix construct for such communications is the socket. Code was developed to test this method, but the overhead  Chapter 5: Interprocess Communication  52  involved in the sending of packets over the network was sufficient to slow the transfer down significantly. While the raw speed of ethernet is 10Mbps, the best rate that could be managed (even on a single machine, without network contention) was only 1.5 Hz for a “packet” of 32 bytes (170 bps). Possible ways to improve the transfer rate would be to ensure that each “packet” of information containing the joint data would only correspond to one IP packet. This was not pursued further, as the serial link was fast enough to move the bottleneck from communication to the graphics (though it would be nice to get rid of the cable!).  5.3 Machine implementation The serial communication task with the graphics process is unnecessary for the installation of the force feedback controller on the actual machine. In this instance, the joystick controller must communicate with the excavator controller mounted in the same YME cage. The main controller for the excavator is a transputer based system with a SPARC running Unix to act as the file system host. Possible communication methods include: •  file read/writes via Unix host  •  sockets (network communications)  •  serial link  •  globally accessible memory  The only possibilities that would be fast enough would be serial communication, in the same fashion as the simulation used, or using common memory as a mailbox. Fortunately, the transputer contains dual ported memory which is accessible via the VME bus, so this is being used for high speed communication between the SPARC running VxWorks and the transputer based excavator controller. Communication performance in this instance exceeds the processing speed of both systems by a wide margin. Catastrophic failure of transputer code is possible however if the  Chapter 5: Interprocess Communication  53  mailbox area is used for code or data when the VxWorks processor is attempting to communicate with it. This can be avoided if all code written for the transputer reserves and avoids the space for the mailbox, whether or not it will be used, or through the use of a dedicated memory card for use as the mailbox.  CAT 215 Log Loader Figure 5.26 CAT 215 controller  Chapter 6 Graphics Simulator  6.1 HOOPS The graphical simulation of the operator viewpoint from the cab of the simulated machine was implemented in HOOPS. HOOPS (Hierarchical Object Oriented Programming System) is a set of graphics routines that can be linked with either C or FORTRAN source (all current work uses C source code). While previous work [181 used the native graphics system of the Silicon Graphics workstation (IRIS GL) for the simulation of an excavator and a log loader, the system was changed to HOOPS for a variety of reasons.  6.1.1 Advantages / Disadvantages HOOPS is available for several platforms. The graphics system used previously was restricted to running on a SGI, of which only one is available. As a several projects require the use of the SGI, access was quite limited, and the ability to develop graphics applications on the considerably more plentiful Sun workstations was very useful (HOOPS is also available on PC’s and other micros). HOOPS is also moving towards becoming a graphics standard as many graphics programs and computer aided design (CAD) programs (e.g. VersaCAD) are being based on it. •  Development time of new applications is quite a bit faster. HOOPS is organized into a structured set of objects and subobjects which considerably simplifies the development of linked objects such as a manipulator. The basic structure of a robot can be set up in a few hours. 54  Chapter 6: Graphics Simulator  •  55  HOOPS include many features to handle hidden faces, lighting etc and perspective, making the implementation of such features trivial. In addition, system functions are provided to handle timing and input events.  •  The graphics program which had been previously developed for the excavator and log loader had grown over a few years and several programmers. As a result it was large and difficult to modify further. Development of the basic graphics code for another (more complicated !) machine, the feller/buncher in HOOPS resulted in a significant reduction in the amount of source code ( 20 kbytes vs  -  200 kbytes).  While HOOPS was chosen for several of its advantages, it was not perfect. Graphics code written in HOOPS was approximately half the speed of similar code written in the native graphics format of an SGI. This was not considered to be a significant problem, as the new SGI was more than twice as fast as the old one.  6.2 Simulation evolution The first version of the feller/buncher simulator used the SOl to run the dynamics simulation of the load as well as the graphical simulation of the operator viewpoint. The maximum update rate for the graphics with all faces on and lighting was 30 Hz when double buffering was enabled (to reduce fficker). This version of the simulator served to test the inverse Jacobian approach to resolved motion control, but had difficulty when used with force feedback. While the first bottleneck in the simulation loop was the graphics update rate, this could be eliminated by subsampling and reducing graphics update rate with respect to the dynamic update. Unfortunately, even with no graphics delay, the loop update was held back by the communication delay between the joystick controller and the dynamics simulation. The absolute maximum serial update was 120 Hz, which caused oscillations in the fedback force. To eliminate the communication delay  Chapter 6: Graphics Simulator  56  between the joystick controller and the dynamic simulation, the dynamic calculations were moved to the joystick controller, and the code running on the SGI was reduced to a graphical output of the machine joint angles.  Figure 6.27 Operator viewpoint from HOOPS simulation  Chapter 7 Machine/Operator Considerations 7.1 Workspace The operator interface of a teleoperated manipulator can have a significant effect on the training period and the operator workload.  Intuitive interfaces in which the operator can  manipulate the endpoint position within his own coordinate frame are much faster to learn than systems which require the endpoint to be controlled in joint space. While it is possible for a well trained operator to control a machine in joint space faster than a beginner using resolved motion, typically the training period required to achieve this speed is on the order of months. The scaling between the command movement and the corresponding slave movement affects the trade-off between precision and effort.  A large master workspace can give the operator  greater precision on endpoint placement, but requires more effort to operate, while a small master workspace would provide the opposite.  7.2 Force Sensing Force feedback naturally requires some method of measuring the forces on the machine. For a hydraulic machine, various possibilities exist, including derivation from hydraulic pressures, measurement of link deflection or simply using an endpoint sensor. The two methods investigated for the log loader experiment were using the main hydraulic pressures and a load cell mounted between the stick and grapple.  7.2.1 Pressure Transducers Pressure transducers initially showed promise as the easiest and most robust method of determining the endpoint forces. Hydraulic pressures are used in the Haztrack remote operator 57  Chapter 7: Machine/Operator Considerations  58  [151 to determine joint torques. In this case however, the joint torques are returned directly to a kinematically equivalent master and are not resolved to the endpoint forces. Arm dynamics are included in the torques returned. To permit a transparent feel of the load the arm dynamics should be removed. This requires knowledge of the link mass and inertial parameters. Uncertainty in these parameters contributes to errors in the computed end point forces, as do errors the pressure measurements. Friction, especially static components, are difficult to model and can potentially have a larger effect. For these reasons it is generally recommended to measure desired parameters as closely as possible and avoid unnecessary derivations [21]. Pressure transducers do have the advantage of being robust. The sensors themselves can be mounted in a safe location away from the machine endpoint, and the machine physical integrity does not have to be altered. These advantages can be significant when considering a practical system for use in the field.  7.2.2 Endpoint Force Sensors Endpoint force sensors are the most direct method of obtaining the necessary feedback. Placing the sensor at the endpoint bypasses the problem of link estimation and increases the usable bandwidth of the returned signal.  It does have the added complication of requiring  modifications to the machine. As an endpoint sensor typically measures the forces as strain in a deformable member, the sensor itself is usually the weakest component of a manipulator. Increasing the strength of the sensor will increase the size of the minimum force resolvable (typical resolution for load sensors is from 0.1% FS to 0.01% FS). A trade-off must be made between fine resolution and sensor strength. Other considerations when using a load sensor are fatigue strength and resonant frequency of the combined sensor load system.  Chapter 7: Machine/Operator Considerations  59  7.3 Force/Position Scaling  7.3.1 Force As the slave manipulator in hydraulic equipment applications typically has a force application range of greater than 100 times that of the operator, scaling of the feedback forces is a necessary. The forces at the endpoint can be up to 75 kN, (as determined by maximum hydraulic pressures) though the machine would flip over long before this limit is reached (the loader currently used for machine experiments is limited by the current controller to approximately 20 kN maximum endpoint force), while the maximum force applicable by the operator is more like 100 N, and this would be extremely fatiguing to apply for an extended period. A trade-off between sensitivity to small loads (better feel) and the ability to handle large loads must be found. The force returned to the operator can simply be scaled down by a constant factor, though a better solution is to provide the operator with some method to adjust the gains while working. In addition to scaling the returned forces, there should be the provision to handle the subtraction of constant offsets from the endpoint forces. This way, the operator would be able to grasp and carry a large object, and still be able to have the feedback gain large enough to provide useful feel of perturbations in the endpoint force. The constant offsets should also be under the control of the operator. While the control workload of the operator could be reduced by automating the scaling and offset adjustments, any automatic system would introduce nonlinearities in the operation of the control system, and could cause confusion. For example, if the system automatically compensated for an offset force (after some time delay) the operator would be unsure whether the load being felt is low because it actually is low or because the load is large, but it has been automatically  Chapter 7. Machine/Operator Considerations  60  compensated for. Until such a time as the control system can anticipate the desires of the human operator, load scaling is best left to the operator.  7.3.2 Position Control of the endpoint position of a slave manipulator using a master with a smaller physical workspace will require some scaling between the manipulator and the master. Using pure position control would require the complete machine workspace ( 360 ° rotation, radius from 1.5 to 8.5 m, height from —5.5 to 8 m [24]) to be mapped to the considerable smaller master workspace ( 1 cc). This corresponds to a position ratio of 1000: 1 i.e. 10 cm precision in the machine workspace would require 0.1 mm in the operator workspace. This would be next to impossible under the best conditions and completely out of the question in a moving machine. Position control would therefore require restricting the slave workspace to a subset of the complete workspace and then indexing the subspace through the rest of the slave space as necessary. Indexing the position subspace would be accomplished by moving the endpoint in rate mode or explicitly redefining the endpoint center position through the machine (or simulation) controller. One way to incorporate position indexing into the same hand controller as used for position control is to define a rate control region of the joystick in the outer portion of the master. In this way, the operator can control the endpoint in position mode, and to reach beyond the position workspace would naturally push harder, to the limit of the joystick, thus causing the endpoint to move in rate mode toward the desired position. A problem with the direct implementation of this method is that the endpoint will “snapback” when the joystick is returned to the center. It would not be possible to touch a surface in position mode, except at the very edge of the position workspace. This can be worked around by introducing a deadband on the position command until the joystick handle returns to the center. This way the operator can approach an object in rate mode, then  Chapter 7: Machine/Operator Considerations  61  return the joystick to the position mode to stop motion. The endpoint will remain fixed until the joystick is returned to center, after which the operator can move the endpoint in position mode. Rate control permits an arbitrarily large workspace for the slave to be controlled with fine precision, with the endpoint velocity limited by the maximum joystick rate. This situation is more physical than the position control case, as the machine joints are limited in speed by power and safety requirements. In position control, the operator could easily command a velocity greater than that which is possible, with the result that the operator hand becomes out of phase with the slave position (and hence fedback forces). While it is possible in the rate control case to exceed the maximum possible accelerations, it is less of a problem than velocity overshoot. In both  cases, a coordinating force to give the operator a feel for the arm inertia can help reduce these overshoot errors, though like any proportional control scheme, it can not eliminate it.  Chapter 8 Simulation Experiments The goal of this research is the implementation of a useful force feedback system on a hydraulic machine.  To speed up development of the system and to permit greater latitude  in experiments, a fellerfbuncher simulator has been developed which integrates the maglev joystick with a real time dynamic and graphic simulation. The system was used to test different control methods (position, rate and hybrid schemes) and different feedback methods (force, force derivative and stiffness). It also served to test integration of the maglev hand controller to a machine controller, as well as test resolved motion control using the Jacobian approach rather than direct inverse kinematics. The complete simulation system consists of the force reflecting hand controller, a machine controller, load dynamics simulation and a graphical simulation of the operator viewpoint from the machine cabin (Fig. 5.24). Several different versions of the simulation system were developed as hardware became available.  Basic operation of the control system was tested and many  preliminary tuning tests were completed prior to the installation of the force feedback system in the log loader. Originally, the feller/buncher simulator resided on the SGI, along with the code to produce the graphics display (Chapter 6). The dynamic simulation implemented the computation of the inertial and gravity loading of the grapple  in  response to the commanded motion by the joystick.  As described in Chapter 5, the dynamic simulation communicated with the joystick controller via a relatively slow serial line. This initial simulator operated in rate mode only, with the joint velocities determined by a straight forward implementation of the inverse Jacobian. It provided a test bed for this control 62  Chapter 8: Simulation Experiments  63  method, as well as some preliminary force feedback experiments. It was found that direct force feedback was unstable at any feedback gains large enough to be detected. Similar results were obtained for force derivative reflection. Stiffness control resulted in much better feel of the load dynamics. A simple linear relation was used (3.33) with no deadband. Oscillation could still occur (and did occur) when  0,  as would be expected given (3.36), but the system quickly stabilized once the joystick handle was released. The next version of the simulator moved the dynamic portion of the code to the DSP processor to eliminate the effects of communication delay. The simulation was simplified to one dimension, which permitted the trivial implementation of a position control scheme. Initial experiments attached a virtual mass to the flotor, and fedback forces to the hand as if the flotor mass had been increased by m1 d. In addition to the virtual load, virtual walls were placed at the 0 limits of flotor travel to experiment with hard surface contact. As the accelerations derived from the position sensors were quite noisy, they were filtered before calculating the inertial forces from the virtual load.  Zero order hold  Figure 8.28 Virtual mass simulation on DSP controller  Chapter 8: Simulation Experiments  64  Experiments with this simulation determined that for optimum feel, the cutoff of the acceleration filter should be around 100 Hz for a sample rate of 1 kHz. Experiments varying the virtual load found that the maglev joystick parameters could be varied over a wide range. For example, the mass could be varied from -0.5 kg (i.e. most of the fiotor mass could be cancelled) to 5 kg. In addition to testing the position mode force reflection capabilities of the maglev joystick, the simulation was also tested in rate control mode with force reflection. The unnatural feel (mass feels like damping etc.) predicted in (3.28) was confirmed, as well as instability in contact with a hard surface.  When the equipment became available, the DSP based simulation was ported over to a VME controller (Fig. 5.25). It was first tested with the same one dimensional model used in the DSP simulation. The previous results were confirmed, though the actual values for the maximum virtual loads were slightly smaller as the maximum loop rate was quite a bit less (350 Hz).  The complete feller/buncher dynamic model was then ported to the VME controller. This slowed the maximum speed of the system further, and considerable time was spent optimizing the code to get the ioop rate back up to a flyable level (200 Hz). A position control scheme with joystick limit indexing was implemented, as well as the simple rate control based simulation that ran on the SGI originally. Code was tested for both a “snapback” as well as a position space deadband model. When the force feedback was implemented, a 17 Hz vibration became noticeable. Further examination revealed that it was present at all times and was merely magnified by force reflection. The source of the vibration was finally traced to the Jacobian convergence as the joint increments were applied to the model. This was eliminated by computing the Jacobian iteration off-line, and only applying a joint command after the final joint value had been arrived at. This unfortunately reduced the input command rate significantly, as only one iteration cycle  Chapter 8: Simulation Experiments  65  could be completed for each sample period. Input commands are now limited to one tenth the main loop rate (which should be accurate with sufficient filtering). The simulation experiments proved to be quite useful background for the development of the force feedback controller. With the addition of more powerful computers and a more realistic simulation, it is hoped that the simulation can be used to expand further on the results available from the machine experiments.  Chapter 9 Machine Experiments  The force feedback control system was implemented on a working machine. The machine used for this is a Caterpillar 215 excavator. It can be fitted with various end effectors (bucket, grapple, felling head), and is cunently outfitted with a grapple for log loader experiments. This particular machine has been the subject of other research, specifically parallel dynamic simulation [25], complex hydraulic system simulation [17] and various experiments in resolved motion control [10]. The Caterpillar 215 excavator was initially outfitted with a VME based controller, which interfaced to the actuators via proportional control valves. Arm movement can be controlled either in individual joint control mode or in resolved mode. The original master input device is a conventional four degree of freedom joystick (master position sensing is via potentiometers). Pressures can be measured in both the main actuator cylinders and the pilot system. Joint positions are sensed by resolvers (i.e. they provide absolute position). Velocity and accelerations of the joints must be derived from the joint position. The system required the addition of some form of endpoint sensing. The first machine experiments involved trying to model the machine well enough to obtain endpoint forces from the existing pressure transducers. These had some limited success, though did not provide the desired force resolution. Experiments with a load cell yielded higher resolution, but the sensor did not prove durable enough, and force control experiments were completed using pressure derived forces.  66  Chapter 9: Machine Experiments  67  9.1 Endpoint Force Sensing  There are two basic approaches to determining the end point forces on the actual machine. The first and initially most straight forward is to simply mount a sensor at the endpoint that is capable of measuring the desired forces directly. Such a method is computationally simple and is recommended for optimum resolution/performance [21]. The other approach is to try and determine the endpoint forces by deriving them from forces that are more easily measured using existing transducers. This avoids the mechanical complications of an endpoint sensor, but introduces the additional problem of accurately modeling the intervening links in order to subtract their effects. Both methods were tried on the excavator.  9.1.1 Load Cell As initial attempts to derive the endpoint forces from hydraulic pressures showed that the best force resolution possible was 1 —2 kN, a load cell was purchased and installed between the grapple and the end of the arm in order to provide direct measurement of the endpoint forces in the Z direction. This method avoided the difficulties in machine parameter estimation but introduced the additional complications of mounting the transducers and instrumentation amplifier. The mechanical complications involved in using a load cell turned out to be quite a bit bigger than initially anticipated. The load cell purchased was a single axis unit manufactured by Wagner Instruments. It was a low profile style, consisting of a central mounting hole and an outer mounting ring, with four deflection members connecting the two. One side mounted to the grapple and the other to the end of the stick. While this style was recommended as the best for handling side loading, there were no specifications available for bending moments. The unit was designed for 10,000 lb (45 kN), with a safety factor of 150%.  Chapter 9: Machine Experiments  68  The first problem involved fixing the grapple from rotating in the Z direction, as the only force holding it in one direction was friction between the washer and the stick surface. This proved to be insufficient, and pins had to be added to prevent this rotation. The load cell was then used to determine the grapple weight (4060 N) but was quickly damaged after contact with the ground. As at all times, the forces exerted were less than half of full scale, the probable cause of failure was permanent deformation due to bending moments. Initially the damage seemed to simply be a nonlinearity about zero, but it quickly grew and failed outright after 2 days. The load cell was left in place after the failure of its strain gauge bridge in order to save grapple remounting time. However, the load cell broke apart during later thals, requiring its removal anyway (as well as the replacement of damaged grapple hoses). Examination of the cell afterward showed that two of the deflection members had failed in torsion and two in shear. The probable cause was an excessive bending moment about an axis perpendicular to the Z axis. The basic problem with the load cell was that it was a unit that was not designed for the job. A six axis force sensor that is designed for multi-axis forces would have worked much better. Unfortunately, none were available on short notice, and the cost was from 5 to 20 times that of the single axis load cell.  9.1.2 Pressure Sensors The hydraulic force applied by the machine can be computed from the pressures applied to the cylinders and cylinder geometry. 1 F  =  (9.46)  —  The pressure transducers have an error associated with them, typically from ±0.5 to ±0.1 % of full scale output (FS). Errors in computed forces will then be: =  zp(A +  otit) 4  (9.47)  Chapter 9: Machine Experiments  69  Instead of two absolute transducers, a differential and an absolute transducer could be used:  1 = PdjffAj F 1 + PQut(AjT  —  ) 0 A  (9.48)  where Pdff =  —  0 P  (9.49)  If both the differential and the absolute transducer have the same error, then this will not provide any advantage. However, the differential can have a lower range and hence a lower absolute error. In addition, the percentage error can be quite a bit lower. In the instance of the excavator currently being used, the absolute transducers have errors of ±0.5% of a full scale of 5000 psi  (25 psi). High accuracy differential transducers (±0.1 % FS) were ordered in a effort to reduce the computed force errors. Theoretically, the computed force errors should have been halved. Unfortunately, one was not within the specified error tolerance and any accuracy improvement from the remaining unit was completely obscured by other factors such as stiction, unmodeled dynamics etc. The differential transducers gave no noticeable improvement over the original absolute units. Using the machine geometry and the forces determined above, the applied torque at the joint can be determined. Either pure geometric principles or virtual work can be used, with the principle of virtual work taking slightly less computation. Using virtual work, the joint torques  can be computed from the cylinder forces using:  jojTt = T  Fl : 8 )  (9.50)  The applied joint torque consists of the dynamic components due to the excavator motion, the dissipative components from friction, and the torques due to the endpoint load. While the viscous portion of the joint friction can easily be determined, the stiction component is more difficult to  Chapter 9: Machine Experiments  70  model and requires knowledge of the endpoint forces. Previous work by Sepehri [17] indicated that friction components were not significant, and so these terms were neglected. Dynamic joint torques are determined by a recursive Newton-Euler algorithm [26]. The endpoint forces, Fend follow from: Tdyy’rrj =  D(q)j + C(q, )ci + g(q)  (9.51)  fy FerLd  =  (J(q)T)  t 1 j 0 ‘(r  —  Tdynumics)  (9.52)  Note that all six force components can only determined for a 6 DOF (or greater) manipulator. Force measurement for machines with fewer DOF’s are restricted to the number of independent torques available. For example, using just the boom and stick pressures of an excavator, only the  f  and  2 f  forces can be determined.  The dynamic components were initially determined by an implementation of the algorithm by Angeles [26] for recursive Newton Euler of a rigid manipulator. The mass and inertial parameters for the particular CAT 215 machine were taken from the simulation work by Darrell Wong[25]. The forces derived did not coincide with reality however. The calculated forces showed a lateral component when the only endpoint force was the weight of the grapple straight down. In addition, the computed vertical weight varied throughout the workspace, despite the fact that the grapple mass did not. Evidently, one or more of the assumptions about the machine model were incorrect.  9.1.3 Link parameter estimation The original source of the link inertial parameters was not locatable, and the manufacturer was unable to find this data. With no measuring devices capable of determining these values  Chapter 9: Machine Experiments  71  directly, an attempt was made to estimate them using pressure data. To simplify the problem, as most force control situations involve minimal movement of the endpoint, the quasi-static case of  Tdyrurjcs  = g(q) was examined. In this instance, only the mass, center of gravity locations  and nominal end forces due to the grapple need to be found. Further simplification restricted force computation to the RZ plane, so only the two endpoint force components  T f  and  2 f  can  be measured.  fZ  3 cg R a 3 a f  Wstjck  r  Wgrap Figure  9.29 Static machine forces  The nominal joint torques due to gravity can be derived quite simply from the geometry. = VV tjck(Cg3rC23 8 cg3ys23) + Wgrapa3C23 —  (9.53) , 0 Tboori  =  Tgtjk,m  +  boor,(C92xC2 4 T T  —  cg2ys2)  + ( VVgrap +  tk)a2c2 8 W  where S2 s1n(02)  C2  COS  (02)  S23  Sifl  (023)  =  C23  COS  (023)  =  (9.54)  sin (02 + 03) cos (02 + 03)  Rearranging to find the independent parameters that can be estimated gives: S23 —  It =  FThoo’rnnoml  L  TStjck,om  [a  i La =  b c b 0  dl  C23  0]  S2 C2  (9.55)  Chapter 9: Machine Experiments  72  where a  —  b  =  c  =  d  =  —W k 8 cg3  + Wgiapa3  (9.56) —WbooT,tcg2y  WOQ,cg2. + (1V +  1’Vtk)a2  Pressure data for various boom and stick angles was recorded and sorted to obtain static values only, and then converted to the applied joint torque L ji. The joint torques and angles were then 0 substituted into (9.55) and the equation solved for a,b,c and d via a least squares fit. Using data from several runs in different parts of the machine workspace resulted in the following estimates.  Parameter  Average Estimate (Nm)  Std. Deviation (Nm)  a  511  763  b  6452  909  c  -7585  1290  d  82729  993  Table 9.1 Link Parameter Estimates: average of data collected May-June 1992  As one can see, the estimates do not converge very well (the above estimates are based on “300 data points). Some of the error will be due to random errors in the pressure readings, while some is due to incomplete modeling of the joint torques. A likely contributor to the estimation errors is static friction in the cylinders and joints, which will have its greatest impact in the quasi-static case being examined. The parameter estimates also depend on the particular part of the workspace the machine is operated in. Repeating the above measurements for a couple of runs where the endpoint was just moved in and out just above the ground resulted in somewhat different estimates.  Chapter 9: Machine Experiments  Parameter  73  Run #1 Estimate (Nm)  Run #2 Estimate (Nm)  a  608  655  b  7571  6680  c  -3332  -6672  d  79200  82100  Table 9.2 Link Parameter Estimates: Ground level mapping. Aug. 13 1992  As the variation in parameter estimates could have been caused by changes in the machine over the summer (aging of grease, transducer drift) another complete mapping run was done to obtain a current set of estimates for the entire machine workspace.  Parameter  Estimate (Nm)  a  376  b  6650  c  -9300  d  84380  Table 9.3 Link Parameter Estimates: Whole workspace mapping. Aug. 13 1992  The load cell, had it functioned longer, would have provided endpoint force resolution on the order of 10 N or less. This would have easily been sufficient to detect the extra weight of a log in the grapple jaws. Force determination by hydraulic pressures in contrast had at best 1000 N resolution. This poor resolution was a major limiting factor during the force feedback experiments.  9.1.4 Force computation errors In an effort to bypass problems in the identification of the link parameters, a brute force mapping technique was tried. To keep the problem to a reasonable scale, the static case only was considered. A mapping was made between the boom and stick angles to the applied torques  Chapter 9: Machine Experiments  74  at each joint. Unfortunately, the various factors which caused the poor convergence of the mass parameters created a similar variance in the torque mapping. Endpoint force resolution remained greater than 1000 N. The common factor between both of these methods is the computation of the net joint torque from the cylinder pressures. This depends on the pressure transducers, the cylinder dimensions, the link dimensions and cylinder and joint friction. The cylinder and linkage measurements were checked and found to be correct. While the zero offset of the pressure transducers was also calibrated out, precise calibration throughout the pressure range was not possible. The theoretical force errors due to pressures variations within the manufacturer’s specifications (±0.5% FS) could have resulted in endpoint force errors of up to 600 N. Additional errors are probably caused by unmodeled friction components. These components would have their greatest effect in the static condition in which the parameters were estimated and in which the machine will be operating in for most force feedback work.  9.2 Loader Experiments.  The endpoint of the loader was controlled in rate mode, with stiffness feedback to the operator (ref Fig. 3.17). Initial tuning started using a hybrid position/rate control scheme, but it became  evident that, from the operator viewpoint at least, position control was too unconventional to be used in this particular application. Experiments to evaluate the utility of endpoint force feedback were limited by the poor force resolution and the time lag between the operator command and endpoint movement (on the order of 0.4 s)  .  However, tests were carried out with three subjects to determine roughly the range  of forces which could be detected and controlled.  Chapter 9. Machine Experiments  75  The object of these experiments was simply to determine if it was possible for an operator to perform repeatable application of a desired force, using the joystick stiffness as the feedback mechanism. The experimental procedure simply consisted of the operator trying to match a series of target forces given.  As the maximum force in tension possible was 10 kN (the  weight of one concrete block) and the maximum force in compression was 15 kN (limited by a boom overpressure safety limit), the target forces ranged from —9000 N (tension) to 11000 N (compression). Once given a target force to achieve, the operator manoeuvred the endpoint into position (motion was restricted to the vertical axis only). When ready to begin, the operator actuated the start trigger and then moved the endpoint up or down to apply the desired force to the end. Once he had reached the desired force (as determined from joystick stiffness) the stop trigger was actuated, at which point the actual applied force was recorded. The sequence was then repeated for the next target force. Each target force value was repeated three dines and the operator was informed after each trial how close the force was achieved to help him correlate stiffness to force. A typical plot of the actual force vs time for a series of trials follows:  .A.ctijevd fc,rc vs desired  ‘Time (s)  Figure 9.30 Sample data from force trials (tension  -,  compression  .i-)  Chapter 9: Machine Experiments  76  To isolate the operator from the machine and ensure that the force was determined solely by feedback through the joystick, the machine was operated remotely. While performing the force feedback trials, the operator was also directed to not look at the machine, as is possible to guess at the endpoint forces based on link deflections. Examination of Figure 9.30 shows oscillations in the force as the machine was moved into position for each target. This is caused by the unmodeled dynamics of the machine (the force computations were only valid for a quasi-static case). To minimize the disturbance caused these oscillations, the returned force was filtered using a 1st order low pass filter with a cutoff of 2 Hz. Results of the complete set of trials are summarized below:  Target Force (N)  Mean Force Applied (N)  Std. Dev. (N)  Num. of Trials  -9000  -7597  1715  3  -7000  -6734  1221  30  -6000  -5892  1704  13  -5000  -5434  1911  30  -3000  -2815  1302  15  -2000  -3681  1283  8  3000  4473  1928  10  6000  6350  1708  17  9000  9626  1844  18  11000  11052  1694  14  Table 9.4 Results of Endpoint Force Control Experiments  Chapter 9: Machine Experiments  77  4 x10  Actual forces vs Target forces: Aug 15/92 trials  1.5  I  4! +  1  +  +  +  ++  44  rw ++  •+1- r 3  4:  g  0.5  o  It  0  &  +  8 +  P..  +  ••++ ++  ++ +  -0.5 +  ÷  + ++  +  +44  ++ +  + ++  t++ ++  +  0  +++  20  +++  +  + +++  +44+ ++++  t +  +  40  60  80  100  120  140  160  Trial # (sorted by target) Figure 9.31 Endpoint force applied vs. desired force  One can see that the error in the achieved force is relatively constant and approximately equal to the average error in the measurement of the endpoint forces (ito 2 kN) derived from pressure data. The 95% confidence interval for the endpoint force error (desired force minus actual force) is from —410 N to 125 N. While the number of trials are small and the errors are quite large, it would appear that it is indeed possible to control the endpoint force using stiffness control.  Chapter 10 Conclusions  A force reflecting joystick has been integrated into a hydraulic machine simulation, as well  as a real machine. The maglev joystick used is capable of effective force feedback, with the dynamic control of the joystick parameters making it possible to use the joystick in novel force reflection schemes such as stiffness control. A graphical simulation has been written and can be controlled using the joystick. Force feedback has been successfully added to a CAT 215 log loader, and it has been used to permit the machine operator to control the applied endpoint force. A force measurement scheme using the main hydraulic pressures has been implemented, and is able to determine endpoint  forces in the vertical and radial directions to a resolution of 1 to 2 kN. A hand controller stiffness feedback scheme has been developed that can give stable force feedback while the endpoint is moved in rate mode. A hybrid position/rate control mode was developed to permit the operation of the machine in position mode while maintaining access to the complete workspace. Endpoint force sensing proved to be a difficult problem to solve, The resolution possible using pressure sensors is not very good, on the order of 2-3% of the absolute maximum forces possible using the machine, and 10% of a typical working load. The use of differential transducers to minimize errors due to the transducers themselves is not worthwhile, as joint friction and other unmodelled parameters swamp any minor improvement in pressure resolution. Friction is not  negligible, at least not the static case. Better force resolution is possible using a direct force sensor at the endpoint, but the force sensor used must be capable of withstanding any extraneous loads in addition to the loads being measured. 78  Chapter 10: Conclusions  79  The endpoint control method preferred by operators is rate control, as this is closer to the conventional controls presently used. Further evaluation of position control is needed to determine the utility of this mode in the operation of hydraulic machinery.  10.1 Further Work The use of force feedback in hydraulic machines will require considerably more development before it can be practically applied. In the near future, force feedback experiments should be repeated with a wider range of subjects and tasks. The endpoint force resolution will have to be improved, either through more complete modeling of friction and other machine parameters not currently modeled, or through the addition of a robust endpoint force sensor. Simulation and machine experiments should be carried out to further investigate position vs rate control. The stiffness feedback method needs more tuning for different loads and should be tested in multiple DOF. In the long term, machine experiments on a wider variety of machines such as fefler/bunchers and excavators will help determine the scope of application of this technology.  80  References [1] E. 0. Johnsen and W. R. Corliss, Human Factors Applications in Teleoperator Design and Operation. Wiley Series in Human Factors, John Wiley and Sons, 1971. [2] G. Burdea and J. Zhuang, “Dextrous telerobotics with force feedback an overview, part 2:control and implementation,” Robotica, vol. 9, pp. 291-298, 1991. [3] D. Stokic, M. Vukobratovic, and D. Hristic, “Implementation of force feedback in manipulation robots,” International Journal of Robotics Research, vol. 5, pp. 66—76, Spring 1986. [4] J. A. Maples and J. J. Becker, “Experiments in force control of robotic manipulators,” in Proc. IEEE Robotics and Automation Intl. Conf, (San Francisco), pp. 695—702, April 1986. [5] T. Brooks and A. Bejczy, “Hand controllers for teleoperation,” Tech. Rep. NASA-JPL Pub. 85-11, National Aeronautics and Space Institute, March 1985. [6] 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,” IEEE Tr. Robotics and Automation, vol. 7, pp. 320—332, June 1991. [7] T. Brooks, “Telerobot response requirements,” Tech. Rep. STX/ROB/90-03, STX Robotics, March 1990. -  [8] 0. Burdea and J. Zhuang, “Dextrous telerobotics with force feedback an overview, part 1:human factors,” Robotica, vol. 9, pp. 171—178, 1991. [9] 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. -  [10] U. Wallersteiner, P. Stager, and P. Lawrence, “A human factors evaluation of teleoperator hand controllers,” in Proceeding of International Symposium Teleoperation and Control, pp. 291—296, IFS Ltd., July 1988. [11] 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. [12] R. J. Anderson and M. W. Spong, “Bilateral control of teleoperators with time delay,” IEEE Tr. Automatic Control, vol. 34, pp. 494-501, May 1989. [13] 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. [14] M. Ostoja-Starzewski and M. J. Skibniewski, “A master-slave manipulator for excavation and construction tasks,” Robotics and Autonomous Systems, pp. 333—337, 1989. [15] R. Langreth, “Smart shovel,” Popular Science, pp. 82—84,108—109, 1992.  81 [16] M. W. Spong and M. Vidyasagar, Robot Dynamics and Control. John Wiley and Sons, 1989. [17] N. Sepehri, Dynamic Simulation and Control of Teleoperated Heavy-Duty Hydraulic Manipulators. PhD thesis, University of British Columbia, September 1990. [18] N. Sepehri, D. Wong, D. Chan, P. D. Lawrence, and F. Sassani, “Development of a real time simulator for a teleoperated industrial machine,” in lASTED Conference on Modeling, Simulation and Control (M. Hamza, ed.), pp. 69—72, lASTED, August 1992. [19] A. K. Bejczy and J. K. S. Jr., “Kinesthetic coupling between operator and remote manipulator,” in Proceedings of the International Computer Technology Conference, pp. 197— 211, American Society of Mechanical Engineers, August 1980. [20] N. K. Ho, “Tms dsp d/a a/d cards and the tins trace debugger.” Elec 474 project report: UBC, August 1991. [21] C. H. An, C. G. Atkeson, and J. M. Hollerbach, Model-Based Control ofa Robot Manipulator, ch. 4. Load Estimation, pp. 65—85. MIT Press, 1988. [22] N. Parker, “Adaptive control with an indirect self tuning regulator.” Elec 589 project report: UBC, April 1991. [23] R. Duncan, Advanced MSDOS Programming. Microsoft Press, 2nd ed., 1988. [24] Caterpillar, June 1984. CAT 2l5B Sales Literature. [25] D. Wong, Parallel Implementation of Multibody Dynamics for Real-Time Simulation. PhD thesis, University of British Columbia, May 1991. [26] J. Angeles and 0. Ma, “An algorithm for the inverse dynamics of n-axis general manipulators using kanes’s equations,” Computers Math. Applic., vol. 17, no. 12, pp. 1545—1561, 1989.  


Citation Scheme:


Citations by CSL (citeproc-js)

Usage Statistics



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"
                            async >
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:


Related Items