Optimization-Based Constrained Trajectory Generation for Robot-Assisted Stitching in Endonasal Surgery

Abstract

The reduced workspace in endonasal endoscopic surgery (EES) hinders the execution of complex surgical tasks such as suturing. Typically, surgeons need to manipulate non-dexterous long surgical instruments with an endoscopic view that makes it difficult to estimate the distances and angles required for precise suturing motion. Recently, robot-assisted surgical systems have been used in laparoscopic surgery with promising results. Although robotic systems can provide enhanced dexterity, robot-assisted suturing is still highly challenging. In this paper, we propose a robot-assisted stitching method based on an online optimization-based trajectory generation for curved needle stitching and a constrained motion planning framework to ensure safe surgical instrument motion. The needle trajectory is generated online by using a sequential convex optimization algorithm subject to stitching kinematic constraints. The constrained motion planner is designed to reduce surrounding damages to the nasal cavity by setting a remote center of motion over the nostril. A dual concurrent inverse kinematics (IK) solver is proposed to achieve convergence of the solution and optimal time execution, in which two constrained IK methods are performed simultaneously; a task-priority based IK and a nonlinear optimization-based IK. We evaluate the performance of the proposed method in a stitching experiment with our surgical robotic system in a robot-assisted mode and an autonomous mode in comparison to the use of a conventional surgical tool. Our results demonstrate a noticeable improvement in the stitching success ratio in the robot-assisted mode and the shortest completion time for the autonomous mode. In addition, the force interaction with the tissue was highly reduced when using the robotic system.

1. Introduction

Endoscopic endonasal surgery (EES) is a common procedure for treating pituitary lesions and has proven to be effective in reducing tissue trauma and speeding up the recovery time. However, due to the reduced workspace and the lack of direct visualization, much dexterity is required for the execution of complex surgical tasks with increased surgeon’s work effort. Suturing is considered one of the most challenging and time-consuming tasks in minimally invasive surgery [1], and is a common procedure in EES to secure the reconstructed dura after an endonasal tumor resection (see Figure 1). Inadequate suture can lead to cerebrospinal fluid leakage, which is a common postoperative complication in EES. The limited degrees of freedom (DOFs) in conventional surgical tools reduce the range of motion for needle manipulation. Furthermore, the two-dimensional endoscopic view makes it difficult to recognize the relative position of the tissue from the surgical instruments, resulting in multiple attempts and increased tissue trauma to achieve a proper suture [2].

Recently, there has been an increasing interest on the development of robotic surgical systems to perform complex tasks such as suturing in a limited workspace with reduced trauma, by autonomous procedures with higher dexterity [3]. Robotic systems can also help to reduce the steep learning curve for surgeons associated with minimally invasive suturing. However, the development of commercial robotic surgical systems for EES is still limited. For example, the da Vinci Surgical System (Intuitive Surgical, Sunnyvale, CA, USA) has been used successfully in urological, gynecological, and gastrointestinal surgery. The use of their high-dexterity endo-wrist technology facilitates the manipulation of surgical needles on a limited workspace. However, the size of the current surgical instruments (5–8 mm in shaft diameter), the large required workspace, and complex preoperational setup still prevent the use of the da Vinci Surgical System in EES.

In our previous work [4], we have focused on the development of a human–robot interface for controlling multi-DOF articulated forceps in EES. This paper expands our prior work with a robot-assisted stitching framework based on optimal needle trajectory generation subject to the stitching kinematic constraints, and a dual concurrent constrained control algorithm to track the generated trajectory. The proposed approach restrains and guides the motion of the robot during the needle insertion and extraction through the tissue, while preserving a remote center of motion (RCM) placed over the nostril to reduce additional tissue damage.

The rest of this paper is organized as follows. Section 1.1 introduces the related work in robot-assisted suturing. Section 2 first describes the problem setup. Then, we present the proposed needle trajectory optimization algorithm and the constrained motion planning method. The experimental setup and the performance results discussion is presented in Section 3. Section 4 concludes the paper.

1.1. Related Work

A suturing task comprises several steps: (s0) selection of suitable entry and exit points; (s1) needle grasping, placement, and reorientation over the entry point; (s2) needle insertion and extraction; (s3) create a suture loop for knot tying; and (s4) tighten and secure the knot [5]. In minimally invasive surgery, suturing is a frequent, repetitive, and yet time-consuming task. Previous studies have focused on the automation of one or some of the above-mentioned steps in the suturing task in order to reduce surgeon’s fatigue and operation time.

Learning-by-demonstration techniques have been proposed for autonomous knot-tying tasks (s3–s4). Knoll et al. [6] introduced a skill transfer approach from human demonstrations based on knot-tying primitives decomposition, feature extraction, and task generalization using the da Vinci robot. However, the implementation was not suitable for online planning because of the significant computational cost. In [7], Osa et al. use a set of demonstrated trajectories under various environmental conditions to learn the knot-tying process, and proposed an online trajectory regeneration for adapting over changes of the dynamic surgical environment. On the other hand, Van der Berg et al. [8] proposed the use of iterative learning control to determine a task trajectory without the need of the task description. Different approaches have been proposed to perform autonomous surgical tasks without learning. Multi-step sequential trajectory specifically designed for knot-tying was proposed in [9]. Chow et al. [10] proposed a knot-tying automated path generation based on a binary-star search method over objective metrics defined for candidate motion patterns.

Autonomous stitching (s0–s2) has also been studied, mainly focused on the generation of an optimal needle trajectory, and can be divided into two types: constant curvature paths and adjustable curvature paths. In the case of a constant curvature path, the needle rotates around its center to reduce trauma when puncturing the tissue. Nageotte et al. [11] presented a kinematic analysis of the stitching task, and used an A*-based method to find the optimal needle path given the desired entry and exit points. Liu et al. [12] introduced an offline optimization framework for optimal entry port selection and needle grasping pose. However, the exhaustive search methods require a high computational cost and is not suitable for real-time implementation. Staub et al. [13] proposed a visual-servoing control to position the needle over a desired point marked by a laser pointer and performs a circular needle motion to pierce the tissue. D’Ettore et al. [14] also applied a vision based control for autonomous needle grasping, but did not consider additional requirements for subsequents suturing steps. Iyer et al. [15] proposed a visual-servoing control for autonomous stitching, which provides smooth needle steering by calculating the needle optimal center point of rotation. Pedram et al. [16] used a nonlinear optimization algorithm to generate a needle constant curvature path subject to the tissue geometry, desired entry/exit points and kinematic constraints.

