Manipulation of a Whole Surgical Tool Within Safe Regions Utilizing Barrier Artificial Potentials

Active constraint enforcement in robotic-assisted surgery is critical for reducing the intra-operative risk of unintentionally damaging sensitive tissues by the surgical instrument. This work considers surgical instruments which can be circumscribed by a geometric capsule and forbidden regions which can be approximated by point clouds in order to produce a repulsive wrench by the control action to guarantee manipulation within safe regions. This work details the control scheme which is based on barrier artificial potentials when considering the whole tool extending our previous results on the tool point. A proof of the control system’s passivity and non constraint violation is provided together with experimental results using a 7-dof KUKA LWR4+ manipulator as a master device in a virtual surgical scene in order to demonstrate the effectiveness of the proposed scheme.


Introduction
Barrier artificial potentials are positive scalar functions defined in a subset of a system's state with the sole purpose of enforcing actively i.e. via the control action, the confinement of the system evolution within this region. They take infinite values on the boundaries of the region thus creating a barrier and they are minimized at the system's equilibrium that can be an isolated point or even a whole set of states. Utilizing the gradient of these potentials in a control law is equivalent of creating a repulsive force away from the boundaries of the forbidden regions; the closest to the boundary the higher the repulsive force. These type of artificial potential fields were initially proposed for obstacle avoidance in robot's autonomous operation [1]. In physical human robot interaction, they were also utilized to impose spatial forbidden regions for ensuring the safety of the robot and the environment. A study on the passivity, state boundedness, constraint satisfaction and performance of the system under the exertion of a human force is studied in [2]. The study is restricted to the case of spatial constraints defined analytically in the robot's world frame and for only the manipulator's tool tip position. Being a promising solution for confining a robot within a restricted area, these type of potentials have been proposed for use in the application area of robotic surgery in order to guard the safety of sensitive regions like vessels surrounding the organ that is being operated from accidental penetrations. They are one way of implementing active constraints (AC), an extensive review of which can be found in [3]. Surgical sites consisting of organs, vessels and surrounding tissue have complex geometries and analytical expressions of sensitive areas are not available. What is available during robotic surgery is an image of the surgical site provided by the endoscopic camera usually in the form of a dense point cloud. For this reason, subsets of these 3D-point clouds are used to describe the constrained regions or even the operating regions as reported in several related previous works [4], [5], [6]. The point cloud characterization can be performed by the doctors via an appropriate interface. In our previous work [6], an active constraint controller was proposed based on a barrier artificial potential applied in point cloud regions that are characterized as forbidden areas. The work is restricted for the case of constraining the tool tip so that it does not touch or penetrate the sensitive areas and includes a proof of the passivity and constraint satisfaction. However, the protection of the sensitive area requires the consideration of the whole tool body which can be a scalper with a cutting edge running along one of its edges. Previous work on AC enforcement on the whole tool is reported in [4] but it does not include any theoretical justification. We have extended our method for the whole tool and initial experimental results are reported in [5]. The latter however does not elaborate on the whole tool method neither provides a stability analysis for the case of the whole tool.
In this work, we detail our proposed method for the whole tool and include a stability analysis which proves the passivity and non constraint violation given point cloud representation of the forbidden regions. The method involves tool modelling by a capsule and finding the points of the capsule that are closer than a prespecified distance from the points of the forbidden area. The proposed active constraint controller essentially aggregates the repulsive force signal at all these points producing a total force and torque on the robot's wrist. Such a force can be visualized or even fed back for haptic transparency. Experimental results are presented with a KUKA robot and a virtual slave site.

Active Constraint Control
The robot manipulator must be controlled in such a way that, firstly, will actively resist the human operator by exerting a repulsive generalized force when he guides the instrument near the forbidden area not allowing to enter it and, secondly, will maintain the passivity property and state boundedness irrespective of whether its instrument is away or near the constrained space.
The surgical instrument (e.g. Endowrist Permanent Cautery Spatula or Scalpels) is assumed to be rigidly fixed in the robot wrist frame {W } whose position p w ∈ R 3 and orientation R w ∈ SO(3) is known in the inertia world frame {O}. Let the instrument be circumscribed by a capsule which is described by a line segment with length L ∈ R + and a radius r ∈ R + (Fig 1). These parameters can be easily found from the surgical instrument's specifications. Without loss of generality, assume that one of the line segment's end points coincides with the wrist position p w when the instrument is fixed at the robot's wrist. Let the forbidden area be provided by a 3-D point cloud O s from the endoscopic camera with a known and homogeneous density of ρ points per cm 3 . For the purposes of the AC enforcement we consider spheres S( [5] so that empty spaces between the points of O s are covered. The forbidden region is thus mathematically described by the set pi∈Os {p ∈ R 3 : p − p i ≤ d c1 }. The control objective for constraint enforcement is thus achieved if the instrument does not touch the surface of the forbidden region i.e the surface of the overlapping spheres. In order to achieve this objective, for each point p i ∈ O s , we need first to find the nearest point on the capsule's line segment p * i ∈ R 3 (see Fig. 1) and then impose a repulsive force on it. Calculation of the nearest point on the capsule's line segment, the barrier artificial potential and its related control signal are detailed in the following sections.

