By Ngai Mun Wong B.Sc.(EE), University of Manitoba, 1990

Transcription

1 Implementation of a Force-Reflecting Telerobotic System with Magnetically Levitated Master and Wrist By Ngai Mun Wong B.Sc.(EE), University of Manitoba, 1990 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES DEPARTMENT OF ELECTRICAL ENGINEERING We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA December 1992 Ngai Mun Wong, 1992

2 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 Flpctri cal Engipeoripp The University of British Columbia Vancouver, Canada Date Dec. 9, 92 DE-6 (2/88)

3 Abstract Aspects of control and coordination of a new force-reflecting teleoperation system have been addressed in this thesis. A six-degree-of-freedom magnetically levitated fine-motion device is used as the teleoperation master. The slave system is a redundant coarse-fine manipulator system which consists of a conventional robot equipped with a magnetically levitated fine-motion wrist identical to the master. The environment and the human operator applied forces are measured by two six-axis force-torque sensors. Taking advantage of the Lorentz magnetic levitation technology, the system can eliminate mechanical problems such as friction and backlash, and is able to achieve high frequence response, precise positioning and excellent force-reflection quality. With using rate control for large motion and position control for small motion, the slave system can be controlled over a large workspace by the master without operator controlled indexing and without sacrificing position resolution. An overshoot problem due to time delay in the coarse manipulator position data has been solved by using decoupling coarse-fine control approach, in which the coarse manipulator is only responsible for rate control and the fine-motion wrist is responsible for position control. The system performance was quantified by performing general teleoperation tasks, such as free motion tracking, hard contact and exertion of forces. The teleoperation system has over 15Hz position bandwidth and several khz force bandwidth. At present, the force bandwidth is limited by computation delays to a few hundred Hz (this number will increase with a faster computing system). ii

4 Table of Contents Abstract^ ii List of Figures^ viii List of Tables^ xii Acknowledgement^ xiii 1^Introduction 1 1.1^Teleoperation Theory ^ Ideal Teleoperator ^ Teleoperator Bandwidth Requirements ^ General Teleoperation System Design Principles ^ Motivation ^ Survey of Existing Teleoperation Systems ^ Motivation of the Project ^ Thesis Overview ^ 7 iii

5 2 Implementation of the UBC Teleoperation System The UBC Teleoperation System Overview ^ Maglev Fine Motion Wrist ^ ^Hardware Design ^ ^Dynamics and Control using Euler Quaternions ^ Force-Torque Sensor ^ ^Analog and Digital Options ^ ^Sensor/Wrist Dynamic Interaction ^ Coarse Motion Manipulator: The CRS460 Robot ^ Computing Systems ^ ^DSP System ^ ^PC System ^ ^CRS System ^ ^Communication Between Computing Subsystems ^ The Bandwidths of the UBC Teleoperation System ^ 29 3 Control Algorithms of the UBC Teleoperation System Controlling a Large Slave with a Small Master: Hybrid Position/Rate Control ^Problem of Poor Position Resolution in Position Control ^ 31 iv

6 3.1.2 Problems of Positioning and Force-Feedback in Rate Control ^ Hybrid Position and Rate Control ^ Implementation of the Hybrid Position and Rate Control ^ Overshoot Problem due to Time Delay^ Decoupled Coarse-Fine Control Algorithm ^ Discussion and Further Improvement ^ Six Degree-of-Freedom Extension ^ Teleoperation Control at Wrist Level ^ Modelling a Teleoperator as Two Rigid Bodies ^ Force-Reflection by Coordinating Torque ^ Force-Reflection by using Force-Torque Sensors ^ 53 4 System Testing and Analysis^ Free Motion Tracking ^ Position Tracking at Wrist Level ^ Back-Driveability at Wrist Level ^ Rate Tracking (no rate deadband) ^ Hybrid Position and Rate Control ^ Contact Stability ^ 60

7 4.2.1 Hard Contact ^ Soft Contact and Active Environment ^ Exertion of Forces ^ Discussion and Evaluation ^ Slave Flotor Dynamic Effect during CRS Motion ^ Pros and Cons of using Force-Torque Sensor(s) ^ Evaluation ^ 76 5 Conclusions^ Contributions ^ Future Work ^ 80 Bibliography^ 81 Appendices^ 84 A Operation Procedures of the UBC Teleoperation System ^ 84 B Technical Manual of the UBC Teleoperation System^ 87 B.1 Interconnection and Pin-Assignments ^ 87 B.2 The Parallel Communication between PC and CRS Systems ^ 92 B.3 CRS System Programming Technique ^ 94 vi

8 C Experimental Results for the Z-Axis Rotation ^ 97 D Programming Flowcharts of the DSP, PC and CRS Systems^108 vii

9 List of Figures 1.1 A Teleoperation System ^ A Teleoperation System with Universal Hand Controller ^ Main Components of the UBC Teleoperation System ^ Data-Flow of the UBC Teleoperation System ^ Maglev Wrist System ^ The Assembly Sketch of the Maglev Wrist ^ Maglev Wrist Optical Position Sensing System ^ One-Axis Model of the Force Sensor ^ The Real-Time Computing System ^ Computations Performed by the DSP System ^ The CRS REMOTE Operation ^ Hybrid Position and Rate Control ^ Problem of Switching from Rate to Position Control ^ Implementation of the Hybrid Position and Rate Control ^ 38 viii