Notice that the use of a constant curvature paths could fail to meet the suturing requirements, e.g., depth and length, or conflict with the robot kinematic constraints. As a result, capability of exerting forces required for tissue penetration could be reduced. Therefore, subsequent studies have considered adjustable curvature paths, where needle orientation adjustments are allowed. Sen et al. [17] proposed a sequential non-convex optimization framework to find the optimal trajectory, subject to kinematic constraints, bounded needle reorientation, minimum trajectory length, and orthogonal needle poses to reduce tissue trauma. It also includes a mechanical needle guide to reduce needle pose uncertainty. Jackson et al. [18] developed a needle trajectory plan based on the best practices of manual suturing that allows needle reorientation and ensures suture depth and needle handling. Autonomous needle extraction was proposed in [19], in which the visual feedback is used to control two teleoperated robot arms with a single user interface.

Additional studies explored the automation of other surgical sub-tasks: optimal port placement [20], surgical debridement [21], surgical cutting [22,23], and real-time thread tracking [24,25]. Fully autonomous suturing is still considered a high-risk procedure because of the variability in human anatomy and uncertainties in environment modeling (tissue, needle pose, and thread). They also rely on time-consuming complex calibration setups that need frequent readjustments and low operational speeds that extend the operation time.

Cooperative human–robot suturing with shared control between the operator and robot has been also explored as an alternative to autonomous suturing. Here, the robot can guide or restrict the surgeon’s command, or execute automated surgical sub-tasks. In [26], the surgeon commands the robot in subtasks where environment interactions are involved, such as grasping or needle insertion, and automatic execution of pre-learned subtasks are executed sequentially after the manual subtasks. Reed et al. [27] developed a robot-assisted steering system for bevel-tip steerable needles by integrating a stochastic roadmap-based planer, a planar controller, and a torsion compensator. During the surgical procedure, the surgeon can either only pause the insertion to verify the needle location or abort the procedure by retracting the needle.

Virtual constraints are often used to guide or constrain the surgeon’s commanding motion. Kapoor et al. [5] proposed a guidance virtual fixture to assist the surgeon move towards the entry point and along the desired trajectory defined for a stitching task. The constrained motions is formulated as a quadratic programming minimization problem. The surgeon commands the robot through a force-based control, and needle reorientation is allowed to reduce the error between the desired entry/exit points. An impedance virtual fixture framework for needle passing and knot tying was introduced in [28] by constraining the tool tip within a plane, and reducing needle pose uncertainty through a 3D printed needle holder. Selvaggio et al. [29] presented an optimization-based haptic shared control for needle grasping that takes into consideration the robot joint limits and singularities. Marinho et al. [30] developed a looping guidance virtual fixture and a trajectory guidance cylinder based on constrained optimization and haptic feedback to assist the surgeon during a teleoperated knot-tying task. In [31], multiple control strategies for a stitching task were compared: telemanipulation, autonomous, shared control with orientation free, and shared control with orientation constrained.

In this work, we propose an optimization-based needle trajectory planning for a stitching task considering the suturing restrictions for an EES. The contributions of this paper can be summarized as follows.

• An online optimization-based needle trajectory generation method that is used as a reference for a smooth guidance virtual fixture.
• Constrained motion planning based on dual concurrent inverse kinematics (IK) solver that integrates a task-priority based IK and a nonlinear optimization based IK.
• Experimental comparison between the proposed method in a robot-assisted mode and an autonomous mode with the use of a conventional surgical tool.

2. Materials and Methods

2.1. System Overview

2.1.1. Robotic Surgical System

The proposed method is implemented on the system we have developed in [4] (see Figure 2a) composed of two 6-DOF industrial robot arms (VS-050, DENSO Corporation, Aichi, Japan), 4-DOF articulated forceps developed in [32], and user interfaces attached to the robot arms [4]. Each robotic unit has a total of 10-DOF, where 6-DOF are provided by the robot arm and 4-DOF are the contribution from the articulated forceps. The 4-DOF articulated forceps (see Figure 2b) comprises a shaft (diameter: 3.5 mm and length: 233 mm), a triangular-shaped gripper (length: 4 mm), and elastic elements to provide 3-DOF tip movement (bending in two directions and rotation around the axis) and the grasping function. The 4-DOF forceps tip motion is controlled by five DC motors (four motors for bending and grasping, and one motor for the rotation around the axis). The user interface comprises a vertical handle attached to a 6-axis force/torque sensor and an articulated serial link interface with a gripper handle (see Figure 2c). The vertical handle is used during the initial positioning and insertion/extraction of the articulated forceps, and the serial link interface is used for controlling the 4-DOF forceps and the robot arm during the suturing task. The gripper handle contains two push-buttons: one ‌above the handle (up-button), and the second one placed inside the gripper (gripper-button) (see Figure 2d). The upper push-button enables the free motion control of the forceps tip and the gripper push-button activates the robot-assisted stitching mode.

2.1.2. Description of the Stitching Workspace Frames

The coordinate frames for the robotic system are defined as shown in Figure 3, where $F_b$ represents the robot arm base frame, $F_{ee}$ is the forceps tip frame, $F_{n_{tip}}$ is the needle tip frame, $F_{n_{grasp}}$ is the frame associated to the needle grasping point, and $F_{tissue}$ corresponds to the tissue frame with the z-axis aligned to the tissue surface normal vector. We define relative coordinate frames as ${}^a{R}_b$ where b refers to the local frame and a refers to the reference frame. For notational convenience, a full pose (including position and orientation) in the frame a is denoted by ${}^{a}X$ as a combination of a position vector ${}^{a}p$ and a rotation matrix ${}^{a}R$. When no reference frame is specified, it is assumed to be expressed in the base frame $F_b$.

