Integrated planning and control of robotic surgical instruments for task autonomy

2.1. Automating basic motions

Pioneer works of robot-enabled instrument manipulation in surgical applications have targeted positioning of an instrument to registered poses, for example, the tissue cutter in prostatectomy (Kwoh et al., 1988) and the biopsy probe in neurosurgery (Davies et al., 1991). Combining clinical CT data, planning an optimal port placement and/or admissible path of the instrument becomes available to provide a safe referenced position for localized treatment (Schweikard et al., 1993; Adhami and Coste-Manière, 2003). Among these works, the concept of motion planning is limited to generating a deterministic path using an industrial robot arm for the instrument through admissible space without actual manipulation process or constraints. Funda et al. (1996) computed the optimal robot motion of for adjusting the gaze of the laparoscope subject to the remote center-of-motion constraint, which is widely adopted in MIS procedures. To include online feedback, Wei et al. (1997) applied visual servoing to adjust the robot-mounted laparoscope by tracking the instruments from the images. The method is also adopted in Krupa et al. (2003); Osa et al. (2010) to actuate a surgical instrument to a feature-defined target by minimizing image-based errors. However, the main disadvantage is that it requires continuous monitoring of visual features during manipulation, which could not robustly define a 6-DoF goal pose during the motion or provide re-planning strategy. To deal with multi-instrument environment, Preda et al. (2015) used sampling-based planning scheme (RRT-connected algorithm) for planning collision-free motion step for multiple instruments. Sozzi et al. (2019) proposed a DS-based approach with waypoint selection to actuate a non-wristed instrument with collision-free path to another instrument. The work in Marinho et al. (2019) proposed vector fields to guide the motion of a regular instrument with RCM constraint. Aiming for task-specific motion constraints, a few works adopt the method of learning from demonstration (LfD) to automate complex multi-arm coordinated trajectories like suture looping in knot tying or needle insertion (Osa et al., 2017; Schulman et al., 2013; Schwaner et al., 2021) to avoid modelling complex trajectories. However, it requires pre-recorded motion data for each type of task and is unable to deal with unexpected changes of task process. Notably, most above works used robot-mounted regular surgical instrument with only 4-DoF motions instead of a wristed robotic instrument. More recently, the work in Chiu et al. (2021) further introduced reinforcement learning (RL) policy to generate robotic instrument motions. To feed the online robot states to the system, continuous visual tracking of the instrument features is required via stereo images.

All previously mentioned works only focused on feasibility study completing a certain motion/task successfully, but failed to define generic safety features to be required by surgery automation to avoid execution-level uncertainties, which is of highest priority in surgeon’s perspective (Narazaki et al., 2006). There also lacks a unified solution to deploy those safety features across different tasks as well. In this work, we provide a DS-based framework with rigorous proof of global motion stability and trajectory reachability, such that it could handle stable motions to all task-relevant motions.

2.2. Automating contact motions

Interacting surgical instruments to the environment also constitutes a majority of motion steps in surgical tasks. In MIS, legitimate contact include non-invasive contact (e.g., palpation for tumor localization), grasping (e.g., toward a piece of tissue debris or surgical needle), and invasive contact (e.g., inserting a needle into tissue). To this end, Patil and Alterovtz (2010) addressed manipulation required for tissue retraction by selecting optimal grasping point, but didn’t specify the exact motion to safely grasp the tissue in advance. Since then, there are works addressing 6-DoF instrument-based manipulation of soft tissues for tissue dissection (Murali et al., 2015; Nagy et al., 2018) and deformation control (Li et al., 2020). The works in Kehoe et al. (2014); Hwang et al. (2020) automated target grasping in a peg transfer task, which is commonly used in surgical training for novice surgeons. Among them, the instrument targeting and grasping process were executed separately to avoid unsafe tool-target contact, and the grasping orientations highly resembled each other. Meanwhile, efforts have been made to needle grasping in wound suturing. Schulman et al. (2013) introduced LfD to automate manipulation and grasping of a needle with pre-recorded trajectories. The process had also been automated in terms of direct pick-up (D’Ettorre et al., 2018) and needle hand-off (Chiu et al., 2021; Pedram et al., 2020) using VS and LfD, respectively. A static needle was targeted and grasped using visual feedback as the motion input, which could reduce reliability when dealing with awkward grasping poses.

One mutual limitation of these works is that they all regarded grasping as separated steps with a fixed sequence rather than a reactive process, which could be re-planned under sensing/motion disturbances. The motion constraint during target contact should be embedded as well with the aforementioned safety features, but remains an unmet problem in surgery autonomy. We interleave manipulation and contact motion together as a GVM model, such that the robot will adaptively transit between tool-centric or target-centric actions with global stability.

2.3. Automating task-level procedures

Completing the pipeline of a surgical task based on step-level automation is the main goal to achieve task-level autonomy. Researchers have approached various individual surgical tasks with different sub-steps and different controllable DoFs required for the tasks. Works include autonomous field of view control of endoscope in Agustinos et al. (2014); Nageotte et al. (2006); Voros et al. (2006); Yang et al. (2019) that used VS technique to track a predefined visual feature to allow hands-free adjustment intraoperatively. In Kehoe et al. (2014) and Hwang et al. (2020), the authors targeted simulated surgical debridement with the instrument trajectories being performed using model predictive control and pose-to-pose interpolations, respectively. Given the targets’ positions, the instrument autonomously manipulated, grasped, and cleared the debris sequentially. Meanwhile, there are efforts targeting autonomy of tissue-based procedures, including non-invasive tasks like tissue palpation (McKinley et al., 2015; Nichols and Okamura, 2015) for tumor localization, and invasive tasks like tumor ablation (Hu et al., 2015), tissue dissection (Murali et al., 2015; Nagy et al., 2018), blood area detection, and suctioning from vessel rupture (Richter et al., 2021). Targeting more complex tasks like knot tying, Osa and Van Den Berg (Osa et al., 2010; Van Den Berg et al., 2010) automated single knot tying in suturing based on LfD.

Suturing is one of the most common but complicated tasks to be performed in MIS. It contains a long sequence of motion steps, involving dexterous manipulation of a needle and dual-arm coordination. Automating such tedious task could significantly reduce human workload, and thus has gained attention from researchers. For example, the works in Kapoor and Taylor (2008); Nageotte et al. (2005) have planned an optimal trajectory to automate the insertion process of a half-circle surgical needle into the tissue. A complete stitching automation process was given by Nageotte et al. (2009) including entry point planning, pose sensing, manipulation, and stitching of the suturing needle. However, these only constitute incomplete steps required by the suturing guideline. There are also studies addressing hand-off of the needle between two instruments using human–robot collaborative approach (Mikada et al., 2020; Watanabe et al., 2016, 2017), and also autonomous approach (Varier et al., 2020; Chiu et al., 2021) using RL-generated motions. Zhong et al. (Zhong et al., 2019) further addressed active tissue deformation during needle insertion to improve insertion accuracy. Lenard and Shademen (Leonard et al., 2014; Shademan et al., 2016) proposed a suturing device to inherently simplify the suturing task while ensuring a firm stitch. However, the needle size and stitch width could not be customized restricted by the tool. Meanwhile, only a few works have completed a throw of wound suturing. Sen et al. (Sen et al., 2016) provided an optimization-based approach to automate a four-throw suturing task, but does not support online trajectory re-planning due to tool-target motion disturbances with theoretical proof for motion stability. Pedram et al. (Pedram et al., 2020) completed an automatic single-throw suturing process using dual-arm coordination using a VS-based control framework. The instruments are visually tracked during the manipulation, which was reported to own accurate targeting accuracy. However, continuous visual sensing might hinder its applicability to deal with large orientation change. We are unaware of any existing works that provide systematic safety features for complex instrument manipulation in one or more surgical tasks.

3.1. Problem formulation

Performing a surgical task is essentially to manipulate a robotic instrument from pose to pose in SE(3) space across all motion steps. Under any (instant) robot configurations, a 6-DoF goal pose of the robot’s end-effector (i.e., the instrument’s distal tool) is assumed to be available. It could be either computed from online sensoring feedback or from user input. One common method is to add image-based markers on the display (e.g., labeling the tissue’s dissecting trajectory or the wound position for suturing), as a surgeon’s decision and supervision remains critical to reduce uncertainties of target recognition in supervised robotic surgery (Haidegger, 2019). Although computerized surgical image analysis has gained considerable attention to assist target localization (Loukas, 2018), we emphasize that our aim in this work is an autonomous framework to perform robot motions safely and reliably instead of comprehensive detection algorithms.

