A Versatile Mechatronic Tool for Minimally Invasive Surgery

2 downloads 0 Views 243KB Size Report
Abstract – This paper describes a novel handheld mechatronic tool for minimally invasive surgery (MIS) able to assist the surgeon in several surgical acts and ...
A Versatile Mechatronic Tool for Minimally Invasive Surgery Francesco Amato, Marco Carbone, Carlo Cosentino, and Alessio Merola

Michele Morelli, and Fulvio Zullo

BioMechatronics Laboratory Università degli Studi Magna Græcia di Catanzaro Campus Universitario di Germaneto v.le Europa, 88100 Catanzaro, Italy {amato, carlo.cosentino, merola}@unicz.it

Dipartimento di Medicina Sperimentale e Clinica Università degli Studi Magna Græcia di Catanzaro Campus Universitario di Germaneto v.le Europa, 88100 Catanzaro, Italy [email protected]

Abstract – This paper describes a novel handheld mechatronic tool for minimally invasive surgery (MIS) able to assist the surgeon in several surgical acts and enhancing his (or her) dexterity and sensitivity. The main feature of this tool is the limitation of the risk of damage of the biological tissues in a plurality of procedures of manipulation (dissection, blunt dissection, pulling, stretching and stripping), of suture and cutting, in which the actuators included inside the tool assist the surgeon in performing these surgical acts. For this purpose, the mechatronic tool is equipped with an embedded microcontroller and sensors measuring the operating forces, which enable the closed loop force and torque control of the tool– tissue interaction. Through the consciousness and the direct control of the interaction forces, the surgeon can manipulate tissues selectively by the application of the operating forces on the basis of the surgical needs and of the tissue consistence. In particular, this paper discusses the design steps performed via a virtual prototyping approach implemented in a MATLAB/SimulinkTM environment and composed of kinematic, dynamical modeling and control system synthesis. The advantages in terms of simplification of the surgical act, resulting from the properties of servo–assistance of the tool, have been shown by the use of a preliminary version of the tool prototype in a simulation, performed by a pelvi trainer, that has involved complex tasks as suturing and knot tying. Index Terms – biomechatronic instruments, laparoscopic tools, minimally invasive surgery, ergonomics in surgery.

I. INTRODUCTION During the last ten years, the use of minimally invasive surgery has drastically increased, having been used in a wide range of interventions in gynecological, thoracoscopic, arthroscopic, neurosurgical treatments and especially in laparoscopic surgery. In Minimally Invasive Surgery (MIS) for the abdomen, the so–called laparoscopic surgery, the access into the body is achieved via round cannulas (trocars) inserted through small incision (less than 10mm) [1]. First CO2 is insufflated in the abdominal wall of the patient through these cannulas, to raise the wall and to form the workspace in a peritoneal cavity; then the laparoscope, equipped with a CCD camera, is inserted in order to view on a monitor the images of the operating theatre. Finally, in the operating step, the tissues are treated by means of the insertion of several elongated tools (graspers, scissors, needle holders, staplers and other), each having a specific tip. Technology advancements in instrumentation have enabled to exploit and to enhance the benefits of the

minimally invasive technique in surgical intervention. These advantages are: shorter recovery time, limitation of trauma and pain for the patient, good aesthetic results, and reduction of intraoperative and postoperative risk of complications. However, MIS, performed by the use of this technology, has produced many drawbacks in comparison with the conventional surgery. Indeed, the CCD camera supplies a distorted image, principally because of the lack of the stereoscopic effect. Moreover, the elongated tools, because of the fulcrum at the cannula, reduce dexterity and above all – these are the major disadvantages– eliminate the tactile sensation and filter the kinesthetic force feedback due to the friction and the backlash of the transmission mechanism. The abovementioned drawbacks suggest that extensive training is required to the surgeon in order to learn to interact with the patient in MIS operation through this new interface. A recent paper [2] examines advantages and disadvantages of various systems proposed for training in MIS, such as Pelvi-Trainers, Virtual Reality trainers with and without haptic feedback, and robots. Nowadays, MIS experimental tests enable to improve the realism of the surgical trainers by the in vivo measurement of surgical gestures [3] and by the identification of the mechanical behaviour of tissue grasped by means of laparoscopic forceps [4]. A skill based methodology is proposed in [5] for analysis of efficacy and objective evaluation of MIS training. Several reviews explain the fundamental role of the advanced surgical instrumentation capable of enhancing the human range of dexterity and perception, which is available today in the context of the so–called Computer Aided (or Integrated) Surgery (CAS or CIS) [6], in robotic and mechatronic applications for medicine and surgery [7]–[8] and for MIS [9]. Nowadays, the growing use of robotic and tele– operated surgery shades hand–held mechatronic tools. However mechatronic tools offer several benefits compared to robots, since they do not require complex training, exhibit low costs, can perform complex working tasks and, above all, are intrinsically safe since the surgeon controls the tool directly by hand and can promptly interrupt an undesired surgical act. This paper describes a novel handheld mechatronic tool for MIS, particularly for operations of laparoscopic surgery, able to assist the surgeon in several surgical acts and