Nearest point
Given the length L and the unit direction of the surgical tool w n ∈ R 3 expressed in the wrist frame {W } as denoted by the left superscript, we can express the position of all points belonging to the capsule's line segment with respect to the inertia frame {O} by the following parametric equation: where n = R w (t) w n expresses the main unit axis of the capsule w.r.t {O} and σ ∈ [0, 1]. In each control cycle and for each point p i ∈ O s , we find the nearest point Notice that given the analytic expression of the line segment allows the analytic calculation of σ * i as follows: where  Figure 1 illustrates the three possible cases of equation (3). Notice that n T (p i − p w ) is the length of the orthogonal projection of (p i − p w ) on n.

Controller design
A barrier artificial potential initially proposed in [5] for the tool tip case is then applied to each pair (p i , p * i ). It is given by the following equation and illustrated in Fig. 2 : is the Euclidean distance between p i and p * i , d c = d c1 + r moves the point of interest on the capsule's surface and d 0 ∈ R is a positive constant which determines the area of influence of the repulsive potential. Notice that as d(p i , p * i ) tends to d c i.e as a sphere of the forbidden region approaches the capsule surface, the potential (4) tends to infinity.
Hence, considering the first case of (4), For ensuring the safety of the sensitive forbidden region regarding the whole tool, the following inequality constraints should be satisfied at all times, d(p i , p * i ) > d c for each p i . To impose actively these constraints the negative gradient of the function V i is used as a repulsive force f i = − ∂V i ∂p * i acting on p * i , and is analytically given by: where e i ∈ R 3 is the unit vector with direction pointing from the center of the sphere towards to p * i : Then, we aggregate the effect of all these repulsive force by transferring them to the origin of the wrist frame or any other frame of interest. In fact the action of each f i acting on p * i can be produced by the pair of force f i and torque τ i = (p * i − p w ) × f i acting on the wrist p w . The total repulsive wrench can be found by summing the repulsive forces f i and the generated torques τ i using (1) for σ = σ * i : The proposed control signal for enforcing active constraints for the whole tool body is therefore given by: where k > 0 is a scalar gain. Using this control signal, it is guaranteed that the robot's instrument does not violate the forbidden region and that the system is passive. The proof is given in the Appendix.

Experiment
The experimental set up to validate the proposed method is consisted of a 7-dof KUKA LWR4+ robotic manipulator acting as the master device. On the wrist of the master device a handle is attached which is used to guide the tool in a virtual slave scene. This scene consists of an identical virtual manipulator to be used as the slave manipulator; its joint positions copy the joint positions of the real KUKA arm. In the virtual scene a 3-D point cloud of the vessels (forbidden area) and a 3-D point cloud of the kidney (operative area) are imported and registered with respect to the slave arm (Fig. 3). The repulsive wrench calculated in the virtual scene is applied as a wrench input in the wrist of the master device to simulate the haptic rendering of a real master-slave architecture. A wrist force/torque sensor (ATI Mini40) is utilized at the master device to measure the exerted force for visualization purposes. The application of the repulsive wrench input in the master is implemented utilizing KUKA's FRI library with control frequency f s = 500Hz via joint impedance control mode. In the slave scene a capsule circumscribing the surgical tool is considered with radius r = 0.009 m and length L = 0.08m (see Fig. 3).
As the purpose of the surgeon is to be able to remove a tumor from the kidney avoiding to touch the surrounded vessels, the repulsive artificial potential field is designed so that the minimum distances between the sphere of each point of the forbidden area and the capsule are less than 2.3 cm, in other words d 0 = 2.3 cm. Thus the surgeon is able to move freely in the operating area and feels a repulsive force only when accidentally guides the tool closer to the vessels by this distance. The controller gain is set to the value k = 0.007. In order to increase the computational performance, the point cloud of the vessels is down sampled and the radius of the spheres to cover the empty spaces is calculated to the value of d c1 = 2.18mm. To reduce the computational complexity of finding the p * i by Eq. (3) the search space is reduced via a k-d tree search [7,8] to a sphere around the capsule centered at p s (0.5, t) with radius d 0 + d c + L 2 . Then Eq. (2) is utilized for finding the nearest points p * i within the points of O s that belong to this reduced space.
As proposed in [4] and in order to make the repulsive force norm independent from the density of the point cloud, the following formula is utilized for the calculation of the total repulsive force : (10) Notice that in this way the norm of the repulsive force depends only on the nearest repulsive pair p i , p * i ; thus the repulsive force that is generated does not depend on the number of the cloud points that are activated and hence on the local geometry of the activated forbidden area. To validate the proposed controller, the user was asked to guide the tool initially on the kidney and then close to forbidden area (vessels). In the first case no repulsive forces are generated (Fig. 4a) while in the second repulsive forces are produced (Fig. 4b). As the operator tries to come even closer by exerting high guiding forces, even higher repulsive forces on the wrist are produced that cannot be practically overcome thus keeping the vessels safe from penetration (Fig. 4c). The minimum distance from the capsule and the repulsive force and torque exerted in the master's wrist are shown in Fig 5. It is observed that when the minimum distance decreases the repulsive force increases and vice-versa. Notice that the minimum distance is never less than d c1 = 2.18mm as expected. Regarding the repulsive torque notice that it depends not only on the minimum distance but also on the direction of the tool's axis and the point of action of each repulsive force. Hence, as it can be seen in Fig 5 from 50 − 55sec, the torque remains at low levels although the repulsive force increases owing to the decrease of the minimum distance.

