An optical tracker based robot registration and servoing method for ultrasound guided percutaneous renal access

Abstract

Background

Robot-assisted needle steering facilitates the percutaneous renal access (PRA) for their accuracy and consistency over manual operation. However, inaccurate image-robot correspondence and uncertainties in robot parameters make the needle track deviate from the intrarenal target. This paper aims to simplify the image-tracker-robot registration procedure and improves the accuracy of needle alignment for robot assisted ultrasound-guided PRA.

Methods

First, a semi-automatic rigid registration is used for the alignment of the preoperative MR volume and the intraoperative orthogonal US slices. Passive markers are mounted both on US probe and robot end-effector, the planned puncture path is transferred from the MR volume frame into optical tracker frame. Tracker-robot correspondence and robot calibration are performed iteratively using a simplified scheme, both position and orientation information are incorporated to estimate the transformation matrix, only several key structural robot parameters and joint zero-positions are calibrated for simplicity in solving the inverse kinematic. Furthermore, an optical tracker feedback control is designed for compensating inaccuracies in robot parameters and tracker-robot correspondence, and improving the accuracy of needle alignment. The intervention procedure was implemented by a telemanipulated 5R1P robot, two experiments were conducted to validate the efficiency of robot-tracker registration method and the optical tracker feedback control, robot assisted needle insertion experiment was conducted on kidney phantom to evaluate the system performance.

Results

The relative positioning accuracy of needle alignment is 0.24 ± 0.08 mm, the directional accuracy is 6.78 ± 1.65 × 10^-4rad; the needle-target distance of needle insertion is 2.15 ± 0. 17 mm. The optical tracker feedback control method performs stable against wide range of angular disturbance over (0 ~ 0.4) radians, and the length disturbance over (0 ~ 100) mm.

Conclusions

The proposed optical tracker based robot registration and servoing method is capable of accurate three dimension needle operation for PRA procedure with improved precision and shortened time.

Background

Robot-assisted needle insertion facilitates many minimally invasive percutaneous procedures such as biopsy, electrolytic ablation and renal intervention, where a similar establishment of reliable and consistent access track from skin to the inside anatomical feature is required. In percutaneous renal intervention, it is important to locate the needle tip as well as the track of the needle shaft under intra-operative guidance of x-ray or ultrasound images [1–3]. Accurate steering and placement of needle is challenging due to uncertainties in image-robot correspondence, which makes the needle track deviate from the target.

Numbers of robotic systems have been proposed for eliminating radiation exposure and simultaneously increasing accuracy in radiologic interventions. Bzostek et al. [1] used a stereo-pair of two x-ray views registered to a common fiducial system with an active robot to assist needle placement. Yu Zhou et al. introduced a CT-guided robotic needle biopsy technique for lung nodules. Based on the nodule respiratory motion model, needle placement is planned to follow an optimal timing and path, and is triggered based on the respiratory phase tracking [4]. The PAKY-RCM incorporates a passive robotic arm and a friction transmission with axial loading system, which allows a urologist to remotely align the needle along a selected trajectory path under fluoroscopic guidance using the superimposed registration principle [2]. These methods all require time consuming pre-operative registration procedures between robot, imaging system and the patient's anatomy. Patriciu et al. uses the laser markers readily available on any CT scanner for robot registration in computer tomography imaging systems. This approach does not require additional hardware, laser alignment being performed on the instrument used in the clinical application [5]. An automatic image-guided control based on visual servoing and principles of projective geometry is presented for automatic and uncalibrated needle placement under CT-fluoroscopy. The approach demonstrated good targeting accuracy by using the procedure needle as a marker, without additional registration hardware [6].

Robotic percutaneous interventions guided by ultrasound are developed in recent decades for that ultrasound (US) is radiation-free, real-time and easy-to-use [7]. J Hong et al. proposed an ultrasound-driven needle insertion robot for percutaneous cholecystostomy, which is capable of modifying the needle path by real-time motion compensation through visual servo control before needle insertion [8]. Robot assisted and ultrasound guided ablative therapy and biopsy operation are also studied [9, 10], an optical/electromagnetic marker mounted on ultrasound probe are used to measure the transducer’s position and orientation, once the puncture path is defined, the robotic arm moved automatically to the planned insertion path. Ultrasound image-based visual servoing techniques have not been used in percutaneous interventions for that the abdominal US is often related to limited anatomy identification and targeting abilities, providing only two-dimensional(2D) anatomical information with poor quality [3, 11, 12].