10 3.4 Dynamic Model of the Coarse-Fine Manipulator ^ Block Diagram of the Coarse-Fine System ^ Step Responses of G11 and G21 ^ Step Response of the Coarse-Fine System ^ Overshoot Effect due to the Delay H1 ^ Block Diagram of the Decoupling Coarse-Fine Control ^ Block Diagram of the Improved Decoupling Coarse-Fine Control ^ Block Diagram of the Six DOF Implementation ^ Rate Control Deadband Shapes ^ Robot Velocity Profiles ^ Single-Axis Model for Teleoperation Control ^ Controller #1: Position Tracking of the Maglev Wrists ^ Controller #2: Position Tracking of the Maglev Wrists ^ Controller #1: Back-Driveability of the Maglev Wrists ^ Controller #2: Back-Driveability of the Maglev Wrists ^ Rate control ^ Hybrid Position and Rate Control ^ Controller #1: Hard Contact ^ 64 ix

11 4.8 Controller #2: Unstable Contact ^ Controller #2: Stable Contact with Large Damping ^ Controller #2: Stable Contact with a Damping Function ^ Controller #1: Soft Contact and Active Environment ^ Controller #2: Soft Contact and Active Environment ^ Controller #1: Exertion of Forces ^ Controller #2: Exertion of Forces ^ 73 B.1 Interconnection of the UBC Teleoperation System ^ 88 B.2 Orientation Relationships Among the Flotor, JR 3 sensor and CRS Robot... ^ 89 B.3 The Parallel Communication Handshaking Algorithm ^ 93 C.1 Controller #1: Position Tracking of the Maglev Wrists ^ 98 C.2 Controller #2: Position Tracking of the Maglev Wrists ^ 98 C.3 Controller #1: Back-Driveability of the Maglev Wrists ^ 99 C.4 Controller #2: Back-Driveability of the Maglev Wrists ^ 99 C.5 Rate Control ^ 100 C.6 Hybrid Position and Rate Control ^ 101 C.7 Controller #1: Hard Contact ^ 102 C.8 Controller #2: Stable Contact with Large Damping ^ 103 x

12 C.9 Controller #1: Soft Contact and Active Environment ^ 104 C.10 Controller #2: Soft Contact and Active Environment ^ 105 C.11 Controller #1: Exertion of Forces ^ 106 C.12 Controller #2: Exertion of Forces ^ 107 xi

13 List of Tables 1.1 Human Operator Input/Output Capabilities ^ JR3 Sensor Load Rating ^ Controller Gains used in the Experiments Reported ^ 56 B.1 Pin-Assignments of the D/A card ^ 87 B.2 Pin-Assignments of the A/D card #1 ^ 90 B.3 Pin-Assignments of the Digital Port ^ 91 B.4 Pin-Assignments of the A/D card #2 ^ 92 C.1 Controller Gains used in the Experiments ^ 97 C.2 The Corresponding Figures (Results) in each Experiment ^ 97 xii

14 Acknowledgement I wish to thank my supervisor, Dr. Tim Salcudean, for his support, guidance and encouragement. Dr. Salcudean is the kind of supervisor who provides students a lot of opportunities to keep contact with the industry world, through field trips and frequent demonstrations to visitors. He also gave me an opportunity to attend a conference in France. Thanks are also due to C. Sheffield for the assistance in component purchases, to D. Fletcher for metal work, and to N. Ho, T. Vlaar and D. Goertz for developing the DSP debugger, robot kinematic codes and electronics work. I would like to thank my fellow graduate students C.T. Chen, N. Parker and A. Chahal for their valuable input, discussion and friendship. Thanks are also due to W. Johnson of the JR 3 Inc. and T. Jone of the CRS-Plus Company, who provided the expertise I needed to integrate JR3 sensors and CRS robot into the system. Finally, I would like to thank my parents. It would not have been the same without them, and it won't be. This thesis was supported in part by NSERC and the UBC Graduate Fellowship.

15 Chapter 1 Introduction As illustrated in Figure 1.1, a teleoperation system consists of a human-operated master manipulator 1 and a remotely controlled slave manipulator, in which the slave robot tracks the motion of the master. On one hand, due to the immature development of artificial intelligence, Figure 1.1: A Teleoperation System applications of fully automated robot systems can only exist in highly controlled environments and robots can only perform mostly predictable or repetitive tasks (e.g., automobile assembly line), while for performing general tasks, humans still offer the most versatility. On the other hand, many tasks (such as handling toxic material in a hazardous environment) cannot be carried out by human beings. Through teleoperation, human skills and robot capabilities can be augmented to perform the tasks which are not suitable for either humans or robots alone. 1 In this thesis, the term "master" is the same as "hand controller" or "joystick". "Robot" is the same as "manipulator", and "teleoperation system" is same as "teleoperator". 1

16 Chapter 1. Introduction ^ 2 Starting from the handling of radioactive materials in early 40s, the applications of teleoperation have spread over a wide range of areas: from undersea to outer space explorations, from aids to the handicapped to aids for surgery[1, 2], from construction to military operations[3, 4]. Aside from the traditional meaning of teleoperation as an extension of a person's sensing and manipulation capacity to a remote location, the notion of teleoperation has started to encompass the extension of such capabilities through barriers of scale. Examples cover both ends of the spectrum - [5, 6] present work towards teleoperation systems concerned with micromanipulation at nano-meter level scales and below, while work reported in [7] attempts to build a force amplification exoskeleton "worn" by a human worker for heavy-duty tasks and [8] deals with resolved motion teleoperation of large forestry machines. Teleoperation may also emerge as a useful tool for robot programming for manufacturing through teaching and trajectory playback (including forces and torques). Such a method would be inherently safe, user friendly, and might allow assembly sequences to be programmed without complicated modelling of the dynamics of mating parts. To improve task performance, an ideal teleoperation system should be as "transparent" as possible so that the human operator could not detect that he was remotely located from the task. To achieve such feeling of telepresence, the teleoperator, in addition to the basic slave tracking master function, should be able to provide the human operator with kinesthetic feedback from environment forces. Experiments show that teleoperation task completion time is dramatically improved with environment force feedback [9, 10].

