Epipolar Geometry-Based Visual Servoing of Soft Endoscopic Manipulator for Transoral Laser Ablation

Abstract

Soft manipulators integrated with optical laser fibers offer new opportunities for endoscopic noncontact laser surgery. To achieve precise laser projection in a confined workspace and avoid damage, a controller with high accuracy and stability is required. An effective way is to close the control loop. Therefore, a visual servo controller that allows automatic laser spot steering using soft manipulators is proposed. An epipolar geometry model is established to acquire the inverse transition mapping from image to actuation. With this mapping, a feedback controller is derived without prior information of tissue surface geometry. Experimental validation demonstrates accurate path following using a magnetic resonance-safe manipulator, with root-mean-square (RMS) tracking errors <4 pixels (140 μm) in the camera view. This is maintained even throughout 70 repeated cycles, with a maximum tracking error <11.32 pixels (≈396.2 μm). The controller enables laser spot following of the mouse cursor with an RMS tracking error of 3.97 pixels (≈180.5 μm). Ex vivo tissue ablation tests are conducted to validate the laser ablation pattern, with an RMS tracking error of about 5.75 pixels (≈201.3 μm). A laser ablation test is conducted under magnetic resonance imaging (MRI) to validate the feasibility of MRI-guided laser surgery.

Introduction

With the development of minimally invasive surgical techniques^{[ 1–3 ]} and medical lasers, transoral laser microsurgery (TLM) has attracted growing attention in the treatment of head-and-neck cancers (HNCs) (Figure  1a–c).^{[ 4 ]} TLM performs laser beam projection transorally for tumor resection, eliminating external incisions as in conventional open-neck surgeries.^{[ 5 ]} The high-power laser beam needs to be projected at small size (≈250 μm,^{[ 6 ]}) and manipulated precisely (<1 mm,^{[ 7 ]}) in the narrow laryngeal area (≈20 × 20 mm²). It is demanding in terms of accuracy due to the high risk of damage to the vocal muscles.^{[ 8 ]} This also challenges the development of laser steering devices and corresponding control schemes.

Figure 1. Current setup of TLM. a) Larynx cancer near the vocal cords[58]; b) nasopharyngeal cancer[59]; c) pharynx cancer in the tonsil wall[60]; d) setup of current transoral laser surgery[50]; e) tumor invasion depth measured on an anatomical MR image of tongue base[22]; f) temperature map acquired by MR thermometry used for high intensity focused ultrasound ablation.[61].

In conventional TLM setups, surgeons manually operated a joystick to manipulate a beam-splitter mirror that projects the laser beam at the targeted lesions.^{[ 5 ]} However, such manual manipulation requires surgeons to master high psychomotor skills to achieve dexterous hand–eye–foot coordination. Recently, the motorization of external manipulators was introduced to improve the laser aiming precision and efficiency.^{[ 9 ]} Motorized laser scanners (e.g., Lumenis AcuBlade, KLS Martin SoftScan, and DEKA HiScan) allowing preprogrammed scan patterns are commercially available, providing improved laser incision quality. However, these systems still need surgeons to position the pattern using a manual micromanipulator. New motorized micromanipulators were proposed to enable the incorporation of a stylus-based machine-surgeon interface to release surgeons from looking through the microscope during laser manipulation, thus offering improved ergonomics.^{[ 10,11 ]} However, these laryngeal laser surgery setups are mainly based on the design of straight-line laser beam projection from an external manipulator to the surgical site (Figure 1d). To ensure sufficient exposure of tumors to the laser beam, patients may need to maintain high extension of the neck. Moreover, the long working distance (400 mm) causes a leveraging effect on the laser beam, which can amplify inaccuracies in laser steering.

Recently, the advance of fiber optics technology brings the opportunity to guide medical lasers toward target tumors located in the deep laryngeal region, which promotes the development of endoscopic laser manipulator. Renevier et al.^{[ 12 ]} designed a two degrees-of-freedom (DOF) actuated micromirror based on a micro-electromechanical system, which could be integrated into an endoscopic tip. The laser scanner achieved a path-following accuracy of 80.50 μm. However, the inclination of reflecting mirrors may result in an elliptical laser spot. Kundrat et al.^{[ 13 ]} further extended this work to a customized flexible robot for laser phonomicrosurgery. Acemoglu et al.^{[ 14 ]} proposed a flexible robotic laser scanner, which used four electromagnetic coils to manipulate a laser fiber. The robot enabled an accuracy of 39 μm in trajectory-following tasks, while the projection workspace of laser spot was restricted to a 5 × 5 mm square. A 5 cm long rigid laser scalpel using the Risley prism beam steering mechanism was designed, which could be mounted on a modified laryngoscope.^{[ 15 ]} An extensible continuum robot has been investigated to achieve focal adaptation with an accuracy of 0.25 mm for noncontacted laser surgery.^{[ 16 ]} Zhao et al.^{[ 17 ]} proposed a cable-driven parallel mechanism to achieve 5-DoF manipulation of an optical fiber tip with an accuracy of 0.054 ± 0.028 mm, but it has to maintain a close distance to tissue, e.g., 1 mm for incision and 2–5 mm for ablation and hemostasis. Even assisted by robots to manipulate laser beam, surgeons still heavily rely on their experience to estimate the laser ablation depth beyond the crucial artery/nerve while dissecting these tumors. Typically, the surgeon performs frozen section analysis (FSA) during the surgery, to determine the completeness of cancerous cell resection.^{[ 18 ]} However, FSA can only evaluate the gross tumor volumes that are resected, instead of the unresected region left in the oral, nasopharyngeal (ONP) cavity.

Recently, online estimation of tissue temperature^{[ 19 ]} and ablation depth^{[ 20,21 ]} based on modeling the laser-tissue thermal interaction were investigated. However, this method requires collecting actual data from the laser ablation process on ex vivo tissues, which could be inaccurate for in vivo applications. An alternative is to introduce intraoperative (intra-op) sensing modality to monitor the laser ablation process. Magnetic resonance imaging (MRI) can provide superior image contrast of ONP soft tissues.^{[ 22 ]} Its capability of intra-op imaging even enables real-time assessment of tumor ablation progress (Figure 1e). Intra-op MRI also offers powerful tools such as magnetic resonance (MR) thermometry^{[ 23 ]} which can visualize the heat-affected zone and the ablated depth of the tumor (Figure 1f). It can play a crucial role in providing thermal feedback to control the laser spot and titrate the thermal dose during surgery. During the laser ablation, real-time (up to subsecond^{[ 24 ]}) MR thermometry can be conducted to measure the temperature diffusion in tissues, offering an online monitoring of tumor ablation margin. It can even be overlaid on the camera view to alert the overheating to surroundings. After each ablation run, the intra-op anatomical MR scan can be performed for in situ assessment of the completeness of lesion cutting/ablation.

