Deukhee Lee

Alexandre Krupa
email: alexandre.krupa@inria.fr

Intensity-based visual servoing for non-rigid motion compensation of soft tissue structures due to physiological motion using 4D ultrasound

This paper presents a visual-servoing method for compensating motion of soft tissue structures using 4D ultrasound. The motion of soft tissue structures caused by physiological and external motion makes it difficult to investigate them for diagnostic and therapeutic purposes. The main goal is to track non-rigidly moving soft tissue structures and compensate the motion in order to keep a lesion on its target position during a treatment. We define a 3D non-rigid motion model by extending the Thin-Plate Spline (TPS) algorithm. The motion parameters are estimated with intensity-value changes of a points set in a tracking soft tissue structure. Finally, the global rigid motion is compensated with a 6-DOF robot according to the motion parameters of the tracking structure. Simulation experiments are performed with recorded 3D US images of in-vivo soft tissue structures and validate the effectiveness of the non-rigid motion tracking method. Robotic experiments demonstrated the success of our method with a deformable phantom.

I. INTRODUCTION

Medical imaging modalities make it possible to observe soft tissue structures non-invasively. Among them, ultrasound (US) imaging has many benefits. It is cheap, real-time, safe for human body, and non-interactive with ferromagnetic materials. For these reasons, US is the most widespread medical imaging modality.

Physiological motions such as respiration and heartbeat move soft tissue structures globally and deform them locally. Therefore this motion of soft tissue structures makes it difficult to investigate them for diagnostic and therapeutic purposes. Especially the target position of non-invasive therapy should follow the physiological motion of a moving lesion.

There are some literatures to deal with motion compensation using US imaging. In [START_REF] Gee | Sensorless freehand 3D ultrasound in real tissue: speckle decorrelation without fully developed speckle[END_REF] and [START_REF] Krupa | Real-time Motion Stabilization with B-mode Ultrasound Using Image Speckle Information and Visual Servoing[END_REF], speckle decorrelation is used to estimate elevational motion of 2D US probe, and Krupa et al. [START_REF] Krupa | Real-time Motion Stabilization with B-mode Ultrasound Using Image Speckle Information and Visual Servoing[END_REF] successfully compensate the soft tissue rigid motion with a 2D probe attached to a robot. In [START_REF] Mebarki | 2-D Ultrasound Probe Complete Guidance by Visual Servoing Using Image Moments[END_REF], an US image-based visual servoing method is presented to position a 2D probe on a desired organ section and track it thanks to the use of image moments obtained from contour segmentation. Nadeau tracks 3D rigid motion using a virtual 3D US probe in [START_REF] Nadeau | A multi-plane approach for ultrasound visual servoing : application to a registration task[END_REF]. In [START_REF] Harris | Speckle tracking in a phantom and feature-based tracking in liver in the presence of respiratory motion using 4D ultrasound[END_REF], 3D translational motions are estimated using 4D US. However, non-rigid motion is not considered in the above methods.

The contributions of this paper are to track 3D nonrigid motion using 4D US in real time and to compensate the motion. At our best knowledge, unlike other non-rigid registration procedures, our method is processed in real time. In the rest of this paper, a non-rigid motion model is defined using 3D Thin-Plate Spline (TPS). Then, an intensity-based tracking method is described to estimate the parameters of the motion model. Global rigid motion such as 3 translations and 3 rotations are extracted from non-rigid motion parameters. Finally, a 6 degree-of-freedom (DOF) robot equipped with a 4D US probe is controlled to compensate the rigid motion. Tracking accuracy is discussed from simulation experiments using in-vivo US images and the effectiveness of the proposed method is also verified from robotic experiments with a deformable tissue-mimicking (TM) phantom.

II. NON-RIGID MOTION TRACKING

Before compensating the 3D motion of soft tissue structures, a new method to estimate 3D motion of deformable soft tissue is proposed in this section.

A. 3D Scan Conversion

We use a SonixTOUCH Research 4D US scanner (Ultrasonix Medical Corporation, Canada) and a motorized US 3D transducer (Model: 4DC7-3/40, Ultrasonix Medical Corporation, Canada). Since the 4D US scanner is designed for the purpose of research works, we can access digital data before the conversion into an image. Afterwards, a set of volume data is converted into a volumetric image, called as 3D scan conversion.

For scan conversion, geometries of the probe (the 2D US transducer's radius R probe and the motor's radius R motor ) and imaging parameters (the number of sample data in a A-line N samples , the number of A-lines in a frame N lines , and the number of frames in a scan volume N f rames , angle between neighboring A-lines α line , and angle between neighboring frames α f rame ) are considered in [START_REF] Gee | Sensorless freehand 3D ultrasound in real tissue: speckle decorrelation without fully developed speckle[END_REF]. In Fig. 1, a sample s(i, j, k), which is the i-th datum along the j-th A-line in the k-th frame, is relocated into a point p(r, ϕ, θ ) in a volumetric image, which is represented as p(x, y, z) in Cartesian coordinates according to (1) and [START_REF] Krupa | Real-time Motion Stabilization with B-mode Ultrasound Using Image Speckle Information and Visual Servoing[END_REF].

Note that our 3D US probe continuously scans volumes while its motor is sweeping the volume in forward and backward directions. Additionally, sweeping direction d (which is 1 in the forward direction and 0 in the backward direction in (1c)) and motor's rotating motion should be considered. In (1c), we assumed that the motor stops during the time to scan A-line so that scan lines are straight.

r = D sample × i + R probe (1a) ϕ = -0.5α line (N lines -1) + α line j (1b) θ = (-1) d { α f rame N f rames 2 - α f rame N f rames N lines N f rames -1 ( j + N lines k)} (1c)
In (1a), D sample means the distance between neighboring two sample data.

x = (r cos ϕ -C) cos θ +C (2a) y = r sin ϕ (2b) z = (r cos ϕ -C) sin θ (2c)
In [START_REF] Krupa | Real-time Motion Stabilization with B-mode Ultrasound Using Image Speckle Information and Visual Servoing[END_REF], C is the offset distance from the probe's origin to the motor's origin, C = R probe -R motor .

B. Non-Rigid Motion Model

Soft tissue structures move and deform due to physiological motion. Therefore, we can model the motion of soft tissue as an affine motion part and a deformation part as shown in (3):

p 1 = A t 0 1×3 1 p 1 + n 0 (3)
where a point p = (x, y, z) T moves to a point p = (x , y , z ) T according to the affine motion matrix (the first matrix in the right side of the equation) and the deformation vector n = (n x , n y , n z ) T . The deformation vector n is modeled using TPS ( [START_REF] Bookstein | Principal Warps : Thin-Plate Splines and the Decomposition of Deformations[END_REF]):

n = n ∑ i=1 w i U(c i -p) ( 4
)
where n is defined with a number of n control points c i = ( xi , yi , ži ) T and their weights w i = (w x i , w y i , w z i ) T . The base function U(s) usually denotes the Euclidean norm of s in 3D cases.

As a result, the non-rigid motion model (3) is the threedimensional extended version (ℜ 3 → ℜ 3 ) of TPS warping models (ℜ 2 → ℜ 2 and ℜ 2 → ℜ 3 , respectively) presented in [START_REF] Lim | A Direct Method for Modeling Non-Rigid Motion with Thin Plate Spline[END_REF] and [START_REF] Richa | Three-dimensional Motion Tracking for Beating Heart Surgery Using a Thin-plate Spline Deformable Model[END_REF]. The parameters of the motion model, ( A, t, and W = {w 1 , w 2 , . . . , w n }), are estimated with a set of points P = {p 1 , . . . , p n } in the reference image and the corresponding set of points P = {p 1 , . . . , p n } in sequential images. As the set of initial control points C moves to the corresponding set of control points C , the points p ∈ C also move to the points p ∈ C . Therefore, the parameters are given by the relationship:

L C C T 0 4×4 W t A T = C 0 4×3 (5)
where L is a n × n matrix, whose element is