Conclusion
In this paper, details are provided on a control scheme that guarantees that the tool body will never violate the forbidden area. The proof of the passivity and constraint non-violation of the control system is given. Experiments are performed with a 7-dof KUKA LWR4+ manipulator used as master device and a virtual scene consisted of an identical virtual manipulator as a slave arm and point clouds of a kidney and its surrounding vessels characterized as forbidden regions. The results demonstrate how the repulsive force and torque produced by the control action and exerted at the wrist of the master manipulator cannot be practically overcome thus keeping the vessels safe from penetration. Our future work aims at extending the methodology for the case of articulated tools. where where J Q is the mapping between the angular velocity of the frame {W } and the unit quaternion rates. Notice that Ω is the union of all wrist configurations in which the instrument does not touch the spheres S(p i , d c1 ).
Theorem 1. For system (15) under the exertion of the external human force F h the forbidden area is not violated and the system is strictly output passive with respect to the wrist velocity v.
Proof. Let the initial state be such that u c = 0 . Utilizing the following candidate Lyapunov-like function: its time derivative, substituting Λ xv from (11) and utilizing the skew-symmetry of matrix Λ x − 2C x , yileds: The time derivative of the nearest point, using the time derivative of (1) for σ = σ * , given by: T the wrist's angular velocity expressed in the inertia frame and (ˆ) operator denoting the skew symmetric matrix of a vector. Substitutingṗ * i in (17) yields : Utilizing (9) in (8) yields: Using the distributive property of the dot product and the property a(b × c) = (a × b)c the above equation becomes: For the first case of (3) utilizing (1) for σ = σ * i yields that n T (p * i − p i ) = 0 and for the other two casesσ * i = 0. Therefore: As f i is in the direction of p * i − p i and using the above property in (20) the time derivative of V becomes:V Hence, system (15) is strictly output passive (see Definition 6.3 in [9]). Rewriting (21) by completing the squares, yields: Notice that F h represents the force applied by the human to guide the robot. Thus, F h is bounded and therefore F T h D −1 d F h is also a bounded function of time. Additionally the human forces have bounded energy so by integrating equation (22) we get: Thus, states ξ and v are bounded under the exertion of human force or otherwise there exist compact sets Ω ξ , Ω v such that ξ ∈ Ω ξ ⊂ R and v ∈ Ω v ⊂ R 6 . As a consequence, there exists a positive constant ε i such that V i ≤ ε i , which for the logarithmic function defined in (4) yields: 0 ≤ ψ i ≤ 1 − 1 e √ 2εi < 1, i = 1, ..., M , and this means that p * i − p i > d c i.e. the instrument will never touch the sensitive area.
In the case that all p i in O s are such as d(p i , p * i ) ≥ d c + d 0 i.e. u c = 0 the Lyapunov function take the form V = 1 2 v T Λ x v and the time derivativė Thus, the system is strictly output passive w.r.t v. Notice that when F h = 0, v converges exponentially to zero. Hence, using Lemma 1 of the [5], we can conclude that x converges to a constant.