Accrediting to the high contrast of soft tissues, intra-op MRI guidance has been widely investigated in various applications, such as cardiovascular intervention,^{[ 25 ]} biopsy,^{[ 26 ]} and neurosurgery.^{[ 27–29 ]} Open MRI was studied to provide additional visualization of liver vessels and tumor margins in laparoscopic liver resection.^{[ 30 ]} Utilizing its unique capability of temperature measurement, MRI-guided laser ablation has been applied in treatment of tumors in liver,^{[ 31 ]} prostate,^{[ 32 ]} and brain,^{[ 33 ]} etc. However, most of existing MRI-guided laser surgery are based on contact laser, without the need to frequently manipulate the laser beam. Recently, we proposed a soft robotic laser steering manipulator (Ø12 × 100 mm) that meets the requirement of MR safety,^{[ 34 ]} which was validated in cadaveric trial. Soft manipulators^{[ 35–37 ]} driven by pressurized fluid flow can be used to address this challenge. Hydraulic actuation can be employed to ensure intrinsic MR safety as well as fast robot response.^{[ 38 ]} Optical laser fibers can be housed inside the soft robot, which provides a wide workspace of laser spot with a small robot size. Soft robots are low cost, disposable, and easy sterilized, making them a viable choice in single-use endoscopic applications. Furthermore, soft robotic manipulators can be fabricated using MR-safe materials,^{[ 39 ]} such as polymers,^{[ 40 ]} and allow rapid prototyping with customizable, patient-specific designs.

However, the fabrication uncertainty of soft robots and the nonlinearity of their actuation poses a challenge to precise motion control.^{[ 41 ]} In conventional robotics, the addition of sensors such as joint encoders and electromagnetic trackers can allow reliable closed-loop control. However, the harsh MRI environment severely limits available sensing modalities, which is further limited by the lack of sensors mechanically compatible with soft structures. Therefore, we aim to close the control loop with the use of an MR-compatible fiber optic camera. Visual servoing is a robotic control technique that utilizes this camera feedback. McCandless et al.^{[ 42 ]} developed a computer vision algorithm for navigating a soft robotic bronchoscope to target regions in lung surgery. Wang et al.^{[ 43 ]} proposed a visual servo controller for a tendon-driven soft manipulator based on constant curvature robot kinematics. The controller has also been further extended to adapt with constrained environment.^{[ 44 ]} However, the kinematic model of the eye-in-hand platform cannot be directly migrated to the laser projection case. Our previous work proposed an adaptive eye-in-hand visual servo control framework using a machine learning technique.^{[ 45 ]} However, the controller needs to be initialized based on acquired operation data, which is not available in medical application. Although the training data can be collected in an ex vivo scene and online updated,^{[ 46 ]} the pre-learning model could not be adapted quickly to the changing scenes in the camera view. There are also visual servo controllers proposed for concentric tube robots, where friction, torsion, shear, and nonlinear constitutive behavior induced challenges in accurate kinematic modeling.^{[ 47 ]} Yang et al.^{[ 48 ]} presented a model-free visual servoing controller circumventing hand–eye calibration. The image deviations were mapped to robot actuation variables using a dynamic numerical Jacobian estimated from an adaptive square-root unscented Kalman filter, but the performance needs further validation in surgical scenarios. Deep reinforcement learning approaches, e.g., Q-learning,^{[ 49 ]} were also employed in visual servoing to achieve target searching tasks, but the application of reinforcement learning in medical robot control is still limited due to the complicated controller training and discrepancy in environments.

Eye-in-hand setup with camera moving with robot tip is normally used for guiding robotic navigation to a targeted region-of-interest (ROI). In contrast, laser spot steering is conducted with the camera immobile and viewing a fixed ROI, which is an eye-to-hand setup. Vision-guided laser steering control has been investigated to automate laser spot projection.^{[ 50,51 ]} Epipolar or trifocal visual servoing demonstrated excellent control accuracy in laser spot steering. However, the controller was constructed based on the geometric model of actuated mirrors, which is not suitable for soft laser manipulators. In contrast to actuated mirrors with well-formulated kinematics, the proposed soft robotic laser manipulator has nonlinear kinematics and a time-varying origin of the laser beam, resulting in the coupling of robot kinematics and the epipolar geometry. To this end, we propose a geometric model that combines the epipolar constraint with the soft robot kinematics to achieve precise control of laser spot projection. The controllable workspace of the laser spot is not only limited to a 2D plane, but any location that satisfies two conditions such that the 3D surface has to be 1) covered by the robot's laser projection range; and 2) visible to the camera. Through this approach, the inverse transition mapping from image space to actuation space can be obtained without limitation to the variable projection distance. Based on the mapping, a feedback controller could be formed to ensure accurate laser spot targeting in the camera view. Under the circumstance of visual servoing control, the operator can prescribe desired ablation locations or paths in the camera view. As a result, the control task can be accomplished by servoing the (noninvasive) laser spot visible on the region, thus tracking to apply invasive (nonvisible) laser spot along the prescribed locations/paths. The control performance is validated on an MR-safe robotic laser manipulating platform, integrated with a fiber optic camera. The key contributions of this work are listed as follows: 1) Detailed formulation of epipolar geometry-based visual servoing for MR-safe soft laser manipulator, which enables precise laser spot control without prior knowledge of the tissue surface geometry; 2) Dynamic estimation of soft robot configurations by fusing the model prediction and camera feedback, thus allowing accurate calculation of robot kinematics; 3) Experimental validation of the proposed laser spot visual servoing, as well as the ablation performance through lab-based and MRI-based ex vivo tests.

Experimental Section Schematics of the Soft Robotic Laser Steering System

This paper aims to propose laser spot visual servoing controllers for a typical design of soft actuators, which comprises inflatable chambers to provide omnidirectional bending. A lens-pigtailed laser fiber can be housed inside the center channel of soft actuator to offer laser projection. The laser steering actuator can be further assembled with an additional bending segment,^{[ 34 ]} which offers navigation of the actuator tip to target regions. In our experimental validation, we will use a spring-reinforced soft manipulator,^{[ 34 ]} which comprises three 120°-separated elastomer chambers, with each chamber inflated by a hydraulic cylinder.