L i j = U(c j -c i ). C =    1 x1 y1 ž1 . . . . . . . . . . . . 1 xn yn žn    is a initial n × 4 control points matrix and C = [ c 1 . . . c n ] T is a n × 3 destination control points matrix.
According to [START_REF] Bookstein | Principal Warps : Thin-Plate Splines and the Decomposition of Deformations[END_REF], the motion model ( 3) is rewritten as [START_REF] Bookstein | Principal Warps : Thin-Plate Splines and the Decomposition of Deformations[END_REF].

p T = M(p)K * (C)C (6)
where

M(p) = [ U(p -c 1 ) . . . U(p -c n ) 1 x y z ]
and K * (C) denotes the (n + 4) × n sub-matrix of the inverted form of the leftmost matrix in [START_REF] Harris | Speckle tracking in a phantom and feature-based tracking in liver in the presence of respiratory motion using 4D ultrasound[END_REF]. When a set of control points C and a set of point P are initialized as C 0 and P 0 respectively, M(P 0 ) and K * (C 0 ) in ( 6) become constant matrices. Therefore, the corresponding points P obtained after deformation are just up to the set of moving control points C . This means that the control points can be considered as the motion parameters of the non-rigid motion model:

P T = M(P 0 )K * (C 0 )C .

C. Intensity-Based Tracking

Now, we propose to use the parametric motion model defined in the previous section to track a deformable object from a sequence of 3D US images. Based on the same principle of the 2D region tracking method presented in [START_REF] Hager | Efficient region tracking with parametric models of geometry and illumination[END_REF] we define a parametric motion model:

p = f (p 0 , µ µ µ(t)) (7)
where p 0 and p represent respectively the initial and transformed coordinates of a 3D point, and µ µ µ(t) is a motion parameters vector at time t.

In our non-rigid motion model ( 6), we define

µ µ µ(t) = [ x1 (t) y1 (t) ž1 (t) . . . xn (t) yn (t) žn (t) ] T from a set of n control points C at time t.
We define a tracking region with a set of intensity values at several 3D points P within the target region in a volumetric image. This tracking region I is a function of a set of initial coordinates of P and the µ µ µ vector at time t given by:

I(P 0 , µ µ µ(t)) =      I( f (p 0 1 , µ µ µ(t))) I( f (p 0 2 , µ µ µ(t))) . . . I( f (p 0 N , µ µ µ(t)))      (8)
where I(p i ) denotes the intensity value at the location p i = (x i , y i , z i ) T at time t and N is the number of 3D points. Now, we define a Jacobian matrix J µ µ µ , which links the time variation of the tracking region I to the time variation of the motion parameters vector µ µ µ:

∂ I ∂t = J µ µ µ dµ µ µ dt = J µ µ µ v µ µ µ (9)
where v µ µ µ is the velocity vector of motion parameters. The Jacobian matrix J µ µ µ can be calculated from the motion model, p = f (p 0 , µ µ µ(t)), and the gradient values ∇ P 0 I of the tracking region I at time t as shown in [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF].

J µ µ µ = ∂ I ∂ µ µ µ = ∂ I ∂ P 0 × ∂ P 0 ∂ P × ∂ P ∂ µ µ µ = ∇ P 0 I × f -1 P 0 × f µ µ µ (10)
where the gradient values ∇ P 0 I can be measured using a 3 × 3 × 3 or 5 × 5 × 5 Sobel filter. f P 0 and f µ µ µ are calculated according to [START_REF] Umeyama | Least-Squares Estimation of Transformation Parameters Between Two Point Patterns[END_REF] and [START_REF] Wilson | Relative end-effector control using cartesian position based visual servoing[END_REF].

f p 0 =    ∂ x ∂ x 0 ∂ x ∂ y 0 ∂ x ∂ z 0 ∂ y ∂ x 0 ∂ y ∂ y 0 ∂ y ∂ z 0 ∂ z ∂ x 0 ∂ z ∂ y 0 ∂ z ∂ z 0    =       ∂ M ∂ x 0 ∂ M ∂ y 0 ∂ M ∂ z 0    K * C    T (11) f µ µ µ =    ∂ x ∂ x1 ∂ x ∂ y1 ∂ x ∂ ž1 . . . ∂ x ∂ xn ∂ x ∂ yn ∂ x ∂ žn ∂ y ∂ x1 ∂ y ∂ y1 ∂ y ∂ ž1 . . . ∂ y ∂ xn ∂ y ∂ yn ∂ y ∂ žn ∂ z ∂ x1 ∂ z ∂ y1 ∂ z ∂ ž1 . . . ∂ z ∂ xn ∂ z ∂ yn ∂ z ∂ žn    =   c 1 0 0 . . . c n 0 0 0 c 1 0 . . . 0 c n 0 0 0 c 1 . . . 0 0 c n   (12)
where c i is the i-th element of (MK * ) T .

The strategy of the intensity-based region tracking is to minimize the error e(P 0 , µ µ µ(t)) = I(P 0 , µ µ µ(t)) -I * , where I * is the reference target region fixed at time t 0 : I * = I(P 0 , µ µ µ(t 0 ))

Considering an exponential decrease of the error, let the time variation of the error ė = -λ e where λ is the proportional coefficient involved in the exponential convergence of e. ė is the same with the time variation of the tracking region: ė = ∂ I ∂t . As a result, the velocity vector of motion parameters is given by:

v µ µ µ = -λ J + µ µ µ (I -I * ) (13)
where J + µ µ µ is the Moore-Penrose pseudo-inverse of J µ µ µ , that is

J + µ µ µ = (J T µ µ µ J µ µ µ ) -1 J T µ µ µ if N > n.
Finally, motion parameters are estimated as µ µ µ(t + τ) = µ µ µ(t) + v µ µ µ τ where τ is the sampling period of tracking processes.

∇ P 0 I and f P 0 changes according to µ µ µ(t) even though P 0 is initialized and fixed. In practice, it is time-consuming to calculate J µ µ µ and J + µ µ µ every time because the size(N × 3n) of J µ µ µ is usually very large. So, we may use the approximation of J µ µ µ using initial ∇ P 0 I and initial f P 0 at time t 0 . In this case, the approximation of the pseudo-inverse J + µ µ µ is fixed and used instead of J + µ µ µ in (13).

III. VISUAL SERVOING WITH 4D ULTRASOUND

A 6-DOF robot equipped with a 4D US probe is used to compensate the motion of a target region in soft tissue structures. Therefore, only rigid motions such as 3D translations and 3D rotations are considered for the motion compensation. Nevertheless, a non-rigid motion model is necessary in order to track the deformable region. In this section, a rigid-motion extraction method and a 3D US position-based visual servoing approach are described.

A. Rigid Motion Extraction from Motion Parameters

The motion parameters of the non-rigid model above proposed are a set of control points. A rotation matrix R and a translation vector t describing the rigid motion are calculated using the following Least-Squares method with a set of initial control points C 0 and a set of corresponding control points C estimated by the non-rigid motion model. So, we will find R and t that minimize the following mean square error:

e 2 (R, t, s) = 1 n n ∑ i=1 c i -(sRc 0 i + t) 2 ( 14
)
where s is a scale factor, n denotes the number of control points, and c 0 i = ( x0 i , y0 i , ž0 i ) T and c i = ( xi , yi , ži ) T are an initial control point and its corresponding control point observed in the current 3D image. The solution of ( 14) is given by [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF] [11] as explained below.

Let

c 0 = 1 n n ∑ i=1 c 0 i , c = 1 n n ∑ i=1 c i (15a) σ 2 c 0 = 1 n n ∑ i=1 c 0 i -c 0 2 , σ 2 c = 1 n n ∑ i=1 c i -c 2 (15b)
, and

Σ c 0 c = 1 n n ∑ i=1 (c 0 i -c 0 )(c i -c) T . ( 15c
)
Now, we can calculate R, t, and s using a singular value decomposition of the covariance matrix Σ c 0 c expressed by UΣ Σ ΣV T . R = USV T , t = c -sRc 0 , and s = 1

σ 2 c 0 tr(Σ Σ ΣS) (16)
where

S = I , if det(U)det(V) = 1 diag(1, 1, . . . , 1, -1) , else .

B. 4D Ultrasound-Based Visual Servoing