In our previous works, an augmenting intraoperative ultrasound with preoperative magnetic resonance planning models for PRA was proposed and evaluated by urologists on a kidney phantom. With careful setup it can be efficient for overcoming the limitation of current US-guided PRA [13, 14]. In this paper, a telemanipulated 5R1P robot is employed for needle operation. We present an optical tracker based robot registration and servoing method for ultrasound-guided PRA, optical tracker serves as intermediate coupling tool for image-robot registration and error feedback control for needle alignment. The rest of the paper is organized as follows, introduction of experiment setup and navigation systems, image-robot registration and robot control scheme are illustrated in Sec. II. The last two sections describe the experiment and discussions.

Methods

Procedures of robot assisted percutaneous renal intervention

The robot assisted percutaneous renal intervention surgery workflow consists of preoperative surgical planning, intraoperative surgical navigation and semi-autonomous telemanipulated needle operation. First, the patient is scanned by magnetic resonance (MR), kidney, vessels and tumor are then segmented from the MR volume as 3D model, such that a surgeon can make a optimal surgical plan preoperatively. During the surgery, a semi-automatic rigid registration is performed for the alignment of the US slices and the MR volume, the preoperative planning can be transferred onto the patient in situ. With an image-guidance interface, the surgeon guide the robot to the insertion point, needle alignment and interventional puncture can be performed autonomously in accordance with the surgical planning. Verification that the needle has successfully gained access to the collecting system will be provided by the return of urine through the trocar needle. The needle will be detached from the robot, and subsequent surgical procedure continues.

Experiment setup

The prototype system for needle insertion has been set up in our integrated operating room (see Figure 1). The master was the PHANToM OMNI haptic device (SensAble Technologies Inc., USA) ,which provided force position measurements at its end point and feedback in three DOFs. A 5R1P industrial robot (REBo-V-6R-650, Shenzhen Reinovo Technology Co. Ltd., China) was employed for needle operation, five rotational joints uniquely determine the needle orientation and position, the last linear DC-servomotor (Quickshaft LM1247, Faulhaber Group, Germany) served for needle insertion with positional accuracy 180 μm and maximum 3N force. The last two joint axes of the wrist and the translational axis are intersecting in a single point. Needle orientation and puncture are independently activated by the corresponding joints and safe button of the haptic device. An 18-gauge trocar needle (090020-ET, Cook Urological Inc., USA) with a triangular diamond tip was attached to the end-effector by a detachable unit, which was equipped with a force sensor (AL311-BL, Honeywell Inc., USA), the force value were collected by data acquisition card (DAQ 6229-USB card, NI Inc., USA). A 3D ultrasound system (DC-7, Mindray Medical Ltd.,China) was used as an intraoperative navigator. In order to track the 6D positions of needle and ultrasound frame, passive optical markers were mounted to ultrasonic probe and needle holder, the receiver was optical tracking systems with positioning accuracy RMS error 0.35 mm (Polaris Spectra, Northern Digital Inc. (NDI), Canada). All experiments were conducted on the silicon phantom from Computerized Imaging Reference Systems (CIRS), no ethical concern is involved.

Figure 1

Block diagram of the master–slave experimental set-up for needle intervention.

Registration of image-robot-tracker

Registration of the robot to the image space provides us with the essential relationship between the needle location and the targets in image coordinate. Indeed, inaccurate robot-image calibration has a direct impact on the accuracy of the needle steering.

(i). Image-Tracker registration

At preoperative surgical planning stage, we applied the semi-autonomous algorithm from [15] to segment kidney parenchyma and vascular structures from magnetic resonance images. A 3D plan can be then defined in MR volume coordinate frame F _MR by the surgeon. During the surgery, the tracker reads the positions of maker fixed on the robot end-effector and the US probe, while the preoperative data and the surgical plan are registered to the calibrated intraoperative US images. First, two pairs of orthogonal ultrasound images were collected near the 11th intercostals space at the maximum exhalation positions, all images should contain clearly visible kidney contours. Next, the iterate closet point (ICP) algorithm was performed for the alignment of the US slices and the MR volume, using kidney surface and large vessel surface as registration features [16]. Based on ultrasound-MR volume transformation $T_{\mathit{US}}^{\mathit{MR}}$ and tracker-ultrasound transformation $T_{\mathit{US}}^T$, the planned puncture path can be transferred from the preoperative MR volume frame F _MR into intraoperative tracker frame F _Tracker . Once robot-tracker registration ${\mathbf{T}}_R^T$ is done, the surgical plan can be transferred from preoperative MR volume frame F _MR to robot frame F _Robot . At the maximum exhalation, the needle is rapidly inserted into the intrarenal target under navigated guidance. The image-robot-tracker registration is shown in Figure 2.