To control the laser beam projection on oral tissue, a desired path or region for laser ablation is defined on a visualization interface. Using this path, the controller can automatically steer the laser spot on the 2D image plane. As shown in Figure  2, a hydraulic-driven soft robot is used to manipulate the laser fiber collimator, which is housed in the central channel of robot. An external camera is equipped on the outer shell to provide visual feedback of laser spot on surgical site. The actuation space of soft manipulator is represented by the lengths of soft chambers, L, adjusting the chamber lengths varies the configurations of the soft manipulator, which drives the collimator toward different directions; ${\tilde{p}}_0$ and ${\tilde{p}}_1$ are the laser spot images in the camera view; Δu and Δv are the linear displacements over the u and v directions, respectively. We propose to model the nonlinear inverse transition mapping from image space to robot actuation based on constant-curvature robot kinematics and epipolar geometry. Referring to the desired laser spot position and current robot state, actuation commands can be generated through the inverse transition mapping.

Figure 2. Schematics of the laser steering system. A soft manipulator is actuated by adjusting the fluid volume of soft chambers, which steers the laser spot from p˜0 to p˜1within the camera view.

Laser Spot Detection

Precise detection of the actual laser spot position on the projection scene is the prerequisite of closed-loop control, thus allowing automatic correction of the targeting errors. Due to the monochrome and high intensity of laser spot, the laser aiming region can be extracted from the live camera video based on intensity thresholding.^{[ 52 ]} The threshold value can be experimentally obtained using the corresponding laser projection setup. An erosion filter is applied to remove small particles, clears the background noise, and only keeps the laser spot blob in the output image. By averaging the coordinates of all remaining white pixels, the centroid position of laser spot in the image frame can be obtained.

However, due to the scattering effect of light on tissue, the laser spot seen in the camera image could be irregular, noncircular, or even split into multiple laser spots. This introduces noise into the centroid calculation and may cause sudden jumps of the measured laser spot position. To ensure smooth tracking of the laser spot, we also integrate a linear motion model to predict its position, which is combined with the measured centroid position through Kalman filtering. The linear motion model is defined as[Image Omitted. See PDF]where Λ is the state transition matrix; subscripts “t” and “t + dt” denote the time step t and t + dt, respectively; dt is the sampling period of camera images; ${\tilde{p}}_{\mathrm{t}}$ and ${\stackrel{.}{\tilde{p}}}_{\mathrm{t}}$ are the combined position and velocity; ${\tilde{p}}_{\text{t+d}t}^{\ast }$ and ${\stackrel{.}{\tilde{p}}}_{\text{t+d}t}^{\ast }$ represent the predicted laser spot position and velocity, respectively. The observation equation is given as[Image Omitted. See PDF]where H is the observation matrix, and ${\tilde{p}}_{\text{obs}}$ is the laser spot position observed from the model. The combined laser spot position ${\tilde{p}}_{\text{t+d}t}$ and velocity ${\stackrel{.}{\tilde{p}}}_{\text{t+d}t}$ are calculated as[Image Omitted. See PDF]where ${\tilde{p}}_{\text{mea}}$ is the measured laser spot position from camera images; and $G_{\mathrm{t}}$ is the Kalman gain at time step t, which can be updated as follows[Image Omitted. See PDF]where ${\widehat{X}}_{\text{t+d}t}$ and $X_{\text{t+d}t}$ denote the predicted and updated state covariance matrix at time step t + dt, respectively; Q is the covariance matrix of process noise; Y denotes the covariance matrix of measurement noise.

Laser Spot Visual Servoing using Epipolar Geometry

In this section, epipolar geometry is investigated to relate the robot configuration and laser spot image, thus establishing the inverse transition mapping from image space to robot actuation. As shown in Figure  3 , the laser manipulator is represented as a bending curve. Point $O_{\mathrm{o}}$ is the intersection of the tangent lines at robot base $O_{\mathrm{w}}$ and robot tip. Assume that the soft manipulator bends with constant curvature, we can obtain the translation vector of $O_{\mathrm{o}}$ relative to the base coordinate $O_{\mathrm{w}}$ [Image Omitted. See PDF]

Figure 3. Epipolar geometry-based modeling. a) Illustration of the epipolar geometry. The 3-D coordinates Oc and Oo denote the camera center and the robot center, respectively. Along with the laser projection spot s, these three points form the epipolar plane. b) Three main parameters characterize the robot configuration, namely the bending radius r, plane angle ϕ, and bending angle θ.

Through hand–eye calibration, we can obtain the homogeneous transformation ${}_w^{c}T$, the rotation matrix ${}_w^{c}R$, and the translation vector ${}_w^c\mathbf{t}$, from robot base coordinate $O_{\mathrm{w}}$ relative to camera coordinate $O_{\mathrm{c}}$.

Coordinate $O_{\mathrm{o}}$ is defined in parallel to the base coordinate $O_{\mathrm{w}}$, thus the transformation matrix ${}_o^{w}T$ from coordinate $O_{\mathrm{o}}$ relative to $O_{\mathrm{w}}$ can be determined only by the translation vector ${}_o^w\mathbf{t}$. The parallelism is consistently satisfied since the coordinate $O_{\mathrm{o}}$ is defined referring to the constant-curvature kinematic model's coordinate. The translation distance/vector can be estimated based on the geometric relationship in the model. Then, we have the homogeneous transformation between coordinate $O_{\mathrm{o}}$ and $O_{\mathrm{c}}$ [Image Omitted. See PDF]

Virtual Camera and Epipolar Constraint

Let us denote s as the spatial projection spot of the laser beam, $p_{\mathrm{c}}$ as the position of laser spot in the camera coordinate, $p_{\mathrm{e}}$ as the position of robot tip in the reference coordinate $O_{\mathrm{o}}$, which can be assumed as a virtual camera. The laser beam is considered as the optical axis, and the robot tip is regarded as the virtual image of laser spot. Therefore, points $p_{\mathrm{c}}$ and $p_{\mathrm{e}}$ can be regarded as the images of the same spatial point s in two camera frames. In the frame of virtual camera, we can concisely form the laser projection vector based on the robot configuration, i.e., Equation (8). The epipolar constraint^{[ 53 ]} is accordingly applied to link the two frames[Image Omitted. See PDF]where $p_{\mathrm{e}}$ can be expressed as[Image Omitted. See PDF]where $\Vert p_{\mathrm{e}}\Vert =r\cdot \tan (\theta /2)$, r is the bending radius of soft manipulator, ϕ is the plane angle, and θ is the bending angle as defined in Figure 3b; F represents the fundamental matrix of the two camera frames,^{[ 53 ]} and can be given as[Image Omitted. See PDF]where ${(\cdot )}^{\wedge }$ maps a vector to an rotation matrix, based on the Lie algebra of rotation group SO(3).^{[ 54 ]}