Due to the delicate nature of surgical tasks, we define safety for task autonomy to be decomposed into the following two parts: 1) There must be theoretical proof to globally guarantee that the robot motion is admissible in every (future) time instant, and the goal state can be smoothly and stably arrived. The term “global” indicates it should apply for any admissible initial/final robot states, and 2) the robot motions must be able to deal with obstacle avoidance (both tool-tool and tool-tissue), joint limit avoidance, etc. The first part is of primary concern which has not been systematically addressed. The second part consists of basic safety properties. We especially target “twisted” motions where the distal articulated joints experience large-range adjustment ( > 90 ° ) , and the robot state especially of SO(3) is significantly tuned ( ∼ 180 ° ) , which is normally used in surgery for flipping the grasping orientation of a suturing needle and/or tissue. Such motion might approach the edge of admissible robot workspace manifold, which is nonconvex due to small joint motion range of the two distal joints (±90°). To inherently maximize the performance of system response, the workspace constraints should be embedded into the control model rather than iterative approaches. The subproblems of instrument non-contact manipulation and contact motion control are processed under the same core in an integrated DS, and must be applicable to all motions appeared in one or more tasks.

3.2. Nomenclature

We regard the entire structure of the detachable tube-like instrument and its proximal motorized joints (totally 7 DoFs) as a surgical robot (or just “robot” if not further clarified). We call the distal structure of the instrument to achieve clamping, grasping, etc. as the “tool,” which is also the end-effector in robotics. To detail the workspace control problem, we use the robot’s 6-DoF end-effector pose (denoted by a 3D Cartesian coordinate frame F ) or its corresponding state X (computed from a newly proposed set of space parameters) to represent the robot’s present or goal configuration. The acronyms appeared in this article have been listed out in Table 1.

OPEN IN VIEWER

Table 1.

List of acronyms used in this article (in alphabetical order).

Acronym	Definition
DAS	Dynamic attraction state
DHc	Denavit–Hartenberg convention
DoF	Degree-of-freedom
DS	Dynamical system
GVM	Goal-varying manipulation
LfD	Learning from demonstration
MIS	Minimally invasive surgery
MoB	Mode of behavior
NSS	Nodal state space
RAMIS	Robot-assisted minimally invasive surgery
RCM	Remote center-of-motion
RL	Reinforcement learning
SMA	Sequential motion allocation
SMP	Surgical motion primitive
VS	Visual servoing

5.1. DS-guided instrument manipulation

5.1.1. End-effector control system

The DS is an effective mathematical approach to generate stable, convergent robot motions which are efficiently reactive to instant state variations and/or disturbances. This endows DS-guided manipulation with powerful adaptability in motion re-planning without the need to analyze the entire trajectory (compared to optimization-based approaches), which is superior to tackle robot manipulation once the motion constraints are embedded into the system model.

We aim to incorporate the instrument’s kinematic property using NSS analysis to the DS to enable globally guaranteed safe instrument manipulation planning. We start by giving the general form of a time-invariant nonlinear dynamical system as follows:

ζ . ( t ) = g ( ζ ( t ) , u ( t ) )

(11)

which indicates the evolution of the system’s states ζ (t) subject to the control input u, and g(⋅) denotes the differentiable vector-valued function. To apply such technique to robot’s motion generation, the robot states ζ (t) must be appropriately defined to enable its dynamics (via g(⋅)) to guide the robot with demanded performances as the system evolves over time. The states are usually defined as the 6-DoF end-effector pose or configuration space (Mirrazavi Salehian et al., 2016) and thus, g ( ⋅ ) : R 6 l ↦ R 6 . Here, unlike the above methods, we use the nodal vectors to encode the relationship between the robot’s present and goal configurations based on its body parts. Consider an arbitrarily defined F g ( O g , x g , y g , z g ) as the target pose to reach, we particularly introduce a new parametrization space H which is defined as follows:

η ( q ) = [ η 1 η 2 η 3 η 4 ] : = [ d * ( x s , x s g ) 〈 ξ 1 , x → g 〉 〈 ξ 2 , z → g 〉 〈 ξ 3 , y → g 〉 ] ⊆ H ⊆ R 4

(12)

where the parameters η₂, η₃, η₄ depict via inner product the orientation alignment of the end-effector from its target, x s g is the goal position of x _s computed from F g . The scalar d * ( ⋅ ) = x s ⊺ ( x s − x s g ) / ‖ x s − x s g ‖ 2 defines the signed distance of x _s from x s g via vector projection. Note that getting x s g requires computation of inverse kinematics which could not be analytically solved due to the coupled position and orientation. Here, we compute a “naive” but also closed-form x ∼ s g

x ∼ s g = x g − x t + x s

(13)

from the knowledge of distal joints (encoded by x _t (q) which are uncontrollable by x _s (q _s ). Note that in (13), x ∼ s g is derived based on the instant forward kinematics, which provides “coarse” guidance towards the end-effector’s goal position using only q _s . However, we will later show that such x ∼ s g does not affect the performance of the target DS and could converge to its genuine value subject to robot control. We name H as the nodal state space (NSS) which characterizes the robot’s goal-oriented spatial relationship using the skeleton features of the nodes. The robot’s current state X and goal state X g could now be reformulated by the components in (12) as follows

X : = [ x s ξ 1 ξ 2 ξ 3 ] ⊺ , X g : = [ x s g x → g y → g z → g ] ⊺ .

(14)

Now, we will use “state” in (14) to describe the robot’s status which is equivalent to a 3D Cartesian space, as we aim to solve automation via Cartesian workspace analysis. Several properties that NSS could contribute to robot motion guidance are highlighted.

Proposition 1

η (q) has a globally unique equilibrium η * with η * = 0.

Proof

Regarding η₁, any given (feasible) x s g will correspond to a globally unique solution of q s g , as the local kinematics x _s = f _s (q _s ) Cartesian workspace manifold being a solid sphere (From, 2013). This shows that f _s (⋅) is convex and non-singular for regulating η₁. Meanwhile, according to (12), there exists multiple solutions of ξ _i+1 to satisfy η _i = 0 (i = 2, 3, 4). However, we will prove in Proposition 2 that due to the joint limits of the instrument, only one ξ _i+1 is physically reachable subject to x s → x s d , which further corresponds to a globally unique q_i+3. Thus, there exists a unique q to meet η _i = 0 for i = 1, 2, 3 for any feasible X g interpreted by F g .

Proposition 2

Given a 3D unit vector v _r based on F g and a nodal vector ξ _i (q _s , q_i+3) of node n _i (i = 1, 2, 3), there exists a unique solution q i + 3 * for ξ _i such that v _r ^⊺ ξ _i (q_i+3) = 0 subject to q _s → 0.

Proof

According to (10), the derivation of the nodal vector ξ _i is simplified to the following:

ξ i = C i + 2 R i + 3 ( q i + 3 ) ξ i ¯ s . t . q s → 0

(15)

where the variation of the rotation matrix C ∈ R 3 × 3 is negligible due to the settled proximal robot parts. R_i+3 is the fundamental z-axis rotation matrix which acts as a periodical factor applied to ξ i ¯ and is solely regulated by q_i+3. Thus, ξ _i has a period of 2π subject to change of [q_i+3, q_i+3 + 2π], as its components own the following form:

ξ i , j = a 1 + a 2 cos ( q i + 3 ) + a 3 sin ( q i + 3 ) , j = 1,2,3

(16)

where a₁, a₂, and a₃ are constant scalars. Therefore, there are two possible solutions of q_i+3 for ξ _i = v _r ^⊺ ξ _i (q_i+3) = 0 within 2π. For surgical robots, the motion ranges of the two distal joints q _w and q _t are limited to [−π/2, π/2]. Thus, it is impossible to generate two toggled directions of ξ _i . There exists only one q_i+3 that satisfies v _r ^⊺ ξ _i = 0 within the robot’s feasible workspace (i.e., [q⁻_i+3, q⁺_i+3]) to yield η _i = 0 (shown in Figure 3(b)). That corresponds to the unique solution of ξ _i , denoted by ξ i * . Meanwhile, the direction of vector ξ _i periodically changes through [q_i+3, q_i+3 + 2π], which sweeps ξ _i through a circle whose center being exactly the position of n _i within a plane π ξ i (see Figure 3 for geometrical interpretation). ³

Now, we establish a dynamical system to guide the instrument end-effector to its target pose. The NSS H ⊆ R 4 are defined as the state variables, and then due to Proposition 1, there exists a globally unique equilibrium at origin, that is, η * = 0_4×1. u is of q . or q only. Thus, we propose the following controller:

u = − ( ∂ η ( q ) ∂ q ) − 1 Γ η ( q )

(17)

where M is the Γ ∈ R 6 × 6 ≻ 0 is the diagonal gain matrix applied to joint velocities. Then, by applying the control input u = q . and substituting (17) to η ˙ ( q ) leads to the following:

η ˙ = − Γ η

(18)

which clearly indicates an asymptotically stable DS (with sole negative real parts in phase plane analysis) whose the attractor is exactly η * = 0_4×1. Its evolution guides the instrument motions via q . ( t ) to reach the end-effector F e to any feasible F g . As η is also bounded due to (12), from (18), the DS that regulates η is globally asymptotically stable from Lyapunov stability theory as each equilibrium in η * is globally unique (refer to Proposition 2) (Slotine and Li, 1991). The system is also of C¹ continuity as (1) and (10) are all differentiable regarding both q . ( t ) and q(t).

OPEN IN VIEWER

Figure 3.

Geometric representation of the variability of Ξ subject to q_i+3 in a), and the solution uniqueness to achieve v _r ^⊺ ξ _i (q_i+3) = 0 when considering the physical joint motion of q_i+3 in 3D workspace in b). Respectively, q i + 3 − , q i + 3 0 , and q i + 3 + denotes the negative limit, zero position, and positive limit.