$$T_R^{\mathit{MR}}=T_{\mathit{US}}^{\mathit{MR}}{\left(T_{\mathit{US}}^T\right)}^{-1}{T}_R^T$$

(1)

(ii). Robot-Tracker correspondence

Figure 2

Image-Tracker-Robot registration, the optical tracker acts as an intermediate coupling tool.

In this section, we propose a simplified registration method for both robot-tracker correspondence and robot calibration.

The coordinate systems of the 5R1P needle operation robot is depicted in Figure 3, frame F _Tracker  = (x _T ,y _T ,z _T ) is attached to the optical tracker base, there are total 8 coordinate systems attached to the manipulator, the robot based F _Robot  = (x _R ,y _R ,z _R ) is used as reference, the last frame F _Needle  = (x _N ,y _N ,z _N ) is attached to the needle, the passive optical marker with frame F _Marker  = (x _M ,y _M ,z _M ) is mounted on the end-effector, the other coordinate systems (x _i ,y _i ,z _i ), i = 1⋯5 are attached to the links. The transformation from marker frame to tracker base and robot frame can be expressed respectively with matrix of the form

$${\mathbf{T}}_M^T=\left\{\begin{array}{cc}\hfill {\mathbf{R}}_M^T\hfill & \hfill {\mathbf{d}}_M^T\hfill \\ \hfill \mathbf{0}\hfill & \hfill 1\hfill \end{array}\right\},{\mathbf{T}}_M^R=\left\{\begin{array}{cc}\hfill {\mathbf{R}}_M^R\hfill & \hfill {\mathbf{p}}_M^R\hfill \\ \hfill \mathbf{0}\hfill & \hfill 1\hfill \end{array}\right\}$$

(2)

where ${\mathbf{p}}_M^R$ and ${\mathbf{d}}_M^T$ are marker positions observed in F _Robot and F _Tracker , rotation matrix ${\mathbf{R}}_M^T=\left[{\mathbf{m}}_1,{\mathbf{m}}_2,{\mathbf{m}}_3\right]$ are axis vectors x _M , y _M , z _M that expressed in tracker frame, and ${\mathbf{R}}_M^R=\left[{\mathbf{n}}_1,{\mathbf{n}}_2,{\mathbf{n}}_3\right]$ refers to the axis expression in robot frame. They are ideally related by robot-tracker transformation matrix ${\mathbf{T}}_R^T$, shown as

$${\mathbf{T}}_R^T=\left\{\begin{array}{cc}\hfill {\mathbf{R}}_R^T\hfill & \hfill {\mathbf{p}}_R^T\hfill \\ \hfill \mathbf{0}\hfill & \hfill 1\hfill \end{array}\right\}$$

(3)

$${\mathbf{T}}_M^T={\mathbf{T}}_R^T{\mathbf{T}}_M^R$$

(4)

Figure 3

Coordinate systems of the 5R1P needle operation robot.

Affected by measurement noise U and V _i , i = 1, 2, 3, the expansions of (4) are

$${\mathbf{d}}_M^T={\mathbf{R}}_R^T\mathbf{p}+{\mathbf{p}}_M^R+\mathbf{U}$$

(5)

$${\mathbf{m}}_i={\mathbf{R}}_R^T{\mathbf{n}}_i+{\mathbf{V}}_i$$

(6)

K corresponded pose pairs ${\left\{{\mathbf{T}}_M^T,{\mathbf{T}}_M^R\right\}}_k$, k = 1 ⋯ K were recorded at different configurations of robot angle setting. Solving the optimal transformation ${\mathbf{T}}_R^T$ typically requires minimizing a least square error criterion given by

$${\displaystyle \sum ={\displaystyle \sum _{k=1}^K{\displaystyle \sum _{i=1}^3{\alpha }_{\mathit{ki}}}{∥{\mathbf{m}}_{\mathit{ki}}-{\mathbf{R}}_R^T{\mathbf{n}}_{\mathit{ki}}∥}^2+{\displaystyle \sum _{k=1}^K{\beta }_i}}}{∥{\mathbf{d}}_{\mathit{Mk}}^T-{\mathbf{R}}_R^T{\mathbf{p}}_{\mathit{Mk}}^R-{\mathbf{p}}_R^T∥}^2$$

(7)

