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

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.

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

(ii). Robot-Tracker correspondence

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

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)

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

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;

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

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.

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).

Robot-tracker calibration

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.

Optical tracker based error compensation experiment

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.

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)).

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.