Remark 1

Deploying the controller (17) to robot’s target-reaching control requires interpretation of a F g to η . F g is normally provided as a 3D Cartesian coordinate frame for task-oriented robot manipulation. Then, according to (6) and (7), η _2,3,4 are all computable. Meanwhile, we use x ∼ s g as in (13) as approximation of x s g to avoid computing inverse kinematics, which is not differentiable for on-the-fly manipulation guidance.

5.1.2. Sequential motion allocation (SMA)

The current form of DS only addresses the convergent motion of the end-effector to a goal pose and does not necessarily guarantee the feasibility of the in-process trajectory. Surgical tasks commonly require grasping the needle and tissue from specific poses to minimize potential trauma. When transitioning between awkward poses, the robot might encounter joint limits and/or near-singular configurations. In terms of a serial robot manipulator, one robot joint could parallelly tune different η. Therefore, regulating a specific η _i might cause disturbances to other (distal) η j , j > i , j ∈ N + , which could decrease the overall control effectiveness and target reachability.

To solve the above issues, new constraints must be added to the system improve the instrument’s manipulation delicacy. Here, we introduce the sequential motion allocation (SMA) that progressively allocates the regulation output from proximal to distal robot joints. This will facilitate “rhythmic” movements of the robot’s body parts and avoid “clumsy” joint motions, which could be harmful to both the robot and the environment. We achieve SMA by proposing a new set of state-based parameters λ ∈ R as the nodal allocation coefficients to dynamically weight the control output deployed to η for each node based on (12) and (17). The definition of λ is chosen by considering the following properties to meet the task-relevant motion characteristics:

1. We set all coefficients to λ ∈ [ 0,1 ] ⊆ R , λ ∈ C 0 to normalize the scales of all η among different between types of robot joints and/or their different ranges of motion during regulation. λ = 1 and λ = 0 indicates the motion output that regulates the corresponding η has been fully deployed and suspended, respectively.

2. To achieve sequential actuation of the skeleton nodes, the λ _i that allocates the regulating motions to the current η _i should initiate posterior to the elevation of the previous (proximal) λ_i−1. This is interpreted as λ_i−1 ≥ λ _i ∀λ_i−1, λ _i ∈ [0, 1].

3. To incorporate SMA to whole-body coordination, λ _i needs to open a gap to the rising λ_i−1 ahead, but also needs to catch up with λ_i−1 as λ_i−1 → 1 to eventually enable full regulation. This indicates the boundary conditions of λ _i (x) to yield λ . i ( x ) | x = 0 = 0 ∀ i and λ . i ( x ) | x = 1 = 1 , ∀ i > 1 with x = λ_i−1.

All λ will constitute a space L whose dimension corresponds is equal to that of H , with λ = [ λ 1 , λ 2 , λ 3 , λ 4 ] ⊺ ∈ L ⊆ R 4 .

A feasible mathematical form of λ to achieve SMA could be given as follows:

λ i = { exp ( − k 1 ‖ η 1 ‖ ) , i = 2 exp ( − k i ( 1 / λ i − 1 − 1 ) ) , i > 2

(19)

where k _i is the preset gain for each λ _i . We set k _i = 1 ∀k > 1 to meet lim λ i − 1 → 0 λ . i ( λ i − 1 ) = 0 ∀ i . The definitions in (28) shows that to initiate SMA, λ₁ needs to be exclusively modelled apart from the others. Once η₁ → 0, the control output will be smoothly allocated to the next node with η₂ by elevating λ₂, and vice versa. The subsequent λ _i are tuned directly based on λ_i−1 to ensure a robust and flexible SMA process regardless of the robot’s initial/final state or its control input. ⁴

u ¯ = L u

(20)

with

L = [ 1 0 1 × 3 0 3 × 1 diag ( λ ) 3 × 3 ] ∈ R 4 × 4

(21)

Remark 2

In (28), λ₁ is only used as a reference to proceed the SMA under ||η₁|| → 0, and at any time instant, deviation of η₁ off 0 will suspend and restart the whole SMA process until η₁ converges again. This implies the instrument’s position (via η₁) is always regulated prior to the orientation (via η_2,3,4) due to the “weak coupling” effect of the instrument’s end-effector pose. This is a demanding motion constraint in instrument manipulation, as the position misalignment of the end-effector caused by η₁ becomes significantly larger than that by η_2,3,4, also thanks to the “weak coupling” effect (see Appendix A for mathematical explanations how “weak coupling” appears on a wristed robotic instrument).

One main advantage of introducing SMA to the DS-guided instrument manipulation is that the whole-body robot trajectory subject to (18) becomes always feasible. This is led by (28) which enforces the regulation of a specific η _i , i > 1 to be always accompanied by a settling η_i−1. This further transforms the state space representation (12) into:

η i = ( C i + 2 i + 2 R i + 3 ( q i + 3 ) ξ ¯ i − 1 ) ⊺ e i ⇐ λ i − 1 → 1

(22)

with i = 1, 2, 3, where C_3,4,5 are matrices whose variations are negligible due to λ_1,2,3 → 1, e 1 = x → g , e 2 = y → g , e 3 = z → g . Note that the regulation of each η is now dominated by only one robot joint. Then, based on the property revealed in Proposition 2 and the controller (17), the process of SMA is transformed to a sequence of globally stable control subsystems coordinated by L. As long as F g is reachable, H must be reachable and thus η = 0 could be reached by using (17). Examples of state evolution upon SMA and its performance when dealing with disturbances from η are demonstrated in Figure 4.

Another explicit advantage of SMA is that the motion control of the robot’s end-effector and body parts does not rely on computing inverse kinematics and thus allows a differentiable system, which could guarantee efficiency and smoothness when dealing with complicated motions. Recall the goal position of x _s is coarsely computed as x ∼ s g in (13). During the manipulation, movements of the distal joints (e.g., q_5,6) might change x _e and also x ∼ s g , which generates disturbance to η₁ and also the whole SMA process. However, this facilitates x ∼ s g → x s g as “self-propagating” goal pose reaching process among H , as η_2,3,4 are strictly bounded and globally converges to 0. This together with the “weak coupling” effect of the wrist kinematics leads x _t to a bounded disturbance which does not affect our control system.

OPEN IN VIEWER

Figure 4.

Evolution of λ subject to ||η||, where a) denotes an undisturbed evolution with exponentially descending ||η||; b) evolution upon slight increase of ||η|| during t ∈ [150, 200]; and c) evolution upon large increase of ||η|| during t ∈ [125, 175]. Note that the proximal λ stays greater than the distal one to enforce sequential coordination.

5.1.3. Collision avoidance

In this part, we briefly introduce our framework to deal with obstacle avoidance. In robotic surgery, obstacle avoidance mainly solves two scenarios: tool-tool collision when performing dual-arm surgical tasks within a shared workspace, and tool-tissue collision to prevent trauma to surrounding tissues. As the obstacles might be described as a single obstacle O or a set of obstacles O = { O 1 , O 2 , … , O n } from image-based measurements, without loss of generality, we start by considering a single obstacle identity O as a sphere (which could be easily extended to lines or surfaces) with 3D position x _o . From robot kinematics and eye-to-hand data, the closest point x _clo on the robot skeleton relative the obstacle, and its local Jacobian matrix J c l o ∈ R 3 × 3 can be computed. Note that the position of x _clo on the robot might be changing upon robot motions. Then, by giving a minimum tolerant distance r _o when avoiding collision (i.e., the radius of the sphere), the avoidance could be done by finding the tangent direction for the sphere O from x _clo There are infinite directions to satisfy such, but one could choose the closest one to x s g − x c l o to reduce trajectory length. Thus, an instant target position x_g,o for x _clo could be derived by letting

x g , c l o = x c l o + a e e g

(23)

where e g ∈ R 3 is a unit vector that denotes the selected avoiding direction and a _e is a scalar denoting the target motion speed. This indicates that we directly seek for obstacle avoidance by deriving the escaping velocity to be followed by the x _clo . This allows that once x _clo goes through x_g,clo, the tangent direction changes and then varies x_g,clo upon (23) such that the collision avoidance motion continues. Meanwhile, we monitor whether the obstacle still obstructs the robot to reach the target by computing the distance between x _obs and the line formed by x g s − x g , c l o . Finally, we decide whether the robot completes obstacle avoidance by the following