2.1.3. Algorithm Overview

The high-level schematic overview of the proposed algorithm is shown in Figure 4. The proposed algorithm assumes that the forceps have been inserted into the nasal cavity through a remote center of motion (RCM) placed over the patient’s nostril [4], which is kept fixed by the constrained motion planner during the entire execution of the stitching task. First, the surgeon can command the forceps tip pose through the user interface (see Figure 2c) in a teleoperation mode. An initial calibration procedure can be performed to define the desired entry and exit points, and to determine the orientation plane of the tissue surface, by positioning the forceps tip over the desired points and recording them in a YAML file. The calibration file can be either generated online or retrieved from the previously recorded ones at any time during the trajectory generation process. During the teleoperation mode, the needle pose can be freely controlled. The surgeon places the needle in a suitable initial position and activates the robot-assisted stitching sequence by pressing the gripper-button located in the user interface handle. The robotic system will automatically generate an optimal trajectory to be followed by the needle tip during the stitching task. The generated trajectory is stored as a CSV file that is synchronized with a simulator for trajectory visualization. The optimal trajectory defines the desired initial needle tip pose close to the entry point over the tissue. The robot reorients the needle and approaches to the desired initial needle pose following the trajectory defined by a linear interpolation for the translational motion and a spherical linear interpolation for the rotational motion. When the needle is expected to be in an initial contact with the tissue, the robot stops and the stitching guided mode is activated. Now the robotic system follows a shared control scheme, in which the robot ensures that the needle tip pose (position and orientation) follows the optimal trajectory generated by the proposed algorithm, while the surgeon is in charge of the control of the insertion speed and direction of the motion. In this way, the optimal trajectory works as a virtual guidance constraint for the surgeon’s command. In case of changes in the trajectory generation parameters while performing the stitching sequence (e.g., changes on the desired entry or exit positions), the needle can be retracted along the optimal trajectory until full extraction, and a new trajectory can be generated with the updated parameters. Moreover, the surgeon can return to the free needle motion at any time by pressing the up-button located in the user interface handle. Then, the guidance virtual constraint is disabled and the surgeon can freely manipulate the needle pose.

2.2. Online Optimization-Based Trajectory Generation

Schulman et al. [33] proposed a non-convex sequential optimization for bevel-tip needle steering based on a curvature-constrained kinematic model. Sen et al. [17] extended this concept for curved needles. Building on this model, we formulate the stitching needle trajectory generation problem as a non-convex constrained optimization problem to be solved by ${\ell }_1$ penalized trust-region based sequential optimizations. The notation used in this paper is listed in

Table 1. The list of notation used in this paper.

Notation	Description
${\mu }^{\left(k\right)}$	k-iteration penalty
${\delta }^{\left(k\right)}$	k-iteration trust region size
$u^{\left(k\right)}$	k-iteration control variable
${\eta }_r$	step rejection threshold
${\eta }_a$	step acceptance threshold
${\gamma }_s$	trust region shrinkage factor
${\gamma }_e$	trust region expansion factor
${\mu }_s$	penalty scaling factor
${ϵ}_m$	convergence threshold for merit
${ϵ}_u$	convergence threshold for control variable
${ϵ}_c$	constraint satisfaction threshold
$C_t$	needle pose at step t
N	number of trajectory steps
b	needle insertion distance at each step
${\xi }_t$	twist applied at step t
${\zeta }_t$	needle deviation at step t
$\overline{\zeta }$	max. needle deviation
$l_g$	needle length free for grasping
$l_n$	needle length
${\kappa }_n$	needle natural curvature
${ϵ}_{entry}$	entry position tolerance
${ϵ}_{exit}$	exit position tolerance

.

2.2.1. Sequential Convex Programming

The goal of a general nonlinear programming problem (NLP) is to find an optimal set of parameters $(u\in {\mathbb{R}}^n)$ that minimizes a given cost function $\left(f_{\left(u\right)}\right)$, while satisfying a set of equality $\left(h_{\left(u\right)}\right)$ and inequality $\left(g_{\left(u\right)}\right)$ constraints as

$$\begin{array}{cc}\hfill arg& \underset{u}{min}{f}_{\left(u\right)}\hfill \\ \hfill \mathrm{subject}\mathrm{to}& \\ \hfill & h_{i_{\left(u\right)}}=0i=0,\dots ,n_{eq}\hfill \\ \hfill & g_{j_{\left(u\right)}}\le 0j=0,\dots ,n_{ineq}.\hfill \end{array}$$

(1)

In general, NLPs are difficult to solve, but one of the most effective methods is the use of Sequential Convex Programming (SCP). The SCP method approximates a given NLP into convex subproblems (convex cost function, convex inequality constraints, and affine equality constraints) and sequentially update the solution estimation by solving the convex subproblem. NLP convexification can be performed by representing the cost function and constraints through Taylor series expansions around the current iteration u as

$$\begin{array}{cc}\hfill {\tilde{f}}_{(\Delta u)}& =f_{\left(u\right)}+\nabla f_{\left(u\right)}^{\top }\Delta u+\frac{1}{2}{\Delta u}^{\top }{\nabla }^2{f}_{\left(u\right)}\Delta u\hfill \\ \hfill {\tilde{h}}_{i_{(\Delta u)}}& =h_{i_{\left(u\right)}}+\nabla h_{i_{\left(u\right)}}^{\top }\Delta u\hfill \\ \hfill {\tilde{g}}_{j_{(\Delta u)}}& =g_{j_{\left(u\right)}}+\nabla g_{j_{\left(u\right)}}^{\top }\Delta u.\hfill \end{array}$$

(2)