17 Chapter 1. Introduction^ Teleoperation Theory Ideal Teleoperator An ideal teleoperation system is a man-machine interface which provides the human operator a feeling of directly holding the tool to interact with the remote task environment. This direct manipulation concept can be achieved if the following criteria are met: xs = xm ; fh = fe, where x m, x, are the master and slave positions; fh and f, are the human operator hand force applied to the master and environment force applied to the slave, respectively. The tool carried by the slave is defined as part of the environment. The ideal teleoperator concept can also be represented by an infinitely stiff, massless, mechanical linkage which presents to the operator the identical inertial-viscous-spring reactions as are present at the task [10] Teleoperator Bandwidth Requirements An ideal teleoperation system should not have any force or position bandwidth limit. This is not achievable in practice. Fortunately, since the motion and sensory bandwidths of humans are also limited, it is adequate for a teleoperation system to have bandwidths which are higher than or close to the human operator bandwidth capacity. Since humans have asymmetrical input/output capabilities as shown in Table 1.1 [11], teleoperation systems could also have asymmetrical data flow. In the master to slave direction (human output perspective), a position and force bandwidth of 7-10Hz would be more than sufficient. However, in the slave to master (human input perspective), a position bandwidth of 20Hz and a force bandwidth of 320Hz may not be enough for a delicate teleoperation tasks.

18 Chapter 1. Introduction^ 4 Table 1.1: Human Operator Input/Output Capabilities Output Arbitrary trajectory tracking Periodic (or known) trajectory tracking Reflexive responses Isometric responses (i.e., force responses without motion) Input Proprioceptive/kinesthetic sensing Tactile sensing (Low-amplitude vibrations) 1Hz 7Hz 10Hz 10Hz 20-30Hz 320Hz General Teleoperation System Design Principles Teleoperation system design principles are quite dependent on the application areas. However, in order to keep enough flexibility to perform many human functions, the following are desirable features: Spatial correspondence (e.g., position and velocity) between master and slave. Sensory correspondence (e.g., force, vision). Optimal joystick gains (e.g., position and force scaling) for human comfort. Posture of the human operator (e.g., left/right hand motion, single handed control, avoid feeling fatigue for long working hours). Modular design for faster and easier repair or replacement. Safety.

19 Chapter 1. Introduction^ Motivation Survey of Existing Teleoperation Systems Hand Controllers Most of the existing teleoperator designs are based on a geometric analogy between the master and slave systems. As described in the fascinating book by Vertut and Coiffet [3], teleoperators have evolved away from mechanically and electrically mirrored systems. With the advent of inexpensive computing power, it became possible to move away from kinematic equivalence and employ an universal teleoperation master for different slave manipulators. A teleoperation system using an universal hand controller is shown in Figure 1.2. The motion of the master is Figure 1.2: A Teleoperation System with Universal Hand Controller "resolved" to that of the slave (through the joint motions of the slave manipulator by a computing system). The forces sensed at the slave are transformed to the hand controller frame for force feedback. The use of the universal hand controllers has a number of advantages, including

20 Chapter 1. Introduction^ 6 manageable size, cost, no absolute limit on the number of axes of the slave being control [12], no restriction on control strategies (e.g., rate control), standardization of the human interface and optimization of the design with regard to safety, operator fatigue and human arm limitations. There are few such universal controllers available today: Force/Torque Ball Joysticks can accept input commands in cartesian frame without any displacement. However, it would not be possible to incorporate force feedback in these devices. A number of commercially available hand controllers are based on such designs [13, 14]. The CAE hand controller has been found to be a very effective teleoperation master in the aerospace markets [15]. However, its main disadvantages are lack of force reflecting capacity and its high cost. The University of Texas hand controller [16] is driven by nine wires and thus it provides a light and low cost design with capability of force reflection. However, it suffers from high friction level associated with the wire drives. The JPL hand controller [17] is based on serial kinematic design which gives a good range of motion. Each degree-of-freedom can be backdriven by a motor. However, the maximum force and torque attainable in the current design are lon and 0.5Nm which may not be sufficient for some applications. It is believed that an input device based on Stewart Platform geometry is a promising alternative in hand controller design. It provides force feedback capacity, compact size and low cost. Siva [18] has described one such hand controller which is under development at Harwell Laboratory. However, performance of such device has not been quantified yet.

21 Chapter 1. Introduction^ 7 Slave Manipulators Most slave systems consist of a single conventional serial link manipulator. Low frequency response, high impedance and friction, the inability to exert accurate forces, backlash and poor positioning, etc., are typical drawbacks of conventional manipulators Motivation of the Project This project was motivated by the inadequacy of existing teleoperator hardware, both at the master and at the slave level. The proposed system, named after the University of British Columbia (UBC), takes advantage of the Lorentz magnetic levitation technology introduced in [19, 20]. A six degree-of-freedom (DOF) 2 magnetically levitated (maglev) fine-motion wrist is used as an universal teleoperation master. The maglev wrist has high frequency response, accurate positioning and force reflection capacity. The slave system is a redundant coarse-fine manipulator consisting of a 6 DOF conventional robot equipped with a second maglev wrist identical to the master, and thus, the advantages of the maglev wrist are also available in the slave system. Furthermore, it provides the system with an advantageous wrist-level kinematic equivalence. In this thesis project, the proposed system was implemented and tested. The details of system design and performance will be described later. 1.3 Thesis Overview Chapter 1 introduces some background on teleoperator design principles and provides the motivation for the work presented in this thesis. 2 Six degrees of freedom consist of translation in X, Y, Z axes and rotation about X, Y, Z axes so that every position and orientation can be reached. A robot is considered to have full degrees of freedom if it has 6 DOF.

