Dynamic Active Constraints for Surgical Robots using Vector Field Inequalities

Dynamic Active Constraints for Surgical Robots using Vector Field Inequalities

Murilo M. Marinho, , Bruno V. Adorno, , Kanako Harada, , and Mamoru Mitsuishi, This research was funded by the ImPACT Program of the Council for Science, Technology and Innovation (Cabinet Office, Government of Japan). Murilo M. Marinho, Kanako Harada, and Mamoru Mitsuishi are with the Department of Mechanical Engineering, the University of Tokyo, Tokyo, Japan. Email:{murilo, kanako, mamoru}@nml.t.u-tokyo.ac.jp. Murilo M. Marinho has been supported by the Japanese Ministry of Education, Culture, Sports, Science, and Technology (MEXT).Bruno V. Adorno is with the Department of Electrical Engineering and with the Graduate Program in Electrical Engineering - Federal University of Minas Gerais, Belo Horizonte-MG, Brazil. Email: adorno@ufmg.br. Bruno V. Adorno has been supported by the Brazilian agencies CAPES, CNPq, FAPEMIG, and by the project INCT (National Institute of Science and Technology) under the grant CNPq (Brazilian National Research Council) 465755/2014-3, FAPESP (São Paulo Research Foundation) 2014/50851-0.

Robotic assistance allows surgeons to perform dexterous and tremor-free procedures, but robotic aid is still underrepresented in procedures with constrained workspaces, such as deep brain neurosurgery and endonasal surgery. In those, surgeons have restricted vision to areas near the surgical tool tips, which increases the risk of unexpected collisions between the shafts of the instruments and their surroundings, in special when those parts are outside the surgical field-of-view. Dynamic active constraints can be used to prevent collisions between moving instruments and prevent damage to static or moving tissues. In this work, our vector field inequality method is extended to provide dynamic active constraints to any number of robots and moving objects sharing the same workspace. The method is evaluated in experiments and simulations in which the robot tools have to avoid collisions, autonomously and in real-time, with a constrained endonasal surgical environment and between each other. Results show that both manipulator-manipulator and manipulator-boundary collisions can be effectively prevented using the vector field inequalities.

virtual fixtures; collision avoidance; dual quaternions; optimization-based control

I Introduction

Microsurgery requires surgeons to operate with submillimeter precision while handling long thin tools and viewing the workspace through an endoscope or microscope. For instance, in endonasal and deep neurosurgery, surgeons use a pair of 100-300mm long 3-4mm diameter instruments. Furthemore, whereas in endonasal surgery images are obtained with a 4mm diameter endoscope, deep neurosurgery requires a microscope. The workspace in both cases can be approximated by a 80-110mm long truncated cone with 20-30mm diameter [1, 2].

In hands-on microsurgery, the surgeon is fully aware of the tools’ positions with respect to the workspace and is able to feel forces to prevent damage to structures. However, constant collisions between surrounding tissues and other instruments reduce surgeon dexterity and increases the difficulty of the task. As vision is often limited to a region near the surgical tool tips, it is difficult for the surgeons to compensate for restrictions induced by unseen collision points. Moreover, the disturbance caused by hand tremor may be larger than the structures being treated, which is problematic as accuracy is paramount in those surgical procedures. In this context, robots are used as an assistive equipment to increase accuracy and safety, and to reduce hand tremor and mental load [3, 4, 5, 6, 1, 2].

To increase accuracy and attenuate hand tremor, most surgical robots are commanded in task space coordinates either through teleoperation [4, 5, 3, 6, 1] or comanipulation [7, 8]. Motion commands go through a scaling and/or filtering stage, whose output is used to generate joint space control inputs by using a kinematic control law based on the robot differential kinematics [9]. Kinematic control laws are valid when low accelerations are imposed in the joint space, and are ubiquitous in control algorithms designed for surgical robotics [10, 11] since low accelerations are expected in those scenarios. In microsurgery, slow and low amplitude motions are the rule.

Together with such kinematic control laws, active constraints (virtual fixtures) can be used in order to provide an additional layer of safety to keep the surgical tool from entering a restricted region, or from leaving a safe region, even if the robot is commanded otherwise [11, 10]. A thorough survey on active constraints was done by Bowyer et al. [12]. More recent papers published after that survey presented the use of guidance virtual fixtures to assist in knot tying in robotic laparoscopy [13], and used virtual fixtures to allow surgeons to feel the projected force feedback from the distal end of a tool to the proximal end of a tool in a comanipulation context [14].