enhancing his (or her) dexterity and sensitivity. The main feature of this tool is the limitation of the risk of damage of the biological tissues in a plurality of procedures of manipulation (dissection, blunt dissection, pulling, stretching and stripping), of suture and cutting, in which the actuators included inside the tool assist the surgeon in performing these surgical acts. For this purpose, the mechatronic tool is equipped with an embedded microcontroller and sensors measuring the operating forces, which enable the closed loop force and torque control of the tool–tissue interaction. In addition, the human–machine interface (HMI) allows the surgeon to control precisely and safely the interaction forces between the tool tip and the tissue, while he (or she) feels on his (or her) hand the perception of the magnitude of the same forces. Through the consciousness and the direct control of the interaction forces, the surgeon can manipulate tissues selectively by the application of the operating forces on the basis of the surgical needs and of the tissue consistence. Furthermore the tool enables the surgeon to obtain a rough measure of the tissue consistence by integrating the force perception supplied by the tool with the visual feedback – obtained by the laparoscope – of the spread variation of the grasping tip. In particular, this paper presents the functional description of the mechatronic tool and describes the design steps performed via a virtual prototyping approach by kinematic, dynamical modeling and control system synthesis implemented in a MATLAB/SimulinkTM environment. The advantages in terms of simplification of the surgical act, resulting from the properties of servo–assistance of the tool, has been shown by the use of a preliminary version of the tool prototype in a simulation, performed by a pelvi trainer, that has involved complex tasks as suturing and knot tying. The paper is organized as follows: Section II deals with the characteristics of the device and its modeling; Section III presents and discusses the results obtained both by computer simulations and by experimental trials in the clinical environment; in Section IV some concluding remarks and plans for future work are given. II. MATERIALS AND METHODS In the state–of–the–art tools used for manipulating soft tissues (such as graspers and needle holders) in MIS, the surgeon interacts with the tissue by an handle that transmits the forces to the tip through the elongated body of the tool (Fig. 1). The mathematical model in [10] and the experimental test in [11] show the nonlinear relationship between the magnitude of the grasping tip force and the handle force. Moreover, the mechanical efficiency of the transmission mechanism of these forces is lower than 50% [12]. It is evident that the attenuation of the surgeon’s kinesthetic force feedback of the tissue interaction is the effect of the non–linearity and the low mechanical efficiency of the transmission mechanism of the forces. Indeed, in [13] and [14] it is verified by experimental tests that the surgeon’s ability to determine the tissue properties by means of laparoscopic grasping forceps is highly dependent on the mechanical efficiency of the instrument.

Fig. 1 Set of laparoscopic tools.