A dual number quaternion based algorithm was employed to estimate the transformation matrix [17], which incorporates both orientation and translation information. However, inaccuracy in robot forward kinematics seriously affects the validity of registration result. Robot calibration is required to reduce the registration error as well as inaccuracies in robot parameters of links and joint angles.

(iii). Calibration of robot parameters

The forward kinematics of the 5R1P needle manipulating robot is cascadely constructed by the transformations between consecutive joint frames based on the modified D-H parameters [18]. The needle was axially attached to the linear motor shaft, the optical marker was mounted on the outer shell of motor. The transformation matrix ${\mathbf{T}}_M^R$ of maker can be read via robot forward kinematics,

$${\mathbf{T}}_M^R={\mathbf{T}}_1^R{\mathbf{T}}_2^1\dots {\mathbf{T}}_5^4{\mathbf{T}}_M^5=F\left(\mathbf{X},\mathbf{\Theta }\right)$$

(8)

$${\mathbf{T}}_{i+1}^i={\mathbf{R}}_x\left({\alpha }_i\right){\mathbf{T}}_x\left(a_{i-1}\right){\mathbf{R}}_z\left({\theta }_i\right){\mathbf{T}}_z\left(d_i\right){\mathbf{R}}_y\left({\beta }_i\right)$$

(9)

in which, X = (a, d, α, β, p)^T are link structural parameters, a = (a₁, a₂ ⋯ a₆)^T, d = (d₁, d₂ ⋯ d₆)^T, α = (α₁, α₂, ⋯, α₆)^T, β = (β₁, β₂, ⋯, β₆)^T, p = (p _x , p _y , p _z )^T are positions of the optical marker relative to the robot end-effector, Θ = (θ₁, θ₂, ⋯ θ₆)^Tare joint variables. Variations in robot geometric parameters due to manufacturing tolerances or component limitations account for the accuracy of robot kinematics. Considering the 2nd and 3rd joint axes are nearly parallel, only β₂ is necessary. The marker pose with nominal link parameters is noted as ${\widehat{T}}_M^R$, the correct pose of the marker with kinematic errors is given by ${\mathbf{T}}_M^R$, it can be expressed as

$${\mathbf{T}}_M^R={\hat{\mathbf{T}}}_M^{\mathit{R}}+d{\mathbf{T}}_M^R=F\left(\mathbf{X}+\mathrm{\Delta }\mathbf{X},\mathbf{\Theta }+\mathrm{\Delta }\mathbf{\Theta }\right)$$

(10)

The differential translation and rotation transformation $\delta {\mathbf{T}}_M^R$ can be written as

$$\delta {\mathbf{T}}_M^R=d{\mathbf{T}}_M^R{\left({\hat{\mathbf{T}}}_M^{\mathit{R}}\right)}^{-1}$$

(11)

Using the first-order approximation for the differential error matrix, the translation deviations d = [dx, dy, dz]^Tin robot frame due to parameter errors can be written in the following linear form [19]

$$\mathbf{d}={\mathbf{W}}_{\theta }\mathrm{\Delta }\mathbf{\theta }+{\mathbf{W}}_{\alpha }\mathrm{\Delta }\mathbf{\alpha }+{\mathbf{W}}_a\mathrm{\Delta }\mathbf{a}+{\mathbf{W}}_d\mathrm{\Delta }\mathbf{d}+{\mathbf{W}}_{\beta }\mathrm{\Delta }\mathbf{\beta }+{\mathbf{W}}_p\mathrm{\Delta }\mathbf{p}$$

(12)

where Δ θ, Δ a, Δ d, Δ α, Δ β, Δ p refer to the disturbances in robot parameters. After N measurements of the corresponded marker positions, the identification equation is constructed as

$$\mathbf{D}=\mathbf{J}\mathrm{\Delta }\mathbf{X}$$

(13)

in which $\mathbf{D}={\left[{\mathbf{d}}_1^T,{\mathbf{d}}_2^T\cdots {\mathbf{d}}_N^T\right]}^T$, d _i is the i _th measured marker position error in robot frame,

$${\mathbf{d}}_i={\mathbf{p}}_{\mathit{Mi}}^R-{\mathbf{R}}_T^R\left({\mathbf{d}}_{\mathit{Mi}}^T-{\mathbf{p}}_R^T\right)$$

(14)

$$\mathbf{J}={\left[{\mathbf{W}}_1^T{\mathbf{W}}_2^T\cdots {\mathbf{W}}_N^T\right]}^T$$