We used a position-based visual servo control (PBVS) scheme described in [START_REF] Wilson | Relative end-effector control using cartesian position based visual servoing[END_REF] since the relative probe pose with respect to the soft tissue structures to track is calculated from the 3D rigid motion (R and t) of a target region extracted in a sequence of 3D US images. The sequential 3D US images are acquired from a 4D US probe that is mounted on the endeffector of a 6-DOF robot. We calculate the probe control velocity v c = (v c , w c ) of the 4D US probe according to the 3D rigid motion of the target region where v c and w c are the translational velocity vector and the angular velocity vector of the the probe frame. Coordinate frames for position-based visual servo using 4D ultrasound

We set a probe frame F c and an object frame F o in the center of the initial target region and the current target region, respectively shown in Fig. 2. The objective of the PBVS is to move the probe to the center of the current target region in such a way to align the probe frame F c on the object frame

F o .
The image feature s is defined as ( c * t c , θ u), in which c * t c and θ u are a translation vector and the angle/axis parameterizations for the rotation matrix c * R c which give the coordinate of the origin and the orientation of the probe frame F c expressed in the desired probe frame F c * to achieve.

c * t c and c * R c are given from the extracted rigid motion t and R of a target region as:

c * t c = -t, c * R c = R -1 .
The strategy of the PBVS control scheme is to minimize the error e(t) between the current image feature (s(t) = ( c * t c , θ u)) and the desired image feature (s * = 0):

e(t) = s(t) -s * = s(t)
The variation of s with respect to the velocity of the probe is given by:

ṡ = L s v c = c * R c O O L θ u v c w c ( 17
)
where L s is the 6 × 6 interaction matrix related to s.

The time variation of the error e is the same as the variation of s: ė = L e v c where L e = L s . We apply the following control law to decrease the error e exponentially, ė = -λ e.

v c = -λ L -1 e e = -λ c * R c T O O L -1 θ u c * t c θ u (18)
Therefore,

v c = -λ c * R c T c * t c , and
w c = -λ θ u (19) since L -1 θ u θ u = θ u.
Target motion component can be added to the above control law in order to enhance target tracking performance. In this case, the control law becomes:

v c = -λ L -1 e e -L -1 e ∂ e ∂t
where ∂ e ∂t is an approximation of the time variation of e due to the target motion, that we estimate thanks to a Kalman filter

o(t) M o(t-∆t) = o(t) M c(t) • c(t-∆t) M -1 c(t) • o(t-∆t) M -1 c(t-∆t) (20)
where a M b defines the homogeneous transformation matrix from frame b to frame a.

Note that the tracking target region I should be adjusted every control loop as shown in the control scheme presented in Fig. 3 because the robot compensates only the rigid motion part and the target region is not fully compensated. All the points P in the target region and all the control points C are adjusted according to the homogeneous matrix v(t-∆t) M v(t) describing the relative motion of the 3D US image frame F v during time ∆t:

c i (t) 1 = v(t-∆t) M -1 v(t) c i (t -∆t) 1 , ∀i ∈ {1, . . . , n} (21a) p i (t) 1 = v(t-∆t) M -1 v(t) p i (t -∆t) 1 , ∀i ∈ {1, . . . , N} (21b)
IV. SIMULATION RESULTS We simulated 4DUS in the presence of respiratory motion for the ground truth. The deformed images of a target region in 3D US images are generated using the non-rigid motion model above mentioned. We put a 19 × 19 × 19 grid (called as a warping grid) in a warping region and 3 × 3 × 3 control points (called as warping control points) in a target region of a 3D US image captured from an in-vivo human kidney as shown in Fig. 4. As each warping control point moves according to the respiratory motion model describe in the below section, all the vertices of the warping grid are relocated according to the motion model [START_REF] Bookstein | Principal Warps : Thin-Plate Splines and the Decomposition of Deformations[END_REF]. Then, the 3D texture in each cell of the initial warping grid is mapped into the corresponding cell of the current warping grid.

The visual tracking algorithm was verified in a sequence of simulated 3D US images. We put control points for tracking at the same positions as the warping control points. The tracking results were assessed from the known positions of the warping control points. Furthermore, visual servoing was performed in the simulated environment to compensate the rigid motion.

Respiratory motion was modeled as (22) according to [START_REF] Harris | Speckle tracking in a phantom and feature-based tracking in liver in the presence of respiratory motion using 4D ultrasound[END_REF].

c i (t) = c 0 i + a -b cos 2n (πt/τ -φ ) + η η η i i i (22)
where t, a, b, τ, φ , n, and η η η i i i are the time in seconds, the position vector at inhale, the peak-to-peak extent vector of 4. While all the tracking control points follow the motions induced by all the warping control points, the image features s of a target region and a warping region are extracted. As a result, the tracking error was the mean absolute values of (0.32, 0.33, 0.17, 0.05, 0.06, 0.04) in mm and degree units as we can see in Fig. 5. Furthermore, a servoing task to compensate the rigid motion was also performed successfully. The result are given in Fig. 6 and Fig. 7 with the maximum servo tracking error e max = (1.89, 3.28, 2.05, 0.74, 0.68, 1.03) in mm and degree units.

! " #! #" $! $" %! %" #! " ! " #! &'()*+,)-./0,1 2*3.,'&'./*+((1 ! " #! #" $! $" %! %" #! " ! " #! &'()*+,)-./0,1 4*3.,'&'./*+((1 ! " #! #" $! $" %! %" #! " ! " #! &'()*+,)-./0,1 5*3.,'&'./*+((1 ! " #! #" $! $" %! %" $ # ! # $ &'()*+,)-./0,1 2*.6')/&7&'./*+0)81 ! " #! #" $! $" %! %" $ # ! # $ &'()*+,)-./0,1 4*.6')/&7&'./*+0)81 ! " #! #" $! $" %! %" $ # ! # $ &'()*+,)-./0,

V. ROBOTIC EXPERIMENTS

A 4D US probe (model: 4DC7-3, Ultrasonix, Canada) was mounted on the end effector of a 6-DOF anthropomorphic robot to compensate the periodic motion induced by another robot as shown in Fig. 8. We used two types of TM phantoms as a target object, which are an US abdominal phantom (model: ABDFAN, Kyoto kagaku, Japan) and a deformable phantom made of gelatin by ourselves. At first, we perform a rigid motion tracking task with an abdominal phantom. The secondary robot repeatedly rotated the abdominal phantom on a turning table in one direction and the opposite direction. In the meantime, the 6-DOF robot holding the 4D US probe is controlled by our method to automatically compensate the rigid motion of the target region within the phantom. The observed feature error and probe trajectory are shown in Fig. 9 and Fig. 10 (left). To maintain the firm contact between the probe and the phantom, we used a force control along the X axis of the 3D US image.

Secondly, a non-rigid motion tracking task was performed with a deformable phantom. The secondary robot repeatedly compresses/releases the deformable phantom laid in a drawer, and the 6-DOF robot conducted the tracking task as shown in Fig. 8 (right). Fig. 10 presents the evolution of the probe motion and Fig. 12 shows the observed feature error during the automatic compensation task performed with the deformable phantom. The visual tracking of the target region during the compression stage was successfully performed as shown in Fig. 11.

At the above experiments, sequential 3D US images were acquired at the rate of 4.6 volumes/second. The control loop time was 100 milliseconds. In order to perform all the processes explained in the section II and III such as 3D scan conversion, the non-rigid motion estimation and the rigid motion extraction in the control loop time (100 ms), we implemented them using nvidia CUDA.

VI. CONCLUSIONS

This paper has presented a method to compensate 3D non-rigid motion of soft tissue structures in the presence of respiration using 4D ultrasound. Motion parameters are estimated with respect to the changes of intensity values under the multiple tracking points within a target region. The rigid motion of the target region extracted from the motion parameters is compensated with a 6-DOF robot equipped with a 4D US probe. The non-rigid motion tracking ,+5.56+417()=: >1.>68 ?1.>68 @1.>68 Fig. 12. Feature error that corresponds to the pose of a target region with respect to the current probe frame -case of a deformable phantom method was validated in simulation by tracking the warping motion of an US volumetric image captured from in-vivo soft tissue. 4D US-based visual servoing tasks were performed successfully in the simulated deformations of a 3D US image. Furthermore, robotic experiments demonstrated nonrigid motion compensation with a deformable TM phantom in real time.