As shown in Figure 3a, the spatial point s and its image in the camera and robot (virtual camera) frames, namely $p_{\mathrm{c}}$ and $p_{\mathrm{e}}$, and the origins of the two frames, $O_{\mathrm{o}}$ and $O_{\mathrm{c}}$, form the epipolar plane. The epipolar constraint in Equation (7) can be decomposed as the dot product of two orthogonal vectors in the robot frame, $\{O_{\mathrm{o}}\}$, and the camera frame, $\{O_{\mathrm{c}}\}$, respectively[Image Omitted. See PDF]where $\mathbf{F}{\mathbf{p}}_{\mathrm{c}}$ and ${\mathbf{F}}^{\mathrm{T}}{\mathbf{p}}_{\mathrm{e}}$ are the normal vectors to the epipolar plane, which are expressed in the frame of robot and camera, respectively.^{[ 50 ]}

Laser spot position, $p_{\mathrm{c}}$, is normalized in the form of $\begin{array}{ccc}[p_x& p_y& 1\end{array}]{}^{\mathrm{T}}$, and linked with its homogeneous pixel coordinate $\tilde{p}$, namely $\begin{array}{ccc}[u& v& 1\end{array}]{}^T$, by multiplying camera intrinsic matrix K [Image Omitted. See PDF]

By differentiating the epipolar constraint in Equation (7), we get[Image Omitted. See PDF]where ${\stackrel{.}{p}}_{\mathrm{c}}$ is the velocity of laser spot, the time derivative of robot tip position ${\stackrel{.}{\mathrm{p}}}_{\mathrm{e}}$ (Equation (8)) can be linearized with a Jacobian matrix $J_1$, namely[Image Omitted. See PDF]

The time derivative of the fundamental matrix $\stackrel{.}{F}$ only depends on a nonconstant term in ${}_o^w\mathbf{t}$, i.e., $r\tan (\theta /2)$. Notice that it would not be zero since the virtual camera has a time-varying translation from the robot base coordinate. Then we can obtain[Image Omitted. See PDF]where the multiplier η can be given as[Image Omitted. See PDF]where d(·)/dt denotes the derivative of time, and $J_2$ is a Jacobian matrix containing first-order partial derivatives. Thus, the last term in Equation (12) can be related to other two terms through the velocity of robot tip ${\stackrel{.}{p}}_{\mathrm{e}}$, namely[Image Omitted. See PDF]where $k_1$ is a scalar obtained by multiplying the terms before the multiplier η given by Equation (15).

Inverse Mapping-Based Controller I

By decomposing ${\stackrel{.}{p}}_{\mathrm{e}}$ along the normal vector of epipolar plane and two orthogonal vectors inside the epipolar plane,^{[ 50 ]} we obtain[Image Omitted. See PDF]where ${}^{\mathrm{o}}\mathbf{h}=(\mathbf{F}{\mathbf{p}}_{\mathrm{c}})/\Vert \mathbf{F}{\mathbf{p}}_{\mathrm{c}}\Vert$. $\Vert \cdot \Vert$ denotes the 2-norm of vector. γ, ${\beta }_1$, and ${\beta }_2$ are the decomposing coefficients along each vector. Substituting Equation (17) into the middle term of epipolar constraint in Equation (12) and applying the orthogonal property in Equation (10), we derive the coefficient γ as[Image Omitted. See PDF]

The first term of γ can be regarded as the projection of laser spot velocity ${\stackrel{.}{p}}_{\mathrm{c}}$ onto the vector, $({\mathbf{F}}^{\mathrm{T}}{\mathbf{p}}_{\mathrm{e}})/\Vert \mathbf{F}{\mathbf{p}}_{\mathrm{c}}\Vert$, which is orthogonal to the epipolar plane. We can decompose ${\stackrel{.}{p}}_{\mathrm{c}}$ into a component along this vector and a remaining component[Image Omitted. See PDF]where ${}^{\mathrm{c}}\mathbf{h}=({\mathbf{F}}^{\mathrm{T}}{\mathbf{p}}_{\mathrm{e}})/\Vert {\mathbf{F}}^{\mathrm{T}}{\mathbf{p}}_{\mathrm{e}}\Vert$. By projecting the remaining component of ${\stackrel{.}{p}}_{\mathrm{c}}$ onto the second and third term in Equation (17),^{[ 50 ]} we can approximately get ${\beta }_1$ and ${\beta }_2$ [Image Omitted. See PDF] [Image Omitted. See PDF]

As a consequence, we have[Image Omitted. See PDF]where the matrix A can be given as[Image Omitted. See PDF]

Thus, we have obtained an inverse mapping from the image velocity $\stackrel{.}{\tilde{p}}$ to the robot tip velocity ${\stackrel{.}{p}}_{\mathrm{e}}$ [Image Omitted. See PDF]

The robot tip velocity ${\stackrel{.}{p}}_{\mathrm{e}}$ (Equation (8)), which is determined by the velocity of configuration parameters, ${[\stackrel{.}{r}\stackrel{.}{\theta }\stackrel{.}{\varphi }]}^{\mathrm{T}}$, can be related to the variation velocity of chamber lengths, $\stackrel{.}{L}$, using a Jacobian matrix $J_{\text{act}}$ obtained based on the constant curvature model.^{[ 55 ]} The detailed formulation is given in the Appendix. The inverse transition mapping from image space to actuation space is accordingly obtained as[Image Omitted. See PDF]

Thus, the actuation command of each chamber can be generated according to the desired laser spot displacement.

A proportional feedback control law can be constructed based on the time-varying error in pixels between the current and target positions of the laser spot[Image Omitted. See PDF]where ${\tilde{p}}^{*}$ is the desired laser spot position, and the proportional gain λ is a positive constant. Note that the control law in Equation (26) will ensure exponential convergence of the laser spot tracking errors in camera view.

Note that the epipolar plane (Figure 3) can be formed as long as the laser spot is in the camera view, resulting in the corresponding inverse transition mapping for motion control. The mapping is not limited to a specific tissue surface geometry. This indicates that we can control the laser spot targeting to any locations within the laser projection workspace and visible to the camera. Despite the continuous change of laser lens-to-tissue distance during the robotic laser steering, the proposed controller with the use of epipolar geometry modeling has actually no dependence on such distances, giving rise to its adaptability to uneven tissue surfaces.

Forward Mapping-Based Controller II