is the identification Jacobian matrix, each row block W _i refers to the i _th coefficient matrix of d _i , W _i  = [W _θi , W _αi , W _ai , W _di , W _βi , W _pi ]. The least-square estimation of robot parameter deviation Δ X is calculated by the pseudo-inverse matrix J^† of J,

$$\mathrm{\Delta }\mathbf{X}={\mathbf{J}}^{†}\mathbf{D}$$

(15)

then the robot parameters can be compensated by $\mathbf{X}=\widehat{X}+\mathrm{\Delta }\mathbf{X}$, $\mathbf{\Theta }=\widehat{\Theta }+\mathrm{\Delta }\mathbf{\Theta }$. The least square method tends to change the mechanical structure of robot completely when the estimated parameters deviate a lot from the actual ones. Only 5 rotational joint zero-positions, 4 link lengths and 3 marker positions are chosen to calibrate for consistency and simplicity in solving the inverse kinematic.

(iv). Simplified two-step scheme for robot-track registration

In this section, we introduce a simplified two-step registration scheme for both robot-tracker correspondence and robot calibration. The entire registration is summarized as follow.

Input: corresponded frame pairs, k = 1 ⋯ K of the optical makers measured via optical tracker and robot forward kinematics respectively;

Output: transformation ${\mathbf{T}}_R^T$ and robot parameters (X, Θ);

Initialization: robot parameters (X₀, Θ₀) are initialized by the nominal settings;

Iteration: for n = 1 to n_max or the registration error Σ converges, do

• 1. Compute the transformation matrix ${\mathbf{T}}_R^T$ by minimizing the object function (7);

• 2. Update the marker positions and deviation matrix using the newer robot kinematics F(X _n , Θ _n );

• 3. Calibrate and compensate robot parameters

  $$\begin{array}{c}\hfill {\mathbf{X}}_n={\mathbf{X}}_{n-1}+\mathrm{\Delta }\mathbf{X}\hfill \\ \hfill {\mathbf{\Theta }}_n={\mathbf{\Theta }}_{n-1}+\mathrm{\Delta }\mathbf{\Theta }\hfill \end{array}$$

  (16)

• 4. End the iteration when n = n _max or the decrease of the MSE below a threshold h.

Control scheme

With the image-guidance interface, the surgeon telemanipulated the robot to approach the insertion point manually in free space, needle alignment and interventional puncture are performed autonomously in accordance with the surgical planning. In this study, the haptic device acted as the master controller, while the 5R1P robot performed as the slave needle operator. The master and the slave were connected through a communication network.

(i). Master–slave control for manually needle approaching

Since the operation space of the master is not in proportion to that of the slave, a joint-joint velocity scaling control was applied to the master–slave system. Operations on the master side were scaled down to the slave side directly, the master joint velocities ${\dot{\Theta }}_{\mathit{\text{master}}}$ were mapped to the corresponding slave joint velocities ${\dot{\Theta }}_{\mathit{\text{slave}}}$ by

$$\begin{array}{ll}{\dot{\Theta }}_{\mathit{\text{slave}}}=\mathbf{\Lambda }{\dot{\Theta }}_{\mathit{\text{master}}}& \mathbf{\Lambda }=diag\left({\lambda }_1,{\lambda }_2,\cdots {\lambda }_6\right)\end{array}$$

(17)

where Λ is a scaling diagonal matrix, different scaling ratio was assigned to each joint pair according to their contributions to the translation and rotation of the end-effector. Small ratio helps reduce disturbances in manual input. The calculated joint velocities were then sent to the Mitsubishi alternate current servo-unit, all five joints were controlled simultaneously to approach the puncture point, the linear motor were controlled by safe button on the joystick of master separately for needle insertion.

(ii). Optical tracker feedback control for needle alignment

Inaccuracy of robot-tracker correspondence and robot parameters impacts the absolute precision severely when using the internal control system of the robot itself. But since the relative accuracy is better than the allowed tolerances the robot can be adjusted until the absolute accuracy is good enough [20, 21]. This section presents an optical tracker feedback control method to improve the accuracy of needle alignment for manual or robotic needle steering operations in soft tissue.