The solution of the subproblem generates a step $\Delta u$ to update the current solution towards the optimal solution. However, infeasibility is a common problem during an SCP that can be caused by a poor convexification or initial solution guess. One possible solution is to move the infeasible constraints into the objective function as penalties, which will converge to zero and ensure feasibility. Compared with other common penalty functions, the use of ${\ell }_1$ penalty provides an exact representation of the deviation from the constraint and numerical stability [34]. To avoid complications because of its non-smooth characteristic, it is common to implement ${\ell }_1$ penalties by using slack variables ($v,w$ and t in Equation (3)) that do not modify the convexity of the original problem [35].

A trust-region method is also used to ensure global convergence. It is represented as a step boundary constraint ${\parallel \Delta u\parallel }_p\le \delta$, where $\delta$ is the trust region radius and p represents the p-norm used for the boundary conditions. The minimization problem is reformulated for a positive penalty $\mu$ as

$$\begin{array}{cc}\hfill arg\underset{\Delta u,v,w,t}{min}& {\tilde{f}}_{(\Delta u)}+\mu \left[\sum _{i=1}^{n_{eq}}(v_i+w_i)+\sum _{j=1}^{n_{ineq}}{t}_j\right]\hfill \\ \hfill \mathrm{subject}\mathrm{to}& \\ \hfill {\tilde{h}}_{i_{(\Delta u)}}& =v_i-w_i,i=0,\dots ,n_{eq}\hfill \\ \hfill {\tilde{g}}_{j_{(\Delta u)}}& \le t_i,j=0,\dots ,n_{ineq}\hfill \\ \hfill v,w,t& \ge 0\hfill \\ \hfill {∥\Delta u∥}_{\infty }& \le \delta .\hfill \end{array}$$

(3)

To determine the acceptance of the current step, the SCP method uses the actual-to-predicted cost reduction ratio $\rho$, defined by

$$\begin{array}{c}\hfill \rho =\frac{{\varphi }_{1_{(u,\mu )}}-{\varphi }_{1_{(u+\Delta u,\mu )}}}{q_{\left(0\right)}-q_{(\Delta u)}}\end{array}$$

(4)

where the ${\ell }_1$ merit function ${\varphi }_{1_{(u,\mu )}}$ is given as

$${\varphi }_{1_{(u,\mu )}}=f_{\left(u\right)}+\mu \left[\sum _{i=0}^{n_{eq}}\left|h_{i_{\left(u\right)}}\right|+\sum _{j=0}^{n_{ineq}}{\left[g_{j_{\left(u\right)}}\right]}^{+}\right]$$

(5)

and the approximation of the objective function $q_{(\Delta u,\mu )}$ is given as

$$\begin{array}{cc}\hfill q_{(\Delta u,\mu )}=& f_{\left(u\right)}+\nabla f_{\left(u\right)}^{\top }\Delta u+\frac{1}{2}{\Delta u}^{\top }{H}_{\left(u\right)}\Delta u+\hfill \\ & \mu \left[\sum _{i=0}^{n_{eq}}|h_{i_{\left(u\right)}}+\nabla h_{i_{\left(u\right)}}^{\top }\Delta u|+\sum _{j=0}^{n_{ineq}}{[g_{j_{\left(u\right)}}+\nabla g_{j_{\left(u\right)}}^{\top }\Delta u]}^{+}\right]\hfill \end{array}$$

(6)

where $H_{\left(u\right)}$ represents a numerical approximation of the Hessian of the cost function ${\nabla }^2{f}_{\left(u\right)}$, and ${\left[y\right]}^{+}=max(y,0)$. If $q_{(\Delta u,\mu )}$ is an accurate representation of the ${\ell }_1$ merit function $(\rho \ge {\eta }_a)$, the trust region $\delta$ is expanded by a factor of ${\gamma }_e$ and the control variable $\Delta u$ is updated. On the hand, if $\delta$ is rejected $(\rho \le {\eta }_r)$, $\delta$ is shrunk by a factor of ${\gamma }_s$. The sequential iteration stops when the convergence criteria are satisfied or the maximum number of iterations is exceeded. The outline of ${\ell }_1$ penalized trust region-based sequential optimization algorithm is shown in Algorithm 1.

Algorithm 1:${\ell }_1$ penalty trust region based sequential optimization.

2.2.2. Problem Definition

The stitching needle trajectory is discretized into N time intervals and represented as a set of needle tip poses at each time step $C:\{C_0,C_1,...,C_{N-1}\}$, where each pose is defined as a homogeneous transformation matrix $C_t=\left[\begin{array}{cc}{R}_t& p_t\\ \mathbf{0}& 1\end{array}\right]\in SE\left(3\right)$. The stitching trajectory planning problem can be defined as follows.

Input: The desired entry and exit points $p_{entry},p_{exit}\in {\mathbb{R}}^3$ are given by the surgeon during the calibration process. The needle size and shape ($l_n$, ${\kappa }_n$), the suture depth $z_{depth}$, and the maximum needle curvature allowed $\overline{\zeta }$ are also provided.

Output: The set of desired needle tip poses $C_d:\{C_{d_0},C_{d_1},\dots ,C_{d_{N-1}}\}$.

Assumptions: It is assumed that the gripper holds the needle firmly so that needle deviations during the stitching task do not occur. We consider a flexible tissue where the friction forces produced during the tissue penetration can be neglected.

2.2.3. Optimization Model

The stitching needle trajectory generation problem is formulated as a constrained optimization problem as

$$\begin{array}{c}\hfill arg\underset{\Delta u}{min}{\alpha }_{1}Cos{t}_L+{\alpha }_{2}Cos{t}_{O_{entry}}+{\alpha }_{3}Cos{t}_{O_{exit}}\\ \mathrm{subject}\mathrm{to}\hfill & \end{array}$$

(7)

$$log{\left(C_{t+1}{(exp\left({\xi }_t\right)C_t)}^{-1}\right)}^{\vee }={\mathbf{0}}_6$$

(8)

$${(p_0-p_{entry})}^{\top }(p_0-p_{entry})\le {ϵ}_{entry}^2$$

(9)

$${(p_{N-1}-p_{exit})}^{\top }(p_{N-1}-p_{exit})\le {ϵ}_{exit}^2$$

(10)

$$Nb+l_g\le l_n$$