In real applications, some of the objects in the surgical workspace are dynamic. Usually those elements are other tools sharing the same restricted workspace or moving organs. Most of the literature in dynamic active constraints regards the latter and, more specifically, the development of techniques to avoid collisions between tool tip and the beating heart [15, 16, 17, 18]. For instance, Gibo et al. [15] studied the proxy method with moving forbidden zones, Ryden and Chizeck [18] proposed virtual fixtures for dynamic restricted zones using point clouds, and Ren et al. [19] proposed dynamic active constraints using medical images to build potential fields, all aiming to reduce contact force between tool tip and the beating heart surface. A work that considered the entire robot instead of just the tool tip was proposed by Kwok et al. [20], who developed a control framework for a snake robot based on an optimization problem. That work used dynamic active constraints in the context of beating heart surgery [16] and guidance virtual fixtures [21], both of which were specialized for snake-robots.

I-a Enforcement of constraints

Literature on static and moving virtual fixtures is generally focused on interactions between the tool tip and the environment. In those cases, generating force feedback in the master side is quite straightforward and may be enough to avoid damaging healthy tissue. In microsurgery, however, interactions also occur far from the tool tips. Indeed, our long-standing cooperation with medical doctors [4, 5, 6, 1, 2] indicates that, as the workspace is highly constrained, surgical tools may suffer collisions along their structures, whose effects in the tool tip can be complex; therefore, effectively projecting those collisions to the master interface for effective force feedback is challenging.

Other forms of feedback, such as visual cues [6, 22], that warn the user about proximity to restricted regions can be ineffective when using redundant robotic systems [1], since the mapping between the master interface and the robot configuration is not one to one. In such cases, moving the tool tip in a given direction may correspond to an infinite number of configurations due to redundancy, which can be counterintuitive for the surgeon as the same tool tip trajectory can correspond to collision-free motions or motions with collision, depending on the robot configuration. In fact, it is safer and simpler if the robot avoids restricted zones—in special those outside the field-of-view—autonomously. This way, the increasing prevalence of redundant robots is turned into an advantage, if used together with a proper control framework as the one proposed in this work.

I-B Related work

To the best of the authors’ knowledge, the framework in the medical robotics literature that more closely meets microsurgical requirements is the one initially developed by Funda et al. [23], which uses quadratic programming. That framework does not require a specific robot geometry, it can handle points in the robot besides the tool tip, and also considers equality and inequality constraints, which gives hard distance limits between any point in the manipulator and objects in the environment. That framework was extended by Kapoor et al. [10], who developed a library of virtual fixtures using five task primitives that can be combined into customized active constraints using nonlinear constraints or linear approximations. Li et al. [11] used the extended framework to aid the operators in moving, without collision, a single surgical tool in a highly constrained space in sinus surgery; however, due to how the obstacles were modeled using a covariance tree, which requires a long computational time, dynamic virtual fixtures cannot be considered [19, 20].

In prior approaches [11, 10, 23, 15, 16, 21, 20, 17, 18], obstacle constraints are activated whenever the robot reaches the boundary of a restricted zone, which might result in acceleration peaks. Some authors attempted to address this issue. For instance, Xia et al. [24] reduced the proportional gain in an admittance control law proportionally to the distance between the tool tip and the nearest obstacle. This allowed the system to smoothly avoid collisions, but also impeded motion tangential to the obstacle boundary. Prada and Payandeh [25] proposed adding a time-varying gain to smooth the attraction force of attractive virtual fixtures.

Outside of active constraints/virtual fixtures literature, Faverjon and Tournassoud [26] proposed a path planner based on velocity dampers, which are an alternative to potential fields, to allow the generation of restricted zones without affecting tangential motion. His technique was evaluated in simulations of a manipulator navigating through a nuclear reactor. To reduce computational costs, his method used the approximate distance between convex solids obtained through iterative optimization. Kanoun et al. [27] extended velocity dampers to a task-priority framework. Their technique was validated in a simulated humanoid robot.