Once the needle tip approached the puncture point, autonomous needle alignment is conducted in accordance with the surgical planning ${\mathbf{T}}_{\mathit{EdR}}^T$ in tracker frame. In needle alignment stage, the needle shaft maintains straight and without touching the tissue, the needle tip pose measured by optical tracker is noted as ${\mathbf{T}}_{\mathit{EdC}}^T$ in tracker frame and ${\mathbf{T}}_{\mathit{EdC}}^R$ in robot frame. The robot reports nominal needle pose ${\mathbf{T}}_{\mathit{EdN}}^R$, which does not ensure accuracy due to disturbances in robot parameters Δ X and Δ Θ. Inaccuracies also appear in the measured pose ${\mathbf{T}}_{\mathit{EdC}}^R$ due to robot-tracker transformation error $\mathrm{\Delta }{\mathbf{T}}_R^T$. Deviations between the measured ${\mathbf{T}}_{\mathit{EdC}}^T$ and the reference ${\mathbf{T}}_{\mathit{EdR}}^T$ are noted as ${\mathbf{T}}_E^T$ in tracker frame and ${\mathbf{T}}_E^R$ robot frame.

$${\mathbf{T}}_{\mathit{EdN}}^R=\mathbf{F}\left(\widehat{X},\widehat{\Theta }\right)$$

$${\mathbf{T}}_{\mathit{EdC}}^R=\mathbf{F}\left(\widehat{X}+\mathrm{\Delta }\mathbf{X},\widehat{\Theta }+\mathrm{\Delta }\mathbf{\Theta }\right)={\left({\mathbf{T}}_R^T\right)}^{-1}{\mathbf{T}}_{\mathit{EdC}}^T$$

(18)

$${\mathbf{T}}_E^T={\mathbf{T}}_{\mathit{EdR}}^T{\left({\mathbf{T}}_{\mathit{EdC}}^T\right)}^{-1}$$

(19)

$${\mathbf{T}}_E^R={\left({\mathbf{T}}_R^T\right)}^{-1}{\mathbf{T}}_E^T$$

(20)

$${\mathbf{T}}_E^R={\mathbf{T}}_{\mathit{EdN}}^R{\left({\mathbf{T}}_{\mathit{EdC}}^R\right)}^{-1}={\left({\mathbf{T}}_R^T\right)}^{-1}{\mathbf{T}}_E^T{\mathbf{T}}_R^T$$

(21)

$${\mathbf{T}}_R^T={\hat{\mathbf{T}}}_R^{\mathit{T}}+\mathrm{\Delta }{\mathbf{T}}_R^T$$

(22)

The goal is to make ${\mathbf{T}}_{\mathit{EdC}}^T$ to be close to ${\mathbf{T}}_{\mathit{EdR}}^T$ as possible while robust to inaccuracy in robot-tracker calibration. $\widehat{X}$, $\widehat{\Theta }$ are estimated robot parameters, ${\widehat{T}}_R^T$ is estimated the robot-tracker transformation matrix, Δ X and Δ Θ, $\Delta {\mathbf{T}}_R^T$ are deviations. The goal is achieved by commanding the robot to a new pose iteratively by error compensation. The control scheme is shown in Figure 3. Here we outline the optical tracker feedback control scheme as follow (Figure 4).

Figure 4

Optical tracker feedback control for needle alignment.

For k =0 to k _max , do

• 1. Initialize the pose of end-effector ${\mathbf{T}}_{C0}^R={\left({\widehat{T}}_R^T\right)}^{-1}{\mathbf{T}}_{\mathit{EdR}}^T$ by the estimated ${\widehat{T}}_R^T$;

• 2. Solve the inverse kinematics of robot ${\mathbf{\Theta }}_k={\mathbf{F}}^{-1}\left(\widehat{X},\widehat{\Theta },{\mathbf{T}}_{\mathit{Ck}}^R\right)$, command the joints move to Θ _k ;

• 3. Measure the actual pose of end-effector ${\mathbf{T}}_{\mathit{EdC}}^R={\left({\widehat{T}}_R^T\right)}^{-1}{\mathbf{T}}_{\mathit{EdC}}^T$ using optical tracker, and compute the error,

  $${\mathbf{T}}_E^R={\left({\mathbf{T}}_{\mathit{EdC}}^R\right)}^{-1}{\mathbf{T}}_{\mathit{EdR}}^R={\left({\mathbf{T}}_{\mathit{EdC}}^T\right)}^{-1}{\mathbf{T}}_{\mathit{EdR}}^T$$

• 4. Modify a new command by error compensation ${\mathbf{T}}_{\mathit{Ck}+1}^R={\mathbf{T}}_{\mathit{Ck}}^R{\mathbf{T}}_E^{\mathit{Ed}}$, go to step 2;

• 5. Stop until iteration time k = k _max or the error below threshold h.

Results and discussion

Three experiments were conducted to validate the efficiency of robot-tracker registration method and the optical tracker feedback control for needle alignment task.