Fig. 1 .

 1 Fig. 1. 3D scan data (left) are reconstructed into a 3D ultrasound image (right).

  Fig. 2.Coordinate frames for position-based visual servo using 4D ultrasound

Fig. 3 .

 3 Fig. 3. 4D ultrasound-based visual servoing structure

!Fig. 4 .

 4 Fig. 4. Tracking of a target region (right) in a 3D US image deformed with a respiratory motion model (left)

Fig. 5 .

 5 Fig. 5. Global rigid motion tracking results of a deformable region using simulated 4D US

!Fig. 6 .Fig. 7 .

 67 Fig. 6. Rigid motion compensation (right) in simulated environment (left)

Fig. 8 . 6 -Fig. 9 .Fig. 10 .

 86910 Fig. 8. 6-DOF robot holds a 4D-US probe, and the other robot moves an abdominal phantom (left) and compresses/releases a deformable phantom (right)

Fig. 11 .

 11 Fig. 11. Successive images obtained during the target region tracking process

ACKNOWLEDGMENT

The authors acknowledge the support of the ANR project USComp of the French National Research Agency


Caroline Nadeau

Alexandre Krupa

Intensity-based direct visual servoing of an ultrasound probe

Keywords: Visual servoing, ultrasound guided robotic system, intensity-based control

This paper presents a new image-based approach to the control of a robotic system equipped with an ultrasound imaging device. For diagnostic applications, the proposed method makes it possible to position an ultrasound probe on a desired organ section and to track it by compensating for rigid motions of the organ. Both in-plane and out-of-plane motions of the probe are controlled by the proposed method. The main contribution of this work is the direct use of the ultrasound image as visual feature which spares any segmentation or image processing time consuming step. Simulation and robotic experiments are performed on a realistic abdominal phantom and validate this ultrasound intensity-based visual servoing approach.

I. INTRODUCTION

Among the different medical imaging modalities, ultrasound (US) imaging has many benefits for the patients as well as for the specialists. Indeed, this modality is cheap, real time and contrarily to MRI or CT devices, the US transducer is not cumbersome and can be easily used in an operating room. Moreover US waves are safe for human body and do not interact with ferromagnetic medical instruments. Therefore the use of US imaging during a medical intervention does not bring any additional constraint. Because of such advantages, US is a promising imaging modality to deal with image guided robotized systems. However the information carried by an US slice is far different from the one carried by a camera view which is traditionally used in visual control. In particular, the major remaining challenges in US visual servoing concern the image processing and the control of the out-of-plane motions of the US device.

Previous works dealing with US image-based robotic systems mainly focus on two different system configurations. In the eye-to-hand configuration, the US probe is observing a surgical instrument mounted on the robot end-effector. The robotic manipulation offers a better accuracy than the human one and the proposed applications concern needle insertion procedures [START_REF] Hong | A motion adaptable needle placement instrument based on tumor specific ultrasonic image segmentation[END_REF] or cardiac surgery [START_REF] Novotny | Real-time visual servoing of a robot using three-dimensional ultrasound[END_REF], [START_REF] Vitrani | Robust ultrasoundbased visual servoing for beating heart intracardiac surgery[END_REF]. In [START_REF] Hong | A motion adaptable needle placement instrument based on tumor specific ultrasonic image segmentation[END_REF], two degrees of freedom (dof) of a needle-insertion robot are controlled by visual servoing to perform a percutaneous cholecystostomy while compensating involuntary patient motions. The target and the needle are automatically segmented in the US images and their respective poses are used to guide the robot. In [START_REF] Novotny | Real-time visual servoing of a robot using three-dimensional ultrasound[END_REF], a robotic system is proposed to track a surgical instrument C. Nadeau is with Université de Rennes I, IRISA, INRIA Rennes-Bretagne Atlantique, Lagadic research group, 35042 Rennes, France. A. Krupa is with INRIA Rennes-Bretagne Atlantique, IRISA, Lagadic research group, 35042 Rennes, France Caroline.Nadeau@irisa.fr, Alexandre.Krupa@inria.fr and move it to a desired target. 3D US images are processed to localize here again the respective positions of the target and the instrument tip, then the position error is used to control the surgical robot. In [START_REF] Vitrani | Robust ultrasoundbased visual servoing for beating heart intracardiac surgery[END_REF], the four dof of a surgical forceps inserted in a beating heart through a trocar are controlled by US image-based visual servoing. In relation with this work, the authors of [START_REF] Sauvee | US image based visual servoing of a surgical instrument through non-linear model predictive control[END_REF] developed a predictive control scheme to keep the forceps visible in the US image.

The other configuration, namely eye-in-hand configuration, allows the direct control of the US sensor mounted on the robot end-effector for diagnostic purpose [START_REF] Abolmaesumi | Image-guided control of a robot for medical ultrasound[END_REF], [START_REF] Mebarki | 2D ultrasound probe complete guidance by visual servoing using image moments[END_REF] or medical procedure [START_REF] Lee | Ultrasound-based visual servoing system for lithotripsy[END_REF]. In [START_REF] Abolmaesumi | Image-guided control of a robot for medical ultrasound[END_REF], the three in-plane dof of a robotic system are controlled to maintain a visual feature centered in the US image during a manual out-of-plane translation of the US probe. In [START_REF] Lee | Ultrasound-based visual servoing system for lithotripsy[END_REF], two US probes and a HIFU transducer are mounted on the end effector of a XYZ stage robot to follow a target kidney stone while compensating physiological motions. For a positioning task, [START_REF] Mebarki | 2D ultrasound probe complete guidance by visual servoing using image moments[END_REF] proposed a method to automatically reach a desired cross section of an organ of interest by servoing the six dof of a 2D US probe.

In order to control one to six dof of the robotic manipulator, the efficiency of the visual servoing approaches is highly dependent on the choice of appropriate image features. Depending on the configuration, these features can be created by the intersection of the surgical tool with the US plane (eye-to-hand configuration) [START_REF] Hong | A motion adaptable needle placement instrument based on tumor specific ultrasonic image segmentation[END_REF]- [START_REF] Sauvee | US image based visual servoing of a surgical instrument through non-linear model predictive control[END_REF] or by anatomical landmarks (eye-in-hand configuration) [START_REF] Abolmaesumi | Image-guided control of a robot for medical ultrasound[END_REF]- [START_REF] Mebarki | 2D ultrasound probe complete guidance by visual servoing using image moments[END_REF].

In robotic systems where the US probe itself is controlled, which are more particularly within the scope of this work, the image features can only be anatomic ones. In [START_REF] Abolmaesumi | Image-guided control of a robot for medical ultrasound[END_REF], five features extraction methods are compared to track an anatomic point which is the center of an artery in order to servo the in-plane motions of the probe. These methods are based on image similarity measures such as cross correlation and Sequential Similarity Detection (SSD) or on contour segmentation by a Star [START_REF] Friedland | Automatic ventricular cavity boundary detection from sequential ultrasound images using simulated annealing[END_REF] or Snake algorithm. In another work [START_REF] Lee | Ultrasound-based visual servoing system for lithotripsy[END_REF], the translational motions of a robotic effector are controlled using the center position of a segmented renal stone. Recently, a few authors provided solutions to control the six dof of the probe. In [START_REF] Krupa | Real time motion stabilization with B-mode ultrasound using image speckle information and visual servoing[END_REF], an approach based on the speckle correlation observed in successive US images is detailed. However only tracking tasks can be considered with such approach. Then, different approaches have been proposed to perform positioning tasks using six geometric features built from 2D moments extracted from a single US image [START_REF] Mebarki | 2D ultrasound probe complete guidance by visual servoing using image moments[END_REF] or three orthogonal images [START_REF] Nadeau | A multi-plane approach for ultrasound visual servoing: application to a registration task[END_REF]. However the moment computation requires a previous contour segmentation step whose efficiency is dependent on the organ shape and which is not robust to organ topology changes.

In this paper we propose to avoid any image processing step by using directly the US image intensity in our visual servoing approach. In this case, the visual features are the set of image pixel intensities. The contribution of this paper is therefore to provide an efficient model of the interaction between the variation of these features and the velocity of an actuated 2D US probe to control the six dof of this probe.

