Using the WAM as a Master Controller

Transcription

1 Using the WAM as a Master Controller Renbin Zhou William R. Hamel A. S. Hariharan Mark W. Noakes* rhou@utk.edu whamel@utk.edu aharihar@utk.edu noakesmw@ornl.gov 44 Doughert Engineering Building, *Oak Ridge National Lab The Universit of Tennessee, Knoville One Bethel Kell Rd. Knoville, TN Oak Ridge, TN Abstract- This paper presents technical issues related to a specific master-slave manipulator teleoperation sstem and its position control in the Cartesian space. The master is a whole arm manipulator-wam, which is a seven degrees-offreedom (7-D) manipulator b the Barrett Technolog, Inc. The slave is a Titan II 6-D manipulator b the Schilling Robotics LLC. A distributed real-time control sstem involving three computers is used for the sstem. The communication among the computers is via a dedicated local area network (LAN). A simulation framework is also presented for sstem development and test. The Cartesian space position control is achieved b using inverse velocit mapping method. A heuristic technique based on singular value composition (SVD) is presented to deal with the Titan II singularit problems. I. INTRODUCTION Teleoperation is widel used in human adverse environments like nuclear waste remediation and space eploration. Teleoperation usuall involves a master and a slave device. One of the commonl used teleoperation sstem is the one that features a master device that is suitable for joint space control, for eample, a miniature of the slave. In this case, the operator will depend on video feedback in order to have a good estimate of slave position. Commonl used teleoperation sstems generall have one control computer, which hosts both master and slave control programs. This configuration is convenient to communicate between the two programs, for eample, b using inter-process calls. In this paper, a teleoperation sstem is proposed b using a 7-D WAM to control a 6-D TITAN II. Due to the kinematic dissimilarit between the two, Cartesian space control must be used. Due to the fact that the master control and slave control program are running in different operating sstems, a new distributed control sstem framework is used. This paper is organied as: In section II, the sstem framework will be introduced, including both the teleoperation sstem and the simulation sstem. In section III, the Cartesian space mapping based on an analtical solution will be discussed, and simulation results will be analed. In section IV, results based on eperiments will be analed. Section V will outline future research. II. A SYSTEM FRAMEWORK The proposed teleoperation sstem is part of dual arm teleoperation platform of the Universit of Tennessee, Knoville and the Oak Ridge National Laborator (ORNL). It is a real time distributed control and simulation sstem which has five PCs connected on a LAN. Figure shows the sstem hardware configuration. PC QNX Titan II Develop PC QNX Server PC Server PC Linu Linu Mapping Simulation PC Windows K Roboworks WAM PC Linu/RTAI Figure. A teleoperation sstem framework There are two paths in this sstem. The first path includes a WAM PC, a server PC, a PC, and a development PC. The first three PCs in this path are the actual teleoperation sstem. The development PC is used to develop Titan II control software and download it to the PC through a Qnet, which stands for QNX networking. The second path includes the WAM PC, the server PC, and the simulation PC; this path is used for the sstem design and test. All five computers are connected b a dedicated LAN. The WAM PC hosts the low level control software. It also has an Ethernet client module, which sends WAM QNX is provided b QNX Software Sstems, a leading provider of superior real-time operating sstem software.