Finding the exact distance and distance derivative between different relevant primitives can increase computational speed and accuracy. This is of high importance in microsurgical applications, as properly using the constrained workspace might require the tool shafts to get arbitrarily close to each other without collision, in addition to the fact that all calculations must be performed in real-time.

I-C Statement of contributions

In our prior work111Preprint available at https://arxiv.org/abs/1804.03883. [28], the concept of vector field inequalities applied for active constraints was first introduced. Primitives were found involving the distance between static elements and one dynamic element kinematically coupled to the robot. A proof-of-concept study was done in a simulation of deep neurosurgery, in which a pair of robots shared the same constrained workspace and only one of the robots moved. Therefore, the static robot was considered as an obstacle for the moving one.

In this work, we further extend the research done in [28]. First, we extend the framework to take into consideration dynamic active constraints, which may include any number of robotic systems sharing the same workspace as well as dynamic objects whose velocity can be tracked or estimated. The distance and its derivative between elements is found algebraically, allowing accurate and fast computation of the constraints, which is paramount for real-time evaluation in microsurgical settings. In addition, we introduce more relevant primitives that are coupled to the robots in order to enrich the description of constraints. Second, we evaluate the dynamic active constraints framework in in-depth simulation studies, which demonstrate the advantages of dynamic active constraints over static ones. Third, the framework is implemented in a physical system composed of a pair of robotic manipulators. Experiments are performed, including an autonomous tool tip tracing experiment using a realistic endonasal surgical setup, with an anatomically correct 3D printed head model, in which active constraints are used to avoid collisions.

In contrast with prior techniques in the medical robotics literature [11, 10, 23, 15, 16, 21, 20, 17, 18], our technique provides smooth velocities at the boundary of restricted zones. Moreover, it does not disturb or prevent tangential velocities, differently from [24] and [25]. When considering specifically the literature on dynamic active constraints, our technique considers an arbitrary number of simultaneous frames in the robot, not only the tool tip as in [15, 16, 17, 18]; furthermore, it is applicable to general robot geometry, in contrast to solutions for snake robots [16, 21, 20]. Outside of the medical robotics applications, our framework is a generalization of velocity dampers [26].

I-D Organization of the paper

Section II introduces the required mathematical background; more specifically, it presents a review of dual quaternion algebra, differential kinematics, and quadratic programming. Section III explains the proposed vector field inequalities and Section IV introduces primitives used to model the relation between points, lines, planes, and to enforce the task’s geometrical constraints. Section V presents simulations to evaluate different combinations of dynamic and static active constraints, in addition to experiments in a real platform to evaluate the exponential behavior of the velocity constraint towards a static plane, as well as an experiment in a realistic environment for endonasal surgery. Finally, Section VI concludes the paper and presents suggestions for future works.

Ii Mathematical Background

Notation Meaning
Set of quaternions and pure quaternions (the latter is isomorphic to under addition)
Set of dual quaternions and pure dual quaternions (the latter is isomorphic to under addition)
Set of unit quaternions and unit dual quaternions
position of a point in space with known first order kinematics
line in space with known first order kinematics
plane in space with known first order kinematics
vector of joints’ configurations
Jacobian relating joint velocities and an degrees-of-freedom task
position of a point in the robot
orientation of a frame in the robot
pose of a frame in the robot
line passing through the -axis of a frame in the robot
distance function between geometric entities and
squared distance between geometric entities and
task-space proportional gain
, gains for the distance or squared distance constraints
TABLE I: Mathematical notation.

The proposed virtual fixtures framework makes extensive use of dual quaternion algebra thanks to its several advantages over other representations. For instance, unit dual quaternions do not have representational singularities and are more compact and computationally efficient than homogeneous transformation matrices [29]. Their strong algebraic properties allow different robots to be systematically modeled [30, 29, 31, 32]. Furthermore, dual quaternions can be used to represent rigid motions, twists, wrenches and several geometrical primitives—e.g., Plücker lines, planes, cylinders—in a very straightforward way [33]. The next subsection introduces the basic definitions of quaternions and dual quaternions; more information can be found in [30, 33, 34].

Ii-a Quaternions and Dual Quaternions

Quaternions can be regarded as an extension of complex numbers. The quaternion set is

in which the imaginary units , , and have the following properties: . The dual quaternion set is