The structure of our paper is as follows. We initially describe our US intensity-based approach and detail the computation of the interaction matrix to control in-plane but also out-of-plane motions of the probe in section II. In section III, we present results of the proposed approach for positioning and tracking tasks performed in a simulation environment. Finally, in section IV, a robotic tracking experiment involving a hybrid force/vision control demonstrates the validity of the approach in real environment.

II. ULTRASOUND VISUAL SERVOING

Traditional visual servoing methods refer to vision data acquired with a camera mounted on a robotic system. In this case, the vision sensor provides a projection of the 3D world into a 2D image and the coordinates of a set of 2D geometric primitives can be used to control the six dof of the system. However, a 2D US transducer provides complete information in its image plane but not any outside of this plane. Therefore, the interaction matrix relating the variation of the chosen visual features to the probe motion is far different and has to be modeled.

A. The control law

An image-based visual servoing control scheme consists in minimizing the error e(t) = s(t)s * between a current set of visual features s and a desired one s * . Considering an exponential decrease of this error, the classical control law [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF] is given by:

v c = -λ L s + (s(t) -s * ) , (1)
where λ is the proportional gain involved in the exponential decrease of the error (ė = -λ e). In an eye-in-hand configuration, v c is the instantaneous velocity applied to the visual sensor and L s + is the pseudo-inverse of an estimation of the interaction matrix L s that relates the variation of the visual features to the velocity v c .

According to [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], the control scheme ( 1) is known to be locally asymptotically stable when a correct estimation L s of L s is used (i.e., as soon as

L s L s -1 > 0).

B. Selection of intensity features

In this paper, we propose to avoid any US image preprocessing step by choosing as visual feature the image itself. In this case, the visual features considered are the intensity value of each pixel contained in the US image. The size of the image features vector s is therefore equal to the size of the US image I(r) acquired at the pose r ∈ SE 3 of the imaging device.

s(r) = {I r (u, v), ∀ (u, v) ∈ [1, M] × [1, N]} (2)
where M and N are respectively the width and the height of the US image and where I r (u, v) represents the intensity of the pixel of coordinates (u, v) in the image I(r).

C. Computation of the interaction matrix

In a B-mode US scan, the intensity of the US signal is represented in term of pixel luminance intensity. The higher this luminance, the higher the US wave reflexion. Then as the US reflexion only depends on the organ structure and interfaces, its value remains roughly constant for a given anatomic micro structure. As a result, we will consider that the luminance intensity in a B-mode US image of a physical 3D point remains also constant during a time interval dt.

As stated in a previous work dealing with photometry based approach for camera images [START_REF] Collewet | Visual servoing set free from image processing[END_REF], the hypothesis of intensity conservation allows us to link the time variation of the features vector s(r) to the motion of the imaging device. As the considered visual features belong to the US image plane, their 3D coordinates x expressed in the probe frame can be computed from the image pixel coordinates:

x = (x, y, z) ⊤ = (s x (u -u 0 ), s y (v -v 0 ), 0) ⊤ ,
with (s x , s y ) the image pixel size and (u 0 , v 0 ) the pixel coordinates of the image center. Given such a 3D point expressed in the imaging device frame and dx = (dx, dy, dz) ⊤ an elementary motion of this point in the 3D space, the intensity conservation is expressed by the relationship:

I r (x + dx, y + dy, z + dz,t + dt) -I r (x,t) = 0. (3)
A first order Taylor series expansion of (3) yields to:

∂ I r ∂ x dx + ∂ I r ∂ y dy + ∂ I r ∂ z dz + ∂ I r ∂t dt = 0. (4)
Then, the time variation of each image feature I r (u, v) can be expressed as a function of the 3D point motion :

İr = -(∇I x ∇I y ∇I z )   ẋ ẏ ż   , (5)
with ∇I = (∇I x ∇I y ∇I z ) ⊤ the 3D image gradient. The interaction matrix L I of size 1 × 6 associated to each visual feature is then:

L I = -(∇I x ∇I y ∇I z ) L x , (6)
where L x relates the velocity of a 3D point x to the motion of the US probe v c according to the kinematics fundamental relationship:

ẋ = L x v c , L x =   -1 0 0 0 -z y 0 -1 0 z 0 -x 0 0 -1 -y x 0   . ( 7
)
The final interaction matrix L s used in the control law is built by stacking all the previously defined matrices L I associated to each pixel of the considered sub image I(r).

D. Image gradient computation

To control the six dof of the US probe, the variation of the visual features is related to both in-plane and out-of-plane motions of the probe. In our US intensity-based approach, this variation is given by the 3D image gradient, which is computed from the current probe image and at least two parallel additional images.

With a conventional 2D US probe mounted on a robotic arm, a small back and forth translational motion along the elevation direction can be applied before each iteration of the visual control algorithm in order to acquire these additional images I a and I b . We design then a set of three 3D Gaussian derivatives filters of size 3 × 3 × 3 applied in each pixel of the current image I 0 to compute the 3D gradient (see Fig. 1).

III. SIMULATION VALIDATION

To validate our approach, we use a software simulator that we have developed to reconstruct and display a dense volume from a set of parallel images. In addition to this display functionality, the simulator allows us to move the reconstructed volume with respect to a fixed cartesian frame and to control a 2D virtual probe which generates an US image by cubic interpolation process.

For the simulation experiments, an US complete volume of the right kidney of a realistic abdominal phantom is loaded in the simulator (see Fig. 2). This volume is created from a set of 335 250 × 250 parallel images, the voxel size is 0.6 × 0.6 × 0.3mm 3 .

A. Positioning task

We first simulate a positioning task, using the simulation environment to obtain a ground truth of the evolution of the pose error of the US probe. We position the probe on the kidney volume and we consider the corresponding US scan as the desired one. In the same time, the corresponding pose of the probe in the simulator frame r * is saved. Then the probe is moved away to a new pose where the observed organ section is considered as the initial image.

To avoid continuous out-of-plane motions of the 2D US probe required to compute the 3D image gradient during the visual servoing process, we propose to use the interaction matrix estimated at the desired pose L s * in the control law. However, with such an approximation of the interaction matrix, the convergence of the control law (1) is not guaranteed from a far initialization. We propose then to solve the minimization problem with the Levenberg-Marquardt algorithm which is a combination of the Gauss-Newton algorithm with the steepest descent method and ensures a better convergence than the Gauss-Newton one (1) from a far initialization. The implemented control law for the positioning task is then:

v c = -λ (H + µdiag(H)) -1 L s * ⊤ (s(t) -s * ) ,
with H = L s * ⊤ L s * and µ = 0.05. The 3D gradient components are computed with 5 × 5 × 5 filters by using four additional images. These filters, built on the same Gaussian derivative model than the 3 × 3 × 3 filters described in Fig. 1, increase the robustness of the control scheme. The Fig. 3 shows the visual convergence of the algorithm. The images difference between the current and the desired US scans are displayed for the initial and final probe poses. The uniform gray color of this images difference observed after the convergence of the algorithm proves the success of the positioning task since the final image perfectly coincides with the desired one. The behavior of the automatic positioning is shown through the evolution of the probe pose error (see Fig. 4). We also define an image error measure C (r) = (s(r)s(r * )) ⊤ (s(r)s(r * )) to observe the minimization of the visual features. Without any image processing step during the process, the iteration loop is performed in only 10ms on a PC equipped with a 3GHz Dual Core Xeon Intel processor. From the initial pose error ∆r init = (-18mm, 12mm, 6mm, -8 • , 12 • , 15 • ), the desired pose is then reached in 3s. After the algorithm convergence, the final pose error is on the order of 10 -6 in meter and degree.

B. Tracking task

For a tracking task, we set the desired image to be the initial observed one. In such a configuration, the interaction matrix can therefore be pre computed at the initial pose without being updated during the servoing task. As previously, the desired interaction matrix L s * is then used in the control law. However, this approximation is well justified in a tracking application and ensures this time a good behavior of the Gauss-Newton control law (1). The results of the tracking task are shown in Fig. 5. For validation purpose, we define in the simulation environment the object frame as superimposed with the probe frame at the first iteration of the algorithm. The relative pose error oM p between both frames, displayed during the tracking task, remains lower than 0.4mm in translation and 0.15 • in rotation and shows the efficiency of the visual control.