Robot-tracker calibration

The correspondence of robot-tracker as well as the robot parameters were calculated by the simplified two-step scheme proposed in section II. Nominal link parameters were listed in Table 1. Corresponded frame pairs $\left\{{\mathbf{T}}_M^R,{\mathbf{T}}_M^T\right\}$ of the maker that fixed on the end-effector were collected via optical tracker and robot nominal forward kinematics at 72 different configurations of joint settings (degree) θ₁ = 20i, i = − 1, 0, 1; θ₂ = − 90 + 20j, j = 0, 1; θ₃ = 20k, k = 0, 1; θ₄ = 15l, l = − 1, 0, 1; θ₅ = 45 + 15m, m = 0, 1. A geometrical robot-tracker calibration was conducted for comparison. The end-effector moved along semicircle paths by driving joint 1 and 2 individually, the orthogonal joint axes were calculated by circle fitting to estimate the robot base. Robot parameter calibration was carried out by the least square method using the corresponded pairs $\left\{{\mathbf{T}}_M^R,{\mathbf{T}}_M^T\right\}$. In this method, robot- tracker registration and robot calibration were conducted in sequence, additional data were required. Only 5 joint zero-positions, 4 link lengths and 3 positional parameters of the marker were chosen to calibrate. The calibrated robot parameters are listed in Table 2, there wasn’t obvious difference between these methods.

Table 1 The nominal parameters of robot

Table 2 Calibrated robot parameter

A fixed robot-tracker correspondence was used in the geometric method, while iterative searching for the optimal ${\mathbf{T}}_R^T$ was employed in the simplified scheme, the rotational and translational differences between the estimated matrices ${\mathbf{T}}_R^T$ were (0.0003,0.0016,0.0008) rad and (0.3399, -0.1184, -0.9176)mm. As shown in Figure 5, the registrated MSE errors of marker position were plotted, the simplified method performed better than the geometric method both in accuracy and speed. Residual error still remained after the registration procedure due to the linearization of error model and the inherited positioning error of the optical tracker. Open-loop control can’t eliminate these residual error, it is necessary to design a closed-loop scheme to compensate the influence caused by robot parameter deviations.

Figure 5

Registration error by two-step method and geometric method.

Optical tracker based error compensation experiment

This experiment is to evaluate how the errors in robot-tracker correspondence and robot parameters affect the needle alignment precision and how they are compensated with the optical tracker feedback control. Gaussian distributed N(0, σ²) disturbances are introduced to link lengths, joints zero-position, position of the optical marker and robot-tracker transformation matrix ${\mathbf{T}}_R^T$, the angular disturbance in joint angles and orientation of $\Delta {\mathbf{T}}_R^T$ ranges over (0 ~ 0.4) radians, while the length disturbance in robot links, marker position and translation part of $\Delta {\mathbf{T}}_R^T$ varies from 0 to 100 mm. Their influence on the precision of needle alignment were analyzed both independently and jointly. In this experiment, the robot was commanded to a fixed pose ${\mathbf{T}}_{\mathit{EdR}}^T$, the positional errors of needle tip and rotational errors of needle shaft were measured by optical tracker after the open loop positioning. The translational error δp refers to the deviation of needle tip P _N to the target P _R , rotational error is the difference δv between the actual orientation v _N of needle shaft and the referenced direction v _R ,

$$\mathit{\delta p}=∥{\mathbf{p}}_N-{\mathbf{p}}_R∥$$

$$\mathit{\delta v}=\mathit{arccos}\left({\mathbf{v}}_N,{\mathbf{v}}_R\right)\approx ∥{\mathbf{v}}_N-{\mathbf{v}}_R∥$$

the approximation holds only for small directional deviations. To compensate robot parameter disturbances, the robot was driven to the modified poses iteratively, and the minimum error was selected in 10 iterations with position threshold 0.2. In this case, the same target pose was used in both stages.

Figure 6(a-b) illustrate the influences of joint disturbances and rotational error of ${\mathbf{T}}_R^T$ individually, the final pose of the needle shaft goes far away from the reference dramatically as the disturbance level grows, the feedback scheme can limit these errors in a reasonable range. As shown in Figure 6(c-d), the positioning error grows linearly with disturbances in link lengths and translation part of ${\mathbf{T}}_R^T$, the feedback control performs stable over these variations.

Figure 6

Positioning error of the needle shaft measured by tracker before and after feedback compensation.

