Dynamic Active Constraints for Surgical Robots using Vector Field Inequalities
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.
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 . 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. . More recent papers published after that survey presented the use of guidance virtual fixtures to assist in knot tying in robotic laparoscopy , 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 .
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.  studied the proxy method with moving forbidden zones, Ryden and Chizeck  proposed virtual fixtures for dynamic restricted zones using point clouds, and Ren et al.  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. , 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  and guidance virtual fixtures , 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 , 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. , 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. , 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.  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.  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  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  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.  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. , 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 . 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  and . 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 .
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
|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|
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 . 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 . 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 . 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 . 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 . The set equipped with the multiplication operation forms the group , which double covers .
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 .
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 . Similarly, given the end effector position and the end effector orientation , such that , we have
where are also calculated from using dual quaternion algebra . 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 
is the set of minimizers222In optimization literature (e.g. ), 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
in which and . Problem 10 is the optimization of a convex quadratic function, since over the polyhedron . 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
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
, , 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  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
The vector field inequality for dynamic elements requires:
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);
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  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
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 
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 . 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 .
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.
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.
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 . Constraint 16 then becomes , and any reasoning is otherwise unaltered.
Iii-B Keeping the robot entity inside a safe region
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.
|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)|
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.
Iv-B The dual quaternion line and line Jacobian
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 .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
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 
For notational convenience we define whose derivative is
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
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 777Given a function , where , it is possible to show that . For more details, see .
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 
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