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
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 and tracker-ultrasound transformation , 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 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.
(1)
(ii). Robot-Tracker correspondence
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
(2)
where and are marker positions observed in F
Robot
and F
Tracker
, rotation matrix are axis vectors x
M
, y
M
, z
M
that expressed in tracker frame, and refers to the axis expression in robot frame. They are ideally related by robot-tracker transformation matrix , shown as
(3)
Affected by measurement noise U and V
i
, i = 1, 2, 3, the expansions of (4) are
(5)
(6)
K corresponded pose pairs , k = 1 ⋯ K were recorded at different configurations of robot angle setting. Solving the optimal transformation typically requires minimizing a least square error criterion given by
(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 of maker can be read via robot forward kinematics,
(8)
(9)
in which, X = (a, d, α, β, p)T are link structural parameters, a = (a1, a2 ⋯ a6)T, d = (d1, d2 ⋯ d6)T, α = (α1, α2, ⋯, α6)T, β = (β1, β2, ⋯, β6)T, p = (p
x
, p
y
, p
z
)T are positions of the optical marker relative to the robot end-effector, Θ = (θ1, θ2, ⋯ θ6)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 β2 is necessary. The marker pose with nominal link parameters is noted as , the correct pose of the marker with kinematic errors is given by , it can be expressed as
(10)
The differential translation and rotation transformation can be written as
(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]
(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
in which , d
i
is the i
th
measured marker position error in robot frame,
(14)
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,
then the robot parameters can be compensated by , . 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 and robot parameters (X, Θ);
Initialization: robot parameters (X0, Θ0) are initialized by the nominal settings;
Iteration: for n = 1 to nmax or the registration error Σ converges, do
-
1.
Compute the transformation matrix 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
(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 were mapped to the corresponding slave joint velocities by
(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 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 in tracker frame and in robot frame. The robot reports nominal needle pose , which does not ensure accuracy due to disturbances in robot parameters Δ X and Δ Θ. Inaccuracies also appear in the measured pose due to robot-tracker transformation error . Deviations between the measured and the reference are noted as in tracker frame and robot frame.
(18)
(19)
(20)
(21)
(22)
The goal is to make to be close to as possible while robust to inaccuracy in robot-tracker calibration. , are estimated robot parameters, is estimated the robot-tracker transformation matrix, Δ X and Δ Θ, 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).
For k =0 to k
max
, do
-
1.
Initialize the pose of end-effector by the estimated ;
-
2.
Solve the inverse kinematics of robot , command the joints move to Θ
k
;
-
3.
Measure the actual pose of end-effector using optical tracker, and compute the error,
-
4.
Modify a new command by error compensation , go to step 2;
-
5.
Stop until iteration time k = k max or the error below threshold h.