IV. ROBOTIC EXPERIMENT

In a medical robotic application, safety issues imply to combine the visual control with a force control insofar as the US probe relies on the patient skin. We propose then to add to the visual task a force constraint which guarantees a constant force applied on the patient.

A. Force and image control combination

Two sensors are now used to control the system. We implement a hybrid vision/force control based on an external control loop approach. The force control is used to servo the translational motion along the y-axis of the probe frame while the five remaining dof are controlled by the visual servoing control scheme. Fig. 6. The robot end effector (frame F e ) is equipped with a force sensor (frame F s ) and an US probe (frame F p ).

1) Force control:

We implement a torque/force control law to guarantee a constant resulting force of 1N applied on the contact point pc of the probe with the object surface along the y-axis of the probe frame. pc H pc corresponds to the contact force tensor expressed in the frame F pc , which is centered on the contact point and aligned with the probe frame F p (see Fig. 6). It is given by the following relationship:

pc H pc = pc F s ( s H s -s F g g H g ), (8)
a F b is the transformation matrix used to express in the frame F a a force tensor known in the frame F b :

a F b = a R b 0 3×3 [ a t b ] × a R b a R b , (9)
where a t b and a R b are the translation vector and the rotation matrix of the frame F b with respect to the frame F a and [ a t b ] × is the skew symetric matrix related to a t b .

s H s is the total force tensor measured by the force sensor and s F g g H g is the gravity force applied on the force sensor due to the mass m p of the US probe, both are expressed in the force sensor frame. The g H g tensor is defined as g H g = [0 0 9.81m p 0 0 0] ⊤ in the frame F g centered on the mass center of the probe as indicated in Fig. 6.

We use a 6 × 6 selection matrix Ms = diag(0, 1, 0, 0, 0, 0) to apply the force control only along the y-axis of the probe. We express then the resulting force tensor in the force sensor frame F s and we compute the instantaneous velocity of the sensor v s from the following proportional force control law:

v s = - K s F pc (Ms pc H pc -pc H pc * ) k , (10)
where pc H pc * = [0 1 0 0 0 0] ⊤ is the desired contact force, k is the contact stiffness and K is the control gain.

2) Vision control: As we chose for safety reasons to give priority to the force control over the vision control, the latter can fail to converge to the desired image since the ytranslational velocity component due to the image control is not applied to the probe. To deal with this issue, we propose to separate the dof controlled by the force control from the others. The five velocity components corresponding to the translations along the x and z axis of the probe and to the three rotations are applied physically to the US device while the last component corresponding to the y translation is virtually applied to a window containing the region of interest (ROI) and included in the US image (see Fig. 7). The velocity applied to the probe due to the image control is then such as v p = v c given by (1), except for the component corresponding to the translation along the y-axis of the probe which is set to zero.

3) Sensor fusion: To combine the force and the vision control, we send the following angular velocity q to the end effector of the robotic arm:

q = e J e + v e = e J e + ( e V s v s + e V p v p ), (11)
where e J e + is the pseudo inverse of the robot Jacobian. Both image and force sensors being rigidly attached to the robot end effector, the control velocity of the effector v e can be expressed in function of the control velocity of each device v s and v p through the transformation matrices e V s and e V p , which are similarly defined in the following way:

e V p = e R p [ e t p ] × e R p 0 3×3 e R p , (12)
where e t p and e R p are the translation vector and the rotation matrix of the probe frame F p expressed in the coordinates system of the end effector F e .

In addition, the v c y component computed with the vision control is applied to the considered window of interest to readapt its position in the US image.

B. Results

Experiments are performed with an anthropomorphic robotic arm equipped with a convex 2-5 MHz US transducer and a force sensor (see Fig. 8(a)). The same realistic US abdominal phantom already used for the simulation validation represents the patient body. The considered application is a tracking task which allows us to compute the interaction matrix only at the initial pose of the probe. The image 3D gradient is computed with the 5 × 5 × 5 filters and a Kalman filter is implemented to predict the phantom motion and increase the reactivity of the control. The Kalman filter is on a constant velocity model and takes as input the measures of the image features variation and the probe instantaneous velocity. The estimated object velocity vo is finally reinjected into the control law (1) as a prediction term:

v c = -λ L s * ⊤ (s(t) -s * ) + vo .
To validate the efficiency of the task, the relative pose of the probe with respect to the object is used as ground truth data. Both poses of the object and the probe are estimated by virtual visual servoing [START_REF] Marchand | Virtual visual servoing: A framework for real-time augmented reality[END_REF] thanks to a well calibrated camera observing the experiment scene and visual markers fixed on the probe and the phantom (see Fig. 8(b)). The 3D poses of the probe cam M probe and the US phantom cam M ph expressed in the camera frame are then used to compute their relative pose:

ph M probe = cam M -1 ph cam M probe
We position the 2D US probe on the abdominal phantom and we use a ROI of the observed US B-scan as the desired image to consider only relevant anatomic data in the visual control (see Fig. 9). The force/vision servo process is launched after the small automatic back and forth outof-plane translation used to estimate the 3D image gradient. Then we manually apply various translational and rotational motions to the phantom. The Fig. 9 shows the results of one tracking experiment. When important changes are applied on the object motion, the error of the probe pose with respect to the phantom and the image measure error both increase because of the tracking delay. However the image based algorithm is robust enough and the probe converges to the desired pose. Then, at the end of the tracking task, initial and final respective poses of the probe with respect to the phantom are compared and their difference is computed: ∆ ph r probe = (-0.28mm, 0.03mm, 0.24mm, -0.13 • , 1.74 • , -0.17 • ). Compared to the maximum motion amplitude applied to the phantom along each direction in translation and rotation, this error is on the order of 0.1% in translation and 1% in rotation, which demonstrates the success of the task. More tracking experiments results with visual and pose validations are presented in the attached video.

V. CONCLUSION This paper presents a new approach of US image based robotic control. In order to avoid any segmentation process and to be robust to changes in the organ topology, the proposed control directly uses the B-scan image provided by the US probe as visual feature. The interaction matrix enabling the control of the six dof of the system from the image intensity is computed from the 3D image gradient of the US scan. As the estimation of this 3D gradient requires additional parallel images, we focus on tracking and local positioning tasks where the interaction matrix can be pre computed at the desired pose and used in the algorithm without being updated. However, in further works, positioning tasks involving the current interaction matrix can be considered using a 3D US probe in order to take into account higher initial pose errors. The challenge remains also in considering physiological non rigid motions.

Fig. 1 .

 1 Fig. 1. The three cubic filters are applied in each pixel of the image I 0 to compute the gradient components (∇I x , ∇I y , ∇I z ) using the additional parallel images I a and I b acquired on both sides of I 0 .

Fig. 2 .

 2 Fig. 2. (a) The US abdominal phantom (Kyoto Kagaku). (b) The volume loaded in the simulator is represented by two orthogonal slices and the virtual probe plane, defined with the frame F p , is displayed in red.

Fig. 3 .

 3 Fig. 3. Automatic probe positioning on a desired cross section of the right kidney. Full 250 × 250 initial (a) and desired (b) images. (c) and (d) are respectively the initial and final image differences I(r * ) -I(r) where I(r) is the 170 × 170 sub image considered as visual feature.

Fig. 4 .

 4 Fig. 4. Evolution of the visual (a) and pose error (b) in mm and deg, (the θ u representation is used to describe the orientation).

Fig. 5 .

 5 Fig. 5. Results of the tracking of a kidney cross-section (a) while sinusoidal motions are applied to the US volume. (b) Relative pose of the probe in the object frame. Evolution of the translational (c) and rotational (d) components of the object (line) and probe (dots) poses expressed in the simulator world frame.

Fig. 7 .

 7 Fig. 7. Tracking application with five dof physically actuated. (a) The US slice to track with in red the desired ROI. (b) The final view of the US probe including the current ROI in cyan. (c) and (d) show the displacement between the initial and final poses of both object and US plane.

Fig. 8 .

 8 Fig. 8. (a) The ADEPT Viper robotic system. (b) View of the external camera used to compute the probe and object respective poses.