2 joint angles at HZ to the server PC. Communication between low level control program and Ethernet module is b a real time FIFO []. The entire program in the WAM PC is developed under Linu/RTAI, RTAI stands for Real time application Interface, which is hard real time Linu kernel etension founded b department of aerospace engineering of Politecnico di Milano (DIAPM). The low level WAM control loop rate is 5H. The PC hosts the low level control program for titan II. It also has an Ethernet client module; communication between the two is b shared memor and semaphores. The Titan II low level control software is developed under QNX and its control loop update rate is H. The server PC receives joint angles from the WAM PC and the PC, eecutes the high level position mapping, and generates the Titan II joint angle command, which is then sent to the PC via Ethernet. The server PC is a Redhat Linu PC. The Simulation PC has a commercial software package, Roboworks, for robot simulation under Windows operating sstem. It has a TCP/IP module to receive input via Ethernet. As will be discussed in section III, in order to have precise velocit mapping, the difference between the clock ccles in the PC and the WAM PC must be sufficientl small, which is guaranteed b using real-time operating sstems (RTOS). The teleoperation loop update rate is H, which is much lower than the bandwidth of a dedicated LAN, so time dela is not an issue in this project. III. A MAPPING METHOD For a 6-D Titan II manipulator, the Jocobian is a 6 b 6 square matri, and mapping of 7-D master to a 6-D slave manipulator can be solved analticall. But in practice, techniques must be adopted to avoid uncontrollable slave movements that can result from numerical problems when the Titan II approaches a singularit configuration. Generall speaking, a teleoperation sstem can have two fundamental control schemes: open loop and close loop []. An open loop scheme includes a forward kinematic (FK) routine and followed b an inverse kinematic (IK) routine. Four steps are involved in this scheme, and its general procedure is outlined in Figure. The FK routine calculates the Cartesian space velocities from the measurement of the master joint angles; and the Cartesian space velocities are then used as command inputs to the slave IK routine, which produces the joint velocities of slave; the joint angles are finall used as slave actuator command inputs. Where, () v v -cartesian space velocit car wamjnt θ = () v = J v wamjont car wam wamjnt (3) v = J v titanjnt titian car t (4) θ titanjnt = v titanjnt i t v, θ, J -wam joint velocit, angle, jacobian wamjnt wamjnt wam + v, θ, J -titan joint velocit,angle,pseudo-inverse jacobian titanjnt titanjnt titan t - time step Figure. Open loop Cartesian space mapping Step () and step (4) both have a time increment, and the two time steps must be the same in order to get eact tracking. In this sstem, the timers that keep track of the time steps are in two different computers, and thus make it ver critical to have RTOS on the WAM PC and the PC- 4. The clock difference between the two timers under RTOS can be neglected, since the time dela of RTOS is in the magnitude of several microseconds, which, together with the LAN time delas, will introduce some tracking error in the sstem. The effects of those errors are fairl small [3, 4] and outside of the focus this paper. A close loop scheme is formed b feeding back the actual Cartesian positions of the slave. The purpose of having feedback is to reduce the tracking errors and stabilie the sstem []. Under these two schemes, various control methods have been studied in the past. The common objective of these methods is to tr to attain good performance in event of redundanc or singularit, and in these cases, the Jacobian matrices are rank deficient. One of the commonl used methods was the damped least square (DSL) inverse proposed b Y. Nakamura and H. Hanafusa [5], which augments the Jacobian to a full ranked matri when it is rank deficient. It is reported in [6] that the DSL has poor performance near the singularit. Optimiation-based algorithms can be found in [6, 7], these methods generall use some kind of weighting matri, and the selection of the weighting matri is based on heuristic tuning method. It is also reported that the optimiation based algorithm could be times slower than the analtical solution [6]. This paper studied a new technique based on open loop scheme analtical solution, which has good performance near the singularities. As mentioned earlier, the Jacobian of the Titan II is a square matri, and the analtical inverse Jacobian alwas +