(11)

$$C_{N/2}\le z_{depth}$$

(12)

$${\zeta }_t-\overline{\zeta }\le 0,t=0,\dots ,N-1$$

(13)

$$-{\zeta }_t-\overline{\zeta }\le 0,t=0,\dots ,N-1$$

(14)

$${\zeta }_{t+1}^2-{\zeta }_t^2\le 0,t=0,\dots ,N-2$$

(15)

Costs (Equation (7)): 

The objective of $Cos{t}_L$ penalizes longer trajectories to reduce tissue trauma. Additionally, needle deviation from an orthogonal pose with respect to the tissue surface is also penalized during the needle insertion with $Cos{t}_{O_{entry}}$ and extraction $Cos{t}_{O_{exit}}$ to facilitate tissue penetration. The concrete forms of these cost functions are given as

$$Cos{t}_{L}Nb$$

(16)

$$Cos{t}_{O_{entry}}{(log{\left(R_0{R}_{entry}^{\top }\right)}^{\vee })}^{\top }log{\left(R_0{R}_{entry}^{\top }\right)}^{\vee }$$

(17)

$$Cos{t}_{O_{exit}}{(log{\left(R_{N-1}{R}_{exit}^{\top }\right)}^{\vee })}^{\top }log{\left(R_{N-1}{R}_{exit}^{\top }\right)}^{\vee }.$$

(18)

Stitching kinematic constraints (Equation (8)): 

We use the curvature constrained kinematic model proposed by Sen et al. [17] for curved needles. At each time step, the needle is inserted following a fixed length b with a curvature ${\kappa }_n+{\zeta }_t$, where ${\kappa }_n$ is the needle natural curvature and ${\zeta }_t$ are local curvature changes applied for needle reorientation at the time step t. The transformation between consecutive needle poses can be represented as a twist ${}^{n_{tip}}{\xi }_t\left[0,0,b,-b({\kappa }_n+{\zeta }_t),0,0\right]$, composed of a translational motion b along the needle tip tangent vector, and a rotational motion $-b({\kappa }_n+{\zeta }_t)$ along the normal vector to the needle’s plane. The corresponding twist in the base frame can then be computed as ${\xi }_t={A{d}_{n_{tip}}}^{n_{tip}}{\xi }_t$, with the adjoint matrix defined as $A{d}_{n_{tip}}=\left[\begin{array}{cc}{R}_{n_{tip}}& {\left[p_{n_{tip}}\right]}_{\times }{R}_{n_{tip}}\\ \mathbf{0}& R_{n_{tip}}\end{array}\right]$.

The lie algebra exponential and logarithmic mapping in $SE\left(3\right)$ [36] are used to represent the kinematic constraint $C_{t+1}=exp\left({\xi }_t\right)C_t$ at each time step.

Desired Entry/Exit Port Constraints (Equations (9) and (10)): 

The generated trajectory entry/exit points should be within a tolerance ${ϵ}_{entry},{ϵ}_{exit}$ from the desired entry/exit point, respectively.

Needle Constraints (Equation (11)): 

The total trajectory length including the distance for the forceps to hold the needle $Nb+l_g$ should be less than the needle length $l_n$.

Suture depth constraint (Equation (12)): 

The trajectory should have a minimum depth $z_{depth}$ to ensure a proper tissue penetration.

Needle reorientation constraints (Equations (13)–(15)): 

Small curvature changes ${\zeta }_t$ are allowed from the needle natural curvature ${\kappa }_n$, but are bounded by $\overline{\zeta }$ and limited to be monotonically decreasing.

2.3. Constrained Motion Planning

EES constrains the motion of the surgical tools along the nostril and nasal cavity. It is important to ensure safe tool manipulation by implementing a remote center of motion constraint over the nostril. We propose a constrained motion planning based on a guidance virtual fixture along the optimally generated needle trajectory, and a concurrent constrained inverse kinematic solver implementation based on a task-priority IK method and a nonlinear programming IK method. Figure 5 depicts a block diagram of the proposed constrained motion control. Given a surgeon’s pose command through the serial-link interface $X_h$, the scaled desired end-effector pose $X_{des}$ is computed. A guidance virtual fixture constrains the needle tip motion along a continuous optimal trajectory and generates the constrained end-effector pose $X_c$. Then, a concurrent IK solver performs a simultaneous prioritized Jacobian-based inverse kinematics and a constrained nonlinear optimization inverse kinematics. The task-priority IK can find a solution faster, but joint limits and higher priority tasks may produce convergence failures. On the other hand, nonlinear optimization takes longer to compute, but can better handle joint limits and additional constraints. When one of them finds an acceptable solution, the other IK solver is stopped, and the computed joint command $q_d$ is distributed between the robot arm ($q_{d_{arm}}$) and the articulated forceps ($q_{d_{forceps}}$).

2.3.1. Guidance Virtual Fixture

A continuous and differentiable position and orientation trajectory $c_{\left(s\right)}=\{c_{p_{\left(s\right)}},c_{o_{\left(s\right)}}\}$ is generated by cubic b-spline interpolation of the control points from the optimal stitching needle trajectory obtained in Section 2.2 (see Figure 6) as

$$\begin{array}{c}\hfill c_{p_{\left(s\right)}}=p_{d_0}+\sum _{i=0}^{N-1}(p_{d_i}-p_{d_0}){\mathbf{B}}_{{i,k+1}_{\left(s\right)}},s\in [0,1]\\ \hfill c_{o_{\left(s\right)}}=exp\left(\sum _{i=0}^{N-1}log{\left(R_{d_i}{R}_{d_0}^{\top }\right)}^{\vee }{\mathbf{B}}_{{i,k+1}_{\left(s\right)}}\right)R_{d_0},s\in [0,1]\end{array}$$

(19)