22 Chapter 1. Introduction^ 8 Chapter 2 describes each of the hardware modules of the UBC teleoperation system. The modules include the maglev wrist, force-torque sensor, a conventional robot and the overall computing system. Communications among computing subsystems are also presented. Chapter 3 outlines a hybrid position and rate control method used in the system. A time delay problem is analyzed and a solution is proposed. It also examines the teleoperation controllers used at the maglev wrist level. Chapter 4 is dedicated to the testing conducted in the system. The testing includes free motion tracking, contact stability and exertion of forces to an environment. The experimental results are reported and analyzed. Then the system will be evaluated. Chapter 5 summarizes the contributions of the thesis and suggests topics for further research.

23 Chapter 2 Implementation of the UBC Teleoperation System 2.1 The UBC Teleoperation System Overview The main components of the UBC teleoperation system are shown in Figure 2.1. A six degreeof-freedom (DOF) magnetically levitated (maglev) wrist is used as the teleoperation master. The maglev wrist is actuated by active magnetic forces to eliminate actuator friction and backlash. Therefore, the wrist has a better frequency response and precise positioning ability than conventional robots. It also has 6-axis force feedback capacity. These advantages make the wrist an ideal teleoperation master. The teleoperation slave system consists of an identical maglev wrist mounted on a 6 DOF conventional robot (CRS460). The CRS robot can be interpreted as a 6 DOF transporter to increase the workspace of the slave wrist 1. Therefore, the advantages of the maglev wrist can be maintained in the slave side. The slave system is actually a coarse-fine manipulation system, as the CRS robot acts as a coarse manipulator and the wrist is as the fine motion device. Each maglev wrist is equipped with a 6 DOF force-torque sensor (from JR 3 Inc.). Therefore, we can measure the human operator hand force at the master side and environment force at the slave side. An IBM PC compatible using Intel microprocessor (pp) houses a Digital Signal Processing (DSP) board, as well as A/D and D/A boards. Most computations (master and slave wrist control, CRS robot set-point calculation etc.) are performed by the DSP board. Figure 2.2 illustrates the data flow of the system. The slave wrist's set-points are calculated 1 From now on, the term "master" refers to the master maglev wrist; "Slave wrist" refers to the slave maglev wrist. "Robot" refers to the CRS robot. 9

24 Chapter 2. Implementation of the UBC Teleoperation System^ 10 CRS Robot Figure 2.1: Main Components of the UBC Teleoperation System

25 Chapter 2. Implementation of the UBC Teleoperation System^ 11 Slave Wrist Location (in World Coordinates) Master Master's Location,.._ Master to Slave Wrist Coordinate Transformation Slave Wrist Set point Slave Wrist Slave Wrist Location (in Wrist Coordinates) Slave Wrist to World Coordinate Transformation CRS Set point ^ Calculation Set point CRS Robot CRS Location Figure 2.2: Data-Flow of the UBC Teleoperation System from the master and the robot location data 2. The slave wrist location, by using the robot data, can be expressed in world coordinates and can be used in the master controller. The CRS robot can be commanded by the master or the slave wrist. The teleoperation controller, as well as the method for controlling this large workspace slave system with the small workspace master, will be presented in the next chapter. This chapter will first describe each hardware module (namely the maglev wrist, force sensor and the CRS robot). Then, it will present each computing subsystem and how they interact with each other. Finally, we will discuss the overall bandwidths of the system. This chapter is intended for general readers. Technical details of implementation and use are given in the "Technical Manual" (in Appendix B). 2From now on, the term "location" refers to position and orientation.

26 Chapter 2. Implementation of the UBC Teleoperation System^ Maglev Fine Motion Wrist This project inherited the two maglev wrists and their kinematic and dynamic transformations software. The two maglev wrist systems were designed by Salcudean and built at the UBC along the principles described in [20]. Each maglev wrist system consists of a current driver, two power supplies, a signal conditioning box and the maglev wrist, as shown in Figure 2.3. The maglev wrist consists of two rigid elements a stationary part called stator and a moving Actuation Force/Torque Data Set-poH Inverse Dynamic Transformation Controller Coil Current Data D/A I I Lorentz Forces Maglev Wrist System Current Driver rd Power Supply Coils I LEDs I Flotor LED Beams Power Supply Rotor Location (w.r.t. Stator) Kinematic Transformation PSD Data Figure 2.3: Maglev Wrist System part called flotor. Six Lorentz forces are generated between the two elements and the flotor is actively levitated. Thus, except for a thin flexible ribbon cable for power, there is no mechanical connection between the two elements. The coil currents are computed from the inverse dynamic transformation and the force/torque information from the controller. The flotor location is computed by the kinematic transformation on the position data obtained from the wrist's internal optical position sensors which consist of light-emitting diodes (LEDs) and position sensitive diodes (PSDs).