The influence of disturbance in robot parameters was also studied jointly. Table 3 outlines nine levels of combined disturbance in robot parameters. Table 4 lists the results of the feedback control for these cases. The results indicate that even with significant 10 centimeters error in link lengths and robot-tracker translational part, 0.45 radian in joint offsets and optical robot-tracker rotational part, 1 mm positioning accuracy and highly rotational precision can be easily achieved. Even though the feedback control is capable of compensating large range disturbances, the iteration times and correction step grows linearly with the disturbance level, a calibration process is necessary to reduce the initial error and limits the movement magnitude.

Table 3 Combined disturbance levels in robot parameters

Table 4 Error magnitudes of optical tracker feedback control

Robot assisted needle insertion experiment

A triple-modality (CT, MR, US) abdominal phantom model 057 from Computerized Imaging Reference Systems (CIRS) was used for the in vivo data Test. The internal structure of the model 057 includes partial abdominal aorta, partial vena cava, spine and two partial kidneys each with a lesion. The lesions are high contrast relative to the background in MR and can be barely identified in US.

First, the phantom was scanned with Siemens MAGNETOM Trio Tim 3.0 T machine, meanwhile the robot was calibrated to the optical tracker frame following the process in section 2. To avoid the accumulated US-MR registration error in robot assisted needle insertion experiment, 7 silicon square makers were attached to the surface of phantom, a rigid registration was employed to transform the MR image to optical tracker frame directly by the corresponded position pairs of the silicon markers both in MR image frame and optical tracker frame.

And then, six planned trajectories were defined, each including an entry point on the skin and a target point within a lesion near the left kidney. All trajectories were transferred into the optical tracker frame, the robot was commanded to complete the needle alignment operation autonomously using the optical tracker feedback control. Once the needle shaft was aligned along the planed direction outside the phantom, the linear motor was controlled to drive the needle to the desired depth by safe button on the joystick of master, a NiTi alloy wire guide(RFSPC-035145-0-I-AQ, Cook Urological Incorporated) was inserted into the target lesion through the trocar to trace the insertion trajectory afterwards(seen in Figure 7(a)). After all six insertions were finished, the phantom was scanned with the Siemens MAGNETOM Trio Tim 3.0 T machine again to evaluate the final insertion accuracy, the needle-target distance was measured based on the multiplanar reconstructed images (seen in Figure 7(b)).

Figure 7

Results of robot assisted needle insertion on kidney phantom.

The position and orientation of needle shaft were also measured after needle alignment by the optical tracker, Table 5 lists the results of robot assisted needle insertion experiment. The needle-target distance over the six insertion trails was 2.15 ± 0.17 mm, difference between the six tests was relatively small, indicating a repeatable performance for the six different insertion trajectories. The total needle insertion error come from the image-tracker registration error 1.13 ± 0.31 mm, optical tracker positioning error 0.18 ± 0.14 mm for passive rigid markers [22], robot assisted needle alignment error 0.24 ± 0.08 mm, needle deflection and phantom deformation.

Table 5 Results of robot assisted needle insertion experiment on kidney

In previous study [3], the accuracy of the volume navigation was evaluated via puncture tests on a customized phantom. The mean needle-target distance was 2.7 mm for the trials performed by an experienced radiologist, while 3.1 mm for a medical resident without experience. With the help of the optical tracker based feedback control, precise needle alignment could facilitate the follow-up manual needle insertion or robotic needle steering. When the positioning accuracy of tracking system increases, the absolute positioning accuracy of needle alignment will increase. However, in needle steering stage, the positioning information from the tracker was incorrect due to bending of needle shaft in soft tissue. Further work will use magnetic sensor to track the precise needle tip or intra-operation visual servoing technique, more dexterous needle steering inside tissue will be studied.

Conclusions

This paper presents an integrated needle operation robot system for percutaneous renal intervention. A simplified image-tracker-robot registration procedure was introduced. Variations in robot geometric parameters and tracker-robot correspondence account for the needle positioning accuracy of robot. An optical tracker feedback control was proposed and validated to compensate these disturbance for needle alignment. The accuracy is inherited from the optical positioning system. Experiments show that the control scheme is capable of providing accurate 3D needle alignment, and compensating wide range of disturbance in robot parameters and tracker-robot correspondence. Robot assisted needle insertion experiments were performed on kidney phantom, precise needle alignment could improve the precision of needle insertion. Robot-assisted needle steering has the potential to improve the accuracy through more dexterous control of the needle-tip trajectory, further work will involve tip/base needle manipulation for needle steering in soft tissue [23].