3 eists. B theor, the IK problem has close form analtical solution. In realit, problems like abrupt joint angle jumps due to numerical problems can happen when Titan II is near its singular point. For eample, when joints through 5 are lined up, the Titan II can not moves further in the direction, which is generall referred to degree-of-freedom loss. If a Cartesian space velocit command as shown in Equation () is used, the Titan II moves to its singular point quickl with its initial configuration shown in Figure 3. { v, v, v, ω, ω, ω } = {,3,4,,,} p r v, v, v ω, ω, ω p r velocit in,, and ais (inch/sec) angular velocit about,, and ais (rad/sec) Figure 3. Initial configuration of Titan II When Titan II is approaching the singularit, the simulation result shows the joint angles jump abruptl between adjacent time steps (.5 seconds). This tpe of behavior will cause actuator saturation and thus should be avoided. Figure 4 shows the Titan II simulation result of joint velocit ies. The position tracking is lost due to the singularit, which is shown in Figure 6. 3 Joint Velocit vs. time 4 () variation in the command input will be disproportionatel amplified b the inverse calculation, thus causing abrupt changes in the output joint angular velocities. This tpe of situation can be avoided b setting a heuristic threshold for the condition number, and if a small singular value goes below the threshold it is set to ero before the inverse routine. However this is not eactl what occurred in the simulation because the input commands are constant. Figure 5 plots the ratios of the largest singular value to each single singular value. First observation is that the condition number has abrupt changes. Further eamination of Figure 4 and Figure 5 shows that the joint velocit jumps happen at the eact locations where the condition number is undergoing large changes. Condition Number Condition number vs. time Figure 5. Ratio between largest and each singular value of Titan II Jacobian matrices Joint Velocit (rad/sec) Position Error(inch) Figure 4. Titan II joint velocities From Figure 4, the Titan II joint velocit numerical results undergo large jump s under the constant Cartesian speed command. It is commonl known that when using the procedure step () shown in Figure to solve IK problem, the Jacobian matri must have a sufficientl small condition number, which is defined as the ratio between the largest and the smallest singular value; Otherwise, an small Figure 6. Titan II position error in, and direction The eplanation of this situation is that because of the abrupt condition number change between adjacent Jacobian matrices, especiall when a large condition number is involved, some command velocities are disproportionatel amplified b the inverse calculation, and thus makes ver abrupt output speed changes.

4 Same heuristic technique can be used in this case. For eample, b setting the condition number threshold to 5, and keeping everthing else unchanged, a new simulation result shows a much smoother joint velocit trajector. The results are shown in Figure 7 and Figure 8. Joint Velocit (rad/sec) Joint Velocit vs. time Figure 7. Titan II joint velocities Condition number vs. time All errors are in similar magnitude before and after a condition number threshold is used. When the Titan II is moving awa from a singular point, the performance of the analtical solution is ver good without setting a condition number threshold. For eample, when changing the command in Equation () to {-, 3, 4,,, }, and the Titan II starts from the same initial configuration. The titan II will move awa from the singular point. Numerical simulation results are shown in Figure and Figure. Joint Velocit (rad/sec) Joint Velocit vs. time Condition Number Figure. Titan II joint velocities Figure 8. Ratio between largest and ever single singular value of Titan II Jacobian matri Position Error(inch) Position Error(inch) Figure 9. Titan II position error in,, and ais Comparing the position errors as shown in Figure 6 and Figure 9, the errors in the direction for both cases are ver large, which is because Titan II is near singularit, while in the and directions, the errors are relative small Figure. Titan II position error in,, and ais The joint velocities are smooth and tracking errors are small. Notice that the condition number threshold is not set. Based on the above analsis, the conclusion is that b setting an appropriate condition number threshold when the Titan II is moving into a region near a singularit point, the analtical IK solution can perform well near the singularit.