A device that can accurately transmit the interaction forces and reproduce faithfully the mechanical impedance of the tissue is desirable in order to help the surgeon to feel and to control the tissue interaction safely and precisely. In this context, the tool here presented is capable of restoring to the surgeon the full control and the entire perception of the tool tip–tissue interaction forces by a simple and reliable mechatronic configuration provided with an intuitive HMI. Therefore, in MIS operation, soft tissue is not damaged and the local consistence of the tissue can be detected. In particular, this last feature is useful both to discriminate anatomic details and to identify tissues in pathological conditions. This is acquired by the surgeon that, conscious of the grasping force on the tissue, integrates this force perception with the feedback obtained by camera images – if they are adequate – of the spread variation of the grasping tip in a pinch. Although the advantage of restoring the force feedback for controlling the tool–tissue interaction to the surgeon has been widely revealed, some problems in current technologies limit the possibility of implementing force feedback in the operating room (OR). Currently, force feedback is performed in some laparoscopic simulators in systems for Virtual Reality [15] and in teleoperated laparoscopic forceps [16]–[19]. In these cases, the difficulty in rendering realistic force feedback to the surgeon is often recognized, because of the low stiffness of the soft tissue that makes the force reflection on the master more difficult. In addition, an exhaustive dynamic model capable of describing the behaviour of various soft tissues in a procedure of manipulation is not still defined. The abovementioned inconveniences and the structural complexity, which involves an high chance of failure, make current apparatus implementing force feedback unsafe for the use in OR. On the contrary, the mechatronic tool here presented offers to the surgeon a simple and reliable technology for controlling operating forces and limiting the damage to tissues, immediately available for OR and intrinsecally safe. This tool is intuitive, since it keeps the same structure of the conventional laparoscopic instrument, and enables opening/closing and orientation of the tip.

(a)

(b) Fig. 2 (a) 3D Model and (b) Control Scheme of the mechatronic tool

Through the kinesthetic perception of the magnitude of the interaction forces with the tissues, provided on the surgeon’s hand by the HMI designed for this purpose, he can control these interaction forces precisely and safely. The opening/closing and orientation of the tip are obtained by means of the force and torque actuators included in the tool respectively, under the supervision of the closed loop micro–controller and on the basis of the set– point of force and torque on the tip determined by the surgeon. The mechatronic tool offers strong advantages in the complex tasks (such as suture and knot tying) that in MIS generally require of the surgeon conspicuous dexterity to orient the instrument tip. In these surgical acts, the variation of the tip orientation often exceeds the stroke limits of flexion–extension of the surgeon’s wrist. Because of the uncomfortable and awkward position of the wrist and the arm and, above all, because of the repetitive and difficult gestures in performing these acts, the surgeon suffers hand fatigue and grip weakness. For these problems, operating time and probability of mistakes increase. Instead, without ergonomic problems and stroke limits, in this tool the surgeon can determine the variation of the tip orientation by acting on the command of the torque on the tip. In addition, by the direct control of the torque exerted on the tissue when rotating the needle, it is possible to avoid the break of the needle itself. This tool is useful in several MIS and laparoscopic procedures because it is provided with a joint enabling the interchangeability of the surgical utensil.

The characteristics of the versatile mechatronic tool will be shown in conceptual and structural details in Sections II-A and II-B. A. Design of the versatile mechatronic tool The conceptual characteristics of the mechatronic tool have been defined according to the indications given by the team of laparoscopic surgeons of the University of Catanzaro. The design work has been based on the definition of a suitable apparatus able to satisfy the surgical requirements in terms of increase of ergonomics, precision and safety of the surgical act (fig. 2.a). Particular care has been devoted to the definition of the HMI. This includes the commands through which the surgeon determines the set– point of the force on the tissue between the jaws and of the torque for the orientation of the tip respectively. These commands consist of levers linked at one side to a spring and, at the other side, to an electric potentiometer measuring the stroke of the levers. In addition, the lever for the command of force includes a mechanism preventing retrograde motion. In this way, the lever remains blocked when the surgeon releases it. In particular, this feature is useful when the tool works as needle holder. The command of the operating forces is imparted by the surgeon proportionally by varying the position of the appropriate levers and it is transmitted to the controller through the measurement of the lever position supplied to the potentiometer. The reaction of the spring included in every lever enables the surgeon both to receive, by means of the kinesthetic perception on his (or her) hand, a measure of the