where is the dual (or Clifford) unit [34]. Addition and multiplication are defined for dual quaternions analogously to complex numbers, hence we just need to respect the properties of the imaginary and dual units.

Given such that , we define the operators


The conjugate of is , and its norm is given by .

The set has a bijective relation with . This way, the quaternion represents the point . The set of quaternions with unit norm is , and can always be written as , where is the rotation angle around the rotation axis [33]. Elements of the set are called unit dual quaternions and represent tridimensional poses (i.e., combined position and orientation) of rigid bodies. Given , it can always be written as , where and represent the orientation and position, respectively [34]. The set equipped with the multiplication operation forms the group , which double covers .

Elements of the set are called pure dual quaternions. Given , the inner product and the cross product are respectively [30, 33]


The operator maps quaternions into and maps dual quaternions into . For instance, and .

Given , the Hamilton operators are matrices that satisfy . Analogously, given , the Hamilton operators satisfy [33].

From (1), one can find by direct calculation that the inner product of any two pure quaternions and is a real number given by


Furthermore, the cross product between and is mapped into as


When it exists, the time-derivative of the squared norm of a time-varying quaternion is given by


Ii-B Differential kinematics

Differential kinematics is the relation between task space velocities and joint space velocities, in the general form , in which is the vector of manipulator joints’ configurations, is the vector of task-space variables, and is a Jacobian matrix.

For instance, given the robot end-effector pose , the differential kinematics is given by


where is the dual quaternion analytical Jacobian, which can be found using dual quaternion algebra [29]. Similarly, given the end effector position and the end effector orientation , such that , we have


where are also calculated from using dual quaternion algebra [35]. In general applications, , , and are not restricted to the end effector, and may refer to any point kinematically coupled to the robot.

In this paper, points, lines, and planes kinematically coupled to the robot are called robot entities.

Ii-C Quadratic programming for differential inverse kinematics

In closed-loop differential inverse kinematics, we first define a task space target, , and a task error . Considering , , and a gain , the analytical solution to the convex optimization problem [36]


is the set of minimizers222In optimization literature (e.g. [36]), a minimizer is usually denoted with a superscript asterisk, such as . However, we use the superscript , as in , to avoid notational confusions with the dual-quaternion conjugate.

in which is the Moore-Penrose inverse of , is an identity matrix of proper size, and is an arbitrary vector. In special, the analytical solution is the solution to the following optimization problem


Adding linear constraints to Problem 8 turns it into a linearly constrained quadratic programming problem requiring a numerical solver [37]. The standard form of Problem 8 with linear constraints is

subject to

in which and . Problem 10 is the optimization of a convex quadratic function, since over the polyhedron [36]. Furthermore, Problem 10 does not limit the joint velocity norm, hence it may generate unfeasible velocities. To avoid a nested optimization such as in Problem 9—which requires a higher computational time—, we resort to adding a damping factor to Problem 10 to find

subject to

which limits joint velocity norm.

More than one robot can be controlled simultaneously with Problem 11 in a centralized architecture, which is suitable for most applications in surgical robotics. Suppose that robots should follow their own independent task space trajectories. For , let each robot have joints, joint velocity vector , task Jacobian , and task error . Problem 10 becomes

subject to

in which

, , and is a matrix of zeros with appropriate dimensions.

The vector field inequalities proposed in Section III use appropriate linear constraints to generate dynamic active constraints. In addition, by solving Problem 12, we locally ensure the smallest trajectory tracking error for a collision-free path.

In this work we assume that the desired trajectory, , is generated online by the surgeon, may it be through teleoperation or co-manipulation. In this case, trajectory tracking controllers that require future knowledge of the trajectory [38] cannot be used, and a set-point regulator given by the solution of Problem 12 is a proper choice.

Iii Vector field inequality for dynamic entities

Fig. 1: The proposed vector field inequalities can have two types of behavior, in which the black circle represents a robot entity: keeping a robot entity inside a safe zone, (left); or outside of a restricted zone, (right). Restricted zones are checked for contrast with safe zones.

The vector field inequality for dynamic elements requires:

  1. A function that encodes the (signed) distance between the two collidable entities. The robot entity is kinematically coupled to the robot, and the other entity, called restricted zone, is part of the workspace (or part of another robot);

  2. A Jacobian relating the time derivative of the distance function and the joints’ velocities in the general form


    in which the residual contains the distance dynamics unrelated to the joints’ velocities.