27 Chapter 2. Implementation of the UBC Teleoperation System^ Hardware Design The assembly sketch of the maglev wrist is shown in Figure 2.4.^The flotor carries three horizontal and three vertical coils (Fl in Figure 2.4), and also three LEDs (in F2) for optical position sensing. The horizontal plate of the flotor (F1) has holes to allow supporting posts (S3) holding the stator's magnets and PSDs (S4, S5). The holes in the flotor are large enough to avoid collision with the supporting posts when the flotor is moving within its workspace. The motion range (i.e., the workspace) of the flotor is constrained by the width of the magnetic gaps in the six identical actuation elements. There is a trade-off between the workspace and the achievable forces - the widther the magnetic gap, the larger the flotor workspace, but the weaker the magnetic field strength (i.e., weaker the actuation force). The workspace of the wrist is roughly ±4.5mm in translation and ±7 in rotation from the nominal center. Using the coils' geometry, magnetic field and current directions with respect to (w.r.t.) the coordinate frame attached to the flotor center (see the top view of F1), the direct dynamic transformation (i.e., finding the forces/torques acting on the flotor center from the six coil currents) can be devised. The details can be found in [20] and will not be repeated here. Due to the small motion range of the flotor, the transformation can be approximated by a constant matrix M [20]. Thus, the wrench vector acting on the flotor (at center position) is given by, M fx II fy fz /3 = (2.1) Tx /4 7 Y rz _ /6 where [/1 /2.../6]T is the vector of coil currents. The units for the first three rows of the above matrix are in N/A and the entries of the last

28 Chapter 2. Implementation of the UBC Teleoperation System^ 14 PSD mounted here Magnets Bolted to F2 t t Fl S3^rl (3 pairs) ri (Support S4/S5) Magnets S2 Figure 2.4: The Assembly Sketch of the Maglev Wrist

29 Chapter 2. Implementation of the UBC Teleoperation System^ 15 three rows are in N-m/A. By inspecting the above transformation matrix M, readers can get a feeling on the actuation forces of such a 6 DOF in-parallel actuated device. The current drivers can generate up to 10A at 40V in each coil, and is powered from a single 1000W (25V - 40A) power supply. Each coil has about 1SI resistance and heat is generated by coil dissipation during operation. Currents will be cut by the thermal fuses if the flotor temperature is exceed 95 C. Due to thermal constraints, the wrist cannot sustain indefinitely a load higher than 15-20N, although, from the transformation matrix M and the specifications of the current driver, it results that transient forces larger than 40N can be obtained. The torque capability exceeds 0.6N-m for indefinite loads, and more than 1.2N-m for transient loads. This is more than adequate for a hand controller. The inverse dynamics, i.e., finding the currents from a desired force/torque vector, can be easily achieved by finding the inverse of M. In fact, the matrix M -1 is precomputed and used in the real-time control software. The maglev wrist optical position sensor (see Figure 2.5) consists of three LEDs (carried by the flotor) that project narrow co-planar beam at 120 from each other, onto the surfaces Figure 2.5: Maglev Wrist Optical Position Sensing System

30 Chapter 2. Implementation of the UBC Teleoperation System^ 16 of three two-dimensional duo-lateral position sensitive diodes (PSD) which are mounted on the stator. The PSDs (with the 12-bit A/D conversion in the DSP system) can detect flotor motion less than 5itm / 10/i-rad. Two coordinate systems will be defined: one attached to the flotor and one attached to the stator. The flotor frame is at the center of the flotor and coincides with the intersection of the LED light beams (see Figures 2.4 and 2.5). When the flotor is at its nominal location, the flotor frame coincides with the stator frame and the LED light beams projected on the centers of the PSD surfaces. Provided that the flotor is moving within the wrist workspace, the flotor position (represented by a vector r = [x, y, z ]T) and orientation (represented by a proper rotation matrix Q) with respect to the stator frame can always be computed from the PSD's readings. The method of calculating {r, Q} and the dynamic transformation M can be found in [20]. Calibration procedures of the current driver and position sensors are described in [21] Dynamics and Control using Euler Quaternions Euler Quaternions To save computation, one can replace the rotation matrix, Q, by Euler quaternions (defined by P A [do AT ]T = [cos(02) sin( /2) st, where s is the normalized axis of rotation and (/) is the angle of rotation) [20, 22]: Q = exp(0 sx) = I (13x) + 2(f3x) 2^(2.2) where, for any vector a = [al a2 a3] T ax = 0^ a3^a 2 a3^0^ al a2^al^0 (2.3)