magnitude of the operating forces and to perform a steadier control of these forces. In addition, the HMI on the tool includes an emergency button that allows to stop a surgical procedure in case of failure. The contact force between jaws and tissue may be a cutting force if the utensil is a scissors, or a grasping force in case of forceps. The force on the tip jaws is produced by a linear electromagnetic actuator (LEMA) (solenoid), the torque on the tip is obtained through a coreless DC motor with speed reducer. The closed loop control scheme (Fig. 2.b) of the force between the tip jaws and of the torque at the tip includes sensors of force and torque adopting strain gauges. The layout of the sensors has been defined by evaluating advantages and disadvantages of the feasible solutions. In a first solution, the sensors are as near as possible to the jaws. In this case, the control system has the major advantages in terms of performaces, since the measurements of the control variables are direct and not affected by friction and backlash of the transmission mechanism. In the other solution, which has been preferred, the sensors are placed inside the cylindrical body of the tool. The latter solution has a negative impact on the performances of the control system but it offers major advantages in terms of constructive simplicity and functionality of the instrument. Hence the detachable portion, in which the sensors are not included, can be sterilized without problems of damage to the sensors or, in case of disposable use, with little waste. In this last configuration, the controller indirectly determines the measure of the force between the jaws that, as described in Section II–B, depends on the applied force on the internal elongated shaft of the tool, through the kinematic state of the mechanism transmitting this force to the tip jaws. Therefore, the tool includes a Hall–effect sensor that determines the linear motion of the internal elongated shaft actuating the tip jaws. In addition, the micro–controller enables to set programmable force limits depending on the surgical task in order to protect the tissues from not suitable operating forces. B. Kinematic and dynamical modeling of the transmission mechanism of the force to the tip jaws The main purpose of this section is to develop the mathematical models of the kinematics and of the dynamics of the transmission mechanism of the force to the tip jaws both in the conventional instrument and in the mechatronic tool. As described in Section III, these models have been implemented in the MATLAB/SimulinkTM environment, in order to carry out an evaluation of the performances of the tool. The typical laparoscopic tool comprises two subsystems (handle and tip) interconnected through an elongated shaft (Fig. 3). Clearly, the mechatronic version of the tool is characterized by the lack of the kinematic chain of the handle. Both in the conventional instrument and in the mechatronic tool, the elongated shaft translates alternately. The mechanism crank–connecting rod at the tip transforms this linear motion in the spread of the jaws.

l

P1

Fig. 3 Kinematic scheme of the transmission mechanism

For the solution of the dynamic problem, a Lagrangian formulation has been preferred since it allows to obtain an analytical dynamic model of the mechanisms; this step is necessary for the further synthesis of the controller. The equations of motion contain strongly nonlinear terms due to the inclusion of the inertial loads, with the added difficulty that the forces exerted by the tissue on the tip depend on the instantaneous kinematic configuration of the joints. 1) Kinematic modeling: The transmission mechanism has clearly one degree of freedom, both in the conventional instrument and in the mechatronic tool. In order to describe the kinematic state in the conventional forceps, it is sufficient to compute the angular spread of the jaws (Tt) as function of the angular spread of the handle named Th. Referring to Fig. 3, we obtain

Tt

T t max

2 §§ ¨ ¨ r  1 Q ·¸ p  1 lP 4  Q iy 2 2 ¨ 2 r¹ 2 r l  T10  tan 1 ¨ © 1 Q 1 Q2 § · ¨ ¨r  ¸ P  lpiy 4  2 2 ¨ 2 r¹ 2 r l ©©

· ¸ ¸ ¸ ¸ ¸ ¹

(1)

where P

2

piy  LJ 1J 2 (cosTh  cosTh0 )  LJ 2 J 3  (LJ 1P1  LJ 1J 2 sinTh )2  (2) 2

 LJ 2 J 3  (LJ 1P1  LJ1J 2 sinTh0 )2

Q

2 P 2  piy  r 2  l 2 ,

(3)

T10 and Th0 denote the initial values of T1 and Th respectively, Ttmax is the maximum angular spread of the jaws, LHK represents the distance between the points H and K, and pix and piy denote the initial Cartesian coordinates of the pivot B with respect to the frame on the tip.

exerts on the tissue from the driving force on the elongated shaft.

Fig. 4 Simulation results of a tissue pinch (a)

Fig. 5 Pinch of various tissues (b)

Furthermore, it is useful to determine the relationship between the magnitude of the force on the arm of the tip jaws (Ft) and the force applied to the handle (Fh) as a function of the kinematic parameters. The force on the jaws (Ft) is localized at the end of the jaw arm Ft

2 Fh LJ 1P r cos(T 2  T1 ) sin T 2 Lt L J 1J 2 sin T h

(4)

Fig. 6 Mechatronic tool tip depicted in performing suture (a) and knot tying (b) in pelvi trainer

Cr contains the reaction torque of the tissue and of the constraint at the end stroke. For the scope of this work, a linear relationship between the angular displacement of the tip and the reaction torque of the tissue has been judged adequate to describe the interaction of the instrument with the biological tissue. III. RESULTS