Following the similar approach as in Controller I, we can form a forward mapping between the image velocity $\stackrel{.}{\tilde{p}}$ and robot tip velocity ${\stackrel{.}{p}}_{\mathrm{e}}$. By decomposing ${\stackrel{.}{p}}_{\mathrm{c}}$ along the normal vector of the epipolar plane and two orthogonal vectors inside the epipolar plane, we obtain[Image Omitted. See PDF]where the coefficient δ can be derived by substituting Equation (27) into (12), given as follows[Image Omitted. See PDF]

Similar to Equation (19), the robot tip velocity ${\stackrel{.}{p}}_{\mathrm{e}}$ can be decomposed as a component along the normal vector of epipolar plane, and a remaining vector[Image Omitted. See PDF]

By projecting the remaining component of ${\stackrel{.}{p}}_e$ onto the second and third term in Equation (27), we can accordingly get the coefficients ${\epsilon }_1$ and ${\epsilon }_2$ [Image Omitted. See PDF] [Image Omitted. See PDF]

Substituting these coefficients into Equation (27), the forward mapping can be formed as[Image Omitted. See PDF]where the transition matrix M has similar form as Equation (23)[Image Omitted. See PDF]

Note that the third element of ${\stackrel{.}{p}}_{\mathrm{c}}$ and $\stackrel{.}{\tilde{p}}$ are zeros, thus they can be related by $\stackrel{.}{\tilde{p}}=\tilde{K}{\stackrel{.}{p}}_{\mathrm{c}}$, where $\tilde{K}$ keeps the diagonal elements of camera intrinsic matrix K with other elements set as zero. Thus, the transition between the laser spot velocity in pixel coordinates and the chamber length variation rate can be obtained as[Image Omitted. See PDF]

A feedback Controller is designed as[Image Omitted. See PDF]where the proportional gain λ is positive constant.

To prove the stability of the Controller in Equation (35), we design a Lyapunov function as[Image Omitted. See PDF]

By differentiating Equation (36), we get[Image Omitted. See PDF]

Therefore, the proposed feedback controller in Equation (35) is asymptotically convergent. The epipolar constraint is degenerate when the laser spot is located at the line between the robot frame centre and camera frame centre. However, this case is avoided by the restricted bending workspace of robot.

Robot State Observer

The soft robot configurations are determined by the lengths of fluid chambers, which thus can be used to represent the robot state. The accurate estimation of chamber lengths is significant on the calculation of transition mappings, namely Equation (25) and (34), thus the control commands. However, it is always difficult to measure the chamber lengths directly. To this end, we estimate the chamber lengths by combining the information from the model predication and that from the image feedback. To achieve a desired laser spot displacement, the required change of chamber lengths can be approximately calculated using the proposed controllers, Equation (25 and 26) or Equation (35). Then the laser spot position at the next time step can be predicted based on the forward mapping as in Equation (34).

As given in Equation (38 and 39), a Kalman filter is constructed as the state observer combining the predicted and measured laser spot positions. The transition matrixes in the forward mapping (Equation (34)) are approximated as the observation matrix $H_k$, which is calculated based on robot state at time step k.[Image Omitted. See PDF]

The combined robot state, $\widehat{L}(k)$, is given as[Image Omitted. See PDF]where ${\tilde{p}}_{\text{mea}}(k)$ is the measured laser spot position at time step k. G _k is the Kalman gain, which can be updated based on the same rule as Equation (4). Increasing the covariance of process noise will reduce the weight of model prediction. Increasing the covariance of measurement noise will enhance the smoothing of noise but decrease the rate of convergence.

The architecture of the proposed control method is shown in Figure  4 , which combines the components of laser spot detection, the epipolar geometry-based controller, and the robot state observer. The control target defined in the camera view can be a reference trajectory, or even a path drawn by an operator using a mouse cursor. As the formulation of the proposed controllers do not include any information of the laser projection depth, they will allow stable laser spot visual servoing even on an irregular projection surface.

Figure 4. Control diagram of the proposed laser spot visual servoing. The inverse transition mapping is estimated with constant curvature and epipolar geometry model. The robot states, namely lengths of chambers, are observed through Kalman filter fusing the actuation command with the image feedback.

Experiments, Results, and Discussion Simulation and Results

To validate the performance of proposed controllers, simulation studies are first conducted. A constant curvature model is employed to represent the kinematics of soft manipulator, which projects a laser spot s on a planar surface, thus a corresponding image $\tilde{p}$ appears in the camera view[Image Omitted. See PDF]where $T_1=[\begin{array}{cc}{I}_{3\times 3}& 0_{1\times 3}\end{array}]$. The laser spot position in the robot coordinate O _o can be calculated as[Image Omitted. See PDF]where d is the depth of laser spot referring to the robot base coordinate O _w.

The robot and camera parameters are set to match our experimental platform. The natural length of the hydraulic chambers is 9 mm, and the radial distance between chamber and central channel is 3 mm. The camera is housed in parallel to the long axis of the soft manipulator, with an axial distance of 7 mm. Referring to the camera pixel size and focal length, the camera intrinsic matrix is defined as[Image Omitted. See PDF]

The soft manipulator with the laser collimator is placed 20 mm above a planar projection surface.

Controller I versus Controller II

The performance of Controller I and II is compared via a target tracking task. As shown in Figure  5a, both two controllers enabled accurate tracing of a goal point in the camera view. Bottom Figure 5a shows the tracking errors of two controllers in the u and v directions, respectively. Controller I converge faster than Controller II with the same proportional gain due to the smaller step size of Controller II, which can be increased by using higher gain. Both controllers achieved equal convergent speed along two directions. Figure 5b shows the tracking results with the camera rotation angle with respect to the laser manipulator intentionally perturbed by 20°. Controller II achieved a straighter path than Controller I, while ensuring similar convergence rate. This indicates that Controller II is more robust to the uncertainties in camera parameters. In addition, Controller II does not involve any frequent matrix inversion. Therefore, Controller II will be utilized in our following simulations and experiments.

Figure 5. Performance of laser spot servoing in simulation. a) Simulation of target tracking using Controller I versus Controller II; b) simulation of target tracking, while the camera rotation angle with respect to the laser manipulator is intentionally perturbed by 20°; c) simulation of a spiral path following. Gaussian random noise is induced to the length estimates of chambers at each time step. Actual footprint of laser spot compared with the reference path is plotted. The larger deviation of laser spot from reference path, the warmer color of its footprint.

With versus Without Observer

Figure 5c shows the simulation results on following a spiral path with and without using the robot state observer, respectively. This result demonstrates that the state observer can compensate for the Gaussian noise and model uncertainty, by taking the measured laser spot position into account. This would be especially advantageous in the hydraulic actuation system, as the desired change of chamber lengths may be not fully completed due to the possible air bubbles existing in the hydraulic tubes.

