Geometric Motion Estimation and Control for                          Robotic-Assisted Beating-Heart Surgery               ...
ration component does not contain a rotation, which may                 The relative velocity of two frames, meaning both ...
B11B12   B1k    B1nB11components: the respiratory phase and frequency can be                                              ...
history prediction                                                                            4    combined motionthe meas...
surgical tool to the motion of the area of interest. This prob-     B. Asymptotically stabilizing control lawlem is usuall...
V                                                                           alternative to the (implicit) model-based cont...
Upcoming SlideShare
Loading in …5

Geometric Motion Estimation and Control for Robotic-Assisted ...


Published on

  • Be the first to comment

  • Be the first to like this

No Downloads
Total views
On SlideShare
From Embeds
Number of Embeds
Embeds 0
No embeds

No notes for slide

Geometric Motion Estimation and Control for Robotic-Assisted ...

  1. 1. Geometric Motion Estimation and Control for Robotic-Assisted Beating-Heart Surgery Vincent Duindam and Shankar Sastry Department of EECS, University of California, Berkeley, CA 94720, USA {vincentd,sastry} Abstract— One of the potential benefits of robotic systemsin cardiac surgery is that their use can increase the numberof possible off-pump (beating heart) coronary artery bypassgrafting procedures. Robotic systems can actively synchronizethe motion of surgical tools to the motion of the surface of theheart, with the surgeon specifying only the relative motion ofthe tool with respect to the heart. Accurate prediction of the Ψdmotion of the heart surface is obviously of crucial importancefor the safety and robustness of such a system. Ψ0 This paper presents a novel approach to predict the motionof the surface of a beating heart. We show how ECG andrespiratory information can be used to extract two periodic diaphragmcomponents from the quasi-periodic motion of the heart sur- Ψrface. Contrary to most existing literature, we consider thefull geometric motion, including rotation due to respiration. ΨhWe then show how to combine the periodic components to heartaccurately predict future motion of the heart surface, and howthis information can be used to design an explicit controller thatasymptotically stabilizes the relative motion of the surgical toolto a desired relative distance and orientation. I. I NTRODUCTION Fig. 1. Schematic system representation and definition of various coordi- Robotic assisted surgery is becoming increasingly com- nate frames. The motion of a robotic tool (with attached frame Ψr ) is tomon, and advanced robotic systems are more widely avail- be synchronized to the motion of a certain small area on the heart surfaceable in hospitals. One of the advantages of robotic tools is (frame Ψh ). The motion of the heart surface is caused by a combination of motion of the diaphragm due to breathing (frame Ψd ) and the beatingthe potential to facilitate more advanced minimally invasive motion of the heart procedures, which have the benefits of shorter recov-ery time and less invasive scarring. Although current robotictools are usually programmed to only follow the surgeon’s One of the most crucial (and in some sense most difficult)movements, their true potential lies in the possibility to aspects of such a motion synchronization system is thecontrol them in a more active way. accurate measuring and prediction of the motion of the heart One of the applications in which this could make a surface. First, it requires accurate sensors to measure thedifference is in cardiac operations such as coronary artery motion of the surface, and several different technologies havebypass grafting (CABG). Currently, many cardiac procedures been proposed in literature, including high-speed cameras [6]require the heart to be stopped (and a heart-lung machine to and piezoelectric crystals [7]. But secondly, and this is thebe used) to allow surgery on a motionless surface. However, topic of the present paper, the motion of the heart surfacethe use of a heart-lung machine can have damaging effects is quasi-periodic, which means it is the combination of twofor the patient and should therefore be used as little as periodic motions, as illustrated in Fig. 1. One periodic motionpossible. Actively controlled robotic tools could be employed is caused by the diaphragm, which moves at the frequencyinstead, and be synchronized to move at a fixed relative of respiration, and the other periodic motion is caused by thedistance and orientation with respect to the heart surface beating of the heart itself.(see [1], [2], [3] for more details on this idea). Through this The approach in literature to predict this quasi-periodicrelative stabilization, together with stabilized visual feedback motion is to assume that it is simply the sum of two periodic[4], [5] from the operating area to the surgeon, a surgeon only components, and then to separate these two componentsneeds to provide the desired relative motion of the tool with using e.g. a low-pass filter [2], an adaptive harmonic filterrespect to the heart, thus possibly eliminating the need to bank [1], or direct time series analysis [8]. As shown instop the heart. Section III, however, this implicitly assumes that the respi-
  2. 2. ration component does not contain a rotation, which may The relative velocity of two frames, meaning both thenot be accurate enough for high-precision applications [9]. linear and angular velocity, can be concisely represented byIn addition, the methods only consider the motion of points, an element of se(3), the Lie algebra of SE(3). Numerically, c,adiscarding rotational information, even though the orientation it can be expressed as a 6 × 1 vector Tb or 4 × 4 matrixof the heart surface needs to be known in order to properly ˆc,a of the form Tborient the surgical tool. c,a c,a ωb ˆc,a ω c,a ˆ c,a vb The purpose of this paper is to present a general method Tb := c,a or Tb := b (4) vb 0 0to separate a quasi-periodic rigid 3-D motion into its twoperiodic components, and to use the result in the design c,a c,a with ωb , vb ∈ R3 the relative angular and linear velocity.of an asymptotically synchronizing controller. We assume c,a The vector Tb is called a twist and describes the relativethe frequencies of the two motion components to be known, velocity of frame Ψb with respect to Ψa , expressed in framefor example based on an ECG signal and information from Ψc . It is related to the relative configuration (1) asthe mechanical ventilator used during surgery. The estimated ˆc,a c ˙a b T b = Ha Hb Hc (5)components are then combined to predict future motion ofthe heart surface. This information, in turn, is used in the ˙ with H denoting the time-derivative of H. Finally, thedesign of an explicit controller that stabilizes the relative following two relations are used in Section IV.motion of a surgical tool to a desired distance and orientation dwith respect to the heart surface. d,a c,a Rc 0 Tb = AdHc Tb with AdHc := d d d d d After the necessary mathematical preliminaries in Sec- ˆ pc R c Rction II, we discuss in Section III that the total motion of the d ˆ d,d ωc 0heart surface is a (nonlinear) geometric phenomenon, and AdHc = adTc AdHc with adTc := d,d d d,d d d,d dt ˆ vc ˆ d,d ωcshow how the periodic respiratory and cardiac motions canbe extracted. We then present a control law (Section IV) III. Q UASI -P ERIODIC MOTION PREDICTION IN SE(3)that uses the predicted motion to asymptotically stabilize The motion of the heart surface is caused by the combinedthe motion of the surgical tool to a desired distance and dynamics of respiration (mainly affecting the heart throughorientation with respect to the heart surface. We illustrate motion of the diaphragm) and cardiac muscle contractions.the results in simulations. The full dynamics are very complex and hard to predict, and require many patient-specific parameters to be fitted [12]. II. M ATHEMATICAL P RELIMINARIES To simplify the analysis and allow real-time prediction, we We first briefly summarize the theory and notation used to do not take the cause of the motion into consideration, andrepresent rigid 3-D motions and velocities. We refer to [10] only consider the kinematics of a local area of interest onand [11] for a more extensive background. the heart surface. Its motion is assumed to be a combination The relative configuration of a right-handed coordinate of two periodic motions: one due to respiration (with periodframe Ψa with respect to a right-handed frame Ψb , meaning Td , with d for ‘diaphragm’), and one due to heart beatingtheir relative position and orientation, can be described by (with period Th , with h for ‘heart’). The combination of twoan element of the Lie group SE(3), the Special Euclidean periodic motions is called a quasi-periodic Numerically, this configuration can be expressed by a Since the two periods Td and Th are generally not integer atime-varying 4 × 4 homogeneous matrix Hb (t) of the form multiples of each other, the period of the total motion (the time after which it repeats itself) can be arbitrarily long. a a a Rb (t) pb (t) As the frequencies of the two periodic motions are different Hb (t) := (1) 0 1 (typically Td ≈ 4 s and Th ≈ 1 s), a low-pass filter may a awhere Rb (t) is a 3 × 3 orthogonal matrix with det(Rb ) = 1 seem a relatively simple solution to separate the respiratory athat describes the relative orientation, and pb (t) is a 3 × 1 component [2]. However, this assumes that the respirationvector that describes the relative position of the origins. is a purely sinusoidal motion without higher harmonics, and A rotation can not only be described by an appropriate moreover, that the two signals mix linearly. As shown in3 × 3 matrix, but also by so-called unit quaternions, which Section III-A, this second assumption does not hold whencan be thought of as a set of four real numbers rotation is taken into account. In addition, a (causal) low-pass filter that cleanly distinguishes between two signals with Td (q0 , q) := (q0 , q1 , q2 , q3 ) ∈ R4 (2) and Th in this close range will generally suffer from a large phase shift that can cause stability and performance problemssatisfying (q0 , q) ◦ (q0 , −q) = (1, 0), with ◦ the (non- in feedback loops.commutative) Grassman product defined as Fortunately, in this application, we have a reasonably good (q0 , q) ◦ (p0 , p) := (q0 p0 − q T p, q0 p + p0 q + pq) ˆ (3) estimate of the phase1 and frequency of the two motion 1 The precise definition of phase is not important in this context; we canwith x := x∧ for x ∈ R3 . Unit quaternion representa- ˆ choose any number that indicates the ‘part’ of the cycle as a function oftions can be converted to matrix representations (and back) time. For example, it is sufficient to choose a linear interpolation from zerothrough relatively straight-forward computations. to one between every two consecutive QRS-complexes in the ECG cycle.
  3. 3. B11B12 B1k B1nB11components: the respiratory phase and frequency can be φ1obtained from the mechanical ventilator, and the cardiac φ1 x1phase and frequency are detected by an ECG monitor. We tcan exploit this prior knowledge to extract the two periodic xcomponents from measurements of the combined motion. compute optimal aij φ1 x2 tA. Heart motion as a group product φ2 As the first step to characterize the motion of the heart B21 B22 B2k B21 φ2surface, we define what we mean exactly by ‘combination’ tof two motions. Since both the respiratory motion and φ2the cardiac motion generally contain translation as well as Fig. 2. Schematic representation of the algorithm. Based on measurementsrotation, we can write the relative displacement of the heart of a quasi-periodic signal x and the phase trajectories of its two periodicsurface as a group product of two homogeneous matrices Hd 0 components, and given a choice of basis functions Bij , the optimal pa- d 0 rameters aij are computed. The resulting representation of the componentsand Hh , as illustrated in Fig. 1. The matrix Hd describes xi (φi ) can be used to predict future x(t).the relative motion of the diaphragm Ψd with respect to an dinertial frame Ψ0 , and Hh describes the relative motion ofthe area of interest Ψh on the heart surface with respect to as a sum of basis elements Bij (φi ) and write the total signalΨd . Each component is assumed to have a (roughly constant) x(t) at time t asperiod but unknown shape, i.e. x(t) = x1 (φ1 (t)) ⊕ x2 (φ2 (t)) ⊕ ǫ(t) (8) 0 0 d d Hd (t + Td ) = Hd (t) Hh (t + Th ) = Hh (t) (6) = a1k B1k (φ1 (t)) ⊕ a2m B2m (φ2 (t)) ⊕ ǫ(t)The ‘combination’ of the two motions is the product of the k mtwo matrices, i.e. where ⊕ denotes the composition of the two periodic signals, 0 d 0 d ǫ(t) is a measurement error, and aij are the weights for 0 0 d Rd Rh p0 + R d ph d Hh = H d Hh = (7) the basis elements. Given sufficient measurements, we can 0 1 estimate the parameters aij such that ǫ(t) is minimized,It is important to note that, even if we are only interested e.g. in the least-squares sense. Depending on the choice ofin the relative positions, the combination of the positions basis functions Bij , the coefficients may not be uniquelyis not a simple linear sum of the vectors p0 and pd , but d h determined. For example, the two periodic components canalso nonlinearly depends on the rotation of the diaphragm only be determined up to a constant: if ⊕ denotes the 0Rd . As shown in research on cardiac image registration [9], regular sum operator, then adding a constant to one of theeven though the rotation of the diaphragm is generally only components and subtracting the same constant from the othera few degrees, it can still cause substantial deviations if not component does not change their composition, and similarlyproperly taken into account. For that reason, we consider when ⊕ is multiplication. Other ambiguities will occur if thein this paper both rotation and translation components. The basis functions are chosen in an unfortunate way that makesorientation of the heart surface in itself is also a useful signal them ‘fit’ in both periodic predict, since generally we want to stabilize both the We now apply this general approach separately to therelative position and the relative orientation of the surgical rotation and translation components of the quasi-periodictool with respect to the heart surface. 0 motion Hh (t), as given by (7). We start with rotation and Note that the physical location of the intermediate frame choose to use a representation in unit quaternion coordinates,Ψd is not clearly defined, which results in ambiguities. More as described in Section II. Unit quaternions provide a good 0 dprecisely, given any combination (Hd , Hh ) that satisfies (7), balance between minimizing the dimension of the signal 0 ˜ ˜ −1 dthe combination (Hd H, H Hh ) also satisfies (7), for any space (four for quaternions) and simplifying the expression ˜H of the form (1). This ambiguity can be resolved by for ‘combining’ two rotations (defined by the Grassmanchoosing a convenient initial position and orientation for Ψd , product for quaternions). Although choosing exponentiale.g. such that pd (0) = 0 and Rd (0) = I. h 0 coordinates [13] would further reduce the dimension of the signal space to three, writing the product of two rotationsB. Filtering periodic components in terms of their exponential coordinates produces a quite Given a general quasi-periodic signal x(t), composed of involved expression, which is much harder to optimize thantwo periodic components x1 (t) and x2 (t). Suppose we have the simple bi-linear expression for quaternions:sufficient measurements of x(t) and of the two phases φ1 (t) Q(t) = ǫ(t) ◦ Qh (φh (t)) ◦ Qd (φd (t)) (9)and φ2 (t) of the composing signals, then we can estimatethe periodic components x1 (φ1 ) and x2 (φ2 ) themselves as in which Q(t) is the quaternion expression for the measuredfunctions of their respective phases. The approach we take total rotation at time t, Qd and Qh are the quaternion expres- 0 dis illustrated in Fig. 2. We parameterize each periodic signal sions for the rotations Rd and Rh , respectively, ǫ(t) describes
  4. 4. history prediction 4 combined motionthe measurement error (close to the (1, 0) quaternion), and◦ is the Grassman product defined in (3). We parameterize each component in (9) as in (8) by choos- 0ing a set of basis functions Bij for the quaternions Qd (φd )and Qh (φh ). With this choice and sufficient measurements −4 0 2 4 6 8 t (s) 10Q(t), the next step is to estimate the coefficients aij . An extra diaphragm motion 2complication arises here because of the constraint that Qdand Qh should have unit length at all times, and hence their 0four components cannot be chosen arbitrarily. This can besolved by including constraints in the optimization process, −2or, more pragmatically though not mathematically sound, to 0 2 4 6 8 t (s) 10just freely optimize over all quaternions and normalize the cardiac motion 3result afterwards. A second aspect is the choice of metric forthe optimization, i.e. how to judge what ǫ in (9) is minimal. 0The simplest solution is to minimize (Q−1 ◦Qd ◦Qh )−(1, 0)in the least squares sense, or even just Q − Qd ◦ Qh . Both −3 0 2 4 6 8 t (s) 10are not proper metrics on SO(3) but are easy to optimize (a) Rotation components.and give good results in simulations. 0 Once the rotation components, in particular Rd , have been history prediction combined motiondetermined, we can write the estimation problem (8) for the 4translational components as follows. 0 p0 (t0 ) = Rd (t0 )pd (φh (t0 )) + p0 (φd (t0 )) + ǫ(t0 ) h 0 h d (10) −4with p0 and Rd known and pd and pd to be estimated. This h 0 h 0 0 2 4 6 8 t (s) 10 diaphragm motionrelation is unconstrained and linear in the unknowns and can 3be solved e.g. by a standard linear least squares computation. For both rotation and translation components, several 0design choices have to be made. First, the number of basisfunctions Bij (φi ) and their shapes need to be determined. −2 0 2 4 6 8 t (s) 10Secondly, a metric on the data space needs to be chosen, i.e. a cardiac motionmapping that weighs the relative influence of older and newer 3samples in the estimation of the coefficients aij and hence 0in the prediction of the future signal. If the signal is highlyperiodic, then older samples should be weighed as well asnew samples, such that any random noise is averaged and −4 0 2 4 6 8 t (s) 10thus reduced. If the shape of the signal changes significantly (b) Translation components.over several periods, then only the newer samples should be Fig. 3. Extraction of diaphragm (period 4 s) and cardiac (period 1.1 s)taken into account. Studying experimental data from various motions from noisy measurements of the combined motion. The figurespatients should help make these choices. show the three degrees of freedom for rotation and translation, with x (solid), Once the coefficients aij for rotation and translation have y (dashed), and z (dotted) components. Reconstructed signals are shown on top of measured noisy signals.been determined and hence the shapes of the two periodic 0 0components of Hh are known, future values of Hh can bepredicted by combining the values of the two components 0 d and the periodic signals reconstructed (clean signals in theHd (φd ) and Hh (φh ), evaluated at their predicted phases. bottom graphs, overlaid on the noisy real periodic sourceThe prediction of the phases φd and φh can be a simple components, which are unknown to the algorithm). Using theextrapolation at the current respiration and heart beat rate, obtained parameters and the basis functions, the combinedrespectively. These rates are stabilized by a mechanical signal can be reconstructed (t < 6 s) and its future valuesventilator and by medication and generally only vary quite (t > 6 s) can be accurately predicted. Note that Fig. 3(a)slowly. shows three components for each rotation (exponential coor- Fig. 3 shows a simulation of the extraction of the periodic dinates) although the optimization was performed using fourdiaphragm motion (with period 4 s) and the periodic cardiac components (quaternions).motion (with period 1.1 s) from an artificial combined motionsignal, contaminated with zero-mean Gaussian noise with a IV. E XPLICIT A SYMPTOTIC C ONTROL FOR R ELATIVEstandard deviation of 0.1. We used 20 equally spaced trian- M OTION S YNCHRONIZATIONgular windows as the basis functions for each component.From the measurements (noisy signals in the top graphs) After computing a prediction of the motion of the heartbetween 0 and 6 seconds, the coefficients aij were optimized surface, the next step is to synchronize the motion of the
  5. 5. surgical tool to the motion of the area of interest. This prob- B. Asymptotically stabilizing control lawlem is usually cast as a three-dimensional position tracking Using the error function (12), we propose the followingproblem and often solved using a form of (adaptive) model explicit control law for the robot that moves the surgicalpredictive control [1], [2]. In this section, we generalize the tool. We do not consider specific robot dynamics at thisproblem to include not only position, but also orientation ˙ 0,0 point and only specify the desired inertial acceleration Trinformation, and we present an alternative explicit model- of its end effector frame Ψr . For a particular robot, thisbased controller. desired acceleration should be achieved by a suitable lower level controller. The proposed desired acceleration is theA. Choice of objective function following. The objective of the controller is to stabilize the relative ˙ 0,0 Tr ˙ 0,0 ˙ 0,h = Th − AdHh K1 (dJ) + adT 0,0 Trconfiguration of the robot with respect to a certain area on 0 des h (16)the heart surface. This relative configuration is described by h,h − AdHh K2 Tr + K1 dJ 0a homogeneous matrix of the form h with K1 , K2 > 0 two symmetric positive-definite matrices, Rr ph r ry rz p 0 0,0 ˙ 0,0 and Hh , Th , and Th the estimated configuration, velocity, h Hr = r =: x (11) 0 1 0 0 0 1 and acceleration of the frame Ψh at the area of interest on h,hwith index r for ‘robot’. To quantify the control objective, the heart surface. Intuitively, this controller drives Tr to hwe choose an error function J(Hr ) as follows −K1 dJ (by a gain K2 ), which makes the configuration move along the direction of steepest descent of J (defined using K1 h 1 as a metric). To prove asymptotic stability of the equilibrium J(Hr ) := Jtran + Jrot := kp (p − ∆rz )T (p − ∆rz ) 2 (12) J = 0 for the closed loop system, we consider the following + kx (1 − eT rx ) + ky (1 − eT ry ) + kz (1 − eT rz ) x y z candidate Lyapunov function.with ∆ the desired relative distance between the tool and the 1 h,h T h,h V := κ1 J + Tr + K1 dJ K2 Tr + K1 dJ −1heart, E := ex ey ez a rotation matrix describing the 2desired orientation of the tool frame (with ez = (0, 0, 1)), with 0 < κ1 < 4σmin (K1 ), i.e. κ1 is strictly less than fourand kp , kx , ky , kz > 0 constant parameters. The function J times the smallest singular value of K1 (which is strictly hhas a minimum equal to zero only if p = ∆rz and Rr = E, positive since K1 > 0). Assuming the robot achieves perfecti.e. the origin of Ψr is ∆ along the surface normal away from ˙ 0,0 tracking of (Tr )des , we can compute the time-derivative ofthe origin of Ψh , and the axes of the tool frame are properly V along system trajectories as follows.aligned. The error function increases for distances different ˙ −1 ˙ h,h ˙ V = κ1 (dJ)T Tr + (Tr + K1 dJ)T K2 (Tr + K1 dJ) h,h h,hfrom ∆ and for deviations from the desired orientationspecified by E. Note that when kx = ky = kz , Jrot is = κ1 (dJ)T Tr − (Tr + K1 dJ)T Tr + K1 dJ h,h h,h h,hproportional to 3 − trace(E T Rr ) = 1 − cos(θ), with θ the h h,h κ1 T h,h κ1angle of rotation away from E. = − Tr + (K1 − I)dJ Tr + (K1 − I)dJ 2 2 One of the advantages of the cost function (12) is that its T κ1 − κ1 (dJ) K1 − I dJ (17)time-derivative along the trajectories of the system takes a 4particularly simple form, as shown below. This expression is non-positive since we chose κ1 such that ˙ K1 − κ1 is strictly positive definite. Thus, V = 0 only when ˙ J = kp (p − ∆rz )T (vr − pωr + ∆ˆz ωr ) h,h ˆ h,h r h,h h,h 4 Tr = 0 and dJ = 0. From (13), we see that dJ = 0 when + kx eT rx ωr + ky eT ry ωr + kz ez rz ωr xˆ h,h yˆ h,h T ˆ h,h the following two equations are satisfied. T h,h kx ex rx + ky ey ry + kz ez rz ˆ ˆ ˆ ωr 0 = p − ∆rz (18) = h,h kp (p − ∆rz ) vr 0 = kx ex rx + ky ey ry + kz ez rz ˆ ˆ ˆ (19) =: (dJ)T Tr h,h (13) The first equation has only one solution, p = ∆rz . TheWhere we used the fact that, from (5) and (11), we have second equation, together with the constraint that (rx , ry , rz ) defines an orthonormal frame, gives rise to several possible ˙h ω h,h r ˆ ˆ h,h ωr ry ˆ h,h ωr rz ˆ h,h h,h ω r p + vr Hr = r x (14) h solutions. Only the solution Rr = E is stable, all other 0 0 0 0 solutions are unstable and oriented at least 90 degrees awayand that (ˆ − ∆ˆz )(p − ∆rz ) = 0. Equation (13) is used p r from E, hence they can be safely ignored for practicalin the following sections, as is the expression for the time- purposes. By La Salle’s Principle [14], this proves thatderivative of dJ given below. the proposed control law (16) semi-globally asymptotically stabilizes the system to the desired state. ˙ ˆ ˆ h,h ˆ ˆ h,h ˆ ˆ h,h −kx ex rx ωr − ky ey ry ωr − kz ez rz ωr Fig. 4 shows a simulation of the proposed controller (dJ) = h,h h,h h,h kp (vr − pωr + ∆ˆz ωr ) ˆ r applied to the ideal robot system −kx ex rx − ky ey ry − kz ez rz ˆ ˆ ˆ ˆ ˆ ˆ 0 = T h,h (15) ˙ 0,0 Tr = Tr˙ 0,0 (20) −kp (ˆ − ∆ˆz ) p r kp I r des
  6. 6. V alternative to the (implicit) model-based control algorithms J known from literature. 20 Jtran In future work, we plan to extend the motion separation Jrot algorithm to a recursive version that can quickly recompute the optimal motion parameters when a few new samples arrive. We will also further investigate what sensors and 10 sensing techniques can most reliably and robustly extract the heart motion, which is the input for the presented algorithm. When measurements of the full rigid motion of the heart surface are available, we can verify the signal estimation and 0 5 10 15 20 control approach presented in this paper on real experimental t (s) change in (ex , ey ) change in ∆ data, and tune the various parameters in the algorithms. (a) Time trajectories of the Lyapunov and error functions. ACKNOWLEDGMENTS This research is sponsored through a Rubicon grant from the Netherlands Organization for Scientific Research (NWO). R EFERENCES [1] R. Ginhoux, J. A. Gangloff, M. F. de Mathelin, L. Soler, M. M. A. Sanchez, and J. Marescaux, “Active filtering of physiological motion in robotized surgery using predictive control,” IEEE Transactions on Robotics, vol. 21, no. 1, pp. 67–79, February 2005. [2] O. Bebek and M. C. Cavuso˘ lu, “Predictive control algorithms using ¸ ¸ g biological signals for active relative motion cancelling in robotic assisted heart surgery,” in Proceedings of the IEEE International Conference on Robotics and Automation, May 2006, pp. 237–244. [3] T. J. Ortmaier, “Motion compensation in minimally invasive robotic surgery,” Ph.D. dissertation, Technical University of Munich, March (b) Snapshots as viewed from the tool frame Ψh along the 2003. negative z direction (plus sign denotes the center of the picture). [4] M. Gr¨ ger and G. Hirzinger, “Image stabilisation of the beating heart o by local linear interpolation,” Medical Imaging 2006: Visualization, Fig. 4. Simulation of the control law applied to an ideal robot. Image-Guided Procedures, and Display, vol. 6141, pp. 274–285, March 2006. [5] D. Stoyanov, M. ElHewl, B. P. Lo, A. J. Chung, F. Bello, and G. Z. Yang, “Current issues in photorealistic renderig for virtual andwith all control gains set to unity. Starting from some initial augmented reality in minimally invasive surgery,” in Proceedings of 0configuration and velocity, and with Hh (t) an arbitrary the IEEE International Conference on Information Visualization, 2003,motion similar to Fig. 3, the position and orientation of pp. 350–359. [6] R. Ginhoux, J. A. Gangloff, M. F. de Mathelin, L. Soler, M. M. A.the robot tool converge to the desired relative distance and Sanchez, and J. Marescaux, “Beating heart tracking in robotic surgeryorientation. At t = 7, the desired orientation of the tool, using 500 Hz visual servoing, model predictive control and an adaptivespecified by E, is changed by 90 degrees around the vertical observer,” in Proceedings of the IEEE Conference on Robotics and Automation, April 2004, pp. 274–279.axis (the viewing direction). At t = 14, the desired distance [7] M. B. Ratcliffe, K. B. Gupta, J. T. Streicher, E. B. Savage, D. K.∆ is reduced. In both cases, the system recovers from the step Bogen, and L. H. Edmunds, “Use of sonomicrometry and multi-change in the reference signals and converges asymptotically dimensional scaling to determine the three-dimensional coordinates of multiple cardiac locations: Feasibility and initial implementation,”to the new equilibrium. IEEE Transactions on Biomedical Engineering, vol. 42, no. 6, pp. 587–598, June 1995. V. C ONCLUSIONS AND F UTURE W ORK [8] T. Ortmaier, M. Gr¨ ger, D. H. Boehm, V. Falk, and G. Hirzinger, o “Motion estimation in beating heart surgery,” IEEE Transactions on This paper presents a novel method to extract periodic 3-D Biomedical Engineering, vol. 52, no. 10, pp. 1729–1740, Octobermotion signals from noisy measurements of a quasi-periodic 2005.motion and knowledge of the phase of the composing signals. [9] G. Shechter, C. Ozturk, J. R. Resar, and E. R. McVeigh, “Respiratory motion of the heart from free breating coronary angiograms,” IEEEThe total quasi-periodic motion of a region on the surface of Transactions on Medical Imaging, vol. 23, no. 8, pp. 1046–1056,a beating heart is a nonlinear combination of respiratory and August 2004.cardiac motions, if rotation of the diaphragm due to respira- [10] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994.tion is taken into account. Using a quaternion representation, [11] S. Stramigioli, Modeling and IPC Control of Interactive Mechanicalthe nonlinear signal estimation problem can be formulated as Systems – A Coordinate-free Approach. Springer-Verlag, 2001.a reasonably simple optimization problem, from which the [12] R. Haddad, P. Clarysse, M. Orkisz, P. Croisille, D. Revel, and I. E. Magnin, Functional Imaging and Modeling of the Heart, ser. Lectureperiodic respiratory and cardiac motions can be extracted and Notes in Computer Science. Springer, 2005, vol. 3504, ch. A Realisticfuture heart surface motions can be predicted. Anthropomorphic Numerical Model of the Beating Heart, pp. 384– The predicted motion signal can then be used in the design 393. [13] J. M. Selig, Geometric Fundamentals of Robotics, 2nd ed. Springer-of an explicit model based controller that asymptotically Verlag, 2005.synchronizes the motion of a surgical tool to the motion [14] S. S. Sastry, Nonlinear Systems: Analysis, Stability, and Control, ser.of a region on the heart surface. This controller provides an Interdisciplinary Applied Mathematics. Springer, 1999.