2) Dynamical modeling: The Lagrangian formulation of the dynamic model of the mechanism in the conventional tool yields the following equations of motion B (q ) q  C (q, q ) q  Fv q C  C r ,

(5)

where B(q) is the inertia of the mechanism that depends on the lagrangian variable q identified by the angle Th at the handle, C ( q, q ) consists of Coriolis’s and centrifugal terms, Fv q identifies the torques of viscous friction, C is the torque applied by the surgeon on the handle, Cr contains the equivalent torque on the handle produced by the tissue interaction, the reactions exerted by the constraints of the end stroke, the equivalent torque resulting from the viscous friction at the elongated shaft and at the tip, and the contribution of the elastic torques created by the spring at the handle. Gravitational terms can be neglected. The equation of motion of the mechanism in the mechatronic tool has the same form as in (5). In this last case, q is the spread between the jaws T t and C is the torque that the jaw

The reliability of the proposed tool has been tested both computer simulations, performed in the MATLAB/SimulinkTM environment, and in the clinical context. The dynamical behaviour of the conventional forceps, resulting from the interaction with the tissue, has been evaluated by the simulation of a procedure of pinch. For this purpose, the conventional forceps has been modelled as a dynamic system having as input the force on the handle exerted by the surgeon and as output the spread between the jaws (Fig. 4). Several simulations have been performed in order to evaluate the response of the system interacting with various tissues (Fig. 5). These simulations have confirmed the low efficiency in the transmission mechanism of the force due to the nonlinearity, evidenced by the experimental studies in [11]–[13]. The simulations performed on the Simulink model of the mechatronic tool have aimed at the analysis of the performances of the system and at the controller synthesis. Via simulation, it has been possible to compare the performances of the mechatronic tool with those ones of the conventional instrument. In particular, the via

MATLAB/SimulinkTM model of the mechatronic tool has been

used as virtual test bench in order to define the mechatronic configuration (electromechanic parts and control system) suitable for the requirements of the application. In the clinical environment, the increase of precision and ergonomics of the surgical act, resulting from the use of the mechatronic tool, has been demonstrated by simulating, in a pelvi trainer and by means of a preliminary version of the tool prototype, some procedures of suture (Fig. 6.a) and knot tying (Fig. 6.b). In the conventional laparoscopic procedure of suture, the surgeon handles two forceps: one for manipulating the tissue edges, the other for holding the needle. For passing the needle through the tissue edges, generally the surgeon rotates the forceps on the axis of the elongated shaft by the limited torsions of his (or her) wrist or, in improved versions of the tool, he uses a wheel included in the forceps. In both cases, the surgical act is fatiguing and not ergonomic. On the contrary, the simulations on the pelvi trainer have shown that the use of the mechatronic tool allows the reduction of the fatigue of the surgeon and an increase of dexterity and precision. Indeed, in the mechatronic tool, the tip orientation is obtained without stroke limits by acting easily on the ergonomic lever for the torque command.

[3]

[4]

[5]

[6]

[7]

[8]

[9]

[10] [11]

[12]

IV. CONCLUSIONS The aim of this work was to develop a mechatronic solution simple and reliable, able to increase safety and dexterity of several MIS procedures. For this purpose, an innovative tool has been developed and tested both virtually and in the clinical environment. Due to the properties of servo–assistance, the tool successfully supports the surgeon in terms of reduction of fatigue and of increase of dexterity, particularly in those procedures involving a conspicuous and repeated employment of the wrist. The kinesthetic perception of the magnitude of the operating forces on his (or her) hand, provided by the tool, enables the surgeon to control these servo–assisted procedures precisely and safely for the tissues. The design work has focused on the development of the hardware of the tool. Therefore, some features can be improved and developed, as, for example, an HMI interface enabling to set programmable force limits depending on the surgical task. Concerning the modeling of the instrument, some investigations are required about the characterization of the viscoelastic and dynamical behavior of the tissue in a pinch, in order to perform a more realistic assessment of the tool performances by computer simulation. Finally, further tests on a new prototype are needed in order to render the tool suitable for the clinical practice in all the features. REFERENCES [1] [2]

L. W. Way, Fundamental of Laparoscopic Surgery, Churchill Livingstone, 1995. J. Dankelman, “Surgical robots and other training tools in minimally invasive surgery,” IEEE International Conference on Systems, Man and Cybernetics, 2004, pp. 2459–2464.