Parametric Analysis in Simulation

To implement the proposed visual servoing controller, the camera intrinsic and extrinsic parameters as well as the robot geometry parameters need to be initialized. Obtaining the exact value of those parameters is challenging, and as such, the controller should be robust against error that arises from them. To this end, we conducted a simulation study on the robustness of our controller by introducing additional noise to the modeling parameters. The simulated control task is to follow a circle (center: (400 400), radius: 50 pixels) in the camera view. With exact robot and camera parameters, the controller achieved a root-mean-square (RMS) tracking error of 0.08 pixel (≈1.2 μm). All simulations were conducted to ensure the same path following speed, one cycle in 1000 steps.

Camera Intrinsic Parameters

It was found to have a limited influence on the control accuracy. For example, we multiply the second diagonal element of intrinsic matrix K by 2 and divide the principle coordinate along u axis by 1.2. The controller maintains stable path following with a root-mean-square error (RMSE) of 0.31 pixel (≈4.5 μm).

Camera Extrinsic Parameters

By introducing 20% error (≈2 mm) to the translation parameters and rotating the camera 30° about the x, y, and z axes, respectively, we obtained corresponding RMSEs of 1.51, 0.22, and 0.31 pixels, i.e., around 21.9, 3.2, and 4.5 μm. It infers that the proposed controller can be applied in endoscopic applications, of which the assembly accuracy of camera is around 5° and 1 mm.^{[ 51 ]}

Robot Geometry Parameters

The controller was found to be robust to relatively large change of the robot structural parameters. We added 20% error to the natural chamber length, as well as the radial distance between robot central channel and fluid chambers. The RMSE of following a circle is only about 0.37 pixels (≈5.4 μm).

Experimental Platform

Experimental setup of our laser spot control test is illustrated in Figure  6a. The laser steering module comprises three cylindrical elastomeric chambers separated by 120°, which are inflated and deflated by hydraulic cylinders.^{[ 34 ]} The soft manipulator is 3D printed with AgilusClear30 material. The volume of each cylinder is precisely controlled by a stepper motor. The hydraulic transmission is achieved through 10 m-long pipelines (OD = 4 mm). Upon pressurization of the elastomer, the corresponding elastic chamber will elongate individually. Adjusting the pressure of three chambers enables omni-directional bending (+/– 26°) of the optical laser fiber housed in the center channel. The fiber end is assembled with a collimator (Figure 6b) to produce a small laser spot projection at a longer distance compared to a free-end fiber. The collimator (Huaxin Smart Link Ltd., China) selected is a gradient-index (GRIN) rod lens with parameters: 0.23 pitch, face angle = 8°, outer diameter = 1.0 mm, length = 3 mm, and antireflection coating at 1550 nm. It is packaged with the optical fiber ferrule using a glass tube. The distance between the fiber end and lens’ front end is near zero. The size of the laser spot on the target is about 1 mm, which could be further reduced by optimizing the lens parameters, e.g., pitch value and length. The laser manipulator provides a scanning area of 20 × 20 mm² at a projection distance of ≈20 mm. The executable space is ensured by imposing maximum limits on each motor. This noncontact ablation reduces the chance of overheating surrounding healthy tissues, which can be caused by touching and sticking between the fiber end and tissue. An additional outer shell is designed to protect the soft fluid chambers (Figure 6c). As a result, the laser collimator steering will not be disturbed by possible contact between manipulator and the oral, nasopharyngeal (ONP) tissue. This strengthens the robustness of robot control by isolating the laser manipulator from the surrounding anatomy, therefore allowing laser ablation to be performed with higher accuracy. Due to the high magnetic field inside the MRI, choices of scope or camera is very limited, only a fiberscope can be used, of which the images are in low definition. An optical-fiber-based camera (FIGH-30-850N, Myriad Fiber Imaging Tech., Inc., USA) with 30 000 pixels is integrated to offer visual feedback. The image is captured at a frame rate of 30 fps by the imaging fiber bundle (10-meter long), which is coupled to a high-resolution RGB camera of 1280 × 960 pixels. Benefiting from the fully fluid-driven robot setting, an additional irrigation channel with a small diameter as in many standard endoscopes can be added in our future work. The control algorithm was implemented in MATLAB (Intel Core i7-6700HQ [email protected] GHz with 16-GB RAM).

Figure 6. Experimental setup. a) Laser steering module integrated with a fiber optics camera that ensures the MR compatibility; b) laser fiber integrated with GRIN collimator; c) bottom view of the soft manipulator; d) performances on tracking multiple target points using Controller I compared with using Controller II. The robot is controlled to aim the laser spot at four target points one by one (targets 1–4).

Point-To-Point Tracking

A point tracking experiment is conducted to validate the performance of the proposed controllers. As discussed in Section 3.1.3, the controller shows adaptivity/robustness to the camera and robot parameters. Therefore, without thorough camera calibration, we just use the nominal values for camera intrinsic parameters. The homogeneous transformation between robot and camera coordinate was initialized based on robot geometry parameters. The rotational angle of camera view is estimated from laser spot movement in camera image while inflating one soft chamber. As shown in Figure 6d, the robot is controlled to track four points one by one. Once the laser spot enters a region within 3 pixels (≈105 μm) of a target point, the desired target will be switched to the next one. Similar to the simulation result (Figure 5), both controllers achieve accurate tracking of the four targets. However, the laser spot trajectory with Controller II is straighter than Controller I, especially from targets 2 to 3, which implies that Controller II has a more precise approximation of the inverse transition mapping.

Path Following

The performance of laser spot visual servoing using Controller II is evaluated by a path following task (Video Episode S1, Supporting Information). A desired path of the laser spot is predefined in the 1280 × 960 pixels camera frame. The robot steers the laser spot to trace a reference target moving along the path. The tracking error is measured as the shortest distance in pixels between the current laser spot and the predefined reference path.

The laser spot footprint and tracking errors in two successive cycles are depicted in Figure  7a,b, respectively. In the first cycle, the RMS tracking error is 2.37 pixels (≈83 μm), and the maximum tracking error is 7.47 pixels (≈261.5 μm). This tracking performance is maintained in the second cycle, during which the RMSE and maximum error are 1.82 pixels (≈63.7 μm) and 5.92 pixels (≈207.2 μm), respectively. The projection distance is about 20 mm (Figure 7c), corresponding to a projected “Mickey Mouse” pattern of about 10.5 mm (300 pixels) wide on the target surface. Figure 7d shows the laser spot image superimposed with its footprint throughout the tracking trajectory. Thus, the mean control accuracy of laser spot projection can be estimated as <105 μm (3 pixels). This indicates that the controller can achieve precise approximation of the nonlinear response of the system. Such precise tracking is typically challenging for soft robots due to the need for sudden volume changes of the robot's soft actuation chambers, coupled with the compliance of the robot body. This is even more challenging for our MR-safe setup, which requires the use of 10 m-long water pipelines for hydraulic transmission.