X g = { X o , f o ( X , X p , p o , r o ) ≤ 0 X p , f o ( X , X p , p o , r o ) > 0

(24)

where f _o (⋅) denotes the scalar function that compute the aforementioned point-to-line distance subtracted by r _o . X o is the end-effector state for collision avoidance constructed by x _g with unchanged orientation, as the local Jacobian only regulates the 3D position of x _clo . For practicality, to avoid abrupt change of the instant target state led by (24), one could apply linear fusion of two states to achieve smooth state transition.

5.2. Integrated planning and control

Apart from instrument manipulation, a majority of surgical instrument tasks (e.g., dissection and suturing) involve reaching and contact a specific target. The end-effector is normally manipulated to a proper candidate pose first (i.e., the tool-centric action) and then proceed to the final contact phase (i.e., the target-centric action) to could avoid unnecessary collisions or hazardous movements (Cao et al., 1996; Jun et al., 2012). Coordination between these two actions should be based on online awareness of the tool-target situation such that it could adaptively decide whether the robot is ready for contact or requires further adjustment. To this end, instead of regarding them as separated steps, we propose a unified strategy that integrates the tool-centric and target-centric actions to a single motion step to facilitate safer, smoother, and more efficient motion performance.

Recall the instrument’s goal configuration described by frame F g , It now also needs to by used to guide the end-effector to direct contact a target besides manipulation (mostly grasping an object). To this end, we extend F g to two poses, the pre-contact pose F p and a contact pose F c to be reached by the instrument’s end-effector. It should be noted that F c is usually computed via online sensoring feedback or assigned by a user, and F p is computed based on F c which should be reached in advance in order to proceed to the contact safely. Then, we define a known end-effector trajectory ρ(τ(t)) from F p to F c to be tracked by the end-effector to clear the final tool-target distance for finishing the contact, which also exactly defines the target-centric action. The parameter τ ( t ) = [ 0,1 ] ∈ R denotes the progress of the trajectory and is of class C 1 . Thus, the appearance of τ enables a dynamic F g and leads the differentiation of η , currently of X ( q ) and X g ( τ ) , to the following extended form:

η ˙ ( X ( q ) , X g ( τ ) ) = ∂ η ( ⋅ ) ∂ X ( q ) ∂ X ( q ) ∂ q q . + ∂ η ( ⋅ ) ∂ X g ( τ ) ( X c − X p ) τ ˙

(25)

u = ( ∂ X ( q ) ∂ q ) − 1 ( ∂ η ( ⋅ ) ∂ X ( q ) ) − 1 ( ∂ η ( ⋅ ) ∂ X g ( τ ) ( X p − X c ) τ ˙ − Γ η ( ⋅ ) )

(26)

lim t → ∞ X = X g = τ X c + ( 1 − τ ) X p

(27)

lim t → ∞ ‖ η ( ⋅ ) ‖ = 0 ⇒ X = { X c , lim t → ∞ τ = ∞ X p , lim t → ∞ τ = 0

(28)

τ ˙ ( η ) = γ ( 2 exp ( − κ ‖ η ‖ ) − 1 )

(29)

τ ˙ = { − γ , lim t → ∞ ‖ η ‖ = ∞ γ , lim t → ∞ ‖ η ‖ = 0

(30)

η ( T 1 ) < − 1 κ ln 1 2

(31)

∫ T T + T ′ τ ˙ d t > 1

(32)

τ ( t k + 1 ) = { 1 , τ ( t k ) + τ ˙ ( t k ) Δ t > 1 0 , τ ( t k ) + τ ˙ ( t k ) Δ t < 0

(33)

∂ τ ˙ ∂ η | η → ∞ → 0 , ∂ τ ˙ ∂ η | η = 0 = − 2 κ γ

(34)

During the adjustment of τ, we regard η as an external input whose regulation is solely subject to (26). Note that the introduction of GVM using τ on the basis of DS-guided manipulation in (25) further integrates the trajectory planning and the motion control of versatile robot manipulation steps to a unified solver. Selection of individual X g and their connected trajectory ρ will enable versatile motion characteristics, which could be either predefined or solved on-the-fly.

The above GVM framework could be used to automate typical instrument motions in robotic surgery. First, it could solve delicate motions required for target contact. We can simply define X p whose orientation is identical to that of F c but with a constant position difference d along the tool’s pointing direction, with

d > l w + l t

(35)

OPEN IN VIEWER

Figure 5.

The schematic diagram of our integrated planning and control framework using NSS η to represent and control the robot instead of end-effector pose. GVM serves as an adaptive planning policy that provides active guidance using the goal state X g via τ, which is reactive to obstacle avoidance.

We briefly emphasize the superiority of our architecture from a combined technical and practical perspective that contributes to surgical applications. The introduction of NSS to represent the robot's present-to-goal variation provides distinct modelling advantages. The global asymptotic stability of (18) and (25) guarantees that the robot can be stably maneuvered to any arbitrary feasible state with smooth positions and velocities. Incorporating SMA then provides versatile planning of the robot’s goal-reaching trajectory as well. They solve the issues of “the robot could reach a goal” and “the robot knows how to reach a goal” simultaneously using our framework via a mathematically determined solver without prior knowledge or online iterations. GVM then plays a role to solve “the robot knows when to act” which picks up the last components required for a motion-level architecture. Its capability to transfer robot motion through different types of motions is illustrated in Figure 6.OPEN IN VIEWER

For surgical applicability, GVM allows motion constraints applied to the in-process trajectory, which makes it powerful to solve delicate target contact motions, where a constrained path is required to contact tissue via specified poses to avoid unnecessary tissue trauma. Particularly, the tool-centric action guarantees avoidance of premature contact to the target, and the instrument will not approach the target (at X c ) before it reaches X p (s.t. η → 0). This could avoid entanglement of the tool to the target before the final phase of manipulation (i.e., X g → X c ) is settled. In addition, as the tuning of GVP τ is smooth and bounded, the robot actions can be transferred smoothly as well.

6.1. Surgical motion primitive (SMP)

To systematically entail these motion details, we propose the notion of surgical motion primitive (SMP) based on GVM as a generalized model to define surgery-related instrument motions. The SMP characterizes a single-instrument manipulation step into three progressive phases:

The current setting of our GVM can only achieve consecutive execution across Phase II and Phase III. Thus, we must modify GVM such that it is applicable to the whole SMP. In this regard, we extend the validity of τ to Phase I such that it must retract the end-effector from the target to a safe position prior to Phase II. We define the initial pose as X c 0 where contact still exists, and the retracted state as X p 0 , which is also the ending state for Phase I. Without loss of generality, we maintain the relationship between them as that of X c and X p , and their in-between trajectory ρ remains a line segment and should be performed rapid by the instrument to avoid unnecessary contact. The GVP τ is also set to 1 at X c 0 and 0 at X p 0 . Thus, during Phase I, we need to enforce τ ˙ = − c (with c being a positive scalar) to lead the instrument to X p 0 subject to τ : 1 → 0, which exactly reverses the process of Phase III. To smoothly connect Phase I to Phase II, we equip τ with the following properties

X g = { X p 0 , ϵ < τ ≤ 1 X p , 0 ≤ τ < ϵ

(36)

6.2. Modes of behavior (MoBs)

Finally, we utilize SMP to define and automate the entire pipeline of instrument motion sequences for different surgical tasks. Although we define three consecutive phases that form the single SMP, some instrument motion steps might only involve one or two of them. For example, an off-target manipulation can be fully performed by initiating only Phase II. Retracting the instrument from an object to a stand-by contact-free target could be done by using Phase I and Phase II. To make SMP customizable to such motion differences, we combine different motion phases and generate the following five typical modes with their application scenarios:

• Mode I (Phase II): The instrument motions is freely adjusted via DS + SMA with no additional workspace constraints and thus no GVM (τ = 0).

They are also defined as the MoBs, as they exhibit own motion constraints while sharing the identical control architecture, and are available to each SMP (see Figure 7 for conceptual illustration). To achieve task-level autonomy, we first utilize a chain of SMPs that govern each motion step in the surgical task to construct the whole pipeline. Then, we equip each SMP (or each motion step) with a specific MoB according to its prescribed functionality. As the guidelines for surgical tasks are already mature in clinical practice, the MoBs could be preset for each task such that it could successfully follow the procedure. In Figure 8, we demonstrate the applicability of our framework to characterize many different existing surgical tasks by selecting different SMPs and MoBs, based on clinically adopted surgical guidelines (Cao et al., 1996). The validity covers not only intra-corporeal MIS procedures (e.g., tissue dissection, wound suturing, and knot tying) but also other procedures like robotic palpation and biopsy, which is generalized framework compared to existing task-specific approaches. Although each elementary motion has different constraints, they could be set by five modes of behavior to form a pipeline. For example, the specification for dual-arm suturing could be easily interpreted as 3-5-0-0-0-4-1 for PSM 1 and 0-0-3-5-2-1-2 for PSM 2 (where 0 means staying idle in this step). This could all be preset in the framework as long as the task type to be automated is determined, which is very easy to specify. We will show later in our experiments that the ability of coordinating motion execution process adapting to the online robot-environment situation is critical to improve the success rate and reliability of task autonomy.OPEN IN VIEWER

OPEN IN VIEWER

Figure 8.

Demonstration of complete pipelines of various independent surgical tasks which could be fully formulated by our framework using the five MoBs for task automation.

7.1. Overview

We implement our algorithm framework on the Virtual Robot Experimentation Platform (V-REP), which is then interfacing to MATLAB R2017a (MathWorks Inc) via remote API on a Core i7 2.8 GHz system without GPU acceleration. In the simulation platform, we use the virtual model of da Vinci Research Kit (dVRK) provided by Fontanelli et al. (2018). The dVRK is a dual-arm surgical robot with two wristed robotic instruments, which are named as the Patient-Side Manipulators (PSMs). Both two attached instruments are Large Needle Drivers (LNDs) which is the typical selection in dVSS-enabled RAMIS to perform delicate intra-corporeal procedures like suturing. Virtual objects, either pegs or a surgical needle, involve pick-and-place operation, which is one common surgical motion type and is frequently used as a proxy for generalized surgical dexterity (Cao et al., 1996). Here, we will use them to define practical task-relevant movement steps for evaluation of instrument’s motions. The distance between the RCM of the PSMs to the peg transfer objects are selected to around 150 mm which resembles the genuine robot-target configuration during RAMIS. We do not apply dynamic motion properties and interactions throughout the simulations, as we mainly investigate the motion behavior of the instruments in 3D workspace. The detailed parameter setup of our system is shown in Table 2.

OPEN IN VIEWER

Table 2.

Parameter setting of our algorithm in simulations.

Parameter	Value
Γ	[ 0.1 0 0 0 2 0 0 0 2 ]
l h	[ − 1 0 0 ] ⊺
l v	[ 0 1 0 ] ⊺
M	[ 0 0 1 ] ⊺
k 1	× (random)
k 2	0.2
k 3	1
k 4	1
Γ	0.05
Κ	0.5
ϵ	0.01

7.2. Scene I: Complex tool manipulation

We first simulate a peg-transfer setup where a plastic ring is to be manipulated by the robot from one peg to another, which is commonly used in surgical skill training for novice surgeons. The initial/final state of the pegs is usually selected that corresponds to awkward robot configurations (i.e., either near-singularity or near-limit joint states). The robot trajectory needs to experience large-range orientation adjustment in order to align the ring to the peg to finish the manipulation properly, which might be hard to plan within confined space. To verify the validity of our framework to tackle such extreme scenarios, we assign three typical cases, as shown in Figure 9:

• Case I: Transfer the ring from a horizontal peg to an adjacent vertical peg.

OPEN IN VIEWER

Figure 9.

The three cases of pose-to-pose manipulation for simulated peg transfer tasks where the instrument needs to drastically twist the wrist. The end-effector trajectories are visualized from green to red along the evolution of time.

The target states of the robot are known and remain static at this stage of simulation. The process is actuated using Mode III for our proposed SMP. Particularly, the differences between initial and final positions of the distal joints q w 1 , q w 2 , and q _t in three cases are [1.16, 1.487, 2.1] ^⊺ rad, [1.672, 2.294, 1.296] ^⊺ rad, and [−2.853, 0.732, 2.2002] ^⊺ rad, respectively, which indicate an average of > 100 ° motion for each distal joint in each case. The results show that the DS-based framework could simultaneous plan and control the instrument’s motion through twisted motions and reach the target configuration in all cases. The guaranty of global unique solution in Proposition 2 in our new robot model, the instrument smoothly reaches the target pose without being trapped in false equilibriums, which might appear in optimization-based approaches due to the nonconvex workspace manifold. Meanwhile, the trajectory remains feasible despite the target joint positions of specific joints being adjacent to its limits. We compare the resultant joint actions with and without deploying the SMA (as shown in Figure 10. Without SMA, the distal joint motions tend to overshoot from its final positions before convergence. This might result in exceeding joint limits ((e.g., q _t in Case I and II, and q w 2 in Case III, note that the joint limit of q w 2 and q _t are both ± π/2 rad, illustrated by gray shaded areas). The convergent performance is also not affected by the sequential actions.OPEN IN VIEWER

Then, we verify the capability of GVM to inherently avoid unnecessary contact to the target during manipulation. The main symbol of inadvertent tool-target collision is the time instant(s) that the end-effector position goes behind the target contact point, which is mathematically interpreted by the following metric:

ϕ : = 〈 g x t , z → g 〉 < 0

(37)

Guiding the end-effector through § p and § c by implementing GVM avoids ϕ < 0 throughout three cases (refer to the dark green curves in Figure 11). Without GVM, the end-effector will intrude the target area (shown in Figure 11 the red curve segments) during manipulation which could damage the tissue in real surgical procedures. GVM allows the twisted motions to be properly adjusted first safely away from the target before reaching it. Note that such performance is not particular to the selected cases, but could be easily guaranteed upon proper setting of X p according to X c in (27) could guarantee Note also from Figure 11 that a reactive X g does not affect the convergent performance of instrument manipulation.OPEN IN VIEWER

7.3. Scene II: Dual-arm collision avoidance

Then, we investigate the applicability of our algorithm to deal with collision avoidance. Aiming for a common but challenging scenario in robotic surgery, we consider a tool-tool collision situation where two robotic instruments are manipulated to their respective targets, respectively, and have to move close to each other within shared workspace. The initial and final states of two instruments are defined such that their trajectories will make the robot skeleton collide to each other if no avoidance is actuated.

Figure 12 shows the setup of this scene and the simulation snapshots for the dual-arm manipulation considering tool-tool collision avoidance. Our framework allows simultaneously dual-arm collision avoidance as the planning strategy is decentralized. The minimum tolerant distance is set to 10 mm as the tool shaft size is 8 mm, leaving the minimum tool-tool distance to be 2 mm. Both two instruments start collision avoidance as soon as the process begins and consider the other one as the moving obstacle. By inspecting the instant nearest obstacle point on the skeleton, two shafts move around each other and retract the distal part to prevent the whole robot from collision. After the other instrument stays clear from its target, the instrument proceeds on the tool-centric action and target-centric action.OPEN IN VIEWER

Figure 13 shows the resultant dual-arm trajectories. For comparison, the trajectories for direct target reaching are shown as well. It is clear that to avoid collision, the trajectories are more complex, but remain smooth and do not affect the subsequent target-reaching process where a constrained path is required. The collision avoidance input only applies on demand when the obstacle obstructs target reaching, and will smoothly fade once the obstacle is clear.OPEN IN VIEWER

7.4. Scene III: Perturbed interaction

In this subsection, we validate the motion performance of the instrument when reacting to different target situations led by the GVM. A typical action during surgery is to manipulate the instrument’s distal tool from a safe idle configuration to contacting a target object, usually for grasping purposes. The contact must be made via a proper reaching pose to ensure safe tool-target interaction. We use a half-circle (or 1/2) surgical needle as a target, where the robot aims to grasp the needle body at its two-thirds curvature point as a standard needle pick-up procedure. The needle is attached to a support to visually emphasize its pose change during simulation, which is also known to the robot’s base. Again, we apply Mode III motion behavior to the instrument, where X g moves between X p and X c subject to on-the-fly regulation of τ. X p is assigned with the same orientation as of X c but with a negative z-axis offset position from the grasping point. Figure 14 illustrates the tool reaching process of the instrument from its idle position [−116.04, 16.85, −56.13] ^⊺ mm to the ideal needle grasping point [−135.68, 18.60, −61.17] ^⊺ mm. The trajectory of the end-effector indicates that the robot performs tool-centric action that guides the tool to the pre-grasp pose first with τ maintaining close to 0. While X g is nearly aligned with X p , the GVP τ adaptively transits from 0 to 1 to further allow the instrument to eliminate the remaining distance for contact, that is, proceeding to target-centric action. Note that during such action, the orientation of the distal tool remains unchanged despite the varying X g at the final phase of contact motion, which helps achieve a proper tool-target contact configuration. It is also shown in Figure 15 that the regulation of the end-effector’s orientation is already settling (with the robot approaching X p ) before the target-centric action begins (with the robot approaching X c ). This allows the tool to be “well-prepared” in advance to delicately reach the object, which will be shown later in our experiments to improve task-level reliability.OPEN IN VIEWER

OPEN IN VIEWER

Figure 15.

Evolution of two examples of target reaching shows that orientation is alignment in tool-centric action phase and then comes to translation to eliminate the remaining distance to avoid premature collision.

We next study how GVM will react to a target under different situations. The target pose of the robot might be perturbed, where the disturbances could be from sensoring noises or physiological motions during surgical procedures. We add uniform random noises to the target 3D position with the magnitude of 0.003 mm for all axial directions and is deployed throughout the process. The orientation remains unchanged at this stage. The comparison of the target-reaching motion performance with and without applying noises is shown in Figure 15. It is clear that the perturbed setting results in not only longer convergent time of the tool’s position e _t , but also remaining a stabilized position difference (denoted by e_t,s) from the ideal contact point. This results from the untrackable noise which makes the robot controller hard to converge η sufficiently to 0, and thus prevent τ from settling to 1, although the orientation convergence is not affected. This indicates the noisy target could lead to an “inconfident” targeting attempt by our planning and control framework.

To further study the robot motion behavior using our system to deal with target disturbances, we apply different noises to the identical setup and further obtain the relationship between the e _t and the noise configurations, shown in Figure 16. Obviously, larger noises indicate larger stabilized position error e_t,s from which the relationship is relatively linear. Meanwhile, selecting smaller gain κ that tunes the reaction of τ ˙ to η will lead to decreased e_t,s. This is because a smaller κ relaxes the condition of position alignment between X to X g and ease τ to move to 1. When the noise magnitude is elevated to a certain turning point, the stabilized position rapidly increases but remain linear afterward. This is caused by the τ rewinding from nearly 1 to ≪1 due to the noise-induced misalignment of the tool to the target (or the unsettled η . X g then returns to X p to make easier for the robot to reach and thus automatically creating a new balance. As the change of X g ’s orientation provides extra disturbance to the position of X p , the turning point of e_t,s upon increasing κ comes earlier, while the level-off e_t,s is also higher than that without orientation noise. However, the maximum e_t,s under different configuration will not exceed d in (35), as in the worst case, τ will only drop to 0. The above performance reflects that the disturbance of the target state could significantly change the motion behavior of the instrument when attempting to reach the target. Target perturbation will make the strategy cautious when performing the target-centric action, and might even abandon it if the disturbance is severe enough due to the impossibility to settle η to a. The instrument will still settle to X g with a contact-ready orientation upon tool-centric action, but then keeps a (preset) safe distance from the target. If the noise magnitude becomes small, the robot controller is still able to converge η close to 0 such that τ maintains positive as in (30). The strategy is then “confident” enough to guide the end-effector to the X c , or the final contact pose (e.g., d < 2 mm or so). Note that among all the above cases, a stabilized X and X g is always achieved. The performance of the GVM using the proposed framework remains stable regardless of the noises being added.OPEN IN VIEWER

7.5. Scene IV: Target-varying interaction

Finally, we conduct simulation based on Scene II to study how our framework could deal with a moving target. The instrument might reach a target which is being moved by another instrument (e.g., needle hand-off in wound suturing) or need to follow specific trajectories, such as needle insertion into tissue and palpation through tissue surfaces. The target is either moving or to be moved after being grasped. Here, we propose the following three typical cases that cover different target motion types to further evaluate the applicability of our framework to different manipulation strategies:

• Reaching an intermittently moving target along a linear trajectory with unchanged orientation.

The first and second cases that correspond to decentralized dual-arm coordination for target hand-off are set. In the first case, the final contact position x _c (or the needle) moves from [−102.59 − 86.77 − 62.46] ^⊺ mm to [−163.49 − 85.99 − 58.49] ^⊺ mm through a straight line but halts at the midpoint for 100 steps. The movement starts at t = 180 and the overall travel distance is around 60 mm. The target orientation is [2.70130.5169 − 1.8761] and remains unchanged. The translating velocity of the needle is set to 0.1 mm per step. In Figure 17(a), the end-effector trajectory is first adjusted for tool-centric action for target orientation alignment, and then proceed to target-centric action while τ is moving to 1. However, after the needle starts to move (in orange), the misalignment between the tool and the target makes contact process immediately suspended, where the instrument position rapidly moves back to X p and track the needle’s pose with a stabilized distance d. While the needle temporarily stops, the instrument attempts to reach the target again, and is suspended again as the needle moves. Finally, after the needle comes to its final stop, the instrument manages to reach the final contact point as τ eventually settles at 1.OPEN IN VIEWER

Case II is similar to Case I but with 6-DoF pose change over time. By reserving the translation motion of the needle, we add rotary motion of the needle with respect to its own frame, that is, 0.15* sin 0.025t rad along y-axis and 0.25* sin 0.025t along z-axis. The motion starts at t = 150 and continuously lasts for δt = 600. As shown in Figure 17(b), the end-effector manages to track the moving X g induced by X c without contacting the needle. When the motion disappears, the tool is finally guided to the contact point where the needle could be directly grasped.

In Case III, we show how GVM contribute to stable trajectory tracking of the instrument. We first define an arc-shaped trajectory whose distance to the rotation center is 15 mm, and the total equivalent rotation range is 2/3π. Such needle insertion path is commonly used in MIS to minimize tissue trauma during penetration. The GVP τ becomes the parametric value that guides X g along the trajectory. The robot will move to the starting point of the trajectory first and then follow the trajectory as τ elevates to 1. While the robot output meets disturbances, that is, deliberate deviation of the tool from the prescribed path, the value of τ will not move until the controller guides the tool back on where it deviates from (illustrated in Figure 17(c)).

7.6. Summary of simulations

The simulation results demonstrate the motion-level behavior of our robot planning and control framework in different scenes. Overall, the GVM method is validated to be capable to dealing with the following three scenarios:

• Pose transfer through extreme configurations: The planning and control motions remain feasible and efficient to large-range pose adjustment ( > 100 average distal joint position differences) near joint limits.

Basically, the framework provides a reactive planning and control process which could be used to characterized different types of elementary steps in surgical procedures, and could lead to a “fail-in-safety” performance which highly reacts to the on-the-fly information. This is essential to further improve task-level reliability when automating a sequence of motion steps in order to complete a task successfully. The quantified results will be later addressed in experiments.

8.1. System setup

In this section, we present results of on-site experiments for the robot to automatically perform simulated surgical tasks. We use a dVRK with two PSMs as the robot platform. Both PSMs are equipped with LNDs, which is a common type of instrument adopted when conducting tissue arrangement and suturing in RAMIS. Both instruments have a 7-DoF joint set with the first four accounting for the RCM motions for minimally invasive setup, the middle two joints for providing wristed motions, and the last DoF for controlling the tool actuation (or the jaw opening angle). The physical setup is shown in Figure 18. We use a Core i7 3.4 GHz user-side PC controller (with 156 GB RAM and without GPU acceleration) which connects to the robot controller with TCP/TP. The cisst/SAW software environment is used and the algorithms run in MATLAB 2020b under the Robot Operating System (ROS). A pair of industrial CMOS cameras is used for online visual feedback data acquisition with 640 × 480 resolution and 30 frames per second. The base position of the two robots (i.e., the RCM) is set such that their distances to the target to be operated are both around 150 mm, which is commonly adopted in the clinical practice of RAMIS. The camera is fixed between two PSMs, which is a typical setup scale for multi-port RAMIS (Escobar and Falcone, 2014). The positions of the left camera relative to PSM1 and PSM2 are [34.5, 70.2, 39.1] ^⊺ mm and [66.9, 104.6, 51.60] ^⊺ mm, respectively. The overall control loop of the system is around 20 Hz without software acceleration. The robot end-effector motions are saturated to 0.8 mm per step, which indicates a maximum 16 mm/s linear velocity for the tool.OPEN IN VIEWER

We define the jaw angle to stay 0 during tool-centric actions because the 7 ^th is not considered in tool-centric actions. While the instrument enters target-centric actions (with τ > 0 and τ ˙ > 0 ), the jaw angle smoothly opens to a predefined angle (30°) and will only close when τ → 1. When inverting the trajectory, the above sequence will be toggled accordingly. This applies to any elementary motions which involve Modes II, III, and IV.

Several sensing algorithms are required for providing accurate 3D information to facilitate task completion is introduced in this subsection. The focal lengths of the two cameras are tuned to 30 mm. ⁵ The image resolution of both cameras are set to 640 × 480 which acquire video streams both at 30 frames per second. The stereo camera has been calibrated in advance (Zhang, 2000) with its backprojection being 0.1582 pixel upon data acquisition of 30 pairs of sample images. The camera-robot transformation has been calibrated using the method based on our previous work (Zhong et al., 2020) to all the engaged PSMs. The performance of our framework is systematically assessed by respectively performing single-arm tasks like debridement and membrane dissection, a dual-arm task like suturing. Task effectiveness in terms of efficiency, success rate and accuracy will be validated.

8.2. Task I: Debridement

The first scenario simulates the surgical tissue debridement procedure, which is designed to illustrate the performance of our framework to automate repetitive reach-and-grasp motions of the robotic instrument to different individual targets. We fabricate a group of L-shape silicone gel blocks to simulate tissue debris. Each block has a bounded 12 mm cube of size which simulates a plumped piece of tissue with a graspable edge, and is attached by a unique fiducial marker of AprilTag (Olson, 2011) with 25h9 tag family for 6-DoF target localization and multi-target identification. The target is online detected and tracked during the task. We apply the MoB to be Mode II (refer to Figure 8) for automating the block reaching with grasping and Mode III for retracting with tissue releasing for debris collection to a plastic plate.

We design a multi-block debridement task to be completed by PSM 1. The blocks are random placed within a rough volume of 50 × 50 mm² table surface, whose orientation are randomly settled as well. As all the blocks are identical in sizes, their poses are online tracked by using the fiducial markers to generate grasping configurations. Their pick-up sequence is then randomly assigned to avoid adding bias to statistical performance analysis. The releasing pose T _rel of the robot’s end-effector for debris collection is assumed constant with

T r e l = [ − 0.924 − 0.222 0.310 0.117 − 0.176 0.970 0.170 − 0.050 − 0.338 0.103 − 0.936 − 0.0527 0 0 0 1 ]

(38)

We have conducted ten trials of six-block debridement and ten trials for four blocks. As the main goal of the task is to clear the blocks from their initial positions, we evaluate the quality of this task by assessing the overall success rate of the whole task and the time distribution for each pick-up step. Figure 19 illustrates the snapshots of two trials of six-block trials. The overall success rate for the six-block and the four-block cases are all 90%, that is, both 9 out of 10. Note that for each case, we rearrange the blocks by random placement. Despite different poses, the instrument manages to collect all blocks smoothly. The algorithm could guide the instrument adjacent to the block first and then finalize the contact reaching to achieve a safe grasp thanks to the GVM scheme. It avoids premature tool-target collision during tool-centric action phase which can prevent the tissue from being dislocated or being damaged unexpectedly. This is important as it is normally difficult to track the instant change of tissue’s position shortly before contact due to visual occlusion. We also show in Figure 20 that during the grasping attempt, inaccurate instrument localization might unexpectedly deviate the target from its initial position. Our framework could rapidly react by stating clear from the target first and initiates further adjustment. As long as the target is detectable, the instrument will start another attempt once the tool-target configuration is desirable again. Figure 21 shows the average execution time of each motion step during the task. The median duration for collecting each block is 12.8/11.9 s for four/six block trials. The four-block cases take longer time to complete one debris collection, as the more scattered placement of the blocks leads to longer instrument travel distances. The random block setting does not significant affect the completion time for each attempt, which shows good consistency.OPEN IN VIEWER

OPEN IN VIEWER

Figure 20.

Target uncertainty during task execution, where the block is unexpectedly deviated to a new position (deviated motion marked in orange arrows) due to premature tool-target collisions. The planned/re-planned tool position is conceptually denoted as yellow arrows.

OPEN IN VIEWER

Figure 21.

Motion step durations during debridement, where the number 4/6 indicates the number of fragments, and the subscript 1/2 denotes the reaching/collecting motion of the fragment, respectively.

8.3. Task II: Tissue membrane dissection

We next set up an experiment to simulate the dissection task to a tissue membrane, which is a typically demanded in surgeries involving organ removal like laparoscopic cholecystectomy (Reynolds Jr, 2001). The task includes highly repetitive cutting sub-steps in order to dissect the superficial layer of the tissue via a prescribed trajectory. We use a soft handkerchief paper with average thickness of 1 mm as a phantom tissue layer. The layer is fixed by a static support which keeps the tension of the surface to simulate the connectivity of the target to surrounding tissues and to reduce tissue deformation. PSM2 is used to perform the task using the Potts Scissors instrument to provide cutting shear force to the target. The tissue is of size 90 mm * 80 mm, and the dissection path on the tissue is marked by a red line that goes across the layer surface. We set two types of path, one starts from the short edge and ends at the opposite one (with total length of 90 mm), and the other spans diagonally to the midpoint of the long edge (with totally length of around 60 mm). The 3D positions of the path endpoints are detected first using the stereo camera and then are used to interpolate the path profile by comparing the backprojection error with respect to the observed one. Finally, and the interval distance d _c to between consecutive cutting steps is computed to determine the cutting points. For empirical experiences, we select such distance to be d _c = 3 mm, which indicates a 30-time cutting motion sequence for a 90 mm path and 20 cuts for 60 mm path.

We have conducted five consecutive trials for this task. Three trials are set We use Mode III to perform the dissection procedure (could be also solved by Mode IV but slightly slower) and the clamping motion of the scissors is done after settling the manipulation. The setting of d _c is set smaller than the length of the scissors (around 9 mm) allows a backward shifting before proceeding to the next step to avoid the tissue-tool adhesivity to after each cutting. During the procedure, the instrument automatically plans and dissects the phantom layer progressively through repetitive cutting steps and separates the layer in half via the labelled paths. Figure 22 shows the snapshots of the recorded clips of the real-time automated dissection process using a single instrument. The robot successfully completes four out of five trials with the overall success rate thus being 80%. The failure case is caused by tool-tissue entanglement with uncontrollable path deviation. The average duration for executing the whole task is 81.1 ± 1.2 s and 54.3 ± 0.9 s for the 90 mm case and 50 mm case, respectively. The time cost for performing each single cutting step is 3.9 ± 0.3 s. The dissected layout of the phantom tissue layers are shown in Figure 23. Upon manual measurement, the maximum lateral deviation from the prescribed linear path is around 1.5 mm for both 50 mm and 90 mm case (we assume the highest resolution for measurement is 0.5 mm as the smallest scale of the used ruler is 1 mm).OPEN IN VIEWER

OPEN IN VIEWER

Figure 23.

Demonstration so the dissected phantom layers placed on the table with the prescribed cutting paths in red lines, and the fixation points to the support marked in red dots.

8.4. Task III: Wound suturing

We finally conduct autonomous dual-arm multi-throw wound suturing as a comprehensive evaluation of our framework to perform complicated tasks. The task appears in most surgical interventions, where a suturing needle is guided through wound edges for tissue approximation and wound closure. The guideline is characterized by our framework in Figure 8 into seven individual steps for each throw.

8.4.2. Single-throw suturing

We first show the capability of our framework to complete a single-throw pipeline in suturing. Figure 24 shows the snapshots of individual motion steps of a single-throw pipeline on the artificial tissue. The needle’s insertion and exiting point on the tissue is directly assigned by user input that yields the preset d _w and d _t . The instruments start from preset idle configurations which are cleared from the target tissue. The manipulation in each step is converged to the goal configuration with || η || down to a scale ϵ _g before proceeding to the next step. To perform target-centric action to the needle during targeting, the tool-needle transformation is computed using image-based needle pose detection at the start of a throw. During needle hand-off, two instruments work in a decentralized manner, where PSM2 guides the needle to a precomputed hand-off pose on top of the wound, with PSM1 automatically track the motion of PSM2 to decide a proper hand-off configuration subject to GVM scheme. An example is particularly shown in Figure 25 that demonstrates the decentralized needle hand-off process by human–robot collaboration. The complete parameter settings for this scenario are shown in Table 3. Note that the pipeline in a single throw covers target contact (e.g., grasping the needle), trajectory tracking (e.g., needle insertion/existing), and dual-arm coordination (e.g., needle hand-off). Due to the theoretical proofs of trajectory feasibility and motion stability, all the elementary motions are characterized and automated, involving target contact (including needle insertion, regrasping, and hand-off) while the path avoids premature collisions to the target tissue (refer to our supplementary video for demonstration).OPEN IN VIEWER

Figure 24.

Step illustration of a single-throw suturing during task automation. From left to right: 1) idle (the start of a throw); 2) needle (pre-entry) targeting; 3) needle insertion; 4) needle regrasping; 5) needle exiting; 6) suture pulling; 7) needle hand-off; and 8) resetting (for starting the next throw).