We assume that the residual is known but it cannot be controlled. For instance, the residual might encode the movement of a geometrical entity, which can be related to the workspace, that can be tracked or estimated.333The vector field inequalities proposed in our earlier work [28] is a special case of the framework proposed in this work with .

Using distance functions and distance Jacobians, complex dynamic restricted zones can be generated, either by maintaining the distance above a desired level or by keeping the distance below a certain level, as illustrated in Fig. 1.

Iii-a Keeping the robot entity outside a restricted region

Fig. 2: A vector field inequality that keeps a point outside of the restricted zone, , whose boundary is a plane. To each point in space is assigned a maximum approach velocity (the lower vector in each vector pair), and a maximum separation velocity (the upper vector in each vector pair). The approach velocity decreases exponentially with the distance, and the maximum separating velocity is unconstrained.

To keep the robot entity outside of a restricted zone, we define a minimum safe distance , which delineates the time-dependent boundary of the restricted zone, and a signed distance


The restricted zone and the safe zone are

The signed distance dynamics is given by


A positive means that the robot entity and the restricted zone are moving away from each other, whereas a negative means that they are moving closer to each other.

Given , the signed distance dynamics is constrained by [26]


Constraint 16 assigns to each point in space a velocity constraint for the robot entity—as illustrated in Fig. 2—, which has at least exponential behavior according to the Gronwall’s lemma [27]. To understand the physical meaning of the constraint, first suppose that , which means that the robot entity is in the safe zone when . In this situation, any increase in distance is always allowed, which implies . However, when the distance decreases, and the maximum distance decrease rate is exponential, given by . This way, as the robot entity comes closer to the restricted zone, the allowed approach velocity between the restricted zone and the system gets smaller. Any slower approaching motion is also allowed, hence . As soon as , the restriction becomes and therefore the robot entity will not enter the restricted zone.

Now consider that ; that is, the system starts inside the restricted zone. In this case, Constraint 16 will only be fulfilled if , which means that the system will, at least, be pushed towards the safe zone with minimum distance decrease rate given by .

Using (13) and (15), Constraint 16 is written explicitly in terms of joint velocities as


where the residual takes into account the effects, on the distance, of a moving obstacle with residual and of a time-varying safe-zone boundary . If , the restricted zone contributes444The motion of the restricted zone takes into account the actual obstacle motion and the motion of the safe boundary (i.e., when ). to an increase in the distance between itself and the robot entity. Conversely, if , then the restricted zone contributes to a decrease in the distance between itself and the robot entity. If , then the robot has to actively move away from the restricted zone.

To fit into Problem 12, we rewrite Constraint 17 as


Notice that any number of constraints in the form of Constraint 18 can be found for different interactions between robot entities and restricted zones in the robot workspace. Moreover, by describing the interaction as a distance function, complex interactions will be only one degree-of-freedom constraints.

Remark 1.

When the distance is obtained using the Euclidean norm, its derivative is singular when the norm is zero. The squared distance is useful to avoid such singularities, as shown in Sections IV-C and IV-E. When the squared distance is used, the signed distance (14) is redefined as in which [35]. Constraint 16 then becomes , and any reasoning is otherwise unaltered.

Iii-B Keeping the robot entity inside a safe region

Using the same methodology of Subsection III-A, we redefine to maintain the robot inside a safe region; that is,

with final solution, assuming the desired signed distance dynamics (16), given by


Iv Squared distance functions and corresponding Jacobians

In order to use the vector field inequalities (18) and (19), we define (squared) distance functions for relevant geometrical primitives (point, line, and plane) and then we find the corresponding Jacobians and residuals. These geometrical primitives can be easily combined to obtain other primitives. For instance, a point combined with a positive scalar yields a sphere, whereas a line combined with a positive scalar yields an infinite cylinder; the intersection between an infinite cylinder and two parallel planes results in a finite cylinder; polyhedra can be defined as intersections of planes. Table II summarizes the distance functions and corresponding Jacobians.