Figure 7. Performance of laser spot tracking on a predefined “Mickey Mouse” path. a) Laser spot following the reference path within the camera view in two cycles; b) tracking errors throughout the 90-second journey; c) experiment setup; d) footprint of the laser spot under the proposed servoing.

The maximum path following speed is tested as about 58 pixels s⁻¹ (≈2 mm s⁻¹), corresponding to an RMSE of 6.13 pixels (≈214.6 μm). The tested soft actuator features a bandwidth of about 1 Hz.^{[ 34 ]} In our application, we intend to ensure a precise surgical margin with slow energy delivery using a low-power laser in coordination with the low-speed laser steering. MR imaging can be utilized for monitoring the heat diffusion process within the ablation margin.

Repeatability

Figure  8a shows the repeatability of Controller II when performing path following. The controller was set to track a “Mickey Mouse” path with 70 loops. The RMS tracking error remains small within 4 pixels (≈140 μm), while the maximum error maintains about 10 pixels (≈350 μm). It demonstrates that the laser manipulation system along with the proposed visual servo controller is stable and reliable.

Figure 8. Performance of laser spot tracking on “Mickey Mouse” path. a) Repeatability of the control performances in following the “Mickey Mouse” path as in Figure 7 (70 cycles in total); b) performance on path following with open-loop control and closed-loop control. We intentionally caused water leakage by punctured one water pipes to a soft chamber, thereby actuation disturbance. The closed-loop controller could still compensate it and maintain precise path following. c) Performance on path following with versus without state observer. The corresponding tracking errors are shown in d).

Open-Loop versus Closed-Loop

To validate the robustness of closed-loop controller, we intentionally punctured one water pipe connected to a soft chamber, inducing water leakage during the robot operation. Figure 8b shows the tracking results using open-loop control compared with closed-loop control. The laser spot cannot follow the desired path with open-loop actuation. However, the closed-loop controller can compensate the actuation disturbance, thus ensuring accurate path following.

With versus Without State Observer

To validate the effect of state observer, we also conducted path following tests while intentionally introducing water leakage by puncturing one water pipes to a soft chamber. Figure 8c,d shows the tracking results using state observer compared with omitting state observer. It demonstrates that the state observer reduced the RMS tracking error from 4.26 pixels (≈149.1 μm) to 2.96 pixels (≈103.6 μm).

Hand-Drawing Following

The performance of visual servoing Controller II is validated via a hand-drawing following task. The positions of mouse cursor in camera view are detected as the desired target of laser spot. The drawing speed is estimated to be about 10 pixels s⁻¹. The laser spot is controlled to instantly follow the mouse cursor manipulated by operator. As shown in Figure  9 and Video Episode S2, Supporting Information, the laser spot can accurately follow the mouse cursor, with a RMS tracking error of 3.97 pixels (≈180.5 μm) and maximum error of 12.26 pixels (≈557.3 μm).

Figure 9. User-defined path “drawn” by a mouse cursor. Both the laser spot footprint and the drawn path are superimposed in the camera view, so as to evaluate the tracking errors (RMSE and Max.).

Path Following on 3D Surfaces

The path following experiment is also evaluated on 3D surfaces (Video Episode S3, Supporting Information). All experimental settings including the controller are as same as in Section 3.4, except the projection surface is changed to the alveolar process and the condylar process on a mandible model. A 2D (“four-leaf”) reference path is prescribed on the 2D camera view (Figure  10 ).

Figure 10. “Four-leaf” reference pattern prescribed on the 2D camera view. Two cycles of path following were conducted by visually servoing the laser spot reflection from a 3D surface of phantom anatomy – alveolar process.

When the laser spot is projected on the alveolar process, its footprint in two successive cycles is depicted in Figure 10. In the first cycle, the RMS tracking error is 1.98 pixels (≈69.3 μm), and the maximum tracking error is 5.76 pixels (≈201.6 μm). This tracking performance is maintained and even improved in the second cycle, during which the RMSE and maximum error are 1.11 pixels (≈38.9 μm) and 4.05 pixels (≈141.8 μm), respectively. Although there is a height difference of ≈4 mm on the projection surface (Figure  11a), the path following accuracy remains stable when suffering such challenges. When projected on another location, the condylar process (Figure 11b), the laser spot encounters a larger lens-to-tissue distance (20 to 29 mm) variation, where the tracking error maintains small (1st cycle: 2.20 ± 1.28 pixels, 2nd cycle: 1.63 ± 1.03 pixels). This indicates that the controller can adapt to significant variances of the projection distance, achieving accurate laser spot visual servoing even on 3D surfaces. As far as the laser spot can be reflected and visualized in the 2D images, the presented visual servoing controller can perform well in tracking the spot along any (2D or 3D) surfaces.”

Figure 11. Tracking errors and setups of experiments on a) alveolar process and b) condylar process. Even with much different change of visual depth as indicated (≈4 and ≈9 mm). Both the tracking RMSE could be maintained below 2.20 pixels (≈77 μm).

Ex vivo Laser Ablation Test

The laser steering system, including the visual servoing Controller II, is validated by a laser ablation test on ex vivo swine stomach tissue (Figure  12a and Video Episode S4, Supporting Information). All ex vivo tissues used in this paper were purchased from the local butcher shop. The tissue was taken out of the refrigerator and naturally defrosted at room temperature of 20 °C for two hours before the test. A 1550 nm laser source (STL1550T2-10W, Stone Laser Ltd., China) is selected in ex vivo ablation test because of its high absorption rate on water. The laser source power is 3 W in continuous-wave mode. A pilot green laser (532 nm) is also coupled to provide visual feedback. To imitate the clinical conditions of a transoral laser surgery, the laser steering module is fixed toward the tissue surface with a tilted angle of about 30°. As seen in the endoscopic camera view, the laser spot can accurately follow the desired trajectory even though the projection distance and incline angle of the robot are totally unknown. A “Mickey Mouse” ablation boundary can also be observed on the tissue surface. The distortion of the ablated trajectory is caused by the tissue shrinkage during ablation and the irregular surface geometry of tissue.