5 IV. EXPERIMENTAL RESULTS Currentl, the real sstem eperiment results are not completed; and in this eperiment, the Titan II result is demonstrated b a model simulation in the Roboworks. The eperimental results are obtained as follows: The operator moves the WAM, and the WAM joint angle values are sent to the server PC at H for FK and IK procedure, which calculated the Titan II joint angle command, and this command is sent to Roboworks Titan II model on the simulation PC for a visual demonstration. The eperiment results are also plotted using Matlab. The WAM initial position and base frame can be shown in Figure (left), and the initial position and base frame of Titan II is shown in Figure (right). Figure. WAM &Titan initial position and base frame Two WAM trajectories were tested. The first one is to open the elbow and then close it back to the initial position. The second one is to lift the shoulder and open the elbow simultaneousl, and then move them back to the initial position. The purpose of the first test is to see the performance when the Titan II is far awa from the singularit, while the second one it to check the performance while the Titan II is near the singularit. In the first test, the condition number threshold is not set; while in the second, the condition number threshold is set to be when the titan is moving towards a singular point, 5 while it is moving awa from the singular point, when Titan II is far awa from the singularit, the threshold is not set. The results of the first test are shown in Figure 3 through Figure 6. The results of second tests are shown in Figure 7 through Figure. The selection of condition number threshold depends on the geometric construction of the manipulator; in this paper, an operational space condition number map was used to help determine an appropriate threshold, other than that, it is basicall a trial and error method. Position Error(mm) - Cartesian position vs. time position (mm) Figure 3. Titan II position in,, and ais Figure 4. Titan II position error in,, and ais Velocit Error (mm/sec) Cartesian space velocit error vs. time v v v Figure 5. Titan II velocit errors in, and ais

6 Joint Velocit vs. time 4 Cartesian space velocit error vs. time.5 - Joint Velocit (deg/sec) Velocit Error (mm/sec) v..5-6 v v Figure 6. Titan II joint velocities Figure 9. Titan II velocit errors in, and ais Joint Velocit vs. time 5 Cartesian position vs. time position (mm) Joint Velocit (deg/sec) Figure 7. Titan II position in,, and ais Figure. Titan II joint Velocities Position Error(mm) Figure 4 and Figure 5 show that good position and velocit tracking are achieved when the Titan II is far awa from the singularit points. Figure 8 and Figure 9 show that when the Titan II moves into a region near the singular point, before 4 seconds, position and velocit tracking are lost. When it moves out of this region, after 4.5 seconds, position and velocit tracking are achieved approimatel. In all cases, the Titan II joint movements are smooth and the Titan II is in a controllable state even it is close to a singular point Figure 8. Titan II position error in,, and ais V. FUTURE WORK Interests in future work are: ) Complete the sstem implementation; ) Further develop an intelligent routine to chose an appropriate condition number; 3) Attack some of the control issues associated with the hdraulic actuators of Titan II;

7 4) Develop and implement a close loop control scheme; 5) Stud redundanc allocation strategies with the 7- D WAM; 6) Stud the haptic implementation and its effect on teleoperation. ACKNOWLEDGEMENTS This research was sponsored b DOE under Grant No. DE-FG-ER8337. The authors would like to thanks ORNL and Barrett Technolog for providing the hardware and initial control software, and QNX for provide the QNX 6.3 Neutrino professional license through its education program. REFERENCES. Mantegaa, P., DIAPM RTAI programming guide.., Lineo, Inc.: Lindon, Utah.. Sciavicco, L. and B. Sicilianno, Modeling and control of robot manipulators. 996, New York: McGraw-Hill. 3. Ip, B. Performance analsis of VWorks and RTLinu. in the First Embedded Software Contest. 3. Korea. 4. Proctor, F.M. and W.P. Shackleford. Real time operating sstem timing jitter and its impact on motor control. in SPIE Conference on Sensors and Controls for Intelligent Manufacturing.. Wuhan, China: SPIE. 5. Nakamura, Y. and H. Hanafusa, Inverse kinematic solutions with singularit robustness for robot manipulator. Journal of Dnamic sstems, measurement, and control, 986(8): p Schreiber, G., M. Otter, and G. Hiringer. Solving the singularit problem of non-redundant manipulators b constraint optimiation. in IEEE, Conference on Intelligent Robots and Sstems Kongju, Korea. 7. English, J.D. and A.A. Maciejewski, On the implementation of velocit control for kinematicall redundant manipulators. IEEE transactions on sstems, man, and cbernetics,. 3(3): p