Fig. 9 .

 9 Fig. 9. Tracking application with the robotic arm. The initial (a) and final (b) images are displayed with in cyan the ROI. Although large translational (c) and rotational motions (d) are applied to the phantom, the respective pose of the probe in the object frame (e), (f) and the image measure error (g) remain roughly constant. The force applied to the probe (h) is maintained around 1N during the tracking process as expected from the force control.

ACKNOWLEDGMENT

The authors acknowledge the support of the ANR project US-Comp of the French National Research Agency.


Caroline Nadeau

Alexandre Krupa

Jacques Gangloff

Automatic Tracking of an Organ Section with an Ultrasound Probe: Compensation of Respiratory Motion

Keywords: visual servoing, ultrasound, motion compensation

In minimally invasive surgery or needle insertion procedures, the ultrasound imaging can easily and safely be used to visualize the target to reach. However the manual stabilization of the view of this target, which undergoes the physiological motions of the patient, can be a challenge for the surgeon. In this paper, we propose to perform this stabilization with a robotic arm equipped with a 2D ultrasound probe. The six degrees of freedom of the probe are controlled by an image-based approach, where we choose as visual feedback the image intensity. The accuracy of the control law is ensured by the consideration of the periodicity of the physiological motions in a predictive controller. Tracking tasks performed on a realistic abdominal phantom validate the proposed approach and its robustness to deformation is assessed on a gelatin-made deformable phantom.

Introduction

Among the different medical imaging modalities, ultrasound (US) imaging is particularly attractive for providing intra-operatively real-time images. Indeed, this modality is safe and non-invasive for the patient and the US transducer, cheap and not cumbersome, can be easily used in an operating room. In particular, the US modality can provide to the surgeon a view of an organ or a tumor to reach throughout a minimally invasive surgery or a needle insertion procedure. In such applications, image-guided robotic systems could assist the surgeon insofar as they can follow an organ that undergoes physiological motions and thus automatically stabilize the US image of the target. Some previous works dealing with US image-based robotic systems have focused on target tracking applications. In [START_REF] Abolmaesumi | Image-guided control of a robot for medical ultrasound[END_REF], the three in-plane motions of an US probe are controlled to automatically center in the US image the cross-section of an artery while an out-of-plane translation of the probe is manually performed. Five features extraction methods based on image similarity measures and contour segmentations are compared to track the artery center. In another work [START_REF] Lee | Ultrasound-based visual servoing system for lithotripsy[END_REF], the three translations of a XYZ stage robot equipped with two US probes and a HIFU transducer are controlled to follow a kidney stone while compensating for physiological motions during a lithotripsy procedure. In [START_REF] Krupa | Real time motion stabilization with B-mode ultrasound using image speckle information and visual servoing[END_REF], an approach based on the speckle correlation observed in successive US images is detailed. However, a region of fully developped speckle has to be segmented and a step of learning of the speckle decorrelation curves is required. Moreover in these different approaches, the visual error minimization is realized with a proportional control law which does not benefit from the model of the disturbance.

On the other hand, physiological motions compensation using predictive controller in the control loop of the robotic system is a promising approach to take advantage of the repetitiveness of these disturbances. It has been developed in previous works where the tracking of natural or artificial landmarks is performed using an endoscopic camera. This visual feedback gives then a measure of the organ motion which is combined with a model of the periodic or quasi-periodic disturbance to predict and anticipate this motion. Many contributions focus on cardiac motion compensation using endoscopic cameras. In [START_REF] Nakamura | Heartbeat synchronization for robotic cardiac surgery[END_REF], Nakamura et al. present a tele-operated system to assist surgeons during beating heart surgeries. The slave arm is synchronized with the heart beats thanks to the visual feedback of a high speed camera and provides to the surgeon a stabilized image of the heart. In the same context of beating heart surgery, a motion prediction scheme is developed in [START_REF] Ortmaier | Motion estimation in beating heart surgery[END_REF] to increase the robustness of the detection and the tracking of natural landmarks on the heart surface in a laparoscopic view. More generally, Bebek et al. [START_REF] Bebek | Intelligent control algorithms for robotic-assisted beating heart surgery[END_REF] describe improvements in motion canceling by taking into account biological signals (ECG) in the predictive algorithm. Recently, a comparison between various predictive filtering methods has been proposed in [START_REF] Yuen | Robotic motion compensation for beating heart intracardiac surgery[END_REF] to predict the motion of the mitral valve annulus. Under the assumption of a major 1D translational motion of this annulus, the motion compensation of one degree of freedom (dof) based on an extended Kalman filter is validated in situation of high noise, time delay and heart rate variability.

In this paper, we propose to control the six dof of a robotic arm holding a 2D US probe in order to follow a desired abdominal cross section while compensating for the respiratory motion. The considered applications are for instance the assistance for diagnoses or hepatic tumor biopsies where the liver and the tumors mainly undergo the respiratory motion [START_REF] Clifford | Assessment of hepatic motion secondary to respiration for computer assisted interventions[END_REF]. Other clinic applications, such as prostate cancer brachytherapy have been identified in [START_REF] Krupa | Real time motion stabilization with B-mode ultrasound using image speckle information and visual servoing[END_REF] that could benefit from such robotic image stabilization. To deal with the low quality of the US images, we propose to directly use the intensity of the image as visual feature, which has been successfully applied in camera-based visual servoing [START_REF] Collewet | Photometric visual servoing[END_REF]. The structure of our paper is as follows: the second section details the principle of the intensity-based approach and the computation of the interaction matrix that links the intensity features variation to the motion of the 2D probe. The section 3 presents the proportional and predictive control laws implemented and compared with simulation results. The results of the tracking task performed on an abdominal and on a deformable phantom are presented in section 4 and included in a video 4 . imaging modalities, making the extraction of natural landmarks particularly complicated. We propose therefore to consider as visual features s the intensity values of the pixels of a region of interest (ROI) of the US image:

s = {I(1, 1), ..., I(u, v), ..., I(M, N)} , (1)
where M and N are respectively the width and the height of the ROI and where I(u, v) represents the intensity of the pixel of coordinates (u, v) in the US image. Given R p (x p , y p , z p ) the frame attached to the US probe where (x p , y p ) defines the image plane and z p corresponds to the elevation axis, the coordinates x = (x, y, z) of the image features in this frame are such as:

(x, y, z) = (s x (u -u 0 ), s y (v -v 0 ), 0),
with (s x , s y ) the image pixel size and (u 0 , v 0 ) the pixel coordinates of the image center, and z = 0 since the considered visual features belong to the US image plane.

The computation of the interaction matrix L s that links the variation of these image features to the motion of the probe is based on the constancy of the US wave reflexion by a physical 3D point. Given such a 3D point at the position x at the time t, which moves to the new position x + dx at the time t + dt and considering that the US reflexion is converted in an intensity value in a B-mode US image, the US reflexion conservation yields to the following intensity conservation equation:

I(x + dx,t + dt) -I(x,t) = 0. (2)
We can expand this equation in the form of a first order Taylor series:

∂ I ∂ x dx + ∂ I ∂ y dy + ∂ I ∂ z dz + ∂ I ∂t dt = 0. (3)
Then, the time variation of each pixel intensity I(u, v) can be expressed as a function of the corresponding 3D point motion :

İ = -∇I ẋ, (4)
with ∇I = (∇I x ∇I y ∇I z ) the 3D image gradient, which is computed from the current probe image and at least two out-of-plane additional images. In practice, with a conventional 2D US probe mounted on a robotic arm, a small back and forth translational motion along the elevation direction is applied to acquire these additional images. According to the kinematics fundamental relationship, the velocity of the 3D point ẋ is linked to the probe velocity v c through the interaction matrix L x :

ẋ = L x v c , L x =   -1 0 0 0 -z y 0 -1 0 z 0 -x 0 0 -1 -y x 0   . (5)
The interaction matrix L I of size 1 × 6 associated to each visual feature is then defined as L I = -∇I L x and the complete interaction matrix L s , defined as ṡ = L s v c , is built by stacking the M × N matrices L I associated to each pixel.

Control Strategy

Classical Control Law

An image-based visual servoing control scheme consists in minimizing the error between a current set of visual features s and a desired one s * . In the case of a number p of visual features exceeding the number m of dof of the controlled system, a combination matrix C of size m × p and full rank m is introduced to define the task function to minimize e(t) = C (s(t)s * ) .