[13]

[14]

[15]

[16]

[17]

[18]

[19]

P. Dubois, Q. Thommen, and A.C. Jambon, “In vivo measurement of surgical gestures,” IEEE Transaction on Biomedical Engineering vol. 49, no. 1, pp. 49-54, Jan. 2002. A. Bicchi, G. Canepa, D. De Rossi, P. Iacconi, and E. Scilingo, “A sensor based minimally invasive surgery tool for detecting tissue elastic properties,” IEEE International Conference on Engineering in Medicine and Biology, 1997, pp. 961–964. J. Rosen, J.D. Brown, L.Chang, M. Barreca, M.Sinanan, and B. Hannaford, “The blueDRAGON–a system for measuring the kinematics and the dynamics of minimally invasive surgical tool invivo,” IEEE International Conference on Robotics and Automation, 2002, pp. 1876–1881. R.H. Taylor, S. Lavallèe, G.C. Burdea, and R. Mosgës, Eds., Computer Integrated Surgery: Techonology and Clinical Applications,. Cambridge, MA: MIT Press, 1996. P. Dario, E. Guglielminelli, B. Allotta, and M.C. Carrozza, “Robotics for medical applications,” IEEE Robotics and Automation Magazine, vol. 3 , pp. 44-56, Sept. 1996. P. Dario, M.C. Carrozza, B. Allotta, and E. Guglielminelli, “Micromechatronics in medicine,” IEEE/ASME Transaction on Mechatronics, vol. 1 , pp. 137-148, June 1996. P. Dario, B. Hannaford, and A. Menciassi, “Smart Surgical Tool and Augmenting Devices,” IEEE Transaction on Robotics and Automation, vol. 19 , pp. 782-792, Oct. 2003. S. Payandeh, “Force propagation Models in Laparoscopic Tools and Trainer,” IEEE/EMBS International Conference, 1997, pp. 957–960. S.M. Sukthankar, and N.P. Reddy, “Towards force feedback in laparoscopic surgical tool,” IEEE/EMBS International Conference, 1994, pp-1041-1042. W. Sjodersma, J.L. Herder, M.J. Horward, A. Jansen, J.J.G. Bannenberg, and G.A. Grimbergen, “Force transmission of laparoscopic grasping instrument,” Minimally Invasive Therapy & Allied Techonologies, vol. 6, no. 4, pp. 274-278, 1997. K.T. Den Boer, J.L. Herder, W. Sjoerdsma, D.W. Meijer, D.J. Gouma, and H.G. Stassen, “Sensitivity of laparoscopic dissectors. What can you feel?,” Surgical Endoscopy, Ultrasound and Interventional Techniques 13, pp.869-873, 1999. E.A.M. Heijnsdijk, A. Pasdeloup, A.J. van der Pijil, J. Dankelman, and D.J. Gouma, “The influence of force feedback and visual feedback in grasping tissue laparoscopically, Surgical Endoscopy 18, pp. 980-985, 2004. M. Schijven, and J.Jakimowicz, “Virtual reality surgical laparoscopic simulator: how to choose,” Surgical Endoscopy, vol.17, no. 12, pp. 1943-1950, 2003. J. Rosen, B. Hannaford, M.P. MacFarlane, and. M. N..Sinanan, “Force controlled and teleoperated endoscopic grasper for minimally invasive surgery – experimental performance evaluation, IEEE Transaction on Biomedical Engineering, vol. 46, no. 10, pp. 12121221, Jan. 2002. M.Lazeroms, A. La Haye, W. Sjoerdsma, W. Schreurs, W. Jongkind, G. Honderd, and C. Grimbergen, “A Hydraulic forceps with forcefeedback for use in minimally invasive surgery,” Mechatronics, vol. 6, no. 4, pp.437-446, 1996. J.L.Herder, M.J. Horward, and W. Sjoerdsma, “A Laparoscopic grasper with force perception,” Minimally Invasive Therapy & Allied Techonologies, vol. 6, pp. 279-286, 1997. M.C. Cavusoglu, F. Tendick, M. Cohn, S.S. Sastry, "A laparoscopic telesurgical workstation," IEEE Transactions on Robotics and Automation, vol. 15, no. 4, pp.728 - 739, Aug. 1999.