$$\begin{gathered} {\mathbf{B}}_{{i,1}_{\left(s\right)}}=\left\{\begin{array}{cc}1\hfill & {\lambda }_i\le t<{\lambda }_{i+1}\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right. \\ {\mathbf{B}}_{{i,k+1}_{\left(s\right)}}=\frac{s-{\lambda }_i}{{\lambda }_{i+k}-{\lambda }_i}{\mathbf{B}}_{{i,k}_{\left(s\right)}}+\frac{{\lambda }_{i+k+1}-s}{{\lambda }_{i+k+1}-{\lambda }_{i+1}}{\mathbf{B}}_{{i+1,k}_{\left(s\right)}} \end{gathered}$$

(20)

where ${\mathbf{B}}_{{i,k+1}_{\left(s\right)}}$ are the b-spline basis functions and ${\lambda }_i\in [0,1]$ are the uniformly distributed knots.

The continuous trajectory $c_{\left(s\right)}$ is then used as reference curve for a hard virtual fixture [37]. The desired position $p_{des}$ obtained from the user interface command is projected onto the normalized tangent direction of the reference curve $u_{c_{\left(s_t\right)}}=\frac{{\delta }_{c_{\left(s_t\right)}}}{\parallel {\delta }_{c_{\left(s_t\right)}}\parallel }$ with ${\delta }_{c_{\left(s_t\right)}}=\frac{d}{ds}{c}_{p_{\left(s\right)}}{|}_{s=s_t}$, and scaled by $K_c$ to update the interpolation parameter $s_t$ as

$$s_{t+1}=s_t+K_c{\Delta p_{des}}^{\top }{u}_{c_{\left(s_t\right)}}.$$

(21)

The constrained pose $X_c$ is then computed by evaluating $c_{\left(s_{t+1}\right)}$.

2.3.2. Task-Priority Inverse Kinematics

The RCM constraint can be defined as a higher priority task in a hierarchical null-space projection method [38]. Based on this, Azimian et al. [39] proposed an RCM implementation by projecting the end-effector tracking task over the null space of the RCM task. Sandoval et al. [40] proposed a similar RCM implementation for torque-controlled redundant manipulator, but based on the RCM distance to a trocar that does not depend on the insertion velocity. We define the RCM constraint problem also using a prioritized Jacobian method but defining the RCM position as the nostril projection onto the forceps shaft.

Figure 7 depicts the variables used in the RCM Jacobian computation. The unit vector $\widehat{p_s}=\frac{p_s}{\parallel p_s\parallel }$ with $p_s=p_{j_8}-p_{j_7}$ represents the direction of the forceps shaft and $p_r=p_{nostril}-p_{j_7}$ is the vector between the nostril and the base of the forceps shaft. We can compute the RCM position by

$$p_{rcm}=p_{j_7}+p_r^{\top }\widehat{p_s}\widehat{p_s}.$$

(22)

Then we can differentiate $p_{rcm}$:

$$\frac{\partial p_{rcm}}{\partial q}=J_{j_7}+\widehat{p_s}\frac{\partial p_r^{\top }\widehat{p_s}}{\partial q}+p_r^{\top }\widehat{p_s}\frac{\partial \widehat{p_s}}{\partial q}.$$

(23)

By matrix differentiation, we obtain the RCM Jacobian $J_{rcm}$ as

$$J_{rcm}=\frac{\partial p_{rcm}}{\partial q}=\left(\mathbf{I}-\widehat{p_s}{\widehat{p_s}}^{\top }\right)J_{j_7}+\left(\widehat{p_s}{p}_r^{\top }+p_r^{\top }\widehat{p_s}\mathbf{I}\right)\frac{\partial \widehat{p_s}}{\partial q}$$

(24)

where

$$\frac{\partial \widehat{p_s}}{\partial q}=\frac{1}{\parallel p_s\parallel }\left(\mathbf{I}-\widehat{p_s}{\widehat{p_s}}^{\top }\right)\left(J_{j_8}-J_{j_7}\right).$$

(25)

We assume the constrained motion planning is initialized with the forceps shaft over the nostril, so that the RCM Jacobian velocity must be zero ${\dot{p}}_{rcm}=0$. By projecting the end-effector tracking task over the null space of the RCM task we can compute the joint velocity command as

$$\dot{q}=\left(\mathbf{I}-J_{rcm}^{†}{J}_{rcm}\right)J_{ee}^{†}{\dot{e}}_{ee}$$

(26)

2.3.3. Nonlinear Optimization Inverse Kinematics

The previous prioritized Jacobian-based IK can be susceptible to convergence failure when close to joint limits. Beeson et al. [41] proposed a concurrent inverse kinematics library for generic inverse kinematic (TRAC-IK) that addresses the convergence problem by simultaneously executing a nonlinear optimization based IK solver. In a similar way, we propose a constrained nonlinear optimization problem to be executed simultaneously with our previous task-priority IK solver.

We define the constrained nonlinear optimization problem as

$$\begin{array}{cc}\hfill arg\underset{\Delta q}{min}& {\beta }_1{e}_p^{\top }{e}_p+{\beta }_2{e}_o^{\top }{e}_o+{\beta }_3{e}_{rcm}^{\top }{e}_{rcm}+{\beta }_4{\Delta q}^{\top }\Delta q\hfill \\ \hfill & s.t.\underline{q}\le q+\Delta q\le \overline{q}\hfill \end{array}$$

(27)

where ${\beta }_n$ are the positive coefficients, $e_{p_{(\Delta q)}}=p_{des}-p_{(\Delta q)}$ is the Cartesian position end-effector error, $e_{o_{(\Delta q)}}=log{\left(R_{des}{R}_{(\Delta q)}^{\top }\right)}^{\vee }$ is the orientation error expressed using the logarithmic mapping, and $e_{rc{m}_{(\Delta q)}}=p_{nostril}-p_{{rcm}_{(\Delta q)}}$ is the RCM position error. By increasing the value of ${\beta }_3$ over the other coefficients, we can prioritize the minimization of an RCM error.

3. Experiments and Discussion

3.1. Implementation Details

The proposed methodology is implemented on a 2.4 GHz Core i7 CONTEC computer running Linux (Ubuntu 16.04, Canonical) with real-time patches (RT-PREEMPT) and the Robot Operating System (ROS) framework on top of it. The control loop runs at a rate of 125 Hz in synchronous mode with the industrial robot arm controller. In each cycle, the current robot pose is updated, and then the target robot pose is computed and sent to the robot arm and the articulated forceps controllers [4]. The rigid body kinematic is implemented by using the template library Pinocchio [42], and the optimization framework is built on CasADi [43], which allows C-code generation to speed up the optimization process. The quadratic programming solver used for the stitching needle trajectory generation is the Gurobi Optimizer [44]. For the nonlinear optimization based IK, we use the IPOPT solver [45].

3.2. Simulation Environment

To evaluate the performance of our proposed scheme, we initially validate the trajectory generation and constrained motion control in a simulation environment developed on CoppeliaSim EDU V4.1 [46], in which we model the nostril constraint and simulate the robot kinematics. Figure 8a shows the simulation environment created for system implementation. In Figure 8b, the desired entry point and exit points are represented with a blue and red sphere, respectively, with a 1.5 mm tolerance radius. An initial needle trajectory is provided as half-turn trajectory that follows the natural needle curvature and is shown as a red curve. The blue curve corresponds to the optimal trajectory obtained from the sequential convex optimization, and the green curve corresponds to the forceps tip reference trajectory. In simulation conditions, the stitching needle trajectory generation takes approximately 500 ms and the maximum RCM deviation is 1 mm.

3.3. Stitching Experiments

We conduct a stitching experiment to validate the performance our system in a physically realistic environment, and compare three stitching modes:

• Manual: by using conventional forceps for endoscopic endonasal surgery.
• Robot-assisted: by using the proposed method. The system generates the optimal trajectory and constrains the needle pose.
• Autonomous: the robot starts in a fixed initial pose and executes the stitching task without human assistance.

The experimental setup is presented in Figure 9. It comprises a 3D printed nose model and an acrylic platform for the stitching testbed. A six-axis force/torque sensor (Nano17, ATI Industrial Automation, North Carolina, USA) is placed behind the elastic tissue to measure the compression and shear forces applied to the tissue during the stitching task (see Figure 9b). We assumed the compression forces to be perpendicular to the tissue surface and aligned to the z-axis of the force sensor, whereas the shear forces are tangential to the tissue surface. For the force analysis, we use the root sum square (RSS) of the resulting forces given by the square root of the sum of squares of the forces in each direction measured by the force sensor (Equation (28)). The forces are sampled at a frequency of 1 kHz and the testbed’s weight related forces are subtracted.

$$\mathrm{RSS}\mathrm{Force}=\sqrt{f_x^2+f_y^2+f_z^2}$$

(28)

A magnetic motion capture system (Aurora, Northern Digital Inc., Ontario, Canada) is used to record the motions of the surgical instrument for both the manual and robot operation. A rigid endoscope (${30}^{\circ }$, 2.7 mm diameter) is used for visualization and targeting the area of interest.

The participants were asked to perform a stitching task. The task consisted of grasping a 6-0 surgical needle and puncture an elastic tissue, on which the desired entry/exit point were marked (see Figure 9d). In the manual stitching mode, the participants were asked to follow a three-step procedure: (1) puncture the right tissue from the marked point, (2) penetrate the right tissue until the needle tip is placed under the left tissue, and (3) puncture the left tissue from underneath. In the robot-assisted stitching mode, the participants were asked to position the needle in a suitable initial position and command the needle insertion speed (0–0.5 mm/s) and direction through the user interface, while the robot constrained the needle pose along the reference trajectory and kept the RCM constraints. In both cases, assistance from the surgical tool on the left hand was also allowed to tighten the left tissue, but no needle manipulation assistance was allowed. The autonomous stitching mode was performed without human assistance, and the needle insertion speed was set at the maximum allowed (0.5 mm/s). In all the stitching modes, the trial starts with the needle grasped by the surgical tool and placed about 2 cm above the center frame of the tissue.

Seven subjects between the ages of 25 and 35 who had no previous surgical training took part in this experiment. Before the experiment, each participant was instructed about the procedure and practiced for up to 10 min until they were familiarized with the operation. All subjects were given informed consent before they participated in the study. This study was approved by the Ethical Research Committee of Nagoya University.

We evaluate participants’ performance using the following metrics:

• Task completion time (s): the total time in which participants performed the task.

  $$\mathrm{Completion}\mathrm{Time}:=t_{end}-t_{start}$$

  (29)
• Success ratio (%): the percentage of succeed stitching from the total number of attempts.

  $$\mathrm{Success}\mathrm{ratio}:=\frac{\mathrm{number}\mathrm{of}\mathrm{success}\mathrm{insertions}}{\mathrm{number}\mathrm{of}\mathrm{attempts}}$$

  (30)
• Entry point error (mm): the Euclidean distance between the desired entry point and the actual entry point.

  $$\mathrm{Entry}\mathrm{point}\mathrm{error}:= \parallel p_{{entry}_{des}}-p_{{entry}_{act}}\parallel$$

  (31)
• Exit point error (mm): the Euclidean distance between the desired exit point and the actual exit point.

  $$\mathrm{Exit}\mathrm{point}\mathrm{error}:= \parallel p_{{exit}_{des}}-p_{{exit}_{act}}\parallel$$

  (32)
• Maximum RCM error (mm): the maximum Euclidean distance between the RCM position and the center of the nostril.

  $$\mathrm{Max}.\mathrm{RCM}\mathrm{error}:=max\left(∥p_{rcm}-p_{nostril}∥\right)$$

  (33)
• Maximum RSS force (N): the maximum RSS force applied on the tissue.

  $$\mathrm{Maximum}\mathrm{RSS}\mathrm{force}:=max\left(\right|f\left|\right)$$

  (34)
• Distribution of RSS force samples: graphical representation of the number of RSS force measurements within equally distributed force intervals with respect to the total number of RSS force samples obtained during the stitching task.

Figure 10 summarizes the results obtained for the three stitching modes (manual, robot-assisted, and autonomous). The number of trials and success ratio per subject are presented in

Table 2. Success ratio for manual operation.

Subject	# of Trials	# of Success	Success Ratio (%)
1	12	4	33.3
2	15	4	26.7
3	12	4	33.3
4	8	2	25.0
5	15	2	13.3
6	12	1	8.3
7	8	3	37.5
Total	82	20
Mean Success Ratio (%)			25.4

,

Table 3. Success ratio for robot-assisted operation.

Subject	# of Trials	# of Success	Success Ratio (%)
1	12	5	41.7
2	10	5	50.0
3	8	5	62.5
4	8	5	62.5
5	7	4	57.1
6	6	6	100.0
7	6	5	83.3
Total	57	35
Mean Success Ratio (%)			65.3

and

Table 4. Success ratio for autonomous operation.

# of Trials	# of Success	Success Ratio (%)
16	7	43.8

. Note that the task completion time, entry point error, and exit point error are computed only for the successful trials. Figure 10a depicts the task completion time. The reliability of using the robotic system is evident as the standard deviation is greatly reduced compared with the manual operation. The median time for the robot-assisted is about 25% longer than that of the autonomous mode. However, there was no significant improvement with respect to the manual operation. Figure 10b shows the success ratio of the stitching task, where the robot-assisted mode achieved the success ratio of 65.3%, much higher than that of the autonomous mode (43.8%) and the manual operation (25.4%). Needle–tissue interaction forces arising at the needle tip may induce undesired changes in the needle orientation, which could result in a failed attempt to penetrate the tissue. Unlike the autonomous mode where the speed is fixed, the robot-assisted mode allows the user to freely control the insertion speed and reduce the effect of such interaction forces. Moreover, in the robot-assisted mode, the user can also retract the needle along the optimal trajectory if proper penetration is not achieved (e.g., because of tissue deformation). The needle can be pulled back and reinserted without the need of recomputing the optimal trajectory as long as the needle orientation with respect to the forceps tip has not changed. As a consequence, the robot-assisted mode can require additional time to complete the task compared with the autonomous method. Therefore, a trade-off exists between assuring proper tissue penetration and the stitching completion time. Figure 10c,d depicts the entry/exit point error. The autonomous mode showed the smaller entry point error among the three cases, but no improvement in exit point error can be observed when using the robotic system. This is expected, as the stitching trajectory generation algorithm will try to keep the changes in the curvature bounded to facilitate tissue penetration and reduce tissue deformation, which can produce a larger deviation from the desired exit point. By increasing the allowed range of curvature changes, the exit error is expected to be reduced, but higher forces might be generated.

We evaluate the constrained motion planning performance by the maximum RCM error defined as the minimum distance between the nostril and forceps shaft. The RCM constraint implemented for the robotic operation (including robot-assisted and autonomous modes) was able to keep the maximum RCM error below 4.2 mm as shown in Figure 11a. Figure 11b illustrates an example of the forceps displacement from the center of the nostril during task execution by one of the participants. The blue line represents the position of the forceps shaft intersection with the nostril plane for the manual operation, and the red line represents the intersection trajectory obtained when using the robotic system.

We also measured the forces applied during the stitching task to evaluate the possibility of potential tissue trauma.

Table 5. Maximum RSS force measured during the stitching task.

	Mean (N)	SD (N)
Manual operation	0.349	0.178
Robotic system	0.096	0.046

summarizes the maximum RSS force applied. The use of the robotic system demonstrates a reduction of approximately 70% of the maximum force compared with manual operation. During the manual operation, frequent undesired collision of the forceps on the tissue occurred because of the lack of the depth information by the 2D endoscopic visualization. Figure 12 shows the distribution of the RSS force samples contained within the force intervals of 0.005 N. The larger the number of bins close to 0 N, the smaller amount of continuous force applied to the tissue (less potential damage). With the use of the robot system (robot-assisted and autonomous modes), approximately 90% of the RSS force samples are within the range of [0, 0.05] N, while for the manual operation, the 90% of the samples are found within the range of [0, 0.15] N. The use of the robotic system reduced effectively the range of forces applied over the tissue in approximately 66%.

Figure 13a depicts the needle positioned into the stitching workspace. An example of the complete stitching sequence achieved with the robot-assisted mode is shown in Figure 13b–g, with a total duration of 58 s. The user initially placed the needle close to the entry point (t = 0, Figure 13b) and activates the robot-assisted stitching sequence. An optimal trajectory is generated with an initial needle tip pose. The robot reorients the needle (Figure 13c) and approaches to the entry point located over the tissue (Figure 13d). Under the user control, the needle penetrates the right tissue (Figure 13e), commands the needle under the tissue toward the exit point (Figure 13f), and finally penetrates the left tissue from underneath (Figure 13g).

4. Conclusions

In this paper, we have proposed an optimization-based trajectory generation method for robot-assisted stitching based on a sequential convex optimization to find the optimal needle trajectory and a dual concurrent IK solver to constrain the surgical tool around the nostril. The trajectory generation algorithm was capable of generating optimal trajectory subject to the suturing requirements. The surgeon can regenerate the trajectory online through the user interface. Integrating two constrained IK methods allowed us to achieve the convergence of the solution and meet time constraints. We compared the performance of our system with a manual stitching operation and an autonomous operation. The results showed a noticeable improvement in the stitching success ratio and reduction of the force interaction with the tissue. In the robot-assisted mode, the surgeon can control the initial positioning and insertion speed, which avoids potential rotations in the needle and ensure a proper initial tissue penetration. However, the task completion time did not significantly improve compared with the manual mode. Although the robot task completion time could be reduced by increasing the maximum speed allowed for the needle, it might reduce the insertion success. In addition, the interaction force distribution showed that the use of robot reduced about 70% of the forces showed in a manual stitching task. This is important to avoid any potential tissue trauma. The results also showed that the proposed system could safely constrain the motion of the articulated multi-DOF forceps for an endoscopic endonasal surgical stitching. Future work will focus on the use of visual feedback for dynamic generation of virtual constraints and extend the proposed method into other surgical tasks such as knot tying.