OPEN IN VIEWER

Figure 25.

When PSM1 is manually controlled by a user (with the shaky trajectory in red arrows), PSM2 stably tracks and makes contact attempt (in yellow arrow) when the tool-target alignment becomes desirable.

OPEN IN VIEWER

Table 3.

Task-relevant parameters of our framework for multi-throw suturing.

	Pos. of idled PSM1/2	Pos. of first insertion/exiting point	Pos. of last insertion/exiting point	Pos. of wound endpoints
Artificial tissue	[0.131 −0.067 −0.054] ⊺ [−0.091 −0.012 −0.045] ⊺	[0.117 −0.066 −0.096]⊺ [−0.133 −0.044 −0.091]⊺	[0.111 −0.053 −0.090] ⊺ [−0.122 −0.032 −0.095] ⊺	[0.130 −0.066 −0.093] ⊺ [0.116 −0.007 −0.060] ⊺
Porcine tissue	[0.116 −0.051 −0.055]⊺ [−0.099 −0.044 −0.091]⊺	[0.117 −0.074 −0.086] ⊺ [−0.140 −0.042 −0.079]⊺	[0.103 −0.053 −0.082]⊺ [−0.123 −0.020 −0.078]⊺	[0.131 −0.071 −0.083]⊺ [0.119 −0.045 −0.081]⊺
*All positions (unit: m) are calculated relative to the respective robot base except the wound endpoints positions (from the camera).