31 Chapter 2. Implementation of the UBC Teleoperation System^ 17 Note that p and p represent the same rotation in (2.2). The orientation vector /3 that corresponds to 00 > 0 can be obtained from (2.2) and is given by 1 13 x =^1 (Q Q),^ (2.4) 2(1+ trqp where trq denotes the trace of Q. By using a small angle approximation, the components of 0 can be interpreted as the rotation angle (in the unit of 2 radius) about the stator axes. Control Since the flotor is just a rigid levitated mass, it can be modelled accurately by rigid body dynamics Therefore, the differential equations of describing the body motion can be transformed to decoupled double integrator form (see [20] for details). This makes the wrist flotor control very simple. For instance, a PID controller can be as follows: u 1 = kp(rd r) + ky(rd r) + ki f (rd r) + f9^(2.5) u2 = rcp(pd 0) + iiv(4id 41 ) + KJ I (Pd 0) + TS^(2.6) where rd, fid is the desired flotor position and orientation, the gains k p, kv (etc.) are diagonal matrices and [uit u2 T] T is the computed wrench vector in equation (2.1). The effect of fg and r9 is to compensate the gravitational force and the gravitational force induced torque 3 : f9 = m g T9= r x f 9^9^9 where m, g and r9 are the flotor mass, gravitational acceleration and the flotor's center of mass position, respectively. The f9 and r9 are always implemented in the maglev wrists and will not appear in the control laws (or the term "actuation force") in the later discussion. From now on, we can interpret the flotor as a weightless rigid body. 3 When [u1 T u2 T] T (i.e., f9) is not acting on the flotor center of gravity, 7-, is needed to compensate the torque induced by f9.

32 Chapter 2. Implementation of the UBC Teleoperation System^ Force-Torque Sensor The JR3 force-torque sensor load rating is shown in Table 2.1. Although the voltage range of Table 2.1: JR3 Sensor Load Rating Axis Sensor Load rating (±10V) i loading (±2.5V).f ±113N ±28.25N fy +113N N h ±226N ±56.50N Tx ±8.6N-m ±2.15N-m Ty ±8.6N-m ±2.15N-m Tz +8.6N-m ±2.15N-m the DSP A/D input is one-fourth of the JR3 sensor's range, the external force/torque in our application is normally within 1/4 of the sensor loading (see Table 2.1). Therefore, scaling on the JR3 analog signal is optional. Nevertheless, a circuit has been built for using the force sensor full range. With using 1/4 sensor loading and 12-bit A/D conversion, the sensor can detect force/torque less than 0.1N/0.01N-m Analog and Digital Options Ideally, we should use the decoupled and conditioned digital data from the JR3 supporting system 4. However, it requires PC CPU time for digital data communication and also uses up one PC expansion slot for digital I/O. In this project, all expansion slots of the PC have been fully occupied. Furthermore, the data rate provided by the JR3 supporting system is too slow 5. Therefore, the analog data is used. All the essential functions that were performed by the JR 3 A/D board and processor board were moved to the DSP system. Those functions include A/D 'The JR3 supporting system (come with the JR 3 strain-gauge sensor) consists of an analog signal conditioning board, a D/A board and a processor board. Conditioned digital data are available from the processor board. 5 1f the JR3 supporting system is used, the sampling rate should be kept under 250Hz as recommended in the JR3 manual.

33 Chapter 2. Implementation of the UBC Teleoperation System^ 19 conversion, cross coupling removal, proper unit scaling and offset weight removal Sensor/Wrist Dynamic Interaction The sensor is divided into an upper half and a lower half with strain-gauges mounted at both ends. The sensor measures forces and torques by the tiny elongation or contraction of its foil strain-gauges which change resistance accordingly. The one-axis model for sensing environment forces is illustrated in Figure 2.6, where m 1 is the total masses of the flotor, adapter plate k j w fi bj X2 Figure 2.6: One-Axis Model of the Force Sensor and the sensor lower half, m 2 is the masses of the sensor upper half and any load (e.g., tool) it carries, fs and f, are the slave flotor actuation force and environment force respectively, ki and bj are the JR3 sensor stiffness and damping, and x 1 and x 2 are the position of m 1 and m 2 respectively. We also define ox = x 2 x 1 and thus the sensor reading is fj = Ox. From Figure 2.6, assuming zero initial condition, m1 2 ^ S Xi 2 - m2 S X2 =^k3^bj s =^ kj s bi (2.7) (2.8) where hatted variables denote Laplace transforms. Combining (2.7) and (2.8), we have mz [m bi ( m2 )s ki (1 + m2 )1 = f m1^m1^m1 (2.9)

34 Chapter 2. Implementation of the UBC Teleoperation System^ 20 At low frequencies, this leads to By putting fj = ki 8x, equation (2.10) becomes kj (1 -I- 17n ) bi = fe m2 fs ^ (2.10) ml^m1 fe = +^cri + 1 (2.11) If the sensor is mounted on a conventional robot, m1 >> m 2 and the sensor can measure the force as expected. If m 1 m2 (e.g., the slave carrying a heavy tool), equation (2.11) should be taken into account. Let us consider two extreme cases for the maglev wrists. In the first case, when the wrist exerts a force to a hard surface (e.g., a solid steer), the exerted force will equal the environment reaction force, h = f, and the sensor will measure the force correctly (i.e., fi = fe). In the second case, the wrist moves freely and fe = 0. Equation (2.11) becomes 1 fj = ^ fs 1+ mi/m2 (2.12) In other words, the sensor reading is most inaccurate during free motion. For the maglev wrist, mi/m2 > 5 and h is very small during free motion (f3 just overcomes the small flotor inertia). For simplicity, this 17% measurement error will be neglected. Due to the stiffness ki and extremely small (5x of the sensor, we can treat the flotor and the sensor as a single rigid body (i.e., combining m 1 and m 2 as a rigid mass) in the later discussion. 2.4 Coarse Motion Manipulator: The CRS460 Robot The coarse manipulator used in the project is a CRS460 robot. It is a 6 DOF serial-link manipulator with 3kg payload and the maximum speed of 4.57m/s. Besides the robot arm, the CRS system comes with a controller, current drivers and a teach pendant. Please refer to CRS manual [23] for details.

35 Chapter 2. Implementation of the UBC Teleoperation System^ Computing Systems The real-time computing system is shown in Figure 2.7. An IBM PC-AT compatible computer hosts a Spectrum Inc. Digital Signal Processing (DSP) board using a Texas Instruments Inc. TMS320C30 DSP chip, as well as D/A and A/D cards. In order to avoid PC-bus bottleneck, these are linked through a fast, private bus whose data transfer rate can be as high as 5 million 16-bit words per second. Dual-ported memory 6 is used for the communication between the PC and the DSP systems. The PC is connected through a digital parallel I/O port to the CRS robot controller. The CRS system uses an Intel 8086 pp and 8087 co-processor, with the clock rate 7.33MHz. During operation, set-points of the robot's six joint motors are delivered to six axis control cards every 4ms. Each axis card implements a PID controller running on Intel 8095 pp. The control update rate is lms DSP System The TMS320C30 processor runs at 33MHz with 32-bit floating point arithmetic. It has eight 40-bit accumulators and 2 Kwords internal RAM. In the board level, the system has 172 Kwords local RAM (25ns) and a 64 Kwords dual-ported RAM (35ns) between the board and the PC bus. The 32-channel A/D card has 12-bit resolution. The sampling rate depends on the number of channels being used from 7KHz 32-channel sampling to 230KHz 1-channel sampling, which is more than enough for our application. The 16-channel D/A card also has 12-bit resolution and allows 16-channel simultaneous analog updating. The D/A conversion settling time is 1psec. maximum. During operation, the DSP system does most of the control and coordinating work, by 6 Dual-ported memory is the memory locations which can be read or written by both PC and DSP systems.

36 Chapter 2. Implementation of the UBC Teleoperation System^ 22 Figure 2.7: The Real-Time Computing System

37 Chapter 2. Implementation of the UBC Teleoperation System^ 23 performing the tasks shown in Figure 2.8. Through forward kinematic transformation on the CRS joint data, the robot position and orientation are first computed so that coordinate transformation on the slave flotor location and environment force data can be performed. The hand and environment forces/torques are calculated from the JR 3 sensors' strain-gauge readings and can be used in the wrist's controllers. Position and orientation of the maglev flotors are calculated from their internal PSD position sensors and the CRS robot end-effector position data. Currents required for actuation forces/torques are obtained via the inverse dynamic transformations. Then the CRS set-point is computed and expressed in joint space through the CRS inverse kinematics. In the program, the units of translation and rotation are in micron and 0 1 milli-radians, and force and torque are in Newton and Newton-decimeter, for better numeric range of display in the DSP monitoring program. To sum up, the DSP mainly performs three tasks: (1) the master and slave maglev wrist control, (2) the hand and environment forces/torques calculation, and (3) the CRS set-point and location computations. With the DSP board running at about 3.8 Mflops, the computations on the force/torque data and the maglev wrist control can be completed in 2ms, and the CRS set-point and kinematic computations will take about 5ms. The flowchart of the DSP program are given in Appendix D PC System The functions of the PC are to: 1. Provide terminal functions for the DSP and CRS systems. A monitoring program has been developed for the DSP system through a Summer project [24]. This monitoring program is like a simple operating system, in which, by issuing some commands, one can load a program to the DSP system, or continually trace any global variable of the DSP program, or poke any global variable at run time (e.g., change the

38 Chapter 2. Implementation of the UBC Teleoperation System^ 24 Figure 2.8: Computations Performed by the DSP System

39 Chapter 2. Implementation of the UBC Teleoperation System^ 25 wrist controller gains). Please refer to [24] for details. For the CRS system, normal terminal function can be provided through the PC and CRS's serial port. 2. Provide a software development environment. The PC can provide various software development tools for the DSP and CRS programming. After being compiled in the PC, the DSP and the CRS programs can be loaded to their destinations for execution. 3. Collect data. Data of the wrists, force sensors or the CRS robot can be found in the DSP program. Since the DSP on-board memory is limited, the PC gets the required data from the dual-ported memory and temporarily stores them in its RAM. After the operation is terminated, the data is stored to a file. Thus, the operation will not be slowed down because of the PC is writing to its hard-drive. Also, PC RAM is not expensive these days. 4. Provide a communication channel between the DSP and CRS systems. It does dual-ported memory communication with the DSP and digital parallel communication with the CRS controller. Details will be described later. Since the PC performs more than one task, a multi-tasking operating system is preferable. However, only the communication function (item #4 in above list) is essential in the real-time operation. To avoid slowing down the operation, the monitoring features (item #1) can be inactivated during operation. The details can be found in Appendix A. Flowchart of the PC program are given in Appendix D CRS System The program running in the CRS controller just performs the parallel communication with the PC and writes the robot set-point to an internal data buffer for real-time path control.

40 Chapter 2. Implementation of the UBC Teleoperation System^ 26 An undocumented 7 CRS state called REMOTE is used in this project for the real-time path control. REMOTE places the robot controller into a mode in which it expects position set-points to be delivered by an external computing system via serial or parallel I/O. Under this mode (Figure 2.9), the CRS computing system reads the new set-point value from an internal data buffer at a fixed REMOTE loop clock rate, say, 32ms which is the CRS default setting. That means the CRS's motion path is updated once every 32ms. In each cycle, the CRS will calculate the motion speed that is required for it to move to the new desired location. The robot changes Figure 2.9: The CRS REMOTE Operation 7There are quite a lot of undocumented and hidden features of the CRS system. Readers can refer to Appendix B and [25] as a supplement to the CRS manuals. Bug-fixing information and programming technique which are not given in the CRS manuals can be found in these references too.

41 Chapter 2. Implementation of the UBC Teleoperation System^ 27 its speed and motion in the next REMOTE cycle. New set-point should be arrived within 32ms to avoid robot stepping motion (e.g., between points P2 to P3 in the figure). The REMOTE set-point can be specified in either absolute or incremental value. Incremental mode are used in this project because the speed of the robot can be easily limited by limiting its incremental set-point value at each REMOTE cycle. In addition, absolute mode may cause some safety problems, such as the robot hitting its base when a move to the origin of the cartesian coordinate is executed due to accidental sending of zero as the cartesian set-point values (which can be caused by a loose connector). The REMOTE set-point value can be specified in either cartesian coordinates or joint coordinates. In the early stage of the project, cartesian coordinates were used and the CRS system was used to do the kinematic calculations. Due to the relatively slow kcp in the CRS system, the fastest REMOTE clock rate achievable was only 55ms 8. It is too slow even when it is compared with other conventional robots. For instance, a PUMA 500 robot can perform a similar task in 28ms. To avoid this bottleneck, the kinematic calculations were moved from the CRS system to the much faster DSP board. Please refer to [26] for the CRS kinematic calculation. Now, using the set-point in joint coordinates, the fastest REMOTE clock rate achievable is improved to 16ms Communication Between Computing Subsystems Digital Parallel Communication between the PC and CRS Systems During operation, the CRS system must send the robot end-effector position and orientation (i.e., six floating point numbers) and receives the incremental position and orientation setpoint (also six floating point numbers) from the PC. The CRS system can communicate with the outside world through its serial and parallel ports. 8 The 55ms includes the robot forward and inverse kinematics, passing CRS location to PC and receiving set-point from PC, and storing the set-point to the REMOTE data buffer.

42 Chapter 2. Implementation of the UBC Teleoperation System^ 28 The serial communication provided by the CRS system can be up to 19.2 Kbaud with a special protocol (see the ACI section in the CRS manual [23]). However, the protocol involves too much overhead. It requires at least 42ms to receive and send six floating point numbers, and is therefore too slow for our application. In this project, the digital parallel communication alternative was selected and used between the PC and CRS systems. The PC uses a 24-channel I/O card (Metrabyte PI012) which carries an Intel 8255 chip (see Appendix B and i8255 data book for usage). Since the CRS digital input and output channels are physically separated (i.e., it only allows one directional transmission for each channel), the Metrabyte card, among its 24 channels, have to be divided into input and output channels accordingly. The data resolution directly depends on the number of channels used. Fourteen channels (i.e., 14 bits) are dedicated for CRS location data and six channels are for the CRS incremental set-point, and the rest (2 inputs, 2 outputs) are for handshaking Fortunately, the CRS set-point is specified in incremental form and the magnitude should be very small (i.e., the amount the robot should move during the 16ms REMOTE set-point update period). In the implementation, the resolution of each robot joint set-point is set to be 0.02 while maintaining the maximum speed of each robot joint to be 40 per second. By using the 14 channels, the data resolution of the robot end-effector location (in joint angle form) is These data resolutions and robot maximum speed are enough for general teleoperation tasks. Since the PC and CRS systems have different execution speeds, a special handshaking algorithm (see Appendix B) was developed to insure that each uses the most recent data and that the transmission of data is correct. At present, the transmission of the robot end-effector location and set-point (with handshaking) takes less than 2.8ms. Dual-ported Memory Communication between the DSP and PC Systems Through the 64 Kwords dual-ported RAM, the DSP system can send the CRS set-point and get the CRS location from the PC. Since the DSP system is much faster than the PC and the

43 Chapter 2. Implementation of the UBC Teleoperation System^ 29 CRS systems, it is inefficient for the DSP to wait for the response of the PC. Therefore, the DSP software is running on its own (i.e., not running hand-in-hand with the PC program). The DSP program only passes the CRS set-point to the PC if it is required by the PC (by using a flag) and gets the CRS location from the PC if it is available. Using this method, the DSP program can bypass the CRS set-point and kinematic computations most of the time and increase the wrist control update rate. 2.6 The Bandwidths of the UBC Teleoperation System The DSP system takes about 5ms to complete the CRS robot set-point, forward and inverse kinematics computations. However, these computations are only required for every 16ms (i.e., the maximum REMOTE rate). Without performing the CRS calculations, the DSP system can complete force sensing and, master and slave wrist controls within 2ms, so the maglev wrist control update rate is roughly 500 Hz most of the time. Since the force bandwidth of the wrist mechanical system can be up to several khz, the overall force bandwidth (at the wrist level) is limited by the computational delay. The bandwidth can be increased by a faster DSP system. Of course, the position bandwidth (at wrist level, both direction) is limited by the mechanical system. The position response of the maglev wrist exceeds 30Hz for translation and 15Hz for rotation, which is more than enough for general teleoperation tasks. One may not feel comfortable that the wrist control has different update rate (i.e., the DSP interrupt period changes from 2ms to 7ms) when the DSP needs to compute the CRS robot set-point and location. The DSP program can be easily modified to standardize the update rate. Since the wrist control is interrupt-driven, we can move the CRS computations from the interrupt routine to the "main program" and extend the interrupt cycle to (say) 2.5ms, so that the "main program" will have 0.5ms for the CRS computations in every 2.5ms. The robot setpoint will be ready before 16ms have lapsed. Although it will introduce computational delay in

44 Chapter 2. Implementation of the UBC Teleoperation System^ 30 the robot location data, this effect can be ignored by using the decoupling coarse-fine control which will be presented in next chapter. Due to time constraint and non-necessity at this point, the modification has not been implemented yet. For the CRS system, the position command update rate is about 60Hz which depends on the REMOTE clock rate (i.e., 1/16ms REMOTE rate = 62.5Hz). As with most conventional manipulators, the position response bandwidth of the CRS robot is about 2-3Hz (over a small motion range). Since the CRS robot is only responsible for coarse motion and is controlled in velocity mode (which will be described in next chapter), the 2-3Hz velocity bandwidth is enough for a human operator.