In an eye-in-hand system, the instantaneous velocity applied to the imaging sensor v c is computed from the task function and the interaction matrix L s . More particularly, the classical visual servoing control law described in [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF] is based on an exponential decrease of the task function (ė = -λ e, λ > 0) and presents the best behavior with C = L s + , where L s + is the pseudo-inverse of the matrix estimate L s , which yields to:

v c = -λ L s + L s -1 L s + (s(t) -s * ) = -λ L s + (s(t) -s * ) (6)

Predictive Controller

In the previous formulation, the controller does not take advantage of some knowledge on the disturbance model and more particularly of its periodicity. We propose then to implement in the control loop a predictive controller, based on the principle of the unconstrained Generalized Predictive Controller (GPC) [START_REF] Clarke | Generalized predictive control -Part I: The basic algorithm[END_REF], in order to anticipate the effect of the disturbance. We consider a system with multiple inputs u and outputs y. The former are the six velocity components applied to the probe and the latter correspond to the image features observed in the current image s. In practice, in order to avoid an excessive number of visual components and to ensure the controllability of the system, we introduce the combination matrix in the system loop to consider an output vector of dimension 6: y = Cs = L s + s. The system is described with an ARIMAX model:

A(z -1 )y(k) = B(z -1 )u(k -1) + P(z -1 ) ∆ (z -1 ) b(k), (7)
where the polynomials A and B represent the theoretical model of the robotic system, approximated by a pure integrator. The noise is modeled by a white noise term b(k) which is colored by the polynomial matrix P and made non stationary by the operator ∆ (z -1 ). In the classical GPC, this operator is taken as an integrator:

∆ (z -1 ) = 1 -z -1 .
In the case of periodic disturbances with a known fixed period, Gangloff et al [START_REF] Gangloff | Model predictive control for compensation of cyclic organ motions in teleoperated laparoscopic surgery[END_REF] proposed to consider a repetitive noise model by modifying this operator as follows:

∆ R (z -1 ) = 1 -αz -T , 0 < α ≤ 1
where α is a forgetting factor making the noise model repetitive with a period T corresponding to the number of sampling periods in one period of the disturbance signal.

To ensure that this repetitive noise model only affects the disturbance rejection and not the response to the visual reference, the new controller called R-GPC is a combination of two GPC (see Fig. 1). The controller GPC 1 is applied to the theoretical model of the system with no noise term which allows the robot to anticipate the future references r = Cs * . The second controller GPC 2 includes the repetitive noise model previously defined to cancel the disturbance effect on the output vector. The cost function minimized by the R-GPC approach is therefore composed of the cost functions of GPC 1 and GPC 2 as detailed in [START_REF] Gangloff | Model predictive control for compensation of cyclic organ motions in teleoperated laparoscopic surgery[END_REF]:

J(u = u 1 + u 2 , k) = N 2 ∑ j=N 1 ŷth (k + j) -r(k + j) 2 + N 2 ∑ j=N 1 ε(k + j) 2 + λ N u ∑ j=1 δ u 1 (k + j -1) 2 + µ N u ∑ j=1 δ u 2 (k + j -1) 2 (8)
where N1, N2 are the bounds of the prediction horizon and N u is the length of the control horizon. λ and µ weight the control energies respectively for reference tracking and disturbance rejection. With this structure, the control signal u(t) applied to the real system is composed of the two components u 1 (t) and u 2 (t). The former corresponds to the input of the theoretical system model without disturbance that generates the theoretical output y th , the latter allows the rejection of the error ε(t) due to noises and disturbance.

Results and Discussion

Simulation Results with a Human Liver

To validate our approach, we use a software simulator that we have developed to reconstruct and display a dense volume from a set of parallel images. In addition to this display functionality, the simulator allows us to move the reconstructed volume wrt a fixed Cartesian frame and to control a 2D virtual probe which generates an US image by cubic interpolation process. For the simulation experiments, an US complete volume of a human liver is loaded in the simulator. This volume is created from a set of 218 parallel images of resolution 308 × 278, the voxel size is 0.5 × 0.5 × 0.5mm 3 .

In abdominal surgery, the hepatic motions are mainly induced by the respiration [START_REF] Clifford | Assessment of hepatic motion secondary to respiration for computer assisted interventions[END_REF]. The cranio-caudal translation is the most important motion with an established range of 10 to 26mm and additional translations of about 8mm in antero-posterior and lateral directions are observed. In line with these data, a periodic translational motion is applied to the liver volume with amplitudes of 16mm, 5.5mm and 7mm along the x, y and z axes. The results of the simulation are displayed in Fig. 2. During three breathing periods no compensation is applied, then at t = 15s, the predictive controller R-GPC is launched and compared with the classical control law [START_REF] Bebek | Intelligent control algorithms for robotic-assisted beating heart surgery[END_REF]. From the first period the probe follows the liver motion and the misalignment measure defined as C (s) = (ss * ) ⊤ (ss * ) is significantly reduced with both approaches (see Fig. 2(c) and (e)). With the R-GPC, the periodic disturbance is predicted and the accuracy of the tracking is improved.

Experimental Results with the Predictive Controller

The experiments have been performed with a robotic arm equipped with a convex 2-5 MHz US transducer on an abdominal phantom and a deformable one (see Fig. 3).

For the considered medical applications, we combine the visual control with a force control insofar as the US probe relies on the patient skin. We therefore add to the visual task a force constraint which guarantees a constant force of 1N applied to the phantom. The force control is used to servo the translational motion along the y-axis of the probe frame while the five remaining dof are controlled by the visual servoing control scheme. The Fig. 4 shows the results of one tracking experiment realized with the predictive controller where a sinusoidal perturbation is applied to the abdominal phantom with a period of 6s, mainly along the x and z axes in translation and around the y-axis in rotation. The visual error is reduced from the second period, which guarantees the stabilization of the organ cross section in the US image during the disturbance cycles. The robustness of the control law when dealing with deformable objects is assessed using a gelatin-made phantom filled with cereal fibers to create US speckle and olives modeling hepatic tumors (see Fig. 5). At t = 5s, a periodic translational motion of about 2cm is applied to this phantom which is also pressed against a wall to create local deformations. In this particular case, the relative pose of the probe wrt the phantom can not been measured and the success of the task is visually validated by the display of the view of the US probe and the corresponding image difference with the desired image (see Fig. The tracking results validate the intensity-based approach to deal with the stabilization of a 2D US image by compensating periodic disturbances with a predictive controller. The limitation due to the low quality of the US images is overcome by the choice of the image intensity as visual feature and the six dof of the probe are controlled thanks to the modeling of the interaction matrix associated to these features.

Fig. 1 .

 1 Fig. 1. Bloc diagram of the R-GPC including a control delay of one sampling period and a delay of d sampling periods due to the measure.

Fig. 2 .

 2 Fig.2. Results of the tracking of a liver cross-section (a) that undergoes a periodic translational motion (b). For the same task, the results obtained with the proportional control law (c,d) and with the predictive one (e,f) are compared in term of visual error (c,e) and pose error (d,f).

Fig. 3 .

 3 Fig. 3. Experimental setup with the abdominal (left) and gelatin-made (right) phantoms.

Fig. 4 .

 4 Fig. 4. Tracking of an abdominal cross section (a) while the phantom undergoes a periodic external motion applied at t = 6s in translation (b) and rotation (c). The low value of the visual error (d) and the constant position (e) and orientation (f) of the probe wrt the phantom validate the task.

  5(b) and (c)) at t = 37s, which corresponds to the maximum error in the misalignment measure (see Fig.5(d)) observed during the tracking task.

Fig. 5 .

 5 Fig. 5. Tracking results with the deformable phantom of an US slice containing two tumors.

US Image-based ApproachThe US images of the human body present a low quality due to the noise, called speckle, generated by the propagation of the US waves in the soft tissues. Because of this speckle, the processing of the US images is often more complex than with other 4 http://www.irisa.fr/lagadic/demo/demo-us-servoing-intensity/intensity us servoing.html

Acknowledgment. The authors acknowledge the support of the ANR project US-Comp of the French National Research Agency.