8.4.3. Multi-throw suturing

We define a five-throw suturing task for artificial tissue and a four-throw suturing task on porcine tissue to comprehensively evaluate the task-level applicability of our framework. The suturing will be performed using single-continuous suture pattern, which is a common and efficient type of suturing technique in RAMIS. We do not include knot tying at the end of suturing due to the complexity of detecting the suture’s 3D topology, which is not the main goal of this work. The needle’s insertion/exiting positions are computed based on two virtual 3D markers (named as the “wound positioners”) manually input by the users via stereo images prior to the start of the task. The needle insertion orientation is computable once the insertion and exiting point is known (Pedram et al., 2017). Note that we select d _t as 8 mm for the porcine tissue to evaluate task performance under different settings. The moving distance of the suture pulling step is computed based on its consumed length of suture in each throw, which is generically given as 14 mm measured from teleoperation based on the assigned d _w and d _t . Trials on each type of tissue are conducted continuously to maintain identical setting throughout the experiments.

We perform ten trials for each type of tissue, respectively, under identical setting. The task-level assessment include task success rate to evaluate reliability, duration of a single throw and the entire task for in-process performance, and resultant suture pattern accuracy for task quality. Figure 26 illustrates the frames of task execution process on both the artificial and porcine tissue viewing from the sensing (left) camera. The task is defined as “successful” only if the suture passes through both edges of the wound for enough times without user interference. The overall success rate of performing five-throw suturing is 80% on the artificial tissue (i.e., 8/10) and 70% on the porcine tissue (i.e., 7/10). The failure cases are either subject to needle-suture entanglement or missing the exiting point during insertion. It could be further elevated to 90% (i.e., 9/10) for both the artificial tissue and porcine tissue suturing, respectively, if manual suture arrangement is provided during the task. Note that the above data is calculated by directly regarding a task as a failed trial once any step failure occurs during the process. For step-wise failure, we manually halt the task once a failed step occurs and rearrange the needle/suture accordingly, and then let the robot proceed to the rest of the task to compute the accumulative number of step failure. In this case, a total of 5 motion failures during 10 trials of five-throw suturing (or among the totally 640 motion steps), which indicates 0.8% of motion failure.OPEN IN VIEWER