Primitive Distance function Jacobian Residual
Point-to-Point (Eq. 21) (Eq. 22) (Eq. 22)
Point-to-Line (Eq. 29) (Eq. 32) (Eq. 32)
Line-to-Point (Eq. 33) (Eq. 34) (Eq. 34)
Line-to-Line (Eq. 50) (Eq. 48) (Eq. 49)
Plane-to-Point (Eq. 54) (Eq. 56) (Eq. 55)
Point-to-Plane (Eq. 57) (Eq. 59) (Eq. 58)
TABLE II: Summary of primitives

Iv-a Manipulator point-to-point squared distance and Jacobian

The Euclidean distance between two points is given by555The quaternion norm is equivalent to the Euclidean norm.


Since the time derivative of (20) is singular at [35], we use the squared distance, whose time derivative is defined everywhere:


Given a point in the robot, where is the time-varying joints’ configuration, and an arbitrary point in space, we use (4) and (6) to find


Iv-B The dual quaternion line and line Jacobian

Fig. 3: Representation of a Plücker line , where is the line direction, is the line moment, and is an arbitrary point in the line.

In minimally invasive surgery and microsurgery, tools have long and thin shafts, which may have some parts outside of the field-of-view; therefore, collisions with other tools or the environment might happen. By describing tool-shafts using Plücker lines, such interactions can be mathematically modeled. A Plücker line (see Fig. 3) belongs to the set , and thus is represented by a unit dual quaternion such as [33, 30]


where is a pure quaternion with unit norm that represents the line direction, and the line moment is given by , in which is an arbitrary point in the line.

When the robot forward kinematics is described using the Denavit-Hartenberg convention, the -axis of each frame passes through the corresponding joint actuation axis [9].666The method described here is general enough and different conventions can be used (for instance, the joint actuation axis could be the - or the -axis), such that the derivation of the corresponding equations follows the same procedure. Hence, it is specially useful to find the Jacobian relating joint velocities and the line that passes through the -axis of a reference frame in the robot.

Consider a Plücker line on top of the -axis of a frame of interest, whose pose is given by , in a robotic manipulator kinematic chain. Since the frame of interest changes according to the robot motion, the line changes accordingly, hence


where and . The derivative of (24) is


where , thus


in which and is given by (7); in addition, the derivative of the line moment is , hence we use (3), (6), and (26) to obtain


Finally, we rewrite (25) explicitly in term of joint-velocities using (26) and (27) as


Iv-C Manipulator point-to-line squared distance and Jacobian

Considering the robot joints’ configuration vector and a point in the robot, the squared distance between and a arbitrary line in space is given by [39]


For notational convenience we define whose derivative is


Using (2), (3), (4), and 30, we obtain


Letting , we use (6) to obtain the squared-distance time derivative explicitly in terms of the robot joints’ velocities:


Iv-D Manipulator line-to-point squared distance and Jacobian

Given the line , as in (24), on top of the -axis of a frame rigidly attached to the robot and an arbitrary point in space, we use (29) to obtain


Similarly to (32), we write the time derivative of 33 explicitly in terms of the joints’ velocities:


This primitive (i.e., the function together with its derivative ) is useful to define a compliant remote center-of-motion by having a nonzero safe distance.

Iv-E Manipulator line-to-line distance and Jacobian

The distance between two Plücker lines is obtained through inner and cross product operations. The inner product between and is [30]777Given a function , where , it is possible to show that . For more details, see [33].


where and are the distance and the angle between and , respectively. Moreover, given —the line perpendicular to both and —such that the cross product between and is [30]


The squared distance between and , when they are not parallel (i.e., ), is obtained using (35) and (36):


The squared distance between and when they are parallel (i.e., ) is obtained as


To find the distance Jacobian and residual between a line in the robot and an arbitrary line in space, we begin by finding the inner product Jacobian and residual in Subsection IV-E1 and the cross product Jacobian and residual in Subsection IV-E2. Those are used in Subsection IV-E3 to find the derivative of (37), and in Subsection IV-E4 to find the derivative of (38). Finally, those are unified in the final form of the distance Jacobian and residual in Subsection IV-E5.

Iv-E1 Inner product Jacobian,

The time derivative of the inner product between is given by

Hence, using (28) we obtain

which can be explicitly written in terms of the primary and dual parts as


Iv-E2 Cross product Jacobian,

The time derivative of the cross product between