Figure 12. Ex vivo laser ablation test. a) Laser ablation test on ex vivo swine stomach tissue. The corresponding endoscopic view is shown with the laser spot and its footprint. b) Laser ablation test on ex vivo pig tongue tissue.

Another ablation test is conducted with ex vivo swine tongue tissue, which contains less fluid and has higher stiffness. The laser source power is increased to 4 W. As shown in Figure 12b and Video Episode S5, Supporting Information, the swine tongue tissue showed less shrinkage upon ablation than swine stomach tissue (Figure 12a). A “Mickey Mouse” ablated path (14 × 12 mm) can be observed on the tissue surface, as well as the camera view. The RMS tacking error during the ablation is about 5.75 pixels (≈201.3 μm). In current practice of treating oropharyngeal tumors, patients have to be under general anesthesia with muscle paralysis. Physiological motion-induced tissue deformation is very minimal since no swallowing can occur, thus not affecting the control performance. The smog induced by laser ablation was not so severe to block the view of watching the surgical site. We only use the point position of the 2D laser spot as feedback, which can still be continuously distinguished from its background due to its high intensity and monochrome. As a result, the proposed controller can be robust to poor image quality.

MRI-Based Laser Ablation Test

The soft robotic laser steering system is comprised entirely of nonmetallic components, which ensures MR safety according to ASTM F2503 standard. We conducted a laser ablation experiment in the MR environment with ex vivo swine tissue. The robot was controlled teleoperatively via a 10 m hydraulic tubing passing through the waveguide of the control room. A “Mickey Mouse” pattern was inputted as the laser spot trajectory. The ex vivo tissue was ablated with a 4 W 1550 nm laser, repeated for 3 loops (Figure  13 and Video Episode S6, Supporting Information). To incrementally monitor the laser ablation effect, 3D T1-gradient echo (GRE) acquisition was continuously performed with 18 s temporal resolution. After the laser ablation with three repeated loops, we can observe a shadowed “Mickey Mouse” shape on MR image. The trajectory is deformed due to the tissue shrinkage under ablation. As each acquisition of the MR image takes about 18 s, tissue shrinkage happens due to the cooling effect of MRI room temperature.

Figure 13. MRI-based laser ablation test. a) Experimental setup of the soft laser manipulator in a 1.5T MRI scanner; b) image captured by an MR-conditional camera; c) MR image (T1-GRE) of ex vivo swine tissue after ablation with 3 repeated loops showing a “Mickey Mouse”-like pattern; d) ablated swine tissue model reconstructed from 6 slices of MR images.

Conclusion and Future Work

This paper has presented a visual servo controller for soft robotic laser steering system. Epipolar geometry is first constructed to relate the camera frame with soft robot kinematics, thus modeling the inverse transition mapping. Based on this inverse mapping, a feedback controller is established, without having to incorporate prior knowledge of the tissue surface geometry. A state observer is also designed to provide dynamic estimation of robot configurations, enabling accurate computation of the inverse transition mapping. The closed-loop controller demonstrated robustness to actuation disturbances compared to the open-loop control. It brings new opportunities to precise laser projection using soft manipulator, which could be equipped on endoscope tip for endoscopic laser surgery. Moreover, the MRI-compatibility of soft laser manipulator enables further application in MRI-guided laser treatment, which could enhance the intra-op monitoring of laser ablation outcome.

Due to the limited larynx workspace, precise laser spot projection requires delicate volume adjustment of the small and soft fluid chambers, which are inflated and deflated through 10 m long water pipes. These introduce demands for a controller with high accuracy and stability. Our work has demonstrated accurate path following using a hydraulic-driven soft laser manipulator. The RMS tracking errors on path following is less than 4 pixels in the camera view, corresponding to a physical error of laser spot projection within 140 μm. This is maintained even throughout 70 repeated cycles, with the maximum tracking error smaller than 11.32 pixels (≈396.2 μm). The ex vivo laser ablation test has shown that the laser projection setup combined with the visual servo controller enables precise laser ablation pattern, despite on uneven tissue surfaces. The feasibility of MRI-guided laser surgery has also been validated through the laser ablation test under MRI.

In future work, we will investigate the influence of MRI sequence parameters on imaging of tissue being ablated. Real-time tracking under MRI^{[ 56 ]} will be incorporated to enable registration between MRI images and robotic navigation. We aim to incorporate the fast streaming of MRI scanning data, thus interfacing with various types of MR imaging feedback (e.g., MR thermometry) in real time. It would give additional information for monitoring the heat diffusion process, thus further enhancing the ablation safety. Visual servo based on MR-thermometry will also be of our interest. Proper surgical laser sources, e.g., pulsed laser, will be investigated to achieve tissue cutting or ablation in cooperation with the laser beam manipulation. We also intend to improve the laser spot control performance by decoupling the velocity profile and path following.^{[ 57 ]}

Appendix

The Jacobian matrix $J_1$ can be derived by differentiating Equation (8) relative to the robot configurations, $[\begin{array}{ccc}r& \theta & \varphi \end{array}]$ [Image Omitted. See PDF]where each term is calculated as[Image Omitted. See PDF] [Image Omitted. See PDF] [Image Omitted. See PDF]Similarly, the Jacobian matrix $J_2$ can be obtained as[Image Omitted. See PDF]The velocity of configuration parameters, $[\begin{array}{ccc}\stackrel{.}{r}& \stackrel{.}{\theta }& \stackrel{.}{\varphi }\end{array}]$, can be related to the variation velocity of chamber lengths, $[\begin{array}{ccc}{\stackrel{.}{L}}_1& {\stackrel{.}{L}}_2& {\stackrel{.}{L}}_3\end{array}]$, using constant curvature model[Image Omitted. See PDF] [Image Omitted. See PDF]where $c_1={\sum }_{i=1}^3{L}_i^2-L_1\cdot L_2-L_2\cdot L_3-L_1\cdot L_3$.[Image Omitted. See PDF] [Image Omitted. See PDF]where, $c_2=\frac{3{(L_2-L_3)}^2}{3{(L_2-L_3)}^2+\sqrt{3}(L_3+L_2-2L_1)}$.

Acknowledgements

G.F. and X.W. are treated as co-first authors. This work was supported in part by the Research Grants Council (RGC) of Hong Kong (17206818, 17205919, 17209021, 17207020, and T42-409/18-R), in part by the Innovation and Technology Commission (ITC) (MRP/029/20X), and in part by the Multi-Scale Medical Robotics Center Limited, InnoHK.

Conflict of Interest

The authors declare no conflict of interest.

Data Availability Statement

The data that support the findings of this study are available from the corresponding author upon reasonable request.