Figure 26.

Frames of dual-arm wound suturing using our framework on the artificial tissue (first row) and the porcine tissue (second row). Each image is captured at the same moment in the step (during needle targeting in the first row and during needle insertion in the next row).

Suturing the porcine tissue owns lower success rate, as the tissue surface might encounter irreversible deformation after many stitches. The trajectories of the two instruments’ distal tool for completing such task are shown in Figure 27. It can be seen that the in-process trajectories maintain good consistency among individual throws without unpredictable movements. Figure 28 demonstrates the suturing accuracy on the artificial tissue. ⁶ Figure 29 shows the time cost for completing individual throws in the task. The average time of performing one throw is 48.3 ± 1.4 s and 45.1 ± 1.0 s for artificial/porcine tissue, with the time variation percentage for being only 2.9% and 2.2%, respectively, which shows good step-level consistency. The average time cost for completing each throw gradually decreases (shown in Figure 29), as the decreased residual suture length lowers the travel distance for suture pulling. Furthermore, we characterize the outcome quality by manually measuring the deviation of the insertion/exiting position accuracy. The total average accuracy is 1.7 ± 1.1 mm, with the maximum 2.0 mm of wound deviation as the number of throws increases due to the irreversible deformation after several trials. The deformation remains limited influence on suturing accuracy, and is regarded tolerable unless it causes misplacement of the needle during insertion (which is directly regarded as failure).OPEN IN VIEWER

Figure 27.

Trajectories of the two instruments during the five-throw suturing task, where the red/yellow/green/blue/purple indicates the trajectories from 1st to 5th throw, respectively.

OPEN IN VIEWER

Figure 28.

The suture pattern on along the tissue wound edges after five-throw suturing (left-upper three: results on artificial tissue; left-bottom three: results on porcine tissue), and the manually measured suturing accuracy on the insertion/exiting positions (the right figure).

OPEN IN VIEWER

Figure 29.

Duration required for each throw for suturing an artificial tissue and a porcine tissue, respectively. The gradual descent appears via consecutive throws in attributed to the shorter residual length of suture and thus shorter suture pulling distance under saturated tool velocity.

8.5. Overall assessment

In this subsection, we validate the task-level performance of our framework by comparing with existing works reporting either manual task operations by novice surgeons, or state-of-the-art automation approaches. We focus on aspects including the time cost, the step/task failure, and number of individual trials in each task which are all metrics that reflect the task execution quality and consistency during performance assessment (Martin et al., 1997). The existing works which aim to automate the identical tasks (e.g., continuous debridement and suturing) using the standard guidelines with articulated surgical robots will be especially selected and compared to ours as side-by-side analysis. Note that we are unable to provide identical task-relevant parameter settings for performance comparison, as many works do not provide detailed parameter settings. The comparison will be conducted based on their best reported results as well.

We first summarize the duration performance from the above experiments into time cost for completing each task-relevant motion step (results shown in Figure 30. The time cost for completing the different task-relevant motions under different setups are shown in Figure 31. We calculate the variation percentage of the duration for each step in terms of the average time cost is 3.3%/4.5% for the 4-/6-block debridement, 3.7%/3.7% for the 5/8 mm dissection procedure, and 1.8%/1.4% for the four-/five-throw suturing on the artificial/porcine tissue, respectively. They indicate good consistency of robot’s planning and control performance under different setups. As there are current no existing works that automate different surgical tasks, the performance could be used as a benchmark for future works to refer to for multi-task automation.OPEN IN VIEWER

OPEN IN VIEWER

Figure 31.

Time consistency of completing task-relevant motions with different settings (D/4 and D/6 denote 4-/6-block debridement, C/5 and C/5 denote 50-/80-mm dissection path length, SA and SP denote suturing on artificial/porcine tissue, respectively.

We have also listed results among different works in terms of setups, the time cost, and the accuracy (if available), where the results are shown in Table 4. For comparing task duration, we post-process the data of each work by computing the average time to execute one single step for each task (as shown in the data in blue font). Our framework takes averagely 6.2 ± 0.3 s to finish collecting a single fragment in debridement, which is slightly faster than many of the results using manual operations by novices, but a lot greater in time consistency. Regarding suturing, our framework takes averagely 46.7 ± 1.3 s for a single-throw suturing throw on the assigned tissue surface (maximum tool velocity currently set to 16 mm/s), which is significantly more efficient (mostly 1.5-2 times faster) than manual operations, and the time variation is more than an order of magnitude smaller. Note that we are also the first to conduct five-throw suturing on phantom tissue and also the first to use wristed robotic instruments for multi-throw suturing on porcine tissue. The performance is mainly attributed to a close-form solution for integrated motion planning and control framework, which owns looping time of < 1 ms without software acceleration, and does not involve data-driven knowledge or iterative computations. Although the works appeared in Table 4 own different setups and the performance might not be compared with identical scenarios, we show the capability of our work to be a baseline for generic surgery autonomy.

OPEN IN VIEWER

Table 4.

Comparison of manual/robot-assisted/autonomous tasks execution in physical environments. The time cost in blue font indicates the computed equivalent time cost for each elementary procedure in a task. Failure rate (column "Fail”) is computed with average number of failures per task and/or total accumulative failure percentage (N.A. indicates that the data is not given or uninterpretable).

Works	Tasks
	Debridement				Suturing
	Target	Qty.	Time cost (s)	Fail	Setting	Qty.	Time cost (s)	Fail
Garcia-Ruiz et al. (1998)	Bead	10	M 68±32 (6.8) RZ 183±42 (18.3)	5% 3%	Needle 2-O Width 10 mm	4	M 154±40 (38.5) RZ 605±57 (151.25)	8% 8%
Dakin and Gagner (2003)	Peanut	10	M 53.5 (5.35) RZ 128.5 (12.85) RD 61.0 (6.1)	2.22 (22.2%) 3.11 (31.1%) 2.44 (24.4%)	Needle 4-O Width 5 mm	4	M 172.0 (43.0) RZ 426.1 (106.5) RD 236.2 (59.1)	3 2.56 1.78
Hubens et al. (2003)	Ring	1	M 19.0±5.7 RD 9.5±8.7	2.25 0 *median	Needle 4-O Width 5 mm	4	M 356.4±112.6 (89.1) RD 60.5±39.2 (15.2)	38 2
Shah et al. (2009)	N.A.				Needle 20 mm Width 5 mm	4	RZ 258±93 (64.5)	53
Fard et al. (2018)	N.A.				Needle 4-O Width 5 mm	4	RD 126.84±55.86 (31.7)	N.A.
Kehoe et al. (2014)	Foam	1	M 29.0 AR 91.8	5.0% 8.7%	N.A.
Mahler et al. (2014)	Foam	1	AR 15.8	N.A.	N.A.
Seita et al. (2018)	Seed	8	AD 57.62 (7.20)	10 (8.3%)	N.A.
Leonard et al. (2014)	N.A.				Needle 3-O Width 10 mm	9	M 560.4±358.6 (62.3) RD 342.6±226.2 (38.1) A* 64.51±0.81 (7.2)	N.A.
Sen et al. (2016)	N.A.				Needle 39 mm Width 5 mm	4	M 136.85 (34.2) AD 383.00 (95.8)	50%
Pedram et al. (2020)	N.A.				Needle 30.55 mm Width 16 mm	1	AR ∼200	N.A.
Ours	Syn-Gel	6	AD 48.8±1.3 (6.2±0.3)	0.1 1.7%	Needle 34 mm Width 10 mm	5	AD 243.5±1.9 (46.7±1.3)	0.5 1.4%
Notation of task setting in the “time cost” columns.
First letter: 1) M: manual execution with regular instruments; 2) R: robot-assisted surgeon-centered execution; 3) A: autonomy.
Second letter: 1) Z: Zeus robot; 2) D: dVSS/dVRK; 3) R: Raven Robot; 4) *: other types of robots.